test

CMakeLists.txt.user

 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.5.2, 2021-12-29T15:27:02. -->
+<!-- Written by QtCreator 4.5.2, 2022-01-04T18:54:51. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>
@@ -72,7 +72,7 @@
     <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/juefx/workspace/multitraj/build-multitraj-Desktop-Debug</value>
     <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
      <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
-      <value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>
+      <value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments">-j8</value>
       <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets">
        <value type="QString">all</value>
       </valuelist>
@@ -208,14 +208,15 @@
      <value type="int">13</value>
      <value type="int">14</value>
     </valuelist>
-    <value type="int" key="PE.EnvironmentAspect.Base">-1</value>
+    <value type="QString" key="CMakeProjectManager.CMakeRunConfiguation.Title">multi_traj</value>
+    <value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.Arguments"></value>
+    <value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.UserWorkingDirectory"></value>
+    <value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.UserWorkingDirectory.default">/home/juefx/workspace/multitraj/build-multitraj-Desktop-Debug/apps</value>
+    <value type="int" key="PE.EnvironmentAspect.Base">2</value>
     <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
-    <value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.Arguments"></value>
-    <value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.Executable"></value>
-    <value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.WorkingDirectory">%{buildDir}</value>
-    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Custom Executable</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">multi_traj</value>
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
-    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeRunConfiguration.multi_traj</value>
     <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
     <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
     <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>

apps/function/multi_traj_functions.cpp

@@ -20,6 +20,160 @@ vector<string> getActiveTraces(
     return ret;
 }
 
