init

apps/function/multi_traj_functions.cpp

 #include "multi_traj_functions.h"
+#include <pcl/octree/octree.h>
+
 using namespace boost::filesystem;
 
 vector<string> getCloseTasks(
@@ -26,7 +28,7 @@ vector<string> getCloseTasks(
         if(trajPoint.translation.norm() > 30000){
             continue;
         }
-        closeTasks.push_back(task_iter->path().string());
+        closeTasks.push_back(iePath);
     }
     return closeTasks;
 }
@@ -63,3 +65,101 @@ bool getFstTraj(const string &iePath,
     trajPoint.rotation = EulerToMatrix3d(Vector3d(ppk_raw_info.pitch, ppk_raw_info.roll, ppk_raw_info.yaw));
     return true;
 }
+
+vector<string> getCrossTasks(
+        const vector<string> &closeTasks,
+        boost::shared_ptr<Trajectory> currTraj)
+{
+    vector<string> crossTasks;
+    vector<boost::shared_ptr<Trajectory>> trajectories =
+            multiThreadInitTrajectory(closeTasks);
+
+    trajectories.push_back(currTraj);
+
+    vector<PointCloudExport::Ptr> pointClouds
+            = multiThreadGetPointCloud(trajectories);
+
+    float resolution = 5.f;
+    pcl::octree::OctreePointCloudSearch<PointExport> octree(resolution);
+    octree.setInputCloud(pointClouds.back());
+    octree.addPointsFromInputCloud();
+    for(size_t pcIndex = 0; pcIndex < pointClouds.size() - 1; pcIndex++){
+        bool found = false;
+        for(const PointExport& query : pointClouds[pcIndex]->points){
+            int result_index;
+            float sqr_distance;
+            octree.approxNearestSearch(query, result_index, sqr_distance);
+            if(sqr_distance < 10){
+                found = true;
+                break;
+            }
+        }
+        if(found){
+            crossTasks.push_back(closeTasks[pcIndex]);
+        }
+    }
+    return crossTasks;
+}
+
+vector<boost::shared_ptr<Trajectory>> multiThreadInitTrajectory(
+        const vector<string> &iePaths)
+{
+    vector<ThreadPtr> thread_vec;
+    vector<boost::shared_ptr<Trajectory>> trajectories;
+    trajectories.resize(iePaths.size());
+    for(uint8_t thread_index = 0; thread_index < iePaths.size(); thread_index++){
+
+        thread_vec.emplace_back(ThreadPtr(new boost::thread(boost::bind(&initTrajectory,
+                                                                        iePaths[thread_index],
+                                                                        trajectories[thread_index]))));
+    }
+    for(uint8_t thread_index = 0; thread_index < thread_vec.size(); thread_index++){
+        thread_vec[thread_index]->join();
+        thread_vec[thread_index].reset();
+    }
+    return trajectories;
+}
+
+void initTrajectory(
+        const string &iePath,
+        boost::shared_ptr<Trajectory> &trajectory)
+{
+    trajectory.reset(new Trajectory(iePath, PPK));
+    trajectory->init();
+}
+
+vector<PointCloudExport::Ptr> multiThreadGetPointCloud(
+        vector<boost::shared_ptr<Trajectory>> &trajectories)
+{
+    vector<ThreadPtr> thread_vec;
+    vector<PointCloudExport::Ptr> pointClouds;
+    pointClouds.resize(trajectories.size());
+    for(uint8_t thread_index = 0; thread_index < trajectories.size(); thread_index++){
+
+        thread_vec.emplace_back(ThreadPtr(new boost::thread(boost::bind(&getPointCloud,
+                                                                        trajectories[thread_index],
+                                                                        pointClouds[thread_index]))));
+    }
+    for(uint8_t thread_index = 0; thread_index < thread_vec.size(); thread_index++){
+        thread_vec[thread_index]->join();
+        thread_vec[thread_index].reset();
+    }
+    return pointClouds;
+}
+
+void getPointCloud(
+        boost::shared_ptr<Trajectory> &trajectory,
+        PointCloudExport::Ptr& pointCloud)
+{
+    if(!pointCloud){
+        pointCloud.reset(new PointCloudExport);
+    }
+    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();
+        pointCloud->push_back(point);
+    }
+}

apps/function/multi_traj_functions.h

@@ -3,6 +3,7 @@
 
 #include "utils/basic_types.h"
 #include "trajectory/trajectory.h"
+#include "utils/pcl_point_type.h"
 
 vector<string> getCloseTasks(
         const string &parentPath,
@@ -12,4 +13,20 @@ bool getFstTraj(const string &iePath,
                 TrajPoint& trajPoint,
                 boost::shared_ptr<LocalCartesian> proj);
 
+vector<string> getCrossTasks(
+        const vector<string> &closeTasks,
+        boost::shared_ptr<Trajectory> currTraj);
+
+vector<boost::shared_ptr<Trajectory>> multiThreadInitTrajectory(
+        const vector<string> &iePaths);
+
+void initTrajectory(const string &iePath,
+        boost::shared_ptr<Trajectory> &trajectory);
+
+vector<PointCloudExport::Ptr> multiThreadGetPointCloud(
+        vector<boost::shared_ptr<Trajectory> > &trajectories);
+
+void getPointCloud(
+        boost::shared_ptr<Trajectory> &trajectory,
+        PointCloudExport::Ptr& pointCloud);
 #endif // MULTI_TRAJ_FUNCTIONS_H

apps/multi_traj.cpp

@@ -39,14 +39,18 @@ int main(int argc, char *argv[])
         LOG(INFO) << "close task chekc in: " << task;
     }
 
-    string outputPath = FLAGS_base_dir + "/slam/cartographer.bag.pbstream.interpolated.pose";
 
+    vector<string> crossTasks = getCrossTasks(closeTasks, ppk_traj_);
+    for(string task : crossTasks){
+        LOG(INFO) << "cross task chekc in: " << task;
+    }
+
+    string outputPath = FLAGS_base_dir + "/slam/cartographer.bag.pbstream.interpolated.pose";
     std::ofstream ofs(outputPath);
     if(!ofs){
         LOG(WARNING) << outputPath << " load fail!";
         return 0;
     }
-
     ofs.close();
     return 1;
 }