根据点云及其对应的四元数与GPS计算出其相对坐标系的经纬坐标(matlab)

数据提取

提取出包含标志物的单帧点云及该帧点云对应的四元数与GPS

//*****************************************************************************************************
//     保存单帧点云及其四元素与GPS
//
//  rosrun pcl_ros bag_to_pcd 555.bag /velodyne_points /home/xx/catkin_my/pcd
//         rosrun point_imu pcdpoints
//         pcl_viewer 11.pcd   //可视化点云
//**************************************************************************************************#include<ros/ros.h>
#include<iostream>
#include<fstream>
#include<sstream>
#include<string>
#include<vector>
#include <math.h>#include <stdio.h>#include<sensor_msgs/PointCloud2.h>  #include "std_msgs/String.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/filter.h>
#include "tf/transform_datatypes.h"
#include <tf2_msgs/TFMessage.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/impl/io.hpp>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/passthrough.h>  //直通滤波
#include <sbg_driver/SbgImuData.h>
#include <sbg_driver/SbgEkfQuat.h>
#include <sbg_driver/SbgGpsPos.h>
#include <sbg_driver/SbgEkfNav.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/time_synchronizer.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>using namespace std;
using namespace pcl;void filter_point_cloud(const pcl::PointCloud<pcl::PointXYZ>&original_cloud,pcl::PointCloud<pcl::PointXYZ>&tmp_cloud_filtered)
{tmp_cloud_filtered.clear();float x_pass;float y_pass;float z_pass; for (int i = 0; i < original_cloud.size(); i++){x_pass=original_cloud.points[i].x;y_pass=original_cloud.points[i].y;z_pass=original_cloud.points[i].z;tmp_cloud_filtered.push_back(original_cloud.points[i]);}}
int i=0;pcl::visualization::CloudViewer viewer("pcd viewer");
void chatterCallback(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& pointmsg,const  boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg,const  boost::shared_ptr<const sbg_driver::SbgEkfQuat>& quatmsg)
{cout<<"i="<<i<<endl;//pcl::io::savePCDFileASCII("write_pcd_test.pcd", pcl_pc2);//保存pcd  /*imu_data.orientation.x = quatmsg->quaternion.x;imu_data.orientation.y = quatmsg->quaternion.y;imu_data.orientation.z = quatmsg->quaternion.z;imu_data.orientation.w = quatmsg->quaternion.w;imu_data.angular_velocity.x = gpsmsg->position.x;imu_data.angular_velocity.y = gpsmsg->position.y;imu_data.angular_velocity.z = gpsmsg->position.z;
*/// 保存数据到文件中pcl::PCLPointCloud2 pcl_pc2;pcl_conversions::toPCL(*pointmsg,pcl_pc2);pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::fromPCLPointCloud2(pcl_pc2,*tmp_cloud);pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);//filter_point_cloud(*tmp_cloud,*tmp_cloud_filtered);viewer.showCloud(tmp_cloud);if(i==50) //修改此处，提取单帧数据{//string ss=i+".pcd";pcl::io::savePCDFileASCII("14.pcd",*tmp_cloud);cout<< setprecision(10) <<quatmsg->quaternion.w<<endl;cout<< setprecision(10) <<quatmsg->quaternion.x<<endl;cout<< setprecision(10) <<quatmsg->quaternion.y<<endl;cout<< setprecision(10) <<quatmsg->quaternion.z<<endl;cout<< setprecision(10) <<gpsmsg->position.x<<endl;cout<< setprecision(10) <<gpsmsg->position.y<<endl;cout<< setprecision(10) <<gpsmsg->position.z<<endl;FILE *fpWrite=fopen("data.txt","a");fprintf(fpWrite,"%16.10f %16.10f %16.10f %16.10f %16.10f %16.10f %16.10f \n",quatmsg->quaternion.w,quatmsg->quaternion.x,quatmsg->quaternion.y,quatmsg->quaternion.z,gpsmsg->position.x,gpsmsg->position.y,gpsmsg->position.z);fclose(fpWrite);}i++;
}int main(int argc,char** argv)
{ros::init (argc, argv, "ros_points");ros::NodeHandle n;message_filters::Subscriber<sensor_msgs::PointCloud2> points_sub(n, "/velodyne_points", 2000);message_filters::Subscriber<sbg_driver::SbgEkfNav> gps_sub(n, "/sbg/ekf_nav", 2000);     message_filters::Subscriber<sbg_driver::SbgEkfQuat> quat_sub(n, "/sbg/ekf_quat", 2000);   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,sbg_driver::SbgEkfNav,sbg_driver::SbgEkfQuat> MySyncPolicy;//sensor_msgs::PointCloud2,message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(2000),points_sub,gps_sub, quat_sub);  // 同步points_sub,sync.registerCallback(boost::bind(&chatterCallback, _1,_2,_3));   // _1, ros::spin ();
}

