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