精华内容
下载资源
问答
  • 多传感器融合定位(1)1-课程概述

    多传感器融合定位(1)1-课程概述

    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    展开全文
  • 记录深蓝学院《多传感器融合定位与建图》课程的作业
  • 如何在本地运行多传感器融合定位模块 ​ 定位技术横跨好几个专业,包括测绘、导航、计算机视觉知识、以及点云处理的知识。业界所说的“传感器融合”,都是指对摄像头、激光雷达、毫米波雷达、超声波雷达等多种...

    如何在本地运行多传感器融合定位模块

    ​ 定位技术横跨好几个专业,包括测绘、导航、计算机视觉知识、以及点云处理的知识。业界所说的“多传感器融合”,都是指对摄像头、激光雷达、毫米波雷达、超声波雷达等多种传感器各自分别收集到的数据所做的数据融合。

    ​ 定位模块依赖的硬件以及数据,包括惯性测量单元 IMU、车端天线、基站、LiDAR、以及定位地图;

    ​ GNSS定位以及激光点云定位模块,GNSS定位(基站和车端天线辅助)输出位置及速度信息,点云定位(LiDAR和定位地图)输出位置及航向角信息;

    ​ 融合框架:惯性导航解算、Kalman滤波(卡尔曼滤波器/)是核心模块);融合定位输出是一个6-dof的位置和姿态,以及协方差矩阵,其结果会反过来用于GNSS定位和点云定位的预测。

    配置定位模块

    ​ 为了使定位模块正确运行,需要对地图路径和传感器外参进行配置。假设下载的定位数据的所在路径为DATA_PATH。在进行以下步骤前,首先确定你在docker容器中。

    1. 配置传感器外参

    将定位数据中的传感器外参拷贝至指定文件夹下。

    cp DATA_PATH/params/ant_imu_leverarm.yaml /apollo/modules/localization/msf/params/gnss_params/  
    cp DATA_PATH/params/velodyne64_novatel_extrinsics_example.yaml /apollo/modules/localization/msf/params/velodyne_params/  
    cp DATA_PATH/params/velodyne64_height.yaml /apollo/modules/localization/msf/params/velodyne_params/
    

    各个外参的意义:

    ant_imu_leverarm.yaml: 杆臂值参数,GNSS天线相对Imu的距离

    velodyne64_novatel_extrinsics_example.yaml:Lidar相对Imu的外参

    velodyne64_height.yaml: Lidar相对地面的高度

    1. 设置地图路径
    在/apollo/modules/localization/conf/localization.conf中添加关于地图路径的配置:
    # Redefine the map_dir in global_flagfile.txt--map_dir=DATA_PATH
    这将会覆盖global_flagfile.txt中的默认值。
    

    运行多传感容和定位模块

    ./scripts/localization.sh
    

    定位程序将在后台运行,可以通过以下命令进行查看。

    ps -e | grep localization
    

    在/apollo/data/log目录下,可以看到定位模块输出的相关文件。

    localization.INFO : INFO级别的log信息

    localization.WARNING : WARNING级别的log信息

    localization.ERROR : ERROR级别的log信息

    localization.out : 标准输出重定向文件

    localizaiton.flags : 启动localization模块使用的配置

    播放演示rosbag

    cd DATA_PATH/bag 
    rosbag play *.bag
    

    从播放数据到定位模块开始输出定位消息,大约需要30s左右。

    记录可视化定位结果

    1. 记录定位结果

    该脚本会在后台运行录包程序,并将存放路径输出到终端上。

    ./scripts/record_bag.sh
    
    1. 可视化定位结果
    ./scripts/localization_online_visualizer.sh
    

    该可视化工具首先根据定位地图生成用于可视化的缓存文件,存放在/apollo/data/map_visual目录下。

    然后接收以下topic并进行可视化绘制。

    /apollo/sensor/velodyne64/compensator/PointCloud2
    /apollo/localization/msf_lidar
    /apollo/localization/msf_gnss
    /apollo/localization/pose
    
    1. 可视化效果如下: [外链图片转存失败(img-3p9BaRn1-1568096776319)(/home/zx/文档/o4YBAFwRxPyAbFd8AAU50JUQQEU029.png)]

    2. 如果发现可视化工具运行时卡顿,可使用如下命令重新编译可视化工具:

      cd /apollo bazel build -c opt //modules/localization/msf/local_tool/local_visualization/online_visual:online_local_visualizer
      

      编译选项-c opt优化程序性能,从而使可视化工具可以实时运行。

    结束定位模块

    ./scripts/localization.sh stop
    

    如果之前有运行步骤5的录包脚本,还需执行

    ./scripts/record_bag.sh stop
    

    验证定位结果

    假设步骤5中录取的数据存放路径为OUTPUT_PATH,杆臂值外参的路径为ANT_IMU_PATH

    1. 运行脚本:

    ./scripts/msf_local_evaluation.sh OUTPUT_PATH ANT_IMU_PATH

    该脚本会以RTK定位模式为基准,将多传感器融合模式的定位结果进行对比。注意只有在GNSS信号良好,RTK定位模式运行良好的区域,这样的对比才是有意义的。

    1. 获得如下统计结果:

      [外链图片转存失败(img-bDyH4HBG-1568096776319)(/home/zx/文档/o4YBAFwRxP-AJ5KKAAKBgsAJhGY865.png)]

    2. 可以看到三组统计结果,第一组是组合导航(输出频率200hz)的统计结果,第二组是点云定位(输出频率5hz)的统计结果,第三组是GNSS定位(输出频率约1hz)的统计结果。

      表格中各项的意义:

      error: 平面误差,单位为米

      error lon: 车前进方向的误差,单位为米

      error lat: 车横向方向的误差,单位为米

      error roll: 翻滚角误差,单位为度

      error pit: 俯仰角误差,单位为度

      error yaw: 偏航角误差,单位为度

      mean: 误差的平均值

      std: 误差的标准差

      max: 误差的最大值

      <30cm: 距离误差少于30cm的帧所占的百分比

      <1.0d: 角度误差小于1.0d的帧所占的百分比

      con_frame(): 满足括号内条件的最大连续帧数


      2019年09月10日14:23:06

    展开全文
  • 深蓝学院-多传感器融合定位-第三章作业

    题目内容

    题目

    第一题:残差模型推导

    在这里插入图片描述

    在这里插入图片描述
    在这里插入图片描述

    补充:鉴于写代码时发现A-LOAM的更新的参数块是四元数param_q和平移矢量param_t,所以我重新写了关于R,t的李代数求导,这样就可以分别对R和t进行残差更新:
    在这里插入图片描述

    第二题: Ceres解析求导

    大师兄:Ceres库有自动求导、数值求导和解析求导,A-LOAM中用的是自动求导 但是效率会比较低。

    本题代码就是要实现Ceres的解析求导,所以我们主要的工作是输入第一题中得到的Jacobian。
    (听起来很简单对吧?)

    首先我们可以看到在alom_laser_odometry_node.cpp中Line 382左右是线特征的CostFunction:

    ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
    problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
    

    大师兄:这就是我们要着手的地方了,接着进入LidarEdgeFactor,看看这个它自动求导是怎么写的,以及我们如何自己手写解析求导。
    小萝卜:大师兄,且慢!problem.AddResidualBlock()每个参数的含义是什么?
    大师兄:小萝卜,这就对了,懂得问问题啦!
    大师兄:这里的cost_function的我们要定义残差Residual和导数Jacobian的地方,loss_function其实就是Ceres的核函数(暂且不管他),para_q和para_t分别是我们要优化的参数块。

    小萝卜:谢谢大师兄,我还有一问!为什么我们一开始手写的Jacobian是用了李代数,算Residual对Transformation矩阵的导数,现在怎么变成对q和t求导了呢?对四元数求导?我可不会呀!
    大师兄:这个问题问得很好!老纳一开始也在这个问题上栽了根头。我就在纳闷儿,我们公式是对T求导,然后咱们分成了R和t求导,再后来还变成了q和t求导了呢!
    大师兄:其实啊,就是咱们这里把Ceres对T的优化,拆分开了分别对R和t进行优化(SO3),然而由于咱们的输入parameters是四元数q和t,所以咱们还要把R里面3维的旋转向量v转成4维的q。(当然也可以不拆分开用SE(3))
    大师兄:这一步,其实Ceres自带的LocalParameterization已经帮我们解决了!1

    那我们再来看看他是怎么定义的自动求导:

    struct LidarEdgeFactor
    {
    	LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
    					Eigen::Vector3d last_point_b_, double s_)
    		: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}
    
    	template <typename T>
    	bool operator()(const T *q, const T *t, T *residual) const
    	{
    
    		Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
    		Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
    		Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};
    
    		//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
    		Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
    		Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
    		q_last_curr = q_identity.slerp(T(s), q_last_curr);
    		Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
    
    		Eigen::Matrix<T, 3, 1> lp;
    		lp = q_last_curr * cp + t_last_curr;
    
    		Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);
    		Eigen::Matrix<T, 3, 1> de = lpa - lpb;
    
    		residual[0] = nu.x() / de.norm();
    		residual[1] = nu.y() / de.norm();
    		residual[2] = nu.z() / de.norm();
    
    		return true;
    	}
    
    	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
    									   const Eigen::Vector3d last_point_b_, const double s_)
    	{
    		return (new ceres::AutoDiffCostFunction<LidarEdgeFactor, 3, 4, 3>(new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
    	}
    
    	Eigen::Vector3d curr_point, last_point_a, last_point_b;
    	double s;
    };
    

    Ceres的使用

    小萝卜:大师兄大师兄,这个Ceres的代码好复杂,我看不懂呀!
    大师兄:看不懂就对了!我也是参考了好几个大佬的博客才明白呢。

    请各位同学参考这几个博客网页1 2来学习Ceres。

    Ceres解析求导:线特征残差

    // EdgeAnalyticCostFunction
    class LidarEdgeAnalyticCostFunction : public ceres::SizedCostFunction<3, 4, 3> {
    public:
        LidarEdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
    								Eigen::Vector3d last_point_b_, double s_)
    		: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}
    
      	virtual bool Evaluate(double const* const* parameters,
                            double* residuals,
                            double** jacobians) const 
    	{
    		Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
    		Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);
    		Eigen::Vector3d lp;
    		Eigen::Vector3d lp_r;
    		lp_r = q_last_curr * curr_point; // for computing Jacobian of Rotation: dp_by_dr
    		lp = q_last_curr * curr_point + t_last_curr; //new point
    		Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b);
    		Eigen::Vector3d de = last_point_a - last_point_b;
    
    		residuals[0] = nu.x() / de.norm();
    		residuals[1] = nu.y() / de.norm();
    		residuals[2] = nu.z() / de.norm();
    
    		if(jacobians != NULL)
    		{
    			if(jacobians[0] != NULL)
    			{
    				Eigen::Vector3d re = last_point_b - last_point_a;
    				Eigen::Matrix3d skew_re = skew(re);
    				//  Rotation
    				Eigen::Matrix3d skew_lp_r = skew(lp_r);
    				Eigen::Matrix<double, 3, 3> dp_by_dr;
    				dp_by_dr.block<3,3>(0,0) = -skew_lp_r;
    				Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor> > J_so3_r(jacobians[0]);
    				J_so3_r.setZero();
    				J_so3_r.block<3,3>(0,0) = skew_re * dp_by_dr / de.norm();
    
    				// Translation
    				Eigen::Matrix<double, 3, 3> dp_by_dt;
    				(dp_by_dt.block<3,3>(0,0)).setIdentity();
    				Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > J_so3_t(jacobians[1]);
    				J_so3_t.setZero();
    				J_so3_t.block<3,3>(0,0) = skew_re * dp_by_dt / de.norm();	
    			}
    		}
    
    		return true;
     
    	}
    
    protected:
    	Eigen::Vector3d curr_point, last_point_a, last_point_b;
    	double s;
    };
    

    这里写完以后,记得改problem的costFunction:

    ceres::CostFunction *cost_function = new LidarEdgeAnalyticCostFunction(curr_point, last_point_a, last_point_b, s);  
    problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
    

    Ceres解析求导:面特征残差

    // PlaneAnalyticCostFunction
    class LidarPlaneAnalyticCostFunction : public ceres::SizedCostFunction<1, 4, 3> {
    public:
        LidarPlaneAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
    								Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
    		: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_), last_point_m(last_point_m_), s(s_) {}
    
      	virtual bool Evaluate(double const* const* parameters,
                            double* residuals,
                            double** jacobians) const 
    	{
    		Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
    		Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);
    		Eigen::Vector3d lp;
    		Eigen::Vector3d lp_r;
    		lp_r = q_last_curr * curr_point; // for computing Jacobian of Rotation: dp_dr
    		lp = q_last_curr * curr_point + t_last_curr; //new point
    		Eigen::Vector3d de = (last_point_l-last_point_j).cross(last_point_m-last_point_j);
    		double nu = (lp-last_point_j).dot(de);
    		
    		residuals[0] = nu / de.norm();
    
    		if(jacobians != NULL)
    		{
    			if(jacobians[0] != NULL)
    			{
    				Eigen::Vector3d dX_dp = de / de.norm();
    				double X = nu / de.norm();
    				Eigen::Vector3d ddh_dp = X * dX_dp / std::abs(X);
    				//  Rotation
    				Eigen::Matrix3d skew_lp_r = skew(lp_r);
    				Eigen::Matrix<double, 3, 3> dp_dr;
    				dp_dr.block<3,3>(0,0) = -skew_lp_r;
    				Eigen::Map<Eigen::Matrix<double, 1, 4, Eigen::RowMajor> > J_so3_r(jacobians[0]);
    				J_so3_r.setZero();
    				J_so3_r.block<1,3>(0,0) = ddh_dp.transpose() * dp_dr;
    
    				// Translation
    				Eigen::Matrix<double, 3, 3> dp_dt;
    				(dp_dt.block<3,3>(0,0)).setIdentity();
    				Eigen::Map<Eigen::Matrix<double, 1, 3, Eigen::RowMajor> > J_so3_t(jacobians[1]);
    				J_so3_t.setZero();
    				J_so3_t.block<1,3>(0,0) = ddh_dp.transpose() * dp_dt;
    			}
    		}
    
    		return true;
     
    	}
    
    protected:
    	Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
    	double s;
    };
    

    Ceres和Eigen的坑点

    1. Eigen Library requires the data type to be consistent, eg.
    // Since de is Vector3d, thus nu must be double
    Eigen::Vector3d de = (last_point_l-last_point_j).cross(last_point_m-last_point_j);
    double nu = (lp-last_point_j).dot(de);
    
    1. Eigen Vector product must use explicitly .dot() pr .cross()

    第三题:EVO结果分析

    1. APE
      在这里插入图片描述
      在这里插入图片描述

    2. RPE
      在这里插入图片描述
      在这里插入图片描述
      RVIZ中的结果,熟悉的Kitti图,可以看到在最后转弯的时候还是有点偏离绿色的GNSS轨迹,不过整体效果还是不错!
      在这里插入图片描述


    1. yuntian_li, Ceres详解(一) CostFunction ↩︎ ↩︎

    2. leeayu, Ceres Tutotial —— 最小二乘建模 ↩︎

    展开全文
  • 深蓝学院-多传感器融合定位-第7章作业 作业解析及代码

    Github: https://github.com/WeihengXia0123/LiDar-SLAM

    在这里插入图片描述

    1. 及格题

    (大概整了一两天,终于可以编译了…关键问题在于,docker的环境可能被很多次作业打乱了。在助教葛大佬的提示下,新创建了一个assignments/来放第七章作业的代码,神奇地解决了初始代码的编译问题)

    及格部分的代码就是按照深蓝07章课件中的Error-State Kalman Filter的公式来。

    注意: 和EKF有点区别。

    //
    // TODO: perform Kalman prediction
    //
    X_ = F * X_;                                          // fix this
    P_ = F * P_ * F.transpose() + B * Q_ * B.transpose(); // fix this
    
    
    //
    // TODO: set measurement:
    // 误差:预测值 - 观测值
    //
    Eigen::Vector3d P_nn_obs = pose_.block<3,1>(0,3) - T_nb.block<3,1>(0,3);             // fix this
    Eigen::Matrix3d R_nn_obs = T_nb.block<3,3>(0,0).transpose() * pose_.block<3,3>(0,0); // fix this
    
    YPose_.block<3, 1>(0, 0) = P_nn_obs;
    YPose_.block<3, 1>(3, 0) = Sophus::SO3d::vee(R_nn_obs - Eigen::Matrix3d::Identity());
    
    Y = YPose_;
    
    // set measurement equation:
    G = GPose_;
    
    //
    // TODO: set Kalman gain:
    //
    MatrixRPose R = RPose_; // fix this
    MatrixCPose C = Eigen::MatrixXd::Identity(6,6);
    K = P_ * G.transpose() * (G*P_*G.transpose() + C*R*C.transpose()).inverse(); // fix this               
    
    //
    // TODO: set measurement:
    // 误差:预测值 - 观测值
    //
    Eigen::Vector3d P_nn_obs = pose_.block<3,1>(0,3) - T_nb.block<3,1>(0,3);             // fix this
    Eigen::Matrix3d R_nn_obs = T_nb.block<3,3>(0,0).transpose() * pose_.block<3,3>(0,0); // fix this
    
    YPose_.block<3, 1>(0, 0) = P_nn_obs;
    YPose_.block<3, 1>(3, 0) = Sophus::SO3d::vee(R_nn_obs - Eigen::Matrix3d::Identity());
    
    Y = YPose_;
    
    // set measurement equation:
    G = GPose_;
    
    //
    // TODO: set Kalman gain:
    //
    MatrixRPose R = RPose_; // fix this
    MatrixCPose C = Eigen::MatrixXd::Identity(6,6);
    K = P_ * G.transpose() * (G*P_*G.transpose() + C*R*C.transpose()).inverse();   
    
    //
    // TODO: perform Kalman correct:
    //
    
    P_ = (MatrixP::Identity() - K*G) * P_; // fix this
    X_ = X_ + K * (Y - G*X_);              // fix this
    
    
    void ErrorStateKalmanFilter::EliminateError(void) {
      //
      // TODO: correct state estimation using the state of ESKF
      //
      // a. position:
      pose_.block<3, 1>(0, 3) =
              pose_.block<3, 1>(0, 3) - X_.block<3,1>(INDEX_ERROR_POS, 0); // fix this
      // b. velocity:
      vel_ = vel_ - X_.block<3,1>(INDEX_ERROR_VEL, 0);                     // fix this
      // c. orientation:
      Eigen::Matrix3d R_nn =
          Sophus::SO3d::hat(X_.block<3, 1>(INDEX_ERROR_ORI, 0)).matrix();
      pose_.block<3, 3>(0, 0) = pose_.block<3, 3>(0, 0) * (Eigen::MatrixXd::Identity(3,3) - R_nn); // fix this
    
      // d. gyro bias:
      if (COV.PROCESS.BIAS_FLAG && IsCovStable(INDEX_ERROR_GYRO)) {
        gyro_bias_ += X_.block<3, 1>(INDEX_ERROR_GYRO, 0);
      }
    
      // e. accel bias:
      if (COV.PROCESS.BIAS_FLAG && IsCovStable(INDEX_ERROR_ACCEL)) {
        accl_bias_ += X_.block<3, 1>(INDEX_ERROR_ACCEL, 0);
      }
    }
    

    滤波功能基本正常。在这里插入图片描述

    2. 良好题

    Parameter - verison 1

    ​        process:
    ​            gyro: 1.0e-4
    ​            accel: 2.5e-3
    ​            bias_accel: 2.5e-3
    ​            bias_gyro: 1.0e-4
    ​            bias_flag: True
    ​        measurement:
    ​            pose:
    ​                pos: 1.0e-3
    ​                ori: 1.0e-4
    ​            pos: 1.0e-4
    ​            vel: 2.5e-3
    

    Result - laser
    APE w.r.t. full transformation (unit-less)

       max	1.500344
      mean	0.902466
    median	0.894143
       min	0.161967
      rmse	0.917469
       sse	3696.964333
       std	0.165240
    

    Result - fused
    APE w.r.t. full transformation (unit-less)

       max	1.549884
      mean	0.898618
    median	0.898169
       min	0.034132
      rmse	0.916721
       sse	3690.936975
       std	0.181281
    

    Parameter - verison 2

            process:
                gyro: 1.0e-3
                accel: 2.5e-2
                bias_accel: 2.5e-3
                bias_gyro: 1.0e-4
                bias_flag: True
            measurement:
                pose:
                    pos: 1.0e-3
                    ori: 1.0e-4
                pos: 1.0e-4
                vel: 2.5e-3
    

    Result - laser
    APE w.r.t. full transformation (unit-less)

       max	1.500344
      mean	0.901874
    median	0.893900
       min	0.161967
      rmse	0.916922
       sse	3690.876634
       std	0.165440
    

    Result - fused

    APE w.r.t. full transformation (unit-less)

       max	1.539430
      mean	0.898373
    median	0.897470
       min	0.058437
      rmse	0.916334
       sse	3686.143600
       std	0.180540
    

    Parameter - verison 3

            process:
                gyro: 1.0e-5
                accel: 2.5e-6
                bias_accel: 2.5e-3
                bias_gyro: 1.0e-4
                bias_flag: True
            measurement:
                pose:
                    pos: 1.0e-3
                    ori: 1.0e-4
                pos: 1.0e-4
                vel: 2.5e-3
    

    Result - laser
    APE w.r.t. full transformation (unit-less)
    (not aligned)

       max	1.136680
      mean	0.231626
    median	0.164014
       min	0.017465
      rmse	0.289811
       sse	368.550415
       std	0.174183
    

    Result - fused

    APE w.r.t. full transformation (unit-less)
    (not aligned)

       max	1.013023
      mean	0.258347
    median	0.200838
       min	0.019931
      rmse	0.309749
       sse	421.004733
       std	0.170884
    

    Parameter - verison 4

            process:
                gyro: 1.0e-5
                accel: 2.5e-6
                bias_accel: 2.5e-3
                bias_gyro: 1.0e-4
                bias_flag: True
            measurement:
                pose:
                    pos: 1.0e-4
                    ori: 1.0e-5
                pos: 1.0e-4
                vel: 2.5e-3
    

    Result - laser
    APE w.r.t. full transformation (unit-less)

       max	1.500344
      mean	0.901922
    median	0.893900
       min	0.161967
      rmse	0.916961
       sse	3691.186379
       std	0.165389
    

    Result - fused

    APE w.r.t. full transformation (unit-less)

       max	1.530646
      mean	0.900078
    median	0.896358
       min	0.033429
      rmse	0.917532
       sse	3695.785994
       std	0.178115
    

    Parameter - verison 5

            process:
                gyro: 1.0e-6
                accel: 2.5e-7
                bias_accel: 2.5e-3
                bias_gyro: 1.0e-4
                bias_flag: True
            measurement:
                pose:
                    pos: 1.0e-3
                    ori: 1.0e-4
                pos: 1.0e-4
                vel: 2.5e-3
    

    Result - laser
    APE w.r.t. full transformation (unit-less)

       max	1.500344
      mean	0.902180
    median	0.894120
       min	0.161967
      rmse	0.917157
       sse	3692.766704
       std	0.165068
    

    Result - fused
    APE w.r.t. full transformation (unit-less)

       max	1.554055
      mean	0.898355
    median	0.896839
       min	0.058776
      rmse	0.916404
       sse	3686.702189
       std	0.180979
    

    3. 优秀题

    我加了个bias_flag,只要设为Flase就可以不考虑随即游走的bias。

      // d. gyro bias:
      if (COV.PROCESS.BIAS_FLAG && IsCovStable(INDEX_ERROR_GYRO)) {
        gyro_bias_ += X_.block<3, 1>(INDEX_ERROR_GYRO, 0);
      }
    
      // e. accel bias:
      if (COV.PROCESS.BIAS_FLAG && IsCovStable(INDEX_ERROR_ACCEL)) {
        accl_bias_ += X_.block<3, 1>(INDEX_ERROR_ACCEL, 0);
      }
    

    由于跑出来太花时间,这里直只放了一组数据:

    Parameter - verison 6 (without Rand-Walk Bias)

           process:
                gyro: 1.0e-5
                accel: 2.5e-6
                bias_accel: 2.5e-3
                bias_gyro: 1.0e-4
                bias_flag: False
            measurement:
                pose:
                    pos: 1.0e-3
                    ori: 1.0e-4
                pos: 1.0e-4
                vel: 2.5e-3
    

    Result - laser
    APE w.r.t. full transformation (unit-less)

       max	1.136680
      mean	0.231211
    median	0.164014
       min	0.017465
      rmse	0.289257
       sse	367.644384
       std	0.173813
    

    Result - fused

    APE w.r.t. full transformation (unit-less)

       max	1.012434
      mean	0.257726
    median	0.201350
       min	0.008808
      rmse	0.309306
       sse	420.375118
       std	0.171020
    

    下面是KITTI数据集跑出来的效果图。

    黄色线表示ground-truth,蓝色和红色分别是LiDar单独和LiDar/IMU融合的轨迹。(这里几乎重合,原因是这个参数调得很好,红红火火哈哈哈哈)
    在这里插入图片描述

    在这里插入图片描述

    展开全文
  • 针对高速铁路列车运行控制系统中的列车定位问题,提出了GPS/DR/MM组合定位的方案,利用卡尔曼滤波对多传感器的数据信息进行融合之后再与电子地图匹配,实时提供列车的定位信息。与单一传感器定位方式相比,可以...
  • 多传感器融合定位-3D激光里程计整体流程激光里程计方案基于SVD的ICP介绍进一步推导旋转部分求解 整体流程 点云地图构建过程分为前端后端两部分,其中的前端过程可以看做激光里程计过程。 激光里程计方案 前端里程计...
  • 深蓝学院-多传感器融合定位-第6章作业1. 及格题目2. 良好题目3. 优秀题目 1. 及格题目 2. 良好题目 3. 优秀题目
  • 深蓝学院-多传感器融合定位-第4章作业一. 及格部分1. 代码跑通二. 良好部分三. 优秀部分Scan Context版本GNSS版本 一. 及格部分 1. 代码跑通 直接编译,跑通: catkin config --install && catkin build ...
  • 下面做一个多传感器融合定位实验,采集到的数据为两种交替出现的数据。 代码整体框架如图所示: 前言 无论是KF处理线性问题还是EKF处理非线性问题,它都只涉及单一传感器的障碍物跟踪。激光雷达测量位置精度更高...
  • 多传感器融合定位(GPS+Wheel+camera)(1)-读取传感器数据 文章目录1、读取Kaist数据集到融合系统中 1、读取Kaist数据集到融合系统中 int main(int argc, char** argv) { if (argc != 3) { LOG(ERROR) << ...
  • 多传感器融合定位(4-基于滤波的融合方法)kitti数据集 IMU频率改为100HZ 本博文参考GEYAO 大神的github 到kitti 官网下载 2011_10_03_drive_0027 的extract数据集 (原来的 sync数据集为去除相机畸变的数据集,但是...
  • 前言 LZ最近在看Udacity的无人...本节将用最简洁的话讲解卡尔曼滤波KF、非线性卡尔曼滤波EKF等知识点,并就此实现一个多传感器融合定位的小demo,后面会就粒子滤波PF专门开一个章节讲解。 一、卡尔曼滤波 KF 1、引子...
  • 多传感器融合定位(2-点云地图构建及基于地图定位)2-后端优化与点云地图构建 后端优化 点云地图的建立
  • 多传感器融合定位(2-3D激光里程计)1-前端里程计ICP 机械激光雷达与固态激光雷达 整体流程 ICP point2point ICP推导 把旋转和平移进行解耦 去除与R无关项 trace 不等式证明
  • 多传感器融合定位(4-点云地图构建及基于地图定位)4-通过GNSS 实现地图定位 eg:因为个人水平有限,这次作业全靠DK大哥的指导,方法不唯一 目的:调用 GNSS的位姿,实现初始化 调整前,默认初始gps位姿为 0 , 0 ,0...
  • 多传感器融合定位(2-3D激光里程计)3 -前端里程计LOAM系列 LOAM 线雷达 线与线之间有两度的夹角 特征点的选取
  • 多传感器融合定位(2-3D激光里程计)2-前端里程计NDT NDT 计算分布于分布之间的差异 NDT的优点: 1.鲁棒性较强 2.相对于ICP而言,运算量更少 转化为优化问题
  • 多传感器融合定位(1-3D激光里程计)6-实现手写icp ps: 因为个人能力不足,本次代码 和 伪代码 框图 主要是参考 go on 大佬的,本人只是搬运工做记录 ps:本文 来自 go on 助教的讲解ppt 伪代码框图 ps:图来自 go ...
  • 多传感器融合定位(3-点云地图构建及基于地图定位)3-实现ScanContext 回环检测 参考博客 从零开始做自动驾驶定位(十一): 闭环修正 自动驾驶系统进阶与项目实战(四)自动驾驶高精度地图构建中的三维场景识别和闭环...
  • 点击上方“3D视觉工坊”,选择“星标”干货第一时间送达导读高德定位业务包括云上定位和端上定位两大模块。其中,云上定位主要解决Wifi指纹库、AGPS定位、轨迹挖掘和聚类等问题;端上定位解...
  • 多传感器融合定位(2-点云地图构建及基于地图定位)1-回环检测及代码实现 scan context 核心思想:三维点云匹配下,计算量巨大,基于三维匹配,初值问题无法解决,scan context 对三维进行将将维度,并且scan ...
  • 多传感器融合定位(GPS+Wheel+camera)(2)FilterFusionSystem类介绍 1、系统初始化读取参数 FilterFusionSystem::FilterFusionSystem(const std::string& param_file) : initialized_(false), odom_G_R_O_...
  • 多传感器融合定位(3-惯性导航原理及误差分析)6-Allan方差 进行随机误差分析 参考博客: 组合导航系列文章(四):Allan方差分析 使用imu_utils工具生成IMU的Allan方差标定曲线 手写VIO-使用Allen方差工具标定IMU ...
  • 自动驾驶领域目前最强的MSF(传感器融合)定位算法,再次...连多传感器融合定位算法达到SOTA的百度Apollo,在仿真环境也中了招。这项最新研究,来自加州大学尔湾分校(UCI),目前已发表在信息安全领域四大顶会之一的...
  • 点云地图构建及基于地图定位习题一、闭环修正及精度评价二、位姿初始化 一、闭环修正及精度评价 提供的工程框架中已经给出了闭环的流程和实现,但是是基于ICP的,这是在已知粗略位姿情况下的实现方式。在未知粗略位姿...
  • 多传感器融合定位(3-惯性导航原理及误差分析)2-IMU误差分析及处理:随机误差理论分析&allan方差分析及实现
  • 多传感器融合定位(2-3D激光里程计)6-实现调用pcl-icp、ndt -3 evo里程计精度评价 参考博客: 从零开始做自动驾驶定位(七): 里程计精度评价 下载evo 工具 pip install evo --upgrade --no-binary evo evo用法,...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 11,983
精华内容 4,793
关键字:

多传感器融合定位