slam14讲后端优化代码注释（一）（第二版ch9）
slam14讲后端优化代码注释（一）（第二版ch9）
- bundle_adjustment_g2o.cpp
- bundle_adjustment_ceres.cpp
- common.cpp
- common.h
- random.h
- rotation.h
- SnavelyReprojectionError.h
bundle_adjustment_g2o.cpp
#include bundle_adjustment_ceres.cpp
#include common.cpp
#include
...
...
...
其中，相机和点索引从0开始。每个相机是一组9个参数 - R，t，f，k1和k2。 旋转R被指定为罗德里格斯的向量。
例：
1. BAL数据集说明
第一行：
16 22106 83718
16个相机，22106个点，共进行83718次相机对点的观测
第2行到83719行：
6 18595 3.775000e+01 4.703003e+01
第6个相机观测18595个点，得到的相机的观测数据为3.775000e+01 4.703003e+01
第83720行到83720+16*9=83864
共16个相机的9纬参数：-R（3维），t（3维），f（焦距），k1,k2畸变参数
第83864到83864+3*22106=150182
22106个点的三维坐标
*/
// 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_]; //取出3D路标点的标号
camera_index_ = new int[num_observations_]; //相机的标号
observations_ = new double[2 * num_observations_]; //观测的像素点
num_parameters_ = 9 * num_cameras_ + 3 * num_points_; //每个相机9个参数，每个路标3个参数
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); //观测到的像素坐标
}
}
//每个相机是一组9个参数，-R:3维(罗德里格斯向量) t:3维 f,k1,k2。后面是3D路标的数据3维
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) {
//其他参数 t（3维），f（焦距），k1,k2畸变参数 直接赋值
*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_); //第一行 ：相机数（两次是不是作者写晕了～） 路标个数，观测个数
//每行输出 那个相机 观测那个路标点 的 观测值 x ， y （四个数据）
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()函数进行复制，从 parameters_ + 10 * i + 4 位置开始的 6 * sizeof(double) 内存空间的数据放入起始地址为angleaxis + 3的内存空间里
memcpy(angleaxis + 3, parameters_ + 10 * i + 4, 6 * sizeof(double));
} else {
//这里用memcpy()函数进行复制，从 parameters_ + 9 * i + 4 位置开始的 9 * sizeof(double) 内存空间的数据放入起始地址为angleaxis 的内存空间里
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.
//创建两个数组，用于承接CameraToAngelAxisAndCenter()解析出来的相机旋转姿态和相机位置中心
double angle_axis[3];
double center[3];
for (int i = 0; i < num_cameras(); ++i) {
//循环写入，首先写入的相机中心点参数，个数控制肯定是相机数据个数
const double *camera = cameras() + camera_block_size() * i;
//从cameras头指针开始，每次步进相机维度，这里为9，就读到了每个相机参数
CameraToAngelAxisAndCenter(camera, angle_axis, center);
//用CameraToAngelAxisAndCenter()函数将从相机参数中解析出来相机姿势和相机位置。当然这里只用位置了。
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] << ' ';
//维度循环，写入xyz
}
//加上颜色，最后要有回车
of << "255 255 255\n";
}
of.close();
}
//camera数据中的旋转向量以及平移向量解析相机世界坐标系的姿态,(依旧是旋转向量)和位置（世界坐标系下的XYZ）
//具体参数说明：
//camera要解析的相机参数，前三维旋转，接着三维平移
//angle_axis解析出的相机姿态承接数组，也是旋转向量形式
//center是相机原点在世界坐标系下的定义
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
//如何计算center
//center是相机原点在世界坐标系下的定义
//PW_center:世界坐标系下的相机坐标
//PC_center:相机坐标系下的相机原点坐标（0,0,0）
//根据相机坐标系与世界坐标系的转换关系：PW_center×R+t=PC_center
//PW_center= -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;
}
//有点数据初始化的意思，功能上看就是对待优化的point和camera数据进行了一下处理，
//用法上也是在读入了数据后，直接调用此程序进行处理。
//目前我也没弄懂这些数据处理是做什么用
void BALProblem::Normalize() {
// Compute the marginal median of the geometry
std::vector<double> tmp(num_points_);
Eigen::Vector3d median;
double *points = mutable_points(); //获取路标3D点的位置 即parameters_ 中首个3d坐标的地址
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < num_points_; ++j) {
tmp[j] = points[3 * j + i]; //将point的3d点 逐个放在tmp（vector）
}
median(i) = Median(&tmp); //返回中位数，如果是偶数，取平均值 即median为point的中位数 分别对应 x，y，z
}
for (int i = 0; i < num_points_; ++i) {
VectorRef point(points + 3 * i, 3); //typedef Eigen::Map VectorRef;
tmp[i] = (point - median).lpNorm<1>(); //（每个点 - 中位数） 的LP范数
}
//关于.lpNorm<1>()参见下面网址帖子，是一个范数模板函数
//http://blog.csdn.net/skybirdhua1989/article/details/17584797
//这里用的是L1范数:||x||为x向量各个元素绝对值之和。
//简单数一下p范数：向量各元素绝对值的p阶和的p阶根
//lpNorm<>()函数定义是这么说的：returns the p-th root of the sum of the p-th powers of the absolute values
//很明显，p为1的话就是各元素绝对值之和，为2就是模长
const double median_absolute_deviation = Median(&tmp); //（每个点 - 中位数） 的LP范数 的中位数
// 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); //对每个3D点进行处理，MAP是引用，会改变原数据
}
double *cameras = mutable_cameras(); /// camera参数的起始地址
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,
//然后添加噪声，添加完后再用AngleAxisAndCenterToCamera()重构camera参数
//平移部分就直接用PerturbPoint3()添加了
CameraToAngelAxisAndCenter(camera, angle_axis, center); //主要是通过camera求angle_axis以及center(相机原点在世界坐标系下的坐标)
if (rotation_sigma > 0.0) {
//添加旋转噪声
PerturbPoint3(rotation_sigma, angle_axis);
}
AngleAxisAndCenterToCamera(angle_axis, center, camera);//将添加噪声之后的值重新加到camera中
if (translation_sigma > 0.0) //添加平移噪声
PerturbPoint3(translation_sigma, camera + camera_block_size() - 6);
}
}
common.h
#pragma once
/// 从文件读入BAL dataset
class BALProblem {
public:
/// load bal data from text file
explicit BALProblem(const std::string &filename, bool use_quaternions = false);
~BALProblem() {
//析构函数 delete指针
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,
const double translation_sigma,
const double point_sigma); //添加高斯噪声
int camera_block_size() const {
return use_quaternions_ ? 10 : 9; } //返回相机位姿参数的内存大小 （是否使用四元数）
int point_block_size() const {
return 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_; } //参数（pose和point）首地址
const double *cameras() const {
return parameters_; } //相机参数首地址
const double *points() const {
return parameters_ + camera_block_size() * num_cameras_; } //路标point的首地址
/// camera参数的起始地址
//mutable 修饰的代表在使用时慧改变原有值
double *mutable_cameras() {
return parameters_; } //相机参数首地址
double *mutable_points() {
return parameters_ + camera_block_size() * num_cameras_; } //路标point的首地址
double *mutable_camera_for_observation(int i) {
return mutable_cameras() + camera_index_[i] * camera_block_size(); //第i个观测值对应的相机参数地址
}
double *mutable_point_for_observation(int i) {
return mutable_points() + point_index_[i] * point_block_size(); //第i个观测值对应的路标point
}
const double *camera_for_observation(int i) const {
//第i个观测值对应的相机参数地址
return cameras() + camera_index_[i] * camera_block_size();
}
const double *point_for_observation(int i) const {
//第i个观测值对应的路标point
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_;
};
random.h
#ifndef RAND_H
#define RAND_H
#include rotation.h
#ifndef ROTATION_H
#define ROTATION_H
#include SnavelyReprojectionError.h
#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H
#include