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 keypoints3 field to Image class

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1652 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-08-14 20:29:43 +00:00
parent 7cf89b6d76
commit 9aca634b28
6 changed files with 272 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
corelib/include/rtabmap/core/Features2d.h
View File

@@ -66,8 +66,21 @@ void RTABMAP_EXP filterKeypointsByDepth(
float cy,
float maxDepth);
void RTABMAP_EXP filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
std::vector<cv::Point3f> & keypoints3,
float maxDepth);
void RTABMAP_EXP filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
std::vector<cv::Point3f> & keypoints3,
cv::Mat & descriptors,
float maxDepth);
void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> keypoints3, int maxKeypoints);
void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> keypoints3, cv::Mat & descriptors, int maxKeypoints);
cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);

47
corelib/include/rtabmap/core/Image.h
View File

@@ -47,19 +47,7 @@ public:
int id = 0,
const cv::Mat & descriptors = cv::Mat(),
Feature2D::Type featureType = Feature2D::kFeatureUndef,
const std::vector<cv::KeyPoint> & keypoints = std::vector<cv::KeyPoint>()) :
_image(image),
_id(id),
_descriptors(descriptors),
_featureType(featureType),
_keypoints(keypoints),
_fx(0.0f),
_fy(0.0f),
_cx(0.0f),
_cy(0.0f),
_localTransform(Transform::getIdentity())
{
}
const std::vector<cv::KeyPoint> & keypoints = std::vector<cv::KeyPoint>());
// Metric constructor
Image(const cv::Mat & image,
@@ -70,19 +58,7 @@ public:
float cy,
const Transform & pose,
const Transform & localTransform,
int id = 0) :
_image(image),
_id(id),
_featureType(Feature2D::kFeatureUndef),
_depth(depth),
_fx(fx),
_fy(fy),
_cx(cx),
_cy(cy),
_pose(pose),
_localTransform(localTransform)
{
}
int id = 0);
// Metric constructor + 2d depth
Image(const cv::Mat & image,
@@ -94,20 +70,7 @@ public:
float cy,
const Transform & pose,
const Transform & localTransform,
int id = 0) :
_image(image),
_id(id),
_featureType(Feature2D::kFeatureUndef),
_depth(depth),
_depth2d(depth2d),
_fx(fx),
_fy(fy),
_cx(cx),
_cy(cy),
_pose(pose),
_localTransform(localTransform)
{
}
int id = 0);
virtual ~Image() {}
@@ -117,8 +80,9 @@ public:
const cv::Mat & descriptors() const {return _descriptors;}
Feature2D::Type featureType() const {return _featureType;}
const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
const std::vector<cv::Point3f> & keypoints3() const {return _keypoints3;}
void setDescriptors(const cv::Mat & descriptors, Feature2D::Type featureType) {_descriptors = descriptors; _featureType=featureType;}
void setKeypoints(const std::vector<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}
void setKeypoints(const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> * keypoints3 = 0);
bool isMetric() const {return !_depth.empty() || _fx != 0.0f || _fy != 0.0f || !_pose.isNull();}
void setPose(const Transform & pose) {_pose = pose;}
@@ -137,6 +101,7 @@ private:
cv::Mat _descriptors;
Feature2D::Type _featureType;
std::vector<cv::KeyPoint> _keypoints;
std::vector<cv::Point3f> _keypoints3;
// Metric stuff
cv::Mat _depth;

1
corelib/src/CMakeLists.txt
View File

