专业做财经直播网站有哪些,网站同步到新浪微博怎么做,猎头公司是什么,数字创意设计包括哪些工具在g2o的基础上改成ceres优化#xff0c;高博都写好了其他的部分, 后面改ceres就很简单了. 这块我用的是ceres的自动求导#xff0c;很方便#xff0c;就是转化为模板仿函数的时候有点麻烦#xff0c; 代码部分如下
ceres_type.h : ceres优化核心库的头文件
这个文件写的内…在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 (tanhaozhangconnect.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.hnamespace sad
{namespace ceres_optimization
{class PreintegrationCostFunctionCore {public:PreintegrationCostFunctionCore(std::shared_ptrsad::IMUPreintegration imu_preinit, const Eigen::Vector3d gravaty): preinit_(imu_preinit), dt_(imu_preinit-dt_), grav_(gravaty) {}template typename Tbool operator()(const T* const i, const T* const j, T* residual) const {Eigen::MatrixT, 3, 1 r_i(i[0], i[1], i[2]);Eigen::MatrixT, 3, 1 r_j(j[0], j[1], j[2]);Eigen::MatrixT, 3, 1 p_i(i[3], i[4], i[5]);Eigen::MatrixT, 3, 1 p_j(j[3], j[4], j[5]);Eigen::MatrixT, 3, 1 v_i(i[6], i[7], i[8]);Eigen::MatrixT, 3, 1 v_j(j[6], j[7], j[8]);Eigen::MatrixT, 3, 1 bg(i[9], i[10], i[11]);Eigen::MatrixT, 3, 1 ba(i[12], i[13], i[14]);Sophus::SO3double dR preinit_-GetDeltaRotation(preinit_-bg_);Eigen::Matrixdouble, 3, 1 dvd preinit_-GetDeltaVelocity(preinit_-bg_, preinit_-ba_);Eigen::MatrixT, 3, 1 dv(T(dvd.x()), T(dvd.y()), T(dvd.z()));Eigen::Matrixdouble, 3, 1 dpd preinit_-GetDeltaPosition(preinit_-bg_, preinit_-ba_);Eigen::MatrixT, 3, 1 dp(T(dpd.x()), T(dpd.y()), T(dpd.z()));Sophus::SO3T, 0 R_i Sophus::SO3T, 0::exp(r_i);Sophus::SO3T, 0 R_j Sophus::SO3T, 0::exp(r_j);Eigen::MatrixT, 3, 1 grav(T(grav_.x()), T(grav_.y()), T(grav_.z()));Eigen::MatrixT, 3, 1 er (dR.inverse() * R_i.inverse() * R_j).log();Eigen::MatrixT, 3, 3 RiT R_i.matrix();Eigen::MatrixT, 3, 1 ev RiT * (v_j - v_i - grav * T(dt_)) - dv;Eigen::MatrixT, 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_ptrsad::IMUPreintegration preinit_ nullptr;const Eigen::Vector3d grav_;};ceres::CostFunction* CreatePreintegrationCostFunction(std::shared_ptrsad::IMUPreintegration imu_preinit, const Eigen::Vector3d gravaty) {return new ceres::AutoDiffCostFunctionPreintegrationCostFunctionCore, 9, 15, 15(new PreintegrationCostFunctionCore(imu_preinit, gravaty));}class BiasCostFunctionCore {public:BiasCostFunctionCore(){}template typename Tbool operator() (const T* const i, const T* const j, T* residual) const{Eigen::MatrixT, 3, 1 bg_i(i[9], i[10], i[11]);Eigen::MatrixT, 3, 1 bg_j(j[9], j[10], j[11]);Eigen::MatrixT, 3, 1 ba_i(i[12], i[13], i[14]);Eigen::MatrixT, 3, 1 ba_j(j[12], j[13], j[14]);Eigen::MatrixT, 3, 1 d_ba ba_j - ba_i;Eigen::MatrixT, 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::AutoDiffCostFunctionBiasCostFunctionCore, 6, 15, 15(new BiasCostFunctionCore());}class PriorCostFunctionCore {public:PriorCostFunctionCore(const std::shared_ptrsad::NavStated prior) : prior_(prior) {}template typename Tbool 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::Matrixdouble, 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_ptrsad::NavStated prior_;};ceres::CostFunction* CreatePriorCostFunction(const std::shared_ptrsad::NavStated prior) {return new ceres::AutoDiffCostFunctionPriorCostFunctionCore, 15, 15(new PriorCostFunctionCore(prior));}class GnssCostFunctionCore {public:GnssCostFunctionCore(const Sophus::SE3d gnss_states) : gnss_states_(gnss_states){}template typename Tbool operator() (const T* const i, T* residual) const{Eigen::MatrixT, 3, 1 r_i(i[0], i[1], i[2]);Sophus::SO3T, 0 R_i Sophus::SO3T, 0::exp(r_i);Eigen::MatrixT, 3, 1 t_i(i[3], i[4], i[5]);Eigen::MatrixT, 3, 1 e_r (gnss_states_.so3().inverse() * R_i).log();Eigen::MatrixT, 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::AutoDiffCostFunctionGnssCostFunctionCore, 6, 15 (new GnssCostFunctionCore(gnss_states));}class OdomCostFunctionCore {public:OdomCostFunctionCore(const Eigen::Vector3d odom_speed_states) : odom_speed_states_(odom_speed_states) {}template typename Tbool operator() (const T* const j, T* residual ) const {Eigen::MatrixT, 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::AutoDiffCostFunctionOdomCostFunctionCore, 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);//GNSSproblem.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);//ODOMVec3d vel_world Vec3d::Zero();Vec3d vel_odom Vec3d::Zero();if (last_odom_set_) {// velocity obsdouble 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;}// 重置integoptions_.preinteg_options_.init_bg_ this_frame_-bg_;options_.preinteg_options_.init_ba_ this_frame_-ba_;pre_integ_ std::make_sharedIMUPreintegration(options_.preinteg_options_);
}上面部分代码为了使用autodiff做了好多不必要的数据处理, 如果有更好的解题思路欢迎留言. 上面代码分为以下几步: 1. 初始处理 2. 添加残差 3. ceres优化 4. 更新 效果如下
感觉还行, 没有评价精度
问题
没有评价精度是不是比g2o好一些 没有评价算力是不是比g2o小一些 没有实现解析求导, 正在搞
