本文小编为大家详细介绍“C++Node类Cartographer开始轨迹的处理深度源码分析”，内容详细，步骤清晰，细节处理妥当，希望这篇“C++Node类Cartographer开始轨迹的处理深度源码分析”文章能帮助大家解决疑惑，下面跟着小编的思路慢慢深入，一起来学习新知识吧。在Node.h中,包含了以下几个部分(程序太大,就不贴了):构造,析构,拷贝构造,赋值函数的构建轨迹有关部分传感器数据部分其他部分.比如传感器采样设置,位姿推测器等部分.Node构造函数最重要的两个传入变量是node_options和map_builder. 这两个在上一节中已经详细说过了,map_builder是Cartographer算法部分,包含了前端和后端.构造函数通过使用初始化列表去初始化一些私有变量(node_options_, map_builder_bridge_), 然后使用MutexLock上锁. 初始化列表和智能锁比较基础就不详细介绍了.总之,这个构造函数使用node_options初始化了map_builder_bridge, 而map_builder_bridge又调用Cartographer算法部分的map_builder(前,后端), 同时还确定了要发布的topic,和可视化所需的topic.现在介绍一下Node::AddTrajectory。这块函数是这一节的重中之重了,是node.cc中的核心部分. 它维护了传感器列表, 添加了一条轨迹,新增了一个位姿估计器,传感器数据采样器,还有订阅所需的topic和注册对应的回调函数. 为了确保topic没有重复,他还保存了注册topic的键值对,以供查询是否重复.添加轨迹的函数是AddTrajectory调用了Node的函数ComputeExpectedSensorIds, 作用是根据配置文件,去返回一个传感器列表:在看传感器列表之前咱们先看一下Cartographer中传感器类型的定义:它规定了一个传感器的类型与一个对应的topic的名字. 传感器的类型是一个限域枚举(枚举类). 总之作用就是联系topic与topic对应的传感器类型, 以便后续维护.回到之前的代码,不难看出, 一个轨迹的传感器的列表有一下命名规则:如果只有一个传感器, 那订阅的topic就是topic如果是多个传感器, 那订阅的topic就是topic_1,topic_2, 依次类推(多个超声雷达)3d slam必须有imu, 2d可有可无, imu的topic的个数只能有一个里程计可有可无, topic的个数只能有一个gps可有可无, topic的个数只能有一个Landmark可有可无, topic的个数只能有一个接下来就是最重要的函数, AddTrajectory, Cartographer的核心, 传感器数据和Cartographer算法库连接处的大门, 调用了ros部分的map_builder_bridge和算法部分的map_builder产生联系. 联系的实现方法相当复杂, 将在另一节详细说这一行调用了Map_builder_bridge_的AddTrajectory添加一条轨迹. 传入的参数有一个std::set<...sensor_id>类型的变量,std::set是一个容器,可以简单理解为键值对,而键就是值,值就是键.(比较基础不细说啦).另一个就是从node_main.cc就跟着我们的TrajectoryOptions. 也就是配置文件读取的内容. 返回值很简单,就是新建轨迹的编号. Carto免费云主机域名grapher允许有多个轨迹同时维护,而且后面我们会发现, Cartographer定位其实就是把建好的地图和定位作为两个不同的轨迹实现的. 这个函数是整个Cartographer的功能实现, 方法很复杂, 将会在以后MapBuilder部分详细说.这个函数功能是通过IMU和里程计(轮编码器)去预估下一次可能的位姿,给定位一个初始值这里面有个重点变量:看PoseExtrapolator这个类, 发现功能是使用IMU和/或里程计数据（如果有）来改善预测估计速度与运动. 因为机器人运动,角速度和线速度都不可能变化特别大, 所以可以用上一次的速度去预测下一次的位姿,然后用预测的位姿为初始值去进行优化, 这样的好处是可以以较小的迭代次数获得更好的结果,而且不容易陷入局部最小值.这一部分很简单, 通过使用配置文件, 去给某条轨迹的各个传感器得到的值进行采样,看看sensor_samplers_, 定义如下看看TrajectorySensorSamplers, 发现作用只有控制各个传感器的采样频率.这个函数就挺有意思的, 有值得学的的编程技巧. 同样, 传入的参数只有配置文件和轨迹ID.咱们进入到node.cc里看这个函数本身, 发现它是首先通过配置options判断了是否用了某个传感器, 然后把SubscribeWithHandler压入subscribers_里. 现在, 这里有两个疑问, subscribers_是啥, SubscribeWithHandler又是啥. 咱们先看subscribers_subscribers_是一个无序表, 键是轨迹id, 值是一个数组, 里面放的都是Subscriber:Subscriber是一个结构体, 里面是ros的订阅器和对应的topic名字.所以subscribers_表示某条轨迹的ros订阅器以及订阅topic的名字.然后就是SubscribeWithHandler这个地方有点晦涩, 不过特别优美, 也是值得学习的部分, 通过SubscribeWithHandler这个函数, 实现了所有传感器的订阅,回调函数注册,以及订阅器的维护,简洁明了.首先, 定义了一个模板, 用来实现同一个函数适应不同的传感器类型. 返回值是ros的Subscriber类. 对于第一个参数, 这是一个函数指针,也就是函数的地址. 第二个和第三个就是轨迹id和message的topic, 第四个是ros的nodehandle, 掌控ros节点的开启与关闭,让我们能使用ros的Subscribe, 不重要. 最后一个参数就是当前node类本身,让我们可以在后续使用node类相关的方法和变量.咱们以LaserScan为例看看整体看来,就是通过SubscribeWithHandler这个模板函数,把LaserScan类型的message, 回调函数, 轨迹id, 订阅话题名称等打包成ros的subscriber, 连同订阅话题名称一起压入某条轨迹的订阅维护器.看看这个回调函数咋传的: 参数就是回调函数指针(地址), 而回调函数指针的定义在这里定义:可以看到,这个函数是无返回值,函数指针名叫handler的, 传入参数类型为int, string,和当前模板的传感器类常量指针.在看看SubscribeWithHandler. 第一个参数就是接收到LaserScan这个类型的message之后执行的回调函数的地址,让ros的Subscribe有回调函数调用, 后面的参数在上面已经提到, 就不多说了. 把SubscribeWithHandler的定义和使用对照着看, 发现返回值是一个ros的subscribe, 写法上和我们自己写最基础的ros订阅是一样的, topic+数字+回调函数. 回调函数用的是boost::function, 作用和std::function差不多, 用来构造函数. 函数本身又是一个lambda表达式:这个lambda捕获了外部的node类本身,也就是this, 轨迹id, topic名字, 传入了当前message的类型, 执行了该回调函数(HandleLaserScanMessage), 注意啊,现在node->*handler就是HandleLaserScanMessage了, 因为在传进SubscribeWithHandler的时候就确定了这个函数指针是啥. 所以再来看看HandleLaserScanMessage和定义的函数指针一样传入参数类型为int, string,和当前模板的传感器类常量指针. 然后把这些数据MapBuilder的HandleLaserScanMessage进行处理.读到这里，这篇“C++Node类Cartographer开始轨迹的处理深度源码分析”文章已经介绍完毕，想要掌握这篇文章的知识点还需要大家自己动手实践使用过才能领会，如果想了解更多相关内容的文章，欢迎关注百云主机行业资讯频道。
这篇“el-tree树组件懒加载怎么实现”文章的知识点大部分人都不太理解，所以小编给大家总结了以下内容，内容详细，步骤清晰，具有一定的借鉴价值，希望大家阅读完这篇文章能有所收获，下面我们一起来看看这篇“el-tree树组件懒加载怎么实现”文章吧。实现懒加载tr…
免责声明：本站发布的图片视频文字，以转载和分享为主，文章观点不代表本站立场，本站不承担相关法律责任；如果涉及侵权请联系邮箱：360163164@qq.com举报，并提供相关证据，经查实将立刻删除涉嫌侵权内容。
微信扫一扫 
