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

Increased version to 0.20.7. OdometryF2M: added support for intensity field, removed ignored key frames when there is low scan complexity. RegistrationIcp: added Icp/PMMatcherIntensity, Icp/PointToPlaneGroundNormalsUp and Icp/PointToPlaneLowComplexityStrategy parameters. Rtabmap: when graph optimized from end, increased optimization error before warning that resulting map correction is not identity (this could happen with GTSAM as the root is not perfectly fixed). CloudViewer: added coordinate frame scaling option, added rainbow colormap option for scan intensity. DBViewer: fixed local proximity merged scans not shown modified after refining those links, show intensity, fixed constraints view not updated after rejecting a link. MainWindow: added intesity support with odometry scans.

Browse Source
This commit is contained in:
matlabbe
2020-11-28 17:28:34 -05:00
parent 7859313beb
commit d733029565
24 changed files with 1032 additions and 237 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
corelib/include/rtabmap/core/CameraThread.h
View File

@@ -72,6 +72,15 @@ public:
void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap());
void disableIMUFiltering();
RTABMAP_DEPRECATED(void setScanParameters(
bool fromDepth,
int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
float rangeMin=0.0f,
float rangeMax=0.0f,
float voxelSize = 0.0f,
int normalsK = 0,
int normalsRadius = 0.0f,
bool forceGroundNormalsUp = false) , "Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.");
void setScanParameters(
bool fromDepth,
int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
@@ -80,17 +89,7 @@ public:
float voxelSize = 0.0f,
int normalsK = 0,
int normalsRadius = 0.0f,
bool forceGroundNormalsUp = false)
{
_scanFromDepth = fromDepth;
_scanDownsampleStep=downsampleStep;
_scanRangeMin = rangeMin;
_scanRangeMax = rangeMax;
_scanVoxelSize = voxelSize;
_scanNormalsK = normalsK;
_scanNormalsRadius = normalsRadius;
_scanForceGroundNormalsUp = forceGroundNormalsUp;
}
float groundNormalsUp = 0.0f);
void postUpdate(SensorData * data, CameraInfo * info = 0) const;
@@ -119,7 +118,7 @@ private:
float _scanVoxelSize;
int _scanNormalsK;
float _scanNormalsRadius;
bool _scanForceGroundNormalsUp;
float _scanForceGroundNormalsUp;
StereoDense * _stereoDense;
clams::DiscreteDepthDistortionModel * _distortionModel;
bool _bilateralFiltering;

13
corelib/include/rtabmap/core/Parameters.h
View File

@@ -643,13 +643,15 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Icp, Epsilon, float, 0, "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
RTABMAP_PARAM(Icp, CorrespondenceRatio, float, 0.1, "Ratio of matching correspondences to accept the transform.");
#ifdef RTABMAP_POINTMATCHER
RTABMAP_PARAM(Icp, PointToPlane, bool, true, "Use point to plane ICP.");
RTABMAP_PARAM(Icp, PointToPlane, bool, true, "Use point to plane ICP.");
#else
RTABMAP_PARAM(Icp, PointToPlane, bool, false, "Use point to plane ICP.");
RTABMAP_PARAM(Icp, PointToPlane, bool, false, "Use point to plane ICP.");
#endif
RTABMAP_PARAM(Icp, PointToPlaneK, int, 5, "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
RTABMAP_PARAM(Icp, PointToPlaneRadius, float, 1.0, "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.02, "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.");
RTABMAP_PARAM(Icp, PointToPlaneK, int, 5, "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
RTABMAP_PARAM(Icp, PointToPlaneRadius, float, 1.0, "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
RTABMAP_PARAM(Icp, PointToPlaneGroundNormalsUp, float, 0.0, "Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans.");
RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.02, uFormat("Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when %s=true.", kIcpPointToPlaneLowComplexityStrategy().c_str(), kIcpPointToPlane().c_str()));
RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept \"as is\" the transform computed by PointToPoint.", kIcpPointToPlaneMinComplexity().c_str()));
// libpointmatcher
#ifdef RTABMAP_POINTMATCHER
@@ -660,6 +662,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM_STR(Icp, PMConfig, "", uFormat("Configuration file (*.yaml) used by libpointmatcher. Note that data filters set for libpointmatcher are done after filtering done by rtabmap (i.e., %s, %s), so make sure to disable those in rtabmap if you want to use only those from libpointmatcher. Parameters %s, %s and %s are also ignored if configuration file is set.", kIcpVoxelSize().c_str(), kIcpDownsamplingStep().c_str(), kIcpIterations().c_str(), kIcpEpsilon().c_str(), kIcpMaxCorrespondenceDistance().c_str()).c_str());
RTABMAP_PARAM(Icp, PMMatcherKnn, int, 1, "KDTreeMatcher/knn: number of nearest neighbors to consider it the reference. For convenience when configuration file is not set.");
RTABMAP_PARAM(Icp, PMMatcherEpsilon, float, 0.0, "KDTreeMatcher/epsilon: approximation to use for the nearest-neighbor search. For convenience when configuration file is not set.");
RTABMAP_PARAM(Icp, PMMatcherIntensity, bool, false, uFormat("KDTreeMatcher: among nearest neighbors, keep only the one with the most similar intensity. This only work with %s>1.", kIcpPMMatcherKnn().c_str()));
RTABMAP_PARAM(Icp, PMOutlierRatio, float, 0.95, "TrimmedDistOutlierFilter/ratio: For convenience when configuration file is not set. For kinect-like point cloud, use 0.65.");
// Stereo disparity

3
corelib/include/rtabmap/core/RegistrationIcp.h
View File

@@ -69,11 +69,14 @@ private:
bool _pointToPlane;
int _pointToPlaneK;
float _pointToPlaneRadius;
float _pointToPlaneGroundNormalsUp;
float _pointToPlaneMinComplexity;
int _pointToPlaneLowComplexityStrategy;
bool _libpointmatcher;
std::string _libpointmatcherConfig;
int _libpointmatcherKnn;
float _libpointmatcherEpsilon;
bool _libpointmatcherIntensity;
float _libpointmatcherOutlierRatio;
void * _libpointmatcherICP;
};

