深蓝学院高翔《自动驾驶与机器人中的SLAM技术》第五章作业
第一题
- 使用 g2o
- 在图优化外部进行最近邻搜索
- 分别实现点到点 ICP ， 点到线 ICP
// 在 ICP 类中增加两个成员函数
bool AlignGaussNewtonPoint2PointWithg2o(SE2& init_pose);
bool AlignGaussNewtonPoint2PlaneWithg2o(SE2& init_pose);
点到点的 ICP
- 首先需要设置图优化所需的边，在 g2o_types.h 中增加 EdgeSE2Point2Point 类
pandoc input.md -o output.pdf
class EdgeSE2Point2Point : public g2o::BaseUnaryEdge<2, Vec2d, VertexSE2> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeSE2Point2Point(const Vec2d& point)
: point_(point){}
void computeError() override {
VertexSE2* v = (VertexSE2*)_vertices[0];
SE2 pose = v->estimate();
Vec2d pw = pose * point_;
_error = pw - measurement();
}
bool read(std::istream& is) override { return true; }
bool write(std::ostream& os) const override { return true; }
private:
Vec2d point_;
};
- 实现点到点 ICP
bool Icp2d::AlignGaussNewtonPoint2PointWithg2o(SE2& init_pose)
{
int iterations = 10;
SE2 current_pose = init_pose;
const float max_dis = 0.3; // 最近邻时的最远距离
const int min_effect_pts = 20; // 最小有效点数
// pose graph optimization
using BlockSolverType = g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>>;
using LinearSolverType = g2o::LinearSolverCholmod<BlockSolverType::PoseMatrixType>;
auto* solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
for (int i = 0; i < iterations; i++) {
optimizer.clear();
auto* v = new VertexSE2();
v->setId(0);
v->setEstimate(current_pose);
optimizer.addVertex(v);
int effective_num = 0; // 有效点数
for (size_t j = 0; j < source_scan_->ranges.size(); ++j) {
float r = source_scan_->ranges[j];
if (r < source_scan_->range_min || r > source_scan_->range_max) {
continue;
}
float angle = source_scan_->angle_min + j * source_scan_->angle_increment;
Vec2d pd = Vec2d(r * std::cos(angle), r * std::sin(angle));
Vec2d pw = current_pose * pd;
Point2d pt;
pt.x = pw.x();
pt.y = pw.y();
std::vector<int> nn_idx;
std::vector<float> dis;
kdtree_.nearestKSearch(pt, 1, nn_idx, dis);
if (nn_idx.size() > 0 && dis[0] < max_dis) {
effective_num++;
auto* edge = new EdgeSE2Point2Point(pd);
edge->setId(j);
edge->setVertex(0, v);
edge->setMeasurement(Vec2d(target_cloud_->points[nn_idx[0]].x, target_cloud_->points[nn_idx[0]].y));
edge->setInformation(Mat2d::Identity());
optimizer.addEdge(edge);
}
}
if (effective_num < min_effect_pts) {
return false;
}
optimizer.setVerbose(true);
optimizer.initializeOptimization();
optimizer.optimize(5);
current_pose = v->estimate();
}
init_pose = current_pose;
LOG(INFO) << "estimated pose: " << current_pose.translation().transpose()
<< ", theta: " << current_pose.so2().log();
return true;
}
点到线的 ICP
- 同样需要设置图优化所需的边，在 g2o_types.h 中增加 EdgeSE2Point2Plane 类
class EdgeSE2Point2Plane : public g2o::BaseUnaryEdge<1, double, VertexSE2> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeSE2Point2Plane(const Vec2d& point, const Vec3d& plane)
: point_(point), plane_(plane){}
void computeError() override {
VertexSE2* v = (VertexSE2*)_vertices[0];
SE2 pose = v->estimate();
Vec2d pw = pose * point_;
_error[0] = plane_[0] * pw[0] + plane_[1] * pw[1] + plane_[2];
}
bool read(std::istream& is) override { return true; }
bool write(std::ostream& os) const override { return true; }
private:
Vec2d point_;
Vec3d plane_;
};
- 实现点到线 ICP
bool Icp2d::AlignGaussNewtonPoint2PlaneWithg2o(SE2& init_pose)
{
int iterations = 10;
SE2 current_pose = init_pose;
const float max_dis = 0.3; // 最近邻时的最远距离
const int min_effect_pts = 20; // 最小有效点数
// pose graph optimization
using BlockSolverType = g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>>;
using LinearSolverType = g2o::LinearSolverCholmod<BlockSolverType::PoseMatrixType>;
auto* solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
for (int iter = 0; iter < iterations; iter++) {
optimizer.clear();
auto* v = new VertexSE2();
v->setId(0);
v->setEstimate(current_pose);
optimizer.addVertex(v);
int effective_num = 0; // 有效点数
for (size_t i = 0; i < source_scan_->ranges.size(); ++i) {
float r = source_scan_->ranges[i];
if (r < source_scan_->range_min || r > source_scan_->range_max) {
continue;
}
float angle = source_scan_->angle_min + i * source_scan_->angle_increment;
float theta = current_pose.so2().log();
Vec2d pd = Vec2d(r * std::cos(angle), r * std::sin(angle));
Vec2d pw = current_pose * pd;
Point2d pt;
pt.x = pw.x();
pt.y = pw.y();
// 查找5个最近邻
std::vector<int> nn_idx;
std::vector<float> dis;
kdtree_.nearestKSearch(pt, 5, nn_idx, dis);
std::vector<Vec2d> effective_pts; // 有效点
for (int j = 0; j < nn_idx.size(); ++j) {
if (dis[j] < max_dis) {
effective_pts.emplace_back(
Vec2d(target_cloud_->points[nn_idx[j]].x, target_cloud_->points[nn_idx[j]].y));
}
}
// 拟合直线，有效点数不足，跳过
if (effective_pts.size() < 3) {
continue;
}
// 拟合直线，组装J、H和误差
Vec3d line_coeffs;
if (math::FitLine2D(effective_pts, line_coeffs)) {
effective_num++;
auto* edge = new EdgeSE2Point2Plane(pd, line_coeffs);
edge->setId(i);
edge->setVertex(0, v);
edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
optimizer.addEdge(edge);
}
}
if (effective_num < min_effect_pts) {
return false;
}
optimizer.setVerbose(true);
optimizer.initializeOptimization();
optimizer.optimize(5);
current_pose = v->estimate();
}
init_pose = current_pose;
LOG(INFO) << "estimated pose: " << current_pose.translation().transpose()
<< ", theta: " << current_pose.so2().log();
return true;
}
第二题
- 使用 math_utils.h 中的 GetPixelValue() 进行双线性插值，修改likelihood_field.cc 中求图像梯度的部分
// 图像梯度，把似然场函数定义成一个光滑的函数，这里用中心差分
// float dx = 0.5 * (field_.at<float>(pf[1], pf[0] + 1) - field_.at<float>(pf[1], pf[0] - 1));
// float dy = 0.5 * (field_.at<float>(pf[1] + 1, pf[0]) - field_.at<float>(pf[1] - 1, pf[0]));
// 双线性插值
float dx = 0.5 * (math::GetPixelValue<float>(field_, pf[0] + 1, pf[1]) -
math::GetPixelValue<float>(field_, pf[0] - 1, pf[1]));
float dy = 0.5 * (math::GetPixelValue<float>(field_, pf[0], pf[1] + 1) -
math::GetPixelValue<float>(field_, pf[0], pf[1] - 1));
第三题
- 首先截取示例数据中的两帧作为检测是否退化的数据点云
- 使用欧式聚类对点云进行聚类
- 对聚类的结果中类中点数超过150的类进行直线拟合，采用math_utils.h中的直线拟合方法FitLine2D()
- 计算各个直线的斜率与角度
- 比较所有拟合直线间的角度差异，存在差距大的就说明在该帧没有退化现象，如果拟合直线间角度差距很小，说明可能是空旷场景没有拟合直线或者在类似长廊中的退化场景中
// 读取点云
pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>::Ptr cloud_filtered(new pcl::PointCloud<PointType>);
pcl::io::loadPCDFile(FLAGS_pcd_path, *cloud);
LOG(INFO) << "cloud points: " << cloud->size();
pcl::StatisticalOutlierRemoval<PointType> sor;
sor.setInputCloud(cloud);
sor.setMeanK(30);
sor.setStddevMulThresh(1.0);
sor.filter(*cloud_filtered);
pcl::search::KdTree<PointType>::Ptr kdtree_;
kdtree_ = boost::make_shared<pcl::search::KdTree<PointType>>();
kdtree_->setInputCloud(cloud_filtered);
pcl::EuclideanClusterExtraction<PointType> cluster;
std::vector<pcl::PointIndices> clusters; // 创建一个向量来存储聚类的结果
cluster.setClusterTolerance(0.03); // 设置聚类的距离阈值为0.03m
cluster.setMinClusterSize(10); // 设置聚类的最小点数为10个点
cluster.setMaxClusterSize(1000); // 设置聚类的最大点数为1000个点
cluster.setSearchMethod(kdtree_); // 设置点云的搜索机制
cluster.setInputCloud(cloud_filtered);
cluster.extract(clusters); // 从点云中提取聚类，并将点云索引保存在cluster_indices中
LOG(INFO) << "cluster size: " << clusters.size();
pcl::visualization::PCLVisualizer cluster_viewer("cluster viewer");
cluster_viewer.setBackgroundColor(0, 0, 0);
cluster_viewer.addPointCloud<PointType>(cloud_filtered, "cloud");
int cluster_num = 1;
int line_num = 0;
std::vector<Eigen::Vector2d> linecoeffs_cloud;
std::vector<double> slope;
std::vector<double> angle;
for (const auto& indice : clusters) {
if(indice.indices.size() < 150) {
LOG(INFO) << "Cluster " << cluster_num << " has " << indice.indices.size() << " points." << "点数太少，不进行直线拟合";
cluster_num++;
continue;
}
LOG(INFO) << "Cluster " << cluster_num << " has " << indice.indices.size() << " points.";
pcl::PointCloud<PointType>::Ptr cluster_cloud(new pcl::PointCloud<PointType>);
pcl::copyPointCloud(*cloud_filtered, indice, *cluster_cloud);
// 将pcl::PointXYZ类型的点云转换为Eigen::Vector3d类型的点云
std::vector<Eigen::Vector2d> pts;
pts.reserve(cluster_cloud->size());
for (const PointType& pt : *cluster_cloud) {
pts.emplace_back(Eigen::Vector2d(pt.x, pt.y));
}
Eigen::Vector3d line_coeffs;
// 利用当前点附近的几个有效近邻点，基于SVD奇异值分解，拟合出ax+by+c=0 中的最小直线系数 a,b,c
if (sad::math::FitLine2D(pts, line_coeffs)) {
if(line_coeffs[0] < 0.0)
line_coeffs = -line_coeffs;
double s = -line_coeffs[0] / line_coeffs[1];
slope.emplace_back(s);
double a = std::atan(slope[line_num]);
angle.emplace_back(a);
LOG(INFO) << "line_coeffs: " << line_coeffs[0] << ", " << line_coeffs[1] << ", " << line_coeffs[2];
LOG(INFO) << "slope: " << slope[line_num];
LOG(INFO) << "angle: " << angle[line_num];
line_num++;
linecoeffs_cloud.emplace_back(line_coeffs.head<2>());
}
// 为聚类分配不同的颜色
double r = static_cast<double>(rand()) / RAND_MAX;
double g = static_cast<double>(rand()) / RAND_MAX;
double b = static_cast<double>(rand()) / RAND_MAX;
std::string clusterId = "cluster_" + std::to_string(cluster_num);
cluster_viewer.addPointCloud<PointType>(cluster_cloud, clusterId);
cluster_viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, r, g, b, clusterId);
cluster_num++;
}
bool k = true;
for (int n = 0; n < angle.size(); n++) {
for (int m = n + 1; m < angle.size(); m++) {
if (std::abs(angle[n] - angle[m]) > 0.2) {
LOG(INFO) << "存在不朝一个方向的拟合直线，不处于退化场景中";
k = false;
}
}
}
if (k) {
LOG(INFO) << "拟合直线之间夹角较小，可能处于退化场景中";
}
-
正常场景
-
下图中可以看到三类主要直线，颜色不同，有两个直线组合之间角度很大
-
退化场景
-
下图中可以看到两类主要直线，颜色不同，两直线之间的角度差异很小，在类似走廊的退化场景中
第四题
-
增加传感器：通过添加额外的传感器（例如：RGBD深度相机等）来捕捉实际场景中的立体深度信息，更好地建立地图，并避开悬空物体和低矮物体。
-
人工干预：对于特定场景中存在的悬空物体和低矮物体，可以事先通过手动标注或预先建模，将这些特殊物体的位置和高度信息加入到地图中，以帮助机器人进行更准确的导航和避障。
1623
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
