Ceres解决实车SLAM后端优化问题
一家之言，仅作分享，如有不合理或需要改进的地方，欢迎各位讨论。
BA与位姿图优化的区别
SLAM后端优化常用的方法有BA和位姿图优化。二者的区别有：
- 优化变量不同。BA优化的是观测到的路标和相机位姿。位姿图优化（pose graph）优化的是相机的位姿。
- 代价函数的误差项不同。BA的代价函数的误差项是3D路标点重投影后的像素坐标值与实际观测到的像素坐标值的误差。位姿图优化的误差项是两个相邻帧的位姿之间变换的观测值与估计值的误差（观测值计算方法例如icp，估计值计算方法例如dr）以及回环检测时两个达到回环的非相邻帧位姿之间变换的观测值与估计值的误差。
重投影误差
重投影也就是指的第二次投影，
- 其实第一次投影指的就是相机在拍照的时候三维空间点投影到图像上，
- 然后我们利用这些图像对一些特征点进行三角定位（triangulation），利用几何信息（对极几何） 构建三角形来确定三维空间点的位置，
- 最后利用我们计算得到的三维点的坐标（注意不是真实的）和我们计算得到的相机位姿（当然也不是真实的）进行第二次投影，也就是重投影。
重投影误差：指的真实三维空间点在图像平面上的投影（也就是图像上的像素点）和重投影（其实是用我们的计算值得到的虚拟的像素点）的差值。
因为种种原因计算得到的值和实际情况不会完全相符，也就是这个差值不可能恰好为0，此时也就需要将这些差值的和最小化获取最优的相机位姿参数及三维空间点的坐标，即为BA优化。
位姿图优化的扩展：加入GPS约束
在实际复杂大尺度城市环境中，车辆往往经过很长时间/距离的驾驶才能回到过去访问的地点，传统的闭环检测方法不适用。所以引入 GPS 约束把闭环前的累积误差降低在米级，使得闭环的搜索范围缩小到可计算的区间。
这样，可以新增一种边：某一时刻车辆的位姿点的估计值与对应GPS值的误差，可根据不同GNSS状态调整信息矩阵。
至此，位姿图优化共有三种边：
- 相邻关键帧的位姿变化的估计值与观测值的误差，用来约束平滑；
- 每一关键帧的GPS值与估计值的误差，用来降低单帧位姿累积误差；
- 实现闭环时的两个关键帧的位姿变化的估计值与对submap的观测值的误差，用来纠正累积误差。
bool PoseGraph2d::Optimize(int max_iter, bool useGpsConstraint, bool isGlobal)
{
if(poses_.empty()){
return 0;
}
if (constraints_.empty()){
return 0;
}
ceres::Problem problem;
ceres::Solver::Options options;
options.max_num_iterations = max_iter;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
ceres::LossFunction* loss_function = NULL;
ceres::LocalParameterization* angle_local_parameteration = AngleLocalParameterization::Create();
if(useGpsConstraint == true)
{
if(isGlobal == true) // do global optimization with gps constraint.
{
for (int i=0; i<constraints_.size(); i++)
{
std::map<int, Pose2d>::iterator pose_begin_iter = poses_.find(constraints_[i].id_begin);
if(pose_begin_iter == poses_.end())
{
continue;
}
std::map<int, Pose2d>::iterator pose_end_iter = poses_.find(constraints_[i].id_end);
if(pose_end_iter == poses_.end())
{
continue;
}
const Eigen::Matrix3d sqrt_information = constraints_[i].information.llt().matrixL();
ceres::CostFunction* cost_function =
PoseGraph2dErrorTermWithGpsConstraint::Create( constraints_[i].x, constraints_[i].y,
constraints_[i].yaw_radians,
pose_end_iter->second,
useGpsConstraint,
sqrt_information);
problem.AddResidualBlock(cost_function, loss_function,
&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);
problem.SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameteration);
problem.SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameteration);
}
// fix first node
std::map<int, Pose2d>::iterator pose_start_iter = poses_.begin();
if(pose_start_iter == poses_.end()){
return 0;
}
problem.SetParameterBlockConstant(&pose_start_iter->second.x);
problem.SetParameterBlockConstant(&pose_start_iter->second.y);
problem.SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
}
else // do local optimization with gps constraint.
{
int numPos = std::min<int>(poses_.size(), 10);
std::vector<Constraint2d> tmpCons;
std::map<int, Pose2d>::reverse_iterator rit;
for (rit=poses_.rbegin(); rit!=poses_.rend(); rit++)
{
if(numPos--<0)
break;
int numCons = std::min<int>(constraints_.size(), 100);
for (int i=constraints_.size()-numCons; i<constraints_.size(); i++)
{
if (constraints_[i].id_begin == rit->first)
{
tmpCons.push_back(constraints_[i]);
}
}
}
for (int i=0; i<tmpCons.size(); i++)
{
std::map<int, Pose2d>::iterator pose_begin_iter = poses_.find(tmpCons[i].id_begin);
if(pose_begin_iter == poses_.end())
{
continue;
}
std::map<int, Pose2d>::iterator pose_end_iter = poses_.find(tmpCons[i].id_end);
if(pose_end_iter == poses_.end())
{
continue;
}
const Eigen::Matrix3d sqrt_information = tmpCons[i].information.llt().matrixL();
ceres::CostFunction* cost_function =
PoseGraph2dErrorTermWithGpsConstraint::Create( tmpCons[i].x, tmpCons[i].y,
tmpCons[i].yaw_radians,
pose_end_iter->second,
useGpsConstraint,
sqrt_information);
problem.AddResidualBlock(cost_function, loss_function,
&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);
problem.SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameteration);
problem.SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameteration);
}
//fix first node
int minID = tmpCons[0].id_begin;
for(size_t i=0; i<tmpCons.size(); i++)
{
if(tmpCons[i].id_begin < minID)
minID = tmpCons[i].id_begin;
}
std::map<int, Pose2d>::iterator pose_start_iter = poses_.find(minID);
if(pose_start_iter == poses_.end())
return 0;
problem.SetParameterBlockConstant(&pose_start_iter->second.x);
problem.SetParameterBlockConstant(&pose_start_iter->second.y);
problem.SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
}
}
else
{
if(isGlobal == true) // do global optimization without gps constraint.
{
for (int i=0; i<constraints_.size(); i++)
{
std::map<int, Pose2d>::iterator pose_begin_iter = poses_.find(constraints_[i].id_begin);
if(pose_begin_iter == poses_.end())
{
continue;
}
std::map<int, Pose2d>::iterator pose_end_iter = poses_.find(constraints_[i].id_end);
if(pose_end_iter == poses_.end())
{
continue;
}
const Eigen::Matrix3d sqrt_information = constraints_[i].information.llt().matrixL();
ceres::CostFunction* cost_function =
PoseGraph2dErrorTerm::Create( constraints_[i].x, constraints_[i].y, constraints_[i].yaw_radians,
sqrt_information);
problem.AddResidualBlock(cost_function, loss_function,
&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);
problem.SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameteration);
problem.SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameteration);
}
// fix first node
std::map<int, Pose2d>::iterator pose_start_iter = poses_.begin();
if(pose_start_iter == poses_.end()){
return 0;
}
problem.SetParameterBlockConstant(&pose_start_iter->second.x);
problem.SetParameterBlockConstant(&pose_start_iter->second.y);
problem.SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
}
else // do local optimization without gps constraint.
{
int numPos = std::min<int>(poses_.size(), 10);
std::vector<Constraint2d> tmpCons;
std::map<int, Pose2d>::reverse_iterator rit;
for (rit=poses_.rbegin(); rit!=poses_.rend(); rit++)
{
if(numPos--<0)
break;
int numCons = std::min<int>(constraints_.size(), 100);
for (int i=constraints_.size()-numCons; i<constraints_.size(); i++)
{
if (constraints_[i].id_begin == rit->first)
{
tmpCons.push_back(constraints_[i]);
}
}
}
for (int i=0; i<tmpCons.size(); i++)
{
std::map<int, Pose2d>::iterator pose_begin_iter = poses_.find(tmpCons[i].id_begin);
if(pose_begin_iter == poses_.end())
{
continue;
}
std::map<int, Pose2d>::iterator pose_end_iter = poses_.find(tmpCons[i].id_end);
if(pose_end_iter == poses_.end())
{
continue;
}
const Eigen::Matrix3d sqrt_information = tmpCons[i].information.llt().matrixL();
ceres::CostFunction* cost_function =
PoseGraph2dErrorTerm::Create( tmpCons[i].x, tmpCons[i].y, tmpCons[i].yaw_radians,
sqrt_information);
problem.AddResidualBlock(cost_function, loss_function,
&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);
problem.SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameteration);
problem.SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameteration);
}
// fix first node
int minID = tmpCons[0].id_begin;
for(size_t i=0; i<tmpCons.size(); i++)
{
if(tmpCons[i].id_begin < minID)
minID = tmpCons[i].id_begin;
}
std::map<int, Pose2d>::iterator pose_start_iter = poses_.find(minID);
if(pose_start_iter == poses_.end())
return 0;
problem.SetParameterBlockConstant(&pose_start_iter->second.x);
problem.SetParameterBlockConstant(&pose_start_iter->second.y);
problem.SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
}
}
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
return true;
}
对应的代价函数的仿函数如下：
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* x_a, const T* y_a, const T* yaw_rad_a,
const T* x_b, const T* y_b, const T* yaw_rad_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::Matrix<T, 2, 1> pos_err = RotationMatrix2D(*yaw_rad_a).transpose()*(p_b-p_a) - p_ab_.cast<T>();
T angle_err = ceres::slam2d::NormalizeAngle(
(*yaw_rad_b - *yaw_rad_a) - static_cast<T>(yaw_ab_radians_));
Eigen::Matrix<T,3,1> err_mat;
err_mat(0,0) = pos_err(0,0);
err_mat(1,0) = pos_err(1,0);
err_mat(2,0) = angle_err;
err_mat = sqrt_information_.template cast<T>() * err_mat;
residuals_ptr[0] = err_mat(0,0);
residuals_ptr[1] = err_mat(1,0);
residuals_ptr[2] = err_mat(2,0);
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)));
}
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_;
};
// add gps constraint
class PoseGraph2dErrorTermWithGpsConstraint {
public:
PoseGraph2dErrorTermWithGpsConstraint(double x_ab, double y_ab, double yaw_ab_radians,
Pose2d curpose, int useGpsConstraint,
const Eigen::Matrix3d& sqrt_information)
: p_ab_(x_ab, y_ab),
yaw_ab_radians_(yaw_ab_radians),
gps_b_(curpose.gps_x, curpose.gps_y, curpose.yaw_radians, curpose.isFixed),
useGpsConstraint_(useGpsConstraint),
sqrt_information_(sqrt_information) {}
template <typename T>
bool operator()(const T* x_a, const T* y_a, const T* yaw_rad_a,
const T* x_b, const T* y_b, const T* yaw_rad_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::Matrix<T, 2, 1> pos_err = RotationMatrix2D(*yaw_rad_a).transpose()*(p_b-p_a) - p_ab_.cast<T>();
T angle_err = ceres::slam2d::NormalizeAngle(
(*yaw_rad_b - *yaw_rad_a) - static_cast<T>(yaw_ab_radians_));
Eigen::Matrix<T,3,1> err_mat;
err_mat(0,0) = pos_err(0,0);
err_mat(1,0) = pos_err(1,0);
err_mat(2,0) = angle_err;
err_mat = sqrt_information_.template cast<T>() * err_mat;
residuals_ptr[0] = err_mat(0,0);
residuals_ptr[1] = err_mat(1,0);
residuals_ptr[2] = err_mat(2,0);
//gps constraint
const T delta_gps_x = T(gps_b_[0]) - *x_b;
const T delta_gps_y = T(gps_b_[1]) - *y_b;
const T err_gps[3] = {delta_gps_x,
delta_gps_y,
ceres::slam2d::NormalizeAngle(T(gps_b_[2]) - *yaw_rad_b)};
double gps_weight;
if(gps_b_[3] == 1) // gps is fixed.
{
gps_weight = 1;
}
else
{
gps_weight = 0.02;
}
if(useGpsConstraint_ == true)
{
err_mat(0,0) = T(sqrt_information_(0,0)*0.5*gps_weight);
err_mat(1,0) = T(sqrt_information_(1,1)*0.5*gps_weight);
err_mat(2,0) = T(sqrt_information_(2,2)*0.1);
}
else
{
err_mat(0,0) = T(0.0);
err_mat(1,0) = T(0.0);
err_mat(2,0) = T(0.0);
}
residuals_ptr[3] = err_mat(0,0)*err_gps[0];
residuals_ptr[4] = err_mat(1,0)*err_gps[1];
residuals_ptr[5] = err_mat(2,0)*err_gps[2];
return true;
}
static ceres::CostFunction* Create(double x_ab, double y_ab, double yaw_ab_radians,
Pose2d curpose, bool useGpsConstraint,
const Eigen::Matrix3d& sqrt_information) {
return (new ceres::AutoDiffCostFunction<PoseGraph2dErrorTermWithGpsConstraint, 6, 1, 1, 1, 1, 1, 1>
(new PoseGraph2dErrorTermWithGpsConstraint(
x_ab, y_ab, yaw_ab_radians, curpose, useGpsConstraint,
sqrt_information)));
}
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_;
// gps info of B : x, y, yaw_radiant, isFixed;
const Eigen::Vector4d gps_b_;
// use gps constraint
const bool useGpsConstraint_;
};
BA优化的扩展：不局限于图像重投影误差约束的路标与位姿优化
传统意义的BA是针对视觉SLAM的，这里在解决语义slam时，考虑到的路标都是二维的空间点信息，我们是不需要重投影这个操作的。但在建图时也需要对路标和位姿同时进行优化，代价函数考虑的则是空间三维路标点在某一时刻关键帧的位姿估计值下进行坐标转换为自车坐标值与这一时刻该关键帧位姿下的观测值的误差。与传统BA的区别主要在于路标点进行转换后的坐标系不同，传统BA是通过重投影得到像素坐标系下的估计值，而语义SLAM中用到的是通过旋转平移矩阵得到的自车坐标下下的估计值。用ceres解决优化问题，只是新增了一种边，这里不做代码说明。