ros c++，kitti 数据集转txt，并求法向量（可选），裁剪（可选），保存点云拼接结果（可选）

庄王

1132人浏览 · 2022-01-18 22:21:36

庄王 · 2022-01-18 22:21:36 发布

#include <iostream>
#include <fstream>
#include <iterator>
#include <string>
#include <vector>
#include <opencv2/opencv.hpp>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <geometry_msgs/PoseStamped.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <eigen3/Eigen/Dense>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter.h>
#include <pcl/visualization/cloud_viewer.h>
#include <vtkAutoInit.h>
#include <pcl/common/utils.h>

#include <pcl/features/integral_image_normal.h>
#include <pcl/io/pcd_io.h>
#include <Eigen/Dense>

using namespace std;

std::vector<float> read_lidar_data(const std::string lidar_data_path)
{
std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary);
lidar_data_file.seekg(0, std::ios::end);
const size_t num_elements = lidar_data_file.tellg() / sizeof(float);
lidar_data_file.seekg(0, std::ios::beg);
std::vector<float> lidar_data_buffer(num_elements);
lidar_data_file.read(reinterpret_cast<char*>(&lidar_data_buffer[0]), num_elements*sizeof(float));
return lidar_data_buffer;
}

int main(int argc, char** argv)
{
ros::init(argc, argv, "bin2txt");
ros::NodeHandle n("~");
std::string dataset_folder,pose_folder,point_normals_folder;
bool viewer,saveMap,useGroundTruth,calNormals;
int sample;
double leafSize;
double x_limit,y_limit,z_limitp,z_limitn;
n.getParam("dataset_folder", dataset_folder);
n.getParam("pose_folder", pose_folder);
n.getParam("point_normals_folder", point_normals_folder);
n.getParam("viewer", viewer);
n.getParam("saveMap", saveMap);
n.getParam("sample",sample);
n.getParam("useGroundTruth",useGroundTruth);
n.getParam("calNormals", calNormals);
n.getParam("leafSize",leafSize);
n.getParam("x_limit", x_limit);
n.getParam("y_limit",y_limit);
n.getParam("z_limitp",z_limitp);
n.getParam("z_limitn",z_limitn);

std::string binDataPath(dataset_folder+"velodyne/");

DIR *dir = opendir(binDataPath.c_str());

if (dir == NULL)
{
cout << "opendir error" << endl;
}
struct dirent *entry;
vector<string> allPath;

std::ifstream input_file(pose_folder.c_str());
std::vector<Eigen::Affine3d,Eigen::aligned_allocator<Eigen::Affine3d>> posesarray;
int count=0;

Eigen::Affine3d Tr;
Tr(0,0)=4.276802385584e-04;
Tr(0,1)=-9.999672484946e-01;
Tr(0,2)=-8.084491683471e-03;
Tr(0,3)=-1.198459927713e-02;
Tr(1,0)=-7.210626507497e-03;
Tr(1,1)=8.081198471645e-03;
Tr(1,2)=-9.999413164504e-01;
Tr(1,3)=-5.403984729748e-02;
Tr(2,0)=9.999738645903e-01;
Tr(2,1)=4.859485810390e-04;
Tr(2,2)=-7.206933692422e-03;
Tr(2,3)=-2.921968648686e-01;

if (input_file.is_open())
{
std::string poseline;

Eigen::Affine3d world2lidar;

while(getline(input_file,poseline)&&!poseline.empty())
{
std::istringstream stringGet(poseline);
stringGet>>world2lidar(0,0)>>world2lidar(0,1)>>world2lidar(0,2)>>world2lidar(0,3)>>world2lidar(1,0)>>world2lidar(1,1)>>world2lidar(1,2)>>world2lidar(1,3)>>world2lidar(2,0)>>world2lidar(2,1)>>world2lidar(2,2)>>world2lidar(2,3);
// cout<<endl;
// std::cout<<"world2lidar:"<<world2lidar(0,0)<<" "<<world2lidar(0,1)<<" "<<world2lidar(0,2)<<" "<<world2lidar(0,3)<<std::endl;
// std::cout<<"world2lidar:"<<world2lidar(1,0)<<" "<<world2lidar(1,1)<<" "<<world2lidar(1,2)<<" "<<world2lidar(1,3)<<std::endl;
// std::cout<<"world2lidar:"<<world2lidar(2,0)<<" "<<world2lidar(2,1)<<" "<<world2lidar(2,2)<<" "<<world2lidar(2,3)<<std::endl;
// cout<<endl;
posesarray.push_back(world2lidar);
// std::cout<<"world2lidar(0,3):"<<world2lidar(0,3)<<std::endl;
count++;
}
cout<<"count:"<<count<<std::endl;
}
input_file.close();

pcl::PointCloud<pcl::PointXYZ>::Ptr map(new pcl::PointCloud<pcl::PointXYZ>);

while (((entry = readdir(dir)) != NULL)&&ros::ok())
{

if(entry->d_name[0]!='.')

{
ofstream outputfile;
ostringstream temp;
string filename;
temp<<dataset_folder<<point_normals_folder<<string(entry->d_name).substr(0,string(entry->d_name).length()-4)<<".txt";
filename=temp.str();
std::cout<<"filename:"<<filename<<std::endl;
temp.clear();
outputfile.open(filename.c_str(), ios::trunc);

std::stringstream lidar_data_path;
lidar_data_path << dataset_folder<<"velodyne/" << string(entry->d_name);
std::cout<<"lidar_data_path:"<<lidar_data_path.str()<<std::endl; //for test
std::vector<float> lidar_data = read_lidar_data(lidar_data_path.str());
// std::cout << "totally " << lidar_data.size() / 4.0 << " points in this lidar frame \n"; //for test

std::vector<Eigen::Vector3d> lidar_points;
std::vector<float> lidar_intensities;
pcl::PointCloud<pcl::PointXYZ> laser_cloud;
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;

Eigen::Affine3d world2lidar_temp;
int num =atoi(entry->d_name);
std::cout<<"num:"<<num<<std::endl;
if (num<posesarray.size())
{
world2lidar_temp(0,0)= posesarray[num](0,0);
world2lidar_temp(0,1)= posesarray[num](0,1);
world2lidar_temp(0,2)= posesarray[num](0,2);
world2lidar_temp(0,3)= posesarray[num](0,3);
world2lidar_temp(1,0)= posesarray[num](1,0);
world2lidar_temp(1,1)= posesarray[num](1,1);
world2lidar_temp(1,2)= posesarray[num](1,2);
world2lidar_temp(1,3)= posesarray[num](1,3);
world2lidar_temp(2,0)= posesarray[num](2,0);
world2lidar_temp(2,1)= posesarray[num](2,1);
world2lidar_temp(2,2)= posesarray[num](2,2);
world2lidar_temp(2,3)= posesarray[num](2,3);
}

for (std::size_t i = 0; i < lidar_data.size(); i += 4)
{
lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]);
lidar_intensities.push_back(lidar_data[i+3]);
pcl::PointXYZ point;

//////transform to world coordinate
Eigen::Vector4d lidarpoint,worldpoint;
if (lidar_data[i]<x_limit && lidar_data[i]>-x_limit &&lidar_data[i+1]<y_limit && lidar_data[i+1]>-y_limit && lidar_data[i+2]<z_limitp &&lidar_data[i+2]>-z_limitn) //只用一定范围内的数据
{
lidarpoint(0)=lidar_data[i];
lidarpoint(1)=lidar_data[i+1];
lidarpoint(2)=lidar_data[i+2];
lidarpoint(3)=1;
}
else continue;

if(useGroundTruth)// ground truth 是相对于相机坐标系的位姿
{
lidarpoint(2)=lidar_data[i];
lidarpoint(0)=-lidar_data[i+1];
lidarpoint(1)=-lidar_data[i+2];
lidarpoint(3)=1;
worldpoint=world2lidar_temp*lidarpoint;
point.x = worldpoint(2);
point.y = -worldpoint(0);
point.z = -worldpoint(1);

// world2lidar_temp=Tr.inverse()*world2lidar_temp*Tr;
// worldpoint=world2lidar_temp*lidarpoint;
// point.x = worldpoint(0);
// point.y = worldpoint(1);
// point.z = worldpoint(2);
}
else
{
worldpoint=world2lidar_temp*lidarpoint;
point.x = worldpoint(0);
point.y = worldpoint(1);
point.z = worldpoint(2);
}
if(!calNormals) outputfile<<point.x<<' '<<point.y<<' '<<point.z<<std::endl;
if(saveMap) map->push_back(point);
laser_cloud.push_back(point);
}

if (calNormals)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud=laser_cloud.makeShared();
ne.setInputCloud(cloud);
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud);
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

