[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [55909] trunk/blender/extern/libmv/libmv/ simple_pipeline/bundle.cc: Bundle adjustment improvements
Sergey Sharybin
sergey.vfx at gmail.com
Mon Apr 8 19:05:52 CEST 2013
Revision: 55909
http://projects.blender.org/scm/viewvc.php?view=rev&root=bf-blender&revision=55909
Author: nazgul
Date: 2013-04-08 17:05:52 +0000 (Mon, 08 Apr 2013)
Log Message:
-----------
Bundle adjustment improvements
- Get rid of rotation matrix parameterization,
use angle-axis instead.
Also Joined rotation and translation into a
single parameter block.
This made minimization go significantly faster,
like 1.3x times in average.
- Fix first camera when bundling. This is to
address orientation ambiguity.
Reconstruction result could still vary in
size, but that's another issue to be addressed
later.
Additional change:
Split EuclideanBundleCommonIntrinsics into
smaller functions, so it's now a bit easier
to follow.
Modified Paths:
--------------
trunk/blender/extern/libmv/libmv/simple_pipeline/bundle.cc
Modified: trunk/blender/extern/libmv/libmv/simple_pipeline/bundle.cc
===================================================================
--- trunk/blender/extern/libmv/libmv/simple_pipeline/bundle.cc 2013-04-08 17:05:48 UTC (rev 55908)
+++ trunk/blender/extern/libmv/libmv/simple_pipeline/bundle.cc 2013-04-08 17:05:52 UTC (rev 55909)
@@ -61,8 +61,8 @@
template <typename T>
bool operator()(const T* const intrinsics,
- const T* const R, // Rotation 3x3 column-major.
- const T* const t, // Translation 3x1.
+ const T* const R_t, // Rotation denoted by angle axis
+ // followed with translation
const T* const X, // Point coordinates 3x1.
T* residuals) const {
// Unpack the intrinsics.
@@ -77,10 +77,12 @@
// Compute projective coordinates: x = RX + t.
T x[3];
- x[0] = R[0]*X[0] + R[3]*X[1] + R[6]*X[2] + t[0];
- x[1] = R[1]*X[0] + R[4]*X[1] + R[7]*X[2] + t[1];
- x[2] = R[2]*X[0] + R[5]*X[1] + R[8]*X[2] + t[2];
+ ceres::AngleAxisRotatePoint(R_t, X, x);
+ x[0] += R_t[3];
+ x[1] += R_t[4];
+ x[2] += R_t[5];
+
// Compute normalized coordinates: x /= x[2].
T xn = x[0] / x[2];
T yn = x[1] / x[2];
@@ -121,70 +123,6 @@
double observed_y;
};
-// TODO(keir): Get rid of the parameterization! Ceres will work much faster if
-// the rotation block is angle-axis and also the translation is merged into a
-// single parameter block.
-struct RotationMatrixPlus {
- template<typename T>
- bool operator()(const T* R_array, // Rotation 3x3 col-major.
- const T* delta, // Angle-axis delta
- T* R_plus_delta_array) const {
- T angle_axis[3];
-
- ceres::RotationMatrixToAngleAxis(R_array, angle_axis);
-
- angle_axis[0] += delta[0];
- angle_axis[1] += delta[1];
- angle_axis[2] += delta[2];
-
- ceres::AngleAxisToRotationMatrix(angle_axis, R_plus_delta_array);
-
- return true;
- }
-};
-
-// TODO(sergey): would be nice to have this in Ceres upstream
-template<typename PlusFunctor, int kGlobalSize, int kLocalSize>
-class AutodiffParameterization : public ceres::LocalParameterization {
- public:
- AutodiffParameterization(const PlusFunctor &plus_functor)
- : plus_functor_(plus_functor) {}
-
- virtual ~AutodiffParameterization() {}
-
- virtual bool Plus(const double* x,
- const double* delta,
- double* x_plus_delta) const {
- return plus_functor_(x, delta, x_plus_delta);
- }
-
- virtual bool ComputeJacobian(const double* x, double* jacobian) const {
- double zero_delta[kLocalSize] = { 0.0 };
- double x_plus_delta[kGlobalSize];
- const double* parameters[2] = { x, zero_delta };
- double* jacobians_array[2] = { NULL, jacobian };
-
- Plus(x, zero_delta, x_plus_delta);
-
- return ceres::internal::AutoDiff<PlusFunctor,
- double,
- kGlobalSize, kLocalSize>
- ::Differentiate(plus_functor_,
- parameters,
- kGlobalSize,
- x_plus_delta,
- jacobians_array);
-
- return true;
- }
-
- virtual int GlobalSize() const { return kGlobalSize; }
- virtual int LocalSize() const { return kLocalSize; }
-
- private:
- const PlusFunctor &plus_functor_;
-};
-
void BundleIntrinsicsLogMessage(int bundle_intrinsics) {
if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
LG << "Bundling only camera positions.";
@@ -220,6 +158,83 @@
}
}
+// Pack intrinsics from object to an array for easier
+// and faster minimization
+void PackIntrinisicsIntoArray(CameraIntrinsics *intrinsics,
+ double ceres_intrinsics[8]) {
+ ceres_intrinsics[OFFSET_FOCAL_LENGTH] = intrinsics->focal_length();
+ ceres_intrinsics[OFFSET_PRINCIPAL_POINT_X] = intrinsics->principal_point_x();
+ ceres_intrinsics[OFFSET_PRINCIPAL_POINT_Y] = intrinsics->principal_point_y();
+ ceres_intrinsics[OFFSET_K1] = intrinsics->k1();
+ ceres_intrinsics[OFFSET_K2] = intrinsics->k2();
+ ceres_intrinsics[OFFSET_K3] = intrinsics->k3();
+ ceres_intrinsics[OFFSET_P1] = intrinsics->p1();
+ ceres_intrinsics[OFFSET_P2] = intrinsics->p2();
+}
+
+// Unpack intrinsics back from an array to an object
+void UnpackIntrinsicsFromArray(CameraIntrinsics *intrinsics,
+ double ceres_intrinsics[8]) {
+ intrinsics->SetFocalLength(ceres_intrinsics[OFFSET_FOCAL_LENGTH],
+ ceres_intrinsics[OFFSET_FOCAL_LENGTH]);
+
+ intrinsics->SetPrincipalPoint(ceres_intrinsics[OFFSET_PRINCIPAL_POINT_X],
+ ceres_intrinsics[OFFSET_PRINCIPAL_POINT_Y]);
+
+ intrinsics->SetRadialDistortion(ceres_intrinsics[OFFSET_K1],
+ ceres_intrinsics[OFFSET_K2],
+ ceres_intrinsics[OFFSET_K3]);
+
+ intrinsics->SetTangentialDistortion(ceres_intrinsics[OFFSET_P1],
+ ceres_intrinsics[OFFSET_P2]);
+}
+
+// Get a vector of camera's rotations denoted by angle axis
+// conjuncted with translations into single block
+//
+// Element with index i matches to a rotation+translation for
+// camera at image i.
+vector<Vec6> PackCamerasRotationAndTranslation(
+ const Tracks &tracks,
+ EuclideanReconstruction *reconstruction) {
+ vector<Vec6> cameras_R_t;
+ int max_image = tracks.MaxImage();
+
+ cameras_R_t.resize(max_image + 1);
+
+ for (int i = 0; i <= max_image; i++) {
+ EuclideanCamera *camera = reconstruction->CameraForImage(i);
+
+ if (!camera)
+ continue;
+
+ ceres::RotationMatrixToAngleAxis(&camera->R(0, 0),
+ &cameras_R_t[i](0));
+ cameras_R_t[i].tail<3>() = camera->t;
+ }
+
+ return cameras_R_t;
+}
+
+// Convert cameras rotations fro mangle axis back to rotation matrix
+void UnpackCamerasRotationAndTranslation(
+ const Tracks &tracks,
+ EuclideanReconstruction *reconstruction,
+ vector<Vec6> cameras_R_t) {
+ int max_image = tracks.MaxImage();
+
+ for (int i = 0; i <= max_image; i++) {
+ EuclideanCamera *camera = reconstruction->CameraForImage(i);
+
+ if (!camera)
+ continue;
+
+ ceres::AngleAxisToRotationMatrix(&cameras_R_t[i](0),
+ &camera->R(0, 0));
+ camera->t = cameras_R_t[i].tail<3>();
+ }
+}
+
} // namespace
void EuclideanBundle(const Tracks &tracks,
@@ -240,29 +255,24 @@
vector<Marker> markers = tracks.AllMarkers();
ceres::Problem::Options problem_options;
- problem_options.local_parameterization_ownership =
- ceres::DO_NOT_TAKE_OWNERSHIP;
-
ceres::Problem problem(problem_options);
// Residual blocks with 10 parameters are unwieldly with Ceres, so pack the
// intrinsics into a single block and rely on local parameterizations to
// control which intrinsics are allowed to vary.
double ceres_intrinsics[8];
- ceres_intrinsics[OFFSET_FOCAL_LENGTH] = intrinsics->focal_length();
- ceres_intrinsics[OFFSET_PRINCIPAL_POINT_X] = intrinsics->principal_point_x();
- ceres_intrinsics[OFFSET_PRINCIPAL_POINT_Y] = intrinsics->principal_point_y();
- ceres_intrinsics[OFFSET_K1] = intrinsics->k1();
- ceres_intrinsics[OFFSET_K2] = intrinsics->k2();
- ceres_intrinsics[OFFSET_K3] = intrinsics->k3();
- ceres_intrinsics[OFFSET_P1] = intrinsics->p1();
- ceres_intrinsics[OFFSET_P2] = intrinsics->p2();
+ PackIntrinisicsIntoArray(intrinsics, ceres_intrinsics);
- RotationMatrixPlus rotation_matrix_plus;
- AutodiffParameterization<RotationMatrixPlus, 9, 3>
- rotation_parameterization(rotation_matrix_plus);
+ // Convert cameras rotations to angle axis and merge with translation
+ // into single parameter block for maximal minimization speed
+ //
+ // Block for minimization has got the following structure:
+ // <3 elements for angle-axis> <3 elements for translation>
+ vector<Vec6> cameras_R_t =
+ PackCamerasRotationAndTranslation(tracks, reconstruction);
int num_residuals = 0;
+ bool have_locked_camera = false;
for (int i = 0; i < markers.size(); ++i) {
const Marker &marker = markers[i];
EuclideanCamera *camera = reconstruction->CameraForImage(marker.image);
@@ -271,24 +281,28 @@
continue;
}
+ // Rotation of camera denoted in angle axis
+ double *camera_R_t = &cameras_R_t[camera->image] (0);
+
problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
- OpenCVReprojectionError, 2, 8, 9, 3, 3>(
+ OpenCVReprojectionError, 2, 8, 6, 3>(
new OpenCVReprojectionError(
marker.x,
marker.y)),
NULL,
ceres_intrinsics,
- &camera->R(0, 0),
- &camera->t(0),
+ camera_R_t,
&point->X(0));
- // It's fine if the parameterization for one camera is set repeatedly.
- problem.SetParameterization(&camera->R(0, 0),
- &rotation_parameterization);
+ // We lock first camera for better deal with
+ // scene orientation ambiguity
+ if (!have_locked_camera) {
+ problem.SetParameterBlockConstant(camera_R_t);
+ have_locked_camera = true;
+ }
- if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
+ if (bundle_constraints & BUNDLE_NO_TRANSLATION)
problem.SetParameterBlockConstant(&camera->t(0));
- }
num_residuals++;
}
@@ -301,9 +315,6 @@
BundleIntrinsicsLogMessage(bundle_intrinsics);
- scoped_ptr<ceres::SubsetParameterization>
- subset_parameterization(NULL);
-
if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
// No camera intrinsics are refining,
// set the whole parameter block as constant for best performance
@@ -328,12 +339,13 @@
// Always set K3 constant, it's not used at the moment.
constant_intrinsics.push_back(OFFSET_K3);
- subset_parameterization.reset(
- new ceres::SubsetParameterization(8, constant_intrinsics));
+ ceres::SubsetParameterization *subset_parameterization =
@@ Diff output truncated at 10240 characters. @@
More information about the Bf-blender-cvs
mailing list