i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27: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);
corelib/src/Features2d.cpp
+67
View File
@@ -114,6 +114,73 @@ void Feature2D::filterKeypointsByDepth(
}
}
void Feature2D::filterKeypointsByDisparity(
std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
float minDisparity)
{
cv::Mat descriptors;
filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity);
}
void Feature2D::filterKeypointsByDisparity(
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors,
const cv::Mat & disparity,
float minDisparity)
{
if(!disparity.empty() && minDisparity > 0.0f && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
{
std::vector<cv::KeyPoint> output(keypoints.size());
std::vector<int> indexes(keypoints.size(), 0);
int oi=0;
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);
if(u >=0 && u<disparity.cols && v >=0 && v<disparity.rows)
{
float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
if(d!=0.0f && uIsFinite(d) && d >= minDisparity)
{
output[oi++] = keypoints[i];
indexes[i] = 1;
}
}
}
output.resize(oi);
keypoints = output;
if(!descriptors.empty() && (int)keypoints.size() != descriptors.rows)
{
if(keypoints.size() == 0)
{
descriptors = cv::Mat();
}
else
{
cv::Mat newDescriptors(keypoints.size(), descriptors.cols, descriptors.type());
int di = 0;
for(unsigned int i=0; i<indexes.size(); ++i)
{
if(indexes[i] == 1)
{
if(descriptors.type() == CV_32FC1)
{
memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
}
else // CV_8UC1
{
memcpy(newDescriptors.ptr<char>(di++), descriptors.ptr<char>(i), descriptors.cols*sizeof(char));
}
}
}
descriptors = newDescriptors;
}
}
}
}
void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
{
cv::Mat descriptors;
corelib/src/Memory.cpp
+281 -138
View File
@@ -98,7 +98,16 @@ Memory::Memory(const ParametersMap & parameters) :
_icp2MaxIterations(Parameters::defaultLccIcp2Iterations()),
_icp2MaxFitness(Parameters::defaultLccIcp2MaxFitness()),
_icp2CorrespondenceRatio(Parameters::defaultLccIcp2CorrespondenceRatio()),
_icp2VoxelSize(Parameters::defaultLccIcp2VoxelSize())
_icp2VoxelSize(Parameters::defaultLccIcp2VoxelSize()),
_stereoFlowWinSize(Parameters::defaultStereoWinSize()),
_stereoFlowIterations(Parameters::defaultStereoIterations()),
_stereoFlowEpsilon(Parameters::defaultStereoEps()),
_stereoFlowMaxLevel(Parameters::defaultStereoMaxLevel()),
_stereoSubPixWinSize(Parameters::defaultStereoSubPixWinSize()),
_stereoSubPixIterations(Parameters::defaultStereoSubPixIterations()),
_stereoSubPixEps(Parameters::defaultStereoSubPixEps())
{
_vwd = new VWDictionary(parameters);
this->parseParameters(parameters);
@@ -383,6 +392,15 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kLccIcp2CorrespondenceRatio(), _icp2CorrespondenceRatio);
Parameters::parse(parameters, Parameters::kLccIcp2VoxelSize(), _icp2VoxelSize);
//stereo
Parameters::parse(parameters, Parameters::kStereoWinSize(), _stereoFlowWinSize);
Parameters::parse(parameters, Parameters::kStereoIterations(), _stereoFlowIterations);
Parameters::parse(parameters, Parameters::kStereoEps(), _stereoFlowEpsilon);
Parameters::parse(parameters, Parameters::kStereoMaxLevel(), _stereoFlowMaxLevel);
Parameters::parse(parameters, Parameters::kStereoSubPixWinSize(), _stereoSubPixWinSize);
Parameters::parse(parameters, Parameters::kStereoSubPixIterations(), _stereoSubPixIterations);
Parameters::parse(parameters, Parameters::kStereoSubPixEps(), _stereoSubPixEps);
UASSERT_MSG(_bowMinInliers >= 1, uFormat("value=%d", _bowMinInliers).c_str());
UASSERT_MSG(_bowInlierDistance > 0.0f, uFormat("value=%f", _bowInlierDistance).c_str());
UASSERT_MSG(_bowIterations > 0, uFormat("value=%d", _bowIterations).c_str());
@@ -498,7 +516,10 @@ bool Memory::update(const SensorData & data, Statistics * stats)
//============================================================
if(_incrementalMemory)
{
this->rehearsal(signature, stats);
if(_similarityThreshold < 1.0f)
{
this->rehearsal(signature, stats);
}
t=timer.ticks()*1000;
if(stats) stats->addStatistic(Statistics::kTimingMemRehearsal(), t);
UDEBUG("time rehearsal=%f ms", t);
@@ -1848,72 +1869,79 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
cv::Mat newDepth = ctNew.getUncompressedData();
if(!oldDepth.empty() && !newDepth.empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloudXYZ = util3d::getICPReadyCloud(
oldDepth,
oldS.getDepthFx(),
oldS.getDepthFy(),
oldS.getDepthCx(),
oldS.getDepthCy(),
_icpDecimation,
_icpMaxDepth,
_icpVoxelSize,
_icpSamples,
oldS.getLocalTransform());
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
newDepth,
newS.getDepthFx(),
newS.getDepthFy(),
newS.getDepthCx(),
newS.getDepthCy(),
_icpDecimation,
_icpMaxDepth,
_icpVoxelSize,
_icpSamples,
guess * newS.getLocalTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ);
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
oldCloud = util3d::removeNaNNormalsFromPointCloud(oldCloud);
// 3D
double fitness = 0;
bool hasConverged = false;
Transform icpT;
if(newCloud->size() && oldCloud->size())
if(oldDepth.type() == CV_8UC1 || newDepth.type() == CV_8UC1)
{
icpT = util3d::icpPointToPlane(newCloud,
oldCloud,
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
hasConverged,
fitness);
//pcl::io::savePCDFile("old.pcd", *oldCloudXYZ);
//pcl::io::savePCDFile("newguess.pcd", *newCloudXYZ);
//newCloudXYZ = util3d::transformPointCloud(newCloudXYZ, icpT);
//pcl::io::savePCDFile("newicp.pcd", *newCloudXYZ);
UDEBUG("fitness=%f", fitness);
if(!icpT.isNull() && hasConverged && (_icpMaxFitness == 0 || fitness < _icpMaxFitness))
{
transform = icpT * guess;
transform = transform.inverse();
}
else
{
msg = uFormat("Cannot compute transform (hasConverged=%s fitness=%f/%f)",
hasConverged?"true":"false", fitness, _icpMaxFitness);
UINFO(msg.c_str());
}
UERROR("ICP 3D cannot be done on stereo images!");
}
else
{
msg = "Clouds empty ?!?";
UWARN(msg.c_str());
pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloudXYZ = util3d::getICPReadyCloud(
oldDepth,
oldS.getDepthFx(),
oldS.getDepthFy(),
oldS.getDepthCx(),
oldS.getDepthCy(),
_icpDecimation,
_icpMaxDepth,
_icpVoxelSize,
_icpSamples,
oldS.getLocalTransform());
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
newDepth,
newS.getDepthFx(),
newS.getDepthFy(),
newS.getDepthCx(),
newS.getDepthCy(),
_icpDecimation,
_icpMaxDepth,
_icpVoxelSize,
_icpSamples,
guess * newS.getLocalTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ);
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
oldCloud = util3d::removeNaNNormalsFromPointCloud(oldCloud);
// 3D
double fitness = 0;
bool hasConverged = false;
Transform icpT;
if(newCloud->size() && oldCloud->size())
{
icpT = util3d::icpPointToPlane(newCloud,
oldCloud,
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
hasConverged,
fitness);
//pcl::io::savePCDFile("old.pcd", *oldCloudXYZ);
//pcl::io::savePCDFile("newguess.pcd", *newCloudXYZ);
//newCloudXYZ = util3d::transformPointCloud(newCloudXYZ, icpT);
//pcl::io::savePCDFile("newicp.pcd", *newCloudXYZ);
UDEBUG("fitness=%f", fitness);
if(!icpT.isNull() && hasConverged && (_icpMaxFitness == 0 || fitness < _icpMaxFitness))
{
transform = icpT * guess;
transform = transform.inverse();
}
else
{
msg = uFormat("Cannot compute transform (hasConverged=%s fitness=%f/%f)",
hasConverged?"true":"false", fitness, _icpMaxFitness);
UINFO(msg.c_str());
}
}
else
{
msg = "Clouds empty ?!?";
UWARN(msg.c_str());
}
}
}
else
@@ -3011,49 +3039,6 @@ void Memory::copyData(const Signature * from, Signature * to)
UDEBUG("Merging time = %fs", timer.ticks());
}
void Memory::extractKeypointsAndDescriptors(
const cv::Mat & image,
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors) const
{
extractKeypointsAndDescriptors(image, cv::Mat(), keypoints, descriptors);
}
void Memory::extractKeypointsAndDescriptors(
const cv::Mat & image,
const cv::Mat & depth,
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors) const
{
if(_wordsPerImageTarget >= 0)
{
UTimer timer;
if(_feature2D)
{
cv::Rect roi = Feature2D::computeRoi(image, _roiRatios);
keypoints = _feature2D->generateKeypoints(image, 0, roi);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
Feature2D::filterKeypointsByDepth(keypoints, depth, _wordsMaxDepth);
Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
}
else
{
UWARN("feature2D not set!");
}
if(keypoints.size())
{
descriptors = _feature2D->generateDescriptors(image, keypoints);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, timer.ticks());
}
}
else
{
UDEBUG("_wordsPerImageTarget(%d)<0 so don't extract any descriptors...", _wordsPerImageTarget);
}
}
class PreUpdateThread : public UThreadNode
{
public:
@@ -3076,6 +3061,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
UASSERT(data.depth().empty() || data.depth().type() == CV_16UC1 || data.depth().type() == CV_32FC1);
UASSERT(data.rightImage().empty() || data.rightImage().type() == CV_8UC1);
UASSERT(data.depth2d().empty() || data.depth2d().type() == CV_32FC2);
UASSERT(_feature2D != 0);
PreUpdateThread preUpdateThread(_vwd);
@@ -3126,29 +3112,125 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
preUpdateThread.start();
}
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3D(new pcl::PointCloud<pcl::PointXYZ>);
if(data.keypoints().size() == 0)
{
// Extract features
cv::Mat imageMono;
// convert to grayscale
if(data.image().channels() > 1)
if(_wordsPerImageTarget >= 0)
{
cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY);
// Extract features
cv::Mat imageMono;
// convert to grayscale
if(data.image().channels() > 1)
{
cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY);
}
else
{
imageMono = data.image();
}
cv::Rect roi = Feature2D::computeRoi(imageMono, _roiRatios);
if(!data.rightImage().empty())
{
//stereo
cv::Mat disparity;
keypoints = _feature2D->generateKeypoints(imageMono, 0, roi);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
if(_stereoSubPixWinSize > 0 && _stereoSubPixIterations > 0)
{
cv::cornerSubPix( imageMono, leftCorners,
cv::Size( _stereoSubPixWinSize, _stereoSubPixWinSize ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _stereoSubPixIterations, _stereoSubPixEps ) );
UDEBUG("time subpix left kpts=%fs", timer.ticks());
}
//generate a disparity map
disparity = util3d::disparityFromStereoImages(
imageMono,
data.rightImage(),
leftCorners,
_stereoFlowWinSize,
_stereoFlowMaxLevel,
_stereoFlowIterations,
_stereoFlowEpsilon);
UDEBUG("generate disparity = %fs", timer.ticks());
if(_wordsMaxDepth > 0.0f)
{
// disparity = baseline * fx / depth;
float minDisparity = data.baseline() * data.fx() / _wordsMaxDepth;
Feature2D::filterKeypointsByDisparity(keypoints, disparity, minDisparity);
UDEBUG("time filter keypoints by disparity (%d) = %fs", (int)keypoints.size(), timer.ticks());
}
if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
{
Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
UDEBUG("time limit keypoints max (%d) = %fs", _wordsPerImageTarget, timer.ticks());
}
if(keypoints.size())
{
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, timer.ticks());
keypoints3D = util3d::generateKeypoints3DDisparity(keypoints, disparity, data.fx(), data.baseline(), data.cx(), data.cy(), data.localTransform());
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), timer.ticks());
}
}
else if(!data.depth().empty())
{
//depth
keypoints = _feature2D->generateKeypoints(imageMono, 0, roi);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
if(_wordsMaxDepth > 0.0f)
{
Feature2D::filterKeypointsByDepth(keypoints, data.depth(), _wordsMaxDepth);
UDEBUG("time filter keypoints by depth (%d) = %fs", (int)keypoints.size(), timer.ticks());
}
if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
{
Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
UDEBUG("time limit keypoints max (%d) = %fs", _wordsPerImageTarget, timer.ticks());
}
if(keypoints.size())
{
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, timer.ticks());
keypoints3D = util3d::generateKeypoints3DDepth(keypoints, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform());
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), timer.ticks());
}
}
else
{
//RGB only
keypoints = _feature2D->generateKeypoints(imageMono, _wordsPerImageTarget, roi);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
if(keypoints.size())
{
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, timer.ticks());
}
}
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
{
descriptors = cv::Mat();
}
}
else
{
imageMono = data.image();
}
this->extractKeypointsAndDescriptors(imageMono,
data.depth(),
keypoints,
descriptors);
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
{
descriptors = cv::Mat();
UDEBUG("_wordsPerImageTarget(%d)<0 so don't extract any descriptors...", _wordsPerImageTarget);
}
}
else
@@ -3156,10 +3238,72 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
keypoints = data.keypoints();
descriptors = data.descriptors().clone();
Feature2D::filterKeypointsByDepth(keypoints, descriptors,
data.depth(),
_wordsMaxDepth);
Feature2D::limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
// filter by depth
if(!data.rightImage().empty())
{
//stereo
cv::Mat imageMono;
// convert to grayscale
if(data.image().channels() > 1)
{
cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY);
}
else
{
imageMono = data.image();
}
//generate a disparity map
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
cv::Mat disparity = util3d::disparityFromStereoImages(
imageMono,
data.rightImage(),
leftCorners,
_stereoFlowWinSize,
_stereoFlowMaxLevel,
_stereoFlowIterations,
_stereoFlowEpsilon);
UDEBUG("generate disparity = %fs", timer.ticks());
if(_wordsMaxDepth)
{
// disparity = baseline * fx / depth;
float minDisparity = data.baseline() * data.fx() / _wordsMaxDepth;
Feature2D::filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity);
}
if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
{
Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
UDEBUG("time limit keypoints max (%d) = %fs", _wordsPerImageTarget, timer.ticks());
}
keypoints3D = util3d::generateKeypoints3DDisparity(keypoints, disparity, data.fx(), data.baseline(), data.cx(), data.cy(), data.localTransform());
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), timer.ticks());
}
else if(!data.depth().empty())
{
//depth
if(_wordsMaxDepth)
{
Feature2D::filterKeypointsByDepth(keypoints, descriptors, _wordsMaxDepth);
UDEBUG("time filter keypoints by depth (%d) = %fs", (int)keypoints.size(), timer.ticks());
}
if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
{
Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
UDEBUG("time limit keypoints max (%d) = %fs", _wordsPerImageTarget, timer.ticks());
}
keypoints3D = util3d::generateKeypoints3DDepth(keypoints, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform());
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), timer.ticks());
}
else
{
// RGB only
Feature2D::limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
}
}
if(_parallelized)
@@ -3188,23 +3332,22 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
}
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, pcl::PointXYZ> words3D;
if(wordIds.size() > 0)
{
UASSERT(wordIds.size() == keypoints.size());
UASSERT(keypoints3D->size() == 0 || keypoints3D->size() == wordIds.size());
unsigned int i=0;
for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
{
words.insert(std::pair<int, cv::KeyPoint>(*iter, keypoints[i]));
if(keypoints3D->size())
{
words3D.insert(std::pair<int, pcl::PointXYZ>(*iter, keypoints3D->at(i)));
}
}
}
//3d words
std::multimap<int, pcl::PointXYZ> words3;
if(!data.depth().empty() && data.fx() && data.fy())
{
words3 = util3d::generateWords3(words, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform());
}
Signature * s;
if(keepRawData)
{
@@ -3235,7 +3378,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
s = new Signature(id,
_idMapCount,
words,
words3,
words3D,
data.pose(),
util3d::compressData(data.depth2d()),
imageBytes,
@@ -3246,14 +3389,14 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
data.cy(),
data.localTransform());
s->setImageRaw(data.image());
s->setDepthRaw(data.depth());
s->setDepthRaw(depthOrRightImage);
}
else
{
s = new Signature(id,
_idMapCount,
words,
words3,
words3D,
data.pose(),
util3d::compressData(data.depth2d()));
}
corelib/src/Odometry.cpp
+34 -6
View File
@@ -152,6 +152,29 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
// Memory's stereo parameters, copy from Odometry
int flowWinSize = Parameters::defaultOdomFlowWinSize();
int flowIterations_ = Parameters::defaultOdomFlowIterations();
double flowEps_ = Parameters::defaultOdomFlowEps();
int flowMaxLevel_ = Parameters::defaultOdomFlowMaxLevel();
int subPixWinSize_ = Parameters::defaultOdomFlowSubPixWinSize();
int subPixIterations_ = Parameters::defaultOdomFlowSubPixIterations();
double subPixEps_ = Parameters::defaultOdomFlowSubPixEps();
Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize);
Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
Parameters::parse(parameters, Parameters::kOdomFlowSubPixWinSize(), subPixWinSize_);
Parameters::parse(parameters, Parameters::kOdomFlowSubPixIterations(), subPixIterations_);
Parameters::parse(parameters, Parameters::kOdomFlowSubPixEps(), subPixEps_);
customParameters.insert(ParametersPair(Parameters::kStereoWinSize(), uNumber2Str(flowWinSize)));
customParameters.insert(ParametersPair(Parameters::kStereoIterations(), uNumber2Str(flowIterations_)));
customParameters.insert(ParametersPair(Parameters::kStereoEps(), uNumber2Str(flowEps_)));
customParameters.insert(ParametersPair(Parameters::kStereoMaxLevel(), uNumber2Str(flowMaxLevel_)));
customParameters.insert(ParametersPair(Parameters::kStereoSubPixWinSize(), uNumber2Str(subPixWinSize_)));
customParameters.insert(ParametersPair(Parameters::kStereoSubPixIterations(), uNumber2Str(subPixIterations_)));
customParameters.insert(ParametersPair(Parameters::kStereoSubPixEps(), uNumber2Str(subPixEps_)));
// add only feature stuff
for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
@@ -434,7 +457,7 @@ OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomFlowSubPixEps(), subPixEps_);
ParametersMap::const_iterator iter;
Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
{
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
@@ -531,7 +554,6 @@ Transform OdometryOpticalFlow::computeTransformStereo(
{
lastCornersKept[ki] = lastCorners_[i];
newCornersKept[ki] = newCorners[i];
cv::Point2f pt = lastCorners_[i] - newCorners[i];
++ki;
}
}
@@ -602,11 +624,11 @@ Transform OdometryOpticalFlow::computeTransformStereo(
float newDisparity = newCornersKept[i].x - newCornersKeptRight[i].x;
if(lastDisparity > 0.0f && newDisparity > 0.0f)
{
pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3d(
pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D(
lastCornersKept[i],
lastDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
pcl::PointXYZ newPt3D = util3d::projectDisparityTo3d(
pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D(
newCornersKept[i],
newDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
@@ -828,7 +850,7 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols-1)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows-1)))
{
pcl::PointXYZ pt = util3d::getDepth(data.depth(), newCorners[i].x, newCorners[i].y,
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
if(pcl::isFinite(pt) &&
uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
@@ -971,7 +993,7 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)-1.0f) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)-1.0f))
{
pcl::PointXYZ pt = util3d::getDepth(data.depth(), newCorners[i].x, newCorners[i].y,
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
if(pcl::isFinite(pt) &&
uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
@@ -1059,6 +1081,12 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
unsigned int minPoints = 100;
if(!data.depth().empty())
{
if(data.depth().type() == CV_8UC1)
{
UERROR("ICP 3D cannot be done on stereo images!");
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
data.depth(),
data.fx(),
corelib/src/toro3d/transformation3.hxx
+5 -5
View File
@@ -216,7 +216,7 @@ inline Vector3<T> Quaternion<T>::toAngles() const{
T n = this->norm();
T s = n > 0?2./(n*n):0.;
T m00, m01, m02, m10, m11, m12, m20, m21, m22;
T m00, m10, m20, m21, m22;
T phi,theta,psi;
@@ -238,17 +238,17 @@ inline Vector3<T> Quaternion<T>::toAngles() const{
T zz = this->z*zs;
m00 = 1.0 - (yy + zz);
m11 = 1.0 - (xx + zz);
//m11 = 1.0 - (xx + zz);
m22 = 1.0 - (xx + yy);
m10 = xy + wz;
m01 = xy - wz;
//m01 = xy - wz;
m20 = xz - wy;
m02 = xz + wy;
//m02 = xz + wy;
m21 = yz + wx;
m12 = yz - wx;
//m12 = yz - wx;
phi = atan2(m21,m22);
theta = atan2(-m20,sqrt(m21*m21 + m22*m22));
corelib/src/util3d.cpp
+302 -26
View File
@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/nonfree/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/video/tracking.hpp>
#include <rtabmap/core/VWDictionary.h>
#include <cmath>
#include <stdio.h>
@@ -318,8 +319,8 @@ cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
return depth32F;
}
std::multimap<int, pcl::PointXYZ> generateWords3(
const std::multimap<int, cv::KeyPoint> & words,
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
float fx,
float fy,
@@ -327,26 +328,133 @@ std::multimap<int, pcl::PointXYZ> generateWords3(
float cy,
const Transform & transform)
{
std::multimap<int, pcl::PointXYZ> words3;
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
if(!depth.empty())
{
pcl::PointXYZ pt = util3d::getDepth(
depth,
iter->second.pt.x,
iter->second.pt.y,
keypoints3d->resize(keypoints.size());
for(unsigned int i=0; i!=keypoints.size(); ++i)
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(
depth,
keypoints[i].pt.x,
keypoints[i].pt.y,
cx,
cy,
fx,
fy,
true);
if(!transform.isNull() && !transform.isIdentity())
{
pt = pcl::transformPoint(pt, util3d::transformToEigen3f(transform));
}
keypoints3d->at(i) = pt;
}
}
return keypoints3d;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform)
{
UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F));
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
keypoints3d->resize(keypoints.size());
for(unsigned int i=0; i!=keypoints.size(); ++i)
{
pcl::PointXYZ pt = util3d::projectDisparityTo3D(
keypoints[i].pt,
disparity,
cx,
cy,
fx,
fy,
true);
baseline);
if(!transform.isNull() && !transform.isIdentity())
if(pcl::isFinite(pt) && !transform.isNull() && !transform.isIdentity())
{
pt = pcl::transformPoint(pt, util3d::transformToEigen3f(transform));
}
words3.insert(std::make_pair(iter->first, pt));
keypoints3d->at(i) = pt;
}
return words3;
return keypoints3d;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr 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,
int flowWinSize,
int flowMaxLevel,
int flowIterations,
double flowEps)
{
UASSERT(!leftImage.empty() && !rightImage.empty() &&
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols);
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
std::vector<cv::Point2f> rightCorners;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
leftImage,
rightImage,
leftCorners,
rightCorners,
status,
err,
cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
keypoints3d->resize(keypoints.size());
float bad_point = std::numeric_limits<float>::quiet_NaN ();
UASSERT(status.size() == keypoints.size());
for(unsigned int i=0; i<status.size(); ++i)
{
pcl::PointXYZ pt(bad_point, bad_point, bad_point);
if(status[i])
{
float disparity = leftCorners[i].x - rightCorners[i].x;
if(disparity > 0.0f)
{
pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
leftCorners[i],
disparity,
cx, cy, fx, baseline);
if(pcl::isFinite(tmpPt))
{
pt = tmpPt;
if(!transform.isNull() && !transform.isIdentity())
{
pt = pcl::transformPoint(pt, util3d::transformToEigen3f(transform));
}
}
}
}
keypoints3d->at(i) = pt;
}
return keypoints3d;
}
std::multimap<int, cv::KeyPoint> aggregate(
@@ -435,7 +543,7 @@ void findCorrespondences(
inliers2.resize(oi);
}
pcl::PointXYZ getDepth(
pcl::PointXYZ projectDepthTo3D(
const cv::Mat & depthImage,
float x, float y,
float cx, float cy,
@@ -685,6 +793,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
float fx, float fy,
int decimation)
{
UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(decimation < 1)
{
@@ -706,7 +815,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
{
pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
pcl::PointXYZ ptXYZ = getDepth(imageDepth, w, h, cx, cy, fx, fy, false);
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
@@ -725,6 +834,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
int decimation)
{
UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(decimation < 1)
{
@@ -770,7 +880,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
pt.r = v;
}
pcl::PointXYZ ptXYZ = getDepth(imageDepth, w, h, cx, cy, fx, fy, false);
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
@@ -835,7 +945,7 @@ 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);
pcl::PointXYZ ptXYZ = projectDisparityTo3d(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
pcl::PointXYZ ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
@@ -844,7 +954,35 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
return cloud;
}
cv::Mat disparityFromStereoImages(const cv::Mat & leftImage, const cv::Mat & rightImage)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
const cv::Mat & imageLeft,
const cv::Mat & imageRight,
float cx, float cy,
float fx, float fy,
int decimation)
{
UASSERT(imageRight.type() == CV_8UC1);
cv::Mat leftMono;
if(imageLeft.channels() == 3)
{
cv::cvtColor(imageLeft, leftMono, CV_BGR2GRAY);
}
else
{
leftMono = imageLeft;
}
return rtabmap::util3d::cloudFromDisparityRGB(
imageLeft,
util3d::disparityFromStereoImages(leftMono, imageRight),
cx, cy,
fx, fy,
decimation);
}
cv::Mat disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage)
{
UASSERT(!leftImage.empty() && !rightImage.empty() &&
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
@@ -852,13 +990,132 @@ cv::Mat disparityFromStereoImages(const cv::Mat & leftImage, const cv::Mat & rig
leftImage.rows == rightImage.rows);
cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET, 160, 15);
cv::Mat disparity;
stereo(leftImage, rightImage, disparity, CV_16S);
stereo(leftImage, rightImage, disparity, CV_16SC1);
cv::filterSpeckles(disparity, 0, 1000, 16);
return disparity;
}
cv::Mat disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
int flowWinSize,
int flowMaxLevel,
int flowIterations,
double flowEps)
{
UASSERT(!leftImage.empty() && !rightImage.empty() &&
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
leftImage.cols == rightImage.cols &&
leftImage.rows == rightImage.rows);
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
std::vector<cv::Point2f> rightCorners;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
leftImage,
rightImage,
leftCorners,
rightCorners,
status,
err,
cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
return disparityFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status);
}
cv::Mat depthFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
float fx,
float baseline,
int flowWinSize,
int flowMaxLevel,
int flowIterations,
double flowEps)
{
UASSERT(!leftImage.empty() && !rightImage.empty() &&
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
leftImage.cols == rightImage.cols &&
leftImage.rows == rightImage.rows);
UASSERT(fx > 0.0f && baseline > 0.0f);
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
std::vector<cv::Point2f> rightCorners;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
leftImage,
rightImage,
leftCorners,
rightCorners,
status,
err,
cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
return depthFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, fx, baseline);
}
cv::Mat disparityFromStereoCorrespondences(
const cv::Mat & leftImage,
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
const std::vector<unsigned char> & mask)
{
UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
cv::Mat disparity = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
for(unsigned int i=0; i<leftCorners.size(); ++i)
{
if(mask.size() == 0 || mask[i])
{
float d = leftCorners[i].x - rightCorners[i].x;
if(d > 0.0f)
{
disparity.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
}
}
}
return disparity;
}
cv::Mat 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)
{
UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
cv::Mat depth = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
for(unsigned int i=0; i<leftCorners.size(); ++i)
{
if(mask.size() == 0 || mask[i])
{
float disparity = leftCorners[i].x - rightCorners[i].x;
if(disparity > 0.0f)
{
float d = baseline * fx / disparity;
depth.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
}
}
}
return depth;
}
// inspired from ROS image_geometry/src/stereo_camera_model.cpp
pcl::PointXYZ projectDisparityTo3d(
pcl::PointXYZ projectDisparityTo3D(
const cv::Point2f & pt,
float disparity,
float cx, float cy, float fx, float baseline)
@@ -872,18 +1129,36 @@ pcl::PointXYZ projectDisparityTo3d(
return pcl::PointXYZ(bad_point, bad_point, bad_point);
}
pcl::PointXYZ projectDisparityTo3D(
const cv::Point2f & pt,
const cv::Mat & disparity,
float cx, float cy, float fx, float baseline)
{
UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
int u = int(pt.x+0.5f);
int v = int(pt.y+0.5f);
float bad_point = std::numeric_limits<float>::quiet_NaN ();
if(uIsInBounds(u, 0, disparity.cols-1) &&
uIsInBounds(v, 0, disparity.rows-1))
{
float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
return projectDisparityTo3D(pt, d, cx, cy, fx, baseline);
}
return pcl::PointXYZ(bad_point, bad_point, bad_point);
}
cv::Mat depthFromDisparity(const cv::Mat & disparity,
float cx, float cy, float fx, float baseline,
float fx, float baseline,
int type)
{
UASSERT(disparity.type() == CV_32FC1 || disparity.type() == CV_16S);
UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
UASSERT(type == CV_32FC1 || type == CV_16U);
cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
for (int i = 0; i < disparity.rows; i++)
{
for (int j = 0; j < disparity.cols; j++)
{
float disparity_value = disparity.type() == CV_16S?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
if (disparity_value > 0.0f)
{
// baseline * focal / disparity
@@ -1132,8 +1407,8 @@ void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2
iter!=correspondences.end();
++iter)
{
pcl::PointXYZ pt1 = getDepth(depthImage1, iter->first.x, iter->first.y, cx, cy, fx, fy, true);
pcl::PointXYZ pt2 = getDepth(depthImage2, iter->second.x, iter->second.y, cx, cy, fx, fy, true);
pcl::PointXYZ pt1 = projectDepthTo3D(depthImage1, iter->first.x, iter->first.y, cx, cy, fx, fy, true);
pcl::PointXYZ pt2 = projectDepthTo3D(depthImage2, iter->second.x, iter->second.y, cx, cy, fx, fy, true);
if(pcl::isFinite(pt1) && pcl::isFinite(pt2) &&
(maxDepth <= 0 || (pt1.z <= maxDepth && pt2.z<=maxDepth)))
{
@@ -1691,6 +1966,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
int samples,
const Transform & transform)
{
UASSERT(!depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = cloudFromDepth(
depth,
@@ -1767,7 +2043,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr get3DFASTKpts(
pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
for(unsigned int i=0; i<kpts.size(); ++i)
{
pcl::PointXYZ pt = getDepth(imageDepth, kpts[i].pt.x, kpts[i].pt.y, 0, 0, 1.0f/constant, 1.0f/constant, true);
pcl::PointXYZ pt = projectDepthTo3D(imageDepth, kpts[i].pt.x, kpts[i].pt.y, 0, 0, 1.0f/constant, 1.0f/constant, true);
if(uIsFinite(pt.z) && (maxDepth <= 0 || pt.z <= maxDepth))
{
points->push_back(pt);
examples/RGBDMapping/MapBuilder.h
+22 -8
View File
@@ -46,7 +46,9 @@ class MapBuilder : public QWidget, public UEventsHandler
{
Q_OBJECT
public:
MapBuilder()
MapBuilder() :
_processingStatistics(false),
_lastOdometryProcessed(true)
{
this->setWindowFlags(Qt::Dialog);
this->setWindowTitle(tr("3D Map"));
@@ -60,7 +62,7 @@ public:
this->setLayout(layout);
qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
qRegisterMetaType<rtabmap::SensorData>("rtabmap::Image");
qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
}
virtual ~MapBuilder()
@@ -128,15 +130,14 @@ private slots:
}
}
cloudViewer_->render();
_lastOdometryProcessed = true;
}
void processStatistics(const rtabmap::Statistics & stats)
{
if(!this->isVisible())
{
return;
}
_processingStatistics = true;
const std::map<int, Transform> & poses = stats.poses();
QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
@@ -198,6 +199,8 @@ private slots:
}
cloudViewer_->render();
_processingStatistics = false;
}
protected:
@@ -208,19 +211,30 @@ protected:
RtabmapEvent * rtabmapEvent = (RtabmapEvent *)event;
const Statistics & stats = rtabmapEvent->getStats();
// Statistics must be processed in the Qt thread
QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats));
if(this->isVisible())
{
QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats));
}
}
else if(event->getClassName().compare("OdometryEvent") == 0)
{
OdometryEvent * odomEvent = (OdometryEvent *)event;
// Odometry must be processed in the Qt thread
QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::SensorData, odomEvent->data()));
if(this->isVisible() &&
_lastOdometryProcessed &&
!_processingStatistics)
{
_lastOdometryProcessed = false; // if we receive too many odometry events!
QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::SensorData, odomEvent->data()));
}
}
}
private:
CloudViewer * cloudViewer_;
Transform lastOdomPose_;
bool _processingStatistics;
bool _lastOdometryProcessed;
};
examples/RGBDMapping/main.cpp
+67 -3
View File
@@ -36,12 +36,35 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "MapBuilder.h"
void showUsage()
{
printf("\nUsage:\n"
"rtabmap-rgbd_mapping driver\n"
" driver Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n");
exit(1);
}
using namespace rtabmap;
int main(int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
int driver = 0;
if(argc < 2)
{
showUsage();
}
else
{
driver = atoi(argv[argc-1]);
if(driver < 0 || driver > 4)
{
UERROR("driver should be between 0 and 4.");
showUsage();
}
}
// GUI stuff, there the handler will receive RtabmapEvent and construct the map
QApplication app(argc, argv);
MapBuilder mapBuilder;
@@ -51,8 +74,49 @@ int main(int argc, char * argv[])
// Create the OpenNI camera, it will send a CameraEvent at the rate specified.
// Set transform to camera so z is up, y is left and x going forward
CameraRGBD * camera = new CameraOpenni("", 10, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
//CameraRGBD * camera = new CameraFreenect(0, 10, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
CameraRGBD * camera = 0;
Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
if(driver == 1)
{
if(!CameraOpenNI2::available())
{
UERROR("Not built with OpenNI2 support...");
exit(-1);
}
camera = new CameraOpenNI2(0, opticalRotation);
}
else if(driver == 2)
{
if(!CameraFreenect::available())
{
UERROR("Not built with Freenect support...");
exit(-1);
}
camera = new CameraFreenect(0, 0, opticalRotation);
}
else if(driver == 3)
{
if(!CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new CameraOpenNICV(false, 0, opticalRotation);
}
else if(driver == 4)
{
if(!CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new CameraOpenNICV(true, 0, opticalRotation);
}
else
{
camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
}
CameraThread cameraThread(camera);
if(!cameraThread.init())
{
@@ -61,7 +125,7 @@ int main(int argc, char * argv[])
}
// Create an odometry thread to process camera events, it will send OdometryEvent.
OdometryThread odomThread(new OdometryBOW()); // 0=SURF 1=SIFT
OdometryThread odomThread(new OdometryBOW());
// Create RTAB-Map to process OdometryEvent
guilib/src/DatabaseViewer.cpp
+11 -23
View File
@@ -1410,19 +1410,9 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
if(depthA.type() == CV_8UC1)
{
cv::Mat leftImg;
if(imageA.channels() == 3)
{
cv::cvtColor(imageA, leftImg, CV_BGR2GRAY);
}
else
{
leftImg = imageA;
}
cv::Mat disparity = util3d::disparityFromStereoImages(leftImg, depthA);
cloudA = rtabmap::util3d::cloudFromDisparityRGB(
cloudA = rtabmap::util3d::cloudFromStereoImages(
imageA,
disparity,
depthA,
cxA, cyA,
fxA, fyA,
1);
@@ -1443,18 +1433,9 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
if(depthB.type() == CV_8UC1)
{
cv::Mat leftImg;
if(imageB.channels() == 3)
{
cv::cvtColor(imageB, leftImg, CV_BGR2GRAY);
}
else
{
leftImg = imageB;
}
cloudB = rtabmap::util3d::cloudFromDisparityRGB(
cloudB = rtabmap::util3d::cloudFromStereoImages(
imageB,
util3d::disparityFromStereoImages(leftImg, depthB),
depthB,
cxB, cyB,
fxB, fyB,
1);
@@ -1786,6 +1767,13 @@ void DatabaseViewer::refineConstraint(int from, int to)
cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1)
{
QMessageBox::critical(this, tr("ICP failed"), tr("ICP cannot be done on stereo images!"));
UERROR("ICP 3D cannot be done on stereo images!");
return;
}
cloudA = util3d::getICPReadyCloud(depthA,
fxA, fyA, cxA, cyA,
ui_->spinBox_icp_decimation->value(),
guilib/src/LoopClosureViewer.cpp
+36 -12
View File
@@ -147,12 +147,24 @@ void LoopClosureViewer::updateView(const Transform & transform)
//cloud 3d
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
cloudA = util3d::cloudFromDepthRGB(
imageA,
depthA,
sA_->getDepthCx(), sA_->getDepthCy(),
sA_->getDepthFx(), sA_->getDepthFy(),
decimation);
if(depthA.type() == CV_8UC1)
{
cloudA = util3d::cloudFromStereoImages(
imageA,
depthA,
sA_->getDepthCx(), sA_->getDepthCy(),
sA_->getDepthFx(), sA_->getDepthFy(),
decimation);
}
else
{
cloudA = util3d::cloudFromDepthRGB(
imageA,
depthA,
sA_->getDepthCx(), sA_->getDepthCy(),
sA_->getDepthFx(), sA_->getDepthFy(),
decimation);
}
cloudA = util3d::removeNaNFromPointCloud(cloudA);
@@ -167,12 +179,24 @@ void LoopClosureViewer::updateView(const Transform & transform)
cloudA = util3d::transformPointCloud(cloudA, sA_->getLocalTransform());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
cloudB = util3d::cloudFromDepthRGB(
imageB,
depthB,
sB_->getDepthCx(), sB_->getDepthCy(),
sB_->getDepthFx(), sB_->getDepthFy(),
decimation);
if(depthB.type() == CV_8UC1)
{
cloudB = util3d::cloudFromStereoImages(
imageB,
depthB,
sB_->getDepthCx(), sB_->getDepthCy(),
sB_->getDepthFx(), sB_->getDepthFy(),
decimation);
}
else
{
cloudB = util3d::cloudFromDepthRGB(
imageB,
depthB,
sB_->getDepthCx(), sB_->getDepthCy(),
sB_->getDepthFx(), sB_->getDepthFy(),
decimation);
}
cloudB = util3d::removeNaNFromPointCloud(cloudB);
guilib/src/MainWindow.cpp
+19 -6
View File
@@ -3766,12 +3766,25 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createCloud(
float maxDepth) const
{
UTimer timer;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
rgb,
depth,
cx, cy,
fx, fy,
decimation);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(depth.type() == CV_8UC1)
{
cloud = util3d::cloudFromStereoImages(
rgb,
depth,
cx, cy,
fx, fy,
decimation);
}
else
{
cloud = util3d::cloudFromDepthRGB(
rgb,
depth,
cx, cy,
fx, fy,
decimation);
}
if(cloud->size())
{
guilib/src/OdometryViewer.cpp
+18 -6
View File
@@ -98,12 +98,24 @@ void OdometryViewer::processData()
// visualization: buffering the clouds
// Create the new cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = util3d::cloudFromDepthRGB(
data.image(),
data.depth(),
data.cx(), data.cy(),
data.fx(), data.fy(),
decimation_);
if(data.depth().type() == CV_8UC1)
{
cloud = util3d::cloudFromStereoImages(
data.image(),
data.depth(),
data.cx(), data.cy(),
data.fx(), data.fy(),
decimation_);
}
else
{
cloud = util3d::cloudFromDepthRGB(
data.image(),
data.depth(),
data.cx(), data.cy(),
data.fx(), data.fy(),
decimation_);
}
if(voxelSize_ > 0.0f)
{
guilib/src/PreferencesDialog.cpp
+9
View File
@@ -482,6 +482,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_flow_subpix_iterations->setObjectName(Parameters::kOdomFlowSubPixIterations().c_str());
_ui->odom_flow_subpix_eps->setObjectName(Parameters::kOdomFlowSubPixEps().c_str());
//Stereo
_ui->stereo_winSize->setObjectName(Parameters::kStereoWinSize().c_str());
_ui->stereo_maxLevel->setObjectName(Parameters::kStereoMaxLevel().c_str());
_ui->stereo_iterations->setObjectName(Parameters::kStereoIterations().c_str());
_ui->stereo_eps->setObjectName(Parameters::kStereoEps().c_str());
_ui->stereo_subpix_winSize->setObjectName(Parameters::kStereoSubPixWinSize().c_str());
_ui->stereo_subpix_iterations->setObjectName(Parameters::kStereoSubPixIterations().c_str());
_ui->stereo_subpix_eps->setObjectName(Parameters::kStereoSubPixEps().c_str());
setupSignals();
// custom signals
connect(_ui->doubleSpinBox_kp_roi0, SIGNAL(valueChanged(double)), this, SLOT(updateKpROI()));
guilib/src/ui/preferencesDialog.ui
+243 -1
View File
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>9</number>
<number>22</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -5833,6 +5833,248 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
</layout>
</widget>
<widget class="QWidget" name="page_29">
<layout class="QVBoxLayout" name="verticalLayout_57">
<item>
<widget class="QGroupBox" name="groupBox_stereo2">
<property name="title">
<string>Stereo</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_56">
<item>
<widget class="QLabel" name="label_187">
<property name="text">
<string>These parameters are used to find the 3D positions of visual words using disparity between the left and right images. Optical flow is used to find corresponding corners in the right image of words extracted from the left image. Sub pixel corners may not be used for features like SURF/SIFT, which are already sub pixel.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_10">
<property name="title">
<string>calcOpticalFlowPyrLK()</string>
</property>
<layout class="QGridLayout" name="gridLayout_33" columnstretch="0,1">
<item row="0" column="0">
<widget class="QSpinBox" name="stereo_winSize">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>21</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_205">
<property name="text">
<string>Window size.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="stereo_iterations">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>30</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_206">
<property name="text">
<string>Iterations.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="stereo_eps">
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>0.100000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.010000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_207">
<property name="text">
<string>Epsilon.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_208">
<property name="text">
<string>Max level.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QSpinBox" name="stereo_maxLevel">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>3</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_9">
<property name="title">
<string>cornerSubPix()</string>
</property>
<layout class="QGridLayout" name="gridLayout_32" columnstretch="0,1">
<item row="0" column="0">
<widget class="QSpinBox" name="stereo_subpix_winSize">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>5</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_200">
<property name="text">
<string>Window size for sub pixel estimation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="stereo_subpix_iterations">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>20</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_203">
<property name="text">
<string>Iterations for sub pixel estimation. 0 disables sub pixel refining.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="stereo_subpix_eps">
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>0.100000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.030000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_204">
<property name="text">
<string>Epsilon for sub pixel estimation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer_29">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>464</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<widget class="QWidget" name="page_12">
<layout class="QVBoxLayout" name="verticalLayout_32">
<item>
tools/OdometryViewer/main.cpp
+12 -11
View File
@@ -44,8 +44,8 @@ void showUsage()
"odometryViewer [options]\n"
"Options:\n"
" -driver # Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n"
" -o # Odometry type (default 0): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF, 5=GFTT/FREAK, 6=GFTT/BRIEF, 7=BRISK\n"
" -nn # Nearest neighbor strategy (default 1): kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4\n"
" -o # Odometry type (default 6): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF, 5=GFTT/FREAK, 6=GFTT/BRIEF, 7=BRISK\n"
" -nn # Nearest neighbor strategy (default 3): kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4\n"
" -nndr # Nearest neighbor distance ratio (default 0.7)\n"
" -icp Use ICP odometry\n"
" -flow Use optical flow odometry.\n"
@@ -55,17 +55,17 @@ void showUsage()
" -clouds # Maximum clouds shown (default 10, zero means inf)\n"
" -sec #.# Delay (seconds) before reading the database (if set)\n"
"\n"
" -in #.# Inliers maximum distance, features/ICP (default 0.005 m)\n"
" -in #.# Inliers maximum distance, features/ICP (default 0.01 m)\n"
" -max # Max features used for matching (default 0=inf)\n"
" -min # Minimum inliers to accept the transform (default 20)\n"
" -depth #.# Maximum features depth (default 5.0 m)\n"
" -i # RANSAC/ICP iterations (default 100)\n"
" -i # RANSAC/ICP iterations (default 30)\n"
" -r #.# Words ratio (default 0.5)\n"
" -lu # Linear update (default 0.0 m)\n"
" -au # Angular update (default 0.0 radian)\n"
" -reset # Reset countdown (default 0 = disabled)\n"
" -gpu Use GPU\n"
" -lh # Local history (default 0)\n"
" -lh # Local history (default 1000)\n"
"\n"
" -brief_bytes # BRIEF bytes (default 32)\n"
" -fast_thr # FAST threshold (default 30)\n"
@@ -84,7 +84,7 @@ void showUsage()
" odometryViewer -odom 4 -nn 2 -lh 1000 FAST/BRIEF example\n"
" odometryViewer -odom 3 -nn 2 -lh 1000 FAST/FREAK example\n"
" odometryViewer -icp -in 0.05 -i 30 ICP example\n"
" odometryViewer -flow Optical flow example\n");
" odometryViewer -flow -in 0.02 Optical flow example\n");
exit(1);
}
@@ -97,17 +97,17 @@ int main (int argc, char * argv[])
float rate = 0.0;
std::string inputDatabase;
int driver = 0;
int odomType = 0;
int odomType = 6;
bool icp = false;
bool flow = false;
int nnType =1;
int nnType =3;
float nndr = 0.7f;
float distance = 0.005;
float distance = 0.01;
int maxWords = 0;
int minInliers = 20;
float wordsRatio = 0.5;
float maxDepth = 5.0f;
int iterations = 100;
int iterations = 30;
float linearUpdate = 0.0f;
float angularUpdate = 0.0f;
int resetCountdown = 0;
@@ -120,7 +120,7 @@ int main (int argc, char * argv[])
int fastThr = 30;
float sec = 0.0f;
bool gpu = false;
int localHistory = 0;
int localHistory = 1000;
bool p2p = false;
for(int i=1; i<argc; ++i)
@@ -683,6 +683,7 @@ int main (int argc, char * argv[])
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomInlierDistance(), uNumber2Str(distance)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMinInliers(), uNumber2Str(minInliers)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), uNumber2Str(iterations)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeatureType(), uNumber2Str(odomType)));
odom = new rtabmap::OdometryOpticalFlow(parameters);
}
else