feat:添加中文注释
parent
3c4963d289
commit
fc91a30636
|
|
@ -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<bool>("log_to_file");
|
||||
std::string log_path = YAMLConfig::Instance()->Get<std::string>("log_path");
|
||||
std::string lidar = YAMLConfig::Instance()->Get<std::string>("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<fs::path> 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::milliseconds>(
|
||||
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系统处理当前点云
|
||||
}
|
||||
|
|
@ -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<bool>();
|
||||
is_vis_ = config_["vis"].as<bool>(); // 是否启用可视化
|
||||
// 创建提取器实例
|
||||
extractor_.reset(common::Registerer<Extractor>::NewInstance(
|
||||
"Extractor" + config_["lidar"].as<std::string>()));
|
||||
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<std::string>()));
|
||||
|
|
@ -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<Feature> 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<OhmyloamVisFrame> frame(new OhmyloamVisFrame);
|
||||
std::shared_ptr<OhmyloamVisFrame> 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<common::Point>(
|
||||
cloud_in, cloud_out, [&](const common::Point &pt) {
|
||||
// 去除不合理的点（非有限值或距离过小的点）
|
||||
return !common::IsFinite(pt) ||
|
||||
common::DistanceSquare(pt) < kPointMinDist * kPointMinDist;
|
||||
});
|
||||
|
|
|
|||
Loading…
Reference in New Issue