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"?> <?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject> <!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> <qtcreator>
<data> <data>
<variable>EnvironmentId</variable> <variable>EnvironmentId</variable>
......
apps/function/multi_traj_functions.cpp
View file @ be1956b5
...@@ -97,45 +97,45 @@ bool getTaskInfo( ...@@ -97,45 +97,45 @@ 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"; // string validPath = taskPath + "/raw_trace/valid.txt";
std::ifstream ifs(validPath); // std::ifstream ifs(validPath);
if(ifs) { // if(ifs) {
string line; // string line;
Quaterniond q; // Quaterniond q;
getline(ifs, line); // getline(ifs, line);
{ // {
std::istringstream iss(line); // std::istringstream iss(line);
string subline; // string subline;
uint32_t start_week; // uint32_t start_week;
uint32_t start_sec; // uint32_t start_sec;
uint32_t end_week; // uint32_t end_week;
uint32_t end_sec; // uint32_t end_sec;
getline(iss, subline, ','); // getline(iss, subline, ',');
std::istringstream(subline) >> start_week; // std::istringstream(subline) >> start_week;
if(start_week > 1e9){ // if(start_week > 1e9){
taskInfo.startTime = start_week; // taskInfo.startTime = start_week;
uint32_t end_time; // uint32_t end_time;
getline(iss, subline, ','); // getline(iss, subline, ',');
std::istringstream(subline) >> end_time; // std::istringstream(subline) >> end_time;
taskInfo.endTime = end_time; // taskInfo.endTime = end_time;
}else{ // }else{
getline(iss, subline, ','); // getline(iss, subline, ',');
std::istringstream(subline) >> start_sec; // std::istringstream(subline) >> start_sec;
getline(iss, subline, ','); // getline(iss, subline, ',');
std::istringstream(subline) >> end_week; // std::istringstream(subline) >> end_week;
getline(iss, subline, ','); // getline(iss, subline, ',');
std::istringstream(subline) >>end_sec; // std::istringstream(subline) >>end_sec;
taskInfo.startTime = Trajectory::gpsToUtc(start_week, start_sec); // taskInfo.startTime = Trajectory::gpsToUtc(start_week, start_sec);
taskInfo.endTime = Trajectory::gpsToUtc(end_week, end_sec); // taskInfo.endTime = Trajectory::gpsToUtc(end_week, end_sec);
} // }
LOG(INFO) << setprecision(15) << taskPath << " valid startTime: " << taskInfo.startTime // LOG(INFO) << setprecision(15) << taskPath << " valid startTime: " << taskInfo.startTime
<< " endTime: " << taskInfo.endTime; // << " endTime: " << taskInfo.endTime;
} // }
}else{ // }else{
LOG(WARNING) << "dont exists: " << validPath; // LOG(WARNING) << "dont exists: " << validPath;
return false; // return false;
} // }
return true; return true;
} }
...@@ -446,14 +446,14 @@ vector<CloseTrajInfo> crossFilter( ...@@ -446,14 +446,14 @@ vector<CloseTrajInfo> crossFilter(
octree.setInputCloud(pointClouds.front()); octree.setInputCloud(pointClouds.front());
octree.addPointsFromInputCloud(); octree.addPointsFromInputCloud();
LOG(INFO) << "pointClouds[" << 0 << "]->size(): " << pointClouds[0]->size(); LOG(INFO) << "pointClouds[" << 0 << "]->size(): " << pointClouds[0]->size();
if(pointClouds[0]->size()){ // if(pointClouds[0]->size()){
pcl::io::savePCDFileBinary("/tmp/" + to_string(filteredTaskInfo[0].meshId) + ".pcd", *pointClouds[0]); // pcl::io::savePCDFileBinary("/tmp/" + to_string(filteredTaskInfo[0].meshId) + ".pcd", *pointClouds[0]);
} // }
for(size_t pcIndex = 1; pcIndex < pointClouds.size(); pcIndex++){ for(size_t pcIndex = 1; pcIndex < pointClouds.size(); pcIndex++){
LOG(INFO) << "pointClouds[" << pcIndex << "]->size(): " << pointClouds[0]->size(); LOG(INFO) << "pointClouds[" << pcIndex << "]->size(): " << pointClouds[0]->size();
if(pointClouds[0]->size()){ // if(pointClouds[0]->size()){
pcl::io::savePCDFileBinary("/tmp/" + to_string(filteredTaskInfo[pcIndex].meshId) + ".pcd", *pointClouds[pcIndex]); // pcl::io::savePCDFileBinary("/tmp/" + to_string(filteredTaskInfo[pcIndex].meshId) + ".pcd", *pointClouds[pcIndex]);
} // }
bool found = false; bool found = false;
bool inPeriod = false; bool inPeriod = false;
uint64_t start = 0, end = 0; 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) ...@@ -317,8 +317,7 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
return; return;
} }
PPK_Raw_Info ppk_raw_info; PPK_Raw_Info ppk_raw_info;
ppk_raw_info.gps_week = stoi(vec[1]); double timestamp = stod(vec[2]);
ppk_raw_info.gps_sec = stod(vec[2]);
ppk_raw_info.lat = stod(vec[3]); ppk_raw_info.lat = stod(vec[3]);
ppk_raw_info.lng = stod(vec[4]); ppk_raw_info.lng = stod(vec[4]);
ppk_raw_info.height = stod(vec[5]); ppk_raw_info.height = stod(vec[5]);
...@@ -351,13 +350,13 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index) ...@@ -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.pitch/180*M_PI, Eigen::Vector3d::UnitX())
* Eigen::AngleAxisd(ppk_raw_info.roll/180*M_PI, Eigen::Vector3d::UnitY()); * Eigen::AngleAxisd(ppk_raw_info.roll/180*M_PI, Eigen::Vector3d::UnitY());
trajPoint.nodeId = trajectory_.size(); trajPoint.nodeId = trajectory_.size();
trajPoint.timestamp = gpsToUtc(ppk_raw_info.gps_week, ppk_raw_info.gps_sec); trajPoint.timestamp = timestamp;
if(fabs(trajPoint.timestamp - 1675018189.7) < 0.01){ // if(fabs(trajPoint.timestamp - 1675018189.7) < 0.01){
LOG(INFO) << "1675018189.7: " << ppk_raw_info.yaw; // LOG(INFO) << "1675018189.7: " << ppk_raw_info.yaw;
} // }
if(fabs(trajPoint.timestamp - 1675021549.1) < 0.01){ // if(fabs(trajPoint.timestamp - 1675021549.1) < 0.01){
LOG(INFO) << "1675021549.1: " << ppk_raw_info.yaw; // LOG(INFO) << "1675021549.1: " << ppk_raw_info.yaw;
} // }
if(trajPoint.timestamp > startTime_ && trajPoint.timestamp < endTime_){ 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);
......
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