examples/bal_problem.cc - ceres-solver - Git at Google

 // Ceres Solver - A fast non-linear least squares minimizer
 // Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
 // http://code.google.com/p/ceres-solver/
 //
 // Redistribution and use in source and binary forms, with or without
 // modification, are permitted provided that the following conditions are met:
 //
 // * Redistributions of source code must retain the above copyright notice,
 //   this list of conditions and the following disclaimer.
 // * Redistributions in binary form must reproduce the above copyright notice,
 //   this list of conditions and the following disclaimer in the documentation
 //   and/or other materials provided with the distribution.
 // * Neither the name of Google Inc. nor the names of its contributors may be
 //   used to endorse or promote products derived from this software without
 //   specific prior written permission.
 //
 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 //
 // Author: sameeragarwal@google.com (Sameer Agarwal)

 #include "bal_problem.h"

 #include <cstdio>
 #include <cstdlib>
 #include <string>
 #include <vector>
 #include "Eigen/Core"
 #include "ceres/rotation.h"
 #include "glog/logging.h"
 #include "random.h"

 namespace ceres {
 namespace examples {
 namespace {
 typedef Eigen::Map<Eigen::VectorXd> VectorRef;
 typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;

 template<typename T>
 void FscanfOrDie(FILE* fptr, const char* format, T* value) {
   int num_scanned = fscanf(fptr, format, value);
   if (num_scanned != 1) {
     LOG(FATAL) << "Invalid UW data file.";
   }
 }

 void PerturbPoint3(const double sigma, double* point) {
   for (int i = 0; i < 3; ++i) {
     point[i] += RandNormal() * sigma;
   }
 }

 double Median(std::vector<double>* data) {
   int n = data->size();
   std::vector<double>::iterator mid_point = data->begin() + n / 2;
   std::nth_element(data->begin(), mid_point, data->end());
   return *mid_point;
 }

 }  // namespace

 BALProblem::BALProblem(const std::string& filename, bool use_quaternions) {
   FILE* fptr = fopen(filename.c_str(), "r");

   if (fptr == NULL) {
     LOG(FATAL) << "Error: unable to open file " << filename;
     return;
   };

   // This wil die horribly on invalid files. Them's the breaks.
   FscanfOrDie(fptr, "%d", &num_cameras_);
   FscanfOrDie(fptr, "%d", &num_points_);
   FscanfOrDie(fptr, "%d", &num_observations_);

   VLOG(1) << "Header: " << num_cameras_
           << " " << num_points_
           << " " << num_observations_;

   point_index_ = new int[num_observations_];
   camera_index_ = new int[num_observations_];
   observations_ = new double[2 * num_observations_];

   num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
   parameters_ = new double[num_parameters_];

   for (int i = 0; i < num_observations_; ++i) {
     FscanfOrDie(fptr, "%d", camera_index_ + i);
     FscanfOrDie(fptr, "%d", point_index_ + i);
     for (int j = 0; j < 2; ++j) {
       FscanfOrDie(fptr, "%lf", observations_ + 2*i + j);
     }
   }

   for (int i = 0; i < num_parameters_; ++i) {
     FscanfOrDie(fptr, "%lf", parameters_ + i);
   }

   fclose(fptr);

   use_quaternions_ = use_quaternions;
   if (use_quaternions) {
     // Switch the angle-axis rotations to quaternions.
     num_parameters_ = 10 * num_cameras_ + 3 * num_points_;
     double* quaternion_parameters = new double[num_parameters_];
     double* original_cursor = parameters_;
     double* quaternion_cursor = quaternion_parameters;
     for (int i = 0; i < num_cameras_; ++i) {
       AngleAxisToQuaternion(original_cursor, quaternion_cursor);
       quaternion_cursor += 4;
       original_cursor += 3;
       for (int j = 4; j < 10; ++j) {
        *quaternion_cursor++ = *original_cursor++;
       }
     }
     // Copy the rest of the points.
     for (int i = 0; i < 3 * num_points_; ++i) {
       *quaternion_cursor++ = *original_cursor++;
     }
     // Swap in the quaternion parameters.
     delete []parameters_;
     parameters_ = quaternion_parameters;
   }
 }

 // This function writes the problem to a file in the same format that
 // is read by the constructor.
 void BALProblem::WriteToFile(const std::string& filename) const {
   FILE* fptr = fopen(filename.c_str(), "w");

   if (fptr == NULL) {
     LOG(FATAL) << "Error: unable to open file " << filename;
     return;
   };

   fprintf(fptr, "%d %d %d\n", num_cameras_, num_points_, num_observations_);

   for (int i = 0; i < num_observations_; ++i) {
     fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]);
     for (int j = 0; j < 2; ++j) {
       fprintf(fptr, " %g", observations_[2 * i + j]);
     }
     fprintf(fptr, "\n");
   }

   for (int i = 0; i < num_cameras(); ++i) {
     double angleaxis[9];
     if (use_quaternions_) {
       // Output in angle-axis format.
       QuaternionToAngleAxis(parameters_ + 10 * i, angleaxis);
       memcpy(angleaxis + 3, parameters_ + 10 * i + 4, 6 * sizeof(double));
     } else {
       memcpy(angleaxis, parameters_ + 9 * i, 9 * sizeof(double));
     }
     for (int j = 0; j < 9; ++j) {
       fprintf(fptr, "%.16g\n", angleaxis[j]);
     }
   }

   const double* points = parameters_ + camera_block_size() * num_cameras_;
   for (int i = 0; i < num_points(); ++i) {
     const double* point = points + i * point_block_size();
     for (int j = 0; j < point_block_size(); ++j) {
       fprintf(fptr, "%.16g\n", point[j]);
     }
   }

   fclose(fptr);
 }

 void BALProblem::CameraToAngleAxisAndCenter(const double* camera,
                                             double* angle_axis,
                                             double* center) {
   VectorRef angle_axis_ref(angle_axis, 3);
   if (use_quaternions_) {
     QuaternionToAngleAxis(camera, angle_axis);
   } else {
     angle_axis_ref = ConstVectorRef(camera, 3);
   }

   // c = -R't
   Eigen::VectorXd inverse_rotation = -angle_axis_ref;
   AngleAxisRotatePoint(inverse_rotation.data(),
                        camera + camera_block_size() - 6,
                        center);
   VectorRef(center, 3) *= -1.0;
 }

 void BALProblem::AngleAxisAndCenterToCamera(const double* angle_axis,
                                             const double* center,
                                             double* camera) {
   ConstVectorRef angle_axis_ref(angle_axis, 3);
   if (use_quaternions_) {
     AngleAxisToQuaternion(angle_axis, camera);
   } else {
     VectorRef(camera, 3) = angle_axis_ref;
   }

   // t = -R * c
   AngleAxisRotatePoint(angle_axis,
                        center,
                        camera + camera_block_size() - 6);
   VectorRef(camera + camera_block_size() - 6, 3) *= -1.0;
 }


 void BALProblem::Normalize() {
   // Compute the marginal median of the geometry.
   std::vector<double> tmp(num_points_);
   Eigen::Vector3d median;
   double* points = mutable_points();
   for (int i = 0; i < 3; ++i) {
     for (int j = 0; j < num_points_; ++j) {
       tmp[j] = points[3 * j + i];
     }
     median(i) = Median(&tmp);
   }

   for (int i = 0; i < num_points_; ++i) {
     VectorRef point(points + 3 * i, 3);
     tmp[i] = (point - median).lpNorm<1>();
   }

   const double median_absolute_deviation = Median(&tmp);

   // Scale so that the median absolute deviation of the resulting
   // reconstruction is 100.
   const double scale = 100.0 / median_absolute_deviation;

   VLOG(2) << "median: " << median.transpose();
   VLOG(2) << "median absolute deviation: " << median_absolute_deviation;
   VLOG(2) << "scale: " << scale;

   // X = scale * (X - median)
   for (int i = 0; i < num_points_; ++i) {
     VectorRef point(points + 3 * i, 3);
     point = scale * (point - median);
   }

   double* cameras = mutable_cameras();
   double angle_axis[3];
   double center[3];
   for (int i = 0; i < num_cameras_; ++i) {
     double* camera = cameras + camera_block_size() * i;
     CameraToAngleAxisAndCenter(camera, angle_axis, center);
     // center = scale * (center - median)
     VectorRef(center, 3) = scale * (VectorRef(center, 3) - median);
     AngleAxisAndCenterToCamera(angle_axis, center, camera);
   }
 }

 void BALProblem::Perturb(const double rotation_sigma,
                          const double translation_sigma,
                          const double point_sigma) {
   CHECK_GE(point_sigma, 0.0);
   CHECK_GE(rotation_sigma, 0.0);
   CHECK_GE(translation_sigma, 0.0);

   double* points = mutable_points();
   if (point_sigma > 0) {
     for (int i = 0; i < num_points_; ++i) {
       PerturbPoint3(point_sigma, points + 3 * i);
     }
   }

   for (int i = 0; i < num_cameras_; ++i) {
     double* camera = mutable_cameras() + camera_block_size() * i;

     double angle_axis[3];
     double center[3];
     // Perturb in the rotation of the camera in the angle-axis
     // representation.
     CameraToAngleAxisAndCenter(camera, angle_axis, center);
     if (rotation_sigma > 0.0) {
       PerturbPoint3(rotation_sigma, angle_axis);
     }
     AngleAxisAndCenterToCamera(angle_axis, center, camera);

     if (translation_sigma > 0.0) {
       PerturbPoint3(translation_sigma, camera + camera_block_size() - 6);
     }
   }
 }

 BALProblem::~BALProblem() {
   delete []point_index_;
   delete []camera_index_;
   delete []observations_;
   delete []parameters_;
 }

 }  // namespace examples
 }  // namespace ceres
	// Ceres Solver - A fast non-linear least squares minimizer
	// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
	// http://code.google.com/p/ceres-solver/
	//
	// Redistribution and use in source and binary forms, with or without
	// modification, are permitted provided that the following conditions are met:
	//
	// * Redistributions of source code must retain the above copyright notice,
	// this list of conditions and the following disclaimer.
	// * Redistributions in binary form must reproduce the above copyright notice,
	// this list of conditions and the following disclaimer in the documentation
	// and/or other materials provided with the distribution.
	// * Neither the name of Google Inc. nor the names of its contributors may be
	// used to endorse or promote products derived from this software without
	// specific prior written permission.
	//
	// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
	// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
	// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
	// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
	// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
	// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
	// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
	// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
	// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
	// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
	// POSSIBILITY OF SUCH DAMAGE.
	//
	// Author: sameeragarwal@google.com (Sameer Agarwal)

	#include "bal_problem.h"

	#include <cstdio>
	#include <cstdlib>
	#include <string>
	#include <vector>
	#include "Eigen/Core"
	#include "ceres/rotation.h"
	#include "glog/logging.h"
	#include "random.h"

	namespace ceres {
	namespace examples {
	namespace {
	typedef Eigen::Map<Eigen::VectorXd> VectorRef;
	typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;

	template<typename T>
	void FscanfOrDie(FILE* fptr, const char* format, T* value) {
	int num_scanned = fscanf(fptr, format, value);
	if (num_scanned != 1) {
	LOG(FATAL) << "Invalid UW data file.";
	}
	}

	void PerturbPoint3(const double sigma, double* point) {
	for (int i = 0; i < 3; ++i) {
	point[i] += RandNormal() * sigma;
	}
	}

	double Median(std::vector<double>* data) {
	int n = data->size();
	std::vector<double>::iterator mid_point = data->begin() + n / 2;
	std::nth_element(data->begin(), mid_point, data->end());
	return *mid_point;
	}

	} // namespace

	BALProblem::BALProblem(const std::string& filename, bool use_quaternions) {
	FILE* fptr = fopen(filename.c_str(), "r");

	if (fptr == NULL) {
	LOG(FATAL) << "Error: unable to open file " << filename;
	return;
	};

	// This wil die horribly on invalid files. Them's the breaks.
	FscanfOrDie(fptr, "%d", &num_cameras_);
	FscanfOrDie(fptr, "%d", &num_points_);
	FscanfOrDie(fptr, "%d", &num_observations_);

	VLOG(1) << "Header: " << num_cameras_
	<< " " << num_points_
	<< " " << num_observations_;

	point_index_ = new int[num_observations_];
	camera_index_ = new int[num_observations_];
	observations_ = new double[2 * num_observations_];

	num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
	parameters_ = new double[num_parameters_];

	for (int i = 0; i < num_observations_; ++i) {
	FscanfOrDie(fptr, "%d", camera_index_ + i);
	FscanfOrDie(fptr, "%d", point_index_ + i);
	for (int j = 0; j < 2; ++j) {
	FscanfOrDie(fptr, "%lf", observations_ + 2*i + j);
	}
	}

	for (int i = 0; i < num_parameters_; ++i) {
	FscanfOrDie(fptr, "%lf", parameters_ + i);
	}

	fclose(fptr);

	use_quaternions_ = use_quaternions;
	if (use_quaternions) {
	// Switch the angle-axis rotations to quaternions.
	num_parameters_ = 10 * num_cameras_ + 3 * num_points_;
	double* quaternion_parameters = new double[num_parameters_];
	double* original_cursor = parameters_;
	double* quaternion_cursor = quaternion_parameters;
	for (int i = 0; i < num_cameras_; ++i) {
	AngleAxisToQuaternion(original_cursor, quaternion_cursor);
	quaternion_cursor += 4;
	original_cursor += 3;
	for (int j = 4; j < 10; ++j) {
	quaternion_cursor++ = original_cursor++;
	}
	}
	// Copy the rest of the points.
	for (int i = 0; i < 3 * num_points_; ++i) {
	quaternion_cursor++ = original_cursor++;
	}
	// Swap in the quaternion parameters.
	delete []parameters_;
	parameters_ = quaternion_parameters;
	}
	}

	// This function writes the problem to a file in the same format that
	// is read by the constructor.
	void BALProblem::WriteToFile(const std::string& filename) const {
	FILE* fptr = fopen(filename.c_str(), "w");

	if (fptr == NULL) {
	LOG(FATAL) << "Error: unable to open file " << filename;
	return;
	};

	fprintf(fptr, "%d %d %d\n", num_cameras_, num_points_, num_observations_);

	for (int i = 0; i < num_observations_; ++i) {
	fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]);
	for (int j = 0; j < 2; ++j) {
	fprintf(fptr, " %g", observations_[2 * i + j]);
	}
	fprintf(fptr, "\n");
	}

	for (int i = 0; i < num_cameras(); ++i) {
	double angleaxis[9];
	if (use_quaternions_) {
	// Output in angle-axis format.
	QuaternionToAngleAxis(parameters_ + 10 * i, angleaxis);
	memcpy(angleaxis + 3, parameters_ + 10 * i + 4, 6 * sizeof(double));
	} else {
	memcpy(angleaxis, parameters_ + 9 * i, 9 * sizeof(double));
	}
	for (int j = 0; j < 9; ++j) {
	fprintf(fptr, "%.16g\n", angleaxis[j]);
	}
	}

	const double* points = parameters_ + camera_block_size() * num_cameras_;
	for (int i = 0; i < num_points(); ++i) {
	const double* point = points + i * point_block_size();
	for (int j = 0; j < point_block_size(); ++j) {
	fprintf(fptr, "%.16g\n", point[j]);
	}
	}

	fclose(fptr);
	}

	void BALProblem::CameraToAngleAxisAndCenter(const double* camera,
	double* angle_axis,
	double* center) {
	VectorRef angle_axis_ref(angle_axis, 3);
	if (use_quaternions_) {
	QuaternionToAngleAxis(camera, angle_axis);
	} else {
	angle_axis_ref = ConstVectorRef(camera, 3);
	}

	// c = -R't
	Eigen::VectorXd inverse_rotation = -angle_axis_ref;
	AngleAxisRotatePoint(inverse_rotation.data(),
	camera + camera_block_size() - 6,
	center);
	VectorRef(center, 3) *= -1.0;
	}

	void BALProblem::AngleAxisAndCenterToCamera(const double* angle_axis,
	const double* center,
	double* camera) {
	ConstVectorRef angle_axis_ref(angle_axis, 3);
	if (use_quaternions_) {
	AngleAxisToQuaternion(angle_axis, camera);
	} else {
	VectorRef(camera, 3) = angle_axis_ref;
	}

	// t = -R * c
	AngleAxisRotatePoint(angle_axis,
	center,
	camera + camera_block_size() - 6);
	VectorRef(camera + camera_block_size() - 6, 3) *= -1.0;
	}


	void BALProblem::Normalize() {
	// Compute the marginal median of the geometry.
	std::vector<double> tmp(num_points_);
	Eigen::Vector3d median;
	double* points = mutable_points();
	for (int i = 0; i < 3; ++i) {
	for (int j = 0; j < num_points_; ++j) {
	tmp[j] = points[3 * j + i];
	}
	median(i) = Median(&tmp);
	}

	for (int i = 0; i < num_points_; ++i) {
	VectorRef point(points + 3 * i, 3);
	tmp[i] = (point - median).lpNorm<1>();
	}

	const double median_absolute_deviation = Median(&tmp);

	// Scale so that the median absolute deviation of the resulting
	// reconstruction is 100.
	const double scale = 100.0 / median_absolute_deviation;

	VLOG(2) << "median: " << median.transpose();
	VLOG(2) << "median absolute deviation: " << median_absolute_deviation;
	VLOG(2) << "scale: " << scale;

	// X = scale * (X - median)
	for (int i = 0; i < num_points_; ++i) {
	VectorRef point(points + 3 * i, 3);
	point = scale * (point - median);
	}

	double* cameras = mutable_cameras();
	double angle_axis[3];
	double center[3];
	for (int i = 0; i < num_cameras_; ++i) {
	double* camera = cameras + camera_block_size() * i;
	CameraToAngleAxisAndCenter(camera, angle_axis, center);
	// center = scale * (center - median)
	VectorRef(center, 3) = scale * (VectorRef(center, 3) - median);
	AngleAxisAndCenterToCamera(angle_axis, center, camera);
	}
	}

	void BALProblem::Perturb(const double rotation_sigma,
	const double translation_sigma,
	const double point_sigma) {
	CHECK_GE(point_sigma, 0.0);
	CHECK_GE(rotation_sigma, 0.0);
	CHECK_GE(translation_sigma, 0.0);

	double* points = mutable_points();
	if (point_sigma > 0) {
	for (int i = 0; i < num_points_; ++i) {
	PerturbPoint3(point_sigma, points + 3 * i);
	}
	}

	for (int i = 0; i < num_cameras_; ++i) {
	double* camera = mutable_cameras() + camera_block_size() * i;

	double angle_axis[3];
	double center[3];
	// Perturb in the rotation of the camera in the angle-axis
	// representation.
	CameraToAngleAxisAndCenter(camera, angle_axis, center);
	if (rotation_sigma > 0.0) {
	PerturbPoint3(rotation_sigma, angle_axis);
	}
	AngleAxisAndCenterToCamera(angle_axis, center, camera);

	if (translation_sigma > 0.0) {
	PerturbPoint3(translation_sigma, camera + camera_block_size() - 6);
	}
	}
	}

	BALProblem::~BALProblem() {
	delete []point_index_;
	delete []camera_index_;
	delete []observations_;
	delete []parameters_;
	}

	} // namespace examples
	} // namespace ceres