使用GPS和velodyne 64拼接地图

输入：GPS X Y Z，roll pitch yaw；时间对齐的velodyne 64E 激光数据

输出：拼接成的激光点云地图

注意事项：

1、激光数据是X 右，Y前，Z上，惯导记录的数据是，X前， Y左，Z是上（全局的X，Y，Z分别指向东、北、天，偏航角是与东向的夹角。车辆初始位置朝东，此时朝向近似为0度，往前（东）走，X增加；接着左转（往北），Y增加。

由此得出的激光数据和位置数据的关系，先将激光的方向投影成和全局方向一致，即调整所得到的点云x=y;y=-x）

2、所存储的GPS数据是惯导的数据，激光安装位置在惯导前1.5米，所以x,y坐标要根据车辆的偏航角进行一个平移

#include<Eigen\Dense>
#include<iostream>
#include<string>
#include<vector>
#include<fstream>
//#include <cstring>
#define NOMINMAX
#include <windows.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#define  M_PI       3.14159265358979323846using namespace Eigen;
using namespace std;
typedef float T;
typedef Eigen::Matrix<T, 3, 1> Vector3t;
typedef Eigen::Matrix<T, 4, 4> Matrix4t;
typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXt;//列向量，列数是1
typedef Eigen::Quaternion<T> Quaterniont;void getFileNames(string dir, vector<string> &fileName);
VectorXt f(const VectorXt& state, const VectorXt& control, float dt);template<class Type> Type string2num(const string& str) {istringstream iss(str);Type num;iss >> num;return num;
}
template<class Type> string num2string(const Type &num) {stringstream ss;string str;ss << num;ss >> str;return str;
}void loadGps(string gps, vector<float> &gps_vec)
{float temp;string str, tempstr;str = gps;//cout<<str<<endl;for (int i = 0; i<22; i++){tempstr = str.substr(0, str.find_first_of(" "));str = str.substr(str.find_first_of(" ") + 1);temp = string2num<float>(tempstr);gps_vec.push_back(temp);//cout<<temp<<" ";}//cout<<endl;
}//比较函数
bool computePairNum(std::string pair1, std::string pair2)
{return pair1 < pair2;
}
//排序
void sort_filelists(std::vector<std::string>& filists, std::string type)
{if (filists.empty())return;std::sort(filists.begin(), filists.end(), computePairNum);
}void readIMUData(vector<float> &gps_vec, VectorXt &control, const string &imuFileName, float timeSec)
{static float x0 = 0, y0 = 0, z0 = 0;static bool count = 0;ifstream imuFile;string imuString;imuFile.open(imuFileName.c_str());getline(imuFile, imuString);//读取数据到gps_vec中loadGps(imuString, gps_vec);double x, y, z, roll, pitch, yaw, vx, vy, vz, ax, ay, az, rollv, pitchv, yawv;if (count == 0){x0 = gps_vec[1];y0 = gps_vec[2];z0 = gps_vec[3];count++;}x = gps_vec[1] - x0;y = gps_vec[2] - y0;z = gps_vec[3] - z0;vx = gps_vec[7];vy = gps_vec[8];vz = gps_vec[9];//这里ax加了一个负号，转换为正方向,他妈的不能加ax = gps_vec[10] * 100;ay = gps_vec[11] * 100;az = gps_vec[12] * 100;//醉了，这里惯导顺序是roll,pitch,yaw，分别是绕y,x,z轴旋转的结果,注意！！！//但是在录激光数据的时候，存放的顺序是yaw pitch roll！！fuck//要转换为弧度制//惯导下的角度,pitch 是绕x ,roll是绕y，这里做一个转换，变为roll绕x,pitch绕yyawv = gps_vec[13] * 100;pitchv = gps_vec[15] * 100;rollv = gps_vec[14] * 100;//还是角度变换的锅！！！yaw = M_PI / 2 - gps_vec[4] / 180 * M_PI;pitch = gps_vec[6] / 180 * M_PI;roll = gps_vec[5] / 180 * M_PI;//车长float L = 1.5;gps_vec[1] = x + L * cos(yaw);gps_vec[2] = y + L * sin(yaw);gps_vec[3] = z;gps_vec[4] = roll;gps_vec[5] = pitch;gps_vec[6] = yaw;control.head<3>() = Eigen::Vector3f(ax, ay, az);control.tail<3>() = Eigen::Vector3f(rollv, pitchv, yawv);imuFile.close();/*cout << "pos:  " << x << " " << y << " " << z << "   "<< "angle:  " << roll / 180 * M_PI << " " << pitch / 180 * M_PI << " " << yaw / 180 * M_PI << "  "<< "vel:  " << vx << "," << vy << "," << vz << " "<< "acc:  " << ax << "," << ay << "," << az << "  "<< "angelRate:    " << rollv << "," << pitchv << "," << yawv << endl;*///cout << "acc:  " << ax << "," << ay << "," << az << "  " << endl;
}void rgb2xyzi(pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB) {int m = cloudRGB->points.size();for (int i = 0; i < m; i++) {pcl::PointXYZI p;/*p.x = cloudRGB->points[i].y;p.y = -cloudRGB->points[i].x;p.z = cloudRGB->points[i].z;*/p.x = cloudRGB->points[i].y;p.y = -cloudRGB->points[i].x;p.z = cloudRGB->points[i].z;p.intensity = cloudRGB->points[i].r;cloudI->points.push_back(p);}cloudI->width = m;cloudI->height = 1;
}
//加载XYZRGB格式
void readCollectData(std::string &in_file, pcl::PointCloud<pcl::PointXYZI>::Ptr points_xyzi)
{pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudXYZRGB(new pcl::PointCloud<pcl::PointXYZRGB>);pcl::PCDReader reader;reader.read(in_file, *cloudXYZRGB);rgb2xyzi(points_xyzi, cloudXYZRGB);
}int main(int argc, char **argv)
{//文件名后一定要记得加/,不然文件读不到std::string imu_path, pcd_path;imu_path = "J:\\schoolData\\20190106\\siyuan2\\IMU\\";pcd_path = "J:\\schoolData\\20190106\\siyuan2\\HDL\\";//        imu_path="/media/localization/Localization/tu/HDL/";//nh.getParam("PCD_directory",imu_path);cout << "数据文件夹： " << imu_path << endl;//时间戳vector<float> time_list;//原始文件名列表std::vector<std::string> file_lists, pcd_lists;//根据前后两帧时间差,处理过后的文件名列表std::vector<std::string> sample_file_lists, sample_pcd_lists;//读取文件名getFileNames(imu_path, file_lists);sort_filelists(file_lists, "txt");cout << "file_lists size " << file_lists.size() << endl;//读取时间到time_list中for (int i = 0; i < file_lists.size(); ++i){string time_string = file_lists[i].substr(0, file_lists[i].length() - 4) + " ";vector<float> time_vec;string str, tempstr;str = time_string;for (int j = 0; j < 8; j++){tempstr = str.substr(0, str.find_first_of(" "));str = str.substr(str.find_first_of(" ") + 1);if (tempstr == " ")continue;time_vec.push_back(string2num<float>(tempstr));}float hour = time_vec[4];float minute = time_vec[5];float second = time_vec[6];float uSecond = time_vec[7];float timeSeq = hour * 3600 + minute * 60 + second + uSecond / 1000;if (i == 0){time_list.push_back(timeSeq);sample_file_lists.push_back(file_lists[0]);}else{   //如果前后帧大于0.095if (timeSeq - time_list.back() >= 0.98){time_list.push_back(timeSeq);sample_file_lists.push_back(file_lists[i]);}}}//所有地图pcl::PointCloud<pcl::PointXYZI>::Ptr all_GPSMap(new pcl::PointCloud<pcl::PointXYZI>());//for (int i = 1; i < 10; ++i)for (int i = 1; i < sample_file_lists.size(); ++i){std::string imuFileName = imu_path + sample_file_lists[i];std::string pcdFileName = pcd_path + sample_file_lists[i].substr(0,sample_file_lists[i].length()-4)+".pcd";pcl::PointCloud<pcl::PointXYZI>::Ptr cloudXYZI(new pcl::PointCloud<pcl::PointXYZI>);//读数据pcl::PCDReader reader;readCollectData(pcdFileName, cloudXYZI);vector<float> imu_vec;VectorXt control(6);readIMUData(imu_vec, control, imuFileName, time_list[i]);float dt = time_list[i] - time_list[i - 1];//旋转矩阵Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();//x y z transform_2.translation() << imu_vec[1], imu_vec[2], imu_vec[3];//roll pitch yaw transform_2.rotate(Eigen::AngleAxisf(imu_vec[4], Eigen::Vector3f::UnitX()));transform_2.rotate(Eigen::AngleAxisf(imu_vec[5], Eigen::Vector3f::UnitY()));transform_2.rotate(Eigen::AngleAxisf(imu_vec[6], Eigen::Vector3f::UnitZ()));printf("\nMethod #2: using an Affine3f\n");std::cout << transform_2.matrix() << std::endl;// Executing the transformationpcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZI>());// You can either apply transform_1 or transform_2; they are the samepcl::transformPointCloud(*cloudXYZI, *transformed_cloud, transform_2);*all_GPSMap += *transformed_cloud;}all_GPSMap->width = all_GPSMap->points.size();all_GPSMap->height = 1;//降采样存储地图pcl::PointCloud<pcl::PointXYZI>::Ptr map_filtered(new pcl::PointCloud<pcl::PointXYZI>());pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;downSizeFilter.setLeafSize(0.2, 0.2, 0.2);downSizeFilter.setInputCloud(all_GPSMap);downSizeFilter.filter(*map_filtered);pcl::io::savePCDFile("GPS_Map_downFilter_offset.pcd", *map_filtered, true);return 0;
}博客：C/C++遍历某个单一目录下的所有文件，目录下若有目录则会出现错误，目录最后要加\\ https://www.cnblogs.com/collectionne/p/6792301.htmlvoid getFileNames(string dir, vector<string> &fileName)
{dir += "*.*";HANDLE hFind;WIN32_FIND_DATA findData;LARGE_INTEGER size;hFind = FindFirstFile(dir.c_str(), &findData);if (hFind == INVALID_HANDLE_VALUE){cout << "Failed to find first file!\n";return;}do{// 忽略"."和".."两个结果 if (strcmp(findData.cFileName, ".") == 0 || strcmp(findData.cFileName, "..") == 0)continue;if (findData.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY)    // 是否是目录 {cout << findData.cFileName << "\t<dir>\n";}else{size.LowPart = findData.nFileSizeLow;size.HighPart = findData.nFileSizeHigh;fileName.push_back(findData.cFileName);//cout << findData.cFileName << "\t" << size.QuadPart << " bytes\n";}} while (FindNextFile(hFind, &findData));
}

使用GPS和velodyne 64拼接地图相关推荐

Velodyne 64线激光雷达协议
作者:ShownSun 公众号:时沿科技文章目录 Velodyne 64线激光雷达协议雷达简介适应的型号排列方式工作方式传输速率 UDP IP地址协议详解协议包格式传输数据格式 GP ...
GIS开发二：批量下载和拼接地图瓦片
文章目录 1.简介 1.1 Web墨卡托投影 1.2 经纬度坐标系 1.3 瓦片定义 1.4 瓦片编号 1.5 瓦片和像素 1.6 瓦片计算公式 1.7 网络地图服务(WMS) 1.8 切片地图服务( ...
【iOS-Cocos2d游戏开发之十八】解决滚屏背景/拼接地图有黑边(缝隙)以及禁止游戏中自动锁屏问题！...
Himi 原创, 欢迎转载,转载请在明显处注明! 谢谢. 原文地址:http://blog.csdn.net/xiaominghimi/article/details/6926913 本章节主要为大家 ...
gps数据转坐标c语言,GPS原始坐标转百度地图坐标(纯C代码)
一.环境介绍 GPS模块型号:中科微电子GPS模块 GPS输出的原始数据帧: $GNGGA,114955.000,2842.4158,N,11549.5439,E,1,05,3.8,54.8,M,0. ...
GPS坐标显示在百度地图上（Qt+百度地图）
Qt在5.6以后的版本就不支持通过mingw编译器来调用webview控件了,这里我用的是Qt5.4的版本,用mingw编译器调试的. 下面简单介绍下Qt与html中的javascript调用交互过程 ...
android gps 火星坐标,GPS真实坐标与火星地图坐标/百度地图坐标的转换
#include #include #include static const uint32_t GPSBaud = 9600; TinyGPSPlus gps; HardwareSerial ss( ...
GPS 点可视化（英文地图打点，热力图，测距）
GPS 点可视化 mapbox-gl开发的web端实现英文地图上显示GPS点.热力图. 自己做研究.写论文时会用到在英文地图上可视化的功能,但之前可用的工具莫名下架了,所以自己挤时间做了一个. 链接地 ...
ROS采集GPS/北斗数据在百度地图中可视化位置
关注微信公众号"混沌无形",后台回复:13462F2.免费获取完整工程源码! 基于USB-RS232协议输出UM220-III/3-N/北斗GPS模块定位信息,使用ROS与js的接 ...
Gps信息转国内谷歌地图和高德地图
谷歌国内的地图和高德地图使用的坐标都是GCJ-02,一般后台所给到的Gps信息需要转换GCJ-02,以下代码对Gps转换为GCJ-02 我这边做的是高德和google共用的一个方法,区别是返回的信息高 ...
GPS经纬度转化为百度地图/Google坐标及互转方案
http://blog.csdn.net/ma969070578/article/details/41013547

使用GPS和velodyne 64拼接地图

使用GPS和velodyne 64拼接地图相关推荐

最新文章

热门文章