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