UZ-SLAMLab
How to set initial pose in ORB-SLAM3 Mono
Does anyone have experience with setting the pose of the initial keyframe?
I'm attempting to translate the entire map using a translation vector. I've incorporated this translation during the MonocularMapInitialization process and have also translated the initial MapPoints. However, after the translation, the Tracking process fails. I suspect that the TrackReferenceKeyFrame() function, which estimates the pose of the third frame, might be encountering inaccuracies in the estimated pose, leading to the "Fail to track" error.
This is how I modified the initialization:
void Tracking::MonocularInitialization()
{
if(!mbReadyToInitializate)
{
// Set Reference Frame
if(mCurrentFrame.mvKeys.size()>100)
{
mInitialFrame = Frame(mCurrentFrame);
mLastFrame = Frame(mCurrentFrame);
mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
if (mSensor == System::IMU_MONOCULAR)
{
if(mpImuPreintegratedFromLastKF)
{
delete mpImuPreintegratedFromLastKF;
}
mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib);
mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
}
mbReadyToInitializate = true;
return;
}
}
else
{
if (((int)mCurrentFrame.mvKeys.size()<=100)||((mSensor == System::IMU_MONOCULAR)&&(mLastFrame.mTimeStamp-mInitialFrame.mTimeStamp>1.0)))
{
mbReadyToInitializate = false;
return;
}
// Find correspondences
ORBmatcher matcher(0.9,true);
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
// Check if there are enough correspondences
if(nmatches<100)
{
mbReadyToInitializate = false;
return;
}
Sophus::SE3f Tcw;
vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
if(mpCamera->ReconstructWithTwoViews(mInitialFrame.mvKeysUn,mCurrentFrame.mvKeysUn,mvIniMatches,Tcw,mvIniP3D,vbTriangulated))
{
for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
{
if(mvIniMatches[i]>=0 && !vbTriangulated[i])
{
mvIniMatches[i]=-1;
nmatches--;
}
}
// Set Frame Poses
//mInitialFrame.SetPose(Sophus::SE3f());
mCurrentFrame.SetPose(Tcw);
// Define the desired translation vector
Eigen::Vector3f translationVector(0.0, 0.0, 210.0); // Adjust the values as needed
// Apply the translation to the origin
mInitialFrame.SetPose(Sophus::SE3f(Eigen::Quaternionf::Identity(), translationVector));
// Translate map points
for (size_t i = 0; i < mvIniP3D.size(); ++i) {
mvIniP3D[i].x += translationVector.x(); // Translate x-coordinate
mvIniP3D[i].y += translationVector.y(); // Translate y-coordinate
mvIniP3D[i].z += translationVector.z(); // Translate z-coordinate
}
Sophus::SE3f currentFramePose = Tcw * Sophus::SE3f(Eigen::Quaternionf::Identity(), translationVector);
// Set the pose of the current frame
mCurrentFrame.SetPose(currentFramePose);
CreateInitialMapMonocular();
}
}
}
void Tracking::CreateInitialMapMonocular()
{
// Create KeyFrames
KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB);
KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB);
if(mSensor == System::IMU_MONOCULAR)
pKFini->mpImuPreintegrated = (IMU::Preintegrated*)(NULL);
pKFini->ComputeBoW();
pKFcur->ComputeBoW();
// Insert KFs in the map
mpAtlas->AddKeyFrame(pKFini);
mpAtlas->AddKeyFrame(pKFcur);
for(size_t i=0; i<mvIniMatches.size();i++)
{
if(mvIniMatches[i]<0)
continue;
//Create MapPoint.
// Eigen::Vector3f worldPos;
// Define the 3D point worldPos
Eigen::Vector3f worldPos(mvIniP3D[i].x, mvIniP3D[i].y, mvIniP3D[i].z);
// Print the coordinates of the 3D point
std::cout << "Original point coordinates: " << mvIniP3D[i].x << ", " << mvIniP3D[i].y << ", " << mvIniP3D[i].z << std::endl;
// // Define the desired initial translation vector
Eigen::Vector3f initialTranslation(0.0, 0.0, 210.0);
// // Apply the translation directly to the point
worldPos += initialTranslation;
MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpAtlas->GetCurrentMap());
pKFini->AddMapPoint(pMP,i);
pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
pMP->AddObservation(pKFini,i);
pMP->AddObservation(pKFcur,mvIniMatches[i]);
pMP->ComputeDistinctiveDescriptors();
pMP->UpdateNormalAndDepth();
//Fill Current Frame structure
mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;
//Add to Map
mpAtlas->AddMapPoint(pMP);
`}`
If you only want to translate the output pose returned by TrackMonocular(), you could just replace the return statement with your code:
Eigen::Vector3f translationVector(0.0, 0.0, 210.0); // Adjust the values as needed
Sophus::SE3f currentFramePose = Tcw * Sophus::SE3f(Eigen::Quaternionf::Identity(), translationVector);
return currentFramePose;
If you actually want to shift everything in the map, you could try passing a translation vector into the Frame constructor used in TrackMonocular and set the pose at the top of the constructor. Changing the end of MonocularInitialization() to:
// Set the pose of the current frame
mCurrentFrame.SetPose(mInitialFrame.GetPose() * Tcw);
CreateInitialMapMonocular();
should set the second Frame correctly, and you won't have to manually change the MapPoints. Hope this helps.
I would like to connect the maps. So you second approach would be better, but unfortunately during initialization the Triangulation() function do not take care of the pose of the initial Frame. It estimate the relative Transformation and use it as the pose of the second Frame.
If you only want to translate the output pose returned by TrackMonocular(), you could just replace the return statement with your code:
Eigen::Vector3f translationVector(0.0, 0.0, 210.0); // Adjust the values as needed Sophus::SE3f currentFramePose = Tcw * Sophus::SE3f(Eigen::Quaternionf::Identity(), translationVector); return currentFramePose;If you actually want to shift everything in the map, you could try passing a translation vector into the Frame constructor used in TrackMonocular and set the pose at the top of the constructor. Changing the end of MonocularInitialization() to:
// Set the pose of the current frame mCurrentFrame.SetPose(mInitialFrame.GetPose() * Tcw); CreateInitialMapMonocular();should set the second Frame correctly, and you won't have to manually change the MapPoints. Hope this helps.
Hi thank you for your reply, I have tried second version to merge maps, but got the same when wrote as bellow
mInitialFrame.SetPose(Sophus::SE3f()); mCurrentFrame.SetPose(Tcw); CreateInitialMapMonocular();
please can you explain me this part, how to merge maps correctly, if I get recently relocalization
