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

1作用

帧或关键帧的光心求解。

2 所属位置及代码

2.1 Frame::SetPose()

（1）声明：include/Frame.h

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

（2）定义：src/Frame.cc

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

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

（2）定义：src/KeyFrame.cc

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;
}

3 原理解析