2021SC@SDUSC
(15) LVI Sam source code reading 13 -- visual_loop reading 6 - callback function + process analysis
This time, we will mainly analyze loop_ detection_ Four callback functions in the node file and a function executed in parallel
visual_loop
Subscribe to the topic in the main function and start a new thread
ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 30, image_callback); //The following three topics are in visual_ estimator/utility/visualization. The topics published in CPP need to be analyzed ros::Subscriber sub_pose = n.subscribe(PROJECT_NAME + "/vins/odometry/keyframe_pose", 3, pose_callback); ros::Subscriber sub_point = n.subscribe(PROJECT_NAME + "/vins/odometry/keyframe_point", 3, point_callback); ros::Subscriber sub_extrinsic = n.subscribe(PROJECT_NAME + "/vins/odometry/extrinsic", 3, extrinsic_callback); //Release topic pub_match_img = n.advertise<sensor_msgs::Image> (PROJECT_NAME + "/vins/loop/match_image", 3); pub_match_msg = n.advertise<std_msgs::Float64MultiArray> (PROJECT_NAME + "/vins/loop/match_frame", 3); pub_key_pose = n.advertise<visualization_msgs::MarkerArray>(PROJECT_NAME + "/vins/loop/keyframe_pose", 3);
//Start a new thread and call the process function std::thread measurement_process; //Need analysis measurement_process = std::thread(process);
image_callback
image_callback subscribed messages and visual_ IMG in feature module_ The same as callback. First, let's learn about visual analysis of my teammates_ IMG in feature_ callback.
Through the analysis of teammates, we can know that img_ The message subscribed by callback is about the characteristics of the image, sensor_ The type of msgs has also been analyzed before. In visual_ In the feature module, img_ The function of callback is to track the feature points of the new image.
void image_callback(const sensor_msgs::ImageConstPtr &image_msg)
{
if(!LOOP_CLOSURE)
return;
m_buf.lock(); //Acquire lock
image_buf.push(image_msg); //Add the acquired image message to the queue
m_buf.unlock(); //Release lock
// detect unstable camera stream
//Detect unstable camera streams
//Defines the timestamp of the last frame of the variable record
static double last_image_time = -1;
//First, it is necessary to determine whether the current image is the first frame image
if (last_image_time == -1) //If it is the first frame image, directly modify the time of the previous frame image (initialization processing)
last_image_time = image_msg->header.stamp.toSec();
else if (image_msg->header.stamp.toSec() - last_image_time > 1.0 || image_msg->header.stamp.toSec() < last_image_time)
{
//If it is not the first frame image, it is necessary to judge whether the time interval between the time stamp of the current image and the time stamp of the previous frame image is more than 1s 𞓜 whether the time stamp of the current image is earlier than the previous frame image due to some error
//If the conditions are not met, you need to return to warning and empty the previously recorded queue
ROS_WARN("image discontinue! detect a new sequence!");
new_sequence();
}
last_image_time = image_msg->header.stamp.toSec(); //Meet the conditions
First, check some previous variable definitions in this file:
queue<sensor_msgs::ImageConstPtr> image_buf; queue<sensor_msgs::PointCloudConstPtr> point_buf; queue<nav_msgs::Odometry::ConstPtr> pose_buf; std::mutex m_buf; std::mutex m_process;
Actually, image_buf for callback function img_callback, used to save a continuous image frame;
m_buf is image_ The mutex corresponding to buf and the other two queues.
new_sequence:
void new_sequence()
{//Empty the three queues maintained
m_buf.lock();
while(!image_buf.empty())
image_buf.pop();
while(!point_buf.empty())
point_buf.pop();
while(!pose_buf.empty())
pose_buf.pop();
m_buf.unlock();
}
Therefore, the function of the callback function is summarized as follows: a queue is maintained to save each received frame image and record the time stamp of the last frame image; If the current incoming image is discontinuous, the queue is cleared.
point_callback pose_callback
point_ The callback function receives a point cloud type message, pose_ The callback function receives a pose message. Through img_ Through the analysis of the callback function, it can be inferred that piont, IMG and pose should be a group of data, so in piont_callback and pose_ In the callback function, you only need to add the data to the queue without judging the timestamp and other information.
void point_callback(const sensor_msgs::PointCloudConstPtr &point_msg)
{
if(!LOOP_CLOSURE)
return;
m_buf.lock();
point_buf.push(point_msg);
m_buf.unlock();
}
void pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
if(!LOOP_CLOSURE)
return;
m_buf.lock();
pose_buf.push(pose_msg);
m_buf.unlock();
}
extrinsic_callback
The message received by the callback function is the message received from the estimator. Through the communication with teammates, it can be simply understood that the pose information here is the pose information simply processed after the initialization of the estimator.
void extrinsic_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
m_process.lock();
tic = Vector3d(pose_msg->pose.pose.position.x,
pose_msg->pose.pose.position.y,
pose_msg->pose.pose.position.z);
qic = Quaterniond(pose_msg->pose.pose.orientation.w,
pose_msg->pose.pose.orientation.x,
pose_msg->pose.pose.orientation.y,
pose_msg->pose.pose.orientation.z).toRotationMatrix();
m_process.unlock();
}
It can be seen that the callback function should be related to the parallel thread executed in the main function. The callback function modifies tic and qic with the obtained message.
Eigen::Vector3d tic; Eigen::Matrix3d qic;
Through reading the above callback functions, we can guess that these callback functions need to share some variables with the process function, so they need to be locked during use to ensure that there will be no read-write conflict. The following is to read the process function to verify the conjecture.
process function
void process()
{
if (!LOOP_CLOSURE)
return;
while (ros::ok()) //Loop execution
{
//Initialize three messages of image point pose
sensor_msgs::ImageConstPtr image_msg = NULL;
sensor_msgs::PointCloudConstPtr point_msg = NULL;
nav_msgs::Odometry::ConstPtr pose_msg = NULL;
// find out the messages with same time stamp
// Looking for messages with the same timestamp
m_buf.lock();
//The following branch condition is summarized as follows: if the timestamp of three queues is inconsistent, the data will be lost
if(!image_buf.empty() && !point_buf.empty() && !pose_buf.empty()) //
{
//The judgment mechanism downstairs is: through the pairwise comparison of three queues, the team head elements of the final three queues are the same time stamp
if (image_buf.front()->header.stamp.toSec() > pose_buf.front()->header.stamp.toSec())
{
pose_buf.pop();
printf("throw pose at beginning\n");
}
else if (image_buf.front()->header.stamp.toSec() > point_buf.front()->header.stamp.toSec())
{
point_buf.pop();
printf("throw point at beginning\n");
}
else if (image_buf.back()->header.stamp.toSec() >= pose_buf.front()->header.stamp.toSec()
&& point_buf.back()->header.stamp.toSec() >= pose_buf.front()->header.stamp.toSec())
{
pose_msg = pose_buf.front();
pose_buf.pop();
while (!pose_buf.empty())
pose_buf.pop();
while (image_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec())
image_buf.pop();
image_msg = image_buf.front();
image_buf.pop();
while (point_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec())
point_buf.pop();
point_msg = point_buf.front();
point_buf.pop();
}
}
m_buf.unlock();
if (pose_msg != NULL) //Pose queue is not empty
{
// skip fisrt few
static int skip_first_cnt = 0;
if (skip_first_cnt < SKIP_FIRST_CNT)
{
//SKIP_FIRST_CNT = 10 represents the number of times that should be ignored
skip_first_cnt++;
continue;
}
// limit frequency
static double last_skip_time = -1;
//If the interval between two frames is too small, it is not processed
if (pose_msg->header.stamp.toSec() - last_skip_time < SKIP_TIME)
continue;
else
last_skip_time = pose_msg->header.stamp.toSec();
// get keyframe pose
//Obtain pose information
static Eigen::Vector3d last_t(-1e6, -1e6, -1e6);
Vector3d T = Vector3d(pose_msg->pose.pose.position.x,
pose_msg->pose.pose.position.y,
pose_msg->pose.pose.position.z);
Matrix3d R = Quaterniond(pose_msg->pose.pose.orientation.w,
pose_msg->pose.pose.orientation.x,
pose_msg->pose.pose.orientation.y,
pose_msg->pose.pose.orientation.z).toRotationMatrix();
// add keyframe
//Judge whether a loop is formed between two frames
if((T - last_t).norm() > SKIP_DIST)
{
// convert image
//Convert the original image data type to the img type in cv
cv_bridge::CvImageConstPtr ptr;
if (image_msg->encoding == "8UC1")
{
sensor_msgs::Image img;
img.header = image_msg->header;
img.height = image_msg->height;
img.width = image_msg->width;
img.is_bigendian = image_msg->is_bigendian;
img.step = image_msg->step;
img.data = image_msg->data;
img.encoding = "mono8";
ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
}
else
ptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8);
cv::Mat image = ptr->image;
vector<cv::Point3f> point_3d;
vector<cv::Point2f> point_2d_uv;
vector<cv::Point2f> point_2d_normal;
vector<double> point_id;
//Traverse point cloud information
for (unsigned int i = 0; i < point_msg->points.size(); i++)
{
cv::Point3f p_3d;
p_3d.x = point_msg->points[i].x;
p_3d.y = point_msg->points[i].y;
p_3d.z = point_msg->points[i].z;
point_3d.push_back(p_3d);
cv::Point2f p_2d_uv, p_2d_normal;
double p_id;
p_2d_normal.x = point_msg->channels[i].values[0];
p_2d_normal.y = point_msg->channels[i].values[1];
p_2d_uv.x = point_msg->channels[i].values[2];
p_2d_uv.y = point_msg->channels[i].values[3];
p_id = point_msg->channels[i].values[4];
point_2d_normal.push_back(p_2d_normal);
point_2d_uv.push_back(p_2d_uv);
point_id.push_back(p_id);
}
// new keyframe
static int global_frame_index = 0;
//The filtered key frames are generated into keyframe s for further loop detection
KeyFrame* keyframe = new KeyFrame(pose_msg->header.stamp.toSec(), global_frame_index,
T, R,
image,
point_3d, point_2d_uv, point_2d_normal, point_id);
// detect loop
m_process.lock();
loopDetector.addKeyFrame(keyframe, 1); //Add this key frame to loopDetector
m_process.unlock();
//inspect
loopDetector.visualizeKeyPoses(pose_msg->header.stamp.toSec());
global_frame_index++;
last_t = T;
}
}
std::chrono::milliseconds dura(5);
std::this_thread::sleep_for(dura);
}
}
After reading through the whole process code, we can basically verify that the above conjecture is correct: the main work of the process function is to find two loopback frames from the various information of the image stored through the callback function, and then carry out further loopback detection.
The question here is whether the meaning of stamp in image means timestamp, because in the above judgment, it is to find frames with the same timestamp. This problem needs further study to solve.
summary
After analyzing the above callback functions and process functions, visual_ The analysis of the loop part is basically completed. Because it takes a long time to study the theoretical basis and various tools and environments in the early stage, the analysis of the core code is a little hasty, and the understanding of some details is not very in-depth. It is necessary to continue to process these places with insufficient learning after the end of the course.