在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 table的尺寸。
每一个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。
viz -d $(rospack find kimera_semantics_ros)/rviz/kimera_semantics_gtr.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。
接下来是一些三维重建的参数,其中一个的
<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更新。
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){
pointcloud_queue_.push(pointcloud_msg_in);
...
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){
...
ptcloud_timer.Stop();
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(){
LongIndexHashMapType<AlignedVector<size_t>>::type voxel_map;
LongIndexHashMapType<AlignedVector<size_t>>::type clear_map;
std::unique_ptr<ThreadSafeIndex> index_getter(
ThreadSafeIndexFactory::get(config_.integration_order_mode, points_C));
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;
GlobalIndex voxel_index =
getGridIndexFromPoint<GlobalIndex>(point_G, voxel_size_inv_);
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);
integrateRays(T_G_C, points_C, colors, config_.enable_anti_grazing, false,
voxel_map, clear_map);
进入integrateRays
函数
...
integration_threads.emplace_back(
&MergedTsdfIntegrator::integrateVoxels, this, T_G_C, points_C, colors,
enable_anti_grazing, clearing_ray, voxel_map, clear_map, i);
进入到integrateVoxels
函数
LongIndexHashMapType<AlignedVector<size_t>>::type::const_iterator it;
...
integrateVoxel(T_G_C, points_C, colors, enable_anti_grazing, clearing_ray, *it, voxel_map)
...
it++
进入integrateVoxel
函数
...
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
构造函数
const Ray unit_ray = (point_G - origin).normalized();
...
ray_end = point_G + unit_ray * truncation_distance;
ray_start = voxel_carving_enabled
? origin
: (point_G - unit_ray * truncation_distance);
...
setupRayCaster(end_scaled, start_scaled);
进入setupRayCaster
函数
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_ = AnyIndex(signum(ray_scaled.x()), signum(ray_scaled.y()),
signum(ray_scaled.z()));
...
一直返回到tsdf_integrator.cc
的MergedTsdfIntegrator::integrateVoxel
...
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);
GlobalIndex global_voxel_idx;
while (ray_caster.nextRayIndex(&global_voxel_idx)){
}
进入nextRayIndex
函数
if (current_step_++ > ray_length_in_steps_) {
return false;
}
...
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;
TsdfVoxel* voxel =
allocateStorageAndGetVoxelPtr(global_voxel_idx, &block, &block_idx);
updateTsdfVoxel(origin, merged_point_G, global_voxel_idx, merged_color,
merged_weight, voxel);
...
}
进入updateTsdfVoxel
函数
const Point voxel_center =
getCenterPointFromGridIndex(global_voxel_idx, voxel_size_);
const float sdf = computeDistance(origin, point_G, voxel_center);
...
const float new_weight = tsdf_voxel->weight + updated_weight;
const float new_sdf =
(sdf * updated_weight + tsdf_voxel->distance * tsdf_voxel->weight) /
new_weight;
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);
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
评论(7)
您还未登录,请登录后发表或查看评论