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

Added new options to filter source laser scans.

Browse Source 
SensorData can support laser scans CV_32FC6 format (point cloud with normals). Refactored RegistrationICP and updated CloudViewer to show PointNormal data.
Fixed bug with stereo clouds deterioration if decimation is set (CameraModel::scale()).
This commit is contained in:
matlabbe
2016-01-08 19:15:49 -05:00
parent e7565db5d0
commit 75f85f6b2a
31 changed files with 866 additions and 371 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/CameraInfo.h
View File

@@ -42,6 +42,7 @@ public:
timeCapture(0.0f),
timeDisparity(0.0f),
timeMirroring(0.0f),
timeImageDecimation(0.0f),
timeScanFromDepth(0.0f)
{
}
@@ -52,6 +53,7 @@ public:
float timeCapture;
float timeDisparity;
float timeMirroring;
float timeImageDecimation;
float timeScanFromDepth;
};

4
corelib/include/rtabmap/core/CameraModel.h
View File

@@ -72,6 +72,8 @@ public:
virtual ~CameraModel() {}
void initRectificationMap();
bool isValid() const {return !K_.empty() &&
!D_.empty() &&
!R_.empty() &&
@@ -104,7 +106,7 @@ public:
bool load(const std::string & directory, const std::string & cameraName);
bool save(const std::string & directory) const;
void scale(double scale);
CameraModel scaled(double scale) const;
double horizontalFOV() const; // in degrees
double verticalFOV() const; // in degrees

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

@@ -79,11 +79,21 @@ public:
void setScanPath(
const std::string & dir,
int maxScanPts = 0,
int downsampleStep = 1,
float voxelSize = 0.0f,
int normalsK = 0, // compute normals if > 0
const Transform & localTransform=Transform::getIdentity())
{
_scanPath = dir;
_scanLocalTransform = localTransform;
_scanMaxPts = maxScanPts;
_scanDownsampleStep = downsampleStep;
_scanNormalsK = normalsK;
_scanVoxelSize = voxelSize;
if(_scanDownsampleStep>1)
{
_scanMaxPts /= _scanDownsampleStep;
}
}
void setGroundTruthPath(const std::string & filePath, int format = 0)
@@ -120,6 +130,9 @@ private:
std::string _scanPath;
Transform _scanLocalTransform;
int _scanMaxPts;
int _scanDownsampleStep;
float _scanVoxelSize;
int _scanNormalsK;
bool _filenamesAreTimestamps;
std::string timestampsPath_;

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

@@ -54,9 +54,22 @@ public:
void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
void setImageDecimation(int decimation) {_imageDecimation = decimation;}
void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
void setScanFromDepth(bool enabled, int decimation=4, float maxDepth=4.0f)
{_scanFromDepth = enabled; _scanDecimation=decimation; _scanMaxDepth = maxDepth;}
void setScanFromDepth(
bool enabled,
int decimation=4,
float maxDepth=4.0f,
float voxelSize = 0.0f,
int normalsK = 0)
{
_scanFromDepth = enabled;
_scanDecimation=decimation;
_scanMaxDepth = maxDepth;
_scanVoxelSize = voxelSize;
_scanNormalsK = normalsK;
}
//getters
bool isPaused() const {return !this->isRunning();}
@@ -73,10 +86,13 @@ private:
Camera * _camera;
bool _mirroring;
bool _colorOnly;
int _imageDecimation;
bool _stereoToDepth;
bool _scanFromDepth;
int _scanDecimation;
float _scanMaxDepth;
float _scanVoxelSize;
int _scanNormalsK;
StereoDense * _stereoDense;
};

2
corelib/include/rtabmap/core/Odometry.h
View File

@@ -61,7 +61,6 @@ public:
const Transform & getPose() const {return _pose;}
bool isInfoDataFilled() const {return _fillInfoData;}
const Transform & previousTransform() const {return previousTransform_;}
bool isVarianceFromInliersCount() const {return _varianceFromInliersCount;}
private:
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
@@ -80,7 +79,6 @@ private:
float _particleNoiseR;
float _particleLambdaR;
bool _fillInfoData;
bool _varianceFromInliersCount;
float _kalmanProcessNoise;
float _kalmanMeasurementNoise;
Transform _pose;

