51 volatile int pushed_key;
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)
93 mrpt::make_aligned_shared<CObservationIMU>();
97 if (!hard_error && there_is_obs)
99 std::atomic_store(&
p.new_obs, obs);
100 std::atomic_store(&
p.new_obs_imu, obs_imu);
103 if (
p.pushed_key != 0)
105 switch (
p.pushed_key)
119 p.Hz = nImgs / tictac.
Tac();
125 catch (std::exception& e)
127 cout <<
"Exception in Kinect thread: " << e.what() << endl;
144 cout <<
"Waiting for sensor initialization...\n";
148 std::atomic_load(&thrPar.
new_obs);
152 std::this_thread::sleep_for(10ms);
153 }
while (!thrPar.
quit);
156 if (thrPar.
quit)
return;
162 win3D.setCameraAzimuthDeg(140);
163 win3D.setCameraElevationDeg(30);
164 win3D.setCameraZoom(10.0);
166 win3D.setCameraPointingToPoint(2.5, 0, 0);
170 mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
171 gl_points->setPointSize(2.5);
175 mrpt::make_aligned_shared<mrpt::opengl::CPlanarLaserScan>();
176 gl_2d_scan->enablePoints(
true);
177 gl_2d_scan->enableLine(
true);
178 gl_2d_scan->enableSurface(
true);
179 gl_2d_scan->setSurfaceColor(0, 0, 1, 0.3);
182 mrpt::make_aligned_shared<mrpt::opengl::CFrustum>(
183 0.2f, 5.0f, 90.0f, 5.0f, 2.0f,
true,
true);
185 const double aspect_ratio =
194 scene->insert(gl_points);
195 scene->insert(gl_2d_scan);
196 scene->insert(gl_frustum);
200 mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>();
201 gl_grid->setColor(0.6, 0.6, 0.6);
202 scene->insert(gl_grid);
207 gl_corner->setScale(0.2);
208 scene->insert(gl_corner);
213 const int VW_HEIGHT = aspect_ratio * VW_WIDTH;
214 const int VW_GAP = 30;
218 win3D.addTextMessage(
219 30, -25 - 1 * (VW_GAP + VW_HEIGHT),
"Range data",
TColorf(1, 1, 1),
221 viewRange = scene->createViewport(
"view2d_range");
222 viewRange->setViewportPosition(
223 5, -10 - 1 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
225 win3D.addTextMessage(
226 30, -25 - 2 * (VW_GAP + VW_HEIGHT),
"Intensity data",
228 viewInt = scene->createViewport(
"view2d_int");
229 viewInt->setViewportPosition(
230 5, -10 - 2 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
232 win3D.unlockAccess3DScene();
239 while (win3D.isOpen() && !thrPar.
quit)
242 std::atomic_load(&thrPar.
new_obs);
244 (!last_obs || possiblyNewObs->timestamp != last_obs->timestamp))
247 last_obs = possiblyNewObs;
248 last_obs_imu = std::atomic_load(&thrPar.
new_obs_imu);
251 bool do_refresh =
false;
254 if (last_obs->hasRangeImage)
261 range2D = last_obs->rangeImage *
264 img.setFromMatrix(range2D);
266 win3D.get3DSceneAndLock();
267 viewRange->setImageView_fast(
img);
268 win3D.unlockAccess3DScene();
273 if (last_obs->hasRangeImage)
277 mrpt::make_aligned_shared<CObservation2DRangeScan>();
285 last_obs->convertTo2DScan(*obs_2d, sp);
288 gl_2d_scan->setScan(*obs_2d);
292 if (last_obs->hasIntensityImage)
294 win3D.get3DSceneAndLock();
295 viewInt->setImageView(
296 last_obs->intensityImage);
299 win3D.unlockAccess3DScene();
304 if (last_obs->hasPoints3D)
306 win3D.get3DSceneAndLock();
310 last_obs->project3DPointsFromDepthImageInto(*gl_points, pp);
311 win3D.unlockAccess3DScene();
316 win3D.get3DSceneAndLock();
318 win3D.addTextMessage(
322 win3D.addTextMessage(
324 "'o'/'i'-zoom out/in, '2'/'3'/'f':show/hide 2D/3D/frustum "
325 "data, mouse: orbit 3D, ESC: quit",
328 win3D.addTextMessage(
330 "Show: 3D=%s 2D=%s Frustum=%s (vert. FOV=%.1fdeg)",
331 gl_points->isVisible() ?
"YES" :
"NO",
332 gl_2d_scan->isVisible() ?
"YES" :
"NO",
333 gl_frustum->isVisible() ?
"YES" :
"NO",
334 gl_frustum->getVertFOV()),
336 win3D.unlockAccess3DScene();
339 if (last_obs_imu && last_obs_imu->dataIsPresent[
IMU_X_ACC])
341 win3D.get3DSceneAndLock();
342 win3D.addTextMessage(
344 "Acc: x=%.02f y=%.02f z=%.02f",
345 last_obs_imu->rawMeasurements[
IMU_X_ACC],
346 last_obs_imu->rawMeasurements[
IMU_Y_ACC],
347 last_obs_imu->rawMeasurements[
IMU_Z_ACC]),
349 win3D.unlockAccess3DScene();
354 if (do_refresh) win3D.repaint();
362 const int key = tolower(win3D.getPushedKey());
368 win3D.setCameraZoom(win3D.getCameraZoom() * 1.2);
372 win3D.setCameraZoom(win3D.getCameraZoom() / 1.2);
376 gl_2d_scan->setVisibility(!gl_2d_scan->isVisible());
379 gl_points->setVisibility(!gl_points->isVisible());
383 gl_frustum->setVisibility(!gl_frustum->isVisible());
386 gl_frustum->setVertFOV(gl_frustum->getVertFOV() + 1);
389 gl_frustum->setVertFOV(gl_frustum->getVertFOV() - 1);
398 std::this_thread::sleep_for(1ms);
401 cout <<
"Waiting for grabbing thread to exit...\n";
407 int main(
int argc,
char** argv)
413 std::this_thread::sleep_for(50ms);
416 catch (std::exception& e)
418 std::cout <<
"EXCEPCION: " << e.what() << std::endl;
423 printf(
"Another exception!!");