init

libs/trajectory/trajectory.cpp

@@ -337,7 +337,7 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
         if(multiTrajectory_[index].size() == 0){
             multiTrajectory_[index].push_back(trajPoint);
         }else{
-            if((trajPoint.translation - multiTrajectory_[index].begin()->translation).norm() > 1){
+            if((trajPoint.translation - multiTrajectory_[index].back().translation).norm() > 1){
                 multiTrajectory_[index].push_back(trajPoint);
             }
         }