Skip to content

M
multiTraj
Commit 046e5fb8 authored by limingbo's avatar limingbo
Browse files
Options

change filter by position stratgy and add startTime and endTime

parent b3ec79fe
Show whitespace changes
Inline Side-by-side
Showing with 217 additions and 72 deletions
CMakeLists.txt.user
View file @ 046e5fb8
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject> <!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 4.5.2, 2023-02-09T13:22:15. --> <!-- Written by QtCreator 4.5.2, 2023-02-09T16:55:42. -->
<qtcreator> <qtcreator>
<data> <data>
<variable>EnvironmentId</variable> <variable>EnvironmentId</variable>
......
apps/function/multi_traj_functions.cpp
View file @ 046e5fb8
#include "multi_traj_functions.h" #include "multi_traj_functions.h"
#include <pcl/octree/octree.h> #include <pcl/octree/octree.h>
using namespace boost::filesystem; using namespace boost::filesystem;
const float Squared30 = 30 * 30; const float Squared30 = 30 * 30;
const float Squared40 = 40 * 40;
vector<string> getActiveTraces( vector<string> getActiveTraces(
const string &activeTracePath) const string &activeTracePath)
...@@ -65,6 +66,7 @@ void getCrossTasks( ...@@ -65,6 +66,7 @@ void getCrossTasks(
crossTasks = crossFilter(closeTaskInfo); crossTasks = crossFilter(closeTaskInfo);
} }
bool getTaskInfo( bool getTaskInfo(
const string &taskPath, const string &taskPath,
TaskInfo &taskInfo) TaskInfo &taskInfo)
...@@ -94,6 +96,35 @@ bool getTaskInfo( ...@@ -94,6 +96,35 @@ bool getTaskInfo(
} }
taskInfo.meshId = taskInfo.meshId =
stoi(boost::filesystem::path(taskPath).filename().string()); stoi(boost::filesystem::path(taskPath).filename().string());
string validPath = taskPath + "/raw_trace/valid.txt";
std::ifstream ifs(validPath);
if(ifs) {
string line;
Quaterniond q;
getline(ifs, line);
{
std::istringstream iss(line);
string subline;
uint32_t start_week;
uint32_t start_sec;
uint32_t end_week;
uint32_t end_sec;
getline(iss, subline, ',');
std::istringstream(subline) >> start_week;
getline(iss, subline, ',');
std::istringstream(subline) >> start_sec;
getline(iss, subline, ',');
std::istringstream(subline) >> end_week;
getline(iss, subline, ',');
std::istringstream(subline) >>end_sec;
taskInfo.startTime = Trajectory::gpsToUtc(start_week, start_sec);
taskInfo.endTime = Trajectory::gpsToUtc(end_week, end_sec);
}
}else{
LOG(WARNING) << "dont exists: " << validPath;
return false;
}
return true; return true;
} }
...@@ -186,88 +217,160 @@ void filterByPosition( ...@@ -186,88 +217,160 @@ void filterByPosition(
vector<CloseTrajInfo> &ret, vector<CloseTrajInfo> &ret,
const vector<PointCloudExport::Ptr> &pointClouds){ const vector<PointCloudExport::Ptr> &pointClouds){
vector<CloseTrajInfo> filteredRet; vector<CloseTrajInfo> filteredRet;
vector<pair<size_t, size_t>> mergedIndexVec; float resolution = 5.f;
pcl::octree::OctreePointCloudSearch<PointExport> octree(resolution);
PointCloudExport::Ptr totalPeriodCloud(new PointCloudExport);
octree.setInputCloud(totalPeriodCloud);
for(size_t retIndex = 0; retIndex < ret.size(); retIndex++){ for(size_t retIndex = 0; retIndex < ret.size(); retIndex++){
const auto& oneRet = ret.at(retIndex); const auto& oneRet = ret.at(retIndex);
if(0 == retIndex){
filteredRet.push_back(oneRet);
for(size_t periodIndex = 0; periodIndex < oneRet.periods.size(); periodIndex++){ for(size_t periodIndex = 0; periodIndex < oneRet.periods.size(); periodIndex++){
if(checkCurrPeriodMerged(mergedIndexVec, retIndex, periodIndex)){
continue;
}
auto period = oneRet.periods.at(periodIndex); auto period = oneRet.periods.at(periodIndex);
PointCloudExport periodCloud = PointCloudExport periodCloud =
getCloudFromPeriod(pointClouds.at(oneRet.index), period); getCloudFromPeriod(pointClouds.at(oneRet.index), period);
// LOG(INFO) << "periodCloud.size(): " << periodCloud.size();
if(0 == periodCloud.size()){ if(0 == periodCloud.size()){
continue; continue;
} }
float resolution = 5.f; for(auto p : periodCloud){
pcl::octree::OctreePointCloudSearch<PointExport> octree(resolution); octree.addPointToCloud(p, totalPeriodCloud);
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; continue;
} }
auto tarPeriod = tarOneRet.periods.at(tarPeriodIndex); vector<pair<uint64_t, uint64_t>> timePeriods;
PointCloudExport tarPeriodCloud = for(size_t periodIndex = 0; periodIndex < oneRet.periods.size(); periodIndex++){
getCloudFromPeriod(pointClouds.at(tarOneRet.index), tarPeriod); auto period = oneRet.periods.at(periodIndex);
if(0 == tarPeriodCloud.size()){ PointCloudExport periodCloud =
getCloudFromPeriod(pointClouds.at(oneRet.index), period);
if(0 == periodCloud.size()){
continue; continue;
} }
uint32_t hitCnt = 0; bool inPeriod = false;
for(const PointExport& query : tarPeriodCloud.points){ uint64_t start = 0, end = 0;
vector<PointExport> filterdPeriod;
for(const PointExport& query : periodCloud.points){
int result_index; int result_index;
float sqr_distance; float sqr_distance;
octree.approxNearestSearch(query, result_index, sqr_distance); octree.approxNearestSearch(query, result_index, sqr_distance);
if(inPeriod){
if(sqr_distance < Squared30){ if(sqr_distance < Squared30){
hitCnt++; end = query.info;
} inPeriod = false;
} if(end - start > 1){
if((float)hitCnt / tarPeriodCloud.size() > 0.8){ timePeriods.push_back(make_pair(start, end));
aroundIndexVec.push_back(make_pair(tarRetIndex, tarPeriodIndex)); for(auto p : filterdPeriod){
} octree.addPointToCloud(p, totalPeriodCloud);
} }
} }
pair<size_t, size_t> longestIndex = make_pair(retIndex, periodIndex); vector<PointExport>().swap(filterdPeriod);
uint32_t longestPeriod = period.second - period.first; }else{
LOG(INFO) << setprecision(15) << "filterByPosition src: " << oneRet.index filterdPeriod.push_back(query);
<< " start: " << period.first << " period: " << longestPeriod;
for(auto index : aroundIndexVec){
auto aroundPeriod = ret.at(index.first).periods.at(index.second);
uint32_t periodDuration = aroundPeriod.second - aroundPeriod.first;
LOG(INFO) << setprecision(15) << "tar: " << ret.at(index.first).index
<< " start: " << aroundPeriod.first << " period: " << periodDuration;
if(periodDuration > longestPeriod){
longestPeriod = periodDuration;
longestIndex = index;
} }
mergedIndexVec.push_back(index); }else{
if(sqr_distance > Squared30){
start = query.info;
inPeriod = true;
filterdPeriod.push_back(query);
} }
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);
} }
if(timePeriods.size()){
auto finalOneRet = oneRet;
finalOneRet.periods = timePeriods;
filteredRet.push_back(finalOneRet);
} }
} }
filteredRet.swap(ret); filteredRet.swap(ret);
} }
//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);
//// LOG(INFO) << "periodCloud.size(): " << periodCloud.size();
// if(0 == periodCloud.size()){
// continue;
// }
// 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);
// if(0 == tarPeriodCloud.size()){
// continue;
// }
// uint32_t hitCnt = 0;
// for(const PointExport& query : tarPeriodCloud.points){
// int result_index;
// float sqr_distance;
// octree.approxNearestSearch(query, result_index, sqr_distance);
// if(sqr_distance < Squared30){
// hitCnt++;
// }
// }
// if((float)hitCnt / tarPeriodCloud.size() > 0.8){
// aroundIndexVec.push_back(make_pair(tarRetIndex, tarPeriodIndex));
// }
// }
// }
// pair<size_t, size_t> longestIndex = make_pair(retIndex, periodIndex);
// uint32_t longestPeriod = period.second - period.first;
// LOG(INFO) << setprecision(15) << "filterByPosition src: " << oneRet.index
// << " start: " << period.first << " period: " << longestPeriod;
// for(auto index : aroundIndexVec){
// auto aroundPeriod = ret.at(index.first).periods.at(index.second);
// uint32_t periodDuration = aroundPeriod.second - aroundPeriod.first;
// LOG(INFO) << setprecision(15) << "tar: " << ret.at(index.first).index
// << " start: " << aroundPeriod.first << " period: " << periodDuration;
// 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( vector<CloseTrajInfo> crossFilter(
const vector<TaskInfo> &filteredTaskInfo){ const vector<TaskInfo> &filteredTaskInfo){
vector<CloseTrajInfo> ret; vector<CloseTrajInfo> ret;
...@@ -278,7 +381,7 @@ vector<CloseTrajInfo> crossFilter( ...@@ -278,7 +381,7 @@ vector<CloseTrajInfo> crossFilter(
vector<boost::shared_ptr<Trajectory>> trajectories( vector<boost::shared_ptr<Trajectory>> trajectories(
filteredTaskInfo.size()); filteredTaskInfo.size());
for(size_t i = 0; i < filteredTaskInfo.size(); i++){ for(size_t i = 0; i < filteredTaskInfo.size(); i++){
initTrajectory(filteredTaskInfo[i].trajPath, trajectories[i]); initTrajectory(filteredTaskInfo[i], trajectories[i]);
} }
boost::shared_ptr<Trajectory> currTrajectory = boost::shared_ptr<Trajectory> currTrajectory =
trajectories.front(); trajectories.front();
...@@ -299,26 +402,58 @@ vector<CloseTrajInfo> crossFilter( ...@@ -299,26 +402,58 @@ vector<CloseTrajInfo> crossFilter(
uint64_t start = 0, end = 0; uint64_t start = 0, end = 0;
CloseTrajInfo oneRet; CloseTrajInfo oneRet;
vector<pair<uint64_t, uint64_t>> timePeriods; vector<pair<uint64_t, uint64_t>> timePeriods;
pcl::octree::OctreePointCloudSearch<PointExport> taskOctree(resolution);
PointCloudExport::Ptr taskPeriodCloud(new PointCloudExport);
taskOctree.setInputCloud(taskPeriodCloud);
float currTime; float currTime;
vector<PointExport> filterdPeriod;
for(const PointExport& query : pointClouds[pcIndex]->points){ for(const PointExport& query : pointClouds[pcIndex]->points){
int result_index; int result_index;
float sqr_distance; float sqr_distance;
octree.approxNearestSearch(query, result_index, sqr_distance); octree.approxNearestSearch(query, result_index, sqr_distance);
if(sqrt(sqr_distance) < 30){ float self_sqr_distance = -1;
found = true; if(taskPeriodCloud->size()){
if(!inPeriod){ taskOctree.approxNearestSearch(query, result_index, self_sqr_distance);
start = query.info;
inPeriod = true;
// LOG(INFO) << setprecision(15) << "start: " << start;
} }
}else if(sqrt(sqr_distance) > 40){
if(inPeriod){ if(inPeriod){
if(sqr_distance > Squared40 ||
(self_sqr_distance >= 0 && self_sqr_distance < Squared30)){
end = query.info; end = query.info;
inPeriod = false; inPeriod = false;
timePeriods.push_back(make_pair(start, end)); timePeriods.push_back(make_pair(start, end));
// LOG(INFO) << setprecision(15) << "end: " << end; for(auto p : filterdPeriod){
taskOctree.addPointToCloud(p, taskPeriodCloud);
}
vector<PointExport>().swap(filterdPeriod);
}else{
filterdPeriod.push_back(query);
}
}else{
if(sqr_distance < Squared30 &&
(-1 == self_sqr_distance || self_sqr_distance >= Squared30)){
start = query.info;
inPeriod = true;
found = true;
filterdPeriod.push_back(query);
} }
} }
// if(sqr_distance < Squared30){
// found = true;
// if(!inPeriod){
// start = query.info;
// inPeriod = true;
//// LOG(INFO) << setprecision(15) << "start: " << start;
// }
// }else if(sqr_distance > Squared40){
// if(inPeriod){
// end = query.info;
// inPeriod = false;
// timePeriods.push_back(make_pair(start, end));
//// LOG(INFO) << setprecision(15) << "end: " << end;
// }
// }
currTime = query.info; currTime = query.info;
} }
if(inPeriod){ if(inPeriod){
...@@ -349,9 +484,10 @@ vector<CloseTrajInfo> crossFilter( ...@@ -349,9 +484,10 @@ vector<CloseTrajInfo> crossFilter(
} }
bool initTrajectory( bool initTrajectory(
const string &trajPath, const TaskInfo &taskInfo,
boost::shared_ptr<Trajectory> &trajectory) boost::shared_ptr<Trajectory> &trajectory)
{ {
auto trajPath = taskInfo.trajPath;
string trajFileName = string trajFileName =
boost::filesystem::path(trajPath).filename().string(); boost::filesystem::path(trajPath).filename().string();
// LOG(INFO) << "trajFileName: " << trajFileName; // LOG(INFO) << "trajFileName: " << trajFileName;
...@@ -362,7 +498,7 @@ bool initTrajectory( ...@@ -362,7 +498,7 @@ bool initTrajectory(
}else{ }else{
return false; return false;
} }
trajectory->init(); trajectory->init(taskInfo.startTime, taskInfo.endTime);
return true; return true;
} }
......
apps/function/multi_traj_functions.h
View file @ 046e5fb8
...@@ -10,6 +10,8 @@ struct TaskInfo{ ...@@ -10,6 +10,8 @@ struct TaskInfo{
string trajPath = ""; string trajPath = "";
string meshOutPath = ""; string meshOutPath = "";
uint32_t meshId = 0; uint32_t meshId = 0;
double startTime = 0;
double endTime = 0;
}; };
struct CloseTrajInfo{ struct CloseTrajInfo{
...@@ -46,7 +48,7 @@ bool checkMeshOut( ...@@ -46,7 +48,7 @@ bool checkMeshOut(
vector<CloseTrajInfo> crossFilter( vector<CloseTrajInfo> crossFilter(
const vector<TaskInfo> &filteredTaskInfo); const vector<TaskInfo> &filteredTaskInfo);
bool initTrajectory(const string &trajPath, bool initTrajectory(const TaskInfo &taskInfo,
boost::shared_ptr<Trajectory> &trajectory); boost::shared_ptr<Trajectory> &trajectory);
vector<PointCloudExport::Ptr> multiThreadGetPointCloud( vector<PointCloudExport::Ptr> multiThreadGetPointCloud(
......
libs/trajectory/trajectory.cpp
View file @ 046e5fb8
...@@ -21,8 +21,10 @@ Trajectory::~Trajectory() ...@@ -21,8 +21,10 @@ Trajectory::~Trajectory()
if(io_) delete io_; if(io_) delete io_;
} }
bool Trajectory::init() bool Trajectory::init(double startTime, double endTime)
{ {
startTime_ = startTime;
endTime_ = endTime;
string range; string range;
switch(trajType_){ switch(trajType_){
case PPK: case PPK:
...@@ -303,9 +305,6 @@ bool Trajectory::checkSatisfied(const int32_t &supposed_floor_index, const doubl ...@@ -303,9 +305,6 @@ bool Trajectory::checkSatisfied(const int32_t &supposed_floor_index, const doubl
} }
} }
double gpsToUtc(const uint32_t &gps_week, const double &gps_sec) {
return (315936000 + gps_week*7*24*3600 + gps_sec - 18);
}
void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index) void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
{ {
...@@ -350,6 +349,7 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index) ...@@ -350,6 +349,7 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
trajPoint.rotation = EulerToMatrix3d(Vector3d(ppk_raw_info.pitch, ppk_raw_info.roll, ppk_raw_info.yaw)); trajPoint.rotation = EulerToMatrix3d(Vector3d(ppk_raw_info.pitch, ppk_raw_info.roll, ppk_raw_info.yaw));
trajPoint.nodeId = trajectory_.size(); trajPoint.nodeId = trajectory_.size();
trajPoint.timestamp = gpsToUtc(ppk_raw_info.gps_week, ppk_raw_info.gps_sec); trajPoint.timestamp = gpsToUtc(ppk_raw_info.gps_week, ppk_raw_info.gps_sec);
if(trajPoint.timestamp > startTime_ && trajPoint.timestamp < endTime_){
if(multiTrajectory_[index].size() == 0){ if(multiTrajectory_[index].size() == 0){
multiTrajectory_[index].push_back(trajPoint); multiTrajectory_[index].push_back(trajPoint);
}else{ }else{
...@@ -357,6 +357,7 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index) ...@@ -357,6 +357,7 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
multiTrajectory_[index].push_back(trajPoint); multiTrajectory_[index].push_back(trajPoint);
} }
} }
}
break; break;
} }
......
libs/trajectory/trajectory.h
View file @ 046e5fb8
...@@ -16,7 +16,7 @@ public: ...@@ -16,7 +16,7 @@ public:
~Trajectory(); ~Trajectory();
bool init(); bool init(double startTime, double endTime);
string getTrajPath() const; string getTrajPath() const;
...@@ -47,6 +47,10 @@ public: ...@@ -47,6 +47,10 @@ public:
Vector3d reverse(const TrajPoint& trajPoint); Vector3d reverse(const TrajPoint& trajPoint);
double getFrequency(); double getFrequency();
static double gpsToUtc(const uint32_t &gps_week, const double &gps_sec) {
return (315936000 + gps_week*7*24*3600 + gps_sec - 18);
}
private: private:
bool checkSatisfied(const int32_t &supposed_floor_index, const double &timestamp) const; bool checkSatisfied(const int32_t &supposed_floor_index, const double &timestamp) const;
...@@ -67,6 +71,8 @@ private: ...@@ -67,6 +71,8 @@ private:
boost::shared_ptr<LocalCartesian> proj_; boost::shared_ptr<LocalCartesian> proj_;
boost::shared_mutex Mtx_; boost::shared_mutex Mtx_;
double startTime_;
double endTime_;
}; };
#endif // TRAJECTORY_H #endif // TRAJECTORY_H
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