i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +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 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(
localMap_,
newSignature->getWords3(),
*inliers1,
*inliers2,
this->getMaxDepth(),
0,
&uniqueCorrespondences);
if((int)inliers1->size() >= this->getMinInliers())
@@ -266,9 +269,7 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
if(newSignature->getWords3().count(*iter) == 1)
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt) &&
(pt.x != 0 || pt.y != 0 || pt.z != 0) &&
(this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
if(pcl::isFinite(pt))
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform);
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())
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt) &&
(pt.x != 0 || pt.y != 0 || pt.z != 0) &&
(this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
if(pcl::isFinite(pt))
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform);
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)
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt) &&
(pt.x != 0 || pt.y != 0 || pt.z != 0) &&
(this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
if(pcl::isFinite(pt))
{
localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt));
}