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:
Sergey Sharybin
2013-02-28 14:24:42 +00:00
parent 2a5ed5293d
commit 52f34f017d
10 changed files with 377 additions and 61 deletions

View File

@@ -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

View File

@@ -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

View File

@@ -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);

View File

@@ -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]);

View 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

View File

@@ -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;

View File

@@ -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.

View File

@@ -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), &current_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), &current_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;

View File

@@ -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="")

View File

@@ -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 {