应用于三维重建的TSDF(三)voxblox代码解析

188
0
2020年10月29日 09时23分

voxblox结构图

 

1

 

上一讲我们说过insertPointCloud函数负责voxblox_io.png中的TSDF Integrator部分,而updateMeshEvent函数负责Mesh Integrator部分。这一讲我们就讲updateMeshEvent如何更新mesh可视化的。
TsdfServer::TsdfServer构造函数里设置好mesh更新频率之后,updateMeshEvent函数会按照这个频率运行。

  if (update_mesh_every_n_sec > 0.0) {
    update_mesh_timer_ =
        nh_private_.createTimer(ros::Duration(update_mesh_every_n_sec),
                                &TsdfServer::updateMeshEvent, this);
  }

  double publish_map_every_n_sec = 1.0;
  nh_private_.param("publish_map_every_n_sec", publish_map_every_n_sec,
                    publish_map_every_n_sec);

 

进入updateMeshEvent

 

void TsdfServer::updateMeshEvent(const ros::TimerEvent& /*event*/) {
  updateMesh();
}

 

 

进入updateMesh

 

...
  constexpr bool only_mesh_updated_blocks = true;
  constexpr bool clear_updated_flag = true;
  mesh_integrator_->generateMesh(only_mesh_updated_blocks, clear_updated_flag);

 

 

进入mesh_integrator.hgenerateMesh函数

 

...
//返回所有voxel有更新的block的index
    if (only_mesh_updated_blocks) {
      sdf_layer_const_->getAllUpdatedBlocks(Update::kMesh, &all_tsdf_blocks);
    }
//mesh和block有对应关系,如果有新建的block而没有对应的mesh,则为mesh分配新的空间。
    // Allocate all the mesh memory
    for (const BlockIndex& block_index : all_tsdf_blocks) {
      mesh_layer_->allocateMeshPtrByIndex(block_index);
    }
...
//多线程运行generateMeshBlocksFunction函数
    std::list<std::thread> integration_threads;
    for (size_t i = 0; i < config_.integrator_threads; ++i) {
      integration_threads.emplace_back(
          &MeshIntegrator::generateMeshBlocksFunction, this, all_tsdf_blocks,
          clear_updated_flag, index_getter.get());
    }

 

进入generateMeshBlocksFunction函数

 

//每个线程要遍历`all_tsdf_blocks`里的部分block
while (index_getter->getNextIndex(&list_idx)){
      const BlockIndex& block_idx = all_tsdf_blocks[list_idx];
      updateMeshForBlock(block_idx);
}

 

 

进入updateMeshForBlock函数,针对某个的block_id更新mesh

 

//根据已建立的mesh和block的对应关系,找到各自的指针
    Mesh::Ptr mesh = mesh_layer_->getMeshPtrByIndex(block_index);
    mesh->clear();

    typename Block<VoxelType>::ConstPtr block =
        sdf_layer_const_->getBlockPtrByIndex(block_index);

extractBlockMesh(block, mesh);

 

 

进入extractBlockMesh

 

//对block里的每一个voxel进行操作。
    IndexElement vps = block->voxels_per_side();
    VertexIndex next_mesh_index = 0;

    VoxelIndex voxel_index;
    for (voxel_index.x() = 0; voxel_index.x() < vps - 1; ++voxel_index.x()) {
      for (voxel_index.y() = 0; voxel_index.y() < vps - 1; ++voxel_index.y()) {
        for (voxel_index.z() = 0; voxel_index.z() < vps - 1;
             ++voxel_index.z()) {
//获取block里每一个voxel的x,y,z坐标
          Point coords = block->computeCoordinatesFromVoxelIndex(voxel_index);
          extractMeshInsideBlock(*block, voxel_index, coords, &next_mesh_index,
                                 mesh.get());
        }
      }
    }

 

 

进入extractMeshInsideBlock函数

 

