C-Blox代码阅读
1.play_rosbag.launch
<node name="cblox" pkg="cblox_ros" type="tsdf_submap_server" output="screen" args="-alsologtostderr" clear_params="true">
其中node的关键信息type对应的是**“必须有一个具有相同名称的相应可执行文件”**,那么我们转到这个文件
2.tsdf_submap_server_node.cc
#include <glog/logging.h>
#include <ros/ros.h>
#include <cblox_ros/submap_server.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "cblox");
google::InitGoogleLogging(argv[0]);//使用glog之前必须先初始化库，要生成日志文件只需在开始log之前调用一次：
google::ParseCommandLineFlags(&argc, &argv, false);// 初始化 gflag
google::InstallFailureSignalHandler();//默认捕捉 SIGSEGV
ros::NodeHandle nh;
ros::NodeHandle nh_private("~");
cblox::SubmapServer<cblox::TsdfSubmap> node(nh, nh_private);// 关键语句，表明ros会捕捉并执行TsdfSubmap
ros::MultiThreadedSpinner spinner;
spinner.spin();
return 0;
}
3.tsdf_submap.h
// Makes a submap by SHARING the underlying TsdfMap with another party.
// 通过与另一方共享底层TsdfMap来创建子映射。
TsdfSubmap(const Transformation& T_M_S, const SubmapID submap_id,
TsdfMap::Ptr tsdf_map_ptr): Submap(T_M_S, submap_id), tsdf_map_(tsdf_map_ptr) {
CHECK(tsdf_map_ptr);
// I think uniform initialization wont work until c++14 apparently. Lame.
config_.tsdf_voxel_size = tsdf_map_ptr->getTsdfLayer().voxel_size();
config_.tsdf_voxels_per_side = tsdf_map_ptr->getTsdfLayer().voxels_per_side();
}
3.1 Tsdf_map.h
struct Config {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
FloatingPoint tsdf_voxel_size = 0.2;
size_t tsdf_voxels_per_side = 16u;
std::string print() const;
};
explicit TsdfMap(const Config& config):tsdf_layer_(new Layer<TsdfVoxel>(config.tsdf_voxel_size, config.tsdf_voxels_per_side)),interpolator_(tsdf_layer_.get())
{
block_size_ = config.tsdf_voxel_size * config.tsdf_voxels_per_side;
}
这个函数可以看出TsdfMap中的存储中包含tsdf_layer__结构，并且还存储了block_size_。
3.2 layer.h
explicit Layer(FloatingPoint voxel_size, size_t voxels_per_side)
: voxel_size_(voxel_size), voxels_per_side_(voxels_per_side) {
// CHECK_GT定义如下所示#define CHECK_GT(val1, val2) CHECK_OP(_GT, > , val1, val2)
// 意思是就判断第一个参数是否大于第二个参数
CHECK_GT(voxel_size_, 0.0f);
voxel_size_inv_ = 1.0 / voxel_size_;
block_size_ = voxel_size_ * voxels_per_side_;
CHECK_GT(block_size_, 0.0f);
block_size_inv_ = 1.0 / block_size_;
CHECK_GT(voxels_per_side_, 0u);
voxels_per_side_inv_ = 1.0f / static_cast<FloatingPoint>(voxels_per_side_);
}
3.3 voxel.h
struct TsdfVoxel {
float distance = 0.0f;
float weight = 0.0f;
Color color;
};
struct EsdfVoxel {
float distance = 0.0f;
bool observed = false;
/**
* Whether the voxel was copied from the TSDF (false) or created from a pose
* or some other source (true). This member is not serialized!!!
*/
bool hallucinated = false;
bool in_queue = false;
bool fixed = false;
/**
* Relative direction toward parent. If itself, then either uninitialized
* or in the fixed frontier.
*/
Eigen::Vector3i parent = Eigen::Vector3i::Zero();
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
struct OccupancyVoxel {
float probability_log = 0.0f;
bool observed = false;
};
struct IntensityVoxel {
float intensity = 0.0f;
float weight = 0.0f;
};
在voxblox的存储结构中，在TsdfMap对应多个TsdfLayer，而Layer中又有block结构，block是由固定的voxel组成的（代码中定义体素大小0.2，每条边16个体素）。
但是在c-blox结构中，多了一个子图结构，子图可以收集生成map
4. cblox_ros/CMakeLists.txt
cs_add_library(${
PROJECT_NAME}
src/submap_server.cc
src/active_submap_visualizer.cc
src/trajectory_visualizer.cc
src/submap_conversions.cc
)
依次阅读上述提到的依赖代码
4.1 submap_server.cc
可以获得几个信息，首先
TsdfSubmap::Ptr submap_ptr = submap_collection_ptr_->getSubmapPtr(submap_id);
voxblox::Layer<voxblox::TsdfVoxel>* layer = submap_ptr->getTsdfMapPtr()->getTsdfLayerPtr();
可知，layer是和submap对应的，之后对blocks中的voxels进行一些处理（包括weight和color）
layer->getAllAllocatedBlocks(&block_list);
int block_num = 0;
for (const voxblox::BlockIndex& block_id : block_list) {
if (!layer->hasBlock(block_id)) continue;
voxblox::Block<voxblox::TsdfVoxel>::Ptr block =
layer->getBlockPtrByIndex(block_id);
for (size_t voxel_id = 0; voxel_id < block->num_voxels(); voxel_id++) {
const voxblox::TsdfVoxel& voxel = block->getVoxelByLinearIndex(voxel_id);
voxblox::Point position =
submap_ptr->getPose() *
block->computeCoordinatesFromLinearIndex(voxel_id);
if (voxel.weight < 1e-6) {
continue;
}
color_msg.r = 0.0;
color_msg.g = 0.0;
if (voxel.weight >= 1e-6) {
color_msg.r = std::max(
std::min((max_dist - voxel.distance) / 2.0 / max_dist, 1.0), 0.0);
color_msg.g = std::max(
std::min((max_dist + voxel.distance) / 2.0 / max_dist, 1.0), 0.0);
}
if (std::abs(position.z() - slice_height_) <
submap_ptr->getTsdfMapPtr()->voxel_size() / 2) {
vertex_marker.id =
block_num +
voxel_id * std::pow(10, std::round(std::log10(block_list.size())));
tf::pointEigenToMsg(position.cast<double>(), point_msg);
vertex_marker.points.push_back(point_msg); // 处理后的点转化为Msg进行可视化，以及color
vertex_marker.colors.push_back(color_msg);
}
}
block_num++;
}
4.2 active_submap_visualizer.cc
void ActiveSubmapVisualizer::switchToSubmap(const SubmapID submap_id) {
CHECK(tsdf_submap_collection_ptr_);
active_submap_id_ = submap_id;
if (mesh_layers_.find(submap_id) == mesh_layers_.end()) {
if (verbose_) {
ROS_INFO_STREAM("Creating mesh layer for submap id: " << submap_id);
}
createMeshLayer();
updateIntegrator();
} else {
if (verbose_) {
ROS_INFO_STREAM("Recovering mesh layer for submap id: " << submap_id);
}
recoverMeshLayer();
updateIntegrator();
}
}
4.2.1 mesh_layer.h
在submap_visualizer中涉及到了mesh的创建
size_t new_index = 0u;
for (const BlockIndex& block_index : mesh_indices) {
Mesh::ConstPtr mesh = getMeshPtrByIndex(block_index);
// Check assumption that all meshes have same configuration regarding
// colors, normals and indices.
if (!mesh->vertices.empty()) {
CHECK_EQ(has_colors, mesh->hasColors());
CHECK_EQ(has_normals, mesh->hasNormals());
CHECK_EQ(has_indices, mesh->hasTriangles());
}
// Copy the mesh content into the combined mesh. This is done in triplets
// for readability only, as one loop iteration will then copy one
// triangle.
for (size_t i = 0; i < mesh->vertices.size(); i += 3, new_index += 3) {
CHECK_LT(new_index + 2, mesh_size);
combined_mesh->vertices.push_back(mesh->vertices[i]);
combined_mesh->vertices.push_back(mesh->vertices[i + 1]);
combined_mesh->vertices.push_back(mesh->vertices[i + 2]);
if (has_colors) {
combined_mesh->colors.push_back(mesh->colors[i]);
combined_mesh->colors.push_back(mesh->colors[i + 1]);
combined_mesh->colors.push_back(mesh->colors[i + 2]);
}
if (has_normals) {
combined_mesh->normals.push_back(mesh->normals[i]);
combined_mesh->normals.push_back(mesh->normals[i + 1]);
combined_mesh->normals.push_back(mesh->normals[i + 2]);
}
if (has_indices) {
combined_mesh->indices.push_back(new_index);
combined_mesh->indices.push_back(new_index + 1);
combined_mesh->indices.push_back(new_index + 2);
}
}
}
4.2.2 mesh_integrator.h
还涉及到了mesh的整合，其实就是从tsdf层中的blocks来生成mesh层
void generateMesh(bool only_mesh_updated_blocks, bool clear_updated_flag) {
CHECK(!clear_updated_flag || (sdf_layer_mutable_ != nullptr))
<< "If you would like to modify the updated flag in the blocks, please "
<< "use the constructor that provides a non-const link to the sdf "
<< "layer!";
BlockIndexList all_tsdf_blocks;
if (only_mesh_updated_blocks) {
sdf_layer_const_->getAllUpdatedBlocks(Update::kMesh, &all_tsdf_blocks); // 这一行是仅对更新后的Blocks处理
} else {
sdf_layer_const_->getAllAllocatedBlocks(&all_tsdf_blocks); // 这一行是对所有的Blocks处理
}
// Allocate all the mesh memory
for (const BlockIndex& block_index : all_tsdf_blocks) {
mesh_layer_->allocateMeshPtrByIndex(block_index);
}
std::unique_ptr<ThreadSafeIndex> index_getter(
new MixedThreadSafeIndex(all_tsdf_blocks.size()));
std::list<std::thread> integration_threads;
for (size_t i = 0; i < config_.integrator_threads; ++i) {
// 多线程创建Mesh
integration_threads.emplace_back(
&MeshIntegrator::generateMeshBlocksFunction, this, all_tsdf_blocks,
clear_updated_flag, index_getter.get());
}
4.3 trajectory_visualizer.cc
void TrajectoryVisualizer::getTrajectoryMsg(
nav_msgs::Path* path_msg_ptr) const {
CHECK_NOTNULL(path_msg_ptr);
// Note(alexmillane): The trajectory is being regenerated at each published
// pose. This could become heavy... Perhaps building up the message
// incrementally might be a good idea.
for (const Transformation& T_G_C : T_G_C_array_) {
geometry_msgs::PoseStamped pose_stamped; //声明一个空的位姿变量
tf::poseKindrToMsg(T_G_C.cast<double>(), &pose_stamped.pose); //将该位姿中的pose转为对应的消息
path_msg_ptr->poses.push_back(pose_stamped);
}
}
4.4 submap_conversions.cc
4.4.1 conversions.h
//将voxblox中的color与Msg格式相互转换
colorVoxbloxToMsg(const Color& color,std_msgs::ColorRGBA* color_msg);
colorMsgToVoxblox(const std_msgs::ColorRGBA& color_msg,Color* color);
// 将点云格式转换为PCL的不同格式，保留不同数据
pointcloudToPclXYZRGB(const Pointcloud& ptcloud, const Colors& colors,pcl::PointCloud<pcl::PointXYZRGB>* ptcloud_pcl);
pointcloudToPclXYZ(const Pointcloud& ptcloud,pcl::PointCloud<pcl::PointXYZ>* ptcloud_pcl);
pointcloudToPclXYZI(const Pointcloud& ptcloud,const std::vector<float>&intensities,pcl::PointCloud<pcl::PointXYZI>* ptcloud_pcl);
// 将PCL点云类型转换为voxblox点云
convertPointcloud(const typename pcl::PointCloud<PCLPoint>& pointcloud_pcl,const std::shared_ptr<ColorMap>& color_map, Pointcloud* points_C,Colors* colors);
5. tsdf_submap_collection_integrator.h（cpp）
这块对应的显然是论文中的C部分，即tsdf_integrator
5.1 integratePointCloud（其中具体的代码得跳转很多遍）
// Integrate a pointcloud to the currently active submap.
// NOTE(alexmilane): T_G_S - Transformation between camera frame (C) and
// the global tracking frame (G).
void integratePointCloud(const Transformation& T_G_C,
const Pointcloud& points_C, const Colors& colors);
void TsdfSubmapCollectionIntegrator::integratePointCloud(
const Transformation& T_G_C, const Pointcloud& points_C,const Colors& colors) {
CHECK(!tsdf_submap_collection_ptr_->empty())
<< "Can't integrate. No submaps in collection.";
CHECK(tsdf_integrator_)
<< "Can't integrate. Need to update integration target.";
// Getting the submap relative transform
// NOTE(alexmilane): T_S_C - Transformation between Camera frame (C) and
// the submap base frame (S).
const Transformation T_S_C = getSubmapRelativePose(T_G_C); // 下面有相关代码
// Passing data to the tsdf integrator
tsdf_integrator_->integratePointCloud(T_S_C, points_C, colors); // 下面有相关代码
}
// 下面将摄像机跟踪提供的估计传感器姿态T_G_C转换为活动子体积帧
// 即论文中的ΠA的活动子体积坐标（在全局坐标系下）转置乘以相机数据的第i帧的坐标（在全局坐标系下）
// Transform to the currently targeted submap
// NOTE(alexmilane): T_G_S - Transformation between Submap base frame (S) and
// the global tracking frame (G).
Transformation T_G_S_active_;
Transformation TsdfSubmapCollectionIntegrator::getSubmapRelativePose(
const Transformation& T_G_C) const {
return (T_G_S_active_.inverse() * T_G_C);
}
5.1.1 SimpleTsdfIntegrator
// 该simple方法即我们理解的所有voxel投影到当前相机的坐标系下，比较图像3d点的z和深度voxel的z的差获得tsdf。
// 其中Simple和Merged都是voxblox中就使用的方法
void SimpleTsdfIntegrator::integratePointCloud(const Transformation& T_G_C,
const Pointcloud& points_C,
const Colors& colors,
const bool freespace_points) {
timing::Timer integrate_timer("integrate/simple");
CHECK_EQ(points_C.size(), colors.size());
std::unique_ptr<ThreadSafeIndex> index_getter(
ThreadSafeIndexFactory::get(config_.integration_order_mode, points_C));
std::list<std::thread> integration_threads;
for (size_t i = 0; i < config_.integrator_threads; ++i) {
integration_threads.emplace_back(&SimpleTsdfIntegrator::integrateFunction,
this, T_G_C, points_C, colors,
freespace_points, index_getter.get());
}
for (std::thread& thread : integration_threads) {
thread.join();
}
integrate_timer.Stop();
timing::Timer insertion_timer("inserting_missed_blocks");
updateLayerWithStoredBlocks();
insertion_timer.Stop();
}
5.1.2 MergedTsdfIntegrator
void MergedTsdfIntegrator::integratePointCloud(const Transformation& T_G_C,
const Pointcloud& points_C,
const Colors& colors,
const bool freespace_points) {
timing::Timer integrate_timer("integrate/merged");
CHECK_EQ(points_C.size(), colors.size());
// 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里
// 有些不valid的点是需要清除的(距离相机太近或太远之类，见IsValid函数)除此之外，那些投影到一个voxel_index下的不同poin_idx都会被push到voxel_map里。
bundleRays(T_G_C, points_C, freespace_points, index_getter.get(), &voxel_map,
&clear_map);
// 求得属于一个voxel内的点的平均点和颜色
integrateRays(T_G_C, points_C, colors, config_.enable_anti_grazing, false,
voxel_map, clear_map);
timing::Timer clear_timer("integrate/clear");
// 最后更新时采用的是一个体素中的全部点的平均值，提升了约20倍的效率（论文作者自述）
integrateRays(T_G_C, points_C, colors, config_.enable_anti_grazing, true,
voxel_map, clear_map);
clear_timer.Stop();
integrate_timer.Stop();
}
5.1.3 FastTsdfIntegrator
这个方法是cblox中提出的新的方法
void FastTsdfIntegrator::integratePointCloud(const Transformation& T_G_C,
const Pointcloud& points_C,
const Colors& colors,
const bool freespace_points) {
timing::Timer integrate_timer("integrate/fast");
CHECK_EQ(points_C.size(), colors.size());
integration_start_time_ = std::chrono::steady_clock::now(); // 记录算法起始时间，记录算法执行时间用
static int64_t reset_counter = 0;
if ((++reset_counter) >= config_.clear_checks_every_n_frames) {
// 算法默认clear_checks_every_n_frames = 1，意思是每次重新执行FastIntegrator算法时重新清空数组
reset_counter = 0;
start_voxel_approx_set_.resetApproxSet(); // 对应论文starting location集
voxel_observed_approx_set_.resetApproxSet(); // 对应论文observed voxel集
// resetApproxset对应数组还原置零
}
// ThreadSafeIndexFactory是保证多线程执行该积分任务时，同时操作索引同一个数组时的互斥性。
std::unique_ptr<ThreadSafeIndex> index_getter(
ThreadSafeIndexFactory::get(config_.integration_order_mode, points_C));
// 多线程执行integrateFunction
// 声明线程list，并使他们并发执行（这一方法在simple中是类似的并行执行的，但是merged没有这么执行）
std::list<std::thread> integration_threads;
for (size_t i = 0; i < config_.integrator_threads; ++i) {
integration_threads.emplace_back(&FastTsdfIntegrator::integrateFunction,
this, T_G_C, points_C, colors,
freespace_points, index_getter.get());
}
for (std::thread& thread : integration_threads) {
thread.join();
}
integrate_timer.Stop(); // 结束时间戳
timing::Timer insertion_timer("inserting_missed_blocks"); // 记录insert的时间开始
// 主要是去执行insertBlock函数，将全部已存储的block存入layer中，主要是有些block会在前面的步骤中忽略（没点），不知道理解的对不对，先打个问号这个函数
updateLayerWithStoredBlocks(); // ??
insertion_timer.Stop(); // 结束
}
void FastTsdfIntegrator::integrateFunction(const Transformation& T_G_C,
const Pointcloud& points_C,
const Colors& colors,
const bool freespace_points,
ThreadSafeIndex* index_getter) {
DCHECK(index_getter != nullptr);
size_t point_idx;
// while判定条件为还有后续的点（同时）且处理的时间还未超过设定的max_integration_time_s，*1000000是因为前面通过系统时间获得的是微秒，而设定的时间单位为秒
while (index_getter->getNextIndex(&point_idx) &&
(std::chrono::duration_cast<std::chrono::microseconds>(
std::chrono::steady_clock::now() - integration_start_time_)
.count() < config_.max_integration_time_s * 1000000)) {
const Point& point_C = points_C[point_idx];
const Color& color = colors[point_idx];
bool is_clearing;
// 其中ray_distance = point_C.norm();，而这个norm在源码中指的是Frobenius norm，也就是平方和开根号，那么这个距离也就是距离相机原点的距离
// 判定在1.射线的距离ray_distance小于设定的最小值(0.1)返回false，或者
// 2.在满足大于设定最大值(5.0)且满足（config_.allow_clear || freespace_point)返回true（超过极限距离，清除后续的点）
// 3.其他情况返回true
// 也就是说，太小的距离无效点，过大的时候有特殊情况是有效的，而中间距离的一律有效，判断有效后继续执行
if (!isPointValid(point_C, freespace_points, &is_clearing)) {
continue;
}
const Point origin = T_G_C.getPosition(); // 转换到世界坐标的原点坐标
const Point point_G = T_G_C * point_C; // 获得当前点的世界坐标
// Checks to see if another ray in this scan has already started 'close'
// to this location. If it has then we skip ray casting this point. We
// measure if a start location is 'close' to another points by inserting
// the point into a set of voxels. This voxel set has a resolution
// start_voxel_subsampling_factor times higher then the voxel size.
// 作者定义的“起始位置”集
GlobalIndex global_voxel_idx;
// 这个getGridIndexFromPoint函数意思是取得某个点对应的网格坐标
global_voxel_idx = getGridIndexFromPoint<GlobalIndex>(
point_G, config_.start_voxel_subsampling_factor * voxel_size_inv_);
if (!start_voxel_approx_set_.replaceHash(global_voxel_idx)) {
// replaceHash作用将masked_hash中的元素替换为hash，也就是如果起始位置已经有点了，则跳过
continue;
}
constexpr bool cast_from_origin = false;
RayCaster ray_caster(origin, point_G, is_clearing,
config_.voxel_carving_enabled,
config_.max_ray_length_m, voxel_size_inv_,
config_.default_truncation_distance, cast_from_origin);
int64_t consecutive_ray_collisions = 0;
Block<TsdfVoxel>::Ptr block = nullptr;
BlockIndex block_idx;
while (ray_caster.nextRayIndex(&global_voxel_idx)) {
// Check if the current voxel has been seen by any ray cast this scan.
// If it has increment the consecutive_ray_collisions counter, otherwise
// reset it. If the counter reaches a threshold we stop casting as the
// ray is deemed to be contributing too little new information.
// 对应的是“观察体素”集
if (!voxel_observed_approx_set_.replaceHash(global_voxel_idx)) {
++consecutive_ray_collisions;// 如果该观察体素已存在
} else {
consecutive_ray_collisions = 0;
}
// 默认 max_consecutive_ray_collisions = 2，如果同一个体素遇到两次以上的冲突，即重复更新三次，则提前结束
if (consecutive_ray_collisions > config_.max_consecutive_ray_collisions) {
break;
}
// 文章最开始有介绍，我们是有8X8X8的voxel组成一个block。只有在block里有的voxel才会被分配空间真正储存起来(有tsdf值，rgb值等)。下面这行函数就是检测该voxel是否已经属于某个block还是并不属于任何一个block，如果不，则新建一个block分配空间。返回在这个block里的对应index的voxel
TsdfVoxel* voxel =
allocateStorageAndGetVoxelPtr(global_voxel_idx, &block, &block_idx);
// 权重来自voxblox，近似简化后的RGBD模型与behind-surface下降模型结合
const float weight = getVoxelWeight(point_C);
// 终于，做了那么多准备，更新tsdf（同voxblox的merged）
updateTsdfVoxel(origin, point_G, global_voxel_idx, color, weight, voxel);
}
}
}
RayCaster::RayCaster(const Point& origin, const Point& point_G,
const bool is_clearing_ray,
const bool voxel_carving_enabled,
const FloatingPoint max_ray_length_m,
const FloatingPoint voxel_size_inv,
const FloatingPoint truncation_distance,
const bool cast_from_origin) {
// 计算单位射线的长度，就是简单的求点到原点的差值，求得他们的norm(均方值)再每一个维度除以norm归一化。
const Ray unit_ray = (point_G - origin).normalized();
Point ray_start, ray_end;
if (is_clearing_ray) {
// 如果这个射线是需要清除的
// norm()是取出二范数，也就是射线的模长
FloatingPoint ray_length = (point_G - origin).norm();
// std::max取出的是射线位于对应表面前的距离，除去截断距离。
// std::min取出的是该差值与射线最值的较小值
ray_length = std::min(std::max(ray_length - truncation_distance,
static_cast<FloatingPoint>(0.0)),
max_ray_length_m);
// origin到point_G
ray_end = origin + unit_ray * ray_length;
// 射线的开始点为原点(voxel_carving_enabled==true)，为否，射线直接为0 end-end
ray_start = voxel_carving_enabled ? origin : ray_end;
} else {
// 起始为目标点前后的截断距离
ray_end = point_G + unit_ray * truncation_distance;
ray_start = voxel_carving_enabled
? origin
: (point_G - unit_ray * truncation_distance);
}
const Point start_scaled = ray_start * voxel_size_inv;
const Point end_scaled = ray_end * voxel_size_inv;
if (cast_from_origin) {
setupRayCaster(start_scaled, end_scaled);
} else {
setupRayCaster(end_scaled, start_scaled);
}
}
void RayCaster::setupRayCaster(const Point& start_scaled,
const Point& end_scaled) {
// 点的起始或终点坐标无效
if (std::isnan(start_scaled.x()) || std::isnan(start_scaled.y()) ||
std::isnan(start_scaled.z()) || std::isnan(end_scaled.x()) ||
std::isnan(end_scaled.y()) || std::isnan(end_scaled.z())) {
ray_length_in_steps_ = 0;
return;
}
curr_index_ = getGridIndexFromPoint<GlobalIndex>(start_scaled);
const GlobalIndex end_index = getGridIndexFromPoint<GlobalIndex>(end_scaled);
const GlobalIndex diff_index = end_index - curr_index_;
current_step_ = 0;
ray_length_in_steps_ = std::abs(diff_index.x()) + std::abs(diff_index.y()) +
std::abs(diff_index.z());
const Ray ray_scaled = end_scaled - start_scaled;
//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()));
const AnyIndex corrected_step(std::max(0, ray_step_signs_.x()),
std::max(0, ray_step_signs_.y()),
std::max(0, ray_step_signs_.z()));
const Point start_scaled_shifted =
start_scaled - curr_index_.cast<FloatingPoint>();
Ray distance_to_boundaries(corrected_step.cast<FloatingPoint>() -
start_scaled_shifted);
t_to_next_boundary_ = Ray((std::abs(ray_scaled.x()) < 0.0)
? 2.0
: distance_to_boundaries.x() / ray_scaled.x(),
(std::abs(ray_scaled.y()) < 0.0)
? 2.0
: distance_to_boundaries.y() / ray_scaled.y(),
(std::abs(ray_scaled.z()) < 0.0)
? 2.0
: distance_to_boundaries.z() / ray_scaled.z());
// Distance to cross one grid cell along the ray in t.
// Same as absolute inverse value of delta_coord.
t_step_size_ = Ray(
(std::abs(ray_scaled.x()) < 0.0) ? 2.0
: ray_step_signs_.x() / ray_scaled.x(),
(std::abs(ray_scaled.y()) < 0.0) ? 2.0
: ray_step_signs_.y() / ray_scaled.y(),
(std::abs(ray_scaled.z()) < 0.0) ? 2.0
: ray_step_signs_.z() / ray_scaled.z());
}
5.2 switchToActiveSubmap
这一个函数应当是将活动子卷和对应的TSDF层相关联，并获取位姿变换
// Changes the active submap to the last one on the collection
// 定期创建一个新的子卷，标记为活动的子卷，最后一个活动子卷被转移到集合中，从而增加映射。对应的cblox_ros代码
/*
// Creating the submap
const SubmapID submap_id = submap_collection_ptr_->createNewSubmap(T_G_C);
// Activating the submap in the frame integrator
tsdf_submap_collection_integrator_ptr_->switchToActiveSubmap();
// Resetting current submap counters
num_integrated_frames_current_submap_ = 0;
// Updating the active submap mesher
active_submap_visualizer_ptr_->switchToActiveSubmap();
*/
void TsdfSubmapCollectionIntegrator::switchToActiveSubmap() {
// Setting the server members to point to this submap
// NOTE(alexmillane): This is slightly confusing because the collection is
// increased in size elsewhere but we change the
// integration target to the new submap here. The result is
// that between new submap creation and activation the
// integrator wont be affecting the latest submap in the
// collection.
updateIntegratorTarget(tsdf_submap_collection_ptr_->getActiveTsdfMapPtr()); // 括号里获取最后一个TSDF地图的指针
T_G_S_active_ = tsdf_submap_collection_ptr_->getActiveSubmapPose(); // 获得这个活动子卷的世界位姿
}
template <typename SubmapType>
TsdfMap::Ptr SubmapCollection<SubmapType>::getActiveTsdfMapPtr() {
const auto it = id_to_submap_.find(active_submap_id_);
CHECK(it != id_to_submap_.end());
return (it->second)->getTsdfMapPtr();
}
void TsdfSubmapCollectionIntegrator::updateIntegratorTarget(
const TsdfMap::Ptr& tsdf_map_ptr) {
CHECK(tsdf_map_ptr);
// Creating the integrator if not yet created.
// Otherwise, changing the integration target.
if (tsdf_integrator_ == nullptr) {
initializeIntegrator(tsdf_map_ptr); // 如果没有integrator，则初始化一个并整合该tsdf地图
} else {
tsdf_integrator_->setLayer(tsdf_map_ptr->getTsdfLayerPtr());
}
}
void TsdfSubmapCollectionIntegrator::initializeIntegrator(
const TsdfMap::Ptr& tsdf_map_ptr) {
CHECK(tsdf_map_ptr);
// Creating with the voxblox provided factory
tsdf_integrator_ = voxblox::TsdfIntegratorFactory::create(
method_, tsdf_integrator_config_, tsdf_map_ptr->getTsdfLayerPtr());
}
void TsdfIntegratorBase::setLayer(Layer<TsdfVoxel>* layer) {
CHECK_NOTNULL(layer);
layer_ = layer;
voxel_size_ = layer_->voxel_size();
block_size_ = layer_->block_size();
voxels_per_side_ = layer_->voxels_per_side();
voxel_size_inv_ = 1.0 / voxel_size_;
block_size_inv_ = 1.0 / block_size_;
voxels_per_side_inv_ = 1.0 / voxels_per_side_;
}
6. submap_collection.h（submap_collection_inl.h）
显然这个算法对应的论文E部分，实现的是子地图融合。(不清楚是不是同时包括D部分)
6.1 SubmapCollection
// 构造函数。从tsdf子图列表构造子图集合
template <typename SubmapType>
SubmapCollection<SubmapType>::SubmapCollection(
const typename SubmapType::Config& submap_config,
const std::vector<typename SubmapType::Ptr>& tsdf_sub_maps)
: submap_config_(submap_config) {
// Constructing from a list of existing submaps
// NOTE(alexmillane): Relies on the submaps having unique submap IDs...
// 只是根据ID的唯一性生成一个新的子图集合
for (const auto& tsdf_submap_ptr : tsdf_sub_maps) {
const auto ret =
id_to_submap_.insert({
tsdf_submap_ptr->getID(), tsdf_submap_ptr});
CHECK(ret.second) << "Attempted to construct collection from vector of "
"submaps containing at least one duplicate ID.";
}
}
6.2 createNewSubmap（不重要）
有两种刚创建新的子图的方法，一种是指定一个submap——id，另一种只是给一个位姿变换。
template <typename SubmapType>
void SubmapCollection<SubmapType>::createNewSubmap(const Transformation& T_G_S,
const SubmapID submap_id) {
// Checking if the submap already exists
// NOTE(alexmillane): This hard fails the program if the submap already
// exists. This is fairly brittle behaviour and we may want to change it at a
// later date. Currently the onus is put in the caller to exists() before
// creating a submap.
const auto it = id_to_submap_.find(submap_id);
CHECK(it == id_to_submap_.end());
// Creating the new submap and adding it to the list 加入子图列表
typename SubmapType::Ptr tsdf_sub_map(
new SubmapType(T_G_S, submap_id, submap_config_));
id_to_submap_.emplace(submap_id, std::move(tsdf_sub_map));
// Updating the active submap，该新的子图设定为活动子图
active_submap_id_ = submap_id;
}
template <typename SubmapType>
SubmapID SubmapCollection<SubmapType>::createNewSubmap(
const Transformation& T_G_S) {
// Creating a submap with a generated SubmapID
// NOTE(alexmillane): rbegin() returns the pair with the highest key.
SubmapID new_ID = 0;
if (!id_to_submap_.empty()) {
// 只要id_to_submap_也就是子图列表不为空，取出其中id的最大值加一，设定为该Submap的ID
new_ID = id_to_submap_.rbegin()->first + 1;
}
createNewSubmap(T_G_S, new_ID);
return new_ID;
}
6.3 fuseSubmapPair（应当是核心）
// 给定两个子体积π_i和π_j在参考系M_i和M_j下被融合;然后找到它们与全局地图之间的转换。
template <typename SubmapType>
void SubmapCollection<SubmapType>::fuseSubmapPair(
const SubmapIdPair& submap_id_pair) {
// Extracting the submap IDs
SubmapID submap_id_1 = submap_id_pair.first;
SubmapID submap_id_2 = submap_id_pair.second;
LOG(INFO) << "Fusing submap pair: (" << submap_id_1 << ", " << submap_id_2
<< ")";
// Getting the requested submaps
const auto id_submap_pair_1_it = id_to_submap_.find(submap_id_1);
const auto id_submap_pair_2_it = id_to_submap_.find(submap_id_2);
// If the submaps are found
if ((id_submap_pair_1_it != id_to_submap_.end()) &&
(id_submap_pair_2_it != id_to_submap_.end())) {
// Getting the submaps
typename SubmapType::Ptr submap_ptr_1 = (*id_submap_pair_1_it).second;
typename SubmapType::Ptr submap_ptr_2 = (*id_submap_pair_2_it).second;
// Checking that we're not trying to fuse a submap into itself. This can
// occur due to fusing submap pairs in a triangle.
if (submap_ptr_1->getID() == submap_ptr_2->getID()) {
return;
}
// Getting the tsdf submap and its pose
const Transformation& T_G_S1 = submap_ptr_1->getPose();
const Transformation& T_G_S2 = submap_ptr_2->getPose();
const Transformation T_S1_S2 = T_G_S1.inverse() * T_G_S2;
// Merging the submap layers
// 提供了naiveTransformLayer和transformLayer两种方法
mergeLayerAintoLayerB(submap_ptr_2->getTsdfMap().getTsdfLayer(), T_S1_S2,
submap_ptr_1->getTsdfMapPtr()->getTsdfLayerPtr());
// Deleting Submap #2
const size_t num_erased = id_to_submap_.erase(submap_id_2);
CHECK_EQ(num_erased, 1);
LOG(INFO) << "Erased the submap: " << submap_ptr_2->getID()
<< " from the submap collection";
} else {
LOG(WARNING) << "Could not find the requested submap pair during fusion.";
}
}
6.3.1 naiveTransformLayer
/**
* Similar to transformLayer in functionality, however the system only makes
* use of the forward transform and nearest neighbor interpolation. This will
* result in artifacts and other issues in the result, however it should be
* several orders of magnitude faster.
*/
template <typename VoxelType>
void naiveTransformLayer(const Layer<VoxelType>& layer_in,
const Transformation& T_out_in,
Layer<VoxelType>* layer_out) {
BlockIndexList block_idx_list_in;
// 将所有的block取出并存入block_idx_list_in中，这是一个索引list
layer_in.getAllAllocatedBlocks(&block_idx_list_in);
// 线性插值函数
Interpolator<VoxelType> interpolator(&layer_in);
for (const BlockIndex& block_idx : block_idx_list_in) {
// 按照block列表中索引依次取出
const Block<VoxelType>& input_block = layer_in.getBlockByIndex(block_idx);
for (IndexElement input_linear_voxel_idx = 0;
input_linear_voxel_idx <
static_cast<IndexElement>(input_block.num_voxels());
++input_linear_voxel_idx) {
// find voxel centers location in the output
// 是先找到in的center，然后转换到out的center point
const Point voxel_center =
T_out_in *
input_block.computeCoordinatesFromLinearIndex(input_linear_voxel_idx);
// grid（应当是global体素坐标）
const GlobalIndex global_output_voxel_idx =
getGridIndexFromPoint<GlobalIndex>(voxel_center,
layer_out->voxel_size_inv());
// allocate it in the output，分配空间
typename Block<VoxelType>::Ptr output_block =
layer_out->allocateBlockPtrByIndex(getBlockIndexFromGlobalVoxelIndex(
global_output_voxel_idx, layer_out->voxels_per_side_inv()));
if (output_block == nullptr) {
std::cerr << "invalid block" << std::endl;
}
// get the output voxel，寻找到out的local体素
VoxelType& output_voxel =
output_block->getVoxelByVoxelIndex(getLocalFromGlobalVoxelIndex(
global_output_voxel_idx, layer_out->voxels_per_side()));
// 前面的一系列操作只是为了找到in体素的center与out体素的center（都是对应各自local坐标系）
// 对in和out有两种体素处理的方法，分别如下
if (interpolator.getVoxel(voxel_center, &output_voxel, false)) {
// 清空out
output_block->has_data() = true;
}
}
}
}
template <typename VoxelType>
bool Interpolator<VoxelType>::getInterpVoxel(const Point& pos,
VoxelType* voxel) const {
CHECK_NOTNULL(voxel);
// get voxels of 8 surrounding voxels and weights vector
const VoxelType* voxels[8];
InterpVector q_vector;
// 先获取周围八个体素的信息
if (!getVoxelsAndQVector(pos, voxels, &q_vector)) {
return false;
} else {
/*
采用下面这个矩阵进行插值，包括weight、color、sdf值
(InterpTable() <<
1, 0, 0, 0, 0, 0, 0, 0,
-1, 0, 0, 0, 1, 0, 0, 0,
-1, 0, 1, 0, 0, 0, 0, 0,
-1, 1, 0, 0, 0, 0, 0, 0,
1, 0, -1, 0, -1, 0, 1, 0,
1, -1, -1, 1, 0, 0, 0, 0,
1, -1, 0, 0, -1, 1, 0, 0,
-1, 1, 1, -1, 1, -1, -1, 1
)
*/
*voxel = interpVoxel(q_vector, voxels);
return true;
}
}
template <typename VoxelType>
bool Interpolator<VoxelType>::getNearestVoxel(const Point& pos,
VoxelType* voxel) const {
CHECK_NOTNULL(voxel);
// 获取in的block指针，确保有对应
typename Layer<VoxelType>::BlockType::ConstPtr block_ptr =
layer_->getBlockPtrByCoordinates(pos);
if (block_ptr == nullptr) {
return false;
}
// 获取in的体素
*voxel = block_ptr->getVoxelByCoordinates(pos);
// 如果该体素确实被观察到，也就是之前射线处理过，应该就是意味着这种方法直接用in的体素了
return utils::isObservedVoxel(*voxel);
}
6.3.2 transformLayer（和naive差不多，只不过会涉及到反向transform，时间开销更高一点）
/**
* Performs a 3D transform on the input layer and writes the results to the
* output layer. During the transformation resampling occurs so that the voxel
* and block size of the input and output layer can differ.
*/
template <typename VoxelType>
void transformLayer(const Layer<VoxelType>& layer_in,
const Transformation& T_out_in,
Layer<VoxelType>* layer_out) {
CHECK_NOTNULL(layer_out);
// first mark all the blocks in the output layer that may be filled by the
// input layer (we are conservative here approximating the input blocks as
// spheres of diameter sqrt(3)*block_size)
IndexSet block_idx_set;
BlockIndexList block_idx_list_in;
layer_in.getAllAllocatedBlocks(&block_idx_list_in);
for (const BlockIndex& block_idx : block_idx_list_in) {
const Point c_in =
getCenterPointFromGridIndex(block_idx, layer_in.block_size());
// forwards transform of center
const Point c_out = T_out_in * c_in;
// Furthest center point of neighboring blocks.
FloatingPoint offset =
kUnitCubeDiagonalLength * layer_in.block_size() * 0.5;
// Add index of all blocks in range to set.
for (FloatingPoint x = c_out.x() - offset; x < c_out.x() + offset;
x += layer_out->block_size()) {
for (FloatingPoint y = c_out.y() - offset; y < c_out.y() + offset;
y += layer_out->block_size()) {
for (FloatingPoint z = c_out.z() - offset; z < c_out.z() + offset;
z += layer_out->block_size()) {
const Point current_center_out = Point(x, y, z);
BlockIndex current_idx = getGridIndexFromPoint<BlockIndex>(
current_center_out, 1.0f / layer_out->block_size());
block_idx_set.insert(current_idx);
}
}
}
}
// get inverse transform
const Transformation T_in_out = T_out_in.inverse();
Interpolator<VoxelType> interpolator(&layer_in);
// we now go through all the blocks in the output layer and interpolate the
// input layer at the center of each output voxel position
for (const BlockIndex& block_idx : block_idx_set) {
typename Block<VoxelType>::Ptr block =
layer_out->allocateBlockPtrByIndex(block_idx);
for (IndexElement voxel_idx = 0;
voxel_idx < static_cast<IndexElement>(block->num_voxels());
++voxel_idx) {
VoxelType& voxel = block->getVoxelByLinearIndex(voxel_idx);
// find voxel centers location in the input
const Point voxel_center =
T_in_out * block->computeCoordinatesFromLinearIndex(voxel_idx);
// interpolate voxel
if (interpolator.getVoxel(voxel_center, &voxel, true)) {
block->has_data() = true;
// if interpolated value fails use nearest
} else if (interpolator.getVoxel(voxel_center, &voxel, false)) {
block->has_data() = true;
}
}
if (!block->has_data()) {
layer_out->removeBlock(block_idx);
}
}
}
6.4 getProjectedMap （生成一个TSDF map）
template <typename SubmapType>
TsdfMap::Ptr SubmapCollection<SubmapType>::getProjectedMap() const {
// Creating the global tsdf map and getting its tsdf layer
TsdfMap::Ptr projected_tsdf_map_ptr =
TsdfMap::Ptr(new TsdfMap(submap_config_));
Layer<TsdfVoxel>* combined_tsdf_layer_ptr =
projected_tsdf_map_ptr->getTsdfLayerPtr();
// Looping over the current submaps
for (const auto& id_submap_pair : id_to_submap_) {
// Getting the tsdf submap and its pose
const TsdfMap& tsdf_map = (id_submap_pair.second)->getTsdfMap();
const Transformation& T_G_S = (id_submap_pair.second)->getPose();
// Merging layers the submap into the global layer
mergeLayerAintoLayerB(tsdf_map.getTsdfLayer(), T_G_S,
combined_tsdf_layer_ptr);
}
// Returning the new map
return projected_tsdf_map_ptr;
}
7.merge_integrator(evaluateLayerRmseAtPoses)
应该是其中关于D部分的实现，主要是地图的融合过程中需要判定一些度量。
/**
* This function will align layer B to layer A, transforming and interpolating
* layer B into voxel grid A for every transformation in `transforms_A_B` and
* evaluate them. If `aligned_layers_and_error_layers` is set, this function
* returns a vector containing the aligned layer_B and an error layer for every
* transformation. The error layer contains the absolute SDF error for every
* voxel of the comparison between layer_A and aligned layer_B. This function
* currently only supports SDF type layers, like TsdfVoxel and EsdfVoxel.
*/
template <typename VoxelType>
void evaluateLayerRmseAtPoses(
const utils::VoxelEvaluationMode& voxel_evaluation_mode,
const Layer<VoxelType>& layer_A, const Layer<VoxelType>& layer_B,
const std::vector<Transformation>& transforms_A_B,
std::vector<utils::VoxelEvaluationDetails>* voxel_evaluation_details_vector,
std::vector<std::pair<typename voxblox::Layer<VoxelType>::Ptr,
typename voxblox::Layer<VoxelType>::Ptr>>*
aligned_layers_and_error_layers = nullptr) {
CHECK_NOTNULL(voxel_evaluation_details_vector);
// Check if layers are compatible.
CHECK_NEAR(layer_A.voxel_size(), layer_B.voxel_size(), 1e-8);
CHECK_EQ(layer_A.voxels_per_side(), layer_B.voxels_per_side());
// Check if world TSDF layer agrees with merged object at all object poses.
for (size_t i = 0u; i < transforms_A_B.size(); ++i) {
const Transformation& transform_A_B = transforms_A_B[i];
// Layer B transformed to the coordinate frame A.
typename Layer<VoxelType>::Ptr aligned_layer_B(
new Layer<VoxelType>(layer_B.voxel_size(), layer_B.voxels_per_side()));
Layer<VoxelType>* error_layer = nullptr;
if (aligned_layers_and_error_layers != nullptr) {
if (aligned_layers_and_error_layers->size() != transforms_A_B.size()) {
aligned_layers_and_error_layers->clear();
aligned_layers_and_error_layers->resize(transforms_A_B.size());
}
// Initialize and get ptr to error layer to fill out later.
(*aligned_layers_and_error_layers)[i].second =
typename voxblox::Layer<VoxelType>::Ptr(new voxblox::Layer<VoxelType>(
layer_A.voxel_size(), layer_A.voxels_per_side()));
error_layer = (*aligned_layers_and_error_layers)[i].second.get();
// Store the aligned object as well.
(*aligned_layers_and_error_layers)[i].first = aligned_layer_B;
}
// Transform merged object into the world frame.
transformLayer<VoxelType>(layer_B, transform_A_B, aligned_layer_B.get());
utils::VoxelEvaluationDetails voxel_evaluation_details;
// Evaluate the RMSE of the merged object layer in the world layer.
utils::evaluateLayersRmse(layer_A, *aligned_layer_B, voxel_evaluation_mode,
&voxel_evaluation_details, error_layer);
voxel_evaluation_details_vector->push_back(voxel_evaluation_details);
}
}
7.1 evaluateLayersRmse
template <typename VoxelType>
void evaluateLayerRmseAtPoses(
const utils::VoxelEvaluationMode& voxel_evaluation_mode,
const Layer<VoxelType>& layer_A, const Layer<VoxelType>& layer_B,
const std::vector<Eigen::Matrix<float, 4, 4>,
Eigen::aligned_allocator<Eigen::Matrix<float, 4, 4>>>&
transforms_A_B,
std::vector<utils::VoxelEvaluationDetails>* voxel_evaluation_details_vector,
std::vector<std::pair<typename voxblox::Layer<VoxelType>::Ptr,
typename voxblox::Layer<VoxelType>::Ptr>>*
aligned_layers_and_error_layers = nullptr) {
CHECK_NOTNULL(voxel_evaluation_details_vector);
std::vector<Transformation> kindr_transforms_A_B;
for (const Eigen::Matrix<float, 4, 4>& transform_A_B : transforms_A_B) {
kindr_transforms_A_B.emplace_back(transform_A_B);
}
evaluateLayerRmseAtPoses(
voxel_evaluation_mode, layer_A, layer_B, kindr_transforms_A_B,
voxel_evaluation_details_vector, aligned_layers_and_error_layers);
}
template <typename VoxelType>
FloatingPoint evaluateLayersRmse(
const Layer<VoxelType>& layer_gt, const Layer<VoxelType>& layer_test,
const VoxelEvaluationMode& voxel_evaluation_mode,
VoxelEvaluationDetails* evaluation_result = nullptr,
Layer<VoxelType>* error_layer = nullptr) {
// Iterate over all voxels in the test layer and look them up in the ground
// truth layer. Then compute RMSE.
BlockIndexList block_list;
layer_test.getAllAllocatedBlocks(&block_list);
size_t vps = layer_test.voxels_per_side();
size_t num_voxels_per_block = vps * vps * vps;
VoxelEvaluationDetails evaluation_details;
double total_squared_error = 0.0;
for (const BlockIndex& block_index : block_list) {
const Block<VoxelType>& test_block =
layer_test.getBlockByIndex(block_index);
if (!layer_gt.hasBlock(block_index)) {
for (size_t linear_index = 0u; linear_index < num_voxels_per_block;
++linear_index) {
const VoxelType& voxel = test_block.getVoxelByLinearIndex(linear_index);
if (isObservedVoxel(voxel)) {
++evaluation_details.num_non_overlapping_voxels;
}
}
continue;
}
const Block<VoxelType>& gt_block = layer_gt.getBlockByIndex(block_index);
typename Block<VoxelType>::Ptr error_block;
if (error_layer != nullptr) {
error_block = error_layer->allocateBlockPtrByIndex(block_index);
}
for (size_t linear_index = 0u; linear_index < num_voxels_per_block;
++linear_index) {
FloatingPoint error = 0.0;
const VoxelEvaluationResult result =
computeVoxelError(gt_block.getVoxelByLinearIndex(linear_index),
test_block.getVoxelByLinearIndex(linear_index),
voxel_evaluation_mode, &error);
switch (result) {
case VoxelEvaluationResult::kEvaluated:
total_squared_error += error * error;
evaluation_details.min_error =
std::min(evaluation_details.min_error, std::abs(error));
evaluation_details.max_error =
std::max(evaluation_details.max_error, std::abs(error));
++evaluation_details.num_evaluated_voxels;
++evaluation_details.num_overlapping_voxels;
if (error_block) {
VoxelType& error_voxel =
error_block->getVoxelByLinearIndex(linear_index);
setVoxelSdf<VoxelType>(std::abs(error), &error_voxel);
setVoxelWeight<VoxelType>(1.0, &error_voxel);
}
break;
case VoxelEvaluationResult::kIgnored:
++evaluation_details.num_ignored_voxels;
++evaluation_details.num_overlapping_voxels;
break;
case VoxelEvaluationResult::kNoOverlap:
++evaluation_details.num_non_overlapping_voxels;
break;
default:
LOG(FATAL) << "Unkown voxel evaluation result: "
<< static_cast<int>(result);
}
}
}
// Iterate over all blocks in the grond truth layer and look them up in the
// test truth layer. This is only done to get the exact number of
// non-overlapping voxels.
BlockIndexList gt_block_list;
layer_gt.getAllAllocatedBlocks(>_block_list);
for (const BlockIndex& gt_block_index : gt_block_list) {
const Block<VoxelType>& gt_block = layer_gt.getBlockByIndex(gt_block_index);
if (!layer_test.hasBlock(gt_block_index)) {
for (size_t linear_index = 0u; linear_index < num_voxels_per_block;
++linear_index) {
const VoxelType& voxel = gt_block.getVoxelByLinearIndex(linear_index);
if (isObservedVoxel(voxel)) {
++evaluation_details.num_non_overlapping_voxels;
}
}
}
}
// Return the RMSE.
if (evaluation_details.num_evaluated_voxels == 0) {
evaluation_details.rmse = 0.0;
} else {
evaluation_details.rmse =
sqrt(total_squared_error / evaluation_details.num_evaluated_voxels);
}
// If the details are requested, output them.
if (evaluation_result != nullptr) {
*evaluation_result = evaluation_details;
}
VLOG(2) << evaluation_details.toString();
return evaluation_details.rmse;
}
Reference：
ROS之roslaunch中node标签解读_launch中node节点_宗而研之的博客-CSDN博客