test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -281,9 +281,9 @@ vector<IsometryData> AdjustPPK::HandleLocalPPK(
     for(; ppkIndex_ < localPoseVec_.size(); ppkIndex_++){
         const auto &pose = localPoseVec_.at(ppkIndex_);
         if(0 == ppkIndex_){
-            mapPose_ = localPoseVec_.front().pose.cast<float>();
+            mapPose_ = localPoseVec_.front().pose;
         }else{
-            mapPose_ = mapPose_ * (localPoseVec_.at(ppkIndex_ - 1).pose.inverse() * pose.pose).cast<float>();
+            mapPose_ = mapPose_ * (localPoseVec_.at(ppkIndex_ - 1).pose.inverse() * pose.pose);
         }
         if(pose.timestamp > backPTime){
             break;
@@ -361,33 +361,33 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
     }
     static std::ofstream ofs(ieBaseDir_ + "/CloudPredictPose.txt", ios::app);
 
-    Quaternionf q(mapPose_.linear());
-    Vector3f rpy= RotationQuaternionToEulerVector(q);
+    Quaterniond q(mapPose_.linear());
+    Vector3d rpy= RotationQuaternionToEulerVector(q);
     LOG(INFO) << setprecision(15) << "cloud time: " << cloudInfo.timestamp
               << " 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>();
+    guessPose.map_point = mapPose_;
     auto increaseRpy = RotationQuaternionToEulerVector(
                 Quaterniond(cloudInfo.filterPoseInfo.increasePose.linear()));
     guessPose.movement = cloudInfo.filterPoseInfo.increasePose.translation().norm();
     guessPose.rotation = fabs(increaseRpy.z());
     guessPose.use_gnss = false;
-    if(!currMatcher_->IsMapLoaded(mapPose_.translation().cast<double>())){
-        currMatcher_->loadArea(mapPose_.translation().cast<double>());
+    if(!currMatcher_->IsMapLoaded(mapPose_.translation())){
+        currMatcher_->loadArea(mapPose_.translation());
     }
     ofs << setprecision(16) << cloudInfo.timestamp << ", " << mapPose_.translation().x()
         << ", " << mapPose_.translation().y() << ", " << mapPose_.translation().z() << endl;
     guessPose.precision_type = MATCH_PRECISION_FAST;
-    float score = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose_);
+    float score = currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose_.cast<float>());
     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_);
+    currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, mapPose_.cast<float>());
     return true;
 }
 
@@ -460,10 +460,10 @@ bool AdjustPPK::LoadMesh(const string &streamPath, boost::shared_ptr<VoxelMapMat
 
     LOG(INFO) << "mesh to load: " << streamPath;
     VoxelMapMatcherOption option;
-    option.option.filter_resolution = 0.5;
+    option.option.filter_resolution = 0.6;
     option.option.cloud_range = 80;
-    option.fast_option.accepted_score = 0.45;
-    option.fast_option.accepted_low_score = 0.55;
+    option.fast_option.accepted_score = 0.35;
+    option.fast_option.accepted_low_score = 0.45;
     matcher.reset(new VoxelMapMatcher(option));
     matcher->SetMapPath(streamPath);
     return true;

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.h

@@ -108,7 +108,7 @@ private:
 
     Vector3d center_ = Vector3d::Zero();
 
-    Isometry3f mapPose_ = Isometry3f::Identity();
+    Isometry3d mapPose_ = Isometry3d::Identity();
 
     uint32_t cloudCnt_ = 0;
 };