void
StereoDepth::leftRightMatch(PyramidLevel *left_level,
PyramidLevel *right_level,
Points2d* matched_right_keypoints)
{
tictoc("leftRightMatch");
matched_right_keypoints->clear();
int num_kp_left = left_level->getNumKeypoints();
int num_kp_right = right_level->getNumKeypoints();
int level_num = left_level->getLevelNum();
float adj_max_dist_epipolar_line = _max_dist_epipolar_line*(1 << level_num);
//assert (left_level->getNumLevel() == right_level->getNumLevel());
_legal_matches.resize(num_kp_left);
for (int left_kp_ind = 0; left_kp_ind < num_kp_left; ++left_kp_ind) {
Eigen::Vector2d ref_rect_base_uv = left_level->getKeypointRectBaseUV(left_kp_ind);
std::vector<int>& left_candidates(_legal_matches[left_kp_ind]);
left_candidates.clear();
for (int right_kp_ind=0; right_kp_ind < num_kp_right; ++right_kp_ind) {
Eigen::Vector2d diff = ref_rect_base_uv - right_level->getKeypointRectBaseUV(right_kp_ind);
// TODO some sort of binary search
if (diff(1) < -adj_max_dist_epipolar_line) { break; }
// epipolar and disparity constraints
if ((fabs(diff(1)) < adj_max_dist_epipolar_line) &&
(diff(0) > MIN_DISPARITY) &&
(diff(0) < _max_disparity)) {
left_candidates.push_back(right_kp_ind);
}
}
}
int max_num_matches = std::min(num_kp_left, num_kp_right);
if (_matches_capacity < max_num_matches) {
_matches_capacity = static_cast<int>(1.2*max_num_matches);
delete[] _matches;
_matches = new FeatureMatch[_matches_capacity];
}
_num_matches = 0;
_matcher.matchFeatures(left_level, right_level, _legal_matches, &_matches[0], &_num_matches);
// subpixel refinement on correspondences
for (int n=0; n < _num_matches; ++n) {
FeatureMatch& match(_matches[n]);
KeypointData* left_kpdata(match.ref_keypoint);
KeypointData* right_kpdata(match.target_keypoint);
Eigen::Vector2d left_uv(left_kpdata->kp.u, left_kpdata->kp.v);
Eigen::Vector2d init_right_uv(right_kpdata->kp.u, right_kpdata->kp.v);
Eigen::Vector2d refined_right_uv;
float delta_sse = -1;
refineFeatureMatch(left_level, right_level, left_uv, init_right_uv,
&refined_right_uv, &delta_sse);
double ds = (init_right_uv - refined_right_uv).norm();
Eigen::Vector2d refined_right_base_uv = refined_right_uv * (1 << level_num);
Eigen::Vector2d rect_refined_right_base_uv;
_right_frame->rectify(refined_right_base_uv, &rect_refined_right_base_uv);
Eigen::Vector2d diff = left_kpdata->rect_base_uv - rect_refined_right_base_uv;
double disparity = diff(0);
// re-enforce disparity constraints, epipolar constraints, and excessive
// refinement displacement
if ((disparity < MIN_DISPARITY) ||
(disparity > _max_disparity) ||
(ds > _max_refinement_displacement) ||
(fabs(diff(1)) > adj_max_dist_epipolar_line)) {
left_kpdata->has_depth = false;
left_kpdata->xyzw = Eigen::Vector4d(NAN, NAN, NAN, NAN);
left_kpdata->xyz = Eigen::Vector3d(NAN, NAN, NAN);
left_kpdata->disparity = NAN;
continue;
}
Eigen::Vector4d uvd1(left_kpdata->rect_base_uv(0),
left_kpdata->rect_base_uv(1),
disparity,
1);
left_kpdata->xyzw = (*_uvd1_to_xyz) * uvd1;
left_kpdata->xyz = left_kpdata->xyzw.head<3>() / left_kpdata->xyzw.w();
left_kpdata->has_depth = true;
left_kpdata->disparity = disparity;
// TODO we are relying on matches being ordered by increasing left keypoint index.
matched_right_keypoints->push_back(std::make_pair(refined_right_uv(0), refined_right_uv(1)));
}
tictoc("leftRightMatch");
}
IGL_INLINE bool igl::copyleft::boolean::mesh_boolean(
const Eigen::PlainObjectBase<DerivedVA> & VA,
const Eigen::PlainObjectBase<DerivedFA> & FA,
const Eigen::PlainObjectBase<DerivedVB> & VB,
const Eigen::PlainObjectBase<DerivedFB> & FB,
const WindingNumberOp& wind_num_op,
const KeepFunc& keep,
const ResolveFunc& resolve_fun,
Eigen::PlainObjectBase<DerivedVC > & VC,
Eigen::PlainObjectBase<DerivedFC > & FC,
Eigen::PlainObjectBase<DerivedJ > & J)
{
#ifdef MESH_BOOLEAN_TIMING
const auto & tictoc = []() -> double
{
static double t_start = igl::get_seconds();
double diff = igl::get_seconds()-t_start;
t_start += diff;
return diff;
};
const auto log_time = [&](const std::string& label) -> void {
std::cout << "mesh_boolean." << label << ": "
<< tictoc() << std::endl;
};
tictoc();
#endif
typedef typename DerivedVC::Scalar Scalar;
//typedef typename DerivedFC::Scalar Index;
typedef CGAL::Epeck Kernel;
typedef Kernel::FT ExactScalar;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,3> MatrixX3S;
//typedef Eigen::Matrix<Index,Eigen::Dynamic,Eigen::Dynamic> MatrixXI;
typedef Eigen::Matrix<typename DerivedJ::Scalar,Eigen::Dynamic,1> VectorXJ;
// Generate combined mesh.
typedef Eigen::Matrix<
ExactScalar,
Eigen::Dynamic,
Eigen::Dynamic,
DerivedVC::IsRowMajor> MatrixXES;
MatrixXES V;
DerivedFC F;
VectorXJ CJ;
{
DerivedVA VV(VA.rows() + VB.rows(), 3);
DerivedFC FF(FA.rows() + FB.rows(), 3);
VV << VA, VB;
FF << FA, FB.array() + VA.rows();
//// Handle annoying empty cases
//if(VA.size()>0)
//{
// VV<<VA;
//}
//if(VB.size()>0)
//{
// VV<<VB;
//}
//if(FA.size()>0)
//{
// FF<<FA;
//}
//if(FB.size()>0)
//{
// FF<<FB.array()+VA.rows();
//}
resolve_fun(VV, FF, V, F, CJ);
}
#ifdef MESH_BOOLEAN_TIMING
log_time("resolve_self_intersection");
#endif
// Compute winding numbers on each side of each facet.
const size_t num_faces = F.rows();
Eigen::MatrixXi W;
Eigen::VectorXi labels(num_faces);
std::transform(CJ.data(), CJ.data()+CJ.size(), labels.data(),
[&](int i) { return i<FA.rows() ? 0:1; });
bool valid = true;
if (num_faces > 0)
{
valid = valid &
igl::copyleft::cgal::propagate_winding_numbers(V, F, labels, W);
} else
{
W.resize(0, 4);
}
assert((size_t)W.rows() == num_faces);
if (W.cols() == 2)
{
assert(FB.rows() == 0);
Eigen::MatrixXi W_tmp(num_faces, 4);
W_tmp << W, Eigen::MatrixXi::Zero(num_faces, 2);
W = W_tmp;
} else {
assert(W.cols() == 4);
}
#ifdef MESH_BOOLEAN_TIMING
log_time("propagate_input_winding_number");
#endif
// Compute resulting winding number.
Eigen::MatrixXi Wr(num_faces, 2);
for (size_t i=0; i<num_faces; i++)
{
Eigen::MatrixXi w_out(1,2), w_in(1,2);
w_out << W(i,0), W(i,2);
w_in << W(i,1), W(i,3);
Wr(i,0) = wind_num_op(w_out);
Wr(i,1) = wind_num_op(w_in);
}
#ifdef MESH_BOOLEAN_TIMING
log_time("compute_output_winding_number");
#endif
// Extract boundary separating inside from outside.
auto index_to_signed_index = [&](size_t i, bool ori) -> int
{
return (i+1)*(ori?1:-1);
};
//auto signed_index_to_index = [&](int i) -> size_t {
// return abs(i) - 1;
//};
std::vector<int> selected;
for(size_t i=0; i<num_faces; i++)
{
auto should_keep = keep(Wr(i,0), Wr(i,1));
if (should_keep > 0)
{
selected.push_back(index_to_signed_index(i, true));
} else if (should_keep < 0)
{
selected.push_back(index_to_signed_index(i, false));
}
}
const size_t num_selected = selected.size();
DerivedFC kept_faces(num_selected, 3);
DerivedJ kept_face_indices(num_selected, 1);
for (size_t i=0; i<num_selected; i++)
{
size_t idx = abs(selected[i]) - 1;
if (selected[i] > 0)
{
kept_faces.row(i) = F.row(idx);
} else
{
kept_faces.row(i) = F.row(idx).reverse();
}
kept_face_indices(i, 0) = CJ[idx];
}
#ifdef MESH_BOOLEAN_TIMING
log_time("extract_output");
#endif
// Finally, remove duplicated faces and unreferenced vertices.
{
DerivedFC G;
DerivedJ JJ;
igl::resolve_duplicated_faces(kept_faces, G, JJ);
igl::slice(kept_face_indices, JJ, 1, J);
#ifdef DOUBLE_CHECK_EXACT_OUTPUT
{
// Sanity check on exact output.
igl::copyleft::cgal::RemeshSelfIntersectionsParam params;
params.detect_only = true;
params.first_only = true;
MatrixXES dummy_VV;
DerivedFC dummy_FF, dummy_IF;
Eigen::VectorXi dummy_J, dummy_IM;
igl::copyleft::cgal::SelfIntersectMesh<
Kernel,
MatrixXES, DerivedFC,
MatrixXES, DerivedFC,
DerivedFC,
Eigen::VectorXi,
Eigen::VectorXi
> checker(V, G, params,
dummy_VV, dummy_FF, dummy_IF, dummy_J, dummy_IM);
if (checker.count != 0)
{
throw "Self-intersection not fully resolved.";
}
}
#endif
MatrixX3S Vs(V.rows(), V.cols());
for (size_t i=0; i<(size_t)V.rows(); i++)
{
for (size_t j=0; j<(size_t)V.cols(); j++)
{
igl::copyleft::cgal::assign_scalar(V(i,j), Vs(i,j));
}
}
Eigen::VectorXi newIM;
igl::remove_unreferenced(Vs,G,VC,FC,newIM);
}
#ifdef MESH_BOOLEAN_TIMING
log_time("clean_up");
#endif
return valid;
}
ScopedTictoc::ScopedTictoc(const char* algorithmPart) :
_algorithmPart(algorithmPart)
{
tictoc(_algorithmPart.c_str());
}
ScopedTictoc::~ScopedTictoc()
{
tictoc(_algorithmPart.c_str());
}
IGL_INLINE void igl::copyleft::cgal::propagate_winding_numbers(
const Eigen::PlainObjectBase<DerivedV>& V,
const Eigen::PlainObjectBase<DerivedF>& F,
const Eigen::PlainObjectBase<DerivedL>& labels,
Eigen::PlainObjectBase<DerivedW>& W) {
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
const auto & tictoc = []()
{
static double t_start = igl::get_seconds();
double diff = igl::get_seconds()-t_start;
t_start += diff;
return diff;
};
tictoc();
#endif
const size_t num_faces = F.rows();
//typedef typename DerivedF::Scalar Index;
Eigen::MatrixXi E, uE;
Eigen::VectorXi EMAP;
std::vector<std::vector<size_t> > uE2E;
igl::unique_edge_map(F, E, uE, EMAP, uE2E);
if (!propagate_winding_numbers_helper::is_orientable(F, uE, uE2E)) {
std::cerr << "Input mesh is not orientable!" << std::endl;
}
Eigen::VectorXi P;
const size_t num_patches = igl::extract_manifold_patches(F, EMAP, uE2E, P);
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
std::cout << "extract manifold patches: " << tictoc() << std::endl;
#endif
DerivedW per_patch_cells;
const size_t num_cells =
igl::copyleft::cgal::extract_cells(
V, F, P, E, uE, uE2E, EMAP, per_patch_cells);
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
std::cout << "extract cells: " << tictoc() << std::endl;;
#endif
typedef std::tuple<size_t, bool, size_t> CellConnection;
std::vector<std::set<CellConnection> > cell_adjacency(num_cells);
for (size_t i=0; i<num_patches; i++) {
const int positive_cell = per_patch_cells(i,0);
const int negative_cell = per_patch_cells(i,1);
cell_adjacency[positive_cell].emplace(negative_cell, false, i);
cell_adjacency[negative_cell].emplace(positive_cell, true, i);
}
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
std::cout << "cell connection: " << tictoc() << std::endl;
#endif
auto save_cell = [&](const std::string& filename, size_t cell_id) {
std::vector<size_t> faces;
for (size_t i=0; i<num_patches; i++) {
if ((per_patch_cells.row(i).array() == cell_id).any()) {
for (size_t j=0; j<num_faces; j++) {
if ((size_t)P[j] == i) {
faces.push_back(j);
}
}
}
}
Eigen::MatrixXi cell_faces(faces.size(), 3);
for (size_t i=0; i<faces.size(); i++) {
cell_faces.row(i) = F.row(faces[i]);
}
Eigen::MatrixXd vertices(V.rows(), 3);
for (size_t i=0; i<(size_t)V.rows(); i++) {
assign_scalar(V(i,0), vertices(i,0));
assign_scalar(V(i,1), vertices(i,1));
assign_scalar(V(i,2), vertices(i,2));
}
writePLY(filename, vertices, cell_faces);
};
#ifndef NDEBUG
{
// Check for odd cycle.
Eigen::VectorXi cell_labels(num_cells);
cell_labels.setZero();
Eigen::VectorXi parents(num_cells);
parents.setConstant(-1);
auto trace_parents = [&](size_t idx) {
std::list<size_t> path;
path.push_back(idx);
while ((size_t)parents[path.back()] != path.back()) {
path.push_back(parents[path.back()]);
}
return path;
};
for (size_t i=0; i<num_cells; i++) {
if (cell_labels[i] == 0) {
cell_labels[i] = 1;
std::queue<size_t> Q;
Q.push(i);
parents[i] = i;
while (!Q.empty()) {
size_t curr_idx = Q.front();
Q.pop();
int curr_label = cell_labels[curr_idx];
for (const auto& neighbor : cell_adjacency[curr_idx]) {
if (cell_labels[std::get<0>(neighbor)] == 0) {
cell_labels[std::get<0>(neighbor)] = curr_label * -1;
Q.push(std::get<0>(neighbor));
parents[std::get<0>(neighbor)] = curr_idx;
} else {
if (cell_labels[std::get<0>(neighbor)] !=
curr_label * -1) {
std::cerr << "Odd cell cycle detected!" << std::endl;
auto path = trace_parents(curr_idx);
path.reverse();
auto path2 = trace_parents(std::get<0>(neighbor));
path.insert(path.end(),
path2.begin(), path2.end());
for (auto cell_id : path) {
std::cout << cell_id << " ";
std::stringstream filename;
filename << "cell_" << cell_id << ".ply";
save_cell(filename.str(), cell_id);
}
std::cout << std::endl;
}
// Do not fail when odd cycle is detected because the resulting
// integer winding number field, although inconsistent, may still
// be used if the problem region is local and embedded within a
// valid volume.
//assert(cell_labels[std::get<0>(neighbor)] == curr_label * -1);
}
}
}
}
}
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
std::cout << "check for odd cycle: " << tictoc() << std::endl;
#endif
}
#endif
size_t outer_facet;
bool flipped;
Eigen::VectorXi I;
I.setLinSpaced(num_faces, 0, num_faces-1);
igl::copyleft::cgal::outer_facet(V, F, I, outer_facet, flipped);
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
std::cout << "outer facet: " << tictoc() << std::endl;
#endif
const size_t outer_patch = P[outer_facet];
const size_t infinity_cell = per_patch_cells(outer_patch, flipped?1:0);
Eigen::VectorXi patch_labels(num_patches);
const int INVALID = std::numeric_limits<int>::max();
patch_labels.setConstant(INVALID);
for (size_t i=0; i<num_faces; i++) {
if (patch_labels[P[i]] == INVALID) {
patch_labels[P[i]] = labels[i];
} else {
assert(patch_labels[P[i]] == labels[i]);
}
}
assert((patch_labels.array() != INVALID).all());
const size_t num_labels = patch_labels.maxCoeff()+1;
Eigen::MatrixXi per_cell_W(num_cells, num_labels);
per_cell_W.setConstant(INVALID);
per_cell_W.row(infinity_cell).setZero();
std::queue<size_t> Q;
Q.push(infinity_cell);
while (!Q.empty()) {
size_t curr_cell = Q.front();
Q.pop();
for (const auto& neighbor : cell_adjacency[curr_cell]) {
size_t neighbor_cell, patch_idx;
bool direction;
std::tie(neighbor_cell, direction, patch_idx) = neighbor;
if ((per_cell_W.row(neighbor_cell).array() == INVALID).any()) {
per_cell_W.row(neighbor_cell) = per_cell_W.row(curr_cell);
for (size_t i=0; i<num_labels; i++) {
int inc = (patch_labels[patch_idx] == (int)i) ?
(direction ? -1:1) :0;
per_cell_W(neighbor_cell, i) =
per_cell_W(curr_cell, i) + inc;
}
Q.push(neighbor_cell);
} else {
#ifndef NDEBUG
// Checking for winding number consistency.
// This check would inevitably fail for meshes that contain open
// boundary or non-orientable. However, the inconsistent winding number
// field would still be useful in some cases such as when problem region
// is local and embedded within the volume. This, unfortunately, is the
// best we can do because the problem of computing integer winding
// number is ill-defined for open and non-orientable surfaces.
for (size_t i=0; i<num_labels; i++) {
if ((int)i == patch_labels[patch_idx]) {
int inc = direction ? -1:1;
//assert(per_cell_W(neighbor_cell, i) ==
// per_cell_W(curr_cell, i) + inc);
} else {
//assert(per_cell_W(neighbor_cell, i) ==
// per_cell_W(curr_cell, i));
}
}
#endif
}
}
}
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
std::cout << "propagate winding number: " << tictoc() << std::endl;
#endif
W.resize(num_faces, num_labels*2);
for (size_t i=0; i<num_faces; i++) {
const size_t patch = P[i];
const size_t positive_cell = per_patch_cells(patch, 0);
const size_t negative_cell = per_patch_cells(patch, 1);
for (size_t j=0; j<num_labels; j++) {
W(i,j*2 ) = per_cell_W(positive_cell, j);
W(i,j*2+1) = per_cell_W(negative_cell, j);
}
}
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
std::cout << "save result: " << tictoc() << std::endl;
#endif
}
