0
  • 聊天消息
  • 系统消息
  • 评论与回复
登录后你可以
  • 下载海量资料
  • 学习在线课程
  • 观看技术视频
  • 写文章/发帖/加入社区
会员中心
创作中心

完善资料让更多小伙伴认识你,还能领取20积分哦,立即完善>

3天内不再提示

反光板导航SLAM:VEnus代码浅析

3D视觉工坊 来源:古月居 2023-03-14 10:54 次阅读

通过研究具体的代码我们可以简单了解VEnus中对于反光柱定位的具体流程。

1、IntensityExtraction::Extract

IntensityExtraction::IntensityRange2D &cloud,
                 VEnus::IntensityRange2D &candidate_cloud)

Extract函数的主要作用是从激光点云中提取出高反点,然后存储到对应的容器中。输入的数据类型为

VEnus::IntensityRange2D

IntensityRange2D数据类型是定义在sensor/proto/sensor.proto文件内

message IntensityRange2D
{
  int64 timestamp = 1;
  string frame_id = 2;
  repeated IntensityPoint2D points = 3;
}

IntensityPoint2D的数据格式如下:

message IntensityPoint2D
{
  float x = 1;
  float y = 2;
  float intensity = 3;
}

repeated 类似于std的vector,可以用来存放N个相同类型的内容。所以这个函数的输入cloud是一系列带有强度信息的2D坐标点。函数的candidate_cloud代表的是匹配到的可能的反光柱点。所以它们数据结构是一样的。

然后是函数实现,这个函数其实很简单,它只是对整个点云进行了一次遍历,取连续的n个点,这个由一个参数intensity_median_filter_param决定,如果反光柱粗一点的或者激光分辨率高一点的话可以设置大一点,否则的话可以设置小一点,例如3。

然后对这些点进行判断,只要其中有一半的点云强度超过阈值intensity_threshold则认为这里存在一个反光柱,将其中的点云强度处于中间值的那个点存入到容器candidate_cloud。这里其实有点随机性在里面,因为没有把所有的点都加入到容器中,所以是存在一定遗漏的。

2、DBscanAssociation::Association

DBscanAssociation::IntensityRange2D& candidate_cloud,
                  VEnus::Feature2DList& feature_list)

前面我们提取出了反光柱的中心点,但是这里的点云中可能会出现很多个点代表同一个反光柱的情况。所以这个函数即是对刚才的这些点进行一次类似于聚类的操作。

candidate_cloud为第一步中选出来的高反点。feature_list为可能的反光柱中心。然后我们具体看一下函数实现:

Association的第一部分主要是遍历了所有点,通过调用expand_cluster函数进行一个分类:

 for (; iter < dataset.end(); ++iter) {
    if (iter->status == UNCLASSIFIED) {
   //聚类,将所有高反点根据相互之间距离分类,分类完成的点status为CLASSIFIED,同时处于同一类的点ID一致。
   if (expand_cluster(iter, cluster_id)) {
    cluster_id++;
   }
  }
 }

dataset里面存储的就是当前帧的高反点,来自于candidate_cloud,数据类型为:

PointDBSCAN(double px, double py) {
   x = px;
   y = py;
   status = UNCLASSIFIED;
   id = 0;
  }

可以看到对于每一个高反点,都有坐标x/y、状态status以及id等几个属性。初始状态下状态都为UNCLASSIFIED未区分,id都为0。然后函数对于每个点调用expand_cluster函数。

这个函数的作用是:对于每一个candidate_cloud,找到所有candidate_cloud中的点与其接近的点(两者间距离小于一定阈值)这些点的status都会被设置为CLASSIFIED防止重复判断。id都会被设置为相同代表同一个反光柱。

通过这种方式可以将所有点都划分成一个个点的区域,每个区域代表了一个反光柱。

在划分完成后,当然是要对每个区域做处理:

