#include "multi_traj_functions.h" #include <pcl/octree/octree.h> using namespace boost::filesystem; vector<string> getActiveTraces( const string &activeTracePath) { vector<string> ret; std::ifstream ifs(activeTracePath); if(!ifs){ LOG(WARNING) << activeTracePath << " load fail!"; return ret; } string line; while(getline(ifs, line)){ ret.push_back(line); } ifs.close(); return ret; } void getCrossTasks( const vector<string> &allTask, vector<CloseTrajInfo> &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(boost::filesystem::path(task).filename().string() == boost::filesystem::path(allTask.front()).filename().string()){ continue; } if(!getTaskInfo(task, taskInfo)){ continue; } allTaskInfo.push_back(taskInfo); } if(allTaskInfo.size() < 2){ return; } vector<TaskInfo> closeTaskInfo = allTaskInfo; if(allTaskInfo.front().meshOutPath != ""){ closeTaskInfo = meshOutFilter(allTaskInfo); } // closeTaskInfo.insert(closeTaskInfo.begin(), allTaskInfo.front()); if(closeTaskInfo.size() < 2){ LOG(INFO) << "closeTaskInfo.size() < 2"; closeTaskInfo = allTaskInfo; } for(size_t i = 1; i < closeTaskInfo.size(); i++){ TaskInfo task = closeTaskInfo[i]; LOG(INFO) << "closeTask: " << task.taskPath; } crossTasks = crossFilter(closeTaskInfo); } bool getTaskInfo( const string &taskPath, TaskInfo &taskInfo) { if (!is_directory(taskPath)){ // LOG(INFO) << "no directory: " << taskPath; return false; } string rawTraceDir = taskPath + "/raw_trace"; if(!is_directory(rawTraceDir)){ // LOG(INFO) << "no directory: " << rawTraceDir; return false; } string trajPath = rawTraceDir + "/sample.gps"; if(!exists(trajPath)){ trajPath = rawTraceDir + "/ie.txt"; } if(!exists(trajPath)){ // LOG(INFO) << "dont exists: " << trajPath; return false; } taskInfo.taskPath = taskPath; taskInfo.trajPath = trajPath; string meshOutPath = taskPath + "/meshes.out"; if(exists(meshOutPath)){ taskInfo.meshOutPath = meshOutPath; } taskInfo.meshId = stoi(boost::filesystem::path(taskPath).filename().string()); return true; } vector<TaskInfo> meshOutFilter( const vector<TaskInfo> &allTaskInfo){ vector<TaskInfo> ret; if(allTaskInfo.size() < 2){ return ret; } TaskInfo currTaskInfo = allTaskInfo.front(); ret.push_back(currTaskInfo); MeshOut currMeshOut = parseMeshOut(currTaskInfo.meshOutPath); if(currMeshOut.size() == 0){ LOG(INFO) << "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; } PointCloudExport getCloudFromPeriod( const PointCloudExport::Ptr cloud, pair<uint64_t, uint64_t> period){ PointCloudExport ret; for(const auto& p : *cloud){ if(p.info > period.first && p.info < period.second){ ret.push_back(p); } } return ret; } bool checkCurrPeriodMerged( const vector<pair<size_t, size_t>>& mergedIndexVec, size_t retIndex, size_t periodIndex){ for(auto mergedIndex : mergedIndexVec){ if(mergedIndex.first == retIndex && mergedIndex.second == periodIndex){ return true; } } return false; } void filterByPosition( vector<CloseTrajInfo> &ret, const vector<PointCloudExport::Ptr> &pointClouds){ vector<CloseTrajInfo> filteredRet; vector<pair<size_t, size_t>> mergedIndexVec; for(size_t retIndex = 0; retIndex < ret.size(); retIndex++){ const auto& oneRet = ret.at(retIndex); for(size_t periodIndex = 0; periodIndex < oneRet.periods.size(); periodIndex++){ if(checkCurrPeriodMerged(mergedIndexVec, retIndex, periodIndex)){ continue; } auto period = oneRet.periods.at(periodIndex); PointCloudExport periodCloud = getCloudFromPeriod(pointClouds.at(oneRet.index), period); float resolution = 5.f; pcl::octree::OctreePointCloudSearch<PointExport> octree(resolution); octree.setInputCloud(boost::make_shared<PointCloudExport>(periodCloud)); octree.addPointsFromInputCloud(); vector<pair<size_t, size_t>> aroundIndexVec; for(size_t tarRetIndex = retIndex + 1; tarRetIndex < ret.size(); tarRetIndex++){ const auto& tarOneRet = ret.at(tarRetIndex); for(size_t tarPeriodIndex = 0; tarPeriodIndex < tarOneRet.periods.size(); tarPeriodIndex++){ if(checkCurrPeriodMerged(mergedIndexVec, tarRetIndex, tarPeriodIndex)){ continue; } auto tarPeriod = tarOneRet.periods.at(tarPeriodIndex); PointCloudExport tarPeriodCloud = getCloudFromPeriod(pointClouds.at(tarOneRet.index), tarPeriod); for(const PointExport& query : tarPeriodCloud.points){ int result_index; float sqr_distance; octree.approxNearestSearch(query, result_index, sqr_distance); if(sqrt(sqr_distance) < 30){ aroundIndexVec.push_back(make_pair(tarRetIndex, tarPeriodIndex)); break; } } } } pair<size_t, size_t> longestIndex = make_pair(retIndex, periodIndex); uint32_t longestPeriod = period.second - period.first; for(auto index : aroundIndexVec){ auto aroundPeriod = ret.at(index.first).periods.at(index.second); uint32_t periodDuration = aroundPeriod.second - aroundPeriod.first; if(periodDuration > longestPeriod){ longestPeriod = periodDuration; longestIndex = index; } mergedIndexVec.push_back(index); } size_t index = longestIndex.first; bool findIndex = false; for(auto &filteredOneRet : filteredRet){ if(filteredOneRet.index == ret.at(index).index){ filteredOneRet.periods.push_back(ret.at(index).periods.at(longestIndex.second)); findIndex = true; break; } } if(!findIndex){ CloseTrajInfo closeTrajInfo; closeTrajInfo.index = ret.at(index).index; closeTrajInfo.periods.push_back(ret.at(index).periods.at(longestIndex.second)); closeTrajInfo.trajPath = ret.at(index).trajPath; filteredRet.push_back(closeTrajInfo); } } } filteredRet.swap(ret); } vector<CloseTrajInfo> crossFilter( const vector<TaskInfo> &filteredTaskInfo){ vector<CloseTrajInfo> ret; if(filteredTaskInfo.size() < 2){ return ret; } vector<boost::shared_ptr<Trajectory>> trajectories( filteredTaskInfo.size()); 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()); LOG(INFO) << "pointClouds.size(): " << pointClouds.size(); float resolution = 5.f; pcl::octree::OctreePointCloudSearch<PointExport> octree(resolution); octree.setInputCloud(pointClouds.front()); octree.addPointsFromInputCloud(); pcl::io::savePCDFileBinary("/tmp/" + to_string(filteredTaskInfo[0].meshId) + ".pcd", *pointClouds[0]); for(size_t pcIndex = 1; pcIndex < pointClouds.size(); pcIndex++){ pcl::io::savePCDFileBinary("/tmp/" + to_string(filteredTaskInfo[pcIndex].meshId) + ".pcd", *pointClouds[pcIndex]); bool found = false; bool inPeriod = false; uint64_t start = 0, end = 0; CloseTrajInfo oneRet; vector<pair<uint64_t, uint64_t>> timePeriods; float currTime; for(const PointExport& query : pointClouds[pcIndex]->points){ int result_index; float sqr_distance; octree.approxNearestSearch(query, result_index, sqr_distance); if(sqrt(sqr_distance) < 30){ found = true; if(!inPeriod){ start = query.info; inPeriod = true; // LOG(INFO) << setprecision(15) << "start: " << start; } }else if(sqrt(sqr_distance) > 40){ if(inPeriod){ end = query.info; inPeriod = false; timePeriods.push_back(make_pair(start, end)); // LOG(INFO) << setprecision(15) << "end: " << end; } } currTime = query.info; } if(inPeriod){ timePeriods.push_back(make_pair(start, currTime)); } if(found){ oneRet.index = pcIndex; oneRet.trajPath = filteredTaskInfo[pcIndex].taskPath; oneRet.periods = timePeriods; ret.push_back(oneRet); } } filterByPosition(ret, pointClouds); for(size_t retIndex = 0; retIndex < ret.size(); retIndex++){ const auto& oneRet = ret.at(retIndex); for(size_t periodIndex = 0; periodIndex < oneRet.periods.size(); periodIndex++){ PointCloudExport periodCloud = getCloudFromPeriod(pointClouds.at(oneRet.index), oneRet.periods.at(periodIndex)); if(periodCloud.size()){ pcl::io::savePCDFileBinary("/tmp/" + to_string(filteredTaskInfo[oneRet.index].meshId) + "_" + to_string(oneRet.periods.at(periodIndex).first) + ".pcd", periodCloud); } } } return ret; } bool initTrajectory( const string &trajPath, boost::shared_ptr<Trajectory> &trajectory) { string trajFileName = boost::filesystem::path(trajPath).filename().string(); // LOG(INFO) << "trajFileName: " << trajFileName; if(trajFileName == "ie.txt"){ trajectory.reset(new Trajectory(trajPath, PPK)); }else if(trajFileName == "sample.gps"){ trajectory.reset(new Trajectory(trajPath, PPK, 10)); }else{ return false; } trajectory->init(); return true; } vector<PointCloudExport::Ptr> multiThreadGetPointCloud( vector<boost::shared_ptr<Trajectory>> &trajectories, boost::shared_ptr<LocalCartesian> proj) { vector<ThreadPtr> thread_vec; vector<PointCloudExport::Ptr> pointClouds; pointClouds.resize(trajectories.size()); for(uint8_t thread_index = 0; thread_index < trajectories.size(); thread_index++){ pointClouds[thread_index].reset(new PointCloudExport); thread_vec.emplace_back(ThreadPtr(new boost::thread(boost::bind(&getPointCloud, proj, 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<LocalCartesian> proj, boost::shared_ptr<Trajectory> &trajectory, PointCloudExport::Ptr& pointCloud) { if(!trajectory){ return; } vector<TrajPoint> traj = trajectory->getTrajectory(); for(const TrajPoint& trajPoint : traj){ PointExport point; 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(); point.info = trajPoint.timestamp; pointCloud->push_back(point); } }