Merge pull request #10 from vrabaud/master

add an RGBD module

modules/rgbd/CMakeLists.txt

+set(the_description "RGBD algorithms")
+ocv_warnings_disable(CMAKE_CXX_FLAGS -Wundef)
+ocv_define_module(rgbd opencv_core opencv_calib3d opencv_contrib opencv_highgui opencv_imgproc)

modules/rgbd/doc/rgbd.rst

+**************************
+rgbd. RGB-Depth Processing
+**************************
+
+.. highlight:: cpp
+
+.. toctree::
+    :maxdepth: 2
+
+    depth_image
+    geometry
+    odometry

modules/rgbd/samples/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8)
+project(map_test)
+find_package(OpenCV REQUIRED)
+
+set(SOURCES odometry_evaluation.cpp)
+
+include_directories(${OpenCV_INCLUDE_DIRS})
+add_executable(odometry_evaluation ${SOURCES} ${HEADERS})
+target_link_libraries(odometry_evaluation ${OpenCV_LIBS})

modules/rgbd/samples/odometry_evaluation.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <opencv2/rgbd/rgbd.hpp>
+
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/contrib/contrib.hpp"
+#include "opencv2/calib3d/calib3d.hpp"
+
+#include <dirent.h>
+#include <iostream>
+#include <fstream>
+
+using namespace std;
+using namespace cv;
+
+#define BILATERAL_FILTER 0// if 1 then bilateral filter will be used for the depth
+
+static
+void writeResults( const string& filename, const vector<string>& timestamps, const vector<Mat>& Rt )
+{
+    CV_Assert( timestamps.size() == Rt.size() );
+
+    ofstream file( filename.c_str() );
+    if( !file.is_open() )
+        return;
+
+    cout.precision(4);
+    for( size_t i = 0; i < Rt.size(); i++ )
+    {
+        const Mat& Rt_curr = Rt[i];
+        if( Rt_curr.empty() )
+            continue;
+
+        CV_Assert( Rt_curr.type() == CV_64FC1 );
+
+        Mat R = Rt_curr(Rect(0,0,3,3)), rvec;
+        Rodrigues(R, rvec);
+        double alpha = norm( rvec );
+        if(alpha > DBL_MIN)
+            rvec = rvec / alpha;
+
+        double cos_alpha2 = std::cos(0.5 * alpha);
+        double sin_alpha2 = std::sin(0.5 * alpha);
+
+        rvec *= sin_alpha2;
+
+        CV_Assert( rvec.type() == CV_64FC1 );
+        // timestamp tx ty tz qx qy qz qw
+        file << timestamps[i] << " " << fixed
+             << Rt_curr.at<double>(0,3) << " " << Rt_curr.at<double>(1,3) << " " << Rt_curr.at<double>(2,3) << " "
+             << rvec.at<double>(0) << " " << rvec.at<double>(1) << " " << rvec.at<double>(2) << " " << cos_alpha2 << endl;
+
+    }
+    file.close();
+}
+
+static
+void setCameraMatrixFreiburg1(float& fx, float& fy, float& cx, float& cy)
+{
+    fx = 517.3f; fy = 516.5f; cx = 318.6f; cy = 255.3f;
+}
+
+static
+void setCameraMatrixFreiburg2(float& fx, float& fy, float& cx, float& cy)
+{
+    fx = 520.9f; fy = 521.0f; cx = 325.1f; cy = 249.7f;
+}
+
+/*
+ * This sample helps to evaluate odometry on TUM datasets and benchmark http://vision.in.tum.de/data/datasets/rgbd-dataset.
+ * At this link you can find instructions for evaluation. The sample runs some opencv odometry and saves a camera trajectory
+ * to file of format that the benchmark requires. Saved file can be used for online evaluation.
+ */
+int main(int argc, char** argv)
+{
+    if(argc != 4)
+    {
+        cout << "Format: file_with_rgb_depth_pairs trajectory_file odometry_name [Rgbd or ICP or RgbdICP]" << endl;
+        return -1;
+    }
+    
+    vector<string> timestamps;
+    vector<Mat> Rts;
+
+    const string filename = argv[1];
+    ifstream file( filename.c_str() );
+    if( !file.is_open() )
+        return -1;
+
+    char dlmrt = '/';
+    size_t pos = filename.rfind(dlmrt);
+    string dirname = pos == string::npos ? "" : filename.substr(0, pos) + dlmrt;
+
+    const int timestampLength = 17;
+    const int rgbPathLehgth = 17+8;
+    const int depthPathLehgth = 17+10;
+
+    float fx = 525.0f, // default
+          fy = 525.0f,
+          cx = 319.5f,
+          cy = 239.5f;
+    if(filename.find("freiburg1") != string::npos)
+        setCameraMatrixFreiburg1(fx, fy, cx, cy);
+    if(filename.find("freiburg2") != string::npos)
+        setCameraMatrixFreiburg2(fx, fy, cx, cy);
+    Mat cameraMatrix = Mat::eye(3,3,CV_32FC1);
+    {
+        cameraMatrix.at<float>(0,0) = fx;
+        cameraMatrix.at<float>(1,1) = fy;
+        cameraMatrix.at<float>(0,2) = cx;
+        cameraMatrix.at<float>(1,2) = cy;
+    }
+
+    Ptr<OdometryFrame> frame_prev = new OdometryFrame(),
+                           frame_curr = new OdometryFrame();
+    Ptr<Odometry> odometry = Algorithm::create<Odometry>("RGBD." + string(argv[3]) + "Odometry");
+    if(odometry.empty())
+    {
+        cout << "Can not create Odometry algorithm. Check the passed odometry name." << endl;
+        return -1;
+    }
+    odometry->set("cameraMatrix", cameraMatrix);
+
+    TickMeter gtm;
+    int count = 0;
+    for(int i = 0; !file.eof(); i++)
+    {
+        string str;
+        std::getline(file, str);
+        if(str.empty()) break;
+        if(str.at(0) == '#') continue; /* comment */
+
+        Mat image, depth;
+        // Read one pair (rgb and depth)
+        // example: 1305031453.359684 rgb/1305031453.359684.png 1305031453.374112 depth/1305031453.374112.png
+#if BILATERAL_FILTER
+        TickMeter tm_bilateral_filter;
+#endif
+        {
+            string rgbFilename = str.substr(timestampLength + 1, rgbPathLehgth );
+            string timestap = str.substr(0, timestampLength);
+            string depthFilename = str.substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
+
+            image = imread(dirname + rgbFilename);
+            depth = imread(dirname + depthFilename, -1);
+
+            CV_Assert(!image.empty());
+            CV_Assert(!depth.empty());
+            CV_Assert(depth.type() == CV_16UC1);
+
+            cout << i << " " << rgbFilename << " " << depthFilename << endl;
+
+            // scale depth
+            Mat depth_flt;
+            depth.convertTo(depth_flt, CV_32FC1, 1.f/5000.f);
+#if not BILATERAL_FILTER
+            depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), depth == 0);
+            depth = depth_flt;
+#else
+            tm_bilateral_filter.start();
+            depth = Mat(depth_flt.size(), CV_32FC1, Scalar(0));
+            const double depth_sigma = 0.03;
+            const double space_sigma = 4.5;  // in pixels
+            Mat invalidDepthMask = depth_flt == 0.f;
+            depth_flt.setTo(-5*depth_sigma, invalidDepthMask);
+            bilateralFilter(depth_flt, depth, -1, depth_sigma, space_sigma);
+            depth.setTo(std::numeric_limits<float>::quiet_NaN(), invalidDepthMask);
+            tm_bilateral_filter.stop();
+            cout << "Time filter " << tm_bilateral_filter.getTimeSec() << endl;
+#endif
+            timestamps.push_back( timestap );
+        }
+
+        {
+            Mat gray;
+            cvtColor(image, gray, CV_BGR2GRAY);
+            frame_curr->image = gray;
+            frame_curr->depth = depth;
+            
+            Mat Rt;
+            if(!Rts.empty())
+            {
+                TickMeter tm;
+                tm.start();
+                gtm.start();
+                bool res = odometry->compute(frame_curr, frame_prev, Rt);
+                gtm.stop();
+                tm.stop();
+                count++;
+                cout << "Time " << tm.getTimeSec() << endl;
+#if BILATERAL_FILTER
+                cout << "Time ratio " << tm_bilateral_filter.getTimeSec() / tm.getTimeSec() << endl;
+#endif
+                if(!res)
+                    Rt = Mat::eye(4,4,CV_64FC1);
+            }
+
+            if( Rts.empty() )
+                Rts.push_back(Mat::eye(4,4,CV_64FC1));
+            else
+            {
+                Mat& prevRt = *Rts.rbegin();
+                cout << "Rt " << Rt << endl;
+                Rts.push_back( prevRt * Rt );
+            }
+
+            if(!frame_prev.empty())
+                frame_prev->release();
+            std::swap(frame_prev, frame_curr);
+        }
+    }
+
+    std::cout << "Average time " << gtm.getTimeSec()/count << std::endl;
+    writeResults(argv[2], timestamps, Rts);
+
+    return 0;
+}

