/// Camera pair epipole (Projection of camera center 2 in the image plane 1) static Vec3 epipole_from_P(const Mat34& P1, const Pose3& P2) { const Vec3 c = P2.center(); Vec4 center; center << c(0), c(1), c(2), 1.0; return P1*center; }
// Init a frustum for each valid views of the SfM scene void Frustum_Filter::initFrustum ( const SfM_Data & sfm_data ) { for (NearFarPlanesT::const_iterator it = z_near_z_far_perView.begin(); it != z_near_z_far_perView.end(); ++it) { const View * view = sfm_data.GetViews().at(it->first).get(); if (!sfm_data.IsPoseAndIntrinsicDefined(view)) continue; Intrinsics::const_iterator iterIntrinsic = sfm_data.GetIntrinsics().find(view->id_intrinsic); if (!isPinhole(iterIntrinsic->second->getType())) continue; const Pose3 pose = sfm_data.GetPoseOrDie(view); const Pinhole_Intrinsic * cam = dynamic_cast<const Pinhole_Intrinsic*>(iterIntrinsic->second.get()); if (!cam) continue; if (!_bTruncated) // use infinite frustum { const Frustum f( cam->w(), cam->h(), cam->K(), pose.rotation(), pose.center()); frustum_perView[view->id_view] = f; } else // use truncated frustum with defined Near and Far planes { const Frustum f(cam->w(), cam->h(), cam->K(), pose.rotation(), pose.center(), it->second.first, it->second.second); frustum_perView[view->id_view] = f; } } }
Pose3 operator () (const Pose3 & pose) const { return Pose3( pose.rotation() * _pose.rotation().transpose(), this->operator()(pose.center())); }
int main() { const std::string sInputDir = stlplus::folder_up(string(THIS_SOURCE_DIR)) + "/imageData/SceauxCastle/"; const string jpg_filenameL = sInputDir + "100_7101.jpg"; const string jpg_filenameR = sInputDir + "100_7102.jpg"; Image<unsigned char> imageL, imageR; ReadImage(jpg_filenameL.c_str(), &imageL); ReadImage(jpg_filenameR.c_str(), &imageR); //-- // Detect regions thanks to an image_describer //-- using namespace openMVG::features; std::unique_ptr<Image_describer> image_describer(new SIFT_Image_describer); std::map<IndexT, std::unique_ptr<features::Regions> > regions_perImage; image_describer->Describe(imageL, regions_perImage[0]); image_describer->Describe(imageR, regions_perImage[1]); const SIFT_Regions* regionsL = dynamic_cast<SIFT_Regions*>(regions_perImage.at(0).get()); const SIFT_Regions* regionsR = dynamic_cast<SIFT_Regions*>(regions_perImage.at(1).get()); const PointFeatures featsL = regions_perImage.at(0)->GetRegionsPositions(), featsR = regions_perImage.at(1)->GetRegionsPositions(); // Show both images side by side { Image<unsigned char> concat; ConcatH(imageL, imageR, concat); string out_filename = "01_concat.jpg"; WriteImage(out_filename.c_str(), concat); } //- Draw features on the two image (side by side) { Image<unsigned char> concat; ConcatH(imageL, imageR, concat); //-- Draw features : for (size_t i=0; i < featsL.size(); ++i ) { const SIOPointFeature point = regionsL->Features()[i]; DrawCircle(point.x(), point.y(), point.scale(), 255, &concat); } for (size_t i=0; i < featsR.size(); ++i ) { const SIOPointFeature point = regionsR->Features()[i]; DrawCircle(point.x()+imageL.Width(), point.y(), point.scale(), 255, &concat); } string out_filename = "02_features.jpg"; WriteImage(out_filename.c_str(), concat); } std::vector<IndMatch> vec_PutativeMatches; //-- Perform matching -> find Nearest neighbor, filtered with Distance ratio { // Define a matcher and a metric to find corresponding points typedef SIFT_Regions::DescriptorT DescriptorT; typedef L2_Vectorized<DescriptorT::bin_type> Metric; typedef ArrayMatcherBruteForce<DescriptorT::bin_type, Metric> MatcherT; // Distance ratio squared due to squared metric getPutativesMatches<DescriptorT, MatcherT>( ((SIFT_Regions*)regions_perImage.at(0).get())->Descriptors(), ((SIFT_Regions*)regions_perImage.at(1).get())->Descriptors(), Square(0.8), vec_PutativeMatches); IndMatchDecorator<float> matchDeduplicator( vec_PutativeMatches, featsL, featsR); matchDeduplicator.getDeduplicated(vec_PutativeMatches); std::cout << regions_perImage.at(0)->RegionCount() << " #Features on image A" << std::endl << regions_perImage.at(1)->RegionCount() << " #Features on image B" << std::endl << vec_PutativeMatches.size() << " #matches with Distance Ratio filter" << std::endl; // Draw correspondences after Nearest Neighbor ratio filter svgDrawer svgStream( imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height())); svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height()); svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width()); for (size_t i = 0; i < vec_PutativeMatches.size(); ++i) { //Get back linked feature, draw a circle and link them by a line const SIOPointFeature L = regionsL->Features()[vec_PutativeMatches[i]._i]; const SIOPointFeature R = regionsR->Features()[vec_PutativeMatches[i]._j]; svgStream.drawLine(L.x(), L.y(), R.x()+imageL.Width(), R.y(), svgStyle().stroke("green", 2.0)); svgStream.drawCircle(L.x(), L.y(), L.scale(), svgStyle().stroke("yellow", 2.0)); svgStream.drawCircle(R.x()+imageL.Width(), R.y(), R.scale(),svgStyle().stroke("yellow", 2.0)); } const std::string out_filename = "03_siftMatches.svg"; std::ofstream svgFile( out_filename.c_str() ); svgFile << svgStream.closeSvgFile().str(); svgFile.close(); } // Essential geometry filtering of putative matches { Mat3 K; //read K from file if (!readIntrinsic(stlplus::create_filespec(sInputDir,"K","txt"), K)) { std::cerr << "Cannot read intrinsic parameters." << std::endl; return EXIT_FAILURE; } //A. prepare the corresponding putatives points Mat xL(2, vec_PutativeMatches.size()); Mat xR(2, vec_PutativeMatches.size()); for (size_t k = 0; k < vec_PutativeMatches.size(); ++k) { const PointFeature & imaL = featsL[vec_PutativeMatches[k]._i]; const PointFeature & imaR = featsR[vec_PutativeMatches[k]._j]; xL.col(k) = imaL.coords().cast<double>(); xR.col(k) = imaR.coords().cast<double>(); } //B. Compute the relative pose thanks to a essential matrix estimation std::pair<size_t, size_t> size_imaL(imageL.Width(), imageL.Height()); std::pair<size_t, size_t> size_imaR(imageR.Width(), imageR.Height()); sfm::RelativePose_Info relativePose_info; if (!sfm::robustRelativePose(K, K, xL, xR, relativePose_info, size_imaL, size_imaR, 256)) { std::cerr << " /!\\ Robust relative pose estimation failure." << std::endl; return EXIT_FAILURE; } std::cout << "\nFound an Essential matrix:\n" << "\tprecision: " << relativePose_info.found_residual_precision << " pixels\n" << "\t#inliers: " << relativePose_info.vec_inliers.size() << "\n" << "\t#matches: " << vec_PutativeMatches.size() << std::endl; // Show Essential validated point svgDrawer svgStream( imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height())); svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height()); svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width()); for (size_t i = 0; i < relativePose_info.vec_inliers.size(); ++i) { const SIOPointFeature & LL = regionsL->Features()[vec_PutativeMatches[relativePose_info.vec_inliers[i]]._i]; const SIOPointFeature & RR = regionsR->Features()[vec_PutativeMatches[relativePose_info.vec_inliers[i]]._j]; const Vec2f L = LL.coords(); const Vec2f R = RR.coords(); svgStream.drawLine(L.x(), L.y(), R.x()+imageL.Width(), R.y(), svgStyle().stroke("green", 2.0)); svgStream.drawCircle(L.x(), L.y(), LL.scale(), svgStyle().stroke("yellow", 2.0)); svgStream.drawCircle(R.x()+imageL.Width(), R.y(), RR.scale(),svgStyle().stroke("yellow", 2.0)); } const std::string out_filename = "04_ACRansacEssential.svg"; std::ofstream svgFile( out_filename.c_str() ); svgFile << svgStream.closeSvgFile().str(); svgFile.close(); //C. Triangulate and export inliers as a PLY scene std::vector<Vec3> vec_3DPoints; // Setup camera intrinsic and poses Pinhole_Intrinsic intrinsic0(imageL.Width(), imageL.Height(), K(0, 0), K(0, 2), K(1, 2)); Pinhole_Intrinsic intrinsic1(imageR.Width(), imageR.Height(), K(0, 0), K(0, 2), K(1, 2)); const Pose3 pose0 = Pose3(Mat3::Identity(), Vec3::Zero()); const Pose3 pose1 = relativePose_info.relativePose; // Init structure by inlier triangulation const Mat34 P1 = intrinsic0.get_projective_equivalent(pose0); const Mat34 P2 = intrinsic1.get_projective_equivalent(pose1); std::vector<double> vec_residuals; vec_residuals.reserve(relativePose_info.vec_inliers.size() * 4); for (size_t i = 0; i < relativePose_info.vec_inliers.size(); ++i) { const SIOPointFeature & LL = regionsL->Features()[vec_PutativeMatches[relativePose_info.vec_inliers[i]]._i]; const SIOPointFeature & RR = regionsR->Features()[vec_PutativeMatches[relativePose_info.vec_inliers[i]]._j]; // Point triangulation Vec3 X; TriangulateDLT(P1, LL.coords().cast<double>(), P2, RR.coords().cast<double>(), &X); // Reject point that is behind the camera if (pose0.depth(X) < 0 && pose1.depth(X) < 0) continue; const Vec2 residual0 = intrinsic0.residual(pose0, X, LL.coords().cast<double>()); const Vec2 residual1 = intrinsic1.residual(pose1, X, RR.coords().cast<double>()); vec_residuals.push_back(fabs(residual0(0))); vec_residuals.push_back(fabs(residual0(1))); vec_residuals.push_back(fabs(residual1(0))); vec_residuals.push_back(fabs(residual1(1))); } // Display some statistics of reprojection errors float dMin, dMax, dMean, dMedian; minMaxMeanMedian<float>(vec_residuals.begin(), vec_residuals.end(), dMin, dMax, dMean, dMedian); std::cout << std::endl << "Triangulation residuals statistics:" << "\n" << "\t-- Residual min:\t" << dMin << "\n" << "\t-- Residual median:\t" << dMedian << "\n" << "\t-- Residual max:\t " << dMax << "\n" << "\t-- Residual mean:\t " << dMean << std::endl; // Export as PLY (camera pos + 3Dpoints) std::vector<Vec3> vec_camPos; vec_camPos.push_back( pose0.center() ); vec_camPos.push_back( pose1.center() ); exportToPly(vec_3DPoints, vec_camPos, "EssentialGeometry.ply"); } return EXIT_SUCCESS; }