Skip to content

Y
yiqi-frame-mapping
Commit 0bf6b9f9 authored by xiebojie's avatar xiebojie
Browse files
Options

merge files from pcl-1 online code

parent 01ea89a7
Hide whitespace changes
Inline Side-by-side
Showing with 1152 additions and 1142 deletions
libs.zip 0 → 100644
View file @ 0bf6b9f9
File added
libs/lidar/pcl_point_type.h
View file @ 0bf6b9f9
......@@ -3,26 +3,27 @@
#ifndef PCL_POINT_TYPE_H
#define PCL_POINT_TYPE_H
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
namespace pcl {
struct PointInternal {
PCL_ADD_POINT4D
PCL_ADD_RGB
float intensity;
float ring;
float label = 0;
float distance;
double timestamp;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
PCL_ADD_POINT4D
PCL_ADD_RGB
float intensity;
float ring;
float label = 0;
float distance;
double timestamp;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN16;
}//end namespace pcl
} // end namespace pcl
POINT_CLOUD_REGISTER_POINT_STRUCT(
pcl::PointInternal, (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)
(float, intensity, intensity)(float, ring, ring)(float, label, label)
(float, distance, distance)(double, timestamp, timestamp))
pcl::PointInternal,
(float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)(
float, intensity, intensity)(float, ring, ring)(float, label, label)(
float, distance, distance)(double, timestamp, timestamp))
typedef pcl::PointInternal Point;
typedef pcl::PointCloud<Point> PointCloud;
......@@ -32,19 +33,19 @@ typedef pcl::PointCloud<Point> PointCloud;
///
namespace pcl {
struct PointRos {
PCL_ADD_POINT4D
float intensity;
float ring;
float label = 0;
float timestamp;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
PCL_ADD_POINT4D
float intensity;
float ring;
float label = 0;
float timestamp;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN32;
}//end namespace pcl
} // end namespace pcl
POINT_CLOUD_REGISTER_POINT_STRUCT(
pcl::PointRos, (float, x, x)(float, y, y)(float, z, z)
(float, intensity, intensity)(float, ring, ring)(float, label, label)
(float, timestamp, timestamp))
pcl::PointRos,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(
float, ring, ring)(float, label, label)(float, timestamp, timestamp))
typedef pcl::PointRos PointRos;
typedef pcl::PointCloud<PointRos> PointCloudRos;
......@@ -54,18 +55,19 @@ typedef pcl::PointCloud<PointRos> PointCloudRos;
///
namespace pcl {
struct PointExport {
PCL_ADD_POINT4D
PCL_ADD_RGB
float intensity = 0;
float label = 0;
float info = 0;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
PCL_ADD_POINT4D
PCL_ADD_RGB
float intensity = 0;
float label = 0;
float info = 0;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN16;
}//end namespace pcl
} // end namespace pcl
POINT_CLOUD_REGISTER_POINT_STRUCT(
pcl::PointExport, (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)
(float, intensity, intensity)(float, label, label)(float, info, info))
pcl::PointExport,
(float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)(
float, intensity, intensity)(float, label, label)(float, info, info))
typedef pcl::PointExport PointExport;
typedef pcl::PointCloud<PointExport> PointCloudExport;
......@@ -73,21 +75,39 @@ typedef pcl::PointCloud<PointExport> PointCloudExport;
/////////////////////////////////////////////////////////////
namespace pcl {
struct PointTemp1 {
PCL_ADD_POINT4D
float intensity;
float ring;
float label = 0;
float time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
PCL_ADD_POINT4D
float intensity;
float ring;
float label = 0;
float time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN16;
}//end namespace pcl
} // end namespace pcl
POINT_CLOUD_REGISTER_POINT_STRUCT(
pcl::PointTemp1, (float, x, x)(float, y, y)(float, z, z)
(float, intensity, intensity)(float, ring, ring)(float, label, label)
(float, time, time))
pcl::PointTemp1,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(
float, ring, ring)(float, label, label)(float, time, time))
typedef pcl::PointTemp1 PointTemp1;
/////////////////////////////////////////////////////////////
namespace pcl {
struct PointI {
PCL_ADD_POINT4D
float intensity;
float stamp_offset;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN16;
} // end namespace pcl
POINT_CLOUD_REGISTER_POINT_STRUCT(
pcl::PointI,
(float, x, x)(float, y, y)(float, z, z)(float, intensity,
intensity)(float, stamp_offset,
stamp_offset))
typedef pcl::PointI PointI;
typedef pcl::PointCloud<PointI> PointCloudI;
#endif // PCL_POINT_TYPE_H
#endif // PCL_POINT_TYPE_H
libs/reader/pcd_reader.cpp
View file @ 0bf6b9f9
......@@ -49,7 +49,8 @@ bool PcdReader::loadFile() {
bool PcdReader::getNextFrame(PointCloud*& frame) {
if (pcdIndex_ >= pcdPathVec_.size()) return false;
bool suceess = false;
PointCloud tempFrame;
PointCloudI tempFrame;
double time;
while (!suceess) {
string pcdPath = pcdPathVec_.at(pcdIndex_);
pcdIndex_++;
......@@ -60,17 +61,25 @@ bool PcdReader::getNextFrame(PointCloud*& frame) {
if (iter == frameTimeMap_.end()) {
continue;
}
double time = iter->second;
time = iter->second;
int8_t ret = checkTime(time, durations_);
if (ret != 1) {
continue;
}
for (auto& point : tempFrame) {
point.timestamp = time;
}
// LOG(INFO) << setprecision(15) << "frameId: " << frameId
// << " time: " << time;
suceess = true;
}
frame = new PointCloud;
*frame = tempFrame;
for (auto& pointI : tempFrame) {
Point point;
point.x = pointI.x;
point.y = pointI.y;
point.z = pointI.z;
point.timestamp = time + pointI.stamp_offset * 0.001;
point.intensity = pointI.intensity;
point.distance = sqrt(pow(point.x, 2) + pow(point.y, 2) + pow(point.z, 2));
frame->push_back(point);
}
return true;
}
libs/task/maptask.cpp
View file @ 0bf6b9f9
......@@ -5,778 +5,767 @@
MapTask::MapTask(const vector<vector<Time_Duration>> &timeDurations,
boost::shared_ptr<StrategyControl> strategyControl,
boost::shared_ptr<ParamsServer> paramsServer)
:timeDurations_(timeDurations),
: timeDurations_(timeDurations),
strategyControl_(strategyControl),
paramsServer_(paramsServer)
{
voxeled_pointcloud_.reset(new PointCloud);
export_cloud_.reset(new PointCloudExport);
filtered_cloud_.reset(new PointCloudExport);
uint16_t cal_miss_cnt = strategyControl_->getStageInfos().size() - 1;
dynamicRemover_.reset(new DynamicRemover(
0.06, MISS_PER_HIT_PER_STAGE * cal_miss_cnt, voxeled_pointcloud_));
for(const auto &durationvec : timeDurations_){
for(const auto &duration : durationvec){
LOG(INFO) <<setprecision(15) << "MapTask "
<< " duration.start_sec: " << duration.start_sec
<<" duration.end_sec: " << duration.end_sec;
}
}
configTrajectory();
paramsServer_(paramsServer) {
voxeled_pointcloud_.reset(new PointCloud);
export_cloud_.reset(new PointCloudExport);
filtered_cloud_.reset(new PointCloudExport);
uint16_t cal_miss_cnt = strategyControl_->getStageInfos().size() - 1;
dynamicRemover_.reset(new DynamicRemover(
0.06, MISS_PER_HIT_PER_STAGE * cal_miss_cnt, voxeled_pointcloud_));
for (const auto &durationvec : timeDurations_) {
for (const auto &duration : durationvec) {
LOG(INFO) << setprecision(15) << "MapTask "
<< " duration.start_sec: " << duration.start_sec
<< " duration.end_sec: " << duration.end_sec;
}
}
configTrajectory();
}
void MapTask::configTrajectory()
{
auto traj_mode = strategyControl_->getTrajMode();
LOG(INFO) << "traj_mode: " << traj_mode;
if(traj_mode == PPK_UNDISORT_NODE_MAP_MODE){
undistortion_traj_ = paramsServer_->getPPKTraj();
mapping_traj_ = paramsServer_->getNodeTraj();
}else if(traj_mode == NODE_MODE){
undistortion_traj_ = paramsServer_->getNodeTraj();
mapping_traj_ = paramsServer_->getNodeTraj();
}else if(traj_mode == PPK_MODE){
undistortion_traj_ = paramsServer_->getPPKTraj();
mapping_traj_ = paramsServer_->getPPKTraj();
}
void MapTask::configTrajectory() {
auto traj_mode = strategyControl_->getTrajMode();
LOG(INFO) << "traj_mode: " << traj_mode;
if (traj_mode == PPK_UNDISORT_NODE_MAP_MODE) {
undistortion_traj_ = paramsServer_->getPPKTraj();
mapping_traj_ = paramsServer_->getNodeTraj();
} else if (traj_mode == NODE_MODE) {
undistortion_traj_ = paramsServer_->getNodeTraj();
mapping_traj_ = paramsServer_->getNodeTraj();
} else if (traj_mode == PPK_MODE) {
undistortion_traj_ = paramsServer_->getPPKTraj();
mapping_traj_ = paramsServer_->getPPKTraj();
}
}
bool MapTask::addTrajectory(const boost::shared_ptr<Trajectory> &trajectory)
{
TrajType trajType = trajectory->getTrajType();
auto traj_mode = strategyControl_->getTrajMode();
if(traj_mode == PPK_UNDISORT_NODE_MAP_MODE){
if(trajType == PPK){
if(!undistortion_traj_){
undistortion_traj_ = trajectory;
}else{
LOG(INFO) << "undistortion_traj_: " << undistortion_traj_->getTrajPath()
<< " already set!";
return false;
}
}else if(trajType == NODE){
if(!mapping_traj_){
mapping_traj_ = trajectory;
}else{
LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
<< " already set!";
return false;
}
}
}else if(traj_mode == NODE_MODE){
if(trajType == NODE){
if(!mapping_traj_){
mapping_traj_ = trajectory;
undistortion_traj_ = trajectory;
}else{
LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
<< " already set!";
return false;
}
}
}else if(traj_mode == PPK_MODE){
if(trajType == PPK){
if(!mapping_traj_){
mapping_traj_ = trajectory;
undistortion_traj_ = trajectory;
}else{
LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
<< " already set!";
return false;
}
}
bool MapTask::addTrajectory(const boost::shared_ptr<Trajectory> &trajectory) {
TrajType trajType = trajectory->getTrajType();
auto traj_mode = strategyControl_->getTrajMode();
if (traj_mode == PPK_UNDISORT_NODE_MAP_MODE) {
if (trajType == PPK) {
if (!undistortion_traj_) {
undistortion_traj_ = trajectory;
} else {
LOG(INFO) << "undistortion_traj_: " << undistortion_traj_->getTrajPath()
<< " already set!";
return false;
}
} else if (trajType == NODE) {
if (!mapping_traj_) {
mapping_traj_ = trajectory;
} else {
LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
<< " already set!";
return false;
}
}
} else if (traj_mode == NODE_MODE) {
if (trajType == NODE) {
if (!mapping_traj_) {
mapping_traj_ = trajectory;
undistortion_traj_ = trajectory;
} else {
LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
<< " already set!";
return false;
}
}
} else if (traj_mode == PPK_MODE) {
if (trajType == PPK) {
if (!mapping_traj_) {
mapping_traj_ = trajectory;
undistortion_traj_ = trajectory;
} else {
LOG(INFO) << "mapping_traj_: " << mapping_traj_->getTrajPath()
<< " already set!";
return false;
}
}
return true;
}
return true;
}
void MapTask::build()
{
configTraj();
if(MERGE == strategyControl_->getTaskMode() && !isMerged){
return;
}
if(taskTrajectory_.size() == 0 && strategyControl_->getOption() != "rawframe"){
return;
}
vector<StageInfo> stageInfos = strategyControl_->getStageInfos();
request_to_end_ = false;
for(stage_ = 0; stage_ < stageInfos.size(); stage_++){
stageInfo_ = strategyControl_->getStageInfos()[stage_];
LOG(INFO) << "stage_ " << stage_;
if(0 == stage_){
establishProcessTrds(trdVec_);
LOG(INFO) << "establishProcessTrds Done";
}
getRawPointCloud(stageInfos[stage_]);
LOG(INFO) << "getRawPointCloud Done";
waitUntilFramesEmpty();
LOG(INFO) << "waitUntilFramesEmpty Done";
if(stageInfos.size() - 1 == stage_){
joinProcessTrds(trdVec_);
LOG(INFO) << "joinProcessTrds Done";
}
if(strategyControl_->getFilterMode() == DIRECT_OUTPUT_MODE){
auto voxeledPointcloud = dynamicRemover_->getVoxeledPointcloud();
for(size_t i = 0; i < voxeledPointcloud->size(); i++){
auto p = voxeledPointcloud->at(i);
PointExport point;
point.x = p.x;point.y = p.y;point.z = p.z;
point.intensity = p.intensity;
export_cloud_->push_back(point);
}
return;
}
// dynamicRemover_->grid_coord_->printGrid();
}
dynamicRemover_->multiThreadFilter(export_cloud_, filtered_cloud_);
LOG(INFO) << "multiThreadFilter";
void MapTask::build() {
configTraj();
if (MERGE == strategyControl_->getTaskMode() && !isMerged) {
return;
}
if (taskTrajectory_.size() == 0 &&
strategyControl_->getOption() != "rawframe") {
return;
}
vector<StageInfo> stageInfos = strategyControl_->getStageInfos();
request_to_end_ = false;
for (stage_ = 0; stage_ < stageInfos.size(); stage_++) {
stageInfo_ = strategyControl_->getStageInfos()[stage_];
LOG(INFO) << "stage_ " << stage_;
if (0 == stage_) {
establishProcessTrds(trdVec_);
LOG(INFO) << "establishProcessTrds Done";
}
getRawPointCloud(stageInfos[stage_]);
LOG(INFO) << "getRawPointCloud Done";
waitUntilFramesEmpty();
LOG(INFO) << "waitUntilFramesEmpty Done";
if (stageInfos.size() - 1 == stage_) {
joinProcessTrds(trdVec_);
LOG(INFO) << "joinProcessTrds Done";
}
if (strategyControl_->getFilterMode() == DIRECT_OUTPUT_MODE) {
auto voxeledPointcloud = dynamicRemover_->getVoxeledPointcloud();
for (size_t i = 0; i < voxeledPointcloud->size(); i++) {
auto p = voxeledPointcloud->at(i);
PointExport point;
point.x = p.x;
point.y = p.y;
point.z = p.z;
point.intensity = p.intensity;
export_cloud_->push_back(point);
}
return;
}
// dynamicRemover_->grid_coord_->printGrid();
}
dynamicRemover_->multiThreadFilter(export_cloud_, filtered_cloud_);
LOG(INFO) << "multiThreadFilter";
}
void MapTask::buildMappedFrame(
PointCloud*& frame, const StageInfo &stageInfo,
size_t &guessUndisortIndex, size_t &guessMapIndex)
{
if(strategyControl_->getOption() == "rawframe"){
boost::unique_lock<boost::mutex> lock(Mtx_);
if(frame->size()){
tempFrames_.push_back(*frame);
}
return;
}
auto frameSize = frame->size();
Matrix4d calib_1;
calib_1 = configCalib(stageInfo, 1);
if(stageInfo.pointcloudSource == BAG &&
strategyControl_->getBagMode() == Disorted){
calib_1.block(0, 0, 3, 3) = Matrix3d::Identity();
// LOG(INFO) << "calib linear = identity";
}
Isometry3d T1;
T1.translation() = calib_1.block(0, 3, 3, 1);
T1.linear() = calib_1.block(0, 0, 3, 3);
Isometry3d T = T1;
Matrix4d calib_2 = configCalib(stageInfo, 2);
Isometry3d T2;
T2.translation() = calib_2.block(0, 3, 3, 1);
T2.linear() = calib_2.block(0, 0, 3, 3);
T = T2 * T1;
Matrix4d calib;
calib.block(0, 3, 3, 1) = T.translation();
calib.block(0, 0, 3, 3) = T.linear();
conductCalib(frame, calib);
if((stageInfo.pointcloudSource == PCAP)
|| strategyControl_->getBagMode() == Undisorted){
conductMappingByPoint(frame, guessUndisortIndex);
if(0 == frame->size()){
return;
}
if(PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode()){
conductInverse(frame);
}
}
if(strategyControl_->getOption() == "undisortedframe"){
if(PPK_UNDISORT_NODE_MAP_MODE != strategyControl_->getTrajMode()){
conductInverse(frame);
}
boost::unique_lock<boost::mutex> lock(Mtx_);
if(frame->size()){
tempFrames_.push_back(*frame);
}
return;
}
if(PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode() &&
strategyControl_->getMappingLidar() == Fill &&
stageInfo.lidarPosition == TopLidar &&
stageInfo.hirMissmode == HIT){
PointCloud topGroundCloud;
for(const auto &p : *frame){
if(p.z < 0 && p.z > -3 &&
fabs(p.x) < 10 && fabs(p.y) < 15){
topGroundCloud.push_back(p);
}
}
topGroundCloud.sensor_orientation_ = frame->sensor_orientation_;
topGroundCloud.sensor_origin_ = frame->sensor_origin_;
frame->swap(topGroundCloud);
// pcl::io::savePCDFileBinary("/tmp/temp.pcd", *frame);
// exit(0);
}
if(PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode()){
for(auto &p : *frame){
if(p.z < -1.4){
p.data[3] = 1.0f;
}else{
p.data[3] = 0.0f;
}
}
conductMapping(frame, guessMapIndex);
}else{
for(auto &p : *frame){
p.x -= local_traj_center_.x();
p.y -= local_traj_center_.y();
p.z -= local_traj_center_.z();
}
}
if(strategyControl_->getOption() == "mappedframe"){
boost::unique_lock<boost::mutex> lock(Mtx_);
auto cloud = *frame;
Vector3d local_center_from_center = local_traj_center_ +
paramsServer_->getCenterToSlamStartOffset();
for(auto &p : cloud){
p.x += local_center_from_center.x();
p.y += local_center_from_center.y();
p.z += local_center_from_center.z();
}
if(cloud.size()){
tempFrames_.push_back(cloud);
}
return;
}
void MapTask::buildMappedFrame(PointCloud *&frame, const StageInfo &stageInfo,
size_t &guessUndisortIndex,
size_t &guessMapIndex) {
if (strategyControl_->getOption() == "rawframe") {
boost::unique_lock<boost::mutex> lock(Mtx_);
if (frame->size()) {
tempFrames_.push_back(*frame);
}
return;
}
Matrix4d calib_1;
calib_1 = configCalib(stageInfo, 1);
if (stageInfo.pointcloudSource == BAG &&
strategyControl_->getBagMode() == Disorted) {
calib_1.block(0, 0, 3, 3) = Matrix3d::Identity();
// LOG(INFO) << "calib linear = identity";
}
Isometry3d T1;
T1.translation() = calib_1.block(0, 3, 3, 1);
T1.linear() = calib_1.block(0, 0, 3, 3);
Isometry3d T = T1;
Matrix4d calib_2 = configCalib(stageInfo, 2);
Isometry3d T2;
T2.translation() = calib_2.block(0, 3, 3, 1);
T2.linear() = calib_2.block(0, 0, 3, 3);
T = T2 * T1;
Matrix4d calib;
calib.block(0, 3, 3, 1) = T.translation();
calib.block(0, 0, 3, 3) = T.linear();
conductCalib(frame, calib);
if ((stageInfo.pointcloudSource != BAG) ||
strategyControl_->getBagMode() == Undisorted) {
conductMappingByPoint(frame, guessUndisortIndex);
if (0 == frame->size()) {
return;
}
if (PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode()) {
conductInverse(frame);
}
}
if (strategyControl_->getOption() == "undisortedframe") {
if (PPK_UNDISORT_NODE_MAP_MODE != strategyControl_->getTrajMode()) {
conductInverse(frame);
}
boost::unique_lock<boost::mutex> lock(Mtx_);
if (frame->size()) {
tempFrames_.push_back(*frame);
}
return;
}
if (PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode() &&
strategyControl_->getMappingLidar() == Fill &&
stageInfo.lidarPosition == TopLidar && stageInfo.hirMissmode == HIT) {
PointCloud topGroundCloud;
for (const auto &p : *frame) {
if (p.z < 0 && p.z > -3 && fabs(p.x) < 10 && fabs(p.y) < 15) {
topGroundCloud.push_back(p);
}
}
topGroundCloud.sensor_orientation_ = frame->sensor_orientation_;
topGroundCloud.sensor_origin_ = frame->sensor_origin_;
frame->swap(topGroundCloud);
// pcl::io::savePCDFileBinary("/tmp/temp.pcd", *frame);
// exit(0);
}
if (PPK_UNDISORT_NODE_MAP_MODE == strategyControl_->getTrajMode()) {
for (auto &p : *frame) {
if (p.z < -1.4) {
p.data[3] = 1.0f;
} else {
p.data[3] = 0.0f;
}
}
conductMapping(frame, guessMapIndex);
} else {
for (auto &p : *frame) {
p.x -= local_traj_center_.x();
p.y -= local_traj_center_.y();
p.z -= local_traj_center_.z();
}
}
if (strategyControl_->getOption() == "mappedframe") {
boost::unique_lock<boost::mutex> lock(Mtx_);
auto cloud = *frame;
Vector3d local_center_from_center =
local_traj_center_ + paramsServer_->getCenterToSlamStartOffset();
for (auto &p : cloud) {
p.x += local_center_from_center.x();
p.y += local_center_from_center.y();
p.z += local_center_from_center.z();
}
if (cloud.size()) {
tempFrames_.push_back(cloud);
}
return;
}
}
void MapTask::directOutput(vector<PointCloud::Ptr> &Frames)
{
if(Frames.size() == 0){
LOG(WARNING) << "Frames.size() == 0";
return;
}
sort(Frames.begin(), Frames.end(),
[](const PointCloud::Ptr& query, const PointCloud::Ptr& target) {
return query->front().timestamp < target->front().timestamp;
});
for(const auto &frame : Frames){
for(const auto &point : *frame){
PointExport p;
p.x = point.x;p.y = point.y;p.z = point.z;
p.intensity = point.intensity;p.label = point.label;
p.rgb = point.rgb;p.info = point.ring;
export_cloud_->push_back(p);
}
}
export_cloud_->sensor_origin_ = Frames.front()->sensor_origin_;
export_cloud_->sensor_orientation_ = Frames.front()->sensor_orientation_;
void MapTask::directOutput(vector<PointCloud::Ptr> &Frames) {
if (Frames.size() == 0) {
LOG(WARNING) << "Frames.size() == 0";
return;
}
sort(Frames.begin(), Frames.end(),
[](const PointCloud::Ptr &query, const PointCloud::Ptr &target) {
return query->front().timestamp < target->front().timestamp;
});
for (const auto &frame : Frames) {
for (const auto &point : *frame) {
PointExport p;
p.x = point.x;
p.y = point.y;
p.z = point.z;
p.intensity = point.intensity;
p.label = point.label;
p.rgb = point.rgb;
p.info = point.ring;
export_cloud_->push_back(p);
}
}
export_cloud_->sensor_origin_ = Frames.front()->sensor_origin_;
export_cloud_->sensor_orientation_ = Frames.front()->sensor_orientation_;
}
vector<vector<TrajPoint>> MapTask::getTaskTrajectory()
{
return taskTrajectory_;
vector<vector<TrajPoint>> MapTask::getTaskTrajectory() {
return taskTrajectory_;
}
PointCloudExport::Ptr MapTask::getExportCloud()
{
return export_cloud_;
}
PointCloudExport::Ptr MapTask::getExportCloud() { return export_cloud_; }
PointCloudExport::Ptr MapTask::getFilteredCloud()
{
return filtered_cloud_;
}
PointCloudExport::Ptr MapTask::getFilteredCloud() { return filtered_cloud_; }
PointCloud::Ptr MapTask::getVoxeledCloud()
{
return voxeled_pointcloud_;
}
PointCloud::Ptr MapTask::getVoxeledCloud() { return voxeled_pointcloud_; }
Vector3d MapTask::getLocalOffset()
{
return local_traj_center_;
}
Vector3d MapTask::getLocalOffset() { return local_traj_center_; }
void MapTask::waitUntilFramesEmpty()
{
while(!frames_.empty()){
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
void MapTask::waitUntilFramesEmpty() {
while (!frames_.empty()) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
void MapTask::getRawPointCloud(const StageInfo &stageInfo)
{
for(const auto &meshDuration : timeDurations_){
if(!reader_){
DevicePosition lidarPos = stageInfo.lidarPosition;
PointCloudSource pcSource = stageInfo.pointcloudSource;
reader_.reset(new TaskReader(meshDuration, lidarPos, pcSource, paramsServer_));
}
PointCloud* frame;
uint32_t boostSize = 0;
bool dataRemain = true;
while(dataRemain){
auto start = system_clock::now();
dataRemain = reader_->getNextFrame(frame);
auto end = system_clock::now();
auto duration = duration_cast<milliseconds>(end - start);
double elapse = double(duration.count()) / 1000;
// if(elapse > 0.1){
// std::this_thread::sleep_for(std::chrono::milliseconds(50));
// }
LOG_EVERY_N(INFO, 999) << "process take " << elapse << " seconds";
if(dataRemain){
frames_.push(frame);
}
// LOG(INFO) << "boostSize: " << boostSize;
boostSize++;
if(boostSize == MAX_THREAD_CNT * 10){
waitUntilFramesEmpty();
boostSize = 0;
// LOG(INFO) << "waitUntilFramesEmpty";
}
// if(strategyControl_->getOption().find("frame") != string::npos){
// if(boostSize >= 1500){
// break;
// }
// }
}
reader_.reset();
}
void MapTask::getRawPointCloud(const StageInfo &stageInfo) {
for (const auto &meshDuration : timeDurations_) {
if (!reader_) {
DevicePosition lidarPos = stageInfo.lidarPosition;
PointCloudSource pcSource = stageInfo.pointcloudSource;
reader_.reset(
new TaskReader(meshDuration, lidarPos, pcSource, paramsServer_));
}
PointCloud *frame;
uint32_t boostSize = 0;
bool dataRemain = true;
while (dataRemain) {
auto start = system_clock::now();
dataRemain = reader_->getNextFrame(frame);
auto end = system_clock::now();
auto duration = duration_cast<milliseconds>(end - start);
double elapse = double(duration.count()) / 1000;
// if(elapse > 0.1){
// std::this_thread::sleep_for(std::chrono::milliseconds(50));
// }
LOG_EVERY_N(INFO, 999) << "process take " << elapse << " seconds";
if (dataRemain) {
frames_.push(frame);
}
// LOG(INFO) << "boostSize: " << boostSize;
boostSize++;
if (boostSize == MAX_THREAD_CNT * 10) {
waitUntilFramesEmpty();
boostSize = 0;
// LOG(INFO) << "waitUntilFramesEmpty";
}
// if(strategyControl_->getOption().find("frame") !=
// string::npos){
// if(boostSize >= 1500){
// break;
// }
// }
}
reader_.reset();
}
}
void MapTask::configTraj()
{
while(!paramsServer_->getTrajectoryLoded()){
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
if(taskTrajectory_.size() == 0){
vector<vector<TrajPoint>> poseTraj, ppkTraj;
vector<Time_Duration> durations;
for(const auto &durationvec : timeDurations_){
for(const auto &duration : durationvec){
durations.push_back(duration);
}
}
if(mapping_traj_){
size_t guessTrajIndex = 0;
auto basePoseTraj = mapping_traj_->getDurationsTrajs(timeDurations_.front(), guessTrajIndex);
guessTrajIndex = 0;
poseTraj = mapping_traj_->getDurationsTrajs(durations, guessTrajIndex);
LOG(INFO) << "basePoseTraj: " << basePoseTraj.size()
<< " poseTraj: " << poseTraj.size();
isMerged = basePoseTraj.size() != poseTraj.size();
if(MERGE == strategyControl_->getTaskMode() && !isMerged){
return;
}
mapping_mesh_.reset(new Trajectory("", NODE));
vector<TrajPoint> meshTraj;
for(const auto &traj : poseTraj){
if(traj.size() == 0){
continue;
}
for(const auto &traj_point : traj){
meshTraj.push_back(traj_point);
}
}
sort(meshTraj.begin(), meshTraj.end(),
[](const TrajPoint& query, const TrajPoint& target) {
return query.timestamp < target.timestamp;
});
mapping_mesh_->setTrajectory(meshTraj);
}
if(undistortion_traj_){
size_t guessTrajIndex = 0;
ppkTraj = undistortion_traj_->getDurationsTrajs(durations, guessTrajIndex);
auto traj_mode = strategyControl_->getTrajMode();
if(traj_mode == NODE_MODE){
undisort_mesh_.reset(new Trajectory("", NODE));
}else{
undisort_mesh_.reset(new Trajectory("", PPK));
}
// undisort_mesh_.reset(new Trajectory("", PPK));
vector<TrajPoint> meshTraj;
for(const auto &traj : ppkTraj){
if(traj.size() == 0){
continue;
}
for(const auto &traj_point : traj){
meshTraj.push_back(traj_point);
}
}
sort(meshTraj.begin(), meshTraj.end(),
[](const TrajPoint& query, const TrajPoint& target) {
return query.timestamp < target.timestamp;
});
undisort_mesh_->setTrajectory(meshTraj);
}
if(mapping_traj_){
taskTrajectory_ = poseTraj;
}else if(undistortion_traj_){
taskTrajectory_ = ppkTraj;
}
LOG(INFO) << "taskTrajectory_.size(): " << taskTrajectory_.size();
uint32_t trajpoint_cnt = 0;
double dis = 0;
for(const auto &traj : taskTrajectory_){
if(traj.size() == 0){
continue;
}
Vector3d last_trans = traj.front().translation;
for(const auto &traj_point : traj){
local_traj_center_ += traj_point.translation;
dis += (traj_point.translation - last_trans).norm();
last_trans = traj_point.translation;
}
trajpoint_cnt += traj.size();
}
local_traj_center_ /= trajpoint_cnt;
LOG(INFO) << "local_traj_center_: " << local_traj_center_.transpose();
Vector3d local_center_from_center = local_traj_center_ + paramsServer_->getCenterToSlamStartOffset();
Vector3d mod_006_offset = Vector3d::Zero();
mod_006_offset.x() = fmod(local_center_from_center.x(), 0.06);
mod_006_offset.y() = fmod(local_center_from_center.y(), 0.06);
LOG(INFO) << "mod_006_offset: " << mod_006_offset.transpose();
local_traj_center_ -= mod_006_offset;
LOG(INFO) << "dis: " << dis;
LOG(INFO) << "trajpoint_cnt - taskTrajectory_.size(): " << trajpoint_cnt - taskTrajectory_.size();
avg_dis_ = dis / (trajpoint_cnt - taskTrajectory_.size());
if(mapping_traj_->getTrajType() == NODE){
setupGridCoord();
}
}
void MapTask::configTraj() {
while (!paramsServer_->getTrajectoryLoded()) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
if (taskTrajectory_.size() == 0) {
vector<vector<TrajPoint>> poseTraj, ppkTraj;
vector<Time_Duration> durations;
for (const auto &durationvec : timeDurations_) {
for (const auto &duration : durationvec) {
durations.push_back(duration);
}
}
if (mapping_traj_) {
size_t guessTrajIndex = 0;
auto basePoseTraj = mapping_traj_->getDurationsTrajs(
timeDurations_.front(), guessTrajIndex);
guessTrajIndex = 0;
poseTraj = mapping_traj_->getDurationsTrajs(durations, guessTrajIndex);
LOG(INFO) << "basePoseTraj: " << basePoseTraj.size()
<< " poseTraj: " << poseTraj.size();
isMerged = basePoseTraj.size() != poseTraj.size();
if (MERGE == strategyControl_->getTaskMode() && !isMerged) {
return;
}
mapping_mesh_.reset(new Trajectory("", NODE));
vector<TrajPoint> meshTraj;
for (const auto &traj : poseTraj) {
if (traj.size() == 0) {
continue;
}
for (const auto &traj_point : traj) {
meshTraj.push_back(traj_point);
}
}
sort(meshTraj.begin(), meshTraj.end(),
[](const TrajPoint &query, const TrajPoint &target) {
return query.timestamp < target.timestamp;
});
mapping_mesh_->setTrajectory(meshTraj);
}
if (undistortion_traj_) {
size_t guessTrajIndex = 0;
ppkTraj =
undistortion_traj_->getDurationsTrajs(durations, guessTrajIndex);
auto traj_mode = strategyControl_->getTrajMode();
if (traj_mode == NODE_MODE) {
undisort_mesh_.reset(new Trajectory("", NODE));
} else {
undisort_mesh_.reset(new Trajectory("", PPK));
}
// undisort_mesh_.reset(new Trajectory("", PPK));
vector<TrajPoint> meshTraj;
for (const auto &traj : ppkTraj) {
if (traj.size() == 0) {
continue;
}
for (const auto &traj_point : traj) {
meshTraj.push_back(traj_point);
}
}
sort(meshTraj.begin(), meshTraj.end(),
[](const TrajPoint &query, const TrajPoint &target) {
return query.timestamp < target.timestamp;
});
undisort_mesh_->setTrajectory(meshTraj);
}
if (mapping_traj_) {
taskTrajectory_ = poseTraj;
} else if (undistortion_traj_) {
taskTrajectory_ = ppkTraj;
}
LOG(INFO) << "taskTrajectory_.size(): " << taskTrajectory_.size();
uint32_t trajpoint_cnt = 0;
double dis = 0;
for (const auto &traj : taskTrajectory_) {
if (traj.size() == 0) {
continue;
}
Vector3d last_trans = traj.front().translation;
for (const auto &traj_point : traj) {
local_traj_center_ += traj_point.translation;
dis += (traj_point.translation - last_trans).norm();
last_trans = traj_point.translation;
}
trajpoint_cnt += traj.size();
}
local_traj_center_ /= trajpoint_cnt;
LOG(INFO) << "local_traj_center_: " << local_traj_center_.transpose();
Vector3d local_center_from_center =
local_traj_center_ + paramsServer_->getCenterToSlamStartOffset();
Vector3d mod_006_offset = Vector3d::Zero();
mod_006_offset.x() = fmod(local_center_from_center.x(), 0.06);
mod_006_offset.y() = fmod(local_center_from_center.y(), 0.06);
LOG(INFO) << "mod_006_offset: " << mod_006_offset.transpose();
local_traj_center_ -= mod_006_offset;
LOG(INFO) << "dis: " << dis;
LOG(INFO) << "trajpoint_cnt - taskTrajectory_.size(): "
<< trajpoint_cnt - taskTrajectory_.size();
avg_dis_ = dis / (trajpoint_cnt - taskTrajectory_.size());
if (mapping_traj_->getTrajType() == NODE) {
setupGridCoord();
}
}
}
void MapTask::process()
{
PointCloud* frame;
LOG(INFO) << "thread in threadCnt_: " << threadCnt_++;
size_t guessUndisortIndex = 0;
size_t guessMapIndex = 0;
while(!request_to_end_ ){
if(!frames_.pop(frame)){
std::this_thread::sleep_for(std::chrono::milliseconds(5));
LOG_EVERY_N(WARNING, 1000000) << "request_to_end_: " << request_to_end_;
continue;
}
if(nullptr == frame || frame->size() == 0){
continue;
}
// if(!reader_){
// if(frames_->size() == 0){
// tempSwitch = true;
// pid = std::this_thread::get_id();
// LOG(INFO) << "pid: " << pid;
// }
// }
if((stageInfo_.lidarPosition == TopLidar
&& paramsServer_->getL0SN() == "PA4039CA5B9839CA53") ||
(stageInfo_.lidarPosition == BackLidar
&& paramsServer_->getL1SN() == "PA4039CA5B9839CA53")){
PointCloud removeLine39;
for(auto p : frame->points){
if(p.ring == 39){
continue;
}
removeLine39.push_back(p);
}
frame->swap(removeLine39);
}
PointCloud* originFrame;
if(stageInfo_.hirMissmode == MISS){
originFrame = new PointCloud;
*originFrame = *frame;
for(auto &p : *originFrame){
p.x = 0;p.y = 0;p.z = 0;
}
}
buildMappedFrame(frame, stageInfo_, guessUndisortIndex, guessMapIndex);
if(0 == frame->size()){
continue;
}
if(strategyControl_->getOption().size() != 0){
continue;
}
if(stageInfo_.hirMissmode == HIT){
uint8_t type = 0;
if(stageInfo_.pointcloudSource == BAG){
type = 1;
}
conductHits(*frame, stage_, type);
}else if(stageInfo_.hirMissmode == MISS){
buildMappedFrame(originFrame, stageInfo_, guessUndisortIndex, guessMapIndex);
conductMisses(frame, originFrame);
delete originFrame;
}
// tempSwitch = false;
delete frame;
}
LOG(INFO) << "thread quit threadCnt_: " << threadCnt_--;
void MapTask::process() {
PointCloud *frame;
LOG(INFO) << "thread in threadCnt_: " << threadCnt_++;
size_t guessUndisortIndex = 0;
size_t guessMapIndex = 0;
while (!request_to_end_) {
if (!frames_.pop(frame)) {
std::this_thread::sleep_for(std::chrono::milliseconds(5));
LOG_EVERY_N(WARNING, 1000000) << "request_to_end_: " << request_to_end_;
continue;
}
if (nullptr == frame || frame->size() == 0) {
continue;
}
// if(!reader_){
// if(frames_->size() == 0){
// tempSwitch = true;
// pid = std::this_thread::get_id();
// LOG(INFO) << "pid: " << pid;
// }
// }
if ((stageInfo_.lidarPosition == TopLidar &&
paramsServer_->getL0SN() == "PA4039CA5B9839CA53") ||
(stageInfo_.lidarPosition == BackLidar &&
paramsServer_->getL1SN() == "PA4039CA5B9839CA53")) {
PointCloud removeLine39;
for (auto p : frame->points) {
if (p.ring == 39) {
continue;
}
removeLine39.push_back(p);
}
frame->swap(removeLine39);
}
PointCloud *originFrame = nullptr;
if (stageInfo_.hirMissmode == MISS) {
originFrame = new PointCloud;
*originFrame = *frame;
for (auto &p : *originFrame) {
p.x = 0;
p.y = 0;
p.z = 0;
}
}
buildMappedFrame(frame, stageInfo_, guessUndisortIndex, guessMapIndex);
if (0 == frame->size()) {
continue;
}
if (strategyControl_->getOption().size() != 0) {
continue;
}
if (stageInfo_.hirMissmode == HIT) {
uint8_t type = 0;
if (stageInfo_.pointcloudSource == BAG) {
type = 1;
}
conductHits(*frame, stage_, type);
} else if (stageInfo_.hirMissmode == MISS) {
if (nullptr != originFrame) {
buildMappedFrame(originFrame, stageInfo_, guessUndisortIndex,
guessMapIndex);
conductMisses(frame, originFrame);
delete originFrame;
}
}
// tempSwitch = false;
delete frame;
}
LOG(INFO) << "thread quit threadCnt_: " << threadCnt_--;
}
void MapTask::establishProcessTrds(vector<ThreadPtr> &trdVec, uint32_t trdCnt)
{
for(size_t i = 0; i < trdCnt; i++){
ThreadPtr trd;
trd.reset(new boost::thread(boost::bind(&MapTask::process, this)));
trdVec.push_back(trd);
}
void MapTask::establishProcessTrds(vector<ThreadPtr> &trdVec, uint32_t trdCnt) {
for (size_t i = 0; i < trdCnt; i++) {
ThreadPtr trd;
trd.reset(new boost::thread(boost::bind(&MapTask::process, this)));
trdVec.push_back(trd);
}
}
void MapTask::joinProcessTrds(vector<ThreadPtr> &trdVec)
{
request_to_end_ = true;
// for(size_t i = 0; i < trdVec.size(); i++){
// frames_->stopQueue();
// usleep(10000);
// }
// LOG(INFO) << "stopQueue";
for(auto &trd : trdVec){
trd->timed_join(boost::posix_time::seconds(1));
}
void MapTask::joinProcessTrds(vector<ThreadPtr> &trdVec) {
request_to_end_ = true;
// for(size_t i = 0; i < trdVec.size(); i++){
// frames_->stopQueue();
// usleep(10000);
// }
// LOG(INFO) << "stopQueue";
for (auto &trd : trdVec) {
trd->timed_join(boost::posix_time::seconds(5));
}
}
Matrix4d MapTask::configCalib(const StageInfo &stageInfo, const uint8_t &type)
{
if(type == 1){
return paramsServer_->getDeviceCalib(stageInfo.lidarPosition);
}else if(type == 2){
return paramsServer_->getDeviceCalib(IMU);
}else if(type == 3){
return paramsServer_->getCalib(stageInfo.lidarPosition);
}
return Matrix4d::Identity();
Matrix4d MapTask::configCalib(const StageInfo &stageInfo, const uint8_t &type) {
if (type == 1) {
return paramsServer_->getDeviceCalib(stageInfo.lidarPosition);
} else if (type == 2) {
return paramsServer_->getDeviceCalib(IMU);
} else if (type == 3) {
return paramsServer_->getCalib(stageInfo.lidarPosition);
}
return Matrix4d::Identity();
}
void MapTask::conductCalib(PointCloud* &Frame)
{
conductCalib(Frame, calib_);
}
void MapTask::conductCalib(PointCloud *&Frame) { conductCalib(Frame, calib_); }
void MapTask::conductCalib(PointCloud* &Frame, const Matrix4d &calib)
{
pcl::transformPointCloud(*Frame, *Frame, calib);
void MapTask::conductCalib(PointCloud *&Frame, const Matrix4d &calib) {
pcl::transformPointCloud(*Frame, *Frame, calib);
}
bool checkTimeStamp(const double &time,
const double &time_1,
const double &time_2){
if(time >= time_1 && time < time_2){
return true;
}else{
return false;
}
bool checkTimeStamp(const double &time, const double &time_1,
const double &time_2) {
if (time >= time_1 && time < time_2) {
return true;
} else {
return false;
}
}
void MapTask::conductMappingByPoint(PointCloud* &Frame, size_t &guessTrajIndex)
{
if(0 == Frame->size()){
return;
}
double time_1 = 0, time_2 = 0;
bool frame_sensor_odom_set = false;
PointCloud locatedFrame;
Time_Duration duration;
double timeBuffer = 0.05;
if(NODE_MODE == strategyControl_->getTrajMode()){
timeBuffer = 1;
}
duration.start_sec = Frame->front().timestamp - timeBuffer;
duration.end_sec = Frame->back().timestamp + timeBuffer;
vector<TrajPoint> trajInFrame = undisort_mesh_->getDurationsTraj(duration, guessTrajIndex);
if(trajInFrame.size() < 2){
LOG_EVERY_N(WARNING, 99) << setprecision(15) << "trajInFrame.size() < 2: " << Frame->front().timestamp;
Frame->clear();
return;
void MapTask::conductMappingByPoint(PointCloud *&Frame,
size_t &guessTrajIndex) {
if (0 == Frame->size()) {
return;
}
double time_1 = 0, time_2 = 0;
bool frame_sensor_odom_set = false;
PointCloud locatedFrame;
Time_Duration duration;
double timeBuffer = 0.05;
if (NODE_MODE == strategyControl_->getTrajMode()) {
timeBuffer = 1;
}
duration.start_sec = Frame->front().timestamp - timeBuffer;
duration.end_sec = Frame->back().timestamp + timeBuffer;
vector<TrajPoint> trajInFrame =
undisort_mesh_->getDurationsTraj(duration, guessTrajIndex);
if (trajInFrame.size() < 2) {
LOG_EVERY_N(WARNING, 99)
<< setprecision(15)
<< "trajInFrame.size() < 2: " << Frame->front().timestamp;
Frame->clear();
return;
}
uint32_t lastTrajIndex = 0;
TrajPoint currTraj, nextTraj;
for (const auto &point : *Frame) {
double curr_point_timestamp = point.timestamp;
Point transformed_point = point;
bool found = false;
if (!checkTimeStamp(curr_point_timestamp, time_1, time_2)) {
for (uint32_t i = lastTrajIndex; i < trajInFrame.size() - 1; i++) {
currTraj = trajInFrame.at(i);
nextTraj = trajInFrame.at(i + 1);
if (checkTimeStamp(curr_point_timestamp, currTraj.timestamp,
nextTraj.timestamp)) {
time_1 = currTraj.timestamp;
time_2 = nextTraj.timestamp;
found = true;
break;
}
}
if (!found) {
continue;
}
}
uint32_t lastTrajIndex = 0;
TrajPoint currTraj, nextTraj;
for(const auto &point : *Frame){
double curr_point_timestamp = point.timestamp;
Point transformed_point = point;
bool found = false;
if(!checkTimeStamp(curr_point_timestamp, time_1, time_2)){
for(uint32_t i = lastTrajIndex; i < trajInFrame.size() - 1; i++){
currTraj = trajInFrame.at(i);
nextTraj = trajInFrame.at(i + 1);
if(checkTimeStamp(curr_point_timestamp, currTraj.timestamp, nextTraj.timestamp)){
time_1 = currTraj.timestamp;
time_2 = nextTraj.timestamp;
found = true;
break;
}
}
if(!found){
continue;
}
}
TrajPoint interpolated_traj;
double radio = (curr_point_timestamp - currTraj.timestamp) / (nextTraj.timestamp - currTraj.timestamp);
interpolated_traj = currTraj;
interpolated_traj.translation = currTraj.translation + radio * (nextTraj.translation - currTraj.translation);
interpolated_traj.rotation = currTraj.rotation.slerp(radio, nextTraj.rotation);
interpolated_traj.timestamp = curr_point_timestamp;
// LOG_EVERY_N(INFO, 999) << "interpolated_traj.translation.transpose(): " << interpolated_traj.translation.transpose();
if(!frame_sensor_odom_set){
locatedFrame.sensor_origin_ = {interpolated_traj.translation.x(),
interpolated_traj.translation.y(),
interpolated_traj.translation.z(),
0};
locatedFrame.sensor_orientation_ = interpolated_traj.rotation.cast<float>();
frame_sensor_odom_set = true;
}
Vector3d point_vec(point.x, point.y, point.z);
Vector3d transformed_vec = interpolated_traj.rotation * point_vec + interpolated_traj.translation;
transformed_point.x = transformed_vec.x();
transformed_point.y = transformed_vec.y();
transformed_point.z = transformed_vec.z();
locatedFrame.push_back(transformed_point);
}
Frame->swap(locatedFrame);
TrajPoint interpolated_traj;
double radio = (curr_point_timestamp - currTraj.timestamp) /
(nextTraj.timestamp - currTraj.timestamp);
interpolated_traj = currTraj;
interpolated_traj.translation =
currTraj.translation +
radio * (nextTraj.translation - currTraj.translation);
interpolated_traj.rotation =
currTraj.rotation.slerp(radio, nextTraj.rotation);
interpolated_traj.timestamp = curr_point_timestamp;
// LOG_EVERY_N(INFO, 999) <<
// "interpolated_traj.translation.transpose(): " <<
// interpolated_traj.translation.transpose();
if (!frame_sensor_odom_set) {
locatedFrame.sensor_origin_ = {interpolated_traj.translation.x(),
interpolated_traj.translation.y(),
interpolated_traj.translation.z(), 0};
locatedFrame.sensor_orientation_ =
interpolated_traj.rotation.cast<float>();
frame_sensor_odom_set = true;
}
Vector3d point_vec(point.x, point.y, point.z);
Vector3d transformed_vec =
interpolated_traj.rotation * point_vec + interpolated_traj.translation;
transformed_point.x = transformed_vec.x();
transformed_point.y = transformed_vec.y();
transformed_point.z = transformed_vec.z();
locatedFrame.push_back(transformed_point);
}
Frame->swap(locatedFrame);
}
void MapTask::conductInverse(PointCloud* &Frame)
{
auto sensor_t = Frame->sensor_origin_;
Matrix3f sensor_r = Matrix3f(Frame->sensor_orientation_);
Matrix4f frame_T;
frame_T.block(0, 0, 3, 3) = sensor_r.inverse();
frame_T.block(0, 3, 3, 1) = sensor_r.inverse() * Vector3f(-sensor_t.x(), -sensor_t.y(), -sensor_t.z());
pcl::transformPointCloud(*Frame, *Frame, frame_T);
void MapTask::conductInverse(PointCloud *&Frame) {
auto sensor_t = Frame->sensor_origin_;
Matrix3f sensor_r = Matrix3f(Frame->sensor_orientation_);
Matrix4f frame_T;
frame_T.block(0, 0, 3, 3) = sensor_r.inverse();
frame_T.block(0, 3, 3, 1) =
sensor_r.inverse() *
Vector3f(-sensor_t.x(), -sensor_t.y(), -sensor_t.z());
pcl::transformPointCloud(*Frame, *Frame, frame_T);
}
void MapTask::conductMapping(PointCloud* &Frame, size_t &guessTrajIndex)
{
if(Frame->size() == 0){
return;
}
PointCloud dummy_frame = *Frame;
Frame->clear();
double curr_frame_timestamp = dummy_frame.front().timestamp;
bool isSkip;
if(!mapping_mesh_->findFloorTrajPoint(curr_frame_timestamp, guessTrajIndex, isSkip)){
return;
}
if(isSkip){
return;
}
TrajPoint local_traj;
Isometry3d frameTransform;
TrajPoint interpolated_traj;
if(!mapping_mesh_->interpolation(curr_frame_timestamp, guessTrajIndex, interpolated_traj)){
LOG_EVERY_N(WARNING, 99) << "interpolation fail";
return;
}
local_traj = interpolated_traj;
Matrix4d interpolated_node = Trajectory::getTransformFromTrajPoint(local_traj);
frameTransform.translation() = interpolated_node.block(0, 3, 3, 1);
frameTransform.linear() = interpolated_node.block(0, 0, 3, 3);
frameTransform.translation() -= local_traj_center_;
Matrix4d frameMatrix;
frameMatrix.block(0, 3, 3, 1) = frameTransform.translation();
frameMatrix.block(0, 0, 3, 3) = frameTransform.linear();
pcl::transformPointCloud(dummy_frame, dummy_frame, frameMatrix);
local_traj.translation = frameTransform.translation();
StageInfo stageInfo = strategyControl_->getStageInfos()[stage_];
if(stageInfo.hirMissmode == HIT && mapping_mesh_->getTrajType() == NODE
&& strategyControl_->getFilterMode() != DIRECT_OUTPUT_MODE){
PointCloud dynamicAccepted_frame;
for(auto &point : dummy_frame){
double distance = point.distance;
int res = dynamicRemover_->grid_coord_->isDynamicallyAccepted(
point.x, point.y, distance, local_traj);
if(res >= 0) {
// point.ring = res;
dynamicAccepted_frame.push_back(point);
}
}
dummy_frame.swap(dynamicAccepted_frame);
}
// double traj_dis = 0;
// if(traj_index > 0){
// TrajPoint traj_upper = mapping_traj_->getTrajPointByIndex(traj_index + 1);
// if(traj_upper.translation.x() != 0){
// traj_dis = (traj_at_index.translation - traj_upper.translation).norm();
// }
// }
Frame->swap(dummy_frame);
Frame->sensor_origin_ = {local_traj.translation.x(),
local_traj.translation.y(),
local_traj.translation.z(),
0};
Frame->sensor_orientation_ = Matrix3f(frameTransform.linear().cast<float>());
void MapTask::conductMapping(PointCloud *&Frame, size_t &guessTrajIndex) {
if (Frame->size() == 0) {
return;
}
PointCloud dummy_frame = *Frame;
Frame->clear();
double curr_frame_timestamp = dummy_frame.front().timestamp;
bool isSkip;
if (!mapping_mesh_->findFloorTrajPoint(curr_frame_timestamp, guessTrajIndex,
isSkip)) {
return;
}
if (isSkip) {
return;
}
TrajPoint local_traj;
Isometry3d frameTransform;
TrajPoint interpolated_traj;
if (!mapping_mesh_->interpolation(curr_frame_timestamp, guessTrajIndex,
interpolated_traj)) {
LOG_EVERY_N(WARNING, 99) << "interpolation fail";
return;
}
local_traj = interpolated_traj;
Matrix4d interpolated_node =
Trajectory::getTransformFromTrajPoint(local_traj);
frameTransform.translation() = interpolated_node.block(0, 3, 3, 1);
frameTransform.linear() = interpolated_node.block(0, 0, 3, 3);
frameTransform.translation() -= local_traj_center_;
Matrix4d frameMatrix;
frameMatrix.block(0, 3, 3, 1) = frameTransform.translation();
frameMatrix.block(0, 0, 3, 3) = frameTransform.linear();
pcl::transformPointCloud(dummy_frame, dummy_frame, frameMatrix);
local_traj.translation = frameTransform.translation();
StageInfo stageInfo = strategyControl_->getStageInfos()[stage_];
if (stageInfo.hirMissmode == HIT && mapping_mesh_->getTrajType() == NODE &&
strategyControl_->getFilterMode() != DIRECT_OUTPUT_MODE) {
PointCloud dynamicAccepted_frame;
for (auto &point : dummy_frame) {
double distance = point.distance;
int res = dynamicRemover_->grid_coord_->isDynamicallyAccepted(
point.x, point.y, distance, local_traj);
if (res >= 0) {
// point.ring = res;
dynamicAccepted_frame.push_back(point);
}
}
dummy_frame.swap(dynamicAccepted_frame);
}
// double traj_dis = 0;
// if(traj_index > 0){
// TrajPoint traj_upper = mapping_traj_->getTrajPointByIndex(traj_index
// + 1); if(traj_upper.translation.x() != 0){
// traj_dis = (traj_at_index.translation -
// traj_upper.translation).norm();
// }
// }
Frame->swap(dummy_frame);
Frame->sensor_origin_ = {local_traj.translation.x(),
local_traj.translation.y(),
local_traj.translation.z(), 0};
Frame->sensor_orientation_ = Matrix3f(frameTransform.linear().cast<float>());
}
void MapTask::conductHits(
const PointCloud &Frame, uint8_t stage, uint8_t type)
{
// double radio = Frame->sensor_origin_[3] / avg_dis_;
dynamicRemover_->calculateHits(Frame, stage, type);
void MapTask::conductHits(const PointCloud &Frame, uint8_t stage,
uint8_t type) {
// double radio = Frame->sensor_origin_[3] / avg_dis_;
dynamicRemover_->calculateHits(Frame, stage, type);
}
void MapTask::conductMisses(const PointCloud *mappedFrame,
const PointCloud *mappedOriginFrame)
{
// double radio = mappedFrame->sensor_origin_[3] / avg_dis_;
dynamicRemover_->calculateMisses(mappedFrame, mappedOriginFrame, 1);
const PointCloud *mappedOriginFrame) {
// double radio = mappedFrame->sensor_origin_[3] / avg_dis_;
dynamicRemover_->calculateMisses(mappedFrame, mappedOriginFrame, 1);
}
void MapTask::setupGridCoord()
{
auto localTrajactory = taskTrajectory_;
for(auto &traj : localTrajactory){
for(auto &traj_point : traj){
traj_point.translation -= local_traj_center_;
}
void MapTask::setupGridCoord() {
auto localTrajactory = taskTrajectory_;
for (auto &traj : localTrajactory) {
for (auto &traj_point : traj) {
traj_point.translation -= local_traj_center_;
}
}
string path = "/tmp/localTrajactory.txt";
std::ofstream ofs(path);
if(!ofs){
LOG(WARNING) << path << " load fail!";
}
for(auto& trajectory : localTrajactory){
for(auto& pt : trajectory){
ofs << setprecision(15) << pt.translation[0] << " " << pt.translation[1] << " "
<< pt.translation[2] << " " << pt.nodeId << endl;
}
string path = "/tmp/localTrajactory.txt";
std::ofstream ofs(path);
if (!ofs) {
LOG(WARNING) << path << " load fail!";
}
for (auto &trajectory : localTrajactory) {
for (auto &pt : trajectory) {
ofs << setprecision(15) << pt.translation[0] << " " << pt.translation[1]
<< " " << pt.translation[2] << " " << pt.nodeId << endl;
}
ofs.close();
}
ofs.close();
for(auto &traj : localTrajactory){
for(auto &traj_point : traj){
dynamicRemover_->grid_coord_->updateArea(traj_point);
}
for (auto &traj : localTrajactory) {
for (auto &traj_point : traj) {
dynamicRemover_->grid_coord_->updateArea(traj_point);
}
}
dynamicRemover_->grid_coord_->peakTrajDrationAround();
dynamicRemover_->grid_coord_->peakTrajDrationAround();
// dynamicRemover_->grid_coord_->printGrid();
LOG(INFO) << "setupGridCoord done!";
// dynamicRemover_->grid_coord_->printGrid();
LOG(INFO) << "setupGridCoord done!";
}
libs/trajectory/trajectory.cpp
View file @ 0bf6b9f9
......@@ -7,444 +7,436 @@ uint16_t MIN_NODE_LINE_ITEM_CNT = 10;
float SquaredMinDis = pow(15, 2);
Trajectory::Trajectory(const string &traj_path, const TrajType &trajType)
:trajectory_path_(traj_path),
trajType_(trajType)
{
switch(trajType_){
: trajectory_path_(traj_path), trajType_(trajType) {
switch (trajType_) {
case PPK:
frequency_ = 1000;
break;
frequency_ = 1000;
break;
case NODE:
frequency_ = 10;
break;
}
frequency_ = 10;
break;
}
}
Trajectory::~Trajectory()
{
}
Trajectory::~Trajectory() {}
bool Trajectory::init()
{
string range;
switch(trajType_){
bool Trajectory::init() {
string range;
switch (trajType_) {
case PPK:
range = ",";
break;
range = ",";
break;
case NODE:
range = " ";
break;
}
LOG(INFO) << "Trajectory set frequency: " << frequency_;
io_.reset(new TextIO(trajectory_path_, range));
boost::function<void(const vector<string>&)> parse_callback =
boost::bind(&Trajectory::parseAndPush, this, _1);
io_->setParseCallback(parse_callback);
if(!io_){
LOG(WARNING) << "io_ is nullptr!";
return false;
}
if(!io_->load()){
LOG(WARNING) << "io_ load fail!";
range = " ";
break;
}
LOG(INFO) << "Trajectory set frequency: " << frequency_;
io_.reset(new TextIO(trajectory_path_, range));
boost::function<void(const vector<string> &)> parse_callback =
boost::bind(&Trajectory::parseAndPush, this, _1);
io_->setParseCallback(parse_callback);
if (!io_) {
LOG(WARNING) << "io_ is nullptr!";
return false;
}
if (!io_->load()) {
LOG(WARNING) << "io_ load fail!";
return false;
}
if (taskTrajInfo_.size()) {
setupBaseTrajOctree();
onRelativeTraj_ = true;
for (auto iter = taskTrajInfo_.begin(); iter != taskTrajInfo_.end();
iter++) {
const string &taskTrajPath = iter->first;
taskTrajPoseDiff_ = iter->second;
auto ppk_dir = taskTrajPath + "/raw_trace/ie.txt";
auto pose_dir = taskTrajPath + "/slam/cartographer.bag.pbstream.pose";
if (PPK == trajType_) {
io_.reset(new TextIO(ppk_dir, range));
} else if (NODE == trajType_) {
io_.reset(new TextIO(pose_dir, range));
} else {
return false;
}
boost::function<void(const vector<string> &)> parse_callback =
boost::bind(&Trajectory::parseAndPush, this, _1);
io_->setParseCallback(parse_callback);
io_->load();
LOG(INFO) << "relative traj " << taskTrajPath << " done";
}
if(taskTrajInfo_.size()){
setupBaseTrajOctree();
onRelativeTraj_ = true;
for(auto iter = taskTrajInfo_.begin();
iter != taskTrajInfo_.end(); iter++){
const string &taskTrajPath = iter->first;
taskTrajPoseDiff_ = iter->second;
auto ppk_dir = taskTrajPath + "/raw_trace/ie.txt";
auto pose_dir = taskTrajPath + "/slam/cartographer.bag.pbstream.pose";
if(PPK == trajType_){
io_.reset(new TextIO(ppk_dir, range));
}else if(NODE == trajType_){
io_.reset(new TextIO(pose_dir, range));
}else{
return false;
}
boost::function<void(const vector<string>&)> parse_callback =
boost::bind(&Trajectory::parseAndPush, this, _1);
io_->setParseCallback(parse_callback);
io_->load();
LOG(INFO) << "relative traj " << taskTrajPath << " done";
}
}
sort(trajectory_.begin(), trajectory_.end(),
[](const TrajPoint &query, const TrajPoint &target) {
return query.timestamp < target.timestamp;
});
LOG(INFO) << "trajectory_ size: " << trajectory_.size();
if (NODE == trajType_) {
fullTraj_ = trajectory_;
Vector3d lastTranslation;
int cnt = 0;
for (auto iter = trajectory_.begin(); iter != trajectory_.end();) {
if (0 != cnt && (lastTranslation - iter->translation).norm() < 0.39) {
iter = trajectory_.erase(iter);
} else {
lastTranslation = iter->translation;
iter++;
}
cnt++;
}
}
sort(trajectory_.begin(), trajectory_.end(),
[](const TrajPoint& query, const TrajPoint& target) {
return query.timestamp < target.timestamp;
});
LOG(INFO) << "trajectory_ size: " << trajectory_.size();
if(NODE == trajType_){
fullTraj_ = trajectory_;
Vector3d lastTranslation;
int cnt = 0;
for(auto iter = trajectory_.begin(); iter != trajectory_.end();){
if(0 != cnt && (lastTranslation - iter->translation).norm() < 0.39){
iter = trajectory_.erase(iter);
}else{
lastTranslation = iter->translation;
iter++;
}
cnt++;
}
}
LOG(INFO) << "after filter trajectory_ size: " << trajectory_.size();
return true;
LOG(INFO) << "after filter trajectory_ size: " << trajectory_.size();
return true;
}
string Trajectory::getTrajPath() const
{
return trajectory_path_;
}
string Trajectory::getTrajPath() const { return trajectory_path_; }
TrajType Trajectory::getTrajType() const
{
return trajType_;
}
TrajType Trajectory::getTrajType() const { return trajType_; }
bool Trajectory::findFloorTrajPoint(const double &timestamp, size_t &guessTrajIndex, bool& isSkip) const
{
isSkip = false;
if(trajectory_.size() < 2){
return false;
}
if(trajectory_.front().timestamp > timestamp){
return false;
bool Trajectory::findFloorTrajPoint(const double &timestamp,
size_t &guessTrajIndex,
bool &isSkip) const {
isSkip = false;
if (trajectory_.size() < 2) {
return false;
}
if (trajectory_.front().timestamp > timestamp) {
return false;
}
for (; guessTrajIndex < trajectory_.size() - 1; guessTrajIndex++) {
if (trajectory_.at(guessTrajIndex).timestamp > timestamp) {
guessTrajIndex = 0;
return findFloorTrajPoint(timestamp, guessTrajIndex, isSkip);
}
for(; guessTrajIndex < trajectory_.size() - 1; guessTrajIndex++){
if(trajectory_.at(guessTrajIndex).timestamp > timestamp){
guessTrajIndex = 0;
return findFloorTrajPoint(timestamp, guessTrajIndex, isSkip);
}
auto pre = trajectory_.at(guessTrajIndex);
auto pro = trajectory_.at(guessTrajIndex + 1);
if(pre.timestamp <= timestamp && pro.timestamp > timestamp){
if((pro.timestamp - pre.timestamp) * frequency_ > 20){
isSkip = true;
}
// LOG_EVERY_N(INFO, 9) << setprecision(15)
// << "hit!!!! timestamp: " << timestamp
// << " pre: " << pre.timestamp
// << " pro: " << pro.timestamp
// << " guessTrajIndex: " << guessTrajIndex;
return true;
}
auto pre = trajectory_.at(guessTrajIndex);
auto pro = trajectory_.at(guessTrajIndex + 1);
if (pre.timestamp <= timestamp && pro.timestamp > timestamp) {
if ((pro.timestamp - pre.timestamp) * frequency_ > 20) {
isSkip = true;
}
// LOG_EVERY_N(INFO, 9) << setprecision(15)
// << "hit!!!! timestamp: " << timestamp
// << " pre: " << pre.timestamp
// << " pro: " << pro.timestamp
// << " guessTrajIndex: " <<
// guessTrajIndex;
return true;
}
return false;
}
return false;
}
int32_t Trajectory::traversalFindTrajPoint(const uint32_t &start_index,
const uint32_t &end_index,
const double &timestamp) const
{
for(uint32_t index = start_index; index < end_index; index++){
if(checkSatisfied(index, timestamp)){
return index;
}
const double &timestamp) const {
for (uint32_t index = start_index; index < end_index; index++) {
if (checkSatisfied(index, timestamp)) {
return index;
}
return -1;
}
return -1;
}
bool Trajectory::interpolation(const double &timestamp,
const int32_t &floor_index,
TrajPoint &res_traj) const
{
int32_t upper_index = floor_index + 1;
if((uint32_t)upper_index >= trajectory_.size()){
LOG_EVERY_N(WARNING, 99) << "upper_index: " << upper_index
<< "trajectory_.size(): " << trajectory_.size();
return false;
}
const TrajPoint &traj_floor = trajectory_[floor_index];
const TrajPoint &traj_upper = trajectory_[upper_index];
double radio = (timestamp - traj_floor.timestamp) / (traj_upper.timestamp - traj_floor.timestamp);
res_traj = traj_floor;
res_traj.translation = traj_floor.translation + radio * (traj_upper.translation - traj_floor.translation);
res_traj.rotation = traj_floor.rotation.slerp(radio, traj_upper.rotation);
res_traj.timestamp = timestamp;
return true;
TrajPoint &res_traj) const {
int32_t upper_index = floor_index + 1;
if ((uint32_t)upper_index >= trajectory_.size()) {
LOG_EVERY_N(WARNING, 99) << "upper_index: " << upper_index
<< "trajectory_.size(): " << trajectory_.size();
return false;
}
const TrajPoint &traj_floor = trajectory_[floor_index];
const TrajPoint &traj_upper = trajectory_[upper_index];
double radio = (timestamp - traj_floor.timestamp) /
(traj_upper.timestamp - traj_floor.timestamp);
res_traj = traj_floor;
res_traj.translation =
traj_floor.translation +
radio * (traj_upper.translation - traj_floor.translation);
res_traj.rotation = traj_floor.rotation.slerp(radio, traj_upper.rotation);
res_traj.timestamp = timestamp;
return true;
}
Matrix4d Trajectory::getTransformFromTrajPoint(const TrajPoint &trajPoint)
{
Matrix4d T;
T.block(0, 0, 3, 3) = trajPoint.rotation.toRotationMatrix();
T.block(0, 3, 3, 1) = trajPoint.translation;
return T;
Matrix4d Trajectory::getTransformFromTrajPoint(const TrajPoint &trajPoint) {
Matrix4d T;
T.block(0, 0, 3, 3) = trajPoint.rotation.toRotationMatrix();
T.block(0, 3, 3, 1) = trajPoint.translation;
return T;
}
vector<vector<TrajPoint>> Trajectory::getDurationsTrajs(
const vector<Time_Duration> &durations, size_t &guessTrajIndex) const
{
vector<vector<TrajPoint>> ret;
for(const auto &duration : durations){
vector<TrajPoint> durationsTrajs;
bool isSkip;
if(!findFloorTrajPoint(duration.start_sec, guessTrajIndex, isSkip)){
LOG(INFO) << setprecision(15) << "findFloorTrajPoint fail:"
"duration.start_sec: " << duration.start_sec
<< " guessTrajIndex: " << guessTrajIndex;
guessTrajIndex = 0;
}
int32_t traj_index = guessTrajIndex;
for(uint32_t index = traj_index; index < trajectory_.size(); index++){
auto &traj = trajectory_.at(index);
if(traj.timestamp < duration.start_sec){
continue;
}else if(traj.timestamp <= duration.end_sec){
durationsTrajs.push_back(traj);
}else{
break;
}
}
if(durationsTrajs.size() > 1){
ret.push_back(durationsTrajs);
}
}
return ret;
}
vector<TrajPoint> Trajectory::getDurationsTraj(
const Time_Duration &duration, size_t &guessTrajIndex) const
{
vector<TrajPoint> ret;
const vector<Time_Duration> &durations, size_t &guessTrajIndex) const {
vector<vector<TrajPoint>> ret;
for (const auto &duration : durations) {
vector<TrajPoint> durationsTrajs;
bool isSkip;
int32_t traj_index = findFloorTrajPoint(duration.start_sec, guessTrajIndex, isSkip);
if(isSkip) return ret;
if(traj_index < 0){
LOG(INFO) << setprecision(15)
<< "getDurationsTrajs findFloorTrajPoint fail, error code = " << traj_index
<< " duration.start_sec: " << duration.start_sec;
traj_index = 0;
if (!findFloorTrajPoint(duration.start_sec, guessTrajIndex, isSkip)) {
LOG(INFO) << setprecision(15)
<< "findFloorTrajPoint fail:"
"duration.start_sec: "
<< duration.start_sec << " guessTrajIndex: " << guessTrajIndex;
guessTrajIndex = 0;
}
for(uint32_t index = traj_index; index < trajectory_.size(); index++){
auto &traj = trajectory_.at(index);
if(traj.timestamp < duration.start_sec){
continue;
}else if(traj.timestamp <= duration.end_sec){
ret.push_back(traj);
}else{
break;
}
int32_t traj_index = guessTrajIndex;
for (uint32_t index = traj_index; index < trajectory_.size(); index++) {
auto &traj = trajectory_.at(index);
if (traj.timestamp < duration.start_sec) {
continue;
} else if (traj.timestamp <= duration.end_sec) {
durationsTrajs.push_back(traj);
} else {
break;
}
}
return ret;
if (durationsTrajs.size() > 1) {
ret.push_back(durationsTrajs);
}
}
return ret;
}
TrajPoint Trajectory::getTrajPointByIndex(const uint32_t &index) const
{
if(index < trajectory_.size()){
return trajectory_.at(index);
}else{
return TrajPoint();
vector<TrajPoint> Trajectory::getDurationsTraj(const Time_Duration &duration,
size_t &guessTrajIndex) const {
vector<TrajPoint> ret;
bool isSkip;
int32_t traj_index =
findFloorTrajPoint(duration.start_sec, guessTrajIndex, isSkip);
if (isSkip) return ret;
if (traj_index < 0) {
LOG(INFO) << setprecision(15)
<< "getDurationsTrajs findFloorTrajPoint fail, error code = "
<< traj_index << " duration.start_sec: " << duration.start_sec;
traj_index = 0;
}
for (uint32_t index = traj_index; index < trajectory_.size(); index++) {
auto &traj = trajectory_.at(index);
if (traj.timestamp < duration.start_sec) {
continue;
} else if (traj.timestamp <= duration.end_sec) {
ret.push_back(traj);
} else {
break;
}
}
return ret;
}
TrajPoint Trajectory::getTrajPointByNodeId(const uint32_t &nodeId) const
{
for(const TrajPoint &point : fullTraj_){
if(point.nodeId == nodeId){
return point;
}
}
TrajPoint Trajectory::getTrajPointByIndex(const uint32_t &index) const {
if (index < trajectory_.size()) {
return trajectory_.at(index);
} else {
return TrajPoint();
}
}
vector<TrajPoint> Trajectory::getTrajectory() const
{
return trajectory_;
TrajPoint Trajectory::getTrajPointByNodeId(const uint32_t &nodeId) const {
for (const TrajPoint &point : fullTraj_) {
if (point.nodeId == nodeId) {
return point;
}
}
return TrajPoint();
}
boost::shared_ptr<LocalCartesian> Trajectory::getProj() const
{
return proj_;
}
vector<TrajPoint> Trajectory::getTrajectory() const { return trajectory_; }
void Trajectory::insertRelativeTaskInfo(
const string &trajPath, const Isometry3d &taskPoseDiff)
{
taskTrajInfo_.insert(make_pair(trajPath, taskPoseDiff));
boost::shared_ptr<LocalCartesian> Trajectory::getProj() const { return proj_; }
void Trajectory::insertRelativeTaskInfo(const string &trajPath,
const Isometry3d &taskPoseDiff) {
taskTrajInfo_.insert(make_pair(trajPath, taskPoseDiff));
}
bool Trajectory::checkSatisfied(const int32_t &supposed_floor_index, const double &timestamp) const
{
if(supposed_floor_index < 0){
return false;
}
if((uint32_t)supposed_floor_index >= trajectory_.size() - 2){
return false;
}
auto currTime = trajectory_[supposed_floor_index].timestamp;
auto nextTime = trajectory_[supposed_floor_index + 1].timestamp;
if(currTime <= timestamp && nextTime >= timestamp &&
(nextTime - currTime) * frequency_ < 3){
return true;
}else{
return false;
}
bool Trajectory::checkSatisfied(const int32_t &supposed_floor_index,
const double &timestamp) const {
if (supposed_floor_index < 0) {
return false;
}
if ((uint32_t)supposed_floor_index >= trajectory_.size() - 2) {
return false;
}
auto currTime = trajectory_[supposed_floor_index].timestamp;
auto nextTime = trajectory_[supposed_floor_index + 1].timestamp;
if (currTime <= timestamp && nextTime >= timestamp &&
(nextTime - currTime) * frequency_ < 3) {
return true;
} else {
return false;
}
}
double gpsToUtc(const uint32_t &gps_week, const double &gps_sec) {
return (315936000 + gps_week*7*24*3600 + gps_sec - 18);
return (315936000 + gps_week * 7 * 24 * 3600 + gps_sec - 18);
}
void Trajectory::parseAndPush(const vector<string> &vec)
{
switch(trajType_){
case PPK:
{
if(vec.size() < MIN_PPK_LINE_ITEM_CNT){
LOG_EVERY_N(WARNING, 99999)
<< io_->getPath() << " line item size < " << MIN_PPK_LINE_ITEM_CNT;
return;
void Trajectory::parseAndPush(const vector<string> &vec) {
switch (trajType_) {
case PPK: {
if (vec.size() < MIN_PPK_LINE_ITEM_CNT) {
LOG_EVERY_N(WARNING, 99999)
<< io_->getPath() << " line item size < " << MIN_PPK_LINE_ITEM_CNT;
return;
}
PPK_Raw_Info ppk_raw_info;
ppk_raw_info.gps_week = stoi(vec[1]);
ppk_raw_info.gps_sec = stod(vec[2]);
ppk_raw_info.lat = stod(vec[3]);
ppk_raw_info.lng = stod(vec[4]);
ppk_raw_info.height = stod(vec[5]);
ppk_raw_info.roll = stod(vec[9]);
ppk_raw_info.pitch = stod(vec[10]);
ppk_raw_info.yaw =
-stod(vec[11]); // ppk yaw take minus!!!!!!!!!!!!!!!!!!!!
{
if (!proj_) {
boost::unique_lock<boost::mutex> lock(Mtx_);
if (!proj_) {
proj_.reset(new LocalCartesian(ppk_raw_info.lat, ppk_raw_info.lng,
ppk_raw_info.height));
LOG(INFO) << setprecision(15) << "LocalCartesian center: "
<< Vector3d(ppk_raw_info.lat, ppk_raw_info.lng,
ppk_raw_info.height)
.transpose();
}
}
PPK_Raw_Info ppk_raw_info;
ppk_raw_info.gps_week = stoi(vec[1]);
ppk_raw_info.gps_sec = stod(vec[2]);
ppk_raw_info.lat = stod(vec[3]);
ppk_raw_info.lng = stod(vec[4]);
ppk_raw_info.height = stod(vec[5]);
ppk_raw_info.roll = stod(vec[9]);
ppk_raw_info.pitch = stod(vec[10]);
ppk_raw_info.yaw = - stod(vec[11]); //ppk yaw take minus!!!!!!!!!!!!!!!!!!!!
{
if(!proj_){
boost::unique_lock<boost::mutex> lock(Mtx_);
if(!proj_){
proj_.reset(new LocalCartesian(ppk_raw_info.lat,
ppk_raw_info.lng,
ppk_raw_info.height));
LOG(INFO) << setprecision(15) << "LocalCartesian center: "
<< Vector3d(ppk_raw_info.lat, ppk_raw_info.lng, ppk_raw_info.height).transpose();
}
}
}
TrajPoint trajPoint;
Vector3d translation;
proj_->Forward(ppk_raw_info.lat, ppk_raw_info.lng, ppk_raw_info.height,
translation.x(),
translation.y(),
translation.z());
trajPoint.translation = translation;
trajPoint.rotation = EulerToMatrix3d(Vector3d(ppk_raw_info.pitch, ppk_raw_info.roll, ppk_raw_info.yaw),
1, 0, 2);
if(preTransform_ != Matrix4d::Identity()){
trajPoint.rotation = trajPoint.rotation * preTransform_.block(0, 0, 3, 3).inverse();
}
// trajPoint.rotation = trajPoint.rotation * Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ());
trajPoint.nodeId = trajectory_.size();
trajPoint.timestamp = gpsToUtc(ppk_raw_info.gps_week, ppk_raw_info.gps_sec);
if(onRelativeTraj_){
trajPoint.rotation = taskTrajPoseDiff_.linear() * trajPoint.rotation * taskTrajPoseDiff_.inverse().linear();
}
boost::unique_lock<boost::mutex> lock_ppk(Mtx_);
trajectory_.push_back(trajPoint);
break;
}
TrajPoint trajPoint;
Vector3d translation;
proj_->Forward(ppk_raw_info.lat, ppk_raw_info.lng, ppk_raw_info.height,
translation.x(), translation.y(), translation.z());
trajPoint.translation = translation;
trajPoint.rotation = EulerToMatrix3d(
Vector3d(ppk_raw_info.pitch, ppk_raw_info.roll, ppk_raw_info.yaw), 1,
0, 2);
if (preTransform_ != Matrix4d::Identity()) {
trajPoint.rotation =
trajPoint.rotation * preTransform_.block(0, 0, 3, 3).inverse();
}
// trajPoint.rotation = trajPoint.rotation *
// Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ());
trajPoint.nodeId = trajectory_.size();
trajPoint.timestamp =
gpsToUtc(ppk_raw_info.gps_week, ppk_raw_info.gps_sec);
if (onRelativeTraj_) {
trajPoint.rotation = taskTrajPoseDiff_.linear() * trajPoint.rotation *
taskTrajPoseDiff_.inverse().linear();
}
boost::unique_lock<boost::mutex> lock_ppk(Mtx_);
trajectory_.push_back(trajPoint);
break;
}
case NODE:
{
if(vec.size() < MIN_NODE_LINE_ITEM_CNT){
LOG_EVERY_N(WARNING, 99999)
<< io_->getPath() << " line item size < " << MIN_NODE_LINE_ITEM_CNT;
return;
}
NODE_Raw_Info node_raw_info;
node_raw_info.node_id = stoi(vec[1]);
node_raw_info.time = stod(vec[2]);
node_raw_info.x = stod(vec[3]);
node_raw_info.y = stod(vec[4]);
node_raw_info.z = stod(vec[5]);
node_raw_info.w = stod(vec[6]);
node_raw_info.qx = stod(vec[7]);
node_raw_info.qy = stod(vec[8]);
node_raw_info.qz = stod(vec[9]);
// if(node_raw_info.node_id > 82000 && node_raw_info.node_id < 82100){
// return;
// }
// if(node_raw_info.node_id > 91552 && node_raw_info.node_id < 91744){
// return;
// }
// if(node_raw_info.node_id > 72192 && node_raw_info.node_id < 72256){
// return;
// }
// bool skip = (node_raw_info.node_id > 10907 && node_raw_info.node_id < 13417) ||
// (node_raw_info.node_id > 86905 && node_raw_info.node_id < 89253) ||
// (node_raw_info.node_id > 3076 && node_raw_info.node_id < 5752) ;
// skip = !skip;
// if(skip){
// return;
// }
if(trajectory_.size() == 0){
trajId_ = stoi(vec[0]);
case NODE: {
if (vec.size() < MIN_NODE_LINE_ITEM_CNT) {
LOG_EVERY_N(WARNING, 99999)
<< io_->getPath() << " line item size < " << MIN_NODE_LINE_ITEM_CNT;
return;
}
NODE_Raw_Info node_raw_info;
node_raw_info.node_id = stoi(vec[1]);
node_raw_info.time = stod(vec[2]);
node_raw_info.x = stod(vec[3]);
node_raw_info.y = stod(vec[4]);
node_raw_info.z = stod(vec[5]);
node_raw_info.w = stod(vec[9]);
node_raw_info.qx = stod(vec[6]);
node_raw_info.qy = stod(vec[7]);
node_raw_info.qz = stod(vec[8]);
// if(node_raw_info.node_id > 82000 && node_raw_info.node_id <
// 82100){
// return;
// }
// if(node_raw_info.node_id > 91552 && node_raw_info.node_id <
// 91744){
// return;
// }
// if(node_raw_info.node_id > 72192 && node_raw_info.node_id <
// 72256){
// return;
// }
// bool skip = (node_raw_info.node_id > 10907 &&
// node_raw_info.node_id < 13417) ||
// (node_raw_info.node_id > 86905 && node_raw_info.node_id
// < 89253) || (node_raw_info.node_id > 3076 &&
// node_raw_info.node_id < 5752) ;
// skip = !skip;
// if(skip){
// return;
// }
if (trajectory_.size() == 0) {
trajId_ = stoi(vec[0]);
}
TrajPoint trajPoint;
trajPoint.nodeId = node_raw_info.node_id;
trajPoint.timestamp = node_raw_info.time;
trajPoint.translation = {node_raw_info.x, node_raw_info.y,
node_raw_info.z};
trajPoint.rotation = {node_raw_info.w, node_raw_info.qx, node_raw_info.qy,
node_raw_info.qz};
if (onRelativeTraj_) {
trajPoint.translation =
taskTrajPoseDiff_.linear() * trajPoint.translation +
taskTrajPoseDiff_.translation();
int result_index;
float sqr_distance;
pcl::PointXYZ query;
query.x = trajPoint.translation.x();
query.y = trajPoint.translation.y();
query.z = trajPoint.translation.z();
octree->approxNearestSearch(query, result_index, sqr_distance);
if (sqr_distance < SquaredMinDis) {
return;
}
TrajPoint trajPoint;
trajPoint.nodeId = node_raw_info.node_id;
trajPoint.timestamp = node_raw_info.time;
trajPoint.translation = {node_raw_info.x, node_raw_info.y, node_raw_info.z};
trajPoint.rotation = {node_raw_info.w, node_raw_info.qx, node_raw_info.qy, node_raw_info.qz};
if(onRelativeTraj_){
trajPoint.translation = taskTrajPoseDiff_.linear() * trajPoint.translation
+ taskTrajPoseDiff_.translation();
int result_index;
float sqr_distance;
pcl::PointXYZ query;
query.x = trajPoint.translation.x();
query.y = trajPoint.translation.y();
query.z = trajPoint.translation.z();
octree->approxNearestSearch(query, result_index, sqr_distance);
if(sqr_distance < SquaredMinDis){
return;
}
trajPoint.rotation = taskTrajPoseDiff_.linear() * trajPoint.rotation * taskTrajPoseDiff_.inverse().linear();
}
trajPoint.rotation = taskTrajPoseDiff_.linear() * trajPoint.rotation *
taskTrajPoseDiff_.inverse().linear();
}
//pose yaw take -M_PI/2!!!!!!!!!!!!!!!!!!
// trajPoint.rotation = trajPoint.rotation * Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d::UnitZ());
// pose yaw take -M_PI/2!!!!!!!!!!!!!!!!!!
// trajPoint.rotation = trajPoint.rotation *
// Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d::UnitZ());
// Matrix3d tempRot = Matrix3d::Identity() * Eigen::AngleAxisd(-2.032297, Vector3d::UnitZ());
// Vector3d tempTrans(-3.7193, 9.0219, 15.2070);
// trajPoint.translation = tempRot * trajPoint.translation + (-tempTrans);
// trajPoint.rotation = tempRot * trajPoint.rotation;
// Matrix3d tempRot = Matrix3d::Identity() *
// Eigen::AngleAxisd(-2.032297, Vector3d::UnitZ()); Vector3d
// tempTrans(-3.7193, 9.0219, 15.2070); trajPoint.translation =
// tempRot * trajPoint.translation + (-tempTrans);
// trajPoint.rotation = tempRot * trajPoint.rotation;
boost::unique_lock<boost::mutex> lock_node(Mtx_);
trajectory_.push_back(trajPoint);
boost::unique_lock<boost::mutex> lock_node(Mtx_);
trajectory_.push_back(trajPoint);
break;
}
break;
}
}
}
void Trajectory::setupBaseTrajOctree()
{
octree.reset(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>(5));
pcl::PointCloud<pcl::PointXYZ> trajCloud;
for(const auto& trajPoint : trajectory_){
pcl::PointXYZ point;
point.x = trajPoint.translation.x();
point.y = trajPoint.translation.y();
point.z = trajPoint.translation.z();
trajCloud.push_back(point);
}
octree->setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>(trajCloud));
octree->addPointsFromInputCloud();
void Trajectory::setupBaseTrajOctree() {
octree.reset(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>(5));
pcl::PointCloud<pcl::PointXYZ> trajCloud;
for (const auto &trajPoint : trajectory_) {
pcl::PointXYZ point;
point.x = trajPoint.translation.x();
point.y = trajPoint.translation.y();
point.z = trajPoint.translation.z();
trajCloud.push_back(point);
}
octree->setInputCloud(
boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>(trajCloud));
octree->addPointsFromInputCloud();
}
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