Skip to content

Commit

Permalink
Fix typos
Browse files Browse the repository at this point in the history
  • Loading branch information
ccamposm committed Aug 7, 2020
1 parent 159bb55 commit b4ff9c6
Show file tree
Hide file tree
Showing 5 changed files with 9 additions and 178 deletions.
3 changes: 1 addition & 2 deletions Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -219,9 +219,8 @@ void ImageGrabber::SyncWithImu()

if((tImLeft-tImRight)>maxTimeDiff || (tImRight-tImLeft)>maxTimeDiff)
{
std::cout << "big time difference" << std::endl;
// std::cout << "big time difference" << std::endl;
continue;
// std::abort();
}
if(tImLeft>mpImuGb->imuBuf.back()->header.stamp.toSec())
continue;
Expand Down
8 changes: 4 additions & 4 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,10 +174,10 @@ For a stereo input from topic `/camera/left/image_raw` and `/camera/right/image_
```

### Running Stereo-Inertial Node
For a stereo input from topics `/camera/left/image_raw` and `/camera/right/image_raw`, and an inertial input from topic `/imu`, run node ORB_SLAM3/Stereo. You will need to provide the vocabulary file and a settings file, including rectification matrices if required in a similar way to Stereo case:
For a stereo input from topics `/camera/left/image_raw` and `/camera/right/image_raw`, and an inertial input from topic `/imu`, run node ORB_SLAM3/Stereo_Inertial. You will need to provide the vocabulary file and a settings file, including rectification matrices if required in a similar way to Stereo case:

```
rosrun ORB_SLAM3 Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION [EQUALIZATION]
rosrun ORB_SLAM3 Stereo_Inertial PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION [EQUALIZATION]
```

### Running RGB_D Node
Expand All @@ -187,7 +187,7 @@ For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_regist
rosrun ORB_SLAM3 RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
```

**Running ROS example:** Download a rosbag (e.g. V1_02_medium.bag) from the EuRoC dataset (http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets). Open 3 tabs on the terminal and run the following command at each tab:
**Running ROS example:** Download a rosbag (e.g. V1_02_medium.bag) from the EuRoC dataset (http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets). Open 3 tabs on the terminal and run the following command at each tab for a Stereo-Inertial configuration:
```
roscore
```
Expand All @@ -197,7 +197,7 @@ For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_regist
```

```
rosbag play --pause V1_01_easy.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw /imu0:=/imu
rosbag play --pause V1_02_medium.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw /imu0:=/imu
```

Once ORB-SLAM3 has loaded the vocabulary, press space in the rosbag tab.
Expand Down
2 changes: 0 additions & 2 deletions src/Frame.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,7 +118,6 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt


N = mvKeys.size();
std::cout << "N = " << N << std::endl;
if(mvKeys.empty())
return;

Expand Down Expand Up @@ -336,7 +335,6 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extra
invfy = 1.0f/fy;

mbInitialComputations=false;
std::cout << mnMaxX << ", " << mnMinX << ", " << mnMaxY << ", " << mnMinY << "\n";
}


Expand Down
169 changes: 4 additions & 165 deletions src/Optimizer.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1678,11 +1678,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, vector<Ke
vpNonEnoughOptKFs.push_back(pKFi);
}
}
//Verbose::PrintMess("LM-LBA: Num fixed cameras " + to_string(num_fixedKF), Verbose::VERBOSITY_DEBUG);
//Verbose::PrintMess("LM-LBA: Num Points " + to_string(lLocalMapPoints.size()), Verbose::VERBOSITY_DEBUG);
//Verbose::PrintMess("LM-LBA: Num optimized cameras " + to_string(lLocalKeyFrames.size()), Verbose::VERBOSITY_DEBUG);
//Verbose::PrintMess("----------", Verbose::VERBOSITY_DEBUG);


