Skip to content

Commit

Permalink
Style cleanup, mainly pointed by Sameer in Ceres's codereview
Browse files Browse the repository at this point in the history
  • Loading branch information
@sergeyvfx
sergeyvfx committed May 11, 2013
1 parent 42da053 commit 7e16226
Show file tree
Hide file tree
Showing 2 changed files with 72 additions and 72 deletions.
138 changes: 69 additions & 69 deletions src/libmv/simple_pipeline/bundle.cc
View file
Expand Up @@ -24,7 +24,6 @@

#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include "libmv/base/scoped_ptr.h"
#include "libmv/base/vector.h"
#include "libmv/logging/logging.h"
#include "libmv/multiview/fundamental.h"
Expand Down Expand Up @@ -56,14 +55,14 @@ enum {
namespace {

struct OpenCVReprojectionError {
OpenCVReprojectionError(double observed_x, double observed_y)
OpenCVReprojectionError(const double observed_x, const double observed_y)
: observed_x(observed_x), observed_y(observed_y) {}

template <typename T>
bool operator()(const T* const intrinsics,
const T* const R_t, // Rotation denoted by angle axis
// followed with translation
const T* const X, // Point coordinates 3x1.
const T* const X, // Point coordinates 3x1.
T* residuals) const {
// Unpack the intrinsics.
const T& focal_length = intrinsics[OFFSET_FOCAL_LENGTH];
Expand Down Expand Up @@ -123,11 +122,11 @@ struct OpenCVReprojectionError {
return true;
}

double observed_x;
double observed_y;
const double observed_x;
const double observed_y;
};

void BundleIntrinsicsLogMessage(int bundle_intrinsics) {
void BundleIntrinsicsLogMessage(const int bundle_intrinsics) {
if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
LG << "Bundling only camera positions.";
} else if (bundle_intrinsics == BUNDLE_FOCAL_LENGTH) {
Expand Down Expand Up @@ -163,22 +162,22 @@ void BundleIntrinsicsLogMessage(int bundle_intrinsics) {
}

// Pack intrinsics from object to an array for easier
// and faster minimization
void PackIntrinisicsIntoArray(CameraIntrinsics *intrinsics,
// and faster minimization.
void PackIntrinisicsIntoArray(const 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();
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]) {
// Unpack intrinsics back from an array to an object.
void UnpackIntrinsicsFromArray(const double ceres_intrinsics[8],
CameraIntrinsics *intrinsics) {
intrinsics->SetFocalLength(ceres_intrinsics[OFFSET_FOCAL_LENGTH],
ceres_intrinsics[OFFSET_FOCAL_LENGTH]);

Expand All @@ -194,37 +193,37 @@ void UnpackIntrinsicsFromArray(CameraIntrinsics *intrinsics,
}

// Get a vector of camera's rotations denoted by angle axis
// conjuncted with translations into single block
// 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;
const EuclideanReconstruction &reconstruction) {
vector<Vec6> all_cameras_R_t;
int max_image = tracks.MaxImage();

cameras_R_t.resize(max_image + 1);
all_cameras_R_t.resize(max_image + 1);

for (int i = 0; i <= max_image; i++) {
EuclideanCamera *camera = reconstruction->CameraForImage(i);
const 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;
&all_cameras_R_t[i](0));
all_cameras_R_t[i].tail<3>() = camera->t;
}

return cameras_R_t;
return all_cameras_R_t;
}

// Convert cameras rotations fro mangle axis back to rotation matrix
// Convert cameras rotations fro mangle axis back to rotation matrix.
void UnpackCamerasRotationAndTranslation(
const Tracks &tracks,
EuclideanReconstruction *reconstruction,
vector<Vec6> cameras_R_t) {
const vector<Vec6> &all_cameras_R_t,
EuclideanReconstruction *reconstruction) {
int max_image = tracks.MaxImage();

for (int i = 0; i <= max_image; i++) {
Expand All @@ -233,14 +232,14 @@ void UnpackCamerasRotationAndTranslation(
if (!camera)
continue;

ceres::AngleAxisToRotationMatrix(&cameras_R_t[i](0),
ceres::AngleAxisToRotationMatrix(&all_cameras_R_t[i](0),
&camera->R(0, 0));
camera->t = cameras_R_t[i].tail<3>();
camera->t = all_cameras_R_t[i].tail<3>();
}
}

// Converts sparse CRSMatrix to Eigen matrix, so it could be used
// all over in the pipeline
// all over in the pipeline.
//
// TODO(sergey): currently uses dense Eigen matrices, best would
// be to use sparse Eigen matrices
Expand Down Expand Up @@ -276,8 +275,8 @@ void EuclideanBundle(const Tracks &tracks,
}

void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
int bundle_intrinsics,
int bundle_constraints,
const int bundle_intrinsics,
const int bundle_constraints,
EuclideanReconstruction *reconstruction,
CameraIntrinsics *intrinsics,
BundleEvaluation *evaluation) {
Expand All @@ -291,44 +290,44 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
// intrinsics into a single block and rely on local parameterizations to
// control which intrinsics are allowed to vary.
double ceres_intrinsics[8];
PackIntrinisicsIntoArray(intrinsics, ceres_intrinsics);
PackIntrinisicsIntoArray(*intrinsics, ceres_intrinsics);

// Convert cameras rotations to angle axis and merge with translation
// into single parameter block for maximal minimization speed
// 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);
vector<Vec6> all_cameras_R_t =
PackCamerasRotationAndTranslation(tracks, *reconstruction);

// Parameterization used to restrict camera motion for
// modal solvers
ceres::SubsetParameterization *motion_parameterization = NULL;
// Parameterization used to restrict camera motion for modal solvers.
ceres::SubsetParameterization *constant_translation_parameterization = NULL;
if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
std::vector<int> constant_motion;
std::vector<int> constant_translation;

// First three elements are rotation, ast three are translation
constant_motion.push_back(3);
constant_motion.push_back(4);
constant_motion.push_back(5);
// First three elements are rotation, ast three are translation.
constant_translation.push_back(3);
constant_translation.push_back(4);
constant_translation.push_back(5);

motion_parameterization =
new ceres::SubsetParameterization(6, constant_motion);
constant_translation_parameterization =
new ceres::SubsetParameterization(6, constant_translation);
}

// Add residual blocks to the problem
// Add residual blocks to the problem.
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);
EuclideanPoint *point = reconstruction->PointForTrack(marker.track);
if (!camera || !point) {
if (camera == NULL || point == NULL) {
continue;
}

// Rotation of camera denoted in angle axis
double *camera_R_t = &cameras_R_t[camera->image] (0);
// Rotation of camera denoted in angle axis followed with
// camera translaiton.
double *current_camera_R_t = &all_cameras_R_t[camera->image](0);

problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
OpenCVReprojectionError, 2, 8, 6, 3>(
Expand All @@ -337,18 +336,20 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
marker.y)),
NULL,
ceres_intrinsics,
camera_R_t,
current_camera_R_t,
&point->X(0));

// We lock first camera for better deal with
// scene orientation ambiguity
// scene orientation ambiguity.
if (!have_locked_camera) {
problem.SetParameterBlockConstant(camera_R_t);
problem.SetParameterBlockConstant(current_camera_R_t);
have_locked_camera = true;
}

if (bundle_constraints & BUNDLE_NO_TRANSLATION)
problem.SetParameterization(camera_R_t, motion_parameterization);
if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
problem.SetParameterization(current_camera_R_t,
constant_translation_parameterization);
}

num_residuals++;
}
Expand All @@ -363,10 +364,10 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,

if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
// No camera intrinsics are refining,
// set the whole parameter block as constant for best performance
// set the whole parameter block as constant for best performance.
problem.SetParameterBlockConstant(ceres_intrinsics);
} else {
// Set intrinsics not being bundles as constant
// Set intrinsics not being bundles as constant.

std::vector<int> constant_intrinsics;
#define MAYBE_SET_CONSTANT(bundle_enum, offset) \
Expand All @@ -391,7 +392,7 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
problem.SetParameterization(ceres_intrinsics, subset_parameterization);
}

// Configure the solver
// Configure the solver.
ceres::Solver::Options options;
options.use_nonmonotonic_steps = true;
options.preconditioner_type = ceres::SCHUR_JACOBI;
Expand All @@ -412,19 +413,19 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,

// Copy rotations and translations back.
UnpackCamerasRotationAndTranslation(tracks,
reconstruction,
cameras_R_t);
all_cameras_R_t,
reconstruction);

