10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "bal_problem.h" 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cstdio> 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cstdlib> 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <string> 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "Eigen/Core" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/rotation.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 4079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "random.h" 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace examples { 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace { 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtypedef Eigen::Map<Eigen::VectorXd> VectorRef; 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtypedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef; 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T> 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid FscanfOrDie(FILE* fptr, const char* format, T* value) { 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_scanned = fscanf(fptr, format, value); 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (num_scanned != 1) { 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(FATAL) << "Invalid UW data file."; 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid PerturbPoint3(const double sigma, double* point) { 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; ++i) { 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong point[i] += RandNormal() * sigma; 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongdouble Median(std::vector<double>* data) { 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int n = data->size(); 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong std::vector<double>::iterator mid_point = data->begin() + n / 2; 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong std::nth_element(data->begin(), mid_point, data->end()); 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return *mid_point; 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongBALProblem::BALProblem(const std::string& filename, bool use_quaternions) { 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong FILE* fptr = fopen(filename.c_str(), "r"); 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (fptr == NULL) { 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(FATAL) << "Error: unable to open file " << filename; 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return; 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // This wil die horribly on invalid files. Them's the breaks. 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong FscanfOrDie(fptr, "%d", &num_cameras_); 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong FscanfOrDie(fptr, "%d", &num_points_); 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong FscanfOrDie(fptr, "%d", &num_observations_); 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(1) << "Header: " << num_cameras_ 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " " << num_points_ 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " " << num_observations_; 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong point_index_ = new int[num_observations_]; 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong camera_index_ = new int[num_observations_]; 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong observations_ = new double[2 * num_observations_]; 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_parameters_ = 9 * num_cameras_ + 3 * num_points_; 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong parameters_ = new double[num_parameters_]; 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_observations_; ++i) { 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong FscanfOrDie(fptr, "%d", camera_index_ + i); 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong FscanfOrDie(fptr, "%d", point_index_ + i); 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < 2; ++j) { 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong FscanfOrDie(fptr, "%lf", observations_ + 2*i + j); 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_parameters_; ++i) { 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong FscanfOrDie(fptr, "%lf", parameters_ + i); 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fclose(fptr); 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong use_quaternions_ = use_quaternions; 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (use_quaternions) { 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Switch the angle-axis rotations to quaternions. 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_parameters_ = 10 * num_cameras_ + 3 * num_points_; 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* quaternion_parameters = new double[num_parameters_]; 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* original_cursor = parameters_; 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* quaternion_cursor = quaternion_parameters; 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_cameras_; ++i) { 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToQuaternion(original_cursor, quaternion_cursor); 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion_cursor += 4; 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong original_cursor += 3; 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 4; j < 10; ++j) { 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *quaternion_cursor++ = *original_cursor++; 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Copy the rest of the points. 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3 * num_points_; ++i) { 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *quaternion_cursor++ = *original_cursor++; 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Swap in the quaternion parameters. 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong delete []parameters_; 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong parameters_ = quaternion_parameters; 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// This function writes the problem to a file in the same format that 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// is read by the constructor. 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid BALProblem::WriteToFile(const std::string& filename) const { 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong FILE* fptr = fopen(filename.c_str(), "w"); 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (fptr == NULL) { 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(FATAL) << "Error: unable to open file " << filename; 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return; 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fprintf(fptr, "%d %d %d\n", num_cameras_, num_points_, num_observations_); 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_observations_; ++i) { 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]); 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < 2; ++j) { 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fprintf(fptr, " %g", observations_[2 * i + j]); 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fprintf(fptr, "\n"); 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_cameras(); ++i) { 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double angleaxis[9]; 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (use_quaternions_) { 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Output in angle-axis format. 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToAngleAxis(parameters_ + 10 * i, angleaxis); 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong memcpy(angleaxis + 3, parameters_ + 10 * i + 4, 6 * sizeof(double)); 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong memcpy(angleaxis, parameters_ + 9 * i, 9 * sizeof(double)); 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < 9; ++j) { 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fprintf(fptr, "%.16g\n", angleaxis[j]); 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* points = parameters_ + camera_block_size() * num_cameras_; 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_points(); ++i) { 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* point = points + i * point_block_size(); 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < point_block_size(); ++j) { 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fprintf(fptr, "%.16g\n", point[j]); 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fclose(fptr); 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid BALProblem::CameraToAngleAxisAndCenter(const double* camera, 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* angle_axis, 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* center) { 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef angle_axis_ref(angle_axis, 3); 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (use_quaternions_) { 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToAngleAxis(camera, angle_axis); 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis_ref = ConstVectorRef(camera, 3); 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // c = -R't 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Eigen::VectorXd inverse_rotation = -angle_axis_ref; 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisRotatePoint(inverse_rotation.data(), 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong camera + camera_block_size() - 6, 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong center); 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef(center, 3) *= -1.0; 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid BALProblem::AngleAxisAndCenterToCamera(const double* angle_axis, 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* center, 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* camera) { 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ConstVectorRef angle_axis_ref(angle_axis, 3); 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (use_quaternions_) { 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisToQuaternion(angle_axis, camera); 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef(camera, 3) = angle_axis_ref; 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // t = -R * c 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisRotatePoint(angle_axis, 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong center, 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong camera + camera_block_size() - 6); 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef(camera + camera_block_size() - 6, 3) *= -1.0; 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid BALProblem::Normalize() { 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Compute the marginal median of the geometry. 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong std::vector<double> tmp(num_points_); 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Eigen::Vector3d median; 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* points = mutable_points(); 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; ++i) { 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < num_points_; ++j) { 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp[j] = points[3 * j + i]; 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong median(i) = Median(&tmp); 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_points_; ++i) { 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef point(points + 3 * i, 3); 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp[i] = (point - median).lpNorm<1>(); 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double median_absolute_deviation = Median(&tmp); 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Scale so that the median absolute deviation of the resulting 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // reconstruction is 100. 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double scale = 100.0 / median_absolute_deviation; 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(2) << "median: " << median.transpose(); 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(2) << "median absolute deviation: " << median_absolute_deviation; 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VLOG(2) << "scale: " << scale; 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // X = scale * (X - median) 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_points_; ++i) { 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef point(points + 3 * i, 3); 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong point = scale * (point - median); 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* cameras = mutable_cameras(); 2490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double angle_axis[3]; 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double center[3]; 2510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_cameras_; ++i) { 2520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* camera = cameras + camera_block_size() * i; 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CameraToAngleAxisAndCenter(camera, angle_axis, center); 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // center = scale * (center - median) 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef(center, 3) = scale * (VectorRef(center, 3) - median); 2560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisAndCenterToCamera(angle_axis, center, camera); 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid BALProblem::Perturb(const double rotation_sigma, 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double translation_sigma, 2620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double point_sigma) { 2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(point_sigma, 0.0); 2640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(rotation_sigma, 0.0); 2650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(translation_sigma, 0.0); 2660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* points = mutable_points(); 2680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (point_sigma > 0) { 2690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_points_; ++i) { 2700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong PerturbPoint3(point_sigma, points + 3 * i); 2710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_cameras_; ++i) { 2750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* camera = mutable_cameras() + camera_block_size() * i; 2760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double angle_axis[3]; 2780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double center[3]; 2790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Perturb in the rotation of the camera in the angle-axis 2800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // representation. 2810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CameraToAngleAxisAndCenter(camera, angle_axis, center); 2820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (rotation_sigma > 0.0) { 2830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong PerturbPoint3(rotation_sigma, angle_axis); 2840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AngleAxisAndCenterToCamera(angle_axis, center, camera); 2860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (translation_sigma > 0.0) { 2880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong PerturbPoint3(translation_sigma, camera + camera_block_size() - 6); 2890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongBALProblem::~BALProblem() { 2940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong delete []point_index_; 2950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong delete []camera_index_; 2960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong delete []observations_; 2970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong delete []parameters_; 2980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace examples 3010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 302