voxblox结构图
voxblox_io.png
上一讲我们说过
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.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.png
一个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即连接插值点得到的表面。
isosurface.jpg
接着浏览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.cc的setMessage函数里，定义了接收到的消息要如何可视化。其中比较重要的部分
// 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
