From fc91a30636606ddc2696e940c82934ca1670b0a6 Mon Sep 17 00:00:00 2001 From: 12345qiupeng Date: Fri, 21 Feb 2025 10:36:49 +0800 Subject: [PATCH] =?UTF-8?q?feat:=E6=B7=BB=E5=8A=A0=E4=B8=AD=E6=96=87?= =?UTF-8?q?=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- examples/main_noros.cc | 37 +++++++++++++++----------- oh_my_loam/oh_my_loam.cc | 57 ++++++++++++++++++++++++---------------- 2 files changed, 55 insertions(+), 39 deletions(-) diff --git a/examples/main_noros.cc b/examples/main_noros.cc index 7ddbff7..a38d0fa 100644 --- a/examples/main_noros.cc +++ b/examples/main_noros.cc @@ -12,56 +12,61 @@ using namespace common; using namespace oh_my_loam; namespace fs = std::filesystem; +// 点云处理函数 void PointCloudHandler(const PointCloudConstPtr &cloud, OhMyLoam *const slam); int main(int argc, char *argv[]) { + // 检查输入参数 if (argc != 3) { std::cerr << "\033[1m\033[31mConfiguration file and input path should be " "specified!\033[m" << std::endl; return -1; } - // config + // 初始化配置文件 YAMLConfig::Instance()->Init(argv[1]); bool is_log_to_file = YAMLConfig::Instance()->Get("log_to_file"); std::string log_path = YAMLConfig::Instance()->Get("log_path"); std::string lidar = YAMLConfig::Instance()->Get("lidar"); - // logging + // 初始化日志系统 InitG3Logging(is_log_to_file, "oh_my_loam_" + lidar, log_path); AUSER << "LOAM start..., lidar = " << lidar; - // SLAM system + // 初始化SLAM系统 OhMyLoam slam; if (!slam.Init()) { - AFATAL << "Failed to initilize slam system."; + AFATAL << "Failed to initilize slam system."; // SLAM系统初始化失败 } - // get input point cloud file names - ACHECK(fs::exists(argv[2])) << "Directory not exist: " << argv[2]; + // 获取输入的点云文件路径 + ACHECK(fs::exists(argv[2])) << "Directory not exist: " << argv[2]; // 检查目录是否存在 std::vector cloud_paths; for (auto &it : fs::directory_iterator(argv[2])) { - if (fs::is_regular_file(it.path())) cloud_paths.push_back(it.path()); + if (fs::is_regular_file(it.path())) cloud_paths.push_back(it.path()); // 仅处理常规文件 } - AWARN_IF(cloud_paths.empty()) + AWARN_IF(cloud_paths.empty()) // 如果目录中没有点云文件，发出警告 << "No point cloud file in directory: " << argv[2]; - AINFO_IF(!cloud_paths.empty()) + AINFO_IF(!cloud_paths.empty()) // 如果有点云文件，输出文件数量信息 << "There are " << cloud_paths.size() << " point clouds in total"; + // 对点云文件进行排序 std::sort(cloud_paths.begin(), cloud_paths.end()); - // load point cloud and process + // 加载点云并处理 for (auto &path : cloud_paths) { PointCloudPtr cloud(new PointCloud); - pcl::io::loadPCDFile(path.string(), *cloud); - PointCloudHandler(cloud, &slam); + pcl::io::loadPCDFile(path.string(), *cloud); // 加载PCD文件 + PointCloudHandler(cloud, &slam); // 处理点云 std::this_thread::sleep_for(std::chrono::milliseconds(100)); // 10 Hz } return 0; } +// 点云处理函数的实现 void PointCloudHandler(const PointCloudConstPtr &cloud, OhMyLoam *const slam) { + // 获取当前时间戳 auto millisecs = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()); - double timestamp = millisecs.count() / 1000.0; + double timestamp = millisecs.count() / 1000.0; // 转换为秒 static size_t frame_id = 0; - AINFO << "Ohmyloam: frame_id = " << ++frame_id + AINFO << "Ohmyloam: frame_id = " << ++frame_id // 帧ID << ", timestamp = " << FMT_TIMESTAMP(timestamp) - << ", point_number = " << cloud->size(); - slam->Run(timestamp, cloud); + << ", point_number = " << cloud->size(); // 输出当前帧信息 + slam->Run(timestamp, cloud); // 使用SLAM系统处理当前点云 } \ No newline at end of file diff --git a/oh_my_loam/oh_my_loam.cc b/oh_my_loam/oh_my_loam.cc index 09278a8..aadb0a2 100644 --- a/oh_my_loam/oh_my_loam.cc +++ b/oh_my_loam/oh_my_loam.cc @@ -8,28 +8,34 @@ namespace oh_my_loam { namespace { -const double kPointMinDist = 0.5; +const double kPointMinDist = 0.5; // 点云中点的最小距离阈值 } // namespace +// 初始化OhMyLoam系统 bool OhMyLoam::Init() { + // 加载配置文件 config_ = common::YAMLConfig::Instance()->config(); - is_vis_ = config_["vis"].as(); + is_vis_ = config_["vis"].as(); // 是否启用可视化 + // 创建提取器实例 extractor_.reset(common::Registerer::NewInstance( "Extractor" + config_["lidar"].as())); - if (!extractor_->Init()) { - AERROR << "Failed to initialize extractor"; + if (!extractor_->Init()) { // 初始化提取器失败 + AERROR << "Failed to initialize extractor"; // 初始化里程计失败 return false; } + // 创建里程计实例 odometer_.reset(new Odometer); if (!odometer_->Init()) { AERROR << "Failed to initialize odometer"; return false; } + // 创建地图生成器实例 mapper_.reset(new Mapper); - if (!mapper_->Init()) { + if (!mapper_->Init()) { // 初始化地图生成器失败 AERROR << "Failed to initialize mapper"; return false; } + // 如果启用了可视化，初始化可视化器 if (is_vis_) { visualizer_.reset( new OhmyloamVisualizer(config_["save_map_path"].as())); @@ -37,49 +43,54 @@ bool OhMyLoam::Init() { return true; } +// 如果启用了可视化，初始化可视化器 void OhMyLoam::Reset() { - AWARN << "OhMySlam RESET"; - extractor_->Reset(); - odometer_->Reset(); - mapper_->Reset(); + AWARN << "OhMySlam RESET"; // 输出重置警告信息 + extractor_->Reset(); // 重置提取器 + odometer_->Reset(); // 重置里程计 + mapper_->Reset(); // 重置地图生成器 } +// 处理每一帧的点云数据 void OhMyLoam::Run(double timestamp, const common::PointCloudConstPtr &cloud_in) { common::PointCloudPtr cloud(new common::PointCloud); - RemoveOutliers(*cloud_in, cloud.get()); + RemoveOutliers(*cloud_in, cloud.get()); // 去除离群点 std::vector features; - extractor_->Process(timestamp, cloud, &features); + extractor_->Process(timestamp, cloud, &features); // 提取特征 common::Pose3d pose_curr2odom; - odometer_->Process(timestamp, features, &pose_curr2odom); + odometer_->Process(timestamp, features, &pose_curr2odom); // 处理里程计数据 common::Pose3d pose_curr2map; - const auto &cloud_corn = odometer_->GetCloudCorn()->makeShared(); - const auto &cloud_surf = odometer_->GetCloudSurf()->makeShared(); + const auto &cloud_corn = odometer_->GetCloudCorn()->makeShared(); // 获取稀疏点云 + const auto &cloud_surf = odometer_->GetCloudSurf()->makeShared(); // 获取平面点云 mapper_->Process(timestamp, cloud_corn, cloud_surf, pose_curr2odom, - &pose_curr2map); + &pose_curr2map); // 处理地图生成 if (is_vis_) { - Visualize(pose_curr2map, cloud_corn, cloud_surf, timestamp); + Visualize(pose_curr2map, cloud_corn, cloud_surf, timestamp); // 可视化 } } +// 可视化函数 void OhMyLoam::Visualize(const common::Pose3d &pose_curr2map, const TPointCloudConstPtr &cloud_corn, const TPointCloudConstPtr &cloud_surf, double timestamp) { - std::shared_ptr frame(new OhmyloamVisFrame); + std::shared_ptr frame(new OhmyloamVisFrame); // 创建可视化帧 frame->timestamp = timestamp; - frame->cloud_map_corn = mapper_->GetMapCloudCorn(); - frame->cloud_map_surf = mapper_->GetMapCloudSurf(); - frame->cloud_corn = cloud_corn; - frame->cloud_surf = cloud_surf; - frame->pose_map = pose_curr2map; - visualizer_->Render(frame); + frame->cloud_map_corn = mapper_->GetMapCloudCorn(); // 获取当前地图的角点点云 + frame->cloud_map_surf = mapper_->GetMapCloudSurf(); // 获取当前地图的平面点云 + frame->cloud_corn = cloud_corn; // 当前帧的稀疏点云 + frame->cloud_surf = cloud_surf; // 当前帧的平面点云 + frame->pose_map = pose_curr2map; // 当前帧的地图位姿 + visualizer_->Render(frame); // 渲染可视化帧 } +// 去除点云中的离群点 void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in, common::PointCloud *const cloud_out) const { common::RemovePoints( cloud_in, cloud_out, [&](const common::Point &pt) { + // 去除不合理的点（非有限值或距离过小的点） return !common::IsFinite(pt) || common::DistanceSquare(pt) < kPointMinDist * kPointMinDist; });