Skip to content

L
localize_for_ppk
Commit ad67ab18 authored by wangdawei's avatar wangdawei
Browse files
Options

test

parent 94d1eebc
Hide whitespace changes
Inline Side-by-side
Showing with 65 additions and 54 deletions
libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp
View file @ ad67ab18
......@@ -97,7 +97,17 @@ void AdjustPPK::ReadCenter()
}
void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
{
{
if(0 == cloud.size()){
return;
}
double backPTime = cloud.back().timestamp;
double frontPTime = cloud.front().timestamp;
vector<IsometryData> periodPose = HandleLocalPPK(backPTime, frontPTime);
cloudCnt_++;
if(cloudCnt_ % 10 != 1){
return;
}
CloudPacket cloudPacket;
ros::Time rosTime;
pcl_conversions::fromPCL(cloud.header.stamp, rosTime);
......@@ -107,12 +117,12 @@ void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
<< " cloud.size(): " << cloud.size()
<< " frontPTime: " << cloud.front().timestamp
<< " backPTime: " << cloud.back().timestamp;
if(cloudPacket.timestamp < 1683806268.156870){
return;
}
if(cloudPacket.timestamp > 1683806268.956870){
exit(0);
}
// if(cloudPacket.timestamp < 1683806268.156870){
// return;
// }
// if(cloudPacket.timestamp > 1683806268.956870){
// exit(0);
// }
// pcl::io::savePCDFileBinary(ieBaseDir_ + "/" + to_string(cloudPacket.timestamp) + "_raw.pcd", cloud);
// exit(0);
for(const auto &p : cloud){
......@@ -126,8 +136,9 @@ void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
cloudPacket.cloud.push_back(point);
}
CloudInfoForMatch cloudInfo;
if(!Undisort(cloudPacket, cloudInfo)){
if(!Undisort(cloudPacket, periodPose, cloudInfo)){
return;
}
......@@ -261,15 +272,18 @@ PointCloudInter::Ptr AdjustPPK::GetBaseFrame(
return frame;
}
bool AdjustPPK::Undisort(
const CloudPacket &rawFrame, CloudInfoForMatch &cloudInfo)
vector<IsometryData> AdjustPPK::HandleLocalPPK(
double backPTime, double frontPTime)
{
vector<IsometryData> periodPose;
periodPose.reserve(150);
double backPTime = rawFrame.cloud.back().time;
double frontPTime = rawFrame.cloud.front().time;
for(; ppkIndex_ < localPoseVec_.size(); ppkIndex_++){
const auto &pose = localPoseVec_.at(ppkIndex_);
if(0 == ppkIndex_){
mapPose_ = localPoseVec_.front().pose.cast<float>();
}else{
mapPose_ = mapPose_ * (localPoseVec_.at(ppkIndex_ - 1).pose.inverse() * pose.pose).cast<float>();
}
if(pose.timestamp > backPTime){
break;
}
......@@ -278,17 +292,23 @@ bool AdjustPPK::Undisort(
}
periodPose.emplace_back(pose);
}
return periodPose;
}
bool AdjustPPK::Undisort(
const CloudPacket &rawFrame, const vector<IsometryData> periodPose, CloudInfoForMatch &cloudInfo)
{
// LOG(INFO) << "periodPose.size(): " << periodPose.size();
if(periodPose.size() < 2){
return false;
}
PointCloudInter::Ptr frame = GetBaseFrame(rawFrame);
pcl::io::savePCDFileBinary(ieBaseDir_ + "/" + to_string(frontPTime) + "_base.pcd", *frame);
// pcl::io::savePCDFileBinary(ieBaseDir_ + "/" + to_string(rawFrame.cloud.front().time) + "_base.pcd", *frame);
Isometry3d framePose, increasePose;
PointCloudJ::Ptr undisortedFrame(new PointCloudJ);
PointCloudJ mappedFrame;
CloudPreprocess::Instance()->Undisort(
frame, undisortedFrame, &mappedFrame, backPTime, &periodPose, framePose, increasePose);
frame, undisortedFrame, &mappedFrame, rawFrame.cloud.back().time, &periodPose, framePose, increasePose);
// pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(rawFrame.timestamp) + "_mapped.pcd", mappedFrame);
cloudInfo.frame = undisortedFrame;
cloudInfo.filterPoseInfo.increasePose = increasePose;
......@@ -297,8 +317,6 @@ bool AdjustPPK::Undisort(
Quaterniond q(increasePose.linear());
Vector3d rpy= RotationQuaternionToEulerVector(q);
LOG_EVERY_N(INFO, 10) << setprecision(15)
<< "frontPTime: " << frontPTime
<< " backPTime: " << backPTime
<< " periodPose.front(): " << periodPose.front().timestamp
<< " periodPose.back(): " << periodPose.back().timestamp
<< " increasePose: " << increasePose.translation().transpose()
......@@ -340,46 +358,35 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
LOG(INFO) << "no currMatcher_!";
return false;
}
static Isometry3f mapPose = Isometry3f::Identity();
static uint32_t cnt = 0;
static std::ofstream ofs(ieBaseDir_ + "/CloudPredictPose.txt", ios::app);
if(0 == cnt){
mapPose = cloudInfo.filterPoseInfo.filterPose.cast<float>();
}
if(cnt % 10 != 0){
mapPose = mapPose * cloudInfo.filterPoseInfo.increasePose.cast<float>();
}else{
Quaternionf q(mapPose.linear());
Vector3f rpy= RotationQuaternionToEulerVector(q);
LOG(INFO) << "guessPose: " << mapPose.translation().transpose()
<< " rpy: " << rpy.transpose();
Quaternionf q(mapPose_.linear());
Vector3f rpy= RotationQuaternionToEulerVector(q);
LOG(INFO) << "guessPose: " << mapPose_.translation().transpose()
<< " rpy: " << rpy.transpose();
// pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(cloudInfo.timestamp) + ".pcd", *cloudInfo.frame);
GnssPoint finalGnssPose;
GuessPose guessPose;
guessPose.map_point = mapPose.cast<double>();
auto increaseRpy = RotationQuaternionToEulerVector(
Quaterniond(cloudInfo.filterPoseInfo.increasePose.linear()));
guessPose.movement = cloudInfo.filterPoseInfo.increasePose.translation().norm();
guessPose.rotation = fabs(increaseRpy.z());
guessPose.use_gnss = false;
LOG(INFO) << "currMatcher_ ptr: " << currMatcher_.get();
if(!currMatcher_->IsMapLoaded(mapPose.translation().cast<double>())){
currMatcher_->loadArea(mapPose.translation().cast<double>());
}
guessPose.precision_type = MATCH_PRECISION_FAST;
float score = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose);
if(score < 0.45){
return false;
}
guessPose.map_point = mapPose.cast<double>();
guessPose.precision_type = MATCH_PRECISION_HIGH;
currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose);
ofs << setprecision(16) << cloudInfo.timestamp << ", " << mapPose.translation().x()
<< ", " << mapPose.translation().y() << ", " << mapPose.translation().z() << endl;
exit(0);
GnssPoint finalGnssPose;
GuessPose guessPose;
guessPose.map_point = mapPose_.cast<double>();
auto increaseRpy = RotationQuaternionToEulerVector(
Quaterniond(cloudInfo.filterPoseInfo.increasePose.linear()));
guessPose.movement = cloudInfo.filterPoseInfo.increasePose.translation().norm();
guessPose.rotation = fabs(increaseRpy.z());
guessPose.use_gnss = false;
LOG(INFO) << "currMatcher_ ptr: " << currMatcher_.get();
if(!currMatcher_->IsMapLoaded(mapPose_.translation().cast<double>())){
currMatcher_->loadArea(mapPose_.translation().cast<double>());
}
cnt++;
guessPose.precision_type = MATCH_PRECISION_FAST;
float score = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose_);
if(score < 0.45){
return false;
}
guessPose.map_point = mapPose_.cast<double>();
guessPose.precision_type = MATCH_PRECISION_HIGH;
currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose_);
ofs << setprecision(16) << cloudInfo.timestamp << ", " << mapPose_.translation().x()
<< ", " << mapPose_.translation().y() << ", " << mapPose_.translation().z() << endl;
return true;
}
......
libs/locate_system/adjust_ppk_by_locate/adjust_ppk.h
View file @ ad67ab18
......@@ -65,7 +65,9 @@ private:
PointCloudInter::Ptr GetBaseFrame(const CloudPacket &cloudData);
bool Undisort(const CloudPacket &baseFrame, CloudInfoForMatch &cloudInfo);
vector<IsometryData> HandleLocalPPK(double backPTime, double frontPTime);
bool Undisort(const CloudPacket &baseFrame, const vector<IsometryData> periodPose, CloudInfoForMatch &cloudInfo);
bool ConfigMap(double timestamp);
......@@ -105,6 +107,8 @@ private:
Vector3d center_ = Vector3d::Zero();
Isometry3f mapPose_ = Isometry3f::Identity();
uint32_t cloudCnt_ = 0;
};
......
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