test

apps/function/multi_traj_functions.cpp

@@ -81,7 +81,7 @@ vector<string> getCrossTasks(
     trajectories.push_back(currTraj);
 
     vector<PointCloudExport::Ptr> pointClouds
-            = multiThreadGetPointCloud(trajectories);
+            = multiThreadGetPointCloud(trajectories, currTraj->getProj());
 
     float resolution = 5.f;
     pcl::octree::OctreePointCloudSearch<PointExport> octree(resolution);
@@ -133,7 +133,8 @@ void initTrajectory(
 }
 
 vector<PointCloudExport::Ptr> multiThreadGetPointCloud(
-        vector<boost::shared_ptr<Trajectory>> &trajectories)
+        vector<boost::shared_ptr<Trajectory>> &trajectories,
+        boost::shared_ptr<LocalCartesian> proj)
 {
     vector<ThreadPtr> thread_vec;
     vector<PointCloudExport::Ptr> pointClouds;
@@ -141,6 +142,7 @@ vector<PointCloudExport::Ptr> multiThreadGetPointCloud(
     for(uint8_t thread_index = 0; thread_index < trajectories.size(); thread_index++){
 
         thread_vec.emplace_back(ThreadPtr(new boost::thread(boost::bind(&getPointCloud,
+                                                                        proj,
                                                                         trajectories[thread_index],
                                                                         pointClouds[thread_index]))));
     }
@@ -151,7 +153,7 @@ vector<PointCloudExport::Ptr> multiThreadGetPointCloud(
     return pointClouds;
 }
 
-void getPointCloud(
+void getPointCloud(boost::shared_ptr<LocalCartesian> proj,
         boost::shared_ptr<Trajectory> &trajectory,
         PointCloudExport::Ptr& pointCloud)
 {
@@ -161,9 +163,15 @@ void getPointCloud(
     vector<TrajPoint> traj = trajectory->getTrajectory();
     for(const TrajPoint& trajPoint : traj){
         PointExport point;
-        point.x = trajPoint.translation.x();
-        point.y = trajPoint.translation.y();
-        point.z = trajPoint.translation.z();
+        Vector3d llh = trajectory->reverse(trajPoint);
+        Vector3d translation;
+        proj->Forward(llh.x(),llh.y(),llh.x(),
+                       translation.x(),
+                       translation.y(),
+                       translation.z());
+        point.x = translation.x();
+        point.y = translation.y();
+        point.z = translation.z();
         pointCloud->push_back(point);
     }
 }

apps/function/multi_traj_functions.h

@@ -24,9 +24,11 @@ void initTrajectory(const string &iePath,
         boost::shared_ptr<Trajectory> &trajectory);
 
 vector<PointCloudExport::Ptr> multiThreadGetPointCloud(
-        vector<boost::shared_ptr<Trajectory> > &trajectories);
+        vector<boost::shared_ptr<Trajectory> > &trajectories,
+        boost::shared_ptr<LocalCartesian> proj);
 
 void getPointCloud(
+        boost::shared_ptr<LocalCartesian> proj,
         boost::shared_ptr<Trajectory> &trajectory,
         PointCloudExport::Ptr& pointCloud);
 #endif // MULTI_TRAJ_FUNCTIONS_H

libs/trajectory/trajectory.cpp

@@ -261,6 +261,16 @@ boost::shared_ptr<LocalCartesian> Trajectory::getProj() const
     return proj_;
 }
 
+Vector3d Trajectory::reverse(const TrajPoint &trajPoint)
+{
+    Vector3d llh;
+    proj_->Reverse(trajPoint.translation.x(),
+                   trajPoint.translation.y(),
+                   trajPoint.translation.z(),
+                   llh.x(),llh.y(),llh.x());
+    return llh;
+}
+
 double Trajectory::getFrequency()
 {
     return frequency_;

libs/trajectory/trajectory.h

@@ -41,6 +41,8 @@ public:
 
     boost::shared_ptr<LocalCartesian> getProj() const;
 
+    Vector3d reverse(const TrajPoint& trajPoint);
+
     double getFrequency();
 private:
     bool checkSatisfied(const int32_t &supposed_floor_index, const double &timestamp) const;