merge files from pcl-1 online code

libs/lidar/pcl_point_type.h

@@ -3,8 +3,8 @@
 #ifndef PCL_POINT_TYPE_H
 #define PCL_POINT_TYPE_H
 
-#include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
 
 namespace pcl {
 struct PointInternal {
@@ -17,12 +17,13 @@ struct PointInternal {
   double timestamp;
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
 } EIGEN_ALIGN16;
-}//end namespace pcl
+}  // end namespace pcl
 
 POINT_CLOUD_REGISTER_POINT_STRUCT(
-    pcl::PointInternal, (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)
-        (float, intensity, intensity)(float, ring, ring)(float, label, label)
-        (float, distance, distance)(double, timestamp, timestamp))
+    pcl::PointInternal,
+    (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)(
+        float, intensity, intensity)(float, ring, ring)(float, label, label)(
+        float, distance, distance)(double, timestamp, timestamp))
 
 typedef pcl::PointInternal Point;
 typedef pcl::PointCloud<Point> PointCloud;
@@ -39,12 +40,12 @@ struct PointRos {
   float timestamp;
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
 } EIGEN_ALIGN32;
-}//end namespace pcl
+}  // end namespace pcl
 
 POINT_CLOUD_REGISTER_POINT_STRUCT(
-    pcl::PointRos, (float, x, x)(float, y, y)(float, z, z)
-        (float, intensity, intensity)(float, ring, ring)(float, label, label)
-        (float, timestamp, timestamp))
+    pcl::PointRos,
+    (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(
+        float, ring, ring)(float, label, label)(float, timestamp, timestamp))
 
 typedef pcl::PointRos PointRos;
 typedef pcl::PointCloud<PointRos> PointCloudRos;
@@ -61,11 +62,12 @@ struct PointExport {
   float info = 0;
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
 } EIGEN_ALIGN16;
-}//end namespace pcl
+}  // end namespace pcl
 
 POINT_CLOUD_REGISTER_POINT_STRUCT(
-    pcl::PointExport, (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)
-        (float, intensity, intensity)(float, label, label)(float, info, info))
+    pcl::PointExport,
+    (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)(
+        float, intensity, intensity)(float, label, label)(float, info, info))
 
 typedef pcl::PointExport PointExport;
 typedef pcl::PointCloud<PointExport> PointCloudExport;
@@ -80,14 +82,32 @@ struct PointTemp1 {
   float time;
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
 } EIGEN_ALIGN16;
-}//end namespace pcl
+}  // end namespace pcl
 
 POINT_CLOUD_REGISTER_POINT_STRUCT(
-    pcl::PointTemp1, (float, x, x)(float, y, y)(float, z, z)
-        (float, intensity, intensity)(float, ring, ring)(float, label, label)
-        (float, time, time))
+    pcl::PointTemp1,
+    (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(
+        float, ring, ring)(float, label, label)(float, time, time))
 
 typedef pcl::PointTemp1 PointTemp1;
 
+/////////////////////////////////////////////////////////////
+namespace pcl {
+struct PointI {
+  PCL_ADD_POINT4D
+  float intensity;
+  float stamp_offset;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+} EIGEN_ALIGN16;
+}  // end namespace pcl
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+    pcl::PointI,
+    (float, x, x)(float, y, y)(float, z, z)(float, intensity,
+                                            intensity)(float, stamp_offset,
+                                                       stamp_offset))
+
+typedef pcl::PointI PointI;
+typedef pcl::PointCloud<PointI> PointCloudI;
 
 #endif  // PCL_POINT_TYPE_H

libs/reader/pcd_reader.cpp

@@ -49,7 +49,8 @@ bool PcdReader::loadFile() {
 bool PcdReader::getNextFrame(PointCloud*& frame) {
   if (pcdIndex_ >= pcdPathVec_.size()) return false;
   bool suceess = false;
-  PointCloud tempFrame;
+  PointCloudI tempFrame;
+  double time;
   while (!suceess) {
     string pcdPath = pcdPathVec_.at(pcdIndex_);
     pcdIndex_++;
@@ -60,17 +61,25 @@ bool PcdReader::getNextFrame(PointCloud*& frame) {
     if (iter == frameTimeMap_.end()) {
       continue;
     }
-    double time = iter->second;
+    time = iter->second;
     int8_t ret = checkTime(time, durations_);
     if (ret != 1) {
       continue;
     }
-    for (auto& point : tempFrame) {
-      point.timestamp = time;
-    }
+    //    LOG(INFO) << setprecision(15) << "frameId: " << frameId
+    //              << " time: " << time;
     suceess = true;
   }
   frame = new PointCloud;
-  *frame = tempFrame;
+  for (auto& pointI : tempFrame) {
+    Point point;
+    point.x = pointI.x;
+    point.y = pointI.y;
+    point.z = pointI.z;
+    point.timestamp = time + pointI.stamp_offset * 0.001;
+    point.intensity = pointI.intensity;
+    point.distance = sqrt(pow(point.x, 2) + pow(point.y, 2) + pow(point.z, 2));
+    frame->push_back(point);
+  }
   return true;
 }