//Points
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
Expand Down Expand Up @@ -1733,29 +1728,12 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap
if(pMP)
if(!pMP->isBad() && pMP->GetMap() == pCurrentMap)
{
/*if(sNumObsMP.find(pMP) == sNumObsMP.end())

if(pMP->mnBALocalForKF!=pKF->mnId)
{
sNumObsMP.insert(pMP);
int index_mp = pMP->GetIndexInKeyFrame(pKFi);
if(pKFi->mvuRight[index_mp]>=0)
{
// Stereo, it has at least 2 observations by pKFi
if(pMP->mnBALocalForKF!=pKF->mnId)
{
lLocalMapPoints.push_back(pMP);
pMP->mnBALocalForKF=pKF->mnId;
}
}
lLocalMapPoints.push_back(pMP);
pMP->mnBALocalForKF=pKF->mnId;
}
else
{*/
if(pMP->mnBALocalForKF!=pKF->mnId)
{
lLocalMapPoints.push_back(pMP);
pMP->mnBALocalForKF=pKF->mnId;
}
//}

}
}
}
Expand Down Expand Up @@ -1872,10 +1850,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap
if(pKFi->mnId>maxKFid)
maxKFid=pKFi->mnId;
}
//Verbose::PrintMess("LM-LBA: Fixed KFs added", Verbose::VERBOSITY_DEBUG);

//Verbose::PrintMess("LM-LBA: opt/fixed KFs: " + to_string(lLocalKeyFrames.size()) + "/" + to_string(lFixedCameras.size()), Verbose::VERBOSITY_DEBUG);
//Verbose::PrintMess("LM-LBA: local MPs: " + to_string(lLocalMapPoints.size()), Verbose::VERBOSITY_DEBUG);

// Set MapPoint vertices
const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();
Expand Down Expand Up @@ -2182,21 +2156,8 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap
bRedrawError = true;
string folder_name = "test_LBA";
string name = "_PreLM_LBA";
//pMap->printReprojectionError(lLocalKeyFrames, pKF, name, folder_name);
name = "_PreLM_LBA_Fixed";
//pMap->printReprojectionError(lFixedCameras, pKF, name, folder_name);
/*for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
MapPoint* pMP = vpMapPointEdgeMono[i];
if(pMP->isBad())
continue;
Verbose::PrintMess("ERROR CHI2: " + to_string(e->chi2()) + "; DEPTH POSITIVE: " + to_string(e->isDepthPositive()), Verbose::VERBOSITY_NORMAL);
}*/

//return;
}


Expand Down Expand Up @@ -2315,11 +2276,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap
Verbose::PrintMess("LM-LBA: Bad observations in mono " + to_string(numBadMonoMP) + " and stereo " + to_string(numBadStereoMP), Verbose::VERBOSITY_DEBUG);
}
}
//Verbose::PrintMess("LM-LBA: Num fixed cameras " + to_string(num_fixedKF), Verbose::VERBOSITY_DEBUG);
//Verbose::PrintMess("LM-LBA: Num Points " + to_string(lLocalMapPoints.size()), Verbose::VERBOSITY_DEBUG);
//Verbose::PrintMess("LM-LBA: Num optimized cameras " + to_string(lLocalKeyFrames.size()), Verbose::VERBOSITY_DEBUG);
//Verbose::PrintMess("----------", Verbose::VERBOSITY_DEBUG);


//Points
for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
Expand Down Expand Up @@ -3932,8 +3888,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2,
const bool bFixScale, Eigen::Matrix<double,7,7> &mAcumHessian, const bool bAllPoints)
{
// bool bShowImages = false;

g2o::SparseOptimizer optimizer;
g2o::BlockSolverX::LinearSolverType * linearSolver;

Expand All @@ -3944,12 +3898,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);

// Calibration
/*const cv::Mat &K1 = pKF1->mK;
const cv::Mat &K2 = pKF2->mK;*/

//const cv::Mat &DistCoeff2 = pKF2->mDistCoef;

