代码DenseSurfelMapping的阅读总结

代码来自：https://github.com/HKUST-Aerial-Robotics/DenseSurfelMapping
论文：《real-time scalable dense surfel mapping》

总览

信息的流向和文件之间的大致关系，下面每个部分对应上图的一个方框。

深度\rgb图像的读取包装与发送

publisher.py 实现了rgb/深度图像的读取并发送。

#功能:加载图片并包装成msgs发送出去
if __name__ == '__main__':
    rospy.init_node('kitti_publisher')
    left_pub = rospy.Publisher("left_image", Image, queue_size=100)
    depth_pub = rospy.Publisher("depth_image", Image, queue_size=100)
    while True:
        left_img = cv2.imread(left_img_path,0) #读取rgb图像
        depth_np = np.load(depth_path) #读取深度图像

        pub_ros_time = rospy.get_rostime()
        left_msg = cv_bridge.cv2_to_imgmsg(left_img, "mono8") #创建消息 并包装 left_msg 
        left_msg.header.stamp = pub_ros_time 
        left_pub.publish(left_msg) #通过 left_pub 将消息发送出去 

        depth_msg = cv_bridge.cv2_to_imgmsg(depth_np, "32FC1")  #创建深度图像消息 并包装 
        depth_msg.header.stamp = pub_ros_time#赋予图像时间戳 
        depth_pub.publish(depth_msg)#发送

        rate.sleep()
        image_index+=1

SLAM系统将图像转化为位姿信息、闭环信息

\ORB_SLAM2\Examples\ROS\ORB_SLAM2\src\ros_rgbd.cc：
功能：调用ORB-SLAM2，跟踪接收到的深度/rgb图像，并将结果发送出去。
步骤：
1 判断两张图像正常接收
2 将 RGB图像+深度图像+时间戳 送入跟踪线程 获得初始位姿 track_result
3 等待localMapping线程完成
4 包装并发送 nav_msgs::Path result_path——记录了关键帧位姿/时间戳序列
5 在关键帧序列中寻找参考帧（应该只是一步检验）
6 包装并发送 nav_msgs::Odometry this_odometry——存储当前帧位姿
7 包装并发送 sensor_msgs::PointCloud ros_loop_info——存储闭环信息

// An highlighted block

int main(int argc, char **argv)
{
    ros::init(argc, argv, "RGBD");
    ros::start();

    if(argc != 3)
    {
        cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings" << endl;        
        ros::shutdown();
        return 1;
    }    

    //创建SLAM系统，初始化参数和三个线程。
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);

    ImageGrabber igb(&SLAM);

    ros::NodeHandle nh;
	//接受图像信息，并处理
    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 1); //用于接收RGB图像
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth_registered/image_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)); //接收到两个图像，进入函数 ImageGrabber::GrabRGBD
    
	 //创建三个发布，用于发布姿态序列、当前姿态、和闭环信息
    path_publish = nh.advertise<nav_msgs::Path>("orb_slam/path", 1000); 
    pose_publish = nh.advertise<nav_msgs::Odometry>("orb_slam/pose", 1000);
    loop_publish = nh.advertise<sensor_msgs::PointCloud>("orb_slam/loop", 1000);

    ros::spin();
    SLAM.Shutdown(); 
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
    ros::shutdown();
    return 0;
}

