i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 10:00:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Lidar low complexity update. util3d::computeNormalsComplexity(): added optional transform (used in RegistrationIcp to get normal vectors in right coordinate frame). RegistrationIcp: Fixed PointToPoint ICP not used with PM when recomputing transform from low complexity. Added more info in warning messages when low complexity happens. OdometryF2M: avoid adding key frame when scan has low complexity. Same for the first frame to init the local scan map.

Browse Source
This commit is contained in:
matlabbe
2020-03-28 16:25:27 -04:00
parent 5122d9ac02
commit 06caa328a4
6 changed files with 205 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

87
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -573,7 +573,14 @@ Transform OdometryF2M::computeTransform(
visKeyFrameThr_ == 0 ||
float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
regInfo.inliers <= visKeyFrameThr_);
bool addGeometricKeyFrame = regPipeline_->isScanRequired() && (scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);
float minComplexity = Parameters::defaultIcpPointToPlaneMinComplexity();
bool p2n = Parameters::defaultIcpPointToPlane();
Parameters::parse(parameters_, Parameters::kIcpPointToPlane(), p2n);
Parameters::parse(parameters_, Parameters::kIcpPointToPlaneMinComplexity(), minComplexity);
bool addGeometricKeyFrame =
regPipeline_->isScanRequired() &&
(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_) &&
(addVisualKeyFrame || !p2n || regInfo.icpStructuralComplexity>=minComplexity);
addKeyFrame = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f;
addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
@@ -1260,37 +1267,65 @@ Transform OdometryF2M::computeTransform(
{
if (lastFrame_->sensorData().laserScanRaw().size())
{
frameValid = true;
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
if (scanMapMaxRange_ > 0 ){
UINFO("Local map will be updated using range instead of time with range threshold set at %f", scanMapMaxRange_);
} else {
scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
}
if(lastFrame_->sensorData().laserScanRaw().is2d())
double complexity = 0.0;;
if(!frameValid)
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
map_->sensorData().setLaserScan(
LaserScan(
util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
0,
0.0f,
LaserScan::kXYNormal,
Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
float minComplexity = Parameters::defaultIcpPointToPlaneMinComplexity();
bool p2n = Parameters::defaultIcpPointToPlane();
Parameters::parse(parameters_, Parameters::kIcpPointToPlane(), p2n);
Parameters::parse(parameters_, Parameters::kIcpPointToPlaneMinComplexity(), minComplexity);
if(p2n && minComplexity>0.0f)
{
complexity = util3d::computeNormalsComplexity(*mapCloudNormals);
if(complexity > minComplexity)
{
frameValid = true;
}
}
else
{
frameValid = true;
}
}
if(frameValid)
{
if (scanMapMaxRange_ > 0 ){
UINFO("Local map will be updated using range instead of time with range threshold set at %f", scanMapMaxRange_);
} else {
scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
}
if(lastFrame_->sensorData().laserScanRaw().is2d())
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
map_->sensorData().setLaserScan(
LaserScan(
util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
0,
0.0f,
LaserScan::kXYNormal,
Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
}
else
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
map_->sensorData().setLaserScan(
LaserScan(
util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
0,
0.0f,
LaserScan::kXYZNormal,
newFramePose.translation()));
}
addKeyFrame = true;
}
else
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
map_->sensorData().setLaserScan(
LaserScan(
util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
0,
0.0f,
LaserScan::kXYZNormal,
newFramePose.translation()));
UWARN("Scan complexity too low (%f) to init first keyframe.", complexity);
}
addKeyFrame = true;
}
else
{