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

0.11.2: First Google Tango release

Browse Source
This commit is contained in:
matlabbe
2016-02-15 19:35:24 -05:00
parent 8dc01c491b
commit c6a46a7238
415 changed files with 73300 additions and 3193 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -79,6 +79,16 @@ std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
int to,
bool checkBothWays = true);
//Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
const std::map<int, Transform> & poses,
const Transform & cameraPose,
float horizontalFOV = 45.0f, // in degrees, xfov = atan((image_width/2)/fx)*2
float verticalFOV = 45.0f, // in degrees, yfov = atan((image_height/2)/fy)*2
float nearClipPlaneDistance = 0.1f,
float farClipPlaneDistance = 100.0f,
bool negative = false);
/**
* Get only the the most recent or older poses in the defined radius.
* @param poses The poses
@@ -170,7 +180,6 @@ int RTABMAP_EXP findNearestNode(
* Get nodes near the query
* @param nodeId the query id
* @param nodes the nodes to search for
* @param maxNearestNeighbors Maximum nearest neighbor to get. 0 means all.
* @param radius radius to search for (m)
* @return the nodes with squared distance to query node.
*/
@@ -181,7 +190,8 @@ std::map<int, float> RTABMAP_EXP getNodesInRadius(
std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
int nodeId,
const std::map<int, Transform> & nodes,
float radius);
float radius,
float angle = 0.0f);
float RTABMAP_EXP computePathLength(
const std::vector<std::pair<int, Transform> > & path,

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

@@ -111,6 +111,7 @@ public:
int maxGraphDepth) const;
void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
void removeLink(int idA, int idB);
void removeRawData(int id);
//getters
const std::map<int, double> & getWorkingMem() const {return _workingMem;}
@@ -123,7 +124,6 @@ public:
std::map<int, Link> getLinks(int signatureId,
bool lookInDatabase = false) const;
std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true) const;
bool isRawDataKept() const {return _rawDataKept;}
bool isBinDataKept() const {return _binDataKept;}
float getSimilarityThreshold() const {return _similarityThreshold;}
std::map<int, int> getWeights() const;
@@ -226,8 +226,8 @@ private:
// parameters
ParametersMap parameters_;
float _similarityThreshold;
bool _rawDataKept;
bool _binDataKept;
bool _rawDescriptorsKept;
bool _saveDepth16Format;
bool _notLinkedNodesKeptInDb;
bool _incrementalMemory;

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

@@ -38,6 +38,8 @@ class RTABMAP_EXP OptimizerG2O : public Optimizer
{
public:
static bool available();
static bool isCSparseAvailable();
static bool isCholmodAvailable();
static bool saveGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
@@ -45,20 +47,19 @@ public:
bool useRobustConstraints = false);
public:
OptimizerG2O(
int iterations = Parameters::defaultOptimizerIterations(),
bool slam2d = Parameters::defaultOptimizerSlam2D(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
double epsilon = Parameters::defaultOptimizerEpsilon(),
bool robust = Parameters::defaultOptimizerRobust()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
OptimizerG2O(const ParametersMap & parameters) :
Optimizer(parameters) {}
Optimizer(parameters),
solver_(Parameters::defaultg2oSolver()),
optimizer_(Parameters::defaultg2oOptimizer())
{
parseParameters(parameters);
}
virtual ~OptimizerG2O() {}
virtual Type type() const {return kTypeG2O;}
virtual void parseParameters(const ParametersMap & parameters);
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
@@ -66,6 +67,10 @@ public:
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
private:
int solver_;
int optimizer_;
};
} /* namespace rtabmap */

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

