00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
00059 if (width == depth_md_->XRes () && height == depth_md_->YRes () && (line_step == width * sizeof (unsigned short)))
00060 {
00061 memcpy (depth_buffer, depth_md_->Data(), depth_md_->DataSize ());
00062 return;
00063 }
00064
00065
00066 unsigned bufferSkip = line_step - width * sizeof (unsigned short);
00067
00068
00069 unsigned xStep = depth_md_->XRes () / width;
00070 unsigned ySkip = (depth_md_->YRes () / height - 1) * depth_md_->XRes ();
00071
00072
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
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
00111 unsigned bufferSkip = line_step - width * sizeof (float);
00112
00113
00114 unsigned xStep = depth_md_->XRes () / width;
00115 unsigned ySkip = (depth_md_->YRes () / height - 1) * depth_md_->XRes();
00116
00117
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
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
00161
00162
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
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 }