2021SC@SDUSC
（十五）lvi-sam源代码阅读13 —— visual_loop阅读6 - 回调函数 + process分析
本次将主要分析loop_detection_node文件中的4个回调函数以及一个并行执行的函数
visual_loop
main函数中的订阅话题 以及开启新线程
ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 30, image_callback);
//以下三个话题在visual_estimator/utility/visualization.cpp中发布的话题 需要分析
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);
//发布话题
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);
//开启一个新的线程，调用process函数
std::thread measurement_process;
//需要分析
measurement_process = std::thread(process);
image_callback
image_callback订阅的消息与visual_feature模块中的img_callback相同，这里首先学习一下我的队友分析的关于visual_feature里的img_callback。
通过队友的分析可以知道，img_callback订阅的消息是有关图像的特征，sensor_msgs的类型在之前也分析过。在visual_feature模块中，img_callback的作用是：对新来的图像进行特征点的追踪。
void image_callback(const sensor_msgs::ImageConstPtr &image_msg)
{
if(!LOOP_CLOSURE)
return;
m_buf.lock(); //获取锁
image_buf.push(image_msg); //将获取的图像消息加入到队列中
m_buf.unlock(); //释放锁
// detect unstable camera stream
//检测不稳定的相机流
//定义变量记录上一帧图像的时间戳
static double last_image_time = -1;
//首先需要判断当前图像是否为第一帧图像
if (last_image_time == -1) //如果是第一帧图像，就直接修改上一帧图像时间（初始化处理）
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)
{
//如果不是第一帧图像，则需要判断当前图像的时间戳与上一帧图像时间戳的时间间隔是否在1s以上 || 当前图像是否是因为某种错误导致时间戳早于上一帧图像
//如果不满足条件，则需要返回warning，并且清空之前记录的队列
ROS_WARN("image discontinue! detect a new sequence!");
new_sequence();
}
last_image_time = image_msg->header.stamp.toSec(); //满足条件
首先查看该文件中之前的一些变量定义：
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;
其实image_buf针对于回调函数img_callback，用于保存一段连续的图像帧；
m_buf则是image_buf和其余两个队列所对应的互斥锁。
new_sequence：
void new_sequence()
{//将维护的三个队列清空
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();
}
所以该回调函数的作用总结来说就是：维护了一个队列用来保存接收到的每一帧图像，并记录上一帧图像的时间戳；如果当前传入的图像出现了不连续的情况，则清空队列。
point_callback pose_callback
point_callback回调函数接收的是一个点云类型的消息，pose_callback回调函数接收的是一个位姿消息。通过对img_callback函数的分析，可以推测出piont和img、pose应该是一组数据，所以在piont_callback和pose_callback函数中只需要将数据添加到队列中，而不需要判断时间戳等信息。
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
该回调函数接收的消息是从estimator中接收的消息。通过和队友的交流后了解到，可以简单理解为这里的pose信息是经过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();
}
可以看出，这个回调函数与main函数内执行的并行线程应该是有联系的，回调函数用获取的消息修改了tic和qic。
Eigen::Vector3d tic;
Eigen::Matrix3d qic;
通过对以上几份回调函数的阅读，可以猜想的是：这些回调函数需要和process函数共享一些变量，所以在使用的过程中需要加锁来保证不会发生读写冲突。下面就是阅读process函数来验证猜想。
process函数
void process()
{
if (!LOOP_CLOSURE)
return;
while (ros::ok()) //循环执行
{
//初始化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
// 寻找具有相同时间戳的消息
m_buf.lock();
//下面的分支条件总结来说就是：如果发现三个队列存在时间戳不一致的情况 就丢掉数据
if(!image_buf.empty() && !point_buf.empty() && !pose_buf.empty()) //
{
//楼下的判断机制是：通过三个队列两两比较，来使得最终的三个队列的队首元素都是时间戳相同的袁术
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) //位姿队列不为空
{
// skip fisrt few
static int skip_first_cnt = 0;
if (skip_first_cnt < SKIP_FIRST_CNT)
{
//SKIP_FIRST_CNT = 10 代表应该被忽略的次数
skip_first_cnt++;
continue;
}
// limit frequency
static double last_skip_time = -1;
//如果两帧之间的间隔太小 也不处理
if (pose_msg->header.stamp.toSec() - last_skip_time < SKIP_TIME)
continue;
else
last_skip_time = pose_msg->header.stamp.toSec();
// get keyframe pose
//获取位姿信息
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
//判断两帧之间是否形成回环
if((T - last_t).norm() > SKIP_DIST)
{
// convert image
//将原始的image数据类型转为cv中的img类型
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;
//遍历点云信息
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;
//将筛选出来的关键帧生成keyframe，用来做进一步的回环检测
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); //将这一关键帧添加到loopDetector里面
m_process.unlock();
//检查
loopDetector.visualizeKeyPoses(pose_msg->header.stamp.toSec());
global_frame_index++;
last_t = T;
}
}
std::chrono::milliseconds dura(5);
std::this_thread::sleep_for(dura);
}
}
通读完成整个process代码后，基本可以验证上面的猜想是正确的：process函数主要的工作就是从之前已经通过回调函数的存储好的图像的各种信息，寻找两帧回环帧，然后进行进一步的回环检测。
这里存在的疑问是：image中的stamp的含义是否表示的就是时间戳，因为在上面的判断中是为了寻找时间戳相同的帧。这个问题需要后面的进一步学习来解决。
总结
分析完上面几个回调函数和process函数后，对visual_loop部分的分析也就基本完成了。因为前期花在理论基础和各种工具类、环境的学习时间比较久，所以对核心代码的分析有些仓促，有一些细节的地方了解的还不是很深入，需要在课程结束后的时间里继续对这些学习不足的地方进行加工。
扫一扫
2004
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