//这里开始涉及到我们上一讲的marching cubes了。设立了一个立方体8个顶点,每个顶点有x,y,z坐标值,所以有<FloatingPoint, 3, 8
//每一个顶点对应一个体素,每个体素内储存着一个tsdf所以有<FloatingPoint, 8, 1
    Eigen::Matrix<FloatingPoint, 3, 8> cube_coord_offsets =
        cube_index_offsets_.cast<FloatingPoint>() * voxel_size_;
    Eigen::Matrix<FloatingPoint, 3, 8> corner_coords;
    Eigen::Matrix<FloatingPoint, 8, 1> corner_sdf;
//获取立方体8个体素的坐标以及tsdf
    for (unsigned int i = 0; i < 8; ++i) {
      VoxelIndex corner_index = index + cube_index_offsets_.col(i);
      const VoxelType& voxel = block.getVoxelByVoxelIndex(corner_index);

      if (!utils::getSdfIfValid(voxel, config_.min_weight, &(corner_sdf(i)))) {
        all_neighbors_observed = false;
        break;
      }

      corner_coords.col(i) = coords + cube_coord_offsets.col(i);
    }
//立方体的8个点都观测到我们才进行marching cube的建立
    if (all_neighbors_observed) {
      MarchingCubes::meshCube(corner_coords, corner_sdf, next_mesh_index, mesh);
    }

 

 

进入位于marching_cube.hMarchingCubes::meshCube,函数有重载,进入传入4个参数的meshCube。这里我们在延伸一下理论部分。如图

 

2

 

一个marching cube的顶点标号是按照上面的顺序,每两个相邻顶点构成一条边,也是按照途中的顺序标号。满足相邻两个顶点tsdf异号的条件后,我们将尝试在边上插入一个点,作为tsdf为0的点。

 

 

//根据8个顶点的sdf,获得一个8位的int常量index,该量上的每一位代表tsdf的正负,如果为正则那一位为1,否则为0
const int index = calculateVertexConfiguration(vertex_sdf);
...
//对12条边进行插值。有符号变化的两个相连的顶点之间就会被插值
    Eigen::Matrix<FloatingPoint, 3, 12> edge_vertex_coordinates;
    interpolateEdgeVertices(vertex_coords, vertex_sdf,
                            &edge_vertex_coordinates);
//根据每个顶点的tsdf的正负获得的index,传入kTriangleTable里,这样我们就知道需要在哪些边上插值。
//打开kTriangleTable你会看到他是256*16的变量。正如我们上一讲讲到的256个cube里插值的可能性
const int* table_row = kTriangleTable[index];

 

 

我们以kTriangleTable[0]和kTriangleTable[8]举例。当index=0时,意味着所有tsdf为负,那么没有任何一条边有插值的必要。如果index等于8(即0001000),意味着顶点3为正,其他为负,那么我们需要在边3,11,2上进行插值,所以kTriangleTable[8]{3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},。为-1的元素会在后面被忽略。那么插值得到的表面就应该如下图。图中isosurface即连接插值点得到的表面。

3
接着浏览meshCube里的代码
    const int* table_row = kTriangleTable[index];
