Skip to content

M
multiTraj
Commit be1956b5 authored by wangdawei's avatar wangdawei
Browse files
Options

change ie format

parent 0642e304
Hide whitespace changes
Inline Side-by-side
Showing with 53 additions and 54 deletions
CMakeLists.txt.user
View file @ be1956b5
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 4.11.1, 2023-08-01T17:10:35. -->
<!-- Written by QtCreator 4.11.1, 2023-10-30T21:02:59. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
......
apps/function/multi_traj_functions.cpp
View file @ be1956b5
......@@ -97,45 +97,45 @@ 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;
if(start_week > 1e9){
taskInfo.startTime = start_week;
uint32_t end_time;
getline(iss, subline, ',');
std::istringstream(subline) >> end_time;
taskInfo.endTime = end_time;
// 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;
// if(start_week > 1e9){
// taskInfo.startTime = start_week;
// uint32_t end_time;
// getline(iss, subline, ',');
// std::istringstream(subline) >> end_time;
// taskInfo.endTime = end_time;
}else{
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);
}
LOG(INFO) << setprecision(15) << taskPath << " valid startTime: " << taskInfo.startTime
<< " endTime: " << taskInfo.endTime;
}
}else{
LOG(WARNING) << "dont exists: " << validPath;
return false;
}
// }else{
// 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);
// }
// LOG(INFO) << setprecision(15) << taskPath << " valid startTime: " << taskInfo.startTime
// << " endTime: " << taskInfo.endTime;
// }
// }else{
// LOG(WARNING) << "dont exists: " << validPath;
// return false;
// }
return true;
}
......@@ -446,14 +446,14 @@ vector<CloseTrajInfo> crossFilter(
octree.setInputCloud(pointClouds.front());
octree.addPointsFromInputCloud();
LOG(INFO) << "pointClouds[" << 0 << "]->size(): " << pointClouds[0]->size();
if(pointClouds[0]->size()){
pcl::io::savePCDFileBinary("/tmp/" + to_string(filteredTaskInfo[0].meshId) + ".pcd", *pointClouds[0]);
}
// if(pointClouds[0]->size()){
// pcl::io::savePCDFileBinary("/tmp/" + to_string(filteredTaskInfo[0].meshId) + ".pcd", *pointClouds[0]);
// }
for(size_t pcIndex = 1; pcIndex < pointClouds.size(); pcIndex++){
LOG(INFO) << "pointClouds[" << pcIndex << "]->size(): " << pointClouds[0]->size();
if(pointClouds[0]->size()){
pcl::io::savePCDFileBinary("/tmp/" + to_string(filteredTaskInfo[pcIndex].meshId) + ".pcd", *pointClouds[pcIndex]);
}
// if(pointClouds[0]->size()){
// pcl::io::savePCDFileBinary("/tmp/" + to_string(filteredTaskInfo[pcIndex].meshId) + ".pcd", *pointClouds[pcIndex]);
// }
bool found = false;
bool inPeriod = false;
uint64_t start = 0, end = 0;
......
libs/trajectory/trajectory.cpp
View file @ be1956b5
......@@ -317,8 +317,7 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
return;
}
PPK_Raw_Info ppk_raw_info;
ppk_raw_info.gps_week = stoi(vec[1]);
ppk_raw_info.gps_sec = stod(vec[2]);
double timestamp = stod(vec[2]);
ppk_raw_info.lat = stod(vec[3]);
ppk_raw_info.lng = stod(vec[4]);
ppk_raw_info.height = stod(vec[5]);
......@@ -351,13 +350,13 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
* Eigen::AngleAxisd(ppk_raw_info.pitch/180*M_PI, Eigen::Vector3d::UnitX())
* Eigen::AngleAxisd(ppk_raw_info.roll/180*M_PI, Eigen::Vector3d::UnitY());
trajPoint.nodeId = trajectory_.size();
trajPoint.timestamp = gpsToUtc(ppk_raw_info.gps_week, ppk_raw_info.gps_sec);
if(fabs(trajPoint.timestamp - 1675018189.7) < 0.01){
LOG(INFO) << "1675018189.7: " << ppk_raw_info.yaw;
}
if(fabs(trajPoint.timestamp - 1675021549.1) < 0.01){
LOG(INFO) << "1675021549.1: " << ppk_raw_info.yaw;
}
trajPoint.timestamp = timestamp;
// if(fabs(trajPoint.timestamp - 1675018189.7) < 0.01){
// LOG(INFO) << "1675018189.7: " << ppk_raw_info.yaw;
// }
// if(fabs(trajPoint.timestamp - 1675021549.1) < 0.01){
// LOG(INFO) << "1675021549.1: " << ppk_raw_info.yaw;
// }
if(trajPoint.timestamp > startTime_ && trajPoint.timestamp < endTime_){
if(multiTrajectory_[index].size() == 0){
multiTrajectory_[index].push_back(trajPoint);
......
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