depth_cleaner.cpp 10.2 KB
Newer Older
Vincent Rabaud's avatar
Vincent Rabaud committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35
/*
 * 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.
 *
 */

36
#include "precomp.hpp"
Vincent Rabaud's avatar
Vincent Rabaud committed
37 38 39

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

40 41 42
namespace cv
{
namespace rgbd
Vincent Rabaud's avatar
Vincent Rabaud committed
43 44 45 46
{
  class DepthCleanerImpl
  {
  public:
47
    DepthCleanerImpl(int window_size, int depth, DepthCleaner::DEPTH_CLEANER_METHOD method)
Vincent Rabaud's avatar
Vincent Rabaud committed
48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70
        :
          depth_(depth),
          window_size_(window_size),
          method_(method)
    {
    }

    virtual
    ~DepthCleanerImpl()
    {
    }

    virtual void
    cache()=0;

    bool
    validate(int depth, int window_size, int method) const
    {
      return (window_size == window_size_) && (depth == depth_) && (method == method_);
    }
  protected:
    int depth_;
    int window_size_;
71
    DepthCleaner::DEPTH_CLEANER_METHOD method_;
Vincent Rabaud's avatar
Vincent Rabaud committed
72 73 74 75 76 77 78 79 80 81 82 83
  };

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  /** Given a depth image, compute the normals as detailed in the LINEMOD paper
   * ``Gradient Response Maps for Real-Time Detection of Texture-Less Objects``
   * by S. Hinterstoisser, C. Cagniart, S. Ilic, P. Sturm, N. Navab, P. Fua, and V. Lepetit
   */
  template<typename T>
  class NIL: public DepthCleanerImpl
  {
  public:
84 85
    typedef Vec<T, 3> Vec3T;
    typedef Matx<T, 3, 3> Mat33T;
Vincent Rabaud's avatar
Vincent Rabaud committed
86

87
    NIL(int window_size, int depth, DepthCleaner::DEPTH_CLEANER_METHOD method)
Vincent Rabaud's avatar
Vincent Rabaud committed
88 89 90 91 92 93 94 95
        :
          DepthCleanerImpl(window_size, depth, method)
    {
    }

    /** Compute cached data
     */
    virtual void
96
    cache() CV_OVERRIDE
Vincent Rabaud's avatar
Vincent Rabaud committed
97 98 99 100 101 102 103 104
    {
    }

    /** Compute the normals
     * @param r
     * @return
     */
    void
105
    compute(const Mat& depth_in, Mat& depth_out) const
Vincent Rabaud's avatar
Vincent Rabaud committed
106 107 108 109 110
    {
      switch (depth_in.depth())
      {
        case CV_16U:
        {
111 112
          const Mat_<unsigned short> &depth(depth_in);
          Mat depth_out_tmp;
Ilya Lavrenov's avatar
Ilya Lavrenov committed
113
          computeImpl<unsigned short, float>(depth, depth_out_tmp, 0.001f);
Vincent Rabaud's avatar
Vincent Rabaud committed
114 115 116 117 118
          depth_out_tmp.convertTo(depth_out, CV_16U);
          break;
        }
        case CV_32F:
        {
119
          const Mat_<float> &depth(depth_in);
Vincent Rabaud's avatar
Vincent Rabaud committed
120 121 122 123 124
          computeImpl<float, float>(depth, depth_out, 1);
          break;
        }
        case CV_64F:
        {
125
          const Mat_<double> &depth(depth_in);
Vincent Rabaud's avatar
Vincent Rabaud committed
126 127 128 129 130 131 132 133 134 135 136 137 138
          computeImpl<double, double>(depth, depth_out, 1);
          break;
        }
      }
    }

  private:
    /** Compute the normals
     * @param r
     * @return
     */
    template<typename DepthDepth, typename ContainerDepth>
    void
139
    computeImpl(const Mat_<DepthDepth> &depth_in, Mat & depth_out, ContainerDepth scale) const
Vincent Rabaud's avatar
Vincent Rabaud committed
140
    {
Ilya Lavrenov's avatar
Ilya Lavrenov committed
141
      const ContainerDepth theta_mean = (float)(30. * CV_PI / 180);
Vincent Rabaud's avatar
Vincent Rabaud committed
142 143 144 145
      int rows = depth_in.rows;
      int cols = depth_in.cols;

      // Precompute some data
Ilya Lavrenov's avatar
Ilya Lavrenov committed
146
      const ContainerDepth sigma_L = (float)(0.8 + 0.035 * theta_mean / (CV_PI / 2 - theta_mean));
147
      Mat_<ContainerDepth> sigma_z(rows, cols);
Vincent Rabaud's avatar
Vincent Rabaud committed
148 149
      for (int y = 0; y < rows; ++y)
        for (int x = 0; x < cols; ++x)
Ilya Lavrenov's avatar
Ilya Lavrenov committed
150
          sigma_z(y, x) = (float)(0.0012 + 0.0019 * (depth_in(y, x) * scale - 0.4) * (depth_in(y, x) * scale - 0.4));
Vincent Rabaud's avatar
Vincent Rabaud committed
151 152

      ContainerDepth difference_threshold = 10;
153 154
      Mat_<ContainerDepth> Dw_sum = Mat_<ContainerDepth>::zeros(rows, cols), w_sum =
          Mat_<ContainerDepth>::zeros(rows, cols);
Vincent Rabaud's avatar
Vincent Rabaud committed
155 156 157 158 159 160 161 162 163 164 165 166 167 168
      for (int y = 0; y < rows - 1; ++y)
      {
        // Every pixel has had the contribution of previous pixels (in a row-major way)
        for (int x = 1; x < cols - 1; ++x)
        {
          for (int j = 0; j <= 1; ++j)
            for (int i = -1; i <= 1; ++i)
            {
              if ((j == 0) && (i == -1))
                continue;
              ContainerDepth delta_u = sqrt(
                  ContainerDepth(j) * ContainerDepth(j) + ContainerDepth(i) * ContainerDepth(i));
              ContainerDepth delta_z;
              if (depth_in(y, x) > depth_in(y + j, x + i))
Ilya Lavrenov's avatar
Ilya Lavrenov committed
169
                delta_z = (float)(depth_in(y, x) - depth_in(y + j, x + i));
Vincent Rabaud's avatar
Vincent Rabaud committed
170
              else
Ilya Lavrenov's avatar
Ilya Lavrenov committed
171
                delta_z = (float)(depth_in(y + j, x + i) - depth_in(y, x));
Vincent Rabaud's avatar
Vincent Rabaud committed
172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190
              if (delta_z < difference_threshold)
              {
                delta_z *= scale;
                ContainerDepth w = exp(
                    -delta_u * delta_u / 2 / sigma_L / sigma_L - delta_z * delta_z / 2 / sigma_z(y, x) / sigma_z(y, x));
                w_sum(y, x) += w;
                Dw_sum(y, x) += depth_in(y + j, x + i) * w;
                if ((j != 0) || (i != 0))
                {
                  w = exp(
                      -delta_u * delta_u / 2 / sigma_L / sigma_L - delta_z * delta_z / 2 / sigma_z(y + j, x + i)
                                                                   / sigma_z(y + j, x + i));
                  w_sum(y + j, x + i) += w;
                  Dw_sum(y + j, x + i) += depth_in(y, x) * w;
                }
              }
            }
        }
      }
191
      Mat(Dw_sum / w_sum).copyTo(depth_out);
Vincent Rabaud's avatar
Vincent Rabaud committed
192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287
    }
  };

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  /** Default constructor of the Algorithm class that computes normals
   */
  DepthCleaner::DepthCleaner(int depth, int window_size, int method_in)
      :
        depth_(depth),
        window_size_(window_size),
        method_(method_in),
        depth_cleaner_impl_(0)
  {
    CV_Assert(depth == CV_16U || depth == CV_32F || depth == CV_64F);
  }

  /** Destructor
   */
  DepthCleaner::~DepthCleaner()
  {
    if (depth_cleaner_impl_ == 0)
      return;
    switch (method_)
    {
      case DEPTH_CLEANER_NIL:
      {
        switch (depth_)
        {
          case CV_16U:
            delete reinterpret_cast<const NIL<unsigned short> *>(depth_cleaner_impl_);
            break;
          case CV_32F:
            delete reinterpret_cast<const NIL<float> *>(depth_cleaner_impl_);
            break;
          case CV_64F:
            delete reinterpret_cast<const NIL<double> *>(depth_cleaner_impl_);
            break;
        }
        break;
      }
    }
  }

  void
  DepthCleaner::initialize_cleaner_impl() const
  {
    CV_Assert(depth_ == CV_16U || depth_ == CV_32F || depth_ == CV_64F);
    CV_Assert(window_size_ == 1 || window_size_ == 3 || window_size_ == 5 || window_size_ == 7);
    CV_Assert( method_ == DEPTH_CLEANER_NIL);
    switch (method_)
    {
      case (DEPTH_CLEANER_NIL):
      {
        switch (depth_)
        {
          case CV_16U:
            depth_cleaner_impl_ = new NIL<unsigned short>(window_size_, depth_, DEPTH_CLEANER_NIL);
            break;
          case CV_32F:
            depth_cleaner_impl_ = new NIL<float>(window_size_, depth_, DEPTH_CLEANER_NIL);
            break;
          case CV_64F:
            depth_cleaner_impl_ = new NIL<double>(window_size_, depth_, DEPTH_CLEANER_NIL);
            break;
        }
        break;
      }
    }

    reinterpret_cast<DepthCleanerImpl *>(depth_cleaner_impl_)->cache();
  }

  /** Initializes some data that is cached for later computation
   * If that function is not called, it will be called the first time normals are computed
   */
  void
  DepthCleaner::initialize() const
  {
    if (depth_cleaner_impl_ == 0)
      initialize_cleaner_impl();
    else if (!reinterpret_cast<DepthCleanerImpl *>(depth_cleaner_impl_)->validate(depth_, window_size_, method_))
      initialize_cleaner_impl();
  }

  /** Given a set of 3d points in a depth image, compute the normals at each point
   * using the SRI method described in
   * ``Fast and Accurate Computation of Surface Normals from Range Images``
   * by H. Badino, D. Huber, Y. Park and T. Kanade
   * @param depth depth a float depth image. Or it can be rows x cols x 3 is they are 3d points
   * @param window_size the window size on which to compute the derivatives
   * @return normals a rows x cols x 3 matrix
   */
  void
  DepthCleaner::operator()(InputArray depth_in_array, OutputArray depth_out_array) const
  {
288
    Mat depth_in = depth_in_array.getMat();
Vincent Rabaud's avatar
Vincent Rabaud committed
289 290 291 292
    CV_Assert(depth_in.dims == 2);
    CV_Assert(depth_in.channels() == 1);

    depth_out_array.create(depth_in.size(), depth_);
293
    Mat depth_out = depth_out_array.getMat();
Vincent Rabaud's avatar
Vincent Rabaud committed
294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319

    // Initialize the pimpl
    initialize();

    // Clean the depth
    switch (method_)
    {
      case (DEPTH_CLEANER_NIL):
      {
        switch (depth_)
        {
          case CV_16U:
            reinterpret_cast<const NIL<unsigned short> *>(depth_cleaner_impl_)->compute(depth_in, depth_out);
            break;
          case CV_32F:
            reinterpret_cast<const NIL<float> *>(depth_cleaner_impl_)->compute(depth_in, depth_out);
            break;
          case CV_64F:
            reinterpret_cast<const NIL<double> *>(depth_cleaner_impl_)->compute(depth_in, depth_out);
            break;
        }
        break;
      }
    }
  }
}
320
}