2
corelib/include/rtabmap/core/odometry/OdometryF2M.h
View File

@@ -78,7 +78,7 @@ private:
Signature * map_;
Signature * lastFrame_;
int lastFrameOldestNewId_;
std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
bool initGravity_;
std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>

31
corelib/include/rtabmap/core/util3d_filtering.h
View File

@@ -56,7 +56,16 @@ LaserScan RTABMAP_EXP commonFiltering(
float voxelSize = 0.0f,
int normalK = 0,
float normalRadius = 0.0f,
bool forceGroundNormalsUp = false);
float groundNormalsUp = 0.0f);
RTABMAP_DEPRECATED(LaserScan RTABMAP_EXP commonFiltering(
const LaserScan & scan,
int downsamplingStep,
float rangeMin,
float rangeMax,
float voxelSize,
int normalK,
float normalRadius,
bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp=0.8, otherwise set groundNormalsUp=0.0.");
LaserScan RTABMAP_EXP rangeFiltering(
const LaserScan & scan,
@@ -288,6 +297,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cropBox(
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP cropBox(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const Eigen::Vector4f & min,
const Eigen::Vector4f & max,
const Transform & transform = Transform::getIdentity(),
bool negative = false);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP cropBox(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Eigen::Vector4f & min,
@@ -451,6 +466,12 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
/**
* For convenience.
*/
pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
float radiusSearch,
float maxAngle = M_PI/4.0f,
int minNeighborsInRadius = 1);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
@@ -467,6 +488,14 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
* @param radiusSearch the radius in meter.
* @return the indices of the points satisfying the parameters.
*/
pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
float radiusSearch,
float maxAngle = M_PI/4.0f,
int minNeighborsInRadius = 1);
pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,

31
corelib/include/rtabmap/core/util3d_registration.h
View File

@@ -66,12 +66,25 @@ void RTABMAP_EXP computeVarianceAndCorrespondences(
double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference
double & variance,
int & correspondencesOut);
void RTABMAP_EXP computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference
double & variance,
int & correspondencesOut);
void RTABMAP_EXP computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double & variance,
int & correspondencesOut);
void RTABMAP_EXP computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double & variance,
int & correspondencesOut);
Transform RTABMAP_EXP icp(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
@@ -82,6 +95,15 @@ Transform RTABMAP_EXP icp(
pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
float epsilon = 0.0f,
bool icp2D = false);
Transform RTABMAP_EXP icp(
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
float epsilon = 0.0f,
bool icp2D = false);
Transform RTABMAP_EXP icpPointToPlane(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
@@ -92,6 +114,15 @@ Transform RTABMAP_EXP icpPointToPlane(
pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
float epsilon = 0.0f,
bool icp2D = false);
Transform RTABMAP_EXP icpPointToPlane(
const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
float epsilon = 0.0f,
bool icp2D = false);
} // namespace util3d
} // namespace rtabmap

35
corelib/include/rtabmap/core/util3d_surface.h
View File

@@ -358,6 +358,12 @@ float RTABMAP_EXP computeNormalsComplexity(
bool is2d = false,
cv::Mat * pcaEigenVectors = 0,
cv::Mat * pcaEigenValues = 0);
float RTABMAP_EXP computeNormalsComplexity(
const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
const Transform & t = Transform::getIdentity(),
bool is2d = false,
cv::Mat * pcaEigenVectors = 0,
cv::Mat * pcaEigenValues = 0);
float RTABMAP_EXP computeNormalsComplexity(
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
const Transform & t = Transform::getIdentity(),
@@ -387,18 +393,39 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
float dilationVoxelSize = 1.0f, // VOXEL_GRID_DILATION
int dilationIterations = 0); // VOXEL_GRID_DILATION
LaserScan RTABMAP_EXP adjustNormalsToViewPoint(
RTABMAP_DEPRECATED(LaserScan RTABMAP_EXP adjustNormalsToViewPoint(
const LaserScan & scan,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp);
bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
LaserScan RTABMAP_EXP adjustNormalsToViewPoint(
const LaserScan & scan,
const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
float groundNormalsUp = 0.0f);
RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
void RTABMAP_EXP adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
bool forceGroundNormalsUp = false);
float groundNormalsUp = 0.0f);
RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
void RTABMAP_EXP adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
bool forceGroundNormalsUp = false);
float groundNormalsUp = 0.0f);
RTABMAP_DEPRECATED(void RTABMAP_EXP adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp), "Use version with groundNormalsUp as float. For forceGroundNormalsUp=true, set groundNormalsUp to 0.8f, otherwise set groundNormalsUp to 0.0f.");
void RTABMAP_EXP adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
float groundNormalsUp = 0.0f);
void RTABMAP_EXP adjustNormalsToViewPoints(
const std::map<int, Transform> & poses,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,

33
corelib/src/CameraThread.cpp
View File

@@ -129,6 +129,39 @@ void CameraThread::disableIMUFiltering()
_imuFilter = 0;
}
void CameraThread::setScanParameters(
bool fromDepth,
int downsampleStep,
float rangeMin,
float rangeMax,
float voxelSize,
int normalsK,
int normalsRadius,
bool forceGroundNormalsUp)
{
setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f);
}
void CameraThread::setScanParameters(
bool fromDepth,
int downsampleStep, // decimation of the depth image in case the scan is from depth image
float rangeMin,
float rangeMax,
float voxelSize,
int normalsK,
int normalsRadius,
float groundNormalsUp)
{
_scanFromDepth = fromDepth;
_scanDownsampleStep=downsampleStep;
_scanRangeMin = rangeMin;
_scanRangeMax = rangeMax;
_scanVoxelSize = voxelSize;
_scanNormalsK = normalsK;
_scanNormalsRadius = normalsRadius;
_scanForceGroundNormalsUp = groundNormalsUp;
}
void CameraThread::mainLoopBegin()
{
ULogger::registerCurrentThread("Camera");

7
corelib/src/Odometry.cpp
View File

@@ -308,8 +308,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
if(stereoModel_.isRectificationMapInitialized())
{
data.setImageRaw(stereoModel_.left().rectifyImage(data.imageRaw()));
data.setDepthOrRightRaw(stereoModel_.right().rectifyImage(data.rightRaw()));
data.setStereoImage(
stereoModel_.left().rectifyImage(data.imageRaw()),
stereoModel_.right().rectifyImage(data.rightRaw()),
stereoModel_,
false);
}
}
else