// Copy intrinsics back.
if (bundle_intrinsics != BUNDLE_NO_INTRINSICS)
UnpackIntrinsicsFromArray(intrinsics, ceres_intrinsics);
UnpackIntrinsicsFromArray(ceres_intrinsics, intrinsics);

LG << "Final intrinsics: " << *intrinsics;

if (evaluation) {
int max_track = tracks.MaxTrack();
// Number of camera rotations equals to number of translation,
int num_cameras = cameras_R_t.size();
int num_cameras = all_cameras_R_t.size();
int num_points = 0;

for (int i = 0; i <= max_track; i++) {
Expand All @@ -436,12 +437,11 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
LG << "Number of cameras " << num_cameras;
LG << "Number of points " << num_points;

// which is for sure equals to number of cameras
evaluation->num_cameras = num_cameras;
evaluation->num_points = num_points;

if (evaluation->evaluate_jacobian) {
// Evaluate jacobian matrix
// Evaluate jacobian matrix.
ceres::CRSMatrix evaluated_jacobian;
ceres::Problem::EvaluateOptions eval_options;

Expand All @@ -453,11 +453,11 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
if (camera) {
// All cameras are variable now.
if (is_first_camera) {
problem.SetParameterBlockVariable(&cameras_R_t[i](0));
problem.SetParameterBlockVariable(&all_cameras_R_t[i](0));
is_first_camera = false;
}

eval_options.parameter_blocks.push_back(&cameras_R_t[i](0));
eval_options.parameter_blocks.push_back(&all_cameras_R_t[i](0));
}
}

Expand Down
6 changes: 3 additions & 3 deletions src/libmv/simple_pipeline/bundle.h
View file
Expand Up @@ -124,8 +124,8 @@ enum BundleConstraints {
BUNDLE_NO_TRANSLATION = 1,
};
void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
int bundle_intrinsics,
int bundle_constraints,
const int bundle_intrinsics,
const int bundle_constraints,
EuclideanReconstruction *reconstruction,
CameraIntrinsics *intrinsics,
BundleEvaluation *evaluation);
Expand All @@ -142,7 +142,7 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
The cameras and bundles (homogeneous 3D points) are refined in-place.
\note This assumes an outlier-free set of markers.
\note This assumes that radial distortion is already corrected for, but
\note This assumes that radial distortion is already corrected for, but
does not assume that that other intrinsics are.
\sa ProjectiveResect, ProjectiveIntersect, ProjectiveReconstructTwoFrames
Expand Down

0 comments on commit 7e16226

Please sign in to comment.