关于KITTI数据中点云bin文件转成rosbag包的方法
KITTI数据集作为无人驾驶中的经典数据集，他对于点云数据的格式存储为bin文件，但是该文件对于熟悉ros环境下的小伙伴不是很友好，所以想办法将其该为rosbag包的形式就比较方便处理。
在github上有阿里员工做的转化工具，使用了一下，发现还是十分方便的，原始地址如下：
https://github.com/AbnerCSZ/lidar2rosbag_KITTI
但是其将文件夹下的所有bin文件写入一个rosbag包内，常常会出现存储空间不够的情况。为此，索性不用将所有的点云都存为一个rosbag，我们可以使用把当前bin文件的点云以pointcloud的形式发布话题，然后使用rosbag record来记录自己所需要的片段数据即可，或者直接使用监听topic来处理点云，不用使用rosbag包。简单的修改了以下（就删了几个不需要用的），剩下的代码如下：
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
ros::Publisher pubLaserCloud;
std::vector file_lists;
std::vector times_lists;
using namespace pcl;
using namespace std;
namespace po = boost::program_options;
void read_filelists(const std::string& dir_path,std::vector& out_filelsits,std::string type)
{
struct dirent *ptr;
DIR *dir;
dir = opendir(dir_path.c_str());
out_filelsits.clear();
while ((ptr = readdir(dir)) != NULL){
std::string tmp_file = ptr->d_name;
//std::cout << tmp_file <d_name);
}else{
if (tmp_file.size() < type.size())continue;
std::string tmp_cut_type = tmp_file.substr(tmp_file.size() - type.size(),type.size());
if (tmp_cut_type == type){
out_filelsits.push_back(ptr->d_name);
}
}
}
}
bool computePairNum(std::string pair1,std::string pair2)
{
return pair1 < pair2;
}
void sort_filelists(std::vector& filists,std::string type)
{
if (filists.empty())return;
std::sort(filists.begin(),filists.end(),computePairNum);
}
void Benchmark2TUM(const string &num)
{
cout << endl << "Reading KITTI benchmark from " << num << " ..." << endl;
ifstream f;
std::string in_path = "/data/Test/poses/00.txt";
f.open(num.c_str());
ifstream ft;
std::string times_path = "/data/KITTI/dataset/sequences/"+num + "/times.txt";
ft.open(num.c_str());
ofstream fo;
std::string out_path = "/data/Test/posesTUM/" +num + ".txt";
fo.open(out_path.c_str());
std::cout<<"in: "< t_lists;
if(ft.is_open())
{
double time = 0.0f;
while(!ft.eof() )
{
ft >>setprecision(12)>> time;
t_lists.push_back(time);
}
}
else{
printf("no times file\n");
}
ft.close();
if(f.is_open() )
{
int i=0;
while(!f.eof())
{
string s;
getline(f,s);
if(!s.empty())
{
stringstream ss;
ss << s;
Eigen::Matrix TwI;
double a,b,c,d, e,f,g,h, i,j,k,l;
ss>>a; ss>>b; ss>>c; ss>>d;
ss>>e; ss>>f; ss>>g; ss>>h;
ss>>i; ss>>j; ss>>k; ss>>l;
TwI << a, b, c, d,
e, f, g, h,
i, j, k, l,
0.,0.,0.,1.;
Eigen::Quaterniond q(TwI.block<3,3>(0,0));
Eigen::Vector3d t = TwI.block<3,1>(0,3);
fo << setprecision(12) << t_lists[i] << " " << t(0) << " " << t(1) << " " << t(2)
<< " " << q.x()<< " " << q.y() << " " << q.z() << " " << q.w() << endl;
}
i++;
}
f.close();
}
else
printf("ERROR: can not find the benchmark file!\n");
}
int main(int argc, char **argv) {
ros::init(argc, argv, "lidar2rosbag");
ros::NodeHandle nh;
if(argc<2)
{
printf("ERROR: Please follow the example: rosrun pkg node input num_output:\n rosrun lidar2rosbag lidar2rosbag /data/KITTI/dataset/sequences/04/ 04 \n");
return -2;
}
pubLaserCloud = nh.advertise("/velodyne_points_pub", 2);
std::string input_dir = argv[1];
std::string bin_path = input_dir + "velodyne/" ;//"/data/KITTI/dataset/sequences/04/velodyne/";
std::string times_path = input_dir + "times.txt";
//load times
times_lists.clear();
ifstream timeFile(times_path, std::ios::in);
if(timeFile.is_open())
{
double time = 0.0f;
while(!timeFile.eof() )
{
timeFile >>setprecision(12)>> time;
times_lists.push_back(time);
}
}
read_filelists( bin_path, file_lists, "bin" );
sort_filelists( file_lists, "bin" );
for(int i =0;i::Ptr points (new pcl::PointCloud);
pcl::PointCloud points;
const size_t kMaxNumberOfPoints = 1e6; // From the Readme of raw files.
points.clear();
points.reserve(kMaxNumberOfPoints);
int i;
for (i=0; input.is_open() && !input.eof(); i++) {
PointXYZI point;
input.read((char *) &point.x, 3*sizeof(float));
input.read((char *) &point.intensity, sizeof(float));
points.push_back(point);
}
input.close();
ros::Time timestamp_ros(times_lists[iter]==0? ros::TIME_MIN.toSec() :times_lists[iter]);
points.header.stamp = times_lists[iter] ;
points.header.frame_id = "velodyne";
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(points, output);
output.header.stamp = timestamp_ros ;
output.header.frame_id = "velodyne";
pubLaserCloud.publish(output);
std::cout<<"ros time : "<< output.header.stamp.toSec() <<" with "< 按着要求进行编译：
catkin_make -DCMAKE_BUILD_TYPE=Release
source ~/catkin_ws/devel/setup.bash
rosrun lidar2rosbag lidar2rosbag /media/data_odometry_velodyne/00/在rviz下显示可以看到：