for(std::size_t i=0; i<laser_cloud.size();i++)
{
pcl::PointXYZ searchPoint = laser_cloud[i];
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
kdtree.nearestKSearchT(searchPoint,20,pointIdxRadiusSearch,pointRadiusSquaredDistance);
float curvature;
Eigen::Vector4f plane_parameters;
ne.setViewPoint(0, 0, 1);
ne.computePointNormal(laser_cloud,pointIdxRadiusSearch,plane_parameters,curvature);
if(viewer)
{
pcl::Normal singleNormal;
singleNormal.normal_x=plane_parameters.x();
singleNormal.normal_y=plane_parameters.y();
singleNormal.normal_z=plane_parameters.z();
normals->push_back(singleNormal);
}
if (!plane_parameters.hasNaN())
{
outputfile<<laser_cloud.points[i].x<<' '<<laser_cloud.points[i].y<<' '<<laser_cloud.points[i].z<<' '<<plane_parameters.x()<<' '<<plane_parameters.y()<<' '<<plane_parameters.z()<<std::endl;
if (saveMap)
{
if(i%sample==0)
map->push_back(searchPoint);
}
}

}


if(viewer)
{
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); //创建视窗对象，定义标题栏名称“3D Viewer”
viewer->addPointCloud<pcl::PointXYZ>(cloud, "original_cloud");   //将点云添加到视窗对象中，并定义一个唯一的ID“original_cloud”
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0.5, "original_cloud"); //点云附色，三个字段，每个字段范围0-1
viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, normals, 10, 0.05, "normals");   //每十个点显示一个法线，长度为0.05

