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"?>
<!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>
<data>
<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>
using namespace boost::filesystem;
const float Squared30 = 30 * 30;
const float Squared40 = 40 * 40;
vector<string> getActiveTraces(
const string &activeTracePath)
......@@ -65,6 +66,7 @@ void getCrossTasks(
crossTasks = crossFilter(closeTaskInfo);
}
bool getTaskInfo(
const string &taskPath,
TaskInfo &taskInfo)
......@@ -94,6 +96,35 @@ bool getTaskInfo(
}
taskInfo.meshId =
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;
}
......@@ -186,88 +217,160 @@ void filterByPosition(
vector<CloseTrajInfo> &ret,
const vector<PointCloudExport::Ptr> &pointClouds){
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++){
const auto& oneRet = ret.at(retIndex);
if(0 == retIndex){
filteredRet.push_back(oneRet);
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)){
for(auto p : periodCloud){
octree.addPointToCloud(p, totalPeriodCloud);
}
}
continue;
}
auto tarPeriod = tarOneRet.periods.at(tarPeriodIndex);
PointCloudExport tarPeriodCloud =
getCloudFromPeriod(pointClouds.at(tarOneRet.index), tarPeriod);
if(0 == tarPeriodCloud.size()){
vector<pair<uint64_t, uint64_t>> timePeriods;
for(size_t periodIndex = 0; periodIndex < oneRet.periods.size(); periodIndex++){
auto period = oneRet.periods.at(periodIndex);
PointCloudExport periodCloud =
getCloudFromPeriod(pointClouds.at(oneRet.index), period);
if(0 == periodCloud.size()){
continue;
}
uint32_t hitCnt = 0;
for(const PointExport& query : tarPeriodCloud.points){
bool inPeriod = false;
uint64_t start = 0, end = 0;
vector<PointExport> filterdPeriod;
for(const PointExport& query : periodCloud.points){
int result_index;
float sqr_distance;
octree.approxNearestSearch(query, result_index, sqr_distance);
if(inPeriod){
if(sqr_distance < Squared30){
hitCnt++;
}
}
if((float)hitCnt / tarPeriodCloud.size() > 0.8){
aroundIndexVec.push_back(make_pair(tarRetIndex, tarPeriodIndex));
}
end = query.info;
inPeriod = false;
if(end - start > 1){
timePeriods.push_back(make_pair(start, end));
for(auto p : filterdPeriod){
octree.addPointToCloud(p, totalPeriodCloud);
}
}
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;
vector<PointExport>().swap(filterdPeriod);
}else{
filterdPeriod.push_back(query);
}
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);
}
//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(
const vector<TaskInfo> &filteredTaskInfo){
vector<CloseTrajInfo> ret;
......@@ -278,7 +381,7 @@ vector<CloseTrajInfo> crossFilter(
vector<boost::shared_ptr<Trajectory>> trajectories(
filteredTaskInfo.size());
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 =
trajectories.front();
......@@ -299,26 +402,58 @@ vector<CloseTrajInfo> crossFilter(
uint64_t start = 0, end = 0;
CloseTrajInfo oneRet;
vector<pair<uint64_t, uint64_t>> timePeriods;
pcl::octree::OctreePointCloudSearch<PointExport> taskOctree(resolution);
PointCloudExport::Ptr taskPeriodCloud(new PointCloudExport);
taskOctree.setInputCloud(taskPeriodCloud);
float currTime;
vector<PointExport> filterdPeriod;
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;
float self_sqr_distance = -1;
if(taskPeriodCloud->size()){
taskOctree.approxNearestSearch(query, result_index, self_sqr_distance);
}
}else if(sqrt(sqr_distance) > 40){
if(inPeriod){
if(sqr_distance > Squared40 ||
(self_sqr_distance >= 0 && self_sqr_distance < Squared30)){
end = query.info;
inPeriod = false;
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;
}
if(inPeriod){
......@@ -349,9 +484,10 @@ vector<CloseTrajInfo> crossFilter(
}
bool initTrajectory(
const string &trajPath,
const TaskInfo &taskInfo,
boost::shared_ptr<Trajectory> &trajectory)
{
auto trajPath = taskInfo.trajPath;
string trajFileName =
boost::filesystem::path(trajPath).filename().string();
// LOG(INFO) << "trajFileName: " << trajFileName;
......@@ -362,7 +498,7 @@ bool initTrajectory(
}else{
return false;
}
trajectory->init();
trajectory->init(taskInfo.startTime, taskInfo.endTime);
return true;
}
......
apps/function/multi_traj_functions.h
View file @ 046e5fb8
......@@ -10,6 +10,8 @@ struct TaskInfo{
string trajPath = "";
string meshOutPath = "";
uint32_t meshId = 0;
double startTime = 0;
double endTime = 0;
};
struct CloseTrajInfo{
......@@ -46,7 +48,7 @@ bool checkMeshOut(
vector<CloseTrajInfo> crossFilter(
const vector<TaskInfo> &filteredTaskInfo);
bool initTrajectory(const string &trajPath,
bool initTrajectory(const TaskInfo &taskInfo,
boost::shared_ptr<Trajectory> &trajectory);
vector<PointCloudExport::Ptr> multiThreadGetPointCloud(
......
libs/trajectory/trajectory.cpp
View file @ 046e5fb8
......@@ -21,8 +21,10 @@ Trajectory::~Trajectory()
if(io_) delete io_;
}
bool Trajectory::init()
bool Trajectory::init(double startTime, double endTime)
{
startTime_ = startTime;
endTime_ = endTime;
string range;
switch(trajType_){
case PPK:
......@@ -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)
{
......@@ -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.nodeId = trajectory_.size();
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){
multiTrajectory_[index].push_back(trajPoint);
}else{
......@@ -357,6 +357,7 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
multiTrajectory_[index].push_back(trajPoint);
}
}
}
break;
}
......
libs/trajectory/trajectory.h
View file @ 046e5fb8
......@@ -16,7 +16,7 @@ public:
~Trajectory();
bool init();
bool init(double startTime, double endTime);
string getTrajPath() const;
......@@ -47,6 +47,10 @@ public:
Vector3d reverse(const TrajPoint& trajPoint);
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:
bool checkSatisfied(const int32_t &supposed_floor_index, const double &timestamp) const;
......@@ -67,6 +71,8 @@ private:
boost::shared_ptr<LocalCartesian> proj_;
boost::shared_mutex Mtx_;
double startTime_;
double endTime_;
};
#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