314
corelib/src/RegistrationIcp.cpp
View File

@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifdef RTABMAP_POINTMATCHER
#include <fstream>
#include "pointmatcher/PointMatcher.h"
#include "nabo/nabo.h"
typedef PointMatcher<float> PM;
typedef PM::DataPoints DP;
@@ -182,6 +183,7 @@ DP laserScanToDP(const rtabmap::LaserScan & scan)
descLabels.push_back(Label("intensity", 1));
}
// create cloud
DP cloud(featLabels, descLabels, scan.size());
cloud.getFeatureViewByName("pad").setConstant(1);
@@ -360,6 +362,110 @@ typename PointMatcher<T>::TransformationParameters eigenMatrixToDim(const typena
return out;
}
template<typename T>
struct KDTreeMatcherIntensity : public PointMatcher<T>::Matcher
{
typedef PointMatcherSupport::Parametrizable Parametrizable;
typedef PointMatcherSupport::Parametrizable P;
typedef Parametrizable::Parameters Parameters;
typedef Parametrizable::ParameterDoc ParameterDoc;
typedef Parametrizable::ParametersDoc ParametersDoc;
typedef typename Nabo::NearestNeighbourSearch<T> NNS;
typedef typename NNS::SearchType NNSearchType;
typedef typename PointMatcher<T>::DataPoints DataPoints;
typedef typename PointMatcher<T>::Matcher Matcher;
typedef typename PointMatcher<T>::Matches Matches;
typedef typename PointMatcher<T>::Matrix Matrix;
inline static const std::string description()
{
return "This matcher matches a point from the reading to its closest neighbors in the reference.";
}
inline static const ParametersDoc availableParameters()
{
return {
{"knn", "number of nearest neighbors to consider it the reference", "1", "1", "2147483647", &P::Comp<unsigned>},
{"epsilon", "approximation to use for the nearest-neighbor search", "0", "0", "inf", &P::Comp<T>},
{"searchType", "Nabo search type. 0: brute force, check distance to every point in the data (very slow), 1: kd-tree with linear heap, good for small knn (~up to 30) and 2: kd-tree with tree heap, good for large knn (~from 30)", "1", "0", "2", &P::Comp<unsigned>},
{"maxDist", "maximum distance to consider for neighbors", "inf", "0", "inf", &P::Comp<T>}
};
}
const int knn;
const T epsilon;
const NNSearchType searchType;
const T maxDist;
protected:
std::shared_ptr<NNS> featureNNS;
Matrix filteredReferenceIntensity;
public:
KDTreeMatcherIntensity(const Parameters& params = Parameters()) :
PointMatcher<T>::Matcher("KDTreeMatcherIntensity", KDTreeMatcherIntensity::availableParameters(), params),
knn(Parametrizable::get<int>("knn")),
epsilon(Parametrizable::get<T>("epsilon")),
searchType(NNSearchType(Parametrizable::get<int>("searchType"))),
maxDist(Parametrizable::get<T>("maxDist"))
{
UINFO("* KDTreeMatcherIntensity: initialized with knn=%d, epsilon=%f, searchType=%d and maxDist=%f", knn, epsilon, searchType, maxDist);
}
virtual ~KDTreeMatcherIntensity() {}
virtual void init(const DataPoints& filteredReference)
{
// build and populate NNS
if(knn>1)
{
filteredReferenceIntensity = filteredReference.getDescriptorCopyByName("intensity");
}
else
{
UWARN("KDTreeMatcherIntensity: knn is not over 1 (%d), intensity re-ordering will be ignored.", knn);
}
featureNNS.reset( NNS::create(filteredReference.features, filteredReference.features.rows() - 1, searchType, NNS::TOUCH_STATISTICS));
}
virtual PM::Matches findClosests(const DP& filteredReading)
{
const int pointsCount(filteredReading.features.cols());
Matches matches(
typename Matches::Dists(knn, pointsCount),
typename Matches::Ids(knn, pointsCount)
);
const BOOST_AUTO(filteredReadingIntensity, filteredReading.getDescriptorViewByName("intensity"));
static_assert(NNS::InvalidIndex == PM::Matches::InvalidId, "");
static_assert(NNS::InvalidValue == PM::Matches::InvalidDist, "");
this->visitCounter += featureNNS->knn(filteredReading.features, matches.ids, matches.dists, knn, epsilon, NNS::ALLOW_SELF_MATCH, maxDist);
if(knn > 1)
{
Matches matchesOrderedByIntensity(
typename Matches::Dists(1, pointsCount),
typename Matches::Ids(1, pointsCount)
);
#pragma omp parallel for
for (int i = 0; i < pointsCount; ++i)
{
float minDistance = std::numeric_limits<float>::max();
for(int k=0; k<knn && k<filteredReferenceIntensity.rows(); ++k)
{
float distIntensity = fabs(filteredReadingIntensity(0,i) - filteredReferenceIntensity(0, matches.ids.coeff(k, i)));
if(distIntensity < minDistance)
{
matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(k, i);
matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(k, i);
minDistance = distIntensity;
}
}
}
matches = matchesOrderedByIntensity;
}
return matches;
}
};
#endif
namespace rtabmap {
@@ -379,11 +485,14 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
_pointToPlane(Parameters::defaultIcpPointToPlane()),
_pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
_pointToPlaneGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
_pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
_pointToPlaneLowComplexityStrategy(Parameters::defaultIcpPointToPlaneLowComplexityStrategy()),
_libpointmatcher(Parameters::defaultIcpPM()),
_libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
_libpointmatcherKnn(Parameters::defaultIcpPMMatcherKnn()),
_libpointmatcherEpsilon(Parameters::defaultIcpPMMatcherEpsilon()),
_libpointmatcherIntensity(Parameters::defaultIcpPMMatcherIntensity()),
_libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
_libpointmatcherICP(0)
{
@@ -414,7 +523,10 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), _pointToPlaneGroundNormalsUp);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneLowComplexityStrategy(), _pointToPlaneLowComplexityStrategy);
UASSERT(_pointToPlaneGroundNormalsUp >= 0.0f && _pointToPlaneGroundNormalsUp <= 1.0f);
UASSERT(_pointToPlaneMinComplexity >= 0.0f && _pointToPlaneMinComplexity <= 1.0f);
Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher);
@@ -422,6 +534,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kIcpPMOutlierRatio(), _libpointmatcherOutlierRatio);
Parameters::parse(parameters, Parameters::kIcpPMMatcherKnn(), _libpointmatcherKnn);
Parameters::parse(parameters, Parameters::kIcpPMMatcherEpsilon(), _libpointmatcherEpsilon);
Parameters::parse(parameters, Parameters::kIcpPMMatcherIntensity(), _libpointmatcherIntensity);
#ifndef RTABMAP_POINTMATCHER
if(_libpointmatcher)
@@ -474,11 +587,26 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
params["maxDist"] = uNumber2Str(_maxCorrespondenceDistance);
params["knn"] = uNumber2Str(_libpointmatcherKnn);
params["epsilon"] = uNumber2Str(_libpointmatcherEpsilon);
if(_libpointmatcherIntensity)
{
PointMatcher<float> matcher;
typedef typename PointMatcherSupport::Registrar< PointMatcher<float>::Matcher >::template GenericClassDescriptor< KDTreeMatcherIntensity<float> > Desc;
matcher.MatcherRegistrar.reg("KDTreeMatcherIntensity", std::make_shared<Desc>() );
#if POINTMATCHER_VERSION_INT >= 10300
icp->matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
icp->matcher = matcher.MatcherRegistrar.create("KDTreeMatcherIntensity", params);
#else
icp->matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
icp->matcher.reset(matcher.MatcherRegistrar.create("KDTreeMatcherIntensity", params));
#endif
}
else
{
#if POINTMATCHER_VERSION_INT >= 10300
icp->matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
#else
icp->matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
#endif
}
params.clear();
params["ratio"] = uNumber2Str(_libpointmatcherOutlierRatio);
@@ -638,15 +766,15 @@ Transform RegistrationIcp::computeTransformationImpl(
}
else
{
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromScan.localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudINormal(fromScan, fromScan.localTransform());
pcl::PointCloud<pcl::PointXYZINormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudINormal(toScan, guess * toScan.localTransform());
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
if(fromCloudNormals->size() > 2 && toCloudNormals->size() > 2)
{
pcl::PCA<pcl::PointNormal> pca;
pcl::PCA<pcl::PointXYZINormal> pca;
pca.setInputCloud(fromCloudNormals);
Eigen::Vector3f valuesFrom = pca.getEigenValues();
pca.setInputCloud(toCloudNormals);
@@ -662,7 +790,7 @@ Transform RegistrationIcp::computeTransformationImpl(
}
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointXYZINormal>());
#ifdef RTABMAP_POINTMATCHER
if(_libpointmatcher)
{
@@ -727,13 +855,13 @@ Transform RegistrationIcp::computeTransformationImpl(
int maxLaserScansTo = toScan.maxPoints();
if(!transformComputed)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromScan.localTransform());
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toScan.localTransform());
pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloud = util3d::laserScanToPointCloudI(fromScan, fromScan.localTransform());
pcl::PointCloud<pcl::PointXYZI>::Ptr toCloud = util3d::laserScanToPointCloudI(toScan, guess * toScan.localTransform());
UDEBUG("Conversion time = %f s", timer.ticks());
if(fromCloud->size() > 2 && toCloud->size() > 2)
{
pcl::PCA<pcl::PointXYZ> pca;
pcl::PCA<pcl::PointXYZI> pca;
pca.setInputCloud(fromCloud);
Eigen::Vector3f valuesFrom = pca.getEigenValues();
pca.setInputCloud(toCloud);
@@ -746,10 +874,11 @@ Transform RegistrationIcp::computeTransformationImpl(
{
info.icpStructuralDistribution = sqrt(valuesTo[0]/toCloud->size());
}
UDEBUG("Computed icpStructuralDistribution %f s",timer.ticks());
}
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloudFiltered = fromCloud;
pcl::PointCloud<pcl::PointXYZI>::Ptr toCloudFiltered = toCloud;
if(_voxelSize > 0.0f)
{
float pointsBeforeFiltering = (float)fromCloudFiltered->size();
@@ -773,7 +902,7 @@ Transform RegistrationIcp::computeTransformationImpl(
timer.ticks());
}
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZI>());
if(_pointToPlane && // ICP Point To Plane
!tooLowComplexityForPlaneToPlane && // if previously rejected above
!((fromScan.is2d()|| toScan.is2d()) && !_libpointmatcher)) // PCL crashes if 2D
@@ -862,16 +991,31 @@ Transform RegistrationIcp::computeTransformationImpl(
}
else
{
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointXYZINormal>);
pcl::concatenateFields(*fromCloudFiltered, *normalsFrom, *fromCloudNormals);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointXYZINormal>);
pcl::concatenateFields(*toCloudFiltered, *normalsTo, *toCloudNormals);
std::vector<int> indices;
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
if(!fromCloudNormals->empty() && !fromScan.is2d() && _pointToPlaneGroundNormalsUp>0.0f)
{
util3d::adjustNormalsToViewPoint(fromCloudNormals,
Eigen::Vector3f(fromScan.localTransform().x(),fromScan.localTransform().y(),fromScan.localTransform().z()+10),
_pointToPlaneGroundNormalsUp);
}
if(!toCloudNormals->empty() && !toScan.is2d() && _pointToPlaneGroundNormalsUp>0.0f)
{
Transform toT = guess * toScan.localTransform();
Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z()+10);
util3d::adjustNormalsToViewPoint(toCloudNormals,
viewpointTo,
_pointToPlaneGroundNormalsUp);
}
// update output scans
if(fromScan.is2d())
{
@@ -880,7 +1024,7 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.rangeMax(),
LaserScan::kXYNormal,
LaserScan::kXYINormal,
fromScan.localTransform()));
}
else
@@ -890,7 +1034,7 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScanFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.rangeMax(),
LaserScan::kXYZNormal,
LaserScan::kXYZINormal,
fromScan.localTransform()));
}
if(toScan.is2d())
@@ -900,7 +1044,7 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.rangeMax(),
LaserScan::kXYNormal,
LaserScan::kXYINormal,
toScan.localTransform()));
}
else
@@ -910,7 +1054,7 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.rangeMax(),
LaserScan::kXYZNormal,
LaserScan::kXYZINormal,
toScan.localTransform()));
}
UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
@@ -919,7 +1063,7 @@ Transform RegistrationIcp::computeTransformationImpl(
if(toCloudNormals->size() && fromCloudNormals->size())
{
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointXYZINormal>());
#ifdef RTABMAP_POINTMATCHER
if(_libpointmatcher)
@@ -999,7 +1143,7 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.rangeMax(),
LaserScan::kXY,
LaserScan::kXYI,
fromScan.localTransform()));
}
else
@@ -1009,7 +1153,7 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScanFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.rangeMax(),
LaserScan::kXYZ,
LaserScan::kXYZI,
fromScan.localTransform()));
}
if(toScan.is2d())
@@ -1019,7 +1163,7 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.rangeMax(),
LaserScan::kXY,
LaserScan::kXYI,
toScan.localTransform()));
}
else
@@ -1029,7 +1173,7 @@ Transform RegistrationIcp::computeTransformationImpl(
util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.rangeMax(),
LaserScan::kXYZ,
LaserScan::kXYZI,
toScan.localTransform()));
}
fromScan = fromSignature.sensorData().laserScanRaw();
@@ -1122,73 +1266,86 @@ Transform RegistrationIcp::computeTransformationImpl(
if(!icpT.isNull() && hasConverged)
{
if(tooLowComplexityForPlaneToPlane)
if(tooLowComplexityForPlaneToPlane && _pointToPlaneLowComplexityStrategy<2)
{
Transform guessInv = guess.inverse();
Transform t = guessInv * icpT.inverse() * guess;
Eigen::Vector3f v(t.x(), t.y(), t.z());
if(complexityVectors.cols == 2)
if(_pointToPlaneLowComplexityStrategy == 0)
{
// 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);
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;
msg = uFormat("Rejecting transform because too low complexity (%s=0)", Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
icpT.setNull();
UWARN(msg.c_str());
}
else if(complexityVectors.rows == 3)
else //if(_pointToPlaneLowComplexityStrategy == 1)
{
// limit translation in direction of the first and second eigen vectors
Eigen::Vector3f n1(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,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 b = v.dot(n2);
Eigen::Vector3f vp = n1*a;
if(secondEigenValue >= _pointToPlaneMinComplexity)
Transform guessInv = guess.inverse();
Transform t = guessInv * icpT.inverse() * guess;
Eigen::Vector3f v(t.x(), t.y(), t.z());
if(complexityVectors.cols == 2)
{
vp += n2*b;
// 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);
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;
}
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
{
UWARN("not supposed to be here!");
v = Eigen::Vector3f(0,0,0);
}
float roll, pitch, yaw;
t.getEulerAngles(roll, pitch, yaw);
t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
icpT = guess * t.inverse() * guessInv;
else if(complexityVectors.rows == 3)
{
// limit translation in direction of the first and second eigen vectors
Eigen::Vector3f n1(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,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 b = v.dot(n2);
Eigen::Vector3f vp = n1*a;
if(secondEigenValue >= _pointToPlaneMinComplexity)
{
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
{
UWARN("not supposed to be here!");
v = Eigen::Vector3f(0,0,0);
}
float roll, pitch, yaw;
t.getEulerAngles(roll, pitch, yaw);
t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
icpT = guess * t.inverse() * guessInv;
if(fromScan.hasNormals() && toScan.hasNormals())
{
// we were using normals, so compute correspondences using normals
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
if(fromScan.hasNormals() && toScan.hasNormals())
{
// we were using normals, so compute correspondences using normals
pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudINormal(fromScan, icpT * fromScan.localTransform());
pcl::PointCloud<pcl::PointXYZINormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudINormal(toScan, guess * toScan.localTransform());
util3d::computeVarianceAndCorrespondences(
fromCloudNormalsRegistered,
toCloudNormals,
_maxCorrespondenceDistance,
_maxRotation,
variance,
correspondences);
}
else
{
util3d::computeVarianceAndCorrespondences(
fromCloudRegistered,
toCloudFiltered,
_maxCorrespondenceDistance,
variance,
correspondences);
util3d::computeVarianceAndCorrespondences(
fromCloudNormalsRegistered,
toCloudNormals,
_maxCorrespondenceDistance,
_maxRotation,
variance,
correspondences);
}
else
{
util3d::computeVarianceAndCorrespondences(
fromCloudRegistered,
toCloudFiltered,
_maxCorrespondenceDistance,
variance,
correspondences);
}
}
}
else
{
if(tooLowComplexityForPlaneToPlane)
{
UWARN("Even if complexity is low , PointToPoint transformation is accepted \"as is\" (%s=2)", Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
}
util3d::computeVarianceAndCorrespondences(
fromCloudRegistered,
toCloudFiltered,
@@ -1263,6 +1420,7 @@ Transform RegistrationIcp::computeTransformationImpl(
else
{
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
info.covariance(cv::Range(3,6),cv::Range(3,6))/=10.0; //orientation error
}
info.icpInliersRatio = correspondencesRatio;
info.icpCorrespondences = correspondences;

2
corelib/src/Rtabmap.cpp
View File

@@ -3098,7 +3098,7 @@ bool Rtabmap::process(
previousMapCorrection = _mapCorrection;
_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
if(_mapCorrection.getNormSquared() > 0.1f && _optimizeFromGraphEnd)
{
bool hasPrior = signature->hasLink(signature->id());
if(!_graphOptimizer->priorsIgnored())

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

@@ -591,14 +591,9 @@ Transform OdometryF2M::computeTransform(
visKeyFrameThr_ == 0 ||
float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
regInfo.inliers <= visKeyFrameThr_);
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);
bool addGeometricKeyFrame = regPipeline_->isScanRequired() &&
(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);
addKeyFrame = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f;
addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
@@ -955,14 +950,13 @@ Transform OdometryF2M::computeTransform(
if(lastFrame_->sensorData().laserScanRaw().size())
{
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan, tmpMap.sensorData().laserScanRaw().localTransform());
pcl::PointCloud<pcl::PointXYZINormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudINormal(mapScan, tmpMap.sensorData().laserScanRaw().localTransform());
Transform viewpoint = newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform();
pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals (new pcl::PointCloud<pcl::PointNormal>());
pcl::PointCloud<pcl::PointXYZINormal>::Ptr frameCloudNormals (new pcl::PointCloud<pcl::PointXYZINormal>());
if(scanMapMaxRange_ > 0)
{
frameCloudNormals = util3d::laserScanToPointCloudNormal(
lastFrame_->sensorData().laserScanRaw());
frameCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw());
frameCloudNormals = util3d::cropBox(frameCloudNormals,
Eigen::Vector4f(-scanMapMaxRange_ / 2, -scanMapMaxRange_ / 2,-scanMapMaxRange_ / 2, 0),
Eigen::Vector4f(scanMapMaxRange_ / 2,scanMapMaxRange_ / 2,scanMapMaxRange_ / 2, 0)
@@ -970,8 +964,7 @@ Transform OdometryF2M::computeTransform(
frameCloudNormals = util3d::transformPointCloud(frameCloudNormals, viewpoint);
} else
{
frameCloudNormals = util3d::laserScanToPointCloudNormal(
lastFrame_->sensorData().laserScanRaw(), viewpoint);
frameCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw(), viewpoint);
}
pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
@@ -998,7 +991,7 @@ Transform OdometryF2M::computeTransform(
if (scanMapMaxRange_ > 0) {
// Copying new points to tmp cloud
// These are the points that have no overlap between mapScan and lastFrame
pcl::PointCloud<pcl::PointNormal> tmp;
pcl::PointCloud<pcl::PointXYZINormal> tmp;
pcl::copyPointCloud(*frameCloudNormals, *frameCloudNormalsIndices, tmp);
if (int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_) // 20 000 points
@@ -1058,7 +1051,7 @@ Transform OdometryF2M::computeTransform(
{
if(scansBuffer_[i].second->size())
{
pcl::PointCloud<pcl::PointNormal> tmp;
pcl::PointCloud<pcl::PointXYZINormal> tmp;
pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
*mapCloudNormals += tmp;
}
@@ -1071,7 +1064,7 @@ Transform OdometryF2M::computeTransform(
// remove old clouds
if(i > 0)
{
std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
int oi = 0;
for(; i<(int)scansBuffer_.size(); ++i)
{
@@ -1086,7 +1079,7 @@ Transform OdometryF2M::computeTransform(
// just append the last cloud
if(scansBuffer_.back().second->size())
{
pcl::PointCloud<pcl::PointNormal> tmp;
pcl::PointCloud<pcl::PointXYZINormal> tmp;
pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
*mapCloudNormals += tmp;
}
@@ -1100,12 +1093,12 @@ Transform OdometryF2M::computeTransform(
if(mapScan.is2d())
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
mapScan = LaserScan(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYNormal);
mapScan = LaserScan(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYINormal);
}
else
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
mapScan = LaserScan(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYZNormal);
mapScan = LaserScan(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYZINormal);
}
modified=true;
}
@@ -1311,7 +1304,7 @@ Transform OdometryF2M::computeTransform(
{
if (lastFrame_->sensorData().laserScanRaw().size())
{
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
pcl::PointCloud<pcl::PointXYZINormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
double complexity = 0.0;;
if(!frameValid)
@@ -1357,7 +1350,7 @@ Transform OdometryF2M::computeTransform(
util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
0,
0.0f,
LaserScan::kXYNormal,
LaserScan::kXYINormal,
Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
}
else
@@ -1368,7 +1361,7 @@ Transform OdometryF2M::computeTransform(
util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
0,
0.0f,
LaserScan::kXYZNormal,
LaserScan::kXYZINormal,
newFramePose.translation()));
}

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

@@ -138,6 +138,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
UASSERT(uContains(poses, rootId));
const Transform & initialPose = poses.at(rootId);
UDEBUG("hasPriorPoses=%s, gpsPriorOnly=%s", hasPriorPoses?"true":"false", gpsPriorOnly?"true":"false");
if(isSlam2d())
{
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasPriorPoses?1e-2:std::numeric_limits<double>::min()));

51
corelib/src/util3d_filtering.cpp
View File

@@ -79,11 +79,11 @@ LaserScan commonFiltering(
float voxelSize,
int normalK,
float normalRadius,
bool forceGroundNormalsUp)
float groundNormalsUp)
{
LaserScan scan = scanIn;
UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f",
scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius);
UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f, groundNormalsUp=%f",
scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, groundNormalsUp);
if(!scan.isEmpty())
{
// combined downsampling and range filtering step
@@ -293,14 +293,27 @@ LaserScan commonFiltering(
}
}
if(scan.size() && !scan.is2d() && scan.hasNormals() && forceGroundNormalsUp)
if(scan.size() && !scan.is2d() && scan.hasNormals() && groundNormalsUp>0.0f)
{
scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,0), forceGroundNormalsUp);
scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,10), groundNormalsUp);
}
}
return scan;
}
LaserScan commonFiltering(
const LaserScan & scanIn,
int downsamplingStep,
float rangeMin,
float rangeMax,
float voxelSize,
int normalK,
float normalRadius,
bool forceGroundNormalsUp)
{
return commonFiltering(scanIn, downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, forceGroundNormalsUp?0.8f:0.0f);
}
LaserScan rangeFiltering(
const LaserScan & scan,
float rangeMin,
@@ -769,6 +782,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cropBox(const pcl::PointCloud<pcl::PointX
{
return cropBoxImpl<pcl::PointXYZRGB>(cloud, min, max, transform, negative);
}
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
{
return cropBoxImpl<pcl::PointXYZINormal>(cloud, min, max, transform, negative);
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
{
return cropBoxImpl<pcl::PointXYZRGBNormal>(cloud, min, max, transform, negative);
@@ -1083,6 +1100,19 @@ pcl::PointCloud<pcl::PointNormal>::Ptr subtractFiltering(
pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out;
}
pcl::PointCloud<pcl::PointXYZINormal>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
float radiusSearch,
float maxAngle,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZINormal>);
pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
@@ -1237,6 +1267,17 @@ pcl::IndicesPtr subtractFiltering(
{
return subtractFilteringImpl<pcl::PointNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
}
pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
float radiusSearch,
float maxAngle,
int minNeighborsInRadius)
{
return subtractFilteringImpl<pcl::PointXYZINormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
}
pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,

147
corelib/src/util3d_registration.cpp
View File

@@ -237,9 +237,10 @@ Transform transformFromXYZCorrespondences(
return Transform();
}
void computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
template<typename PointNormalT>
void computeVarianceAndCorrespondencesImpl(
const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloudA,
const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double maxCorrespondenceAngle,
double & variance,
@@ -247,10 +248,10 @@ void computeVarianceAndCorrespondences(
{
variance = 1;
correspondencesOut = 0;
pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & target = cloudA->size()>cloudB->size()?cloudA:cloudB;
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & source = cloudA->size()>cloudB->size()?cloudB:cloudA;
typename pcl::registration::CorrespondenceEstimation<PointNormalT, PointNormalT>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<PointNormalT, PointNormalT>);
const typename pcl::PointCloud<PointNormalT>::ConstPtr & target = cloudA->size()>cloudB->size()?cloudA:cloudB;
const typename pcl::PointCloud<PointNormalT>::ConstPtr & source = cloudA->size()>cloudB->size()?cloudB:cloudA;
est->setInputTarget(target);
est->setInputSource(source);
pcl::Correspondences correspondences;
@@ -299,16 +300,39 @@ void computeVarianceAndCorrespondences(
}
void computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double maxCorrespondenceAngle,
double & variance,
int & correspondencesOut)
{
computeVarianceAndCorrespondencesImpl<pcl::PointNormal>(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut);
}
void computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double maxCorrespondenceAngle,
double & variance,
int & correspondencesOut)
{
computeVarianceAndCorrespondencesImpl<pcl::PointXYZINormal>(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut);
}
template<typename PointT>
void computeVarianceAndCorrespondencesImpl(
const typename pcl::PointCloud<PointT>::ConstPtr & cloudA,
const typename pcl::PointCloud<PointT>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double & variance,
int & correspondencesOut)
{
variance = 1;
correspondencesOut = 0;
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
typename pcl::registration::CorrespondenceEstimation<PointT, PointT>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<PointT, PointT>);
est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
pcl::Correspondences correspondences;
@@ -331,25 +355,46 @@ void computeVarianceAndCorrespondences(
correspondencesOut = (int)correspondences.size();
}
void computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double & variance,
int & correspondencesOut)
{
computeVarianceAndCorrespondencesImpl<pcl::PointXYZ>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut);
}
void computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double & variance,
int & correspondencesOut)
{
computeVarianceAndCorrespondencesImpl<pcl::PointXYZI>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut);
}
// return transform from source to target (All points must be finite!!!)
Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
template<typename PointT>
Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_source,
const typename pcl::PointCloud<PointT>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
pcl::PointCloud<PointT> & cloud_source_registered,
float epsilon,
bool icp2D)
{
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
pcl::IterativeClosestPoint<PointT, PointT> icp;
// Set the input source and target
icp.setInputTarget (cloud_target);
icp.setInputSource (cloud_source);
if(icp2D)
{
pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>);
typename pcl::registration::TransformationEstimation2D<PointT, PointT>::Ptr est;
est.reset(new pcl::registration::TransformationEstimation2D<PointT, PointT>);
icp.setTransformationEstimation(est);
}
@@ -369,24 +414,51 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
return Transform::fromEigen4f(icp.getFinalTransformation());
}
// return transform from source to target (All points must be finite!!!)
Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
float epsilon,
bool icp2D)
{
return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
}
// return transform from source to target (All points must be finite!!!)
Transform icp(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
float epsilon,
bool icp2D)
{
return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
}
// return transform from source to target (All points/normals must be finite!!!)
Transform icpPointToPlane(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
template<typename PointNormalT>
Transform icpPointToPlaneImpl(
const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloud_source,
const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
pcl::PointCloud<PointNormalT> & cloud_source_registered,
float epsilon,
bool icp2D)
{
pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
pcl::IterativeClosestPoint<PointNormalT, PointNormalT> icp;
// Set the input source and target
icp.setInputTarget (cloud_target);
icp.setInputSource (cloud_source);
pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr est;
est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);
typename pcl::registration::TransformationEstimationPointToPlaneLLS<PointNormalT, PointNormalT>::Ptr est;
est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<PointNormalT, PointNormalT>);
icp.setTransformationEstimation(est);
// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
@@ -413,6 +485,33 @@ Transform icpPointToPlane(
return t;
}
// return transform from source to target (All points/normals must be finite!!!)
Transform icpPointToPlane(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
float epsilon,
bool icp2D)
{
return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
}
// return transform from source to target (All points/normals must be finite!!!)
Transform icpPointToPlane(
const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
float epsilon,
bool icp2D)
{
return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
}
}
}

130
corelib/src/util3d_surface.cpp
View File

@@ -3142,6 +3142,62 @@ float computeNormalsComplexity(
return 0.0f;
}
float computeNormalsComplexity(
const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
const Transform & t,
bool is2d,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
{
//Construct a buffer used by the pca analysis
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::PointXYZINormal & 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] = n.x;
ptr[1] = n.y;
if(!is2d)
{
ptr[2] = n.z;
}
}
}
if(oi>1)
{
cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
if(pcaEigenVectors)
{
*pcaEigenVectors = pca_analysis.eigenvectors;
}
if(pcaEigenValues)
{
*pcaEigenValues = pca_analysis.eigenvalues;
}
// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
}
return 0.0f;
}
float computeNormalsComplexity(
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
const Transform & t,
@@ -3301,6 +3357,13 @@ LaserScan adjustNormalsToViewPoint(
const LaserScan & scan,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp)
{
return adjustNormalsToViewPoint(scan, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
}
LaserScan adjustNormalsToViewPoint(
const LaserScan & scan,
const Eigen::Vector3f & viewpoint,
float groundNormalsUp)
{
if(scan.size() && !scan.is2d() && scan.hasNormals())
{
@@ -3308,6 +3371,7 @@ LaserScan adjustNormalsToViewPoint(
int ny = nx+1;
int nz = ny+1;
cv::Mat output = scan.data().clone();
#pragma omp parallel for
for(int i=0; i<scan.size(); ++i)
{
float * ptr = output.ptr<float>(0, i);
@@ -3318,7 +3382,7 @@ LaserScan adjustNormalsToViewPoint(
float result = v.dot(n);
if(result < 0
|| (forceGroundNormalsUp && ptr[nz] < -0.8 && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
|| (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
ptr[nx] *= -1.0f;
@@ -3339,10 +3403,11 @@ LaserScan adjustNormalsToViewPoint(
return scan;
}
void adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
template<typename PointNormalT>
void adjustNormalsToViewPointImpl(
typename pcl::PointCloud<PointNormalT>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp)
float groundNormalsUp)
{
for(unsigned int i=0; i<cloud->size(); ++i)
{
@@ -3354,7 +3419,7 @@ void adjustNormalsToViewPoint(
float result = v.dot(n);
if(result < 0
|| (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
|| (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
cloud->points[i].normal_x *= -1.0f;
@@ -3365,30 +3430,49 @@ void adjustNormalsToViewPoint(
}
}
void adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp)
{
adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
}
void adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
float groundNormalsUp)
{
adjustNormalsToViewPointImpl<pcl::PointNormal>(cloud, viewpoint, groundNormalsUp);
}
void adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp)
{
for(unsigned int i=0; i<cloud->size(); ++i)
{
pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
if(pcl::isFinite(normal))
{
Eigen::Vector3f v = viewpoint - cloud->points[i].getVector3fMap();
Eigen::Vector3f n(normal.x, normal.y, normal.z);
adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
}
void adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
float groundNormalsUp)
{
adjustNormalsToViewPointImpl<pcl::PointXYZRGBNormal>(cloud, viewpoint, groundNormalsUp);
}
float result = v.dot(n);
if(result < 0
|| (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
cloud->points[i].normal_x *= -1.0f;
cloud->points[i].normal_y *= -1.0f;
cloud->points[i].normal_z *= -1.0f;
}
}
}
void adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp)
{
adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
}
void adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
float groundNormalsUp)
{
adjustNormalsToViewPointImpl<pcl::PointXYZINormal>(cloud, viewpoint, groundNormalsUp);
}
void adjustNormalsToViewPoints(