环境:ubuntu16.04, ros kinetic, kinect v1
问题描述:
使用Kinect v1相机实时的运行由orb_slam2系统修改之后的视觉slam系统,现在无法保存运行之和的地图。有大神能帮助解决吗?最好给出具体代码
代码:
```int main(int argc, char **argv)
{
ros::init(argc, argv, "RGBD");
ros::start();
if(argc != 3)
{
cerr << endl << "Usage: 1111" << endl;
ros::shutdown();
return 1;
}
// 创建SLAM系统,并初始化所有的线程
orb_slam::System SLAM(argv[1],argv[2],orb_slam::System::RGBD,true);
ImageGrabber igb(&SLAM);
ros::NodeHandle nh;
message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_color", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth_registered/hw_registered/image_rect_raw", 1);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub);
sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr global_map(new pcl::PointCloud<pcl::PointXYZRGBA>);
global_map = SLAM.mpPointCloudMapping->GetGlobalMap();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr global_map_copy(new pcl::PointCloud<pcl::PointXYZRGB>);
// signal(SIGINT, MySigintHandler);
while(ros::ok())//使用ros::ok()与ros::spinOnce()搭配实现回调
{
pcl::copyPointCloud(*global_map, *global_map_copy);
//向topic : /orb_slam_with_kinect1/output 发送信息,发送频率为loop_rate(10),即10hz,消息池最大容量为10.
ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("/orb_slam_with_kinect1/output", 10);
sensor_msgs::PointCloud2 output;
// 转换成ROS下的数据类型 最终通过topic发布
pcl::toROSMsg(*global_map_copy,output);
output.header.stamp=ros::Time::now();
output.header.frame_id ="camera_rgb_frame";
ros::Rate loop_rate(10);
pcl_pub.publish(output);
ros::spinOnce();
loop_rate.sleep();
}
// 保存轨迹与地图
SLAM.save();
SLAM.Shutdown();
SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
ros::shutdown();
return 0;
}