consider diffrent direction

CMakeLists.txt.user

 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.5.2, 2023-02-10T18:03:44. -->
+<!-- Written by QtCreator 4.5.2, 2023-02-15T17:39:17. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>
@@ -209,7 +209,7 @@
      <value type="int">14</value>
     </valuelist>
     <value type="QString" key="CMakeProjectManager.CMakeRunConfiguation.Title">multi_traj</value>
-    <value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.Arguments">--base_dir=/home/juefx/workspace/multitraj/test/2464</value>
+    <value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.Arguments">--base_dir=/home/juefx/workspace/multitraj/test/2465</value>
     <value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.UserWorkingDirectory"></value>
     <value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.UserWorkingDirectory.default">/home/juefx/workspace/multitraj/build-multitraj-Desktop-Release/apps</value>
     <value type="int" key="PE.EnvironmentAspect.Base">2</value>

apps/function/multi_traj_functions.h

@@ -55,8 +55,7 @@ vector<PointCloudExport::Ptr> multiThreadGetPointCloud(
         vector<boost::shared_ptr<Trajectory> > &trajectories,
         boost::shared_ptr<LocalCartesian> proj);
 
-void getPointCloud(
-        boost::shared_ptr<LocalCartesian> proj,
+void getPointCloud(boost::shared_ptr<LocalCartesian> proj,
         boost::shared_ptr<Trajectory> &trajectory,
-        PointCloudExport::Ptr& pointCloud);
+        PointCloudExport::Ptr& pointCloud, uint8_t index);
 #endif // MULTI_TRAJ_FUNCTIONS_H

libs/trajectory/trajectory.cpp

@@ -346,9 +346,18 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
                        translation.y(),
                        translation.z());
         trajPoint.translation = translation;
-        trajPoint.rotation = EulerToMatrix3d(Vector3d(ppk_raw_info.pitch, ppk_raw_info.roll, ppk_raw_info.yaw));
+//        trajPoint.rotation = EulerToMatrix3d(Vector3d(ppk_raw_info.pitch, ppk_raw_info.roll, ppk_raw_info.yaw));
+        trajPoint.rotation = Eigen::AngleAxisd(ppk_raw_info.yaw/180*M_PI, Eigen::Vector3d::UnitZ())
+                * Eigen::AngleAxisd(ppk_raw_info.pitch/180*M_PI, Eigen::Vector3d::UnitX())
+                * Eigen::AngleAxisd(ppk_raw_info.roll/180*M_PI, Eigen::Vector3d::UnitY());
         trajPoint.nodeId = trajectory_.size();
         trajPoint.timestamp = gpsToUtc(ppk_raw_info.gps_week, ppk_raw_info.gps_sec);
+        if(fabs(trajPoint.timestamp - 1675018189.7) < 0.01){
+            LOG(INFO) << "1675018189.7: " << ppk_raw_info.yaw;
+        }
+        if(fabs(trajPoint.timestamp - 1675021549.1) < 0.01){
+            LOG(INFO) << "1675021549.1: " << ppk_raw_info.yaw;
+        }
         if(trajPoint.timestamp > startTime_ && trajPoint.timestamp < endTime_){
             if(multiTrajectory_[index].size() == 0){
                 multiTrajectory_[index].push_back(trajPoint);

libs/trajectory/trajectory.h

@@ -51,6 +51,14 @@ public:
     static double gpsToUtc(const uint32_t &gps_week, const double &gps_sec) {
         return (315936000 + gps_week*7*24*3600 + gps_sec - 18);
     }
+
+    bool GetTrajPoint(size_t index, TrajPoint& trajPoint){
+        if(index >= trajectory_.size()){
+            return false;
+        }
+        trajPoint = trajectory_.at(index);
+        return true;
+    }
 private:
     bool checkSatisfied(const int32_t &supposed_floor_index, const double &timestamp) const;
 

libs/utils/pcl_point_type.h

@@ -57,15 +57,16 @@ struct PointExport {
     PCL_ADD_POINT4D
     PCL_ADD_RGB
     float intensity = 0;
-    float label = 0;
     double info = 0;
+    float taskIndex = 0;
+    float pointId = 0;
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
 } EIGEN_ALIGN16;
 }//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)(double, info, info))
+        (float, intensity, intensity)(double, info, info)(float, taskIndex, taskIndex)(float, pointId, pointId))
 
 typedef pcl::PointExport PointExport;
 typedef pcl::PointCloud<PointExport> PointCloudExport;