更新标注
小白逆袭
parent
965140d945
commit
8dcefb5567
|
|
@ -67,7 +67,7 @@ Eigen::Matrix3d InverseRightJacobianSO3(const double x, const double y, const do
|
|||
|
||||
Eigen::Matrix3d NormalizeRotation(const Eigen::Matrix3d &R);
|
||||
|
||||
|
||||
// 相关节点中使用，存放的是imu与cam的内外参
|
||||
class ImuCamPose
|
||||
{
|
||||
public:
|
||||
|
|
@ -103,9 +103,10 @@ public:
|
|||
Eigen::Matrix3d Rwb0;
|
||||
Eigen::Matrix3d DR;
|
||||
|
||||
int its;
|
||||
int its; // 记录更新次数
|
||||
};
|
||||
|
||||
// 逆深度点，后面节点虽然有用到，但是声明的节点并没有使用到，暂时不看
|
||||
class InvDepthPoint
|
||||
{
|
||||
public:
|
||||
|
|
@ -124,6 +125,7 @@ public:
|
|||
};
|
||||
|
||||
// Optimizable parameters are IMU pose
|
||||
// 优化中关于位姿的节点，6自由度
|
||||
class VertexPose : public g2o::BaseVertex<6,ImuCamPose>
|
||||
{
|
||||
public:
|
||||
|
|
@ -140,15 +142,21 @@ public:
|
|||
virtual bool read(std::istream& is);
|
||||
virtual bool write(std::ostream& os) const;
|
||||
|
||||
// 重置函数,设定被优化变量的原始值
|
||||
virtual void setToOriginImpl() {
|
||||
}
|
||||
|
||||
// 更新
|
||||
virtual void oplusImpl(const double* update_){
|
||||
_estimate.Update(update_);
|
||||
// https://github.com/RainerKuemmerle/g2o/blob/master/doc/README_IF_IT_WAS_WORKING_AND_IT_DOES_NOT.txt
|
||||
// 官方讲解cache
|
||||
// 需要在oplusImpl与setEstimate函数中添加
|
||||
updateCache();
|
||||
}
|
||||
};
|
||||
|
||||
// 优化中关于位姿的节点，4自由度 3个平移加一个航偏
|
||||
class VertexPose4DoF : public g2o::BaseVertex<4,ImuCamPose>
|
||||
{
|
||||
// Translation and yaw are the only optimizable variables
|
||||
|
|
@ -172,6 +180,7 @@ public:
|
|||
virtual void setToOriginImpl() {
|
||||
}
|
||||
|
||||
// 强制让旋转的前两维的更新量为0
|
||||
virtual void oplusImpl(const double* update_){
|
||||
double update6DoF[6];
|
||||
update6DoF[0] = 0;
|
||||
|
|
@ -185,6 +194,9 @@ public:
|
|||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 速度节点
|
||||
*/
|
||||
class VertexVelocity : public g2o::BaseVertex<3,Eigen::Vector3d>
|
||||
{
|
||||
public:
|
||||
|
|
@ -206,6 +218,9 @@ public:
|
|||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 陀螺仪偏置节点
|
||||
*/
|
||||
class VertexGyroBias : public g2o::BaseVertex<3,Eigen::Vector3d>
|
||||
{
|
||||
public:
|
||||
|
|
@ -227,7 +242,9 @@ public:
|
|||
}
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief 加速度计偏置节点
|
||||
*/
|
||||
class VertexAccBias : public g2o::BaseVertex<3,Eigen::Vector3d>
|
||||
{
|
||||
public:
|
||||
|
|
@ -251,6 +268,7 @@ public:
|
|||
|
||||
|
||||
// Gravity direction vertex
|
||||
// 重力方向
|
||||
class GDirection
|
||||
{
|
||||
public:
|
||||
|
|
@ -268,6 +286,9 @@ public:
|
|||
int its;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 重力方向节点
|
||||
*/
|
||||
class VertexGDir : public g2o::BaseVertex<2,GDirection>
|
||||
{
|
||||
public:
|
||||
|
|
@ -290,6 +311,9 @@ public:
|
|||
};
|
||||
|
||||
// scale vertex
|
||||
/**
|
||||
* @brief 尺度节点
|
||||
*/
|
||||
class VertexScale : public g2o::BaseVertex<1,double>
|
||||
{
|
||||
public:
|
||||
|
|
@ -315,6 +339,9 @@ public:
|
|||
|
||||
|
||||
// Inverse depth point (just one parameter, inverse depth at the host frame)
|
||||
/**
|
||||
* @brief 没用
|
||||
*/
|
||||
class VertexInvDepth : public g2o::BaseVertex<1,InvDepthPoint>
|
||||
{
|
||||
public:
|
||||
|
|
@ -337,12 +364,13 @@ public:
|
|||
};
|
||||
|
||||
/**
|
||||
* @brief 视觉重投影的边
|
||||
* @brief 单目视觉重投影的边
|
||||
* 这里或许会有疑问，OptimizableTypes.h 里面不是定义了视觉重投影的边么？
|
||||
* 原因是这样，那里面定义的边也用，不过只是在纯视觉时，没有imu情况下，因为用已经定义好的节点就好
|
||||
* 但是加入imu时，优化要有残差的边与重投影的边同时存在，且两个边可能连接同一个位姿节点，所以需要重新弄一个包含imu位姿的节点
|
||||
* 因此，边也需要重新写，并且在imu优化时使用这个边
|
||||
*/
|
||||
// 误差为2维， 类型为Eigen::Vector2d， 节点1类型为g2o::VertexSBAPointXYZ，节点二类型为VertexPose
|
||||
class EdgeMono : public g2o::BaseBinaryEdge<2,Eigen::Vector2d,g2o::VertexSBAPointXYZ,VertexPose>
|
||||
{
|
||||
public:
|
||||
|
|
@ -354,11 +382,12 @@ public:
|
|||
virtual bool read(std::istream& is){return false;}
|
||||
virtual bool write(std::ostream& os) const{return false;}
|
||||
|
||||
// 计算重投影误差
|
||||
void computeError(){
|
||||
const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
|
||||
const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
|
||||
const Eigen::Vector2d obs(_measurement);
|
||||
_error = obs - VPose->estimate().Project(VPoint->estimate(),cam_idx);
|
||||
_error = obs - VPose->estimate().Project(VPoint->estimate(),cam_idx); // 投影
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -379,6 +408,7 @@ public:
|
|||
return J;
|
||||
}
|
||||
|
||||
// 由2*2的像素点信息矩阵变成了9*9的关于旋转平移与三维点坐标的信息矩阵
|
||||
Eigen::Matrix<double,9,9> GetHessian(){
|
||||
linearizeOplus();
|
||||
Eigen::Matrix<double,2,9> J;
|
||||
|
|
@ -391,6 +421,9 @@ public:
|
|||
const int cam_idx;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 单目纯位姿一元边
|
||||
*/
|
||||
class EdgeMonoOnlyPose : public g2o::BaseUnaryEdge<2,Eigen::Vector2d,VertexPose>
|
||||
{
|
||||
public:
|
||||
|
|
@ -426,11 +459,15 @@ public:
|
|||
const int cam_idx;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 双目位姿三维点二元边
|
||||
*/
|
||||
class EdgeStereo : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,g2o::VertexSBAPointXYZ,VertexPose>
|
||||
{
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
// 代码中都是0
|
||||
EdgeStereo(int cam_idx_=0): cam_idx(cam_idx_){}
|
||||
|
||||
virtual bool read(std::istream& is){return false;}
|
||||
|
|
@ -466,7 +503,9 @@ public:
|
|||
const int cam_idx;
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief 双目纯位姿一元边
|
||||
*/
|
||||
class EdgeStereoOnlyPose : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexPose>
|
||||
{
|
||||
public:
|
||||
|
|
@ -497,6 +536,9 @@ public:
|
|||
};
|
||||
|
||||
// TO COMMENT
|
||||
/**
|
||||
* @brief 惯性边（误差为残差）
|
||||
*/
|
||||
class EdgeInertial : public g2o::BaseMultiEdge<9,Vector9d>
|
||||
{
|
||||
public:
|
||||
|
|
@ -510,6 +552,7 @@ public:
|
|||
void computeError();
|
||||
virtual void linearizeOplus();
|
||||
|
||||
// 关于pose1与2 的旋转平移速度，以及之间的偏置的信息矩阵
|
||||
Eigen::Matrix<double,24,24> GetHessian(){
|
||||
linearizeOplus();
|
||||
Eigen::Matrix<double,9,24> J;
|
||||
|
|
@ -522,6 +565,7 @@ public:
|
|||
return J.transpose()*information()*J;
|
||||
}
|
||||
|
||||
// 没用
|
||||
Eigen::Matrix<double,18,18> GetHessianNoPose1(){
|
||||
linearizeOplus();
|
||||
Eigen::Matrix<double,9,18> J;
|
||||
|
|
@ -533,6 +577,7 @@ public:
|
|||
return J.transpose()*information()*J;
|
||||
}
|
||||
|
||||
// 关于pose2 的旋转平移信息矩阵
|
||||
Eigen::Matrix<double,9,9> GetHessian2(){
|
||||
linearizeOplus();
|
||||
Eigen::Matrix<double,9,9> J;
|
||||
|
|
@ -541,14 +586,18 @@ public:
|
|||
return J.transpose()*information()*J;
|
||||
}
|
||||
|
||||
// 预积分中对应的状态对偏置的雅可比
|
||||
const Eigen::Matrix3d JRg, JVg, JPg;
|
||||
const Eigen::Matrix3d JVa, JPa;
|
||||
IMU::Preintegrated* mpInt;
|
||||
const double dt;
|
||||
Eigen::Vector3d g;
|
||||
|
||||
IMU::Preintegrated* mpInt; // 预积分
|
||||
const double dt; // 预积分时间
|
||||
Eigen::Vector3d g; // 0, 0, -IMU::GRAVITY_VALUE
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief 初始化惯性边（误差为残差）
|
||||
*/
|
||||
// Edge inertial whre gravity is included as optimizable variable and it is not supposed to be pointing in -z axis, as well as scale
|
||||
class EdgeInertialGS : public g2o::BaseMultiEdge<9,Vector9d>
|
||||
{
|
||||
|
|
@ -570,6 +619,7 @@ public:
|
|||
const double dt;
|
||||
Eigen::Vector3d g, gI;
|
||||
|
||||
// 关于pose1与2 的旋转平移速度，以及之间的偏置，还有重力方向与尺度的信息矩阵
|
||||
Eigen::Matrix<double,27,27> GetHessian(){
|
||||
linearizeOplus();
|
||||
Eigen::Matrix<double,9,27> J;
|
||||
|
|
@ -584,6 +634,7 @@ public:
|
|||
return J.transpose()*information()*J;
|
||||
}
|
||||
|
||||
// 与上面摆放不同
|
||||
Eigen::Matrix<double,27,27> GetHessian2(){
|
||||
linearizeOplus();
|
||||
Eigen::Matrix<double,9,27> J;
|
||||
|
|
@ -598,6 +649,7 @@ public:
|
|||
return J.transpose()*information()*J;
|
||||
}
|
||||
|
||||
// 关于偏置，重力方向与尺度的信息矩阵
|
||||
Eigen::Matrix<double,9,9> GetHessian3(){
|
||||
linearizeOplus();
|
||||
Eigen::Matrix<double,9,9> J;
|
||||
|
|
@ -609,7 +661,7 @@ public:
|
|||
}
|
||||
|
||||
|
||||
|
||||
// 下面的没有用到，其实也是获取状态的信息矩阵
|
||||
Eigen::Matrix<double,1,1> GetHessianScale(){
|
||||
linearizeOplus();
|
||||
Eigen::Matrix<double,9,1> J = _jacobianOplus[7];
|
||||
|
|
@ -636,7 +688,9 @@ public:
|
|||
};
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 陀螺仪偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化
|
||||
*/
|
||||
class EdgeGyroRW : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,VertexGyroBias,VertexGyroBias>
|
||||
{
|
||||
public:
|
||||
|
|
@ -672,7 +726,9 @@ public:
|
|||
}
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief 加速度计偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化
|
||||
*/
|
||||
class EdgeAccRW : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,VertexAccBias,VertexAccBias>
|
||||
{
|
||||
public:
|
||||
|
|
@ -708,6 +764,9 @@ public:
|
|||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 先验类
|
||||
*/
|
||||
class ConstraintPoseImu
|
||||
{
|
||||
public:
|
||||
|
|
@ -717,7 +776,8 @@ public:
|
|||
const Eigen::Vector3d &bg_, const Eigen::Vector3d &ba_, const Matrix15d &H_):
|
||||
Rwb(Rwb_), twb(twb_), vwb(vwb_), bg(bg_), ba(ba_), H(H_)
|
||||
{
|
||||
H = (H+H)/2;
|
||||
H = (H+H)/2; // 对称化
|
||||
// 令特征值小的位置变为0
|
||||
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,15,15> > es(H);
|
||||
Eigen::Matrix<double,15,1> eigs = es.eigenvalues();
|
||||
for(int i=0;i<15;i++)
|
||||
|
|
@ -725,6 +785,8 @@ public:
|
|||
eigs[i]=0;
|
||||
H = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose();
|
||||
}
|
||||
|
||||
// 形式不同
|
||||
ConstraintPoseImu(const cv::Mat &Rwb_, const cv::Mat &twb_, const cv::Mat &vwb_,
|
||||
const IMU::Bias &b, const cv::Mat &H_)
|
||||
{
|
||||
|
|
@ -753,6 +815,9 @@ public:
|
|||
Matrix15d H;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 先验边，前端优化单帧用到
|
||||
*/
|
||||
class EdgePriorPoseImu : public g2o::BaseMultiEdge<15,Vector15d>
|
||||
{
|
||||
public:
|
||||
|
|
@ -775,6 +840,7 @@ public:
|
|||
return J.transpose()*information()*J;
|
||||
}
|
||||
|
||||
// 没用到，除了旋转平移外的信息矩阵
|
||||
Eigen::Matrix<double,9,9> GetHessianNoPose(){
|
||||
linearizeOplus();
|
||||
Eigen::Matrix<double,15,9> J;
|
||||
|
|
@ -789,6 +855,9 @@ public:
|
|||
};
|
||||
|
||||
// Priors for biases
|
||||
/**
|
||||
* @brief 根据给定值的加速度计先验边，目的是把优化量维持在先验附近
|
||||
*/
|
||||
class EdgePriorAcc : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexAccBias>
|
||||
{
|
||||
public:
|
||||
|
|
@ -813,6 +882,9 @@ public:
|
|||
const Eigen::Vector3d bprior;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 根据给定值的陀螺仪先验边，目的是把优化量维持在先验附近
|
||||
*/
|
||||
class EdgePriorGyro : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexGyroBias>
|
||||
{
|
||||
public:
|
||||
|
|
@ -837,7 +909,9 @@ public:
|
|||
const Eigen::Vector3d bprior;
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief 4DOF的二元边，误差为给定的旋转平移改变量 与 两个输入节点之间的旋转平移改变量的偏差
|
||||
*/
|
||||
class Edge4DoF : public g2o::BaseBinaryEdge<6,Vector6d,VertexPose4DoF,VertexPose4DoF>
|
||||
{
|
||||
public:
|
||||
|
|
|
|||
|
|
@ -167,6 +167,9 @@ void ImuCamPose::SetParam(const std::vector<Eigen::Matrix3d> &_Rcw, const std::v
|
|||
bf = _bf;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 单目投影
|
||||
*/
|
||||
Eigen::Vector2d ImuCamPose::Project(const Eigen::Vector3d &Xw, int cam_idx) const
|
||||
{
|
||||
Eigen::Vector3d Xc = Rcw[cam_idx]*Xw+tcw[cam_idx];
|
||||
|
|
@ -174,6 +177,10 @@ Eigen::Vector2d ImuCamPose::Project(const Eigen::Vector3d &Xw, int cam_idx) cons
|
|||
return pCamera[cam_idx]->project(Xc);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 双目投影，都是0
|
||||
* @return u v u`
|
||||
*/
|
||||
Eigen::Vector3d ImuCamPose::ProjectStereo(const Eigen::Vector3d &Xw, int cam_idx) const
|
||||
{
|
||||
Eigen::Vector3d Pc = Rcw[cam_idx]*Xw+tcw[cam_idx];
|
||||
|
|
@ -219,6 +226,7 @@ void ImuCamPose::Update(const double *pu)
|
|||
|
||||
}
|
||||
|
||||
// 更新世界坐标系
|
||||
void ImuCamPose::UpdateW(const double *pu)
|
||||
{
|
||||
Eigen::Vector3d ur, ut;
|
||||
|
|
@ -606,7 +614,7 @@ void EdgeInertial::linearizeOplus()
|
|||
_jacobianOplus[3].block<3,3>(6,0) = -JPa; // OK
|
||||
|
||||
// Jacobians wrt Pose 2
|
||||
// _jacobianOplus[3] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导
|
||||
// _jacobianOplus[4] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导
|
||||
_jacobianOplus[4].setZero();
|
||||
// rotation
|
||||
_jacobianOplus[4].block<3,3>(0,0) = invJr; // OK
|
||||
|
|
@ -614,7 +622,7 @@ void EdgeInertial::linearizeOplus()
|
|||
_jacobianOplus[4].block<3,3>(6,3) = Rbw1*Rwb2; // OK
|
||||
|
||||
// Jacobians wrt Velocity 2
|
||||
// _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导
|
||||
// _jacobianOplus[5] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导
|
||||
_jacobianOplus[5].setZero();
|
||||
_jacobianOplus[5].block<3,3>(3,0) = Rbw1; // OK
|
||||
}
|
||||
|
|
@ -930,6 +938,6 @@ Eigen::Matrix3d NormalizeRotation(const Eigen::Matrix3d &R)
|
|||
// 2. 对于行列数不同的矩阵，例如3*4 或者 4*3 矩阵只有3个奇异向量，计算的时候如果是Thin 那么得出的UV矩阵列数只能是3，如果是full那么就是4
|
||||
// 3. thin会损失一部分数据，但是会加快计算，对于大型矩阵解算方程时，可以用thin加速得到结果
|
||||
Eigen::JacobiSVD<Eigen::Matrix3d> svd(R,Eigen::ComputeFullU | Eigen::ComputeFullV);
|
||||
return svd.matrixU()*svd.matrixV();
|
||||
return svd.matrixU()*svd.matrixV().transpose();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1600,7 +1600,8 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
|
|||
mFirstTs=vpKF.front()->mTimeStamp;
|
||||
if(mpCurrentKeyFrame->mTimeStamp-mFirstTs<minTime)
|
||||
return;
|
||||
|
||||
|
||||
// 正在做IMU的初始化，在tracking里面使用，如果为true，暂不添加关键帧
|
||||
bInitializing = true;
|
||||
|
||||
// 先处理新关键帧，防止堆积且保证数据量充足
|
||||
|
|
@ -1626,20 +1627,21 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
|
|||
continue;
|
||||
if (!(*itKF)->mPrevKF)
|
||||
continue;
|
||||
// Rwb（imu坐标转到初始化前世界坐标系下的坐标）*更新偏置后的速度，可以理解为在世界坐标系下的速度矢量
|
||||
// 初始化时关于速度的预积分定义Ri.t()*(s*Vj - s*Vi - Rwg*g*tij)
|
||||
dirG -= (*itKF)->mPrevKF->GetImuRotation()*(*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity();
|
||||
// 求取实际的速度，位移/时间
|
||||
cv::Mat _vel = ((*itKF)->GetImuPosition() - (*itKF)->mPrevKF->GetImuPosition())/(*itKF)->mpImuPreintegrated->dT;
|
||||
(*itKF)->SetVelocity(_vel);
|
||||
(*itKF)->mPrevKF->SetVelocity(_vel);
|
||||
}
|
||||
// dirG = sV1 - sVn + n*Rwg*g*t
|
||||
// 归一化
|
||||
dirG = dirG/cv::norm(dirG);
|
||||
// 原本的重力方向
|
||||
cv::Mat gI = (cv::Mat_<float>(3,1) << 0.0f, 0.0f, -1.0f);
|
||||
// 求速度方向与重力方向的角轴
|
||||
cv::Mat v = gI.cross(dirG);
|
||||
// 求角轴长度
|
||||
// 求角轴模长
|
||||
const float nv = cv::norm(v);
|
||||
// 求转角大小
|
||||
const float cosg = gI.dot(dirG);
|
||||
|
|
@ -1664,7 +1666,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
|
|||
mInitTime = mpTracker->mLastFrame.mTimeStamp-vpKF.front()->mTimeStamp;
|
||||
|
||||
std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
|
||||
// 计算残差及偏置差，优化尺度重力方向及Ri Rj Vi Vj Pi Pj
|
||||
// 计算残差及偏置差，优化尺度重力方向及速度偏置，偏置先验为0，双目时不优化尺度
|
||||
Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale, mbg, mba, mbMonocular, infoInertial, false, false, priorG, priorA);
|
||||
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
|
||||
|
||||
|
|
@ -1736,10 +1738,11 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
|
|||
}
|
||||
|
||||
|
||||
mbNewInit=true;
|
||||
mnKFs=vpKF.size();
|
||||
mIdxInit++;
|
||||
mbNewInit=true; // 没用到
|
||||
mnKFs=vpKF.size(); // 没用到
|
||||
mIdxInit++; // 没用到
|
||||
|
||||
// 里面存放的是来到Local线程，但还未处理的关键帧
|
||||
for(list<KeyFrame*>::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++)
|
||||
{
|
||||
(*lit)->SetBadFlag();
|
||||
|
|
|
|||
30
src/Map.cc
30
src/Map.cc
|
|
@ -298,6 +298,14 @@ void Map::RotateMap(const cv::Mat &R)
|
|||
}
|
||||
}
|
||||
|
||||
// 恢复尺度及重力方向
|
||||
/** imu在localmapping中初始化，LocalMapping::InitializeIMU中使用，误差包含三个残差与两个偏置
|
||||
* 地图融合时也会使用
|
||||
* @param R 初始化时为Rgw
|
||||
* @param s 尺度
|
||||
* @param bScaledVel 将尺度更新到速度
|
||||
* @param t 默认cv::Mat::zeros(cv::Size(1,3),CV_32F)
|
||||
*/
|
||||
void Map::ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScaledVel, const cv::Mat t)
|
||||
{
|
||||
unique_lock<mutex> lock(mMutexMap);
|
||||
|
|
@ -310,19 +318,36 @@ void Map::ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScale
|
|||
|
||||
cv::Mat Tyw = Tyx*Txw;
|
||||
Tyw.rowRange(0,3).col(3) = Tyw.rowRange(0,3).col(3)+t;
|
||||
cv::Mat Ryw = Tyw.rowRange(0,3).colRange(0,3);
|
||||
cv::Mat tyw = Tyw.rowRange(0,3).col(3);
|
||||
// Tyw 中旋转部分等于R，平移部分等于t
|
||||
// 做了很多操作，到最后还是得出了一个R跟t？感觉上面的操作像是预留了一些东西，比如在世界坐标系与第一帧有一定Rt时，只不过暂时没有用到
|
||||
cv::Mat Ryw = Tyw.rowRange(0,3).colRange(0,3); // R
|
||||
cv::Mat tyw = Tyw.rowRange(0,3).col(3); // t
|
||||
|
||||
for(set<KeyFrame*>::iterator sit=mspKeyFrames.begin(); sit!=mspKeyFrames.end(); sit++)
|
||||
{
|
||||
// 更新关键帧位姿
|
||||
/**
|
||||
* | Rw2w1 tw2w1 | * | Rw1c s*tw1c | = | Rw2c s*Rw2w1*tw1c + tw2w1 |
|
||||
* | 0 1 | | 0 1 | | 0 1 |
|
||||
* 这么做比正常乘在旋转上少了个s，后面不需要这个s了，因为所有mp在下面已经全部转到了w2坐标系下，不存在尺度变化了
|
||||
*
|
||||
* | s*Rw2w1 tw2w1 | * | Rw1c tw1c | = | s*Rw2c s*Rw2w1*tw1c + tw2w1 |
|
||||
* | 0 1 | | 0 1 | | 0 1 |
|
||||
*/
|
||||
|
||||
KeyFrame* pKF = *sit;
|
||||
cv::Mat Twc = pKF->GetPoseInverse();
|
||||
Twc.rowRange(0,3).col(3)*=s;
|
||||
|
||||
// | Ryc s*Ryw*twc + tyw |
|
||||
// | 0 1 |
|
||||
cv::Mat Tyc = Tyw*Twc;
|
||||
|
||||
cv::Mat Tcy = cv::Mat::eye(4,4,CV_32F);
|
||||
Tcy.rowRange(0,3).colRange(0,3) = Tyc.rowRange(0,3).colRange(0,3).t();
|
||||
Tcy.rowRange(0,3).col(3) = -Tcy.rowRange(0,3).colRange(0,3)*Tyc.rowRange(0,3).col(3);
|
||||
pKF->SetPose(Tcy);
|
||||
// 更新关键帧速度
|
||||
cv::Mat Vw = pKF->GetVelocity();
|
||||
if(!bScaledVel)
|
||||
pKF->SetVelocity(Ryw*Vw);
|
||||
|
|
@ -332,6 +357,7 @@ void Map::ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScale
|
|||
}
|
||||
for(set<MapPoint*>::iterator sit=mspMapPoints.begin(); sit!=mspMapPoints.end(); sit++)
|
||||
{
|
||||
// 更新每一个mp在世界坐标系下的坐标
|
||||
MapPoint* pMP = *sit;
|
||||
pMP->SetWorldPos(s*Ryw*pMP->GetWorldPos()+tyw);
|
||||
pMP->UpdateNormalAndDepth();
|
||||
|
|
|
|||
|
|
@ -5069,7 +5069,8 @@ Eigen::MatrixXd Optimizer::Sparsify(const Eigen::MatrixXd &H, const int &start1,
|
|||
}
|
||||
|
||||
|
||||
void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba, bool bMono, Eigen::MatrixXd &covInertial, bool bFixedVel, bool bGauss, float priorG, float priorA)
|
||||
void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba,
|
||||
bool bMono, Eigen::MatrixXd &covInertial, bool bFixedVel, bool bGauss, float priorG, float priorA)
|
||||
{
|
||||
Verbose::PrintMess("inertial optimization", Verbose::VERBOSITY_NORMAL);
|
||||
int its = 200; // Check number of iterations
|
||||
|
|
|
|||
|
|
@ -4582,12 +4582,25 @@ void Tracking::InformOnlyTracking(const bool &flag)
|
|||
mbOnlyTracking = flag;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 更新了关键帧的位姿，但需要修改普通帧的位姿，因为正常跟踪需要普通帧
|
||||
* localmapping中初始化imu中使用，速度的走向（仅在imu模式使用），最开始速度定义于imu初始化时，每个关键帧都根据位移除以时间得到，经过非线性优化保存于KF中.
|
||||
* 之后使用本函数，让上一帧与当前帧分别与他们对应的上一关键帧做速度叠加得到，后面新的frame速度由上一个帧速度决定，如果使用匀速模型（大多数情况下），通过imu积分更新速度。
|
||||
* 新的关键帧继承于对应帧
|
||||
* @param s 尺度
|
||||
* @param b 初始化后第一帧的偏置
|
||||
* @param pCurrentKeyFrame 当前关键帧
|
||||
*/
|
||||
void Tracking::UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurrentKeyFrame)
|
||||
{
|
||||
Map * pMap = pCurrentKeyFrame->GetMap();
|
||||
unsigned int index = mnFirstFrameId;
|
||||
unsigned int index = mnFirstFrameId; // unused
|
||||
|
||||
// 每一帧的参考关键帧
|
||||
list<ORB_SLAM3::KeyFrame*>::iterator lRit = mlpReferences.begin();
|
||||
list<bool>::iterator lbL = mlbLost.begin();
|
||||
list<bool>::iterator lbL = mlbLost.begin(); // 对应帧是否跟踪丢失
|
||||
// mlRelativeFramePoses 存放的是Tcr
|
||||
// 三个变量一一对应
|
||||
for(list<cv::Mat>::iterator lit=mlRelativeFramePoses.begin(),lend=mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lbL++)
|
||||
{
|
||||
if(*lbL)
|
||||
|
|
@ -4605,11 +4618,11 @@ void Tracking::UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurr
|
|||
(*lit).rowRange(0,3).col(3)=(*lit).rowRange(0,3).col(3)*s;
|
||||
}
|
||||
}
|
||||
|
||||
// 设置偏置
|
||||
mLastBias = b;
|
||||
|
||||
// 设置上一关键帧，如果说mpLastKeyFrame已经是经过添加的新的kf，而pCurrentKeyFrame还是上一个kf，mpLastKeyFrame直接指向之前的kf
|
||||
mpLastKeyFrame = pCurrentKeyFrame;
|
||||
|
||||
// 更新偏置
|
||||
mLastFrame.SetNewBias(mLastBias);
|
||||
mCurrentFrame.SetNewBias(mLastBias);
|
||||
|
||||
|
|
@ -4622,10 +4635,12 @@ void Tracking::UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurr
|
|||
|
||||
while(!mCurrentFrame.imuIsPreintegrated())
|
||||
{
|
||||
// 当前帧需要预积分完毕，这段函数实在localmapping里调用的
|
||||
usleep(500);
|
||||
}
|
||||
|
||||
|
||||
// frame的mpLastKeyFrame只是用于预积分（imu模式）
|
||||
// TODO 如果上一帧正好是上一帧的上一关键帧（mLastFrame.mpLastKeyFrame与mLastFrame不可能是一个，可以验证一下）
|
||||
if(mLastFrame.mnId == mLastFrame.mpLastKeyFrame->mnFrameId)
|
||||
{
|
||||
mLastFrame.SetImuPoseVelocity(mLastFrame.mpLastKeyFrame->GetImuRotation(),
|
||||
|
|
@ -4638,12 +4653,13 @@ void Tracking::UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurr
|
|||
Rwb1 = mLastFrame.mpLastKeyFrame->GetImuRotation();
|
||||
Vwb1 = mLastFrame.mpLastKeyFrame->GetVelocity();
|
||||
t12 = mLastFrame.mpImuPreintegrated->dT;
|
||||
|
||||
// 根据mLastFrame的上一个关键帧的信息（此时已经经过imu初始化了，所以关键帧的信息都是校正后的）以及imu的预积分重新计算上一帧的位姿
|
||||
mLastFrame.SetImuPoseVelocity(Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaRotation(),
|
||||
twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaPosition(),
|
||||
Vwb1 + Gz*t12 + Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaVelocity());
|
||||
}
|
||||
|
||||
// 当前帧是否做了预积分
|
||||
if (mCurrentFrame.mpImuPreintegrated)
|
||||
{
|
||||
twb1 = mCurrentFrame.mpLastKeyFrame->GetImuPosition();
|
||||
|
|
|
|||
Loading…
Reference in New Issue