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 }