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

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,

3
corelib/src/CMakeLists.txt
View File

@@ -158,9 +158,6 @@ IF(FlyCapture2_FOUND)
ENDIF(FlyCapture2_FOUND)
IF(G2O_FOUND)
IF(G2O_CHOMOLD_FOUND)
ADD_DEFINITIONS("-DWITH_G2O_CHOMOLD")
ENDIF(G2O_CHOMOLD_FOUND)
ADD_DEFINITIONS("-DWITH_G2O")
SET(INCLUDE_DIRS
${INCLUDE_DIRS}

1
corelib/src/Camera.cpp
View File

@@ -26,7 +26,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/DBDriver.h"
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UConversion.h>

170
corelib/src/DBDriverSqlite3.cpp
View File

@@ -436,14 +436,14 @@ void DBDriverSqlite3::executeNoResultQuery(const std::string & sql) const
long DBDriverSqlite3::getMemoryUsedQuery() const
{
if(_dbInMemory)
{
//if(_dbInMemory)
//{
return sqlite3_memory_used();
}
else
{
return UFile::length(this->getUrl());
}
//}
//else // Commented because it can lag
//{
// return UFile::length(this->getUrl());
//}
}
long DBDriverSqlite3::getImagesMemoryUsedQuery() const
@@ -841,7 +841,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
// multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(dataSize > 0 && data)
{
@@ -874,6 +874,26 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
dataFloat[4], // baseline
localTransform);
}
else if((unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(6+localTransform.size());
for(int i=0; i<max; i+=6+localTransform.size())
{
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
localTransform.prettyPrint().c_str());
}
}
else
{
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
@@ -1545,9 +1565,18 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
// Prepare the query... Get the map from signature and visual words
std::stringstream query2;
query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
if(uStrNumCmp(_version, "0.11.2") >= 0)
{
query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
else
{
query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
query2 << " ORDER BY word_id"; // Needed for fast insertion below
query2 << ";";
@@ -1563,9 +1592,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int visualWordId = 0;
int descriptorSize = 0;
const void * descriptor = 0;
int dRealSize = 0;
cv::KeyPoint kpt;
std::multimap<int, cv::KeyPoint> visualWords;
std::multimap<int, cv::Point3f> visualWords3;
std::multimap<int, cv::Mat> descriptors;
cv::Point3f depth(0,0,0);
// Process the result if one
@@ -1582,8 +1615,40 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
depth.x = sqlite3_column_double(ppStmt, index++);
depth.y = sqlite3_column_double(ppStmt, index++);
depth.z = sqlite3_column_double(ppStmt, index++);
visualWords.insert(visualWords.end(), std::make_pair(visualWordId, kpt));
visualWords3.insert(visualWords3.end(), std::make_pair(visualWordId, depth));
if(uStrNumCmp(_version, "0.11.2") >= 0)
{
descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
dRealSize = sqlite3_column_bytes(ppStmt, index++);
if(descriptor && descriptorSize>0 && dRealSize>0)
{
cv::Mat d;
if(dRealSize == descriptorSize)
{
// CV_8U binary descriptors
d = cv::Mat(1, descriptorSize, CV_8U);
}
else if(dRealSize/int(sizeof(float)) == descriptorSize)
{
// CV_32F
d = cv::Mat(1, descriptorSize, CV_32F);
}
else
{
UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
}
memcpy(d.data, descriptor, dRealSize);
descriptors.insert(descriptors.end(), std::make_pair(visualWordId, d));
}
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -1596,7 +1661,8 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
{
(*iter)->setWords(visualWords);
(*iter)->setWords3(visualWords3);
ULOGGER_DEBUG("Add %d keypoints and %d 3d points to node %d", visualWords.size(), visualWords3.size(), (*iter)->id());
(*iter)->setWordsDescriptors(descriptors);
ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), (int)visualWords3.size(), (int)descriptors.size(), (*iter)->id());
}
//reset
@@ -2338,22 +2404,29 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
UASSERT((*i)->getWords3().empty() || (*i)->getWords().size() == (*i)->getWords3().size());
if((*i)->getWords3().size())
UASSERT((*i)->getWordsDescriptors().empty() || (*i)->getWords().size() == (*i)->getWordsDescriptors().size());
std::multimap<int, cv::Point3f>::const_iterator p=(*i)->getWords3().begin();
std::multimap<int, cv::Mat>::const_iterator d=(*i)->getWordsDescriptors().begin();
for(std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
{
std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin();
std::multimap<int, cv::Point3f>::const_iterator p=(*i)->getWords3().begin();
for(; w!=(*i)->getWords().end(); ++w, ++p)
cv::Point3f pt(0,0,0);
if(p!=(*i)->getWords3().end())
{
UASSERT(w->first == p->first); // must be same id!
stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, p->second);
pt = p->second;
++p;
}
}
else
{
for(std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
cv::Mat descriptor;
if(d!=(*i)->getWordsDescriptors().end())
{
stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, cv::Point3f(0,0,0));
UASSERT(w->first == d->first); // must be same id!
descriptor = d->second;
++d;
}
stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, pt, descriptor);
}
}
// Finalize (delete) the statement
@@ -2840,19 +2913,21 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
// calibration
std::vector<float> calibration;
// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
// multi-cameras [fx,fy,cx,cy,width,height,local_transform, ... ,fx,fy,cx,cy,width,height,local_transform] (6+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(sensorData.cameraModels().size())
{
calibration.resize(sensorData.cameraModels().size() * (4+Transform().size()));
calibration.resize(sensorData.cameraModels().size() * (6+Transform().size()));
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
calibration[i*(4+localTransform.size())] = sensorData.cameraModels()[i].fx();
calibration[i*(4+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
calibration[i*(4+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
calibration[i*(4+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
calibration[i*(6+localTransform.size())] = sensorData.cameraModels()[i].fx();
calibration[i*(6+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
calibration[i*(6+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
calibration[i*(6+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
calibration[i*(6+localTransform.size())+4] = sensorData.cameraModels()[i].imageWidth();
calibration[i*(6+localTransform.size())+5] = sensorData.cameraModels()[i].imageHeight();
memcpy(calibration.data()+i*(6+localTransform.size())+6, localTransform.data(), localTransform.size()*sizeof(float));
}
}
else if(sensorData.stereoCameraModel().isValidForProjection())
@@ -3052,9 +3127,18 @@ void DBDriverSqlite3::stepWordsChanged(sqlite3_stmt * ppStmt, int nodeId, int ol
std::string DBDriverSqlite3::queryStepKeypoint() const
{
if(uStrNumCmp(_version, "0.11.2") >= 0)
{
return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor) VALUES(?,?,?,?,?,?,?,?,?,?,?,?);";
}
return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z) VALUES(?,?,?,?,?,?,?,?,?,?);";
}
void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt, int nodeId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt) const
void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
int nodeId,
int wordId,
const cv::KeyPoint & kp,
const cv::Point3f & pt,
const cv::Mat & descriptor) const
{
if(!ppStmt)
{
@@ -3083,6 +3167,32 @@ void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt, int nodeId, int wordId
rc = sqlite3_bind_double(ppStmt, index++, pt.z);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//descriptor
if(uStrNumCmp(_version, "0.11.2") >= 0)
{
rc = sqlite3_bind_int(ppStmt, index++, descriptor.cols);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UASSERT(descriptor.empty() || descriptor.type() == CV_32F || descriptor.type() == CV_8U);
if(descriptor.empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
}
else
{
if(descriptor.type() == CV_32F)
{
// CV_32F
rc = sqlite3_bind_blob(ppStmt, index++, descriptor.data, descriptor.cols*sizeof(float), SQLITE_STATIC);
}
else
{
// CV_8U
rc = sqlite3_bind_blob(ppStmt, index++, descriptor.data, descriptor.cols*sizeof(char), SQLITE_STATIC);
}
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

2
corelib/src/DBDriverSqlite3.h
View File

@@ -109,7 +109,7 @@ private:
void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
private:
void loadLinksQuery(std::list<Signature *> & signatures) const;

17
corelib/src/Features2d.cpp
View File

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_features.h"
#include "rtabmap/core/Stereo.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/ULogger.h"
@@ -626,20 +627,18 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
if(_maxDepth > 0.0f || _minDepth > 0.0f)
{
UASSERT(keypoints3D.size() == keypoints.size());
bool isInMM = data.depthRaw().type() == CV_16UC1;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(unsigned int i=0; i<keypoints.size(); ++i)
{
int u = int(keypoints[i].pt.x+0.5f);
int v = int(keypoints[i].pt.y+0.5f);
float d = util2d::getDepth(
data.depthRaw(),
keypoints[i].pt.x/float((data.imageRaw().cols/data.depthRaw().cols)),
keypoints[i].pt.y/float((data.imageRaw().rows/data.depthRaw().rows)),
false);
bool reject = true;
if(u >=0 && u<data.depthRaw().cols && v >=0 && v<data.depthRaw().rows)
if(uIsFinite(d) && d>_minDepth && (_maxDepth <= 0.0f || d < _maxDepth))
{
float d = isInMM?(float)data.depthRaw().at<uint16_t>(v,u)*0.001f:data.depthRaw().at<float>(v,u);
if(uIsFinite(d) && d>_minDepth && (_maxDepth <= 0.0f || d < _maxDepth))
{
reject = false;
}
reject = false;
}
if(reject)
{

84
corelib/src/Graph.cpp
View File

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/GeodeticCoords.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/util3d_filtering.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/eigen.h>
#include <pcl/common/common.h>
@@ -550,6 +551,55 @@ std::multimap<int, int>::const_iterator findLink(
return links.end();
}
std::map<int, Transform> frustumPosesFiltering(
const std::map<int, Transform> & poses,
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)
{
std::map<int, Transform> output;
if(poses.size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int> ids(poses.size());
cloud->resize(poses.size());
ids.resize(poses.size());
int oi=0;
for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(!iter->second.isNull())
{
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
ids[oi++] = iter->first;
}
}
cloud->resize(oi);
ids.resize(oi);
pcl::IndicesPtr indices = util3d::frustumFiltering(
cloud,
pcl::IndicesPtr(new std::vector<int>),
cameraPose,
horizontalFOV,
verticalFOV,
nearClipPlaneDistance,
farClipPlaneDistance,
negative);
for(unsigned int i=0; i<indices->size(); ++i)
{
output.insert(*poses.find(ids[indices->at(i)]));
}
}
return output;
}
std::map<int, Transform> radiusPosesFiltering(
const std::map<int, Transform> & poses,
float radius,
@@ -1441,15 +1491,14 @@ std::map<int, float> getNodesInRadius(
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
kdTree->setInputCloud(cloud);
std::vector<int> ind;
std::vector<float> dist;
std::vector<float> sqrdDist;
pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
kdTree->radiusSearch(pt, radius, ind, dist, 0);
kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
for(unsigned int i=0; i<ind.size(); ++i)
{
if(ind[i] >=0)
{
UDEBUG("Inlier %d: %f", ids[ind[i]], sqrt(dist[i]));
foundNodes.insert(std::make_pair(ids[ind[i]], dist[i]));
foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
}
}
}
@@ -1461,7 +1510,8 @@ std::map<int, float> getNodesInRadius(
std::map<int, Transform> getPosesInRadius(
int nodeId,
const std::map<int, Transform> & nodes,
float radius)
float radius,
float angle)
{
UASSERT(uContains(nodes, nodeId));
std::map<int, Transform> foundNodes;
@@ -1494,15 +1544,31 @@ std::map<int, Transform> getPosesInRadius(
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
kdTree->setInputCloud(cloud);
std::vector<int> ind;
std::vector<float> dist;
std::vector<float> sqrdDist;
pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
kdTree->radiusSearch(pt, radius, ind, dist, 0);
kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
Eigen::Vector3f vA = fromT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
for(unsigned int i=0; i<ind.size(); ++i)
{
if(ind[i] >=0)
{
UDEBUG("Inlier %d: %f", ids[ind[i]], sqrt(dist[i]));
foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
if(angle > 0.0f)
{
const Transform & checkT = nodes.at(ids[ind[i]]);
// same orientation?
Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
if(a <= angle)
{
foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
}
}
else
{
foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
}
}
}
}

159
corelib/src/Memory.cpp
View File

@@ -70,8 +70,8 @@ const int Memory::kIdInvalid = 0;
Memory::Memory(const ParametersMap & parameters) :
_dbDriver(0),
_similarityThreshold(Parameters::defaultMemRehearsalSimilarity()),
_rawDataKept(Parameters::defaultMemImageKept()),
_binDataKept(Parameters::defaultMemBinDataKept()),
_rawDescriptorsKept(Parameters::defaultMemRawDescriptorsKept()),
_saveDepth16Format(Parameters::defaultMemSaveDepth16Format()),
_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
@@ -302,14 +302,14 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
void Memory::close(bool databaseSaved, bool postInitClosingEvents)
{
UDEBUG("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
UINFO("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
if(!databaseSaved || (!_memoryChanged && !_linksChanged))
{
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database.")));
UDEBUG("");
UINFO("No changes added to database.");
if(_dbDriver)
{
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
@@ -324,7 +324,7 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents)
}
else
{
UDEBUG("");
UINFO("Saving memory...");
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory..."));
if(!_memoryChanged && _linksChanged && _dbDriver)
{
@@ -384,8 +384,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
UDEBUG("");
ParametersMap::const_iterator iter;
Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
Parameters::parse(parameters, Parameters::kMemBinDataKept(), _binDataKept);
Parameters::parse(parameters, Parameters::kMemRawDescriptorsKept(), _rawDescriptorsKept);
Parameters::parse(parameters, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
Parameters::parse(parameters, Parameters::kMemReduceGraph(), _reduceGraph);
Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
@@ -2031,6 +2031,18 @@ void Memory::removeLink(int oldId, int newId)
}
}
void Memory::removeRawData(int id)
{
Signature * s = this->_getSignature(id);
if(s)
{
s->sensorData().setImageRaw(cv::Mat());
s->sensorData().setDepthOrRightRaw(cv::Mat());
s->sensorData().setLaserScanRaw(cv::Mat(), s->sensorData().laserScanMaxPts(), s->sensorData().laserScanMaxRange());
s->sensorData().setUserDataRaw(cv::Mat());
}
}
// compute transform fromId -> toId
Transform Memory::computeTransform(
int fromId,
@@ -2064,8 +2076,12 @@ Transform Memory::computeTransform(
{
tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
tmpTo.setWords3(std::multimap<int, cv::Point3f>());
tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
}
Transform guess = Transform::getIdentity();
@@ -2244,7 +2260,7 @@ bool Memory::addLink(const Link & link)
{
UASSERT(link.type() > Link::kNeighbor && link.type() != Link::kUndef);
ULOGGER_INFO("to=%d, from=%d transform: %s", link.to(), link.from(), link.transform().prettyPrint().c_str());
ULOGGER_INFO("to=%d, from=%d transform: %s var=%f", link.to(), link.from(), link.transform().prettyPrint().c_str(), link.transVariance());
Signature * toS = _getSignature(link.to());
Signature * fromS = _getSignature(link.from());
if(toS && fromS)
@@ -3006,8 +3022,12 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
data.imageRaw().type() == CV_8UC1 ||
data.imageRaw().type() == CV_8UC3);
UASSERT_MSG(data.depthOrRightRaw().empty() ||
( (data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_8UC1) &&
((data.imageRaw().empty() && data.depthOrRightRaw().type() != CV_8UC1) || (data.depthOrRightRaw().rows == data.imageRaw().rows && data.depthOrRightRaw().cols == data.imageRaw().cols))),
( ( data.depthOrRightRaw().type() == CV_16UC1 ||
data.depthOrRightRaw().type() == CV_32FC1 ||
data.depthOrRightRaw().type() == CV_8UC1)
&&
( (data.imageRaw().empty() && data.depthOrRightRaw().type() != CV_8UC1) ||
(data.imageRaw().rows % data.depthOrRightRaw().rows == 0 && data.imageRaw().cols % data.depthOrRightRaw().cols == 0))),
uFormat("image=(%d/%d) depth=(%d/%d, type=%d [accepted=%d,%d,%d])",
data.imageRaw().cols,
data.imageRaw().rows,
@@ -3093,9 +3113,18 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
imageMono = data.imageRaw();
}
cv::Mat depthMask;
if(_useDepthAsMask && !data.depthRaw().empty())
{
if(imageMono.rows/data.depthRaw().rows == imageMono.cols/data.depthRaw().cols)
{
depthMask = util2d::interpolate(data.depthRaw(), imageMono.rows/data.depthRaw().rows, 0.1f);
}
}
keypoints = _feature2D->generateKeypoints(
imageMono,
_useDepthAsMask&&!data.depthRaw().empty()?data.depthRaw():cv::Mat());
depthMask);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
@@ -3173,6 +3202,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, cv::Point3f> words3D;
std::multimap<int, cv::Mat> wordsDescriptors;
if(wordIds.size() > 0)
{
UASSERT(wordIds.size() == keypoints.size());
@@ -3196,46 +3226,62 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
{
words3D.insert(std::pair<int, cv::Point3f>(*iter, keypoints3D.at(i)));
}
if(_rawDescriptorsKept)
{
wordsDescriptors.insert(std::pair<int, cv::Mat>(*iter, descriptors.row(i).clone()));
}
}
}
if(words.size() > 8 &&
words3D.size() == 0 &&
!pose.isNull() &&
if(!pose.isNull() &&
data.cameraModels().size() == 1 &&
_signatures.size())
words.size() &&
words3D.size() == 0)
{
UDEBUG("Generate 3D words using odometry");
Signature * previousS = _signatures.rbegin()->second;
if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
bool fillWithNaN = true;
if(_signatures.size())
{
Transform cameraTransform = pose.inverse() * previousS->getPose();
// compute 3D words by epipolar geometry with the previous signature
std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
uMultimapToMapUnique(words),
uMultimapToMapUnique(previousS->getWords()),
data.cameraModels()[0],
cameraTransform);
UDEBUG("Generate 3D words using odometry");
Signature * previousS = _signatures.rbegin()->second;
if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
{
Transform cameraTransform = pose.inverse() * previousS->getPose();
// compute 3D words by epipolar geometry with the previous signature
std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
uMultimapToMapUnique(words),
uMultimapToMapUnique(previousS->getWords()),
data.cameraModels()[0],
cameraTransform);
// words3D should have the same size than words
// words3D should have the same size than words
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
if(jter != inliers.end())
{
words3D.insert(std::make_pair(iter->first, jter->second));
}
else
{
words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
}
}
t = timer.ticks();
UASSERT(words3D.size() == words.size());
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
fillWithNaN = false;
}
}
if(fillWithNaN)
{
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
if(jter != inliers.end())
{
words3D.insert(std::make_pair(iter->first, jter->second));
}
else
{
words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
}
words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
}
t = timer.ticks();
UASSERT(words3D.size() == words.size());
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
}
}
@@ -3245,7 +3291,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
StereoCameraModel stereoCameraModel = data.stereoCameraModel();
// apply decimation?
if((this->isBinDataKept() || this->isRawDataKept()) && _imageDecimation > 1)
if(_imageDecimation > 1)
{
image = util2d::decimate(image, _imageDecimation);
depthOrRightImage = util2d::decimate(depthOrRightImage, _imageDecimation);
@@ -3360,13 +3406,14 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
s->setWords(words);
s->setWords3(words3D);
if(this->isRawDataKept())
{
s->sensorData().setImageRaw(image);
s->sensorData().setDepthOrRightRaw(depthOrRightImage);
s->sensorData().setLaserScanRaw(laserScan, maxLaserScanMaxPts, data.laserScanMaxRange());
s->sensorData().setUserDataRaw(data.userDataRaw());
}
s->setWordsDescriptors(wordsDescriptors);
// set raw data
s->sensorData().setImageRaw(image);
s->sensorData().setDepthOrRightRaw(depthOrRightImage);
s->sensorData().setLaserScanRaw(laserScan, maxLaserScanMaxPts, data.laserScanMaxRange());
s->sensorData().setUserDataRaw(data.userDataRaw());
s->sensorData().setGroundTruth(data.groundTruth());
t = timer.ticks();
@@ -3516,13 +3563,23 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
for(std::list<Signature *>::iterator j=surfSigns.begin(); j!=surfSigns.end(); ++j)
{
const std::vector<int> & keys = uKeys((*j)->getWords());
// Add all references
for(std::vector<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
{
_vwd->addWordRef(*i, (*j)->id());
}
if(keys.size())
{
const VisualWord * wordFirst = _vwd->getWord(keys.front()); //get descriptor size
UASSERT(wordFirst!=0);
//Descriptors used for Memory::computeTransform()
cv::Mat descriptors(keys.size(), wordFirst->getDescriptor().cols, wordFirst->getDescriptor().type());
// Add all references
for(unsigned int i=0; i<keys.size(); ++i)
{
_vwd->addWordRef(keys.at(i), (*j)->id());
const VisualWord * word = _vwd->getWord(keys.at(i));
UASSERT(word != 0);
word->getDescriptor().copyTo(descriptors.row(i));
}
(*j)->sensorData().setFeatures(std::vector<cv::KeyPoint>(), descriptors);
(*j)->setEnabled(true);
}
}

3
corelib/src/OdometryMono.cpp
View File

@@ -100,7 +100,6 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), roi));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "true"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "false"));
@@ -872,7 +871,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
else if(inliersRef.size())
{
// find centroid of the cloud and set it to 1 meter
Eigen::Vector4f centroid;
Eigen::Vector4f centroid(0,0,0,0);
pcl::PointCloud<pcl::PointXYZ> inliersRefCloud;
inliersRefCloud.resize(inliersRef.size());
for(unsigned int i=0; i<inliersRef.size(); ++i)

80
corelib/src/OptimizerG2O.cpp
View File

@@ -35,15 +35,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/OptimizerG2O.h>
#ifdef WITH_G2O
#include "g2o/config.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#ifdef G2O_HAVE_CSPARSE
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#endif
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#ifdef WITH_G2O_CHOMOLD
#ifdef G2O_HAVE_CHOLMOD
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#endif
#include "g2o/types/slam3d/vertex_se3.h"
@@ -52,9 +55,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "g2o/types/slam2d/edge_se2.h"
typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
#ifdef WITH_G2O_CHOMOLD
#ifdef G2O_HAVE_CSPARSE
typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
#endif
#ifdef G2O_HAVE_CHOLMOD
typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearCholmodSolver;
#endif
@@ -76,6 +81,48 @@ bool OptimizerG2O::available()
#endif
}
bool OptimizerG2O::isCSparseAvailable()
{
#ifdef G2O_HAVE_CSPARSE
return true;
#else
return false;
#endif
}
bool OptimizerG2O::isCholmodAvailable()
{
#ifdef G2O_HAVE_CHOLMOD
return true;
#else
return false;
#endif
}
void OptimizerG2O::parseParameters(const ParametersMap & parameters)
{
Optimizer::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kg2oSolver(), solver_);
Parameters::parse(parameters, Parameters::kg2oOptimizer(), optimizer_);
#ifndef G2O_HAVE_CHOLMOD
if(solver_ == 2)
{
UWARN("g2o is not built with chmold, so it cannot be used as solver. Using CSparse instead.");
solver_ = 0;
}
#endif
#ifndef G2O_HAVE_CSPARSE
if(solver_ == 0)
{
UWARN("g2o is not built with csparse, so it cannot be used as solver. Using PCG instead.");
solver_ = 1;
}
#endif
}
std::map<int, Transform> OptimizerG2O::optimize(
int rootId,
const std::map<int, Transform> & poses,
@@ -97,31 +144,32 @@ std::map<int, Transform> OptimizerG2O::optimize(
int solverApproach = 0;
int optimizationApproach = 1;
SlamBlockSolver * blockSolver;
#ifdef WITH_G2O_CHOMOLD
if(solverApproach == 0)
SlamBlockSolver * blockSolver = 0;
if(solverApproach == 2)
{
#ifdef G2O_HAVE_CHOLMOD
//chmold
SlamLinearCholmodSolver * linearSolver = new SlamLinearCholmodSolver();
linearSolver->setBlockOrdering(false);
blockSolver = new SlamBlockSolver(linearSolver);
}
else if
#else
if
#endif
(solverApproach == 1)
{
//pcg
SlamLinearPCGSolver * linearSolver = new SlamLinearPCGSolver();
blockSolver = new SlamBlockSolver(linearSolver);
}
else
else if(solverApproach == 0)
{
#ifdef G2O_HAVE_CSPARSE
//csparse
SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver();
linearSolver->setBlockOrdering(false);
blockSolver = new SlamBlockSolver(linearSolver);
#endif
}
if(blockSolver == 0)
{
//pcg
SlamLinearPCGSolver * linearSolver = new SlamLinearPCGSolver();
blockSolver = new SlamBlockSolver(linearSolver);
}
if(optimizationApproach == 1)

2
corelib/src/Parameters.cpp
View File

@@ -53,7 +53,7 @@ Parameters::~Parameters()
{
}
std::string Parameters::getDefaultWorkingDirectory()
std::string Parameters::createDefaultWorkingDirectory()
{
std::string path = UDirectory::homeDir();
if(!path.empty())

112
corelib/src/RegistrationVis.cpp
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d_features.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/VWDictionary.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/core/Features2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
@@ -71,6 +72,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), _featureParameters.at(Parameters::kVisSubPixWinSize())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), _featureParameters.at(Parameters::kVisSubPixIterations())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), _featureParameters.at(Parameters::kVisSubPixEps())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
this->parseParameters(parameters);
}
@@ -175,17 +177,19 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("%s=%f", Parameters::kVisCorFlowEps().c_str(), _flowEps);
UDEBUG("%s=%d", Parameters::kVisCorFlowMaxLevel().c_str(), _flowMaxLevel);
UDEBUG("Input(%d): from=%d words, %d 3D words, %d kpts, %d descriptors",
UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors, %d kpts, %d descriptors",
fromSignature.id(),
(int)fromSignature.getWords().size(),
(int)fromSignature.getWords3().size(),
(int)fromSignature.getWordsDescriptors().size(),
(int)fromSignature.sensorData().keypoints().size(),
fromSignature.sensorData().descriptors().rows);
UDEBUG("Input(%d): to=%d words, %d 3D words, %d kpts, %d descriptors",
UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d descriptors",
toSignature.id(),
(int)toSignature.getWords().size(),
(int)toSignature.getWords3().size(),
(int)toSignature.getWordsDescriptors().size(),
(int)toSignature.sensorData().keypoints().size(),
toSignature.sensorData().descriptors().rows);
@@ -194,7 +198,9 @@ Transform RegistrationVis::computeTransformationImpl(
////////////////////
// Find correspondences
////////////////////
if((_estimationType<2 || fromSignature.getWords().size()) && // required only for 2D->2D
//recompute correspondences if descriptors are provided
if((fromSignature.getWordsDescriptors().empty() && toSignature.getWordsDescriptors().empty()) &&
(_estimationType<2 || fromSignature.getWords().size()) && // required only for 2D->2D
(_estimationType==0 || toSignature.getWords().size()) && // required only for 3D->2D or 2D->2D
fromSignature.getWords3().size() && // required in all estimation approaches
(_estimationType==1 || toSignature.getWords3().size())) // required only for 3D->3D and 2D->2D
@@ -209,11 +215,15 @@ Transform RegistrationVis::computeTransformationImpl(
UASSERT((fromSignature.getWords().empty() && fromSignature.getWords3().empty())||
(fromSignature.getWords().size() == fromSignature.getWords3().size()));
UASSERT((int)fromSignature.sensorData().keypoints().size() == fromSignature.sensorData().descriptors().rows ||
fromSignature.sensorData().descriptors().rows == 0);
fromSignature.getWords().size() == fromSignature.getWordsDescriptors().size() ||
fromSignature.sensorData().descriptors().rows == 0 ||
fromSignature.getWordsDescriptors().size() == 0);
UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
(toSignature.getWords().size() == toSignature.getWords3().size()));
UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
toSignature.sensorData().descriptors().rows == 0);
toSignature.getWords().size() == toSignature.getWordsDescriptors().size() ||
toSignature.sensorData().descriptors().rows == 0 ||
toSignature.getWordsDescriptors().size() == 0);
UASSERT(fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
fromSignature.sensorData().imageRaw().type() == CV_8UC3);
UASSERT(toSignature.sensorData().imageRaw().type() == CV_8UC1 ||
@@ -232,9 +242,20 @@ Transform RegistrationVis::computeTransformationImpl(
fromSignature.sensorData().setImageRaw(tmp);
}
cv::Mat depthMask;
if(_useDepthAsMask && !fromSignature.sensorData().depthRaw().empty())
{
if(fromSignature.sensorData().imageRaw().rows % fromSignature.sensorData().depthRaw().rows == 0 &&
fromSignature.sensorData().imageRaw().cols % fromSignature.sensorData().depthRaw().cols == 0 &&
fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
{
depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
}
}
kptsFrom = detector->generateKeypoints(
fromSignature.sensorData().imageRaw(),
_useDepthAsMask&&!fromSignature.sensorData().depthRaw().empty()?fromSignature.sensorData().depthRaw():cv::Mat());
depthMask);
}
else
{
@@ -325,8 +346,8 @@ Transform RegistrationVis::computeTransformationImpl(
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] &&
uIsInBounds(cornersTo[i].x, 0.0f, float(toSignature.sensorData().depthOrRightRaw().cols)) &&
uIsInBounds(cornersTo[i].y, 0.0f, float(toSignature.sensorData().depthOrRightRaw().rows)))
uIsInBounds(cornersTo[i].x, 0.0f, float(toSignature.sensorData().imageRaw().cols)) &&
uIsInBounds(cornersTo[i].y, 0.0f, float(toSignature.sensorData().imageRaw().rows)))
{
kptsFrom[ki] = cv::KeyPoint(cornersFrom[i], 1);
kptsFrom3DKept[ki] = kptsFrom3D[i];
@@ -389,9 +410,21 @@ Transform RegistrationVis::computeTransformationImpl(
cv::cvtColor(toSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
toSignature.sensorData().setImageRaw(tmp);
}
cv::Mat depthMask;
if(_useDepthAsMask && !fromSignature.sensorData().depthRaw().empty())
{
if(fromSignature.sensorData().imageRaw().rows % fromSignature.sensorData().depthRaw().rows == 0 &&
fromSignature.sensorData().imageRaw().cols % fromSignature.sensorData().depthRaw().cols == 0 &&
fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
{
depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
}
}
kptsTo = detector->generateKeypoints(
toSignature.sensorData().imageRaw(),
_useDepthAsMask&&!toSignature.sensorData().depthRaw().empty()?toSignature.sensorData().depthRaw():cv::Mat());
depthMask);
}
else
{
@@ -407,22 +440,55 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("kptsFrom=%d", (int)kptsFrom.size());
UDEBUG("kptsTo=%d", (int)kptsTo.size());
cv::Mat descriptorsFrom;
if(kptsFrom.size())
{
if(fromSignature.getWordsDescriptors().size() == (int)kptsFrom.size())
{
descriptorsFrom = cv::Mat(fromSignature.getWordsDescriptors().size(),
fromSignature.getWordsDescriptors().begin()->second.cols,
fromSignature.getWordsDescriptors().begin()->second.type());
int i=0;
for(std::multimap<int, cv::Mat>::const_iterator iter=fromSignature.getWordsDescriptors().begin();
iter!=fromSignature.getWordsDescriptors().end();
++iter, ++i)
{
iter->second.copyTo(descriptorsFrom.row(i));
}
}
else if(fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
{
descriptorsFrom = fromSignature.sensorData().descriptors();
}
else if(!fromSignature.sensorData().imageRaw().empty())
{
descriptorsFrom = detector->generateDescriptors(fromSignature.sensorData().imageRaw(), kptsFrom);
}
}
cv::Mat descriptorsTo;
if(fromSignature.getWords().empty() && fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
if(kptsTo.size())
{
descriptorsFrom = fromSignature.sensorData().descriptors();
}
else
{
descriptorsFrom = detector->generateDescriptors(fromSignature.sensorData().imageRaw(), kptsFrom);
}
if(toSignature.getWords().empty() && toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
{
descriptorsTo = toSignature.sensorData().descriptors();
}
else if(!toSignature.sensorData().imageRaw().empty())
{
descriptorsTo = detector->generateDescriptors(toSignature.sensorData().imageRaw(), kptsTo);
if(toSignature.getWordsDescriptors().size() == (int)kptsTo.size())
{
descriptorsTo = cv::Mat(toSignature.getWordsDescriptors().size(),
toSignature.getWordsDescriptors().begin()->second.cols,
toSignature.getWordsDescriptors().begin()->second.type());
int i=0;
for(std::multimap<int, cv::Mat>::const_iterator iter=toSignature.getWordsDescriptors().begin();
iter!=toSignature.getWordsDescriptors().end();
++iter, ++i)
{
iter->second.copyTo(descriptorsTo.row(i));
}
}
else if(toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
{
descriptorsTo = toSignature.sensorData().descriptors();
}
else if(!toSignature.sensorData().imageRaw().empty())
{
descriptorsTo = detector->generateDescriptors(toSignature.sensorData().imageRaw(), kptsTo);
}
}
// create 3D keypoints

113
corelib/src/Rtabmap.cpp
View File

@@ -83,6 +83,7 @@ Rtabmap::Rtabmap() :
_loopRatio(Parameters::defaultRtabmapLoopRatio()),
_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
_maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
_rawDataKept(Parameters::defaultMemImageKept()),
_statisticLogsBufferedInRAM(Parameters::defaultRtabmapStatisticLogsBufferedInRAM()),
_statisticLogged(Parameters::defaultRtabmapStatisticLogged()),
_statisticLoggedHeaders(Parameters::defaultRtabmapStatisticLoggedHeaders()),
@@ -120,7 +121,7 @@ Rtabmap::Rtabmap() :
_memory(0),
_foutFloat(0),
_foutInt(0),
_wDir("."),
_wDir(""),
_mapCorrection(Transform::getIdentity()),
_lastLocalizationNodeId(0),
_pathStatus(0),
@@ -151,7 +152,7 @@ void Rtabmap::setupLogFiles(bool overwrite)
_foutInt = 0;
}
if(_statisticLogged)
if(_statisticLogged && !_wDir.empty())
{
std::string attributes = "a+"; // append to log files
if(overwrite)
@@ -234,6 +235,10 @@ void Rtabmap::setupLogFiles(bool overwrite)
}
else
{
if(_statisticLogged)
{
UWARN("Working directory is not set, log disabled!");
}
UDEBUG("Log disabled!");
}
}
@@ -314,7 +319,7 @@ void Rtabmap::init(const std::string & configFile, const std::string & databaseP
void Rtabmap::close(bool databaseSaved)
{
UINFO("");
UINFO("databaseSaved=%d", databaseSaved?1:0);
_highestHypothesis = std::make_pair(0,0.0f);
_loopClosureHypothesis = std::make_pair(0,0.0f);
_lastProcessTime = 0.0;
@@ -386,6 +391,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
Parameters::parse(parameters, Parameters::kRtabmapStatisticLogsBufferedInRAM(), _statisticLogsBufferedInRAM);
Parameters::parse(parameters, Parameters::kRtabmapStatisticLogged(), _statisticLogged);
Parameters::parse(parameters, Parameters::kRtabmapStatisticLoggedHeaders(), _statisticLoggedHeaders);
@@ -1073,6 +1079,7 @@ bool Rtabmap::process(
newPose = _mapCorrection * signature->getPose();
}
UDEBUG("Added pose %s", newPose.prettyPrint().c_str());
// Update Poses and Constraints
_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
@@ -1707,10 +1714,10 @@ bool Rtabmap::process(
{
//Compute transform if metric data are present
Transform transform;
float variance = 1.0f;
RegistrationInfo info;
info.variance = 1.0f;
if(_rgbdSlamMode)
{
RegistrationInfo info;
transform = _memory->computeTransform(signature->id(), _loopClosureHypothesis.first, &info);
loopClosureVisualInliers = info.inliers;
rejectedHypothesis = transform.isNull();
@@ -1723,8 +1730,8 @@ bool Rtabmap::process(
if(!rejectedHypothesis)
{
// Make the new one the parent of the old one
UASSERT(variance > 0.0);
rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance, variance));
UASSERT(info.variance > 0.0);
rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, info.variance, info.variance));
if(!rejectedHypothesis)
{
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
@@ -1798,42 +1805,51 @@ bool Rtabmap::process(
iter!=nearestPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0);
++iter)
{
const std::map<int, Transform> & path = *iter;
std::map<int, Transform> path = *iter;
UASSERT(path.size());
//find the nearest pose on the path
//find the nearest pose on the path looking in the same direction
path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
path = graph::getPosesInRadius(signature->id(), path, _localRadius, M_PI/4);
int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
UASSERT(nearestId > 0);
// nearest pose must not be linked to current location and enough
if(!signature->hasLink(nearestId) &&
(_proximityFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
if(nearestId > 0)
{
RegistrationInfo info;
Transform transform = _memory->computeTransform(signature->id(), nearestId, &info);
if(!transform.isNull())
// nearest pose must not be linked to current location and enough
if(!signature->hasLink(nearestId) &&
(_proximityFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
{
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
RegistrationInfo info;
Transform transform = _memory->computeTransform(signature->id(), nearestId, &info);
if(!transform.isNull())
{
UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
signature->id(),
nearestId,
transform.prettyPrint().c_str());
UASSERT(info.variance > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.variance, info.variance));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(_loopClosureHypothesis.first == 0)
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
{
++localSpaceClosuresAddedVisually;
lastLocalSpaceClosureId = nearestId;
UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
signature->id(),
nearestId,
transform.prettyPrint().c_str());
UASSERT(info.variance > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.variance, info.variance));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(loopClosureVisualInliers == 0)
{
loopClosureVisualInliers = info.inliers;
}
if(_loopClosureHypothesis.first == 0)
{
++localSpaceClosuresAddedVisually;
lastLocalSpaceClosureId = nearestId;
}
}
else
{
UWARN("Ignoring local loop closure with %d because resulting "
"transform is to large!? (%fm > %fm)",
nearestId, transform.getNorm(), _proximityFilteringRadius);
}
}
else
{
UWARN("Ignoring local loop closure with %d because resulting "
"transform is to large!? (%fm > %fm)",
nearestId, transform.getNorm(), _proximityFilteringRadius);
}
}
}
@@ -2283,6 +2299,10 @@ bool Rtabmap::process(
{
lastSignatureData = *signature;
}
if(!_rawDataKept)
{
_memory->removeRawData(signature->id());
}
// remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored)
int signatureRemoved = _memory->cleanup();
@@ -2453,10 +2473,12 @@ bool Rtabmap::process(
// place after transfer because the memory/local graph may have changed
statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
statistics_.addStatistic(Statistics::kMemoryDatabase_memory_used(), _memory->getDatabaseMemoryUsed());
std::map<int, Signature> signatures;
if(_publishLastSignatureData)
{
UINFO("Adding data %d (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
signatures.insert(std::make_pair(lastSignatureData.id(), lastSignatureData));
}
// Set local graph
@@ -2618,6 +2640,11 @@ void Rtabmap::setWorkingDirectory(std::string path)
}
}
}
else if(path.empty())
{
_wDir.clear();
setupLogFiles();
}
else
{
ULOGGER_ERROR("Directory \"%s\" doesn't exist!", path.c_str());
@@ -2647,7 +2674,14 @@ void Rtabmap::dumpData() const
UDEBUG("");
if(_memory)
{
_memory->dumpMemory(this->getWorkingDir());
if(this->getWorkingDir().empty())
{
UERROR("Working directory not set.");
}
else
{
_memory->dumpMemory(this->getWorkingDir());
}
}
}
@@ -3003,6 +3037,11 @@ void Rtabmap::dumpPrediction() const
{
if(_memory && _bayesFilter)
{
if(this->getWorkingDir().empty())
{
UERROR("Working directory not set.");
return;
}
std::list<int> signaturesToCompare;
for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin();
iter!=_memory->getWorkingMem().end();

392
corelib/src/RtabmapThread.cpp
View File

@@ -63,11 +63,9 @@ RtabmapThread::~RtabmapThread()
{
UEventsManager::removeHandler(this);
// Stop the thread first
join(true);
close(true);
delete _frameRateTimer;
delete _rtabmap;
}
void RtabmapThread::pushNewState(State newState, const ParametersMap & parameters)
@@ -116,41 +114,68 @@ void RtabmapThread::setDataBufferSize(unsigned int size)
void RtabmapThread::createIntermediateNodes(bool enabled)
{
enabled = _createIntermediateNodes;
_createIntermediateNodes = enabled;
}
void RtabmapThread::close(bool databaseSaved)
{
this->join(true);
if(_rtabmap)
{
_rtabmap->close(databaseSaved);
delete _rtabmap;
_rtabmap = 0;
}
}
void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const
{
std::map<int, Signature> signatures;
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
std::map<int, double> stamps;
std::map<int, std::string> labels;
std::map<int, std::vector<unsigned char> > userDatas;
if(graphOnly)
if(_rtabmap)
{
_rtabmap->getGraph(poses,
constraints,
optimized,
full,
&signatures);
std::map<int, Signature> signatures;
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
std::map<int, double> stamps;
std::map<int, std::string> labels;
std::map<int, std::vector<unsigned char> > userDatas;
if(graphOnly)
{
_rtabmap->getGraph(poses,
constraints,
optimized,
full,
&signatures);
}
else
{
_rtabmap->get3DMap(
signatures,
poses,
constraints,
optimized,
full);
}
this->post(new RtabmapEvent3DMap(
signatures,
poses,
constraints));
}
else
{
_rtabmap->get3DMap(
signatures,
poses,
constraints,
optimized,
full);
UERROR("Rtabmap is null!");
}
}
this->post(new RtabmapEvent3DMap(
signatures,
poses,
constraints));
void RtabmapThread::mainLoopBegin()
{
if(_rtabmap == 0)
{
UERROR("Cannot start rtabmap thread if no rtabmap object is set! Stopping the thread...");
this->kill();
}
}
void RtabmapThread::mainLoopKill()
@@ -297,177 +322,180 @@ void RtabmapThread::mainLoop()
void RtabmapThread::handleEvent(UEvent* event)
{
if(this->isRunning() && event->getClassName().compare("CameraEvent") == 0)
if(this->isRunning())
{
UDEBUG("CameraEvent");
CameraEvent * e = (CameraEvent*)event;
if(e->getCode() == CameraEvent::kCodeData)
if(event->getClassName().compare("CameraEvent") == 0)
{
this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
}
}
else if(event->getClassName().compare("OdometryEvent") == 0)
{
UDEBUG("OdometryEvent");
OdometryEvent * e = (OdometryEvent*)event;
if(!e->pose().isNull())
{
this->addData(*e);
}
else
{
lastPose_.setNull();
}
}
else if(event->getClassName().compare("UserDataEvent") == 0)
{
if(!_paused)
{
UDEBUG("UserDataEvent");
bool updated = false;
UserDataEvent * e = (UserDataEvent*)event;
_userDataMutex.lock();
if(!e->data().empty())
UDEBUG("CameraEvent");
CameraEvent * e = (CameraEvent*)event;
if(e->getCode() == CameraEvent::kCodeData)
{
updated = !_userData.empty();
_userData = e->data();
this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
}
_userDataMutex.unlock();
if(updated)
}
else if(event->getClassName().compare("OdometryEvent") == 0)
{
UDEBUG("OdometryEvent");
OdometryEvent * e = (OdometryEvent*)event;
if(!e->pose().isNull())
{
UWARN("New user data received before the last one was processed... replacing "
"user data with this new one. Note that UserDataEvent should be used only "
"if the rate of UserDataEvent is lower than RTAB-Map's detection rate (%f Hz).", _rate);
this->addData(*e);
}
else
{
pushNewState(kStateAddingUserData);
lastPose_.setNull();
}
}
}
else if(event->getClassName().compare("RtabmapEventCmd") == 0)
{
RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
if(cmd == RtabmapEventCmd::kCmdInit)
else if(event->getClassName().compare("UserDataEvent") == 0)
{
ULOGGER_DEBUG("CMD_INIT");
ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
UASSERT(rtabmapEvent->value1().isStr());
UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->value1().toStr())).second);
pushNewState(kStateInit, parameters);
if(!_paused)
{
UDEBUG("UserDataEvent");
bool updated = false;
UserDataEvent * e = (UserDataEvent*)event;
_userDataMutex.lock();
if(!e->data().empty())
{
updated = !_userData.empty();
_userData = e->data();
}
_userDataMutex.unlock();
if(updated)
{
UWARN("New user data received before the last one was processed... replacing "
"user data with this new one. Note that UserDataEvent should be used only "
"if the rate of UserDataEvent is lower than RTAB-Map's detection rate (%f Hz).", _rate);
}
else
{
pushNewState(kStateAddingUserData);
}
}
}
else if(cmd == RtabmapEventCmd::kCmdClose)
else if(event->getClassName().compare("RtabmapEventCmd") == 0)
{
ULOGGER_DEBUG("CMD_CLOSE");
UASSERT(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().isBool());
ParametersMap param;
param.insert(ParametersPair("saved", uBool2Str(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().toBool())));
pushNewState(kStateClose, param);
}
else if(cmd == RtabmapEventCmd::kCmdResetMemory)
{
ULOGGER_DEBUG("CMD_RESET_MEMORY");
pushNewState(kStateReseting);
}
else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
{
ULOGGER_DEBUG("CMD_DUMP_MEMORY");
pushNewState(kStateDumpingMemory);
}
else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
{
ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
pushNewState(kStateDumpingPrediction);
}
else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
{
ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
UASSERT(rtabmapEvent->value1().isBool());
UASSERT(rtabmapEvent->value2().isStr());
UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value3().isInt() || rtabmapEvent->value3().isUInt());
UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->value2().toStr()));
param.insert(ParametersPair("id", !rtabmapEvent->value1().toBool()?rtabmapEvent->value3().toStr():"0"));
param.insert(ParametersPair("margin", !rtabmapEvent->value1().toBool()?rtabmapEvent->value4().toStr():"0"));
pushNewState(kStateExportingDOTGraph, param);
}
else if(cmd == RtabmapEventCmd::kCmdExportPoses)
{
ULOGGER_DEBUG("CMD_EXPORT_POSES");
UASSERT(rtabmapEvent->value1().isBool());
UASSERT(rtabmapEvent->value2().isBool());
UASSERT(rtabmapEvent->value3().isStr());
UASSERT(rtabmapEvent->value4().isUndef() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
ParametersMap param;
param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("optimized", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("path", rtabmapEvent->value3().toStr()));
param.insert(ParametersPair("type", rtabmapEvent->value4().isInt()?rtabmapEvent->value4().toStr():"0"));
pushNewState(kStateExportingPoses, param);
RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
if(cmd == RtabmapEventCmd::kCmdInit)
{
ULOGGER_DEBUG("CMD_INIT");
ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
UASSERT(rtabmapEvent->value1().isStr());
UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->value1().toStr())).second);
pushNewState(kStateInit, parameters);
}
else if(cmd == RtabmapEventCmd::kCmdClose)
{
ULOGGER_DEBUG("CMD_CLOSE");
UASSERT(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().isBool());
ParametersMap param;
param.insert(ParametersPair("saved", uBool2Str(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().toBool())));
pushNewState(kStateClose, param);
}
else if(cmd == RtabmapEventCmd::kCmdResetMemory)
{
ULOGGER_DEBUG("CMD_RESET_MEMORY");
pushNewState(kStateReseting);
}
else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
{
ULOGGER_DEBUG("CMD_DUMP_MEMORY");
pushNewState(kStateDumpingMemory);
}
else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
{
ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
pushNewState(kStateDumpingPrediction);
}
else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
{
ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
UASSERT(rtabmapEvent->value1().isBool());
UASSERT(rtabmapEvent->value2().isStr());
UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value3().isInt() || rtabmapEvent->value3().isUInt());
UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->value2().toStr()));
param.insert(ParametersPair("id", !rtabmapEvent->value1().toBool()?rtabmapEvent->value3().toStr():"0"));
param.insert(ParametersPair("margin", !rtabmapEvent->value1().toBool()?rtabmapEvent->value4().toStr():"0"));
pushNewState(kStateExportingDOTGraph, param);
}
else if(cmd == RtabmapEventCmd::kCmdExportPoses)
{
ULOGGER_DEBUG("CMD_EXPORT_POSES");
UASSERT(rtabmapEvent->value1().isBool());
UASSERT(rtabmapEvent->value2().isBool());
UASSERT(rtabmapEvent->value3().isStr());
UASSERT(rtabmapEvent->value4().isUndef() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
ParametersMap param;
param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("optimized", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("path", rtabmapEvent->value3().toStr()));
param.insert(ParametersPair("type", rtabmapEvent->value4().isInt()?rtabmapEvent->value4().toStr():"0"));
pushNewState(kStateExportingPoses, param);
}
else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
{
ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
pushNewState(kStateCleanDataBuffer);
}
else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
{
ULOGGER_DEBUG("CMD_PUBLISH_MAP");
UASSERT(rtabmapEvent->value1().isBool());
UASSERT(rtabmapEvent->value2().isBool());
UASSERT(rtabmapEvent->value3().isBool());
ParametersMap param;
param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("optimized", rtabmapEvent->value2().toStr()));
param.insert(ParametersPair("graph_only", rtabmapEvent->value3().toStr()));
pushNewState(kStatePublishingMap, param);
}
else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
{
ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
pushNewState(kStateTriggeringMap);
}
else if(cmd == RtabmapEventCmd::kCmdPause)
{
ULOGGER_DEBUG("CMD_PAUSE");
_paused = !_paused;
}
else if(cmd == RtabmapEventCmd::kCmdGoal)
{
ULOGGER_DEBUG("CMD_GOAL");
UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
ParametersMap param;
param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
pushNewState(kStateSettingGoal, param);
}
else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
{
ULOGGER_DEBUG("CMD_CANCEL_GOAL");
pushNewState(kStateCancellingGoal);
}
else if(cmd == RtabmapEventCmd::kCmdLabel)
{
ULOGGER_DEBUG("CMD_LABEL");
UASSERT(rtabmapEvent->value1().isStr());
UASSERT(rtabmapEvent->value2().isUndef() || rtabmapEvent->value2().isInt() || rtabmapEvent->value2().isUInt());
ParametersMap param;
param.insert(ParametersPair("label", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("id", rtabmapEvent->value2().isUndef()?"0":rtabmapEvent->value2().toStr()));
pushNewState(kStateLabelling, param);
}
else
{
UWARN("Cmd %d unknown!", cmd);
}
}
else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
else if(event->getClassName().compare("ParamEvent") == 0)
{
ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
pushNewState(kStateCleanDataBuffer);
ULOGGER_DEBUG("changing parameters");
pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
}
else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
{
ULOGGER_DEBUG("CMD_PUBLISH_MAP");
UASSERT(rtabmapEvent->value1().isBool());
UASSERT(rtabmapEvent->value2().isBool());
UASSERT(rtabmapEvent->value3().isBool());
ParametersMap param;
param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("optimized", rtabmapEvent->value2().toStr()));
param.insert(ParametersPair("graph_only", rtabmapEvent->value3().toStr()));
pushNewState(kStatePublishingMap, param);
}
else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
{
ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
pushNewState(kStateTriggeringMap);
}
else if(cmd == RtabmapEventCmd::kCmdPause)
{
ULOGGER_DEBUG("CMD_PAUSE");
_paused = !_paused;
}
else if(cmd == RtabmapEventCmd::kCmdGoal)
{
ULOGGER_DEBUG("CMD_GOAL");
UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
ParametersMap param;
param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
pushNewState(kStateSettingGoal, param);
}
else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
{
ULOGGER_DEBUG("CMD_CANCEL_GOAL");
pushNewState(kStateCancellingGoal);
}
else if(cmd == RtabmapEventCmd::kCmdLabel)
{
ULOGGER_DEBUG("CMD_LABEL");
UASSERT(rtabmapEvent->value1().isStr());
UASSERT(rtabmapEvent->value2().isUndef() || rtabmapEvent->value2().isInt() || rtabmapEvent->value2().isUInt());
ParametersMap param;
param.insert(ParametersPair("label", rtabmapEvent->value1().toStr()));
param.insert(ParametersPair("id", rtabmapEvent->value2().isUndef()?"0":rtabmapEvent->value2().toStr()));
pushNewState(kStateLabelling, param);
}
else
{
UWARN("Cmd %d unknown!", cmd);
}
}
else if(event->getClassName().compare("ParamEvent") == 0)
{
ULOGGER_DEBUG("changing parameters");
pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
}
}

21
corelib/src/SensorData.cpp
View File

@@ -481,10 +481,11 @@ void SensorData::setUserData(const cv::Mat & userData)
void SensorData::uncompressData()
{
uncompressData(_imageCompressed.empty()?0:&_imageRaw,
_depthOrRightCompressed.empty()?0:&_depthOrRightRaw,
_laserScanCompressed.empty()?0:&_laserScanRaw,
_userDataCompressed.empty()?0:&_userDataRaw);
cv::Mat tmpA, tmpB, tmpC, tmpD;
uncompressData(_imageCompressed.empty()?0:&tmpA,
_depthOrRightCompressed.empty()?0:&tmpB,
_laserScanCompressed.empty()?0:&tmpC,
_userDataCompressed.empty()?0:&tmpD);
}
void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw)
@@ -493,6 +494,18 @@ void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat
if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
{
_imageRaw = *imageRaw;
//backward compatibility, set image size in camera model if not set
if(!_imageRaw.empty() && _cameraModels.size())
{
cv::Size size(_imageRaw.cols/_cameraModels.size(), _imageRaw.rows/_cameraModels.size());
for(unsigned int i=0; i<_cameraModels.size(); ++i)
{
if(_cameraModels[i].isValidForProjection() && _cameraModels[i].imageWidth() == 0)
{
_cameraModels[i].setImageSize(size);
}
}
}
}
if(depthRaw && !depthRaw->empty() && _depthOrRightRaw.empty())
{

8
corelib/src/Signature.cpp
View File

@@ -195,8 +195,10 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
if(kps.size())
{
std::list<cv::Point3f> pts = uValues(_words3, oldWordId);
std::list<cv::Mat> descriptors = uValues(_wordsDescriptors, oldWordId);
_words.erase(oldWordId);
_words3.erase(oldWordId);
_wordsDescriptors.erase(oldWordId);
_wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
for(std::list<cv::KeyPoint>::const_iterator iter=kps.begin(); iter!=kps.end(); ++iter)
{
@@ -206,6 +208,10 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
{
_words3.insert(std::pair<int, cv::Point3f>(activeWordId, (*iter)));
}
for(std::list<cv::Mat>::const_iterator iter=descriptors.begin(); iter!=descriptors.end(); ++iter)
{
_wordsDescriptors.insert(std::pair<int, cv::Mat>(activeWordId, (*iter)));
}
}
}
@@ -218,12 +224,14 @@ void Signature::removeAllWords()
{
_words.clear();
_words3.clear();
_wordsDescriptors.clear();
}
void Signature::removeWord(int wordId)
{
_words.erase(wordId);
_words3.erase(wordId);
_wordsDescriptors.clear();
}
cv::Mat Signature::getPoseCovariance() const

165
corelib/src/VWDictionary.cpp
View File

@@ -79,7 +79,14 @@ public:
}
else
{
delete (rtflann::Index<rtflann::L2<float> >*)index_;
if(useDistanceL1_)
{
delete (rtflann::Index<rtflann::L1<float> >*)index_;
}
else
{
delete (rtflann::Index<rtflann::L2<float> >*)index_;
}
}
index_ = 0;
}
@@ -101,7 +108,14 @@ public:
}
else
{
return ((const rtflann::Index<rtflann::L2<float> >*)index_)->size();
if(useDistanceL1_)
{
return ((const rtflann::Index<rtflann::L1<float> >*)index_)->size();
}
else
{
return ((const rtflann::Index<rtflann::L2<float> >*)index_)->size();
}
}
}
@@ -118,19 +132,29 @@ public:
}
else
{
return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
if(useDistanceL1_)
{
return ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory()/1000;
}
else
{
return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
}
}
}
// Note that useDistanceL1 doesn't have any effect if LSH is used
void build(
const cv::Mat & features,
const rtflann::IndexParams& params)
const rtflann::IndexParams& params,
bool useDistanceL1)
{
this->release();
UASSERT(index_ == 0);
UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
featuresType_ = features.type();
featuresDim_ = features.cols;
useDistanceL1_ = useDistanceL1;
if(featuresType_ == CV_8UC1)
{
@@ -141,8 +165,16 @@ public:
else
{
rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
if(useDistanceL1_)
{
index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
}
else
{
index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
}
}
if(features.rows == 1)
@@ -193,18 +225,37 @@ public:
else
{
rtflann::Matrix<float> point((float*)feature.data, feature.rows, feature.cols);
rtflann::Index<rtflann::L2<float> > * index = (rtflann::Index<rtflann::L2<float> >*)index_;
index->addPoints(point, 0);
// Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
if(useDistanceL1_)
{
// clean not used features
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
rtflann::Index<rtflann::L1<float> > * index = (rtflann::Index<rtflann::L1<float> >*)index_;
index->addPoints(point, 0);
// Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
{
addedDescriptors_.erase(*iter);
// clean not used features
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
{
addedDescriptors_.erase(*iter);
}
removedIndexes_.clear();
index->buildIndex();
}
}
else
{
rtflann::Index<rtflann::L2<float> > * index = (rtflann::Index<rtflann::L2<float> >*)index_;
index->addPoints(point, 0);
// Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
{
// clean not used features
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
{
addedDescriptors_.erase(*iter);
}
removedIndexes_.clear();
index->buildIndex();
}
removedIndexes_.clear();
index->buildIndex();
}
}
@@ -230,10 +281,15 @@ public:
{
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->removePoint(index);
}
else if(useDistanceL1_)
{
((rtflann::Index<rtflann::L1<float> >*)index_)->removePoint(index);
}
else
{
((rtflann::Index<rtflann::L2<float> >*)index_)->removePoint(index);
}
removedIndexes_.push_back(index);
}
@@ -264,7 +320,14 @@ public:
{
rtflann::Matrix<float> distsF((float*)dists.data, dists.rows, dists.cols);
rtflann::Matrix<float> queryF((float*)query.data, query.rows, query.cols);
((rtflann::Index<rtflann::L2<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
if(useDistanceL1_)
{
((rtflann::Index<rtflann::L1<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
}
else
{
((rtflann::Index<rtflann::L2<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
}
}
}
@@ -274,6 +337,7 @@ private:
int featuresType_;
int featuresDim_;
bool isLSH_;
bool useDistanceL1_; // true=EUCLEDIAN_L2 false=MANHATTAN_L1
// keep feature in memory until the tree is rebuilt
// (in case the word is deleted when removed from the VWDictionary)
@@ -292,6 +356,7 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
_dictionaryPath(Parameters::defaultKpDictionaryPath()),
_newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
_lastWordId(0),
useDistanceL1_(false),
_flannIndex(new FlannIndex()),
_strategy(kNNBruteForce)
{
@@ -491,23 +556,14 @@ void VWDictionary::setNNStrategy(NNStrategy strategy)
#endif
#endif
if(RTABMAP_NONFREE == 0 && strategy == kNNFlannKdTree)
bool update = _strategy != strategy;
_strategy = strategy;
if(update)
{
UWARN("KdTree (%d) nearest neighbor is not available because RTAB-Map isn't built "
"with OpenCV nonfree module (KdTree only used for SURF/SIFT features). "
"NN strategy is not modified (current=%d).", (int)kNNFlannKdTree, (int)_strategy);
}
else
{
bool update = _strategy != strategy;
_strategy = strategy;
if(update)
{
_dataTree = cv::Mat();
_notIndexedWords = uKeysSet(_visualWords);
_removedIndexedWords.clear();
this->update();
}
_dataTree = cv::Mat();
_notIndexedWords = uKeysSet(_visualWords);
_removedIndexedWords.clear();
this->update();
}
}
}
@@ -576,15 +632,15 @@ void VWDictionary::update()
switch(_strategy)
{
case kNNFlannNaive:
_flannIndex->build(w->getDescriptor(), rtflann::LinearIndexParams());
_flannIndex->build(w->getDescriptor(), rtflann::LinearIndexParams(), useDistanceL1_);
break;
case kNNFlannKdTree:
UASSERT_MSG(w->getDescriptor().type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
_flannIndex->build(w->getDescriptor(), rtflann::KDTreeIndexParams());
_flannIndex->build(w->getDescriptor(), rtflann::KDTreeIndexParams(), useDistanceL1_);
break;
case kNNFlannLSH:
UASSERT_MSG(w->getDescriptor().type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
_flannIndex->build(w->getDescriptor(), rtflann::LshIndexParams(12, 20, 2));
_flannIndex->build(w->getDescriptor(), rtflann::LshIndexParams(12, 20, 2), useDistanceL1_);
break;
default:
UFATAL("Not supposed to be here!");
@@ -666,15 +722,15 @@ void VWDictionary::update()
switch(_strategy)
{
case kNNFlannNaive:
_flannIndex->build(_dataTree, rtflann::LinearIndexParams());
_flannIndex->build(_dataTree, rtflann::LinearIndexParams(), useDistanceL1_);
break;
case kNNFlannKdTree:
UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
_flannIndex->build(_dataTree, rtflann::KDTreeIndexParams());
_flannIndex->build(_dataTree, rtflann::KDTreeIndexParams(), useDistanceL1_);
break;
case kNNFlannLSH:
UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
_flannIndex->build(_dataTree, rtflann::LshIndexParams(12, 20, 2));
_flannIndex->build(_dataTree, rtflann::LshIndexParams(12, 20, 2), useDistanceL1_);
break;
default:
break;
@@ -723,6 +779,7 @@ void VWDictionary::clear(bool printWarningsIfNotEmpty)
_mapIdIndex.clear();
_unusedWords.clear();
_flannIndex->release();
useDistanceL1_ = false;
}
int VWDictionary::getNextId()
@@ -764,11 +821,29 @@ void VWDictionary::removeAllWordRef(int wordId, int signatureId)
}
}
std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
int signatureId)
{
UASSERT(signatureId > 0);
cv::Mat descriptors;
if(descriptorsIn.type() == CV_8U)
{
useDistanceL1_ = true;
if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
{
descriptorsIn.convertTo(descriptors, CV_32F);
}
else
{
descriptors = descriptorsIn;
}
}
else
{
descriptors = descriptorsIn;
}
UDEBUG("id=%d descriptors=%d", signatureId, descriptors.rows);
UTimer timer;
std::list<int> wordIds;
@@ -1313,6 +1388,18 @@ void VWDictionary::deleteUnusedWords()
void VWDictionary::exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const
{
if(_visualWords.empty())
{
UWARN("Dictionary is empty, cannot export it!");
return;
}
if(_visualWords.at(0)->getDescriptor().type() != CV_32FC1)
{
UERROR("Exporting binary descriptors is not implemented!");
return;
}
FILE* foutRef = 0;
FILE* foutDesc = 0;
#ifdef _MSC_VER

2
corelib/src/pcl18/readme.txt Normal file
View File

@@ -0,0 +1,2 @@
Copied pcl 1.8 functions to be used in older pcl versions. They are
not included if 1.8 is detected (the originals are used directly).

284
corelib/src/pcl18/surface/impl/organized_fast_mesh.hpp Normal file
View File

@@ -0,0 +1,284 @@
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2011, Dirk Holz (University of Bonn)
* Copyright (c) 2010-2011, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#ifndef PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_
#define PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_
#include <pcl18/surface/organized_fast_mesh.h>
#include <rtabmap/utilite/ULogger.h>
/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::performReconstruction (pcl::PolygonMesh &output)
{
reconstructPolygons (output.polygons);
// Get the field names
int x_idx = pcl::getFieldIndex (output.cloud, "x");
int y_idx = pcl::getFieldIndex (output.cloud, "y");
int z_idx = pcl::getFieldIndex (output.cloud, "z");
if (x_idx == -1 || y_idx == -1 || z_idx == -1)
return;
// correct all measurements,
// (running over complete image since some rows and columns are left out
// depending on triangle_pixel_size)
// avoid to do that here (only needed for ASCII mesh file output, e.g., in vtk files
for (unsigned int i = 0; i < input_->points.size (); ++i)
if (!isFinite (input_->points[i]))
resetPointData (i, output, 0.0f, x_idx, y_idx, z_idx);
}
/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
{
reconstructPolygons (polygons);
}
/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
{
if (triangulation_type_ == TRIANGLE_RIGHT_CUT)
makeRightCutMesh (polygons);
else if (triangulation_type_ == TRIANGLE_LEFT_CUT)
makeLeftCutMesh (polygons);
else if (triangulation_type_ == TRIANGLE_ADAPTIVE_CUT)
makeAdaptiveCutMesh (polygons);
else if (triangulation_type_ == QUAD_MESH)
makeQuadMesh (polygons);
}
/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::makeQuadMesh (std::vector<pcl::Vertices>& polygons)
{
int last_column = input_->width - triangle_pixel_size_columns_;
int last_row = input_->height - triangle_pixel_size_rows_;
int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
int y_big_incr = triangle_pixel_size_rows_ * input_->width,
x_big_incr = y_big_incr + triangle_pixel_size_columns_;
// Reserve enough space
polygons.resize (input_->width * input_->height);
// Go over the rows first
for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
{
// Initialize a new row
i = y * input_->width;
index_right = i + triangle_pixel_size_columns_;
index_down = i + y_big_incr;
index_down_right = i + x_big_incr;
// Go over the columns
for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
i += triangle_pixel_size_columns_,
index_right += triangle_pixel_size_columns_,
index_down += triangle_pixel_size_columns_,
index_down_right += triangle_pixel_size_columns_)
{
if (isValidQuad (i, index_right, index_down_right, index_down))
if (store_shadowed_faces_ || !isShadowedQuad (i, index_right, index_down_right, index_down))
addQuad (i, index_right, index_down_right, index_down, idx++, polygons);
}
}
polygons.resize (idx);
}
/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::makeRightCutMesh (std::vector<pcl::Vertices>& polygons)
{
int last_column = input_->width - triangle_pixel_size_columns_;
int last_row = input_->height - triangle_pixel_size_rows_;
int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
int y_big_incr = triangle_pixel_size_rows_ * input_->width,
x_big_incr = y_big_incr + triangle_pixel_size_columns_;
// Reserve enough space
polygons.resize (input_->width * input_->height * 2);
// Go over the rows first
for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
{
// Initialize a new row
i = y * input_->width;
index_right = i + triangle_pixel_size_columns_;
index_down = i + y_big_incr;
index_down_right = i + x_big_incr;
// Go over the columns
for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
i += triangle_pixel_size_columns_,
index_right += triangle_pixel_size_columns_,
index_down += triangle_pixel_size_columns_,
index_down_right += triangle_pixel_size_columns_)
{
if (isValidTriangle (i, index_down_right, index_right))
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
addTriangle (i, index_down_right, index_right, idx++, polygons);
if (isValidTriangle (i, index_down, index_down_right))
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
addTriangle (i, index_down, index_down_right, idx++, polygons);
}
}
polygons.resize (idx);
}
/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::makeLeftCutMesh (std::vector<pcl::Vertices>& polygons)
{
int last_column = input_->width - triangle_pixel_size_columns_;
int last_row = input_->height - triangle_pixel_size_rows_;
int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
int y_big_incr = triangle_pixel_size_rows_ * input_->width,
x_big_incr = y_big_incr + triangle_pixel_size_columns_;
// Reserve enough space
polygons.resize (input_->width * input_->height * 2);
// Go over the rows first
for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
{
// Initialize a new row
i = y * input_->width;
index_right = i + triangle_pixel_size_columns_;
index_down = i + y_big_incr;
index_down_right = i + x_big_incr;
// Go over the columns
for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
i += triangle_pixel_size_columns_,
index_right += triangle_pixel_size_columns_,
index_down += triangle_pixel_size_columns_,
index_down_right += triangle_pixel_size_columns_)
{
if (isValidTriangle (i, index_down, index_right))
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
addTriangle (i, index_down, index_right, idx++, polygons);
if (isValidTriangle (index_right, index_down, index_down_right))
if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
addTriangle (index_right, index_down, index_down_right, idx++, polygons);
}
}
polygons.resize (idx);
}
/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons)
{
int last_column = input_->width - triangle_pixel_size_columns_;
int last_row = input_->height - triangle_pixel_size_rows_;
int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
int y_big_incr = triangle_pixel_size_rows_ * input_->width,
x_big_incr = y_big_incr + triangle_pixel_size_columns_;
// Reserve enough space
polygons.resize (input_->width * input_->height * 2);
// Go over the rows first
for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
{
// Initialize a new row
i = y * input_->width;
index_right = i + triangle_pixel_size_columns_;
index_down = i + y_big_incr;
index_down_right = i + x_big_incr;
// Go over the columns
for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
i += triangle_pixel_size_columns_,
index_right += triangle_pixel_size_columns_,
index_down += triangle_pixel_size_columns_,
index_down_right += triangle_pixel_size_columns_)
{
const bool right_cut_upper = isValidTriangle (i, index_down_right, index_right);
const bool right_cut_lower = isValidTriangle (i, index_down, index_down_right);
const bool left_cut_upper = isValidTriangle (i, index_down, index_right);
const bool left_cut_lower = isValidTriangle (index_right, index_down, index_down_right);
if (right_cut_upper && right_cut_lower && left_cut_upper && left_cut_lower)
{
float dist_right_cut = fabsf (input_->points[index_down].z - input_->points[index_right].z);
float dist_left_cut = fabsf (input_->points[i].z - input_->points[index_down_right].z);
if (dist_right_cut >= dist_left_cut)
{
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
addTriangle (i, index_down_right, index_right, idx++, polygons);
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
addTriangle (i, index_down, index_down_right, idx++, polygons);
}
else
{
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
addTriangle (i, index_down, index_right, idx++, polygons);
if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
addTriangle (index_right, index_down, index_down_right, idx++, polygons);
}
}
else
{
if (right_cut_upper)
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
addTriangle (i, index_down_right, index_right, idx++, polygons);
if (right_cut_lower)
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
addTriangle (i, index_down, index_down_right, idx++, polygons);
if (left_cut_upper)
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
addTriangle (i, index_down, index_right, idx++, polygons);
if (left_cut_lower)
if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
addTriangle (index_right, index_down, index_down_right, idx++, polygons);
}
}
}
polygons.resize (idx);
}
#define PCL_INSTANTIATE_OrganizedFastMesh(T) \
template class PCL_EXPORTS pcl::OrganizedFastMesh<T>;
#endif // PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_

490
corelib/src/pcl18/surface/organized_fast_mesh.h Normal file
View File

@@ -0,0 +1,490 @@
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2011, Dirk Holz, University of Bonn.
* Copyright (c) 2010-2011, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#ifndef PCL_SURFACE_ORGANIZED_FAST_MESH_H_
#define PCL_SURFACE_ORGANIZED_FAST_MESH_H_
#include <pcl/common/angles.h>
#include <pcl/surface/reconstruction.h>
namespace pcl
{
/** \brief Simple triangulation/surface reconstruction for organized point
* clouds. Neighboring points (pixels in image space) are connected to
* construct a triangular (or quad) mesh.
*
* \note If you use this code in any academic work, please cite:
* D. Holz and S. Behnke.
* Fast Range Image Segmentation and Smoothing using Approximate Surface Reconstruction and Region Growing.
* In Proceedings of the 12th International Conference on Intelligent Autonomous Systems (IAS),
* Jeju Island, Korea, June 26-29 2012.
* <a href="http://purl.org/holz/papers/holz_2012_ias.pdf">http://purl.org/holz/papers/holz_2012_ias.pdf</a>
*
* \author Dirk Holz, Radu B. Rusu
* \ingroup surface
*/
template <typename PointInT>
class OrganizedFastMesh : public pcl::MeshConstruction<PointInT>
{
public:
typedef boost::shared_ptr<OrganizedFastMesh<PointInT> > Ptr;
typedef boost::shared_ptr<const OrganizedFastMesh<PointInT> > ConstPtr;
using pcl::MeshConstruction<PointInT>::input_;
using pcl::MeshConstruction<PointInT>::check_tree_;
typedef typename pcl::PointCloud<PointInT>::Ptr PointCloudPtr;
typedef std::vector<pcl::Vertices> Polygons;
enum TriangulationType
{
TRIANGLE_RIGHT_CUT, // _always_ "cuts" a quad from top left to bottom right
TRIANGLE_LEFT_CUT, // _always_ "cuts" a quad from top right to bottom left
TRIANGLE_ADAPTIVE_CUT, // "cuts" where possible and prefers larger differences in 'z' direction
QUAD_MESH // create a simple quad mesh
};
/** \brief Constructor. Triangulation type defaults to \a QUAD_MESH. */
OrganizedFastMesh ()
: max_edge_length_a_ (0.0f)
, max_edge_length_b_ (0.0f)
, max_edge_length_c_ (0.0f)
, max_edge_length_set_ (false)
, max_edge_length_dist_dependent_ (false)
, triangle_pixel_size_rows_ (1)
, triangle_pixel_size_columns_ (1)
, triangulation_type_ (QUAD_MESH)
, viewpoint_ (Eigen::Vector3f::Zero ())
, store_shadowed_faces_ (false)
, cos_angle_tolerance_ (fabsf (cosf (pcl::deg2rad (12.5f))))
, distance_tolerance_ (-1.0f)
, distance_dependent_ (false)
, use_depth_as_distance_(false)
{
check_tree_ = false;
};
/** \brief Destructor. */
virtual ~OrganizedFastMesh () {};
/** \brief Set a maximum edge length.
* Using not only the scalar \a a, but also \a b and \a c, allows for using a distance threshold in the form of:
* threshold(x) = c*x*x + b*x + a
* \param[in] a scalar coefficient of the (distance-dependent polynom) threshold
* \param[in] b linear coefficient of the (distance-dependent polynom) threshold
* \param[in] c quadratic coefficient of the (distance-dependent polynom) threshold
*/
inline void
setMaxEdgeLength (float a, float b = 0.0f, float c = 0.0f)
{
max_edge_length_a_ = a;
max_edge_length_b_ = b;
max_edge_length_c_ = c;
if ((max_edge_length_a_ + max_edge_length_b_ + max_edge_length_c_) > std::numeric_limits<float>::min())
max_edge_length_set_ = true;
else
max_edge_length_set_ = false;
};
inline void
unsetMaxEdgeLength ()
{
max_edge_length_set_ = false;
}
/** \brief Set the edge length (in pixels) used for constructing the fixed mesh.
* \param[in] triangle_size edge length in pixels
* (Default: 1 = neighboring pixels are connected)
*/
inline void
setTrianglePixelSize (int triangle_size)
{
setTrianglePixelSizeRows (triangle_size);
setTrianglePixelSizeColumns (triangle_size);
}
/** \brief Set the edge length (in pixels) used for iterating over rows when constructing the fixed mesh.
* \param[in] triangle_size edge length in pixels
* (Default: 1 = neighboring pixels are connected)
*/
inline void
setTrianglePixelSizeRows (int triangle_size)
{
triangle_pixel_size_rows_ = std::max (1, (triangle_size - 1));
}
/** \brief Set the edge length (in pixels) used for iterating over columns when constructing the fixed mesh.
* \param[in] triangle_size edge length in pixels
* (Default: 1 = neighboring pixels are connected)
*/
inline void
setTrianglePixelSizeColumns (int triangle_size)
{
triangle_pixel_size_columns_ = std::max (1, (triangle_size - 1));
}
/** \brief Set the triangulation type (see \a TriangulationType)
* \param[in] type quad mesh, triangle mesh with fixed left, right cut,
* or adaptive cut (splits a quad w.r.t. the depth (z) of the points)
*/
inline void
setTriangulationType (TriangulationType type)
{
triangulation_type_ = type;
}
/** \brief Set the viewpoint from where the input point cloud has been acquired.
* \param[in] viewpoint Vector containing the viewpoint coordinates (in the coordinate system of the data)
*/
inline void setViewpoint (const Eigen::Vector3f& viewpoint)
{
viewpoint_ = viewpoint;
}
/** \brief Get the viewpoint from where the input point cloud has been acquired. */
const inline Eigen::Vector3f& getViewpoint () const
{
return viewpoint_;
}
/** \brief Store shadowed faces or not.
* \param[in] enable set to true to store shadowed faces
*/
inline void
storeShadowedFaces (bool enable)
{
store_shadowed_faces_ = enable;
}
/** \brief Set the angle tolerance used for checking whether or not an edge is occluded.
* Standard values are 5deg to 15deg (input in rad!). Set a value smaller than zero to
* disable the check for shadowed edges.
* \param[in] angle_tolerance Angle tolerance (in rad). Set a value <0 to disable.
*/
inline void
setAngleTolerance(float angle_tolerance)
{
if (angle_tolerance > 0)
cos_angle_tolerance_ = fabsf (cosf (angle_tolerance));
else
cos_angle_tolerance_ = -1.0f;
}
inline void setDistanceTolerance(float distance_tolerance, bool depth_dependent = false)
{
distance_tolerance_ = distance_tolerance;
if (distance_tolerance_ < 0)
return;
distance_dependent_ = depth_dependent;
if (!distance_dependent_)
distance_tolerance_ *= distance_tolerance_;
}
/** \brief Use the points' depths (z-coordinates) instead of measured distances (points' distances to the viewpoint).
* \param[in] enable Set to true skips comptations and further speeds up computation by using depth instead of computing distance. false to disable. */
inline void useDepthAsDistance(bool enable)
{
use_depth_as_distance_ = enable;
}
protected:
/** \brief max length of edge, scalar component */
float max_edge_length_a_;
/** \brief max length of edge, scalar component */
float max_edge_length_b_;
/** \brief max length of edge, scalar component */
float max_edge_length_c_;
/** \brief flag whether or not edges are limited in length */
bool max_edge_length_set_;
/** \brief flag whether or not max edge length is distance dependent. */
bool max_edge_length_dist_dependent_;
/** \brief size of triangle edges (in pixels) for iterating over rows. */
int triangle_pixel_size_rows_;
/** \brief size of triangle edges (in pixels) for iterating over columns*/
int triangle_pixel_size_columns_;
/** \brief Type of meshing scheme (quads vs. triangles, left cut vs. right cut ... */
TriangulationType triangulation_type_;
/** \brief Viewpoint from which the point cloud has been acquired (in the same coordinate frame as the data). */
Eigen::Vector3f viewpoint_;
/** \brief Whether or not shadowed faces are stored, e.g., for exploration */
bool store_shadowed_faces_;
/** \brief (Cosine of the) angle tolerance used when checking whether or not an edge between two points is shadowed. */
float cos_angle_tolerance_;
/** \brief distance tolerance for filtering out shadowed/occluded edges */
float distance_tolerance_;
/** \brief flag whether or not \a distance_tolerance_ is distance dependent (multiplied by the squared distance to the point) or not. */
bool distance_dependent_;
/** \brief flag whether or not the points' depths are used instead of measured distances (points' distances to the viewpoint).
This flag may be set using useDepthAsDistance(true) for (RGB-)Depth cameras to skip computations and gain additional speed up. */
bool use_depth_as_distance_;
/** \brief Perform the actual polygonal reconstruction.
* \param[out] polygons the resultant polygons
*/
void
reconstructPolygons (std::vector<pcl::Vertices>& polygons);
/** \brief Create the surface.
* \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices.
*/
virtual void
performReconstruction (std::vector<pcl::Vertices> &polygons);
/** \brief Create the surface.
*
* Simply uses image indices to create an initial polygonal mesh for organized point clouds.
* \a indices_ are ignored!
*
* \param[out] output the resultant polygonal mesh
*/
void
performReconstruction (pcl::PolygonMesh &output);
/** \brief Add a new triangle to the current polygon mesh
* \param[in] a index of the first vertex
* \param[in] b index of the second vertex
* \param[in] c index of the third vertex
* \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
* \param[out] polygons the polygon mesh to be updated
*/
inline void
addTriangle (int a, int b, int c, int idx, std::vector<pcl::Vertices>& polygons)
{
assert (idx < static_cast<int> (polygons.size ()));
polygons[idx].vertices.resize (3);
polygons[idx].vertices[0] = a;
polygons[idx].vertices[1] = b;
polygons[idx].vertices[2] = c;
}
/** \brief Add a new quad to the current polygon mesh
* \param[in] a index of the first vertex
* \param[in] b index of the second vertex
* \param[in] c index of the third vertex
* \param[in] d index of the fourth vertex
* \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
* \param[out] polygons the polygon mesh to be updated
*/
inline void
addQuad (int a, int b, int c, int d, int idx, std::vector<pcl::Vertices>& polygons)
{
assert (idx < static_cast<int> (polygons.size ()));
polygons[idx].vertices.resize (4);
polygons[idx].vertices[0] = a;
polygons[idx].vertices[1] = b;
polygons[idx].vertices[2] = c;
polygons[idx].vertices[3] = d;
}
/** \brief Set (all) coordinates of a particular point to the specified value
* \param[in] point_index index of point
* \param[out] mesh to modify
* \param[in] value value to use when re-setting
* \param[in] field_x_idx the X coordinate of the point
* \param[in] field_y_idx the Y coordinate of the point
* \param[in] field_z_idx the Z coordinate of the point
*/
inline void
resetPointData (const int &point_index, pcl::PolygonMesh &mesh, const float &value = 0.0f,
int field_x_idx = 0, int field_y_idx = 1, int field_z_idx = 2)
{
float new_value = value;
memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_x_idx].offset], &new_value, sizeof (float));
memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_y_idx].offset], &new_value, sizeof (float));
memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_z_idx].offset], &new_value, sizeof (float));
}
/** \brief Check if a point is shadowed by another point
* \param[in] point_a the first point
* \param[in] point_b the second point
*/
inline bool
isShadowed (const PointInT& point_a, const PointInT& point_b)
{
bool valid = true;
Eigen::Vector3f dir_a = viewpoint_ - point_a.getVector3fMap ();
Eigen::Vector3f dir_b = point_b.getVector3fMap () - point_a.getVector3fMap ();
float distance_to_points = dir_a.norm ();
float distance_between_points = dir_b.norm ();
if (cos_angle_tolerance_ > 0)
{
float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points);
if (cos_angle != cos_angle)
cos_angle = 1.0f;
bool check_angle = fabs (cos_angle) >= cos_angle_tolerance_;
bool check_distance = true;
if (check_angle && (distance_tolerance_ > 0))
{
float dist_thresh = distance_tolerance_;
if (distance_dependent_)
{
float d = distance_to_points;
if (use_depth_as_distance_)
d = std::max(point_a.z, point_b.z);
dist_thresh *= d*d;
dist_thresh *= dist_thresh; // distance_tolerance_ is already squared if distance_dependent_ is false.
}
check_distance = (distance_between_points > dist_thresh);
}
valid = !(check_angle && check_distance);
}
// check if max. edge length is not exceeded
if (max_edge_length_set_)
{
float dist = (use_depth_as_distance_ ? std::max(point_a.z, point_b.z) : distance_to_points);
float dist_thresh = max_edge_length_a_;
if (fabs(max_edge_length_b_) > std::numeric_limits<float>::min())
dist_thresh += max_edge_length_b_ * dist;
if (fabs(max_edge_length_c_) > std::numeric_limits<float>::min())
dist_thresh += max_edge_length_c_ * dist * dist;
valid = (distance_between_points <= dist_thresh);
}
return !valid;
}
/** \brief Check if a triangle is valid.
* \param[in] a index of the first vertex
* \param[in] b index of the second vertex
* \param[in] c index of the third vertex
*/
inline bool
isValidTriangle (const int& a, const int& b, const int& c)
{
if (!pcl::isFinite (input_->points[a])) return (false);
if (!pcl::isFinite (input_->points[b])) return (false);
if (!pcl::isFinite (input_->points[c])) return (false);
return (true);
}
/** \brief Check if a triangle is shadowed.
* \param[in] a index of the first vertex
* \param[in] b index of the second vertex
* \param[in] c index of the third vertex
*/
inline bool
isShadowedTriangle (const int& a, const int& b, const int& c)
{
if (isShadowed (input_->points[a], input_->points[b])) return (true);
if (isShadowed (input_->points[b], input_->points[c])) return (true);
if (isShadowed (input_->points[c], input_->points[a])) return (true);
return (false);
}
/** \brief Check if a quad is valid.
* \param[in] a index of the first vertex
* \param[in] b index of the second vertex
* \param[in] c index of the third vertex
* \param[in] d index of the fourth vertex
*/
inline bool
isValidQuad (const int& a, const int& b, const int& c, const int& d)
{
if (!pcl::isFinite (input_->points[a])) return (false);
if (!pcl::isFinite (input_->points[b])) return (false);
if (!pcl::isFinite (input_->points[c])) return (false);
if (!pcl::isFinite (input_->points[d])) return (false);
return (true);
}
/** \brief Check if a triangle is shadowed.
* \param[in] a index of the first vertex
* \param[in] b index of the second vertex
* \param[in] c index of the third vertex
* \param[in] d index of the fourth vertex
*/
inline bool
isShadowedQuad (const int& a, const int& b, const int& c, const int& d)
{
if (isShadowed (input_->points[a], input_->points[b])) return (true);
if (isShadowed (input_->points[b], input_->points[c])) return (true);
if (isShadowed (input_->points[c], input_->points[d])) return (true);
if (isShadowed (input_->points[d], input_->points[a])) return (true);
return (false);
}
/** \brief Create a quad mesh.
* \param[out] polygons the resultant mesh
*/
void
makeQuadMesh (std::vector<pcl::Vertices>& polygons);
/** \brief Create a right cut mesh.
* \param[out] polygons the resultant mesh
*/
void
makeRightCutMesh (std::vector<pcl::Vertices>& polygons);
/** \brief Create a left cut mesh.
* \param[out] polygons the resultant mesh
*/
void
makeLeftCutMesh (std::vector<pcl::Vertices>& polygons);
/** \brief Create an adaptive cut mesh.
* \param[out] polygons the resultant mesh
*/
void
makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons);
};
}
#include <pcl18/surface/impl/organized_fast_mesh.hpp>
#endif // PCL_SURFACE_ORGANIZED_FAST_MESH_H_

