i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40: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

75
corelib/src/RegistrationIcp.cpp
View File

@@ -600,15 +600,34 @@ Transform RegistrationIcp::computeTransformationImpl(
//special case if we have already normals computed and there is no filtering
cv::Mat complexityVectorsFrom, complexityVectorsTo;
double fromComplexity = util3d::computeNormalsComplexity(fromScan, &complexityVectorsFrom);
double toComplexity = util3d::computeNormalsComplexity(toScan, &complexityVectorsTo);
cv::Mat complexityValuesFrom, complexityValuesTo;
double fromComplexity = util3d::computeNormalsComplexity(fromScan, Transform::getIdentity(), &complexityVectorsFrom, &complexityValuesFrom);
double toComplexity = util3d::computeNormalsComplexity(toScan, guess, &complexityVectorsTo, &complexityValuesTo);
float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
info.icpStructuralComplexity = complexity;
if(complexity < _pointToPlaneMinComplexity)
{
tooLowComplexityForPlaneToPlane = true;
complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): %f < %f (%s). PointToPoint is done instead, orientation is still optimized but translation will be limited to direction of normals.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
(complexityVectors.rows == 3 && complexityVectors.cols == 3));
UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). "
"PointToPoint is done instead, orientation is still optimized but translation will be limited to "
"direction of normals (%s: %s).",
fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(),
fromComplexity<toComplexity?"From":"To",
complexityVectors.rows==2?
uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).c_str():
uFormat("n1=%f,%f,%f n2=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2), complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2)).c_str());
if(ULogger::level() == ULogger::kDebug)
{
std::cout << "complexityVectorsFrom = " << std::endl << complexityVectorsFrom << std::endl;
std::cout << "complexityValuesFrom = " << std::endl << complexityValuesFrom << std::endl;
std::cout << "complexityVectorsTo = " << std::endl << complexityVectorsTo << std::endl;
std::cout << "complexityValuesTo = " << std::endl << complexityValuesTo << std::endl;
}
}
else
{
@@ -773,15 +792,33 @@ Transform RegistrationIcp::computeTransformationImpl(
}
cv::Mat complexityVectorsFrom, complexityVectorsTo;
double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.is2d(), &complexityVectorsFrom);
double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.is2d(), &complexityVectorsTo);
cv::Mat complexityValuesFrom, complexityValuesTo;
double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, Transform::getIdentity(), fromScan.is2d(), &complexityVectorsFrom, &complexityValuesFrom);
double toComplexity = util3d::computeNormalsComplexity(*normalsTo, Transform::getIdentity(), toScan.is2d(), &complexityVectorsTo, &complexityValuesTo);
float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
info.icpStructuralComplexity = complexity;
if(complexity < _pointToPlaneMinComplexity)
{
tooLowComplexityForPlaneToPlane = true;
complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): %f < %f (%s). PointToPoint is done instead, orientation is still optimized but translation will be limited to direction of normals.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
(complexityVectors.rows == 3 && complexityVectors.cols == 3));
UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). "
"PointToPoint is done instead, orientation is still optimized but translation will be limited to "
"direction of normals (%s: %s).",
fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(),
fromComplexity<toComplexity?"From":"To",
complexityVectors.rows==2?
uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).c_str():
uFormat("n1=%f,%f,%f n2=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2), complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2)).c_str());
if(ULogger::level() == ULogger::kDebug)
{
std::cout << "complexityVectorsFrom = " << std::endl << complexityVectorsFrom << std::endl;
std::cout << "complexityValuesFrom = " << std::endl << complexityValuesFrom << std::endl;
std::cout << "complexityVectorsTo = " << std::endl << complexityVectorsTo << std::endl;
std::cout << "complexityValuesTo = " << std::endl << complexityValuesTo << std::endl;
}
}
else
{
@@ -976,7 +1013,19 @@ Transform RegistrationIcp::computeTransformationImpl(
if(_pointToPlane)
{
// temporary set PointToPointErrorMinimizer
PM::ICP & icpTmp = icp;
PM::ICP icpTmp = icp;
PM::Parameters params;
params["maxDist"] = uNumber2Str(_voxelSize>0?_voxelSize:_maxCorrespondenceDistance/2.0f);
UWARN("libpointmatcher icp...temporary maxDist=%s (%s=%f, %s=%f)", params["maxDist"].c_str(), Parameters::kIcpMaxCorrespondenceDistance().c_str(), _maxCorrespondenceDistance, Parameters::kIcpVoxelSize().c_str(), _voxelSize);
params["knn"] = uNumber2Str(_libpointmatcherKnn);
params["epsilon"] = uNumber2Str(_libpointmatcherEpsilon);
#if POINTMATCHER_VERSION_INT >= 10300
icp->matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
#else
icpTmp.matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
#endif
#if POINTMATCHER_VERSION_INT >= 10300
icpTmp.errorMinimizer = PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer");
#else
@@ -1043,7 +1092,10 @@ Transform RegistrationIcp::computeTransformationImpl(
// limit translation in direction of the first eigen vector
Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
float a = v.dot(n);
v = n*a;
Eigen::Vector3f vp = n*a;
UWARN("Normals low complexity: Limiting translation from (%f,%f) to (%f,%f)",
v[0], v[1], vp[0], vp[1]);
v= vp;
}
else if(complexityVectors.rows == 3)
{
@@ -1052,8 +1104,11 @@ Transform RegistrationIcp::computeTransformationImpl(
Eigen::Vector3f n2(complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2));
float a = v.dot(n1);
float b = v.dot(n2);
v = n1*a;
v += n2*b;
Eigen::Vector3f vp = n1*a;
vp += n2*b;
UWARN("Normals low complexity: Limiting translation from (%f,%f,%f) to (%f,%f,%f)",
v[0], v[1], v[2], vp[0], vp[1], vp[2]);
v = vp;
}
else
{

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
{

3
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -152,7 +152,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
}
UDEBUG("fill poses to gtsam... rootId=%d", rootId);
UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d gpsPriorOnly=%d landmarksIgnored=%d)",
rootId, priorsIgnored()?1:0, gpsPriorOnly?1:0, landmarksIgnored()?1:0);
gtsam::Values initialEstimate;
std::map<int, bool> isLandmarkWithRotation;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)

86
corelib/src/util3d_surface.cpp
View File

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/Memory.h"
@@ -2902,6 +2903,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
float computeNormalsComplexity(
const LaserScan & scan,
const Transform & t,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
{
@@ -2913,6 +2915,13 @@ float computeNormalsComplexity(
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
int oi = 0;
int nOffset = scan.getNormalsOffset();
bool doTransform = false;
Transform tn;
if(!t.isIdentity() || !scan.localTransform().isIdentity())
{
tn = (t*scan.localTransform()).rotation();
doTransform = true;
}
for (int i = 0; i < scan.size(); ++i)
{
const float * ptrScan = scan.data().ptr<float>(0, i);
@@ -2921,19 +2930,29 @@ float computeNormalsComplexity(
{
if(uIsFinite(ptrScan[nOffset]) && uIsFinite(ptrScan[nOffset+1]))
{
cv::Point3f n(ptrScan[nOffset], ptrScan[nOffset+1], 0);
if(doTransform)
{
n = util3d::transformPoint(n, tn);
}
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = ptrScan[nOffset];
ptr[1] = ptrScan[nOffset+1];
ptr[0] = n.x;
ptr[1] = n.y;
}
}
else
{
if(uIsFinite(ptrScan[nOffset]) && uIsFinite(ptrScan[nOffset+1]) && uIsFinite(ptrScan[nOffset+2]))
{
cv::Point3f n(ptrScan[nOffset], ptrScan[nOffset+1], ptrScan[nOffset+2]);
if(doTransform)
{
n = util3d::transformPoint(n, tn);
}
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = ptrScan[nOffset];
ptr[1] = ptrScan[nOffset+1];
ptr[2] = ptrScan[nOffset+2];
ptr[0] = n.x;
ptr[1] = n.y;
ptr[2] = n.z;
}
}
}
@@ -2963,6 +2982,7 @@ float computeNormalsComplexity(
float computeNormalsComplexity(
const pcl::PointCloud<pcl::PointNormal> & cloud,
const Transform & t,
bool is2d,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
@@ -2971,17 +2991,29 @@ float computeNormalsComplexity(
int sz = static_cast<int>(cloud.size()*2);
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
int oi = 0;
bool doTransform = false;
Transform tn;
if(!t.isIdentity())
{
tn = t.rotation();
doTransform = true;
}
for (unsigned int i = 0; i < cloud.size(); ++i)
{
const pcl::PointNormal & pt = cloud.at(i);
cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
if(doTransform)
{
n = util3d::transformPoint(n, tn);
}
if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
{
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = pt.normal_x;
ptr[1] = pt.normal_y;
ptr[0] = n.x;
ptr[1] = n.y;
if(!is2d)
{
ptr[2] = pt.normal_z;
ptr[2] = n.z;
}
}
}
@@ -3006,6 +3038,7 @@ float computeNormalsComplexity(
float computeNormalsComplexity(
const pcl::PointCloud<pcl::Normal> & normals,
const Transform & t,
bool is2d,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
@@ -3014,17 +3047,29 @@ float computeNormalsComplexity(
int sz = static_cast<int>(normals.size()*2);
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
int oi = 0;
bool doTransform = false;
Transform tn;
if(!t.isIdentity())
{
tn = t.rotation();
doTransform = true;
}
for (unsigned int i = 0; i < normals.size(); ++i)
{
const pcl::Normal & pt = normals.at(i);
cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
if(doTransform)
{
n = util3d::transformPoint(n, tn);
}
if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
{
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = pt.normal_x;
ptr[1] = pt.normal_y;
ptr[0] = n.x;
ptr[1] = n.y;
if(!is2d)
{
ptr[2] = pt.normal_z;
ptr[2] = n.z;
}
}
}
@@ -3049,6 +3094,7 @@ float computeNormalsComplexity(
float computeNormalsComplexity(
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
const Transform & t,
bool is2d,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
@@ -3057,17 +3103,29 @@ float computeNormalsComplexity(
int sz = static_cast<int>(cloud.size()*2);
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
int oi = 0;
bool doTransform = false;
Transform tn;
if(!t.isIdentity())
{
tn = t.rotation();
doTransform = true;
}
for (unsigned int i = 0; i < cloud.size(); ++i)
{
const pcl::PointXYZRGBNormal & pt = cloud.at(i);
cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
if(doTransform)
{
n = util3d::transformPoint(n, tn);
}
if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
{
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = pt.normal_x;
ptr[1] = pt.normal_y;
ptr[0] = n.x;
ptr[1] = n.y;
if(!is2d)
{
ptr[2] = pt.normal_z;
ptr[2] = n.z;
}
}
}