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

最新推荐文章于 2022-11-22 16:02:18 发布

Unite One

最新推荐文章于 2022-11-22 16:02:18 发布

阅读量1.3k

点赞数 2

分类专栏：工具 matlab

本文链接：https://blog.csdn.net/xx970829/article/details/115537036

版权

工具同时被 2 个专栏收录

38 篇文章 2 订阅

订阅专栏

matlab

9 篇文章 9 订阅

订阅专栏

数据提取

提取出包含标志物的单帧点云及该帧点云对应的四元数与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.7196
    0.6300731301    -0.0263394881    -0.0241048057    -0.7757145166    45.7143195978   126.6283929261   143.6432741521   12.54   0.6003   -0.6581
    0.7281139493     0.0404319838    -0.0294394828     0.6836289763    45.7142846109   126.6280872097   147.0383410586    15.39   0.01612   -1.89