void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
{
    //1 判断两张图像正常接收
    cv_bridge::CvImageConstPtr cv_ptrRGB;
    try{cv_ptrRGB = cv_bridge::toCvShare(msgRGB);}
    catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());return;}
    cv_bridge::CvImageConstPtr cv_ptrD;
    try{cv_ptrD = cv_bridge::toCvShare(msgD);}
    catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());return;}

    //2 将 RGB图像+深度图像+时间戳 送入跟踪线程 获得初始位姿 track_result
    cv::Mat track_result;
    track_result = mpSLAM->TrackRGBD(cv_ptrRGB->image, cv_ptrD->image, cv_ptrRGB->header.stamp.toSec());

    //3 等待localMapping线程完成
    while (mpSLAM->LocalMappingStopped()){void();}
    //此时 SLAM系统已经完成了处理,只需要输出建图所需要的的三个参数即可。

    //4 包装并发送 nav_msgs::Path result_path——记录了关键帧位姿/时间戳序列
    std::vector<std::pair<cv::Mat, double>> result_vector;
    mpSLAM->GetAllPoses(result_vector); //取出SLAM系统中Map中所有关键帧位姿和对应时间戳
    nav_msgs::Path result_path;
    result_path.header.stamp = msgRGB->header.stamp; //result_path的时间戳为当前图像的时间戳
    result_path.header.frame_id = "world";  //id为world 
    Eigen::Matrix4d temp_matrix, temp_matrix_inverse;
    Eigen::Matrix4d trans_form = Eigen::Matrix4d::Identity();
    for (int i = 0; i < result_vector.size(); i++)//循环所有关键帧
    {
        geometry_msgs::PoseStamped this_pose;
        for (int j = 0; j < receive_time_stamp.size(); j++) 
        //如果关键帧i的时间与 receive_time_stamp 中记录的某个时间基本一致,采用 receive_time_stamp 中记录的时间
            if (fabs(receive_time_stamp[j].toSec() - result_vector[i].second) < 0.001){this_pose.header.stamp = receive_time_stamp[j];break;}
        //记录关键帧i的位置和四元数方向
        for (int row_i = 0; row_i < 4; row_i++)
            for (int col_i = 0; col_i < 4; col_i++)
                temp_matrix(row_i, col_i) = result_vector[i].first.at<float>(row_i, col_i);
        temp_matrix_inverse = trans_form * temp_matrix.inverse();
        Eigen::Quaterniond rotation_q(temp_matrix_inverse.block<3, 3>(0, 0));
        this_pose.pose.position.x = temp_matrix_inverse(0, 3);
        this_pose.pose.position.y = temp_matrix_inverse(1, 3);
        this_pose.pose.position.z = temp_matrix_inverse(2, 3);
        this_pose.pose.orientation.x = rotation_q.x();
        this_pose.pose.orientation.y = rotation_q.y();
        this_pose.pose.orientation.z = rotation_q.z();
        this_pose.pose.orientation.w = rotation_q.w();
        result_path.poses.push_back(this_pose);// result_path.poses 实际存储位姿/时间戳序列
    }
    path_publish.publish(result_path);//发送

    //5 在关键帧序列中寻找参考帧（应该只是一步检验）
    double reference_stamp;
    reference_stamp = mpSLAM->GetRelativePose(); //参考关键帧的时间戳 
    int reference_index = 0;
    double time_diff = 1e9;
    for (int i = 0; i < result_vector.size(); i++) //循环关键帧，比较关键帧和参考关键帧的时间戳
    {
        double this_time_diff = fabs(result_vector[i].second - reference_stamp);
        if (this_time_diff < time_diff){reference_index = i;time_diff = this_time_diff;}
    }
    if (time_diff < 0.01) //在关键帧序列中找到了参考关键帧  
        printf("the reference keyframe is %d, keyframe number %d.\n", reference_index, result_vector.size());
    else
        printf("cannot find the reference keyframe! time difference %f, the stamp is %f, current is %f.!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n",
               time_diff,
               reference_stamp,
               msgRGB->header.stamp.toSec());

    //6 包装并发送 nav_msgs::Odometry this_odometry——存储当前帧位姿
    LastKeyframeDecision = mpSLAM->GetKeyframeDecision(); //当前帧是否是关键帧。
    if (LastKeyframeDecision) printf("this is keyframe.\n");
    nav_msgs::Odometry this_odometry;
    this_odometry.header.stamp = msgRGB->header.stamp; //赋予当前图像时间戳
    this_odometry.header.frame_id = "world";
    Eigen::Matrix4d T_cw, T_wc;
    for (int row_i = 0; row_i < 4; row_i++)
        for (int col_i = 0; col_i < 4; col_i++)
            T_cw(row_i, col_i) = track_result.at<float>(row_i, col_i);
    T_wc = T_cw.inverse();
    Eigen::Quaterniond rotation_q(T_wc.block<3, 3>(0, 0));
    this_odometry.pose.pose.position.x = T_wc(0, 3);
    this_odometry.pose.pose.position.y = T_wc(1, 3);
    this_odometry.pose.pose.position.z = T_wc(2, 3);
    this_odometry.pose.pose.orientation.x = rotation_q.x();
    this_odometry.pose.pose.orientation.y = rotation_q.y();
    this_odometry.pose.pose.orientation.z = rotation_q.z();
    this_odometry.pose.pose.orientation.w = rotation_q.w(); //存储当前帧的跟踪位姿 
    if (LastKeyframeDecision) this_odometry.pose.covariance[0] = 1; //如果当前帧是关键帧，为1
    else this_odometry.pose.covariance[0] = 0;
    this_odometry.pose.covariance[1] = reference_index; //参考帧 在 位姿序列 中对应的序号 
    pose_publish.publish(this_odometry);//发送

    //7 包装并发送 sensor_msgs::PointCloud ros_loop_info——存储闭环信息
    sensor_msgs::PointCloud ros_loop_info;
    ros_loop_info.header.stamp = msgRGB->header.stamp; //赋予 图像时间戳 
    ros_loop_info.header.frame_id = "this_is_loop_info";
    std::vector<std::pair<double, double>> loop_result;  
    mpSLAM->GetLoopInfo(loop_result); // 返回的是<时间上最新关键帧的时间戳,该关键帧的共视帧、父帧的时间戳>序列 
    sensor_msgs::ChannelFloat32 loop_channel;
    //将闭环信息的 时间戳对 转化为 序列对
    for (int i = 0; i < loop_result.size() && i < 35; i++) //循环所有loop_result
    {
        int first_index = -1;
        int second_index = -1;
        for (int j = 0; j < result_vector.size(); j++) //循环所有关键帧位姿
        {
            if (result_vector[j].second == loop_result[i].first) 
                first_index = j; //找到第i个时间戳对 的 第一个时间戳对应的序列 
            if (result_vector[j].second == loop_result[i].second)
                second_index = j;//找到第i个时间戳对 的 第二个时间戳对应的序列 
        }
        if (first_index > 0 && second_index > 0) //如果 两者都找到了  相当于找到了闭环序列对
        {
            printf("the loop info %d <---> %d\n", first_index, second_index);
            loop_channel.values.push_back(first_index);
            loop_channel.values.push_back(second_index);//两个序列都放入 loop_channel.values 中 
        }
        else
            printf("cannot find corresponding!\n");
    }
    ros_loop_info.channels.push_back(loop_channel);
    loop_publish.publish(ros_loop_info);
}

