50 volatile bool quit{
false};
51 volatile int pushed_key{0};
52 volatile double Hz{0};
68 const std::string cfgFile =
"kinect_calib.cfg";
71 cout <<
"Loading calibration from: " << cfgFile << endl;
75 cerr <<
"Warning: Calibration file [" << cfgFile
76 <<
"] not found -> Using default params.\n";
79 cout <<
"Calling CKinect::initialize()...";
85 bool there_is_obs =
true, hard_error =
false;
87 while (!hard_error && !p.
quit)
90 auto obs = CObservation3DRangeScan::Create();
96 if (!hard_error && there_is_obs)
98 std::atomic_store(&p.
new_obs, obs);
118 p.
Hz = nImgs / tictac.
Tac();
124 catch (
const std::exception& e)
126 cout <<
"Exception in Kinect thread: " << e.what() << endl;
143 cout <<
"Waiting for sensor initialization...\n";
147 std::atomic_load(&thrPar.
new_obs);
151 std::this_thread::sleep_for(10ms);
152 }
while (!thrPar.
quit);
155 if (thrPar.
quit)
return;
161 win3D.setCameraAzimuthDeg(140);
162 win3D.setCameraElevationDeg(30);
163 win3D.setCameraZoom(10.0);
165 win3D.setCameraPointingToPoint(2.5, 0, 0);
170 gl_points->setPointSize(2.5);
175 gl_2d_scan->enablePoints(
true);
176 gl_2d_scan->enableLine(
true);
177 gl_2d_scan->enableSurface(
true);
178 gl_2d_scan->setSurfaceColor(0, 0, 1, 0.3);
181 0.2f, 5.0f, 90.0f, 5.0f, 2.0f,
true,
true);
183 const double aspect_ratio =
192 scene->insert(gl_points);
193 scene->insert(gl_2d_scan);
194 scene->insert(gl_frustum);
199 gl_grid->setColor(0.6, 0.6, 0.6);
200 scene->insert(gl_grid);
205 gl_corner->setScale(0.2);
206 scene->insert(gl_corner);
211 const int VW_HEIGHT = aspect_ratio * VW_WIDTH;
212 const int VW_GAP = 30;
216 win3D.addTextMessage(
217 30, -25 - 1 * (VW_GAP + VW_HEIGHT),
"Range data", 1);
218 viewRange = scene->createViewport(
"view2d_range");
219 viewRange->setViewportPosition(
220 5, -10 - 1 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
222 win3D.addTextMessage(
223 30, -25 - 2 * (VW_GAP + VW_HEIGHT),
"Intensity data", 2);
224 viewInt = scene->createViewport(
"view2d_int");
225 viewInt->setViewportPosition(
226 5, -10 - 2 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
228 win3D.unlockAccess3DScene();
235 while (win3D.isOpen() && !thrPar.
quit)
238 std::atomic_load(&thrPar.
new_obs);
240 (!last_obs || possiblyNewObs->timestamp != last_obs->timestamp))
243 last_obs = possiblyNewObs;
244 last_obs_imu = std::atomic_load(&thrPar.
new_obs_imu);
247 bool do_refresh =
false;
250 if (last_obs->hasRangeImage)
257 range2D = last_obs->rangeImage;
258 range2D *= (1.0 / last_obs->maxRange);
262 win3D.get3DSceneAndLock();
263 viewRange->setImageView(std::move(img));
264 win3D.unlockAccess3DScene();
269 if (last_obs->hasRangeImage)
273 CObservation2DRangeScan::Create();
281 last_obs->convertTo2DScan(*obs_2d, sp);
284 gl_2d_scan->setScan(*obs_2d);
288 if (last_obs->hasIntensityImage)
290 win3D.get3DSceneAndLock();
291 viewInt->setImageView(
292 last_obs->intensityImage);
295 win3D.unlockAccess3DScene();
300 if (last_obs->hasPoints3D)
302 win3D.get3DSceneAndLock();
306 last_obs->unprojectInto(*gl_points, pp);
307 win3D.unlockAccess3DScene();
312 win3D.get3DSceneAndLock();
314 win3D.addTextMessage(-100, -20,
format(
"%.02f Hz", thrPar.
Hz), 100);
316 win3D.addTextMessage(
318 "'o'/'i'-zoom out/in, '2'/'3'/'f':show/hide 2D/3D/frustum "
319 "data, mouse: orbit 3D, ESC: quit",
322 win3D.addTextMessage(
325 "Show: 3D=%s 2D=%s Frustum=%s (vert. FOV=%.1fdeg)",
326 gl_points->isVisible() ?
"YES" :
"NO",
327 gl_2d_scan->isVisible() ?
"YES" :
"NO",
328 gl_frustum->isVisible() ?
"YES" :
"NO",
329 gl_frustum->getVertFOV()),
331 win3D.unlockAccess3DScene();
334 if (last_obs_imu && last_obs_imu->dataIsPresent[
IMU_X_ACC])
336 win3D.get3DSceneAndLock();
337 win3D.addTextMessage(
340 "Acc: x=%.02f y=%.02f z=%.02f",
341 last_obs_imu->rawMeasurements[
IMU_X_ACC],
342 last_obs_imu->rawMeasurements[
IMU_Y_ACC],
343 last_obs_imu->rawMeasurements[
IMU_Z_ACC]),
345 win3D.unlockAccess3DScene();
350 if (do_refresh) win3D.repaint();
358 const int key = tolower(win3D.getPushedKey());
364 win3D.setCameraZoom(win3D.getCameraZoom() * 1.2);
368 win3D.setCameraZoom(win3D.getCameraZoom() / 1.2);
372 gl_2d_scan->setVisibility(!gl_2d_scan->isVisible());
375 gl_points->setVisibility(!gl_points->isVisible());
379 gl_frustum->setVisibility(!gl_frustum->isVisible());
382 gl_frustum->setVertFOV(gl_frustum->getVertFOV() + 1);
385 gl_frustum->setVertFOV(gl_frustum->getVertFOV() - 1);
394 std::this_thread::sleep_for(1ms);
397 cout <<
"Waiting for grabbing thread to exit...\n";
409 std::this_thread::sleep_for(50ms);
412 catch (
const std::exception& e)
419 printf(
"Another exception!!");