//while循环结束的条件就是遇到table_row[table_col] == -1
    int table_col = 0;
    while (table_row[table_col] != -1) {
       //前面interpolateEdgeVertices已经计算好了哪些边有插值点哪些边没有.
//这里我们只需要根据table_row[table_col]选出是哪几条边插值了顶点. push到mesh里。我们就可以根据那几个点建立一个tsdf为0的面了。
      mesh->vertices.emplace_back(
          edge_vertex_coordinates.col(table_row[table_col + 2]));
      mesh->vertices.emplace_back(
          edge_vertex_coordinates.col(table_row[table_col + 1]));
      mesh->vertices.emplace_back(
          edge_vertex_coordinates.col(table_row[table_col]));
      mesh->indices.push_back(*next_index);
      mesh->indices.push_back((*next_index) + 1);
      mesh->indices.push_back((*next_index) + 2);
      const Point& p0 = mesh->vertices[*next_index];
      const Point& p1 = mesh->vertices[*next_index + 1];
      const Point& p2 = mesh->vertices[*next_index + 2];
...

 

至此,marching cube是怎么建立的就讲完了。简要来讲就是在获取了哪些block里的voxel更新了之后,取每个voxel以及它周围的能形成一个立方体的voxel的tsdf,对相邻的tsdf有符号变化的点进行插值,连接插值点可以得到tsdf为0的点构成的表面。
一步步可以退回到updateMesh函数。

 

//完成这一行后,我们上面的marching_cube就建立完毕
mesh_integrator_->generateMesh(only_mesh_updated_blocks, clear_updated_flag);
...
  voxblox_msgs::Mesh mesh_msg;
//把我们得到的marching cube插值得到的表面转化为ros message,发布,可视化
  generateVoxbloxMeshMsg(mesh_layer_, color_mode_, &mesh_msg);
  mesh_msg.header.frame_id = world_frame_;
  mesh_pub_.publish(mesh_msg);

 

其实generateMesh()之后理论部分就已经结束了。接下来只需要把所有插值得到的表面连接起来就可以得到最终rviz上的可视化结果了。
但是后面的代码难度其实不低。因为rviz并不自带voxblox这种mesh的可视化插件,所以voxblox不仅需要自定义mesh的消息类型,还需要自定义rviz的插件,如何可视化这类自定义的消息,我并没有写rviz插件的经历,所以特地去学习了一下。发现里面水还挺深的。
下面部分只属于bonus,简要介绍,学习voxblox的原理到这儿就可以了
rviz的可视化代码都是基于名叫Ogre的开源3d可视化平台[1]的。所以要自己写接收到消息后如何可视化,就得从基本的ogre入手。自定义rviz插件的基本教程在网上也就只有参考[2]这一个,voxblox也是参考它的结构来的。
voxblox_mesh_visual.ccsetMessage函数里,定义了接收到的消息要如何可视化。其中比较重要的部分

 

    // connect mesh把所有mesh连起来
    voxblox::Mesh connected_mesh;
    voxblox::createConnectedMesh(mesh, &connected_mesh);
    // create ogre object 。rviz会根据ogre object的设置来决定如何可视化
    Ogre::ManualObject* ogre_object;
...
//定义ogre要绘制的是一系列三角形面`OT_TRIANGLE_LIST`. `BaseWhiteNoLighting`为三角形可选择的表面打光的方式
//可以选择的方式请自行去ogre官网查看
    ogre_object->begin("BaseWhiteNoLighting",
                       Ogre::RenderOperation::OT_TRIANGLE_LIST);
//在选择了要绘制以三角形为基础的面片之后,就进入for循环,往ogre_object里push数据
    for (size_t i = 0; i < connected_mesh.vertices.size(); ++i) {
      // note calling position changes what vertex the color and normal calls
      // point to
     //由于设置的是绘制三角形,所以这个循环每走三次,push进去三个点,ogre就会自动连接这三个点
      ogre_object->position(connected_mesh.vertices[i].x(),
                            connected_mesh.vertices[i].y(),
                            connected_mesh.vertices[i].z());
...
//之后还需要push每个点的颜色,normal等,ogre会自动插值来决定三角面片的颜色

 

在for循环结束后,基本ogre就知道要如何绘制出图像了。你如果修改程序看看,只让for循环走三次,那么rviz上出现的就是一些小三角形片。

我对ogre也只是粗略地了解了一下,如有错误还请指出。

对TSDF系列的讲解到此结束,写这么多既是想惠及以后要学习的同学,也是整理自己的读代码笔记,让自己回头可以查看。想要讨论的同学欢迎私信。

参考(参考可能需要科学上网)
[1]Ogre
[2]rviz_plugin_tutorial

 

发表评论

后才能评论