@@ -177,7 +177,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Rtabmap, DetectionRate, float, 1.0, "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
RTABMAP_PARAM(Rtabmap, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf).");
RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes, bool, false, "Create intermediate nodes between loop closure detection. Only used when Rtabmap/DetectionRate>0.");
RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, Parameters::getDefaultWorkingDirectory(), "Working directory.");
RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, "", "Working directory.");
RTABMAP_PARAM(Rtabmap, MaxRetrieved, unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM, bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
RTABMAP_PARAM(Rtabmap, StatisticLogged, bool, false, "Logging enabled.");
@@ -192,6 +192,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, RehearsalSimilarity, float, 0.6, "Rehearsal similarity.");
RTABMAP_PARAM(Mem, ImageKept, bool, false, "Keep raw images in RAM.");
RTABMAP_PARAM(Mem, BinDataKept, bool, true, "Keep binary data in db.");
RTABMAP_PARAM(Mem, RawDescriptorsKept, bool, false, "Raw descriptors kept in memory.");
RTABMAP_PARAM(Mem, MapLabelsAdded, bool, true, "Create map labels. The first node of a map will be labelled as \"map#\" where # is the map ID.");
RTABMAP_PARAM(Mem, SaveDepth16Format, bool, true, "Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
RTABMAP_PARAM(Mem, NotLinkedNodesKept, bool, true, "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
@@ -210,14 +211,14 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, UseDepthAsMask, bool, false, "Use depth image as mask for features detection.");
// KeypointMemory (Keypoint-based)
RTABMAP_PARAM_COND(Kp, NNStrategy, int, RTABMAP_NONFREE, 1, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
RTABMAP_PARAM(Kp, NNStrategy, int, 1, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, "");
RTABMAP_PARAM(Kp, IncrementalFlann, bool, true, "When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size).");
RTABMAP_PARAM(Kp, MaxDepth, float, 0.0, "Filter extracted keypoints by depth (0=inf).");
RTABMAP_PARAM(Kp, MinDepth, float, 0.0, "Filter extracted keypoints by depth.");
RTABMAP_PARAM(Kp, MaxFeatures, int, 400, "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
RTABMAP_PARAM(Kp, BadSignRatio, float, 0.5, "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
RTABMAP_PARAM_COND(Kp, NndrRatio, float, RTABMAP_NONFREE, 0.8, 0.9, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
RTABMAP_PARAM(Kp, NndrRatio, float, 0.8, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
RTABMAP_PARAM_COND(Kp, DetectorStrategy, int, RTABMAP_NONFREE, 0, 2, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, true, "Use of the td-idf strategy to compute the likelihood.");
RTABMAP_PARAM(Kp, Parallelized, bool, true, "If the dictionary update and signature creation were parallelized.");
@@ -323,13 +324,16 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed, bool, true, "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
// Graph optimization
RTABMAP_PARAM(Optimizer, Strategy, int, 0, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.")
RTABMAP_PARAM_COND(Optimizer, Strategy, int, RTABMAP_GTSAM, 2, 0, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
RTABMAP_PARAM(Optimizer, Iterations, int, 100, "Optimization iterations.");
RTABMAP_PARAM(Optimizer, Slam2D, bool, false, "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
RTABMAP_PARAM(Optimizer, Epsilon, double, 0.0001, "Stop optimizing when the error improvement is less than this value.");
RTABMAP_PARAM(Optimizer, Robust, bool, true, "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
RTABMAP_PARAM(g2o, Solver, int, 0, "0=csparse 1=pcg 2=cholmod");
RTABMAP_PARAM(g2o, Optimizer, int, 0, "0=Levenberg 1=GaussNewton");
// Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Local Map 1=Frame-to-Frame");
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
@@ -479,9 +483,10 @@ public:
*/
static const ParametersMap & getBackwardCompatibilityMap();
static std::string createDefaultWorkingDirectory();
private:
Parameters();
static std::string getDefaultWorkingDirectory();
private:
static ParametersMap parameters_;

1
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -177,6 +177,7 @@ private:
float _loopRatio;
unsigned int _maxRetrieved;
unsigned int _maxLocalRetrieved;
bool _rawDataKept;
bool _statisticLogsBufferedInRAM;
bool _statisticLogged;
bool _statisticLoggedHeaders;

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

@@ -82,10 +82,14 @@ public:
void setDataBufferSize(unsigned int bufferSize);
void createIntermediateNodes(bool enabled);
// this will delete rtabmap object if set
void close(bool databaseSaved);
protected:
virtual void handleEvent(UEvent * anEvent);
private:
virtual void mainLoopBegin();
virtual void mainLoop();
virtual void mainLoopKill();
void process();

5
corelib/include/rtabmap/core/Signature.h
View File

@@ -107,6 +107,8 @@ public:
void setEnabled(bool enabled) {_enabled = enabled;}
const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
const std::multimap<int, cv::Mat> & getWordsDescriptors() const {return _wordsDescriptors;}
void setWordsDescriptors(const std::multimap<int, cv::Mat> & descriptors) {_wordsDescriptors = descriptors;}
//metric stuff
void setWords3(const std::multimap<int, cv::Point3f> & words3) {_words3 = words3;}
@@ -136,7 +138,8 @@ private:
// times in the signature, it will be 2 times in this list)
// Words match with the CvSeq keypoints and descriptors
std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
std::multimap<int, cv::Point3f> _words3; // word <id, keypoint> // in base_link frame (localTransform applied))
std::multimap<int, cv::Point3f> _words3; // word <id, point> // in base_link frame (localTransform applied))
std::multimap<int, cv::Mat> _wordsDescriptors;
std::map<int, int> _wordsChanged; // <oldId, newId>
bool _enabled;

1
corelib/include/rtabmap/core/Statistics.h
View File

@@ -80,6 +80,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Memory, Working_memory_size,);
RTABMAP_STATS(Memory, Short_time_memory_size,);
RTABMAP_STATS(Memory, Database_memory_used, MB);
RTABMAP_STATS(Memory, Signatures_removed,);
RTABMAP_STATS(Memory, Immunized_globally,);
RTABMAP_STATS(Memory, Immunized_locally,);

1
corelib/include/rtabmap/core/VWDictionary.h
View File

@@ -112,6 +112,7 @@ private:
std::string _dictionaryPath; // a pre-computed dictionary (.txt)
bool _newWordsComparedTogether;
int _lastWordId;
bool useDistanceL1_;
FlannIndex * _flannIndex;
cv::Mat _dataTree;
NNStrategy _strategy;

44
corelib/include/rtabmap/core/impl/util3d_mapping.hpp
View File

@@ -20,6 +20,7 @@ namespace util3d{
template<typename PointT>
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const typename pcl::IndicesPtr & indices,
pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles,
float normalRadiusSearch,
@@ -35,6 +36,7 @@ void segmentObstaclesFromGround(
// Find the ground
pcl::IndicesPtr flatSurfaces = normalFiltering(
cloud,
indices,
groundNormalAngle,
Eigen::Vector4f(0,0,1,0),
normalRadiusSearch*2.0f,
@@ -78,7 +80,7 @@ void segmentObstaclesFromGround(
if(ground->size() != cloud->size())
{
// Remove ground
pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices(cloud, ground);
pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, ground, true);
//Cluster remaining stuff (obstacles)
std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
@@ -93,9 +95,32 @@ void segmentObstaclesFromGround(
}
}
template<typename PointT>
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles,
float normalRadiusSearch,
float groundNormalAngle,
int minClusterSize,
bool segmentFlatObstacles)
{
pcl::IndicesPtr indices(new std::vector<int>);
segmentObstaclesFromGround(
cloud,
indices,
ground,
obstacles,
normalRadiusSearch,
groundNormalAngle,
minClusterSize,
segmentFlatObstacles);
}
template<typename PointT>
void occupancy2DFromCloud3D(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
cv::Mat & ground,
cv::Mat & obstacles,
float cellSize,
@@ -108,7 +133,9 @@ void occupancy2DFromCloud3D(
}
pcl::IndicesPtr groundIndices, obstaclesIndices;
segmentObstaclesFromGround<PointT>(cloud,
segmentObstaclesFromGround<PointT>(
cloud,
indices,
groundIndices,
obstaclesIndices,
cellSize,
@@ -159,6 +186,19 @@ void occupancy2DFromCloud3D(
}
}
template<typename PointT>
void occupancy2DFromCloud3D(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
cv::Mat & ground,
cv::Mat & obstacles,
float cellSize,
float groundNormalAngle,
int minClusterSize)
{
pcl::IndicesPtr indices(new std::vector<int>);
occupancy2DFromCloud3D<PointT>(cloud, indices, ground, obstacles, cellSize, groundNormalAngle, minClusterSize);
}
}
}

9
corelib/include/rtabmap/core/util2d.h
View File

@@ -105,9 +105,11 @@ float RTABMAP_EXP getDepth(
const cv::Mat & depthImage,
float x, float y,
bool smoothing,
float maxZError = 0.02f);
float maxZError = 0.02f,
bool estWithNeighborsIfNull = false);
cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
cv::Mat RTABMAP_EXP interpolate(const cv::Mat & image, int factor, float depthErrorRatio = 0.02f);
// Registration Depth to RGB (return registered depth image)
cv::Mat RTABMAP_EXP registerDepth(
@@ -116,6 +118,11 @@ cv::Mat RTABMAP_EXP registerDepth(
const cv::Mat & colorK,
const rtabmap::Transform & transform);
cv::Mat RTABMAP_EXP fillDepthHoles(
const cv::Mat & depth,
int maximumHoleSize = 1,
float errorRatio = 0.02f);
void RTABMAP_EXP fillRegisteredDepthHoles(
cv::Mat & depthRegistered,
bool vertical,

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

@@ -76,44 +76,73 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
const cv::Mat & imageDepth,
float cx, float cy,
float fx, float fy,
int decimation = 1);
int decimation = 1,
float maxDepth = 0.0f,
std::vector<int> * validIndices = 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDepth,
float cx, float cy,
float fx, float fy,
int decimation = 1);
int decimation = 1,
float maxDepth = 0.0f,
std::vector<int> * validIndices = 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDisparity(
const cv::Mat & imageDisparity,
const StereoCameraModel & model,
int decimation = 1);
int decimation = 1,
float maxDepth = 0.0f,
std::vector<int> * validIndices = 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDisparityRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDisparity,
const StereoCameraModel & model,
int decimation = 1);
int decimation = 1,
float maxDepth = 0.0f,
std::vector<int> * validIndices = 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
const cv::Mat & imageLeft,
const cv::Mat & imageRight,
const StereoCameraModel & model,
int decimation = 1);
int decimation = 1,
float maxDepth = 0.0f,
std::vector<int> * validIndices = 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
const SensorData & sensorData,
int decimation = 1,
float maxDepth = 0.0f,
float voxelSize = 0.0f,
int samples = 0);
int samples = 0,
std::vector<int> * validIndices = 0);
/**
* Create an RGB cloud from the images contained in SensorData. If "voxelSize" and
* "samples" are not set (0), the returned cloud is organized. Otherwise, all NaN
* points are removed and the cloud will be dense.
*
* Note that multiple RGB-D camera images will result in a dense cloud.
*
* @param sensorData, the sensor data.
* @param decimation, images are decimated by this factor before projecting points to 3D. The factor
* should be a factor of the image width and height.
* @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
* @param voxelSize, use a voxel grid filter with this size of voxel.
* @param samples, random sampling filtering target cloud size.
* @param validIndices, the indices of valid points in the cloud
* @return a RGB cloud.
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
const SensorData & sensorData,
int decimation = 1,
float maxDepth = 0.0f,
float voxelSize = 0.0f,
int samples = 0);
int samples = 0,
std::vector<int> * validIndices = 0);
pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
const cv::Mat & depthImage,

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

@@ -51,6 +51,22 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP downsample(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int step);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize);
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize);
@@ -105,6 +121,17 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
float max,
bool negative = false);
//Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
pcl::IndicesPtr RTABMAP_EXP frustumFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Transform & cameraPose,
float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
float verticalFOV, // in degrees, yfov = atan((image_height/2)/fy)*2
float nearClipPlaneDistance,
float farClipPlaneDistance,
bool negative = false);
//Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP frustumFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & cameraPose,
@@ -113,6 +140,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP frustumFiltering(
float nearClipPlaneDistance,
float farClipPlaneDistance,
bool negative = false);
//Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP frustumFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & cameraPose,
@@ -146,6 +174,10 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius);
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius);
/**
* @brief Wrapper of the pcl::RadiusOutlierRemoval class.
@@ -169,6 +201,11 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius);
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius);
/**
* For convenience.
@@ -177,7 +214,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
float radiusSearch,
int minNeighborsInRadius = 0);
int minNeighborsInRadius = 1);
/**
* Subtract a cloud from another one using radius filtering.
@@ -194,7 +231,7 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
float radiusSearch,
int minNeighborsInRadius = 0);
int minNeighborsInRadius = 1);
/**
* For convenience.
@@ -203,7 +240,8 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
float radiusSearch,
int minNeighborsInRadius = 0);
float maxAngle,
int minNeighborsInRadius = 1);
/**
* Subtract a cloud from another one using radius filtering.
@@ -220,7 +258,45 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
float radiusSearch,
int minNeighborsInRadius = 0);
float maxAngle = M_PI/4.0f,
int minNeighborsInRadius = 1);
/**
* Subtract a cloud from another one using radius filtering.
* @param cloud the input cloud.
* @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
* @param cloud the input cloud to subtract.
* @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
* @param radiusSearchRatio the ratio used to compute the radius at different distances (e.g., a ratio of 0.1 at 4 m results in a radius of 4 cm).
* @return the indices of the points satisfying the parameters.
*/
pcl::IndicesPtr RTABMAP_EXP subtractAdaptiveFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
float radiusSearchRatio = 0.01,
int minNeighborsInRadius = 1,
const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0));
/**
* Subtract a cloud from another one using radius filtering.
* @param cloud the input cloud.
* @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
* @param cloud the input cloud to subtract.
* @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
* @param radiusSearchRatio the ratio used to compute the radius at different distances (e.g., a ratio of 0.01 at 4 m results in a radius of 4 cm).
* @return the indices of the points satisfying the parameters.
*/
pcl::IndicesPtr RTABMAP_EXP subtractAdaptiveFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
float radiusSearchRatio = 0.01,
float maxAngle = M_PI/4.0f,
int minNeighborsInRadius = 1,
const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0));
/**
@@ -271,6 +347,13 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint);
pcl::IndicesPtr RTABMAP_EXP normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint);
/**
* For convenience.
@@ -314,13 +397,42 @@ std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
int minClusterSize,
int maxClusterSize = std::numeric_limits<int>::max(),
int * biggestClusterIndex = 0);
std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float clusterTolerance,
int minClusterSize,
int maxClusterSize = std::numeric_limits<int>::max(),
int * biggestClusterIndex = 0);
pcl::IndicesPtr RTABMAP_EXP extractNegativeIndices(
pcl::IndicesPtr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices);
pcl::IndicesPtr RTABMAP_EXP extractNegativeIndices(
const pcl::IndicesPtr & indices,
bool negative);
pcl::IndicesPtr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices);
const pcl::IndicesPtr & indices,
bool negative);
pcl::IndicesPtr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative,
bool keepOrganized);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative,
bool keepOrganized);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP extractIndices(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative,
bool keepOrganized);
} // namespace util3d
} // namespace rtabmap

19
corelib/include/rtabmap/core/util3d_mapping.h
View File

@@ -81,6 +81,16 @@ void RTABMAP_EXP projectCloudOnXYPlane(
// templated methods
template<typename PointT>
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles,
float normalRadiusSearch,
float groundNormalAngle,
int minClusterSize,
bool segmentFlatObstacles = false);
template<typename PointT>
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
pcl::IndicesPtr & ground,
@@ -98,6 +108,15 @@ void occupancy2DFromCloud3D(
float cellSize = 0.05f,
float groundNormalAngle = M_PI_4,
int minClusterSize = 20);
template<typename PointT>
void occupancy2DFromCloud3D(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
cv::Mat & ground,
cv::Mat & obstacles,
float cellSize = 0.05f,
float groundNormalAngle = M_PI_4,
int minClusterSize = 20);
} // namespace util3d
} // namespace rtabmap

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

@@ -34,8 +34,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/TextureMesh.h>
#include <pcl/pcl_base.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/CameraModel.h>
#include <set>
namespace rtabmap
{
@@ -43,6 +45,54 @@ namespace rtabmap
namespace util3d
{
/**
* @brief Given a set of polygons, create two indexes: polygons to neighbor polygons and vertices to polygons.
*
* @param polygons the polygons to be indexed.
* @param cloudSize the size of the cloud of the corresponding mesh to polygons (must be at least as high as
* the highest vertex value contained in the polygons).
* @param neighborPolygons returned index from polygons to neighbor polygons (index size = polygons size).
* @param vertexPolygons returned index from vertices to polygons (index size = cloudSize).
*/
void RTABMAP_EXP createPolygonIndexes(
const std::vector<pcl::Vertices> & polygons,
int cloudSize,
std::vector<std::set<int> > & neighborPolygons,
std::vector<std::set<int> > & vertexPolygons);
std::vector<pcl::Vertices> RTABMAP_EXP organizedFastMesh(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
double angleTolerance = M_PI/16,
bool quad=true,
int trianglePixelSize = 2);
std::vector<pcl::Vertices> RTABMAP_EXP organizedFastMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
double angleTolerance = M_PI/16,
bool quad=true,
int trianglePixelSize = 2);
void RTABMAP_EXP appendMesh(
pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudA,
std::vector<pcl::Vertices> & polygonsA,
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudB,
const std::vector<pcl::Vertices> & polygonsB);
void RTABMAP_EXP filterNotUsedVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
const std::vector<pcl::Vertices> & polygons,
pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud,
std::vector<pcl::Vertices> & outputPolygons);
std::vector<pcl::Vertices> RTABMAP_EXP filterCloseVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,
const std::vector<pcl::Vertices> & polygons,
float radius,
float angle,
bool keepLatestInRadius);
std::vector<pcl::Vertices> RTABMAP_EXP filterInvalidPolygons(
const std::vector<pcl::Vertices> & polygons);
pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloudWithNormals,
float gp3SearchRadius = 0.025,
@@ -63,10 +113,27 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int normalKSearch = 20);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int normalKSearch = 20);
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int normalKSearch = 20);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int normalKSearch = 20);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float maxDepthChangeFactor = 0.02f,
float normalSmoothingSize = 10.0f);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float maxDepthChangeFactor = 0.02f,
float normalSmoothingSize = 10.0f);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -78,6 +145,17 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
int pointDensity = 0, // RANDOM_UNIFORM_DENSITY
float dilationVoxelSize = 1.0f, // VOXEL_GRID_DILATION
int dilationIterations = 0); // VOXEL_GRID_DILATION
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float searchRadius = 0.0f,
int polygonialOrder = 2,
int upsamplingMethod = 0, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION
float upsamplingRadius = 0.0f, // SAMPLE_LOCAL_PLANE
float upsamplingStep = 0.0f, // SAMPLE_LOCAL_PLANE
int pointDensity = 0, // RANDOM_UNIFORM_DENSITY
float dilationVoxelSize = 1.0f, // VOXEL_GRID_DILATION
int dilationIterations = 0); // VOXEL_GRID_DILATION
void RTABMAP_EXP adjustNormalsToViewPoints(
const std::map<int, Transform> & poses,