First, put the entire code framework of LSD SLAM, so that readers can clearly know what position the tracking part is in in the entire SLAM framework.
The main job of the tracking part of LSD SLAM is to solve the pose (R, t) between the keyframe and the current frame. Knowing the 3D point of the high gradient point on the keyframe, you can first initialize a pose (such as using the pose of the previous frame). As the initial value of this frame), and then construct a least squares problem through photometric error, and use optimization method to optimize pose.
1. The main function calls the code of the tracking part of the function:
As shown in the figure above, after the initialization is completed, the member function of the SE3Tracker class in the tracking section is called directly to track.
Second, the function of the tracking part The
tracking part mainly has four files, namely SE3Tracker.cpp, SE3Tracker.h, TrackingReference.cpp, TrackingReference.h.
Among them, SE3Tracker is the most important class in the tracking part. Its member function trackFrame() implements a reference frame and current frame, as well as an initial pose estimation, and outputs an optimized pose.
The specific implementation is as follows:
1. First, call the makePointCloud() function in the trackingReference class to calculate the 3D coordinates of the point with depth in the reference frame. This function mainly records the pixel gradient and the number of point clouds of the reference frame.
2. Then call the calcResidualAndBuffers() function to calculate the residual between the corresponding point luminosity of the two images.
This function mainly inputs the 2d coordinates of the high gradient point on the reference frame and the corresponding 3d point cloud, as well as the current frame and the pose from the reference frame to the current frame. First, the point on the reference frame is transformed by the rotation matrix rotMat and the translation vector transVec, then the u, v coordinates under the current frame are calculated, and then the gray value corresponding to the u, v coordinates is found by interpolation, and the calculation grayscale residuals. Finally, the residuals of all points are averaged, and an average residual is output.
3. Call the callOptimized() function for optimization to obtain the last optimized SE3
参考文献：
[1] https://blog.csdn.net/lancelot_vim/article/details/51758870
[2] Engel, Jakob, Thomas Schöps, and Daniel Cremers. “LSD-SLAM: Large-scale direct monocular SLAM.” European Conference on Computer Vision. Springer, Cham, 2014.
[3] https://github.com/tum-vision/lsd_slam