i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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/discrete_depth_distortion_model.h"
#include "rtabmap/core/clams/frame_projector.h" #include "rtabmap/core/clams/frame_projector.h"
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include "eigen_extensions/eigen_extensions.h" #include "eigen_extensions/eigen_extensions.h"
using namespace std; using namespace std;
@@ -187,14 +188,13 @@ namespace clams
UASSERT(width_ == depth.cols); UASSERT(width_ == depth.cols);
UASSERT(height_ ==depth.rows); UASSERT(height_ ==depth.rows);
UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1); UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
if(depth.type() == CV_32FC1) if(depth.type() == CV_32FC1)
{ {
#pragma omp parallel for #pragma omp parallel for
for(int v = 0; v < height_; ++v) { for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) { for(int u = 0; u < width_; ++u) {
float & z = depth.at<float>(v, u); float & z = depth.at<float>(v, u);
if(z == 0.0f) if(uIsNan(z) || z == 0.0f)
continue; continue;
double zf = z; double zf = z;
frustum(v, u).interpolatedUndistort(&zf); frustum(v, u).interpolatedUndistort(&zf);
@@ -208,7 +208,7 @@ namespace clams
for(int v = 0; v < height_; ++v) { for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) { for(int u = 0; u < width_; ++u) {
unsigned short & z = depth.at<unsigned short>(v, u); unsigned short & z = depth.at<unsigned short>(v, u);
if(z == 0) if(uIsNan(z) || z == 0)
continue; continue;
double zf = z * 0.001; double zf = z * 0.001;
frustum(v, u).interpolatedUndistort(&zf); frustum(v, u).interpolatedUndistort(&zf);