+void getCloseTasks(
+        const vector<string> &allTask,
+        vector<string> &crossTasks)
+{
+    if(allTask.size() < 2){
+        return;
+    }
+    vector<TaskInfo> allTaskInfo;
+    TaskInfo taskInfo;
+    if(!getTaskInfo(allTask.front(), taskInfo)){
+        LOG(WARNING) << "currTask getTaskInfo fail!";
+        return;
+    }
+    allTaskInfo.push_back(taskInfo);
+    for(size_t i = 1; i < allTask.size(); i++){
+        string task = allTask[i];
+        if(!getTaskInfo(task, taskInfo)){
+            continue;
+        }
+        allTaskInfo.push_back(taskInfo);
+    }
+    if(allTaskInfo.size() < 2){
+        return;
+    }
+    vector<TaskInfo> filteredTaskInfo = allTaskInfo;
+    if(allTaskInfo.front().meshOutPath != ""){
+        filteredTaskInfo = meshOutFilter(allTaskInfo);
+    }
+    if(filteredTaskInfo.size() < 2){
+        filteredTaskInfo = allTaskInfo;
+    }
+    crossTasks = crossFilter(filteredTaskInfo);
+}
+
+bool getTaskInfo(
+        const string &taskPath,
+        TaskInfo &taskInfo)
+{
+    if (!is_directory(taskPath)){
+        return false;
+    }
+    string rawTraceDir = taskPath + "/raw_trace";
+    if(!is_directory(rawTraceDir)){
+        return false;
+    }
+    string trajPath = rawTraceDir + "/sample.gps";
+    if(!exists(trajPath)){
+       trajPath = rawTraceDir + "/ie.txt";;
+    }
+    if(!exists(trajPath)){
+        return false;
+    }
+    taskInfo.taskPath = taskPath;
+    taskInfo.trajPath = trajPath;
+    string meshOutPath = taskPath + "/meshes.out";
+    if(!exists(meshOutPath)){
+       taskInfo.meshOutPath = meshOutPath;
+    }
+}
+
+vector<TaskInfo> meshOutFilter(
+        const vector<TaskInfo> &allTaskInfo){
+    vector<TaskInfo> ret;
+    if(allTaskInfo.size() < 2){
+        return ret;
+    }
+    TaskInfo currTaskInfo = allTaskInfo.front();
+    MeshOut currMeshOut =
+            parseMeshOut(currTaskInfo.meshOutPath);
+    if(currMeshOut.size() == 0){
+        return allTaskInfo;
+    }
+    for(size_t i = 1; i < allTaskInfo.size(); i++){
+        TaskInfo taskInfo = allTaskInfo[i];
+        if(checkMeshOut(parseMeshOut(taskInfo.meshOutPath),
+                        currMeshOut)){
+            ret.push_back(taskInfo);
+        }
+    }
+    return ret;
+}
+
+MeshOut parseMeshOut(
+        const string &meshOutPath){
+    MeshOut ret;
+    std::ifstream ifs(meshOutPath);
+    if(!ifs){
+        LOG(WARNING) << meshOutPath << " load fail!";
+        return ret;
+    }
+    string line;
+    while(getline(ifs, line)){
+        vector<string> line_vec;
+        boost::split(line_vec, line, boost::is_any_of(","), boost::token_compress_on);
+        if(line_vec.size() != 2){
+            continue;
+        }
+        ret.push_back(make_pair(line_vec.front(), line_vec.back()));
+    }
+    ifs.close();
+    return ret;
+}
+
+bool checkMeshOut(
+        const MeshOut& query,
+        const MeshOut& target){
+    for(auto &q : query){
+        for(auto &t : target){
+            if(q.first == t.first &&
+                    q.second == t.second){
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+vector<string> crossFilter(
+        const vector<TaskInfo> &filteredTaskInfo){
+    vector<string> ret;
+    if(filteredTaskInfo.size() < 2){
+        return ret;
+    }
+
+    vector<boost::shared_ptr<Trajectory>> trajectories;
+    for(size_t i = 0; i < filteredTaskInfo.size(); i++){
+        initTrajectory(filteredTaskInfo[i].trajPath, trajectories[i]);
+    }
+    boost::shared_ptr<Trajectory> currTrajectory =
+            trajectories.front();
+    vector<PointCloudExport::Ptr> pointClouds
+            = multiThreadGetPointCloud(trajectories, currTrajectory->getProj());
+    float resolution = 5.f;
+    pcl::octree::OctreePointCloudSearch<PointExport> octree(resolution);
+    octree.setInputCloud(pointClouds.front());
+    octree.addPointsFromInputCloud();
+    for(size_t pcIndex = 1; pcIndex < pointClouds.size(); 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){
+            ret.push_back(filteredTaskInfo[pcIndex].taskPath);
+        }
+    }
+    return ret;
+}
+
 vector<string> getCloseTasks(
         const string &parentPath,
         boost::shared_ptr<Trajectory> currTraj)
@@ -210,3 +364,4 @@ void getPointCloud(boost::shared_ptr<LocalCartesian> proj,
         pointCloud->push_back(point);
     }
 }
+

apps/function/multi_traj_functions.h

@@ -5,6 +5,12 @@
 #include "trajectory/trajectory.h"
 #include "utils/pcl_point_type.h"
 
+struct TaskInfo{
+    string taskPath = "";
+    string trajPath = "";
+    string meshOutPath = "";
+};
+
 vector<string> getActiveTraces(
         const string& activeTracePath);
 
@@ -12,6 +18,26 @@ vector<string> getCloseTasks(
         const string &parentPath,
         boost::shared_ptr<Trajectory> currTraj);
 
+void getCloseTasks(const vector<string> &allTask, vector<string> &crossTasks);
+
+bool getTaskInfo(
+        const string &taskPath,
+        TaskInfo& taskInfo);
+
+vector<TaskInfo> meshOutFilter(
+        const vector<TaskInfo> &allTaskInfo);
+
+typedef vector<pair<string, string>> MeshOut;
+MeshOut parseMeshOut(
+        const string &meshOutPath);
+
+bool checkMeshOut(
+        const MeshOut& query,
+        const MeshOut& target);
+
+vector<string> crossFilter(
+        const vector<TaskInfo> &filteredTaskInfo);
+
 bool getFstTraj(const string &trajPath,
                 TrajPoint& trajPoint,
                 boost::shared_ptr<LocalCartesian> proj);

apps/multi_traj.cpp

@@ -30,42 +30,44 @@ int main(int argc, char *argv[])
         return 0;
     }
     vector<string> activeTraces = getActiveTraces(activeTracePath);
+    activeTraces.insert(activeTraces.begin(), FLAGS_base_dir);
 
-    string ppk_dir_ = FLAGS_base_dir + "/raw_trace/sample.gps";
-    if(exists(ppk_dir_)){
-        ppk_traj_.reset(new Trajectory(ppk_dir_, PPK, 10));
-    }else {
-        ppk_dir_ = FLAGS_base_dir + "/raw_trace/ie.txt";
-        if(!exists(ppk_dir_)){
-            return 0;
-        }
-        ppk_traj_.reset(new Trajectory(ppk_dir_, PPK));
-    }
-    ppk_traj_->init();
-    LOG(INFO) << "Trajectory load done!";
-
-    boost::filesystem::path base_dir(FLAGS_base_dir);
-    string parentPath = base_dir.parent_path().string();
-    LOG(INFO) << "parentPath: " << parentPath;
+//    string ppk_dir_ = FLAGS_base_dir + "/raw_trace/sample.gps";
+//    if(exists(ppk_dir_)){
+//        ppk_traj_.reset(new Trajectory(ppk_dir_, PPK, 10));
+//    }else {
+//        ppk_dir_ = FLAGS_base_dir + "/raw_trace/ie.txt";
+//        if(!exists(ppk_dir_)){
+//            return 0;
+//        }
+//        ppk_traj_.reset(new Trajectory(ppk_dir_, PPK));
+//    }
+//    ppk_traj_->init();
+//    LOG(INFO) << "Trajectory load done!";
 
-    vector<string> closeTasks = getCloseTasks(parentPath, ppk_traj_);
-    for(string task : closeTasks){
-        LOG(INFO) << "close task chekc in: " << task;
-    }
+//    boost::filesystem::path base_dir(FLAGS_base_dir);
+//    string parentPath = base_dir.parent_path().string();
+//    LOG(INFO) << "parentPath: " << parentPath;
 
+//    vector<string> closeTasks = getCloseTasks(parentPath, ppk_traj_);
+//    for(string task : closeTasks){
+//        LOG(INFO) << "close task chekc in: " << task;
+//    }
+//    vector<string> crossTasks = getCrossTasks(closeTasks, ppk_traj_);
 
-    vector<string> crossTasks = getCrossTasks(closeTasks, ppk_traj_);
+    vector<string> crossTasks;
+    getCloseTasks(activeTraces, crossTasks);
     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();
+//    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;
 }