340 lines
11 KiB
C++
340 lines
11 KiB
C++
|
// Ceres Solver - A fast non-linear least squares minimizer
|
||
|
|
// Copyright 2015 Google Inc. All rights reserved.
|
||
|
|
// http://ceres-solver.org/
|
||
|
|
//
|
||
|
|
// 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 <fstream>
|
||
|
|
#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);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Write the problem to a PLY file for inspection in Meshlab or CloudCompare.
|
||
|
|
void BALProblem::WriteToPLYFile(const std::string& filename) const {
|
||
|
|
std::ofstream of(filename.c_str());
|
||
|
|
|
||
|
|
of << "ply"
|
||
|
|
<< '\n' << "format ascii 1.0"
|
||
|
|
<< '\n' << "element vertex " << num_cameras_ + num_points_
|
||
|
|
<< '\n' << "property float x"
|
||
|
|
<< '\n' << "property float y"
|
||
|
|
<< '\n' << "property float z"
|
||
|
|
<< '\n' << "property uchar red"
|
||
|
|
<< '\n' << "property uchar green"
|
||
|
|
<< '\n' << "property uchar blue"
|
||
|
|
<< '\n' << "end_header" << std::endl;
|
||
|
|
|
||
|
|
// Export extrinsic data (i.e. camera centers) as green points.
|
||
|
|
double angle_axis[3];
|
||
|
|
double center[3];
|
||
|
|
for (int i = 0; i < num_cameras(); ++i) {
|
||
|
|
const double* camera = cameras() + camera_block_size() * i;
|
||
|
|
CameraToAngleAxisAndCenter(camera, angle_axis, center);
|
||
|
|
of << center[0] << ' ' << center[1] << ' ' << center[2]
|
||
|
|
<< " 0 255 0" << '\n';
|
||
|
|
}
|
||
|
|
|
||
|
|
// Export the structure (i.e. 3D Points) as white points.
|
||
|
|
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) {
|
||
|
|
of << point[j] << ' ';
|
||
|
|
}
|
||
|
|
of << "255 255 255\n";
|
||
|
|
}
|
||
|
|
of.close();
|
||
|
|
}
|
||
|
|
|
||
|
|
void BALProblem::CameraToAngleAxisAndCenter(const double* camera,
|
||
|
|
double* angle_axis,
|
||
|
|
double* center) const {
|
||
|
|
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) const {
|
||
|
|
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
|