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

Odometry: fixed bug using max depth on the local map's points

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1619 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-28 18:34:17 +00:00
parent cd15107e12
commit 0a385ed75f
1 changed files with 7 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
corelib/src/Odometry.cpp
View File

@@ -205,12 +205,15 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
// 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( util3d::findCorrespondences(
localMap_, localMap_,
newSignature->getWords3(), newSignature->getWords3(),
*inliers1, *inliers1,
*inliers2, *inliers2,
this->getMaxDepth(), 0,
&uniqueCorrespondences); &uniqueCorrespondences);
if((int)inliers1->size() >= this->getMinInliers()) if((int)inliers1->size() >= this->getMinInliers())
@@ -266,9 +269,7 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
if(newSignature->getWords3().count(*iter) == 1) if(newSignature->getWords3().count(*iter) == 1)
{ {
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second; const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt) && if(pcl::isFinite(pt))
(pt.x != 0 || pt.y != 0 || pt.z != 0) &&
(this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
{ {
pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform); pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform);
localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt2)); localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt2));
@@ -291,9 +292,7 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
uniqueCorrespondences.find(*iter) == uniqueCorrespondences.end()) uniqueCorrespondences.find(*iter) == uniqueCorrespondences.end())
{ {
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second; const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt) && if(pcl::isFinite(pt))
(pt.x != 0 || pt.y != 0 || pt.z != 0) &&
(this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
{ {
pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform); pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform);
localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt2)); localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt2));
@@ -328,9 +327,7 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
if(newSignature->getWords3().count(*iter) == 1) if(newSignature->getWords3().count(*iter) == 1)
{ {
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second; const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt) && if(pcl::isFinite(pt))
(pt.x != 0 || pt.y != 0 || pt.z != 0) &&
(this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
{ {
localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt)); localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt));
} }