1 #pragma warning (disable :4996)
2 #undef _CRT_SECURE_NO_DEPRECATE
7 using namespace openni;
9 #define REPORT_ERROR(msg) kfusion::cuda::error ((msg), __FILE__, __LINE__)
14 return pixelWidth / (2.0 * tan (fov / 2.0));
30 : depth_focal_length_VGA (0.f)
36 , maxFrameIndex_(1000000)
54 impl_ = cv::Ptr<Impl>(
new Impl () );
58 rc = openni::OpenNI::initialize ();
59 if (rc != openni::STATUS_OK)
61 const string error(openni::OpenNI::getExtendedError());
62 sprintf (impl_->strError,
"Init failed: %s\n",
error.c_str() );
69 rc = impl_->device.open(
filename.c_str() );
72 rc = impl_->device.open(openni::ANY_DEVICE);
74 if (rc != openni::STATUS_OK)
76 const string error(openni::OpenNI::getExtendedError());
77 sprintf (impl_->strError,
"Device open failed: %s\n",
error.c_str() );
81 impl_->has_image = impl_->device.hasSensor(SENSOR_COLOR);
82 impl_->has_depth = impl_->device.hasSensor(SENSOR_DEPTH);
84 rc = impl_->device.setDepthColorSyncEnabled(
true);
85 if (rc != openni::STATUS_OK)
87 sprintf (impl_->strError,
"Init failed: %s\n", openni::OpenNI::getExtendedError() );
91 rc = impl_->colorStream.create(impl_->device, openni::SENSOR_COLOR);
92 if (rc != openni::STATUS_OK)
94 sprintf (impl_->strError,
"Init failed: %s\n", openni::OpenNI::getExtendedError() );
98 rc = impl_->depthStream.create(impl_->device, openni::SENSOR_DEPTH);
99 if (rc != openni::STATUS_OK)
101 sprintf (impl_->strError,
"Init failed: %s\n", openni::OpenNI::getExtendedError() );
107 impl_->device.getPlaybackControl()->setRepeatEnabled(
false);
108 maxFrameIndex_ = impl_->device.getPlaybackControl()->getNumberOfFrames(impl_->depthStream) - 10;
112 openni::VideoMode colorMode;
113 colorMode.setResolution (1280, 1024);
114 colorMode.setFps(30);
115 colorMode.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
117 openni::VideoMode depthMode;
118 depthMode.setResolution (640, 480);
119 depthMode.setFps(30);
121 depthMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
123 rc = impl_->colorStream.setVideoMode(colorMode);
124 if (rc != openni::STATUS_OK)
126 sprintf (impl_->strError,
"Init failed: %s\n", openni::OpenNI::getExtendedError() );
130 rc = impl_->depthStream.setVideoMode(depthMode);
131 if (rc != openni::STATUS_OK)
133 sprintf (impl_->strError,
"Init failed: %s\n", openni::OpenNI::getExtendedError() );
136 Status rc = rec_.create(
string(
"./Captured" + std::to_string(rec_count_++) +
".oni").c_str());
139 printf (
"Error creating Record object.\n");
142 rec_.attach(impl_->depthStream);
143 rec_.attach(impl_->colorStream);
148 rc = impl_->colorStream.start();
149 if (rc != openni::STATUS_OK)
151 sprintf (impl_->strError,
"Init failed: %s\n", openni::OpenNI::getExtendedError() );
155 rc = impl_->depthStream.start();
156 if (rc != openni::STATUS_OK)
158 sprintf (impl_->strError,
"Init failed: %s\n", openni::OpenNI::getExtendedError() );
166 double speed = impl_->device.getPlaybackControl()->getSpeed();
167 if(isOni_ && speed == 1.0)
169 impl_->device.getPlaybackControl()->setSpeed(-1.0);
171 else if (isOni_ && speed == -1.0)
173 impl_->device.getPlaybackControl()->setSpeed(1.0);
180 if(!isOni_ && !recording_)
184 printf (
"Start recording.\n");
186 else if(!isOni_ && recording_)
191 Status rc = rec_.create(
string(
"./Captured" + std::to_string(rec_count_++) +
".oni").c_str());
194 printf (
"Error creating Record object.\n");
197 rec_.attach(impl_->depthStream);
198 rec_.attach(impl_->colorStream);
199 printf (
"Stop recording.\n");
207 impl_->colorStream.stop();
208 impl_->colorStream.destroy();
210 impl_->depthStream.stop();
211 impl_->depthStream.destroy();
213 impl_->device.close();
220 printf (
"Stop recording.\n");
224 depth_focal_length_VGA = 0;
233 Status rc = STATUS_OK;
234 if (impl_->has_depth)
236 rc = impl_->depthStream.readFrame(&impl_->depthFrame);
237 if (rc != openni::STATUS_OK)
239 sprintf (impl_->strError,
"Frame grab failed: %s\n", openni::OpenNI::getExtendedError() );
243 const void* pDepth = impl_->depthFrame.getData();
244 int x = impl_->depthFrame.getWidth();
245 int y = impl_->depthFrame.getHeight();
246 cv::Mat(y, x, CV_16U, (
void*)pDepth).copyTo(depth);
251 printf (
"no depth\n");
254 if (impl_->has_image)
256 rc = impl_->colorStream.readFrame(&impl_->colorFrame);
257 if (rc != openni::STATUS_OK)
259 sprintf (impl_->strError,
"Frame grab failed: %s\n", openni::OpenNI::getExtendedError() );
263 const void* pColor = impl_->colorFrame.getData();
264 int x = impl_->colorFrame.getWidth();
265 int y = impl_->colorFrame.getHeight();
266 cv::Mat(y, x, CV_8UC3, (
void*)pColor).copyTo(color);
267 cv::cvtColor(color, color, cv::COLOR_RGB2BGR);
272 printf (
"no color\n");
275 int frame = impl_->depthFrame.getFrameIndex();
276 if(frame > maxFrameIndex_)
279 return impl_->has_image || impl_->has_depth;
284 openni::Status rc = STATUS_OK;
286 max_depth = impl_->depthStream.getMaxPixelValue();
288 double baseline_local = 0;
289 if ( impl_->depthStream.isPropertySupported (XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE) )
291 rc = impl_->depthStream.getProperty (XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE, &baseline_local);
294 printf (
"Baseline not available.\n");
300 baseline = (float)(baseline_local * 10);
303 int width = impl_->colorStream.getVideoMode().getResolutionX();
304 float hFov = impl_->colorStream.getHorizontalFieldOfView();
310 Status rc = STATUS_OK;
314 rc = impl_->device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR);
315 if (rc != openni::STATUS_OK)
317 sprintf (impl_->strError,
"Setting registration failed: %s\n", openni::OpenNI::getExtendedError() );
323 rc = impl_->device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_OFF);
324 if (rc != openni::STATUS_OK)
326 sprintf (impl_->strError,
"Setting registration failed: %s\n", openni::OpenNI::getExtendedError() );
332 return rc == STATUS_OK;