ros_node.cpp 接收图像信息和SLAM信息并分别送入下一层

ros_node.cpp，接收到图像msgs和SLAM msgs，将信息传递给下一层函数。

// An highlighted block

int main(int argc, char **argv)
{
  ros::init(argc, argv, "surfel_fusion");
  ros::NodeHandle nh("~");

  SurfelMap surfel_map(nh);
  //1 接受图像并处理 
  ros::Subscriber sub_image = nh.subscribe("image", 5000, &SurfelMap::image_input, &surfel_map);
  ros::Subscriber sub_depth = nh.subscribe("depth", 5000, &SurfelMap::depth_input, &surfel_map);
  ros::Subscriber sub_save_map = nh.subscribe("save_map", 5000, &SurfelMap::save_map, &surfel_map);
  //2 接受SLAM信息并处理
  message_filters::Subscriber<sensor_msgs::PointCloud> sub_loop_stamps(nh, "loop_stamps", 1000);
  message_filters::Subscriber<nav_msgs::Path> sub_loop_path(nh, "loop_path", 1000);
  message_filters::Subscriber<nav_msgs::Odometry> sub_this_pose(nh, "this_pose", 1000);
  message_filters::Synchronizer<exact_policy> sync(exact_policy(1000), sub_loop_stamps, sub_loop_path, sub_this_pose);
  sync.registerCallback(boost::bind(&SurfelMap::orb_results_input, &surfel_map, _1, _2, _3));
  while(ros::ok()){ros::spinOnce();}
  string save_name;
  //相关存储 
  if(nh.getParam("save_name", save_name))
  {
    string pcd_name = save_name + ".PCD";
    string mesh_name = save_name + "_mesh.PLY";
    surfel_map.save_cloud(pcd_name);
    surfel_map.save_mesh(mesh_name);
  }
  return EXIT_SUCCESS;
}

SurfelMap::image_input和SurfelMap::depth_input将rgb/深度图像放入image_buffer/depth_buffer

两个函数分别接收到rgb图像和深度图像，并将<时间戳，图像>放入SurfelMap.image_buffer 和 SurfelMap.depth_buffer 中，然后进入 synchronize_msgs()。
SurfelMap.image_buffer 和 SurfelMap.depth_buffer：存储的是未被 synchronize_msgs()处理的图像（处理完成后的待处理队列-1操作也是在该函数实现，是在else{}下进行的）

SurfelMap::orb_results_input() 对SLAM结果的处理

