Skip to content

M
multiTraj
Commit 60333e15 authored by limingbo's avatar limingbo
Browse files
Options

test mergemulti traj at one place

parent 6c4443e3
Hide whitespace changes
Inline Side-by-side
Showing with 148 additions and 116 deletions
apps/function/multi_traj_functions.cpp
View file @ 60333e15
......@@ -22,7 +22,7 @@ vector<string> getActiveTraces(
void getCrossTasks(
const vector<string> &allTask,
vector<pair<string, vector<pair<uint64_t, uint64_t>>>> &crossTasks)
vector<CloseTrajInfo> &crossTasks)
{
if(allTask.size() < 2){
return;
......@@ -155,9 +155,107 @@ bool checkMeshOut(
return false;
}
vector<pair<string, vector<pair<uint64_t, uint64_t>>>> crossFilter(
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;
uint32_t longestPeriod = 0;
for(auto index : aroundIndexVec){
auto period = ret.at(index.first).periods.at(index.second);
uint32_t periodDuration = period.second - period.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 == index){
filteredOneRet.periods.push_back(ret.at(index).periods.at(longestIndex.second));
findIndex = true;
break;
}
}
if(!findIndex){
CloseTrajInfo closeTrajInfo;
closeTrajInfo.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<pair<string, vector<pair<uint64_t, uint64_t>>>> ret;
vector<CloseTrajInfo> ret;
if(filteredTaskInfo.size() < 2){
return ret;
}
......@@ -175,122 +273,50 @@ vector<pair<string, vector<pair<uint64_t, uint64_t>>>> crossFilter(
float resolution = 5.f;
vector<pcl::octree::OctreePointCloudSearch<PointExport>> targetOctrees;
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::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);
// 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;
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){
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)));
found = true;
if(!inPeriod){
start = query.info;
inPeriod = true;
// LOG(INFO) << setprecision(15) << "start: " << start;
}
}else if(sqrt(sqr_distance) > 40){
auto iter = targetPeriods.find(targetIndex + 1); //first is query!plus 1!
if(iter != targetPeriods.end()){
uint64_t end = octree.getInputCloud()->at(result_index).info;
if(end > iter->second.first){
iter->second.second = end;
}else{
iter->second.second = iter->second.first;
iter->second.first = end;
}
if(inPeriod){
end = query.info;
inPeriod = false;
timePeriods.push_back(make_pair(start, end));
// LOG(INFO) << setprecision(15) << "end: " << end;
}
}
currTime = query.info;
}
if(targetPeriods.size() == 0){
continue;
if(inPeriod){
timePeriods.push_back(make_pair(start, currTime));
}
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;
longestPeriod = period;
}
}
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);
}
targetPeriods.clear();
if(found){
oneRet.index = pcIndex;
oneRet.trajPath = filteredTaskInfo[pcIndex].taskPath;
oneRet.periods = 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);
// }
// }
filterByPosition(ret, pointClouds);
return ret;
}
......
apps/function/multi_traj_functions.h
View file @ 60333e15
......@@ -12,6 +12,12 @@ struct TaskInfo{
uint32_t meshId = 0;
};
struct CloseTrajInfo{
size_t index;
string trajPath;
vector<pair<uint64_t, uint64_t>> periods;
};
vector<string> getActiveTraces(
const string& activeTracePath);
......@@ -20,7 +26,7 @@ vector<string> getCloseTasks(
boost::shared_ptr<Trajectory> currTraj);
void getCrossTasks(const vector<string> &allTask,
vector<pair<string, vector<pair<uint64_t, uint64_t> > > > &crossTasks);
vector<CloseTrajInfo> &crossTasks);
bool getTaskInfo(
const string &taskPath,
......@@ -37,7 +43,7 @@ bool checkMeshOut(
const MeshOut& query,
const MeshOut& target);
vector<pair<string, vector<pair<uint64_t, uint64_t> > > > crossFilter(
vector<CloseTrajInfo> crossFilter(
const vector<TaskInfo> &filteredTaskInfo);
bool initTrajectory(const string &trajPath,
......
apps/multi_traj.cpp
View file @ 60333e15
......@@ -35,20 +35,20 @@ int main(int argc, char *argv[])
}
activeTraces.insert(activeTraces.begin(), FLAGS_base_dir);
vector<pair<string, vector<pair<uint64_t, uint64_t>>>> crossTasks;
vector<CloseTrajInfo> crossTasks;
getCrossTasks(activeTraces, crossTasks);
if(crossTasks.size() == 0){
LOG(WARNING) << "crossTasks.size() == 0";
}else{
sort(crossTasks.begin(), crossTasks.end(),
[](const auto& query, const auto& target) {
return stoi(boost::filesystem::path(query.first).filename().string()) <
stoi(boost::filesystem::path(target.first).filename().string());
return stoi(boost::filesystem::path(query.trajPath).filename().string()) <
stoi(boost::filesystem::path(target.trajPath).filename().string());
});
}
for(auto task : crossTasks){
LOG(INFO) << "cross task chekc in: " << task.first;
LOG(INFO) << "cross task chekc in: " << task.trajPath;
}
string outputPath = FLAGS_base_dir + "/multiTraj.txt";
......@@ -61,9 +61,9 @@ int main(int argc, char *argv[])
// ofs << crossTasks.back() << std::endl;
for(auto task : crossTasks){
uint32_t meshId =
stoi(boost::filesystem::path(task.first).filename().string());
stoi(boost::filesystem::path(task.trajPath).filename().string());
ofs << meshId;
for(auto period : task.second){
for(auto period : task.periods){
ofs << setprecision(15) << " " << period.first << " " << period.second - period.first;
}
......
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