orb-slam3：优化状态量是camera坐标系下RPV还是body坐标系下RPV探究

ORB-SLAM3的对于状态量RPV的优化，添加的残差块主要是：
（1）IMU残差（预积分论文）
（2）视觉残差（SLAM14讲-P187）

状态量优化：RPV = RPV + J*△RPV
更新状态是通过J(e/RPV)*△RPV，其中雅可比J(e/RPV)是残差e对于状态量(RPV)的导数

那么优化的状态量RPV究竟是body坐标系的还是camera坐标系的呢？
答案：body系

优化的状态变量是在body坐标系下，从以下几个方面得以验证：

1. 关于状态变量的更新

对于优化变量VertexPose* VP，其更新函数Update()如下
可以看出是Twb进行直接更新的，Tbw以及Tcw是通过Twb进行间接更新的

void ImuCamPose::Update(const double *pu)
{Eigen::Vector3d ur, ut;ur << pu[0], pu[1], pu[2];ut << pu[3], pu[4], pu[5];// Update body posetwb += Rwb * ut;Rwb = Rwb * ExpSO3(ur);// Update camera posesconst Eigen::Matrix3d Rbw = Rwb.transpose();const Eigen::Vector3d tbw = -Rbw * twb;for(int i=0; i<pCamera.size(); i++){Rcw[i] = Rcb[i] * Rbw;tcw[i] = Rcb[i] * tbw + tcb[i];}}

2. IMU残差计算

残差计算都是关于Rwb或者twb的，也就是body坐标系

const Eigen::Vector3d er = LogSO3(dR.transpose()*VP1->estimate().Rwb.transpose()*VP2->estimate().Rwb);
const Eigen::Vector3d ev = VP1->estimate().Rwb.transpose()*(VV2->estimate() - VV1->estimate() - g*dt) - dV;
const Eigen::Vector3d ep = VP1->estimate().Rwb.transpose()*(VP2->estimate().twb - VP1->estimate().twb- VV1->estimate()*dt - g*dt*dt/2) - dP;

雅可比也是关于body坐标系下求导

    // Jacobians wrt Pose 1//顺序: er ev ep 分别对 VPk(VP1)求导 _jacobianOplus[0].setZero();                   // er/Pi = 0  全部设置为0// rotation                                    // ev/Pi = 0  _jacobianOplus[0].block<3,3>(0,0) = -invJr * Rwb2.transpose() * Rwb1; _jacobianOplus[0].block<3,3>(3,0) = Sophus::SO3d::hat(Rbw1*(VV2->estimate() - VV1->estimate() - g*dt));// er/Ri = -J(dR)'*Rj_T*Ri_jacobianOplus[0].block<3,3>(6,0) = Sophus::SO3d::hat(Rbw1*(VP2->estimate().twb - VP1->estimate().twb  // ev/Ri = (Ri_T*(Vj-Vi-g*t))^- VV1->estimate()*dt - 0.5*g*dt*dt));                   // ep/Ri = (Ri_T*(Pj-Pi-Vi*t-1/2*g*t*t))^// translation                                 _jacobianOplus[0].block<3,3>(6,3) = -Eigen::Matrix3d::Identity();        // ep/Pi = -I(3x1)

3. 视觉重投影误差计算

void EdgeMonoOnlyPose::linearizeOplus()
{const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw[cam_idx];const Eigen::Vector3d &tcw = VPose->estimate().tcw[cam_idx];const Eigen::Vector3d Xc = Rcw*Xw + tcw;//相机坐标系转换为body坐标系const Eigen::Vector3d Xb = VPose->estimate().Rbc[cam_idx]*Xc + VPose->estimate().tbc[cam_idx];const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb[cam_idx];//(残差)对(相机坐标系下的P)求导Eigen::Matrix<double,2,3> proj_jac = VPose->estimate().pCamera[cam_idx]->projectJac(Xc);//(body坐标系下的P)对(旋转李代数)求导Eigen::Matrix<double,3,6> SE3deriv;double x = Xb(0);double y = Xb(1);double z = Xb(2);SE3deriv << 0.0,  z , -y , 1.0, 0.0, 0.0,-z , 0.0,  x , 0.0, 1.0, 0.0,y , -x , 0.0, 0.0, 0.0, 1.0;//链式法则：(残差)对(旋转李代数)求导 = (残差)对(相机坐标系下的P)求导 * (相机坐标系下的P)对(body坐标系下的P)求导 * (body坐标系下的P)对(旋转李代数)求导_jacobianOplusXi = proj_jac * Rcb * SE3deriv; // symbol different becasue of update mode
}

雅可比公式(2x6维)_jacobianOplusXi = proj_jac * Rcb * SE3deriv的推导

orb-slam3：优化状态量是camera坐标系下RPV还是body坐标系下RPV探究相关推荐

ORB SLAM3——IMU优化部分精读-VertexPose(ImuCamPose)的更新量到底是什么？
ORB SLAM3--IMU优化部分精读-VertexPose(ImuCamPose)的更新量到底是什么? 先说答案 void ImuCamPose::Update(const double *pu) ...
ORB SLAM3论文大致翻译，手动整理
ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM I. INTRODU ...
ORB SLAM3加载Vocabulary更快ORBvoc.bin
最近ORB SLAM3刚开源,跑了跑,发现源代码加载词袋还是ORBvoc.txt,加载时间比较慢,这里教你修改成ORBvoc.bin加载,1秒内完成加载. 1. 将ORBvoc.bin拷贝到Vocab ...
Android camera (12)---camera ap在特殊的应用场景下额外使用一套独立的camera tuning参数
camera ap在特殊的应用场景下额外使用一套独立的camera tuning参数解决方案: 1, nvram_drv.h core\featureio\drv\inc 中添加下面的interfa ...
Part-1 ORB SLAM3初始化-1
初始化 ORB SLAM3的初始化主要是创建ORB词袋.关键帧数据库.多地图等对象,其步骤如下: 检测配置文件能否打开加载ORB词袋(ORBVocabulary) 创建关键帧数据库(KeyFrame ...
【Java代码】坐标系说明+WGS84\GCJ02\BD09坐标系转换工具+Java坐标系转换及验证源代码分享（粘贴可用）
1. 坐标系说明开发地图应用服务时,一定会接触到各种坐标系,而保证坐标系的正确与合理是一切数据分析的前提,总的来说,坐标系可以分为两大类:地理坐标系GCS(Geographic Coordinate ...
【自动驾驶】33.【图像坐标系】到【像素坐标系】的度量单位变换、【英寸】、【感光芯片】
我们理论分析的时候,使用小孔成像模型来分析. 其中,(u,v)(u,v)(u,v)为像素坐标,(cx,cy)(c_x, c_y)(cx,cy)表示相机主点(principal point),即相机 ...
Windows的三种坐标系：屏幕坐标系，非客户区坐标系，客户区坐标系
1. 屏幕坐标系:以屏幕的左上角为原点,如图所示 GetWindowRect() 函数获得的 RECT 就是以屏幕坐标系算的. 2. 非客户区坐标系(窗口坐标系) 包括标题栏的部分.GetWindow ...
【机器人】关键问题：动捕原点坐标系到机械臂基坐标系的转换
前言:在动作捕捉系统标定与机械臂各坐标系的说明这篇文章中已经对动捕系统原点坐标系和机械臂基坐标系两个坐标系的转换有过一些尝试,但仍到现在,这个坐标系的转换问题仍未解决. 11月4日,在今天下午的实 ...

orb-slam3：优化状态量是camera坐标系下RPV还是body坐标系下RPV探究

orb-slam3：优化状态量是camera坐标系下RPV还是body坐标系下RPV探究相关推荐

最新文章

热门文章