openni_depth_image.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011 Willow Garage, Inc.
00005  *    Suat Gedikli <gedikli@willowgarage.com>
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  */
00037 #include <openni_camera/openni_depth_image.h>
00038 #include <sstream>
00039 #include <limits>
00040 #include <iostream>
00041 
00042 using namespace std;
00043 
00044 namespace openni_wrapper
00045 {
00046 
00047 void DepthImage::fillDepthImageRaw(unsigned width, unsigned height, unsigned short* depth_buffer, unsigned line_step) const throw (OpenNIException)
00048 {
00049   if (width > depth_md_->XRes () || height > depth_md_->YRes ())
00050     THROW_OPENNI_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
00051 
00052   if (depth_md_->XRes () % width != 0 || depth_md_->YRes () % height != 0)
00053     THROW_OPENNI_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
00054 
00055   if (line_step == 0)
00056     line_step = width * sizeof (unsigned short);
00057 
00058   // special case no sclaing, no padding => memcopy!
00059   if (width == depth_md_->XRes () && height == depth_md_->YRes () && (line_step == width * sizeof (unsigned short)))
00060   {
00061     memcpy (depth_buffer, depth_md_->WritableData(), depth_md_->DataSize ());
00062     return;
00063   }
00064   
00065   // padding skip for destination image
00066   unsigned bufferSkip = line_step - width * sizeof (unsigned short);
00067 
00068   // step and padding skip for source image
00069   unsigned xStep = depth_md_->XRes () / width;
00070   unsigned ySkip = (depth_md_->YRes () / height - 1) * depth_md_->XRes ();
00071 
00072   // Fill in the depth image data, converting mm to m
00073   short bad_point = numeric_limits<short>::quiet_NaN ();
00074   unsigned depthIdx = 0;
00075 
00076   for (unsigned yIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip)
00077   {
00078     for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++depth_buffer)
00079     {
00081       if ((*depth_md_)[depthIdx] == 0 ||
00082           (*depth_md_)[depthIdx] == no_sample_value_ ||
00083           (*depth_md_)[depthIdx] == shadow_value_)
00084         *depth_buffer = bad_point;
00085       else
00086       {
00087         *depth_buffer = (unsigned short) (*depth_md_)[depthIdx];
00088       }
00089     }
00090     // if we have padding
00091     if (bufferSkip > 0)
00092     {
00093       char* cBuffer = reinterpret_cast<char*> (depth_buffer);
00094       depth_buffer = reinterpret_cast<unsigned short*> (cBuffer + bufferSkip);
00095     }
00096   }
00097 }
00098 
00099 void DepthImage::fillDepthImage (unsigned width, unsigned height, float* depth_buffer, unsigned line_step) const throw (OpenNIException)
00100 {
00101   if (width > depth_md_->XRes () || height > depth_md_->YRes ())
00102     THROW_OPENNI_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
00103 
00104   if (depth_md_->XRes () % width != 0 || depth_md_->YRes () % height != 0)
00105     THROW_OPENNI_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
00106 
00107   if (line_step == 0)
00108     line_step = width * sizeof (float);
00109 
00110   // padding skip for destination image
00111   unsigned bufferSkip = line_step - width * sizeof (float);
00112 
00113   // step and padding skip for source image
00114   unsigned xStep = depth_md_->XRes () / width;
00115   unsigned ySkip = (depth_md_->YRes () / height - 1) * depth_md_->XRes();
00116 
00117   // Fill in the depth image data, converting mm to m
00118   float bad_point = numeric_limits<float>::quiet_NaN ();
00119   unsigned depthIdx = 0;
00120 
00121   for (unsigned yIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip)
00122   {
00123     for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++depth_buffer)
00124     {
00126       if ((*depth_md_)[depthIdx] == 0 ||
00127           (*depth_md_)[depthIdx] == no_sample_value_ ||
00128           (*depth_md_)[depthIdx] == shadow_value_)
00129         *depth_buffer = bad_point;
00130       else
00131       {
00132         *depth_buffer = (float) (*depth_md_)[depthIdx] * 0.001f;
00133       }
00134     }
00135     // if we have padding
00136     if (bufferSkip > 0)
00137     {
00138       char* cBuffer = reinterpret_cast<char*> (depth_buffer);
00139       depth_buffer = reinterpret_cast<float*> (cBuffer + bufferSkip);
00140     }
00141   }
00142 }
00143 
00144 void DepthImage::fillDisparityImage (unsigned width, unsigned height, float* disparity_buffer, unsigned line_step) const throw (OpenNIException)
00145 {
00146   if (width > depth_md_->XRes () || height > depth_md_->YRes ())
00147     THROW_OPENNI_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
00148 
00149   if (depth_md_->XRes () % width != 0 || depth_md_->YRes () % height != 0)
00150     THROW_OPENNI_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
00151 
00152   if (line_step == 0)
00153     line_step = width * sizeof (float);
00154 
00155   unsigned xStep = depth_md_->XRes () / width;
00156   unsigned ySkip = (depth_md_->YRes () / height - 1) * depth_md_->XRes ();
00157 
00158   unsigned bufferSkip = line_step - width * sizeof (float);
00159 
00160   // Fill in the depth image data
00161   // iterate over all elements and fill disparity matrix: disp[x,y] = f * b / z_distance[x,y];
00162   // focal length is for the native image resolution -> focal_length = focal_length_ / xStep;
00163   float constant = focal_length_ * baseline_ * 1000.0 / (float) xStep;
00164 
00165   for (unsigned yIdx = 0, depthIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip)
00166   {
00167     for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++disparity_buffer)
00168     {
00169       if ((*depth_md_)[depthIdx] == 0 ||
00170           (*depth_md_)[depthIdx] == no_sample_value_ ||
00171           (*depth_md_)[depthIdx] == shadow_value_)
00172         *disparity_buffer = 0.0;
00173       else
00174         *disparity_buffer = constant / (double) (*depth_md_)[depthIdx];
00175     }
00176 
00177     // if we have padding
00178     if (bufferSkip > 0)
00179     {
00180       char* cBuffer = reinterpret_cast<char*> (disparity_buffer);
00181       disparity_buffer = reinterpret_cast<float*> (cBuffer + bufferSkip);
00182     }
00183   }
00184 }
00185 } // namespace


openni_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu
autogenerated on Wed Aug 26 2015 15:08:49