Commit 73904574 authored by wangdawei's avatar wangdawei

test

parent d1b65e57
......@@ -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;
......
......@@ -108,7 +108,7 @@ private:
Vector3d center_ = Vector3d::Zero();
Isometry3f mapPose_ = Isometry3f::Identity();
Isometry3d mapPose_ = Isometry3d::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