//按照ID将所有点放到map容器中
 unordered_map > feature_dict;
 iter = dataset.begin();
 for (; iter < dataset.end(); ++iter) {
    if (iter->status == CLASSIFIED) {
   feature_dict[iter->id].push_back(iter - dataset.begin());
  }
 }
 //找到每一组点的中心点的坐标,存放到tmp_res里面
 vector > tmp_res;
 for (auto pr : feature_dict) {
  double center_x = 0;
  double center_y = 0;
  for (int idx : pr.second) {
   center_x += dataset.at(idx).x;
   center_y += dataset.at(idx).y;
  }
  int sz = pr.second.size();
  //ROS_INFO("center point,x = %f,y = %f",center_x / double(sz),center_y / double(sz));
  tmp_res.push_back(std::make_pair(center_x / double(sz), center_y / double(sz)));
 }
 //判断每个中心点之间的距离,将距离过近的中心点视为代表同一个反光柱,更新容器中反光柱中心点坐标
 merge_cluster(tmp_res);

首先这里通过map维护了每一个待选的反光柱信息,所有的同一个反光柱的信息放到一起。

进行一个划分,然后对每一类点集调用merge_cluster进行一个中心点的求取。求取的方式其实很简单就是简单相加求平均。这样子就可以知道了所有反光柱中心点所在的位置。

3、CartoMapping::InsertFeatureList

这个函数的功能还是挺多的,包括匹配,位姿估计,位姿优化等一系列其实都是在这个函数中实现的。详细看一下这个函数具体情况:

3.1 初始化

InsertFeatureList函数的第一步判断了feature_points容器是否为空,这里面存储的是全局坐标下的反光柱。这个容器为空说明目前没有已经确定的反光柱。则进行初始化。

if (feature_points.empty())

初始化的方式比较简单,将第一帧的反光柱坐标存储到feature_points中作为初始状态下反光柱的中心坐标,然后更新FeatureGraph。

注意到这里的函数:InsertToFeatureGraph,这个函数的意义是对于每一个待插入的点(反光柱),计算它与周围反光柱之间的距离,然后存储到feature_graph中。注意到feature_graph的数据类型

第一个int为对应的feature_points的ID,第二个std::unordered_map中的int为与这个feature_points相互关联的反光柱点的ID,后面的double类型数据为两个点之间的距离。

3.2 特征点匹配

SiftNewFeaturePoints(input_fpts, hit_id_pair, wait_insert);

在确认初始化完成的情况下,调用SiftNewFeaturePoints函数进行反光柱的匹配。这个函数有三个参数:input_fpts为前面识别出来的反光柱坐标,hit_id_pair为当前帧反光柱与世界坐标系下的反光柱匹配上的ID,wait_insert为当前帧中没能跟世界坐标系下反光柱所匹配上的点的ID。函数主要执行了以下工作:

首先,对于所有当前点,建立一个局部的local_feature_graph,记录当前点与点之间的距离。

然后,对于每一个当前帧的点,使用其局部local_feature_graph与全局点进行匹配。

注意这里不是点与点的匹配而是类似于线与线的匹配,如果一个点到其他点的距离与全局点中某个点到其周围点的距离基本一致,则认为这两个点是匹配上了的,这时候会把这一对ID存放到hit_id_pair中。这个方式其实应该是有问题的,如果两个点到周围点的距离都很接近,就可能发生误匹配。

最后,对于input_fpts中剩下没有匹配到的点,则会将其存放到wait_insert中,这个数据代表这里检测出一个反光柱但是在全局中没有,后面需要另外处理。

3.3 初始位姿估计

ComputeCurrentPose(localization_hit, pose)

这个函数emmm其实我没看,因为似乎它基本就没有正确计算出来过。不过也不要紧,直接看下一步

3.4 位姿优化

OptimizeCurrentPose(localization_hit, pose);

OptimizeCurrentPose中需要输入两个变量:localization_hit里面存放的是当前帧下匹配点全局点之间的坐标。

pose是机器人当前初步估计下的位姿,按照逻辑它应该会来自于步骤3,但是由于3总是出问题所以大部分情况下它来自于上一次估计出来的位姿。

