先把代码跑起来,虽然rviz能显示轨迹,但是看左侧Displays中的tracked image下拉的status项显示状态为warn,并提示no image received,看不到提取的点线特征图像,显示No Image。
终端报错[image_node_b-7] process has died [pid 10357, exit code -11, cmd /home/nb/SLAM/Plvins/devel/lib/image_node_b/image_node_b __name:=image_node_b __log:=/home/nb/.ros/log/0c2a72aa-cec6-11ed-aa31-cc3d82ac06b1/image_node_b-7.log].
log file: /home/nb/.ros/log/0c2a72aa-cec6-11ed-aa31-cc3d82ac06b1/image_node_b-7*.l也就是说image_node_b.cpp这个节点挂掉了。
参考博客:原文链接:https://blog.csdn.net/weixin_48135525/article/details/117635275我觉得这篇博客说的不容易理解,所以自己重新总结了下。
问题原因:
挂掉的原因是linefeature_tracker_node中只发布了归一化坐标,没有发布linefeature的startpoint和endpoint的像素坐标。
解决方案:
所以更改image_node_b.cpp中的线重投影的代码:
在void callback函数上方新增加一个函数void project存放像素坐标:
void project(cv::Point2f& pt, cv::Mat const& k)
{
pt.x=k.at<float>(0,0)*pt.x+k.at<float>(0,2);
pt.y=k.at<float>(1,1)*pt.y+k.at<float>(1,2);
}
像这样:
然后 修改线投影的代码,找到如下代码段并注释掉:
cv::Point startPoint = cv::Point(line_feature_msg->channels[3].values[i], line_feature_msg->channels[4].values[i]);
cv::Point endPoint = cv::Point(line_feature_msg->channels[5].values[i], line_feature_msg->channels[6].values[i]);
然后添加新的正确的代码段:
cv::Point2f startPoint = cv::Point2f(line_feature_msg->points[i].x,line_feature_msg->points[i].y );
project (startPoint,K_);
cv::Point2f endPoint = cv::Point2f(line_feature_msg->channels[1].values[i],line_feature_msg->channels[2].values[i]);
project (endPoint,K_);
像这样:
最后为了防止相对路径识别不了,把以下路径改为绝对路径:
更改后保存重新catkin_make下就ok了。