openni_depth_image.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011 Willow Garage, Inc.
5  * Suat Gedikli <gedikli@willowgarage.com>
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
38 #include <sstream>
39 #include <limits>
40 #include <iostream>
41 
42 using namespace std;
43 
44 namespace openni_wrapper
45 {
46 
47 void DepthImage::fillDepthImageRaw(unsigned width, unsigned height, unsigned short* depth_buffer, unsigned line_step) const throw (OpenNIException)
48 {
49  if (width > depth_md_->XRes () || height > depth_md_->YRes ())
50  THROW_OPENNI_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
51 
52  if (depth_md_->XRes () % width != 0 || depth_md_->YRes () % height != 0)
53  THROW_OPENNI_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
54 
55  if (line_step == 0)
56  line_step = width * sizeof (unsigned short);
57 
58  // special case no sclaing, no padding => memcopy!
59  if (width == depth_md_->XRes () && height == depth_md_->YRes () && (line_step == width * sizeof (unsigned short)))
60  {
61  memcpy (depth_buffer, depth_md_->WritableData(), depth_md_->DataSize ());
62  return;
63  }
64 
65  // padding skip for destination image
66  unsigned bufferSkip = line_step - width * sizeof (unsigned short);
67 
68  // step and padding skip for source image
69  unsigned xStep = depth_md_->XRes () / width;
70  unsigned ySkip = (depth_md_->YRes () / height - 1) * depth_md_->XRes ();
71 
72  // Fill in the depth image data, converting mm to m
73  short bad_point = numeric_limits<short>::quiet_NaN ();
74  unsigned depthIdx = 0;
75 
76  for (unsigned yIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip)
77  {
78  for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++depth_buffer)
79  {
81  if ((*depth_md_)[depthIdx] == 0 ||
82  (*depth_md_)[depthIdx] == no_sample_value_ ||
83  (*depth_md_)[depthIdx] == shadow_value_)
84  *depth_buffer = bad_point;
85  else
86  {
87  *depth_buffer = (unsigned short) (*depth_md_)[depthIdx];
88  }
89  }
90  // if we have padding
91  if (bufferSkip > 0)
92  {
93  char* cBuffer = reinterpret_cast<char*> (depth_buffer);
94  depth_buffer = reinterpret_cast<unsigned short*> (cBuffer + bufferSkip);
95  }
96  }
97 }
98 
99 void DepthImage::fillDepthImage (unsigned width, unsigned height, float* depth_buffer, unsigned line_step) const throw (OpenNIException)
100 {
101  if (width > depth_md_->XRes () || height > depth_md_->YRes ())
102  THROW_OPENNI_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
103 
104  if (depth_md_->XRes () % width != 0 || depth_md_->YRes () % height != 0)
105  THROW_OPENNI_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
106 
107  if (line_step == 0)
108  line_step = width * sizeof (float);
109 
110  // padding skip for destination image
111  unsigned bufferSkip = line_step - width * sizeof (float);
112 
113  // step and padding skip for source image
114  unsigned xStep = depth_md_->XRes () / width;
115  unsigned ySkip = (depth_md_->YRes () / height - 1) * depth_md_->XRes();
116 
117  // Fill in the depth image data, converting mm to m
118  float bad_point = numeric_limits<float>::quiet_NaN ();
119  unsigned depthIdx = 0;
120 
121  for (unsigned yIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip)
122  {
123  for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++depth_buffer)
124  {
126  if ((*depth_md_)[depthIdx] == 0 ||
127  (*depth_md_)[depthIdx] == no_sample_value_ ||
128  (*depth_md_)[depthIdx] == shadow_value_)
129  *depth_buffer = bad_point;
130  else
131  {
132  *depth_buffer = (float) (*depth_md_)[depthIdx] * 0.001f;
133  }
134  }
135  // if we have padding
136  if (bufferSkip > 0)
137  {
138  char* cBuffer = reinterpret_cast<char*> (depth_buffer);
139  depth_buffer = reinterpret_cast<float*> (cBuffer + bufferSkip);
140  }
141  }
142 }
143 
144 void DepthImage::fillDisparityImage (unsigned width, unsigned height, float* disparity_buffer, unsigned line_step) const throw (OpenNIException)
145 {
146  if (width > depth_md_->XRes () || height > depth_md_->YRes ())
147  THROW_OPENNI_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
148 
149  if (depth_md_->XRes () % width != 0 || depth_md_->YRes () % height != 0)
150  THROW_OPENNI_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", depth_md_->XRes (), depth_md_->YRes (), width, height);
151 
152  if (line_step == 0)
153  line_step = width * sizeof (float);
154 
155  unsigned xStep = depth_md_->XRes () / width;
156  unsigned ySkip = (depth_md_->YRes () / height - 1) * depth_md_->XRes ();
157 
158  unsigned bufferSkip = line_step - width * sizeof (float);
159 
160  // Fill in the depth image data
161  // iterate over all elements and fill disparity matrix: disp[x,y] = f * b / z_distance[x,y];
162  // focal length is for the native image resolution -> focal_length = focal_length_ / xStep;
163  float constant = focal_length_ * baseline_ * 1000.0 / (float) xStep;
164 
165  for (unsigned yIdx = 0, depthIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip)
166  {
167  for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++disparity_buffer)
168  {
169  if ((*depth_md_)[depthIdx] == 0 ||
170  (*depth_md_)[depthIdx] == no_sample_value_ ||
171  (*depth_md_)[depthIdx] == shadow_value_)
172  *disparity_buffer = 0.0;
173  else
174  *disparity_buffer = constant / (double) (*depth_md_)[depthIdx];
175  }
176 
177  // if we have padding
178  if (bufferSkip > 0)
179  {
180  char* cBuffer = reinterpret_cast<char*> (disparity_buffer);
181  disparity_buffer = reinterpret_cast<float*> (cBuffer + bufferSkip);
182  }
183  }
184 }
185 } // namespace
#define THROW_OPENNI_EXCEPTION(format,...)
General exception class.


openni_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu
autogenerated on Wed Jun 5 2019 20:15:22