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