Modal (aka tripod) solver rework
Several major things are done in this commit: - First of all, logic of modal solver was changed. We do not rely on only minimizer to take care of guessing rotation for frame, but we're using analytical rotation computation for point clouds to obtain initial rotation. Then this rotation is being refined using Ceres minimizer and now instead of minimizing average distance between points of point of two clouds, minimization of reprojection error of point cloud onto frame happens. This gives quite a bit of precision improvement. - Second bigger improvement here is using bundle adjustment for a result of first step when we're only estimating rotation between neighbor images and reprojecting markers. This averages error across the image sequence avoiding error accumulation. Also, this will tweak bundles themselves a bit for better match. - And last bigger improvement here is support of camera intrinsics refirenment. This allowed to significantly improve solution for real-life footage and results after such refining are much more usable than it were before. Thanks to Keir for the help and code review.
This commit is contained in:
1
extern/libmv/CMakeLists.txt
vendored
1
extern/libmv/CMakeLists.txt
vendored
@@ -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
|
||||
|
||||
20
extern/libmv/files.txt
vendored
20
extern/libmv/files.txt
vendored
@@ -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
|
||||
|
||||
27
extern/libmv/libmv-capi.cpp
vendored
27
extern/libmv/libmv-capi.cpp
vendored
@@ -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);
|
||||
|
||||
|
||||
1
extern/libmv/libmv-capi.h
vendored
1
extern/libmv/libmv-capi.h
vendored
@@ -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]);
|
||||
|
||||
181
extern/libmv/libmv/multiview/panography.h
vendored
Normal file
181
extern/libmv/libmv/multiview/panography.h
vendored
Normal file
@@ -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
|
||||
9
extern/libmv/libmv/simple_pipeline/bundle.cc
vendored
9
extern/libmv/libmv/simple_pipeline/bundle.cc
vendored
@@ -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;
|
||||
|
||||
12
extern/libmv/libmv/simple_pipeline/bundle.h
vendored
12
extern/libmv/libmv/simple_pipeline/bundle.h
vendored
@@ -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.
|
||||
|
||||
183
extern/libmv/libmv/simple_pipeline/modal_solver.cc
vendored
183
extern/libmv/libmv/simple_pipeline/modal_solver.cc
vendored
@@ -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;
|
||||
|
||||
// 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);
|
||||
// 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) {
|
||||
Marker marker = tracks.MarkerInImageForTrack(image, track);
|
||||
Vec3 X;
|
||||
ProjectMarkerOnSphere(marker, X);
|
||||
|
||||
if (marker.image == image) {
|
||||
Vec3 X;
|
||||
int last_column = x1.cols();
|
||||
x1.conservativeResize(3, last_column + 1);
|
||||
x2.conservativeResize(3, last_column + 1);
|
||||
|
||||
LG << "Use track " << track << " for rigid registration between image " <<
|
||||
image - 1 << " and " << image;
|
||||
|
||||
ProjectMarkerOnSphere(marker, X);
|
||||
|
||||
points.push_back(point->X);
|
||||
reference_points.push_back(X);
|
||||
}
|
||||
x1.col(last_column) = current_R * point->X;
|
||||
x2.col(last_column) = X;
|
||||
}
|
||||
}
|
||||
|
||||
if (points.size()) {
|
||||
// Find rigid delta transformation to current image
|
||||
RigidRegistration(reference_points, points, R);
|
||||
if (x1.cols() >= 2) {
|
||||
Mat3 delta_R;
|
||||
|
||||
// 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);
|
||||
|
||||
// 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));
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
|
||||
@@ -332,8 +332,7 @@ class CLIP_PT_tools_solve(CLIP_PT_tracking_panel, Panel):
|
||||
col.prop(tracking_object, "keyframe_b")
|
||||
|
||||
col = layout.column(align=True)
|
||||
col.active = (tracking_object.is_camera and
|
||||
not settings.use_tripod_solver)
|
||||
col.active = tracking_object.is_camera
|
||||
col.label(text="Refine:")
|
||||
col.prop(settings, "refine_intrinsics", text="")
|
||||
|
||||
|
||||
@@ -3076,6 +3076,7 @@ void BKE_tracking_reconstruction_solve(MovieReconstructContext *context, short *
|
||||
if (context->motion_flag & TRACKING_MOTION_MODAL) {
|
||||
context->reconstruction = libmv_solveModal(context->tracks,
|
||||
&camera_intrinsics_options,
|
||||
&reconstruction_options,
|
||||
reconstruct_update_solve_cb, &progressdata);
|
||||
}
|
||||
else {
|
||||
|
||||
Reference in New Issue
Block a user