环境问题
ceres 的更新中提到LocalParameterization将在版本2.2中删除,如果编译报错,可安装低版本的ceres。
http://ceres-solver.org/version_history.html#id1
ceres入门
参考文档:
slam中ceres的用法解析: link
ceres的常见用法总结:link
官方文档:http://ceres-solver.org/
简单的优化问题可以分两类:
1、残差的维度和优化变量的维度相同;
2、残差的维度和优化变量的维度不同;
1、维度相同
优化问题最重要的就是构建残差函数和求雅可比矩阵,维度相同时优化更新可以直接相加减,基本步骤是:
1.建立一个最小二乘问题;
2.建立CostFunction;
3.添加残差模块;
4.设置求解器;
用一个简单的例子结合代码说明使用方法,y = a x + b ,随即生成一些含噪声的点(x,y),通过优化得到a和b的值。
// 一个仿函数用于设置残差计算
class ExpResidual {
public:
ExpResidual(double x, double y) : x_(x), y_(y) {}
// 残差计算
// 注意残差计算operator后边是先优化变量再残差,和AutoDiff中相反
template <typename T>
bool operator()(const T* a, const T* b, T* residual) const {
residual[0] = T(y_) - a[0] * T(x_) - b[0];
return true;
}
const double x_;
const double y_;
};
int main(int argc, char **argv)
{
double a = 0.0, b = 0.0;
cv::RNG rng;
double w_sigma = 0.2;
// 1. 建立一个最小二乘问题
ceres::Problem problem;
for (int i = 0; i < 100; ++i) {
// 按照m = 2.2, c = 1.1 来对100个点进行赋值,并给予白噪声
std::cout << "gaussian = " << rng.gaussian(w_sigma * w_sigma) << std::endl;
double y_with_noise = 2.2 * i + 1.1 + rng.gaussian(w_sigma * w_sigma);
// 2. 建立一个CostFunction, 如果是AutoDiff自动求解就如下
// AutoDiffCostFunction<仿函数类,残差的维度,剩下的一堆是优化变量的维度>
ceres::CostFunction* cost_function =
new ceres::AutoDiffCostFunction<ExpResidual, 1, 1, 1>( new ExpResidual(i , y_with_noise));
// 3. 添加残差模块,用problem.AddResidualBlock(costFunc, 鲁邦核函数,优化变量们用指针传)
problem.AddResidualBlock(cost_function, new ceres::CauchyLoss(0.5), &a, &b);
}
// 4. 设置求解器和输出报告
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << std::endl;
std::cout << "a = " << a << " b = " << b << std::endl;
return 0;
}
结果很接近
如果不使用自动求导而使用解析式求导(即直接给出导数的解析形式),可以通过定义一个SizedCostFunction的子类,实现Evaluate函数,手动给出雅可比。代码如下:
// 第一个参数是ResidualBlock的维数
// 第二个参数是ParameterBlock的维数
class LineCostFunction : public ceres::SizedCostFunction<1,1,1>{
public:
LineCostFunction(const double x_, const double y_) : x(x_),y(y_){}
virtual ~LineCostFunction(){}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const{
const double a = parameters[0][0];
const double b = parameters[1][0];
residuals[0] = y - a * x - b;
if(jacobians != NULL)
{
if(jacobians[0] != NULL)
{
jacobians[0][0] = - x ;
jacobians[0][1] = -1.0 ;
}
}
return true;
}
protected:
const double x;
const double y;
};
主函数里面这样构建优化问题:
// 2.1 建立一个CostFunction, 如果是AutoDiff自动求解就如下
// AutoDiffCostFunction<仿函数类,残差的维度,剩下的一堆是优化变量的维度>
// ceres::CostFunction* cost_function =
// new ceres::AutoDiffCostFunction<ExpResidual, 1, 1, 1>( new ExpResidual(i , y_with_noise));
// 2.2 解析式求导数
ceres::CostFunction* cost_function = new LineCostFunction(i , y_with_noise);
// 3. 添加残差模块,用problem.AddResidualBlock(costFunc, 鲁邦核函数,优化变量们用指针传)
problem.AddResidualBlock(cost_function, new ceres::CauchyLoss(0.5), &a, &b);
2、维度不同
优化问题最重要的就是要计算雅克比矩阵和残差,因此如何执行加减法运算对于优化求解至关重要。由于四元数并不是通过简单的加减乘除就可以进行运算,而是有一套自己的计算方法,因此这种情况必须手动定义加运算,在ceres中要增加ceres::LocalParameterization来定义运算。
aloam(自动求导)
该代码优化变量为七维位姿parameters[7], t(3,1) | q(4, 1) 使用ceres自带的计算四元数的方法ceres::EigenQuaternionParameterization()
// 1. 设置鲁邦核函数,用在AddResidualBlock
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
// 2. 设置LocalParameterization,即四元数的加法定义
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
// 3. 建立求解器选项
ceres::Problem::Options problem_options;
//...
ceres::Problem problem(problem_options);
// 4.注意到如果使用了LocalParameterization,那么必须添加AddParameterBlock来添加不规则+-优化变量
problem.AddParameterBlock(parameters, 4, q_parameterization);
problem.AddParameterBlock(parameters + 4, 3);
// 5.使用工厂模式创建cost_function
ceres::CostFunction *cost_function =
LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
// 定义了激光雷达角点计算残差的方式,和上一个例子中class ExpResidual类似
// 与上一个例子class ExpResidual不同的是,增加了Create函数,
// 实际上省掉了一些主函数中要写的东西
struct LidarEdgeFactor {
LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
Eigen::Vector3d last_point_b_, double s_)
: curr_point(curr_point_), last_point_a(last_point_a_),
last_point_b(last_point_b_), s(s_) {}
template<typename T>
bool operator()(const T *q, const T *t, T *residual) const { // 仿函数,用于计算残差
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};
// Eigen::Quaternion<T>
Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]}; // q
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)}; // 单位四元数
q_last_curr = q_identity.slerp(T(s), q_last_curr); // 四元数线性插值, s是0-1之间的值,用于
Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; // t
Eigen::Matrix<T, 3, 1> lp;
lp = q_last_curr * cp + t_last_curr;
// distance = |(pi - pj)x(pi - pl)| / |pj - pl|
Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);
Eigen::Matrix<T, 3, 1> de = lpa - lpb;
residual[0] = nu.x() / de.norm();
residual[1] = nu.y() / de.norm();
residual[2] = nu.z() / de.norm();
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_,
const Eigen::Vector3d last_point_a_,
const Eigen::Vector3d last_point_b_,
const double s_)
{
return (new ceres::AutoDiffCostFunction<LidarEdgeFactor, 3, 4, 3>(
new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)
));
}
Eigen::Vector3d curr_point, last_point_a, last_point_b;
double s;
};
aloam (解析式求导)
线特征雅克比推导:
面特征雅克比推导
代码实现:
反对称矩阵定义
inline Eigen::Matrix<double,3,3> skew(Eigen::Matrix<double,3,1>& mat_in){
Eigen::Matrix<double,3,3> skew_mat;
skew_mat.setZero();
skew_mat(0,1) = -mat_in(2);
skew_mat(0,2) = mat_in(1);
skew_mat(1,2) = -mat_in(0);
skew_mat(1,0) = mat_in(2);
skew_mat(2,0) = -mat_in(1);
skew_mat(2,1) = mat_in(0);
return skew_mat;
}
定义CostFunction
// EdgeAnalyticCostFunction
class LidarEdgeAnalyticCostFunction : public ceres::SizedCostFunction<1, 4, 3> {
public:
LidarEdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
Eigen::Vector3d last_point_b_, double s_)
: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const
{
Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);
Eigen::Vector3d lp;
Eigen::Vector3d lp_r;
lp_r = q_last_curr * curr_point; // for computing Jacobian of Rotation: dp_by_dr
lp = q_last_curr * curr_point + t_last_curr; //new point
Eigen::Vector3d nu = (lp - last_point_b).cross(lp - last_point_a);
Eigen::Vector3d de = last_point_a - last_point_b;
residuals[0] = nu.norm() / de.norm();
if(jacobians != NULL)
{
if(jacobians[0] != NULL)
{
Eigen::Vector3d X = nu;
Eigen::Vector3d re = last_point_a - last_point_b;
Eigen::Matrix3d skew_re = skew(re);
Eigen::Matrix3d skew_de = skew(de);
// Rotation
Eigen::Matrix3d skew_lp_r = skew(lp_r);
Eigen::Matrix<double, 3, 3> dp_by_dr;
dp_by_dr.block<3,3>(0,0) = -skew_lp_r;
Eigen::Map<Eigen::Matrix<double, 1, 4, Eigen::RowMajor> > J_so3_r(jacobians[0]);
J_so3_r.setZero();
J_so3_r.block<1,3>(0,0) = X.transpose() * skew_de * dp_by_dr / X.norm() / de.norm();
// Translation
Eigen::Matrix<double, 3, 3> dp_by_dt;
(dp_by_dt.block<3,3>(0,0)).setIdentity();
Eigen::Map<Eigen::Matrix<double, 1, 3, Eigen::RowMajor> > J_so3_t(jacobians[1]);
J_so3_t.setZero();
J_so3_t.block<1,3>(0,0) = X.transpose() * skew_re * dp_by_dt / X.norm() / de.norm();
}
}
return true;
}
protected:
Eigen::Vector3d curr_point, last_point_a, last_point_b;
double s;
};
// PlaneAnalyticCostFunction
class LidarPlaneAnalyticCostFunction : public ceres::SizedCostFunction<1, 4, 3> {
public:
LidarPlaneAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_), last_point_m(last_point_m_), s(s_) {}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const
{
Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);
Eigen::Vector3d lp;
Eigen::Vector3d lp_r;
lp_r = q_last_curr * curr_point; // for computing Jacobian of Rotation: dp_dr
lp = q_last_curr * curr_point + t_last_curr; //new point
Eigen::Vector3d de = (last_point_l-last_point_j).cross(last_point_m-last_point_j);
de.normalize();
double nu = (lp-last_point_j).dot(de);
residuals[0] = nu / de.norm();
if(jacobians != NULL)
{
if(jacobians[0] != NULL)
{
Eigen::Vector3d dX_dp = de / de.norm();
double X = nu / de.norm();
Eigen::Vector3d ddh_dp = X * dX_dp / std::abs(X);
// Rotation
Eigen::Matrix3d skew_lp_r = skew(lp_r);
Eigen::Matrix<double, 3, 3> dp_dr;
dp_dr.block<3,3>(0,0) = -skew_lp_r;
Eigen::Map<Eigen::Matrix<double, 1, 4, Eigen::RowMajor> > J_so3_r(jacobians[0]);
J_so3_r.setZero();
J_so3_r.block<1,3>(0,0) = ddh_dp.transpose() * dp_dr;
// Translation
Eigen::Matrix<double, 3, 3> dp_dt;
(dp_dt.block<3,3>(0,0)).setIdentity();
Eigen::Map<Eigen::Matrix<double, 1, 3, Eigen::RowMajor> > J_so3_t(jacobians[1]);
J_so3_t.setZero();
J_so3_t.block<1,3>(0,0) = ddh_dp.transpose() * dp_dt;
}
}
return true;
}
protected:
Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
double s;
};
定义旋转参数块
这里可以用ceres的四元数参数块:
config_.q_parameterization_ptr = new ceres::EigenQuaternionParameterization();
自己定义四元数更新参数块
// 自定义旋转残差块
//参考博客 https://blog.csdn.net/jdy_lyy/article/details/119360492
class PoseSO3Parameterization : public ceres::LocalParameterization { // 自定义so3 旋转块
public:
PoseSO3Parameterization() { }
virtual ~PoseSO3Parameterization() { }
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const //参数正切空间上的更新函数
{
Eigen::Map<const Eigen::Quaterniond> quater(x); // 待更新的四元数
Eigen::Map<const Eigen::Vector3d> delta_so3(delta); // delta 值,使用流形 so3 更新
Eigen::Quaterniond delta_quater = Sophus::SO3d::exp(delta_so3).unit_quaternion(); // so3 转换位 delta_p 四元数
Eigen::Map<Eigen::Quaterniond> quter_plus(x_plus_delta); // 更新后的四元数
// 旋转更新公式
quter_plus = (delta_quater*quater).normalized();
return true;
}
virtual bool ComputeJacobian(const double* x, double* jacobian) const // 四元数对so3的偏导数
{
Eigen::Map<Eigen::Matrix<double, 4, 3, Eigen::RowMajor>> j(jacobian);
(j.topRows(3)).setIdentity();
(j.bottomRows(1)).setZero();
return true;
}
// virtual bool MultiplyByJacobian(const double* x,
// const int num_rows,
// const double* global_matrix,
// double* local_matrix) const;//一般不用
virtual int GlobalSize() const {return 4;} // 参数的实际维数
virtual int LocalSize() const {return 3;} // 正切空间上的参数维数
};
解析求导
CeresALOAMRegistration::~CeresALOAMRegistration() {
}
/**
* @brief add residual block for edge constraint from lidar frontend
* @param source, source point
* @param target_x, target point x
* @param target_y, target point y
* @param ratio, interpolation ratio
* @return void
*/
bool CeresALOAMRegistration::AddEdgeFactor(
const Eigen::Vector3d &source,
const Eigen::Vector3d &target_x, const Eigen::Vector3d &target_y,
const double &ratio
) {
/*自动求导*/
/* ceres::CostFunction *factor_edge = LidarEdgeFactor::Create(
source,
target_x, target_y,
ratio
);
problem_.AddResidualBlock(
factor_edge,
config_.loss_function_ptr,
param_.q, param_.t
);*/
/*解析求导*/
ceres::CostFunction *factor_analytic_edge = new LidarEdgeAnalyticCostFunction(
source,
target_x, target_y,
ratio
);
problem_.AddResidualBlock(
factor_analytic_edge, // 约束边 cost_function
config_.loss_function_ptr, // 鲁棒核函数 lost_function
param_.q, param_.t // 关联参数
);
return true;
}
/**
* @brief add residual block for plane constraint from lidar frontend
* @param source, source point
* @param target_x, target point x
* @param target_y, target point y
* @param target_z, target point z
* @param ratio, interpolation ratio
* @return void
*/
bool CeresALOAMRegistration::AddPlaneFactor(
const Eigen::Vector3d &source,
const Eigen::Vector3d &target_x, const Eigen::Vector3d &target_y, const Eigen::Vector3d &target_z,
const double &ratio
) {
/*自动求导*/
/*
ceres::CostFunction *factor_plane = LidarPlaneFactor::Create(
source,
target_x, target_y, target_z,
ratio
);
problem_.AddResidualBlock(
factor_plane,
config_.loss_function_ptr,
param_.q, param_.t
);
*/
/*解析求导*/
ceres::CostFunction *factor_analytic_plane =new LidarPlaneAnalyticCostFunction(
source,
target_x, target_y, target_z,
ratio
);
problem_.AddResidualBlock(
factor_analytic_plane,
config_.loss_function_ptr,
param_.q, param_.t
);
return true;
}
evo 评价