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 
00038 #include <pcl/pcl_config.h>
00039 #ifdef HAVE_OPENNI
00040 
00041 #include <pcl/io/openni_camera/openni_depth_image.h>
00042 #include <sstream>
00043 #include <limits>
00044 #include <iostream>
00045 
00046 using namespace std;
00047 
00048 namespace openni_wrapper
00049 {
00050 
00051   void
00052   DepthImage::fillDepthImageRaw (unsigned width, unsigned height, unsigned short* depth_buffer, unsigned line_step) const
00053   {
00054     if (width > depth_md_->XRes () || height > depth_md_->YRes ())
00055       THROW_OPENNI_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
00056 
00057     if (depth_md_->XRes () % width != 0 || depth_md_->YRes () % height != 0)
00058       THROW_OPENNI_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
00059 
00060     if (line_step == 0)
00061       line_step = width * static_cast<unsigned> (sizeof (unsigned short));
00062 
00063     // special case no sclaing, no padding => memcopy!
00064     if (width == depth_md_->XRes () && height == depth_md_->YRes () && (line_step == width * sizeof (unsigned short)))
00065     {
00066       memcpy (depth_buffer, depth_md_->Data (), depth_md_->DataSize ());
00067       return;
00068     }
00069 
00070     // padding skip for destination image
00071     unsigned bufferSkip = line_step - width * static_cast<unsigned> (sizeof (unsigned short));
00072 
00073     // step and padding skip for source image
00074     unsigned xStep = depth_md_->XRes () / width;
00075     unsigned ySkip = (depth_md_->YRes () / height - 1) * depth_md_->XRes ();
00076 
00077     // Fill in the depth image data, converting mm to m
00078     short bad_point = numeric_limits<short>::quiet_NaN ();
00079     unsigned depthIdx = 0;
00080 
00081     for (unsigned yIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip)
00082     {
00083       for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++depth_buffer)
00084       {
00086         if ((*depth_md_)[depthIdx] == 0 ||
00087             (*depth_md_)[depthIdx] == no_sample_value_ ||
00088             (*depth_md_)[depthIdx] == shadow_value_)
00089           *depth_buffer = bad_point;
00090         else
00091         {
00092           *depth_buffer = static_cast<unsigned short> ((*depth_md_)[depthIdx]);
00093         }
00094       }
00095       // if we have padding
00096       if (bufferSkip > 0)
00097       {
00098         char* cBuffer = reinterpret_cast<char*> (depth_buffer);
00099         depth_buffer = reinterpret_cast<unsigned short*> (cBuffer + bufferSkip);
00100       }
00101     }
00102   }
00103 
00104   void
00105   DepthImage::fillDepthImage (unsigned width, unsigned height, float* depth_buffer, unsigned line_step) const
00106   {
00107     if (width > depth_md_->XRes () || height > depth_md_->YRes ())
00108       THROW_OPENNI_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
00109 
00110     if (depth_md_->XRes () % width != 0 || depth_md_->YRes () % height != 0)
00111       THROW_OPENNI_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
00112 
00113     if (line_step == 0)
00114       line_step = width * static_cast<unsigned> (sizeof (float));
00115 
00116     // padding skip for destination image
00117     unsigned bufferSkip = line_step - width * static_cast<unsigned> (sizeof (float));
00118 
00119     // step and padding skip for source image
00120     unsigned xStep = depth_md_->XRes () / width;
00121     unsigned ySkip = (depth_md_->YRes () / height - 1) * depth_md_->XRes ();
00122 
00123     // Fill in the depth image data, converting mm to m
00124     float bad_point = numeric_limits<float>::quiet_NaN ();
00125     unsigned depthIdx = 0;
00126 
00127     for (unsigned yIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip)
00128     {
00129       for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++depth_buffer)
00130       {
00132         if ((*depth_md_)[depthIdx] == 0 ||
00133             (*depth_md_)[depthIdx] == no_sample_value_ ||
00134             (*depth_md_)[depthIdx] == shadow_value_)
00135           *depth_buffer = bad_point;
00136         else
00137         {
00138           *depth_buffer = static_cast<float> ((*depth_md_)[depthIdx]) * 0.001f;
00139         }
00140       }
00141       // if we have padding
00142       if (bufferSkip > 0)
00143       {
00144         char* cBuffer = reinterpret_cast<char*> (depth_buffer);
00145         depth_buffer = reinterpret_cast<float*> (cBuffer + bufferSkip);
00146       }
00147     }
00148   }
00149 
00150   void
00151   DepthImage::fillDisparityImage (unsigned width, unsigned height, float* disparity_buffer, unsigned line_step) const
00152   {
00153     if (width > depth_md_->XRes () || height > depth_md_->YRes ())
00154       THROW_OPENNI_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
00155 
00156     if (depth_md_->XRes () % width != 0 || depth_md_->YRes () % height != 0)
00157       THROW_OPENNI_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
00158 
00159     if (line_step == 0)
00160       line_step = width * static_cast<unsigned> (sizeof (float));
00161 
00162     unsigned xStep = depth_md_->XRes () / width;
00163     unsigned ySkip = (depth_md_->YRes () / height - 1) * depth_md_->XRes ();
00164 
00165     unsigned bufferSkip = line_step - width * static_cast<unsigned> (sizeof (float));
00166 
00167     // Fill in the depth image data
00168     // iterate over all elements and fill disparity matrix: disp[x,y] = f * b / z_distance[x,y];
00169     // focal length is for the native image resolution -> focal_length = focal_length_ / xStep;
00170     float constant = focal_length_ * baseline_ * 1000.0f / static_cast<float> (xStep);
00171 
00172     for (unsigned yIdx = 0, depthIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip)
00173     {
00174       for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++disparity_buffer)
00175       {
00176         if ((*depth_md_)[depthIdx] == 0 ||
00177             (*depth_md_)[depthIdx] == no_sample_value_ ||
00178             (*depth_md_)[depthIdx] == shadow_value_)
00179           *disparity_buffer = 0.0;
00180         else
00181           *disparity_buffer = constant / static_cast<float> ((*depth_md_)[depthIdx]);
00182       }
00183 
00184       // if we have padding
00185       if (bufferSkip > 0)
00186       {
00187         char* cBuffer = reinterpret_cast<char*> (disparity_buffer);
00188         disparity_buffer = reinterpret_cast<float*> (cBuffer + bufferSkip);
00189       }
00190     }
00191   }
00192 } // namespace
00193 #endif //HAVE_OPENNI


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:00