4
corelib/src/resources/DatabaseSchema.sql.in
View File

@@ -29,7 +29,7 @@ CREATE TABLE Data (
id INTEGER NOT NULL,
image BLOB, -- compressed image (Grayscale or RGB)
depth BLOB, -- compressed image (Depth or Right image)
calibration BLOB, -- fx, fy, cx, cy [,baseline] local_transform
calibration BLOB, -- fx, fy, cx, cy, [baseline,] width, height, local_transform
scan BLOB, -- compressed data (Laser scan)
scan_max_pts INTEGER, -- Laser scan max points
scan_max_range FLOAT, -- Laser max range
@@ -70,6 +70,8 @@ CREATE TABLE Map_Node_Word (
depth_x FLOAT,
depth_y FLOAT,
depth_z FLOAT,
descriptor_size INTEGER,
descriptor BLOB,
FOREIGN KEY (node_id) REFERENCES Node(id),
FOREIGN KEY (word_id) REFERENCES Word(id)
);

18
corelib/src/toro3d/posegraph2.cpp
View File

@@ -54,8 +54,8 @@ typedef unsigned int uint;
#define LINESIZE 81920
#define DEBUG(i) \
if (verboseLevel>i) cerr
//#define DEBUG(i) \
// if (verboseLevel>i) cerr
bool TreePoseGraph2::load(const char* filename, bool overrideCovariances){
@@ -75,8 +75,8 @@ bool TreePoseGraph2::load(const char* filename, bool overrideCovariances){
int id;
Pose p;
ls >> id >> p.x() >> p.y() >> p.theta();
if (addVertex(id,p))
DEBUG(2) << "V " << id << endl;
addVertex(id,p);
//DEBUG(2) << "V " << id << endl;
}
@@ -98,8 +98,8 @@ bool TreePoseGraph2::load(const char* filename, bool overrideCovariances){
TreePoseGraph2::Vertex* v1=vertex(id1);
TreePoseGraph2::Vertex* v2=vertex(id2);
Transformation t(p);
if (addEdge(v1, v2,t ,m))
DEBUG(2) << "E " << id1 << " " << id2 << endl;
addEdge(v1, v2,t ,m);
//DEBUG(2) << "E " << id1 << " " << id2 << endl;
}
}
return true;
@@ -411,11 +411,11 @@ void TreePoseGraph2::collapseEdge(Edge* e){
double s=scale1.values[2][2]*sin(p1x.theta())+ scale2.values[2][2]*sin(p2x.theta());
double c=scale1.values[2][2]*cos(p1x.theta())+ scale2.values[2][2]*cos(p2x.theta());
DEBUG(2) << "p1x= " << p1x.x() << " " << p1x.y() << " " << p1x.theta() << endl;
DEBUG(2) << "p1x_pred= " << p2x.x() << " " << p2x.y() << " " << p2x.theta() << endl;
//DEBUG(2) << "p1x= " << p1x.x() << " " << p1x.y() << " " << p1x.theta() << endl;
//DEBUG(2) << "p1x_pred= " << p2x.x() << " " << p2x.y() << " " << p2x.theta() << endl;
Pose pFinal(p1.x()+p2.x(), p1.y()+p2.y(), atan2(s,c));
DEBUG(2) << "p1x_final= " << pFinal.x() << " " << pFinal.y() << " " << pFinal.theta() << endl;
//DEBUG(2) << "p1x_final= " << pFinal.x() << " " << pFinal.y() << " " << pFinal.theta() << endl;
e1x->transformation=Transformation(pFinal);
e1x->informationMatrix=IM;

4
corelib/src/toro3d/posegraph3.cpp
View File

@@ -51,8 +51,8 @@ namespace AISNavigation {
#define LINESIZE 81920
#define DEBUG(i) \
if (verboseLevel>i) cerr
//#define DEBUG(i) \
// if (verboseLevel>i) cerr
bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool twoDimensions){

54
corelib/src/toro3d/treeoptimizer2.cpp
View File

@@ -51,8 +51,8 @@ using namespace std;
namespace AISNavigation {
#define DEBUG(i) \
if (verboseLevel>i) cerr
//#define DEBUG(i) \
// if (verboseLevel>i) cerr
/** \brief A class (struct) to compute the parameterization of the vertex v **/
struct ParameterPropagator{
@@ -83,9 +83,9 @@ void TreeOptimizer2::initializeTreeParameters(){
void TreeOptimizer2::initializeOptimization(){
// compute the size of the preconditioning matrix
int sz=maxIndex()+1;
DEBUG(1) << "Size= " << sz << endl;
//DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
DEBUG(1) << "allocating M(" << sz << ")" << endl;
//DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
// sorting edges
@@ -99,9 +99,9 @@ void TreeOptimizer2::initializeOptimization(){
void TreeOptimizer2::initializeOnlineOptimization(){
// compute the size of the preconditioning matrix
int sz=maxIndex()+1;
DEBUG(1) << "Size= " << sz << endl;
//DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
DEBUG(1) << "allocating M(" << sz << ")" << endl;
//DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
}
@@ -114,8 +114,8 @@ void TreeOptimizer2::computePreconditioner(){
int edgeCount=0;
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
if (! (edgeCount%10000))
DEBUG(1) << "m";
//if (! (edgeCount%10000))
// DEBUG(1) << "m";
Edge* e=*it;
Transformation t=e->transformation;
@@ -166,7 +166,7 @@ void TreeOptimizer2::propagateErrors(){
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
if (! (edgeCount%10000)) DEBUG(1) << "c";
//if (! (edgeCount%10000)) DEBUG(1) << "c";
Edge* e=*it;
Vertex* top=e->top;
@@ -176,13 +176,13 @@ void TreeOptimizer2::propagateErrors(){
Vertex* v2=e->v2;
double l=e->length;
DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
//DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
Pose p1=getPose(v1, top);
Pose p2=getPose(v2, top);
DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
//DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
//DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
Transformation et=e->transformation;
Transformation t1(p1);
@@ -191,13 +191,13 @@ void TreeOptimizer2::propagateErrors(){
Transformation t12=t1*et;
Pose p12=t12.toPoseType();
DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
//DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
double angle=r.theta();
angle=atan2(sin(angle),cos(angle));
r.theta()=angle;
DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
//DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
InformationMatrix S=e->informationMatrix;
InformationMatrix R;
@@ -216,7 +216,7 @@ void TreeOptimizer2::propagateErrors(){
InformationMatrix W=R*S*R.transpose();
Pose d=W*r*2.;
DEBUG(2) << " d=" << d.x() << " " << d.y() << " " << d.theta() << endl;
//DEBUG(2) << " d=" << d.x() << " " << d.y() << " " << d.theta() << endl;
assert(l>0);
@@ -239,8 +239,8 @@ void TreeOptimizer2::propagateErrors(){
beta[1]=(fabs(beta[1])>fabs(r.values[1]))?r.values[1]:beta[1];
beta[2]=(fabs(beta[2])>fabs(r.values[2]))?r.values[2]:beta[2];
DEBUG(2) << " alpha=" << alpha[0] << " " << alpha[1] << " " << alpha[2] << endl;
DEBUG(2) << " beta=" << beta[0] << " " << beta[1] << " " << beta[2] << endl;
//DEBUG(2) << " alpha=" << alpha[0] << " " << alpha[1] << " " << alpha[2] << endl;
//DEBUG(2) << " beta=" << beta[0] << " " << beta[1] << " " << beta[2] << endl;
for (int dir=0; dir<2; dir++) {
Vertex* n = (dir==0)? v1 : v2;
@@ -253,13 +253,13 @@ void TreeOptimizer2::propagateErrors(){
Pose delta( beta[0]/(M[i].values[0]*tw[0]), beta[1]/(M[i].values[1]*tw[1]), beta[2]/(M[i].values[2]*tw[2]));
delta=delta*sign;
DEBUG(2) << " " << dir << ":" << i <<"," << n->parent->id << ":"
<< n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta() << " -> ";
//DEBUG(2) << " " << dir << ":" << i <<"," << n->parent->id << ":"
// << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta() << " -> ";
n->parameters.x()+=delta.x();
n->parameters.y()+=delta.y();
n->parameters.theta()+=delta.theta();
DEBUG(2) << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta()<< endl;
//DEBUG(2) << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta()<< endl;
n=n->parent;
}
}
@@ -269,9 +269,9 @@ void TreeOptimizer2::propagateErrors(){
Pose pf1=v1->pose;
Pose pf2=v2->pose;
DEBUG(2) << " pf1=" << pf1.x() << " " << pf1.y() << " " << pf1.theta() << endl;
DEBUG(2) << " pf2=" << pf2.x() << " " << pf2.y() << " " << pf2.theta() << endl;
DEBUG(2) << " en=" << p12.x()-pf2.x() << " " << p12.y()-pf2.y() << " " << p12.theta()-pf2.theta() << endl;
//DEBUG(2) << " pf1=" << pf1.x() << " " << pf1.y() << " " << pf1.theta() << endl;
//DEBUG(2) << " pf2=" << pf2.x() << " " << pf2.y() << " " << pf2.theta() << endl;
//DEBUG(2) << " en=" << p12.x()-pf2.x() << " " << p12.y()-pf2.y() << " " << p12.theta()-pf2.theta() << endl;
}
}
@@ -320,8 +320,8 @@ double TreeOptimizer2::error(const Edge* e) const{
Pose p1=v1->pose;
Pose p2=v2->pose;
DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
//DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
//DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
Transformation et=e->transformation;
Transformation t1(p1);
@@ -330,13 +330,13 @@ double TreeOptimizer2::error(const Edge* e) const{
Transformation t12=t1*et;
Pose p12=t12.toPoseType();
DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
//DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
double angle=r.theta();
angle=atan2(sin(angle),cos(angle));
r.theta()=angle;
DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
//DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
InformationMatrix S=e->informationMatrix;
InformationMatrix R;

22
corelib/src/toro3d/treeoptimizer3.cpp
View File

@@ -50,8 +50,8 @@ using namespace std;
namespace AISNavigation {
#define DEBUG(i) \
if (verboseLevel>i) cerr
//#define DEBUG(i) \
// if (verboseLevel>i) cerr
TreeOptimizer3::TreeOptimizer3(){
@@ -180,7 +180,7 @@ double TreeOptimizer3::error(const Edge* e) const{
Pose ps=e->informationMatrix*p12;
double err=p12*ps;
DEBUG(100) << "e(" << v1->id << "," << v2->id << ")" << err << endl;
//DEBUG(100) << "e(" << v1->id << "," << v2->id << ")" << err << endl;
return err;
}
@@ -294,10 +294,10 @@ void TreeOptimizer3::initializeOptimization(EdgeCompareMode mode){
edgeCompareMode=mode;
// compute the size of the preconditioning matrix
int sz=maxIndex()+1;
DEBUG(1) << "Size= " << sz << endl;
//DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
DEBUG(1) << "allocating M(" << sz << ")" << endl;
//DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
// sorting edges
@@ -313,9 +313,9 @@ void TreeOptimizer3::initializeOptimization(EdgeCompareMode mode){
void TreeOptimizer3::initializeOnlineIterations(){
int sz=maxIndex()+1;
DEBUG(1) << "Size= " << sz << endl;
//DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
DEBUG(1) << "allocating M(" << sz << ")" << endl;
//DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
maxRotationalErrors.clear();
maxTranslationalErrors.clear();
@@ -337,18 +337,18 @@ void TreeOptimizer3::initializeOnlineOptimization(EdgeCompareMode mode){
}
void TreeOptimizer3::onStepStart(Edge* e){
DEBUG(5) << "entering edge" << e << endl;
//DEBUG(5) << "entering edge" << e << endl;
}
void TreeOptimizer3::onStepFinished(Edge* e){
DEBUG(5) << "exiting edge" << e << endl;
//DEBUG(5) << "exiting edge" << e << endl;
}
void TreeOptimizer3::onIterationStart(int iteration){
DEBUG(5) << "entering iteration " << iteration << endl;
//DEBUG(5) << "entering iteration " << iteration << endl;
}
void TreeOptimizer3::onIterationFinished(int iteration){
DEBUG(5) << "exiting iteration " << iteration << endl;
//DEBUG(5) << "exiting iteration " << iteration << endl;
}
void TreeOptimizer3::onRestartBegin(){}

14
corelib/src/toro3d/treeoptimizer3_iteration.cpp
View File

@@ -42,8 +42,8 @@ using namespace std;
namespace AISNavigation {
#define DEBUG(i) \
if (verboseLevel>i) cerr
//#define DEBUG(i) \
// if (verboseLevel>i) cerr
//helper functions. Should I explain :-)?
inline double max3( const double& a, const double& b, const double& c){
@@ -94,8 +94,8 @@ void TreeOptimizer3::computePreconditioner(){
int edgeCount=0;
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
if (! (edgeCount%1000))
DEBUG(1) << "m";
//if (! (edgeCount%1000))
// DEBUG(1) << "m";
Edge* e=*it;
//Transformation t=e->transformation;
@@ -138,8 +138,8 @@ void TreeOptimizer3::propagateErrors(bool usePreconditioner){
onIterationStart(iteration);
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
if (! (edgeCount%1000))
DEBUG(1) << "c";
//if (! (edgeCount%1000))
// DEBUG(1) << "c";
if (isDone())
return;
@@ -153,7 +153,7 @@ void TreeOptimizer3::propagateErrors(bool usePreconditioner){
recomputeTransformations(v1,top);
recomputeTransformations(v2,top);
DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
//DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
//BEGIN: Path and weight computation
int pc=0;

265
corelib/src/util2d.cpp
View File

@@ -957,13 +957,22 @@ float getDepth(
const cv::Mat & depthImage,
float x, float y,
bool smoothing,
float maxZError)
float maxZError,
bool estWithNeighborsIfNull)
{
UASSERT(!depthImage.empty());
UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
int u = int(x+0.5f);
int v = int(y+0.5f);
if(u == depthImage.cols && x<float(depthImage.cols))
{
u = depthImage.cols - 1;
}
if(v == depthImage.rows && y<float(depthImage.rows))
{
v = depthImage.rows - 1;
}
if(!(u >=0 && u<depthImage.cols && v >=0 && v<depthImage.rows))
{
@@ -986,6 +995,41 @@ float getDepth(
int v_end = std::min(v+1, depthImage.rows-1);
float depth = isInMM?(float)depthImage.at<unsigned short>(v,u)*0.001f:depthImage.at<float>(v,u);
if((depth==0.0f || !uIsFinite(depth)) && estWithNeighborsIfNull)
{
// all cells no2 must be under the zError to be accepted
float tmp = 0.0f;
int count = 0;
for(int uu = u_start; uu <= u_end; ++uu)
{
for(int vv = v_start; vv <= v_end; ++vv)
{
if((uu == u && vv!=v) || (uu != u && vv==v))
{
float d = isInMM?(float)depthImage.at<unsigned short>(vv,uu)*0.001f:depthImage.at<float>(vv,uu);
if(d!=0.0f && uIsFinite(d))
{
if(tmp == 0.0f)
{
tmp = d;
++count;
}
else if(fabs(d - tmp) < maxZError)
{
tmp+=d;
++count;
}
}
}
}
}
if(count > 1)
{
depth = tmp/float(count);
}
}
if(depth!=0.0f && uIsFinite(depth))
{
if(smoothing)
@@ -1078,6 +1122,117 @@ cv::Mat decimate(const cv::Mat & image, int decimation)
return out;
}
cv::Mat interpolate(const cv::Mat & image, int factor, float depthErrorRatio)
{
UASSERT(factor >= 1);
cv::Mat out;
if(!image.empty())
{
if(factor > 1)
{
if((image.type() == CV_32FC1 || image.type()==CV_16UC1))
{
UASSERT(depthErrorRatio>0.0f);
out = cv::Mat::zeros(image.rows*factor, image.cols*factor, image.type());
for(int j=0; j<out.rows; j+=factor)
{
for(int i=0; i<out.cols; i+=factor)
{
if(i>0 && j>0)
{
float dTopLeft;
float dTopRight;
float dBottomLeft;
float dBottomRight;
if(image.type() == CV_32FC1)
{
dTopLeft = image.at<float>(j/factor-1, i/factor-1);
dTopRight = image.at<float>(j/factor-1, i/factor);
dBottomLeft = image.at<float>(j/factor, i/factor-1);
dBottomRight = image.at<float>(j/factor, i/factor);
}
else
{
dTopLeft = image.at<unsigned short>(j/factor-1, i/factor-1);
dTopRight = image.at<unsigned short>(j/factor-1, i/factor);
dBottomLeft = image.at<unsigned short>(j/factor, i/factor-1);
dBottomRight = image.at<unsigned short>(j/factor, i/factor);
}
if(dTopLeft>0 && dTopRight>0 && dBottomLeft>0 && dBottomRight > 0)
{
float depthError = depthErrorRatio*(dTopLeft+dTopRight+dBottomLeft+dBottomRight)/4.0f;
if(fabs(dTopLeft-dTopRight) <= depthError &&
fabs(dTopLeft-dBottomLeft) <= depthError &&
fabs(dTopLeft-dBottomRight) <= depthError)
{
// bilinear interpolation
// do first and last rows then columns
float slopeTop = (dTopRight-dTopLeft)/float(factor);
float slopeBottom = (dBottomRight-dBottomLeft)/float(factor);
if(image.type() == CV_32FC1)
{
for(int z=i-factor; z<=i; ++z)
{
out.at<float>(j-factor, z) = dTopLeft+(slopeTop*float(z-(i-factor)));
out.at<float>(j, z) = dBottomLeft+(slopeBottom*float(z-(i-factor)));
}
}
else
{
for(int z=i-factor; z<=i; ++z)
{
out.at<unsigned short>(j-factor, z) = (unsigned short)(dTopLeft+(slopeTop*float(z-(i-factor))));
out.at<unsigned short>(j, z) = (unsigned short)(dBottomLeft+(slopeBottom*float(z-(i-factor))));
}
}
// fill the columns
if(image.type() == CV_32FC1)
{
for(int z=i-factor; z<=i; ++z)
{
float top = out.at<float>(j-factor, z);
float bottom = out.at<float>(j, z);
float slope = (bottom-top)/float(factor);
for(int d=j-factor+1; d<j; ++d)
{
out.at<float>(d, z) = top+(slope*float(d-(j-factor)));
}
}
}
else
{
for(int z=i-factor; z<=i; ++z)
{
float top = out.at<unsigned short>(j-factor, z);
float bottom = out.at<unsigned short>(j, z);
float slope = (bottom-top)/float(factor);
for(int d=j-factor+1; d<j; ++d)
{
out.at<unsigned short>(d, z) = (unsigned short)(top+(slope*float(d-(j-factor))));
}
}
}
}
}
}
}
}
}
else
{
cv::resize(image, out, cv::Size(), float(factor), float(factor));
}
}
else
{
out = image;
}
}
return out;
}
// Registration Depth to RGB (return registered depth image)
cv::Mat registerDepth(
const cv::Mat & depth,
@@ -1152,6 +1307,114 @@ cv::Mat registerDepth(
return registered;
}
cv::Mat fillDepthHoles(const cv::Mat & registeredDepth, int maximumHoleSize, float errorRatio)
{
UASSERT(registeredDepth.type() == CV_16UC1);
UASSERT(maximumHoleSize > 0);
cv::Mat output = registeredDepth.clone();
for(int y=0; y<registeredDepth.rows-2; ++y)
{
for(int x=0; x<registeredDepth.cols-2; ++x)
{
float a = registeredDepth.at<unsigned short>(y, x);
float bRight = registeredDepth.at<unsigned short>(y, x+1);
float bDown = registeredDepth.at<unsigned short>(y+1, x);
if(a > 0.0f && (bRight == 0.0f || bDown == 0.0f))
{
bool horizontalSet = bRight != 0.0f;
bool verticalSet = bDown != 0.0f;
int stepX = 0;
for(int h=1; h<=maximumHoleSize && (!horizontalSet || !verticalSet); ++h)
{
// horizontal
if(!horizontalSet)
{
if(x+1+h >= registeredDepth.cols)
{
horizontalSet = true;
}
else
{
float c = registeredDepth.at<unsigned short>(y, x+1+h);
if(c == 0)
{
// ignore this size
}
else
{
// fill hole
float depthError = errorRatio*float(a+c)/2.0f;
if(fabs(a-c) <= depthError)
{
//linear interpolation
float slope = (c-a)/float(h+1);
for(int z=x+1; z<x+1+h; ++z)
{
if(output.at<unsigned short>(y, z) == 0)
{
output.at<unsigned short>(y, z) = (unsigned short)(a+(slope*float(z-x)));
}
else
{
// average with the previously set value
output.at<unsigned short>(y, z) = (output.at<unsigned short>(y, z)+(unsigned short)(a+(slope*float(z-x))))/2;
}
}
}
horizontalSet = true;
stepX = h;
}
}
}
// vertical
if(!verticalSet)
{
if(y+1+h >= registeredDepth.rows)
{
verticalSet = true;
}
else
{
float c = registeredDepth.at<unsigned short>(y+1+h, x);
if(c == 0)
{
// ignore this size
}
else
{
// fill hole
float depthError = errorRatio*float(a+c)/2.0f;
if(fabs(a-c) <= depthError)
{
//linear interpolation
float slope = (c-a)/float(h+1);
for(int z=y+1; z<y+1+h; ++z)
{
if(output.at<unsigned short>(z, x) == 0)
{
output.at<unsigned short>(z, x) = (unsigned short)(a+(slope*float(z-y)));
}
else
{
// average with the previously set value
output.at<unsigned short>(z, x) = (output.at<unsigned short>(z, x)+(unsigned short)(a+(slope*float(z-y))))/2;
}
}
}
verticalSet = true;
}
}
}
}
x+=stepX;
}
}
}
return output;
}
void fillRegisteredDepthHoles(cv::Mat & registeredDepth, bool vertical, bool horizontal, bool fillDoubleHoles)
{
UASSERT(registeredDepth.type() == CV_16UC1);

312
corelib/src/util3d.cpp
View File

@@ -221,7 +221,7 @@ pcl::PointXYZ projectDepthTo3D(
pcl::PointXYZ pt;
float depth = util2d::getDepth(depthImage, x, y, smoothing, maxZError);
if(depth)
if(depth > 0.0f)
{
// Use correct principal point from calibration
cx = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
@@ -243,7 +243,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
const cv::Mat & imageDepth,
float cx, float cy,
float fx, float fy,
int decimation)
int decimation,
float maxDepth,
std::vector<int> * validIndices)
{
UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
UASSERT(imageDepth.rows % decimation == 0);
@@ -259,11 +261,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
cloud->height = imageDepth.rows/decimation;
cloud->width = imageDepth.cols/decimation;
cloud->is_dense = false;
cloud->resize(cloud->height * cloud->width);
if(validIndices)
{
validIndices->resize(cloud->size());
}
int count = 0 ;
int oi = 0;
for(int h = 0; h < imageDepth.rows; h+=decimation)
{
for(int w = 0; w < imageDepth.cols; w+=decimation)
@@ -271,12 +275,26 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
++count;
if(maxDepth<=0.0f || ptXYZ.z <= maxDepth)
{
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
if(validIndices)
{
validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
}
}
else
{
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
}
}
}
if(validIndices)
{
validIndices->reserve(oi);
}
return cloud;
}
@@ -286,13 +304,15 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
const cv::Mat & imageDepth,
float cx, float cy,
float fx, float fy,
int decimation)
int decimation,
float maxDepth,
std::vector<int> * validIndices)
{
UDEBUG("");
UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
UASSERT(imageRgb.rows % imageDepth.rows == 0 && imageRgb.cols % imageDepth.cols == 0);
UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("imageDepth.rows=%d decimation=%d", imageDepth.rows, decimation).c_str());
UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("imageDepth.cols=%d decimation=%d", imageDepth.rows, decimation).c_str());
UASSERT_MSG(imageRgb.rows % decimation == 0, uFormat("imageDepth.rows=%d decimation=%d", imageRgb.rows, decimation).c_str());
UASSERT_MSG(imageRgb.cols % decimation == 0, uFormat("imageDepth.cols=%d decimation=%d", imageRgb.cols, decimation).c_str());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(decimation < 1)
@@ -315,23 +335,41 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
}
//cloud.header = cameraInfo.header;
cloud->height = imageDepth.rows/decimation;
cloud->width = imageDepth.cols/decimation;
cloud->height = imageRgb.rows/decimation;
cloud->width = imageRgb.cols/decimation;
cloud->is_dense = false;
cloud->resize(cloud->height * cloud->width);
for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
if(validIndices)
{
for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
validIndices->resize(cloud->size());
}
float rgbToDepthFactorX = 1.0f/float((imageRgb.cols / imageDepth.cols));
float rgbToDepthFactorY = 1.0f/float((imageRgb.rows / imageDepth.rows));
float depthFx = fx * rgbToDepthFactorX;
float depthFy = fy * rgbToDepthFactorY;
float depthCx = cx * rgbToDepthFactorX;
float depthCy = cy * rgbToDepthFactorY;
UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
imageRgb.cols, imageRgb.rows,
imageDepth.cols, imageDepth.rows,
fx, fy, cx, cy,
rgbToDepthFactorX,
rgbToDepthFactorY,
decimation);
int oi = 0;
for(int h = 0; h < imageRgb.rows && h/decimation < (int)cloud->height; h+=decimation)
{
for(int w = 0; w < imageRgb.cols && w/decimation < (int)cloud->width; w+=decimation)
{
pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
bool invalidColor = false;
if(!mono)
{
pt.b = imageRgb.at<cv::Vec3b>(h,w)[0];
pt.g = imageRgb.at<cv::Vec3b>(h,w)[1];
pt.r = imageRgb.at<cv::Vec3b>(h,w)[2];
invalidColor = pt.b <= 5 && pt.g <= 5 && pt.r <= 5;
}
else
{
@@ -339,29 +377,44 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
pt.b = v;
pt.g = v;
pt.r = v;
invalidColor = v <= 0;
}
if(invalidColor)
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w*rgbToDepthFactorX, h*rgbToDepthFactorY, depthCx, depthCy, depthFx, depthFy, false);
if(pcl::isFinite(ptXYZ) && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
{
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
}
else
{
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
if(validIndices)
{
validIndices->at(oi) = (h/decimation)*cloud->width + (w/decimation);
}
++oi;
}
else
{
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
}
}
}
if(validIndices)
{
validIndices->resize(oi);
}
if(oi == 0)
{
UWARN("Cloud with only NaN values created!");
}
UDEBUG("");
return cloud;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
const cv::Mat & imageDisparity,
const StereoCameraModel & model,
int decimation)
int decimation,
float maxDepth,
std::vector<int> * validIndices)
{
UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
UASSERT(imageDisparity.rows % decimation == 0);
@@ -375,7 +428,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
cloud->width = imageDisparity.cols/decimation;
cloud->is_dense = false;
cloud->resize(cloud->height * cloud->width);
if(validIndices)
{
validIndices->resize(cloud->size());
}
int oi = 0;
if(imageDisparity.type()==CV_16SC1)
{
for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
@@ -384,7 +442,21 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
{
float disp = float(imageDisparity.at<short>(h,w))/16.0f;
cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
if(maxDepth <= 0.0f || pt.z <= maxDepth)
{
cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
if(validIndices)
{
validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
}
}
else
{
cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(
std::numeric_limits<float>::quiet_NaN(),
std::numeric_limits<float>::quiet_NaN(),
std::numeric_limits<float>::quiet_NaN());
}
}
}
}
@@ -396,10 +468,28 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
{
float disp = imageDisparity.at<float>(h,w);
cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
if(maxDepth <= 0.0f || pt.z <= maxDepth)
{
cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
if(validIndices)
{
validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
}
}
else
{
cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(
std::numeric_limits<float>::quiet_NaN(),
std::numeric_limits<float>::quiet_NaN(),
std::numeric_limits<float>::quiet_NaN());
}
}
}
}
if(validIndices)
{
validIndices->resize(oi);
}
return cloud;
}
@@ -407,7 +497,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDisparity,
const StereoCameraModel & model,
int decimation)
int decimation,
float maxDepth,
std::vector<int> * validIndices)
{
UASSERT(!imageRgb.empty() && !imageDisparity.empty());
UASSERT(imageRgb.rows == imageDisparity.rows &&
@@ -434,7 +526,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
cloud->width = imageRgb.cols/decimation;
cloud->is_dense = false;
cloud->resize(cloud->height * cloud->width);
if(validIndices)
{
validIndices->resize(cloud->size());
}
int oi=0;
for(int h = 0; h < imageRgb.rows && h/decimation < (int)cloud->height; h+=decimation)
{
for(int w = 0; w < imageRgb.cols && w/decimation < (int)cloud->width; w+=decimation)
@@ -456,11 +553,26 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
float disp = imageDisparity.type()==CV_16SC1?float(imageDisparity.at<short>(h,w))/16.0f:imageDisparity.at<float>(h,w);
cv::Point3f ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
if(util3d::isFinite(ptXYZ) && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
{
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
if(validIndices)
{
validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
}
}
else
{
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
}
}
}
if(validIndices)
{
validIndices->resize(oi);
}
return cloud;
}
@@ -468,7 +580,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
const cv::Mat & imageLeft,
const cv::Mat & imageRight,
const StereoCameraModel & model,
int decimation)
int decimation,
float maxDepth,
std::vector<int> * validIndices)
{
UASSERT(!imageLeft.empty() && !imageRight.empty());
UASSERT(imageRight.type() == CV_8UC1);
@@ -505,7 +619,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
leftColor,
util2d::disparityFromStereoImages(leftMono, rightMono),
modelDecimation,
decimation);
decimation,
maxDepth,
validIndices);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
@@ -513,7 +629,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
int decimation,
float maxDepth,
float voxelSize,
int samples)
int samples,
std::vector<int> * validIndices)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -532,16 +649,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
sensorData.cameraModels()[i].cy(),
sensorData.cameraModels()[i].fx(),
sensorData.cameraModels()[i].fy(),
decimation);
decimation,
maxDepth,
sensorData.cameraModels().size()==1?validIndices:0);
if(tmp->size())
{
bool filtered = false;
if(tmp->size() && maxDepth)
{
tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
filtered = true;
}
if(tmp->size() && voxelSize)
{
@@ -555,7 +669,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
filtered = true;
}
if(tmp->size() && !filtered)
if(tmp->size() && !filtered && sensorData.cameraModels().size() > 1)
{
tmp = util3d::removeNaNFromPointCloud(tmp);
}
@@ -565,7 +679,14 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
}
*cloud += *tmp;
if(sensorData.cameraModels().size() > 1)
{
*cloud += *tmp;
}
else
{
cloud = tmp;
}
}
}
else
@@ -574,7 +695,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
}
}
if(cloud->size() && voxelSize)
if(cloud->size() && voxelSize && sensorData.cameraModels().size() > 1)
{
cloud = util3d::voxelize(cloud, voxelSize);
}
@@ -596,26 +717,15 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
cloud = cloudFromDisparity(
util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()),
sensorData.stereoCameraModel(),
decimation);
decimation,
maxDepth,
validIndices);
if(cloud->size())
{
bool filtered = false;
if(cloud->size() && maxDepth)
{
cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
filtered = true;
}
if(cloud->size() && voxelSize)
{
cloud = util3d::voxelize(cloud, voxelSize);
filtered = true;
}
if(cloud->size() && !filtered)
{
cloud = util3d::removeNaNFromPointCloud(cloud);
}
if(cloud->size())
@@ -632,7 +742,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
int decimation,
float maxDepth,
float voxelSize,
int samples)
int samples,
std::vector<int> * validIndices)
{
UASSERT(!sensorData.imageRaw().empty());
UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) ||
@@ -644,37 +755,40 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
//depth
UDEBUG("");
UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
UASSERT(sensorData.depthRaw().size() == sensorData.imageRaw().size());
int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
UASSERT(sensorData.imageRaw().cols % sensorData.depthRaw().cols == 0);
UASSERT(sensorData.imageRaw().rows % sensorData.depthRaw().rows == 0);
int subRGBWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
int subDepthWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
if(subRGBWidth % decimation != 0 || subDepthWidth % decimation != 0)
{
UWARN("Image size (rgb=%d,%d depth=%d,%d) modulus decimation (%d) is not null "
"for the cloud creation! Setting decimation to 1...",
subRGBWidth, sensorData.imageRaw().rows,
subDepthWidth, sensorData.depthRaw().rows,
decimation);
decimation = 1;
}
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
if(sensorData.cameraModels()[i].isValidForProjection())
{
if(subImageWidth % decimation != 0 || sensorData.depthRaw().rows % decimation != 0)
{
UWARN("Image size (%d,%d) modulus decimation (%d) is not null "
"for the cloud creation! Setting decimation to 1...",
subImageWidth, sensorData.depthRaw().rows, decimation);
decimation = 1;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)),
cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
cv::Mat(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows)),
cv::Mat(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows)),
sensorData.cameraModels()[i].cx(),
sensorData.cameraModels()[i].cy(),
sensorData.cameraModels()[i].fx(),
sensorData.cameraModels()[i].fy(),
decimation);
decimation,
maxDepth,
sensorData.cameraModels().size() == 1?validIndices:0);
if(tmp->size())
{
bool filtered = false;
if(tmp->size() && maxDepth)
{
tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
filtered = true;
}
if(tmp->size() && voxelSize)
{
tmp = util3d::voxelize(tmp, voxelSize);
@@ -687,7 +801,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
filtered = true;
}
if(tmp->size() && !filtered)
if(tmp->size() && !filtered && sensorData.cameraModels().size() > 1)
{
tmp = util3d::removeNaNFromPointCloud(tmp);
}
@@ -697,7 +811,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
}
*cloud += *tmp;
if(sensorData.cameraModels().size() > 1)
{
*cloud += *tmp;
}
else
{
cloud = tmp;
}
}
}
else
@@ -706,10 +827,20 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
}
}
if(cloud->size() && voxelSize)
if(cloud->size() && voxelSize && sensorData.cameraModels().size() > 1)
{
cloud = util3d::voxelize(cloud, voxelSize);
}
if(cloud->is_dense && validIndices)
{
//generate indices for all points (they are all valid)
validIndices->resize(cloud->size());
for(unsigned int i=0; i<cloud->size(); ++i)
{
validIndices->at(i) = i;
}
}
}
else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection())
{
@@ -718,26 +849,15 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
cloud = cloudFromStereoImages(sensorData.imageRaw(),
sensorData.rightRaw(),
sensorData.stereoCameraModel(),
decimation);
decimation,
maxDepth,
validIndices);
if(cloud->size())
{
bool filtered = false;
if(cloud->size() && maxDepth)
{
cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
filtered = true;
}
if(cloud->size() && voxelSize)
{
cloud = util3d::voxelize(cloud, voxelSize);
filtered = true;
}
if(cloud->size() && !filtered)
{
cloud = util3d::removeNaNFromPointCloud(cloud);
}
if(cloud->size())

47
corelib/src/util3d_features.cpp
View File

@@ -72,20 +72,24 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(
UASSERT(int((depth.cols/cameraModels.size())*cameraModels.size()) == depth.cols);
float subImageWidth = depth.cols/cameraModels.size();
keypoints3d.resize(keypoints.size());
float rgbToDepthFactorX = 1.0f/(cameraModels[0].imageWidth()>0?cameraModels[0].imageWidth()/depth.cols:1);
float rgbToDepthFactorY = 1.0f/(cameraModels[0].imageHeight()>0?cameraModels[0].imageHeight()/depth.rows:1);
for(unsigned int i=0; i!=keypoints.size(); ++i)
{
int cameraIndex = int(keypoints[i].pt.x / subImageWidth);
float x = keypoints[i].pt.x*rgbToDepthFactorX;
float y = keypoints[i].pt.y*rgbToDepthFactorY;
int cameraIndex = int(x / subImageWidth);
UASSERT_MSG(cameraIndex < (int)cameraModels.size(),
uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f",
cameraIndex, (int)cameraModels.size(), keypoints[i].pt.x, subImageWidth).c_str());
uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
cameraIndex, (int)cameraModels.size(), keypoints[i].pt.x, subImageWidth, cameraModels[0].imageWidth()).c_str());
pcl::PointXYZ ptXYZ = util3d::projectDepthTo3D(
depth,
keypoints[i].pt.x-subImageWidth*cameraIndex,
keypoints[i].pt.y,
cameraModels.at(cameraIndex).cx(),
cameraModels.at(cameraIndex).cy(),
cameraModels.at(cameraIndex).fx(),
cameraModels.at(cameraIndex).fy(),
x-subImageWidth*cameraIndex,
y,
cameraModels.at(cameraIndex).cx()*rgbToDepthFactorX,
cameraModels.at(cameraIndex).cy()*rgbToDepthFactorY,
cameraModels.at(cameraIndex).fx()*rgbToDepthFactorX,
cameraModels.at(cameraIndex).fy()*rgbToDepthFactorY,
true);
cv::Point3f pt(ptXYZ.x, ptXYZ.y, ptXYZ.z);
@@ -192,7 +196,9 @@ std::map<int, cv::Point3f> generateWords3DMono(
UASSERT(cameraModel.isValidForProjection());
std::map<int, cv::Point3f> words3D;
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
if(EpipolarGeometry::findPairs(refWords, nextWords, pairs) > 8)
int pairsFound = EpipolarGeometry::findPairs(refWords, nextWords, pairs);
UDEBUG("pairsFound=%d", pairsFound);
if(pairsFound > 8)
{
std::vector<unsigned char> status;
cv::Mat F = EpipolarGeometry::findFFromWords(pairs, status, ransacParam1, ransacParam2);
@@ -277,6 +283,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
cv::Mat pts4D;
cv::triangulatePoints(P0, P, x_norm, xp_norm, pts4D);
UASSERT((int)indexes.size() == pts4D.cols && pts4D.rows == 4);
for(unsigned int i=0; i<indexes.size(); ++i)
{
//if(cloud->at(i).z > 0)
@@ -290,6 +297,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
}
}
UDEBUG("ref guess=%d", (int)refGuess3D.size());
if(refGuess3D.size())
{
// scale estimation
@@ -359,24 +367,25 @@ std::map<int, cv::Point3f> generateWords3DMono(
{
std::vector<cv::Point3f> objectPoints(indexes.size());
std::vector<cv::Point2f> imagePoints(indexes.size());
int oi=0;
int oi2=0;
UASSERT(indexes.size() == newCorners.size());
for(unsigned int i=0; i<indexes.size(); ++i)
{
std::map<int, cv::Point3f>::iterator iter = words3D.find(indexes[i]);
if(util3d::isFinite(iter->second))
if(iter!=words3D.end() && util3d::isFinite(iter->second))
{
iter->second.x *= scale;
iter->second.y *= scale;
iter->second.z *= scale;
objectPoints[oi].x = iter->second.x;
objectPoints[oi].y = iter->second.y;
objectPoints[oi].z = iter->second.z;
imagePoints[oi] = newCorners[i];
++oi;
objectPoints[oi2].x = iter->second.x;
objectPoints[oi2].y = iter->second.y;
objectPoints[oi2].z = iter->second.z;
imagePoints[oi2] = newCorners[i];
++oi2;
}
}
objectPoints.resize(oi);
imagePoints.resize(oi);
objectPoints.resize(oi2);
imagePoints.resize(oi2);
//PnPRansac
Transform guess = cameraModel.localTransform().inverse();

611
corelib/src/util3d_filtering.cpp
View File

@@ -42,6 +42,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/segmentation/extract_clusters.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
namespace rtabmap
{
@@ -144,6 +146,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(
pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize)
{
UASSERT(voxelSize > 0.0f);
@@ -151,11 +154,16 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
pcl::VoxelGrid<pcl::PointXYZ> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
if(indices->size())
{
filter.setIndices(indices);
}
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize)
{
UASSERT(voxelSize > 0.0f);
@@ -163,11 +171,16 @@ pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(
pcl::VoxelGrid<pcl::PointNormal> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
if(indices->size())
{
filter.setIndices(indices);
}
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize)
{
UASSERT(voxelSize > 0.0f);
@@ -175,11 +188,16 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
pcl::VoxelGrid<pcl::PointXYZRGB> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
if(indices->size())
{
filter.setIndices(indices);
}
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize)
{
UASSERT(voxelSize > 0.0f);
@@ -187,10 +205,43 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
pcl::VoxelGrid<pcl::PointXYZRGBNormal> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
if(indices->size())
{
filter.setIndices(indices);
}
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize)
{
pcl::IndicesPtr indices(new std::vector<int>);
return voxelize(cloud, indices, voxelSize);
}
pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
float voxelSize)
{
pcl::IndicesPtr indices(new std::vector<int>);
return voxelize(cloud, indices, voxelSize);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float voxelSize)
{
pcl::IndicesPtr indices(new std::vector<int>);
return voxelize(cloud, indices, voxelSize);
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
float voxelSize)
{
pcl::IndicesPtr indices(new std::vector<int>);
return voxelize(cloud, indices, voxelSize);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr randomSampling(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
@@ -256,6 +307,39 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
return output;
}
pcl::IndicesPtr frustumFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Transform & cameraPose,
float horizontalFOV, // in degrees
float verticalFOV, // in degrees
float nearClipPlaneDistance,
float farClipPlaneDistance,
bool negative)
{
UASSERT(horizontalFOV > 0.0f && verticalFOV > 0.0f);
UASSERT(farClipPlaneDistance > nearClipPlaneDistance);
UASSERT(!cameraPose.isNull());
pcl::IndicesPtr output(new std::vector<int>);
pcl::FrustumCulling<pcl::PointXYZ> fc;
fc.setNegative(negative);
fc.setInputCloud (cloud);
if(indices.get() && indices->size())
{
fc.setIndices(indices);
}
fc.setVerticalFOV (verticalFOV);
fc.setHorizontalFOV (horizontalFOV);
fc.setNearPlaneDistance (nearClipPlaneDistance);
fc.setFarPlaneDistance (farClipPlaneDistance);
fc.setCameraPose (cameraPose.toEigen4f());
fc.filter (*output);
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr frustumFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & cameraPose,
@@ -366,6 +450,14 @@ pcl::IndicesPtr radiusFiltering(
pcl::IndicesPtr indices(new std::vector<int>);
return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
}
pcl::IndicesPtr radiusFiltering(
@@ -458,6 +550,51 @@ pcl::IndicesPtr radiusFiltering(
return output;
}
}
pcl::IndicesPtr radiusFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>(false));
if(indices->size())
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud, indices);
for(unsigned int i=0; i<indices->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
if(k > minNeighborsInRadius)
{
output->at(oi++) = indices->at(i);
}
}
output->resize(oi);
return output;
}
else
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud);
for(unsigned int i=0; i<cloud->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
if(k > minNeighborsInRadius)
{
output->at(oi++) = i;
}
}
output->resize(oi);
return output;
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -481,6 +618,7 @@ pcl::IndicesPtr subtractFiltering(
float radiusSearch,
int minNeighborsInRadius)
{
UASSERT(minNeighborsInRadius > 0);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>(false));
if(indices->size())
@@ -500,7 +638,7 @@ pcl::IndicesPtr subtractFiltering(
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
if(k <= minNeighborsInRadius)
if(k < minNeighborsInRadius)
{
output->at(oi++) = indices->at(i);
}
@@ -525,7 +663,7 @@ pcl::IndicesPtr subtractFiltering(
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
if(k <= minNeighborsInRadius)
if(k < minNeighborsInRadius)
{
output->at(oi++) = i;
}
@@ -539,10 +677,11 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
float radiusSearch,
float maxAngle,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out;
@@ -555,8 +694,10 @@ pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
float radiusSearch,
float maxAngle,
int minNeighborsInRadius)
{
UASSERT(minNeighborsInRadius > 0);
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>(false));
if(indices->size())
@@ -576,7 +717,39 @@ pcl::IndicesPtr subtractFiltering(
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
if(k <= minNeighborsInRadius)
if(k>=minNeighborsInRadius && maxAngle > 0.0f)
{
Eigen::Vector4f normal(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
if (uIsFinite(normal[0]) &&
uIsFinite(normal[1]) &&
uIsFinite(normal[2]))
{
int count = k;
for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
{
Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
if(uIsFinite(v[0]) &&
uIsFinite(v[1]) &&
uIsFinite(v[2]))
{
float angle = pcl::getAngle3D(normal, v);
if(angle > maxAngle)
{
k-=1;
}
}
else
{
k-=1;
}
}
}
else
{
k=0;
}
}
if(k < minNeighborsInRadius)
{
output->at(oi++) = indices->at(i);
}
@@ -601,7 +774,39 @@ pcl::IndicesPtr subtractFiltering(
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
if(k <= minNeighborsInRadius)
if(k>=minNeighborsInRadius && maxAngle > 0.0f)
{
Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
if (uIsFinite(normal[0]) &&
uIsFinite(normal[1]) &&
uIsFinite(normal[2]))
{
int count = k;
for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
{
Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
if(uIsFinite(v[0]) &&
uIsFinite(v[1]) &&
uIsFinite(v[2]))
{
float angle = pcl::getAngle3D(normal, v);
if(angle > maxAngle)
{
k-=1;
}
}
else
{
k-=1;
}
}
}
else
{
k=0;
}
}
if(k < minNeighborsInRadius)
{
output->at(oi++) = i;
}
@@ -611,6 +816,230 @@ pcl::IndicesPtr subtractFiltering(
}
}
pcl::IndicesPtr 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,
int minNeighborsInRadius,
const Eigen::Vector3f & viewpoint)
{
UWARN("Add angle to avoid subtraction of points with opposite normals");
UASSERT(minNeighborsInRadius > 0);
UASSERT(radiusSearchRatio > 0.0f);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>(false));
if(indices->size())
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
if(substractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
}
else
{
tree->setInputCloud(substractCloud);
}
for(unsigned int i=0; i<indices->size(); ++i)
{
if(pcl::isFinite(cloud->at(indices->at(i))))
{
std::vector<int> kIndices;
std::vector<float> kSqrdDistances;
float radius = radiusSearchRatio*uNorm(
cloud->at(indices->at(i)).x-viewpoint[0],
cloud->at(indices->at(i)).y-viewpoint[1],
cloud->at(indices->at(i)).z-viewpoint[2]);
int k = tree->radiusSearch(cloud->at(indices->at(i)), radius, kIndices, kSqrdDistances);
if(k < minNeighborsInRadius)
{
output->at(oi++) = indices->at(i);
}
}
}
output->resize(oi);
return output;
}
else
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
if(substractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
}
else
{
tree->setInputCloud(substractCloud);
}
for(unsigned int i=0; i<cloud->size(); ++i)
{
if(pcl::isFinite(cloud->at(i)))
{
std::vector<int> kIndices;
std::vector<float> kSqrdDistances;
float radius = radiusSearchRatio*uNorm(
cloud->at(i).x-viewpoint[0],
cloud->at(i).y-viewpoint[1],
cloud->at(i).z-viewpoint[2]);
int k = tree->radiusSearch(cloud->at(i), radius, kIndices, kSqrdDistances);
if(k < minNeighborsInRadius)
{
output->at(oi++) = i;
}
}
}
output->resize(oi);
return output;
}
}
pcl::IndicesPtr 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,
float maxAngle,
int minNeighborsInRadius,
const Eigen::Vector3f & viewpoint)
{
UASSERT(minNeighborsInRadius > 0);
UASSERT(radiusSearchRatio > 0.0f);
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>(false));
if(indices->size())
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
if(substractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
}
else
{
tree->setInputCloud(substractCloud);
}
for(unsigned int i=0; i<indices->size(); ++i)
{
if(pcl::isFinite(cloud->at(indices->at(i))))
{
std::vector<int> kIndices;
std::vector<float> kSqrdDistances;
float radius = radiusSearchRatio*uNorm(
cloud->at(indices->at(i)).x-viewpoint[0],
cloud->at(indices->at(i)).y-viewpoint[1],
cloud->at(indices->at(i)).z-viewpoint[2]);
int k = tree->radiusSearch(cloud->at(indices->at(i)), radius, kIndices, kSqrdDistances);
if(k>=minNeighborsInRadius && maxAngle > 0.0f)
{
Eigen::Vector4f normal(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
if (uIsFinite(normal[0]) &&
uIsFinite(normal[1]) &&
uIsFinite(normal[2]))
{
int count = k;
for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
{
Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
if(uIsFinite(v[0]) &&
uIsFinite(v[1]) &&
uIsFinite(v[2]))
{
float angle = pcl::getAngle3D(normal, v);
if(angle > maxAngle)
{
k-=1;
}
}
else
{
k-=1;
}
}
}
else
{
k=0;
}
}
if(k < minNeighborsInRadius)
{
output->at(oi++) = indices->at(i);
}
}
}
output->resize(oi);
return output;
}
else
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
if(substractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
}
else
{
tree->setInputCloud(substractCloud);
}
for(unsigned int i=0; i<cloud->size(); ++i)
{
if(pcl::isFinite(cloud->at(i)))
{
std::vector<int> kIndices;
std::vector<float> kSqrdDistances;
float radius = radiusSearchRatio*uNorm(
cloud->at(i).x-viewpoint[0],
cloud->at(i).y-viewpoint[1],
cloud->at(i).z-viewpoint[2]);
int k = tree->radiusSearch(cloud->at(i), radius, kIndices, kSqrdDistances);
if(k>=minNeighborsInRadius && maxAngle > 0.0f)
{
Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
if (uIsFinite(normal[0]) &&
uIsFinite(normal[1]) &&
uIsFinite(normal[2]))
{
int count = k;
for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
{
Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
if(uIsFinite(v[0]) &&
uIsFinite(v[1]) &&
uIsFinite(v[2]))
{
float angle = pcl::getAngle3D(normal, v);
if(angle > maxAngle)
{
k-=1;
}
}
else
{
k-=1;
}
}
}
else
{
k=0;
}
}
if(k < minNeighborsInRadius)
{
output->at(oi++) = i;
}
}
}
output->resize(oi);
return output;
}
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -681,7 +1110,6 @@ pcl::IndicesPtr normalFiltering(
output->resize(cloud_normals->size());
int oi = 0; // output iterator
Eigen::Vector3f n(normal[0], normal[1], normal[2]);
for(unsigned int i=0; i<cloud_normals->size(); ++i)
{
Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
@@ -742,7 +1170,6 @@ pcl::IndicesPtr normalFiltering(
output->resize(cloud_normals->size());
int oi = 0; // output iterator
Eigen::Vector3f n(normal[0], normal[1], normal[2]);
for(unsigned int i=0; i<cloud_normals->size(); ++i)
{
Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
@@ -757,6 +1184,51 @@ pcl::IndicesPtr normalFiltering(
return output;
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint)
{
pcl::IndicesPtr output(new std::vector<int>());
if(cloud->size())
{
int oi = 0; // output iterator
if(indices->size())
{
output->resize(indices->size());
for(unsigned int i=0; i<indices->size(); ++i)
{
Eigen::Vector4f v(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
float angle = pcl::getAngle3D(normal, v);
if(angle < angleMax)
{
output->at(oi++) = indices->size()!=0?indices->at(i):i;
}
}
}
else
{
output->resize(cloud->size());
for(unsigned int i=0; i<cloud->size(); ++i)
{
Eigen::Vector4f v(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
float angle = pcl::getAngle3D(normal, v);
if(angle < angleMax)
{
output->at(oi++) = indices->size()!=0?indices->at(i):i;
}
}
}
output->resize(oi);
}
return output;
}
std::vector<pcl::IndicesPtr> extractClusters(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -883,28 +1355,141 @@ std::vector<pcl::IndicesPtr> extractClusters(
return output;
}
std::vector<pcl::IndicesPtr> extractClusters(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float clusterTolerance,
int minClusterSize,
int maxClusterSize,
int * biggestClusterIndex)
{
typedef pcl::search::KdTree<pcl::PointXYZRGBNormal> KdTree;
typedef KdTree::Ptr KdTreePtr;
pcl::IndicesPtr extractNegativeIndices(
KdTreePtr tree(new KdTree);
pcl::EuclideanClusterExtraction<pcl::PointXYZRGBNormal> ec;
ec.setClusterTolerance (clusterTolerance);
ec.setMinClusterSize (minClusterSize);
ec.setMaxClusterSize (maxClusterSize);
ec.setInputCloud (cloud);
if(indices->size())
{
ec.setIndices(indices);
tree->setInputCloud(cloud, indices);
}
else
{
tree->setInputCloud(cloud);
}
ec.setSearchMethod (tree);
std::vector<pcl::PointIndices> cluster_indices;
ec.extract (cluster_indices);
int maxIndex=-1;
unsigned int maxSize = 0;
std::vector<pcl::IndicesPtr> output(cluster_indices.size());
for(unsigned int i=0; i<cluster_indices.size(); ++i)
{
output[i] = pcl::IndicesPtr(new std::vector<int>(cluster_indices[i].indices));
if(maxSize < cluster_indices[i].indices.size())
{
maxSize = (unsigned int)cluster_indices[i].indices.size();
maxIndex = i;
}
}
if(biggestClusterIndex)
{
*biggestClusterIndex = maxIndex;
}
return output;
}
pcl::IndicesPtr extractIndices(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices)
const pcl::IndicesPtr & indices,
bool negative)
{
pcl::IndicesPtr output(new std::vector<int>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud);
extract.setIndices(indices);
extract.setNegative(true);
extract.setNegative(negative);
extract.filter(*output);
return output;
}
pcl::IndicesPtr extractNegativeIndices(
pcl::IndicesPtr extractIndices(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices)
const pcl::IndicesPtr & indices,
bool negative)
{
pcl::IndicesPtr output(new std::vector<int>);
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (cloud);
extract.setIndices(indices);
extract.setNegative(true);
extract.setNegative(negative);
extract.filter(*output);
return output;
}
pcl::IndicesPtr extractIndices(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative)
{
pcl::IndicesPtr output(new std::vector<int>);
pcl::ExtractIndices<pcl::PointXYZRGBNormal> extract;
extract.setInputCloud (cloud);
extract.setIndices(indices);
extract.setNegative(negative);
extract.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr extractIndices(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative,
bool keepOrganized)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud);
extract.setIndices(indices);
extract.setNegative(negative);
extract.setKeepOrganized(keepOrganized);
extract.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr extractIndices(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative,
bool keepOrganized)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (cloud);
extract.setIndices(indices);
extract.setNegative(negative);
extract.setKeepOrganized(keepOrganized);
extract.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr extractIndices(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
bool negative,
bool keepOrganized)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::ExtractIndices<pcl::PointXYZRGBNormal> extract;
extract.setInputCloud (cloud);
extract.setIndices(indices);
extract.setNegative(negative);
extract.setKeepOrganized(keepOrganized);
extract.filter(*output);
return output;
}

1
corelib/src/util3d_motion_estimation.cpp
View File

@@ -208,6 +208,7 @@ Transform estimateMotion3DTo3D(
0,
&matches);
UASSERT(inliers1.size() == inliers2.size());
UDEBUG("Unique correspondences = %d", (int)inliers1.size());
if(varianceOut)
{

411
corelib/src/util3d_surface.cpp
View File

@@ -36,17 +36,287 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/features/normal_3d.h>
#include <pcl/surface/mls.h>
#include <pcl/surface/texture_mapping.h>
#include <pcl/features/integral_image_normal.h>
#ifndef DISABLE_VTK
#include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
#endif
#if PCL_VERSION_COMPARE(<, 1, 8, 0)
#include "pcl18/surface/organized_fast_mesh.h"
#else
#include <pcl/surface/organized_fast_mesh.h>
#endif
namespace rtabmap
{
namespace util3d
{
void createPolygonIndexes(
const std::vector<pcl::Vertices> & polygons,
int cloudSize,
std::vector<std::set<int> > & neighbors,
std::vector<std::set<int> > & vertexToPolygons)
{
vertexToPolygons = std::vector<std::set<int> >(cloudSize);
neighbors = std::vector<std::set<int> >(polygons.size());
for(unsigned int i=0; i<polygons.size(); ++i)
{
std::set<int> vertices(polygons[i].vertices.begin(), polygons[i].vertices.end());
for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
{
int v = polygons[i].vertices.at(j);
for(std::set<int>::iterator iter=vertexToPolygons[v].begin(); iter!=vertexToPolygons[v].end(); ++iter)
{
int numSharedVertices = 0;
for(unsigned int k=0; k<polygons.at(*iter).vertices.size() && numSharedVertices<2; ++k)
{
if(vertices.find(polygons.at(*iter).vertices.at(k)) != vertices.end())
{
++numSharedVertices;
}
}
if(numSharedVertices >= 2)
{
neighbors[*iter].insert(i);
neighbors[i].insert(*iter);
}
}
vertexToPolygons[v].insert(i);
}
}
}
std::vector<pcl::Vertices> organizedFastMesh(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
double angleTolerance,
bool quad,
int trianglePixelSize)
{
UASSERT(cloud->is_dense == false);
UASSERT(cloud->width > 1 && cloud->height > 1);
pcl::OrganizedFastMesh<pcl::PointXYZRGB> ofm;
ofm.setTrianglePixelSize (trianglePixelSize);
ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGB>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGB>::TRIANGLE_RIGHT_CUT);
ofm.setInputCloud (cloud);
ofm.setAngleTolerance(angleTolerance);
std::vector<pcl::Vertices> vertices;
ofm.reconstruct (vertices);
if(quad)
{
//flip all polygons (right handed)
std::vector<pcl::Vertices> output(vertices.size());
for(unsigned int i=0; i<vertices.size(); ++i)
{
output[i].vertices.resize(4);
output[i].vertices[0] = vertices[i].vertices[0];
output[i].vertices[3] = vertices[i].vertices[1];
output[i].vertices[2] = vertices[i].vertices[2];
output[i].vertices[1] = vertices[i].vertices[3];
}
return output;
}
return vertices;
}
std::vector<pcl::Vertices> organizedFastMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
double angleTolerance,
bool quad,
int trianglePixelSize)
{
UDEBUG("size=%d angle=%f quand=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
UASSERT(cloud->is_dense == false);
UASSERT(cloud->width > 1 && cloud->height > 1);
pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal> ofm;
ofm.setTrianglePixelSize (trianglePixelSize);
ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::TRIANGLE_RIGHT_CUT);
ofm.setInputCloud (cloud);
ofm.setAngleTolerance(angleTolerance);
std::vector<pcl::Vertices> vertices;
ofm.reconstruct (vertices);
if(quad)
{
//flip all polygons (right handed)
std::vector<pcl::Vertices> output(vertices.size());
for(unsigned int i=0; i<vertices.size(); ++i)
{
output[i].vertices.resize(4);
output[i].vertices[0] = vertices[i].vertices[0];
output[i].vertices[3] = vertices[i].vertices[1];
output[i].vertices[2] = vertices[i].vertices[2];
output[i].vertices[1] = vertices[i].vertices[3];
}
return output;
}
return vertices;
}
void appendMesh(
pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudA,
std::vector<pcl::Vertices> & polygonsA,
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudB,
const std::vector<pcl::Vertices> & polygonsB)
{
UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());
int sizeA = cloudA.size();
cloudA += cloudB;
int sizePolygonsA = polygonsA.size();
polygonsA.resize(sizePolygonsA+polygonsB.size());
for(unsigned int i=0; i<polygonsB.size(); ++i)
{
pcl::Vertices vertices = polygonsB[i];
for(unsigned int j=0; j<vertices.vertices.size(); ++j)
{
vertices.vertices[j] += sizeA;
}
polygonsA[i+sizePolygonsA] = vertices;
}
}
void filterNotUsedVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
const std::vector<pcl::Vertices> & polygons,
pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud,
std::vector<pcl::Vertices> & outputPolygons)
{
UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
std::map<int, int> addedVertices; //<oldIndex, newIndex>
outputCloud.resize(cloud.size());
outputPolygons.resize(polygons.size());
int oi = 0;
for(unsigned int i=0; i<polygons.size(); ++i)
{
pcl::Vertices & v = outputPolygons[i];
v.vertices.resize(polygons[i].vertices.size());
for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
{
std::map<int, int>::iterator iter = addedVertices.find(polygons[i].vertices[j]);
if(iter == addedVertices.end())
{
outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
v.vertices[j] = oi++;
}
else
{
v.vertices[j] = iter->second;
}
}
}
outputCloud.resize(oi);
}
std::vector<pcl::Vertices> filterCloseVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,
const std::vector<pcl::Vertices> & polygons,
float radius,
float angle,
bool keepLatestInRadius)
{
UDEBUG("size=%d polygons=%d radius=%f angle=%f keepLatest=%d",
(int)cloud->size(), (int)polygons.size(), radius, angle, keepLatestInRadius?1:0);
std::vector<pcl::Vertices> outputPolygons;
pcl::KdTreeFLANN<pcl::PointXYZRGBNormal>::Ptr kdtree(new pcl::KdTreeFLANN<pcl::PointXYZRGBNormal>);
kdtree->setInputCloud(cloud);
std::map<int, int> verticesDone;
outputPolygons = polygons;
for(unsigned int i=0; i<outputPolygons.size(); ++i)
{
pcl::Vertices & polygon = outputPolygons[i];
for(unsigned int j=0; j<polygon.vertices.size(); ++j)
{
std::map<int, int>::iterator iter = verticesDone.find(polygon.vertices[j]);
if(iter != verticesDone.end())
{
polygon.vertices[j] = iter->second;
}
else
{
std::vector<int> kIndices;
std::vector<float> kDistances;
kdtree->radiusSearch(polygon.vertices[j], radius, kIndices, kDistances);
if(kIndices.size())
{
int reference = -1;
for(unsigned int z=0; z<kIndices.size(); ++z)
{
if(reference == -1)
{
reference = kIndices[z];
}
else if(keepLatestInRadius)
{
if(kIndices[z] < reference)
{
reference = kIndices[z];
}
}
else
{
if(kIndices[z] > reference)
{
reference = kIndices[z];
}
}
}
if(reference >= 0)
{
for(unsigned int z=0; z<kIndices.size(); ++z)
{
verticesDone.insert(std::make_pair(kIndices[j], reference));
}
polygon.vertices[j] = reference;
}
}
else
{
verticesDone.insert(std::make_pair(polygon.vertices[j], polygon.vertices[j]));
}
}
}
}
return outputPolygons;
}
std::vector<pcl::Vertices> filterInvalidPolygons(const std::vector<pcl::Vertices> & polygons)
{
std::vector<pcl::Vertices> output(polygons.size());
int oi=0;
for(unsigned int i=0; i<polygons.size(); ++i)
{
bool valid = true;
for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
{
if(polygons[i].vertices[j] == polygons[i].vertices[(j+1)%polygons[i].vertices.size()])
{
valid = false;
break;
}
}
if(valid)
{
output[oi++] = polygons[i];
}
}
output.resize(oi);
return output;
}
pcl::PolygonMesh::Ptr createMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloudWithNormals,
float gp3SearchRadius,
@@ -149,9 +419,6 @@ pcl::TextureMesh::Ptr createTextureMesh(
// Original from pcl/gpu/kinfu_large_scale/tools/standalone_texture_mapping.cpp:
// Author: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
// Create the texturemesh object that will contain our UV-mapped mesh
pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
textureMesh->cloud = mesh->cloud;
@@ -223,10 +490,23 @@ pcl::TextureMesh::Ptr createTextureMesh(
pcl::TextureMapping<pcl::PointXYZ> tm; // TextureMapping object that will perform the sort
tm.textureMeshwithMultipleCameras(*textureMesh, cameras);
// compute normals for the mesh
pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals = computeNormals(cloud, 20);
pcl::toPCLPointCloud2 (*cloudWithNormals, textureMesh->cloud);
// compute normals for the mesh if not already here
bool hasNormals = false;
for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
{
if(textureMesh->cloud.fields[i].name.compare("normal_x") == 0)
{
hasNormals = true;
break;
}
}
if(!hasNormals)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals = computeNormals(cloud, 20);
pcl::toPCLPointCloud2 (*cloudWithNormals, textureMesh->cloud);
}
return textureMesh;
}
@@ -234,15 +514,34 @@ pcl::TextureMesh::Ptr createTextureMesh(
pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int normalKSearch)
{
pcl::IndicesPtr indices(new std::vector<int>);
return computeNormals(cloud, indices, normalKSearch);
}
pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int normalKSearch)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
if(indices->size())
{
tree->setInputCloud(cloud, indices);
}
else
{
tree->setInputCloud (cloud);
}
// Normal estimation*
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
n.setInputCloud (cloud);
if(indices->size())
{
n.setIndices(indices);
}
n.setSearchMethod (tree);
n.setKSearch (normalKSearch);
n.compute (*normals);
@@ -258,15 +557,34 @@ pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int normalKSearch)
{
pcl::IndicesPtr indices(new std::vector<int>);
return computeNormals(cloud, indices, normalKSearch);
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int normalKSearch)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud);
if(indices->size())
{
tree->setInputCloud(cloud, indices);
}
else
{
tree->setInputCloud (cloud);
}
// Normal estimation*
pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
n.setInputCloud (cloud);
if(indices->size())
{
n.setIndices(indices);
}
n.setSearchMethod (tree);
n.setKSearch (normalKSearch);
n.compute (*normals);
@@ -279,6 +597,43 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
return cloud_with_normals;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float maxDepthChangeFactor,
float normalSmoothingSize)
{
pcl::IndicesPtr indices(new std::vector<int>);
return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize);
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float maxDepthChangeFactor,
float normalSmoothingSize)
{
UASSERT(cloud->isOrganized());
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
// Normal estimation
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
ne.setMaxDepthChangeFactor(maxDepthChangeFactor);
ne.setNormalSmoothingSize(normalSmoothingSize);
ne.setInputCloud(cloud);
if(indices->size())
{
ne.setIndices(indices);
}
ne.compute(*normals);
// Concatenate the XYZ and normal fields
pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
return cloud_with_normals;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float searchRadius,
@@ -289,10 +644,42 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
int pointDensity, // RANDOM_UNIFORM_DENSITY
float dilationVoxelSize, // VOXEL_GRID_DILATION
int dilationIterations) // VOXEL_GRID_DILATION
{
pcl::IndicesPtr indices(new std::vector<int>);
return mls(cloud,
indices,
searchRadius,
polygonialOrder,
upsamplingMethod,
upsamplingRadius,
upsamplingStep,
pointDensity,
dilationVoxelSize,
dilationIterations);
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float searchRadius,
int polygonialOrder,
int upsamplingMethod, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION
float upsamplingRadius, // SAMPLE_LOCAL_PLANE
float upsamplingStep, // SAMPLE_LOCAL_PLANE
int pointDensity, // RANDOM_UNIFORM_DENSITY
float dilationVoxelSize, // VOXEL_GRID_DILATION
int dilationIterations) // VOXEL_GRID_DILATION
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud);
if(indices->size())
{
tree->setInputCloud (cloud, indices);
}
else
{
tree->setInputCloud (cloud);
}
// Init object (second point type is for the normals)
pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> mls;
@@ -320,6 +707,10 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
// Reconstruct
mls.setInputCloud (cloud);
if(indices->size())
{
mls.setIndices(indices);
}
mls.setSearchMethod (tree);
mls.process (*cloud_with_normals);