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: keir (at) google.com (Keir Mierle) 30 // 31 // A minimal, self-contained bundle adjuster using Ceres, that reads 32 // files from University of Washington' Bundle Adjustment in the Large dataset: 33 // http://grail.cs.washington.edu/projects/bal 34 // 35 // This does not use the best configuration for solving; see the more involved 36 // bundle_adjuster.cc file for details. 37 38 #include <cmath> 39 #include <cstdio> 40 #include <iostream> 41 42 #include "ceres/ceres.h" 43 #include "ceres/rotation.h" 44 45 // Read a Bundle Adjustment in the Large dataset. 46 class BALProblem { 47 public: 48 ~BALProblem() { 49 delete[] point_index_; 50 delete[] camera_index_; 51 delete[] observations_; 52 delete[] parameters_; 53 } 54 55 int num_observations() const { return num_observations_; } 56 const double* observations() const { return observations_; } 57 double* mutable_cameras() { return parameters_; } 58 double* mutable_points() { return parameters_ + 9 * num_cameras_; } 59 60 double* mutable_camera_for_observation(int i) { 61 return mutable_cameras() + camera_index_[i] * 9; 62 } 63 double* mutable_point_for_observation(int i) { 64 return mutable_points() + point_index_[i] * 3; 65 } 66 67 bool LoadFile(const char* filename) { 68 FILE* fptr = fopen(filename, "r"); 69 if (fptr == NULL) { 70 return false; 71 }; 72 73 FscanfOrDie(fptr, "%d", &num_cameras_); 74 FscanfOrDie(fptr, "%d", &num_points_); 75 FscanfOrDie(fptr, "%d", &num_observations_); 76 77 point_index_ = new int[num_observations_]; 78 camera_index_ = new int[num_observations_]; 79 observations_ = new double[2 * num_observations_]; 80 81 num_parameters_ = 9 * num_cameras_ + 3 * num_points_; 82 parameters_ = new double[num_parameters_]; 83 84 for (int i = 0; i < num_observations_; ++i) { 85 FscanfOrDie(fptr, "%d", camera_index_ + i); 86 FscanfOrDie(fptr, "%d", point_index_ + i); 87 for (int j = 0; j < 2; ++j) { 88 FscanfOrDie(fptr, "%lf", observations_ + 2*i + j); 89 } 90 } 91 92 for (int i = 0; i < num_parameters_; ++i) { 93 FscanfOrDie(fptr, "%lf", parameters_ + i); 94 } 95 return true; 96 } 97 98 private: 99 template<typename T> 100 void FscanfOrDie(FILE *fptr, const char *format, T *value) { 101 int num_scanned = fscanf(fptr, format, value); 102 if (num_scanned != 1) { 103 LOG(FATAL) << "Invalid UW data file."; 104 } 105 } 106 107 int num_cameras_; 108 int num_points_; 109 int num_observations_; 110 int num_parameters_; 111 112 int* point_index_; 113 int* camera_index_; 114 double* observations_; 115 double* parameters_; 116 }; 117 118 // Templated pinhole camera model for used with Ceres. The camera is 119 // parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for 120 // focal length and 2 for radial distortion. The principal point is not modeled 121 // (i.e. it is assumed be located at the image center). 122 struct SnavelyReprojectionError { 123 SnavelyReprojectionError(double observed_x, double observed_y) 124 : observed_x(observed_x), observed_y(observed_y) {} 125 126 template <typename T> 127 bool operator()(const T* const camera, 128 const T* const point, 129 T* residuals) const { 130 // camera[0,1,2] are the angle-axis rotation. 131 T p[3]; 132 ceres::AngleAxisRotatePoint(camera, point, p); 133 134 // camera[3,4,5] are the translation. 135 p[0] += camera[3]; 136 p[1] += camera[4]; 137 p[2] += camera[5]; 138 139 // Compute the center of distortion. The sign change comes from 140 // the camera model that Noah Snavely's Bundler assumes, whereby 141 // the camera coordinate system has a negative z axis. 142 T xp = - p[0] / p[2]; 143 T yp = - p[1] / p[2]; 144 145 // Apply second and fourth order radial distortion. 146 const T& l1 = camera[7]; 147 const T& l2 = camera[8]; 148 T r2 = xp*xp + yp*yp; 149 T distortion = T(1.0) + r2 * (l1 + l2 * r2); 150 151 // Compute final projected point position. 152 const T& focal = camera[6]; 153 T predicted_x = focal * distortion * xp; 154 T predicted_y = focal * distortion * yp; 155 156 // The error is the difference between the predicted and observed position. 157 residuals[0] = predicted_x - T(observed_x); 158 residuals[1] = predicted_y - T(observed_y); 159 160 return true; 161 } 162 163 // Factory to hide the construction of the CostFunction object from 164 // the client code. 165 static ceres::CostFunction* Create(const double observed_x, 166 const double observed_y) { 167 return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>( 168 new SnavelyReprojectionError(observed_x, observed_y))); 169 } 170 171 double observed_x; 172 double observed_y; 173 }; 174 175 int main(int argc, char** argv) { 176 google::InitGoogleLogging(argv[0]); 177 if (argc != 2) { 178 std::cerr << "usage: simple_bundle_adjuster <bal_problem>\n"; 179 return 1; 180 } 181 182 BALProblem bal_problem; 183 if (!bal_problem.LoadFile(argv[1])) { 184 std::cerr << "ERROR: unable to open file " << argv[1] << "\n"; 185 return 1; 186 } 187 188 const double* observations = bal_problem.observations(); 189 190 // Create residuals for each observation in the bundle adjustment problem. The 191 // parameters for cameras and points are added automatically. 192 ceres::Problem problem; 193 for (int i = 0; i < bal_problem.num_observations(); ++i) { 194 // Each Residual block takes a point and a camera as input and outputs a 2 195 // dimensional residual. Internally, the cost function stores the observed 196 // image location and compares the reprojection against the observation. 197 198 ceres::CostFunction* cost_function = 199 SnavelyReprojectionError::Create(observations[2 * i + 0], 200 observations[2 * i + 1]); 201 problem.AddResidualBlock(cost_function, 202 NULL /* squared loss */, 203 bal_problem.mutable_camera_for_observation(i), 204 bal_problem.mutable_point_for_observation(i)); 205 } 206 207 // Make Ceres automatically detect the bundle structure. Note that the 208 // standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower 209 // for standard bundle adjustment problems. 210 ceres::Solver::Options options; 211 options.linear_solver_type = ceres::DENSE_SCHUR; 212 options.minimizer_progress_to_stdout = true; 213 214 ceres::Solver::Summary summary; 215 ceres::Solve(options, &problem, &summary); 216 std::cout << summary.FullReport() << "\n"; 217 return 0; 218 } 219
