在g2o的基础上改成ceres优化，高博都写好了其他的部分, 后面改ceres就很简单了. 这块我用的是ceres的自动求导，很方便，就是转化为模板仿函数的时候有点麻烦， 代码部分如下
ceres_type.h : ceres优化核心库的头文件
这个文件写的内容ceres优化的残差块. 把i, j时刻的状态都写成15维的数组, 顺序是r,p,v,bg,ba. 每个元素都是3维的, 所以r 部分涉及到R->r转换, sophus都实现好了.
/**
* @file ceres_types.cc
* @author Frank Zhang (tanhaozhang@connect.polyu.hk)
* @brief
* @version 0.1
* @date 2023-08-15
*
* @copyright Copyright (c) 2023
*
*/
#ifndef SLAM_IN_AUTO_DRIVING_CH4_CERES_TYPE_H_
#define SLAM_IN_AUTO_DRIVING_CH4_CERES_TYPE_H_
#include <glog/logging.h>
#include "common/eigen_types.h"
#include "ceres/ceres.h"
#include "thirdparty/sophus/sophus/so3.hpp"
#include "ch4/imu_preintegration.h"
namespace sad
{
namespace ceres_optimization
{
class PreintegrationCostFunctionCore {
public:
PreintegrationCostFunctionCore(std::shared_ptr<sad::IMUPreintegration> imu_preinit, const Eigen::Vector3d gravaty)
: preinit_(imu_preinit), dt_(imu_preinit->dt_), grav_(gravaty) {}
template <typename T>
bool operator()(const T* const i, const T* const j, T* residual) const {
Eigen::Matrix<T, 3, 1> r_i(i[0], i[1], i[2]);
Eigen::Matrix<T, 3, 1> r_j(j[0], j[1], j[2]);
Eigen::Matrix<T, 3, 1> p_i(i[3], i[4], i[5]);
Eigen::Matrix<T, 3, 1> p_j(j[3], j[4], j[5]);
Eigen::Matrix<T, 3, 1> v_i(i[6], i[7], i[8]);
Eigen::Matrix<T, 3, 1> v_j(j[6], j[7], j[8]);
Eigen::Matrix<T, 3, 1> bg(i[9], i[10], i[11]);
Eigen::Matrix<T, 3, 1> ba(i[12], i[13], i[14]);
Sophus::SO3<double> dR = preinit_->GetDeltaRotation(preinit_->bg_);
Eigen::Matrix<double, 3, 1> dvd = preinit_->GetDeltaVelocity(preinit_->bg_, preinit_->ba_);
Eigen::Matrix<T, 3, 1> dv(T(dvd.x()), T(dvd.y()), T(dvd.z()));
Eigen::Matrix<double, 3, 1> dpd = preinit_->GetDeltaPosition(preinit_->bg_, preinit_->ba_);
Eigen::Matrix<T, 3, 1> dp(T(dpd.x()), T(dpd.y()), T(dpd.z()));
Sophus::SO3<T, 0> R_i = Sophus::SO3<T, 0>::exp(r_i);
Sophus::SO3<T, 0> R_j = Sophus::SO3<T, 0>::exp(r_j);
Eigen::Matrix<T, 3, 1> grav(T(grav_.x()), T(grav_.y()), T(grav_.z()));
Eigen::Matrix<T, 3, 1> er = (dR.inverse() * R_i.inverse() * R_j).log();
Eigen::Matrix<T, 3, 3> RiT = R_i.matrix();
Eigen::Matrix<T, 3, 1> ev = RiT * (v_j - v_i - grav * T(dt_)) - dv;
Eigen::Matrix<T, 3, 1> ep = RiT * (p_j - p_i - v_i * T(dt_) - grav * T(dt_) * T(dt_) * T(0.5)) - dp;
residual[0] = T(er[0]);
residual[1] = T(er[1]);
residual[2] = T(er[2]);
residual[3] = T(ev[0]);
residual[4] = T(ev[1]);
residual[5] = T(ev[2]);
residual[6] = T(ep[0]);
residual[7] = T(ep[1]);
residual[8] = T(ep[2]);
return true;
}
private:
const double dt_;
std::shared_ptr<sad::IMUPreintegration> preinit_ = nullptr;
const Eigen::Vector3d grav_;
};
ceres::CostFunction* CreatePreintegrationCostFunction(std::shared_ptr<sad::IMUPreintegration> imu_preinit, const Eigen::Vector3d gravaty) {
return new ceres::AutoDiffCostFunction<PreintegrationCostFunctionCore, 9, 15, 15>(new PreintegrationCostFunctionCore(imu_preinit, gravaty));
}
class BiasCostFunctionCore {
public:
BiasCostFunctionCore(){}
template <typename T>
bool operator() (const T* const i, const T* const j, T* residual) const
{
Eigen::Matrix<T, 3, 1> bg_i(i[9], i[10], i[11]);
Eigen::Matrix<T, 3, 1> bg_j(j[9], j[10], j[11]);
Eigen::Matrix<T, 3, 1> ba_i(i[12], i[13], i[14]);
Eigen::Matrix<T, 3, 1> ba_j(j[12], j[13], j[14]);
Eigen::Matrix<T, 3, 1> d_ba = ba_j - ba_i;
Eigen::Matrix<T, 3, 1> d_bg = bg_j - bg_i;
residual[0] = T(d_ba[0]);
residual[1] = T(d_ba[1]);
residual[2] = T(d_ba[2]);
residual[3] = T(d_bg[0]);
residual[4] = T(d_bg[1]);
residual[5] = T(d_bg[2]);
return true;
}
};
ceres::CostFunction* CreateBiasCostFunction() {
return new ceres::AutoDiffCostFunction<BiasCostFunctionCore, 6, 15, 15>(
new BiasCostFunctionCore()
);
}
class PriorCostFunctionCore {
public:
PriorCostFunctionCore(const std::shared_ptr<sad::NavStated> prior) : prior_(prior) {}
template <typename T>
bool operator()(const T* const i, T* residual) const {
Eigen::Vector3d prior_r_d = prior_->R_.log();
Eigen::Vector3d prior_p_d = prior_->p_;
Eigen::Vector3d prior_v_d = prior_->v_;
Eigen::Vector3d prior_bg_d = prior_->bg_;
Eigen::Vector3d prior_ba_d = prior_->ba_;
Eigen::Matrix<double, 15, 1> prior_M;
prior_M << prior_r_d, prior_p_d, prior_v_d, prior_bg_d, prior_ba_d;
for (int temp = 0; temp < prior_M.size(); temp++)
{
residual[temp] = T(prior_M[temp]) - i[temp];
}
return true;
}
private:
const std::shared_ptr<sad::NavStated> prior_;
};
ceres::CostFunction* CreatePriorCostFunction(const std::shared_ptr<sad::NavStated> prior) {
return new ceres::AutoDiffCostFunction<PriorCostFunctionCore, 15, 15>(new PriorCostFunctionCore(prior));
}
class GnssCostFunctionCore {
public:
GnssCostFunctionCore(const Sophus::SE3d gnss_states) : gnss_states_(gnss_states){}
template <typename T>
bool operator() (const T* const i, T* residual) const
{
Eigen::Matrix<T, 3, 1> r_i(i[0], i[1], i[2]);
Sophus::SO3<T, 0> R_i = Sophus::SO3<T, 0>::exp(r_i);
Eigen::Matrix<T, 3, 1> t_i(i[3], i[4], i[5]);
Eigen::Matrix<T, 3, 1> e_r = (gnss_states_.so3().inverse() * R_i).log();
Eigen::Matrix<T, 3, 1> e_t = t_i - gnss_states_.translation();
residual[0] = T(e_r[0]);
residual[1] = T(e_r[1]);
residual[2] = T(e_r[2]);
residual[3] = T(e_t[0]);
residual[4] = T(e_t[1]);
residual[5] = T(e_t[2]);
return true;
}
private:
const Sophus::SE3d gnss_states_;
};
static ceres::CostFunction* CreateGnssCostFunction(const Sophus::SE3d gnss_states){
return new ceres::AutoDiffCostFunction<GnssCostFunctionCore, 6, 15> (
new GnssCostFunctionCore(gnss_states)
);
}
class OdomCostFunctionCore {
public:
OdomCostFunctionCore(const Eigen::Vector3d odom_speed_states) : odom_speed_states_(odom_speed_states) {}
template <typename T>
bool operator() (const T* const j, T* residual ) const {
Eigen::Matrix<T, 3, 1> vj(j[6], j[7], j[8]);
residual[0] = T(vj[0] - odom_speed_states_[0]);
residual[1] = T(vj[1] - odom_speed_states_[1]);
residual[2] = T(vj[2] - odom_speed_states_[2]);
return true;
}
private:
const Eigen::Vector3d odom_speed_states_;
};
static ceres::CostFunction* CreatOdomCostFunction(const Eigen::Vector3d odom_speed_states) {
return new ceres::AutoDiffCostFunction<OdomCostFunctionCore, 3, 15> (
new OdomCostFunctionCore(odom_speed_states)
);
}
} // namespace ceres_optimization
} //namespace sad
#endif
上面代码分别实现了预积分, bias, 先验, GNSS, odom的残差以及其工厂函数. 不得不说啊, ceres自动求导用起来真简单.
gins_pre_integ.cc: 实现ceres预积分优化
这一部分调用上面头文件构造的工厂函数实现残差计算, ceres优化与更新. 这里只粘贴一下不同的地方
else {
LOG_FIRST_N(INFO, 1) << "Using Ceres to Solve!";
ceres::Problem problem;
Eigen::Vector3d last_r_vec = last_frame_->R_.log();
Eigen::Vector3d current_r_vec = this_frame_->R_.log();
double last_state[15] = {last_r_vec.x(), last_r_vec.y(), last_r_vec.z(),
last_frame_->p_.x(),last_frame_->p_.y(), last_frame_->p_.z(),
last_frame_->v_.x(), last_frame_->v_.y(), last_frame_->v_.z(),
last_frame_->bg_.x(), last_frame_->bg_.y(), last_frame_->bg_.z(),
last_frame_->ba_.x(), last_frame_->ba_.y(), last_frame_->ba_.z()};
double current_state[15] = {current_r_vec.x(), current_r_vec.y(), current_r_vec.z(),
this_frame_->p_.x(), this_frame_->p_.y(), this_frame_->p_.z(),
this_frame_->v_.x(), this_frame_->v_.y(), this_frame_->v_.z(),
this_frame_->bg_.x(), this_frame_->bg_.y(), this_frame_->bg_.z(),
this_frame_->ba_.x(), this_frame_->ba_.y(), this_frame_->ba_.z()};
//预积分
problem
.AddResidualBlock(ceres_optimization::CreatePreintegrationCostFunction(pre_integ_, options_.gravity_), nullptr, last_state, current_state);
// 两个零偏
problem.AddResidualBlock(ceres_optimization::CreateBiasCostFunction(), nullptr, last_state, current_state);
//GNSS
problem.AddResidualBlock(ceres_optimization::CreateGnssCostFunction(last_gnss_.utm_pose_), nullptr, last_state);
problem.AddResidualBlock(ceres_optimization::CreateGnssCostFunction(this_gnss_.utm_pose_), nullptr,
current_state);
//先验
problem.AddResidualBlock(ceres_optimization::CreatePriorCostFunction(last_frame_), nullptr, last_state);
//ODOM
Vec3d vel_world = Vec3d::Zero();
Vec3d vel_odom = Vec3d::Zero();
if (last_odom_set_) {
// velocity obs
double velo_l = options_.wheel_radius_ * last_odom_.left_pulse_ /
options_.circle_pulse_ * 2 * M_PI /
options_.odom_span_;
double velo_r = options_.wheel_radius_ * last_odom_.right_pulse_ /
options_.circle_pulse_ * 2 * M_PI /
options_.odom_span_;
double average_vel = 0.5 * (velo_l + velo_r);
vel_odom = Vec3d(average_vel, 0.0, 0.0);
vel_world = this_frame_->R_ * vel_odom;
problem.AddResidualBlock(ceres_optimization::CreatOdomCostFunction(vel_world), nullptr, current_state);
// 重置odom数据到达标志位，等待最新的odom数据
last_odom_set_ = false;
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.max_num_iterations = 20;
options.num_threads = 4;
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
Eigen::Vector3d last_r(last_state[0], last_state[1], last_state[2]);
last_frame_->R_ = Sophus::SO3d::exp(last_r);
Eigen::Vector3d last_t(last_state[3], last_state[4], last_state[5]);
last_frame_->p_ = last_t;
Eigen::Vector3d last_v(last_state[6], last_state[7], last_state[8]);
last_frame_->v_ = last_v;
Eigen::Vector3d last_bg(last_state[9], last_state[10], last_state[11]);
last_frame_->bg_ = last_bg;
Eigen::Vector3d last_ba(last_state[12], last_state[13], last_state[14]);
last_frame_->ba_ = last_ba;
Eigen::Vector3d current_r(current_state[0], current_state[1], current_state[2]);
this_frame_->R_ = Sophus::SO3d::exp(current_r);
Eigen::Vector3d current_t(current_state[3], current_state[4], current_state[5]);
this_frame_->p_ = current_t;
Eigen::Vector3d current_v(current_state[6], current_state[7], current_state[8]);
this_frame_->v_ = current_v;
Eigen::Vector3d current_bg(current_state[9], current_state[10], current_state[11]);
this_frame_->bg_ = current_bg;
Eigen::Vector3d current_ba(current_state[12], current_state[13], current_state[14]);
this_frame_->ba_ = current_ba;
}
// 重置integ
options_.preinteg_options_.init_bg_ = this_frame_->bg_;
options_.preinteg_options_.init_ba_ = this_frame_->ba_;
pre_integ_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
}
上面部分代码为了使用autodiff做了好多不必要的数据处理, 如果有更好的解题思路欢迎留言.
上面代码分为以下几步: 1. 初始处理 2. 添加残差 3. ceres优化 4. 更新
效果如下
感觉还行, 没有评价精度
问题
没有评价精度是不是比g2o好一些
没有评价算力是不是比g2o小一些
没有实现解析求导, 正在搞
1114
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
