common.h
#pragma once
/// 从文件读入BAL dataset 创建一个BALPorblem的类，通过创建一个对象，使用其构造函数来读取数据集中的内容
class BALProblem {
public:
/// load bal data from text file
explicit BALProblem(const std::string &filename, bool use_quaternions = false);
~BALProblem() {
delete[] point_index_;
delete[] camera_index_;
delete[] observations_;
delete[] parameters_;
}
/// save results to text file
void WriteToFile(const std::string &filename) const;
/// save results to ply pointcloud
void WriteToPLYFile(const std::string &filename) const;
void Normalize();
void Perturb(const double rotation_sigma,//扰动的大小由rotation_sigma、translation_sigma和point_sigma参数决定。
const double translation_sigma,
const double point_sigma);
int camera_block_size() const { return use_quaternions_ ? 10 : 9; }//用于返回相机参数的块大小。如果使用四元数，块大小为10，否则为9。
int point_block_size() const { return 3; }//用于返回点坐标的块大小，始终为3
int num_cameras() const { return num_cameras_; }//用于返回相机的数量
int num_points() const { return num_points_; }//用于返回点的数量
int num_observations() const { return num_observations_; }//用于返回观测的数量
int num_parameters() const { return num_parameters_; }//用于返回参数的数量
const int *point_index() const { return point_index_; }//用于返回每个观测对应的点索引
const int *camera_index() const { return camera_index_; }//用于返回每个观测对应的相机索引
const double *observations() const { return observations_; }//用于返回观测数据
const double *parameters() const { return parameters_; }//用于返回参数数据
const double *cameras() const { return parameters_; }//用于返回相机参数的起始地址
const double *points() const { return parameters_ + camera_block_size() * num_cameras_; }//用于返回点坐标的起始地址
/// camera参数的起始地址
double *mutable_cameras() { return parameters_; }//txt中存储的先是若干九维相机参数，再是众多三维点参数,该函数定位到txt从何处开始是相机参数
//该函数定位到txt从何处开始是三维点
double *mutable_points() { return parameters_ + camera_block_size() * num_cameras_; }
double *mutable_camera_for_observation(int i) {
return mutable_cameras() + camera_index_[i] * camera_block_size();
}
double *mutable_point_for_observation(int i) {
return mutable_points() + point_index_[i] * point_block_size();
}
const double *camera_for_observation(int i) const {
return cameras() + camera_index_[i] * camera_block_size();
}
const double *point_for_observation(int i) const {
return points() + point_index_[i] * point_block_size();
}
private:
void CameraToAngelAxisAndCenter(const double *camera,//用于将相机参数从角度轴表示转换为角度轴和中心点的表示
double *angle_axis,
double *center) const;
void AngleAxisAndCenterToCamera(const double *angle_axis,
const double *center,
double *camera) const;
int num_cameras_;//存储相机的数量
int num_points_;//存储点的数量
int num_observations_;//存储观测的数量
int num_parameters_;//存储参数的数量
bool use_quaternions_;
int *point_index_; // 每个observation对应的point index
int *camera_index_; // 每个observation对应的camera index
double *observations_;//存储观测
double *parameters_;
};
common.cpp
#include <cstdio>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
#include "common.h"
#include "rotation.h"
#include "random.h"
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);//将指针读取到的某类型的值赋值给value，并将指针向后移动相应的类型字节数到下一个数据类型处
if (num_scanned != 1)
std::cerr << "Invalid UW data file. ";
}
void PerturbPoint3(const double sigma, double *point) {//用于对3D点进行扰动。扰动的大小由sigma参数决定。
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;
}
BALProblem::BALProblem(const std::string &filename, bool use_quaternions) {
FILE *fptr = fopen(filename.c_str(), "r");
if (fptr == NULL) {
std::cerr << "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_);//第一行三个数据:相机的数量，路标点的数量，观测点的数量,一个路标点可以被重复观测
std::cout << "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_];//u,v
//每个相机待有化的变量有9个(旋转3维，平移3维，f焦距，k1,k2畸变系数），每个路标点的优化变量3个空间位置,
//开辟num_parameters_ = 9 * num_cameras_ + 3 * num_points_;这么大的空间用于存储优化变量parameters_ = new double[num_parameters_];
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);//rv2q
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;
}
}
void BALProblem::WriteToFile(const std::string &filename) const {
FILE *fptr = fopen(filename.c_str(), "w");
if (fptr == NULL) {
std::cerr << "Error: unable to open file " << filename;
return;
}
fprintf(fptr, "%d %d %d %d\n", num_cameras_, 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;
CameraToAngelAxisAndCenter(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::CameraToAngelAxisAndCenter(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() {//对读取到points的数据进行归一化处理
// 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);//对每个路标的分别进行3维数据的升序排序，并返回排序后的最中间的坐标值
}
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;
// 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;
CameraToAngelAxisAndCenter(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) {
assert(point_sigma >= 0.0);
assert(rotation_sigma >= 0.0);
assert(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
//好像是通过对三维点加上噪声后再逆向给相机位姿加噪声
CameraToAngelAxisAndCenter(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);
}
}
SnavelyReprojectionError.h
#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H
#include <iostream>
#include "ceres/ceres.h"
#include "rotation.h"
class SnavelyReprojectionError {
public:
SnavelyReprojectionError(double observation_x, double observation_y) : observed_x(observation_x),
observed_y(observation_y) {}
template<typename T>//按照ceres要求定义括号运算符来计算残差块，来实现ceres计算误差的接口
bool operator()(const T *const camera,//指针表T类型数组
const T *const point,
T *residuals) const {
// camera[0,1,2] are the angle-axis rotation
T predictions[2];//h(T_i,P_j),在待优化变量下，会产生什么样的观测，此项与实际观测作差，构成误差项，反映待优化变量的不准确性
CamProjectionWithDistortion(camera, point, predictions);//实现相机成像模型,计算重投影位置 h(T_i,P_j)，并存入predictions,
residuals[0] = predictions[0] - T(observed_x);
residuals[1] = predictions[1] - T(observed_y);
return true;
}
// camera : 9 dims array
// [0-2] : angle-axis rotation rv se3 R
// [3-5] : translateion t
// [6-8] : camera parameter, [6] focal length, [7-8] second and forth order radial distortion 给出fx fy（同一个f），cx、cy未给出，具体解释见书，以及去畸变系数
// point : 3D location.
// predictions : 2D predictions with center of the image plane.
template<typename T>
static inline bool CamProjectionWithDistortion(const T *camera, const T *point, T *predictions) {
// Rodrigues' formula
T p[3];
AngleAxisRotatePoint(camera, point, p);//Pc=Rcw*Pw+t,这里先进行旋转，使用rv形式的Rodrigues' formula对三维点进行旋转
// camera[3,4,5] are the translation 旋转后加上位移t
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
// Compute the center fo distortion 计算归一化坐标
T xp = -p[0] / p[2];
T yp = -p[1] / p[2];
// Apply second and fourth order radial distortion 畸变系数k1,k2
const T &l1 = camera[7];
const T &l2 = camera[8];
T r2 = xp * xp + yp * yp;
T distortion = T(1.0) + r2 * (l1 + l2 * r2);
const T &focal = camera[6];
predictions[0] = focal * distortion * xp;//在畸变后的像素平面计算重投影误差
predictions[1] = focal * distortion * yp;
return true;
}
static ceres::CostFunction *Create(const double observed_x, const double observed_y) {//残差快自动求导
return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
new SnavelyReprojectionError(observed_x, observed_y)));
}
private:
double observed_x;
double observed_y;
};
#endif // SnavelyReprojection.h
bundle_adjustment_ceres.cpp
#include <iostream>
#include <ceres/ceres.h>
#include "common.h"
#include "SnavelyReprojectionError.h"
using namespace std;
void SolveBA(BALProblem &bal_problem);
int main(int argc, char **argv) {
if (argc != 2) {
cout << "usage: bundle_adjustment_ceres bal_data.txt" << endl;
return 1;
}
BALProblem bal_problem(argv[1]);//创建一个BALPorblem的类，通过创建一个对象，使用其构造函数来读取数据集中的内容
bal_problem.Normalize();//数据归一化，好像是后面做畸变用？
bal_problem.Perturb(0.1, 0.5, 0.5);//给数据添加噪声
bal_problem.WriteToPLYFile("initial.ply");//优化前数据
SolveBA(bal_problem);//构建ceres求解器，并对优化问题进行求解
bal_problem.WriteToPLYFile("final.ply");//优化后数据
return 0;
}
void SolveBA(BALProblem &bal_problem) {
const int point_block_size = bal_problem.point_block_size();//3
const int camera_block_size = bal_problem.camera_block_size();//9 or 10
double *points = bal_problem.mutable_points();
double *cameras = bal_problem.mutable_cameras();
// Observations is 2 * num_observations long array observations
// [u_1, u_2, ... u_n], where each u_i is two dimensional, the x
// and y position of the observation.
const double *observations = bal_problem.observations();
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {//把每个观测对应的残差块加入ceres 的problem
ceres::CostFunction *cost_function;
// Each Residual block takes a point and a camera as input
// and outputs a 2 dimensional Residual
cost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]);
//引入残差块计算模型，create设置自动求导
// If enabled use Huber's loss function. 鲁棒核函数采用huber，误差边界设置为1
ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);
// Each observation corresponds to a pair of a camera and a point
// which are identified by camera_index()[i] and point_index()[i]
// respectively.
//camera_index()[i],point_index()[i]分别索引该条观测数据对应的相机帧数和路标点序号,实际是寻找与该观测相关的待优化变量
double *camera = cameras + camera_block_size * bal_problem.camera_index()[i];
double *point = points + point_block_size * bal_problem.point_index()[i];
//将待优化变量值引入残差块，利用重载括号完成残差块计算 将残差块加入总的最小二乘问题
problem.AddResidualBlock(cost_function, loss_function, camera, point);
}
// show some information here ...
std::cout << "bal problem file loaded..." << std::endl;
std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
<< bal_problem.num_points() << " points. " << std::endl;
std::cout << "Forming " << bal_problem.num_observations() << " observations. " << std::endl;
std::cout << "Solving ceres BA ... " << endl;
ceres::Solver::Options options;
options.linear_solver_type = ceres::LinearSolverType::SPARSE_SCHUR;//求解增量方程时利用其稀疏性质进行schur消元，从而减小计算量
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);//求解器对问题进行求解
std::cout << summary.FullReport() << "\n";
}
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
