Commit 5a517f5d authored by wangdawei's avatar wangdawei

matcher reset and laser filter

parent 65f57bcd
......@@ -42,7 +42,7 @@ AdjustPPK::AdjustPPK(const string &ieBaseDir, const string &mapDir)
LOG(INFO) << "calib_: " << calib_.translation().transpose()
<< " rpy: " << rpy.transpose();
trd_.reset(new boost::thread(boost::bind(&AdjustPPK::LoadMap, this)));
skipCnt_ = 20;
skipCnt_ = 40;
}
AdjustPPK::~AdjustPPK()
......@@ -56,7 +56,7 @@ void AdjustPPK::ConfigConvert(const string &firetimeFile, const string &correcti
{
LOG(INFO) << "correctionFile: " << correctionFile;
LOG(INFO) << "firetimeFile: " << firetimeFile;
convert_.reset(new pandar_pointcloud::Convert(firetimeFile, correctionFile));
convert_.reset(new pandar_pointcloud::Convert(firetimeFile, correctionFile, true));
LOG(INFO) << "ConfigConvert done";
}
......@@ -231,6 +231,9 @@ void AdjustPPK::LoadMapInfo()
directory_iterator stream_iter(outputDir);
directory_iterator stream_end_iter;
for(; stream_iter != stream_end_iter; stream_iter++) {
if(stream_iter->path().extension().string() != ".stream"){
continue;
}
auto fileName = stream_iter->path().stem().string();
ulong meshId;
try {
......@@ -488,10 +491,19 @@ bool AdjustPPK::ConfigMap(double timestamp)
return false;
}
if(currMatcher_){
currMatcher_->unLoadAllBlocks();
// currMatcher_->unLoadAllBlocks();
currMatcher_.reset();
}
while(!meshVec_.at(meshId).matcher){
std::this_thread::sleep_for(std::chrono::milliseconds(100));
// while(!meshVec_.at(meshId).matcher){
// std::this_thread::sleep_for(std::chrono::milliseconds(100));
// }
auto &meshInfo = meshVec_.at(meshId);
if(meshInfo.streamPath != ""){
if(LoadMesh(meshInfo.streamPath, meshInfo.matcher)){
LOG(INFO) << "mesh loaded: " << meshId;
}else{
return false;
}
}
currMatcher_ = meshVec_.at(meshId).matcher;
LOG(INFO) << "set currMatcher_: " << meshId;
......@@ -541,16 +553,16 @@ bool AdjustPPK::LocateCloud(const CloudInfoForMatch &cloudInfo)
if(score < 0){
guessPose.precision_type = MATCH_PRECISION_INIT;
currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
mapPose_ = finalPose.cast<double>();
mapPose_.translation() = finalPose.translation().cast<double>();
CloseToPPK(0.1);
skipCnt_ = 5;
skipCnt_ = 10;
return true;
}
skipCnt_ = 20;
skipCnt_ = 40;
guessPose.map_point = finalPose.cast<double>();
guessPose.precision_type = MATCH_PRECISION_HIGH;
currMatcher_->MatchPointCloud(cloudInfo.frame, guessPose, finalGnssPose, finalPose);
mapPose_ = finalPose.cast<double>();
mapPose_.translation() = finalPose.translation().cast<double>();
return true;
}
......@@ -581,9 +593,10 @@ void AdjustPPK::LoadMap()
LOG(INFO) << "mesh exist: " << meshId;
}
if(LoadMesh(streamPath, meshInfo.matcher)){
LOG(INFO) << "mesh loaded: " << meshId;
}
meshInfo.streamPath = streamPath;
// if(LoadMesh(streamPath, meshInfo.matcher)){
// LOG(INFO) << "mesh loaded: " << meshId;
// }
}
// for(size_t id = period.startId; id <= period.endId; id += 100){
// if(requestQuit_){
......@@ -668,7 +681,7 @@ bool AdjustPPK::LoadMesh(const string &streamPath, boost::shared_ptr<VoxelMapMat
LOG(INFO) << "mesh to load: " << streamPath;
VoxelMapMatcherOption option;
option.option.filter_resolution = 1.2;
option.option.filter_resolution = 1.0;
option.option.cloud_range = 80;
option.fast_option.accepted_score = 0.3;
option.fast_option.accepted_low_score = 0.4;
......
......@@ -45,6 +45,7 @@ struct PCDPathInfo{
struct MashMap{
vector<PCDPathInfo> pcdPathVec;
boost::shared_ptr<VoxelMapMatcher> matcher;
string streamPath;
};
class AdjustPPK
......
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