@@ -26,6 +26,7 @@ SET(SRC_FILES
util3d.cpp
Odometry.cpp
Image.cpp
toro3d/posegraph3.cpp
toro3d/treeoptimizer3_iteration.cpp

113
corelib/src/Features2d.cpp
View File

@@ -109,6 +109,66 @@ void filterKeypointsByDepth(
}
}
void filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
std::vector<cv::Point3f> & keypoints3,
float maxDepth)
{
cv::Mat descriptors;
filterKeypointsByDepth(keypoints, keypoints3, descriptors, maxDepth);
}
void filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
std::vector<cv::Point3f> & keypoints3,
cv::Mat & descriptors,
float maxDepth)
{
UASSERT(keypoints.size() == keypoints3.size());
UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
if(keypoints.size())
{
std::vector<cv::KeyPoint> output(keypoints.size());
std::vector<cv::Point3f> output3(keypoints3.size());
std::vector<int> indexes(keypoints.size(), 0);
int oi=0;
for(unsigned int i=0; i<keypoints.size(); ++i)
{
if(uIsFinite(keypoints3[i].z) && keypoints3[i].z < maxDepth)
{
output3[oi] = keypoints3[i];
output[oi++] = keypoints[i];
indexes[i] = 1;
}
}
output.resize(oi);
output3.resize(oi);
keypoints = output;
keypoints3 = output3;
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)
{
memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
}
}
descriptors = newDescriptors;
}
}
}
}
void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
{
cv::Mat descriptors;
@@ -159,6 +219,59 @@ void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors
}
}
void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> keypoints3, int maxKeypoints)
{
cv::Mat descriptors;
limitKeypoints(keypoints, keypoints3, descriptors, maxKeypoints);
}
void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> keypoints3, cv::Mat & descriptors, int maxKeypoints)
{
UASSERT(((int)keypoints.size() == descriptors.rows && keypoints.size() == keypoints3.size()) || descriptors.rows == 0);
if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
{
UTimer timer;
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
// Remove words under the new hessian threshold
// Sort words by hessian
std::multimap<float, int> hessianMap; // <hessian,id>
for(unsigned int i = 0; i <keypoints.size(); ++i)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
}
// Remove them from the signature
int removed = hessianMap.size()-maxKeypoints;
std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
std::vector<cv::Point3f> kpts3Tmp(maxKeypoints);
cv::Mat descriptorsTmp;
if(descriptors.rows)
{
descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
}
for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
{
kptsTmp[k] = keypoints[iter->second];
kpts3Tmp[k] = keypoints3[iter->second];
if(descriptors.rows)
{
memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
keypoints = kptsTmp;
keypoints3 = kpts3Tmp;
if(descriptors.rows)
{
descriptors = descriptorsTmp;
}
}
}
cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
{
if(!image.empty() && roiRatios.size() == 4)

118
corelib/src/Image.cpp Normal file
View File

@@ -0,0 +1,118 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
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 the Universite de Sherbrooke 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 HOLDER 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.
*/
#include "rtabmap/core/Image.h"
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap
{
/**
* An id is automatically generated if id=0.
*/
Image::Image(const cv::Mat & image,
int id,
const cv::Mat & descriptors,
Feature2D::Type featureType,
const std::vector<cv::KeyPoint> & keypoints) :
_image(image),
_id(id),
_descriptors(descriptors),
_featureType(featureType),
_keypoints(keypoints),
_fx(0.0f),
_fy(0.0f),
_cx(0.0f),
_cy(0.0f),
_localTransform(Transform::getIdentity())
{
}
// Metric constructor
Image::Image(const cv::Mat & image,
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
const Transform & pose,
const Transform & localTransform,
int id) :
_image(image),
_id(id),
_featureType(Feature2D::kFeatureUndef),
_depth(depth),
_fx(fx),
_fy(fy),
_cx(cx),
_cy(cy),
_pose(pose),
_localTransform(localTransform)
{
}
// Metric constructor + 2d depth
Image::Image(const cv::Mat & image,
const cv::Mat & depth,
const cv::Mat & depth2d,
float fx,
float fy,
float cx,
float cy,
const Transform & pose,
const Transform & localTransform,
int id) :
_image(image),
_id(id),
_featureType(Feature2D::kFeatureUndef),
_depth(depth),
_depth2d(depth2d),
_fx(fx),
_fy(fy),
_cx(cx),
_cy(cy),
_pose(pose),
_localTransform(localTransform)
{
}
void Image::setKeypoints(
const std::vector<cv::KeyPoint> & keypoints,
const std::vector<cv::Point3f> * keypoints3)
{
_keypoints = keypoints;
if(keypoints3 && keypoints3->size())
{
UASSERT(_keypoints.size() == keypoints3->size());
_keypoints3 = *keypoints3;
}
}
} // namespace rtabmap

38
corelib/src/Memory.cpp
View File

@@ -2906,6 +2906,7 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
UTimer timer;
timer.start();
std::vector<cv::KeyPoint> keypoints;
std::vector<cv::Point3f> keypoints3;
cv::Mat descriptors;
int id = image.id();
if(_generateIds)
@@ -2958,9 +2959,18 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
UASSERT(image.descriptors().type() == CV_32F || image.descriptors().type() == CV_8U);
descriptors = image.descriptors();
keypoints = image.keypoints();
keypoints3 = image.keypoints3();
}
if(keypoints3.size())
{
filterKeypointsByDepth(keypoints, keypoints3, descriptors, _wordsMaxDepth);
limitKeypoints(keypoints, keypoints3, descriptors, _wordsPerImageTarget);
}
else
{
filterKeypointsByDepth(keypoints, descriptors, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), _wordsMaxDepth);
limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
}
filterKeypointsByDepth(keypoints, descriptors, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), _wordsMaxDepth);
limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
}
else
{
@@ -3011,30 +3021,24 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
}
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, pcl::PointXYZ> words3;
if(wordIds.size() > 0)
{
std::vector<cv::KeyPoint>::iterator kpIter = keypoints.begin();
for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
UASSERT(wordIds.size() == keypoints.size());
unsigned int i=0;
for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
{
if(kpIter != keypoints.end())
words.insert(std::pair<int, cv::KeyPoint>(*iter, keypoints[i]));
if(i < keypoints3.size())
{
words.insert(std::pair<int, cv::KeyPoint >(*iter, *kpIter));
++kpIter;
}
else
{
if(keypoints.size())
{
UWARN("Words (%d) and keypoints(%d) are not the same size ?!?", (int)wordIds.size(), (int)keypoints.size());
}
words.insert(std::pair<int, cv::KeyPoint >(*iter, cv::KeyPoint()));
words3.insert(std::pair<int, pcl::PointXYZ>(*iter, pcl::PointXYZ(keypoints3[i].x, keypoints3[i].y, keypoints3[i].z)));
}
}
}
UASSERT(keypoints3.size() == 0 || words3.size() == words.size());
//3d words
std::multimap<int, pcl::PointXYZ> words3;
if(!image.depth().empty() && image.depthFx() && image.depthFy())
if(words3.size() == 0 && !image.depth().empty() && image.depthFx() && image.depthFy())
{
words3 = util3d::generateWords3(words, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), image.localTransform());
}