/// 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;
}
