test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -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

@@ -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;
 };