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

Added GFTT detector which can be used for Odometry

Browse Source 
Modified how depth is computed: Now the mean of neighbors is used as the value instead of interpolation... averaging the Kinect noise.

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1662 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-08-20 23:05:06 +00:00
parent b0af55699d
commit f251b64310
11 changed files with 546 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

137
corelib/src/Features2d.cpp
View File

@@ -705,4 +705,141 @@ cv::Mat FAST_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
return descriptors;
}
//////////////////////////
//GFTT
//////////////////////////
GFTT::GFTT(const ParametersMap & parameters) :
_maxCorners(Parameters::defaultGFTTMaxCorners()),
_qualityLevel(Parameters::defaultGFTTQualityLevel()),
_minDistance(Parameters::defaultGFTTMinDistance()),
_blockSize(Parameters::defaultGFTTBlockSize()),
_useHarrisDetector(Parameters::defaultGFTTUseHarrisDetector()),
_k(Parameters::defaultGFTTK()),
_gftt(0)
{
parseParameters(parameters);
}
GFTT::~GFTT()
{
if(_gftt)
{
delete _gftt;
}
}
void GFTT::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kGFTTMaxCorners(), _maxCorners);
Parameters::parse(parameters, Parameters::kGFTTQualityLevel(), _qualityLevel);
Parameters::parse(parameters, Parameters::kGFTTMinDistance(), _minDistance);
Parameters::parse(parameters, Parameters::kGFTTBlockSize(), _blockSize);
Parameters::parse(parameters, Parameters::kGFTTUseHarrisDetector(), _useHarrisDetector);
Parameters::parse(parameters, Parameters::kGFTTK(), _k);
if(_gftt)
{
delete _gftt;
_gftt = 0;
}
_gftt = new cv::GFTTDetector(_maxCorners, _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
}
std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
std::vector<cv::KeyPoint> keypoints;
cv::Mat imgRoi(image, roi);
_gftt->detect(imgRoi, keypoints); // Opencv keypoints
return keypoints;
}
//////////////////////////
//FAST-BRIEF
//////////////////////////
GFTT_BRIEF::GFTT_BRIEF(const ParametersMap & parameters) :
GFTT(parameters),
bytes_(Parameters::defaultBRIEFBytes()),
_brief(0)
{
parseParameters(parameters);
}
GFTT_BRIEF::~GFTT_BRIEF()
{
if(_brief)
{
delete _brief;
}
}
void GFTT_BRIEF::parseParameters(const ParametersMap & parameters)
{
GFTT::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kBRIEFBytes(), bytes_);
if(_brief)
{
delete _brief;
_brief = 0;
}
_brief = new cv::BriefDescriptorExtractor(bytes_);
}
cv::Mat GFTT_BRIEF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
cv::Mat descriptors;
_brief->compute(image, keypoints, descriptors);
return descriptors;
}
//////////////////////////
//FAST-FREAK
//////////////////////////
GFTT_FREAK::GFTT_FREAK(const ParametersMap & parameters) :
GFTT(parameters),
orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
patternScale_(Parameters::defaultFREAKPatternScale()),
nOctaves_(Parameters::defaultFREAKNOctaves()),
_freak(0)
{
parseParameters(parameters);
}
GFTT_FREAK::~GFTT_FREAK()
{
if(_freak)
{
delete _freak;
}
}
void GFTT_FREAK::parseParameters(const ParametersMap & parameters)
{
GFTT::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
if(_freak)
{
delete _freak;
_freak = 0;
}
_freak = new cv::FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
}
cv::Mat GFTT_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
cv::Mat descriptors;
_freak->compute(image, keypoints, descriptors);
return descriptors;
}
}

8
corelib/src/Memory.cpp
View File

@@ -405,6 +405,14 @@ void Memory::parseParameters(const ParametersMap & parameters)
_feature2D = new ORB(parameters);
_featureType = Feature2D::kFeatureOrb;
break;
case Feature2D::kFeatureGfttFreak:
_feature2D = new GFTT_FREAK(parameters);
_featureType = Feature2D::kFeatureGfttFreak;
break;
case Feature2D::kFeatureGfttBrief:
_feature2D = new GFTT_BRIEF(parameters);
_featureType = Feature2D::kFeatureGfttBrief;
break;
case Feature2D::kFeatureSurf:
default:
_feature2D = new SURF(parameters);

82
corelib/src/Odometry.cpp
View File

