在tsdf的第一讲里我们讲解了最基本的TSDF重构算法。也提出了算法的一些不足之处。
1:体素的大小需要预先设定并固定。这意味着场景超出体素涵盖的范围时，将不能重建。
2:我们利用体素重构表面时，理论上需要选tsdf为0的点，这些点才在表面上。但这几乎是不能满足的，所以参考代码退而求其次，选取tsdf小于某个数值的体素。这种做法比较粗糙也会损失不少的精度。成熟的tsdf based 3d重建实际不会直接用点云常见。
针对第一个问题，目前用的普遍方案是voxel hashing[1]。论文不算难，也有相应的中文资料解读[2] 。不了解hasing的同学可以通过算法书或者leetcode的介绍了解[3]。我只简单聊一下。
voxel hashing实现了只在相机测量过的场景表面划分体素，大大节省了空间。基础单元仍然是voxel，一个包含了sdf，rgb值和weight。算法令8X8X8的voxel为一个voxel block。每一个voxel block有对应的x,y,z坐标，我们会把这个坐标通过下面的hash function 映射到一个hash值。
每一个voxel block都有一个hash值与其对应，方便快速查找/插入新的voxel block。
针对第二个问题，相对成熟的tsdf三维重建系统一般使用tsdf为中间变量，得到marching cube[4,5]进行三维重建。在上一讲我们讲过重建表面一般不直接用点云。我们可以使用以三角形为单位的mesh。我们以8个体素为单位构成一个简单的立方体
立方体的每一个顶点为一个体素。我们考量能构成一条边的两个体素的tsdf，如果两个体素的tsdf符号不同(假设小于0在图中标记为绿色，大于0标记为红色)，则我们利用的插值的方法在这两个体素之间插值一个tsdf刚好为0的体素。
可以看到，如果一个voxel的tsdf大于0，其他的小于0，我们就能构成一个三角形也就是最基本的mesh。将很多这样的mesh连接到一起，我们就能得到要重构的三维表面。因为每个顶点的tsdf都有大于0或者小于等于0两种情形，所以在这一个立方体中一共8个点有2^8次方种表面重构的可能。这点我们会在voxblox的代码中很清楚地看到。
下面我将带领大家读一遍voxblox代码[7]，深入了解tsdf的应用。
voxblox是CPU based的3维重构，可应用于大范围的3维重建(得牺牲精度)，也可以应用于小范围的3维重建。在上一讲中我们看到TSDF的更新计算需要很大的并行性能，因为我们需要对每一个体素在每一帧进入时进行更新，CPU是无法达到的，因此voxblox在这个方面做了些优化。voxblox是紧密和ROS结合在一起的应用，不了解ROS的同学可能需要去我的ROS讲解补补课了，就假设大家了解了ROS的基本应用。
我个人是使用Kimera Semantics[6]的时候开始了解voxblox的。Kimera Semantics[6]是进行语义3D重构的工具，它里面完整地包含了voxblox。当你不提供语义图片时，只进行普通3维重建时，它就等同于voxblox。所以我直接用Kimera Semantics里包含的voxblox进行讲解了。
voxblox结构
我们先来了解下voxblox的输入和输出
虚线框内部属于voxblox。从左边可以看到，voxblox需要传感器提供点云的输入，点云可以来自于单目相机加深度图片或者来自于双目相机等，另外需要提供当前传感器(相机)的位姿估计，传入之后会更新TSDF并更新ESDF，ESDF全称Euclidean Signed Distance Fields，直接给出传感器到障碍物的距离，用于路径规划。我们不会讲到。我们可以看到，TSDF更新的updated voxels会传给mesh integrator，之后会根据TSDF在我们之前介绍到的marching cubes算法中产生mesh，最后可视化出来。
讲解代码内容前，我们先安装Kimera Semantics代码[8]以及下载它的示例ros包kimera_semantics_demo.bag。在终端中运行下面的命令打开rviz。
rviz -d $(rospack find kimera_semantics_ros)/rviz/kimera_semantics_gt.rviz
再运行下面的命令
roslaunch kimera_semantics_ros kimera_semantics.launch play_bag:=true metric_semantic_reconstruction:=false
跑kimera_semantics_demo.bag。命令中我们有metric_semantic_reconstruction:=false特意关闭了语义重建部分，此时kimera会完全等同于voxblox。你应该在rviz中看到类似下面的输出
证明程序运行成功。
launch文件
我们进入launch文件kimera_semantics.launch。内容很多，抽重要的讲解。
1：设置voxel的尺寸以及相机的有效观察距离max_ray_length_m，即只会在距离相机max_ray_length_m范围重建
<arg name="voxel_size" default="0.05"/>
<arg name="max_ray_length_m" default="5"/>
2：把rosbag里的消息topic map到kimera semantics需要的topic
<remap from="/tesse/left_cam" to="/tesse/left_cam/image_raw"/>
<remap from="/tesse/segmentation" to="/tesse/segmentation/image_raw"/>
<remap from="/tesse/depth" to="/tesse/depth/image_raw"/>
3：把语义重建部分设置为false。我们已经在终端的运行命令中设置了，所以这里不用再设置了。
<arg name="metric_semantic_reconstruction" default="true"/>
4：不使用双目模式，使用单目+深度读得到的点云
<arg name="publish_point_clouds" default="true"/>
<arg name="run_stereo_dense" default="false"/>
这样程序会用单目+深度图估算出每一个像素点对应的3d点形成点云。
5：获取来自于单目+深度的点云
<group if="$(arg publish_point_clouds)">
<!-- Launch Nodelet manager: used by depth_image_proc and disparity_image_proc -->
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"
output="screen"/>
...
...
<!-- Converts registered depth image and RGB image into an RGB pointcloud.
Using depth and semantic image, it generates semantic pointcloud. We only
run this if we are not using stereo depth reconstruction instead. -->
<node pkg="nodelet" type="nodelet" name="cloudify"
args="load depth_image_proc/point_cloud_xyzrgb nodelet_manager
-no-bond" output="screen">
<!-- Input -->
<remap from="rgb/camera_info" to="$(arg left_cam_info_topic)"/>
<remap from="rgb/image_rect_color" to="$(arg left_cam_segmentation_topic)" if="$(arg metric_semantic_reconstruction)"/>
<remap from="rgb/image_rect_color" to="$(arg left_cam_topic)" unless="$(arg metric_semantic_reconstruction)"/>
<remap from="depth_registered/image_rect" to="$(arg left_cam_stereo_depth_topic)" if="$(arg run_stereo_dense)"/>
<remap from="depth_registered/image_rect" to="$(arg left_cam_depth_topic)" unless="$(arg run_stereo_dense)"/>
<!-- Output -->
<remap from="depth_registered/points" to="$(arg semantic_pointcloud)"/>
<!-- Params -->
<param name="queue_size" value="20"/>
</node>
</group>
这里使用了ros的nodelet。nodelet的作用是使多个运行在同一进程的算法之间传递数据的时候不拷贝数据，这点在对图像进行处理的时候比较重要。我们这儿不需要了解太多，只需要知道kimera smantics直接调用ros自带的把图像+深度转化为点云的包depth_image_proc/point_cloud_xyzrgb，这个包必须在nodelet这个package下运行才行。我们可以看到这个包的输入为rgb/camera_info，depth_registered/image_rect，rgb/image_rect_color，有了相机内参，深度图以及rgb或者灰度图，我们很容易得到图像对应的点云图，也就是输出depth_registered/points。这个输出会作为输入传递给我们上面图像voxblox_io.png的TSDF_Integrator。
接下来是一些三维重建的参数，其中一个的
<!-- Method to update voxels' information: "fast" or "merged" -->
<param name="method" value="fast" />
选择更新voxel的方法。voxblox提供merged,fast和普通方法。普通方法就是我们上一讲讲到的计算量巨大的voxel更新方法，fast最块，但是只适用于voxel_size小(小于等于0.05m)的小型室内场景，merged稍微慢一些但可适用于大型场景。我们会在后面讲解merged方法，两种方法其实在代码里都有详细的注释。
另一个参数
<param name="update_mesh_every_n_sec" value="0.1" />
更新mesh的速率，如果mesh很多，场景很大，可以选择慢一点更新。
voxblox 代码
在kimera semantics成功安装后，我们应该能看到在它的ros工作空间下有voxblox，我们进入voxblox_ros下的tsdf_server.cc。
在TsdfServer的构造函数中，有几行比较重要的和刚才的launch文件密切相关
pointcloud_sub_ = nh_.subscribe("pointcloud", pointcloud_queue_size_,
&TsdfServer::insertPointcloud, this);
这个subcriber会订阅我们在launch文件中的点输出depth_registered/points。接收到点云后，在insertPointcloud中进行tsdf更新。
// If set, use a timer to progressively integrate the mesh.
double update_mesh_every_n_sec = 1.0;
nh_private_.param("update_mesh_every_n_sec", update_mesh_every_n_sec,
update_mesh_every_n_sec);
if (update_mesh_every_n_sec > 0.0) {
update_mesh_timer_ =
nh_private_.createTimer(ros::Duration(update_mesh_every_n_sec),
&TsdfServer::updateMeshEvent, this);
}
设置更新mesh的频率，在updateMeshEvent中运行。它与insertPointCloud独立运行。
insertPointCloud函数负责voxblox_io.png中的TSDF Integrator部分，而updateMeshEvent函数负责Mesh Integrator部分。相当于两个线程。下面我们分别讲解两个部分
TsdfIntegrator
进入insertPointCloud函数
void TsdfServer::insertPointcloud(
const sensor_msgs::PointCloud2::Ptr& pointcloud_msg_in){
//把点云push到一个队列里，这样点云如果积压来不及处理也可以慢慢一条条处理
pointcloud_queue_.push(pointcloud_msg_in);
...
//getNextPointcloudFromQueue获取queue里点云所在的相机frame的位姿以及对应的点云
while (
getNextPointcloudFromQueue(&pointcloud_queue_, &pointcloud_msg, &T_G_C)) {
constexpr bool is_freespace_pointcloud = false;
processPointCloudMessageAndInsert(pointcloud_msg, T_G_C,
is_freespace_pointcloud);
processed_any = true;
进入processPointCloudMessageAndInsert函数
void TsdfServer::processPointCloudMessageAndInsert(
const sensor_msgs::PointCloud2::Ptr& pointcloud_msg,
const Transformation& T_G_C, const bool is_freespace_pointcloud){
...
//在这行代码之前的内容只是把ros的点云message转换为voxblox更适用的点云形式，其实是转换为Eigen<double,3,1>的向量
ptcloud_timer.Stop();
//如果我们认为自己提供的位姿估计不够准确，需要再修正一下的话我们在launch文件里可以enable_icp，
//但这会消耗不好计算资源，所以我们disable不用管这里面的内容了
if (enable_icp_){
...
}
...
integratePointcloud(T_G_C_refined, points_C, colors, is_freespace_pointcloud);
}
进入integratePointcloud函数
void TsdfServer::integratePointcloud(const Transformation& T_G_C,
const Pointcloud& ptcloud_C,
const Colors& colors,
const bool is_freespace_pointcloud) {
CHECK_EQ(ptcloud_C.size(), colors.size());
tsdf_integrator_->integratePointCloud(T_G_C, ptcloud_C, colors,
is_freespace_pointcloud);
}
进入tsdf_integrator.cc里的integratePointCloud函数。我们可以看到在tsdf_integrator.cc里有三个integratePointCloud函数。分别是
void MergedTsdfIntegrator::integratePointCloud()
void FastTsdfIntegrator::integratePointCloud()
void SimpleTsdfIntegrator::integratePointCloud()
分别对应着我们前面简要提到的voxblox提供的三种tsdf的积分方法。如果我们一开始在launch文件里选择的是merged方法，那么就会进入第一个点云积分方法。
在第一讲里面我们也提到过tsdf的获取方法分为两种。这儿的"三"和"两"不能搞混。两种获取tsdf的方法讲的是如何把传感器的信息融合到tsdf里面去。第一种方法我们在第一讲已经讲到过，是通过把所有voxel投影到当前相机的坐标系下，比较图像3d点的z和深度voxel的z的差距获得tsdf。而第二种方法，是voxblox采用的方法，它会从相机的中心（optical center）投射一条射线(cast a ray)到所有图像2d点投影的3d点，从相机中心到这些点之后的(距离相机更远的)，距离小于截断距离的voxel都会被更新。
针对第二种融合方法，voxblox再提出了两种加速的方法即。其一就是Fast其二就是Merged。普通方法对每一个图像点都投影射线，计算太慢，Merged方法先计算哪些3d点投影到了相同的voxel里，然后取这些点的坐标平均数，视为一个点，只从那个平均点投影射线，利用上面的方法二更新。可以想象，如果voxel_size设置得大，会更容易有更多点投影到一个voxel里被平均，那么它的速度提升会更明显。只是在大尺寸3d重建的时候推荐merged方法的原因之一。Fast方法的详细注释在tsdf_integrator.h里，这里不多讲解。下面我们进入void MergedTsdfIntegrator::integratePointCloud()函数看看是上述过程是怎么实现的。
void MergedTsdfIntegrator::integratePointCloud(){
// Pre-compute a list of unique voxels to end on.
// Create a hashmap: VOXEL INDEX -> index in original cloud.
LongIndexHashMapType<AlignedVector<size_t>>::type voxel_map;
// This is a hash map (same as above) to all the indices that need to be
// cleared.
LongIndexHashMapType<AlignedVector<size_t>>::type clear_map;
std::unique_ptr<ThreadSafeIndex> index_getter(
ThreadSafeIndexFactory::get(config_.integration_order_mode, points_C));
//这个函数用来计算哪些点会被投影到一个voxel里
bundleRays(T_G_C, points_C, freespace_points, index_getter.get(), &voxel_map,
&clear_map);
进入bundleRays()函数
while (index_getter->getNextIndex(&point_idx)) {
...
//获得世界坐标系下的点
const Point point_G = T_G_C * point_C;
//getGridIndexFromPoint会计算世界坐标系下的点在哪个voxel里，其实原理很简单，假设第voxel_size=0.06，那么从原点开始的第一个voxel就包含了0.06X0.06X0.06这么大的空间。
//如果我们有一个点坐标是(0.01,0.01,0.01)，那么我们可以肯定它在这个voxel之内。数学表达为
//voxel_id_x = p.x / voxel_size = p.x * voxel_size_inv
//voxel_id_y = p.y / voxel_size = p.y * voxel_size_inv
//voxel_id_z = p.z / voxel_size = p.z * voxel_size_inv
GlobalIndex voxel_index =
getGridIndexFromPoint<GlobalIndex>(point_G, voxel_size_inv_);
//有些不valid的点是需要清除的(距离相机太近或太远之类，见IsValid函数)除此之外，
//那些投影到一个voxel_index下的不同poin_idx都会被push到voxel_map里。
if (is_clearing) {
(*clear_map)[voxel_index].push_back(point_idx);
} else {
(*voxel_map)[voxel_index].push_back(point_idx);
}
}
回到MergedTsdfIntegrator::integratePointCloud函数
...
bundleRays(T_G_C, points_C, freespace_points, index_getter.get(), &voxel_map,
&clear_map);
//在我们确定了哪些poins属于想同的voxel_idx之后，就可以把那些points作为同一个点去更新voxel的tsdf了。
integrateRays(T_G_C, points_C, colors, config_.enable_anti_grazing, false,
voxel_map, clear_map);
进入integrateRays函数
...
//多线程并行调用integrateVoxels函数
integration_threads.emplace_back(
&MergedTsdfIntegrator::integrateVoxels, this, T_G_C, points_C, colors,
enable_anti_grazing, clearing_ray, voxel_map, clear_map, i);
进入到integrateVoxels函数
//这个迭代器it会挨个迭代每个voxel_map里的元素，记住我们的voxel_map[i]本身也是一个vector，它储存了属于同一个voxel的点
LongIndexHashMapType<AlignedVector<size_t>>::type::const_iterator it;
...
//*it代表属于同一个voxel的点合成的向量，integrateVoxel会把*it里的点的位置/颜色求平均。
integrateVoxel(T_G_C, points_C, colors, enable_anti_grazing, clearing_ray, *it, voxel_map)
...
it++
进入integrateVoxel函数
...
//求得属于一个voxel内的点的平均点和颜色
for (const size_t pt_idx : kv.second) {
...
merged_point_C = (merged_point_C * merged_weight + point_C * point_weight) /
(merged_weight + point_weight);
merged_color =
Color::blendTwoColors(merged_color, merged_weight, color, point_weight);
merged_weight += point_weight;
...
}
...
//利用平均点投影射线
RayCaster ray_caster(origin, merged_point_G, clearing_ray,
config_.voxel_carving_enabled, config_.max_ray_length_m,
voxel_size_inv_, config_.default_truncation_distance);
进入位于integrator_utils.cc的RayCaster构造函数
//计算单位射线的长度，就是简单的求点到原点的差值，求得他们的norm(均方值)再每一个维度除以norm归一化。
const Ray unit_ray = (point_G - origin).normalized();
...
//射线的开始点为原点(voxel_carving_enabled==true)，终止点为平均点后unit_ray * truncation_distance的位置。
//正如我们在前面讲算法时提到的，要计算从原点要当前被观察到的点后距离为truncation_distance的所有voxel
ray_end = point_G + unit_ray * truncation_distance;
ray_start = voxel_carving_enabled
? origin
: (point_G - unit_ray * truncation_distance);
...
//cast_from_origin == false
setupRayCaster(end_scaled, start_scaled);
进入setupRayCaster函数
//首先获取射线开始位置和结束位置对应的voxel的三个方向的index的坐标以及差值
curr_index_ = getGridIndexFromPoint<GlobalIndex>(start_scaled);
const GlobalIndex end_index = getGridIndexFromPoint<GlobalIndex>(end_scaled);
const GlobalIndex diff_index = end_index - curr_index_;
//ray_step_signs_很有意思，这个值的每个维度(x,y,z)只可能为3个值, 1,-1或者0. 我们会在后面看到它的用处
ray_step_signs_ = AnyIndex(signum(ray_scaled.x()), signum(ray_scaled.y()),
signum(ray_scaled.z()));
//后面处理一些边界问题，暂时不讲
...
一直返回到tsdf_integrator.cc的MergedTsdfIntegrator::integrateVoxel
...
//上面RayCast构造函数设置了射线的一些性质，比如它的长度等
RayCaster ray_caster(origin, merged_point_G, clearing_ray,
config_.voxel_carving_enabled, config_.max_ray_length_m,
voxel_size_inv_, config_.default_truncation_distance);
// 进入while循环，循环里会遍历射线上从原点开始到平均点之后的`unit_ray * truncation_distance`距离内的所有voxel
GlobalIndex global_voxel_idx;
while (ray_caster.nextRayIndex(&global_voxel_idx)){
}
进入nextRayIndex函数
if (current_step_++ > ray_length_in_steps_) {
return false;
}
//最关键是下面这行，我们之前说过ray_step_signs_只为0,-1或者1。所以curr_index其实就会加减1或者不动，那其实每调用一次这个函数，voxel三个维度的坐标就会加减1或者不动。一直到遍历的step>ray_length_in_steps_为止。这就是遍历射线上所有射线的方法
...
curr_index_[t_min_idx] += ray_step_signs_[t_min_idx];
...
回到MergedTsdfIntegrator::integrateVoxel的while循环
GlobalIndex global_voxel_idx;
while (ray_caster.nextRayIndex(&global_voxel_idx)){
...
Block<TsdfVoxel>::Ptr block = nullptr;
BlockIndex block_idx;
//文章最开始有介绍，我们是有8X8X8的voxel组成一个block。只有在block里的voxel才会被分配空间真正储存起来(有tsdf值，rgb值等)。下面这行函数就是检测该voxel是否已经属于某个block还是并不属于任何一个block，如果不，则新建一个block分配空间。返回在这个block里的对应index的voxel
TsdfVoxel* voxel =
allocateStorageAndGetVoxelPtr(global_voxel_idx, &block, &block_idx);
//终于，做了那么多准备，更新tsdf
updateTsdfVoxel(origin, merged_point_G, global_voxel_idx, merged_color,
merged_weight, voxel);
...
}
进入updateTsdfVoxel函数
//计算voxel中心的x,y,z坐标
const Point voxel_center =
getCenterPointFromGridIndex(global_voxel_idx, voxel_size_);
//计算voxel中心到原点的直线距离，计算观察到的平均点到原点的直线距离。原点到voxel的射线和平均点到原点的射线一般不会完全共线，因此在这个函数里会voxel的射线投影到平均点的射线上，再做个减法获得sdf。voxblox论文公式1
const float sdf = computeDistance(origin, point_G, voxel_center);
//更新voxel的权重，在线面这行代码之前，其实有一定的计算过程。我们在第一讲里有说到每一帧的每个voxel的权重更新都是简单的加1.voxblox提出了根据截断距离的大小来动态改变权重的方法，可参见论文公式2. updated_weight不再只是1，不过最后的更新的方法不变，加到原本的tsdf_voxel->weight上去
...
const float new_weight = tsdf_voxel->weight + updated_weight;
//论文公式3，更新sdf
const float new_sdf =
(sdf * updated_weight + tsdf_voxel->distance * tsdf_voxel->weight) /
new_weight;
//接下来更新rgb颜色(如果有的话)，并对sdf进行截断得到tsdf
// color blending is expensive only do it close to the surface
if (std::abs(sdf) < config_.default_truncation_distance) {
tsdf_voxel->color = Color::blendTwoColors(
tsdf_voxel->color, tsdf_voxel->weight, color, updated_weight);
}
//截断
tsdf_voxel->distance =
(new_sdf > 0.0) ? std::min(config_.default_truncation_distance, new_sdf)
: std::max(-config_.default_truncation_distance, new_sdf);
//论文公式4.
tsdf_voxel->weight = std::min(config_.max_weight, new_weight);
至此我们终于完成了一个voxel的更新。论文算法描述起来貌似很容易，但是代码实现还是满复杂的，简单回顾一下这个过程
insertPointCloud接收来自相机图像+深度构成的点云数据 ->MergedTsdfIntegrator::integratePointCloud点云与已有的voxel进行融合，获取哪些点属于同一个voxel,求这些点的平均(bundleRays)，再对原点到平均点后的truncate_distance范围内的voxel的tsdf等进行更新(integrateRays)。
有了tsdf的更新，我们在接下来一讲看看voxblox如何把这些voxel的tsdf应用于3维表面的重建上。
参考(参考可能需要科学上网):
[1] Real-time 3D Reconstruction at Scale using Voxel Hashing
[2] voxel hashing 解读
[3] hash table
[4] MARCHING CUBES: A HIGH RESOLUTION 3D SURFACE CONSTRUCTION ALGORITHM
[5] 从TSDF到Marching Cubes的mesh构建
[6] kimera semantics
[7] voxblox github
[8] kimera semantics github
