1.run_kitti_stereo.cpp
Next we start to enter the main project function run_kitti_stereo.cpp. The process is very simple:
- First, a class pointer vo of the VisualOdometry class is instantiated and the address of the config is passed in.
- Then the vo class pointer calls the Init() function in the VisualOdometry class to initialize VO.
- After successful initialization, run the Run() function of the VisualOdometry class and start running VO normally.
#include <gflags/gflags.h>
#include"MYSLAM/visual_odometry.h"
//DEFINE_string parameter 1 is a variable; parameter 2 is specified by the user and is given to parameter 1; parameter 3 is similar to a prompt message indicating this variable
//When using variables defined by the DEFINE_XXX function in a program, you need to add the FLAGS_ prefix before each variable.
DEFINE_string (config_file, "./config/default.yaml" ,"config file path");
int main (int argc, char **argv){
google::ParseCommandLineFlags( & amp;argc, & amp;argv, true);
MYSLAM::VisualOdometry::Ptr vo (new MYSLAM::VisualOdometry(FLAGS_config_file));
// initialization
vo->Init();
// start operation
vo->Run();
return 0;
}
So before writing the main function, we need to implement the VisualOdometry class first
2. VisualOdometry class:
The VO class is an external interface and has the following functions:
- 1. Read the configuration file
- 2. Set the intrinsic parameters of the camera and the extrinsic parameters of the binocular camera through the Dataset::Init() function
- 3. Instantiate and initialize four objects: frontend_, backend_, map_, and viewer_. And create a connection between 3 threads
visualodometry.h:
// VO external interface
// 1. Read the configuration file
// 2. Set the internal parameters of the camera and the external parameters of the binocular camera through the Dataset::Init() function
// 3. Instantiate and initialize the four objects frontend_, backend_, map_, and viewer_. And create a connection between 3 threads
#pragma once
#ifndef MYSLAM_VISUAL_ODOMETRY_H
#define MYSLAM_VISUAL_ODOMETRY_H
#include"MYSLAM/frontend.h"
#include"MYSLAM/backend.h"
#include"MYSLAM/viewer.h"
#include"MYSLAM/common_include.h"
#include"MYSLAM/dataset.h"
namespace MYSLAM{
//VO class
class VisualOdometry{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; //Memory alignment
typedef std::shared_ptr<VisualOdometry>Ptr; //Smart pointer
// Constructor: The parameter is the configuration file path
VisualOdometry(std::string & amp;config_path);
//Initialization, return true if success
bool Init();
// Start running start vo in the dataset
void Run();
//Read and run frame by frame starting from the dataset
bool Step();
// Get front-end status
FrontendStatus GetFrontendStatus() const {return frontend_->GetStatus();}
private:
bool inited_=false; // Whether to initialize
std::string config_file_path_;//Parameter file path
//Instantiate and initialize four objects: frontend_, backend_, map_, viewer_
Frontend::Ptr frontend_=nullptr;
Backend::Ptr backend_=nullptr;
Map::Ptr map_=nullptr;
Viewer::Ptr viewer_=nullptr;
//Instantiate and initialize the dataset object
Dataset::Ptr dataset_=nullptr;
};
}//namespace MYSLAM
#endif // MYSLAM_VISUAL_ODOMETRY_H
Because we need to instantiate and initialize four objects: frontend_, backend_, map_, and viewer_, we need to write additional constructors for the three classes of frontend, backend, and display.
Frontend constructor: Frontend::Frontend()
//Constructor: Create a GFTT feature point extractor based on the parameters of the configuration file (Config class).
// num_features_ is the maximum number of extracted feature points per frame. In addition, a parameter num_features_init_ is saved, which will be used in subsequent map initialization.
Frontend::Frontend() {
//The following is the constructor of cv::GFTTDetector:
// static Ptr<GFTTDetector> create( int maxCorners=1000, double qualityLevel=0.01, double minDistance=1,
// int blockSize=3, bool useHarrisDetector=false, double k=0.04 );
// Among them, maxCorners determines the maximum number of key points to be extracted; qualityLevel represents the key point intensity threshold, and only key points greater than the threshold are retained (threshold = strongest key point intensity * qualityLevel);
// minDistance determines the minimum distance between key points; blockSize determines the accumulation area of the autocorrelation function, and also determines the window size during Sobel gradient;
// useHarrisDetector determines whether to use Harris judgment basis or Shi-Tomasi judgment basis; k is only valid when using Harris judgment basis;
gftt_=
cv::GFTTDetector::create(Config::Get<int>("num_features"),0.01, 20);
num_features_init_=Config::Get<int>("num_features_init");
num_features_= Config::Get<int>("num_features");
}
The initialization of the front end is mainly to create the GFTT feature point extractor based on the parameters of the configuration file (Config class). num_features_ is the maximum number of extracted feature points per frame. In addition, a parameter num_features_init_ is saved, which will be used in subsequent map initialization.
Backend constructor: Backend::Backend()
//Constructor starts the optimization thread and suspends it
Backend::Backend(){
backend_running_.store(true);
backend_thread_ = std::thread(std::bind( & amp;Backend::BackendLoop, this));//Lock and wake up a wait thread
}
In the backend initialization, the main thing is to open a new thread backend_thread_, and then set the function running in this thread to the Backend::BackendLoopI() function. Thread running status backend_running_ is set to true
Display constructor: Viewer::Viewer()
Viewer::Viewer(){
//Lock and wake up a wait thread
viewer_thread_= std::thread( std::bind( & amp; Viewer::ThreadLoop,this));
}
In the initialization of the Viewer class, start a thread for display and bind the Viewer::ThreadLoop() function.
In addition, the constructor of the Map class is empty, and nothing is set in the constructor.
Then let’s look at the functions in visualodometry.h:
Initialization function bool VisualOdometry::Init():
//Initialization, return true if success
bool VisualOdometry::Init(){
// 1. Call the Config::SetParameterFile() function to read the configuration file. If the corresponding file cannot be found, false is returned.
if (Config::SetParameterFile(config_file_path_)==false)
{
return false;
}
// 2. Set the intrinsic parameters of the camera and the extrinsic parameters of the binocular camera through the Dataset::Init() function.
dataset_=Dataset::Ptr(new Dataset(Config::Get<std::string>("dataset_dir")));
CHECK_EQ(dataset_->Init(), true);
// 3. Instantiate and initialize four objects: frontend_, backend_, map_, and viewer_. And create a connection between 3 threads,
frontend_=Frontend::Ptr(new Frontend);
backend_=Backend::Ptr(new Backend);
map_=Map::Ptr(new Map);
viewer_=Viewer::Ptr(new Viewer);
frontend_->SetBackend(backend_);
frontend_->SetMap(map_);
frontend_->SetViewer(viewer_);
frontend_->SetCameras(dataset_->GetCamera(0),dataset_->GetCamera(1));
backend_->SetMap(map_);
backend_->SetCameras(dataset_->GetCamera(0), dataset_->GetCamera(1));
viewer_->SetMap(map_);
return true;
}
Run function: VisualOdometry::Run():
//Start running: Keep looping the step function
void VisualOdometry::Run(){
while (1)
{
if (Step()==false)
{
break;
}
}
// Close the backend thread
backend_->Stop();
// Close the display thread
viewer_->Close();
LOG(INFO) << "VO exit";
}
Keep looping the Step() function in the Run() function until the Step() function returns false, then break out to close the backend and display the thread.
Step() function: bool VisualOdometry::Step():
// Step() function: In fact, it has been looping through the two functions Dataset::NextFrame() and Frontend::AddFrame().
bool VisualOdometry::Step(){
// Read the frame image from the data set, and then call the Frame::CreateFrame() function to assign an ID number to each frame
Frame::Ptr new_frame = dataset_->NextFrame();
if (new_frame == nullptr) return false;
// Call the AddFrame() function: run three different functions, StereoInit(), Track() and Reset() according to the front-end status variable FrontendStatus
auto t1 = std::chrono::steady_clock::now(); //Timing timestamp t1
bool success = frontend_->AddFrame(new_frame);
auto t2 = std::chrono::steady_clock::now(); //Timing timestamp t2
auto time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1); //Calculate time (t2-t1)
LOG(INFO) << "VO cost time: " << time_used.count() << " seconds.";
return success;
}
After that, you will enter the front-end main function Frontend::AddFrame() and start the SLAM process…
Frontend::AddFrame() function:
//External interface, add a frame and calculate its positioning result
bool Frontend::AddFrame(Frame::Ptr frame){//Use frame to receive the incoming new_frame
current_frame_ = frame;//Make the current frame equal to the frame of the previous line
// Determine tracking status
switch(status_){
// If the status is initialization, enter binocular initialization
case FrontendStatus::INITING:
StereoInit();
break;
// If the tracking status is good or the tracking status is poor, enter tracking.
case FrontendStatus::TRACKING_GOOD:
case FrontendStatus::TRACKING_BAD:
Track();
break;
// If tracking is lost, relocate
case FrontendStatus::LOST:
Reset();
break;
}
//After the operation on the current frame is completed, update the frame: change the current frame to the previous frame
last_frame_ =current_frame_;
return true;
}
According to the front-end status variable FrontendStatus, run three different functions, StereoInit(), Track() and Reset()