fixed loadData()

include/laser/utils/laser3d_tools.h

@@ -36,53 +36,47 @@
 #include <pcl/registration/icp_nl.h>
 #include <pcl/registration/transforms.h>
 
+#include <iostream>
+#include <fstream>
+
 typedef pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr RegistrationPtr;
 
 namespace wolf
 {
 void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ> &cloud)
 {
-    std::string s_pcd = ".pcd", s_bin = ".bin";  
-    if (fname.compare(fname.size() - 4, 4, s_pcd) == 0)
+    //std::string s_pcd = ".pcd", s_bin = ".bin";
+    if (fname.compare(fname.size() - 4, 4, ".pcd") == 0)
     {
         pcl::io::loadPCDFile(fname, cloud);
-        // remove NAN points from the cloud
-        pcl::Indices indices;
-        pcl::removeNaNFromPointCloud(cloud, cloud, indices);
-    }
-    else if (fname.compare(fname.size() - 4, 4, s_bin) == 0)  
-    {   // file is binary
-        pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 
-        FILE *stream;
-        stream = std::fopen(fname.c_str(), "rb");
+    }
+    else if (fname.compare(fname.size() - 4, 4, ".bin") == 0)
+    {  // file is binary
+        //pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+
+        std::ifstream stream_input;
+    
+        stream_input.open(fname, std::ios::in);
+        for (int i = 0; stream_input.good() && !stream_input.eof(); i++)
+        {
+            pcl::PointXYZI point_raw;
+            stream_input.read((char *)&point_raw.x, 3 * sizeof(float));
+            stream_input.read((char *)&point_raw.intensity, sizeof(float));
 
-        
-        int32_t num = std::fread(NULL, sizeof(float), 10000000, stream) / 4;  // size of point cloud
-        float  *data = (float *)malloc(num * sizeof(float));
+            pcl::PointXYZ tPoint;
 
-        float  *px   = data + 0;
-        float  *py   = data + 1;
-        float  *pz   = data + 2;
-        float  *pr   = data + 3;
+            tPoint.x = point_raw.x;
+            tPoint.y = point_raw.y;
+            tPoint.z = point_raw.z;
 
-        pcl::PointXYZ tPoint;
-        for (int32_t i = 0; i < num; i++)
-        {
-            tPoint.x = *px;
-            tPoint.y = *py;
-            tPoint.z = *pz;
-
-            pcl_ptr->push_back(tPoint);
-            px += 4;
-            py += 4;
-            pz += 4;
-            pr += 4;
+            cloud.push_back(tPoint);
         }
-        fclose(stream);
-
-        std::vector<int> indices;
-        pcl::removeNaNFromPointCloud(*pcl_ptr, *pcl_ptr, indices);
+        stream_input.close();
+        
+        // remove NAN points from the cloud
+        pcl::Indices indices;
+        pcl::removeNaNFromPointCloud(cloud, cloud, indices);
     }
 };
 
@@ -109,7 +103,7 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref,
     Eigen::Matrix4f transform_guess = _transform_guess.matrix().cast<float>();
 
     // Alignment
-    pcl::PointCloud<pcl::PointXYZ>cloud_out;
+    pcl::PointCloud<pcl::PointXYZ> cloud_out;
     _registration_solver->align(cloud_out, transform_guess);
     transform_pcl = _registration_solver->getFinalTransformation();