1// Ceres Solver - A fast non-linear least squares minimizer 2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3// http://code.google.com/p/ceres-solver/ 4// 5// Redistribution and use in source and binary forms, with or without 6// modification, are permitted provided that the following conditions are met: 7// 8// * Redistributions of source code must retain the above copyright notice, 9// this list of conditions and the following disclaimer. 10// * Redistributions in binary form must reproduce the above copyright notice, 11// this list of conditions and the following disclaimer in the documentation 12// and/or other materials provided with the distribution. 13// * Neither the name of Google Inc. nor the names of its contributors may be 14// used to endorse or promote products derived from this software without 15// specific prior written permission. 16// 17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27// POSSIBILITY OF SUCH DAMAGE. 28// 29// Author: sameeragarwal@google.com (Sameer Agarwal) 30 31#include "bal_problem.h" 32 33#include <cstdio> 34#include <cstdlib> 35#include <string> 36#include <vector> 37#include "Eigen/Core" 38#include "ceres/rotation.h" 39#include "glog/logging.h" 40 41namespace ceres { 42namespace examples { 43namespace { 44typedef Eigen::Map<Eigen::VectorXd> VectorRef; 45typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef; 46 47inline double RandDouble() { 48 double r = static_cast<double>(rand()); 49 return r / RAND_MAX; 50} 51 52// Box-Muller algorithm for normal random number generation. 53// http://en.wikipedia.org/wiki/Box-Muller_transform 54inline double RandNormal() { 55 double x1, x2, w; 56 do { 57 x1 = 2.0 * RandDouble() - 1.0; 58 x2 = 2.0 * RandDouble() - 1.0; 59 w = x1 * x1 + x2 * x2; 60 } while ( w >= 1.0 || w == 0.0 ); 61 62 w = sqrt((-2.0 * log(w)) / w); 63 return x1 * w; 64} 65 66template<typename T> 67void FscanfOrDie(FILE* fptr, const char* format, T* value) { 68 int num_scanned = fscanf(fptr, format, value); 69 if (num_scanned != 1) { 70 LOG(FATAL) << "Invalid UW data file."; 71 } 72} 73 74void PerturbPoint3(const double sigma, double* point) { 75 for (int i = 0; i < 3; ++i) { 76 point[i] += RandNormal() * sigma; 77 } 78} 79 80double Median(std::vector<double>* data) { 81 int n = data->size(); 82 std::vector<double>::iterator mid_point = data->begin() + n / 2; 83 std::nth_element(data->begin(), mid_point, data->end()); 84 return *mid_point; 85} 86 87} // namespace 88 89BALProblem::BALProblem(const std::string& filename, bool use_quaternions) { 90 FILE* fptr = fopen(filename.c_str(), "r"); 91 92 if (fptr == NULL) { 93 LOG(FATAL) << "Error: unable to open file " << filename; 94 return; 95 }; 96 97 // This wil die horribly on invalid files. Them's the breaks. 98 FscanfOrDie(fptr, "%d", &num_cameras_); 99 FscanfOrDie(fptr, "%d", &num_points_); 100 FscanfOrDie(fptr, "%d", &num_observations_); 101 102 VLOG(1) << "Header: " << num_cameras_ 103 << " " << num_points_ 104 << " " << num_observations_; 105 106 point_index_ = new int[num_observations_]; 107 camera_index_ = new int[num_observations_]; 108 observations_ = new double[2 * num_observations_]; 109 110 num_parameters_ = 9 * num_cameras_ + 3 * num_points_; 111 parameters_ = new double[num_parameters_]; 112 113 for (int i = 0; i < num_observations_; ++i) { 114 FscanfOrDie(fptr, "%d", camera_index_ + i); 115 FscanfOrDie(fptr, "%d", point_index_ + i); 116 for (int j = 0; j < 2; ++j) { 117 FscanfOrDie(fptr, "%lf", observations_ + 2*i + j); 118 } 119 } 120 121 for (int i = 0; i < num_parameters_; ++i) { 122 FscanfOrDie(fptr, "%lf", parameters_ + i); 123 } 124 125 fclose(fptr); 126 127 use_quaternions_ = use_quaternions; 128 if (use_quaternions) { 129 // Switch the angle-axis rotations to quaternions. 130 num_parameters_ = 10 * num_cameras_ + 3 * num_points_; 131 double* quaternion_parameters = new double[num_parameters_]; 132 double* original_cursor = parameters_; 133 double* quaternion_cursor = quaternion_parameters; 134 for (int i = 0; i < num_cameras_; ++i) { 135 AngleAxisToQuaternion(original_cursor, quaternion_cursor); 136 quaternion_cursor += 4; 137 original_cursor += 3; 138 for (int j = 4; j < 10; ++j) { 139 *quaternion_cursor++ = *original_cursor++; 140 } 141 } 142 // Copy the rest of the points. 143 for (int i = 0; i < 3 * num_points_; ++i) { 144 *quaternion_cursor++ = *original_cursor++; 145 } 146 // Swap in the quaternion parameters. 147 delete []parameters_; 148 parameters_ = quaternion_parameters; 149 } 150} 151 152// This function writes the problem to a file in the same format that 153// is read by the constructor. 154void BALProblem::WriteToFile(const std::string& filename) const { 155 FILE* fptr = fopen(filename.c_str(), "w"); 156 157 if (fptr == NULL) { 158 LOG(FATAL) << "Error: unable to open file " << filename; 159 return; 160 }; 161 162 fprintf(fptr, "%d %d %d\n", num_cameras_, num_points_, num_observations_); 163 164 for (int i = 0; i < num_observations_; ++i) { 165 fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]); 166 for (int j = 0; j < 2; ++j) { 167 fprintf(fptr, " %g", observations_[2 * i + j]); 168 } 169 fprintf(fptr, "\n"); 170 } 171 172 for (int i = 0; i < num_cameras(); ++i) { 173 double angleaxis[9]; 174 if (use_quaternions_) { 175 // Output in angle-axis format. 176 QuaternionToAngleAxis(parameters_ + 10 * i, angleaxis); 177 memcpy(angleaxis + 3, parameters_ + 10 * i + 4, 6 * sizeof(double)); 178 } else { 179 memcpy(angleaxis, parameters_ + 9 * i, 9 * sizeof(double)); 180 } 181 for (int j = 0; j < 9; ++j) { 182 fprintf(fptr, "%.16g\n", angleaxis[j]); 183 } 184 } 185 186 const double* points = parameters_ + camera_block_size() * num_cameras_; 187 for (int i = 0; i < num_points(); ++i) { 188 const double* point = points + i * point_block_size(); 189 for (int j = 0; j < point_block_size(); ++j) { 190 fprintf(fptr, "%.16g\n", point[j]); 191 } 192 } 193 194 fclose(fptr); 195} 196 197void BALProblem::CameraToAngleAxisAndCenter(const double* camera, 198 double* angle_axis, 199 double* center) { 200 VectorRef angle_axis_ref(angle_axis, 3); 201 if (use_quaternions_) { 202 QuaternionToAngleAxis(camera, angle_axis); 203 } else { 204 angle_axis_ref = ConstVectorRef(camera, 3); 205 } 206 207 // c = -R't 208 Eigen::VectorXd inverse_rotation = -angle_axis_ref; 209 AngleAxisRotatePoint(inverse_rotation.data(), 210 camera + camera_block_size() - 6, 211 center); 212 VectorRef(center, 3) *= -1.0; 213} 214 215void BALProblem::AngleAxisAndCenterToCamera(const double* angle_axis, 216 const double* center, 217 double* camera) { 218 ConstVectorRef angle_axis_ref(angle_axis, 3); 219 if (use_quaternions_) { 220 AngleAxisToQuaternion(angle_axis, camera); 221 } else { 222 VectorRef(camera, 3) = angle_axis_ref; 223 } 224 225 // t = -R * c 226 AngleAxisRotatePoint(angle_axis, 227 center, 228 camera + camera_block_size() - 6); 229 VectorRef(camera + camera_block_size() - 6, 3) *= -1.0; 230} 231 232 233void BALProblem::Normalize() { 234 // Compute the marginal median of the geometry. 235 std::vector<double> tmp(num_points_); 236 Eigen::Vector3d median; 237 double* points = mutable_points(); 238 for (int i = 0; i < 3; ++i) { 239 for (int j = 0; j < num_points_; ++j) { 240 tmp[j] = points[3 * j + i]; 241 } 242 median(i) = Median(&tmp); 243 } 244 245 for (int i = 0; i < num_points_; ++i) { 246 VectorRef point(points + 3 * i, 3); 247 tmp[i] = (point - median).lpNorm<1>(); 248 } 249 250 const double median_absolute_deviation = Median(&tmp); 251 252 // Scale so that the median absolute deviation of the resulting 253 // reconstruction is 100. 254 const double scale = 100.0 / median_absolute_deviation; 255 256 VLOG(2) << "median: " << median.transpose(); 257 VLOG(2) << "median absolute deviation: " << median_absolute_deviation; 258 VLOG(2) << "scale: " << scale; 259 260 // X = scale * (X - median) 261 for (int i = 0; i < num_points_; ++i) { 262 VectorRef point(points + 3 * i, 3); 263 point = scale * (point - median); 264 } 265 266 double* cameras = mutable_cameras(); 267 double angle_axis[3]; 268 double center[3]; 269 for (int i = 0; i < num_cameras_; ++i) { 270 double* camera = cameras + camera_block_size() * i; 271 CameraToAngleAxisAndCenter(camera, angle_axis, center); 272 // center = scale * (center - median) 273 VectorRef(center, 3) = scale * (VectorRef(center, 3) - median); 274 AngleAxisAndCenterToCamera(angle_axis, center, camera); 275 } 276} 277 278void BALProblem::Perturb(const double rotation_sigma, 279 const double translation_sigma, 280 const double point_sigma) { 281 CHECK_GE(point_sigma, 0.0); 282 CHECK_GE(rotation_sigma, 0.0); 283 CHECK_GE(translation_sigma, 0.0); 284 285 double* points = mutable_points(); 286 if (point_sigma > 0) { 287 for (int i = 0; i < num_points_; ++i) { 288 PerturbPoint3(point_sigma, points + 3 * i); 289 } 290 } 291 292 for (int i = 0; i < num_cameras_; ++i) { 293 double* camera = mutable_cameras() + camera_block_size() * i; 294 295 double angle_axis[3]; 296 double center[3]; 297 // Perturb in the rotation of the camera in the angle-axis 298 // representation. 299 CameraToAngleAxisAndCenter(camera, angle_axis, center); 300 if (rotation_sigma > 0.0) { 301 PerturbPoint3(rotation_sigma, angle_axis); 302 } 303 AngleAxisAndCenterToCamera(angle_axis, center, camera); 304 305 if (translation_sigma > 0.0) { 306 PerturbPoint3(translation_sigma, camera + camera_block_size() - 6); 307 } 308 } 309} 310 311BALProblem::~BALProblem() { 312 delete []point_index_; 313 delete []camera_index_; 314 delete []observations_; 315 delete []parameters_; 316} 317 318} // namespace examples 319} // namespace ceres 320