ORB_SLAM2原理及代码解析：SetPose() 函数

WWZZ20252025-10-09 14:06

1作用

帧或关键帧的光心求解。

2 所属位置及代码

2.1 Frame::SetPose()

（1）声明：include/Frame.h

cpp 复制代码

    // Set the camera pose.
    void SetPose(cv::Mat Tcw);

（2）定义：src/Frame.cc

cpp 复制代码

void Frame::SetPose(cv::Mat Tcw)
{
    mTcw = Tcw.clone();
    UpdatePoseMatrices();
}

void Frame::UpdatePoseMatrices()
{ 
    mRcw = mTcw.rowRange(0,3).colRange(0,3);
    mRwc = mRcw.t();
    mtcw = mTcw.rowRange(0,3).col(3);
    mOw = -mRcw.t()*mtcw;
}

2.2 KeyFrame::SetPose()

（1）声明：include/KeyFrame.h

cpp 复制代码

    // Pose functions
    void SetPose(const cv::Mat &Tcw);

（2）定义：src/KeyFrame.cc

cpp 复制代码

void KeyFrame::SetPose(const cv::Mat &Tcw_)
{
    unique_lock<mutex> lock(mMutexPose);
    Tcw_.copyTo(Tcw);
    cv::Mat Rcw = Tcw.rowRange(0,3).colRange(0,3);
    cv::Mat tcw = Tcw.rowRange(0,3).col(3);
    cv::Mat Rwc = Rcw.t();
    Ow = -Rwc*tcw;

    Twc = cv::Mat::eye(4,4,Tcw.type());
    Rwc.copyTo(Twc.rowRange(0,3).colRange(0,3));
    Ow.copyTo(Twc.rowRange(0,3).col(3));
    cv::Mat center = (cv::Mat_<float>(4,1) << mHalfBaseline, 0 , 0, 1);
    Cw = Twc*center;
}

ORB_SLAM2原理及代码解析：SetPose() 函数

1作用

2 所属位置及代码

2.1 Frame::SetPose()

2.2 KeyFrame::SetPose()

3 原理解析