New loader for binaries

include/laser/utils/laser3d_tools.h

@@ -40,12 +40,53 @@ typedef pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr RegistrationPtr;
 
 namespace wolf
 {
-void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ>& cloud)
+void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ> &cloud)
 {
-    pcl::io::loadPCDFile(fname, cloud);
-    // remove NAN points from the cloud
-    pcl::Indices indices;
-    pcl::removeNaNFromPointCloud(cloud, cloud, indices);
+    std::string s_pcd = ".pcd", s_bin = ".bin";  
+    if (fname.compare(fname.size() - 4, 4, s_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");
+
+        
+        int32_t num = std::fread(NULL, sizeof(float), 10000000, stream) / 4;  // size of point cloud
+        float  *data = (float *)malloc(num * sizeof(float));
+
+        float  *px   = data + 0;
+        float  *py   = data + 1;
+        float  *pz   = data + 2;
+        float  *pr   = data + 3;
+
+        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);
+            // m.cloud -> push_back(*px, *py, *pz, *pr);
+            // point_cloud.points.push_back(tPoint(*px, *py, *pz, *pr));
+            px += 4;
+            py += 4;
+            pz += 4;
+            pr += 4;
+        }
+        fclose(stream);
+
+        // remove NAN points from the cloud
+        std::vector<int> indices;
+        pcl::removeNaNFromPointCloud(*pcl_ptr, *pcl_ptr, indices);
+    }
 };
 
 // _cloud_ref: first PointCloud
@@ -86,7 +127,8 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref,
     _transform_final = Eigen::Isometry3d(transform_pcl.cast<double>());
 
     // DURATION --------------------------------------------------------
-    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start);
+    auto duration =
+        std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start);
     WOLF_INFO("Laser3d_tools.h: pairAlign: duration: ", 1e-3 * (duration).count(), " ms");
     // DURATION --------------------------------------------------------
 }