在ros上实时运行视觉slam，保存地图问题

Question

环境：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;
}

Answer 1

https://blog.csdn.net/qq_25368751/article/details/99336817