diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index f5c581b..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright (c) 2010 libmv authors. -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to -# deal in the Software without restriction, including without limitation the -# rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -# sell copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -# IN THE SOFTWARE. - -CMAKE_MINIMUM_REQUIRED(VERSION 2.2) - -ADD_SUBDIRECTORY(src) diff --git a/LICENSE b/LICENSE index 6bb9ead..2b1f8fe 100644 --- a/LICENSE +++ b/LICENSE @@ -1,4 +1,4 @@ -Copyright (c) 2007, 2008 libmv authors. +Copyright (c) 2007-2011 libmv authors. Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to diff --git a/README b/README index 65f08a1..7983fa0 100644 --- a/README +++ b/README @@ -52,9 +52,9 @@ Instead do this: $ ls AUTHORS contrib doc LICENSE Makefile README src - $ mkdir alternate_build - $ cd alternate_build - $ cmake ../ # Your options here + $ mkdir build + $ cd build + $ cmake ../src # Your options here $ make $ make test $ ... diff --git a/src/libmv/correspondence/ArrayMatcher_Kdtree_Flann.h b/src/libmv/correspondence/ArrayMatcher_Kdtree_Flann.h index f12e6f7..90e0d29 100644 --- a/src/libmv/correspondence/ArrayMatcher_Kdtree_Flann.h +++ b/src/libmv/correspondence/ArrayMatcher_Kdtree_Flann.h @@ -54,7 +54,7 @@ class ArrayMatcher_Kdtree_Flann : public ArrayMatcher bool build( const Scalar * dataset, int nbRows, int dimension) { _p.log_destination = NULL; - _p.log_level = LOG_INFO; + _p.log_level = LOG_WARN;//LOG_INFO; // Force KDTREE matching _p.algorithm = KDTREE; diff --git a/src/libmv/correspondence/robust_tracker.cc b/src/libmv/correspondence/robust_tracker.cc index 020b8f7..8fbc846 100755 --- a/src/libmv/correspondence/robust_tracker.cc +++ b/src/libmv/correspondence/robust_tracker.cc @@ -142,8 +142,8 @@ bool RobustTracker::Track(const Image &image, rms_threshold_inlier_, &F, &inliers); - LOG(INFO) << "#inliers = "<< inliers.size() << std::endl; - LOG(INFO) << "#outliers = "<< tracks.size()-inliers.size() << std::endl; + VLOG(2) << "#inliers = "<< inliers.size() << std::endl; + VLOG(2) << "#outliers = "<< tracks.size()-inliers.size() << std::endl; // We remove correspondences that are not inliers size_t max_num_track = 1 + std::max(new_features_graph->matches_.GetMaxTrackID(), diff --git a/src/libmv/image/image_converter.h b/src/libmv/image/image_converter.h index 2c67fe5..f955e0c 100644 --- a/src/libmv/image/image_converter.h +++ b/src/libmv/image/image_converter.h @@ -31,7 +31,7 @@ template // var_R * 0.2126 + var_G * 0.7152 + var_B * 0.0722 inline T RGB2GRAY(const T r,const T g, const T b) { - return r * 0.2126 + g * 0.7152 + b * 0.0722; + return static_cast(r * 0.2126 + g * 0.7152 + b * 0.0722); } /* // Specialization for the uchar type. (that do not want to work...) diff --git a/src/libmv/multiview/bundle.cc b/src/libmv/multiview/bundle.cc index eab4fbb..6c20362 100755 --- a/src/libmv/multiview/bundle.cc +++ b/src/libmv/multiview/bundle.cc @@ -49,7 +49,7 @@ showErrorStatistics(double const f0, Vector2d p = cams[i].projectPoint(distortion, Xs[j]); meanReprojectionError += Square(norm_L2(f0 * (p - measurements[k]))); } - VLOG(2) << "mean reprojection error (in pixels): " + VLOG(3) << "mean reprojection error (in pixels): " << sqrt(meanReprojectionError/K) << std::endl; return sqrt(meanReprojectionError/K); } @@ -108,7 +108,7 @@ void EuclideanBAFull(const vector &x, distortion.p2 = 0; double const f0 = KMat[0][0]; - VLOG(2) << "intrinsic before bundle = "; displayMatrix(KMat); + VLOG(3) << "intrinsic before bundle = "; displayMatrix(KMat); Matrix3x3d Knorm = KMat; // Normalize the intrinsic to have unit focal length. scaleMatrixIP(1.0/f0, Knorm); @@ -170,7 +170,7 @@ void EuclideanBAFull(const vector &x, double const inlierThreshold = 2.0 / f0; Matrix3x3d K0 = cams[0].getIntrinsic(); - VLOG(2) << "K0 = "; displayMatrix(K0); + VLOG(3) << "K0 = "; displayMatrix(K0); CommonInternalsMetricBundleOptimizer opt(mode, inlierThreshold, K0, distortion, @@ -179,9 +179,9 @@ void EuclideanBAFull(const vector &x, correspondingPoint); opt.maxIterations = 50; opt.minimize(); - VLOG(2) << "optimizer status = " << opt.status << endl; - VLOG(2) << "refined K = "; displayMatrix(K0); - VLOG(2) << "distortion = " << distortion.k1 << " " << distortion.k2 << " " + VLOG(3) << "optimizer status = " << opt.status << endl; + VLOG(3) << "refined K = "; displayMatrix(K0); + VLOG(3) << "distortion = " << distortion.k1 << " " << distortion.k2 << " " << distortion.p1 << " " << distortion.p2 << endl; for (int i = 0; i < num_camsN; ++i) cams[i].setIntrinsic(K0); @@ -189,7 +189,7 @@ void EuclideanBAFull(const vector &x, Matrix3x3d Knew = K0; scaleMatrixIP(f0, Knew); Knew[2][2] = 1.0; - VLOG(2) << "Knew = "; displayMatrix(Knew); + VLOG(3) << "Knew = "; displayMatrix(Knew); showErrorStatistics(f0, distortion, cams, Xs, measurements, correspondingView, correspondingPoint); @@ -268,7 +268,7 @@ double EuclideanBA(const vector &x, distortion.p2 = 0; double f0 = KMat[0][0]; - VLOG(2) << "intrinsic before bundle = "; displayMatrix(KMat); + VLOG(3) << "intrinsic before bundle = "; displayMatrix(KMat); Matrix3x3d Knorm = KMat; // Normalize the intrinsic to have unit focal length. scaleMatrixIP(1.0/f0, Knorm); @@ -344,7 +344,7 @@ double EuclideanBA(const vector &x, double const inlierThreshold = 2.0 / f0; Matrix3x3d K0 = cams[0].getIntrinsic(); - VLOG(2) << "K0 = "; displayMatrix(K0); + VLOG(3) << "K0 = "; displayMatrix(K0); CommonInternalsMetricBundleOptimizer opt(mode, inlierThreshold, K0, distortion, @@ -353,9 +353,9 @@ double EuclideanBA(const vector &x, correspondingPoint); opt.maxIterations = 50; opt.minimize(); - VLOG(2) << "optimizer status = " << opt.status << std::endl; - VLOG(2) << "refined K = "; displayMatrix(K0); - VLOG(2) << "distortion = " << distortion.k1 << " " << distortion.k2 << " " + VLOG(3) << "optimizer status = " << opt.status << std::endl; + VLOG(3) << "refined K = "; displayMatrix(K0); + VLOG(3) << "distortion = " << distortion.k1 << " " << distortion.k2 << " " << distortion.p1 << " " << distortion.p2 << std::endl; for (int i = 0; i < num_camsN; ++i) cams[i].setIntrinsic(K0); @@ -363,7 +363,7 @@ double EuclideanBA(const vector &x, Matrix3x3d Knew = K0; scaleMatrixIP(f0, Knew); Knew[2][2] = 1.0; - VLOG(2) << "Knew = "; displayMatrix(Knew); + VLOG(3) << "Knew = "; displayMatrix(Knew); double rms = showErrorStatistics(f0, distortion, cams, Xs, measurements,correspondingView, diff --git a/src/libmv/multiview/euclidean_resection.cc b/src/libmv/multiview/euclidean_resection.cc index 7d65632..37e2d12 100644 --- a/src/libmv/multiview/euclidean_resection.cc +++ b/src/libmv/multiview/euclidean_resection.cc @@ -442,14 +442,14 @@ void EuclideanResectionEPnP(const Mat2X &x_camera, const Mat3X &X_world, ui = x_camera(0, c); vi = x_camera(1, c); M.block(2*c, 0, 2, 12) << a0, 0, - a0*(-ui), a1, 0, - a1*(-ui), a2, 0, - a2*(-ui), a3, 0, - a3*(-ui), 0, - a0, a0*(-vi), 0, - a1, a1*(-vi), 0, - a2, a2*(-vi), 0, - a3, a3*(-vi); + a0*(-ui), a1, 0, + a1*(-ui), a2, 0, + a2*(-ui), a3, 0, + a3*(-ui), 0, + a0, a0*(-vi), 0, + a1, a1*(-vi), 0, + a2, a2*(-vi), 0, + a3, a3*(-vi); } Eigen::SVD MtMsvd = (M.transpose()*M).svd(); diff --git a/src/libmv/multiview/robust_estimation.h b/src/libmv/multiview/robust_estimation.h index 10b3698..250dd22 100644 --- a/src/libmv/multiview/robust_estimation.h +++ b/src/libmv/multiview/robust_estimation.h @@ -112,13 +112,13 @@ typename Kernel::Model Estimate(const Kernel &kernel, vector models; kernel.Fit(sample, &models); - VLOG(2) << "Fitted subset; found " << models.size() << " model(s)."; + VLOG(4) << "Fitted subset; found " << models.size() << " model(s)."; // Compute costs for each fit. for (int i = 0; i < models.size(); ++i) { vector inliers; double cost = scorer.Score(kernel, models[i], all_samples, &inliers); - VLOG(3) << "Fit cost: " << cost + VLOG(5) << "Fit cost: " << cost << ", number of inliers: " << inliers.size(); if (cost < best_cost) { @@ -129,7 +129,7 @@ typename Kernel::Model Estimate(const Kernel &kernel, if (best_inliers) { best_inliers->swap(inliers); } - VLOG(2) << "New best cost: " << best_cost << " with " + VLOG(4) << "New best cost: " << best_cost << " with " << best_num_inliers << " inlying of " << total_samples << " total samples."; } @@ -138,7 +138,7 @@ typename Kernel::Model Estimate(const Kernel &kernel, outliers_probability, best_inlier_ratio); - VLOG(2) << "Max iterations needed given best inlier ratio: " + VLOG(5) << "Max iterations needed given best inlier ratio: " << max_iterations << "; best inlier ratio: " << best_inlier_ratio; } } diff --git a/src/libmv/reconstruction/CMakeLists.txt b/src/libmv/reconstruction/CMakeLists.txt index 1321745..70708ad 100644 --- a/src/libmv/reconstruction/CMakeLists.txt +++ b/src/libmv/reconstruction/CMakeLists.txt @@ -2,7 +2,8 @@ ADD_LIBRARY(reconstruction euclidean_reconstruction.cc export_blender.cc export_ply.cc - image_order_selection.cc + image_selection.cc + keyframe_selection.cc mapping.cc optimization.cc projective_reconstruction.cc @@ -10,11 +11,11 @@ ADD_LIBRARY(reconstruction tools.cc ) -TARGET_LINK_LIBRARIES(reconstruction camera multiview numeric V3D colamd ldl) +TARGET_LINK_LIBRARIES(reconstruction camera multiview numeric V3D colamd ldl glog) LIBMV_INSTALL_LIB(reconstruction) MACRO (RECONSTRUCTION_TEST NAME) - LIBMV_TEST(${NAME} "reconstruction;multiview_test_data;camera;correspondence;multiview;numeric") + LIBMV_TEST(${NAME} "reconstruction;multiview_test_data;camera;correspondence;multiview;numeric;glog") ENDMACRO (RECONSTRUCTION_TEST) -RECONSTRUCTION_TEST(reconstruction) \ No newline at end of file +RECONSTRUCTION_TEST(euclidean_reconstruction) \ No newline at end of file diff --git a/src/libmv/reconstruction/euclidean_reconstruction.cc b/src/libmv/reconstruction/euclidean_reconstruction.cc index 4da9cb1..856841a 100644 --- a/src/libmv/reconstruction/euclidean_reconstruction.cc +++ b/src/libmv/reconstruction/euclidean_reconstruction.cc @@ -1,182 +1,424 @@ -// Copyright (c) 2010 libmv authors. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include "libmv/base/vector_utils.h" -#include "libmv/camera/pinhole_camera.h" -#include "libmv/multiview/five_point.h" -#include "libmv/multiview/fundamental.h" -#include "libmv/multiview/robust_euclidean_resection.h" -#include "libmv/multiview/robust_fundamental.h" -#include "libmv/reconstruction/euclidean_reconstruction.h" -#include "libmv/reconstruction/mapping.h" -#include "libmv/reconstruction/tools.h" - -namespace libmv { - -bool ReconstructFromTwoCalibratedViews(const Matches &matches, - CameraID image_id1, - CameraID image_id2, - const Mat3 &K1, - const Mat3 &K2, - Matches *matches_inliers, - Reconstruction *reconstruction) { - double epipolar_threshold = 1;// in pixels - vector xs(2); - vector tracks; - vector images; - images.push_back(image_id1); - images.push_back(image_id2); - PointMatchMatrices(matches, images, &tracks, &xs); - // TODO(julien) Also remove structures that are on the same location - if (xs[0].cols() < 7) { - LOG(ERROR) << "Error: there are not enough common matches (" - << xs[0].cols()<< "<7)."; - return false; - } - - Mat &x0 = xs[0]; - Mat &x1 = xs[1]; - vector feature_inliers; - Mat3 F; - // Computes fundamental matrix - // TODO(julien) For the calibrated case, we can squeeze the fundamental using - // directly the 5 points algorithm - FundamentalFromCorrespondences7PointRobust(x0,x1, - epipolar_threshold, - &F, &feature_inliers, - 1e-3); - // Only inliers are selected in order to estimation the relative motion - Mat2X v0(2, feature_inliers.size()); - Mat2X v1(2, feature_inliers.size()); - size_t index_inlier = 0; - for (size_t c = 0; c < feature_inliers.size(); ++c) { - index_inlier = feature_inliers[c]; - v0.col(c) = x0.col(index_inlier); - v1.col(c) = x1.col(index_inlier); - } - Mat3 E; - // Computes essential matrix - EssentialFromFundamental(F, K1, K2, &E); - - Mat3 dR; - Vec3 dt; - // Recover motion between the two images - bool is_motion_ok = MotionFromEssentialAndCorrespondence(E, K1, v0.col(0), - K2, v1.col(0), - &dR, &dt); - if (!is_motion_ok) { - LOG(ERROR) << "Error: the motion cannot be estimated."; - return false; - } - - Mat3 R; - Vec3 t; - PinholeCamera * pcamera = NULL; - pcamera = dynamic_cast( - reconstruction->GetCamera(image_id1)); - // If the first image has no associated camera, we choose the center of the - // coordinate frame - if (!pcamera) { - R.setIdentity(); - t.setZero(); - pcamera = new PinholeCamera(K1, R, t); - reconstruction->InsertCamera(image_id1, pcamera); - VLOG(1) << "Add Camera [" - << image_id1 <<"]"<< std::endl <<"R=" - << R << std::endl <<"t= " - << t.transpose() - << std::endl; - } - // Recover the asolute pose: R2 = R1 * dR, t2 = R1 * dt + t1 - R = pcamera->orientation_matrix() * dR; - t = pcamera->orientation_matrix() * dt + pcamera->position(); - - // Creates and adds the second camera - pcamera = new PinholeCamera(K2, R, t); - reconstruction->InsertCamera(image_id2, pcamera); - VLOG(1) << "Add Camera [" - << image_id2 <<"]"<< std::endl <<"R=" - << R << std::endl <<"t= " - << t.transpose() - << std::endl; - - //Adds only inliers matches into - const Feature * feature = NULL; - for (size_t s = 0; s < feature_inliers.size(); ++s) { - feature = matches.Get(image_id1, tracks[feature_inliers[s]]); - matches_inliers->Insert(image_id1, tracks[feature_inliers[s]], feature); - feature = matches.Get(image_id2, tracks[feature_inliers[s]]); - matches_inliers->Insert(image_id2, tracks[feature_inliers[s]], feature); - } - VLOG(1) << "Inliers added: " << feature_inliers.size() << std::endl; - return true; -} - -bool CalibratedCameraResection(const Matches &matches, - CameraID image_id, - const Mat3 &K, - Matches *matches_inliers, - Reconstruction *reconstruction) { - double rms_inliers_threshold = 1;// in pixels - vector structures_ids; - Mat2X x_image; - Mat4X X_world; - // Selects only the reconstructed tracks observed in the image - SelectExistingPointStructures(matches, image_id, *reconstruction, - &structures_ids, &x_image); - - // TODO(julien) Also remove structures that are on the same location - if (structures_ids.size() < 5) { - LOG(ERROR) << "Error: there are not enough points to estimate the pose (" - << structures_ids.size() << "<5)."; - // We need at least 5 tracks in order to do resection - return false; - } - MatrixOfPointStructureCoordinates(structures_ids, *reconstruction, &X_world); - CHECK(x_image.cols() == X_world.cols()); - - Mat3X X; - HomogeneousToEuclidean(X_world, &X); - Mat3 R; - Vec3 t; - vector inliers; - EuclideanResectionEPnPRobust(x_image, X, K, rms_inliers_threshold, - &R, &t, &inliers, 1e-3); - - // TODO(julien) Performs non-linear optimization of the pose. - - // Creates a new camera and add it to the reconstruction - PinholeCamera * camera = new PinholeCamera(K, R, t); - reconstruction->InsertCamera(image_id, camera); - VLOG(1) << "Add Camera [" - << image_id <<"]"<< std::endl <<"R=" - << R << std::endl <<"t= " - << t.transpose() - << std::endl; - //Adds only inliers matches into - const Feature * feature = NULL; - for (size_t s = 0; s < structures_ids.size(); ++s) { - feature = matches.Get(image_id, structures_ids[s]); - matches_inliers->Insert(image_id, structures_ids[s], feature); - } - VLOG(1) << "Inliers added: " << structures_ids.size() << std::endl; - return true; -} -} // namespace libmv +// Copyright (c) 2010 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include "libmv/base/vector_utils.h" +#include "libmv/camera/pinhole_camera.h" +#include "libmv/correspondence/matches.h" +#include "libmv/logging/logging.h" +#include "libmv/multiview/five_point.h" +#include "libmv/multiview/fundamental.h" +#include "libmv/multiview/robust_euclidean_resection.h" +#include "libmv/multiview/robust_fundamental.h" +#include "libmv/reconstruction/euclidean_reconstruction.h" +#include "libmv/reconstruction/export_blender.h" +#include "libmv/reconstruction/export_ply.h" +#include "libmv/reconstruction/image_selection.h" +#include "libmv/reconstruction/keyframe_selection.h" +#include "libmv/reconstruction/mapping.h" +#include "libmv/reconstruction/optimization.h" +#include "libmv/reconstruction/tools.h" + + +namespace libmv { + +// Function that set the given image size to an image +inline bool SetImageSize(Reconstruction &recons, + Matches::ImageID image_id, + const Vec2u &image_size) { + PinholeCamera *camera = dynamic_cast( + recons.GetCamera(image_id)); + if (camera) { + camera->set_image_size(image_size); + return true; + } + return false; +} + +bool CalibratedCameraResection(const Matches &matches, + Matches::ImageID image_id, + const Mat3 &K, + Matches *matches_inliers, + Reconstruction *reconstruction) { + double rms_inliers_threshold = 1;// in pixels + vector structures_ids; + Mat2X x_image; + Mat4X X_world; + // Selects only the reconstructed tracks observed in the image + SelectExistingPointStructures(matches, image_id, *reconstruction, + &structures_ids, &x_image); + + // TODO(julien) Also remove structures that are on the same location + if (structures_ids.size() < 5) { + LOG(ERROR) << "Error: there are not enough points to estimate the pose (" + << structures_ids.size() << "<5)."; + // We need at least 5 tracks in order to do resection + return false; + } + MatrixOfPointStructureCoordinates(structures_ids, *reconstruction, &X_world); + CHECK(x_image.cols() == X_world.cols()); + + Mat3X X; + HomogeneousToEuclidean(X_world, &X); + Mat3 R; + Vec3 t; + vector inliers; + + EuclideanResectionEPnPRobust(x_image, X, K, rms_inliers_threshold, + &R, &t, &inliers, 1e-3); + + // TODO(julien) Performs non-linear optimization of the pose. + + // Create a new camera and add it to the reconstruction + PinholeCamera * camera = new PinholeCamera(K, R, t); + reconstruction->InsertCamera(image_id, camera); + VLOG(1) << "Add Camera [" + << image_id <<"]"<< std::endl <<"R=" + << R << std::endl <<"t= " + << t.transpose() + << std::endl; + //Add only inliers matches into + const Feature * feature = NULL; + for (size_t s = 0; s < structures_ids.size(); ++s) { + feature = matches.Get(image_id, structures_ids[s]); + matches_inliers->Insert(image_id, structures_ids[s], feature); + } + VLOG(1) << "Inliers added: " << structures_ids.size() << std::endl; + return true; +} + +bool InitialReconstructionTwoViews(const Matches &matches, + Matches::ImageID image1, + Matches::ImageID image2, + const Mat3 &K1, + const Mat3 &K2, + const Vec2u &image_size1, + const Vec2u &image_size2, + Reconstruction *recons) { + assert(image1 != image2); + bool is_good = true; + uint num_new_points = 0; + Matches matches_inliers; + + VLOG(2) << " -- Initial Motion Estimation -- " << std::endl; + double epipolar_threshold = 1;// epipolar error in pixels + double outliers_probability = 1e-3;// epipolar error in pixels + vector xs(2); + vector tracks; + vector images; + images.push_back(image1); + images.push_back(image2); + PointMatchMatrices(matches, images, &tracks, &xs); + // TODO(julien) Also remove structures that are on the same location + if (xs[0].cols() < 7) { + LOG(ERROR) << "Error: there are not enough common matches (" + << xs[0].cols()<< "<7)."; + return false; + } + + Mat &x0 = xs[0]; + Mat &x1 = xs[1]; + vector feature_inliers; + Mat3 F; + // Computes fundamental matrix + // TODO(julien) For the calibrated case, we can squeeze the fundamental using + // directly the 5 points algorithm + FundamentalFromCorrespondences7PointRobust(x0,x1, + epipolar_threshold, + &F, &feature_inliers, + outliers_probability); + // Only inliers are selected in order to estimation the relative motion + Mat2X v0(2, feature_inliers.size()); + Mat2X v1(2, feature_inliers.size()); + size_t index_inlier = 0; + for (size_t c = 0; c < feature_inliers.size(); ++c) { + index_inlier = feature_inliers[c]; + v0.col(c) = x0.col(index_inlier); + v1.col(c) = x1.col(index_inlier); + } + Mat3 E; + // Computes essential matrix + EssentialFromFundamental(F, K1, K2, &E); + + Mat3 dR; + Vec3 dt; + // Recover motion between the two images + bool is_motion_ok = MotionFromEssentialAndCorrespondence(E, K1, v0.col(0), + K2, v1.col(0), + &dR, &dt); + if (!is_motion_ok) { + LOG(ERROR) << "Error: the motion cannot be estimated."; + return false; + } + + Mat3 R; + Vec3 t; + PinholeCamera * pcamera = NULL; + pcamera = dynamic_cast(recons->GetCamera(image1)); + // If the first image has no associated camera, we choose the center of the + // coordinate frame + if (!pcamera) { + R.setIdentity(); + t.setZero(); + pcamera = new PinholeCamera(K1, R, t); + recons->InsertCamera(image1, pcamera); + VLOG(1) << "Add Camera [" + << image1 <<"]"<< std::endl <<"R=" + << R << std::endl <<"t= " + << t.transpose() + << std::endl; + } + // Recover the asolute pose: R2 = R1 * dR, t2 = R1 * dt + t1 + R = pcamera->orientation_matrix() * dR; + t = pcamera->orientation_matrix() * dt + pcamera->position(); + + // Creates and adds the second camera + pcamera = new PinholeCamera(K2, R, t); + recons->InsertCamera(image2, pcamera); + VLOG(1) << "Add Camera [" + << image2 <<"]"<< std::endl <<"R=" + << R << std::endl <<"t= " + << t.transpose() + << std::endl; + + //Adds only inliers matches into + const Feature * feature = NULL; + for (size_t s = 0; s < feature_inliers.size(); ++s) { + feature = matches.Get(image1, tracks[feature_inliers[s]]); + matches_inliers.Insert(image1, tracks[feature_inliers[s]], feature); + feature = matches.Get(image2, tracks[feature_inliers[s]]); + matches_inliers.Insert(image2, tracks[feature_inliers[s]], feature); + } + // TODO(julien) remove outliers from matches using matches_inliers. + if (!is_good) { + return false; + } + + SetImageSize(*recons, image1, image_size1); + SetImageSize(*recons, image2, image_size2); + + VLOG(2) << " -- Initial Intersection -- " << std::endl; + num_new_points = PointStructureTriangulationCalibrated(matches_inliers, + image1, + 2, + recons); + VLOG(2) << num_new_points << " points reconstructed." << std::endl; + + ExportToBlenderScript(*recons, "init.py"); + + // Performs projective bundle adjustment + if (num_new_points > 0) { + VLOG(2) << " -- Bundle adjustment -- " << std::endl; + MetricBundleAdjust(matches_inliers, recons); + ExportToBlenderScript(*recons, "init-ba.py"); + // TODO(julien) Remove outliers RemoveOutliers() + BA again + } + return is_good; +} + +bool IncrementalReconstructionKeyframes(const Matches &matches, + const vector &kframes, + const int first_keyframe_index, + const Mat3 &K, + const Vec2u &image_size, + Reconstruction *reconstruction, + int *keyframe_stopped_index) { + bool is_good = true; + int keyframe_index = first_keyframe_index; + int min_num_views_for_triangulation = 2; + uint num_new_points = 0; + Matches::ImageID image_id; + // Estimates the pose every other images by resection-intersection + for (; keyframe_index < kframes.size(); ++keyframe_index) { + *keyframe_stopped_index = keyframe_index; + Matches matches_inliers; + image_id = kframes[keyframe_index]; + VLOG(2) << " -- Incremental Resection -- " << std::endl; + is_good = CalibratedCameraResection(matches, + image_id, K, + &matches_inliers, + reconstruction); + if (!is_good) { + VLOG(1) << "[Warning] Tracking lost!" << std::endl; + // The resection has returned an error: + // we cannot estimate the camera pose by resection so we create + // a new reconstruction (since the new one will not share the same + // scale and coordinate frame. + return false; + } + SetImageSize(*reconstruction, image_id, image_size); + // TODO(julien) remove outliers from matches using matches_inliers. + + VLOG(2) << " -- Incremental Intersection -- " << std::endl; + num_new_points = PointStructureTriangulationCalibrated( + matches, + image_id, + min_num_views_for_triangulation, + reconstruction); + VLOG(2) << num_new_points << " points reconstructed." << std::endl; + + // Performs a bundle adjustment + if (num_new_points > 0) { + VLOG(2) << " -- Bundle adjustment -- " << std::endl; + MetricBundleAdjust(matches, reconstruction); + // TODO(julien) is a local BA sufficient here? + // TODO(julien) Remove outliers RemoveOutliers() + BA again + } + } + return true; +} + +bool ReconstructionNonKeyframes(const Matches &matches, + const Mat3 &K, + const Vec2u &image_size, + std::list *reconstructions) { + bool is_recons_ok = true; + // Perform a bundle adjustment every X new cameras + int num_new_cameras_to_proceed_ba = 10; + bool do_bundle_adjustment = false; + bool is_frame_in_current_recons = false; + bool is_frame_in_next_recons = false; + std::set::iterator img_iter = matches.get_images().begin(); + std::list::iterator recons_iter = reconstructions->begin(); + std::list::iterator recons_iter_next = recons_iter; + recons_iter_next++; + int cpt_image_for_ba = 0, cpt_i = 0; + for (; img_iter != matches.get_images().end(); ++img_iter) { + if (cpt_image_for_ba < num_new_cameras_to_proceed_ba - 1) { + do_bundle_adjustment = false; + cpt_image_for_ba++; + } else { + do_bundle_adjustment = true; + cpt_image_for_ba = 0; + } + // If the image is not a keyframe, it should belong to the same + // reconstruction than the previous image. + // We try to find if the image has an already reconstructed camera for + // the current reconstruction and the next one + is_frame_in_current_recons = (*recons_iter)->ImageHasCamera(*img_iter); + is_frame_in_next_recons = recons_iter_next != reconstructions->end() && + (*recons_iter_next)->ImageHasCamera(*img_iter); + if (!is_frame_in_current_recons && !is_frame_in_next_recons) { + VLOG(2) << " -- Incremental Resection -- " << std::endl; + Matches matches_inliers; + is_recons_ok = CalibratedCameraResection(matches, + *img_iter, K, + &matches_inliers, + *recons_iter); + if (is_recons_ok) { + // TODO(julien) remove outliers from matches using matches_inliers. + // Performs a bundle adjustment + if (do_bundle_adjustment) { + VLOG(2) << " -- Bundle adjustment -- " << std::endl; + MetricBundleAdjust(matches, *recons_iter); + // TODO(julien) should be removed when camera only are optimized + // TODO(julien) Remove outliers RemoveOutliers() + BA again + } + SetImageSize(**recons_iter, *img_iter, image_size); + std::stringstream s; + s << "out-noKF-" << cpt_i << ".py"; + ExportToBlenderScript(**recons_iter, s.str()); + } else { + VLOG(1) << "[Warning] Image " << *img_iter + << " cannot be localized!" << std::endl; + } + cpt_i++; + } else if(is_frame_in_next_recons) { + // In this case, the current image is the first keyframe of the next + // reconstruction so we increment the iterators. + recons_iter++; + recons_iter_next++; + } + } + return true; +} + +bool EuclideanReconstructionFromVideo( + const Matches &matches, + int image_width, + int image_height, + double focal, + std::list *reconstructions) { + if (matches.NumImages() < 2) + return false; + Vec2u image_size; + image_size << image_width, image_height; + double cu = image_width/2 - 0.5, cv = image_height/2 - 0.5; + Mat3 K; K << focal, 0, cu, 0, focal, cv, 0, 0, 1; + + VLOG(2) << "Selecting keyframes." << std::endl; + libmv::vector keyframes; + SelectKeyframesBasedOnMatchesNumber(matches, &keyframes); + + VLOG(2) << "Keyframe list: "; + for (int i = 0; i < keyframes.size(); ++i) + VLOG(2) << keyframes[i] << " "; + VLOG(2) << std::endl; + + if (keyframes.size() < 2) { + VLOG(1) << "Not enough keyframes! " << std::endl; + return false; + } + + bool recons_ok = true; + bool all_keyframes_reconstructed = false; + int keyframe_index = 0; + do { + Reconstruction *cur_recons = new Reconstruction(); + if (keyframe_index + 1 >= keyframes.size()) + break; + recons_ok = InitialReconstructionTwoViews(matches, + keyframes[keyframe_index], + keyframes[keyframe_index + 1], + K,K, + image_size, image_size, + cur_recons); + keyframe_index++; + if (recons_ok) { + keyframe_index++; + // If an initial reconstruction has been done, we keep it and + // compute the next poses by intersection-resection + reconstructions->push_back(cur_recons); + all_keyframes_reconstructed = + IncrementalReconstructionKeyframes(matches, + keyframes, + keyframe_index, + K, image_size, + cur_recons, + &keyframe_index); + std::stringstream s; + s << "out-" << keyframe_index << ".py"; + ExportToBlenderScript(*cur_recons, s.str()); + } else { + // If the initial reconstruction can be estimated between the + // 2 first views, we try with the second image and the third (etc.) + } + } while (!all_keyframes_reconstructed && + (keyframe_index < keyframes.size() - 1)); + + // Reconstructs non-keyframes by resection + // NOTE(julien) are we sure that matches->images is ordered? it's a std:set? + // if not the following non-keyframes reconstruction should be changed. + VLOG(2) << " Non-keyframe reconstruction " << std::endl; + ReconstructionNonKeyframes(matches, + K, image_size, + reconstructions); + return true; +} +} // namespace libmv diff --git a/src/libmv/reconstruction/euclidean_reconstruction.h b/src/libmv/reconstruction/euclidean_reconstruction.h index 3c8e171..2c6fdd2 100644 --- a/src/libmv/reconstruction/euclidean_reconstruction.h +++ b/src/libmv/reconstruction/euclidean_reconstruction.h @@ -25,45 +25,107 @@ namespace libmv { -// Estimates the poses of the two cameras using the fundamental and essential -// matrices. +// Estimates the pose of the camera using the already reconstructed points. // The method: -// selects common matches of the two images -// robustly estimates the fundamental matrix -// estimates the essential matrix from the fundamental matrix -// extracts the relative motion from the essential matrix -// if the first image has no camera, it creates the camera and initializes -// the pose to be the world frame -// estimates the absolute pose of the second camera from the first pose and -// the estimated motion. -// creates and adds it to the reconstruction -// inserts only inliers matches into matches_inliers -// Returns true if the pose estimation has succeed -// Returns false if -// the number of common matches is less than 7 -// there is no solution for the relative motion from the essential matrix -bool ReconstructFromTwoCalibratedViews(const Matches &matches, - CameraID image_id1, - CameraID image_id2, - const Mat3 &K1, - const Mat3 &K2, - Matches *matches_inliers, - Reconstruction *reconstruction); - -// Estimates the pose of the camera using the already reconstructed structures. -// The method: -// selects the tracks that have an already reconstructed structure -// robustly estimates the camera extrinsic parameters (R,t) by resection -// creates and adds the new camera to reconstruction -// inserts only inliers matches into matches_inliers +// - selects the tracks that have an already reconstructed structure +// - robustly estimates the camera extrinsic parameters (R,t) by resection +// - creates and adds the new camera to reconstruction +// - inserts only inliers matches into matches_inliers // Returns true if the resection has succeed // Returns false if -// the number of reconstructed Tracks is less than 5 +// - the number of reconstructed Tracks is less than 5 bool CalibratedCameraResection(const Matches &matches, - CameraID image_id, + Matches::ImageID image_id, const Mat3 &K, Matches *matches_inliers, Reconstruction *reconstruction); + +// Estimates a precise initial reconstruction using the matches of two views. +// The method: +// - selects common matches of the two images +// - robustly estimates the fundamental matrix +// - estimates the essential matrix from the fundamental matrix +// - extracts the relative motion from the essential matrix +// - if the first image has no camera, it creates the camera and initializes +// the pose to be the world coordinate frame +// - estimates the absolute pose of the second camera from the first pose and +// the estimated motion. +// - creates and adds the cameras to the reconstruction +// - reconstructs only the inliers matches (point triangulation) +// - performs a metric bundle adjusment +// TODO(julien) remove outliers from matches or output matches_inliers. +// Returns true if the initial reconstruction has succeed +// Returns false if +// - the number of common matches is less than 7 +// - there is no solution for the relative motion from the essential matrix +bool InitialReconstructionTwoViews(const Matches &matches, + Matches::ImageID image1, + Matches::ImageID image2, + const Mat3 &K1, + const Mat3 &K2, + const Vec2u &image_size1, + const Vec2u &image_size2, + Reconstruction *recons); + +// Estimates the pose of the keyframes using the already reconstructed points. +// For every keyframes (starting the first_keyframe_index th): +// - the keyframe is localized (by resection) +// - if the resection has not failed, the inliers tracks are reconstructed +// by point triangulation +// - if new points are created, a global bundle adjustment is performed +// TODO(julien) a local bundle adjustment would be sufficient? +// The method stops when one keyframe cannot be localized (tracking lost), +// keyframe_stopped_index is the index of this keyframe. +// Returns true if all keyframes have been localized +// Returns false if one keyframe cannot be localized (tracking lost). +bool IncrementalReconstructionKeyframes(const Matches &matches, + const vector &kframes, + const int first_keyframe_index, + const Mat3 &K, + const Vec2u &image_size, + Reconstruction *reconstruction, + int *keyframe_stopped_index); + +// Estimates the pose of non already localized frames using the already +// reconstructed points by resection. +// It performs also a bundle adjustment when X=10 new cameras are localized. +// TODO(julien) a local bundle adjustment would be sufficient? +// The method automatically detect the reconstruction the frame may belongs. +// NOTE: this method works only if the frame in the Matches class are ordered. +// If it is not the case, it will fail. +// Returns true. +bool ReconstructionNonKeyframes(const Matches &matches, + const Mat3 &K, + const Vec2u &image_size, + std::list *reconstructions); + +// Computes the trajectory of a camera using matches as input. +// - First keyframes are detected according a minimum number of shared tracks +// - Next the first two keyframes are used to estimate an initial structure +// - Then every other keyframe are reconstructed based on an euclidean resection +// algorithm with the previously recontructed structures and new structures +// are estimated (points triangulation). A bundle adjustment is performed on +// all the reconstruction each time a keyframe is localized. +// TODO(julien) a local bundle adjustment would be sufficient? +// TODO(julien) +a global bundle adjusment on all data at the end? +// - In a final step, non-keyframes are localized using the resection method. +// A bundle adjusment is periodically performed on all the data. +// In the case that the tracking is lost, a new reconstruction is created. +// TODO(julien) Add the calibration matrix K as input? +// TODO(julien) remove outliers from matches or output inliers matches. +bool EuclideanReconstructionFromVideo( + const Matches &matches, + int image_width, + int image_height, + double focal, + std::list *reconstructions); + +// Computes the poses of all unordered images. +// TODO(julien) implement me. +bool EuclideanReconstructionFromImageSet( + const Matches &matches, + const vector > &image_sizes, + std::list *reconstructions); } // namespace libmv #endif // LIBMV_RECONSTRUCTION_EUCLIDEAN_RECONSTRUCTION_H_ diff --git a/src/libmv/reconstruction/euclidean_reconstruction_test.cc b/src/libmv/reconstruction/euclidean_reconstruction_test.cc new file mode 100644 index 0000000..dbff6c8 --- /dev/null +++ b/src/libmv/reconstruction/euclidean_reconstruction_test.cc @@ -0,0 +1,181 @@ +// Copyright (c) 2010 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. +#include + +#include "libmv/logging/logging.h" +#include "libmv/multiview/projection.h" +#include "libmv/multiview/random_sample.h" +#include "libmv/multiview/test_data_sets.h" +#include "libmv/numeric/numeric.h" +#include "libmv/reconstruction/reconstruction.h" +#include "libmv/reconstruction/euclidean_reconstruction.h" +#include "libmv/reconstruction/mapping.h" +#include "libmv/reconstruction/optimization.h" +#include "libmv/reconstruction/projective_reconstruction.h" +#include "testing/testing.h" + +namespace libmv { +namespace { + +void GenerateMatchesFromNViewDataSet(const NViewDataSet &d, + int noutliers, + Matches *matches, + std::list *list_features) { + Matches::TrackID track_id; + vector wrong_matches; + for (size_t n = 0; n < d.n; ++n) { + //std::cout << "n -> "<< d.x[n]<< std::endl; + // Generates wrong matches + UniformSample(noutliers, d.X.cols(), &wrong_matches); + //std::cout << "Features :"<push_back(feature); + track_id = p; + if (p < noutliers) { + track_id = wrong_matches[p]; + } + matches->Insert(n, track_id, feature); + } + } +} + +TEST(CalibratedReconstruction, TestSynthetic6FullViews) { + // TODO(julien) maybe a better check is the relative motion + int nviews = 6; + int npoints = 100; + int noutliers = 0.4*npoints;// 30% of outliers + NViewDataSet d = NRealisticCamerasFull(nviews, npoints); + + Mat4X X; + EuclideanToHomogeneous(d.X, &X); + // These are the expected precision of the results + // Dont expect much since for now + // - this is an incremental approach + // - the 3D structure is not retriangulated when new views are estimated + // - there is no optimization! + const double kPrecisionOrientationMatrix = 3e-2; + const double kPrecisionPosition = 3e-2; + + Reconstruction reconstruction; + // Create the matches + Matches matches; + Matches matches_inliers; + std::list list_features; + GenerateMatchesFromNViewDataSet(d, noutliers, &matches, &list_features); + + // We fix the gauge by setting the pose of the initial camera to the true pose + PinholeCamera * camera0 = new PinholeCamera(d.K[0], d.R[0], d.t[0]); + Vec2u image_size1; image_size1 << d.K[0](0, 2), d.K[0](1, 2); + Vec2u image_size2; image_size2 << d.K[0](0, 2), d.K[0](1, 2); + + std::cout << "Proceed Initial Motion Estimation" << std::endl; + // Proceed Initial Motion Estimation + bool recons_ok = true; + recons_ok = InitialReconstructionTwoViews(matches, + 0, 1, + d.K[0], d.K[1], + image_size1, image_size2, + &reconstruction); + PinholeCamera * camera = NULL; + EXPECT_EQ(reconstruction.GetNumberCameras(), 2); + camera = dynamic_cast(reconstruction.GetCamera(0)); + EXPECT_TRUE(camera != NULL); + /* + // TODO(julien) Check the reconstruction! + EXPECT_MATRIX_PROP(camera->orientation_matrix(), d.R[0], 1e-8); + EXPECT_MATRIX_PROP(camera->position(), d.t[0], 1e-8); + + double rms = RootMeanSquareError(d.x[0], X, camera->projection_matrix()); + std::cout << "RMSE Camera 0 = " << rms << std::endl; + + camera = dynamic_cast(reconstruction.GetCamera(1)); + EXPECT_TRUE(camera != NULL); + + // This is a monocular reconstruction + // We fix the scale + Mat3 dR = d.R[0].transpose()*d.R[1]; + Vec3 dt = d.R[0].transpose() * (d.t[1] - d.t[0]); + double dt_norm_real = dt.norm(); + dt = camera0->orientation_matrix().transpose() * + (camera->position() - camera0->position()); + dt *= dt_norm_real/dt.norm(); + camera->set_position(camera0->orientation_matrix() * dt + + camera0->position()); + + EXPECT_MATRIX_PROP(camera->orientation_matrix(), d.R[1], + kPrecisionOrientationMatrix); + EXPECT_MATRIX_PROP(camera->position(), d.t[1], kPrecisionPosition); + rms = RootMeanSquareError(d.x[1], X, camera->projection_matrix()); + std::cout << "RMSE Camera 1 = " << rms << std::endl; + + std::cout << "Proceed Initial Intersection" << std::endl; + // Proceed Initial Intersection + uint nInliers_added = 0; + size_t minimum_num_views_batch = 2; + nInliers_added = PointStructureTriangulationCalibrated(matches_inliers, 1, + minimum_num_views_batch, + &reconstruction); + ASSERT_NEAR(nInliers_added, npoints - noutliers, 1); + // TODO(julien) check imqges sizes, etc. + size_t minimum_num_views_incremental = 3; + Mat3 R; + Vec3 t; + // Checks the incremental reconstruction + for (int i = 2; i < nviews; ++i) { + std::cout << "Proceed Incremental Resection" << std::endl; + // Proceed Incremental Resection + CalibratedCameraResection(matches, i, d.K[i], + &matches_inliers, &reconstruction); + + EXPECT_EQ(reconstruction.GetNumberCameras(), i+1); + camera = dynamic_cast(reconstruction.GetCamera(i)); + EXPECT_TRUE(camera != NULL); + EXPECT_MATRIX_PROP(camera->orientation_matrix(), d.R[i], + kPrecisionOrientationMatrix); + EXPECT_MATRIX_PROP(camera->position(), d.t[i], kPrecisionPosition); + + std::cout << "Proceed Incremental Intersection" << std::endl; + // Proceed Incremental Intersection + nInliers_added = PointStructureTriangulationCalibrated( + matches_inliers, i, + minimum_num_views_incremental, + &reconstruction); + ASSERT_NEAR(nInliers_added, 0, 1); + + // TODO(julien) Check the rms with the reconstructed structure + rms = RootMeanSquareError(d.x[i], X, camera->projection_matrix()); + std::cout << "RMSE Camera " << i << " = " << rms << std::endl; + // TODO(julien) Check the 3D structure coordinates and inliers + } + // Performs bundle adjustment + rms = MetricBundleAdjust(matches_inliers, &reconstruction); + std::cout << " Final RMSE = " << rms << std::endl; + // TODO(julien) Check the results of BA*/ + // clear the cameras, structures and features + reconstruction.ClearCamerasMap(); + reconstruction.ClearStructuresMap(); + std::list::iterator features_iter = list_features.begin(); + for (; features_iter != list_features.end(); ++features_iter) + delete *features_iter; + list_features.clear(); +} +} +} // namespace libmv diff --git a/src/libmv/reconstruction/export_blender.cc b/src/libmv/reconstruction/export_blender.cc index 962c499..d0163b6 100644 --- a/src/libmv/reconstruction/export_blender.cc +++ b/src/libmv/reconstruction/export_blender.cc @@ -1,148 +1,148 @@ -// Copyright (c) 2010 libmv authors. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include -#include - -#include "libmv/reconstruction/export_blender.h" - -namespace libmv { - -void ExportToBlenderScript(const Reconstruction &reconstruct, - std::string out_file_name) { - // Avoid to have comas instead of dots (foreign lang.) - setlocale(LC_ALL, "C"); - - FILE* fid = fopen(out_file_name.c_str(), "wb"); - assert(fid); - - fprintf(fid, "# libmv blender camera track export; do not edit\n"); - fprintf(fid, "import Blender\n"); - fprintf(fid, "from Blender import Camera, Object, Scene, NMesh\n"); - fprintf(fid, "from Blender import Mathutils\n"); - fprintf(fid, "from Blender.Mathutils import *\n"); - - fprintf(fid, "cur = Scene.GetCurrent()\n"); - - ////////////////////////////////////////////////////////////////////////// - // Cameras. - fprintf(fid, "# Cameras\n"); - PinholeCamera *pcamera = NULL; - uint i = 0; - Mat3 K, R; Vec3 t; - std::map::const_iterator camera_iter = - reconstruct.cameras().begin(); - for (; camera_iter != reconstruct.cameras().end(); ++camera_iter) { - pcamera = dynamic_cast(camera_iter->second); - // TODO(julien) how to export generic cameras ? - if (pcamera) { - K = pcamera->intrinsic_matrix(); - R = pcamera->orientation_matrix(); - t = pcamera->position(); - // Set up the camera - fprintf(fid, "c%04d = Camera.New('persp')\n", i); - // TODO(pau) we may want to take K(0,0)/K(1,1) and push that into the - // render aspect ratio settings. - double f = K(0,0); - double width = pcamera->image_width(); - // WARNING: MAGIC CONSTANT from blender source - // Look for 32.0 in 'source/blender/render/intern/source/initrender.c' - double lens = f / width * 32.0; - fprintf(fid, "c%04d.lens = %g\n", i, lens); - fprintf(fid, "c%04d.setDrawSize(0.05)\n", i); - fprintf(fid, "o%04d = Object.New('Camera')\n", i); - fprintf(fid, "o%04d.name = 'libmv_cam%04d'\n", i, camera_iter->first); - - // Camera world matrix, which is the inverse transpose of the typical - // 'projection' matrix as generally thought of by vision researchers. - // Usually it is x = K[R|t]X; but here it is the inverse of R|t stacked - // on [0,0,0,1], but then in fortran-order. So that is where the - // weirdness comes from. - fprintf(fid, "o%04d.setMatrix(Mathutils.Matrix(",i); - for (int j = 0; j < 3; ++j) { - fprintf(fid, "["); - for (int k = 0; k < 3; ++k) { - // Opengl's camera faces down the NEGATIVE z axis so we have to - // do a 180 degree X axis rotation. The math works out so that - // the following conditional nicely implements that. - if (j == 2 || j == 1) - fprintf(fid, "%g,", -R(j,k)); // transposed + opposite! - else - fprintf(fid, "%g,", R(j,k)); // transposed! - } - fprintf(fid, "0.0],"); - } - libmv::Vec3 optical_center = -R.transpose() * t; - fprintf(fid, "["); - for (int j = 0; j < 3; ++j) - fprintf(fid, "%g,", optical_center(j)); - fprintf(fid, "1.0]))\n"); - - // Link the scene and the camera together - fprintf(fid, "o%04d.link(c%04d)\n\n", i, i); - fprintf(fid, "cur.link(o%04d)\n\n", i); - i++; - } - } - uint num_cam_exported = i; - - ////////////////////////////////////////////////////////////////////////// - // Point Cloud. - fprintf(fid, "# Point cloud\n"); - fprintf(fid, "ob=Object.New('Mesh','libmv_point_cloud')\n"); - fprintf(fid, "ob.setLocation(0.0,0.0,0.0)\n"); - fprintf(fid, "mesh=ob.getData()\n"); - fprintf(fid, "cur.link(ob)\n"); - std::map::const_iterator track_iter = - reconstruct.structures().begin(); - for (; track_iter != reconstruct.structures().end(); ++track_iter) { - PointStructure * point_s = - dynamic_cast(track_iter->second); - if (point_s) { - fprintf(fid, "v = NMesh.Vert(%g,%g,%g)\n", point_s->coords_affine()(0), - point_s->coords_affine()(1), - point_s->coords_affine()(2)); - fprintf(fid, "mesh.verts.append(v)\n"); - } - } - fprintf(fid, "mesh.update()\n"); - fprintf(fid, "cur.update()\n\n"); - - ////////////////////////////////////////////////////////////////////////// - // Scene node including cameras and points. - fprintf(fid, "# Add a helper object to help manipulating joined camera and" - "points\n"); - fprintf(fid, "scene_dummy = Object.New('Empty','libmv_scene')\n"); - fprintf(fid, "scene_dummy.setLocation(0.0,0.0,0.0)\n"); - fprintf(fid, "cur.link(scene_dummy)\n"); - - fprintf(fid, "scene_dummy.makeParent([ob])\n"); - for (int i = 0; i < num_cam_exported; ++i) { - fprintf(fid, "scene_dummy.makeParent([o%04d])\n", i); - } - fprintf(fid, "\n"); - // fprintf(fid, "scene_dummy.setEuler((-3.141593/2, 0.0, 0.0))\n"); - fprintf(fid, "scene_dummy.SizeX=1.0\n"); - fprintf(fid, "scene_dummy.SizeY=1.0\n"); - fprintf(fid, "scene_dummy.SizeZ=1.0\n"); - - fclose(fid); -} -} // namespace libmv +// Copyright (c) 2010 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include +#include + +#include "libmv/reconstruction/export_blender.h" + +namespace libmv { + +void ExportToBlenderScript(const Reconstruction &reconstruct, + std::string out_file_name) { + // Avoid to have comas instead of dots (foreign lang.) + setlocale(LC_ALL, "C"); + + FILE* fid = fopen(out_file_name.c_str(), "wb"); + assert(fid); + + fprintf(fid, "# libmv blender camera track export; do not edit\n"); + fprintf(fid, "import Blender\n"); + fprintf(fid, "from Blender import Camera, Object, Scene, NMesh\n"); + fprintf(fid, "from Blender import Mathutils\n"); + fprintf(fid, "from Blender.Mathutils import *\n"); + + fprintf(fid, "cur = Scene.GetCurrent()\n"); + + ////////////////////////////////////////////////////////////////////////// + // Cameras. + fprintf(fid, "# Cameras\n"); + PinholeCamera *pcamera = NULL; + uint i = 0; + Mat3 K, R; Vec3 t; + std::map::const_iterator camera_iter = + reconstruct.cameras().begin(); + for (; camera_iter != reconstruct.cameras().end(); ++camera_iter) { + pcamera = dynamic_cast(camera_iter->second); + // TODO(julien) how to export generic cameras ? + if (pcamera) { + K = pcamera->intrinsic_matrix(); + R = pcamera->orientation_matrix(); + t = pcamera->position(); + // Set up the camera + fprintf(fid, "c%04d = Camera.New('persp')\n", i); + // TODO(pau) we may want to take K(0,0)/K(1,1) and push that into the + // render aspect ratio settings. + double f = K(0,0); + double width = pcamera->image_width(); + // WARNING: MAGIC CONSTANT from blender source + // Look for 32.0 in 'source/blender/render/intern/source/initrender.c' + double lens = f / width * 32.0; + fprintf(fid, "c%04d.lens = %g\n", i, lens); + fprintf(fid, "c%04d.setDrawSize(0.05)\n", i); + fprintf(fid, "o%04d = Object.New('Camera')\n", i); + fprintf(fid, "o%04d.name = 'libmv_cam%04d'\n", i, camera_iter->first); + + // Camera world matrix, which is the inverse transpose of the typical + // 'projection' matrix as generally thought of by vision researchers. + // Usually it is x = K[R|t]X; but here it is the inverse of R|t stacked + // on [0,0,0,1], but then in fortran-order. So that is where the + // weirdness comes from. + fprintf(fid, "o%04d.setMatrix(Mathutils.Matrix(",i); + for (int j = 0; j < 3; ++j) { + fprintf(fid, "["); + for (int k = 0; k < 3; ++k) { + // Opengl's camera faces down the NEGATIVE z axis so we have to + // do a 180 degree X axis rotation. The math works out so that + // the following conditional nicely implements that. + if (j == 2 || j == 1) + fprintf(fid, "%g,", -R(j,k)); // transposed + opposite! + else + fprintf(fid, "%g,", R(j,k)); // transposed! + } + fprintf(fid, "0.0],"); + } + libmv::Vec3 optical_center = -R.transpose() * t; + fprintf(fid, "["); + for (int j = 0; j < 3; ++j) + fprintf(fid, "%g,", optical_center(j)); + fprintf(fid, "1.0]))\n"); + + // Link the scene and the camera together + fprintf(fid, "o%04d.link(c%04d)\n\n", i, i); + fprintf(fid, "cur.link(o%04d)\n\n", i); + i++; + } + } + uint num_cam_exported = i; + + ////////////////////////////////////////////////////////////////////////// + // Point Cloud. + fprintf(fid, "# Point cloud\n"); + fprintf(fid, "ob=Object.New('Mesh','libmv_point_cloud')\n"); + fprintf(fid, "ob.setLocation(0.0,0.0,0.0)\n"); + fprintf(fid, "mesh=ob.getData()\n"); + fprintf(fid, "cur.link(ob)\n"); + std::map::const_iterator track_iter = + reconstruct.structures().begin(); + for (; track_iter != reconstruct.structures().end(); ++track_iter) { + PointStructure * point_s = + dynamic_cast(track_iter->second); + if (point_s) { + fprintf(fid, "v = NMesh.Vert(%g,%g,%g)\n", point_s->coords_affine()(0), + point_s->coords_affine()(1), + point_s->coords_affine()(2)); + fprintf(fid, "mesh.verts.append(v)\n"); + } + } + fprintf(fid, "mesh.update()\n"); + fprintf(fid, "cur.update()\n\n"); + + ////////////////////////////////////////////////////////////////////////// + // Scene node including cameras and points. + fprintf(fid, "# Add a helper object to help manipulating joined camera and" + "points\n"); + fprintf(fid, "scene_dummy = Object.New('Empty','libmv_scene')\n"); + fprintf(fid, "scene_dummy.setLocation(0.0,0.0,0.0)\n"); + fprintf(fid, "cur.link(scene_dummy)\n"); + + fprintf(fid, "scene_dummy.makeParent([ob])\n"); + for (int i = 0; i < num_cam_exported; ++i) { + fprintf(fid, "scene_dummy.makeParent([o%04d])\n", i); + } + fprintf(fid, "\n"); + // fprintf(fid, "scene_dummy.setEuler((-3.141593/2, 0.0, 0.0))\n"); + fprintf(fid, "scene_dummy.SizeX=1.0\n"); + fprintf(fid, "scene_dummy.SizeY=1.0\n"); + fprintf(fid, "scene_dummy.SizeZ=1.0\n"); + + fclose(fid); +} +} // namespace libmv diff --git a/src/libmv/reconstruction/export_ply.cc b/src/libmv/reconstruction/export_ply.cc index 9768e09..be0985c 100644 --- a/src/libmv/reconstruction/export_ply.cc +++ b/src/libmv/reconstruction/export_ply.cc @@ -1,80 +1,80 @@ -// Copyright (c) 2010 libmv authors. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include -#include - -#include "libmv/reconstruction/export_ply.h" - -namespace libmv { - -void ExportToPLY(const Reconstruction &reconstruct, std::string out_file_name) { - std::ofstream outfile; - outfile.open(out_file_name.c_str(), std::ios_base::out); - if (outfile.is_open()) { - outfile << "ply" << std::endl; - outfile << "format ascii 1.0" << std::endl; - outfile << "comment Made by libmv authors" << std::endl; - outfile << "comment 3D points structure:" << std::endl; - outfile << "element vertex " << reconstruct.GetNumberStructures() - << std::endl; - outfile << "property float x" << std::endl; - outfile << "property float y" << std::endl; - outfile << "property float z" << std::endl; - outfile << "property uchar red" << std::endl; - outfile << "property uchar green" << std::endl; - outfile << "property uchar blue" << std::endl; - outfile << "comment Cameras positions:" << std::endl; - outfile << "element vertex " << reconstruct.GetNumberCameras() << std::endl; - outfile << "property float x" << std::endl; - outfile << "property float y" << std::endl; - outfile << "property float z" << std::endl; - outfile << "property uchar red" << std::endl; - outfile << "property uchar green" << std::endl; - outfile << "property uchar blue" << std::endl; - outfile << "end_header" << std::endl; - std::map::const_iterator track_iter = - reconstruct.structures().begin(); - for (; track_iter != reconstruct.structures().end(); ++track_iter) { - PointStructure * point_s = - dynamic_cast(track_iter->second); - if (point_s) { - // Exports the point affine position - outfile << point_s->coords_affine().transpose() << " "; - // Exports the point color - outfile << "255 255 255" << std::endl; - } - } - std::map::const_iterator camera_iter = - reconstruct.cameras().begin(); - for (; camera_iter != reconstruct.cameras().end(); ++camera_iter) { - PinholeCamera * camera_pinhole = - dynamic_cast(camera_iter->second); - if (camera_pinhole) { - // Exports the camera position - outfile << camera_pinhole->position().transpose() << " "; - // Exports the camera color - outfile << "255 0 0" << std::endl; - } - } - outfile.close(); - } -} -} // namespace libmv +// Copyright (c) 2010 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include +#include + +#include "libmv/reconstruction/export_ply.h" + +namespace libmv { + +void ExportToPLY(const Reconstruction &reconstruct, std::string out_file_name) { + std::ofstream outfile; + outfile.open(out_file_name.c_str(), std::ios_base::out); + if (outfile.is_open()) { + outfile << "ply" << std::endl; + outfile << "format ascii 1.0" << std::endl; + outfile << "comment Made by libmv authors" << std::endl; + outfile << "comment 3D points structure:" << std::endl; + outfile << "element vertex " << reconstruct.GetNumberStructures() + << std::endl; + outfile << "property float x" << std::endl; + outfile << "property float y" << std::endl; + outfile << "property float z" << std::endl; + outfile << "property uchar red" << std::endl; + outfile << "property uchar green" << std::endl; + outfile << "property uchar blue" << std::endl; + outfile << "comment Cameras positions:" << std::endl; + outfile << "element vertex " << reconstruct.GetNumberCameras() << std::endl; + outfile << "property float x" << std::endl; + outfile << "property float y" << std::endl; + outfile << "property float z" << std::endl; + outfile << "property uchar red" << std::endl; + outfile << "property uchar green" << std::endl; + outfile << "property uchar blue" << std::endl; + outfile << "end_header" << std::endl; + std::map::const_iterator track_iter = + reconstruct.structures().begin(); + for (; track_iter != reconstruct.structures().end(); ++track_iter) { + PointStructure * point_s = + dynamic_cast(track_iter->second); + if (point_s) { + // Exports the point affine position + outfile << point_s->coords_affine().transpose() << " "; + // Exports the point color + outfile << "255 255 255" << std::endl; + } + } + std::map::const_iterator camera_iter = + reconstruct.cameras().begin(); + for (; camera_iter != reconstruct.cameras().end(); ++camera_iter) { + PinholeCamera * camera_pinhole = + dynamic_cast(camera_iter->second); + if (camera_pinhole) { + // Exports the camera position + outfile << camera_pinhole->position().transpose() << " "; + // Exports the camera color + outfile << "255 0 0" << std::endl; + } + } + outfile.close(); + } +} +} // namespace libmv diff --git a/src/libmv/reconstruction/image_order_selection.cc b/src/libmv/reconstruction/image_order_selection.cc index bed9a35..e69de29 100644 --- a/src/libmv/reconstruction/image_order_selection.cc +++ b/src/libmv/reconstruction/image_order_selection.cc @@ -1,240 +0,0 @@ -// Copyright (c) 2010 libmv authors. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include -#include - -#include "libmv/multiview/conditioning.h" -#include "libmv/multiview/robust_fundamental.h" -#include "libmv/multiview/robust_homography.h" -#include "libmv/reconstruction/image_order_selection.h" - -namespace libmv { - -void FillPairwiseMatchesMatrix(const Matches &matches, - Mat *m) { - m->resize(matches.NumImages(), - matches.NumImages()); - m->setZero(); - std::set::const_iterator image_iter1 = - matches.get_images().begin(); - std::set::const_iterator image_iter2; - vector xs2; - for (;image_iter1 != matches.get_images().end(); ++image_iter1) { - image_iter2 = image_iter1; - image_iter2++; - for (;image_iter2 != matches.get_images().end(); ++image_iter2) { - TwoViewPointMatchMatrices(matches, *image_iter1, *image_iter2, &xs2); - (*m)(*image_iter1, *image_iter2) = xs2[0].cols(); - } - } -} - -void FillPairwiseMatchesHomographyMatrix(const Matches &matches, - Mat *m) { - m->resize(matches.NumImages(), - matches.NumImages()); - m->setZero(); - Mat3 H; - vector inliers; - double max_error_h = 1; - std::set::const_iterator image_iter1 = - matches.get_images().begin(); - std::set::const_iterator image_iter2; - vector xs2; - for (;image_iter1 != matches.get_images().end(); ++image_iter1) { - image_iter2 = image_iter1; - image_iter2++; - for (;image_iter2 != matches.get_images().end(); ++image_iter2) { - TwoViewPointMatchMatrices(matches, *image_iter1, *image_iter2, &xs2); - (*m)(*image_iter1, *image_iter2) = xs2[0].cols(); - if (xs2[0].cols() >= 4) { - HomographyFromCorrespondences4PointRobust(xs2[0], xs2[1], - max_error_h, - &H, &inliers, 1e-2); - // TODO(julien) Put this in a function - Vec3 p1; - Vec2 e; - vector all_errors; - all_errors.reserve(inliers.size()); - for (int i = 0; i < inliers.size(); ++i) { - EuclideanToHomogeneous(xs2[0].col(inliers[i]), &p1); - p1 = H * p1; - HomogeneousToEuclidean(p1, &e); - e -= xs2[0].col(inliers[i]); - all_errors.push_back(e.norm()); - } - std::sort(all_errors.begin(), all_errors.end()); - VLOG(1) << "H median:" << all_errors[round(inliers.size()/2)] - << "px.\n"; - (*m)(*image_iter1, *image_iter2) *= all_errors[round(inliers.size()/2)]; - } - } - } -} - -bool AddIndex(int id, vector *id_ordered) { - for (uint i = 0; i < id_ordered->size() ;++i) { - if ((*id_ordered)[i] == id) { - return false; - } - } - id_ordered->push_back(id); - return true; -} - -void RecursivePairwiseHighScoresSearch(Mat &m, - const Vec2i seed, - vector *id_ordered) { - double val_c, val_r; - Vec2i max_c, max_r; - int nothing; - // Set to zero (to avoid to get the same couple) - m(seed[0], seed[1]) = 0; - - // Find the best score for the col - val_c = m.col(seed[1]).maxCoeff(&max_c[0], ¬hing); - max_c[1] = seed[1]; - // Find the best score for the row - val_r = m.row(seed[0]).maxCoeff(¬hing, &max_r[1]); - max_r[0] = seed[0]; - - if (val_c > 0) - m(max_c[0], max_c[1]) = 0; - if (val_r > 0) - m(max_r[0], max_r[1]) = 0; - - if (val_c < val_r) { - if (val_r > 0) { - AddIndex(max_r[1], id_ordered); - RecursivePairwiseHighScoresSearch(m, max_r, id_ordered); - } - if (val_c > 0) { - AddIndex(max_c[0], id_ordered); - RecursivePairwiseHighScoresSearch(m, max_c, id_ordered); - } - } else { - if (val_c > 0) { - AddIndex(max_c[0], id_ordered); - RecursivePairwiseHighScoresSearch(m, max_c, id_ordered); - } - if (val_r > 0){ - AddIndex(max_r[1], id_ordered); - RecursivePairwiseHighScoresSearch(m, max_r, id_ordered); - } - } -} - -void RecoverOrderFromPairwiseHighScores( - const Matches &matches, - Mat &m, - std::list > *connected_graph_list) { - //connected_graph_list->clear(); - std::map map_img_ids; - std::set::const_iterator image_iter1 = - matches.get_images().begin(); - uint i_img = 0; - for (;image_iter1 != matches.get_images().end(); ++image_iter1, ++i_img) { - map_img_ids[i_img] = *image_iter1; - } - - Vec2i max; - double val = 1; - while (val > 0) { - // Find the global best score - val = m.maxCoeff(&max[0], &max[1]); - //From this seed, find the second best score in the same col/row - if (val > 0) { - vector id_ordered; - id_ordered.push_back(max[0]); - id_ordered.push_back(max[1]); - RecursivePairwiseHighScoresSearch(m, max, &id_ordered); - vector v_ids(id_ordered.size()); - for (i_img = 0; i_img < id_ordered.size(); ++i_img) { - v_ids[i_img] = (map_img_ids[id_ordered[i_img]]); - } - connected_graph_list->push_back(v_ids); - } - } -} - -void SelectEfficientImageOrder( - const Matches &matches, - std::list >*images_list) { - Mat m; - FillPairwiseMatchesHomographyMatrix(matches, &m); - VLOG(1) << " M = " << m <<"\n"; - RecoverOrderFromPairwiseHighScores(matches, m, images_list); -} - -void SelectKeyframes( - const Matches &matches, - std::list >*keyframes_list) { - // TODO(julien) It really doesn't work: Finish this! - Mat m; - Mat3 F, H; - double f_err, max_error_f = 1; - double h_err, max_error_h = 1; - double outliers_prob = 1e-2; - uint ni, ne = 0; - vector xs2; - vector inliers; - vector keyframes; - std::set::const_iterator image_iter = - matches.get_images().begin(); - std::set::const_iterator prev_image_iter = image_iter; - ni = matches.NumFeatureImage(*image_iter); - image_iter++; - for (;image_iter != matches.get_images().end(); ++image_iter) { - TwoViewPointMatchMatrices(matches, *prev_image_iter, *image_iter, &xs2); - if (xs2[0].cols() >= 7) { - h_err = HomographyFromCorrespondences4PointRobust(xs2[0], xs2[1], - max_error_h, - &H, &inliers, - outliers_prob); - h_err /= inliers.size(); - f_err = FundamentalFromCorrespondences7PointRobust(xs2[0], xs2[1], - max_error_f, - &F, &inliers, - outliers_prob); - f_err /= inliers.size(); - VLOG(1) << "H error:" << h_err << "px" << std::endl; - VLOG(1) << "F error:" << f_err << "px" << std::endl; - VLOG(1) << "ni:" << ni << " ne:" << ne << std::endl; - // TODO(julien) no sure the ni and ne are the good ones. - // read Pollefeys'03 and Torr98 - ne = inliers.size(); - if (f_err < h_err && ni >= 0.9 * ne) { - VLOG(1) << "Keyframe detected: " << *image_iter << std::endl; - keyframes.push_back(*image_iter); - prev_image_iter = image_iter; - ni = ne; - } - } else { - VLOG(1) << "[Warning] Tracking lost!" << std::endl; - keyframes_list->push_back(keyframes); - keyframes.clear(); - prev_image_iter = image_iter; - ni = ne; - } - } - keyframes_list->push_back(keyframes); -} -} // namespace libmv diff --git a/src/libmv/reconstruction/image_order_selection.h b/src/libmv/reconstruction/image_order_selection.h index f6e129c..e69de29 100644 --- a/src/libmv/reconstruction/image_order_selection.h +++ b/src/libmv/reconstruction/image_order_selection.h @@ -1,43 +0,0 @@ -// Copyright (c) 2010 libmv authors. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#ifndef LIBMV_RECONSTRUCTION_IMAGE_ORDER_SELECTION_H_ -#define LIBMV_RECONSTRUCTION_IMAGE_ORDER_SELECTION_H_ - -#include "libmv/reconstruction/reconstruction.h" - -namespace libmv { - -// This method selects an efficient order of images based on an image -// criterion: median homography error x number of common matches -// The oupout images_list contains a list of connected graphs -// (vectors), each vector contains the ImageID ordered by the criterion. -void SelectEfficientImageOrder( - const Matches &matches, - std::list >*images_list); - -// This method selects only keyframes [Pollefeys'03] -// NOTE It is at least barely started -void SelectKeyframes( - const Matches &matches, - std::list >*keyframes_list); -} // namespace libmv - -#endif // LIBMV_RECONSTRUCTION_IMAGE_ORDER_SELECTION_H_ diff --git a/src/libmv/reconstruction/image_selection.cc b/src/libmv/reconstruction/image_selection.cc new file mode 100644 index 0000000..f57df8f --- /dev/null +++ b/src/libmv/reconstruction/image_selection.cc @@ -0,0 +1,186 @@ +// Copyright (c) 2010 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include +#include + +#include "libmv/multiview/conditioning.h" +#include "libmv/multiview/robust_fundamental.h" +#include "libmv/multiview/robust_homography.h" +#include "libmv/reconstruction/image_selection.h" + +namespace libmv { + +void FillPairwiseMatchesMatrix(const Matches &matches, + Mat *m) { + m->resize(matches.NumImages(), + matches.NumImages()); + m->setZero(); + std::set::const_iterator image_iter1 = + matches.get_images().begin(); + std::set::const_iterator image_iter2; + vector xs2; + for (;image_iter1 != matches.get_images().end(); ++image_iter1) { + image_iter2 = image_iter1; + image_iter2++; + for (;image_iter2 != matches.get_images().end(); ++image_iter2) { + TwoViewPointMatchMatrices(matches, *image_iter1, *image_iter2, &xs2); + (*m)(*image_iter1, *image_iter2) = xs2[0].cols(); + } + } +} + +void FillPairwiseMatchesHomographyMatrix(const Matches &matches, + Mat *m) { + m->resize(matches.NumImages(), + matches.NumImages()); + m->setZero(); + Mat3 H; + vector inliers; + double max_error_h = 1; + std::set::const_iterator image_iter1 = + matches.get_images().begin(); + std::set::const_iterator image_iter2; + vector xs2; + for (;image_iter1 != matches.get_images().end(); ++image_iter1) { + image_iter2 = image_iter1; + image_iter2++; + for (;image_iter2 != matches.get_images().end(); ++image_iter2) { + TwoViewPointMatchMatrices(matches, *image_iter1, *image_iter2, &xs2); + (*m)(*image_iter1, *image_iter2) = xs2[0].cols(); + if (xs2[0].cols() >= 4) { + HomographyFromCorrespondences4PointRobust(xs2[0], xs2[1], + max_error_h, + &H, &inliers, 1e-2); + // TODO(julien) Put this in a function + Vec3 p1; + Vec2 e; + vector all_errors; + all_errors.reserve(inliers.size()); + for (int i = 0; i < inliers.size(); ++i) { + EuclideanToHomogeneous(xs2[0].col(inliers[i]), &p1); + p1 = H * p1; + HomogeneousToEuclidean(p1, &e); + e -= xs2[0].col(inliers[i]); + all_errors.push_back(e.norm()); + } + std::sort(all_errors.begin(), all_errors.end()); + VLOG(1) << "H median:" << all_errors[round(inliers.size()/2)] + << "px.\n"; + (*m)(*image_iter1, *image_iter2) *= all_errors[round(inliers.size()/2)]; + } + } + } +} + +bool AddIndex(int id, vector *id_ordered) { + for (uint i = 0; i < id_ordered->size() ;++i) { + if ((*id_ordered)[i] == id) { + return false; + } + } + id_ordered->push_back(id); + return true; +} + +void RecursivePairwiseHighScoresSearch(Mat &m, + const Vec2i seed, + vector *id_ordered) { + double val_c, val_r; + Vec2i max_c, max_r; + int nothing; + // Set to zero (to avoid to get the same couple) + m(seed[0], seed[1]) = 0; + + // Find the best score for the col + val_c = m.col(seed[1]).maxCoeff(&max_c[0], ¬hing); + max_c[1] = seed[1]; + // Find the best score for the row + val_r = m.row(seed[0]).maxCoeff(¬hing, &max_r[1]); + max_r[0] = seed[0]; + + if (val_c > 0) + m(max_c[0], max_c[1]) = 0; + if (val_r > 0) + m(max_r[0], max_r[1]) = 0; + + if (val_c < val_r) { + if (val_r > 0) { + AddIndex(max_r[1], id_ordered); + RecursivePairwiseHighScoresSearch(m, max_r, id_ordered); + } + if (val_c > 0) { + AddIndex(max_c[0], id_ordered); + RecursivePairwiseHighScoresSearch(m, max_c, id_ordered); + } + } else { + if (val_c > 0) { + AddIndex(max_c[0], id_ordered); + RecursivePairwiseHighScoresSearch(m, max_c, id_ordered); + } + if (val_r > 0){ + AddIndex(max_r[1], id_ordered); + RecursivePairwiseHighScoresSearch(m, max_r, id_ordered); + } + } +} + +void RecoverOrderFromPairwiseHighScores( + const Matches &matches, + Mat &m, + std::list > *connected_graph_list) { + //connected_graph_list->clear(); + std::map map_img_ids; + std::set::const_iterator image_iter1 = + matches.get_images().begin(); + uint i_img = 0; + for (;image_iter1 != matches.get_images().end(); ++image_iter1, ++i_img) { + map_img_ids[i_img] = *image_iter1; + } + + Vec2i max; + double val = 1; + while (val > 0) { + // Find the global best score + val = m.maxCoeff(&max[0], &max[1]); + //From this seed, find the second best score in the same col/row + if (val > 0) { + vector id_ordered; + id_ordered.push_back(max[0]); + id_ordered.push_back(max[1]); + RecursivePairwiseHighScoresSearch(m, max, &id_ordered); + vector v_ids(id_ordered.size()); + for (i_img = 0; i_img < id_ordered.size(); ++i_img) { + v_ids[i_img] = (map_img_ids[id_ordered[i_img]]); + } + connected_graph_list->push_back(v_ids); + } + } +} + +void SelectEfficientImageOrder( + const Matches &matches, + std::list >*images_list) { + Mat m; + FillPairwiseMatchesHomographyMatrix(matches, &m); + VLOG(1) << " M = " << m <<"\n"; + RecoverOrderFromPairwiseHighScores(matches, m, images_list); +} +} // namespace libmv diff --git a/src/libmv/reconstruction/image_selection.h b/src/libmv/reconstruction/image_selection.h new file mode 100644 index 0000000..06c0d5c --- /dev/null +++ b/src/libmv/reconstruction/image_selection.h @@ -0,0 +1,38 @@ +// Copyright (c) 2010 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef LIBMV_RECONSTRUCTION_IMAGE_ORDER_SELECTION_H_ +#define LIBMV_RECONSTRUCTION_IMAGE_ORDER_SELECTION_H_ + +#include "libmv/reconstruction/reconstruction.h" + +namespace libmv { + +// This method selects an efficient order of images based on an image +// criterion: median homography error x number of common matches +// The oupout images_list contains a list of connected graphs +// (vectors), each vector contains the ImageID ordered by the criterion. +void SelectEfficientImageOrder( + const Matches &matches, + std::list >*images_list); + +} // namespace libmv + +#endif // LIBMV_RECONSTRUCTION_IMAGE_ORDER_SELECTION_H_ diff --git a/src/libmv/reconstruction/keyframe_selection.cc b/src/libmv/reconstruction/keyframe_selection.cc new file mode 100644 index 0000000..f8cdd20 --- /dev/null +++ b/src/libmv/reconstruction/keyframe_selection.cc @@ -0,0 +1,131 @@ +// Copyright (c) 2010, 2011 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include +#include + +#include "libmv/logging/logging.h" +#include "libmv/multiview/conditioning.h" +#include "libmv/multiview/robust_fundamental.h" +#include "libmv/multiview/robust_homography.h" +#include "libmv/correspondence/matches.h" +#include "libmv/reconstruction/keyframe_selection.h" + +namespace libmv { + +void SelectKeyframesBasedOnMatchesNumber(const Matches &matches, + vector *keyframes, + float min_matches_pc, + int min_num_matches) { + keyframes->reserve(matches.NumImages()); + std::set::const_iterator image_iter = + matches.get_images().begin(); + Matches::ImageID prev_keyframe = *image_iter; + Matches::ImageID prev_frame_good = *image_iter; + // The first frame is selected as a keyframe + keyframes->push_back(prev_keyframe); + image_iter++; + int num_features_to_keep = min_matches_pc * + matches.NumFeatureImage(prev_keyframe); + num_features_to_keep = std::max(num_features_to_keep, min_num_matches); + VLOG(3) << "# Features to keep: " << num_features_to_keep<< std::endl; + vector two_images; + two_images.push_back(prev_keyframe); + for (;image_iter != matches.get_images().end(); ++image_iter) { + two_images.push_back(*image_iter); + // TODO(julien) make a new function that just count the common tracks! + vector tracks; + TracksInAllImages(matches, two_images, &tracks); + VLOG(3) << "Shared tracks: " << tracks.size() << std::endl; + // If the current frame share not enough common matches with the previous + // keyframe then we select the previous frame + // i.e. the one that has enough common matches + if (tracks.size() < num_features_to_keep && + prev_frame_good != prev_keyframe) { + VLOG(2) << "Keyframe Detected!" << std::endl; + keyframes->push_back(prev_frame_good); + prev_keyframe = prev_frame_good; + num_features_to_keep = min_matches_pc * + matches.NumFeatureImage(prev_keyframe); + num_features_to_keep = std::max(num_features_to_keep, min_num_matches); + VLOG(3) << "# Features to keep: " << num_features_to_keep<< std::endl; + two_images.clear(); + two_images.push_back(prev_keyframe); + } else { + prev_frame_good = *image_iter; + two_images.pop_back(); + } + } +} + +/* + * Not yet finished + * TODO(julien) It really doesn't work: Finish this function! + * */ +void SelectKeyframesBasedOnFAndH( + const Matches &matches, + vector *keyframes) { + keyframes->reserve(matches.NumImages()); + Mat m; + Mat3 F, H; + double f_err, max_error_f = 1; + double h_err, max_error_h = 1; + double outliers_prob = 1e-2; + uint ni, ne = 0; + vector xs2; + vector inliers; + std::set::const_iterator image_iter = + matches.get_images().begin(); + std::set::const_iterator prev_image_iter = image_iter; + ni = matches.NumFeatureImage(*image_iter); + image_iter++; + for (;image_iter != matches.get_images().end(); ++image_iter) { + TwoViewPointMatchMatrices(matches, *prev_image_iter, *image_iter, &xs2); + if (xs2[0].cols() >= 7) { + h_err = HomographyFromCorrespondences4PointRobust(xs2[0], xs2[1], + max_error_h, + &H, &inliers, + outliers_prob); + h_err /= inliers.size(); + f_err = FundamentalFromCorrespondences7PointRobust(xs2[0], xs2[1], + max_error_f, + &F, &inliers, + outliers_prob); + f_err /= inliers.size(); + VLOG(1) << "H error:" << h_err << "px" << std::endl; + VLOG(1) << "F error:" << f_err << "px" << std::endl; + VLOG(1) << "ni:" << ni << " ne:" << ne << std::endl; + // TODO(julien) no sure the ni and ne are the good ones. + // read Pollefeys'03 and Torr98 + ne = inliers.size(); + if (f_err < h_err && ni >= 0.9 * ne) { + VLOG(1) << "Keyframe detected: " << *image_iter << std::endl; + keyframes->push_back(*image_iter); + prev_image_iter = image_iter; + ni = ne; + } + } else { + prev_image_iter = image_iter; + ni = ne; + } + } +} + +} // namespace libmv diff --git a/src/libmv/reconstruction/keyframe_selection.h b/src/libmv/reconstruction/keyframe_selection.h new file mode 100644 index 0000000..865abdf --- /dev/null +++ b/src/libmv/reconstruction/keyframe_selection.h @@ -0,0 +1,43 @@ +// Copyright (c) 2010, 2011 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef LIBMV_RECONSTRUCTION_KEYFRAME_SELECTION_H_ +#define LIBMV_RECONSTRUCTION_KEYFRAME_SELECTION_H_ + +#include "libmv/reconstruction/reconstruction.h" + +namespace libmv { + +// This method selects only 'keyframes' according to the number of shared tracks +// - the first image is selected +// - then an image is selected if the next image has not enough shared tracks +// with the last keyframe +// min_matches_pc represents the minimum percentage of shared tracks between the +// current image and the previous keyframe. If the number of shared tracks drops +// below min_matches_pc *100% of the keyframe total number of tracks or is less +// than min_num_matches, then the previous image is selected as a keyframe. +void SelectKeyframesBasedOnMatchesNumber(const Matches &matches, + vector *keyframes, + float min_matches_pc = 0.15, + int min_num_matches = 50); +} // namespace libmv + +#endif // LIBMV_RECONSTRUCTION_KEYFRAME_SELECTION_H_ + diff --git a/src/libmv/reconstruction/mapping.cc b/src/libmv/reconstruction/mapping.cc index 9b5641e..84c06d2 100644 --- a/src/libmv/reconstruction/mapping.cc +++ b/src/libmv/reconstruction/mapping.cc @@ -1,195 +1,363 @@ -// Copyright (c) 2010 libmv authors. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include "libmv/multiview/conditioning.h" -#include "libmv/multiview/nviewtriangulation.h" -#include "libmv/reconstruction/mapping.h" -#include "libmv/reconstruction/tools.h" - -namespace libmv { - -uint PointStructureTriangulation(const Matches &matches, - CameraID image_id, - size_t minimum_num_views, - Reconstruction *reconstruction, - vector *new_structures_ids) { - // Checks that the camera is in reconstruction - if (!reconstruction->ImageHasCamera(image_id)) { - VLOG(1) << "Error: the image " << image_id - << " has no camera." << std::endl; - return 0; - } - vector structures_ids; - Mat2X x_image; - vector Ps; - vector xs; - Vec2 x; - // Selects only the unreconstructed tracks observed in the image - SelectUnexistingPointStructures(matches, image_id, *reconstruction, - &structures_ids, &x_image); - VLOG(2) << "Points structure selected:" << x_image.cols() << std::endl; - // Computes an isotropic normalization - Mat3 precond; - IsotropicPreconditionerFromPoints(x_image, &precond); - // Selects the point structures that are observed at least in - // minimum_num_views images (images that have an already localized camera) - Mat41 X_world; - uint number_new_structure = 0; - if (new_structures_ids) - new_structures_ids->reserve(structures_ids.size()); - PinholeCamera *camera = NULL; - for (size_t t = 0; t < structures_ids.size(); ++t) { - Matches::Features fp = - matches.InTrack(structures_ids[t]); - Ps.clear(); - xs.clear(); - while (fp) { - camera = dynamic_cast( - reconstruction->GetCamera(fp.image())); - if (camera) { - Ps.push_back(precond * camera->projection_matrix()); - x << fp.feature()->x(), fp.feature()->y(); - xs.push_back(x); - } - fp.operator++(); - } - if (Ps.size() >= minimum_num_views) { - Mat2X x(2, xs.size()); - VectorToMatrix(xs, &x); - Mat xn; - ApplyTransformationToPoints(x, precond, &xn); - // TODO(julien) avoid this copy - x = xn; - NViewTriangulateAlgebraic(x, Ps, &X_world); - bool is_inlier = true; - - if (isnan(X_world.sum())) - is_inlier = false; - - if (X_world(3,0) == 0) - is_inlier = false; - - // TODO(julien) detect and remove outliers - /* - Vec2 q2; double err = 0; - HomogeneousToEuclidean((precond.inverse() * Ps[0] * X_world).col(0), &q2); - Vec3 x3; - x3 << x.col(0), 1; - x3 = precond.inverse() * x3; - err = (x3.block<2,1>(0, 0) - q2).norm(); - VLOG(1) << "Prj:" << err << "px"; - //if (err > 40) - // is_inlier = false; - HomogeneousToEuclidean((precond.inverse() * Ps[1] * X_world).col(0), &q2); - x3 << x.col(1), 1; - x3 = precond.inverse() * x3; - err = (x3.block<2,1>(0, 0) - q2).norm(); - VLOG(1) << " - " << err << "px" << std::endl; - //if (err > 40) - // is_inlier = false;*/ - - if (is_inlier) { - // Creates an add the point structure to the reconstruction - PointStructure * p = new PointStructure(); - p->set_coords(X_world.col(0)); - reconstruction->InsertTrack(structures_ids[t], p); - if (new_structures_ids) - new_structures_ids->push_back(structures_ids[t]); - number_new_structure++; - VLOG(2) << "Add Point Structure [" - << structures_ids[t] <<"] " - << p->coords().transpose() << " (" - << p->coords().transpose() / p->coords()[3] << ")" - << std::endl; - } - } - } - return number_new_structure; -} - -uint PointStructureRetriangulation(const Matches &matches, - CameraID image_id, - Reconstruction *reconstruction) { - // Checks that the camera is in reconstruction - // Checks that the camera is in reconstruction - if (!reconstruction->ImageHasCamera(image_id)) { - VLOG(1) << "Error: the image " << image_id - << " has no camera." << std::endl; - return 0; - } - vector structures_ids; - Mat2X x_image; - vector Ps; - vector xs; - Vec2 x; - // Selects only the reconstructed structures observed in the image - SelectExistingPointStructures(matches, image_id, *reconstruction, - &structures_ids, &x_image); - // Computes an isotropic normalization - Mat3 precond; - IsotropicPreconditionerFromPoints(x_image, &precond); - Mat41 X_world; - uint number_updated_structure = 0; - PinholeCamera *camera = NULL; - PointStructure *pstructure = NULL; - for (size_t t = 0; t < structures_ids.size(); ++t) { - Matches::Features fp = - matches.InTrack(structures_ids[t]); - Ps.clear(); - xs.clear(); - while (fp) { - camera = dynamic_cast( - reconstruction->GetCamera(fp.image())); - if (camera) { - Ps.push_back(camera->projection_matrix()); - x << fp.feature()->x(), fp.feature()->y(); - xs.push_back(x); - } - fp.operator++(); - } - Mat2X x(2, xs.size()); - VectorToMatrix(xs, &x); - Mat xn; - ApplyTransformationToPoints(x, precond, &xn); - // TODO(julien) avoid this copy - x = xn; - NViewTriangulateAlgebraic(x, Ps, &X_world); - bool is_inlier = true; - if (isnan(X_world.sum())) - is_inlier = false; - if (X_world(3,0) == 0) - is_inlier = false; - // TODO(julien) detect and remove outliers - // Creates an add the point structure to the reconstruction - pstructure = dynamic_cast( - reconstruction->GetStructure(structures_ids[t])); - if (is_inlier && pstructure) { - pstructure->set_coords(X_world.col(0)); - number_updated_structure++; - VLOG(2) << "Point structure updated [" - << structures_ids[t] <<"] " - << pstructure->coords().transpose() << " (" - << pstructure->coords().transpose() / pstructure->coords()[3] - << ")" << std::endl; - } - } - return number_updated_structure; -} -} // namespace libmv +// Copyright (c) 2010 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include "libmv/multiview/conditioning.h" +#include "libmv/multiview/nviewtriangulation.h" +#include "libmv/reconstruction/mapping.h" +#include "libmv/reconstruction/tools.h" + +namespace libmv { + +uint PointStructureTriangulationCalibrated( + const Matches &matches, + CameraID image_id, + size_t minimum_num_views, + Reconstruction *reconstruction, + vector *new_structures_ids) { + // Checks that the camera is in reconstruction + if (!reconstruction->ImageHasCamera(image_id)) { + VLOG(1) << "Error: the image " << image_id + << " has no camera." << std::endl; + return 0; + } + vector structures_ids; + Mat2X x_image; + vector Ps; // Contains the projection matrices + vector Rt; // Contains the pose matrices + vector xs; + Vec2 x; + // Selects only the unreconstructed tracks observed in the image + SelectNonReconstructedPointStructures(matches, image_id, *reconstruction, + &structures_ids, &x_image); + VLOG(3) << "Structure points selected:" << x_image.cols() << std::endl; + // Computes an isotropic normalization + Mat3 precond; + IsotropicPreconditionerFromPoints(x_image, &precond); + // Selects the point structures that are observed at least in + // minimum_num_views images (images that have an already localized camera) + Mat41 X_world; + Vec3 X_camera; + uint number_new_structure = 0; + if (new_structures_ids) + new_structures_ids->reserve(structures_ids.size()); + PinholeCamera *camera = NULL; + for (size_t t = 0; t < structures_ids.size(); ++t) { + Matches::Features fp = + matches.InTrack(structures_ids[t]); + Ps.clear(); + Rt.clear(); + xs.clear(); + while (fp) { + camera = dynamic_cast( + reconstruction->GetCamera(fp.image())); + if (camera) { + Ps.push_back(precond * camera->projection_matrix()); + Rt.push_back(camera->GetPoseMatrix()); + x << fp.feature()->x(), fp.feature()->y(); + xs.push_back(x); + } + fp.operator++(); + } + if (Ps.size() >= minimum_num_views) { + Mat2X x(2, xs.size()); + VectorToMatrix(xs, &x); + Mat xn; + ApplyTransformationToPoints(x, precond, &xn); + // TODO(julien) avoid this copy + x = xn; + NViewTriangulateAlgebraic(x, Ps, &X_world); + bool is_inlier = true; + // Let's remove the point if it has NaN values + if (isnan(X_world.sum())) + is_inlier = false; + // Let's remove the point if it is at infinity + if (X_world(3,0) == 0) + is_inlier = false; + + X_world = X_world / X_world(3, 0); + // Let's remove the point if it is reconstructed behind one camera + for (int cam = 0; cam < Rt.size(); ++cam) { + X_camera = (Rt[0] * X_world).col(0); + if (X_camera(2, 0) < 0) { + is_inlier = false; + break; + } + /* TODO(julien) Check the reprojection error? + Vec2 q2; double err = 0; + HomogeneousToEuclidean((precond.inverse() * Ps[0] * X_world).col(0), &q2); + Vec3 x3; + x3 << x.col(0), 1; + x3 = precond.inverse() * x3; + err = (x3.block<2,1>(0, 0) - q2).norm(); + VLOG(1) << "Prj:" << err << "px"; + */ + } + if (is_inlier) { + // Creates an add the point structure to the reconstruction + PointStructure * p = new PointStructure(); + p->set_coords(X_world.col(0)); + reconstruction->InsertTrack(structures_ids[t], p); + if (new_structures_ids) + new_structures_ids->push_back(structures_ids[t]); + number_new_structure++; + VLOG(4) << "Add Point Structure [" + << structures_ids[t] <<"] " + << p->coords().transpose() << " (" + << p->coords().transpose() / p->coords()[3] << ")" + << std::endl; + } + } + } + return number_new_structure; +} + +uint PointStructureRetriangulationCalibrated( + const Matches &matches, + CameraID image_id, + Reconstruction *reconstruction) { + // Checks that the camera is in reconstruction + // Checks that the camera is in reconstruction + if (!reconstruction->ImageHasCamera(image_id)) { + VLOG(1) << "Error: the image " << image_id + << " has no camera." << std::endl; + return 0; + } + vector structures_ids; + Mat2X x_image; + vector Ps; // Contains the projection matrices + vector Rt; // Contains the pose matrices + vector xs; + Vec2 x; + // Selects only the reconstructed structures observed in the image + SelectExistingPointStructures(matches, image_id, *reconstruction, + &structures_ids, &x_image); + // Computes an isotropic normalization + Mat3 precond; + IsotropicPreconditionerFromPoints(x_image, &precond); + Mat41 X_world; + Vec3 X_camera; + uint number_updated_structure = 0; + PinholeCamera *camera = NULL; + PointStructure *pstructure = NULL; + for (size_t t = 0; t < structures_ids.size(); ++t) { + Matches::Features fp = + matches.InTrack(structures_ids[t]); + Ps.clear(); + Rt.clear(); + xs.clear(); + while (fp) { + camera = dynamic_cast( + reconstruction->GetCamera(fp.image())); + if (camera) { + Ps.push_back(camera->projection_matrix()); + x << fp.feature()->x(), fp.feature()->y(); + xs.push_back(x); + } + fp.operator++(); + } + Mat2X x(2, xs.size()); + VectorToMatrix(xs, &x); + Mat xn; + ApplyTransformationToPoints(x, precond, &xn); + // TODO(julien) avoid this copy + x = xn; + NViewTriangulateAlgebraic(x, Ps, &X_world); + bool is_inlier = true; + if (isnan(X_world.sum())) + is_inlier = false; + if (X_world(3,0) == 0) + is_inlier = false; + + X_world = X_world / X_world(3, 0); + // Let's remove the point if it is reconstructed behind one camera + for (int cam = 0; cam < Rt.size(); ++cam) { + X_camera = (Rt[0] * X_world).col(0); + if (X_camera(2, 0) < 0) { + is_inlier = false; + break; + } + /* TODO(julien) Check the reprojection error? + * Vec2 q2; double err = 0; + * HomogeneousToEuclidean((precond.inverse() * Ps[0] * X_world).col(0), &q2); + * Vec3 x3; + * x3 << x.col(0), 1; + * x3 = precond.inverse() * x3; + * err = (x3.block<2,1>(0, 0) - q2).norm(); + * VLOG(1) << "Prj:" << err << "px"; + */ + } + // Creates an add the point structure to the reconstruction + pstructure = dynamic_cast( + reconstruction->GetStructure(structures_ids[t])); + if (is_inlier && pstructure) { + pstructure->set_coords(X_world.col(0)); + number_updated_structure++; + VLOG(4) << "Point structure updated [" + << structures_ids[t] <<"] " + << pstructure->coords().transpose() << " (" + << pstructure->coords().transpose() / pstructure->coords()[3] + << ")" << std::endl; + } + } + return number_updated_structure; +} + +uint PointStructureTriangulationUncalibrated( + const Matches &matches, + CameraID image_id, + size_t minimum_num_views, + Reconstruction *reconstruction, + vector *new_structures_ids) { + // Checks that the camera is in reconstruction + if (!reconstruction->ImageHasCamera(image_id)) { + VLOG(1) << "Error: the image " << image_id + << " has no camera." << std::endl; + return 0; + } + vector structures_ids; + Mat2X x_image; + vector Ps; // Contains the projection matrices + vector xs; + Vec2 x; + // Selects only the unreconstructed tracks observed in the image + SelectNonReconstructedPointStructures(matches, image_id, *reconstruction, + &structures_ids, &x_image); + VLOG(3) << "Structure points selected:" << x_image.cols() << std::endl; + // Computes an isotropic normalization + Mat3 precond; + IsotropicPreconditionerFromPoints(x_image, &precond); + // Selects the point structures that are observed at least in + // minimum_num_views images (images that have an already localized camera) + Mat41 X_world; + uint number_new_structure = 0; + if (new_structures_ids) + new_structures_ids->reserve(structures_ids.size()); + PinholeCamera *camera = NULL; + for (size_t t = 0; t < structures_ids.size(); ++t) { + Matches::Features fp = + matches.InTrack(structures_ids[t]); + Ps.clear(); + xs.clear(); + while (fp) { + camera = dynamic_cast( + reconstruction->GetCamera(fp.image())); + if (camera) { + Ps.push_back(precond * camera->projection_matrix()); + x << fp.feature()->x(), fp.feature()->y(); + xs.push_back(x); + } + fp.operator++(); + } + if (Ps.size() >= minimum_num_views) { + Mat2X x(2, xs.size()); + VectorToMatrix(xs, &x); + Mat xn; + ApplyTransformationToPoints(x, precond, &xn); + // TODO(julien) avoid this copy + x = xn; + NViewTriangulateAlgebraic(x, Ps, &X_world); + bool is_inlier = true; + // Let's remove the point if it has NaN values + if (isnan(X_world.sum())) + is_inlier = false; + if (is_inlier) { + // Creates an add the point structure to the reconstruction + PointStructure * p = new PointStructure(); + p->set_coords(X_world.col(0)); + reconstruction->InsertTrack(structures_ids[t], p); + if (new_structures_ids) + new_structures_ids->push_back(structures_ids[t]); + number_new_structure++; + VLOG(4) << "Add Point Structure [" + << structures_ids[t] <<"] " + << p->coords().transpose() + << std::endl; + } + } + } + return number_new_structure; +} + +uint PointStructureRetriangulationUncalibrated( + const Matches &matches, + CameraID image_id, + Reconstruction *reconstruction) { + // Checks that the camera is in reconstruction + // Checks that the camera is in reconstruction + if (!reconstruction->ImageHasCamera(image_id)) { + VLOG(1) << "Error: the image " << image_id + << " has no camera." << std::endl; + return 0; + } + vector structures_ids; + Mat2X x_image; + vector Ps; + vector xs; + Vec2 x; + // Selects only the reconstructed structures observed in the image + SelectExistingPointStructures(matches, image_id, *reconstruction, + &structures_ids, &x_image); + // Computes an isotropic normalization + Mat3 precond; + IsotropicPreconditionerFromPoints(x_image, &precond); + Mat41 X_world; + uint number_updated_structure = 0; + PinholeCamera *camera = NULL; + PointStructure *pstructure = NULL; + for (size_t t = 0; t < structures_ids.size(); ++t) { + Matches::Features fp = + matches.InTrack(structures_ids[t]); + Ps.clear(); + xs.clear(); + while (fp) { + camera = dynamic_cast( + reconstruction->GetCamera(fp.image())); + if (camera) { + Ps.push_back(camera->projection_matrix()); + x << fp.feature()->x(), fp.feature()->y(); + xs.push_back(x); + } + fp.operator++(); + } + Mat2X x(2, xs.size()); + VectorToMatrix(xs, &x); + Mat xn; + ApplyTransformationToPoints(x, precond, &xn); + // TODO(julien) avoid this copy + x = xn; + NViewTriangulateAlgebraic(x, Ps, &X_world); + bool is_inlier = true; + if (isnan(X_world.sum())) + is_inlier = false; + // Creates an add the point structure to the reconstruction + pstructure = dynamic_cast( + reconstruction->GetStructure(structures_ids[t])); + if (is_inlier && pstructure) { + pstructure->set_coords(X_world.col(0)); + number_updated_structure++; + VLOG(4) << "Point structure updated [" + << structures_ids[t] <<"] " + << pstructure->coords().transpose() + << std::endl; + } + } + return number_updated_structure; +} +} // namespace libmv diff --git a/src/libmv/reconstruction/mapping.h b/src/libmv/reconstruction/mapping.h index 7e7fca4..9bbaa23 100644 --- a/src/libmv/reconstruction/mapping.h +++ b/src/libmv/reconstruction/mapping.h @@ -26,17 +26,17 @@ namespace libmv { // Reconstructs unreconstructed point tracks observed in the image image_id -// using theirs observations (matches). +// using theirs observations (matches) when the instrinsic parameters are known. // To be reconstructed, the tracks need to be viewed in more than // minimum_num_views images. // The method: // selects the tracks that haven't been already reconstructed // reconstructs the tracks into structures -// TODO(julien) only add inliers? +// remove outliers (points behind one camera or at infinity) // creates and add them in reconstruction // Returns the number of structures reconstructed and the list of triangulated // points -uint PointStructureTriangulation( +uint PointStructureTriangulationCalibrated( const Matches &matches, CameraID image_id, size_t minimum_num_views, @@ -44,16 +44,52 @@ uint PointStructureTriangulation( vector *new_structures_ids = NULL); // Retriangulates point tracks observed in the image image_id using theirs -// observations (matches). To be reconstructed, the tracks need to be viewed -// in more than minimum_num_views images. +// observations (matches) when the instrinsic parameters are known. +// To be reconstructed, the tracks need to be viewed in more than +// minimum_num_views images. // The method: // selects the tracks that have been already reconstructed // reconstructs the tracks into structures -// TODO(julien) only add inliers? +// remove outliers (points behind one camera or at infinity) +// updates the coordinates in the reconstruction // Returns the number of structures retriangulated -uint PointStructureRetriangulation(const Matches &matches, - CameraID image_id, - Reconstruction *reconstruction); +uint PointStructureRetriangulationCalibrated( + const Matches &matches, + CameraID image_id, + Reconstruction *reconstruction); + +// Reconstructs unreconstructed point tracks observed in the image image_id +// using theirs observations (matches) when the instrinsic param. are unknown. +// To be reconstructed, the tracks need to be viewed in more than +// minimum_num_views images. +// The method: +// selects the tracks that haven't been already reconstructed +// reconstructs the tracks into structures +// remove outliers (contains NaN coord) TODO(julien) do other tests (rms?) +// creates and add them in reconstruction +// Returns the number of structures reconstructed and the list of triangulated +// points +uint PointStructureTriangulationUncalibrated( + const Matches &matches, + CameraID image_id, + size_t minimum_num_views, + Reconstruction *reconstruction, + vector *new_structures_ids = NULL); + +// Retriangulates point tracks observed in the image image_id using theirs +// observations (matches) when the instrinsic parameters are unknown. +// To be reconstructed, the tracks need to be viewed in more than +// minimum_num_views images. +// The method: +// selects the tracks that have been already reconstructed +// reconstructs the tracks into structures +// remove outliers (contains NaN coord) TODO(julien) do other tests (rms?) +// updates the coordinates in the reconstruction +// Returns the number of structures retriangulated +uint PointStructureRetriangulationUncalibrated( + const Matches &matches, + CameraID image_id, + Reconstruction *reconstruction); } // namespace libmv #endif // LIBMV_RECONSTRUCTION_MAPPING_H_ diff --git a/src/libmv/reconstruction/optimization.cc b/src/libmv/reconstruction/optimization.cc index 4c2a1f8..97e8fa1 100644 --- a/src/libmv/reconstruction/optimization.cc +++ b/src/libmv/reconstruction/optimization.cc @@ -1,213 +1,213 @@ -// Copyright (c) 2010 libmv authors. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include "libmv/multiview/bundle.h" -#include "libmv/reconstruction/optimization.h" -#include "libmv/reconstruction/tools.h" - -namespace libmv { - -double EstimateRootMeanSquareError(const Matches &matches, - Reconstruction *reconstruction) { - PinholeCamera * pcamera = NULL; - vector structures_ids; - Mat2X x_image; - Mat4X X_world; - double sum_rms2 = 0; - size_t num_features = 0; - std::map::iterator stucture_iter; - std::map::iterator camera_iter = - reconstruction->cameras().begin(); - for (; camera_iter != reconstruction->cameras().end(); ++camera_iter) { - pcamera = dynamic_cast(camera_iter->second); - if (pcamera) { - SelectExistingPointStructures(matches, camera_iter->first, - *reconstruction, - &structures_ids, - &x_image); - MatrixOfPointStructureCoordinates(structures_ids, - *reconstruction, - &X_world); - Mat2X dx =Project(pcamera->projection_matrix(), X_world) - x_image; - VLOG(1) << "|Err Cam "<first<<"| = " - << sqrt(Square(dx.norm()) / x_image.cols()) << " (" - << x_image.cols() << " pts)" << std::endl; - // TODO(julien) use normSquare - sum_rms2 += Square(dx.norm()); - num_features += x_image.cols(); - } - } - // TODO(julien) devide by total number of features - return sqrt(sum_rms2 / num_features); -} - -double MetricBundleAdjust(const Matches &matches, - Reconstruction *reconstruction) { - double rms = 0, rms0 = EstimateRootMeanSquareError(matches, reconstruction); - VLOG(1) << "Initial RMS = " << rms0 << std::endl; - size_t ncamera = reconstruction->GetNumberCameras(); - size_t nstructure = reconstruction->GetNumberStructures(); - vector x(ncamera); - vector x_ids(ncamera); - vector Ks(ncamera); - vector Rs(ncamera); - vector ts(ncamera); - Mat3X X(3, nstructure); - vector structures_ids; - std::map map_structures_ids; - - size_t str_id = 0; - PointStructure *pstructure = NULL; - std::map::iterator str_iter = - reconstruction->structures().begin(); - for (; str_iter != reconstruction->structures().end(); ++str_iter) { - pstructure = dynamic_cast(str_iter->second); - if (pstructure) { - X.col(str_id) = pstructure->coords_affine(); - map_structures_ids[str_iter->first] = str_id; - str_id++; - } else { - LOG(FATAL) << "Error: the bundle adjustment cannot handle non point " - << "structure."; - return 0; - } - } - - PinholeCamera * pcamera = NULL; - size_t cam_id = 0; - std::map::iterator cam_iter = - reconstruction->cameras().begin(); - for (; cam_iter != reconstruction->cameras().end(); ++cam_iter) { - pcamera = dynamic_cast(cam_iter->second); - if (pcamera) { - pcamera->GetIntrinsicExtrinsicParameters(&Ks[cam_id], - &Rs[cam_id], - &ts[cam_id]); - SelectExistingPointStructures(matches, cam_iter->first, - *reconstruction, - &structures_ids, &x[cam_id]); - x_ids[cam_id].resize(structures_ids.size()); - for (size_t s = 0; s < structures_ids.size(); ++s) { - x_ids[cam_id][s] = map_structures_ids[structures_ids[s]]; - } - //VLOG(1) << "x_ids = " << x_ids[cam_id].transpose()<<"\n"; - cam_id++; - } else { - LOG(FATAL) << "Error: the bundle adjustment cannot handle non pinhole " - << "cameras."; - return 0; - } - } - // Performs metric bundle adjustment - rms = EuclideanBA(x, x_ids, &Ks, &Rs, &ts, &X, eBUNDLE_METRIC); - // Copy the results only if it's better - if (rms < rms0) { - cam_id = 0; - cam_iter = reconstruction->cameras().begin(); - for (; cam_iter != reconstruction->cameras().end(); ++cam_iter) { - pcamera = dynamic_cast(cam_iter->second); - if (pcamera) { - pcamera->SetIntrinsicExtrinsicParameters(Ks[cam_id], - Rs[cam_id], - ts[cam_id]); - cam_id++; - } - } - str_id = 0; - str_iter = reconstruction->structures().begin(); - for (; str_iter != reconstruction->structures().end(); ++str_iter) { - pstructure = dynamic_cast(str_iter->second); - if (pstructure) { - pstructure->set_coords_affine(X.col(str_id)); - str_id++; - } - } - } - //rms = EstimateRootMeanSquareError(matches, reconstruction); - VLOG(1) << "Final RMS = " << rms << std::endl; - return rms; -} - -uint RemoveOutliers(CameraID image_id, - Matches *matches, - Reconstruction *reconstruction, - double rmse_threshold) { - // TODO(julien) finish it ! - // Checks that the camera is in reconstruction - if (!reconstruction->ImageHasCamera(image_id)) { - return 0; - } - vector structures_ids; - // Selects only the reconstructed structures observed in the image - SelectExistingPointStructures(*matches, image_id, *reconstruction, - &structures_ids, NULL); - Vec2 q, q2; - uint number_outliers = 0; - uint num_views = 0; - bool current_point_removed = false; - PinholeCamera *camera = NULL; - PointStructure *pstructure = NULL; - double err = 0; - for (size_t t = 0; t < structures_ids.size(); ++t) { - pstructure = dynamic_cast( - reconstruction->GetStructure(structures_ids[t])); - if (pstructure) { - Matches::Features fp = - matches->InTrack(structures_ids[t]); - current_point_removed = false; - while (fp) { - camera = dynamic_cast( - reconstruction->GetCamera(fp.image())); - if (camera) { - q << fp.feature()->x(), fp.feature()->y(); - camera->ProjectPointStructure(*pstructure, &q2); - err = (q - q2).norm(); - if (err > rmse_threshold) { - matches->Remove(fp.image(), structures_ids[t]); - if (!current_point_removed) - number_outliers++; - current_point_removed = true; - } - } - fp.operator++(); - } - // TODO(julien) put the check into a function - // Check if a point has enough views (with pinhole cameras) - fp = matches->InTrack(structures_ids[t]); - num_views = 0; - while (fp) { - camera = dynamic_cast( - reconstruction->GetCamera(fp.image())); - if (camera) { - num_views++; - } - fp.operator++(); - } - if (num_views < 2) { - //Delete the point - reconstruction->RemoveTrack(structures_ids[t]); - } - // TODO(julien) also check if a camera has enough points (at least 2) - } - } - std::cout << "#outliers: " << number_outliers << std::endl; - return number_outliers; -} -} // namespace libmv +// Copyright (c) 2010 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include "libmv/multiview/bundle.h" +#include "libmv/reconstruction/optimization.h" +#include "libmv/reconstruction/tools.h" + +namespace libmv { + +double EstimateRootMeanSquareError(const Matches &matches, + Reconstruction *reconstruction) { + PinholeCamera * pcamera = NULL; + vector structures_ids; + Mat2X x_image; + Mat4X X_world; + double sum_rms2 = 0; + size_t num_features = 0; + std::map::iterator stucture_iter; + std::map::iterator camera_iter = + reconstruction->cameras().begin(); + for (; camera_iter != reconstruction->cameras().end(); ++camera_iter) { + pcamera = dynamic_cast(camera_iter->second); + if (pcamera) { + SelectExistingPointStructures(matches, camera_iter->first, + *reconstruction, + &structures_ids, + &x_image); + MatrixOfPointStructureCoordinates(structures_ids, + *reconstruction, + &X_world); + Mat2X dx =Project(pcamera->projection_matrix(), X_world) - x_image; + VLOG(1) << "|Err Cam "<first<<"| = " + << sqrt(Square(dx.norm()) / x_image.cols()) << " (" + << x_image.cols() << " pts)" << std::endl; + // TODO(julien) use normSquare + sum_rms2 += Square(dx.norm()); + num_features += x_image.cols(); + } + } + // TODO(julien) devide by total number of features + return sqrt(sum_rms2 / num_features); +} + +double MetricBundleAdjust(const Matches &matches, + Reconstruction *reconstruction) { + double rms = 0, rms0 = EstimateRootMeanSquareError(matches, reconstruction); + VLOG(1) << "Initial RMS = " << rms0 << std::endl; + size_t ncamera = reconstruction->GetNumberCameras(); + size_t nstructure = reconstruction->GetNumberStructures(); + vector x(ncamera); + vector x_ids(ncamera); + vector Ks(ncamera); + vector Rs(ncamera); + vector ts(ncamera); + Mat3X X(3, nstructure); + vector structures_ids; + std::map map_structures_ids; + + size_t str_id = 0; + PointStructure *pstructure = NULL; + std::map::iterator str_iter = + reconstruction->structures().begin(); + for (; str_iter != reconstruction->structures().end(); ++str_iter) { + pstructure = dynamic_cast(str_iter->second); + if (pstructure) { + X.col(str_id) = pstructure->coords_affine(); + map_structures_ids[str_iter->first] = str_id; + str_id++; + } else { + LOG(FATAL) << "Error: the bundle adjustment cannot handle non point " + << "structure."; + return 0; + } + } + + PinholeCamera * pcamera = NULL; + size_t cam_id = 0; + std::map::iterator cam_iter = + reconstruction->cameras().begin(); + for (; cam_iter != reconstruction->cameras().end(); ++cam_iter) { + pcamera = dynamic_cast(cam_iter->second); + if (pcamera) { + pcamera->GetIntrinsicExtrinsicParameters(&Ks[cam_id], + &Rs[cam_id], + &ts[cam_id]); + SelectExistingPointStructures(matches, cam_iter->first, + *reconstruction, + &structures_ids, &x[cam_id]); + x_ids[cam_id].resize(structures_ids.size()); + for (size_t s = 0; s < structures_ids.size(); ++s) { + x_ids[cam_id][s] = map_structures_ids[structures_ids[s]]; + } + //VLOG(1) << "x_ids = " << x_ids[cam_id].transpose()<<"\n"; + cam_id++; + } else { + LOG(FATAL) << "Error: the bundle adjustment cannot handle non pinhole " + << "cameras."; + return 0; + } + } + // Performs metric bundle adjustment + rms = EuclideanBA(x, x_ids, &Ks, &Rs, &ts, &X, eBUNDLE_METRIC); + // Copy the results only if it's better + if (rms < rms0) { + cam_id = 0; + cam_iter = reconstruction->cameras().begin(); + for (; cam_iter != reconstruction->cameras().end(); ++cam_iter) { + pcamera = dynamic_cast(cam_iter->second); + if (pcamera) { + pcamera->SetIntrinsicExtrinsicParameters(Ks[cam_id], + Rs[cam_id], + ts[cam_id]); + cam_id++; + } + } + str_id = 0; + str_iter = reconstruction->structures().begin(); + for (; str_iter != reconstruction->structures().end(); ++str_iter) { + pstructure = dynamic_cast(str_iter->second); + if (pstructure) { + pstructure->set_coords_affine(X.col(str_id)); + str_id++; + } + } + } + //rms = EstimateRootMeanSquareError(matches, reconstruction); + VLOG(1) << "Final RMS = " << rms << std::endl; + return rms; +} + +uint RemoveOutliers(CameraID image_id, + Matches *matches, + Reconstruction *reconstruction, + double rmse_threshold) { + // TODO(julien) finish it ! + // Checks that the camera is in reconstruction + if (!reconstruction->ImageHasCamera(image_id)) { + return 0; + } + vector structures_ids; + // Selects only the reconstructed structures observed in the image + SelectExistingPointStructures(*matches, image_id, *reconstruction, + &structures_ids, NULL); + Vec2 q, q2; + uint number_outliers = 0; + uint num_views = 0; + bool current_point_removed = false; + PinholeCamera *camera = NULL; + PointStructure *pstructure = NULL; + double err = 0; + for (size_t t = 0; t < structures_ids.size(); ++t) { + pstructure = dynamic_cast( + reconstruction->GetStructure(structures_ids[t])); + if (pstructure) { + Matches::Features fp = + matches->InTrack(structures_ids[t]); + current_point_removed = false; + while (fp) { + camera = dynamic_cast( + reconstruction->GetCamera(fp.image())); + if (camera) { + q << fp.feature()->x(), fp.feature()->y(); + camera->ProjectPointStructure(*pstructure, &q2); + err = (q - q2).norm(); + if (err > rmse_threshold) { + matches->Remove(fp.image(), structures_ids[t]); + if (!current_point_removed) + number_outliers++; + current_point_removed = true; + } + } + fp.operator++(); + } + // TODO(julien) put the check into a function + // Check if a point has enough views (with pinhole cameras) + fp = matches->InTrack(structures_ids[t]); + num_views = 0; + while (fp) { + camera = dynamic_cast( + reconstruction->GetCamera(fp.image())); + if (camera) { + num_views++; + } + fp.operator++(); + } + if (num_views < 2) { + //Delete the point + reconstruction->RemoveTrack(structures_ids[t]); + } + // TODO(julien) also check if a camera has enough points (at least 2) + } + } + std::cout << "#outliers: " << number_outliers << std::endl; + return number_outliers; +} +} // namespace libmv diff --git a/src/libmv/reconstruction/projective_reconstruction.cc b/src/libmv/reconstruction/projective_reconstruction.cc index b60e696..584734f 100644 --- a/src/libmv/reconstruction/projective_reconstruction.cc +++ b/src/libmv/reconstruction/projective_reconstruction.cc @@ -1,213 +1,213 @@ -// Copyright (c) 2010 libmv authors. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include "libmv/base/vector_utils.h" -#include "libmv/camera/pinhole_camera.h" -#include "libmv/multiview/autocalibration.h" -#include "libmv/multiview/fundamental.h" -#include "libmv/multiview/robust_fundamental.h" -#include "libmv/multiview/robust_resection.h" -#include "libmv/reconstruction/mapping.h" -#include "libmv/reconstruction/optimization.h" -#include "libmv/reconstruction/tools.h" -#include "libmv/reconstruction/projective_reconstruction.h" - -namespace libmv { - -bool ReconstructFromTwoUncalibratedViews(const Matches &matches, - CameraID image_id1, - CameraID image_id2, - Matches *matches_inliers, - Reconstruction *reconstruction) { - double epipolar_threshold = 1;// in pixels - vector xs(2); - vector tracks; - vector images; - images.push_back(image_id1); - images.push_back(image_id2); - PointMatchMatrices(matches, images, &tracks, &xs); - // TODO(julien) Also remove structures that are on the same location - if (xs[0].cols() < 7) { - LOG(ERROR) << "Error: there are not enough common matches (" - << xs[0].cols()<< "<7)."; - return false; - } - - Mat &x0 = xs[0]; - Mat &x1 = xs[1]; - vector feature_inliers; - Mat3 F; - // Computes fundamental matrix - // TODO(julien) For the calibrated case, we can squeeze the fundamental using - // directly the 5 points algorithm - FundamentalFromCorrespondences7PointRobust(x0, x1, - epipolar_threshold, - &F, &feature_inliers, - 1e-3); - Mat34 P1; - Mat34 P2; - P1<< Mat3::Identity(), Vec3::Zero(); - PinholeCamera * pcamera = NULL; - pcamera = dynamic_cast( - reconstruction->GetCamera(image_id1)); - // If the first image has no associated camera, we choose the center of the - // coordinate frame - if (!pcamera) { - pcamera = new PinholeCamera(P1); - reconstruction->InsertCamera(image_id1, pcamera); - VLOG(1) << "Add Camera [" - << image_id1 <<"]"<< std::endl <<"P=" - << P1 << std::endl; - } else { - // TODO(julien) what should we do? - // for now we enforce the first projection matrix to be the world reference - VLOG(1) << "Warning: the first projection matrix is overwritten to be the" - << " world reference frame."; - pcamera->set_projection_matrix(P1); - } - // Recover the second projection matrix - ProjectionsFromFundamental(F, &P1, &P2); - // Creates and adds the second camera - pcamera = new PinholeCamera(P2); - reconstruction->InsertCamera(image_id2, pcamera); - VLOG(1) << "Add Camera [" - << image_id2 <<"]"<< std::endl <<"P=" - << P2 << std::endl; - - //Adds only inliers matches into - const Feature * feature = NULL; - for (size_t s = 0; s < feature_inliers.size(); ++s) { - feature = matches.Get(image_id1, tracks[feature_inliers[s]]); - matches_inliers->Insert(image_id1, tracks[feature_inliers[s]], feature); - feature = matches.Get(image_id2, tracks[feature_inliers[s]]); - matches_inliers->Insert(image_id2, tracks[feature_inliers[s]], feature); - } - VLOG(1) << "Inliers added: " << feature_inliers.size() << std::endl; - return true; -} - -bool UncalibratedCameraResection(const Matches &matches, - CameraID image_id, - Matches *matches_inliers, - Reconstruction *reconstruction) { - double rms_inliers_threshold = 1;// in pixels - vector structures_ids; - Mat2X x_image; - Mat4X X_world; - // Selects only the reconstructed tracks observed in the image - SelectExistingPointStructures(matches, image_id, *reconstruction, - &structures_ids, &x_image); - - // TODO(julien) Also remove structures that are on the same location - if (structures_ids.size() < 6) { - LOG(ERROR) << "Error: there are not enough points to estimate the " - << "projection matrix(" << structures_ids.size() << "<6)."; - // We need at least 6 tracks in order to do resection - return false; - } - - MatrixOfPointStructureCoordinates(structures_ids, *reconstruction, &X_world); - CHECK(x_image.cols() == X_world.cols()); - - Mat34 P; - vector inliers; - ResectionRobust(x_image, X_world, rms_inliers_threshold, &P,&inliers, 1e-3); - - // TODO(julien) Performs non-linear optimization of the pose. - - // Creates a new camera and add it to the reconstruction - PinholeCamera * camera = new PinholeCamera(P); - reconstruction->InsertCamera(image_id, camera); - - VLOG(1) << "Add Camera [" - << image_id <<"]"<< std::endl <<"P=" - << P << std::endl; - //Adds only inliers matches into - const Feature * feature = NULL; - for (size_t s = 0; s < structures_ids.size(); ++s) { - feature = matches.Get(image_id, structures_ids[s]); - matches_inliers->Insert(image_id, structures_ids[s], feature); - } - VLOG(1) << "Inliers added: " << structures_ids.size() << std::endl; - return true; -} - -bool UpgradeToMetric(const Matches &matches, - Reconstruction *reconstruction) { - double rms = EstimateRootMeanSquareError(matches, reconstruction); - VLOG(1) << "Upgrade to Metric - Initial RMS:" << rms << std::endl; - AutoCalibrationLinear auto_calibration_linear; - uint image_width = 0; - uint image_height = 0; - PinholeCamera * pcamera = NULL; - std::map::iterator camera_iter = - reconstruction->cameras().begin(); - for (; camera_iter != reconstruction->cameras().end(); ++camera_iter) { - pcamera = dynamic_cast(camera_iter->second); - if (pcamera) { - image_width = pcamera->image_width(); - image_height = pcamera->image_height(); - // Avoid to have null values of image width and height - // TODO(julien) prefer an assert? - if (!image_width && !image_height) { - image_width = 640; - image_height = 480; - } - auto_calibration_linear.AddProjection(pcamera->projection_matrix(), - image_width, image_height); - } - } - // TODO(julien) Put the following in a function. - // Upgrade the reconstruction to metric using {Pm, Xm} = {P*H, H^{-1}*X} - Mat4 H = auto_calibration_linear.MetricTransformation(); - VLOG(1) << "Rectification H = " << H << "\n"; - if (isnan(H.sum())) { - LOG(ERROR) << "Warning: The metric rectification cannot be applied, the " - << "matrix contains NaN values.\n"; - return false; - } - Mat34 P; - camera_iter = reconstruction->cameras().begin(); - for (; camera_iter != reconstruction->cameras().end(); ++camera_iter) { - pcamera = dynamic_cast(camera_iter->second); - if (pcamera) { - P = pcamera->projection_matrix() * H; - pcamera->set_projection_matrix(P); - P_From_KRt(pcamera->intrinsic_matrix(), - pcamera->orientation_matrix(), - pcamera->position(), &P); - // TODO(julien) change this. - pcamera->set_projection_matrix(P); - } - } - Mat4 H_inverse = H.inverse(); - PointStructure * pstructure = NULL; - std::map::iterator stucture_iter = - reconstruction->structures().begin(); - for (; stucture_iter != reconstruction->structures().end(); ++stucture_iter) { - pstructure = dynamic_cast(stucture_iter->second); - if (pstructure) { - pstructure->set_coords(H_inverse * pstructure->coords()); - } - } - MetricBundleAdjust(matches, reconstruction); - return true; -} -} // namespace libmv +// Copyright (c) 2010 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include "libmv/base/vector_utils.h" +#include "libmv/camera/pinhole_camera.h" +#include "libmv/multiview/autocalibration.h" +#include "libmv/multiview/fundamental.h" +#include "libmv/multiview/robust_fundamental.h" +#include "libmv/multiview/robust_resection.h" +#include "libmv/reconstruction/mapping.h" +#include "libmv/reconstruction/optimization.h" +#include "libmv/reconstruction/tools.h" +#include "libmv/reconstruction/projective_reconstruction.h" + +namespace libmv { + +bool ReconstructFromTwoUncalibratedViews(const Matches &matches, + CameraID image_id1, + CameraID image_id2, + Matches *matches_inliers, + Reconstruction *reconstruction) { + double epipolar_threshold = 1;// in pixels + vector xs(2); + vector tracks; + vector images; + images.push_back(image_id1); + images.push_back(image_id2); + PointMatchMatrices(matches, images, &tracks, &xs); + // TODO(julien) Also remove structures that are on the same location + if (xs[0].cols() < 7) { + LOG(ERROR) << "Error: there are not enough common matches (" + << xs[0].cols()<< "<7)."; + return false; + } + + Mat &x0 = xs[0]; + Mat &x1 = xs[1]; + vector feature_inliers; + Mat3 F; + // Computes fundamental matrix + // TODO(julien) For the calibrated case, we can squeeze the fundamental using + // directly the 5 points algorithm + FundamentalFromCorrespondences7PointRobust(x0, x1, + epipolar_threshold, + &F, &feature_inliers, + 1e-3); + Mat34 P1; + Mat34 P2; + P1<< Mat3::Identity(), Vec3::Zero(); + PinholeCamera * pcamera = NULL; + pcamera = dynamic_cast( + reconstruction->GetCamera(image_id1)); + // If the first image has no associated camera, we choose the center of the + // coordinate frame + if (!pcamera) { + pcamera = new PinholeCamera(P1); + reconstruction->InsertCamera(image_id1, pcamera); + VLOG(1) << "Add Camera [" + << image_id1 <<"]"<< std::endl <<"P=" + << P1 << std::endl; + } else { + // TODO(julien) what should we do? + // for now we enforce the first projection matrix to be the world reference + VLOG(1) << "Warning: the first projection matrix is overwritten to be the" + << " world reference frame."; + pcamera->set_projection_matrix(P1); + } + // Recover the second projection matrix + ProjectionsFromFundamental(F, &P1, &P2); + // Creates and adds the second camera + pcamera = new PinholeCamera(P2); + reconstruction->InsertCamera(image_id2, pcamera); + VLOG(1) << "Add Camera [" + << image_id2 <<"]"<< std::endl <<"P=" + << P2 << std::endl; + + //Adds only inliers matches into + const Feature * feature = NULL; + for (size_t s = 0; s < feature_inliers.size(); ++s) { + feature = matches.Get(image_id1, tracks[feature_inliers[s]]); + matches_inliers->Insert(image_id1, tracks[feature_inliers[s]], feature); + feature = matches.Get(image_id2, tracks[feature_inliers[s]]); + matches_inliers->Insert(image_id2, tracks[feature_inliers[s]], feature); + } + VLOG(1) << "Inliers added: " << feature_inliers.size() << std::endl; + return true; +} + +bool UncalibratedCameraResection(const Matches &matches, + CameraID image_id, + Matches *matches_inliers, + Reconstruction *reconstruction) { + double rms_inliers_threshold = 1;// in pixels + vector structures_ids; + Mat2X x_image; + Mat4X X_world; + // Selects only the reconstructed tracks observed in the image + SelectExistingPointStructures(matches, image_id, *reconstruction, + &structures_ids, &x_image); + + // TODO(julien) Also remove structures that are on the same location + if (structures_ids.size() < 6) { + LOG(ERROR) << "Error: there are not enough points to estimate the " + << "projection matrix(" << structures_ids.size() << "<6)."; + // We need at least 6 tracks in order to do resection + return false; + } + + MatrixOfPointStructureCoordinates(structures_ids, *reconstruction, &X_world); + CHECK(x_image.cols() == X_world.cols()); + + Mat34 P; + vector inliers; + ResectionRobust(x_image, X_world, rms_inliers_threshold, &P,&inliers, 1e-3); + + // TODO(julien) Performs non-linear optimization of the pose. + + // Creates a new camera and add it to the reconstruction + PinholeCamera * camera = new PinholeCamera(P); + reconstruction->InsertCamera(image_id, camera); + + VLOG(1) << "Add Camera [" + << image_id <<"]"<< std::endl <<"P=" + << P << std::endl; + //Adds only inliers matches into + const Feature * feature = NULL; + for (size_t s = 0; s < structures_ids.size(); ++s) { + feature = matches.Get(image_id, structures_ids[s]); + matches_inliers->Insert(image_id, structures_ids[s], feature); + } + VLOG(1) << "Inliers added: " << structures_ids.size() << std::endl; + return true; +} + +bool UpgradeToMetric(const Matches &matches, + Reconstruction *reconstruction) { + double rms = EstimateRootMeanSquareError(matches, reconstruction); + VLOG(1) << "Upgrade to Metric - Initial RMS:" << rms << std::endl; + AutoCalibrationLinear auto_calibration_linear; + uint image_width = 0; + uint image_height = 0; + PinholeCamera * pcamera = NULL; + std::map::iterator camera_iter = + reconstruction->cameras().begin(); + for (; camera_iter != reconstruction->cameras().end(); ++camera_iter) { + pcamera = dynamic_cast(camera_iter->second); + if (pcamera) { + image_width = pcamera->image_width(); + image_height = pcamera->image_height(); + // Avoid to have null values of image width and height + // TODO(julien) prefer an assert? + if (!image_width && !image_height) { + image_width = 640; + image_height = 480; + } + auto_calibration_linear.AddProjection(pcamera->projection_matrix(), + image_width, image_height); + } + } + // TODO(julien) Put the following in a function. + // Upgrade the reconstruction to metric using {Pm, Xm} = {P*H, H^{-1}*X} + Mat4 H = auto_calibration_linear.MetricTransformation(); + VLOG(1) << "Rectification H = " << H << "\n"; + if (isnan(H.sum())) { + LOG(ERROR) << "Warning: The metric rectification cannot be applied, the " + << "matrix contains NaN values.\n"; + return false; + } + Mat34 P; + camera_iter = reconstruction->cameras().begin(); + for (; camera_iter != reconstruction->cameras().end(); ++camera_iter) { + pcamera = dynamic_cast(camera_iter->second); + if (pcamera) { + P = pcamera->projection_matrix() * H; + pcamera->set_projection_matrix(P); + P_From_KRt(pcamera->intrinsic_matrix(), + pcamera->orientation_matrix(), + pcamera->position(), &P); + // TODO(julien) change this. + pcamera->set_projection_matrix(P); + } + } + Mat4 H_inverse = H.inverse(); + PointStructure * pstructure = NULL; + std::map::iterator stucture_iter = + reconstruction->structures().begin(); + for (; stucture_iter != reconstruction->structures().end(); ++stucture_iter) { + pstructure = dynamic_cast(stucture_iter->second); + if (pstructure) { + pstructure->set_coords(H_inverse * pstructure->coords()); + } + } + MetricBundleAdjust(matches, reconstruction); + return true; +} +} // namespace libmv diff --git a/src/libmv/reconstruction/reconstruction.cc b/src/libmv/reconstruction/reconstruction.cc index a3398fe..3d19e28 100644 --- a/src/libmv/reconstruction/reconstruction.cc +++ b/src/libmv/reconstruction/reconstruction.cc @@ -1,240 +1,25 @@ -// Copyright (c) 2010 libmv authors. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include "libmv/reconstruction/euclidean_reconstruction.h" -#include "libmv/reconstruction/export_blender.h" -#include "libmv/reconstruction/export_ply.h" -#include "libmv/reconstruction/image_order_selection.h" -#include "libmv/reconstruction/mapping.h" -#include "libmv/reconstruction/optimization.h" -#include "libmv/reconstruction/reconstruction.h" - -namespace libmv { - -bool EuclideanReconstructionFromVideo( - const Matches &matches, - int image_width, - int image_height, - double focal, - std::list *reconstructions) { - PinholeCamera * camera = NULL; - - Vec2u image_size; - image_size << image_width, image_height; - - double cu = image_width/2 - 0.5, - cv = image_height/2 - 0.5; - Mat3 K; - K << focal, 0, cu, - 0, focal, cv, - 0, 0, 1; - - std::list > connected_graph_list; - SelectKeyframes(matches, &connected_graph_list); - - VLOG(2) << " List order: "; - for (size_t i = 0; i < connected_graph_list.begin()->size(); ++i) { - VLOG(2) << (*connected_graph_list.begin())[i] << " "; - } - VLOG(2) << std::endl; - - Matches matches_inliers; - size_t image_id = 0; - size_t index_image_graph = 0; - Reconstruction *recons = NULL; - std::list >::iterator graph_iter = - connected_graph_list.begin(); - for (; graph_iter != connected_graph_list.end(); ++graph_iter) { - if (graph_iter->size() >= 2) { - recons = new Reconstruction(); - reconstructions->push_back(recons); - VLOG(2) << " -- Initial Motion Estimation -- " << std::endl; - ReconstructFromTwoCalibratedViews(matches, - (*graph_iter)[0], - (*graph_iter)[1], - K, K, - &matches_inliers, - recons); - index_image_graph = 0; - image_id = (*graph_iter)[index_image_graph]; - camera = dynamic_cast( - recons->GetCamera(image_id)); - if (camera) { - camera->set_image_size(image_size); - } - index_image_graph = 1; - image_id = (*graph_iter)[index_image_graph]; - camera = dynamic_cast( - recons->GetCamera(image_id)); - if (camera) { - camera->set_image_size(image_size); - } - - VLOG(2) << " -- Initial Intersection -- " << std::endl; - size_t minimum_num_views = 2; - PointStructureTriangulation(matches_inliers, - image_id, - minimum_num_views, - recons); - - // Performs projective bundle adjustment - VLOG(2) << " -- Bundle adjustment -- " << std::endl; - MetricBundleAdjust(matches_inliers, recons); - - /* - VLOG(2) << " -- Bundle adjustment (second pass) -- " << std::endl; - RemoveOutliers(image_id, &matches_inliers, recons, 1.0); - MetricBundleAdjust(matches_inliers, recons);*/ - - //std::string s = "out-1.py"; - //ExportToBlenderScript(*recons, s); - - // Estimation of the pose of other images by resection - minimum_num_views = 3; - for (index_image_graph = 2; index_image_graph < graph_iter->size(); - ++index_image_graph) { - image_id = (*graph_iter)[index_image_graph]; - VLOG(2) << " -- Incremental Resection -- " << std::endl; - CalibratedCameraResection(matches, image_id, K, - &matches_inliers, recons); - // TODO(julien) optimize camera - - camera = dynamic_cast( - recons->GetCamera(image_id)); - if (camera) { - camera->set_image_size(image_size); - } - - VLOG(2) << " -- Incremental Intersection -- " << std::endl; - PointStructureTriangulation(matches, - image_id, - minimum_num_views, - recons); - // TODO(julien) optimize only points - - // Performs bundle adjustment - // TODO(julien) maybe BA can be called not for every images.. - VLOG(2) << " -- Bundle adjustment -- " << std::endl; - MetricBundleAdjust(matches, recons); - /* - VLOG(2) << " -- RemoveOutliers -- " << std::endl; - //RemoveOutliers(image_id, &matches, recons, 2.0); - VLOG(2) << " -- Bundle adjustment -- " << std::endl; - MetricBundleAdjust(matches, recons);*/ - - //std::stringstream s; - //s << "out-" << index_image_graph << ".py"; - //ExportToBlenderScript(*recons, s.str()); - } - } - } - return true; -} - -bool EuclideanReconstructionFromImageSet( - const Matches &matches, - const vector > &image_sizes, - std::list *reconstructions) { - VLOG(0) << "not yet implemented!" << std::endl; - assert(0); - /* - * - SelectEfficientImageOrder(matches, &connected_graph_list); - - Mat3 K1, K2; - size_t index_image = 0; - Matches matches_inliers; - std::set::const_iterator image_iter = - matches.get_images().begin(); - Matches::ImageID previous_image_id = *image_iter; - - double u = image_sizes[index_image].second/2.0; - if (FLAGS_principal_point_u > 0) - u = FLAGS_principal_point_u; - double v = image_sizes[index_image].first/2.0; - if (FLAGS_principal_point_v > 0) - v = FLAGS_principal_point_v; - // The first image is fixed - K1 << FLAGS_focal, 0, u, - 0, FLAGS_focal, v, - 0, 0, 1; - - // Estimation of the second image - image_iter++; - index_image++; - - u = image_sizes[index_image].second/2.0; - if (FLAGS_principal_point_u > 0) - u = FLAGS_principal_point_u; - v = image_sizes[index_image].first/2.0; - if (FLAGS_principal_point_v > 0) - v = FLAGS_principal_point_v; - K2 << FLAGS_focal, 0, u, - 0, FLAGS_focal, v, - 0, 0, 1; - - LOG(INFO) << " -- Initial Motion Estimation -- " << std::endl; - ReconstructFromTwoCalibratedViews(matches, previous_image_id, *image_iter, - K1, K2, - &matches_inliers, &reconstruction); - - LOG(INFO) << " -- Initial Intersection -- " << std::endl; - size_t minimum_num_views = 2; - PointStructureTriangulation(matches_inliers, *image_iter, minimum_num_views, - &reconstruction); - - //LOG(INFO) << " -- Bundle Adjustment -- " << std::endl; - //TODO (julien) Perfom Bundle Adjustment (Euclidean BA) - - // Estimation of the pose of other images by resection - image_iter++; - index_image++; - for (; image_iter != matches.get_images().end(); - ++image_iter, ++index_image) { - u = image_sizes[index_image].second/2.0; - if (FLAGS_principal_point_u > 0) - u = FLAGS_principal_point_u; - v = image_sizes[index_image].first/2.0; - if (FLAGS_principal_point_v > 0) - v = FLAGS_principal_point_v; - K1 << FLAGS_focal, 0, u, - 0, FLAGS_focal, v, - 0, 0, 1; - - LOG(INFO) << " -- Incremental Resection -- " << std::endl; - CalibratedCameraResection(matches, *image_iter, K1, - &matches_inliers, &reconstruction); - - LOG(INFO) << " -- Incremental Intersection -- " << std::endl; - size_t minimum_num_views = 3; - PointStructureTriangulation(matches_inliers, - *image_iter, - minimum_num_views, - &reconstruction); - - LOG(INFO) << " -- Bundle Adjustment -- " << std::endl; - MetricBundleAdjust(matches_inliers, &reconstruction); - } - - */ - return true; -} - -} // namespace libmv +// Copyright (c) 2010 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include "libmv/reconstruction/reconstruction.h" + +namespace libmv { + +} // namespace libmv diff --git a/src/libmv/reconstruction/reconstruction.h b/src/libmv/reconstruction/reconstruction.h index 0c15049..2de95d1 100644 --- a/src/libmv/reconstruction/reconstruction.h +++ b/src/libmv/reconstruction/reconstruction.h @@ -27,7 +27,6 @@ #include "libmv/camera/pinhole_camera.h" #include "libmv/correspondence/matches.h" -#include "libmv/logging/logging.h" #include "libmv/numeric/numeric.h" #include "libmv/multiview/structure.h" @@ -200,22 +199,6 @@ class Reconstruction { Matches matches_; }; -// This function computes the trajectory of a camera using input matches. -//TODO(julien) document more -bool EuclideanReconstructionFromVideo( - const Matches &matches, - int image_width, - int image_height, - double focal, - std::list *reconstructions); - -// This function computes the poses of all cameras using input matches. -//TODO(julien) do it and document more -bool EuclideanReconstructionFromImageSet( - const Matches &matches, - const vector > &image_sizes, - std::list *reconstructions); - } // namespace libmv #endif // LIBMV_RECONSTRUCTION_RECONSTRUCTION_H_ diff --git a/src/libmv/reconstruction/reconstruction_test.cc b/src/libmv/reconstruction/reconstruction_test.cc index c3f700f..e69de29 100644 --- a/src/libmv/reconstruction/reconstruction_test.cc +++ b/src/libmv/reconstruction/reconstruction_test.cc @@ -1,266 +0,0 @@ -// Copyright (c) 2010 libmv authors. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. -#include - -#include "libmv/logging/logging.h" -#include "libmv/multiview/projection.h" -#include "libmv/multiview/random_sample.h" -#include "libmv/multiview/test_data_sets.h" -#include "libmv/numeric/numeric.h" -#include "libmv/reconstruction/reconstruction.h" -#include "libmv/reconstruction/euclidean_reconstruction.h" -#include "libmv/reconstruction/mapping.h" -#include "libmv/reconstruction/optimization.h" -#include "libmv/reconstruction/projective_reconstruction.h" -#include "testing/testing.h" - -namespace libmv { -namespace { - -void GenerateMatchesFromNViewDataSet(const NViewDataSet &d, - int noutliers, - Matches *matches, - std::list *list_features) { - Matches::TrackID track_id; - vector wrong_matches; - for (size_t n = 0; n < d.n; ++n) { - //std::cout << "n -> "<< d.x[n]<< std::endl; - // Generates wrong matches - UniformSample(noutliers, d.X.cols(), &wrong_matches); - //std::cout << "Features :"<push_back(feature); - track_id = p; - if (p < noutliers) { - track_id = wrong_matches[p]; - } - matches->Insert(n, track_id, feature); - } - } -} - -TEST(CalibratedReconstruction, TestSynthetic6FullViews) { - // TODO(julien) maybe a better check is the relative motion - int nviews = 6; - int npoints = 100; - int noutliers = 0.4*npoints;// 30% of outliers - NViewDataSet d = NRealisticCamerasFull(nviews, npoints); - - Mat4X X; - EuclideanToHomogeneous(d.X, &X); - // These are the expected precision of the results - // Dont expect much since for now - // - this is an incremental approach - // - the 3D structure is not retriangulated when new views are estimated - // - there is no optimization! - const double kPrecisionOrientationMatrix = 3e-2; - const double kPrecisionPosition = 3e-2; - - Reconstruction reconstruction; - // Create the matches - Matches matches; - Matches matches_inliers; - std::list list_features; - GenerateMatchesFromNViewDataSet(d, noutliers, &matches, &list_features); - - // We fix the gauge by setting the pose of the initial camera to the true pose - PinholeCamera * camera0 = new PinholeCamera(d.K[0], d.R[0], d.t[0]); - reconstruction.InsertCamera(0, camera0); - - std::cout << "Proceed Initial Motion Estimation" << std::endl; - // Proceed Initial Motion Estimation - ReconstructFromTwoCalibratedViews(matches, 0, 1, - d.K[0], d.K[1], - &matches_inliers, &reconstruction); - PinholeCamera * camera = NULL; - EXPECT_EQ(reconstruction.GetNumberCameras(), 2); - camera = dynamic_cast(reconstruction.GetCamera(0)); - EXPECT_TRUE(camera != NULL); - EXPECT_MATRIX_PROP(camera->orientation_matrix(), d.R[0], 1e-8); - EXPECT_MATRIX_PROP(camera->position(), d.t[0], 1e-8); - - double rms = RootMeanSquareError(d.x[0], X, camera->projection_matrix()); - std::cout << "RMSE Camera 0 = " << rms << std::endl; - - camera = dynamic_cast(reconstruction.GetCamera(1)); - EXPECT_TRUE(camera != NULL); - - // This is a monocular reconstruction - // We fix the scale - Mat3 dR = d.R[0].transpose()*d.R[1]; - Vec3 dt = d.R[0].transpose() * (d.t[1] - d.t[0]); - double dt_norm_real = dt.norm(); - dt = camera0->orientation_matrix().transpose() * - (camera->position() - camera0->position()); - dt *= dt_norm_real/dt.norm(); - camera->set_position(camera0->orientation_matrix() * dt - + camera0->position()); - - EXPECT_MATRIX_PROP(camera->orientation_matrix(), d.R[1], - kPrecisionOrientationMatrix); - EXPECT_MATRIX_PROP(camera->position(), d.t[1], kPrecisionPosition); - rms = RootMeanSquareError(d.x[1], X, camera->projection_matrix()); - std::cout << "RMSE Camera 1 = " << rms << std::endl; - - std::cout << "Proceed Initial Intersection" << std::endl; - // Proceed Initial Intersection - uint nInliers_added = 0; - size_t minimum_num_views_batch = 2; - nInliers_added = PointStructureTriangulation(matches_inliers, 1, - minimum_num_views_batch, - &reconstruction); - ASSERT_NEAR(nInliers_added, npoints - noutliers, 1); - - size_t minimum_num_views_incremental = 3; - Mat3 R; - Vec3 t; - // Checks the incremental reconstruction - for (int i = 2; i < nviews; ++i) { - std::cout << "Proceed Incremental Resection" << std::endl; - // Proceed Incremental Resection - CalibratedCameraResection(matches, i, d.K[i], - &matches_inliers, &reconstruction); - - EXPECT_EQ(reconstruction.GetNumberCameras(), i+1); - camera = dynamic_cast(reconstruction.GetCamera(i)); - EXPECT_TRUE(camera != NULL); - EXPECT_MATRIX_PROP(camera->orientation_matrix(), d.R[i], - kPrecisionOrientationMatrix); - EXPECT_MATRIX_PROP(camera->position(), d.t[i], kPrecisionPosition); - - std::cout << "Proceed Incremental Intersection" << std::endl; - // Proceed Incremental Intersection - nInliers_added = PointStructureTriangulation(matches_inliers, i, - minimum_num_views_incremental, - &reconstruction); - ASSERT_NEAR(nInliers_added, 0, 1); - - // TODO(julien) Check the rms with the reconstructed structure - rms = RootMeanSquareError(d.x[i], X, camera->projection_matrix()); - std::cout << "RMSE Camera " << i << " = " << rms << std::endl; - // TODO(julien) Check the 3D structure coordinates and inliers - } - // Performs bundle adjustment - rms = MetricBundleAdjust(matches_inliers, &reconstruction); - std::cout << " Final RMSE = " << rms << std::endl; - // TODO(julien) Check the results of BA - // clears the cameras, structures and features - reconstruction.ClearCamerasMap(); - reconstruction.ClearStructuresMap(); - std::list::iterator features_iter = list_features.begin(); - for (; features_iter != list_features.end(); ++features_iter) - delete *features_iter; - list_features.clear(); -} - -TEST(UncalibratedReconstruction, TestSynthetic6FullViews) { - int nviews = 6; - int npoints = 100; - int noutliers = 0.4*npoints;// 30% of outliers - double rms; - NViewDataSet d = NRealisticCamerasFull(nviews, npoints); - - Vec2u image_size; - // TODO(julien) Clean this..add image size in NViewDataSet? - image_size << 2*d.K[0](0, 2), 2*d.K[0](1, 2); - Mat4X X; - EuclideanToHomogeneous(d.X, &X); - - // These are the expected precision of the results - // Dont expect much since for now - // - this is an incremental approach - // - the 3D structure is not retriangulated when new views are estimated - // - there is no optimization! - const double kPrecisionOrientationMatrix = 3e-2; - const double kPrecisionPosition = 3e-2; - - Reconstruction reconstruction; - // Create the matches - Matches matches; - Matches matches_inliers; - std::list list_features; - GenerateMatchesFromNViewDataSet(d, noutliers, &matches, &list_features); - - std::cout << "Proceed Initial Motion Estimation" << std::endl; - // Proceed Initial Motion Estimation - ReconstructFromTwoUncalibratedViews(matches, 0, 1, - &matches_inliers, &reconstruction); - - // Set the image size need for the metric rectification - // TODO(julien) find a way to do somewhere else.. - ((PinholeCamera*)reconstruction.GetCamera(0))->set_image_size(image_size); - ((PinholeCamera*)reconstruction.GetCamera(1))->set_image_size(image_size); - - // TODO(julien) Check the two projection matrices - // Proceed Initial Intersection - size_t minimum_num_views_batch = 2; - PointStructureTriangulation(matches_inliers, 1, - minimum_num_views_batch, - &reconstruction); - - // TODO(julien) Check the rms with the reconstructed structure - // TODO(julien) Check the 3D structure coordinates and inliers - size_t minimum_num_views_incremental = 3; - Mat3 R; - Vec3 t; - // Checks the incremental reconstruction - for (int i = 2; i < nviews; ++i) { - std::cout << "Proceed Incremental Resection" << std::endl; - // Proceed Incremental Resection - UncalibratedCameraResection(matches, i, &matches_inliers, &reconstruction); - // TODO(julien) Check the projection matrix - - // Set the image size need for the metric rectification - // TODO(julien) find a way to do somewhere else.. - ((PinholeCamera*)reconstruction.GetCamera(i))->set_image_size(image_size); - - std::cout << "Proceed Incremental Intersection" << std::endl; - // Proceed Incremental Intersection - PointStructureTriangulation(matches_inliers, i, - minimum_num_views_incremental, - &reconstruction); - - // TODO(julien) Check the rms with the reconstructed structure - // TODO(julien) Check the 3D structure coordinates and inliers - } - - std::cout << "Bundle adjustment" << std::endl; - // Performs bundle adjustment - rms = MetricBundleAdjust(matches_inliers, &reconstruction); - std::cout << " Final RMSE = " << rms << std::endl; - // TODO(julien) Check the results of BA - std::cout << "Metric rectification" << std::endl; - // Metric rectification - UpgradeToMetric(matches_inliers, &reconstruction); - // TODO(julien) register the reconstruction so that the first camera - // corresponds to the real pose - // TODO(julien) Check the metric reconstruction - - // clears the cameras, structures and features - reconstruction.ClearCamerasMap(); - reconstruction.ClearStructuresMap(); - std::list::iterator features_iter = list_features.begin(); - for (; features_iter != list_features.end(); ++features_iter) - delete *features_iter; - list_features.clear(); -} -} // namespace -} // namespace libmv diff --git a/src/libmv/reconstruction/tools.cc b/src/libmv/reconstruction/tools.cc index cfd47bd..97bbd98 100644 --- a/src/libmv/reconstruction/tools.cc +++ b/src/libmv/reconstruction/tools.cc @@ -1,99 +1,99 @@ -// Copyright (c) 2010 libmv authors. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include -#include - -#include "libmv/reconstruction/tools.h" - -namespace libmv { - -// Selects only the already reconstructed tracks observed in the image image_id -// and returns a vector of StructureID and their feature coordinates -void SelectExistingPointStructures(const Matches &matches, - CameraID image_id, - const Reconstruction &reconstruction, - vector *structures_ids, - Mat2X *x_image) { - const size_t kNumberStructuresToReserve = 1000; - structures_ids->resize(0); - //TODO(julien) clean this - structures_ids->reserve(kNumberStructuresToReserve); - vector xs; - if (x_image) - xs.reserve(kNumberStructuresToReserve); - Matches::Features fp = - matches.InImage(image_id); - while (fp) { - if (reconstruction.TrackHasStructure(fp.track())) { - structures_ids->push_back(fp.track()); - if (x_image) - xs.push_back(fp.feature()->coords.cast()); - } - fp.operator++(); - } - if (x_image) - VectorToMatrix(xs, x_image); -} - -// Selects only the NOT already reconstructed tracks observed in the image -// image_id and returns a vector of StructureID and their feature coordinates -void SelectUnexistingPointStructures(const Matches &matches, - CameraID image_id, - const Reconstruction &reconstruction, - vector *structures_ids, - Mat2X *x_image) { - const size_t kNumberStructuresToReserve = 1000; - structures_ids->resize(0); - //TODO(julien) clean this - structures_ids->reserve(kNumberStructuresToReserve); - vector xs; - if (x_image) - xs.reserve(kNumberStructuresToReserve); - Matches::Features fp = - matches.InImage(image_id); - while (fp) { - if (!reconstruction.TrackHasStructure(fp.track())) { - structures_ids->push_back(fp.track()); - if (x_image) - xs.push_back(fp.feature()->coords.cast()); - } - fp.operator++(); - } - if (x_image) - VectorToMatrix(xs, x_image); -} - -// Recover the position of the selected point structures -void MatrixOfPointStructureCoordinates( - const vector &structures_ids, - const Reconstruction &reconstruction, - Mat4X *X_world) { - X_world->resize(4, structures_ids.size()); - PointStructure *point_s = NULL; - for (size_t s = 0; s < structures_ids.size(); ++s) { - point_s = dynamic_cast( - reconstruction.GetStructure(structures_ids[s])); - if (point_s) { - X_world->col(s) << point_s->coords(); - } - } -} -} // namespace libmv +// Copyright (c) 2010 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include +#include + +#include "libmv/reconstruction/tools.h" + +namespace libmv { + +// Selects only the already reconstructed tracks observed in the image image_id +// and returns a vector of StructureID and their feature coordinates +void SelectExistingPointStructures(const Matches &matches, + CameraID image_id, + const Reconstruction &reconstruction, + vector *structures_ids, + Mat2X *x_image) { + const size_t kNumberStructuresToReserve = 1000; + structures_ids->resize(0); + //TODO(julien) clean this + structures_ids->reserve(kNumberStructuresToReserve); + vector xs; + if (x_image) + xs.reserve(kNumberStructuresToReserve); + Matches::Features fp = + matches.InImage(image_id); + while (fp) { + if (reconstruction.TrackHasStructure(fp.track())) { + structures_ids->push_back(fp.track()); + if (x_image) + xs.push_back(fp.feature()->coords.cast()); + } + fp.operator++(); + } + if (x_image) + VectorToMatrix(xs, x_image); +} + +// Selects only the NOT already reconstructed tracks observed in the image +// image_id and returns a vector of StructureID and their feature coordinates +void SelectNonReconstructedPointStructures(const Matches &matches, + CameraID image_id, + const Reconstruction &reconstruction, + vector *structures_ids, + Mat2X *x_image) { + const size_t kNumberStructuresToReserve = 10000; + structures_ids->resize(0); + //TODO(julien) clean this + structures_ids->reserve(kNumberStructuresToReserve); + vector xs; + if (x_image) + xs.reserve(kNumberStructuresToReserve); + Matches::Features fp = + matches.InImage(image_id); + while (fp) { + if (!reconstruction.TrackHasStructure(fp.track())) { + structures_ids->push_back(fp.track()); + if (x_image) + xs.push_back(fp.feature()->coords.cast()); + } + fp.operator++(); + } + if (x_image) + VectorToMatrix(xs, x_image); +} + +// Recover the position of the selected point structures +void MatrixOfPointStructureCoordinates( + const vector &structures_ids, + const Reconstruction &reconstruction, + Mat4X *X_world) { + X_world->resize(4, structures_ids.size()); + PointStructure *point_s = NULL; + for (size_t s = 0; s < structures_ids.size(); ++s) { + point_s = dynamic_cast( + reconstruction.GetStructure(structures_ids[s])); + if (point_s) { + X_world->col(s) << point_s->coords(); + } + } +} +} // namespace libmv diff --git a/src/libmv/reconstruction/tools.h b/src/libmv/reconstruction/tools.h index 63133b7..b2cb216 100644 --- a/src/libmv/reconstruction/tools.h +++ b/src/libmv/reconstruction/tools.h @@ -49,11 +49,11 @@ void SelectExistingPointStructures(const Matches &matches, // Selects only the NOT already reconstructed tracks observed in the image // image_id and returns a vector of StructureID and their feature coordinates -void SelectUnexistingPointStructures(const Matches &matches, - CameraID image_id, - const Reconstruction &reconstruction, - vector *structures_ids, - Mat2X *x_image = NULL); +void SelectNonReconstructedPointStructures(const Matches &matches, + CameraID image_id, + const Reconstruction &reconstruction, + vector *structures_ids, + Mat2X *x_image = NULL); // Recover the position of the selected point structures void MatrixOfPointStructureCoordinates( diff --git a/src/tools/reconstruct_video.cc b/src/tools/reconstruct_video.cc index c6d08ac..7d77ba1 100644 --- a/src/tools/reconstruct_video.cc +++ b/src/tools/reconstruct_video.cc @@ -23,14 +23,14 @@ #include "libmv/correspondence/import_matches_txt.h" #include "libmv/correspondence/tracker.h" #include "libmv/logging/logging.h" -#include "libmv/reconstruction/reconstruction.h" +#include "libmv/reconstruction/euclidean_reconstruction.h" #include "libmv/reconstruction/export_blender.h" #include "libmv/reconstruction/export_ply.h" using namespace libmv; DEFINE_string(i, "matches.txt", "Matches input file"); -DEFINE_string(o, "reconstruction.ply", "Reconstruction output file"); +DEFINE_string(o, "reconstruction.py", "Reconstruction output file"); DEFINE_int32(w, 0, "Image width (px)"); DEFINE_int32(h, 0, "Image height (px)"); @@ -91,7 +91,6 @@ int main (int argc, char *argv[]) { // Exports the reconstructions VLOG(0) << "Exporting Reconstructions..." << std::endl; - std::string file_path_name, file_ext; GetFilePathExtention(FLAGS_o, &file_path_name, &file_ext); std::transform(file_ext.begin(), file_ext.end(), file_ext.begin(), ::tolower); diff --git a/src/tools/tracker.cc b/src/tools/tracker.cc index 79b4ca8..56603af 100644 --- a/src/tools/tracker.cc +++ b/src/tools/tracker.cc @@ -49,6 +49,7 @@ #include "libmv/reconstruction/reconstruction.h" #include "libmv/reconstruction/euclidean_reconstruction.h" #include "libmv/reconstruction/export_ply.h" +#include "libmv/reconstruction/export_blender.h" #include "libmv/reconstruction/mapping.h" #include "libmv/reconstruction/optimization.h" #include "libmv/multiview/robust_fundamental.h" @@ -67,8 +68,8 @@ DEFINE_bool (save_features, false, "save images with detected and matched features"); DEFINE_bool (save_matches, false, "save images with matches"); -DEFINE_bool (robust_tracker, false, - "perform a robust tracking (with epipolar filtering)"); +DEFINE_bool (non_robust_tracker, false, + "perform a non robust tracking (without epipolar filtering)"); DEFINE_double(robust_tracker_threshold, 1.0, "Epipolar filtering threshold (in pixels)"); @@ -225,86 +226,34 @@ bool IsArgImage(const std::string & arg) { } void ProceedReconstruction(Matches &matches, - vector > &image_sizes) { - Reconstruction reconstruction; - Mat3 K1, K2; - size_t index_image = 0; - Matches matches_inliers; - std::set::const_iterator image_iter = - matches.get_images().begin(); - Matches::ImageID previous_image_id = *image_iter; - - double u = image_sizes[index_image].second/2.0; - if (FLAGS_principal_point_u > 0) - u = FLAGS_principal_point_u; - double v = image_sizes[index_image].first/2.0; - if (FLAGS_principal_point_v > 0) - v = FLAGS_principal_point_v; - // The first image is fixed - K1 << FLAGS_focal, 0, u, - 0, FLAGS_focal, v, - 0, 0, 1; - - // Estimation of the second image - image_iter++; - index_image++; - - u = image_sizes[index_image].second/2.0; - if (FLAGS_principal_point_u > 0) - u = FLAGS_principal_point_u; - v = image_sizes[index_image].first/2.0; - if (FLAGS_principal_point_v > 0) - v = FLAGS_principal_point_v; - K2 << FLAGS_focal, 0, u, - 0, FLAGS_focal, v, - 0, 0, 1; - - VLOG(1) << " -- Initial Motion Estimation -- " << std::endl; - ReconstructFromTwoCalibratedViews(matches, previous_image_id, *image_iter, - K1, K2, - &matches_inliers, &reconstruction); - - VLOG(1) << " -- Initial Intersection -- " << std::endl; - size_t minimum_num_views = 2; - PointStructureTriangulation(matches_inliers, *image_iter, minimum_num_views, - &reconstruction); - - //VLOG(1) << " -- Bundle Adjustment -- " << std::endl; - //TODO (julien) Perfom Bundle Adjustment (Euclidean BA) - - // Estimation of the pose of other images by resection - image_iter++; - index_image++; - for (; image_iter != matches.get_images().end(); - ++image_iter, ++index_image) { - u = image_sizes[index_image].second/2.0; - if (FLAGS_principal_point_u > 0) - u = FLAGS_principal_point_u; - v = image_sizes[index_image].first/2.0; - if (FLAGS_principal_point_v > 0) - v = FLAGS_principal_point_v; - K1 << FLAGS_focal, 0, u, - 0, FLAGS_focal, v, - 0, 0, 1; - - VLOG(1) << " -- Incremental Resection -- " << std::endl; - CalibratedCameraResection(matches, *image_iter, K1, - &matches_inliers, &reconstruction); - - VLOG(1) << " -- Incremental Intersection -- " << std::endl; - size_t minimum_num_views = 3; - PointStructureTriangulation(matches_inliers, - *image_iter, - minimum_num_views, - &reconstruction); - - VLOG(1) << " -- Bundle Adjustment -- " << std::endl; - MetricBundleAdjust(matches_inliers, &reconstruction); + Vec2 &image_size) { + VLOG(1) << "Euclidean Reconstruction From Video..." << std::endl; + std::list reconstructions; + EuclideanReconstructionFromVideo(matches, + image_size(0), + image_size(1), + FLAGS_focal, + &reconstructions); + VLOG(1) << "Euclidean Reconstruction From Video...[DONE]" << std::endl; + int i = 0; + std::list::iterator iter = reconstructions.begin(); + for (; iter != reconstructions.end(); ++iter) { + std::stringstream s; + if (reconstructions.size() > 1) + s << "./out" << "-" << i << ".py"; + else + s << "./out.py"; + ExportToBlenderScript(**iter, s.str()); } - - ExportToPLY(reconstruction, "./out.ply"); - reconstruction.ClearCamerasMap(); - reconstruction.ClearStructuresMap(); + // Cleaning + VLOG(2) << "Cleaning." << std::endl; + iter = reconstructions.begin(); + for (; iter != reconstructions.end(); ++iter) { + (*iter)->ClearCamerasMap(); + (*iter)->ClearStructuresMap(); + delete *iter; + } + reconstructions.clear(); } int main (int argc, char *argv[]) { @@ -366,7 +315,7 @@ int main (int argc, char *argv[]) { matcher = new correspondence::ArrayMatcher_Kdtree(); tracker::Tracker *points_tracker = NULL; - if (!FLAGS_robust_tracker) { + if (FLAGS_non_robust_tracker) { points_tracker = new tracker::Tracker(pDetector, pDescriber, matcher); } else { tracker::RobustTracker * r_tracker = @@ -446,7 +395,11 @@ int main (int argc, char *argv[]) { // Estimates the camera trajectory and 3D structure of the scene if (FLAGS_pose_estimation) { - ProceedReconstruction(all_features_graph.matches_, image_sizes); + Vec2 image_size; + image_size << image_sizes[0].first, image_sizes[0].second; + // TODO(julien) all images are supposed to have the same size + // for this tracker + ProceedReconstruction(all_features_graph.matches_, image_size); } // Delete the tracker diff --git a/src/ui/nvr/nview.cpp b/src/ui/nvr/nview.cpp index 825d2cc..855026e 100644 --- a/src/ui/nvr/nview.cpp +++ b/src/ui/nvr/nview.cpp @@ -7,6 +7,8 @@ #include #include "libmv/correspondence/ArrayMatcher_Kdtree.h" +#include "libmv/correspondence/export_matches_txt.h" +#include "libmv/correspondence/import_matches_txt.h" #include "libmv/correspondence/robust_tracker.h" #include "libmv/image/image.h" #include "libmv/image/image_io.h" @@ -15,7 +17,8 @@ #include "libmv/reconstruction/euclidean_reconstruction.h" #include "libmv/reconstruction/export_blender.h" #include "libmv/reconstruction/export_ply.h" -#include "libmv/reconstruction/image_order_selection.h" +#include "libmv/reconstruction/image_selection.h" +#include "libmv/reconstruction/keyframe_selection.h" #include "libmv/reconstruction/mapping.h" #include "libmv/reconstruction/optimization.h" #include "libmv/reconstruction/projective_reconstruction.h" @@ -71,39 +74,44 @@ MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent) { QVBoxLayout* layout = new QVBoxLayout(actionsWidget); { QPushButton* button = new QPushButton("Open Images"); - connect(button,SIGNAL(clicked()),SLOT(openImages())); + connect(button,SIGNAL(clicked()),SLOT(OpenImages())); layout->addWidget(button); } { QPushButton* button = new QPushButton("Compute Matches (images)"); - connect(button,SIGNAL(clicked()),SLOT(computeMatches())); + connect(button,SIGNAL(clicked()),SLOT(ComputeMatches())); layout->addWidget(button); } { QPushButton* button = new QPushButton("Compute Relative Matches (video)"); - connect(button,SIGNAL(clicked()),SLOT(computeRelativeMatches())); + connect(button,SIGNAL(clicked()),SLOT(ComputeRelativeMatches())); + layout->addWidget(button); + } + { + QPushButton* button = new QPushButton("Load Matches"); + connect(button,SIGNAL(clicked()),SLOT(LoadMatches())); layout->addWidget(button); } { QPushButton* button = new QPushButton("*Uncalibrated Reconstruction*"); connect(button,SIGNAL(clicked()), - SLOT(computeUncalibratedReconstruction())); + SLOT(ComputeUncalibratedReconstruction())); layout->addWidget(button); } { QPushButton* button = new QPushButton("Metric Rectification"); - connect(button,SIGNAL(clicked()),SLOT(computeMetricRectification())); + connect(button,SIGNAL(clicked()),SLOT(ComputeMetricRectification())); layout->addWidget(button); } { QPushButton* button = new QPushButton("*Calibrated Reconstruction*"); connect(button,SIGNAL(clicked()), - SLOT(computeCalibratedReconstruction())); + SLOT(ComputeCalibratedReconstruction())); layout->addWidget(button); } { QPushButton* button = new QPushButton("Metric Bundle Adjustment"); - connect(button,SIGNAL(clicked()),SLOT(computeBA())); + connect(button,SIGNAL(clicked()),SLOT(ComputeBA())); layout->addWidget(button); } { @@ -134,8 +142,8 @@ MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent) { setCentralWidget( tab ); QStringList args = qApp->arguments(); args.removeFirst(); - openImages(args); - if(!args.isEmpty()) computeMatches(); + OpenImages(args); + if(!args.isEmpty()) ComputeMatches(); is_video_sequence_ = false; } @@ -156,8 +164,8 @@ MainWindow::~MainWindow() { grid_layout_ = NULL; } -void MainWindow::openImages() { - openImages(QFileDialog::getOpenFileNames( +void MainWindow::OpenImages() { + OpenImages(QFileDialog::getOpenFileNames( this, tr("Select Images"),"", "Pictures (*.png *.jpg *.jpeg *.bmp \ *.ppm *.pgm *.xpm *.tif *.tiff *.tga);;All Files (*.*)")); @@ -197,7 +205,7 @@ void MainWindow::SaveReconstructionFile() { } } } -void MainWindow::openImages( QStringList files ) { +void MainWindow::OpenImages( QStringList files ) { if (files.isEmpty()) return; foreach(ImageView* w,images) { grid_layout_->removeWidget(w); @@ -220,7 +228,7 @@ bool progressCallback(QProgressDialog &progress, int n) { progress.setValue(n); return progress.wasCanceled(); } -void MainWindow::warningNotFunctional() { +void MainWindow::WarningNotFunctional() { QMessageBox::warning (this, "Warning", "This process is STILL in development."); } @@ -264,7 +272,7 @@ bool MainWindow::SelectDetectorDescriber(eDetector *detector, return ok; } -void MainWindow::computeMatches() { +void MainWindow::ComputeMatches() { QProgressDialog progress("Computing matches...","Abort", 0, images.count(), this); progress.setWindowModality(Qt::WindowModal); @@ -352,10 +360,10 @@ ByteImage * ConvertToGrayscale(const ByteImage &imageArrayBytes) { return arrayGrayBytes; } -void MainWindow::computeRelativeMatches() { +void MainWindow::ComputeRelativeMatches() { QProgressDialog progress("Computing relative matches...","Abort", 0, images.count(), this); - warningNotFunctional(); + WarningNotFunctional(); progress.setWindowModality(Qt::WindowModal); eDetector detector = detector::FAST_DETECTOR; eDescriber describer = descriptor::DAISY_DESCRIBER; @@ -398,13 +406,29 @@ void MainWindow::computeRelativeMatches() { } matches_.Merge(all_features_graph.matches_); - // TODO(julien) Delete the features graph - //all_features_graph.DeleteAndClear(); UpdateGraph(); is_video_sequence_ = true; + ExportMatchesToTxt(all_features_graph.matches_, "./matches.txt"); + // TODO(julien) Delete the features graph + //all_features_graph.DeleteAndClear(); +} + +void MainWindow::LoadMatches() { + QString s = QFileDialog::getOpenFileName( + this, tr("Select Matches"),"", + "Text files (*.txt);;All Files (*.*)"); + if (s.isEmpty()) return; + + // Imports matches + FeatureSet *fs = new FeatureSet(); + ImportMatchesFromTxt(s.toStdString(), &matches_, fs); + UpdateGraph(); + is_video_sequence_ = true; + // TODO(julien) Delete the features graph + // delete fs } -void MainWindow::computeUncalibratedReconstruction() { +void MainWindow::ComputeUncalibratedReconstruction() { QProgressDialog progress("Computing uncalibrated reconstruction...","Abort",0, images.count(), this); progress.setWindowModality(Qt::WindowModal); @@ -460,10 +484,10 @@ void MainWindow::computeUncalibratedReconstruction() { progress.setLabelText("Initial Intersection"); VLOG(1) << " -- Initial Intersection -- " << std::endl; size_t minimum_num_views = 2; - PointStructureTriangulation(matches_inliers, - image_id, - minimum_num_views, - recons); + PointStructureTriangulationUncalibrated(matches_inliers, + image_id, + minimum_num_views, + recons); // Performs projective bundle adjustment //VLOG(1) << " -- Projective Bundle Adjustment -- " << std::endl; @@ -487,17 +511,17 @@ void MainWindow::computeUncalibratedReconstruction() { // TODO(julien) Avoid to retriangulate, prefer projective BA progress.setLabelText("Retriangulation"); VLOG(1) << " -- Retriangulation -- " << std::endl; - PointStructureRetriangulation(matches_inliers, - image_id, - recons); + PointStructureRetriangulationUncalibrated(matches_inliers, + image_id, + recons); progress.setLabelText("Incremental Intersection"); VLOG(1) << " -- Incremental Intersection -- " << std::endl; // TODO(julien) this do nothing (no points)...fix it - PointStructureTriangulation(matches_inliers, - image_id, - minimum_num_views, - recons); + PointStructureTriangulationUncalibrated(matches_inliers, + image_id, + minimum_num_views, + recons); // TODO(julien) Performs projective bundle adjustment progressCallback(progress, index_image_graph); @@ -510,26 +534,25 @@ void MainWindow::computeUncalibratedReconstruction() { UpdateGraph(); } -void MainWindow::computeCalibratedReconstruction() { +void MainWindow::ComputeCalibratedReconstruction() { QProgressDialog progress("Computing calibrated reconstruction...","Abort", 0, images.count(), this); progress.setWindowModality(Qt::WindowModal); Vec2u image_size; - PinholeCamera * camera = NULL; if (images.count()<2) return; double focal = 1639.47; - double cu = images[0]->GetImageWidth()/2 - 0.5, - cv = images[0]->GetImageHeight()/2 - 0.5; bool ok; //TODO(julien) create a better UI... focal = QInputDialog::getDouble(this, - tr("Set the focal length (in pixels)"), - tr("Focal (px) = image width (px) * focal length (mm) / CCD width (mm):"), focal, 0, 10000, 4, &ok); + tr("Set the focal length (in pixels)"), + tr("Focal (px) = image width (px) * focal length (mm) / CCD width (mm):"), focal, 0, 10000, 4, &ok); if (!ok) return; + /*double cu = images[0]->GetImageWidth()/2 - 0.5, + cv = images[0]->GetImageHeight()/2 - 0.5; cu = QInputDialog::getDouble(this, tr("Set the principal point coordinate (x)"), tr("Principal point coordinate x:"), cu, 0, 10000, 4, &ok); @@ -541,139 +564,53 @@ void MainWindow::computeCalibratedReconstruction() { Mat3 K; K << focal, 0, cu, - 0, focal, cv, - 0, 0, 1; - - // TODO(julien) put the following in the reconstruction lib. - progress.setLabelText("Selecting best initial images..."); - std::list > connected_graph_list; - if (is_video_sequence_) - SelectKeyframes(matches_, &connected_graph_list); - else - SelectEfficientImageOrder(matches_, &connected_graph_list); - - VLOG(1) << " List order:"; - for (size_t i = 0; i < connected_graph_list.begin()->size(); ++i) { - VLOG(1) << (*connected_graph_list.begin())[i] << " "; - } - VLOG(1) << std::endl; - - Matches matches_inliers; - size_t image_id = 0; - size_t index_image_graph = 0; - Reconstruction *recons = NULL; - std::list >::iterator graph_iter = - connected_graph_list.begin(); - for (; graph_iter != connected_graph_list.end(); ++graph_iter) { - if (graph_iter->size() >= 2) { - recons = new Reconstruction(); - reconstructions_.push_back(recons); - progress.setLabelText("Initial Motion Estimation"); - VLOG(1) << " -- Initial Motion Estimation -- " << std::endl; - ReconstructFromTwoCalibratedViews(matches_, - (*graph_iter)[0], - (*graph_iter)[1], - K, K, - &matches_inliers, - recons); - index_image_graph = 0; - image_id = (*graph_iter)[index_image_graph]; - camera = dynamic_cast( - recons->GetCamera(image_id)); - if (camera) { - image_size << images[image_id]->GetImageWidth(), - images[image_id]->GetImageHeight(); - camera->set_image_size(image_size); - } - index_image_graph = 1; - image_id = (*graph_iter)[index_image_graph]; - camera = dynamic_cast( - recons->GetCamera(image_id)); - if (camera) { - image_size << images[image_id]->GetImageWidth(), - images[image_id]->GetImageHeight(); - camera->set_image_size(image_size); - } - progressCallback(progress, 1); - - progress.setLabelText("Initial Intersection"); - VLOG(1) << " -- Initial Intersection -- " << std::endl; - size_t minimum_num_views = 2; - PointStructureTriangulation(matches_inliers, - image_id, - minimum_num_views, - recons); + 0, focal, cv, + 0, 0, 1;*/ - // Performs projective bundle adjustment - VLOG(1) << " -- Bundle adjustment -- " << std::endl; - progress.setLabelText("Bundle adjustment"); - MetricBundleAdjust(matches_inliers, recons); - /* - progress.setLabelText("Remove outliers"); - RemoveOutliers(image_id, &matches_inliers, recons, 1.0); - progress.setLabelText("Bundle adjustment"); - MetricBundleAdjust(matches_inliers, recons);*/ - - std::string s = "out-1.py"; - ExportToBlenderScript(*recons, s); - - // Estimation of the pose of other images by resection - minimum_num_views = 3; - for (index_image_graph = 2; index_image_graph < graph_iter->size(); - ++index_image_graph) { - image_id = (*graph_iter)[index_image_graph]; - progress.setLabelText("Incremental Resection"); - VLOG(1) << " -- Incremental Resection -- " << std::endl; - CalibratedCameraResection(matches_, image_id, K, - &matches_inliers, recons); - // TODO(julien) optimize camera - - camera = dynamic_cast( - recons->GetCamera(image_id)); - if (camera) { - image_size << images[image_id]->GetImageWidth(), - images[image_id]->GetImageHeight(); - camera->set_image_size(image_size); - } - - VLOG(1) << " -- Incremental Intersection -- " << std::endl; - progress.setLabelText("Incremental Intersection"); - PointStructureTriangulation(matches_, - image_id, - minimum_num_views, - recons); - // TODO(julien) optimize only points - - // Performs bundle adjustment - // TODO(julien) maybe BA can be called not for every images.. - VLOG(1) << " -- Bundle adjustment -- " << std::endl; - progress.setLabelText("Bundle adjustment"); - MetricBundleAdjust(matches_, recons); - /* - progress.setLabelText("Remove outliers"); - VLOG(1) << " -- RemoveOutliers -- " << std::endl; - //RemoveOutliers(image_id, &matches_, recons, 2.0); - VLOG(1) << " -- Bundle adjustment -- " << std::endl; - progress.setLabelText("Bundle adjustment"); - MetricBundleAdjust(matches_, recons);*/ - - //DrawNewStructures(new_structures_ids, *recons); - std::stringstream s; - s << "out-" << index_image_graph << ".py"; - ExportToBlenderScript(*recons, s.str()); - - progressCallback(progress, index_image_graph); - } - DrawAllStructures(*recons); + if (is_video_sequence_) { + progress.setLabelText("Euclidean Reconstruction From Video"); + VLOG(1) << "Euclidean Reconstruction From Video..." << std::endl; + EuclideanReconstructionFromVideo(matches_, + images[0]->GetImageWidth(), + images[0]->GetImageHeight(), + focal, + &reconstructions_); + VLOG(1) << "Euclidean Reconstruction From Video...[DONE]" << std::endl; + + //progress.setLabelText("Updating 2D/3D views"); + DrawAllStructures(**reconstructions_.begin()); + + progress.setLabelText("Exporting reconstructions"); + int i = 0; + std::list::iterator iter = reconstructions_.begin(); + for (; iter != reconstructions_.end(); ++iter) { + std::stringstream s; + if (reconstructions_.size() > 1) + s << "./out" << "-" << i << ".py"; + else + s << "./out.py"; + ExportToBlenderScript(**iter, s.str()); } + // Cleaning + /*progress.setLabelText("Cleaning reconstructions"); + VLOG(2) << "Cleaning." << std::endl; + iter = reconstructions_.begin(); + for (; iter != reconstructions_.end(); ++iter) { + (*iter)->ClearCamerasMap(); + (*iter)->ClearStructuresMap(); + delete *iter; + } + reconstructions_.clear();*/ + } else { + WarningNotFunctional(); } //matches_.Clear(); //matches_.Merge(matches_inliers); UpdateGraph(); } -void MainWindow::computeMetricRectification() { - warningNotFunctional(); +void MainWindow::ComputeMetricRectification() { + WarningNotFunctional(); QProgressDialog progress("Computing reconstruction...","Abort", 0, images.count(),this); progress.setWindowModality(Qt::WindowModal); @@ -684,7 +621,7 @@ void MainWindow::computeMetricRectification() { UpgradeToMetric(matches_, *iter); } -void MainWindow::computeBA() { +void MainWindow::ComputeBA() { QProgressDialog progress("Computing reconstruction...","Abort", 0, images.count(),this); progress.setWindowModality(Qt::WindowModal); diff --git a/src/ui/nvr/nview.h b/src/ui/nvr/nview.h index dcd8304..6311d85 100644 --- a/src/ui/nvr/nview.h +++ b/src/ui/nvr/nview.h @@ -115,18 +115,19 @@ class MainWindow : public QMainWindow { MainWindow(QWidget *parent = 0); ~MainWindow(); public slots: - void openImages(); - void openImages( QStringList filenames ); + void OpenImages(); + void OpenImages( QStringList filenames ); void SaveReconstructionFile(); - void computeMatches(); - void computeRelativeMatches(); - void computeUncalibratedReconstruction(); - void computeCalibratedReconstruction(); - void computeMetricRectification(); - void computeBA(); + void LoadMatches(); + void ComputeMatches(); + void ComputeRelativeMatches(); + void ComputeUncalibratedReconstruction(); + void ComputeCalibratedReconstruction(); + void ComputeMetricRectification(); + void ComputeBA(); void UpdateGraph(); - void warningNotFunctional(); + void WarningNotFunctional(); bool SelectDetectorDescriber(eDetector *detector, eDescriber *describer); signals: void setFilter(int i);