// Camera poses
const cv::Mat R1w = pKF1->GetRotation();
const cv::Mat t1w = pKF1->GetTranslation();
Expand Down Expand Up @@ -3979,16 +3927,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
vpEdges21.reserve(2*N);
vbIsInKF2.reserve(2*N);

/*float cx1 = K1.at<float>(0,2);
float cy1 = K1.at<float>(1,2);
float fx1 = K1.at<float>(0,0);
float fy1 = K1.at<float>(1,1);
float cx2 = K2.at<float>(0,2);
float cy2 = K2.at<float>(1,2);
float fx2 = K2.at<float>(0,0);
float fy2 = K2.at<float>(1,1);*/

const float deltaHuber = sqrt(th2);

int nCorrespondences = 0;
Expand All @@ -3997,11 +3935,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
int nOutKF2 = 0;
int nMatchWithoutMP = 0;

/*cv::Mat img1 = cv::imread(pKF1->mNameFile, CV_LOAD_IMAGE_UNCHANGED);
cv::cvtColor(img1, img1, CV_GRAY2BGR);
cv::Mat img2 = cv::imread(pKF2->mNameFile, CV_LOAD_IMAGE_UNCHANGED);
cv::cvtColor(img2, img2, CV_GRAY2BGR);*/

vector<int> vIdsOnlyInKF2;

for(int i=0; i<N; i++)
Expand Down Expand Up @@ -4066,11 +3999,7 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &

vIdsOnlyInKF2.push_back(id2);
}

// cv::circle(img1, pKF1->mvKeys[i].pt, 1, cv::Scalar(0, 0, 255));

continue;

}

if(i2<0 && !bAllPoints)
Expand All @@ -4093,42 +4022,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
obs1 << kpUn1.pt.x, kpUn1.pt.y;

ORB_SLAM3::EdgeSim3ProjectXYZ* e12 = new ORB_SLAM3::EdgeSim3ProjectXYZ();
/*bool inKF1;
if(pMP1)
{
const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];
obs1 << kpUn1.pt.x, kpUn1.pt.y;
inKF1 = true;
}
else
{
float invz = 1/P3D1c.at<float>(2);
float x = P3D1c.at<float>(0)*invz;
float y = P3D1c.at<float>(1)*invz;
obs1 << x, y;
kpUn1 = cv::KeyPoint(cv::Point2f(x, y), pMP1->mnTrackScaleLevel);
inKF1 = false;
}*/

/*if(bShowImages) //TODO test to project the matched points in the image
{
cv::circle(img1, pKF1->mvKeys[i].pt, 1, cv::Scalar(0, 255, 0));
Eigen::Matrix<double,3,1> eigP3D2c = Converter::toVector3d(P3D2c);
Eigen::Matrix<double,3,1> eigP3D1c = g2oS12.map(eigP3D2c);
cv::Mat cvP3D1c = Converter::toCvMat(eigP3D1c);
float invz = 1/cvP3D1c.at<float>(2);
float x = fx1 * cvP3D1c.at<float>(0)*invz + cx1;
float y = fy1 * cvP3D1c.at<float>(1)*invz + cy1;
cv::Point2f ptProjPoint(x, y);
cv::line(img1, pKF1->mvKeys[i].pt, ptProjPoint, cv::Scalar(255, 0, 0), 1);
}*/


e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));
e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
Expand All @@ -4152,64 +4045,18 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
inKF2 = true;

nInKF2++;

