Use ProductParameterization in bundle_adjuster.cc
Previously, when using a quaternion to parameterize the camera
orientation, the camera parameter block was split into two
parameter blocks. One for the rotation and another for the
translation and intrinsics. This was to enable the use of the
Quaternion parameterization.
Now that we have a ProductParameterization which allows us
to compose multiple parameterizations, this is no longer needed
and we use a size 10 parameter block instead.
This leads to a more than 2x improvements in the linear solver time.
Change-Id: I78b8f06696f81fee54cfe1a4ae193ee8a5f8e920
diff --git a/examples/bundle_adjuster.cc b/examples/bundle_adjuster.cc
index 985dbaf..e21c4ee 100644
--- a/examples/bundle_adjuster.cc
+++ b/examples/bundle_adjuster.cc
@@ -219,11 +219,6 @@
// When using axis-angle, there is a single parameter block for
// the entire camera.
ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
- // If quaternions are used, there are two blocks, so add the
- // second block to the ordering.
- if (FLAGS_use_quaternions) {
- ordering->AddElementToGroup(cameras + camera_block_size * i + 4, 1);
- }
}
options->linear_solver_ordering.reset(ordering);
@@ -263,7 +258,6 @@
// [u_1, u_2, ... , u_n], where each u_i is two dimensional, the x
// and y positions of the observation.
const double* observations = bal_problem->observations();
-
for (int i = 0; i < bal_problem->num_observations(); ++i) {
CostFunction* cost_function;
// Each Residual block takes a point and a camera as input and
@@ -286,28 +280,17 @@
double* camera =
cameras + camera_block_size * bal_problem->camera_index()[i];
double* point = points + point_block_size * bal_problem->point_index()[i];
-
- if (FLAGS_use_quaternions) {
- // When using quaternions, we split the camera into two
- // parameter blocks. One of size 4 for the quaternion and the
- // other of size 6 containing the translation, focal length and
- // the radial distortion parameters.
- problem->AddResidualBlock(cost_function,
- loss_function,
- camera,
- camera + 4,
- point);
- } else {
- problem->AddResidualBlock(cost_function, loss_function, camera, point);
- }
+ problem->AddResidualBlock(cost_function, loss_function, camera, point);
}
if (FLAGS_use_quaternions && FLAGS_use_local_parameterization) {
- LocalParameterization* quaternion_parameterization =
- new QuaternionParameterization;
+ LocalParameterization* camera_parameterization =
+ new ProductParameterization(
+ new QuaternionParameterization(),
+ new IdentityParameterization(6));
for (int i = 0; i < bal_problem->num_cameras(); ++i) {
problem->SetParameterization(cameras + camera_block_size * i,
- quaternion_parameterization);
+ camera_parameterization);
}
}
}
diff --git a/examples/snavely_reprojection_error.h b/examples/snavely_reprojection_error.h
index 184accb..a425616 100644
--- a/examples/snavely_reprojection_error.h
+++ b/examples/snavely_reprojection_error.h
@@ -60,7 +60,7 @@
T* residuals) const {
// camera[0,1,2] are the angle-axis rotation.
T p[3];
- ceres::AngleAxisRotatePoint(camera, point, p);
+ AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation.
p[0] += camera[3];
@@ -70,19 +70,20 @@
// Compute the center of distortion. The sign change comes from
// the camera model that Noah Snavely's Bundler assumes, whereby
// the camera coordinate system has a negative z axis.
- const T& focal = camera[6];
- T xp = - p[0] / p[2];
- T yp = - p[1] / p[2];
+ const T xp = - p[0] / p[2];
+ const T yp = - p[1] / p[2];
// Apply second and fourth order radial distortion.
const T& l1 = camera[7];
const T& l2 = camera[8];
- T r2 = xp*xp + yp*yp;
- T distortion = T(1.0) + r2 * (l1 + l2 * r2);
+ const T r2 = xp*xp + yp*yp;
+ const T distortion = T(1.0) + r2 * (l1 + l2 * r2);
+
// Compute final projected point position.
- T predicted_x = focal * distortion * xp;
- T predicted_y = focal * distortion * yp;
+ const T& focal = camera[6];
+ const T predicted_x = focal * distortion * xp;
+ const T predicted_y = focal * distortion * yp;
// The error is the difference between the predicted and observed position.
residuals[0] = predicted_x - T(observed_x);
@@ -115,37 +116,39 @@
: observed_x(observed_x), observed_y(observed_y) {}
template <typename T>
- bool operator()(const T* const camera_rotation,
- const T* const camera_translation_and_intrinsics,
+ bool operator()(const T* const camera,
const T* const point,
T* residuals) const {
- const T& focal = camera_translation_and_intrinsics[3];
- const T& l1 = camera_translation_and_intrinsics[4];
- const T& l2 = camera_translation_and_intrinsics[5];
-
- // Use a quaternion rotation that doesn't assume the quaternion is
- // normalized, since one of the ways to run the bundler is to let Ceres
- // optimize all 4 quaternion parameters unconstrained.
+ // camera[0,1,2,3] is are the rotation of the camera as a quaternion.
+ //
+ // We use QuaternionRotatePoint as it does not assume that the
+ // quaternion is normalized, since one of the ways to run the
+ // bundle adjuster is to let Ceres optimize all 4 quaternion
+ // parameters without a local parameterization.
T p[3];
- QuaternionRotatePoint(camera_rotation, point, p);
+ QuaternionRotatePoint(camera, point, p);
- p[0] += camera_translation_and_intrinsics[0];
- p[1] += camera_translation_and_intrinsics[1];
- p[2] += camera_translation_and_intrinsics[2];
+ p[0] += camera[4];
+ p[1] += camera[5];
+ p[2] += camera[6];
// Compute the center of distortion. The sign change comes from
// the camera model that Noah Snavely's Bundler assumes, whereby
// the camera coordinate system has a negative z axis.
- T xp = - p[0] / p[2];
- T yp = - p[1] / p[2];
+ const T xp = - p[0] / p[2];
+ const T yp = - p[1] / p[2];
// Apply second and fourth order radial distortion.
- T r2 = xp*xp + yp*yp;
- T distortion = T(1.0) + r2 * (l1 + l2 * r2);
+ const T& l1 = camera[8];
+ const T& l2 = camera[9];
+
+ const T r2 = xp*xp + yp*yp;
+ const T distortion = T(1.0) + r2 * (l1 + l2 * r2);
// Compute final projected point position.
- T predicted_x = focal * distortion * xp;
- T predicted_y = focal * distortion * yp;
+ const T& focal = camera[7];
+ const T predicted_x = focal * distortion * xp;
+ const T predicted_y = focal * distortion * yp;
// The error is the difference between the predicted and observed position.
residuals[0] = predicted_x - T(observed_x);
@@ -159,7 +162,7 @@
static ceres::CostFunction* Create(const double observed_x,
const double observed_y) {
return (new ceres::AutoDiffCostFunction<
- SnavelyReprojectionErrorWithQuaternions, 2, 4, 6, 3>(
+ SnavelyReprojectionErrorWithQuaternions, 2, 10, 3>(
new SnavelyReprojectionErrorWithQuaternions(observed_x,
observed_y)));
}