然后根据输入的约束关系localization_hit以及初始位姿pose,调用ceres优化得到一个新的位姿:

 ceres::Problem problem;
 double pose[3] = {robot_pose.x(), robot_pose.y(), robot_pose.theta()};


 for (auto fpt_pair : match_result) {
  problem.AddResidualBlock(new ceres::AutoDiffCostFunction(
                 new FeaturePairCost(fpt_pair.second, fpt_pair.first)),
               new ceres::CauchyLoss(0.2), pose);
 }


 ceres::Options options;
 options.linear_solver_type = ceres::DENSE_QR;
 options.minimizer_progress_to_stdout = false;


 ceres::Summary summary;
 ceres::Solve(options, &problem, &summary);

最后这里输出得到的是一个新的优化后的位姿pose

3.5 更新反光柱信息

注意到前面3.2中还有一个东西没有处理,就是返回的wait_insert容器。

这个里面存放的是当时检测到的反光柱但是这个反光柱没能与地图上其他地方的反光柱相匹配上,说明它可能是一个新的反光柱,对于这些信息我们要将其更新到最新的反光柱信息中:

 Eigen::Isometry2d matrixT = Eigen::Identity();
 matrixT.pretranslate(Eigen::Vector2d(pose.x(), pose.y()));
 matrixT.rotate(pose.theta());
 //  Eigen::Isometry2d matrixTinv = matrixT.inverse();


 vector > new_fpt_wait_insert;
 vector new_fpt_assigned_id;
 for (auto unhit : wait_insert) {
  Eigen::Vector3d local_fpt(unhit.first, unhit.second, 1.0);
  auto global_fpt = matrixT * local_fpt;


  auto new_fpt = make_pair(global_fpt[0], global_fpt[1]);
  feature_points[feature_point_cnt] = new_fpt;
  new_fpt_wait_insert.push_back(new_fpt);
  new_fpt_assigned_id.push_back(feature_point_cnt);
  feature_point_cnt++;
 }

首先是通过矩阵matrixT将这些反光柱点转到世界坐标系下,然后再根据初始化时候的方式一样更新feature_points容器,存储新的反光柱信息。当然最后还要调用:

InsertToFeatureGraph(new_fpt_wait_insert, new_fpt_assigned_id);

来更新feature_graph,也就是反光柱之间的距离信息。

通过以上一系列步骤我们就可以得到一个连续的基于反光柱匹配的位姿估计算法。当然这个算法还有一点小问题,比如线匹配意味着每一个反光柱到周围反光柱之间的距离都要独一无二,否则就可能出现误匹配的问题。

匹配阈值也不能设计的太大,否则也会发生匹配错误,但是设计的太小就可能导致本来是一个地方的反光柱没有成功匹配上,最后该位置会生成出很多个反光柱,类似于这样:

06b9ee62-c003-11ed-bfe3-dac502259ad0.png

这里先介绍完代码思路,具体的代码实现以及优化过程下一章单独展开介绍。

审核编辑:汤梓红

声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉
  • 导航
    +关注

    关注

    7

    文章

    528

    浏览量

    42411
  • 函数
    +关注

    关注

    3

    文章

    4329

    浏览量

    62588
  • 代码
    +关注

    关注

    30

    文章

    4786

    浏览量

    68563
  • SLAM
    +关注

    关注

    23

    文章

    423

    浏览量

    31827
  • Venus
    +关注

    关注

    0

    文章

    7

    浏览量

    2673

原文标题:反光板导航SLAM:VEnus代码浅析

文章出处:【微信号:3D视觉工坊,微信公众号:3D视觉工坊】欢迎添加关注!文章转载请注明出处。