数据处理（matlab）

imutolidar_points.m

%*******************************************************************************
%     根据点云及其对应的四元数与GPS计算出其相对初始坐标系的经纬坐标
%*******************************************************************************clc
clear
%导入旋转矩阵
l_i_z=0.2;   %手测沿惯导坐标系z轴平移
l_i_x=0.1;   %手测沿惯导坐标系x轴平移
l_i_z2=0.14; %手测沿惯导坐标系z轴平移(到雷达中心高度)
a=40;        %手测沿惯导坐标系y轴旋转角度
R=imutolidar(l_i_z,l_i_x,l_i_z2,a);
%打开txt数据
F=importdata('.\data_che.txt');
n=size(F,1);%行数
%以第一个数据GPS建立初始坐标系
[g0_jin,g0_wei]=gpstoMercator(F(1,6),F(1,5));
disp(n);
T=fopen('.\ABCDpoint.txt','w');%创建文件
for i=1:n%四元数转旋转矩阵   R0=quat2dcm([F(i,1) F(i,2) F(i,3) F(i,4)]);R_g=[R0(1,1) R0(1,2) R0(1,3) 0;R0(2,1) R0(2,2) R0(2,3) 0;R0(3,1) R0(3,2) R0(3,3) 0;0 0 0 1];%经纬度转墨卡托[gi_jin,gi_wei]=gpstoMercator(F(i,6),F(i,5));g_jin=gi_jin-g0_jin;g_wei=gi_wei-g0_wei;%输入点云坐标a_l=[F(i,8);F(i,9);F(i,10);1];%惯导坐标系中的点云坐标a_g=R_g*R*a_l;%变化量a_jin_m=g_jin+a_g(2,1);a_wei_m=g_wei+a_g(1,1);ai_jin=a_jin_m+g0_jin;ai_wei=a_wei_m+g0_wei;%墨卡托转经纬度[a_jin,a_wei] = MercatorToGps(ai_jin,ai_wei);%保存fprintf(T,'%6.10f  %6.10f\n',a_jin,a_wei);end
fclose(T);

imutolidar.m

%***********************************************************
%            惯导坐标系到雷达坐标系
%**********************************************************
function R=imutolidar(l_i_z,l_i_x,l_i_z2,a)%沿惯导坐标系z轴平移l_i_zm
t_z=[1 0 0 0;0 1 0 0;0 0 1 l_i_z;0 0 0 1];
%绕惯导坐标系y轴逆时针旋转a度
r_y=[cosd(a) 0 sind(a) 0;0 1 0 0;-sind(a) 0 cosd(a) 0;0 0 0 1];
%沿惯导坐标系x轴平移l_i_x
t_x=[1 0 0 l_i_x;0 1 0 0;0 0 1 l_i_z2;0 0 0 1];
%绕惯导坐标系y轴逆时针旋转180度,然后绕z轴逆时针旋转90度 r_i_l=[0 0 1 0;0 -1 0 0;1 0 1 0;0 0 0 1];
R=t_z*r_y*t_x*r_i_l;end

gpstoMercator.m

function [jing,wei]  = gpstoMercator(j,w )
jing = j * 20037508.34 / 180;ly = log(tan((90+ w)*pi/360))/(pi/180);
wei =  ly *20037508.34/180;
end

MercatorToGps.m

function [jing,wei]  = MercatorToGps(j,w )
jing = j/ 20037508.34 * 180;ly= w/ 20037508.34 * 180;
wei = 180 /pi * (2 *atan(exp(ly *pi / 180)) - pi / 2);
end

