Kitti数据集转txt格式并求法向量

#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/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>
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, sequence_number, output_bag_file;
    n.getParam("dataset_folder", dataset_folder);
   
    std::string binDataPath(dataset_folder+"velodyne/");

//    std::cout<<"binDataPath:"<<binDataPath.c_str()<<std::endl;

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

    if (dir == NULL)
    {
        cout << "opendir error" << endl;
    }
    struct dirent *entry;
    vector<string> allPath;
    while (((entry = readdir(dir)) != NULL))
    {
        if(entry->d_name[0]!='.')

        {
            ofstream outputfile;
            ostringstream temp;
            string filename;
            temp<<dataset_folder<<"velodynetxt/"<<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;

            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;
               point.x = lidar_data[i];
               point.y = lidar_data[i + 1];
               point.z = lidar_data[i + 2];
//               point.intensity = lidar_data[i + 3];  //intensity will not be used
               laser_cloud.push_back(point);
            }

            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
            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.computePointNormal(laser_cloud,pointIdxRadiusSearch,plane_parameters,curvature);
//                pcl::Normal singleNormal;                      //for viewer
//                singleNormal.normal_x=plane_parameters.x();    //  for viewer
//                singleNormal.normal_y=plane_parameters.y();    //   for viewer
//                singleNormal.normal_z=plane_parameters.z();    //   for viewer
//                normals->push_back(singleNormal);             //   for viewer
               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;
            }

//            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()) //for viewer
//            {
//                viewer->spinOnce(100);
//                boost::this_thread::sleep(boost::posix_time::microseconds(100000));   //for viewer
//            }


            laser_cloud.clear();
            outputfile.close();
//            normals->clear(); //  clear normals for viewer
        }

//        r.sleep();
    }
    std::cout << "Done \n";
    closedir(dir);


    return 0;
}