47 std::atomic_bool quit{
false};
48 std::atomic<double> Hz{0};
59 const std::string str =
61 "grabber_type=myntd\n";
67 cout <<
"Calling initialize()...";
85 std::this_thread::sleep_for(10ms);
90 obss.begin()->second);
94 std::atomic_store(&p.
new_obs, obs);
99 p.
Hz = nImgs / tictac.
Tac();
105 catch (
const std::exception& e)
125 cout <<
"Waiting for sensor initialization...\n";
129 std::atomic_load(&thrPar.
new_obs);
133 std::this_thread::sleep_for(10ms);
134 }
while (!thrPar.
quit);
137 if (thrPar.
quit)
return;
145 win.camera().setAzimuthDegrees(140.0f);
146 win.camera().setElevationDegrees(20.0f);
147 win.camera().setZoomDistance(8.0f);
148 win.camera().setCameraFOV(50.0f);
149 win.camera().setCameraPointing(2.5, 0, 0);
155 std::atomic_bool SHOW_FEAT_IDS =
true;
156 std::atomic_bool SHOW_RESPONSES =
true;
158 std::atomic_bool hasToReset =
false;
160 unsigned int icpDecimation = 16;
162 std::mutex strStatuses_mtx;
163 std::array<std::string, 4> strStatuses;
166 std::mutex* viewInt_mtx =
nullptr;
169 ui_data_t() =
default;
176 auto lambdaUpdateThread = [&
win, &thrPar, &ui_data]() {
181 gl_points->setPointSize(1.25f);
182 gl_points_map->setPointSize(1.5f);
188 scene->insert(gl_points_map);
189 scene->insert(gl_points);
190 scene->insert(gl_keyframes);
193 scene->insert(gl_cur_cam_corner);
195 win.background_scene_mtx.lock();
196 win.background_scene = std::move(scene);
197 win.background_scene_mtx.unlock();
201 std::vector<TPose3D> camera_key_frames_path;
204 CPose3D currentCamPose_wrt_last;
206 unsigned int step_num = 0;
209 bool gl_keyframes_must_refresh =
true;
218 CColouredPointsMap::cmFromIntensityImage;
232 std::this_thread::sleep_for(5ms);
235 std::atomic_load(&thrPar.new_obs);
236 if (!possiblyNewObs ||
238 (cur_obs && possiblyNewObs->timestamp == cur_obs->timestamp))
242 cur_obs = possiblyNewObs;
246 cur_points = CColouredPointsMap::Create();
254 cur_obs->unprojectInto(*cur_points, pp);
258 CImage theImg = cur_obs->intensityImage;
266 prev_points = CColouredPointsMap::Create(*cur_points);
270 prev_points.get(), cur_points.get(), lastIcpRelPose, icp_res);
273 cur_obs->unprojectInto(*gl_points);
279 const CPose3D relativePose = icp_out->getMeanVal();
280 lastIcpRelPose = relativePose;
282 ui_data.strStatuses_mtx.lock();
284 "ICP: %d iters, goodness: %.02f%%",
287 ui_data.strStatuses[1] =
288 std::string(
"rel.pose:") + relativePose.
asString();
289 ui_data.strStatuses[2] =
290 string(icp_res.
goodness < 0.3 ?
"LOST! Press restart" :
"");
291 ui_data.strStatuses_mtx.unlock();
304 const CPose3D new_keyframe_global =
305 CPose3D(*camera_key_frames_path.rbegin()) +
308 camera_key_frames_path.push_back(
309 new_keyframe_global.
asTPose());
311 gl_keyframes_must_refresh =
true;
314 currentCamPose_wrt_last =
CPose3D();
316 cout <<
"Adding new key-frame: pose="
317 << new_keyframe_global << endl;
322 cur_points.get(), new_keyframe_global);
324 win.background_scene_mtx.lock();
325 gl_points_map->loadFromPointsMap(&globalPtsMap);
326 win.background_scene_mtx.unlock();
328 prev_points = std::move(cur_points);
332 currentCamPose_wrt_last = relativePose;
339 if (camera_key_frames_path.empty())
342 camera_key_frames_path.clear();
343 camera_key_frames_path.emplace_back(0, 0, 0, 0, 0, 0);
344 gl_keyframes_must_refresh =
true;
347 globalPtsMap.
clear();
350 win.background_scene_mtx.lock();
351 gl_points_map->loadFromPointsMap(&globalPtsMap);
352 win.background_scene_mtx.unlock();
362 CPose3D(*camera_key_frames_path.rbegin()) +
363 currentCamPose_wrt_last;
364 win.background_scene_mtx.lock();
366 cur_obs->unprojectInto(*gl_points);
367 gl_points->setPose(curGlobalPose);
369 gl_cur_cam_corner->setPose(curGlobalPose);
371 win.background_scene_mtx.unlock();
374 if (gl_keyframes_must_refresh)
376 gl_keyframes_must_refresh =
false;
380 win.background_scene_mtx.lock();
381 gl_keyframes->clear();
382 for (
const auto& i : camera_key_frames_path)
387 gl_keyframes->insert(obj);
389 win.background_scene_mtx.unlock();
392 ui_data.strStatuses_mtx.lock();
393 ui_data.strStatuses[3] =
394 format(
"Frames: %.02f Hz", std::atomic_load(&thrPar.Hz));
395 ui_data.strStatuses_mtx.unlock();
401 if (ui_data.hasToReset)
403 ui_data.hasToReset =
false;
408 camera_key_frames_path.clear();
409 gl_keyframes_must_refresh =
true;
410 globalPtsMap.
clear();
411 win.background_scene_mtx.lock();
412 gl_points_map->loadFromPointsMap(&globalPtsMap);
413 win.background_scene_mtx.unlock();
417 ui_data.viewInt_mtx->lock();
418 ui_data.viewInt->setImageView(std::move(theImg));
419 ui_data.viewInt_mtx->unlock();
423 std::thread thWorker = std::thread(lambdaUpdateThread);
426 std::array<nanogui::TextBox*, 4> lbStatuses = {
nullptr,
nullptr,
nullptr,
429 nanogui::Window* subWin2 =
nullptr;
432 auto subWin =
new nanogui::Window(&
win,
"Control");
433 subWin->setLayout(
new nanogui::GroupLayout());
434 subWin->setFixedWidth(400);
436 subWin->add<nanogui::Label>(
"Visualization",
"sans-bold");
438 auto cb = subWin->add<nanogui::CheckBox>(
"Show feature IDs");
440 [&ui_data](
bool checked) { ui_data.SHOW_FEAT_IDS = checked; });
441 cb->setChecked(
true);
445 auto cb = subWin->add<nanogui::CheckBox>(
"Show keypoint responses");
447 [&ui_data](
bool checked) { ui_data.SHOW_RESPONSES = checked; });
448 cb->setChecked(
true);
451 for (
unsigned int i = 0; i < lbStatuses.size(); i++)
452 lbStatuses[i] = subWin->add<nanogui::TextBox>(
"");
454 subWin->add<nanogui::Label>(
"RGB window size");
456 auto cmb = subWin->add<nanogui::ComboBox>(std::vector<std::string>(
457 {
"Hidden",
"200px",
"400px",
"800px",
"1000px"}));
458 cmb->setSelectedIndex(2);
459 cmb->setCallback([&](
int sel) {
460 subWin2->setVisible(sel != 0);
467 glCanvasRGBView->setFixedWidth(200);
470 glCanvasRGBView->setFixedWidth(400);
473 glCanvasRGBView->setFixedWidth(800);
476 glCanvasRGBView->setFixedWidth(1000);
484 nanogui::TextBox* slVal =
485 subWin->add<nanogui::TextBox>(
"Point cloud decimation: 8");
486 nanogui::Slider* sl = subWin->add<nanogui::Slider>();
488 sl->setRange({2, 4});
490 sl->setCallback([&ui_data, slVal](
float v) {
491 const unsigned int decim =
493 ui_data.icpDecimation = decim;
494 auto s = std::string(
"Point cloud decimation: ") +
500 subWin->add<nanogui::Label>(
"Actions",
"sans-bold");
504 subWin->add<nanogui::Button>(
"Reset", ENTYPO_ICON_BACK_IN_TIME);
505 btn->setCallback([&]() { ui_data.hasToReset =
true; });
510 subWin->add<nanogui::Button>(
"Quit", ENTYPO_ICON_ARROW_LEFT);
511 btn->setCallback([&]() {
win.setVisible(
false); });
516 subWin2 =
new nanogui::Window(&
win,
"Visible channel");
517 subWin2->setLayout(
new nanogui::BoxLayout(
518 nanogui::Orientation::Horizontal, nanogui::Alignment::Fill));
521 glCanvasRGBView->setFixedWidth(600);
527 ui_data.viewInt = glCanvasRGBView->
scene->getViewport();
528 ui_data.viewInt_mtx = &glCanvasRGBView->
scene_mtx;
530 subWin2->setPosition({10, 500});
536 win.setLoopCallback([&lbStatuses, &ui_data]() {
537 ui_data.strStatuses_mtx.lock();
538 for (
unsigned int i = 0; i < lbStatuses.size(); i++)
539 lbStatuses[i]->setValue(ui_data.strStatuses[i]);
540 ui_data.strStatuses_mtx.unlock();
545 win.setVisible(
true);
550 cout <<
"Waiting for grabbing thread to exit...\n";
557 #endif // MRPT_HAS_NANOGUI
566 std::this_thread::sleep_for(50ms);
572 catch (
const std::exception& e)