examples/bal_problem.cc

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   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 <fstream>
  36 #include <string>
  37 #include <vector>
  38 #include "Eigen/Core"
  39 #include "ceres/rotation.h"
  40 #include "glog/logging.h"
  41 #include "random.h"
  42
  43 namespace ceres {
  44 namespace examples {
  45 namespace {
  46 typedef Eigen::Map<Eigen::VectorXd> VectorRef;
  47 typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
  48
  49 template<typename T>
  50 void FscanfOrDie(FILE* fptr, const char* format, T* value) {
  51   int num_scanned = fscanf(fptr, format, value);
  52   if (num_scanned != 1) {
  53     LOG(FATAL) << "Invalid UW data file.";
  54   }
  55 }
  56
  57 void PerturbPoint3(const double sigma, double* point) {
  58   for (int i = 0; i < 3; ++i) {
  59     point[i] += RandNormal() * sigma;
  60   }
  61 }
  62
  63 double Median(std::vector<double>* data) {
  64   int n = data->size();
  65   std::vector<double>::iterator mid_point = data->begin() + n / 2;
  66   std::nth_element(data->begin(), mid_point, data->end());
  67   return *mid_point;
  68 }
  69
  70 }  // namespace
  71
  72 BALProblem::BALProblem(const std::string& filename, bool use_quaternions) {
  73   FILE* fptr = fopen(filename.c_str(), "r");
  74
  75   if (fptr == NULL) {
  76     LOG(FATAL) << "Error: unable to open file " << filename;
  77     return;
  78   };
  79
  80   // This wil die horribly on invalid files. Them's the breaks.
  81   FscanfOrDie(fptr, "%d", &num_cameras_);
  82   FscanfOrDie(fptr, "%d", &num_points_);
  83   FscanfOrDie(fptr, "%d", &num_observations_);
  84
  85   VLOG(1) << "Header: " << num_cameras_
  86           << " " << num_points_
  87           << " " << num_observations_;
  88
  89   point_index_ = new int[num_observations_];
  90   camera_index_ = new int[num_observations_];
  91   observations_ = new double[2 * num_observations_];
  92
  93   num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
  94   parameters_ = new double[num_parameters_];
  95
  96   for (int i = 0; i < num_observations_; ++i) {
  97     FscanfOrDie(fptr, "%d", camera_index_ + i);
  98     FscanfOrDie(fptr, "%d", point_index_ + i);
  99     for (int j = 0; j < 2; ++j) {
 100       FscanfOrDie(fptr, "%lf", observations_ + 2*i + j);
 101     }
 102   }
 103
 104   for (int i = 0; i < num_parameters_; ++i) {
 105     FscanfOrDie(fptr, "%lf", parameters_ + i);
 106   }
 107
 108   fclose(fptr);
 109
 110   use_quaternions_ = use_quaternions;
 111   if (use_quaternions) {
 112     // Switch the angle-axis rotations to quaternions.
 113     num_parameters_ = 10 * num_cameras_ + 3 * num_points_;
 114     double* quaternion_parameters = new double[num_parameters_];
 115     double* original_cursor = parameters_;
 116     double* quaternion_cursor = quaternion_parameters;
 117     for (int i = 0; i < num_cameras_; ++i) {
 118       AngleAxisToQuaternion(original_cursor, quaternion_cursor);
 119       quaternion_cursor += 4;
 120       original_cursor += 3;
 121       for (int j = 4; j < 10; ++j) {
 122        *quaternion_cursor++ = *original_cursor++;
 123       }
 124     }
 125     // Copy the rest of the points.
 126     for (int i = 0; i < 3 * num_points_; ++i) {
 127       *quaternion_cursor++ = *original_cursor++;
 128     }
 129     // Swap in the quaternion parameters.
 130     delete []parameters_;
 131     parameters_ = quaternion_parameters;
 132   }
 133 }
 134
 135 // This function writes the problem to a file in the same format that
 136 // is read by the constructor.
 137 void BALProblem::WriteToFile(const std::string& filename) const {
 138   FILE* fptr = fopen(filename.c_str(), "w");
 139
 140   if (fptr == NULL) {
 141     LOG(FATAL) << "Error: unable to open file " << filename;
 142     return;
 143   };
 144
 145   fprintf(fptr, "%d %d %d\n", num_cameras_, num_points_, num_observations_);
 146
 147   for (int i = 0; i < num_observations_; ++i) {
 148     fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]);
 149     for (int j = 0; j < 2; ++j) {
 150       fprintf(fptr, " %g", observations_[2 * i + j]);
 151     }
 152     fprintf(fptr, "\n");
 153   }
 154
 155   for (int i = 0; i < num_cameras(); ++i) {
 156     double angleaxis[9];
 157     if (use_quaternions_) {
 158       // Output in angle-axis format.
 159       QuaternionToAngleAxis(parameters_ + 10 * i, angleaxis);
 160       memcpy(angleaxis + 3, parameters_ + 10 * i + 4, 6 * sizeof(double));
 161     } else {
 162       memcpy(angleaxis, parameters_ + 9 * i, 9 * sizeof(double));
 163     }
 164     for (int j = 0; j < 9; ++j) {
 165       fprintf(fptr, "%.16g\n", angleaxis[j]);
 166     }
 167   }
 168
 169   const double* points = parameters_ + camera_block_size() * num_cameras_;
 170   for (int i = 0; i < num_points(); ++i) {
 171     const double* point = points + i * point_block_size();
 172     for (int j = 0; j < point_block_size(); ++j) {
 173       fprintf(fptr, "%.16g\n", point[j]);
 174     }
 175   }
 176
 177   fclose(fptr);
 178 }
 179
 180 // Write the problem to a PLY file for inspection in Meshlab or CloudCompare.
 181 void BALProblem::WriteToPLYFile(const std::string& filename) const {
 182   std::ofstream of(filename.c_str());
 183
 184   of << "ply"
 185      << '\n' << "format ascii 1.0"
 186      << '\n' << "element vertex " << num_cameras_ + num_points_
 187      << '\n' << "property float x"
 188      << '\n' << "property float y"
 189      << '\n' << "property float z"
 190      << '\n' << "property uchar red"
 191      << '\n' << "property uchar green"
 192      << '\n' << "property uchar blue"
 193      << '\n' << "end_header" << std::endl;
 194
 195   // Export extrinsic data (i.e. camera centers) as green points.
 196   double angle_axis[3];
 197   double center[3];
 198   for (int i = 0; i < num_cameras(); ++i)  {
 199     const double* camera = cameras() + camera_block_size() * i;
 200     CameraToAngleAxisAndCenter(camera, angle_axis, center);
 201     of << center[0] << ' ' << center[1] << ' ' << center[2]
 202        << " 0 255 0" << '\n';
 203   }
 204
 205   // Export the structure (i.e. 3D Points) as white points.
 206   const double* points = parameters_ + camera_block_size() * num_cameras_;
 207   for (int i = 0; i < num_points(); ++i) {
 208     const double* point = points + i * point_block_size();
 209     for (int j = 0; j < point_block_size(); ++j) {
 210       of << point[j] << ' ';
 211     }
 212     of << "255 255 255\n";
 213   }
 214   of.close();
 215 }
 216
 217 void BALProblem::CameraToAngleAxisAndCenter(const double* camera,
 218                                             double* angle_axis,
 219                                             double* center) const {
 220   VectorRef angle_axis_ref(angle_axis, 3);
 221   if (use_quaternions_) {
 222     QuaternionToAngleAxis(camera, angle_axis);
 223   } else {
 224     angle_axis_ref = ConstVectorRef(camera, 3);
 225   }
 226
 227   // c = -R't
 228   Eigen::VectorXd inverse_rotation = -angle_axis_ref;
 229   AngleAxisRotatePoint(inverse_rotation.data(),
 230                        camera + camera_block_size() - 6,
 231                        center);
 232   VectorRef(center, 3) *= -1.0;
 233 }
 234
 235 void BALProblem::AngleAxisAndCenterToCamera(const double* angle_axis,
 236                                             const double* center,
 237                                             double* camera) const {
 238   ConstVectorRef angle_axis_ref(angle_axis, 3);
 239   if (use_quaternions_) {
 240     AngleAxisToQuaternion(angle_axis, camera);
 241   } else {
 242     VectorRef(camera, 3) = angle_axis_ref;
 243   }
 244
 245   // t = -R * c
 246   AngleAxisRotatePoint(angle_axis,
 247                        center,
 248                        camera + camera_block_size() - 6);
 249   VectorRef(camera + camera_block_size() - 6, 3) *= -1.0;
 250 }
 251
 252
 253 void BALProblem::Normalize() {
 254   // Compute the marginal median of the geometry.
 255   std::vector<double> tmp(num_points_);
 256   Eigen::Vector3d median;
 257   double* points = mutable_points();
 258   for (int i = 0; i < 3; ++i) {
 259     for (int j = 0; j < num_points_; ++j) {
 260       tmp[j] = points[3 * j + i];
 261     }
 262     median(i) = Median(&tmp);
 263   }
 264
 265   for (int i = 0; i < num_points_; ++i) {
 266     VectorRef point(points + 3 * i, 3);
 267     tmp[i] = (point - median).lpNorm<1>();
 268   }
 269
 270   const double median_absolute_deviation = Median(&tmp);
 271
 272   // Scale so that the median absolute deviation of the resulting
 273   // reconstruction is 100.
 274   const double scale = 100.0 / median_absolute_deviation;
 275
 276   VLOG(2) << "median: " << median.transpose();
 277   VLOG(2) << "median absolute deviation: " << median_absolute_deviation;
 278   VLOG(2) << "scale: " << scale;
 279
 280   // X = scale * (X - median)
 281   for (int i = 0; i < num_points_; ++i) {
 282     VectorRef point(points + 3 * i, 3);
 283     point = scale * (point - median);
 284   }
 285
 286   double* cameras = mutable_cameras();
 287   double angle_axis[3];
 288   double center[3];
 289   for (int i = 0; i < num_cameras_; ++i) {
 290     double* camera = cameras + camera_block_size() * i;
 291     CameraToAngleAxisAndCenter(camera, angle_axis, center);
 292     // center = scale * (center - median)
 293     VectorRef(center, 3) = scale * (VectorRef(center, 3) - median);
 294     AngleAxisAndCenterToCamera(angle_axis, center, camera);
 295   }
 296 }
 297
 298 void BALProblem::Perturb(const double rotation_sigma,
 299                          const double translation_sigma,
 300                          const double point_sigma) {
 301   CHECK_GE(point_sigma, 0.0);
 302   CHECK_GE(rotation_sigma, 0.0);
 303   CHECK_GE(translation_sigma, 0.0);
 304
 305   double* points = mutable_points();
 306   if (point_sigma > 0) {
 307     for (int i = 0; i < num_points_; ++i) {
 308       PerturbPoint3(point_sigma, points + 3 * i);
 309     }
 310   }
 311
 312   for (int i = 0; i < num_cameras_; ++i) {
 313     double* camera = mutable_cameras() + camera_block_size() * i;
 314
 315     double angle_axis[3];
 316     double center[3];
 317     // Perturb in the rotation of the camera in the angle-axis
 318     // representation.
 319     CameraToAngleAxisAndCenter(camera, angle_axis, center);
 320     if (rotation_sigma > 0.0) {
 321       PerturbPoint3(rotation_sigma, angle_axis);
 322     }
 323     AngleAxisAndCenterToCamera(angle_axis, center, camera);
 324
 325     if (translation_sigma > 0.0) {
 326       PerturbPoint3(translation_sigma, camera + camera_block_size() - 6);
 327     }
 328   }
 329 }
 330
 331 BALProblem::~BALProblem() {
 332   delete []point_index_;
 333   delete []camera_index_;
 334   delete []observations_;
 335   delete []parameters_;
 336 }
 337
 338 }  // namespace examples
 339 }  // namespace ceres