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

OdometryFlow: ignore maxDepth when set to 0

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1966 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-11-04 00:06:40 +00:00
parent bb22d6b065
commit 54084fc5e1
1 changed files with 6 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
corelib/src/Odometry.cpp
View File

@@ -635,8 +635,8 @@ Transform OdometryOpticalFlow::computeTransformStereo(
newDisparity, newDisparity,
data.cx(), data.cy(), data.fx(), data.baseline()); data.cx(), data.cy(), data.fx(), data.baseline());
if(pcl::isFinite(lastPt3D) && uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth()) && if(pcl::isFinite(lastPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())) &&
pcl::isFinite(newPt3D) && uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth())) pcl::isFinite(newPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth())))
{ {
//Add 3D correspondences! //Add 3D correspondences!
lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform()); lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
@@ -855,9 +855,10 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
pcl::PointXYZ pt = util3d::projectDepthTo3D(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); data.cx(), data.cy(), data.fx(), data.fy(), true);
if(pcl::isFinite(pt) && if(pcl::isFinite(pt) &&
(this->getMaxDepth() == 0.0f || (
uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) && uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) && uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.z, 0.0f, this->getMaxDepth())) uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
{ {
pt = util3d::transformPoint(pt, data.localTransform()); pt = util3d::transformPoint(pt, data.localTransform());
correspondencesLast->at(oi) = lastCorners3D_->at(i); correspondencesLast->at(oi) = lastCorners3D_->at(i);
@@ -998,9 +999,10 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
pcl::PointXYZ pt = util3d::projectDepthTo3D(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); data.cx(), data.cy(), data.fx(), data.fy(), true);
if(pcl::isFinite(pt) && if(pcl::isFinite(pt) &&
(this->getMaxDepth() == 0.0f || (
uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) && uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) && uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.z, 0.0f, this->getMaxDepth())) uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
{ {
pt = util3d::transformPoint(pt, data.localTransform()); pt = util3d::transformPoint(pt, data.localTransform());
newCorners3D->at(oi) = pt; newCorners3D->at(oi) = pt;