i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 01:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added parameter "IcpPointToPlaneMaxComplexity". Added util3d::computeNormalsComplexity(). OdomInfo has now RegistrationInfo field to avoid duplicating members.

Browse Source
This commit is contained in:
matlabbe
2017-09-11 13:17:53 -04:00
parent 380fc2cbde
commit aca005c287
25 changed files with 581 additions and 354 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/OdometryEvent.h
+11 -11
View File
@@ -41,7 +41,7 @@ class OdometryEvent : public UEvent
public:
OdometryEvent()
{
_info.covariance = cv::Mat::eye(6,6,CV_64FC1);
_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
}
OdometryEvent(
const SensorData & data,
@@ -51,17 +51,17 @@ public:
_pose(pose),
_info(info)
{
if(_info.covariance.empty())
if(_info.reg.covariance.empty())
{
_info.covariance = cv::Mat::eye(6,6,CV_64FC1);
_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
}
UASSERT(_info.covariance.cols == 6 && _info.covariance.rows == 6 && _info.covariance.type() == CV_64FC1);
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(0,0)) && _info.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(1,1)) && _info.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(2,2)) && _info.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(3,3)) && _info.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(4,4)) && _info.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(5,5)) && _info.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT(_info.reg.covariance.cols == 6 && _info.reg.covariance.rows == 6 && _info.reg.covariance.type() == CV_64FC1);
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(0,0)) && _info.reg.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(1,1)) && _info.reg.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(2,2)) && _info.reg.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(3,3)) && _info.reg.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(4,4)) && _info.reg.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(5,5)) && _info.reg.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
}
virtual ~OdometryEvent() {}
virtual std::string getClassName() const {return "OdometryEvent";}
@@ -69,7 +69,7 @@ public:
SensorData & data() {return _data;}
const SensorData & data() const {return _data;}
const Transform & pose() const {return _pose;}
const cv::Mat & covariance() const {return _info.covariance;}
const cv::Mat & covariance() const {return _info.reg.covariance;}
std::vector<float> velocity() const {
if(_info.interval>0.0)
{
corelib/include/rtabmap/core/OdometryInfo.h
+4 -14
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <map>
#include "rtabmap/core/Transform.h"
#include "rtabmap/core/RegistrationInfo.h"
#include <opencv2/features2d/features2d.hpp>
namespace rtabmap {
@@ -39,9 +40,6 @@ class OdometryInfo
public:
OdometryInfo() :
lost(true),
matches(0),
inliers(0),
icpInliersRatio(0.0f),
features(0),
localMapSize(0),
localScanMapSize(0),
@@ -62,10 +60,7 @@ public:
{
OdometryInfo output;
output.lost = lost;
output.matches = matches;
output.inliers = inliers;
output.icpInliersRatio = icpInliersRatio;
output.covariance = covariance.clone();
output.reg = reg.copyWithoutData();
output.features = features;
output.localMapSize = localMapSize;
output.localScanMapSize = localScanMapSize;
@@ -87,10 +82,7 @@ public:
}
bool lost;
int matches;
int inliers;
float icpInliersRatio;
cv::Mat covariance;
RegistrationInfo reg;
int features;
int localMapSize;
int localScanMapSize;
@@ -108,12 +100,10 @@ public:
Transform transformGroundTruth;
float distanceTravelled;
int type; // 0=F2M, 1=F2F
int type;
// F2M
std::multimap<int, cv::KeyPoint> words;
std::vector<int> wordMatches;
std::vector<int> wordInliers;
std::map<int, cv::Point3f> localMap;
cv::Mat localScanMap;
corelib/include/rtabmap/core/Parameters.h
+1
View File
@@ -513,6 +513,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Icp, PointToPlane, bool, false, "Use point to plane ICP.");
RTABMAP_PARAM(Icp, PointToPlaneK, int, 20, "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
RTABMAP_PARAM(Icp, PointToPlaneRadius, float, 0.0, "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.0, "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.");
// libpointmatcher
RTABMAP_PARAM(Icp, PM, bool, false, "Use libpointmatcher for ICP registration instead of PCL's implementation.");
corelib/include/rtabmap/core/RegistrationIcp.h
+1
View File
@@ -66,6 +66,7 @@ private:
bool _pointToPlane;
int _pointToPlaneK;
float _pointToPlaneRadius;
float _pointToPlaneMinComplexity;
bool _libpointmatcher;
std::string _libpointmatcherConfig;
float _libpointmatcherOutlierRatio;
corelib/include/rtabmap/core/RegistrationInfo.h
+18 -1
View File
@@ -39,10 +39,26 @@ public:
matches(0),
icpInliersRatio(0),
icpTranslation(0.0f),
icpRotation(0.0f)
icpRotation(0.0f),
icpStructuralComplexity(0.0f)
{
}
RegistrationInfo copyWithoutData() const
{
RegistrationInfo output;
output.covariance = covariance.clone();
output.rejectedMsg = rejectedMsg;
output.inliers = inliers;
output.matches = matches;
output.icpInliersRatio = icpInliersRatio;
output.icpTranslation = icpTranslation;
output.icpRotation = icpRotation;
output.icpStructuralComplexity = icpStructuralComplexity;
return output;
}
cv::Mat covariance;
std::string rejectedMsg;
@@ -56,6 +72,7 @@ public:
float icpInliersRatio;
float icpTranslation;
float icpRotation;
float icpStructuralComplexity;
};
}
corelib/include/rtabmap/core/Statistics.h
+4 -1
View File
@@ -77,7 +77,10 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(NeighborLinkRefining, Accepted,);
RTABMAP_STATS(NeighborLinkRefining, Inliers,);
RTABMAP_STATS(NeighborLinkRefining, Inliers_ratio,);
RTABMAP_STATS(NeighborLinkRefining, ICP_inliers_ratio,);
RTABMAP_STATS(NeighborLinkRefining, ICP_rotation, rad);
RTABMAP_STATS(NeighborLinkRefining, ICP_translation, m);
RTABMAP_STATS(NeighborLinkRefining, ICP_complexity,);
RTABMAP_STATS(NeighborLinkRefining, Variance,);
RTABMAP_STATS(NeighborLinkRefining, Pts,);
corelib/include/rtabmap/core/util3d_surface.h
+12
View File
@@ -262,6 +262,18 @@ pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
float normalSmoothingSize = 10.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
float RTABMAP_EXP computeNormalsComplexity(
const cv::Mat & scan);
float RTABMAP_EXP computeNormalsComplexity(
const pcl::PointCloud<pcl::Normal> & normals,
bool is2d = false);
float RTABMAP_EXP computeNormalsComplexity(
const pcl::PointCloud<pcl::PointNormal> & cloud,
bool is2d = false);
float RTABMAP_EXP computeNormalsComplexity(
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
bool is2d = false);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float searchRadius = 0.0f,