Ceres-Solver学习笔记(4)
我学习Ceres的目的主要也是做BA，所以有些例程就不多分析了，简单的介绍一下:
- circle_fit.cc 显示了怎么去拟合一个圆
- ellipse_approximation.cc 我也没太搞明白，这个例程的目的是展示如何使用Solver::Options::dynamic_sparsity，这有利于解决数值稠密，动态稀疏的问题。
- denoising.cc 实现了图像去噪声，使用Fields of Experts 模型。
- nist.cc 实现解决 NIST 非线性回归问题。
- libmv_bundle_adjuster.cc 是 Blender/libmv 使用的BA。
- robot_pose_mle.cc这个例子展示了如何利用DynamicAutoDiffCostFunction 。DynamicAutoDiffCostFunction 意味着参数的维度在编译时未知的情况。
这里我们再学习一下3D图优化的例子slam/pose_graph_3d/pose_graph_3d.cc ，毕竟咱们就是做SLAM的，还有几个例子这里就不提了。
// file type.h
#include 为 MapOfPoses
typedef std::map<int, Pose3d, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, Pose3d> > >
MapOfPoses;
// 在姿态图中两个顶点之间的约束。约束是从顶点id_begin到顶点id_end的转换
struct Constraint3d {
int id_begin;
int id_end;
// 结束帧 E 到开始帧 B 之间的转换. 他把E坐标系下的向量转换到B坐标系。
Pose3d t_be;
// 测量的协方差矩阵的逆。 项的顺序是x, y, z, delta orientation.
Eigen::Matrix<double, 6, 6> information;
static std::string name() {
return "EDGE_SE3:QUAT";
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
std::istream& operator>>(std::istream& input, Constraint3d& constraint) {
Pose3d& t_be = constraint.t_be;
input >> constraint.id_begin >> constraint.id_end >> t_be;
for (int i = 0; i < 6 && input.good(); ++i) {
for (int j = i; j < 6 && input.good(); ++j) {
input >> constraint.information(i, j);
if (i != j) {
constraint.information(j, i) = constraint.information(i, j);
}
}
}
return input;
}
typedef std::vector
#include "Eigen/Core"
#include "ceres/autodiff_cost_function.h"
#include "types.h"
namespace ceres {
namespace examples {
// 计算两个有相对姿态测量的位姿之间的误差项
// hat 变量是测量值,我们有两个姿态x_a,x_b,通过传感器我们可以测量帧A,B之间的变换，表示成t_ab_hat. 我们可以计算当前估计的姿态和测量之间的误差。
//
// 我们把刚体变换表示成一个四元数q和位置p，四元数表示成[x, y, z, w].
// The estimated measurement is:
// t_ab = [ p_ab ] = [ R(q_a)^T * (p_b - p_a) ]
// [ q_ab ] [ q_a^{-1] * q_b ]
//
// ^{-1} 表示逆运算，R(q) i表示四元数到旋转矩阵
// For the orientation error, we will use the
// standard multiplicative error resulting in:
//
// error = [ p_ab - \hat{p}_ab ]
// [ 2.0 * Vec(q_ab * \hat{q}_ab^{-1}) ]
//
// Vec(*) 返回四元数的向量（虚部） 部分.
// 因为测量有一个不确定性与他的精度有关, 我们把误差 乘以 信息矩阵的平方根
//
// residuals = I^{1/2) * error
// I表示信息矩阵 是 协方差的 逆.
class PoseGraph3dErrorTerm {
public:
PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
const Eigen::Matrix<double, 6, 6>& sqrt_information)
: t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}
template <typename T>
bool operator()(const T* const p_a_ptr, const T* const q_a_ptr,
const T* const p_b_ptr, const T* const q_b_ptr,
T* residuals_ptr) const {
// 由p_a_ptr构造 3行1列的矩阵 p_a
Eigen::Map<const Eigen::Matrix3, 1> > p_a(p_a_ptr);
// 由q_a_ptr构造 四元数 q_a
Eigen::Map<const Eigen::Quaternion > q_a(q_a_ptr);
Eigen::Map<const Eigen::Matrix3, 1> > p_b(p_b_ptr);
Eigen::Map<const Eigen::Quaternion > q_b(q_b_ptr);
// Compute the relative transformation between the two frames.
// q_a的逆 等于 q_a的 共軛
Eigen::Quaternion q_a_inverse = q_a.conjugate();
// q_ab = q_a^{-1] * q_b
Eigen::Quaternion q_ab_estimated = q_a_inverse * q_b;
// p_ab = R(q_a)^T * (p_b - p_a) 旋转矩阵的转置是对的，与q_a_inverse相同
Eigen::Matrix3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);
// 计算误差 q_ab_measured × q_ab_estimated^{-1]
Eigen::Quaternion delta_q =
t_ab_measured_.q.template cast() * q_ab_estimated.conjugate();
// 计算误差
// [ position ] [ delta_p ]
// [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
Eigen::Map6, 1> > residuals(residuals_ptr);
residuals.template block<3, 1>(0, 0) =
p_ab_estimated - t_ab_measured_.p.template cast();
residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();
// 乘以信息矩阵的平方根
residuals.applyOnTheLeft(sqrt_information_.template cast());
return true;
}
static ceres::CostFunction* Create(
const Pose3d& t_ab_measured,
const Eigen::Matrix<double, 6, 6>& sqrt_information) {
//构造 CostFunction
return new ceres::AutoDiffCostFunction6, 3, 4, 3, 4>(
new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
// The measurement for the position of B relative to A in the A frame.
const Pose3d t_ab_measured_;
// The square root of the measurement information matrix.
const Eigen::Matrix<double, 6, 6> sqrt_information_;
};
} // namespace examples
} // namespace ceres // file pose_graph_3d.cc
#include <iostream>
#include <fstream>
#include <string>
#include "ceres/ceres.h"
#include "common/read_g2o.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "pose_graph_3d_error_term.h"
#include "types.h"
DEFINE_string(input, "", "The pose graph definition filename in g2o format.");
namespace ceres {
namespace examples {
void BuildOptimizationProblem(const VectorOfConstraints& constraints,
MapOfPoses* poses, ceres::Problem* problem) {
CHECK(poses != NULL);
CHECK(problem != NULL);
if (constraints.empty()) {
LOG(INFO) << "No constraints, no problem to optimize.";
return;
}
ceres::LossFunction* loss_function = NULL;
ceres::LocalParameterization* quaternion_local_parameterization =
new EigenQuaternionParameterization;
// 遍历 所有约束条件
for (VectorOfConstraints::const_iterator constraints_iter =
constraints.begin();
constraints_iter != constraints.end(); ++constraints_iter) {
const Constraint3d& constraint = *constraints_iter;
// 找到起始帧
MapOfPoses::iterator pose_begin_iter = poses->find(constraint.id_begin);
CHECK(pose_begin_iter != poses->end())
<< "Pose with ID: " << constraint.id_begin << " not found.";
// 找到结束帧
MapOfPoses::iterator pose_end_iter = poses->find(constraint.id_end);
CHECK(pose_end_iter != poses->end())
<< "Pose with ID: " << constraint.id_end << " not found.";
// 平方根计算 Cholesky分解
const Eigen::Matrix<double, 6, 6> sqrt_information =
constraint.information.llt().matrixL();
// 创建 CostFunction.
ceres::CostFunction* cost_function =
PoseGraph3dErrorTerm::Create(constraint.t_be, sqrt_information);
// CostFunction需要用到的数据
// 迭代器的second 就是 Pose3d
problem->AddResidualBlock(cost_function, loss_function,
pose_begin_iter->second.p.data(),
pose_begin_iter->second.q.coeffs().data(),
pose_end_iter->second.p.data(),
pose_end_iter->second.q.coeffs().data());
// 这个不太懂，好像与设置求Jacobin有关
problem->SetParameterization(pose_begin_iter->second.q.coeffs().data(),
quaternion_local_parameterization);
problem->SetParameterization(pose_end_iter->second.q.coeffs().data(),
quaternion_local_parameterization);
}
// 姿态图优化有6个自由度，但不是完全约束的，通常叫做gauge freedom
// 你可以对所有的节点应用一个严格的转动，优化还是保持相同的cost
// Levenberg-Marquardt 算法能更好的解决这种情况，但是最好还是固定这个 gauge freedom，
// 可以通过把一个姿态设为常量，优化不去改变他
MapOfPoses::iterator pose_start_iter = poses->begin();
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
// 将初始姿态设置为常量
problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
}
// Returns true if the solve was successful.
bool SolveOptimizationProblem(ceres::Problem* problem) {
CHECK(problem != NULL);
ceres::Solver::Options options;
options.max_num_iterations = 200;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
ceres::Solver::Summary summary;
ceres::Solve(options, problem, &summary);
std::cout << summary.FullReport() << '\n';
return summary.IsSolutionUsable();
}
// Output the poses to the file with format: id x y z q_x q_y q_z q_w.
bool OutputPoses(const std::string& filename, const MapOfPoses& poses) {
std::fstream outfile;
outfile.open(filename.c_str(), std::istream::out);
if (!outfile) {
LOG(ERROR) << "Error opening the file: " << filename;
return false;
}
for (std::map<int, Pose3d, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, Pose3d> > >::
const_iterator poses_iter = poses.begin();
poses_iter != poses.end(); ++poses_iter) {
const std::map<int, Pose3d, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, Pose3d> > >::
value_type& pair = *poses_iter;
outfile << pair.first << " " << pair.second.p.transpose() << " "
<< pair.second.q.x() << " " << pair.second.q.y() << " "
<< pair.second.q.z() << " " << pair.second.q.w() << '\n';
}
return true;
}
} // namespace examples
} // namespace ceres
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
CHECK(FLAGS_input != "") << "Need to specify the filename to read.";
ceres::examples::MapOfPoses poses;
ceres::examples::VectorOfConstraints constraints;
CHECK(ceres::examples::ReadG2oFile(FLAGS_input, &poses, &constraints))
<< "Error reading the file: " << FLAGS_input;
std::cout << "Number of poses: " << poses.size() << '\n';
std::cout << "Number of constraints: " << constraints.size() << '\n';
CHECK(ceres::examples::OutputPoses("poses_original.txt", poses))
<< "Error outputting to poses_original.txt";
ceres::Problem problem;
ceres::examples::BuildOptimizationProblem(constraints, &poses, &problem);
CHECK(ceres::examples::SolveOptimizationProblem(&problem))
<< "The solve was not successful, exiting.";
CHECK(ceres::examples::OutputPoses("poses_optimized.txt", poses))
<< "Error outputting to poses_original.txt";
return 0;
}并不清楚SetParameterization()函数怎么用，感觉Ceres在用法上面不如G2O好用，可能更灵活一点吧，看了两天，还是有很多坑啊