前言

本节对可视化部分进行理解。mesh的可视化使用的是mesh_tools中提供的rviz的插件。代码中已经完美的实现了mesh_msgs::MeshGeometryStamped消息的定义与赋值，但是mesh_tools还包含了很多其他的消息类型，提供了更加丰富的可视化信息，详情可参考“The Mesh Tools Package Introducing Annotated 3D Triangle Maps in ROS”这篇论文。但是可惜的是我没有找到如何对其他类型的消息进行赋值的example，尽管文章中给出了一定的解释，但是代码实现上还是需要摸索下。我们想在SLAMesh已有的可视化中增加颜色信息，遇到了一些问题并解决。

SLAMesh可视化

从代码中可以清晰的看到如何向mesh_msgs::MeshGeometryStamped添加顶点和face。

void  Map::filterMeshGlb(){
    //mesh_msg global scan by direct connectin, visualized by mesh_tool, need to traverse all cell in map
    ROS_DEBUG("filterMeshGlb");
    double variance_point = -1;
    double variance_face_show = param.variance_map_show;
    int n_row(param.num_test), n_col(param.num_test);
    double grid = param.grid;
    int min_show_updated_times = 1;
    double max_show_distance = 70;
    //init msg
    mesh_msgs::MeshGeometryStamped tmp_mesh;
    mesh_msgs::MeshVertexColorsStamped tmp_mesh_color;
    tmp_mesh.header.frame_id = "map";
    tmp_mesh.header.stamp = ros::Time::now();
    tmp_mesh.mesh_geometry.vertices.clear();
    std_msgs::ColorRGBA c;
    tmp_mesh_color.header.frame_id =  "map";
    tmp_mesh_color.header.stamp = tmp_mesh.header.stamp;
    tmp_mesh_color.mesh_vertex_colors.vertex_colors.clear();
    TicToc t_connect_push("t_connect_push");
    for (auto & i_cell : cells_glb){
        // in each cell, dir, build mesh_msg
        if(i_cell.second.average_viewed_distance > max_show_distance ){
            continue;
        }
        for(int dir = 0; dir < 3; dir ++){
            if(i_cell.second.updated_times[dir] < min_show_updated_times){
                continue;
            }
            PointMatrix & vertices = i_cell.second.ary_cell_vertices[dir];
            if(vertices.num_point == 0){
                continue;
            }
            //inside mesh_msg
            int start_vertex_i = tmp_mesh.mesh_geometry.vertices.size();
            for(int vertex_i = 0; vertex_i < vertices.num_point ; vertex_i ++){
                geometry_msgs::Point tmp_point;
                tmp_point.x =  vertices.point(0, vertex_i);
                tmp_point.y =  vertices.point(1, vertex_i);
                tmp_point.z =  vertices.point(2, vertex_i);
                tmp_mesh.mesh_geometry.vertices.push_back(tmp_point);
                //添加vertex的颜色信息，与vertex一一对应
                c.r = 255;
                c.b = 0;
                c.g = 0;
                c.r = 0.5;
                tmp_mesh_color.mesh_vertex_colors.vertex_colors.push_back(c);
            }
            for(int vertex_i = 0; vertex_i < vertices.num_point; vertex_i++){
                int ix_cell = vertex_i % (n_row);
                int iy_cell = vertex_i / (n_row);
                mesh_msgs::MeshTriangleIndices tmp_face;
                
                if(ix_cell + 1 < n_row && iy_cell - 1 >= 0){
                    double variance_face_i = (vertices.variance(0, vertex_i) +
                                              vertices.variance(0, vertex_i + 1) +
                                              vertices.variance(0, vertex_i + 1 - n_row)) / 3.0;
                    if(variance_face_i < variance_face_show){
                        tmp_face.vertex_indices[0] = start_vertex_i + vertex_i;
                        tmp_face.vertex_indices[1] = start_vertex_i + vertex_i + 1;
                        tmp_face.vertex_indices[2] = start_vertex_i + vertex_i + 1 - n_row;
                        tmp_mesh.mesh_geometry.faces.push_back(tmp_face);
                        
                    }
                }
                if(ix_cell + 1 < n_row && iy_cell + 1 < n_row ){
                    double variance_face_i = (vertices.variance(0, vertex_i) +
                                              vertices.variance(0, vertex_i + 1) +
                                              vertices.variance(0, vertex_i + n_row)) / 3.0;
                    if(variance_face_i < variance_face_show){
                        tmp_face.vertex_indices[0] = start_vertex_i + vertex_i;
                        tmp_face.vertex_indices[1] = start_vertex_i + vertex_i + 1;
                        tmp_face.vertex_indices[2] = start_vertex_i + vertex_i + n_row;
                        tmp_mesh.mesh_geometry.faces.push_back(tmp_face);
                    }
                }
            }
        }
    }
    std::cout<<"Gloabl mesh_msg: point " << tmp_mesh.mesh_geometry.vertices.size() 
    <<" "<<tmp_mesh_color.mesh_vertex_colors.vertex_colors.size()<< " faces: " << tmp_mesh.mesh_geometry.faces.size() <<std::endl;
    //std::cout<<"global uuid: "<<tmp_mesh.uuid<<" "<<tmp_mesh_color.uuid <<std::endl;
    std::string s = "global";
    tmp_mesh_color.uuid = s.c_str();
    //std::cout<<"global uuid related: "<<tmp_mesh.uuid<<" "<<tmp_mesh_color.uuid <<std::endl;
    mesh_msg = tmp_mesh;
    vertex_color_msg = tmp_mesh_color;
}

上述代码中我们增加了一个新的消息类型，mesh_msgs::MeshVertexColorsStamped，该消息用于保存顶点对应的颜色信息，其是通过uuid变量与mesh进行关联的。这里我们设置所有的顶点的颜色都是红色
下面是对两个消息的发布

if(option == 3){
            // mesh_tool total map
            map_glb.filterMeshGlb();
            //std::cout<<"uuid: "<<map_glb.mesh_msg.uuid<<" "<<map_glb.vertex_color_msg.uuid<<std::endl;
            //std::cout<<"msg size "<<map_glb.mesh_msg.mesh_geometry.vertices.size()<<" "<<map_glb.vertex_color_msg.mesh_vertex_colors.vertex_colors.size()<<std::endl;
            map_glb.vertex_color_msg.uuid = map_glb.mesh_msg.uuid;
            mesh_pub.publish(map_glb.mesh_msg);
            ros::Duration(0.01).sleep();
            mesh_color_pub.publish(map_glb.vertex_color_msg);
        }

一开始我们没有添加10ms的延时，导致在可视化的时候出现报错：uuid 不匹配，以及 vertex的数量与color的数量不一致两个错误，具体错误来源可以查看mesh_tools中的Mesh_display.cpp
分析原因是topic消息发布的时候两个消息没有严格的对应起来，因此，增加一个延时，问题解决了，出现了一个红色的mesh地图.

另外在rviz中需要设置mesh_map的Display_size为Vertex_Color,以及相应的订阅的topic.

接下来我们将探究更多其他可视化的消息类型.

思考

rviz的插件确实好用.
看到有大神跟作者互动,提供了一个激光去畸变的SLAMesh版本,指出畸变对SLAM系统的影响较大.深以为然,不过作者目前也在做增加IMU融合的事情,也是水到渠成,顺理成章.
最近看到了一个产品VIOBOT, 觉得很不错.