C
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
#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);// 初始化 gflaggoogle::InstallFailureSignalHandler();//默认捕捉 SIGSEGV ros::NodeHandle nh;ros::NodeHandle nh_private("~");cblox::SubmapServer<cblox::TsdfSubmap> node(nh, nh_private);// 关键语句，表明ros会捕捉并执行TsdfSubmapros::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_NEWFloatingPoint 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_serversrc/active_submap_visualizersrc/trajectory_visualizersrc/submap_conversions
)
依次阅读上述提到的依赖代码
4.1 submap_server
可以获得几个信息，首先
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进行可视化，以及colorvertex_marker.colors.push_back(color_msg);}}block_num++;}
4.2 active_submap_visualizer
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 memoryfor (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) { // 多线程创建Meshintegration_threads.emplace_back(&MeshIntegrator::generateMeshBlocksFunction, this, all_tsdf_blocks,clear_updated_flag, index_getter.get());}
4.3 trajectory_visualizer
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
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 integratortsdf_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_Gray_end = origin + unit_ray * ray_length;// 射线的开始点为原点(voxel_carving_enabled==true)，为否，射线直接为0 end-endray_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 submapconst SubmapID submap_id = submap_collection_ptr_->createNewSubmap(T_G_C);// Activating the submap in the frame integratortsdf_submap_collection_integrator_ptr_->switchToActiveSubmap();// Resetting current submap countersnum_integrated_frames_current_submap_ = 0;// Updating the active submap mesheractive_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 factorytsdf_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的IDnew_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 IDsSubmapID 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 submapsconst 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 foundif ((id_submap_pair_1_it != id_to_submap_.end()) &&(id_submap_pair_2_it != id_to_submap_.end())) {// Getting the submapstypename 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 poseconst 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 #2const 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中，这是一个索引listlayer_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 pointconst Point voxel_center =T_out_in *input_blockputeCoordinatesFromLinearIndex(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)) {// 清空outoutput_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 vectorconst 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 centerconst 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 transformconst 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 positionfor (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 inputconst Point voxel_center =T_in_out * block->computeCoordinatesFromLinearIndex(voxel_idx);// interpolate voxelif (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 layerTsdfMap::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 submapsfor (const auto& id_submap_pair : id_to_submap_) {// Getting the tsdf submap and its poseconst 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 layermergeLayerAintoLayerB(tsdf_map.getTsdfLayer(), T_G_S,combined_tsdf_layer_ptr);}// Returning the new mapreturn 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博客
glog 的常用函数介绍_cmake链接glog_liumengyang1992的博客-CSDN博客
Google Glog使用_google-glog_瞻邈的博客-CSDN博客
发布评论