diff options
Diffstat (limited to 'examples/bal_problem.cc')
-rw-r--r-- | examples/bal_problem.cc | 301 |
1 files changed, 301 insertions, 0 deletions
diff --git a/examples/bal_problem.cc b/examples/bal_problem.cc new file mode 100644 index 0000000..5733f46 --- /dev/null +++ b/examples/bal_problem.cc @@ -0,0 +1,301 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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 <string> +#include <vector> +#include "Eigen/Core" +#include "ceres/random.h" +#include "ceres/rotation.h" +#include "glog/logging.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); +} + +void BALProblem::CameraToAngleAxisAndCenter(const double* camera, + double* angle_axis, + double* center) { + 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) { + 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 |