实验数据

已提取：data_che.txt

    0.9990304112     0.0135136694    -0.0186758060     0.0375066400    45.7141597481   126.6282734222   144.2046452498    14.13   1.715   0.7461-0.1075989157    -0.0111061595     0.0182792451    -0.9939642549    45.7143921656   126.6282074678   144.1124210754    13.73   0.2301   0.71960.6300731301    -0.0263394881    -0.0241048057    -0.7757145166    45.7143195978   126.6283929261   143.6432741521   12.54   0.6003   -0.65810.7281139493     0.0404319838    -0.0294394828     0.6836289763    45.7142846109   126.6280872097   147.0383410586    15.39   0.01612   -1.89

效果

根据点云及其对应的四元数与GPS计算出其相对坐标系的经纬坐标(matlab)相关推荐

云原生大数据架构中实时计算维表和结果表的选型实践
简介: 随着互联网技术的日渐发展.数据规模的扩大与复杂的需求场景的产生,传统的大数据架构无法承载. 作者 | 志羽来源 | 阿里技术公众号一前言传统的大数据技术起源于 Google 三架马车 ...
以腾讯云IoT Suite为例谈谈边缘计算在物联网的实践与实现
全球行业数字化转型的浪潮孕育兴起,掀起了新一轮行业变革浪潮.这一波浪潮的显著特点是将"物"纳入智能互联,触发技术服务模式创新,并对价值链.供应链和行业生态产生深远影响.然而,物联网 ...
边云协同的优点_关于边缘计算和边云协同，看这一篇就够了
另外,建筑物生命周期中75% -80%的成本与其后期运营有关.现在很多商业住宅和办公大楼都有自动化控制或管理系统,例如通暖.中央空调以及嵌入传感器的智能照明系统等,它们都能与云平台或者边缘层级的主系统 ...
“云管边端”协同的边缘计算安全防护解决方案
摘要边缘计算是 5G 重要新技术能力,通过低延时.大流量.高性能服务促进新应用创新.边缘计算能力的实施面临物理.网络.协议.应用.管理等多层面的威胁,急需新安全防护能力支撑.该解决方案利用机器学习 ...
PCL点云处理之基于高程的粗糙度计算（一百）
PCL点云处理之基于高程的粗糙度计算(一百) 一.算法介绍二.具体实现 1.代码 2.结果一.算法介绍点云粗糙度是点云的一项重要的局部特征,粗糙度顾名思义,可以理解为点云表面的光滑程度,在点云识 ...
云顶之弈机器人法爆_云顶之弈六法机器人，开局出钩瞬秒后排，后排都没有拿什么和我打...
"大家好,我是百家作者他若夏沫,今天小编来给大家说一说云顶之弈六法机器人,开局出钩瞬秒后排,后排都没有拿什么和我打,有兴趣的同学和小编一起来看一看吧" 在云顶之弈中机器人是一个非常 ...
阿里云十年再出发，边缘计算已启航
3月21日,阿里云峰会北京站正式召开,边缘计算专场邀请到了中国移动.阿里云.英特尔等行业专家以及众多边缘计算行业的先行者,和观众分享在MEC.IoT.视频.边缘AI等多个方向的布局与边缘计算最新技术探 ...
替代还是扩展：云的下一站真是雾计算？
云计算.雾计算.霾计算,关于计算的技术名词层出不穷,常常让人难以辨别. 云计算自从2006年,Google首席执行官埃里克·施密特(Eric Schmidt)在搜索引擎大会(SES San Jose) ...
go实现重新给metric打标签上传到prometheus_案例分析｜云原生监控Prometheus对样本rate计算，出现标签重复？...
0 - 本案例所涉及的知识点云原生.微服务,带你了解大规模容器下的监控方式,通过各个案例分析,熟悉prometheus的内部原理. 涉及知识点:go prometheus 1 - 案例概要收到用户 ...

根据点云及其对应的四元数与GPS计算出其相对坐标系的经纬坐标(matlab)

目录

数据提取

数据处理（matlab）

实验数据

效果

根据点云及其对应的四元数与GPS计算出其相对坐标系的经纬坐标(matlab)相关推荐

最新文章

热门文章