Randomly perturb the bundle adjustment problem. 1. Add the ability to perturb the camera pose and the point positions using user specified parameters. 2. Re-order the flags. 3. Minor name correction. 4. Added Box-Mueller generator to random.h Change-Id: I2c9ce74c237f5bde9a7299cc71b205d1ca9bc742
diff --git a/examples/bal_problem.cc b/examples/bal_problem.cc index 6c65a20..f73e805 100644 --- a/examples/bal_problem.cc +++ b/examples/bal_problem.cc
@@ -34,6 +34,7 @@ #include <cstdlib> #include <string> #include <glog/logging.h> +#include "ceres/random.h" #include "ceres/rotation.h" namespace ceres { @@ -110,6 +111,54 @@ } } +void BALProblem::Perturb(const double rotation_sigma, + const double translation_sigma, + const double point_sigma) { + CHECK_GE(point_sigma, 0.0); + CHECK_GE(rotation_sigma, 0.0); + CHECK_GE(translation_sigma, 0.0); + + double* points = mutable_points(); + if (point_sigma > 0) { + for (int i = 0; i < 3 * num_points_; ++i) { + points[i] += point_sigma * RandNormal(); + } + } + + for (int i = 0; i < num_cameras_; ++i) { + double* camera = mutable_cameras() + camera_block_size() * i; + + // First three coordinates of the camera rotation are shared + // between the angle-axis and the quaternion representations. + if (rotation_sigma > 0.0) { + camera[0] += rotation_sigma * RandNormal(); + camera[1] += rotation_sigma * RandNormal(); + camera[2] += rotation_sigma * RandNormal(); + + if (use_quaternions_) { + camera[4] += rotation_sigma * RandNormal(); + + // Normalize the quaternion. + double norm = 0.0; + for (int j = 0; j < 4; ++j) { + norm += camera[j] * camera[j]; + } + norm = sqrt(norm); + for (int j = 0; j < 4; ++j) { + camera[j] /= norm; + } + } + } + + if (translation_sigma > 0.0) { + // Translation. + camera[camera_block_size() - 5] += translation_sigma * RandNormal(); + camera[camera_block_size() - 4] += translation_sigma * RandNormal(); + camera[camera_block_size() - 3] += translation_sigma * RandNormal(); + } + } +} + BALProblem::~BALProblem() { delete []point_index_; delete []camera_index_;
diff --git a/examples/bal_problem.h b/examples/bal_problem.h index ff411b8..95e8d8f 100644 --- a/examples/bal_problem.h +++ b/examples/bal_problem.h
@@ -47,6 +47,12 @@ explicit BALProblem(const std::string filename, bool use_quaternions); ~BALProblem(); + // Perturb the camera pose and the geometry with random normal + // numbers with corresponding standard deviations. + void Perturb(const double rotation_sigma, + const double translation_sigma, + const double point_sigma); + int camera_block_size() const { return use_quaternions_ ? 10 : 9; } int point_block_size() const { return 3; } int num_cameras() const { return num_cameras_; }
diff --git a/examples/bundle_adjuster.cc b/examples/bundle_adjuster.cc index bb8d3b4..8f1b38b 100644 --- a/examples/bundle_adjuster.cc +++ b/examples/bundle_adjuster.cc
@@ -62,8 +62,19 @@ #include "bal_problem.h" #include "snavely_reprojection_error.h" #include "ceres/ceres.h" +#include "ceres/random.h" DEFINE_string(input, "", "Input File name"); +DEFINE_bool(use_quaternions, false, "If true, uses quaternions to represent " + "rotations. If false, angle axis is used"); +DEFINE_bool(use_local_parameterization, false, "For quaternions, use a local " + "parameterization."); +DEFINE_bool(robustify, false, "Use a robust loss function"); + +DEFINE_string(trust_region_strategy, "lm", "Options are: lm, dogleg"); +DEFINE_double(eta, 1e-2, "Default value for eta. Eta determines the " + "accuracy of each linear solve of the truncated newton step. " + "Changing this parameter can affect solve performance "); DEFINE_string(solver_type, "sparse_schur", "Options are: " "sparse_schur, dense_schur, iterative_schur, cholesky, " "dense_qr, and conjugate_gradients"); @@ -72,23 +83,27 @@ "cluster_tridiagonal"); DEFINE_string(sparse_linear_algebra_library, "suitesparse", "Options are: suitesparse and cxsparse"); -DEFINE_int32(num_iterations, 5, "Number of iterations"); -DEFINE_int32(num_threads, 1, "Number of threads"); -DEFINE_double(eta, 1e-2, "Default value for eta. Eta determines the " - "accuracy of each linear solve of the truncated newton step. " - "Changing this parameter can affect solve performance "); + DEFINE_string(ordering_type, "schur", "Options are: schur, user, natural"); -DEFINE_bool(use_quaternions, false, "If true, uses quaternions to represent " - "rotations. If false, angle axis is used"); -DEFINE_bool(use_local_parameterization, false, "For quaternions, use a local " - "parameterization."); -DEFINE_bool(robustify, false, "Use a robust loss function"); -DEFINE_bool(use_block_amd, true, "Use a block oriented fill reducing ordering."); -DEFINE_string(trust_region_strategy, "lm", "Options are: lm, dogleg"); +DEFINE_bool(use_block_amd, true, "Use a block oriented fill reducing " + "ordering."); + +DEFINE_int32(num_threads, 1, "Number of threads"); +DEFINE_int32(num_iterations, 5, "Number of iterations"); DEFINE_double(max_solver_time, 1e32, "Maximum solve time in seconds."); DEFINE_bool(nonmonotonic_steps, false, "Trust region algorithm can use" " nonmonotic steps"); +DEFINE_double(rotation_sigma, 0.0, "Standard deviation of camera rotation " + "perturbation."); +DEFINE_double(translation_sigma, 0.0, "Standard deviation of the camera " + "translation perturbation."); +DEFINE_double(point_sigma, 0.0, "Standard deviation of the point " + "perturbation"); +DEFINE_int32(random_seed, 38401, "Random seed used to set the state " + "of the pseudo random number generator used to generate " + "the pertubations."); + namespace ceres { namespace examples { @@ -242,6 +257,7 @@ } void BuildProblem(BALProblem* bal_problem, Problem* problem) { + SetRandomState(FLAGS_random_seed); const int point_block_size = bal_problem->point_block_size(); const int camera_block_size = bal_problem->camera_block_size(); double* points = bal_problem->mutable_points(); @@ -258,8 +274,8 @@ // outputs a 2 dimensional residual. if (FLAGS_use_quaternions) { cost_function = new AutoDiffCostFunction< - SnavelyReprojectionErrorWitQuaternions, 2, 4, 6, 3>( - new SnavelyReprojectionErrorWitQuaternions( + SnavelyReprojectionErrorWithQuaternions, 2, 4, 6, 3>( + new SnavelyReprojectionErrorWithQuaternions( observations[2 * i + 0], observations[2 * i + 1])); } else { @@ -307,6 +323,12 @@ void SolveProblem(const char* filename) { BALProblem bal_problem(filename, FLAGS_use_quaternions); Problem problem; + + SetRandomState(FLAGS_random_seed); + bal_problem.Perturb(FLAGS_rotation_sigma, + FLAGS_translation_sigma, + FLAGS_point_sigma); + BuildProblem(&bal_problem, &problem); Solver::Options options; SetSolverOptionsFromFlags(&bal_problem, &options);
diff --git a/examples/snavely_reprojection_error.h b/examples/snavely_reprojection_error.h index e43538f..eaf4129 100644 --- a/examples/snavely_reprojection_error.h +++ b/examples/snavely_reprojection_error.h
@@ -100,10 +100,10 @@ // translation, 1 for focal length and 2 for radial distortion. The // principal point is not modeled (i.e. it is assumed be located at // the image center). -struct SnavelyReprojectionErrorWitQuaternions { +struct SnavelyReprojectionErrorWithQuaternions { // (u, v): the position of the observation with respect to the image // center point. - SnavelyReprojectionErrorWitQuaternions(double observed_x, double observed_y) + SnavelyReprojectionErrorWithQuaternions(double observed_x, double observed_y) : observed_x(observed_x), observed_y(observed_y) {} template <typename T>
diff --git a/internal/ceres/random.h b/internal/ceres/random.h index 769e0b4..cd7a0ea 100644 --- a/internal/ceres/random.h +++ b/internal/ceres/random.h
@@ -27,19 +27,41 @@ // POSSIBILITY OF SUCH DAMAGE. // // Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) #ifndef CERES_INTERNAL_RANDOM_H_ #define CERES_INTERNAL_RANDOM_H_ +#include <cmath> +#include <cstdlib> + namespace ceres { -inline double RandDouble() { - double r = rand(); - return r / RAND_MAX; +inline void SetRandomState(int state) { + srandom(state); } inline int Uniform(int n) { - return rand() % n; + return random() % n; +} + +inline double RandDouble() { + double r = static_cast<double>(random()); + return r / RAND_MAX; +} + +// Box-Muller algorithm for normal random number generation. +// http://en.wikipedia.org/wiki/Box-Muller_transform +inline double RandNormal() { + double x1, x2, w; + do { + x1 = 2.0 * RandDouble() - 1.0; + x2 = 2.0 * RandDouble() - 1.0; + w = x1 * x1 + x2 * x2; + } while ( w >= 1.0 || w == 0.0 ); + + w = sqrt((-2.0 * log(w)) / w); + return x1 * w; } } // namespace ceres