- 博客(71)
- 资源 (2)
- 收藏
- 关注
RSS订阅原创 一起做激光反光板（六）-基于滑窗的EKF-SLAM及外参自动标定公式推导
在第四篇中已经提到，如果场景中反光板不够多，容易造成EKF系统效果不好的问题，且我们还想用上其他的点云信息，保证在反光板不够的情况下仍能够正确的收敛。我们考虑扩充观测信息：（1）角点和线段特征，加法和反光板加法类似，因此，不再详细展开，其中线段特征的提取，我们打算采用黄国权老师实验室github上的工程；（2）激光前后帧的ICP：我们打算使用黄国权老师实验室改进的 libPointMatcher或者ROS自带的点线ICP，做帧间ICP，能够拿到当期帧和上一帧的相对位姿及协方差矩阵；（3）当前激光帧的
2020-11-26 09:08:25
2091
4
原创 一起做激光反光板（五）-基于EKF的激光雷达外参在线估计公式推导
在上一篇中已经提到，我们打算把激光雷达坐标系到车体中心坐标系之间的外参进行在线估计。假如激光雷达的外参标定精度较差，对EKF系统会有影响，此外，假如在车体运动过程中，由于抖动等造成激光雷达安装位置发生了变化，则在线估计在所难免。在以下推导中，我们不再区分建图和定位过程，都统一认为：老反光板地图可有可无，状态空间中维护新的反光板位置。在线外参估计EKF公式推导状态空间至少包含6个自由度：机器人的实时位姿x,y,θx,y,\thetax,y,θ，和激光雷达坐标系到车体坐标系的转换：lbx,lby,lbθ^
2020-11-26 09:07:49
1619
原创 一起做激光反光板（四）-框架搭建
我们已经推导了基本的反光板建图和定位的基本公式，接下来，开始搭建一个框架，尝试进行反光板的建图定位的代码加进去。首先，搭建一个ROS框架，在我的个人github的reflector_ekf_slam上上传一个基于激光反光板的EKF建图定位方案。基本框架：ROS接收topic：scan: sensor_msgs::LaserScanodom: nav_msgs::Odometry需要注意的是：（1）轮速计数据必须有线速度和角速度，轮速计的方差写死了：Qu_ << 0.02 * 0
2020-11-21 22:08:26
3819
20
原创 Cartographer源码阅读3D-后端优化-SPA
和2D基本一致，区别：（1）第一个submap只优化yaw角，不优化平移；（2）可选是否优化z轴，如果优化，则加入IMU相关的残差：线加速度和角速度；如果不优化，则和2D类似，加入轮速计残差和LocalSLAM的sequence边。struct NodeSpec3D { common::Time time; transform::Rigid3d local_pose; transform::Rigid3d global_pose;};struct SubmapSpec3D { t
2020-11-16 17:56:23
697
2
原创 Cartographer源码阅读3D-回环计算-分支定界
计算回环-ConstraintBuilder3D和ConstraintBuilder2D基本流程一致，通过2个接口：MaybeAddConstraint和MaybeAddGlobalConstraint，然后调用统一接口：ComputeConstraint函数，计算分支定界，然后利用最高得分的解作为初始解，进行CSM位姿解算。和2D不同的地方主要包括：（1）分支定界的方法不2D不同：多分辨率网格的形式不一致；离散方式不同：先通过直方图匹配的方法计算进行角度离散，再进行距离离散；分支操作：分支时，
2020-11-16 17:55:46
1079
2
原创 Cartographer源码阅读3D-后端PoseGraph3D
和PoseGraph2D的基本流程差不多，主要的差别是：（1）Constraint的形式不一样。（2）分支定界采用了直方图匹配（基于点云切片的思想），且分支定界的复杂度高于2D；（3）后端SPA优化的残差不一样。3D的Constraint：constraint = submap.global_pose * node.local_pose(包含重力对齐)而2D的Constraint：constraint = submap.global_pose_2d * node.local_pose_2d(
2020-11-16 17:55:15
324
原创 一起做激光反光板（三）-EKF建图公式推导
继续EKF建图公式推导。构建反光板地图和定位篇有很大部分的重复。因为上一篇其实也包含了建图。如果已经有初始的反光板地图，且初始的反光板地图不允许优化，且允许添加新的反光板，则该公式和上一篇所使用的公式一模一样；如果已经有初始的反光板地图，且初始的反光板地图允许被优化，且允许添加新的反光板，该种相当于反光板地图的更新，暂时不考虑反光板地图的更新；本部分只考虑没有初始反光板地图的情况，从零开始添加反光板。建图EKF公式推导初始的状态空间为x,y,θx,y,\thetax,y,θ，在建图过程中，该状态
2020-11-13 20:26:18
1705
7
原创 一起做激光反光板（一）-EKF定位公式推导
前提：本文只考虑平面运动。EKF的公式及原理不再细述：EKF原理观测：观测数据为反光板。反光板的检测暂时也不考虑。（一般来说，反光板的检测都是基于反射强度来做的，需要自己手写，如果有疑问留言）。预测：以轮速计为例，我们只使用轮速计的线速度和角速度，轮速计的模型，我们使用轮速计模型博客中提到的模型。定位EKF公式推导假如我们已经事先构建了一个地图（后续再考虑），地图中包含MMM个反光板：每个反光板还会有一个协方差矩阵，该矩阵的形式为：假设我们只维护位姿，不考虑新增的反光板，且不更新老地图中反
2020-11-11 18:52:16
5129
27
原创 Cartographer源码阅读3D-Submap创建
Cartographer源码阅读3D-前端-Submap创建submap创建接口函数：std::unique_ptr<LocalTrajectoryBuilder3D::InsertionResult>LocalTrajectoryBuilder3D::InsertIntoSubmap( const common::Time time, const sensor::RangeData& filtered_range_data_in_local, const
2020-11-03 09:43:12
817
原创 Cartographer源码阅读3D-前端匹配
Cartographer源码阅读2D&3D-前端匹配暴力匹配：RealTimeCorrelativeScanMatcher3D暴力搜索计算匹配得分，取得最高分对应的位姿。class RealTimeCorrelativeScanMatcher3D { public: explicit RealTimeCorrelativeScanMatcher3D( const scan_matching::proto::RealTimeCorrelativeScanMatcherOptio
2020-10-09 19:10:33
777
原创 Cartographer源码阅读3D-前端数据处理-IMU&3D点云
Cartographer源码阅读2D&3D-3D前端数据处理-IMU&3D点云3D需要传入的数据包括：IMU（必须），点云数据(sensor_msgs::MultiEchoLaserScan或sensor_msgs::PointCloud2)，轮速计数据(可选，nav_msgs::Odometry，和2D相同，不再分析)IMU数据的处理通过接收sensor_msgs::Imu数据，转换为sensor::ImuData数据：struct ImuData { // 时间 comm
2020-10-09 19:05:42
962
原创 Cartographer源码阅读2D&3D-PGM生成及地图发布
Cartographer源码阅读2D&3D-PGM生成及地图发布pgm生成cartographer提供了pbstream转pgm的service，当然，也可以改写相关函数，直接从cartographer的后端取出数据生成pgm，但是需要修改相关代码。将pbstream转为pgm的节点在pbstream_to_ros_map_main.cc文件里实现。主要函数为：void Run(const std::string& pbstream_filename, const std::strin
2020-09-30 16:53:10
1987
2
原创 Cartographer源码阅读2D&3D-Pbstream读写
Cartographer源码阅读2D&3D-Pbstream读写写入proto查看protobuf官方文档：Protobuf文档需要自定义需要写入和读取的message即可。定义好.proto文件，在编译的阶段生成相应的.pb.h文件，通过调用这些文件，保存和读取proto文件。pbstream文件保存的内容保存pbstream文件是通过服务调用的，接口函数为node.cc文件中：bool Node::HandleWriteState( ::cartographer_ros_m
2020-09-30 16:47:10
2636
12
原创 Cartographer源码阅读2D&3D-删除Submap机制
Cartographer源码阅读-删除Submap机制-PureLocalizationTrimmertrim的删除接口在map_builder.cc文件中有所阐述。共包含两种trim的机制，第一种是OverlappingSubmapsTrimmer2D类型的，只用在2D SLAM中；第二种是PureLocalizationTrimmer，在2D和3D中都可以用。删除机制的入口int MapBuilder::AddTrajectoryBuilder( const std::set<Sen
2020-09-30 16:41:07
1345
原创 Cartographer源码阅读2D-后端优化-SPA
Cartographer源码阅读-2D后端优化-SPASPA优化关于SPA，类：// 存放Node的信息struct NodeSpec2D { common::Time time; // 该帧的时间 transform::Rigid2d local_pose_2d;// 不带重力对齐的LocalSLAM下的位姿 transform::Rigid2d global_pose_2d;// 不带重力对齐的GlobalSLAM下的位姿 Eigen::Quaterniond gravity_a
2020-09-30 16:27:31
638
原创 Cartographer源码阅读2D-后端回环计算
Cartographer源码阅读-2D后端回环计算ConstraintBuilder2D很重要的是：（1）2D的回环形式和3D的回环形式不一样，2D回环：submap的global pose（global pose * 重力方向）的逆 * node的global_pose_2d（global pose * 重力的逆）。（2）分支定界搜索的网格仍然是一个submap生成的，不是所有的submap。是拿着一个Node的点云数据，在一个submap里做分支定界搜索。class ConstraintBu
2020-09-30 16:15:18
641
原创 Cartographer源码阅读2D-后端-PoseGraph2D
Cartographer源码阅读-2D后端-PoseGraph2D后端PoseGraph2D后端管理着轨迹及其相关的Node及Submap的位姿，并对各个位姿进行优化，也是分支定界的接口。struct TrajectoryNodePose { // Node的数据 struct ConstantPoseData { // 时间 common::Time time; // 在Local SLAM坐标系下的位姿 transform::Rigid3d local_po
2020-09-30 15:23:08
512
原创 Cartographer源码阅读2D-前端Node稀疏化-MotionFilter
Cartographer源码阅读-2D前端Node稀疏化-MotionFilter在trajectory_builder_2d.cc的LocalTrajectoryBuilder2D::AddAccumulatedRangeData函数中，执行稀疏化任务，即，在AddAccumulatedRangeData函数中调用InsertIntoSubmap函数，在InsertIntoSubmap函数内调用(motion_filter_.IsSimilar(time, pose_estimate)函数，进行稀疏化，
2020-09-28 19:31:48
327
原创 Cartographer源码阅读2D-前端Submap生成及数据插入
Cartographer源码阅读-2D前端Submap生成及数据插入前端CSM位姿计算后，将激光点云转换到Local SLAM的坐标系下，插入submaps中，该submaps指的是前端LocalTrajectoryBuilder2D中维护的ActiveSubmap2d。ActiveSubmap2Dclass ActiveSubmaps2D { public: explicit ActiveSubmaps2D(const proto::SubmapsOptions2D& options)
2020-09-28 19:25:15
1198
原创 Cartographer源码阅读2D-前端CSM-CeresScanMatcher2D
Cartographer源码阅读-2D前端CSM-CeresScanMatcher2D基本原理：其实，在代码里实现的时候，又加上了旋转和平移的残差，防止点云匹配的结果和初值差太多。CeresScanMatcher2Dclass CeresScanMatcher2D { public: explicit CeresScanMatcher2D(const proto::CeresScanMatcherOptions2D& options); virtual ~CeresScanMat
2020-09-21 19:41:12
1115
1
原创 Cartographer源码阅读2D-前端暴力匹配-RealTimeCorrelativeScanMatcher2D
Cartographer源码阅读-2D前端暴力匹配-RealTimeCorrelativeScanMatcher2D使用暴力匹配的思想，解决当前激光帧和submap匹配的问题：假定在SLAM的过程中，车体的运动是连续有界的，即运动速度是不可能达到无穷大，根据车体的运动速度，可以预测当前帧和上一帧之间的最大相对运动大小是多少，假设我们已经知道上一帧经过CSM解算过的位姿，我们将所有可能的位姿和当前激光帧组合到一起，生成可能解。然后，遍历所有可能解，把所有的可能解投射到submap上：计算当前激光帧通过其位
2020-09-21 19:28:45
1260
1
原创 Cartographer源码阅读2D&3D-前端-位姿外推器-PoseExtrapolator
Cartographer源码阅读-2D前端-位姿外推器-PoseExtrapolatorPoseExtrapolator类：class PoseExtrapolator { public: explicit PoseExtrapolator(common::Duration pose_queue_duration, double imu_gravity_time_constant); PoseExtrapolator(const Pos
2020-09-19 00:11:31
996
原创 Cartographer源码阅读2D&3D-前端自适应滤波-AdaptiveVoxelFilter
Cartographer源码阅读-2D前端自适应滤波-AdaptiveVoxelFilter基本原理：调用体素滤波，如果体素滤波后点数大于阈值，则返回，如果小于阈值，则接着使用二分法进行体素滤波。class AdaptiveVoxelFilter { public: // 构造函数读入max_length min_num_points max_range explicit AdaptiveVoxelFilter( const proto::AdaptiveVoxelFilterO
2020-09-18 23:54:44
952
原创 Cartographer源码阅读2D&3D-前端根据时间戳滤波-RangeDataCollator
Cartographer源码阅读-2D前端-根据时间戳滤波-RangeDataCollator基本原理：根据当前激光帧的时间戳和上一帧激光帧的时间戳，找到当前激光帧和上一帧激光帧时间重叠的激光点，滤除时间重复的点。class RangeDataCollator { public: explicit RangeDataCollator( const std::vector<std::string>& expected_range_sensor_ids)
2020-09-18 23:46:53
758
原创 Cartographer源码阅读2D&3D-前端体素滤波-VoxelFilter
Cartographer源码阅读-2D前端体素滤波-VoxelFilter基本原理：将xyz分别除以分辨率，并取整，得到xyz所在cell的id(x,y,z)，再将该三个整数分别放入3*32个字节的数中（很好的技巧，避免了大量的遍历和比较），插入voxelFileter::voxel_set_中，后面如果是第一次插入该数，则保留该点，如果之前存在该数，即意味着有其他点落在该网格内，不再保留该点。class VoxelFilter { public: // 'size' is the length
2020-09-18 23:39:47
995
4
原创 Cartographer源码阅读2D-前端-LocalTrajectoryBuilder2D
Cartographer-2D前端-LocalTrajectoryBuilder2D前端的主要功能：（1）点云数据处理：滤波、畸变校正（2）位姿外推器推算位姿（3）暴力匹配（4）CSM匹配（5）生成Submap（6）生成Node传入后端class LocalTrajectoryBuilder2D { public: struct InsertionResult { // Node数据 std::shared_ptr<const TrajectoryNode::Da
2020-09-18 23:31:01
753
原创 Cartographer源码阅读2D&3D-数据流动-数据分发
Cartographer-2D数据流动-数据分发承接上一篇：轨迹添加Node// 接收轮速计数据void Node::HandleOdometryMessage(const int trajectory_id, const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg)
2020-09-18 22:26:26
704
原创 Cartographer源码阅读2D&3D-数据流动-轨迹添加
Cartographer-2D数据流动-轨迹添加以单线激光雷达数据和轮速计为例。入口在node_main.cc中：void Run(){ constexpr double kTfBufferCacheTimeInSeconds = 10.; // 构建tf_buffer tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)}; tf2_ros::TransformList
2020-09-18 21:16:52
1260
转载 定时查看GPU程序占用的显存
定时查看GPU程序占用的显存https://blog.csdn.net/jasonzzj/article/details/52649174
2019-06-17 17:29:46
3480
转载 CUDA PCL boost错误解决方案
关于CUDA PCL boost库出现的错误解决办法问题描述问题分析解决方法转载自简书问题描述如果包含头文件：#include <pcl/io/pcd_io.h>则报错的大致内容包括：pcl/io/file_io.h(346): error: namespace "boost" has no member "iequals"pcl/io/pcd_io.h(485): e...
2019-05-23 18:44:15
1622
原创 darknet版openpose
darknet：YOLO2darknet源码为c语言，使用makefile，不太习惯使用，习惯使用c++和cmake，因此，将darknet改为cmake版，方便和OpenPose合并。OpenPose:OpenPose是基于caffe的，依赖库比较多。更喜欢使用darknet。操作系统：Ubuntu16.04依赖库：CUDA8.0CUDNN5.1OpenCV3.1.0请自行安装，参考博客：CUD...
2018-06-04 13:04:43
1860
2
原创 Creo4.0显示点云的方法
思路：pcd->pts，Creo显示pts源文件：test.pcdpcd->pts方法：#include <iostream>#include <fstream>#include <pcl\io\pcd_io.h>typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;typedef ...
2018-04-16 21:12:44
2079
转载 Creo4.0安装与VS2015环境下的开发配置
欢迎查看：http://blog.csdn.net/weixin_38354109/article/details/79397218
2018-02-28 10:44:17
2321
2
原创 基于3D关节点的人体动作识别综述
文基于3D关节点的人体动作识别综述 原文：2016，Pattern Recognition： 3D skeleton-based human action classification: A survey 摘要 近年来，基于深度序列的人体动作分类的研究越来越多，出现了基于不同数据源：深度图或RGB视频的3D人体关节的人体动作分类的方法以及特征表示方法。 本文...
2017-12-04 21:56:14
66211
3
原创 Ubuntu16.04使用FLANN出现的错误记录
错误记录：/usr/bin/ld: xx.o: undefined reference to symbol '_ZN3MPI8Datatype4FreeEv'//usr/lib/libmpi_cxx.so.1: error adding symbols: DSO missing from command line解决方法： 在CMakeLists.txt中，加上：ADD_TARGET(xxx x
2017-10-30 21:49:29
2085
darknet+openpose
2018-06-04
空空如也
TA创建的收藏夹 TA关注的收藏夹
TA关注的人