看鲁哥的程序源代码,想要学下ROS顺便改出我需要的部分。
1.图片发布器
调用语句为
rosrun dataset_publisher stereo_kitti_publisher /home/stein/05111658/ 5
读取图片路径部分为
vector<string> vstrImageLeft;
vector<string> vstrImageRight;
vector<double> vTimestamps;
LoadImages(string(argv[1]), vstrImageLeft, vstrImageRight, vTimestamps);
const int nImages = vstrImageLeft.size();
其中vector中存储的是每张图片的绝对路径,nImages是图片总数。
image_transport::Publisher image_pub_left;
image_transport::Publisher image_pub_right;
image_pub_left = it_.advertise("/camera/left/image_raw", 1);
image_pub_right = it_.advertise("/camera/right/image_raw", 1);
上面定义了两个消息发布器,并且指定了发布频段和最大缓存数量(频道中最多保留几张图)。
cv_bridge::CvImage cvi_l;
cv_bridge::CvImage cvi_r;
cvi_l.header.frame_id = "image";
cvi_r.header.frame_id = "image";
cvi_l.encoding = "bgr8";
cvi_r.encoding = "bgr8";
cv_bridge可以将ROS格式图片和Opencv格式互相转化。
Mat img;
cv::resize(imgLeft,img,Size(imgLeft.cols/2,imgLeft.rows/2));
imshow("img",img);
运行时显示图片,除2是因为原图太大。
cvi_l.header.stamp = time;
cvi_r.header.stamp = time;
cvi_l.image = imgLeft;
cvi_r.image = imgRight;
sensor_msgs::Image im_l;
sensor_msgs::Image im_r;
cvi_l.toImageMsg(im_l);
cvi_r.toImageMsg(im_r);
cv_bridge读取图片,将其转化为消息格式存入im_x,通过前面的消息发布器发布。
2.SLAM核心部分
调用语句为
roslaunch ap_slam_core ap_slam_zed.launch
这部分还没细看,rviz可以看到发布了相机、车的位姿以及轨迹,还有匹配时的所有特征点。
3.DenseMapper深度建图部分
调用语句为roslaunch ap_dense_mapper ap_densemapper_zed.launch
我的毕设需要构建俯瞰图,所以只看如何获取的图片和位姿信息。从main函数开始。
ParamCam cam(_config_dir);
这句之前读取了相机参数。
message_filters::Subscriber<sensor_msgs::Image> colorImgLeft_sub(nh, "/colorImgLeft_now", 1);
message_filters::Subscriber<sensor_msgs::Image> colorImgRight_sub(nh, "/colorImgRight_now", 1);
message_filters::Subscriber<geometry_msgs::PoseStamped> pose_sub(nh, "/camera_pose", 1);
message_filters::Subscriber<ap_msgs::APCamInfo> apcaminfo_sub(nh, "/apcam_info", 1);
订阅器定义,指定订阅频道。
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
sensor_msgs::Image,
geometry_msgs::PoseStamped,
ap_msgs::APCamInfo> sync_pol;
message_filters::Synchronizer<sync_pol> sync(sync_pol(10), colorImgLeft_sub,
colorImgRight_sub,
pose_sub,
apcaminfo_sub);
sync.registerCallback(boost::bind(&GrabInfo,_1, _2, _3, _4));
这段没看太懂,感觉是定义了一个回调函数的调用方式,即产生变化就调用GrabInfo函数。
看GrabInfo里面,大概是读取了左右图、位姿和相机信息,并把bNewInfo置true.
接下来DenseMapping。大致就是用接受到的信息进行的操作。