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

Stereo! The memory can now handle directly stereo images. Disparity can be computed on the fly by keeping left and right images, so for features extraction (and for re-extraction on loop closure), we can compute 3D points precisely. New parameters can be found under "RGB-D Mapping->Stereo". Full image disparity is reconstructed in the GUI (not the core).

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1861 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-16 00:14:23 +00:00
parent 8f451029e2
commit fcd3301665
18 changed files with 1230 additions and 260 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Features2d.h
+11
View File
@@ -62,6 +62,7 @@ public:
kFeatureBrisk=7};
static Feature2D * create(Feature2D::Type & type, const ParametersMap & parameters);
static void filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
@@ -72,6 +73,16 @@ public:
const cv::Mat & depth,
float maxDepth);
static void filterKeypointsByDisparity(
std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
float minDisparity);
static void filterKeypointsByDisparity(
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors,
const cv::Mat & disparity,
float minDisparity);
static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
corelib/include/rtabmap/core/Memory.h
+10 -9
View File
@@ -161,15 +161,6 @@ public:
const VWDictionary * getVWDictionary() const;
std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
Feature2D::Type getFeatureType() const {return _featureType;}
void extractKeypointsAndDescriptors(
const cv::Mat & image,
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors) const;
void extractKeypointsAndDescriptors(
const cv::Mat & image,
const cv::Mat & depth,
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors) const;
// RGB-D stuff
void getMetricConstraints(
@@ -276,6 +267,16 @@ private:
float _icp2MaxFitness;
float _icp2CorrespondenceRatio;
float _icp2VoxelSize;
// Stereo stuff
int _stereoFlowWinSize;
int _stereoFlowIterations;
double _stereoFlowEpsilon;
int _stereoFlowMaxLevel;
int _stereoSubPixWinSize;
int _stereoSubPixIterations;
double _stereoSubPixEps;
};
} // namespace rtabmap
corelib/include/rtabmap/core/Parameters.h
+10 -1
View File
@@ -269,7 +269,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Odom, MaxDepth, float, 4.0, "Max depth of the words (0 means no limit).");
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
RTABMAP_PARAM_STR(Odom, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
RTABMAP_PARAM(Odom, RefineIterations, int, 10, "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
RTABMAP_PARAM(Odom, RefineIterations, int, 5, "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
RTABMAP_PARAM(Odom, FeaturesRatio, float, 0.5, "Minimum ratio of keypoints between the current image and the last image to compute odometry.");
// Odometry Bag-of-words
@@ -315,6 +315,15 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, "ICP 2D: Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp2, VoxelSize, float, 0.005, "Voxel size to be used for ICP computation.");
// Stereo disparity
RTABMAP_PARAM(Stereo, WinSize, int, 9, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(Stereo, Iterations, int, 20, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(Stereo, Eps, double, 0.02, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(Stereo, MaxLevel, int, 4, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(Stereo, SubPixWinSize, int, 3, "See cv::cornerSubPix().");
RTABMAP_PARAM(Stereo, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining.");
RTABMAP_PARAM(Stereo, SubPixEps, double, 0.02, "See cv::cornerSubPix().");
public:
virtual ~Parameters();
corelib/include/rtabmap/core/util3d.h
+73 -5
View File
@@ -103,8 +103,8 @@ void RTABMAP_EXP rgbdFromCloud(
cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3(
const std::multimap<int, cv::KeyPoint> & words,
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
float fx,
float fy,
@@ -112,11 +112,34 @@ std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3(
float cy,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDisparity(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & leftImage,
const cv::Mat & rightImage,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform = Transform::getIdentity(),
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
double flowEps = 0.02);
std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
const std::list<int> & wordIds,
const std::vector<cv::KeyPoint> & keypoints);
pcl::PointXYZ RTABMAP_EXP getDepth(
pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
const cv::Mat & depthImage,
float x, float y,
float cx, float cy,
@@ -193,17 +216,62 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDisparityRGB(
float fx, float baseline,
int decimation);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
const cv::Mat & imageLeft,
const cv::Mat & imageRight,
float cx, float cy,
float fx, float fy,
int decimation);
cv::Mat RTABMAP_EXP disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage);
pcl::PointXYZ RTABMAP_EXP projectDisparityTo3d(
cv::Mat RTABMAP_EXP disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
double flowEps = 0.02);
cv::Mat RTABMAP_EXP depthFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
float fx,
float baseline,
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
double flowEps = 0.02);
cv::Mat RTABMAP_EXP disparityFromStereoCorrespondences(
const cv::Mat & leftImage,
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
const std::vector<unsigned char> & mask);
cv::Mat RTABMAP_EXP depthFromStereoCorrespondences(
const cv::Mat & leftImage,
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
const std::vector<unsigned char> & mask,
float fx, float baseline);
pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
const cv::Point2f & pt,
float disparity,
float cx, float cy, float fx, float baseline);
pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
const cv::Point2f & pt,
const cv::Mat & disparity,
float cx, float cy, float fx, float baseline);
cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
float cx, float cy, float fx, float baseline,
float fx, float baseline,
int type = CV_32FC1);
cv::Mat RTABMAP_EXP depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);