提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档
文章目录
- 前言
- 一、ceres是什么？
- 二、基于ceres求解slam的后端
- 1.基本步骤
- 总结
前言
提示：这里可以添加本文要记录的大概内容：
上一个文章记录了使用g2o完成后端的图优化过程，这篇文章记录下使用ceres完成后端图优化。
提示：以下是本篇文章正文内容，下面案例可供参考
一、ceres是什么？
Ceres solver 是谷歌开发的一款用于非线性优化的库, 在激光SLAM和V-SLAM的优化中均有着大量的应用, 如Cartographer, VINS 等等.
二、基于ceres求解slam的后端
1.基本步骤
由于还是基于图优化的理论，不过是使用ceres库完成图优化。所以主要还是构建节点，边，然后进行优化。
1.添加节点；
2.添加边；
3.优化求解。
1.添加节点：
eres内部没有节点和边的概念, 所以在AddNode函数中, 不像g2o那样可以直接将节点添加到g2o中, 这里首先将节点保存起来, 等到优化的时候再进行计算, 再传入到ceres中.
由于ceres中的数据类型是double, 所以在保存的时候进行了格式的转换.
// 添加节点
void CeresSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan> *pVertex)
{
karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();//获取位姿
int pose_id = pVertex->GetObject()->GetUniqueId();//获取节点ID
Pose2d pose2d;//结构体-ceres可以使用
pose2d.x = pose.GetX();//进行保存
pose2d.y = pose.GetY();
pose2d.yaw_radians = pose.GetHeading();
poses_[pose_id] = pose2d;
}
struct Pose2d
{
double x;
double y;
double yaw_radians;
};2.添加边
AddConstraint这个函数中的实现和AddNode函数是差不多的.
由于不能直接将边添加到ceres中, 所以在这先做了数据格式的转换, 然后将这个边保存起来.
这里保存的数据分别是 2个节点的 ID, 2个节点间的位姿变换.
在保存位姿的协方差矩阵时进行了求逆的操作, 所以保存的是信息矩阵.
// 添加约束
void CeresSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan> *pEdge)
{
karto::LocalizedRangeScan *pSource = pEdge->GetSource()->GetObject();//节点1
karto::LocalizedRangeScan *pTarget = pEdge->GetTarget()->GetObject();//节点2
karto::LinkInfo *pLinkInfo = (karto::LinkInfo *)(pEdge->GetLabel());
karto::Pose2 diff = pLinkInfo->GetPoseDifference();//两个位姿间的坐标变换？
karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();//信息矩阵，协方差矩阵的逆
Eigen::Matrix<double, 3, 3> info;
info(0, 0) = precisionMatrix(0, 0);
info(0, 1) = info(1, 0) = precisionMatrix(0, 1);
info(0, 2) = info(2, 0) = precisionMatrix(0, 2);
info(1, 1) = precisionMatrix(1, 1);
info(1, 2) = info(2, 1) = precisionMatrix(1, 2);
info(2, 2) = precisionMatrix(2, 2);//信息矩阵赋值
Eigen::Vector3d measurement(diff.GetX(), diff.GetY(), diff.GetHeading());
Constraint2d constraint2d;
constraint2d.id_begin = pSource->GetUniqueId();
constraint2d.id_end = pTarget->GetUniqueId();
constraint2d.x = measurement(0);
constraint2d.y = measurement(1);
constraint2d.yaw_radians = measurement(2);
constraint2d.information = info;
constraints_.push_back(constraint2d);//存储
}
struct Constraint2d
{
int id_begin;
int id_end;
double x;
double y;
double yaw_radians;
Eigen::Matrix3d information;
};3.优化求解
进行求解, 并将优化后的位姿进行保存.
之前都是在保存数据, 真正的计算就在这2个函数里了, 分别是 BuildOptimizationProblem() 与 SolveOptimizationProblem().
// 优化求解
void CeresSolver::Compute()
{
corrections_.clear();
ROS_INFO("[ceres] Calling ceres for Optimization");
ceres::Problem problem;
BuildOptimizationProblem(constraints_, &poses_, &problem);//搭建最小二乘问题
SolveOptimizationProblem(&problem);//求解
ROS_INFO("[ceres] Optimization finished\n");
for (std::map<int, Pose2d>::const_iterator pose_iter = poses_.begin(); pose_iter != poses_.end(); ++pose_iter)
{
karto::Pose2 pose(pose_iter->second.x, pose_iter->second.y, pose_iter->second.yaw_radians);
corrections_.push_back(std::make_pair(pose_iter->first, pose));//存储
}
}可以在代码中看到，首先是 BuildOptimizationProblem(constraints_, &poses_, &problem);//搭建最小二乘问题，来看下其源代码：
void CeresSolver::BuildOptimizationProblem(const std::vector<Constraint2d> &constraints, std::map<int, Pose2d> *poses,
ceres::Problem *problem)
{
if (constraints.empty())
{
std::cout << "No constraints, no problem to optimize.";
return;
}
ceres::LocalParameterization *angle_local_parameterization = AngleLocalParameterization::Create();//函数定义了角度的更新方式
for (std::vector<Constraint2d>::const_iterator constraints_iter = constraints.begin();
constraints_iter != constraints.end(); ++constraints_iter)
{
const Constraint2d &constraint = *constraints_iter;
std::map<int, Pose2d>::iterator pose_begin_iter = poses->find(constraint.id_begin);
std::map<int, Pose2d>::iterator pose_end_iter = poses->find(constraint.id_end);//找到位姿的起始和终止id
// 对information开根号
const Eigen::Matrix3d sqrt_information = constraint.information.llt().matrixL();
// Ceres will take ownership of the pointer.
ceres::CostFunction *cost_function =
PoseGraph2dErrorTerm::Create(constraint.x, constraint.y, constraint.yaw_radians, sqrt_information);//建立损失函数
problem->AddResidualBlock(cost_function, nullptr,
&pose_begin_iter->second.x,
&pose_begin_iter->second.y,
&pose_begin_iter->second.yaw_radians,
&pose_end_iter->second.x,
&pose_end_iter->second.y,
&pose_end_iter->second.yaw_radians);//添加约束块
//等到迭代遍历时，就加入了每一个constraint内两个pose的yaw角，如下所示：
problem->SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameterization); //为yaw角设置localparameterization
problem->SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameterization);//将这个角度更新方式的限定与角度变量关联起来
}
std::map<int, Pose2d>::iterator pose_start_iter = poses->begin();
problem->SetParameterBlockConstant(&pose_start_iter->second.x);
problem->SetParameterBlockConstant(&pose_start_iter->second.y);
problem->SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
//固定初始位姿，如果所有位姿中没有一个位姿是固定的，那么将有无穷多个解。
}AngleLocalParameterization::Create();//函数定义了角度的更新方式
代码如下（示例）：
// Defines a local parameterization for updating the angle to be constrained in
// [-pi to pi).
class AngleLocalParameterization
{
public:
template <typename T>
bool operator()(const T *theta_radians, const T *delta_theta_radians, T *theta_radians_plus_delta) const
{
*theta_radians_plus_delta = NormalizeAngle(*theta_radians + *delta_theta_radians);
return true;
}//使用仿函数 构建函数更新方式，先是进入operator()，然后使用了内联函数NormalizeAngle，将角度固定在-pi 到 pi 之间。
static ceres::LocalParameterization *Create()
{
return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization, 1, 1>);
}
};
// Normalizes the angle in radians between [-pi and pi).
template <typename T>
inline T NormalizeAngle(const T &angle_radians)
{
// Use ceres::floor because it is specialized for double and Jet types.
T two_pi(2.0 * M_PI);
return angle_radians - two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
}残差计算
首先通过 PoseGraph2dErrorTerm::Create() 函数进行 ceres::CostFunction 的声明, 并传入 约束的 x, y, theta 以及开方后的信息矩阵 到 PoseGraph2dErrorTerm类的构造函数中将数据进行保存.
之后, 通过重载 () 定义了残差的计算方式. 仿函数的参数就是通过 problem->AddResidualBlock() 传入进来的, 分别是2个位姿的xy与角度.
2个位姿间的残差, 可以这样计算.
构造时传入了这2个位姿间的坐标变换, 而通过这两个节点的位姿又可以算出一个坐标变换, 在正常情况下这2个坐标变换应该是相等的, 所以, 可以让这两个坐标变换的差作为残差.（这里面我的理解是，传入的坐标变换应该是前端里程计计算出来的两点间的坐标变换，然后2个坐标的位姿应该是雷达或者其他传感器测量出来的，这样就构建出了测量和估计间的误差项）
代码中的 RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) 就是根据2个节点的位姿算出的坐标变换, p_ab_.cast() 就是传入的这2个位姿间的坐标变换, 通过让2者相减得到 xy的残差.
同理, 角度相减得到角度的残差.
这里, 在计算完成之后, 又将残差左乘了开方后的信息矩阵, 通过协方差矩阵来对计算出的残差添加噪声.
template <typename T>
Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians)
{
const T cos_yaw = ceres::cos(yaw_radians);
const T sin_yaw = ceres::sin(yaw_radians);
Eigen::Matrix<T, 2, 2> rotation;
rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
return rotation;
}
// Computes the error term for two poses that have a relative pose measurement
// between them. Let the hat variables be the measurement.
//
// residual = information^{1/2} * [ r_a^T * (p_b - p_a) - \hat{p_ab} ]
// [ Normalize(yaw_b - yaw_a - \hat{yaw_ab}) ]
//
// where r_a is the rotation matrix that rotates a vector represented in frame A
// into the global frame, and Normalize(*) ensures the angles are in the range
// [-pi, pi).
class PoseGraph2dErrorTerm
{
public:
PoseGraph2dErrorTerm(double x_ab, double y_ab, double yaw_ab_radians, const Eigen::Matrix3d &sqrt_information)
: p_ab_(x_ab, y_ab), yaw_ab_radians_(yaw_ab_radians), sqrt_information_(sqrt_information)
{
}
template <typename T>
bool operator()(const T *const x_a, const T *const y_a, const T *const yaw_a,
const T *const x_b, const T *const y_b, const T *const yaw_b,
T *residuals_ptr) const
{
const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals_ptr);
residuals_map.template head<2>() = RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();
residuals_map(2) = NormalizeAngle((*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
// Scale the residuals by the square root information matrix to account for
// the measurement uncertainty.
residuals_map = sqrt_information_.template cast<T>() * residuals_map;
return true;
}
static ceres::CostFunction *Create(double x_ab, double y_ab, double yaw_ab_radians,
const Eigen::Matrix3d &sqrt_information)
{
return (new ceres::AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(
new PoseGraph2dErrorTerm(x_ab, y_ab, yaw_ab_radians, sqrt_information)));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
// The position of B relative to A in the A frame.
const Eigen::Vector2d p_ab_;
// The orientation of frame B relative to frame A.
const double yaw_ab_radians_;
// The inverse square root of the measurement covariance matrix.
const Eigen::Matrix3d sqrt_information_;
};之后就是求解函数SolveOptimizationProblem(ceres::Problem *problem)
// Returns true if the solve was successful.
bool CeresSolver::SolveOptimizationProblem(ceres::Problem *problem)
{
assert(problem != NULL);
ceres::Solver::Options options;//ceres求解优化项
options.max_num_iterations = 100;//最大迭代次数100次
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;//线性函数求解类型CHOLESKY
ceres::Solver::Summary summary;
ceres::Solve(options, problem, &summary);
std::cout << summary.BriefReport() << '\n';
return summary.IsSolutionUsable();
}