while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}

}

if (viewer) normals->clear(); // clear normals for viewer

}

laser_cloud.clear();
outputfile.close();

}

}

closedir(dir);

if (saveMap)
{
map->is_dense=false;
pcl::io::savePCDFileBinary(dataset_folder+"map.pcd",*map);
}

std::cout << "Done \n";

return 0;
}

腾讯云开发者社区

腾讯云面向开发者汇聚海量精品云计算使用和开发经验，营造开放的云计算技术生态圈。

更多推荐

Elasticsearch复杂数据类型终极指南：从入门到精通

Elasticsearch作为功能强大的搜索引擎，支持多种复杂数据类型，让开发者能够灵活处理各种结构化和非结构化数据。本文将带你全面了解Elasticsearch中的复杂数据类型，从基础概念到实际应用，助你轻松掌握数据建模的核心技巧。## 内部对象：构建层级化数据结构在Elasticsearch中，对象类型（Object）是最基础的复杂数据类型之一，用于表示具有嵌套关系的数据。例如，我们可

腾讯云开发者社区

终极指南：Flink SQL连接器版本管理从混乱到有序的升级之路

Apache Flink作为流处理领域的佼佼者，其SQL连接器的版本管理一直是开发者面临的核心挑战。本文将系统讲解Flink SQL连接器版本管理的最佳实践，帮助你轻松应对版本兼容性问题，实现从混乱到有序的升级之旅。## 连接器版本管理的常见痛点 😫在Flink应用开发中，连接器版本管理常常让开发者头疼不已。不同版本的连接器可能导致各种兼容性问题，例如API变更、功能差异甚至运行时错误。

腾讯云开发者社区

如何快速搭建Neon无服务器PostgreSQL：面向初学者的完整指南

Neon是一款革命性的无服务器PostgreSQL解决方案，它通过分离存储和计算层，实现了自动扩缩容、类代码式数据库分支以及零级扩展能力。本指南将帮助你从零开始搭建Neon开发环境，体验这款创新数据库的强大功能。## 准备工作：环境要求与依赖项在开始搭建Neon环境前，请确保你的系统满足以下要求：- Linux操作系统（推荐Ubuntu 20.04+或Debian 11+）- Git