收藏 人收藏

    评论

    相关推荐

    反光板的选用

    推荐一款用在智能小车上红外测距传感器的反光板吧{:19:}
    发表于 07-19 12:57

    激光导航AGV控制系统解决方案(无反光板非工控机)

    LNM-v1.0(工业级)无轨导航无轨导航LNM-v1.0激光导航模块是一款基于室内复杂坏境的导航模块,该模块通过激光雷达对室内环境进行扫描,自主构建地图并自主规划路径行走。LNM模块
    发表于 06-10 14:07

    AGV激光雷达SLAM定位导航技术

    agv自由行走的问题。  定位和导航一般是相辅相成,传统的定位导航方式(电磁导航、磁条导航)的优缺点如下方图表所示,这些方案的优点和局限性都很明显。稍微灵活点的
    发表于 11-09 15:59

    SLAM技术的应用及发展现状

    结合激光雷达或者摄像头的方法,让扫地机可以高效绘制室内地图,智能分析和规划扫地环境,成功让自己步入了智能导航的阵列。除了扫地机之外,SLAM技术在其他服务机器人(例如商场导购机器人、银行机器人
    发表于 12-06 10:25

    激光SLAM与视觉SLAM有什么区别?

    机器人定位导航中,目前主要涉及到激光SLAM与视觉SLAM,激光SLAM在理论、技术和产品落地上都较为成熟,因而成为现下最为主流的定位导航
    发表于 07-05 06:41

    怎么利用51单片机去实现风扇转速测量的源程序+电路图呢

    项目功能:将被测风扇叶片(三叶风扇)置于红外光电传感器和其反光板之间,当光电传感器接收到对面的反光板反射回来的信号时(即叶片间的空隙通过时)输出低电平,当光电传感器没收到反光板反射回的信号时(即叶片
    发表于 09-08 06:59

    有没有办法用VL53L0X 区分反光板和其他反光材料?

    如您所知,无论反光片表面角度如何,反光片都非常明亮。然而,其他反光材料,如镜子,只有在表面角度垂直于 VL53L0X 时才会亮。在这种情况下,仅凭亮度的差异是很难区分的。 有没有办法用 VL53L0X 区分
    发表于 12-27 06:24

    SLAM算法在AGV小车中的使用

    导航一般是相辅相成,传统的定位导航方式(电磁导航、磁条导航)的优缺点如下方图表所示,这些方案的优点和局限性都很明显。稍微灵活点的导航定位方
    发表于 09-11 16:01 1290次阅读

    艾吉威:智慧物流“刚需”下的推陈出新

    艾吉威成功将无反光板激光自主导航叉车AGV运用于轮胎制造领域,成为全球首例大规模运用无标识导航AGV的案例。
    的头像 发表于 06-19 17:47 2771次阅读

    无人叉车的种类有哪些,关于它的选择方法的介绍

    的无人叉车也成为目前广大用户所关心的问题。 我们在选择无人叉车的时候,要了解它们的性能和结构,其中激光导航叉车可分为分为有反光板和无反光板两种: 我们在选择有反光板激光
    发表于 10-20 12:06 2357次阅读

    关于SEER SRC核心控制器的八问八答

    。 问:SEER 机器人支持哪些导航呢? 答:激光 SLAM 导航,激光反光板导航,二维码导航
    的头像 发表于 11-03 18:18 2191次阅读

    激光SLAM搬运式叉车SFL-CBD20-S

    SFL-CBD20-S采用了目前最稳定最主流的导航方式——激光SLAM导航,无反光板,部署方便,内置仙工智能(SEER)SRC核心控制器,可提供地图构建、定位、
    的头像 发表于 02-04 16:43 2437次阅读

    反光导航开发与实验

    VEnus算法对于反光导航的基本思路,其主要分为了高反点提取、高反点聚类查找中心、高反点与已知反光柱位姿匹配以及调用ceres库进行位姿优化等步骤。
    的头像 发表于 07-14 15:37 640次阅读
    <b class='flag-5'>反光</b>柱<b class='flag-5'>导航</b>开发与实验

    浅析基于SLAM的机器人自主定位导航

    正如图中所示,机器人自主定位导航技术中包括:定位和地图创建(SLAM)与路径规划和运动控制两个部分,而SLAM本身只是完成机器人的定位和地图创建,二者有所区别。
    发表于 08-03 11:12 943次阅读
    <b class='flag-5'>浅析</b>基于<b class='flag-5'>SLAM</b>的机器人自主定位<b class='flag-5'>导航</b>

    光电传感器反光板能否用镜子替代?

    反光板是所有回归反射型光电传感器必不可少的配套元件,它的使命是将光电传感器中的发射器发出去的光反射回来,以便传感器接收器能够接收到光信号。那么一定有不少工程师想过这个问题镜子也可以反光可以用镜子
    的头像 发表于 07-02 08:24 607次阅读
    光电传感器<b class='flag-5'>反光板</b>能否用镜子替代?