feat:map添加中文注释
parent
a92e8b2a9c
commit
e7913bc1dd
|
|
@ -11,11 +11,17 @@ class Row;
|
||||||
|
|
||||||
class Grid;
|
class Grid;
|
||||||
|
|
||||||
|
// Index 结构体，表示地图中的一个索引位置，包含三个维度（k, j, i）
|
||||||
struct Index {
|
struct Index {
|
||||||
int k, j, i;
|
int k, j, i; // 三个维度的索引
|
||||||
|
|
||||||
|
// 默认构造函数
|
||||||
Index() = default;
|
Index() = default;
|
||||||
|
|
||||||
|
// 构造函数，初始化k, j, i
|
||||||
Index(int k, int j, int i) : k(k), j(j), i(i) {}
|
Index(int k, int j, int i) : k(k), j(j), i(i) {}
|
||||||
|
|
||||||
|
// 用于排序的比较函数对象，按k、j、i的顺序依次比较
|
||||||
struct Comp {
|
struct Comp {
|
||||||
bool operator()(const Index &idx1, const Index &idx2) const {
|
bool operator()(const Index &idx1, const Index &idx2) const {
|
||||||
return (idx1.k < idx2.k) || (idx1.k == idx2.k && idx1.j < idx2.j) ||
|
return (idx1.k < idx2.k) || (idx1.k == idx2.k && idx1.j < idx2.j) ||
|
||||||
|
|
@ -24,53 +30,75 @@ struct Index {
|
||||||
};
|
};
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// Map 类，表示一个三维网格地图
|
||||||
class Map {
|
class Map {
|
||||||
public:
|
public:
|
||||||
|
// 构造函数，使用地图的形状和步长初始化地图
|
||||||
Map(const std::vector<int> &shape, const std::vector<double> &step);
|
Map(const std::vector<int> &shape, const std::vector<double> &step);
|
||||||
|
|
||||||
|
// 构造函数，使用地图的形状和步长初始化地图
|
||||||
Map(const std::vector<int> &shape, double step);
|
Map(const std::vector<int> &shape, double step);
|
||||||
|
|
||||||
|
// 根据索引返回指定位置的点云
|
||||||
TPointCloudPtr &at(const Index &index);
|
TPointCloudPtr &at(const Index &index);
|
||||||
|
|
||||||
|
// 根据索引返回指定位置的点云（const版本）
|
||||||
const TPointCloudPtr &at(const Index &index) const;
|
const TPointCloudPtr &at(const Index &index) const;
|
||||||
|
|
||||||
|
// 清空地图
|
||||||
void clear();
|
void clear();
|
||||||
|
|
||||||
|
// Z轴方向上平移地图n个单位
|
||||||
void ShiftZ(int n);
|
void ShiftZ(int n);
|
||||||
|
|
||||||
|
// Y轴方向上平移地图n个单位
|
||||||
void ShiftY(int n);
|
void ShiftY(int n);
|
||||||
|
|
||||||
|
// X轴方向上平移地图n个单位
|
||||||
void ShiftX(int n);
|
void ShiftX(int n);
|
||||||
|
|
||||||
|
// 返回地图的形状
|
||||||
const std::vector<int> shape() const {
|
const std::vector<int> shape() const {
|
||||||
return std::vector<int>(shape_, shape_ + 3);
|
return std::vector<int>(shape_, shape_ + 3);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 根据点的位置计算该点在地图中的索引
|
||||||
Index GetIndex(const TPoint &point) const;
|
Index GetIndex(const TPoint &point) const;
|
||||||
|
|
||||||
|
// 检查索引是否有效
|
||||||
bool CheckIndex(const Index &index) const;
|
bool CheckIndex(const Index &index) const;
|
||||||
|
|
||||||
|
// 获取基于点和子地图形状的点云
|
||||||
TPointCloudPtr GetSubmapPoints(const TPoint &point,
|
TPointCloudPtr GetSubmapPoints(const TPoint &point,
|
||||||
const std::vector<int> &submap_shapes) const;
|
const std::vector<int> &submap_shapes) const;
|
||||||
|
|
||||||
|
// 获取所有的点云
|
||||||
TPointCloudPtr GetAllPoints() const;
|
TPointCloudPtr GetAllPoints() const;
|
||||||
|
|
||||||
|
// 向地图中添加点云
|
||||||
void AddPoints(const TPointCloudConstPtr &cloud,
|
void AddPoints(const TPointCloudConstPtr &cloud,
|
||||||
std::vector<Index> *const indices = nullptr);
|
std::vector<Index> *const indices = nullptr);
|
||||||
|
|
||||||
|
// 对点云进行下采样
|
||||||
void Downsample(double voxel_size);
|
void Downsample(double voxel_size);
|
||||||
|
|
||||||
|
// 对指定的点云索引进行下采样
|
||||||
void Downsample(const std::vector<Index> &indices, double voxel_size);
|
void Downsample(const std::vector<Index> &indices, double voxel_size);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
// 获取指定的Z轴索引的网格
|
||||||
Grid &at(int z_idx);
|
Grid &at(int z_idx);
|
||||||
|
|
||||||
|
// 获取指定的Z轴索引的网格（const版本）
|
||||||
const Grid &at(int z_idx) const;
|
const Grid &at(int z_idx) const;
|
||||||
|
|
||||||
|
// 获取指定Z轴和Y轴索引的行
|
||||||
Row &at(int z_idx, int y_idx);
|
Row &at(int z_idx, int y_idx);
|
||||||
|
|
||||||
|
// 获取指定Z轴和Y轴索引的行（const版本）
|
||||||
const Row &at(int z_idx, int y_idx) const;
|
const Row &at(int z_idx, int y_idx) const;
|
||||||
|
|
||||||
|
// 获取指定Z轴、Y轴和X轴索引的点云
|
||||||
TPointCloudPtr &at(int z_idx, int y_idx, int x_idx);
|
TPointCloudPtr &at(int z_idx, int y_idx, int x_idx);
|
||||||
|
|
||||||
const TPointCloudPtr &at(int z_idx, int y_idx, int x_idx) const;
|
const TPointCloudPtr &at(int z_idx, int y_idx, int x_idx) const;
|
||||||
|
|
@ -78,47 +106,60 @@ class Map {
|
||||||
std::vector<Index> GetSubmapIndices(
|
std::vector<Index> GetSubmapIndices(
|
||||||
const TPoint &point, const std::vector<int> &submap_shapes) const;
|
const TPoint &point, const std::vector<int> &submap_shapes) const;
|
||||||
|
|
||||||
|
// 获取地图的所有索引
|
||||||
std::vector<Index> GetAllIndices() const;
|
std::vector<Index> GetAllIndices() const;
|
||||||
|
|
||||||
int shape_[3], center_[3]; // order: z, y, x
|
int shape_[3], center_[3]; // 地图的形状和中心位置（z, y, x）
|
||||||
|
|
||||||
double step_[3];
|
double step_[3]; // 步长（分辨率）
|
||||||
|
|
||||||
std::vector<Grid> map_;
|
std::vector<Grid> map_; // 存储地图的网格
|
||||||
|
|
||||||
DISALLOW_COPY_AND_ASSIGN(Map);
|
DISALLOW_COPY_AND_ASSIGN(Map); // 禁止拷贝和赋值
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// Row 类，表示地图中的一行，包含多个点云
|
||||||
class Row {
|
class Row {
|
||||||
public:
|
public:
|
||||||
|
// 构造函数，初始化行的大小
|
||||||
explicit Row(int n);
|
explicit Row(int n);
|
||||||
|
|
||||||
|
// 根据索引返回指定位置的点云
|
||||||
TPointCloudPtr &at(int idx);
|
TPointCloudPtr &at(int idx);
|
||||||
|
|
||||||
|
// 根据索引返回指定位置的点云（const版本）
|
||||||
const TPointCloudPtr &at(int idx) const;
|
const TPointCloudPtr &at(int idx) const;
|
||||||
|
|
||||||
|
// 清空该行的点云
|
||||||
void clear();
|
void clear();
|
||||||
|
|
||||||
|
// 获取该行的所有点云
|
||||||
TPointCloudPtr GetAllPoints() const;
|
TPointCloudPtr GetAllPoints() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::vector<TPointCloudPtr> row_;
|
std::vector<TPointCloudPtr> row_; // 存储行的点云
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// Grid 类，表示地图中的一个网格，包含多个行
|
||||||
class Grid {
|
class Grid {
|
||||||
public:
|
public:
|
||||||
|
// 构造函数，初始化网格的大小
|
||||||
Grid(int m, int n);
|
Grid(int m, int n);
|
||||||
|
|
||||||
|
// 清空网格
|
||||||
void clear();
|
void clear();
|
||||||
|
|
||||||
|
// 获取指定索引的行
|
||||||
Row &at(int idx);
|
Row &at(int idx);
|
||||||
|
|
||||||
|
// 获取指定索引的行（const版本）
|
||||||
const Row &at(int idx) const;
|
const Row &at(int idx) const;
|
||||||
|
|
||||||
|
// 获取该网格的所有点云
|
||||||
TPointCloudPtr GetAllPoints() const;
|
TPointCloudPtr GetAllPoints() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::vector<Row> grid_;
|
std::vector<Row> grid_; // 存储网格中的行
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace oh_my_loam
|
} // namespace oh_my_loam
|
||||||
|
|
@ -10,254 +10,307 @@
|
||||||
|
|
||||||
namespace oh_my_loam {
|
namespace oh_my_loam {
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
using common::YAMLConfig;
|
using common::YAMLConfig;
|
||||||
using LineCoeff = Eigen::Matrix<double, 6, 1>; // 定义直线系数类型，包含 6 个元素
|
using LineCoeff =
|
||||||
} // namespace
|
Eigen::Matrix<double, 6, 1>; // 定义直线系数类型，包含 6 个元素
|
||||||
|
} // namespace
|
||||||
|
|
||||||
// 初始化地图参数和设置
|
// 初始化地图参数和设置
|
||||||
bool Mapper::Init() {
|
bool Mapper::Init() {
|
||||||
const auto &config = YAMLConfig::Instance()->config(); // 获取配置文件
|
const auto &config = YAMLConfig::Instance()->config(); // 获取配置文件
|
||||||
config_ = config["mapper_config"]; // 读取映射配置
|
config_ = config["mapper_config"]; // 读取映射配置
|
||||||
is_vis_ = config_["vis"].as<bool>(); // 是否启用可视化
|
is_vis_ = config_["vis"].as<bool>(); // 是否启用可视化
|
||||||
verbose_ = config_["verbose"].as<bool>(); // 是否输出详细日志
|
verbose_ = config_["verbose"].as<bool>(); // 是否输出详细日志
|
||||||
AINFO << "Mapping visualizer: " << (is_vis_ ? "ON" : "OFF"); // 打印可视化状态
|
AINFO << "Mapping visualizer: "
|
||||||
map_shape_ = YAMLConfig::GetSeq<int>(config_["map_shape"]); // 获取地图的形状
|
<< (is_vis_ ? "ON" : "OFF"); // 打印可视化状态
|
||||||
submap_shape_ = YAMLConfig::GetSeq<int>(config_["submap_shape"]); // 获取子地图的形状
|
map_shape_ = YAMLConfig::GetSeq<int>(config_["map_shape"]); // 获取地图的形状
|
||||||
map_step_ = config_["map_step"].as<double>(); // 获取地图分辨率
|
submap_shape_ =
|
||||||
corn_map_.reset(new Map(map_shape_, map_step_)); // 初始化角点地图
|
YAMLConfig::GetSeq<int>(config_["submap_shape"]); // 获取子地图的形状
|
||||||
surf_map_.reset(new Map(map_shape_, map_step_)); // 初始化表面地图
|
map_step_ = config_["map_step"].as<double>(); // 获取地图分辨率
|
||||||
if (is_vis_) visualizer_.reset(new MapperVisualizer); // 如果启用可视化，初始化可视化器
|
corn_map_.reset(new Map(map_shape_, map_step_)); // 初始化角点地图
|
||||||
return true;
|
surf_map_.reset(new Map(map_shape_, map_step_)); // 初始化表面地图
|
||||||
}
|
if (is_vis_)
|
||||||
|
visualizer_.reset(new MapperVisualizer); // 如果启用可视化，初始化可视化器
|
||||||
// 重置地图
|
return true;
|
||||||
void Mapper::Reset() {
|
|
||||||
SetState(UN_INIT); // 设置状态为未初始化
|
|
||||||
}
|
|
||||||
|
|
||||||
// 处理每一帧点云数据
|
|
||||||
void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
|
|
||||||
const TPointCloudConstPtr &cloud_surf,
|
|
||||||
const common::Pose3d &pose_curr2odom,
|
|
||||||
common::Pose3d *const pose_curr2map) {
|
|
||||||
if (GetState() == UN_INIT) { // 如果地图尚未初始化
|
|
||||||
pose_curr2map->SetIdentity(); // 设置当前位姿为单位矩阵
|
|
||||||
pose_odom2map_.SetIdentity(); // 设置里程计到地图的变换为单位矩阵
|
|
||||||
UpdateMap(*pose_curr2map, cloud_corn, cloud_surf); // 更新地图
|
|
||||||
SetState(DONE); // 设置状态为已完成
|
|
||||||
AINFO << "Mapper initialized..."; // 打印初始化完成信息
|
|
||||||
return;
|
|
||||||
}
|
}
|
||||||
if (GetState() == DONE) { // 如果地图已初始化
|
|
||||||
thread_.reset(new std::thread(&Mapper::Run, this, cloud_corn, cloud_surf,
|
// 重置地图
|
||||||
pose_curr2odom)); // 启动新的线程来运行地图更新
|
void Mapper::Reset() {
|
||||||
if (thread_->joinable()) thread_->detach(); // 分离线程
|
SetState(UN_INIT); // 设置状态为未初始化
|
||||||
}
|
}
|
||||||
std::lock_guard<std::mutex> lock(state_mutex_); // 锁住状态，避免多线程竞争
|
|
||||||
*pose_curr2map = pose_odom2map_ * pose_curr2odom; // 计算当前位姿到地图的变换
|
|
||||||
AINFO << "Pose_curr2map = " << pose_curr2map->ToString(); // 打印位姿
|
|
||||||
}
|
|
||||||
|
|
||||||
// 执行映射操作
|
// 处理每一帧点云数据
|
||||||
void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
|
void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
|
||||||
const TPointCloudConstPtr &cloud_surf,
|
const TPointCloudConstPtr &cloud_surf,
|
||||||
const common::Pose3d &pose_curr2odom) {
|
|
||||||
BLOCK_TIMER_START;
|
|
||||||
SetState(RUNNING); // 设置状态为运行中
|
|
||||||
common::Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom; // 计算当前位姿到地图的变换
|
|
||||||
TPoint t_vec(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(),
|
|
||||||
pose_curr2map.t_vec().z()); // 提取平移向量
|
|
||||||
AdjustMap(t_vec); // 调整地图
|
|
||||||
TPointCloudPtr cloud_corn_map =
|
|
||||||
corn_map_->GetSubmapPoints(t_vec, submap_shape_); // 获取角点子地图
|
|
||||||
TPointCloudPtr cloud_surf_map =
|
|
||||||
surf_map_->GetSubmapPoints(t_vec, submap_shape_); // 获取表面子地图
|
|
||||||
kdtree_corn_.setInputCloud(cloud_corn_map); // 设置角点的kdtree
|
|
||||||
kdtree_surf_.setInputCloud(cloud_surf_map); // 设置表面点云的kdtree
|
|
||||||
std::vector<PointLineCoeffPair> pl_pairs;
|
|
||||||
std::vector<PointPlaneCoeffPair> pp_pairs;
|
|
||||||
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) { // 进行ICP迭代
|
|
||||||
std::vector<PointLineCoeffPair>().swap(pl_pairs);
|
|
||||||
std::vector<PointPlaneCoeffPair>().swap(pp_pairs);
|
|
||||||
MatchCorn(cloud_corn, pose_curr2map, &pl_pairs); // 匹配角点
|
|
||||||
MatchSurf(cloud_surf, pose_curr2map, &pp_pairs); // 匹配表面点
|
|
||||||
AINFO_IF(verbose_) << "Iter " << i
|
|
||||||
<< ": matched num: point2line = " << pl_pairs.size()
|
|
||||||
<< ", point2plane = " << pp_pairs.size(); // 输出匹配结果
|
|
||||||
if (pl_pairs.size() + pp_pairs.size() <
|
|
||||||
config_["min_correspondence_num"].as<size_t>()) { // 如果匹配点对太少，跳过当前迭代
|
|
||||||
AWARN << "Too less correspondence: " << pl_pairs.size() << " + "
|
|
||||||
<< pp_pairs.size();
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
PoseSolver solver(pose_curr2map); // 创建姿态求解器
|
|
||||||
for (const auto &pair : pl_pairs) {
|
|
||||||
solver.AddPointLineCoeffPair(pair, 1.0); // 添加点-直线配对
|
|
||||||
}
|
|
||||||
for (const auto &pair : pp_pairs) {
|
|
||||||
solver.AddPointPlaneCoeffPair(pair, 1.0); // 添加点-平面配对
|
|
||||||
}
|
|
||||||
if (!solver.Solve(config_["solve_iter_num"].as<int>(), verbose_,
|
|
||||||
&pose_curr2map)) { // 解决优化问题
|
|
||||||
AWARN << "Mapping solve: no_convergence";
|
|
||||||
}
|
|
||||||
AINFO_IF(verbose_) << "Odometer::ICP: iter_" << i << ": "
|
|
||||||
<< BLOCK_TIMER_STOP_FMT;
|
|
||||||
}
|
|
||||||
UpdateMap(pose_curr2map, cloud_corn, cloud_surf); // 更新地图
|
|
||||||
std::lock_guard<std::mutex> lock(state_mutex_); // 锁住状态，避免多线程竞争
|
|
||||||
pose_odom2map_ = pose_curr2map * pose_curr2odom.Inv(); // 计算新的里程计到地图的变换
|
|
||||||
AINFO << "Pose_curr2map = " << pose_curr2map.ToString(); // 打印位姿
|
|
||||||
AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT;
|
|
||||||
if (is_vis_) Visualize(pl_pairs, pp_pairs, pose_curr2odom, pose_curr2map); // 可视化结果
|
|
||||||
state_ = DONE; // 设置状态为已完成，注意死锁问题
|
|
||||||
}
|
|
||||||
|
|
||||||
// 匹配角点
|
|
||||||
void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr,
|
|
||||||
const common::Pose3d &pose_curr2map,
|
|
||||||
std::vector<PointLineCoeffPair> *const pairs) const {
|
|
||||||
if (kdtree_corn_.getInputCloud()->empty()) return; // 如果输入点云为空，返回
|
|
||||||
std::vector<int> indices;
|
|
||||||
std::vector<float> dists;
|
|
||||||
int nearest_neighbor_k = config_["nearest_neighbor_k"].as<int>(); // 获取最近邻数目
|
|
||||||
float neighbor_point_dist_sq_th =
|
|
||||||
config_["neighbor_point_dist_sq_th"].as<float>(); // 获取邻点距离阈值
|
|
||||||
for (const auto &pt : *cloud_curr) { // 遍历当前点云
|
|
||||||
TPoint pt_query = common::TransformPoint(pose_curr2map, pt); // 变换当前点
|
|
||||||
if (kdtree_corn_.nearestKSearch(pt_query, nearest_neighbor_k, indices,
|
|
||||||
dists) != nearest_neighbor_k) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
if (dists.back() > neighbor_point_dist_sq_th) continue; // 如果距离太大，跳过
|
|
||||||
TPointCloud neighbor_pts;
|
|
||||||
pcl::copyPointCloud(*kdtree_corn_.getInputCloud(), indices, neighbor_pts); // 获取邻域点云
|
|
||||||
double fit_score = 0.0;
|
|
||||||
LineCoeff coeff = common::FitLine3D(neighbor_pts, &fit_score); // 拟合直线
|
|
||||||
if (fit_score < config_["min_line_fit_score"].as<double>()) continue; // 如果拟合分数太低，跳过
|
|
||||||
pairs->emplace_back(pt, coeff); // 添加点-直线配对
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 匹配表面点
|
|
||||||
void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr,
|
|
||||||
const common::Pose3d &pose_curr2map,
|
|
||||||
std::vector<PointPlaneCoeffPair> *const pairs) const {
|
|
||||||
if (kdtree_surf_.getInputCloud()->empty()) return; // 如果输入点云为空，返回
|
|
||||||
std::vector<int> indices;
|
|
||||||
std::vector<float> dists;
|
|
||||||
int nearest_neighbor_k = config_["nearest_neighbor_k"].as<int>(); // 获取最近邻数目
|
|
||||||
float neighbor_point_dist_sq_th =
|
|
||||||
config_["neighbor_point_dist_sq_th"].as<float>(); // 获取邻点距离阈值
|
|
||||||
for (const auto &pt : *cloud_curr) { // 遍历当前点云
|
|
||||||
TPoint pt_query = common::TransformPoint(pose_curr2map, pt); // 变换当前点
|
|
||||||
if (kdtree_surf_.nearestKSearch(pt_query, nearest_neighbor_k, indices,
|
|
||||||
dists) != nearest_neighbor_k) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
if (dists.back() > neighbor_point_dist_sq_th) continue; // 如果距离太大，跳过
|
|
||||||
TPointCloud neighbor_pts;
|
|
||||||
pcl::copyPointCloud(*kdtree_surf_.getInputCloud(), indices, neighbor_pts); // 获取邻域点云
|
|
||||||
double fit_score = 0.0;
|
|
||||||
Eigen::Vector4d coeff = common::FitPlane(neighbor_pts, &fit_score); // 拟合平面
|
|
||||||
if (fit_score < config_["min_plane_fit_score"].as<double>()) continue; // 如果拟合分数太低，跳过
|
|
||||||
TPoint pt1(coeff[0], coeff[1], coeff[2], 0.0); // 生成平面上的点
|
|
||||||
pairs->emplace_back(pt, coeff);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Mapper::AdjustMap(const TPoint ¢er) {
|
|
||||||
Index index = corn_map_->GetIndex(center);
|
|
||||||
int min_idx_z = submap_shape_[0] / 2 + 1;
|
|
||||||
int max_idx_z = map_shape_[0] - min_idx_z - 1;
|
|
||||||
if (index.k < min_idx_z) {
|
|
||||||
corn_map_->ShiftZ(index.k - min_idx_z);
|
|
||||||
surf_map_->ShiftZ(index.k - min_idx_z);
|
|
||||||
}
|
|
||||||
if (index.k > max_idx_z) {
|
|
||||||
corn_map_->ShiftZ(index.k - max_idx_z);
|
|
||||||
surf_map_->ShiftZ(index.k - max_idx_z);
|
|
||||||
}
|
|
||||||
int min_idx_y = submap_shape_[0] / 2 + 1;
|
|
||||||
int max_idx_y = map_shape_[1] - min_idx_y - 1;
|
|
||||||
if (index.j < min_idx_y) {
|
|
||||||
corn_map_->ShiftY(index.j - min_idx_y);
|
|
||||||
surf_map_->ShiftY(index.j - min_idx_y);
|
|
||||||
}
|
|
||||||
if (index.j > max_idx_y) {
|
|
||||||
corn_map_->ShiftY(index.j - max_idx_y);
|
|
||||||
surf_map_->ShiftY(index.j - max_idx_y);
|
|
||||||
}
|
|
||||||
int min_idx_x = submap_shape_[0] / 2 + 1;
|
|
||||||
int max_idx_x = map_shape_[2] - min_idx_x - 1;
|
|
||||||
if (index.i < min_idx_x) {
|
|
||||||
corn_map_->ShiftX(index.i - min_idx_x);
|
|
||||||
surf_map_->ShiftX(index.i - min_idx_x);
|
|
||||||
}
|
|
||||||
if (index.i > max_idx_x) {
|
|
||||||
corn_map_->ShiftX(index.i - max_idx_x);
|
|
||||||
surf_map_->ShiftX(index.i - max_idx_x);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Mapper::UpdateMap(const common::Pose3d &pose_curr2map,
|
|
||||||
const TPointCloudConstPtr &cloud_corn,
|
|
||||||
const TPointCloudConstPtr &cloud_surf) {
|
|
||||||
auto update_map = [&](const TPointCloudConstPtr &cloud,
|
|
||||||
Map *const map /* const TPointCloudPtr &cloud_map*/) {
|
|
||||||
TPointCloudPtr cloud_trans(new TPointCloud);
|
|
||||||
common::TransformPointCloud(pose_curr2map, *cloud, cloud_trans.get());
|
|
||||||
std::vector<Index> indices;
|
|
||||||
map->AddPoints(cloud_trans, &indices);
|
|
||||||
map->Downsample(indices, config_["downsample_voxel_size"].as<double>());
|
|
||||||
};
|
|
||||||
std::lock_guard<std::mutex> lock(map_mutex_);
|
|
||||||
update_map(cloud_corn, corn_map_.get());
|
|
||||||
update_map(cloud_surf, surf_map_.get());
|
|
||||||
}
|
|
||||||
|
|
||||||
TPointCloudPtr Mapper::GetMapCloudCorn() const {
|
|
||||||
std::lock_guard<std::mutex> lock(map_mutex_);
|
|
||||||
return corn_map_->GetAllPoints();
|
|
||||||
}
|
|
||||||
|
|
||||||
TPointCloudPtr Mapper::GetMapCloudSurf() const {
|
|
||||||
std::lock_guard<std::mutex> lock(map_mutex_);
|
|
||||||
return surf_map_->GetAllPoints();
|
|
||||||
}
|
|
||||||
|
|
||||||
TPointCloudPtr Mapper::GetMapCloud() const {
|
|
||||||
TPointCloudPtr map_points(new TPointCloud);
|
|
||||||
std::lock_guard<std::mutex> lock(map_mutex_);
|
|
||||||
*map_points += *corn_map_->GetAllPoints();
|
|
||||||
*map_points += *surf_map_->GetAllPoints();
|
|
||||||
return map_points;
|
|
||||||
}
|
|
||||||
|
|
||||||
Mapper::State Mapper::GetState() const {
|
|
||||||
std::lock_guard<std::mutex> lock(state_mutex_);
|
|
||||||
return state_;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Mapper::SetState(State state) {
|
|
||||||
std::lock_guard<std::mutex> lock(state_mutex_);
|
|
||||||
state_ = state;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Mapper::Visualize(const std::vector<PointLineCoeffPair> &pl_pairs,
|
|
||||||
const std::vector<PointPlaneCoeffPair> &pp_pairs,
|
|
||||||
const common::Pose3d &pose_curr2odom,
|
const common::Pose3d &pose_curr2odom,
|
||||||
const common::Pose3d &pose_curr2map, double timestamp) {
|
common::Pose3d *const pose_curr2map) {
|
||||||
std::shared_ptr<MapperVisFrame> frame(new MapperVisFrame);
|
if (GetState() == UN_INIT) { // 如果地图尚未初始化
|
||||||
frame->timestamp = timestamp;
|
pose_curr2map->SetIdentity(); // 设置当前位姿为单位矩阵
|
||||||
frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared();
|
pose_odom2map_.SetIdentity(); // 设置里程计到地图的变换为单位矩阵
|
||||||
frame->cloud_surf = kdtree_surf_.getInputCloud()->makeShared();
|
UpdateMap(*pose_curr2map, cloud_corn, cloud_surf); // 更新地图
|
||||||
frame->pl_pairs = pl_pairs;
|
SetState(DONE); // 设置状态为已完成
|
||||||
frame->pp_pairs = pp_pairs;
|
AINFO << "Mapper initialized..."; // 打印初始化完成信息
|
||||||
frame->pose_curr2odom = pose_curr2odom;
|
return;
|
||||||
frame->pose_curr2map = pose_curr2map;
|
}
|
||||||
visualizer_->Render(frame);
|
if (GetState() == DONE) { // 如果地图已初始化
|
||||||
}
|
thread_.reset(
|
||||||
|
new std::thread(&Mapper::Run, this, cloud_corn, cloud_surf,
|
||||||
|
pose_curr2odom)); // 启动新的线程来运行地图更新
|
||||||
|
if (thread_->joinable()) thread_->detach(); // 分离线程
|
||||||
|
}
|
||||||
|
std::lock_guard<std::mutex> lock(state_mutex_); // 锁住状态，避免多线程竞争
|
||||||
|
*pose_curr2map = pose_odom2map_ * pose_curr2odom; // 计算当前位姿到地图的变换
|
||||||
|
AINFO << "Pose_curr2map = " << pose_curr2map->ToString(); // 打印位姿
|
||||||
|
}
|
||||||
|
|
||||||
|
// 执行映射操作
|
||||||
|
void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
|
||||||
|
const TPointCloudConstPtr &cloud_surf,
|
||||||
|
const common::Pose3d &pose_curr2odom) {
|
||||||
|
BLOCK_TIMER_START;
|
||||||
|
SetState(RUNNING); // 设置状态为运行中
|
||||||
|
common::Pose3d pose_curr2map =
|
||||||
|
pose_odom2map_ * pose_curr2odom; // 计算当前位姿到地图的变换
|
||||||
|
TPoint t_vec(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(),
|
||||||
|
pose_curr2map.t_vec().z()); // 提取平移向量
|
||||||
|
AdjustMap(t_vec); // 调整地图，使得当前位姿处于地图的中心附近
|
||||||
|
TPointCloudPtr cloud_corn_map =
|
||||||
|
corn_map_->GetSubmapPoints(t_vec, submap_shape_); // 获取角点子地图
|
||||||
|
TPointCloudPtr cloud_surf_map =
|
||||||
|
surf_map_->GetSubmapPoints(t_vec, submap_shape_); // 获取表面子地图
|
||||||
|
kdtree_corn_.setInputCloud(cloud_corn_map); // 设置角点的kdtree
|
||||||
|
kdtree_surf_.setInputCloud(cloud_surf_map); // 设置表面点云的kdtree
|
||||||
|
std::vector<PointLineCoeffPair> pl_pairs;
|
||||||
|
std::vector<PointPlaneCoeffPair> pp_pairs;
|
||||||
|
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) { // 进行ICP迭代
|
||||||
|
std::vector<PointLineCoeffPair>().swap(pl_pairs);
|
||||||
|
std::vector<PointPlaneCoeffPair>().swap(pp_pairs);
|
||||||
|
MatchCorn(cloud_corn, pose_curr2map, &pl_pairs); // 匹配角点
|
||||||
|
MatchSurf(cloud_surf, pose_curr2map, &pp_pairs); // 匹配表面点
|
||||||
|
AINFO_IF(verbose_) << "Iter " << i
|
||||||
|
<< ": matched num: point2line = " << pl_pairs.size()
|
||||||
|
<< ", point2plane = "
|
||||||
|
<< pp_pairs.size(); // 输出匹配结果
|
||||||
|
if (pl_pairs.size() + pp_pairs.size() <
|
||||||
|
config_["min_correspondence_num"]
|
||||||
|
.as<size_t>()) { // 如果匹配点对太少，跳过当前迭代
|
||||||
|
AWARN << "Too less correspondence: " << pl_pairs.size() << " + "
|
||||||
|
<< pp_pairs.size();
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
PoseSolver solver(pose_curr2map); // 创建姿态求解器
|
||||||
|
for (const auto &pair : pl_pairs) {
|
||||||
|
solver.AddPointLineCoeffPair(pair, 1.0); // 添加点-直线配对
|
||||||
|
}
|
||||||
|
for (const auto &pair : pp_pairs) {
|
||||||
|
solver.AddPointPlaneCoeffPair(pair, 1.0); // 添加点-平面配对
|
||||||
|
}
|
||||||
|
if (!solver.Solve(config_["solve_iter_num"].as<int>(), verbose_,
|
||||||
|
&pose_curr2map)) { // 解决优化问题
|
||||||
|
AWARN << "Mapping solve: no_convergence";
|
||||||
|
}
|
||||||
|
AINFO_IF(verbose_) << "Odometer::ICP: iter_" << i << ": "
|
||||||
|
<< BLOCK_TIMER_STOP_FMT;
|
||||||
|
}
|
||||||
|
UpdateMap(pose_curr2map, cloud_corn, cloud_surf); // 更新地图
|
||||||
|
std::lock_guard<std::mutex> lock(state_mutex_); // 锁住状态，避免多线程竞争
|
||||||
|
pose_odom2map_ =
|
||||||
|
pose_curr2map * pose_curr2odom.Inv(); // 计算新的里程计到地图的变换
|
||||||
|
AINFO << "Pose_curr2map = " << pose_curr2map.ToString(); // 打印位姿
|
||||||
|
AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT;
|
||||||
|
if (is_vis_)
|
||||||
|
Visualize(pl_pairs, pp_pairs, pose_curr2odom, pose_curr2map); // 可视化结果
|
||||||
|
state_ = DONE; // 设置状态为已完成，注意死锁问题
|
||||||
|
}
|
||||||
|
|
||||||
|
// 匹配角点
|
||||||
|
void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr,
|
||||||
|
const common::Pose3d &pose_curr2map,
|
||||||
|
std::vector<PointLineCoeffPair> *const pairs) const {
|
||||||
|
if (kdtree_corn_.getInputCloud()->empty()) return; // 如果输入点云为空，返回
|
||||||
|
std::vector<int> indices;
|
||||||
|
std::vector<float> dists;
|
||||||
|
int nearest_neighbor_k =
|
||||||
|
config_["nearest_neighbor_k"].as<int>(); // 获取最近邻数目
|
||||||
|
float neighbor_point_dist_sq_th =
|
||||||
|
config_["neighbor_point_dist_sq_th"].as<float>(); // 获取邻点距离阈值
|
||||||
|
for (const auto &pt : *cloud_curr) { // 遍历当前点云
|
||||||
|
TPoint pt_query = common::TransformPoint(pose_curr2map, pt); // 变换当前点
|
||||||
|
if (kdtree_corn_.nearestKSearch(pt_query, nearest_neighbor_k, indices,
|
||||||
|
dists) != nearest_neighbor_k) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (dists.back() > neighbor_point_dist_sq_th)
|
||||||
|
continue; // 如果距离太大，跳过
|
||||||
|
TPointCloud neighbor_pts;
|
||||||
|
pcl::copyPointCloud(*kdtree_corn_.getInputCloud(), indices,
|
||||||
|
neighbor_pts); // 获取邻域点云
|
||||||
|
double fit_score = 0.0;
|
||||||
|
LineCoeff coeff = common::FitLine3D(neighbor_pts, &fit_score); // 拟合直线
|
||||||
|
if (fit_score < config_["min_line_fit_score"].as<double>())
|
||||||
|
continue; // 如果拟合分数太低，跳过
|
||||||
|
pairs->emplace_back(pt, coeff); // 添加点-直线配对
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 匹配表面点
|
||||||
|
void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr,
|
||||||
|
const common::Pose3d &pose_curr2map,
|
||||||
|
std::vector<PointPlaneCoeffPair> *const pairs) const {
|
||||||
|
if (kdtree_surf_.getInputCloud()->empty()) return; // 如果输入点云为空，返回
|
||||||
|
std::vector<int> indices;
|
||||||
|
std::vector<float> dists;
|
||||||
|
int nearest_neighbor_k =
|
||||||
|
config_["nearest_neighbor_k"].as<int>(); // 获取最近邻数目
|
||||||
|
float neighbor_point_dist_sq_th =
|
||||||
|
config_["neighbor_point_dist_sq_th"].as<float>(); // 获取邻点距离阈值
|
||||||
|
for (const auto &pt : *cloud_curr) { // 遍历当前点云
|
||||||
|
TPoint pt_query = common::TransformPoint(pose_curr2map, pt); // 变换当前点
|
||||||
|
if (kdtree_surf_.nearestKSearch(pt_query, nearest_neighbor_k, indices,
|
||||||
|
dists) != nearest_neighbor_k) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (dists.back() > neighbor_point_dist_sq_th)
|
||||||
|
continue; // 如果距离太大，跳过
|
||||||
|
TPointCloud neighbor_pts;
|
||||||
|
pcl::copyPointCloud(*kdtree_surf_.getInputCloud(), indices,
|
||||||
|
neighbor_pts); // 获取邻域点云
|
||||||
|
double fit_score = 0.0;
|
||||||
|
Eigen::Vector4d coeff =
|
||||||
|
common::FitPlane(neighbor_pts, &fit_score); // 拟合平面
|
||||||
|
if (fit_score < config_["min_plane_fit_score"].as<double>())
|
||||||
|
continue; // 如果拟合分数太低，跳过
|
||||||
|
TPoint pt1(coeff[0], coeff[1], coeff[2], 0.0); // 生成平面上的点
|
||||||
|
pairs->emplace_back(pt, coeff);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// 调整地图的显示范围和更新位置
|
||||||
|
void Mapper::AdjustMap(const TPoint ¢er) {
|
||||||
|
// 获取当前点在角点地图中的索引
|
||||||
|
Index index = corn_map_->GetIndex(center);
|
||||||
|
|
||||||
|
// Z轴方向上的最小和最大索引值，用于调整地图的上下边界
|
||||||
|
int min_idx_z = submap_shape_[0] / 2 + 1; // 最小Z轴索引
|
||||||
|
int max_idx_z = map_shape_[0] - min_idx_z - 1; // 最大Z轴索引
|
||||||
|
if (index.k < min_idx_z) { // 如果当前索引小于最小值
|
||||||
|
// 调整Z轴位置
|
||||||
|
corn_map_->ShiftZ(index.k - min_idx_z);
|
||||||
|
surf_map_->ShiftZ(index.k - min_idx_z);
|
||||||
|
}
|
||||||
|
if (index.k > max_idx_z) { // 如果当前索引大于最大值
|
||||||
|
// 调整Z轴位置
|
||||||
|
corn_map_->ShiftZ(index.k - max_idx_z);
|
||||||
|
surf_map_->ShiftZ(index.k - max_idx_z);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Y轴方向上的最小和最大索引值，用于调整地图的前后边界
|
||||||
|
int min_idx_y = submap_shape_[0] / 2 + 1; // 最小Y轴索引
|
||||||
|
int max_idx_y = map_shape_[1] - min_idx_y - 1; // 最大Y轴索引
|
||||||
|
if (index.j < min_idx_y) { // 如果当前索引小于最小值
|
||||||
|
// 调整Y轴位置
|
||||||
|
corn_map_->ShiftY(index.j - min_idx_y);
|
||||||
|
surf_map_->ShiftY(index.j - min_idx_y);
|
||||||
|
}
|
||||||
|
if (index.j > max_idx_y) { // 如果当前索引大于最大值
|
||||||
|
// 调整Y轴位置
|
||||||
|
corn_map_->ShiftY(index.j - max_idx_y);
|
||||||
|
surf_map_->ShiftY(index.j - max_idx_y);
|
||||||
|
}
|
||||||
|
|
||||||
|
// X轴方向上的最小和最大索引值，用于调整地图的左右边界
|
||||||
|
int min_idx_x = submap_shape_[0] / 2 + 1; // 最小X轴索引
|
||||||
|
int max_idx_x = map_shape_[2] - min_idx_x - 1; // 最大X轴索引
|
||||||
|
if (index.i < min_idx_x) { // 如果当前索引小于最小值
|
||||||
|
// 调整X轴位置
|
||||||
|
corn_map_->ShiftX(index.i - min_idx_x);
|
||||||
|
surf_map_->ShiftX(index.i - min_idx_x);
|
||||||
|
}
|
||||||
|
if (index.i > max_idx_x) { // 如果当前索引大于最大值
|
||||||
|
// 调整X轴位置
|
||||||
|
corn_map_->ShiftX(index.i - max_idx_x);
|
||||||
|
surf_map_->ShiftX(index.i - max_idx_x);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 更新地图：根据当前的位姿和角点、表面点云更新地图
|
||||||
|
void Mapper::UpdateMap(const common::Pose3d &pose_curr2map,
|
||||||
|
const TPointCloudConstPtr &cloud_corn,
|
||||||
|
const TPointCloudConstPtr &cloud_surf) {
|
||||||
|
// 定义一个局部函数，用于更新地图
|
||||||
|
auto update_map = [&](const TPointCloudConstPtr &cloud,
|
||||||
|
Map *const map /* const TPointCloudPtr &cloud_map*/) {
|
||||||
|
TPointCloudPtr cloud_trans(new TPointCloud); // 创建存储变换后点云的指针
|
||||||
|
common::TransformPointCloud(pose_curr2map, *cloud,
|
||||||
|
cloud_trans.get()); // 根据位姿变换点云
|
||||||
|
std::vector<Index> indices; // 存储点云索引
|
||||||
|
map->AddPoints(cloud_trans, &indices); // 将变换后的点云加入地图
|
||||||
|
map->Downsample(
|
||||||
|
indices,
|
||||||
|
config_["downsample_voxel_size"].as<double>()); // 对点云进行下采样
|
||||||
|
};
|
||||||
|
|
||||||
|
// 使用锁来保证多线程安全
|
||||||
|
std::lock_guard<std::mutex> lock(map_mutex_);
|
||||||
|
|
||||||
|
// 更新角点地图和表面地图
|
||||||
|
update_map(cloud_corn, corn_map_.get());
|
||||||
|
update_map(cloud_surf, surf_map_.get());
|
||||||
|
}
|
||||||
|
|
||||||
|
// 获取角点地图的所有点云
|
||||||
|
TPointCloudPtr Mapper::GetMapCloudCorn() const {
|
||||||
|
std::lock_guard<std::mutex> lock(map_mutex_); // 锁住，避免多线程竞争
|
||||||
|
return corn_map_->GetAllPoints(); // 返回角点地图中的所有点
|
||||||
|
}
|
||||||
|
|
||||||
|
// 获取表面地图的所有点云
|
||||||
|
TPointCloudPtr Mapper::GetMapCloudSurf() const {
|
||||||
|
std::lock_guard<std::mutex> lock(map_mutex_); // 锁住，避免多线程竞争
|
||||||
|
return surf_map_->GetAllPoints(); // 返回表面地图中的所有点
|
||||||
|
}
|
||||||
|
|
||||||
|
// 获取所有地图（包括角点和表面点云）
|
||||||
|
TPointCloudPtr Mapper::GetMapCloud() const {
|
||||||
|
TPointCloudPtr map_points(new TPointCloud); // 创建一个新的点云指针
|
||||||
|
std::lock_guard<std::mutex> lock(map_mutex_); // 锁住，避免多线程竞争
|
||||||
|
|
||||||
|
// 将角点地图和表面地图的所有点加入到最终的地图中
|
||||||
|
*map_points += *corn_map_->GetAllPoints();
|
||||||
|
*map_points += *surf_map_->GetAllPoints();
|
||||||
|
|
||||||
|
return map_points; // 返回完整的地图
|
||||||
|
}
|
||||||
|
|
||||||
|
// 获取当前的地图状态
|
||||||
|
Mapper::State Mapper::GetState() const {
|
||||||
|
std::lock_guard<std::mutex> lock(state_mutex_); // 锁住状态
|
||||||
|
return state_; // 返回当前状态
|
||||||
|
}
|
||||||
|
|
||||||
|
// 设置地图的状态
|
||||||
|
void Mapper::SetState(State state) {
|
||||||
|
std::lock_guard<std::mutex> lock(state_mutex_); // 锁住状态
|
||||||
|
state_ = state; // 设置新的状态
|
||||||
|
}
|
||||||
|
|
||||||
|
// 可视化匹配的点对和位姿信息
|
||||||
|
void Mapper::Visualize(const std::vector<PointLineCoeffPair> &pl_pairs,
|
||||||
|
const std::vector<PointPlaneCoeffPair> &pp_pairs,
|
||||||
|
const common::Pose3d &pose_curr2odom,
|
||||||
|
const common::Pose3d &pose_curr2map, double timestamp) {
|
||||||
|
std::shared_ptr<MapperVisFrame> frame(new MapperVisFrame); // 创建可视化帧
|
||||||
|
frame->timestamp = timestamp; // 设置时间戳
|
||||||
|
frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared(); // 设置角点云
|
||||||
|
frame->cloud_surf =
|
||||||
|
kdtree_surf_.getInputCloud()->makeShared(); // 设置表面点云
|
||||||
|
frame->pl_pairs = pl_pairs; // 设置点-直线配对
|
||||||
|
frame->pp_pairs = pp_pairs; // 设置点-平面配对
|
||||||
|
frame->pose_curr2odom = pose_curr2odom; // 设置当前里程计位姿
|
||||||
|
frame->pose_curr2map = pose_curr2map; // 设置当前地图位姿
|
||||||
|
|
||||||
|
// 渲染可视化帧
|
||||||
|
visualizer_->Render(frame);
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace oh_my_loam
|
} // namespace oh_my_loam
|
||||||
Loading…
Reference in New Issue