@@ -88,9 +88,15 @@ void Odometry::reset()
bool Odometry::isLargeEnoughTransform(const Transform & transform)
{
return fabs(transform.x()) > _linearUpdate ||
fabs(transform.y()) > _linearUpdate ||
fabs(transform.z()) > _linearUpdate;
float x,y,z, roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
return (_linearUpdate == 0.0f && _angularUpdate == 0.0f) ||
fabs(x) > _linearUpdate ||
fabs(y) > _linearUpdate ||
fabs(z) > _linearUpdate ||
fabs(roll) > _angularUpdate ||
fabs(pitch) > _angularUpdate ||
fabs(yaw) > _angularUpdate;
}
Transform Odometry::process(SensorData & data, int * quality)
@@ -147,7 +153,8 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
group.compare("BRIEF") == 0 ||
group.compare("FAST") == 0 ||
group.compare("ORB") == 0 ||
group.compare("FREAK") == 0)
group.compare("FREAK") == 0 ||
group.compare("GFTT") == 0)
{
customParameters.insert(*iter);
}
@@ -173,6 +180,34 @@ void OdometryBOW::reset()
localMap_.clear();
}
std::multimap<int,pcl::PointXYZ> OdometryBOW::getLocalMeansMap() const
{
std::multimap<int,pcl::PointXYZ> localMeansMap;
for(std::multimap<int, std::pair<int, pcl::PointXYZ> >::const_iterator iter=localMap_.begin();
iter!= localMap_.end();)
{
int id = iter->first;
pcl::PointXYZ sumPt = iter->second.second;
int count = 1;
++iter;
std::multimap<int, std::pair<int, pcl::PointXYZ> >::const_iterator jter=iter;
while(jter->first == id && jter!= localMap_.end())
{
sumPt.x += jter->second.second.x;
sumPt.y += jter->second.second.y;
sumPt.z += jter->second.second.z;
++count;
++jter;
}
iter = jter;
sumPt.x /= float(count);
sumPt.y /= float(count);
sumPt.z /= float(count);
localMeansMap.insert(std::make_pair(id, sumPt));
}
return localMeansMap;
}
// return not null transform if odometry is correctly computed
Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
@@ -207,11 +242,14 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
//Create the local map with mean of all features
std::multimap<int, pcl::PointXYZ> localMeansMap = getLocalMeansMap();
// No need to set max depth here, it is already applied in extractKeypointsAndDescriptors() above.
// Also! the localMap_ have points not in camera frame anymore (in local map frame), so filtering
// by depth here is wrong!
util3d::findCorrespondences(
localMap_,
localMeansMap,
newSignature->getWords3(),
*inliers1,
*inliers2,
@@ -274,7 +312,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
if(pcl::isFinite(pt))
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform);
localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt2));
localMap_.insert(std::make_pair(*iter, std::make_pair(newSignature->id(), pt2)));
}
}
}
@@ -290,25 +328,35 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
{
if(newSignature->getWords3().count(*iter) == 1 &&
uniqueCorrespondences.find(*iter) == uniqueCorrespondences.end())
if(newSignature->getWords3().count(*iter) == 1)
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt))
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform);
localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt2));
localMap_.insert(std::make_pair(*iter, std::make_pair(newSignature->id(), pt2)));
}
}
}
while(localMap_.size() && (int)localMap_.size() > this->getLocalHistory() && _memory->getStMem().size()>1)
while(localMap_.size() && (int)uUniqueKeys(localMap_).size() > this->getLocalHistory() && _memory->getStMem().size()>1)
{
std::list<int> deletedWords;
_memory->deleteLocation(*_memory->getStMem().begin(), &deletedWords);
for(std::list<int>::iterator iter = deletedWords.begin(); iter!=deletedWords.end(); ++iter)
int nodeId = *_memory->getStMem().begin();
std::list<int> removedPts = uUniqueKeys(_memory->getSignature(nodeId)->getWords3());
for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
{
localMap_.erase(*iter);
bool removed = false;
for(std::multimap<int, std::pair<int, pcl::PointXYZ> >::iterator jter=localMap_.lower_bound(*iter);
jter->first == *iter && !removed;
++jter)
{
if(jter->second.first == nodeId)
{
localMap_.erase(jter);
removed = true;
}
}
}
_memory->deleteLocation(*_memory->getStMem().begin());
}
}
else
@@ -331,7 +379,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt))
{
localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt));
localMap_.insert(std::make_pair(*iter, std::make_pair(newSignature->id(), pt)));
}
}
}
@@ -340,11 +388,13 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
_memory->emptyTrash();
}
UINFO("Odom update time = %fs features=%d inliers=%d/%d dict=%d nodes=%d",
UINFO("Odom update time = %fs features=%d inliers=%d/%d local_map=%d[%d] dict=%d nodes=%d",
timer.elapsed(),
nFeatures,
inliers,
correspondences,
(int)uUniqueKeys(localMap_).size(),
(int)localMap_.size(),
(int)_memory->getVWDictionary()->getVisualWords().size(),
(int)_memory->getStMem().size());
return output;

144
corelib/src/util3d.cpp
View File

@@ -440,7 +440,8 @@ pcl::PointXYZ getDepth(
float x, float y,
float cx, float cy,
float fx, float fy,
bool interpolate)
bool smoothing,
float maxZError)
{
pcl::PointXYZ pt;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
@@ -453,104 +454,73 @@ pcl::PointXYZ getDepth(
return pt;
}
// Use correct principal point from calibration
float center_x = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
float center_y = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f; //cameraInfo.K.at(5)
bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
// Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
float unit_scaling = isInMM?0.001f:1.0f;
float constant_x = unit_scaling / fx; //cameraInfo.K.at(0)
float constant_y = unit_scaling / fy; //cameraInfo.K.at(4)
// Inspired from RGBDFrame::getGaussianMixtureDistribution() method from
// https://github.com/ccny-ros-pkg/rgbdtools/blob/master/src/rgbd_frame.cpp
// Window weights:
// | 1 | 2 | 1 |
// | 2 | 4 | 2 |
// | 1 | 2 | 1 |
int u = int(x+0.5f);
int v = int(y+0.5f);
int u_start = std::max(u-1, 0);
int v_start = std::max(v-1, 0);
int u_end = std::min(u+1, depthImage.cols-1);
int v_end = std::min(v+1, depthImage.rows-1);
float depth = 0.0f;
if(!interpolate || (int(x) < 1 || int(y) < 1 || int(x) >= depthImage.cols-1 || int(y) >= depthImage.rows-1))
float depth = isInMM?(float)depthImage.at<uint16_t>(v,u)*0.001f:depthImage.at<float>(v,u);
if(depth!=0.0f && uIsFinite(depth))
{
if(interpolate)
if(smoothing)
{
UERROR("Cannot interpolate for points on the image side. Falling back to no interpolation.");
float sumWeights = 0.0f;
float sumDepths = 0.0f;
for(int uu = u_start; uu <= u_end; ++uu)
{
for(int vv = v_start; vv <= v_end; ++vv)
{
if(!(uu == u && vv == v))
{
float d = isInMM?(float)depthImage.at<uint16_t>(vv,uu)*0.001f:depthImage.at<float>(vv,uu);
// ignore if not valid or depth difference is too high
if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < maxZError)
{
if(uu == u || vv == v)
{
sumWeights+=2.0f;
d*=2.0f;
}
else
{
sumWeights+=1.0f;
}
sumDepths += d;
}
}
}
}
// set window weight to center point
depth *= 4.0f;
sumWeights += 4.0f;
// mean
depth = (depth+sumDepths)/sumWeights;
}
// select directly to corresponding pixel
depth = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
// Use correct principal point from calibration
cx = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
cy = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f; //cameraInfo.K.at(5)
// Fill in XYZ
pt.x = (x - cx) * depth / fx;
pt.y = (y - cy) * depth / fy;
pt.z = depth;
}
else
{
// Interpolate x axis
float depthX = 0.0f;
float first;
float second;
if(int(x) == int(x+0.5f))
{
first = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)-1):depthImage.at<float>(int(y),int(x)-1);
second = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
}
else
{
first = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
second = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)+1):depthImage.at<float>(int(y),int(x)+1);
}
if(first != 0.0f && uIsFinite(first) && second != 0.0f && uIsFinite(second))
{
// y = ax + b...
float a = second-first;
float b = first - a*(float(int(x))-0.5f);
depthX = a*(x) + b;
//UDEBUG("x=%f, y=%f, first=%f, second=%f, a=%f, b=%f, depth=%f", x,y, first,second, a,b, depthX);
}
if(depthX != 0.0f)
{
depth = depthX;
}
else
{
// Interpolate y axis
float depthY = 0.0f;
if(int(y) == int(y+0.5f))
{
first = isInMM?(float)depthImage.at<uint16_t>(int(y)-1,int(x)):depthImage.at<float>(int(y)-1,int(x));
second = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
}
else
{
first = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
second = isInMM?(float)depthImage.at<uint16_t>(int(y)+1,int(x)):depthImage.at<float>(int(y)+1,int(x));
}
if(first != 0.0f && uIsFinite(first) && second != 0.0f && uIsFinite(second))
{
// y = ax + b...
float a = second-first;
float b = first - a*(float(int(y))-0.5f);
depthY = a*(y) + b;
//UWARN("x=%f, y=%f, first=%f, second=%f, a=%f, b=%f, depth=%f", x,y, first,second, a,b, depthY);
}
if(depthY != 0.0f)
{
depth = depthY;
}
else
{
//UWARN("Could not compute depth for x=%f, y=%f", x,y);
}
}
}
// Check for invalid measurements
if (depth==0.0f || !uIsFinite(depth))
{
pt.x = pt.y = pt.z = bad_point;
}
else
{
// Fill in XYZ
pt.x = (x - center_x) * depth * constant_x;
pt.y = (y - center_y) * depth * constant_y;
pt.z = depth*unit_scaling;
}
return pt;
}