diff options
Diffstat (limited to 'extern/libmv')
-rw-r--r-- | extern/libmv/CMakeLists.txt | 1 | ||||
-rw-r--r-- | extern/libmv/files.txt | 20 | ||||
-rw-r--r-- | extern/libmv/libmv-capi.cpp | 27 | ||||
-rw-r--r-- | extern/libmv/libmv-capi.h | 1 | ||||
-rw-r--r-- | extern/libmv/libmv/multiview/panography.h | 181 | ||||
-rw-r--r-- | extern/libmv/libmv/simple_pipeline/bundle.cc | 9 | ||||
-rw-r--r-- | extern/libmv/libmv/simple_pipeline/bundle.h | 12 | ||||
-rw-r--r-- | extern/libmv/libmv/simple_pipeline/modal_solver.cc | 179 |
8 files changed, 373 insertions, 57 deletions
diff --git a/extern/libmv/CMakeLists.txt b/extern/libmv/CMakeLists.txt index 1c289cd2778..241e8bf2cd1 100644 --- a/extern/libmv/CMakeLists.txt +++ b/extern/libmv/CMakeLists.txt @@ -99,6 +99,7 @@ set(SRC libmv/multiview/homography.h libmv/multiview/homography_parameterization.h libmv/multiview/nviewtriangulation.h + libmv/multiview/panography.h libmv/multiview/projection.h libmv/multiview/resection.h libmv/multiview/triangulation.h diff --git a/extern/libmv/files.txt b/extern/libmv/files.txt index 22c5226adbe..8eead23ea08 100644 --- a/extern/libmv/files.txt +++ b/extern/libmv/files.txt @@ -21,6 +21,7 @@ libmv/multiview/homography.cc libmv/multiview/homography.h libmv/multiview/homography_parameterization.h libmv/multiview/nviewtriangulation.h +libmv/multiview/panography.h libmv/multiview/projection.cc libmv/multiview/projection.h libmv/multiview/resection.h @@ -142,25 +143,6 @@ third_party/glog/src/windows/glog/vlog_is_on.h third_party/glog/src/windows/port.cc third_party/glog/src/windows/port.h third_party/glog/src/windows/preprocess.sh -third_party/ldl/CMakeLists.txt -third_party/ldl/Doc/ChangeLog -third_party/ldl/Doc/lesser.txt -third_party/ldl/Include/ldl.h -third_party/ldl/README.libmv -third_party/ldl/README.txt -third_party/ldl/Source/ldl.c third_party/msinttypes/inttypes.h third_party/msinttypes/README.libmv third_party/msinttypes/stdint.h -third_party/ssba/COPYING.TXT -third_party/ssba/Geometry/v3d_cameramatrix.h -third_party/ssba/Geometry/v3d_distortion.h -third_party/ssba/Geometry/v3d_metricbundle.cpp -third_party/ssba/Geometry/v3d_metricbundle.h -third_party/ssba/Math/v3d_linear.h -third_party/ssba/Math/v3d_linear_utils.h -third_party/ssba/Math/v3d_mathutilities.h -third_party/ssba/Math/v3d_optimization.cpp -third_party/ssba/Math/v3d_optimization.h -third_party/ssba/README.libmv -third_party/ssba/README.TXT diff --git a/extern/libmv/libmv-capi.cpp b/extern/libmv/libmv-capi.cpp index 4d6b35b91f8..74224eb5368 100644 --- a/extern/libmv/libmv-capi.cpp +++ b/extern/libmv/libmv-capi.cpp @@ -444,7 +444,8 @@ int libmv_refineParametersAreValid(int parameters) { static void libmv_solveRefineIntrinsics(libmv::Tracks *tracks, libmv::CameraIntrinsics *intrinsics, libmv::EuclideanReconstruction *reconstruction, int refine_intrinsics, - reconstruct_progress_update_cb progress_update_callback, void *callback_customdata) + reconstruct_progress_update_cb progress_update_callback, void *callback_customdata, + int bundle_constraints = libmv::BUNDLE_NO_CONSTRAINTS) { /* only a few combinations are supported but trust the caller */ int libmv_refine_flags = 0; @@ -465,7 +466,7 @@ static void libmv_solveRefineIntrinsics(libmv::Tracks *tracks, libmv::CameraIntr progress_update_callback(callback_customdata, 1.0, "Refining solution"); libmv::EuclideanBundleCommonIntrinsics(*(libmv::Tracks *)tracks, libmv_refine_flags, - reconstruction, intrinsics); + reconstruction, intrinsics, bundle_constraints); } static void cameraIntrinsicsFromOptions(libmv::CameraIntrinsics *camera_intrinsics, @@ -568,6 +569,7 @@ libmv_Reconstruction *libmv_solveReconstruction(libmv_Tracks *libmv_tracks, struct libmv_Reconstruction *libmv_solveModal(struct libmv_Tracks *libmv_tracks, libmv_cameraIntrinsicsOptions *libmv_camera_intrinsics_options, + libmv_reconstructionOptions *libmv_reconstruction_options, reconstruct_progress_update_cb progress_update_callback, void *callback_customdata) { @@ -582,13 +584,28 @@ struct libmv_Reconstruction *libmv_solveModal(struct libmv_Tracks *libmv_tracks, cameraIntrinsicsFromOptions(camera_intrinsics, libmv_camera_intrinsics_options); - /* Invert the camera intrinsics */ + /* Invert the camera intrinsics. */ libmv::Tracks normalized_tracks = getNormalizedTracks(tracks, camera_intrinsics); - /* actual reconstruction */ + /* Actual reconstruction. */ libmv::ModalSolver(normalized_tracks, reconstruction, &update_callback); - /* finish reconstruction */ + libmv::CameraIntrinsics empty_intrinsics; + libmv::EuclideanBundleCommonIntrinsics(normalized_tracks, + libmv::BUNDLE_NO_INTRINSICS, + reconstruction, + &empty_intrinsics, + libmv::BUNDLE_NO_TRANSLATION); + + /* Refinement. */ + if (libmv_reconstruction_options->refine_intrinsics) { + libmv_solveRefineIntrinsics((libmv::Tracks *)tracks, camera_intrinsics, reconstruction, + libmv_reconstruction_options->refine_intrinsics, + progress_update_callback, callback_customdata, + libmv::BUNDLE_NO_TRANSLATION); + } + + /* Finish reconstruction. */ finishReconstruction(tracks, camera_intrinsics, libmv_reconstruction, progress_update_callback, callback_customdata); diff --git a/extern/libmv/libmv-capi.h b/extern/libmv/libmv-capi.h index 588df8de440..af0d656146b 100644 --- a/extern/libmv/libmv-capi.h +++ b/extern/libmv/libmv-capi.h @@ -110,6 +110,7 @@ struct libmv_Reconstruction *libmv_solveReconstruction(struct libmv_Tracks *libm void *callback_customdata); struct libmv_Reconstruction *libmv_solveModal(struct libmv_Tracks *libmv_tracks, libmv_cameraIntrinsicsOptions *libmv_camera_intrinsics_options, + libmv_reconstructionOptions *libmv_reconstruction_options, reconstruct_progress_update_cb progress_update_callback, void *callback_customdata); int libmv_reporojectionPointForTrack(struct libmv_Reconstruction *libmv_reconstruction, int track, double pos[3]); diff --git a/extern/libmv/libmv/multiview/panography.h b/extern/libmv/libmv/multiview/panography.h new file mode 100644 index 00000000000..6f01beb0ffd --- /dev/null +++ b/extern/libmv/libmv/multiview/panography.h @@ -0,0 +1,181 @@ +// Copyright (c) 2009 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_MULTIVIEW_PANOGRAPHY_H +#define LIBMV_MULTIVIEW_PANOGRAPHY_H + +#include "libmv/numeric/numeric.h" +#include "libmv/numeric/poly.h" +#include "libmv/base/vector.h" + +namespace libmv { + +static bool Build_Minimal2Point_PolynomialFactor( + const Mat & x1, const Mat & x2, + double * P) //P must be a double[4] +{ + assert(2 == x1.rows()); + assert(2 == x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + // Setup the variable of the input problem: + Vec xx1 = (x1.col(0)).transpose(); + Vec yx1 = (x1.col(1)).transpose(); + + double a12 = xx1.dot(yx1); + Vec xx2 = (x2.col(0)).transpose(); + Vec yx2 = (x2.col(1)).transpose(); + double b12 = xx2.dot(yx2); + + double a1 = xx1.squaredNorm(); + double a2 = yx1.squaredNorm(); + + double b1 = xx2.squaredNorm(); + double b2 = yx2.squaredNorm(); + + // Build the 3rd degre polynomial in F^2. + // + // f^6 * p + f^4 * q + f^2* r + s = 0; + // + // Coefficients in ascending powers of alpha, i.e. P[N]*x^N. + // Run panography_coeffs.py to get the below coefficients. + P[0] = b1*b2*a12*a12-a1*a2*b12*b12; + P[1] = -2*a1*a2*b12+2*a12*b1*b2+b1*a12*a12+b2*a12*a12-a1*b12*b12-a2*b12*b12; + P[2] = b1*b2-a1*a2-2*a1*b12-2*a2*b12+2*a12*b1+2*a12*b2+a12*a12-b12*b12; + P[3] = b1+b2-2*b12-a1-a2+2*a12; + + // If P[3] equal to 0 we get ill conditionned data + return (P[3] != 0.0); +} + +// This implements a minimal solution (2 points) for panoramic stitching: +// +// http://www.cs.ubc.ca/~mbrown/minimal/minimal.html +// +// [1] M. Brown and R. Hartley and D. Nister. Minimal Solutions for Panoramic +// Stitching. CVPR07. +// +// The 2-point algorithm solves for the rotation of the camera with a single +// focal length (4 degrees of freedom). +// +// Compute from 1 to 3 possible focal lenght for 2 point correspondences. +// Suppose that the cameras share the same optical center and focal lengths: +// +// Image 1 => H*x = x' => Image 2 +// x (u1j) x' (u2j) +// a (u11) a' (u21) +// b (u12) b' (u22) +// +// The return values are 1 to 3 possible values for the focal lengths such +// that: +// +// [f 0 0] +// K = [0 f 0] +// [0 0 1] +// +static void F_FromCorrespondance_2points(const Mat &x1, const Mat &x2, + vector<double> *fs) { + + // Build Polynomial factor to get squared focal value. + double P[4]; + Build_Minimal2Point_PolynomialFactor(x1, x2, &P[0]); + + // Solve it by using F = f^2 and a Cubic polynomial solver + // + // F^3 * p + F^2 * q + F^1 * r + s = 0 + // + double roots[3]; + int num_roots = SolveCubicPolynomial(P, roots); + for (int i = 0; i < num_roots; ++i) { + if (roots[i] > 0.0) { + fs->push_back(sqrt(roots[i])); + } + } +} + +// Compute the 3x3 rotation matrix that fits two 3D point clouds in the least +// square sense. The method is from: +// +// K. Arun,T. Huand and D. Blostein. Least-squares fitting of 2 3-D point +// sets. IEEE Transactions on Pattern Analysis and Machine Intelligence, +// 9:698-700, 1987. +// +// Given the calibration matrices K1, K2 solve for the rotation from +// corresponding image rays. +// +// R = min || X2 - R * x1 ||. +// +// In case of panography, which is for a camera that shares the same camera +// center, +// +// H = K2 * R * K1.inverse(); +// +// For the full explanation, see Section 8, Solving for Rotation from [1]. +// +// Parameters: +// +// x1 : Point cloud A (3D coords) +// x2 : Point cloud B (3D coords) +// +// [f 0 0] +// K1 = [0 f 0] +// [0 0 1] +// +// K2 (the same form as K1, but may have different f) +// +// Returns: A rotation matrix that minimizes +// +// R = arg min || X2 - R * x1 || +// +static void GetR_FixedCameraCenter(const Mat &x1, const Mat &x2, + const double focal, + Mat3 *R) { + assert(3 == x1.rows()); + assert(2 <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + // Build simplified K matrix + Mat3 K( Mat3::Identity() * 1.0/focal ); + K(2,2)= 1.0; + + // Build the correlation matrix; equation (22) in [1]. + Mat3 C = Mat3::Zero(); + for(int i = 0; i < x1.cols(); ++i) { + Mat r1i = (K * x1.col(i)).normalized(); + Mat r2i = (K * x2.col(i)).normalized(); + C += r2i * r1i.transpose(); + } + + // Solve for rotation. Equations (24) and (25) in [1]. + Eigen::JacobiSVD<Mat> svd(C, Eigen::ComputeThinU | Eigen::ComputeThinV); + Mat3 scale = Mat3::Identity(); + scale(2,2) = ((svd.matrixU() * svd.matrixV().transpose()).determinant() > 0.0) + ? 1.0 + : -1.0; + + (*R) = svd.matrixU() * scale * svd.matrixV().transpose(); +} + +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_PANOGRAPHY_H diff --git a/extern/libmv/libmv/simple_pipeline/bundle.cc b/extern/libmv/libmv/simple_pipeline/bundle.cc index 34117cbbbd7..3e36a78c9df 100644 --- a/extern/libmv/libmv/simple_pipeline/bundle.cc +++ b/extern/libmv/libmv/simple_pipeline/bundle.cc @@ -233,7 +233,8 @@ void EuclideanBundle(const Tracks &tracks, void EuclideanBundleCommonIntrinsics(const Tracks &tracks, int bundle_intrinsics, EuclideanReconstruction *reconstruction, - CameraIntrinsics *intrinsics) { + CameraIntrinsics *intrinsics, + int bundle_constraints) { LG << "Original intrinsics: " << *intrinsics; vector<Marker> markers = tracks.AllMarkers(); @@ -270,7 +271,7 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks, } problem.AddResidualBlock(new ceres::AutoDiffCostFunction< - OpenCVReprojectionError, 2, 8, 9 /* 3 */, 3, 3>( + OpenCVReprojectionError, 2, 8, 9, 3, 3>( new OpenCVReprojectionError( marker.x, marker.y)), @@ -284,6 +285,10 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks, problem.SetParameterization(&camera->R(0, 0), &rotation_parameterization); + if (bundle_constraints & BUNDLE_NO_TRANSLATION) { + problem.SetParameterBlockConstant(&camera->t(0)); + } + num_residuals++; } LG << "Number of residuals: " << num_residuals; diff --git a/extern/libmv/libmv/simple_pipeline/bundle.h b/extern/libmv/libmv/simple_pipeline/bundle.h index 55657cb2d92..573bcf4cc21 100644 --- a/extern/libmv/libmv/simple_pipeline/bundle.h +++ b/extern/libmv/libmv/simple_pipeline/bundle.h @@ -67,6 +67,11 @@ void EuclideanBundle(const Tracks &tracks, BUNDLE_FOCAL_LENGTH | BUNDLE_PRINCIPAL_POINT BUNDLE_FOCAL_LENGTH | BUNDLE_PRINCIPAL_POINT | BUNDLE_RADIAL BUNDLE_FOCAL_LENGTH | BUNDLE_PRINCIPAL_POINT | BUNDLE_RADIAL | BUNDLE_TANGENTIAL + BUNDLE_RADIAL + + Constraints denotes which blocks to keep constant during bundling. + For example it is useful to keep camera translations constant + when bundling tripod motions. \note This assumes an outlier-free set of markers. @@ -83,10 +88,15 @@ enum BundleIntrinsics { BUNDLE_TANGENTIAL_P2 = 32, BUNDLE_TANGENTIAL = 48, }; +enum BundleConstraints { + BUNDLE_NO_CONSTRAINTS = 0, + BUNDLE_NO_TRANSLATION = 1, +}; void EuclideanBundleCommonIntrinsics(const Tracks &tracks, int bundle_intrinsics, EuclideanReconstruction *reconstruction, - CameraIntrinsics *intrinsics); + CameraIntrinsics *intrinsics, + int bundle_constraints = BUNDLE_NO_CONSTRAINTS); /*! Refine camera poses and 3D coordinates using bundle adjustment. diff --git a/extern/libmv/libmv/simple_pipeline/modal_solver.cc b/extern/libmv/libmv/simple_pipeline/modal_solver.cc index bb49b30dbce..aa2fa9a8a9d 100644 --- a/extern/libmv/libmv/simple_pipeline/modal_solver.cc +++ b/extern/libmv/libmv/simple_pipeline/modal_solver.cc @@ -18,11 +18,14 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. +#include "libmv/simple_pipeline/modal_solver.h" + #include <cstdio> +#include "ceres/ceres.h" +#include "ceres/rotation.h" #include "libmv/logging/logging.h" -#include "libmv/simple_pipeline/modal_solver.h" -#include "libmv/simple_pipeline/rigid_registration.h" +#include "libmv/multiview/panography.h" #ifdef _MSC_VER # define snprintf _snprintf @@ -30,7 +33,8 @@ namespace libmv { -static void ProjectMarkerOnSphere(Marker &marker, Vec3 &X) { +namespace { +void ProjectMarkerOnSphere(Marker &marker, Vec3 &X) { X(0) = marker.x; X(1) = marker.y; X(2) = 1.0; @@ -38,18 +42,59 @@ static void ProjectMarkerOnSphere(Marker &marker, Vec3 &X) { X *= 5.0 / X.norm(); } -static void ModalSolverLogProress(ProgressUpdateCallback *update_callback, - double progress) +void ModalSolverLogProress(ProgressUpdateCallback *update_callback, + double progress) { if (update_callback) { char message[256]; - snprintf(message, sizeof(message), "Solving progress %d%%", (int)(progress * 100)); + snprintf(message, sizeof(message), "Solving progress %d%%", + (int)(progress * 100)); update_callback->invoke(progress, message); } } +struct ModalReprojectionError { + ModalReprojectionError(Vec2 marker, Vec3 bundle) + : marker(marker), bundle(bundle) { } + + template <typename T> + bool operator()(const T* quaternion, // Rotation quaternion + T* residuals) const { + + T R[9]; + ceres::QuaternionToRotation(quaternion, R); + + // Convert bundle position from double to T. + T X[3]; + X[0] = T(bundle(0)); + X[1] = T(bundle(1)); + X[2] = T(bundle(2)); + + // Compute projective coordinates: x = RX. + T x[3]; + x[0] = R[0]*X[0] + R[3]*X[1] + R[6]*X[2]; + x[1] = R[1]*X[0] + R[4]*X[1] + R[7]*X[2]; + x[2] = R[2]*X[0] + R[5]*X[1] + R[8]*X[2]; + + // Compute normalized coordinates: x /= x[2]. + T xn = x[0] / x[2]; + T yn = x[1] / x[2]; + + // The error is the difference between reprojected + // and observed marker position. + residuals[0] = xn - T(marker(0)); + residuals[1] = yn - T(marker(1)); + + return true; + } + + Vec2 marker; + Vec3 bundle; +}; +} // namespace + void ModalSolver(Tracks &tracks, EuclideanReconstruction *reconstruction, ProgressUpdateCallback *update_callback) { @@ -59,58 +104,132 @@ void ModalSolver(Tracks &tracks, LG << "Max image: " << max_image; LG << "Max track: " << max_track; - Mat3 R = Mat3::Identity(); + // For minimization we're using quaternions. + Vec3 zero_rotation = Vec3::Zero(); + Vec4 quaternion; + ceres::AngleAxisToQuaternion(&zero_rotation(0), &quaternion(0)); for (int image = 0; image <= max_image; ++image) { vector<Marker> all_markers = tracks.MarkersInImage(image); ModalSolverLogProress(update_callback, (float) image / max_image); - // Skip empty frames without doing anything + // Skip empty images without doing anything. if (all_markers.size() == 0) { - LG << "Skipping frame: " << image; + LG << "Skipping image: " << image; continue; } - vector<Vec3> points, reference_points; + // STEP 1: Estimate rotation analytically. + Mat3 current_R; + ceres::QuaternionToRotation(&quaternion(0), ¤t_R(0, 0)); + + // Construct point cloud for current and previous images, + // using markers appear at current image for which we know + // 3D positions. + Mat x1, x2; + for (int i = 0; i < all_markers.size(); ++i) { + Marker &marker = all_markers[i]; + EuclideanPoint *point = reconstruction->PointForTrack(marker.track); + if (point) { + Vec3 X; + ProjectMarkerOnSphere(marker, X); - // Cnstruct pairs of markers from current and previous image, - // to reproject them and find rigid transformation between - // previous and current image - for (int track = 0; track <= max_track; ++track) { - EuclideanPoint *point = reconstruction->PointForTrack(track); + int last_column = x1.cols(); + x1.conservativeResize(3, last_column + 1); + x2.conservativeResize(3, last_column + 1); - if (point) { - Marker marker = tracks.MarkerInImageForTrack(image, track); + x1.col(last_column) = current_R * point->X; + x2.col(last_column) = X; + } + } - if (marker.image == image) { - Vec3 X; + if (x1.cols() >= 2) { + Mat3 delta_R; - LG << "Use track " << track << " for rigid registration between image " << - image - 1 << " and " << image; + // Compute delta rotation matrix for two point clouds. + // Could be a bit confusing at first glance, but order + // of clouds is indeed so. + GetR_FixedCameraCenter(x2, x1, 1.0, &delta_R); - ProjectMarkerOnSphere(marker, X); + // Convert delta rotation form matrix to final image + // rotation stored in a quaternion + Vec3 delta_angle_axis; + ceres::RotationMatrixToAngleAxis(&delta_R(0, 0), &delta_angle_axis(0)); - points.push_back(point->X); - reference_points.push_back(X); - } + Vec3 current_angle_axis; + ceres::QuaternionToAngleAxis(&quaternion(0), ¤t_angle_axis(0)); + + Vec3 angle_axis = current_angle_axis + delta_angle_axis; + + ceres::AngleAxisToQuaternion(&angle_axis(0), &quaternion(0)); + + LG << "Analytically computed quaternion " + << quaternion.transpose(); + } + + // STEP 2: Refine rotation with Ceres. + ceres::Problem problem; + + ceres::LocalParameterization* quaternion_parameterization = + new ceres::QuaternionParameterization; + + int num_residuals = 0; + for (int i = 0; i < all_markers.size(); ++i) { + Marker &marker = all_markers[i]; + EuclideanPoint *point = reconstruction->PointForTrack(marker.track); + + if (point) { + problem.AddResidualBlock(new ceres::AutoDiffCostFunction< + ModalReprojectionError, + 2, /* num_residuals */ + 4>(new ModalReprojectionError(Vec2(marker.x, marker.y), + point->X)), + NULL, + &quaternion(0)); + num_residuals++; + + problem.SetParameterization(&quaternion(0), + quaternion_parameterization); } } - if (points.size()) { - // Find rigid delta transformation to current image - RigidRegistration(reference_points, points, R); + LG << "Number of residuals: " << num_residuals; + + if (num_residuals) { + // Configure the solve. + ceres::Solver::Options solver_options; + solver_options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; + solver_options.max_num_iterations = 50; + solver_options.update_state_every_iteration = true; + solver_options.gradient_tolerance = 1e-36; + solver_options.parameter_tolerance = 1e-36; + solver_options.function_tolerance = 1e-36; + + // Run the solve. + ceres::Solver::Summary summary; + ceres::Solve(solver_options, &problem, &summary); + + LG << "Summary:\n" << summary.FullReport(); + LG << "Refined quaternion " << quaternion.transpose(); } + // Convert quaternion to rotation matrix. + Mat3 R; + ceres::QuaternionToRotation(&quaternion(0), &R(0, 0)); reconstruction->InsertCamera(image, R, Vec3::Zero()); - // Review if there's new tracks for which position might be reconstructed + // STEP 3: reproject all new markers appeared at image + + // Check if there're new markers appeared on current image + // and reproject them on sphere to obtain 3D position/ for (int track = 0; track <= max_track; ++track) { if (!reconstruction->PointForTrack(track)) { Marker marker = tracks.MarkerInImageForTrack(image, track); if (marker.image == image) { - // New track appeared on this image, project it's position onto sphere + // New track appeared on this image, + // project it's position onto sphere. LG << "Projecting track " << track << " at image " << image; |