这一步的主要作用是1，根据SLAM优化结果，变形所有surfel以满足全局一致。
2，更新关键帧位姿序列poses_database和pose_reference_buffer
其输入是来自SLAM的三个msgs：
this_pose_input)//当前帧姿态
this_pose_input->header.stamp 为图像时间戳
this_pose_input->header.frame_id = “world”;
this_pose_input->pose.pose.position.x/y/z 为位置
this_pose_input->pose.pose.orientation.x/y/z/w 四元数方向
this_pose_input->pose.covariance[0]=1（当前帧为关键帧）0（否则）
this_pose_input->pose.covariance[1] = reference_index 参考帧索引

loop_path_input, //关键帧位姿序列
loop_path_input->header.stamp 图像时间戳
loop_path_input->header.frame_id = “world”
loop_path_input->poses 为位姿vector
其中 loop_path_input->poses[x].pose.position.x/yz 为位置
loop_path_input->poses[x].pose.orientation.x/y/z/w 方向

loop_stamp_input, //闭环信息
loop_stamp_input->header.stamp 图像时间戳
loop_stamp_input->header.frame_id = “this_is_loop_info”
loop_stamp_input->channels.loop_channel.values 是int数组，存储闭环序列对（闭环对1A，闭环对1B，闭环对2A，闭环对2B…）

处理过程分为以下几步：
1 获得适用于该系统的当前帧位姿 input_pose
（从SLAM进入该系统的所有位姿都要经过一个 transform_kitti 的变换，为什么？应该是为了使得该系统的第一帧/原点位于一个指定位置）

2 根据来自SLAM的位姿序列，重新给位姿库 poses_database[i].loop_pose 存储的位姿赋值
（位姿库 poses_database 对应的也是每张关键帧）

3 （如果上一步中出现了赋值前后位姿不一致，说明SLAM系统进行了闭环优化）对surfel进行变换：
a,inactive,存储于姿态库中每个姿态下 poses_database[i].attached_surfels, 变换依据于该位姿的优化变换
b,active , 即 local_surfel 中的surfel ,变换依据于该图像的参考关键帧的优化变换（存储于local_surfels_indexs的首项 ,存储未被处理图像的参考帧索引 ）

4 将闭环信息 添加进位姿数据库中a.poses_database[loop_first].linked_pose_index.push_back(loop_second)
b.poses_database[loop_second].linked_pose_index.push_back(loop_first)

5 如果当前帧作为关键帧，我们将其添加入位姿数据库中
a.PoseElement.cam_pose/loop_pose 记录位姿
b.PoseElement.cam_stamp 记录图像时间戳
c.将参考关系也计入 PoseElement.linked_pose_index 中
d.poses_database.push_back(PoseElement)
e.local_surfels_indexs 添加该关键帧索引

6, 将图像帧的信息<时间戳、当前帧相对于参考帧位姿、参考帧索引>放入 SurfelMap.pose_reference_buffer （存储所有未处理图像帧的位姿信息和参考帧信息）

7.进入 synchronize_msgs();

SurfelMap::synchronize_msgs() 对surfel的创建融合

函数输入：
进入该函数时，系统通过 SurfelMap::image_input()，SurfelMap::depth_input() 和SurfelMap::orb_results_input() 获得了多个将要处理的图像信息序列：
SurfelMap.image_buffer <rgb图像,时间戳>序列
SurfelMap.depth_buffer <深度图像,时间戳>序列
SurfelMap.pose_reference_buffer <时间戳,该图像帧关于其参考关键帧的位姿,其参考帧在位姿序列中的索引>

步骤：
1.通过时间戳判断接收到正确的深度图像/rgb图像/位姿信息
a.从 pose_reference_buffer 取出第一项时间
b.从 image_buffer/depth_buffer 取出第一项时间
c.三者相等,证明找到了相匹配的图像与位姿。

2 取得当前帧位姿fuse_pose：a，相对于参考帧的位姿；b，参考帧索引；c，poses_database中参考帧的位姿

SurfelMap::move_add_surfels()，对local_surfels的维护

因为系统中所使用的surfel，都存储在local_surfels中，而local_surfels仅包含局部的、在视野中的surfels。因此在移动过程中，需要对local_surfels进行维护。这一部分思路较为简单，但代码写的有点复杂。

在此我先贴出一些变量的注释：

