mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 17:40:23 +08:00
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.
This commit is contained in:
@@ -81,6 +81,7 @@ public:
|
|||||||
output.transform = transform;
|
output.transform = transform;
|
||||||
output.transformFiltered = transformFiltered;
|
output.transformFiltered = transformFiltered;
|
||||||
output.transformGroundTruth = transformGroundTruth;
|
output.transformGroundTruth = transformGroundTruth;
|
||||||
|
output.guessVelocity = guessVelocity;
|
||||||
output.distanceTravelled = distanceTravelled;
|
output.distanceTravelled = distanceTravelled;
|
||||||
output.memoryUsage = memoryUsage;
|
output.memoryUsage = memoryUsage;
|
||||||
output.type = type;
|
output.type = type;
|
||||||
|
|||||||
@@ -339,20 +339,24 @@ pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
|
|||||||
|
|
||||||
float RTABMAP_EXP computeNormalsComplexity(
|
float RTABMAP_EXP computeNormalsComplexity(
|
||||||
const LaserScan & scan,
|
const LaserScan & scan,
|
||||||
|
const Transform & t = Transform::getIdentity(),
|
||||||
cv::Mat * pcaEigenVectors = 0,
|
cv::Mat * pcaEigenVectors = 0,
|
||||||
cv::Mat * pcaEigenValues = 0);
|
cv::Mat * pcaEigenValues = 0);
|
||||||
float RTABMAP_EXP computeNormalsComplexity(
|
float RTABMAP_EXP computeNormalsComplexity(
|
||||||
const pcl::PointCloud<pcl::Normal> & normals,
|
const pcl::PointCloud<pcl::Normal> & normals,
|
||||||
|
const Transform & t = Transform::getIdentity(),
|
||||||
bool is2d = false,
|
bool is2d = false,
|
||||||
cv::Mat * pcaEigenVectors = 0,
|
cv::Mat * pcaEigenVectors = 0,
|
||||||
cv::Mat * pcaEigenValues = 0);
|
cv::Mat * pcaEigenValues = 0);
|
||||||
float RTABMAP_EXP computeNormalsComplexity(
|
float RTABMAP_EXP computeNormalsComplexity(
|
||||||
const pcl::PointCloud<pcl::PointNormal> & cloud,
|
const pcl::PointCloud<pcl::PointNormal> & cloud,
|
||||||
|
const Transform & t = Transform::getIdentity(),
|
||||||
bool is2d = false,
|
bool is2d = false,
|
||||||
cv::Mat * pcaEigenVectors = 0,
|
cv::Mat * pcaEigenVectors = 0,
|
||||||
cv::Mat * pcaEigenValues = 0);
|
cv::Mat * pcaEigenValues = 0);
|
||||||
float RTABMAP_EXP computeNormalsComplexity(
|
float RTABMAP_EXP computeNormalsComplexity(
|
||||||
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
|
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
|
||||||
|
const Transform & t = Transform::getIdentity(),
|
||||||
bool is2d = false,
|
bool is2d = false,
|
||||||
cv::Mat * pcaEigenVectors = 0,
|
cv::Mat * pcaEigenVectors = 0,
|
||||||
cv::Mat * pcaEigenValues = 0);
|
cv::Mat * pcaEigenValues = 0);
|
||||||
|
|||||||
@@ -600,15 +600,34 @@ Transform RegistrationIcp::computeTransformationImpl(
|
|||||||
//special case if we have already normals computed and there is no filtering
|
//special case if we have already normals computed and there is no filtering
|
||||||
|
|
||||||
cv::Mat complexityVectorsFrom, complexityVectorsTo;
|
cv::Mat complexityVectorsFrom, complexityVectorsTo;
|
||||||
double fromComplexity = util3d::computeNormalsComplexity(fromScan, &complexityVectorsFrom);
|
cv::Mat complexityValuesFrom, complexityValuesTo;
|
||||||
double toComplexity = util3d::computeNormalsComplexity(toScan, &complexityVectorsTo);
|
double fromComplexity = util3d::computeNormalsComplexity(fromScan, Transform::getIdentity(), &complexityVectorsFrom, &complexityValuesFrom);
|
||||||
|
double toComplexity = util3d::computeNormalsComplexity(toScan, guess, &complexityVectorsTo, &complexityValuesTo);
|
||||||
float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
|
float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
|
||||||
info.icpStructuralComplexity = complexity;
|
info.icpStructuralComplexity = complexity;
|
||||||
if(complexity < _pointToPlaneMinComplexity)
|
if(complexity < _pointToPlaneMinComplexity)
|
||||||
{
|
{
|
||||||
tooLowComplexityForPlaneToPlane = true;
|
tooLowComplexityForPlaneToPlane = true;
|
||||||
complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
|
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
|
else
|
||||||
{
|
{
|
||||||
@@ -773,15 +792,33 @@ Transform RegistrationIcp::computeTransformationImpl(
|
|||||||
}
|
}
|
||||||
|
|
||||||
cv::Mat complexityVectorsFrom, complexityVectorsTo;
|
cv::Mat complexityVectorsFrom, complexityVectorsTo;
|
||||||
double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.is2d(), &complexityVectorsFrom);
|
cv::Mat complexityValuesFrom, complexityValuesTo;
|
||||||
double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.is2d(), &complexityVectorsTo);
|
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;
|
float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
|
||||||
info.icpStructuralComplexity = complexity;
|
info.icpStructuralComplexity = complexity;
|
||||||
if(complexity < _pointToPlaneMinComplexity)
|
if(complexity < _pointToPlaneMinComplexity)
|
||||||
{
|
{
|
||||||
tooLowComplexityForPlaneToPlane = true;
|
tooLowComplexityForPlaneToPlane = true;
|
||||||
complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
|
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
|
else
|
||||||
{
|
{
|
||||||
@@ -976,7 +1013,19 @@ Transform RegistrationIcp::computeTransformationImpl(
|
|||||||
if(_pointToPlane)
|
if(_pointToPlane)
|
||||||
{
|
{
|
||||||
// temporary set PointToPointErrorMinimizer
|
// 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
|
#if POINTMATCHER_VERSION_INT >= 10300
|
||||||
icpTmp.errorMinimizer = PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer");
|
icpTmp.errorMinimizer = PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer");
|
||||||
#else
|
#else
|
||||||
@@ -1043,7 +1092,10 @@ Transform RegistrationIcp::computeTransformationImpl(
|
|||||||
// limit translation in direction of the first eigen vector
|
// limit translation in direction of the first eigen vector
|
||||||
Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
|
Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
|
||||||
float a = v.dot(n);
|
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)
|
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));
|
Eigen::Vector3f n2(complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2));
|
||||||
float a = v.dot(n1);
|
float a = v.dot(n1);
|
||||||
float b = v.dot(n2);
|
float b = v.dot(n2);
|
||||||
v = n1*a;
|
Eigen::Vector3f vp = n1*a;
|
||||||
v += n2*b;
|
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
|
else
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -573,7 +573,14 @@ Transform OdometryF2M::computeTransform(
|
|||||||
visKeyFrameThr_ == 0 ||
|
visKeyFrameThr_ == 0 ||
|
||||||
float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
|
float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
|
||||||
regInfo.inliers <= visKeyFrameThr_);
|
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 = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f;
|
||||||
addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
|
addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
|
||||||
@@ -1260,37 +1267,65 @@ Transform OdometryF2M::computeTransform(
|
|||||||
{
|
{
|
||||||
if (lastFrame_->sensorData().laserScanRaw().size())
|
if (lastFrame_->sensorData().laserScanRaw().size())
|
||||||
{
|
{
|
||||||
frameValid = true;
|
|
||||||
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
|
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_);
|
double complexity = 0.0;;
|
||||||
} else {
|
if(!frameValid)
|
||||||
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);
|
float minComplexity = Parameters::defaultIcpPointToPlaneMinComplexity();
|
||||||
map_->sensorData().setLaserScan(
|
bool p2n = Parameters::defaultIcpPointToPlane();
|
||||||
LaserScan(
|
Parameters::parse(parameters_, Parameters::kIcpPointToPlane(), p2n);
|
||||||
util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
|
Parameters::parse(parameters_, Parameters::kIcpPointToPlaneMinComplexity(), minComplexity);
|
||||||
0,
|
if(p2n && minComplexity>0.0f)
|
||||||
0.0f,
|
{
|
||||||
LaserScan::kXYNormal,
|
complexity = util3d::computeNormalsComplexity(*mapCloudNormals);
|
||||||
Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
|
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
|
else
|
||||||
{
|
{
|
||||||
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
|
UWARN("Scan complexity too low (%f) to init first keyframe.", complexity);
|
||||||
map_->sensorData().setLaserScan(
|
|
||||||
LaserScan(
|
|
||||||
util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
|
|
||||||
0,
|
|
||||||
0.0f,
|
|
||||||
LaserScan::kXYZNormal,
|
|
||||||
newFramePose.translation()));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
addKeyFrame = true;
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -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;
|
gtsam::Values initialEstimate;
|
||||||
std::map<int, bool> isLandmarkWithRotation;
|
std::map<int, bool> isLandmarkWithRotation;
|
||||||
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||||
|
|||||||
@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|||||||
|
|
||||||
#include "rtabmap/core/util3d_surface.h"
|
#include "rtabmap/core/util3d_surface.h"
|
||||||
#include "rtabmap/core/util3d_filtering.h"
|
#include "rtabmap/core/util3d_filtering.h"
|
||||||
|
#include "rtabmap/core/util3d_transforms.h"
|
||||||
#include "rtabmap/core/util3d.h"
|
#include "rtabmap/core/util3d.h"
|
||||||
#include "rtabmap/core/util2d.h"
|
#include "rtabmap/core/util2d.h"
|
||||||
#include "rtabmap/core/Memory.h"
|
#include "rtabmap/core/Memory.h"
|
||||||
@@ -2902,6 +2903,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
|
|||||||
|
|
||||||
float computeNormalsComplexity(
|
float computeNormalsComplexity(
|
||||||
const LaserScan & scan,
|
const LaserScan & scan,
|
||||||
|
const Transform & t,
|
||||||
cv::Mat * pcaEigenVectors,
|
cv::Mat * pcaEigenVectors,
|
||||||
cv::Mat * pcaEigenValues)
|
cv::Mat * pcaEigenValues)
|
||||||
{
|
{
|
||||||
@@ -2913,6 +2915,13 @@ float computeNormalsComplexity(
|
|||||||
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
|
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
|
||||||
int oi = 0;
|
int oi = 0;
|
||||||
int nOffset = scan.getNormalsOffset();
|
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)
|
for (int i = 0; i < scan.size(); ++i)
|
||||||
{
|
{
|
||||||
const float * ptrScan = scan.data().ptr<float>(0, i);
|
const float * ptrScan = scan.data().ptr<float>(0, i);
|
||||||
@@ -2921,19 +2930,29 @@ float computeNormalsComplexity(
|
|||||||
{
|
{
|
||||||
if(uIsFinite(ptrScan[nOffset]) && uIsFinite(ptrScan[nOffset+1]))
|
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);
|
float * ptr = data_normals.ptr<float>(oi++, 0);
|
||||||
ptr[0] = ptrScan[nOffset];
|
ptr[0] = n.x;
|
||||||
ptr[1] = ptrScan[nOffset+1];
|
ptr[1] = n.y;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if(uIsFinite(ptrScan[nOffset]) && uIsFinite(ptrScan[nOffset+1]) && uIsFinite(ptrScan[nOffset+2]))
|
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);
|
float * ptr = data_normals.ptr<float>(oi++, 0);
|
||||||
ptr[0] = ptrScan[nOffset];
|
ptr[0] = n.x;
|
||||||
ptr[1] = ptrScan[nOffset+1];
|
ptr[1] = n.y;
|
||||||
ptr[2] = ptrScan[nOffset+2];
|
ptr[2] = n.z;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -2963,6 +2982,7 @@ float computeNormalsComplexity(
|
|||||||
|
|
||||||
float computeNormalsComplexity(
|
float computeNormalsComplexity(
|
||||||
const pcl::PointCloud<pcl::PointNormal> & cloud,
|
const pcl::PointCloud<pcl::PointNormal> & cloud,
|
||||||
|
const Transform & t,
|
||||||
bool is2d,
|
bool is2d,
|
||||||
cv::Mat * pcaEigenVectors,
|
cv::Mat * pcaEigenVectors,
|
||||||
cv::Mat * pcaEigenValues)
|
cv::Mat * pcaEigenValues)
|
||||||
@@ -2971,17 +2991,29 @@ float computeNormalsComplexity(
|
|||||||
int sz = static_cast<int>(cloud.size()*2);
|
int sz = static_cast<int>(cloud.size()*2);
|
||||||
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
|
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
|
||||||
int oi = 0;
|
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)
|
for (unsigned int i = 0; i < cloud.size(); ++i)
|
||||||
{
|
{
|
||||||
const pcl::PointNormal & pt = cloud.at(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))
|
if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
|
||||||
{
|
{
|
||||||
float * ptr = data_normals.ptr<float>(oi++, 0);
|
float * ptr = data_normals.ptr<float>(oi++, 0);
|
||||||
ptr[0] = pt.normal_x;
|
ptr[0] = n.x;
|
||||||
ptr[1] = pt.normal_y;
|
ptr[1] = n.y;
|
||||||
if(!is2d)
|
if(!is2d)
|
||||||
{
|
{
|
||||||
ptr[2] = pt.normal_z;
|
ptr[2] = n.z;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -3006,6 +3038,7 @@ float computeNormalsComplexity(
|
|||||||
|
|
||||||
float computeNormalsComplexity(
|
float computeNormalsComplexity(
|
||||||
const pcl::PointCloud<pcl::Normal> & normals,
|
const pcl::PointCloud<pcl::Normal> & normals,
|
||||||
|
const Transform & t,
|
||||||
bool is2d,
|
bool is2d,
|
||||||
cv::Mat * pcaEigenVectors,
|
cv::Mat * pcaEigenVectors,
|
||||||
cv::Mat * pcaEigenValues)
|
cv::Mat * pcaEigenValues)
|
||||||
@@ -3014,17 +3047,29 @@ float computeNormalsComplexity(
|
|||||||
int sz = static_cast<int>(normals.size()*2);
|
int sz = static_cast<int>(normals.size()*2);
|
||||||
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
|
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
|
||||||
int oi = 0;
|
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)
|
for (unsigned int i = 0; i < normals.size(); ++i)
|
||||||
{
|
{
|
||||||
const pcl::Normal & pt = normals.at(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))
|
if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
|
||||||
{
|
{
|
||||||
float * ptr = data_normals.ptr<float>(oi++, 0);
|
float * ptr = data_normals.ptr<float>(oi++, 0);
|
||||||
ptr[0] = pt.normal_x;
|
ptr[0] = n.x;
|
||||||
ptr[1] = pt.normal_y;
|
ptr[1] = n.y;
|
||||||
if(!is2d)
|
if(!is2d)
|
||||||
{
|
{
|
||||||
ptr[2] = pt.normal_z;
|
ptr[2] = n.z;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -3049,6 +3094,7 @@ float computeNormalsComplexity(
|
|||||||
|
|
||||||
float computeNormalsComplexity(
|
float computeNormalsComplexity(
|
||||||
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
|
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
|
||||||
|
const Transform & t,
|
||||||
bool is2d,
|
bool is2d,
|
||||||
cv::Mat * pcaEigenVectors,
|
cv::Mat * pcaEigenVectors,
|
||||||
cv::Mat * pcaEigenValues)
|
cv::Mat * pcaEigenValues)
|
||||||
@@ -3057,17 +3103,29 @@ float computeNormalsComplexity(
|
|||||||
int sz = static_cast<int>(cloud.size()*2);
|
int sz = static_cast<int>(cloud.size()*2);
|
||||||
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
|
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
|
||||||
int oi = 0;
|
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)
|
for (unsigned int i = 0; i < cloud.size(); ++i)
|
||||||
{
|
{
|
||||||
const pcl::PointXYZRGBNormal & pt = cloud.at(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))
|
if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
|
||||||
{
|
{
|
||||||
float * ptr = data_normals.ptr<float>(oi++, 0);
|
float * ptr = data_normals.ptr<float>(oi++, 0);
|
||||||
ptr[0] = pt.normal_x;
|
ptr[0] = n.x;
|
||||||
ptr[1] = pt.normal_y;
|
ptr[1] = n.y;
|
||||||
if(!is2d)
|
if(!is2d)
|
||||||
{
|
{
|
||||||
ptr[2] = pt.normal_z;
|
ptr[2] = n.z;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user