static double Error(const TrifocalTensorModel & t, const Vec2 & pt1, const Vec2 & pt2, const Vec2 & pt3) { // Triangulate and return the reprojection error Triangulation triangulationObj; triangulationObj.add(t.P1, pt1); triangulationObj.add(t.P2, pt2); triangulationObj.add(t.P3, pt3); const Vec3 X = triangulationObj.compute(); //- Return max error as a test double pt1ReProj = (Project(t.P1, X) - pt1).squaredNorm(); double pt2ReProj = (Project(t.P2, X) - pt2).squaredNorm(); double pt3ReProj = (Project(t.P3, X) - pt3).squaredNorm(); return std::max(pt1ReProj, std::max(pt2ReProj,pt3ReProj)); }
/// Triangulate a given track from a selection of observations Vec3 track_sample_triangulation( const SfM_Data & sfm_data, const Observations & obs, const std::set<IndexT> & samples) const { Triangulation trianObj; for (auto& it : samples) { const IndexT & idx = it; Observations::const_iterator itObs = obs.begin(); std::advance(itObs, idx); const View * view = sfm_data.views.at(itObs->first).get(); const IntrinsicBase * cam = sfm_data.getIntrinsics().at(view->id_intrinsic).get(); const Pose3 & pose = sfm_data.poses.at(view->id_pose); trianObj.add( cam->get_projective_equivalent(pose), cam->get_ud_pixel(itObs->second.x)); } return trianObj.compute(); }
/// 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); 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() < 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 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(); } } }
virtual void 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(); const IntrinsicBase * cam = sfm_data.getIntrinsics().at(view->id_intrinsic).get(); const Pose3 & pose = sfm_data.poses.at(view->id_pose); 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 { 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); } }