test

apps/function/multi_traj_functions.cpp

@@ -20,7 +20,7 @@ vector<string> getActiveTraces(
     return ret;
 }
 
-void getCloseTasks(
+void getCrossTasks(
         const vector<string> &allTask,
         vector<string> &crossTasks)
 {
@@ -36,6 +36,10 @@ void getCloseTasks(
     allTaskInfo.push_back(taskInfo);
     for(size_t i = 1; i < allTask.size(); i++){
         string task = allTask[i];
+        if(boost::filesystem::path(task).filename().string() ==
+                boost::filesystem::path(allTask.front()).filename().string()){
+            continue;
+        }
         if(!getTaskInfo(task, taskInfo)){
             continue;
         }
@@ -51,6 +55,9 @@ void getCloseTasks(
     if(filteredTaskInfo.size() < 2){
         filteredTaskInfo = allTaskInfo;
     }
+    for(TaskInfo taskInfo : filteredTaskInfo){
+        LOG(INFO) << "closeTask: " << taskInfo.taskPath;
+    }
     crossTasks = crossFilter(filteredTaskInfo);
 }
 
@@ -59,12 +66,12 @@ bool getTaskInfo(
         TaskInfo &taskInfo)
 {
     if (!is_directory(taskPath)){
-        LOG(INFO) << "no directory: " << taskPath;
+//        LOG(INFO) << "no directory: " << taskPath;
         return false;
     }
     string rawTraceDir = taskPath + "/raw_trace";
     if(!is_directory(rawTraceDir)){
-        LOG(INFO) << "no directory: " << rawTraceDir;
+//        LOG(INFO) << "no directory: " << rawTraceDir;
         return false;
     }
     string trajPath = rawTraceDir + "/sample.gps";
@@ -72,7 +79,7 @@ bool getTaskInfo(
        trajPath = rawTraceDir + "/ie.txt";;
     }
     if(!exists(trajPath)){
-        LOG(INFO) << "dont exists: " << trajPath;
+//        LOG(INFO) << "dont exists: " << trajPath;
         return false;
     }
     taskInfo.taskPath = taskPath;
@@ -179,142 +186,13 @@ vector<string> crossFilter(
     return ret;
 }
 
-vector<string> getCloseTasks(
-        const string &parentPath,
-        boost::shared_ptr<Trajectory> currTraj)
-{
-    vector<string> closeTasks;
-    boost::shared_ptr<LocalCartesian> proj = currTraj->getProj();
-    directory_iterator task_iter(parentPath);
-    directory_iterator task_end_iter;
-    for(; task_iter != task_end_iter; task_iter++) {
-        if (!is_directory(task_iter->path())){
-            continue;
-        }
-        string rawTraceDir = task_iter->path().string() + "/raw_trace";
-        if(!is_directory(rawTraceDir)){
-            continue;
-        }
-        string trajPath = rawTraceDir + "/sample.gps";
-        if(!exists(trajPath)){
-           trajPath = rawTraceDir + "/ie.txt";;
-        }
-        if(!exists(trajPath)){
-            continue;
-        }
-        if(currTraj->getTrajPath() == trajPath){
-            continue;
-        }
-
-        TrajPoint trajPoint;
-        if(!getFstTraj(trajPath, trajPoint, proj)){
-            continue;
-        }
-        if(trajPoint.translation.norm() > 30000){
-            continue;
-        }
-        closeTasks.push_back(trajPath);
-    }
-    return closeTasks;
-}
-
-bool getFstTraj(const string &trajPath,
-                TrajPoint& trajPoint,
-                boost::shared_ptr<LocalCartesian> proj){
-    std::ifstream ifs(trajPath);
-    if(!ifs){
-        LOG(WARNING) << trajPath << " load fail!";
-        return false;
-    }
-    string line;
-    getline(ifs, line);
-    getline(ifs, line);
-    ifs.close();
-    vector<string> line_vec;
-    boost::split(line_vec, line, boost::is_any_of(","), boost::token_compress_on);
-    if(line_vec.size() < 13){
-        return false;
-    }
-    PPK_Raw_Info ppk_raw_info;
-    ppk_raw_info.gps_week = stoi(line_vec[1]);
-    ppk_raw_info.gps_sec = stod(line_vec[2]);
-    ppk_raw_info.lat = stod(line_vec[3]);
-    ppk_raw_info.lng = stod(line_vec[4]);
-    ppk_raw_info.height = stod(line_vec[5]);
-    ppk_raw_info.roll = stod(line_vec[9]);
-    ppk_raw_info.pitch = stod(line_vec[10]);
-    ppk_raw_info.yaw = -stod(line_vec[11]); //ppk yaw take minus!!!!!!!!!!!!!!!!!!!!
-    Vector3d translation;
-    proj->Forward(ppk_raw_info.lat, ppk_raw_info.lng, ppk_raw_info.height,
-                   translation.x(),
-                   translation.y(),
-                   translation.z());
-    trajPoint.translation = translation;
-    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, currTraj->getProj());
-
-    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> &trajPaths)
-{
-    vector<ThreadPtr> thread_vec;
-    vector<boost::shared_ptr<Trajectory>> trajectories;
-    trajectories.resize(trajPaths.size());
-    for(uint8_t thread_index = 0; thread_index < trajPaths.size(); thread_index++){
-//        thread_vec.emplace_back(ThreadPtr(new boost::thread(boost::bind(&initTrajectory,
-//                                                                        iePaths[thread_index],
-//                                                                        trajectories[thread_index]))));
-        initTrajectory(trajPaths[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;
-}
-
 bool initTrajectory(
         const string &trajPath,
         boost::shared_ptr<Trajectory> &trajectory)
 {
     string trajFileName =
             boost::filesystem::path(trajPath).filename().string();
-    LOG(INFO) << "trajFileName: " << trajFileName;
+//    LOG(INFO) << "trajFileName: " << trajFileName;
     if(trajFileName == "ie.txt"){
         trajectory.reset(new Trajectory(trajPath, PPK));
     }else if(trajFileName == "sample.gps"){

apps/function/multi_traj_functions.h

@@ -18,7 +18,7 @@ vector<string> getCloseTasks(
         const string &parentPath,
         boost::shared_ptr<Trajectory> currTraj);
 
-void getCloseTasks(const vector<string> &allTask, vector<string> &crossTasks);
+void getCrossTasks(const vector<string> &allTask, vector<string> &crossTasks);
 
 bool getTaskInfo(
         const string &taskPath,
@@ -38,16 +38,6 @@ bool checkMeshOut(
 vector<string> crossFilter(
         const vector<TaskInfo> &filteredTaskInfo);
 
-bool getFstTraj(const string &trajPath,
-                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> &trajPaths);
-
 bool initTrajectory(const string &trajPath,
         boost::shared_ptr<Trajectory> &trajectory);
 

apps/multi_traj.cpp

@@ -30,33 +30,13 @@ int main(int argc, char *argv[])
         return 0;
     }
     vector<string> activeTraces = getActiveTraces(activeTracePath);
+    for(string task : activeTraces){
+        LOG(INFO) << "activeTraces: " << task;
+    }
     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;
-
-//    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;
-    getCloseTasks(activeTraces, crossTasks);
+    getCrossTasks(activeTraces, crossTasks);
     for(string task : crossTasks){
         LOG(INFO) << "cross task chekc in: " << task;
     }