lego_loam——featuerAssociation.cpp

IMU回调函数：

    //imu回调函数void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn){//通过接收到的imuIn里面的四元素得到roll,pitch,yaw三个角double roll, pitch, yaw;tf::Quaternion orientation;tf::quaternionMsgToTF(imuIn->orientation, orientation);tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);// ! step 1 加速度去除重力影响，同时坐标轴进行变换从xyz转换到yzxfloat accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;//!step  2   将欧拉角，加速度，速度保存到循环队列中imuPointerLast = (imuPointerLast + 1) % imuQueLength;imuTime[imuPointerLast] = imuIn->header.stamp.toSec();imuRoll[imuPointerLast] = roll;//横滚角imuPitch[imuPointerLast] = pitch;//俯仰角imuYaw[imuPointerLast] = yaw;//偏航imuAccX[imuPointerLast] = accX;//x轴加速度imuAccY[imuPointerLast] = accY;//y轴加速度imuAccZ[imuPointerLast] = accZ;//z轴加速度imuAngularVeloX[imuPointerLast] = imuIn->angular_velocity.x;//x轴角速度imuAngularVeloY[imuPointerLast] = imuIn->angular_velocity.y;//y轴角速度imuAngularVeloZ[imuPointerLast] = imuIn->angular_velocity.z;//z轴角速度//! step 3对速度，角速度，加速度进行积分，得到位移，角度和速度AccumulateIMUShiftAndRotation();}

难点1：

 // ! step 1 加速度去除重力影响，同时坐标轴进行变换从xyz转换到yzxfloat accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;

本段代码涉及两个操作

1.坐标转换从xyz转换到yzx

imu原始观测值的坐标轴方向为右前上（xyz）
转换为前上右（xyz–>yzx）？？

 // ! step 1 加速度去除重力影响，同时坐标轴进行变换从xyz转换到zxyfloat accX = imuIn->linear_acceleration.y     float accY = imuIn->linear_acceleration.z     float accZ = imuIn->linear_acceleration.x

2.消除重力加速度对于测量的影响：

欧拉角表示的：Rbn：R_b^n ：Rbn：

导航坐标系下重力矢量：gn=[0,0,−9.81]g^n=[0,0,-9.81]gn=[0,0,−9.81]
将重力转换到导航坐标系：(Rbn)T∗gn(R_b^n)^T*g^n(Rbn)T∗gn
原始观测值减去重力影响：acc−(Rbn)T∗gnacc-(R_b^n)^T *g^nacc−(Rbn)T∗gn

 // ! step 1 坐标轴进行变换从xyz转换到zxy，同时去除重力影响。float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;

lego_loam——featuerAssociation.cpp相关推荐

lego_loam安装测试记录
lego_loam的github连接https://github.com/RobustFieldAutonomyLab/LeGO-LOAM 报错参考连接https://blog.csdn.net/we ...
C++ .h(头文件) 与 .cpp(源文件) 的使用
.h 文件: .h是头文件 ,里面主要是写类的声明(包括类里面的成员和方法的声明).函数原型.#define常数等, 注意.h文件写的时候有特定的格式就是开头和结尾 #ifndef TEST_HEAD ...
利用VS+MFC+Opencv显示图像和视频所需添加类（CvvImage.h和CvvImage.cpp的源码）。
CvvImage.h代码: #pragma once #ifndef CVVIMAGE_CLASS_DEF #define CVVIMAGE_CLASS_DEF #include "open ...
UTF-8 CPP的使用
UTF-8 CPP是一个简单.小巧.轻量级.跨平台的UTF-8编码字符串库. 下面对其使用方法进行简单的介绍: 1. 从http://sourceforge.net/projects/utf ...
拇指接龙游戏升级记录03(升级MainScene.cpp)
MainScene是拇指接龙游戏的主游戏场景文件,拥有近5000行代码. 说实在的,实现自cocos2d-x 2.x版本向当下最新的3.8.1版本的升级过程,其中涉及的技术不是一下能够说明的.有些是形 ...
GATB的使用小例子test.cpp
1.touch test.cpp,,文件夹中出现test.cpp touch test.cpp 2. test.cpp的内容 #include <gatb/gatb_core.hpp>i ...
Linux下运行.cpp文件
如何在Ubuntu16下运行一个简单的.cpp文件呢,做法如下: 假设我在桌面上写了一个hell,world程序; 保存为abc.cpp 然后在终端打开: 1.点击保存的文件的属性,看在哪里,我的是在 ...
2020-10-26runtime error: member access within null pointer of type ‘struct ListNode‘ (solution.cpp)错
runtime error: member access within null pointer of type 'struct ListNode' (solution.cpp)错误 /*** Def ...
matlab怎么跑.cpp程序,MATLAB编译cpp文件
目的:打通MATLAB与VC之间的通道,实现用MATLAB调用VC程序,以及VC调用MATLAB程序. 上篇博客实现了将MATLAB的M文件编译成C/C++文件,exe应用程序.这篇实现MATLAB编 ...

lego_loam——featuerAssociation.cpp

lego_loam——featuerAssociation.cpp相关推荐

最新文章

热门文章