/*if(bShowImages)
{
cv::circle(img2, pKF2->mvKeys[i2].pt, 1, cv::Scalar(0, 255, 0));
}*/
}
else
{
float invz = 1/P3D2c.at<float>(2);
float x = P3D2c.at<float>(0)*invz;
float y = P3D2c.at<float>(1)*invz;

/*cv::Mat mat(1,2,CV_32F);
mat.at<float>(0,0) = x;
mat.at<float>(0,1) = y;
mat=mat.reshape(2);
cv::undistortPoints(mat,mat,K2,DistCoeff2,cv::Mat(),K2);
mat=mat.reshape(1);
x = mat.at<float>(0,0);
y = mat.at<float>(0,1);*/

obs2 << x, y;
kpUn2 = cv::KeyPoint(cv::Point2f(x, y), pMP2->mnTrackScaleLevel);

inKF2 = false;
nOutKF2++;

/*if(bShowImages)
{
cv::circle(img2, kpUn2.pt, 1, cv::Scalar(0, 0, 255));
}*/

//TODO print projection, because all of them become in outliers



// Project in image and check it is not outside
//float u = pKF2->fx * x + pKF2->cx;
//float v = pKF2->fy * y + pKF2->cy;
//obs2 << u, v;
//kpUn2 = cv::KeyPoint(cv::Point2f(u, v), pMP2->mnTrackScaleLevel);
}

{
Eigen::Matrix<double,3,1> eigP3D1c = Converter::toVector3d(P3D1c);
Eigen::Matrix<double,3,1> eigP3D2c = g2oS12.inverse().map(eigP3D1c);
cv::Mat cvP3D2c = Converter::toCvMat(eigP3D2c);

/*float invz = 1/cvP3D2c.at<float>(2);
float x = fx2 * cvP3D2c.at<float>(0)*invz + cx2;
float y = fy2 * cvP3D2c.at<float>(1)*invz + cy2;
if(bShowImages)
{
cv::Point2f ptProjPoint(x, y);
cv::line(img2, kpUn2.pt, ptProjPoint, cv::Scalar(255, 0, 0), 1);
}*/
}

ORB_SLAM3::EdgeInverseSim3ProjectXYZ* e21 = new ORB_SLAM3::EdgeInverseSim3ProjectXYZ();
Expand Down Expand Up @@ -4269,13 +4116,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
e12->setRobustKernel(0);
e21->setRobustKernel(0);
}
/*if(bShowImages)
{
string pathImg1 = "./test_OptSim3/KF_" + to_string(pKF1->mnId) + "_Main.jpg";
cv::imwrite(pathImg1, img1);
string pathImg2 = "./test_OptSim3/KF_" + to_string(pKF1->mnId) + "_Matched.jpg";
cv::imwrite(pathImg2, img2);
}*/

Verbose::PrintMess("Sim3: First Opt -> Correspondences: " + to_string(nCorrespondences) + "; nBad: " + to_string(nBad) + "; nBadOutKF2: " + to_string(nBadOutKF2), Verbose::VERBOSITY_DEBUG);

Expand Down Expand Up @@ -4321,7 +4161,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &
//Verbose::PrintMess("Sim3: Initial seed " + g2oS12, Verbose::VERBOSITY_DEBUG);
g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));
g2oS12= vSim3_recov->estimate();

//Verbose::PrintMess("Sim3: Optimized solution " + g2oS12, Verbose::VERBOSITY_DEBUG);

return nIn;
Expand Down
5 changes: 0 additions & 5 deletions src/Tracking.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -672,10 +672,6 @@ void Tracking::PreintegrateIMU()
mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
mCurrentFrame.mpLastKeyFrame = mpLastKeyFrame;

if(!mpLastKeyFrame)
{
cout << "last KF is empty!" << endl;
}
mCurrentFrame.setIntegrated();

Verbose::PrintMess("Preintegration is finished!! ", Verbose::VERBOSITY_DEBUG);
Expand Down Expand Up @@ -1376,7 +1372,6 @@ void Tracking::StereoInitialization()

if (cv::norm(mCurrentFrame.mpImuPreintegratedFrame->avgA-mLastFrame.mpImuPreintegratedFrame->avgA)<0.5)
{
cout << cv::norm(mCurrentFrame.mpImuPreintegratedFrame->avgA) << endl;
cout << "not enough acceleration" << endl;
return;
}
Expand Down

0 comments on commit b4ff9c6

Please sign in to comment.