20
corelib/include/rtabmap/core/OdometryInfo.h
View File

@@ -37,21 +37,23 @@ class OdometryInfo
public:
OdometryInfo() :
lost(true),
matches(-1),
inliers(-1),
variance(-1),
features(-1),
localMapSize(-1),
timeEstimation(-1),
timeParticleFiltering(-1),
matches(0),
inliers(0),
icpInliersRatio(0.0f),
variance(0.0f),
features(0),
localMapSize(0),
timeEstimation(0.0f),
timeParticleFiltering(0.0f),
stamp(0),
interval(0),
distanceTravelled(0),
type(-1)
distanceTravelled(0.0f),
type(0)
{}
bool lost;
int matches;
int inliers;
int icpInliersRatio;
float variance;
int features;
int localMapSize;

12
corelib/include/rtabmap/core/RegistrationInfo.h
View File

@@ -17,18 +17,22 @@ public:
RegistrationInfo() :
variance(0),
inliers(0),
inliersRatio(0),
matches(0)
matches(0),
icpInliersRatio(0)
{
}
float variance;
std::string rejectedMsg;
// RegistrationVis
int inliers;
float inliersRatio;
std::vector<int> inliersIDs;
int matches;
std::vector<int> matchesIDs;
std::string rejectedMsg;
// RegistrationIcp
float icpInliersRatio;
};
}

8
corelib/include/rtabmap/core/SensorData.h
View File

@@ -72,7 +72,7 @@ public:
double stamp = 0.0,
const cv::Mat & userData = cv::Mat());
// RGB-D constructor + 2d laser scan
// RGB-D constructor + laser scan
SensorData(
const cv::Mat & laserScan,
int laserScanMaxPts,
@@ -93,7 +93,7 @@ public:
double stamp = 0.0,
const cv::Mat & userData = cv::Mat());
// Multi-cameras RGB-D constructor + 2d laser scan
// Multi-cameras RGB-D constructor + laser scan
SensorData(
const cv::Mat & laserScan,
int laserScanMaxPts,
@@ -114,7 +114,7 @@ public:
double stamp = 0.0,
const cv::Mat & userData = cv::Mat());
// Stereo constructor + 2d laser scan
// Stereo constructor + laser scan
SensorData(
const cv::Mat & laserScan,
int laserScanMaxPts,
@@ -206,7 +206,7 @@ private:
cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3
cv::Mat _depthOrRightRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
cv::Mat _laserScanRaw; // CV_32FC2
cv::Mat _laserScanRaw; // CV_32FC2 or CV_32FC3
std::vector<CameraModel> _cameraModels;
StereoCameraModel _stereoCameraModel;

14
corelib/include/rtabmap/core/util3d.h
View File

@@ -126,9 +126,14 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
// return CV_32FC3
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
// return CV_32FC6
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
// return CV_32FC2
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
// For laserScan of type CV_32FC2, z is set to null.
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
// For laserScan of type CV_32FC2 or CV_32FC3, normals are set to null.
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform = Transform());
cv::Point3f RTABMAP_EXP projectDisparityTo3D(
const cv::Point2f & pt,
@@ -185,6 +190,15 @@ void RTABMAP_EXP savePCDWords(
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadBINCloud(const std::string & fileName, int dim);
// Load *.pcd, *.ply or *.bin (KITTI format) with optional filtering.
// If normals are computed (normalsK>0), the returned scan type is CV_32FC6 instead of CV_32FC3
cv::Mat RTABMAP_EXP loadScan(
const std::string & path,
const Transform & transform = Transform::getIdentity(),
int downsampleStep = 1,
float voxelSize = 0.0f,
int normalsK = 0);
} // namespace util3d
} // namespace rtabmap

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

@@ -54,6 +54,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP downsample(
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize);
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
float voxelSize);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float voxelSize);

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

@@ -74,7 +74,8 @@ Transform RTABMAP_EXP icp(
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered);
pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
bool icp2D = false);
Transform RTABMAP_EXP icpPointToPlane(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
@@ -84,14 +85,6 @@ Transform RTABMAP_EXP icpPointToPlane(
bool & hasConverged,
pcl::PointCloud<pcl::PointNormal> & cloud_source_registered);
Transform RTABMAP_EXP icp2D(
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);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
const cv::Mat & depth,
float fx,