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

voxblox结构图  
 

  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.h的generateMesh函数  

...
//返回所有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.h的MarchingCubes::meshCube，函数有重载，进入传入4个参数的meshCube。这里我们在延伸一下理论部分。如图  
  一个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];

    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就建立完毕
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);

    // 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会自动插值来决定三角面片的颜色