change filter by position stratgy and add startTime and endTime

CMakeLists.txt.user

 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.5.2, 2023-02-09T13:22:15. -->
+<!-- Written by QtCreator 4.5.2, 2023-02-09T16:55:42. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>

apps/function/multi_traj_functions.h

@@ -10,6 +10,8 @@ struct TaskInfo{
     string trajPath = "";
     string meshOutPath = "";
     uint32_t meshId = 0;
+    double startTime = 0;
+    double endTime = 0;
 };
 
 struct CloseTrajInfo{
@@ -46,7 +48,7 @@ bool checkMeshOut(
 vector<CloseTrajInfo> crossFilter(
         const vector<TaskInfo> &filteredTaskInfo);
 
-bool initTrajectory(const string &trajPath,
+bool initTrajectory(const TaskInfo &taskInfo,
         boost::shared_ptr<Trajectory> &trajectory);
 
 vector<PointCloudExport::Ptr> multiThreadGetPointCloud(

libs/trajectory/trajectory.cpp

@@ -21,8 +21,10 @@ Trajectory::~Trajectory()
     if(io_) delete io_;
 }
 
-bool Trajectory::init()
+bool Trajectory::init(double startTime, double endTime)
 {
+    startTime_ = startTime;
+    endTime_ = endTime;
     string range;
     switch(trajType_){
     case PPK:
@@ -303,9 +305,6 @@ bool Trajectory::checkSatisfied(const int32_t &supposed_floor_index, const doubl
     }
 }
 
-double gpsToUtc(const uint32_t &gps_week, const double &gps_sec) {
-    return (315936000 + gps_week*7*24*3600 + gps_sec - 18);
-}
 
 void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
 {
@@ -350,11 +349,13 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
         trajPoint.rotation = EulerToMatrix3d(Vector3d(ppk_raw_info.pitch, ppk_raw_info.roll, ppk_raw_info.yaw));
         trajPoint.nodeId = trajectory_.size();
         trajPoint.timestamp = gpsToUtc(ppk_raw_info.gps_week, ppk_raw_info.gps_sec);
-        if(multiTrajectory_[index].size() == 0){
-            multiTrajectory_[index].push_back(trajPoint);
-        }else{
-            if((trajPoint.translation - multiTrajectory_[index].back().translation).norm() > 1){
+        if(trajPoint.timestamp > startTime_ && trajPoint.timestamp < endTime_){
+            if(multiTrajectory_[index].size() == 0){
                 multiTrajectory_[index].push_back(trajPoint);
+            }else{
+                if((trajPoint.translation - multiTrajectory_[index].back().translation).norm() > 1){
+                    multiTrajectory_[index].push_back(trajPoint);
+                }
             }
         }
 

libs/trajectory/trajectory.h

@@ -16,7 +16,7 @@ public:
 
     ~Trajectory();
 
-    bool init();
+    bool init(double startTime, double endTime);
 
     string getTrajPath() const;
 
@@ -47,6 +47,10 @@ public:
     Vector3d reverse(const TrajPoint& trajPoint);
 
     double getFrequency();
+
+    static double gpsToUtc(const uint32_t &gps_week, const double &gps_sec) {
+        return (315936000 + gps_week*7*24*3600 + gps_sec - 18);
+    }
 private:
     bool checkSatisfied(const int32_t &supposed_floor_index, const double &timestamp) const;
 
@@ -67,6 +71,8 @@ private:
     boost::shared_ptr<LocalCartesian> proj_;
 
     boost::shared_mutex Mtx_;
+    double startTime_;
+    double endTime_;
 };
 
 #endif // TRAJECTORY_H