modules/rgbd/src/depth_to_3d.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <opencv2/rgbd.hpp>
+#include <limits>
+
+#include "depth_to_3d.h"
+#include "utils.h"
+
+namespace
+{
+  /**
+   * @param K
+   * @param depth the depth image
+   * @param mask the mask of the points to consider (can be empty)
+   * @param points3d the resulting 3d points, a 3-channel matrix
+   */
+  void
+  depthTo3d_from_uvz(const cv::Mat& in_K, const cv::Mat& u_mat, const cv::Mat& v_mat, const cv::Mat& z_mat,
+                     cv::Mat& points3d)
+  {
+    CV_Assert((u_mat.size() == z_mat.size()) && (v_mat.size() == z_mat.size()));
+    if (u_mat.empty())
+      return;
+    CV_Assert((u_mat.type() == z_mat.type()) && (v_mat.type() == z_mat.type()));
+
+    //grab camera params
+    cv::Mat_<float> K;
+
+    if (in_K.depth() == CV_32F)
+      K = in_K;
+    else
+      in_K.convertTo(K, CV_32F);
+
+    float fx = K(0, 0);
+    float fy = K(1, 1);
+    float s = K(0, 1);
+    float cx = K(0, 2);
+    float cy = K(1, 2);
+
+    std::vector<cv::Mat> coordinates(3);
+
+    coordinates[0] = (u_mat - cx) / fx;
+
+    if (s != 0)
+      coordinates[0] = coordinates[0] + (-(s / fy) * v_mat + cy * s / fy) / fx;
+
+    coordinates[0] = coordinates[0].mul(z_mat);
+    coordinates[1] = (v_mat - cy).mul(z_mat) * (1. / fy);
+    coordinates[2] = z_mat;
+    cv::merge(coordinates, points3d);
+  }
+
+  /**
+   * @param K
+   * @param depth the depth image
+   * @param mask the mask of the points to consider (can be empty)
+   * @param points3d the resulting 3d points
+   */
+  void
+  depthTo3dMask(const cv::Mat& depth, const cv::Mat& K, const cv::Mat& mask, cv::Mat& points3d)
+  {
+    // Create 3D points in one go.
+    cv::Mat_<float> u_mat, v_mat, z_mat;
+
+    cv::Mat_<uchar> uchar_mask = mask;
+
+    if (mask.depth() != (CV_8U))
+      mask.convertTo(uchar_mask, CV_8U);
+
+    // Figure out the interesting indices
+    size_t n_points;
+
+    if (depth.depth() == CV_16U)
+      n_points = convertDepthToFloat<uint16_t>(depth, mask, 1.0 / 1000.0f, u_mat, v_mat, z_mat);
+    else if (depth.depth() == CV_16S)
+      n_points = convertDepthToFloat<int16_t>(depth, mask, 1.0 / 1000.0f, u_mat, v_mat, z_mat);
+    else
+    {
+      CV_Assert(depth.type() == CV_32F);
+      n_points = convertDepthToFloat<float>(depth, mask, 1.0f, u_mat, v_mat, z_mat);
+    }
+
+    if (n_points == 0)
+      return;
+
+    u_mat.resize(n_points);
+    v_mat.resize(n_points);
+    z_mat.resize(n_points);
+
+    depthTo3d_from_uvz(K, u_mat, v_mat, z_mat, points3d);
+    points3d = points3d.reshape(3, 1);
+  }
+
+  /**
+   * @param K
+   * @param depth the depth image
+   * @param points3d the resulting 3d points
+   */
+  template<typename T>
+  void
+  depthTo3dNoMask(const cv::Mat& in_depth, const cv::Mat_<T>& K, cv::Mat& points3d)
+  {
+    const T inv_fx = T(1) / K(0, 0);
+    const T inv_fy = T(1) / K(1, 1);
+    const T ox = K(0, 2);
+    const T oy = K(1, 2);
+
+    // Build z
+    cv::Mat_<T> z_mat;
+    if (z_mat.depth() == in_depth.depth())
+      z_mat = in_depth;
+    else
+      rescaleDepthTemplated<T>(in_depth, z_mat);
+
+    // Pre-compute some constants
+    cv::Mat_<T> x_cache(1, in_depth.cols), y_cache(in_depth.rows, 1);
+    T* x_cache_ptr = x_cache[0], *y_cache_ptr = y_cache[0];
+    for (int x = 0; x < in_depth.cols; ++x, ++x_cache_ptr)
+      *x_cache_ptr = (x - ox) * inv_fx;
+    for (int y = 0; y < in_depth.rows; ++y, ++y_cache_ptr)
+      *y_cache_ptr = (y - oy) * inv_fy;
+
+    y_cache_ptr = y_cache[0];
+    for (int y = 0; y < in_depth.rows; ++y, ++y_cache_ptr)
+    {
+      cv::Vec<T, 3>* point = points3d.ptr<cv::Vec<T, 3> >(y);
+      const T* x_cache_ptr_end = x_cache[0] + in_depth.cols;
+      const T* depth = z_mat[y];
+      for (x_cache_ptr = x_cache[0]; x_cache_ptr != x_cache_ptr_end; ++x_cache_ptr, ++point, ++depth)
+      {
+        T z = *depth;
+        (*point)[0] = (*x_cache_ptr) * z;
+        (*point)[1] = (*y_cache_ptr) * z;
+        (*point)[2] = z;
+      }
+    }
+  }
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+namespace cv
+{
+
+  /**
+   * @param K
+   * @param depth the depth image
+   * @param u_mat the list of x coordinates
+   * @param v_mat the list of matching y coordinates
+   * @param points3d the resulting 3d points
+   */
+  void
+  depthTo3dSparse(InputArray depth_in, InputArray K_in, InputArray points_in, OutputArray points3d_out)
+  {
+    // Make sure we use foat types
+    cv::Mat points = points_in.getMat();
+    cv::Mat depth = depth_in.getMat();
+
+    cv::Mat points_float;
+    if (points.depth() != CV_32F)
+      points.convertTo(points_float, CV_32FC2);
+    else
+      points_float = points;
+
+    // Fill the depth matrix
+    cv::Mat_<float> z_mat;
+
+    if (depth.depth() == CV_16U)
+      convertDepthToFloat<uint16_t>(depth, 1.0 / 1000.0f, points_float, z_mat);
+    else if (depth.depth() == CV_16U)
+      convertDepthToFloat<int16_t>(depth, 1.0 / 1000.0f, points_float, z_mat);
+    else
+    {
+      CV_Assert(depth.type() == CV_32F);
+      convertDepthToFloat<float>(depth, 1.0f, points_float, z_mat);
+    }
+
+    std::vector<cv::Mat> channels(2);
+    cv::split(points_float, channels);
+
+    points3d_out.create(channels[0].rows, channels[0].cols, CV_32FC3);
+    cv::Mat points3d = points3d_out.getMat();
+    depthTo3d_from_uvz(K_in.getMat(), channels[0], channels[1], z_mat, points3d);
+  }
+
+  /**
+   * @param depth the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
+   *              (as done with the Microsoft Kinect), otherwise, if given as CV_32F, it is assumed in meters)
+   * @param K The calibration matrix
+   * @param points3d the resulting 3d points. They are of depth the same as `depth` if it is CV_32F or CV_64F, and the
+   *        depth of `K` if `depth` is of depth CV_U
+   * @param mask the mask of the points to consider (can be empty)
+   */
+  void
+  depthTo3d(InputArray depth_in, InputArray K_in, OutputArray points3d_out, InputArray mask_in)
+  {
+    cv::Mat depth = depth_in.getMat();
+    cv::Mat K = K_in.getMat();
+    cv::Mat mask = mask_in.getMat();
+    CV_Assert(K.cols == 3 && K.rows == 3 && (K.depth() == CV_64F || K.depth()==CV_32F));
+    CV_Assert(
+        depth.type() == CV_64FC1 || depth.type() == CV_32FC1 || depth.type() == CV_16UC1 || depth.type() == CV_16SC1);
+    CV_Assert(mask.empty() || mask.channels() == 1);
+
+    // TODO figure out what to do when types are different: convert or reject ?
+    cv::Mat K_new;
+    if ((depth.depth() == CV_32F || depth.depth() == CV_64F) && depth.depth() != K.depth())
+    {
+      K.convertTo(K_new, depth.depth());
+    }
+    else
+      K_new = K;
+
+    // Create 3D points in one go.
+    if (!mask.empty())
+    {
+      cv::Mat points3d;
+      depthTo3dMask(depth, K_new, mask, points3d);
+      points3d_out.create(points3d.size(), CV_MAKETYPE(K_new.depth(), 3));
+      points3d.copyTo(points3d_out.getMat());
+    }
+    else
+    {
+      points3d_out.create(depth.size(), CV_MAKETYPE(K_new.depth(), 3));
+      cv::Mat points3d = points3d_out.getMat();
+      if (K_new.depth() == CV_64F)
+        depthTo3dNoMask<double>(depth, K_new, points3d);
+      else
+        depthTo3dNoMask<float>(depth, K_new, points3d);
+    }
+  }
+}

modules/rgbd/src/depth_to_3d.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef __OPENCV_RGBD_DEPTH_TO_3D_HPP__
+#define __OPENCV_RGBD_DEPTH_TO_3D_HPP__
+
+#ifdef __cplusplus
+
+#include <opencv2/core.hpp>
+#include <limits>
+
+/**
+ * @param depth the depth image, containing depth with the value T
+ * @param the mask, containing CV_8UC1
+ */
+template <typename T>
+size_t
+convertDepthToFloat(const cv::Mat& depth, const cv::Mat& mask, float scale, cv::Mat_<float> &u_mat, cv::Mat_<float> &v_mat, cv::Mat_<float> &z_mat)
+{
+  CV_Assert (depth.size == mask.size);
+
+  cv::Size depth_size = depth.size();
+
+  cv::Mat_<uchar> uchar_mask = mask;
+
+  if (mask.depth() != CV_8U)
+    mask.convertTo(uchar_mask, CV_8U);
+
+  u_mat = cv::Mat_<float>(depth_size.area(), 1);
+  v_mat = cv::Mat_<float>(depth_size.area(), 1);
+  z_mat = cv::Mat_<float>(depth_size.area(), 1);
+
+  // Raw data from the Kinect has int
+  size_t n_points = 0;
+
+  for (int v = 0; v < depth_size.height; v++)
+  {
+    uchar* r = uchar_mask.ptr<uchar>(v, 0);
+
+    for (int u = 0; u < depth_size.width; u++, ++r)
+      if (*r)
+      {
+        u_mat(n_points, 0) = u;
+        v_mat(n_points, 0) = v;
+        T depth_i = depth.at<T>(v, u);
+
+        if (cvIsNaN(depth_i) || (depth_i == std::numeric_limits<T>::min()) || (depth_i == std::numeric_limits<T>::max()))
+          z_mat(n_points, 0) = std::numeric_limits<float>::quiet_NaN();
+        else
+          z_mat(n_points, 0) = depth_i * scale;
+
+        ++n_points;
+      }
+  }
+
+  return n_points;
+}
+
+/**
+ * @param depth the depth image, containing depth with the value T
+ * @param the mask, containing CV_8UC1
+ */
+template <typename T>
+void
+convertDepthToFloat(const cv::Mat& depth, float scale, const cv::Mat &uv_mat, cv::Mat_<float> &z_mat)
+{
+  z_mat = cv::Mat_<float>(uv_mat.size());
+
+  // Raw data from the Kinect has int
+  float* z_mat_iter = reinterpret_cast<float*>(z_mat.data);
+
+  for (cv::Mat_<cv::Vec2f>::const_iterator uv_iter = uv_mat.begin<cv::Vec2f>(), uv_end = uv_mat.end<cv::Vec2f>();
+      uv_iter != uv_end; ++uv_iter, ++z_mat_iter)
+  {
+    T depth_i = depth.at < T > ((*uv_iter)[1], (*uv_iter)[0]);
+
+    if (cvIsNaN(depth_i) || (depth_i == std::numeric_limits < T > ::min())
+        || (depth_i == std::numeric_limits < T > ::max()))
+      *z_mat_iter = std::numeric_limits<float>::quiet_NaN();
+    else
+      *z_mat_iter = depth_i * scale;
+  }
+}
+
+#endif /* __cplusplus */
+
+#endif
+
+/* End of file. */

modules/rgbd/src/rgbd_init.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <opencv2/core.hpp>
+#include <opencv2/rgbd.hpp>
+#include "opencv2/core/utility.hpp"
+#include "opencv2/core/private.hpp"
+
+namespace cv
+{
+  CV_INIT_ALGORITHM(DepthCleaner, "RGBD.DepthCleaner",
+      obj.info()->addParam(obj, "window_size", obj.window_size_);
+      obj.info()->addParam(obj, "depth", obj.depth_);
+      obj.info()->addParam(obj, "method", obj.method_))
+
+  CV_INIT_ALGORITHM(RgbdNormals, "RGBD.RgbdNormals",
+      obj.info()->addParam(obj, "rows", obj.rows_);
+      obj.info()->addParam(obj, "cols", obj.cols_);
+      obj.info()->addParam(obj, "window_size", obj.window_size_);
+      obj.info()->addParam(obj, "depth", obj.depth_);
+      obj.info()->addParam(obj, "K", obj.K_);
+      obj.info()->addParam(obj, "method", obj.method_))
+
+  CV_INIT_ALGORITHM(RgbdPlane, "RGBD.RgbdPlane",
+      obj.info()->addParam(obj, "block_size", obj.block_size_);
+      obj.info()->addParam(obj, "min_size", obj.min_size_);
+      obj.info()->addParam(obj, "method", obj.method_);
+      obj.info()->addParam(obj, "threshold", obj.threshold_);
+      obj.info()->addParam(obj, "sensor_error_a", obj.sensor_error_a_);
+      obj.info()->addParam(obj, "sensor_error_b", obj.sensor_error_b_);
+      obj.info()->addParam(obj, "sensor_error_c", obj.sensor_error_c_))
+
+  CV_INIT_ALGORITHM(RgbdOdometry, "RGBD.RgbdOdometry",
+      obj.info()->addParam(obj, "cameraMatrix", obj.cameraMatrix);
+      obj.info()->addParam(obj, "minDepth", obj.minDepth);
+      obj.info()->addParam(obj, "maxDepth", obj.maxDepth);
+      obj.info()->addParam(obj, "maxDepthDiff", obj.maxDepthDiff);
+      obj.info()->addParam(obj, "iterCounts", obj.iterCounts);
+      obj.info()->addParam(obj, "minGradientMagnitudes", obj.minGradientMagnitudes);
+      obj.info()->addParam(obj, "maxPointsPart", obj.maxPointsPart);
+      obj.info()->addParam(obj, "transformType", obj.transformType);
+      obj.info()->addParam(obj, "maxTranslation", obj.maxTranslation);
+      obj.info()->addParam(obj, "maxRotation", obj.maxRotation);)
+
+  CV_INIT_ALGORITHM(ICPOdometry, "RGBD.ICPOdometry",
+      obj.info()->addParam(obj, "cameraMatrix", obj.cameraMatrix);
+      obj.info()->addParam(obj, "minDepth", obj.minDepth);
+      obj.info()->addParam(obj, "maxDepth", obj.maxDepth);
+      obj.info()->addParam(obj, "maxDepthDiff", obj.maxDepthDiff);
+      obj.info()->addParam(obj, "maxPointsPart", obj.maxPointsPart);
+      obj.info()->addParam(obj, "iterCounts", obj.iterCounts);
+      obj.info()->addParam(obj, "transformType", obj.transformType);
+      obj.info()->addParam(obj, "maxTranslation", obj.maxTranslation);
+      obj.info()->addParam(obj, "maxRotation", obj.maxRotation);
+      obj.info()->addParam(obj, "normalsComputer", obj.normalsComputer, true);)
+
+  CV_INIT_ALGORITHM(RgbdICPOdometry, "RGBD.RgbdICPOdometry",
+      obj.info()->addParam(obj, "cameraMatrix", obj.cameraMatrix);
+      obj.info()->addParam(obj, "minDepth", obj.minDepth);
+      obj.info()->addParam(obj, "maxDepth", obj.maxDepth);
+      obj.info()->addParam(obj, "maxDepthDiff", obj.maxDepthDiff);
+      obj.info()->addParam(obj, "maxPointsPart", obj.maxPointsPart);
+      obj.info()->addParam(obj, "iterCounts", obj.iterCounts);
+      obj.info()->addParam(obj, "minGradientMagnitudes", obj.minGradientMagnitudes);
+      obj.info()->addParam(obj, "transformType", obj.transformType);
+      obj.info()->addParam(obj, "maxTranslation", obj.maxTranslation);
+      obj.info()->addParam(obj, "maxRotation", obj.maxRotation);
+      obj.info()->addParam(obj, "normalsComputer", obj.normalsComputer, true);)
+
+  bool
+  initModule_rgbd(void);
+  bool
+  initModule_rgbd(void)
+  {
+    bool all = true;
+    all &= !RgbdNormals_info_auto.name().empty();
+    all &= !RgbdPlane_info_auto.name().empty();
+    all &= !RgbdOdometry_info_auto.name().empty();
+    all &= !ICPOdometry_info_auto.name().empty();
+    all &= !RgbdICPOdometry_info_auto.name().empty();
+    return all;
+  }
+}

modules/rgbd/src/utils.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <opencv2/rgbd.hpp>
+#include <limits>
+
+#include "utils.h"
+
+namespace cv
+{
+  /** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided
+   * by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits<float>::quiet_NaN()
+   * Otherwise, the image is simply converted to floats
+   * @param in_in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
+   *              (as done with the Microsoft Kinect), it is assumed in meters)
+   * @param depth the desired output depth (floats or double)
+   * @param out_out The rescaled float depth image
+   */
+  void
+  rescaleDepth(InputArray in_in, int depth, OutputArray out_out)
+  {
+    cv::Mat in = in_in.getMat();
+    CV_Assert(in.type() == CV_64FC1 || in.type() == CV_32FC1 || in.type() == CV_16UC1 || in.type() == CV_16SC1);
+    CV_Assert(depth == CV_64FC1 || depth == CV_32FC1);
+
+    int in_depth = in.depth();
+
+    out_out.create(in.size(), depth);
+    cv::Mat out = out_out.getMat();
+    if (in_depth == CV_16U)
+    {
+      in.convertTo(out, depth, 1 / 1000.0); //convert to float so that it is in meters
+      cv::Mat valid_mask = in == std::numeric_limits<uint16_t>::min(); // Should we do std::numeric_limits<uint16_t>::max() too ?
+      out.setTo(std::numeric_limits<float>::quiet_NaN(), valid_mask); //set a$
+    }
+    if (in_depth == CV_16S)
+    {
+      in.convertTo(out, depth, 1 / 1000.0); //convert to float so tha$
+      cv::Mat valid_mask = (in == std::numeric_limits<int16_t>::min()) | (in == std::numeric_limits<int16_t>::max()); // Should we do std::numeric_limits<uint16_t>::max() too ?
+      out.setTo(std::numeric_limits<float>::quiet_NaN(), valid_mask); //set a$
+    }
+    if ((in_depth == CV_32F) || (in_depth == CV_64F))
+      in.convertTo(out, depth);
+  }
+}

modules/rgbd/src/utils.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef __OPENCV_RGBD_UTILS_HPP__
+#define __OPENCV_RGBD_UTILS_HPP__
+
+#ifdef __cplusplus
+
+#include <opencv2/rgbd.hpp>
+
+/** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided
+ * by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits<float>::quiet_NaN()
+ * Otherwise, the image is simply converted to floats
+ * @param in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
+ *              (as done with the Microsoft Kinect), it is assumed in meters)
+ * @param the desired output depth (floats or double)
+ * @param out The rescaled float depth image
+ */
+template<typename T>
+void
+rescaleDepthTemplated(const cv::Mat& in, cv::Mat& out);
+
+template<>
+inline void
+rescaleDepthTemplated<float>(const cv::Mat& in, cv::Mat& out)
+{
+  rescaleDepth(in, CV_32F, out);
+}
+
+template<>
+inline void
+rescaleDepthTemplated<double>(const cv::Mat& in, cv::Mat& out)
+{
+  rescaleDepth(in, CV_64F, out);
+}
+
+#endif /* __cplusplus */
+
+#endif
+
+/* End of file. */

modules/rgbd/test/test_main.cpp

+#include "test_precomp.hpp"
+
+CV_TEST_MAIN("rgbd")

modules/rgbd/test/test_precomp.cpp

+#include "test_precomp.hpp"

modules/rgbd/test/test_precomp.hpp

+#ifndef __OPENCV_TEST_PRECOMP_HPP__
+#define __OPENCV_TEST_PRECOMP_HPP__
+
+#include <opencv2/ts.hpp>
+#include <opencv2/rgbd.hpp>
+#include <iostream>
+
+#endif

modules/rgbd/test/test_utils.cpp

+#include <opencv2/calib3d.hpp>
+
+#include "test_precomp.hpp"
+
+class CV_RgbdDepthTo3dTest: public cvtest::BaseTest
+{
+public:
+  CV_RgbdDepthTo3dTest()
+  {
+  }
+  ~CV_RgbdDepthTo3dTest()
+  {
+  }
+protected:
+  void
+  run(int)
+  {
+    try
+    {
+      // K from a VGA Kinect
+      cv::Mat K = (cv::Mat_<float>(3, 3) << 525., 0., 319.5, 0., 525., 239.5, 0., 0., 1.);
+
+      // Create a random depth image
+      cv::RNG rng;
+      cv::Mat_<float> depth(480, 640);
+      rng.fill(depth, cv::RNG::UNIFORM, 0, 100);
+
+      // Create some 3d points on the plane
+      int rows = depth.rows, cols = depth.cols;
+      cv::Mat_<cv::Vec3f> points3d;
+      cv::depthTo3d(depth, K, points3d);
+
+      // Make sure the points belong to the plane
+      cv::Mat points = points3d.reshape(1, rows*cols);
+      cv::Mat image_points;
+      cv::Mat rvec;
+      cv::Rodrigues(cv::Mat::eye(3,3,CV_32F),rvec);
+      cv::Mat tvec = (cv::Mat_<float>(1,3) << 0, 0, 0);
+      cv::projectPoints(points, rvec, tvec, K, cv::Mat(), image_points);
+      image_points = image_points.reshape(2, rows);
+
+      float avg_diff = 0;
+      for (int y = 0; y < rows; ++y)
+        for (int x = 0; x < cols; ++x)
+          avg_diff += cv::norm(image_points.at<cv::Vec2f>(y,x) - cv::Vec2f(x,y));
+
+      // Verify the function works
+      ASSERT_LE(avg_diff/rows/cols, 1e-4) << "Average error for ground truth is: " << (avg_diff / rows / cols);
+    } catch (...)
+    {
+      ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
+    }
+    ts->set_failed_test_info(cvtest::TS::OK);
+  }
+};
+
+TEST(Rgbd_DepthTo3d, compute)
+{
+  CV_RgbdDepthTo3dTest test;
+  test.safe_run();
+}