Skip to content

M
multiTraj
Commit 1419bf0d authored by limingbo's avatar limingbo
Browse files
Options

test mergemulti traj at one place

parent 8e28a540
Hide whitespace changes
Inline Side-by-side
Showing with 102 additions and 33 deletions
CMakeLists.txt.user
View file @ 1419bf0d
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 4.5.2, 2022-03-04T11:51:45. -->
<!-- Written by QtCreator 4.5.2, 2022-03-17T17:17:26. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
......
apps/function/multi_traj_functions.cpp
View file @ 1419bf0d
......@@ -79,7 +79,7 @@ bool getTaskInfo(
}
string trajPath = rawTraceDir + "/sample.gps";
if(!exists(trajPath)){
trajPath = rawTraceDir + "/ie.txt";;
trajPath = rawTraceDir + "/ie.txt";
}
if(!exists(trajPath)){
// LOG(INFO) << "dont exists: " << trajPath;
......@@ -172,49 +172,118 @@ vector<pair<string, vector<pair<uint64_t, uint64_t>>>> crossFilter(
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("/home/juefx/" + to_string(filteredTaskInfo[0].meshId) + ".pcd", *pointClouds[0]);
vector<pcl::octree::OctreePointCloudSearch<PointExport>> targetOctrees;
for(size_t pcIndex = 1; pcIndex < pointClouds.size(); pcIndex++){
pcl::io::savePCDFileBinary("/home/juefx/" + to_string(filteredTaskInfo[pcIndex].meshId) + ".pcd", *pointClouds[pcIndex]);
bool found = false;
bool inPeriod = false;
uint64_t start =0, end = 0;
pair<string, vector<pair<uint64_t, uint64_t>>> oneRet;
vector<pair<uint64_t, uint64_t>> timePeriods;
float currTime;
for(const PointExport& query : pointClouds[pcIndex]->points){
int result_index;
float sqr_distance;
pcl::octree::OctreePointCloudSearch<PointExport> octree(resolution);
octree.setInputCloud(pointClouds.at(pcIndex));
octree.addPointsFromInputCloud();
targetOctrees.emplace_back(octree);
}
map<uint32_t, pair<uint64_t, uint64_t>> targetPeriods;
for(const PointExport& query : pointClouds.front()->points){
int result_index;
float sqr_distance;
for(size_t targetIndex = 0; targetIndex < targetOctrees.size(); targetIndex++){
auto &octree = targetOctrees.at(targetIndex);
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;
auto iter = targetPeriods.find(targetIndex + 1); //first is query!plus 1!
if(iter == targetPeriods.end()){
uint64_t start = octree.getInputCloud()->at(result_index).info;
targetPeriods.insert(make_pair(targetIndex + 1, make_pair(start, 0)));
}
}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;
auto iter = targetPeriods.find(targetIndex + 1); //first is query!plus 1!
if(iter != targetPeriods.end()){
uint64_t end = octree.getInputCloud()->at(result_index).info;
iter->second.second = end;;
}
}
currTime = query.info;
}
if(inPeriod){
timePeriods.push_back(make_pair(start, currTime));
if(targetPeriods.size() == 0){
continue;
}
if(found){
oneRet = make_pair(filteredTaskInfo[pcIndex].taskPath,
timePeriods);
ret.push_back(oneRet);
bool away = true;
for(auto it = targetPeriods.begin(); it != targetPeriods.end(); it++){
away &= it->second.second != 0;
if(!away){
break;
}
}
if(away){
uint32_t longestId, longestPeriod = 0;
for(auto it = targetPeriods.begin(); it != targetPeriods.end(); it++){
uint32_t period = it->second.second - it->second.first;
if(period > longestPeriod){
longestId = it->first;
}
}
string currTaskPath = filteredTaskInfo[longestId].taskPath;
bool currTaskIn = false;
for(auto& oneRet : ret){
if(oneRet.first == currTaskPath){
currTaskIn = true;
oneRet.second.push_back(targetPeriods.find(longestId)->second);
}
}
if(!currTaskIn){
pair<string, vector<pair<uint64_t, uint64_t>>> oneRet;
vector<pair<uint64_t, uint64_t>> timePeriods;
timePeriods.push_back(targetPeriods.find(longestId)->second);
oneRet = make_pair(filteredTaskInfo[longestId].taskPath,
timePeriods);
ret.push_back(oneRet);
}
}
}
// pcl::octree::OctreePointCloudSearch<PointExport> octree(resolution);
// octree.setInputCloud(pointClouds.front());
// octree.addPointsFromInputCloud();
//// pcl::io::savePCDFileBinary("/home/juefx/" + to_string(filteredTaskInfo[0].meshId) + ".pcd", *pointClouds[0]);
// for(size_t pcIndex = 1; pcIndex < pointClouds.size(); pcIndex++){
//// pcl::io::savePCDFileBinary("/home/juefx/" + to_string(filteredTaskInfo[pcIndex].meshId) + ".pcd", *pointClouds[pcIndex]);
// bool found = false;
// bool inPeriod = false;
// uint64_t start =0, end = 0;
// pair<string, vector<pair<uint64_t, uint64_t>>> 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 = make_pair(filteredTaskInfo[pcIndex].taskPath,
// timePeriods);
// ret.push_back(oneRet);
// }
// }
return ret;
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment