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

undistort: check nan values too

Browse Source
This commit is contained in:
matlabbe
2016-10-05 13:55:02 -04:00
parent 33501fdb0f
commit 9e7b905c0b
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
corelib/src/clams/discrete_depth_distortion_model.cpp
View File

@@ -30,6 +30,7 @@ RTAB-Map integration: Mathieu Labbe
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include "rtabmap/core/clams/frame_projector.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include "eigen_extensions/eigen_extensions.h"
using namespace std;
@@ -187,14 +188,13 @@ namespace clams
UASSERT(width_ == depth.cols);
UASSERT(height_ ==depth.rows);
UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
if(depth.type() == CV_32FC1)
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
float & z = depth.at<float>(v, u);
if(z == 0.0f)
if(uIsNan(z) || z == 0.0f)
continue;
double zf = z;
frustum(v, u).interpolatedUndistort(&zf);
@@ -208,7 +208,7 @@ namespace clams
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
unsigned short & z = depth.at<unsigned short>(v, u);
if(z == 0)
if(uIsNan(z) || z == 0)
continue;
double zf = z * 0.001;
frustum(v, u).interpolatedUndistort(&zf);