输入: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
#include
#include
#include
#include
//#include
#define NOMINMAX
#include
#include
#include
#include
#include
#include
#include
#include
#define M_PI 3.14159265358979323846
using namespace Eigen;
using namespace std;
typedef float T;
typedef Eigen::Matrix Vector3t;
typedef Eigen::Matrix Matrix4t;
typedef Eigen::Matrix VectorXt;//列向量,列数是1
typedef Eigen::Quaternion Quaterniont;
void getFileNames(string dir, vector &fileName);
VectorXt f(const VectorXt& state, const VectorXt& control, float dt);
template Type string2num(const string& str) {
istringstream iss(str);
Type num;
iss >> num;
return num;
}
template string num2string(const Type &num) {
stringstream ss;
string str;
ss << num;
ss >> str;
return str;
}
void loadGps(string gps, vector &gps_vec)
{
float temp;
string str, tempstr;
str = gps;
//cout<(tempstr);
gps_vec.push_back(temp);
//cout<& filists, std::string type)
{
if (filists.empty())return;
std::sort(filists.begin(), filists.end(), computePairNum);
}
void readIMUData(vector &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绕y
yawv = 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::Ptr cloudI, const pcl::PointCloud::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::Ptr points_xyzi)
{
pcl::PointCloud::Ptr cloudXYZRGB(new pcl::PointCloud);
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 time_list;
//原始文件名列表
std::vector file_lists, pcd_lists;
//根据前后两帧时间差,处理过后的文件名列表
std::vector 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 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(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.095
if (timeSeq - time_list.back() >= 0.98)
{
time_list.push_back(timeSeq);
sample_file_lists.push_back(file_lists[i]);
}
}
}
//所有地图
pcl::PointCloud::Ptr all_GPSMap(new pcl::PointCloud());
//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::Ptr cloudXYZI(new pcl::PointCloud);
//读数据
pcl::PCDReader reader;
readCollectData(pcdFileName, cloudXYZI);
vector 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 transformation
pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud());
// You can either apply transform_1 or transform_2; they are the same
pcl::transformPointCloud(*cloudXYZI, *transformed_cloud, transform_2);
*all_GPSMap += *transformed_cloud;
}
all_GPSMap->width = all_GPSMap->points.size();
all_GPSMap->height = 1;
//降采样存储地图
pcl::PointCloud::Ptr map_filtered(new pcl::PointCloud());
pcl::VoxelGrid 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.html
void getFileNames(string dir, vector &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\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));
}