/// Filter inconsistent correspondences by using 3-view correspondences on view triplets void SfM_Data_Structure_Estimation_From_Known_Poses::filter( const SfM_Data & sfm_data, const Pair_Set & pairs, const std::shared_ptr<Regions_Provider> & regions_provider) { // Compute triplets // Triangulate triplet tracks // - keep valid one typedef std::vector< graph::Triplet > Triplets; const Triplets triplets = graph::tripletListing(pairs); C_Progress_display my_progress_bar( triplets.size(), std::cout, "Per triplet tracks validation (discard spurious correspondences):\n" ); #ifdef OPENMVG_USE_OPENMP #pragma omp parallel #endif // OPENMVG_USE_OPENMP for( Triplets::const_iterator it = triplets.begin(); it != triplets.end(); ++it) { #ifdef OPENMVG_USE_OPENMP #pragma omp single nowait #endif // OPENMVG_USE_OPENMP { #ifdef OPENMVG_USE_OPENMP #pragma omp critical #endif // OPENMVG_USE_OPENMP {++my_progress_bar;} const graph::Triplet & triplet = *it; const IndexT I = triplet.i, J = triplet.j , K = triplet.k; openMVG::tracks::STLMAPTracks map_tracksCommon; openMVG::tracks::TracksBuilder tracksBuilder; { PairWiseMatches map_matchesIJK; if(putatives_matches.find(std::make_pair(I,J)) != putatives_matches.end()) map_matchesIJK.insert(*putatives_matches.find(std::make_pair(I,J))); if(putatives_matches.find(std::make_pair(I,K)) != putatives_matches.end()) map_matchesIJK.insert(*putatives_matches.find(std::make_pair(I,K))); if(putatives_matches.find(std::make_pair(J,K)) != putatives_matches.end()) map_matchesIJK.insert(*putatives_matches.find(std::make_pair(J,K))); if (map_matchesIJK.size() >= 2) { tracksBuilder.Build(map_matchesIJK); tracksBuilder.Filter(3, false); tracksBuilder.ExportToSTL(map_tracksCommon); } // Triangulate the tracks for (tracks::STLMAPTracks::const_iterator iterTracks = map_tracksCommon.begin(); iterTracks != map_tracksCommon.end(); ++iterTracks) { { const tracks::submapTrack & subTrack = iterTracks->second; Triangulation trianObj; for (tracks::submapTrack::const_iterator iter = subTrack.begin(); iter != subTrack.end(); ++iter) { const size_t imaIndex = iter->first; const size_t featIndex = iter->second; const View * view = sfm_data.GetViews().at(imaIndex).get(); const IntrinsicBase * cam = sfm_data.GetIntrinsics().at(view->id_intrinsic).get(); const Pose3 pose = sfm_data.GetPoseOrDie(view); const Vec2 pt = regions_provider->regions_per_view.at(imaIndex)->GetRegionPosition(featIndex); trianObj.add(cam->get_projective_equivalent(pose), cam->get_ud_pixel(pt)); } const Vec3 Xs = trianObj.compute(); if (trianObj.minDepth() > 0 && trianObj.error()/(double)trianObj.size() < 4.0) // TODO: Add an angular check ? { #ifdef OPENMVG_USE_OPENMP #pragma omp critical #endif // OPENMVG_USE_OPENMP { openMVG::tracks::submapTrack::const_iterator iterI, iterJ, iterK; iterI = iterJ = iterK = subTrack.begin(); std::advance(iterJ,1); std::advance(iterK,2); triplets_matches[std::make_pair(I,J)].push_back(IndMatch(iterI->second, iterJ->second)); triplets_matches[std::make_pair(J,K)].push_back(IndMatch(iterJ->second, iterK->second)); triplets_matches[std::make_pair(I,K)].push_back(IndMatch(iterI->second, iterK->second)); } } } } } } } // Clear putatives matches since they are no longer required matching::PairWiseMatches().swap(putatives_matches); }
void SfM_Data_Structure_Computation_Blind::triangulate(SfM_Data & sfm_data) const { std::deque<IndexT> rejectedId; std::unique_ptr<C_Progress_display> my_progress_bar; if (_bConsoleVerbose) my_progress_bar.reset( new C_Progress_display( sfm_data.structure.size(), std::cout, "Blind triangulation progress:\n" )); #ifdef OPENMVG_USE_OPENMP #pragma omp parallel #endif for(Landmarks::iterator iterTracks = sfm_data.structure.begin(); iterTracks != sfm_data.structure.end(); ++iterTracks) { #ifdef OPENMVG_USE_OPENMP #pragma omp single nowait #endif { if (_bConsoleVerbose) { #ifdef OPENMVG_USE_OPENMP #pragma omp critical #endif ++(*my_progress_bar); } // Triangulate each landmark Triangulation trianObj; const Observations & obs = iterTracks->second.obs; for(Observations::const_iterator itObs = obs.begin(); itObs != obs.end(); ++itObs) { const View * view = sfm_data.views.at(itObs->first).get(); if (sfm_data.IsPoseAndIntrinsicDefined(view)) { const IntrinsicBase * cam = sfm_data.GetIntrinsics().at(view->id_intrinsic).get(); const Pose3 pose = sfm_data.GetPoseOrDie(view); trianObj.add( cam->get_projective_equivalent(pose), cam->get_ud_pixel(itObs->second.x)); } } if (trianObj.size() < 2) { #ifdef OPENMVG_USE_OPENMP #pragma omp critical #endif { rejectedId.push_front(iterTracks->first); } } else { // Compute the 3D point const Vec3 X = trianObj.compute(); if (trianObj.minDepth() > 0) // Keep the point only if it have a positive depth { iterTracks->second.X = X; } else { #ifdef OPENMVG_USE_OPENMP #pragma omp critical #endif { rejectedId.push_front(iterTracks->first); } } } } } // Erase the unsuccessful triangulated tracks for (auto& it : rejectedId) { sfm_data.structure.erase(it); } }
void MainWindow::registerProject() { if (m_doc._sfm_data.control_points.size() < 3) { QMessageBox msgBox; msgBox.setText("At least 3 control points are required."); msgBox.exec(); return; } // Assert that control points can be triangulated for (Landmarks::const_iterator iterL = m_doc._sfm_data.control_points.begin(); iterL != m_doc._sfm_data.control_points.end(); ++iterL) { if (iterL->second.obs.size() < 2) { QMessageBox msgBox; msgBox.setText("Each control point must be defined in at least 2 pictures."); msgBox.exec(); return; } } //--- // registration (coarse): // - compute the 3D points corresponding to the control point observation for the SfM scene // - compute a coarse registration between the controls points & the triangulated point // - transform the scene according the found transformation //--- std::map<IndexT, Vec3> vec_control_points, vec_triangulated; std::map<IndexT, double> vec_triangulation_errors; for (Landmarks::iterator iterCP = m_doc._sfm_data.control_points.begin(); iterCP != m_doc._sfm_data.control_points.end(); ++iterCP) { Landmark & landmark = iterCP->second; //Triangulate the point: Triangulation trianObj; const Observations & obs = landmark.obs; for(Observations::const_iterator itObs = obs.begin(); itObs != obs.end(); ++itObs) { const View * view = m_doc._sfm_data.views.at(itObs->first).get(); if (!m_doc._sfm_data.IsPoseAndIntrinsicDefined(view)) continue; const openMVG::cameras::IntrinsicBase * cam = m_doc._sfm_data.GetIntrinsics().at(view->id_intrinsic).get(); const openMVG::geometry::Pose3 pose = m_doc._sfm_data.GetPoseOrDie(view); trianObj.add( cam->get_projective_equivalent(pose), cam->get_ud_pixel(itObs->second.x)); } // Compute the 3D point const Vec3 X = trianObj.compute(); if (trianObj.minDepth() > 0) // Keep the point only if it have a positive depth { vec_triangulated[iterCP->first] = X; vec_control_points[iterCP->first] = landmark.X; vec_triangulation_errors[iterCP->first] = trianObj.error()/(double)trianObj.size(); } else { std::cout << "Invalid triangulation" << std::endl; return; } } if (vec_control_points.size() < 3) { QMessageBox msgBox; msgBox.setText("Insufficient number of triangulated control points."); msgBox.exec(); return; } // compute the similarity { // data conversion to appropriate container Mat x1(3, vec_control_points.size()), x2(3, vec_control_points.size()); for (int i=0; i < vec_control_points.size(); ++i) { x1.col(i) = vec_triangulated[i]; x2.col(i) = vec_control_points[i]; } std::cout << "Control points observation triangulations:\n" << x1 << std::endl << std::endl << "Control points coords:\n" << x2 << std::endl << std::endl; Vec3 t; Mat3 R; double S; if (openMVG::geometry::FindRTS(x1, x2, &S, &t, &R)) { openMVG::geometry::Refine_RTS(x1,x2,&S,&t,&R); std::cout << "Found transform:\n" << " scale: " << S << "\n" << " rotation:\n" << R << "\n" << " translation: "<< t.transpose() << std::endl; // Encode the transformation as a 3D Similarity transformation matrix // S * R * X + t const openMVG::geometry::Similarity3 sim(geometry::Pose3(R, -R.transpose() * t/S), S); //-- // Apply the found transformation //-- // Transform the landmark positions for (Landmarks::iterator iterL = m_doc._sfm_data.structure.begin(); iterL != m_doc._sfm_data.structure.end(); ++iterL) { iterL->second.X = sim(iterL->second.X); } // Transform the camera positions for (Poses::iterator iterP = m_doc._sfm_data.poses.begin(); iterP != m_doc._sfm_data.poses.end(); ++iterP) { geometry::Pose3 & pose = iterP->second; pose = sim(pose); } // Display some statistics: std::stringstream os; for (Landmarks::const_iterator iterL = m_doc._sfm_data.control_points.begin(); iterL != m_doc._sfm_data.control_points.end(); ++iterL) { const IndexT CPIndex = iterL->first; // If the control point has not been used, continue... if (vec_triangulation_errors.find(CPIndex) == vec_triangulation_errors.end()) continue; os << "CP index: " << CPIndex << "\n" << "CP triangulation error: " << vec_triangulation_errors[CPIndex] << " pixel(s)\n" << "CP registration error: " << (sim(vec_triangulated[CPIndex]) - vec_control_points[CPIndex]).norm() << " user unit(s)"<< "\n\n"; } std::cout << os.str(); QMessageBox msgBox; msgBox.setText(QString::fromStdString(string_pattern_replace(os.str(), "\n", "<br>"))); msgBox.exec(); } else { QMessageBox msgBox; msgBox.setText("Registration failed. Please check your Control Points coordinates."); msgBox.exec(); } } }
void FlipGraph::compute(int n) { graph_.clear(); int count = 0; std::queue<std::pair<Triangulation *, int> > queue; std::map<Code, int> indices; // build canonical triangulation on n vertices Triangulation *triangulation = new Triangulation(n); Code *code = new Code(*triangulation); // add canonical triangulation int index = count++; indices[*code] = 0; graph_.push_back(std::vector<int>()); codes_.push_back(*code); queue.push(std::make_pair(triangulation, index)); delete code; // explore flip graph_ using a bfs while (!queue.empty()) { // get current triangulation triangulation = queue.front().first; index = queue.front().second; queue.pop(); // loop through neighboring triangulations int m = triangulation->size(); for (int i = 0; i < m; ++i) { Halfedge *halfedge = triangulation->halfedge(i); if (triangulation->is_representative(halfedge) && triangulation->is_flippable(halfedge)) { triangulation->flip(halfedge); Code triangulation_code(*triangulation); int other_index = 0; if (indices.count(triangulation_code) == 0) { // add newly discovered triangulation other_index = count++; indices[triangulation_code] = other_index; graph_.push_back(std::vector<int>()); codes_.push_back(triangulation_code); queue.push(std::make_pair(new Triangulation(*triangulation), other_index)); } else { // get index of triangulation other_index = indices[triangulation_code]; } // add edge if not already present if (std::count(graph_[index].begin(), graph_[index].end(), other_index) == 0 && index != other_index) { graph_[index].push_back(other_index); } // note: after two flips the halfedge and its twin are swapped // all other edges stay in place. this is crucial since // we loop over all edges. triangulation->flip(halfedge); } } delete triangulation; } }