Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/examples/bal_problem.cc b/examples/bal_problem.cc
new file mode 100644
index 0000000..1ee3c81
--- /dev/null
+++ b/examples/bal_problem.cc
@@ -0,0 +1,339 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "bal_problem.h"
+
+#include <cstdio>
+#include <cstdlib>
+#include <fstream>
+#include <string>
+#include <vector>
+#include "Eigen/Core"
+#include "ceres/rotation.h"
+#include "glog/logging.h"
+#include "random.h"
+
+namespace ceres {
+namespace examples {
+namespace {
+typedef Eigen::Map<Eigen::VectorXd> VectorRef;
+typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
+
+template<typename T>
+void FscanfOrDie(FILE* fptr, const char* format, T* value) {
+ int num_scanned = fscanf(fptr, format, value);
+ if (num_scanned != 1) {
+ LOG(FATAL) << "Invalid UW data file.";
+ }
+}
+
+void PerturbPoint3(const double sigma, double* point) {
+ for (int i = 0; i < 3; ++i) {
+ point[i] += RandNormal() * sigma;
+ }
+}
+
+double Median(std::vector<double>* data) {
+ int n = data->size();
+ std::vector<double>::iterator mid_point = data->begin() + n / 2;
+ std::nth_element(data->begin(), mid_point, data->end());
+ return *mid_point;
+}
+
+} // namespace
+
+BALProblem::BALProblem(const std::string& filename, bool use_quaternions) {
+ FILE* fptr = fopen(filename.c_str(), "r");
+
+ if (fptr == NULL) {
+ LOG(FATAL) << "Error: unable to open file " << filename;
+ return;
+ };
+
+ // This wil die horribly on invalid files. Them's the breaks.
+ FscanfOrDie(fptr, "%d", &num_cameras_);
+ FscanfOrDie(fptr, "%d", &num_points_);
+ FscanfOrDie(fptr, "%d", &num_observations_);
+
+ VLOG(1) << "Header: " << num_cameras_
+ << " " << num_points_
+ << " " << num_observations_;
+
+ point_index_ = new int[num_observations_];
+ camera_index_ = new int[num_observations_];
+ observations_ = new double[2 * num_observations_];
+
+ num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
+ parameters_ = new double[num_parameters_];
+
+ for (int i = 0; i < num_observations_; ++i) {
+ FscanfOrDie(fptr, "%d", camera_index_ + i);
+ FscanfOrDie(fptr, "%d", point_index_ + i);
+ for (int j = 0; j < 2; ++j) {
+ FscanfOrDie(fptr, "%lf", observations_ + 2*i + j);
+ }
+ }
+
+ for (int i = 0; i < num_parameters_; ++i) {
+ FscanfOrDie(fptr, "%lf", parameters_ + i);
+ }
+
+ fclose(fptr);
+
+ use_quaternions_ = use_quaternions;
+ if (use_quaternions) {
+ // Switch the angle-axis rotations to quaternions.
+ num_parameters_ = 10 * num_cameras_ + 3 * num_points_;
+ double* quaternion_parameters = new double[num_parameters_];
+ double* original_cursor = parameters_;
+ double* quaternion_cursor = quaternion_parameters;
+ for (int i = 0; i < num_cameras_; ++i) {
+ AngleAxisToQuaternion(original_cursor, quaternion_cursor);
+ quaternion_cursor += 4;
+ original_cursor += 3;
+ for (int j = 4; j < 10; ++j) {
+ *quaternion_cursor++ = *original_cursor++;
+ }
+ }
+ // Copy the rest of the points.
+ for (int i = 0; i < 3 * num_points_; ++i) {
+ *quaternion_cursor++ = *original_cursor++;
+ }
+ // Swap in the quaternion parameters.
+ delete []parameters_;
+ parameters_ = quaternion_parameters;
+ }
+}
+
+// This function writes the problem to a file in the same format that
+// is read by the constructor.
+void BALProblem::WriteToFile(const std::string& filename) const {
+ FILE* fptr = fopen(filename.c_str(), "w");
+
+ if (fptr == NULL) {
+ LOG(FATAL) << "Error: unable to open file " << filename;
+ return;
+ };
+
+ fprintf(fptr, "%d %d %d\n", num_cameras_, num_points_, num_observations_);
+
+ for (int i = 0; i < num_observations_; ++i) {
+ fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]);
+ for (int j = 0; j < 2; ++j) {
+ fprintf(fptr, " %g", observations_[2 * i + j]);
+ }
+ fprintf(fptr, "\n");
+ }
+
+ for (int i = 0; i < num_cameras(); ++i) {
+ double angleaxis[9];
+ if (use_quaternions_) {
+ // Output in angle-axis format.
+ QuaternionToAngleAxis(parameters_ + 10 * i, angleaxis);
+ memcpy(angleaxis + 3, parameters_ + 10 * i + 4, 6 * sizeof(double));
+ } else {
+ memcpy(angleaxis, parameters_ + 9 * i, 9 * sizeof(double));
+ }
+ for (int j = 0; j < 9; ++j) {
+ fprintf(fptr, "%.16g\n", angleaxis[j]);
+ }
+ }
+
+ const double* points = parameters_ + camera_block_size() * num_cameras_;
+ for (int i = 0; i < num_points(); ++i) {
+ const double* point = points + i * point_block_size();
+ for (int j = 0; j < point_block_size(); ++j) {
+ fprintf(fptr, "%.16g\n", point[j]);
+ }
+ }
+
+ fclose(fptr);
+}
+
+// Write the problem to a PLY file for inspection in Meshlab or CloudCompare.
+void BALProblem::WriteToPLYFile(const std::string& filename) const {
+ std::ofstream of(filename.c_str());
+
+ of << "ply"
+ << '\n' << "format ascii 1.0"
+ << '\n' << "element vertex " << num_cameras_ + num_points_
+ << '\n' << "property float x"
+ << '\n' << "property float y"
+ << '\n' << "property float z"
+ << '\n' << "property uchar red"
+ << '\n' << "property uchar green"
+ << '\n' << "property uchar blue"
+ << '\n' << "end_header" << std::endl;
+
+ // Export extrinsic data (i.e. camera centers) as green points.
+ double angle_axis[3];
+ double center[3];
+ for (int i = 0; i < num_cameras(); ++i) {
+ const double* camera = cameras() + camera_block_size() * i;
+ CameraToAngleAxisAndCenter(camera, angle_axis, center);
+ of << center[0] << ' ' << center[1] << ' ' << center[2]
+ << " 0 255 0" << '\n';
+ }
+
+ // Export the structure (i.e. 3D Points) as white points.
+ const double* points = parameters_ + camera_block_size() * num_cameras_;
+ for (int i = 0; i < num_points(); ++i) {
+ const double* point = points + i * point_block_size();
+ for (int j = 0; j < point_block_size(); ++j) {
+ of << point[j] << ' ';
+ }
+ of << "255 255 255\n";
+ }
+ of.close();
+}
+
+void BALProblem::CameraToAngleAxisAndCenter(const double* camera,
+ double* angle_axis,
+ double* center) const {
+ VectorRef angle_axis_ref(angle_axis, 3);
+ if (use_quaternions_) {
+ QuaternionToAngleAxis(camera, angle_axis);
+ } else {
+ angle_axis_ref = ConstVectorRef(camera, 3);
+ }
+
+ // c = -R't
+ Eigen::VectorXd inverse_rotation = -angle_axis_ref;
+ AngleAxisRotatePoint(inverse_rotation.data(),
+ camera + camera_block_size() - 6,
+ center);
+ VectorRef(center, 3) *= -1.0;
+}
+
+void BALProblem::AngleAxisAndCenterToCamera(const double* angle_axis,
+ const double* center,
+ double* camera) const {
+ ConstVectorRef angle_axis_ref(angle_axis, 3);
+ if (use_quaternions_) {
+ AngleAxisToQuaternion(angle_axis, camera);
+ } else {
+ VectorRef(camera, 3) = angle_axis_ref;
+ }
+
+ // t = -R * c
+ AngleAxisRotatePoint(angle_axis,
+ center,
+ camera + camera_block_size() - 6);
+ VectorRef(camera + camera_block_size() - 6, 3) *= -1.0;
+}
+
+
+void BALProblem::Normalize() {
+ // Compute the marginal median of the geometry.
+ std::vector<double> tmp(num_points_);
+ Eigen::Vector3d median;
+ double* points = mutable_points();
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < num_points_; ++j) {
+ tmp[j] = points[3 * j + i];
+ }
+ median(i) = Median(&tmp);
+ }
+
+ for (int i = 0; i < num_points_; ++i) {
+ VectorRef point(points + 3 * i, 3);
+ tmp[i] = (point - median).lpNorm<1>();
+ }
+
+ const double median_absolute_deviation = Median(&tmp);
+
+ // Scale so that the median absolute deviation of the resulting
+ // reconstruction is 100.
+ const double scale = 100.0 / median_absolute_deviation;
+
+ VLOG(2) << "median: " << median.transpose();
+ VLOG(2) << "median absolute deviation: " << median_absolute_deviation;
+ VLOG(2) << "scale: " << scale;
+
+ // X = scale * (X - median)
+ for (int i = 0; i < num_points_; ++i) {
+ VectorRef point(points + 3 * i, 3);
+ point = scale * (point - median);
+ }
+
+ double* cameras = mutable_cameras();
+ double angle_axis[3];
+ double center[3];
+ for (int i = 0; i < num_cameras_; ++i) {
+ double* camera = cameras + camera_block_size() * i;
+ CameraToAngleAxisAndCenter(camera, angle_axis, center);
+ // center = scale * (center - median)
+ VectorRef(center, 3) = scale * (VectorRef(center, 3) - median);
+ AngleAxisAndCenterToCamera(angle_axis, center, camera);
+ }
+}
+
+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 < num_points_; ++i) {
+ PerturbPoint3(point_sigma, points + 3 * i);
+ }
+ }
+
+ for (int i = 0; i < num_cameras_; ++i) {
+ double* camera = mutable_cameras() + camera_block_size() * i;
+
+ double angle_axis[3];
+ double center[3];
+ // Perturb in the rotation of the camera in the angle-axis
+ // representation.
+ CameraToAngleAxisAndCenter(camera, angle_axis, center);
+ if (rotation_sigma > 0.0) {
+ PerturbPoint3(rotation_sigma, angle_axis);
+ }
+ AngleAxisAndCenterToCamera(angle_axis, center, camera);
+
+ if (translation_sigma > 0.0) {
+ PerturbPoint3(translation_sigma, camera + camera_block_size() - 6);
+ }
+ }
+}
+
+BALProblem::~BALProblem() {
+ delete []point_index_;
+ delete []camera_index_;
+ delete []observations_;
+ delete []parameters_;
+}
+
+} // namespace examples
+} // namespace ceres