struct PoseElement{
    //关键帧位姿元素,姿态数据库 poses_database 中的元素
    vector<SurfelElement> attached_surfels; //关联的surfels 
    geometry_msgs::Pose cam_pose; //位姿
    geometry_msgs::Pose loop_pose; //位姿，根据SLAM信息更改位姿时，暂时只更改loop_pose，然后与cam_pose对比.
    vector<int> linked_pose_index; //包含闭环信息和参考关系的关键帧索引
    int points_begin_index; // 由该 inactive 关键帧所产生的inactive surfel在inactive_pointcloud中的起始位置
    int points_pose_index;  // 该inactive关键帧索引 在 pointcloud_pose_index(inactive关键帧索引集合)中的位置
    ros::Time cam_stamp;
    PoseElement() : points_begin_index(-1), points_pose_index(-1) {}
};
class SurfelMap
{
private:
vector<SurfelElement> local_surfels; //active surfel集合
vector<PoseElement> poses_database;  //关键帧/位姿序列
std::set<int> local_surfels_indexs; // active关键帧索引集合
PointCloud::Ptr inactive_pointcloud;// inactive surfels集合（以点云形式存储） 
std::vector<int> pointcloud_pose_index; //inactive关键帧索引集合 

1，取得 active 关键帧索引poses_to_add、inactive关键帧索引 poses_to_remove

get_add_remove_poses(reference_index, poses_to_add, poses_to_remove);

2，从local_surfels中删去surfels，
a. 循环所有 local_surfels 中的surfel，如果surfels.last_update在poses_to_remove中存在，该surfel为inactive surfel
b. inactive_pointcloud 添如由 inactive surfel抽象成的点（继承位置和强度）
c. inactive surfel.update_times = 0 （从local_surfels中删除的标志）
d. pointcloud_pose_index 添加所有 inactive 关键帧索引
e. local_surfels_indexs 删除所有 inactive 关键帧索引
d. 更新poses_database[inactive_index].points_begin_index和poses_database[inactive_index].points_pose_index
e poses_database[inactive_index].attached_surfels 添加所有由他删除的surfels

3，向lcoal_surfels中添加surfels
a. local_surfels_indexs 添加这个re-acitve关键帧索引
b. inactive_pointcloud 删去所有re-active关键帧关联的点
pointcloud_pose_index 删去所有re-active关键帧的索引
其中保持对 所有inative关键帧 PoseElement.points_begin_index 和 PoseElement.points_pose_index 的更新
c. local_surfels 添加 re-acitve关键帧存储的surfels PoseElement.attached_surfels

SurfelMap::fuse_map()根据当前图像实现surfel的融合和创建

过程如下：

    // 功能:结合当前帧进行surfel融合与新建
    // 1 在当前帧上构建超像素 FusionFunctions.superpixel_seeds 及像素索引 FusionFunctions.superpixel_index
    // 2 surfel融合：将超像素信息融合到 local_surfels 中 
    // 3 surfel新建：将没有被融合的超像素创建为新的surfel,并加入 new_surfels 中
    fusion_functions.fuse_initialize_map(
        reference_index,
        image,
        depth,
        pose_input,
        local_surfels,
        new_surfels
    );
	//2 建立被剔除surfel集合 deleted_index 
	//删除标志是 surfel.update_times =0
    vector<int> deleted_index;
    for(int i = 0; i < local_surfels.size(); i++)
    {
        if(local_surfels[i].update_times == 0)
            deleted_index.push_back(i);
    }
	//3 将新建surfel集合 new_surfels 中的surfel 替换掉被剔除surfel 
    int add_surfel_num = 0;
    for(int i = 0; i < new_surfels.size(); i++)
    {
        if(new_surfels[i].update_times != 0)
        {
            SurfelElement this_surfel = new_surfels[i];
            if(deleted_index.size() > 0)
            {
                local_surfels[deleted_index.back()] = this_surfel;
                deleted_index.pop_back();
            }
            else
                local_surfels.push_back(this_surfel);
            add_surfel_num += 1;
        }
    }
	//4 从 local_surfels 中删除被剔除surfel 
    while(deleted_index.size() > 0)
    {
        local_surfels[deleted_index.back()] = local_surfels.back();
        deleted_index.pop_back();
        local_surfels.pop_back();
    }

SurfelMap::synchronize_msgs()的其他部分

//处理完成，待处理队列-1
pose_reference_buffer.pop_front();

//输出结果 
publish_pose_graph(fuse_stamp, relative_index);
publish_camera_position(fuse_stamp, fuse_pose_ros);
publish_inactive_pointcloud(fuse_stamp);
publish_active_pointcloud(fuse_stamp);
calculate_memory_usage();