Example: gui_depth_camera_distortion

C++ example source code:

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#ifdef __EMSCRIPTEN__  // are we actually in a Wasm JS instance?
#include <emscripten.h>
#include <emscripten/html5.h>

#define GL_GLEXT_PROTOTYPES
#define EGL_EGLEXT_PROTOTYPES
#include <GLFW/glfw3.h>

#include <functional>

std::function<void()> loop;
void main_loop() { loop(); }
#endif

#include <mrpt/core/exceptions.h>
#include <mrpt/core/lock_helper.h>
#include <mrpt/gui/CDisplayWindowGUI.h>
#include <mrpt/io/lazy_load_path.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/opengl/CAxis.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CPointCloudColoured.h>
#include <mrpt/opengl/stock_objects.h>

#include <iostream>

#if MRPT_HAS_NANOGUI

constexpr int SMALL_FONT_SIZE = 15;
constexpr int LARGE_FONT_SIZE = 20;

// We need to define all variables here to avoid emscripten memory errors
// if they are defined as *locals* in the function.
struct AppData
{
    AppData() = default;

    mrpt::gui::CDisplayWindowGUI::Ptr win;
    mrpt::opengl::CAxis::Ptr gl_corner_reference;
    mrpt::obs::CObservation3DRangeScan::Ptr obs;

    mrpt::img::TCamera originalCalib;
    mrpt::poses::CPose3D originalSensorPose;

    mrpt::opengl::CPointCloudColoured::Ptr gl_pts;
    mrpt::opengl::CSetOfObjects::Ptr gl_sensorPoseCorner;

    std::vector<nanogui::TextBox*> edBoxes /* cx,cy,... dist[7]*/
        ,
        edBoxesExtrinsics /*x,y,... pitch, roll*/;

    std::vector<nanogui::Slider*> sliders /* cx,cy,... dist[7] */,
        slidersExtrinsics /*x,y,... pitch, roll*/;

    nanogui::ComboBox* cbDistType;

    void obs_params_to_gui();
    void gui_params_to_obs();
};

static AppData app;

// The main function: update all graphs
static void recalcAll()
{
    auto lck = mrpt::lockHelper(app.win->background_scene_mtx);

    if (!app.obs || !app.obs->hasRangeImage)
    {
        app.gl_pts->clear();
        return;
    }

    mrpt::obs::T3DPointsProjectionParams pp;
    pp.takeIntoAccountSensorPoseOnRobot = true;

    app.obs->unprojectInto(*app.gl_pts, pp);

    const auto bbox = app.gl_pts->getBoundingBox();

    app.gl_pts->recolorizeByCoordinate(
        bbox.min.z, bbox.max.z, 2 /*0,1,2=x,y,z*/);

    app.gl_pts->setPointSize(2.0f);

    app.gl_sensorPoseCorner->setPose(app.obs->sensorPose);
};

void AppData::obs_params_to_gui()
{
    ASSERT_(obs);
    ASSERT_EQUAL_(edBoxes.size(), sliders.size());
    ASSERT_EQUAL_(edBoxes.size(), 12U);

    const auto& p = obs->cameraParams;

    edBoxes[0]->setValue(mrpt::format("%.04f", p.cx()));
    edBoxes[1]->setValue(mrpt::format("%.04f", p.cy()));
    edBoxes[2]->setValue(mrpt::format("%.04f", p.fx()));
    edBoxes[3]->setValue(mrpt::format("%.04f", p.fy()));

    cbDistType->setSelectedIndex(static_cast<int>(p.distortion));

    for (int i = 0; i < 8; i++)
        edBoxes[4 + i]->setValue(mrpt::format("%.04e", p.dist[i]));

    // slider ranges:
    const double maxPixelRanges =
        std::max(std::max(p.cx(), p.cy()), std::max(p.fx(), p.fy()));
    const double minPixelRanges =
        std::min(std::min(p.cx(), p.cy()), std::min(p.fx(), p.fy()));
    for (int i = 0; i < 4; i++)
    {
        sliders[i]->setRange({0.75 * minPixelRanges, 1.5 * maxPixelRanges});
    }
    for (int i = 4; i < 12; i++)
    {
        const double absVal =
            std::max(1e-3, std::abs(std::stod(edBoxes[i]->value())));
        sliders[i]->setRange({-2.0 * absVal, 2.0 * absVal});
    }

    // slider values:
    for (size_t i = 0; i < edBoxes.size(); i++)
        sliders[i]->setValue(std::stod(edBoxes[i]->value()));

    // Extrinsics ranges:
    for (int i = 0; i < 3; i++)
        slidersExtrinsics[i]->setRange({-2.0, 2.0});

    slidersExtrinsics[3]->setRange({-180.0, 180.0});
    slidersExtrinsics[4]->setRange({-90.0, 90.0});
    slidersExtrinsics[5]->setRange({-180.0, 180.0});

    // Extrinsics values:
    for (int i = 0; i < 6; i++)
    {
        double val = obs->sensorPose[i];
        if (i > 3) val = mrpt::RAD2DEG(val);

        edBoxesExtrinsics[i]->setValue(mrpt::format("%.04f", val));
        slidersExtrinsics[i]->setValue(val);
    }
}

void AppData::gui_params_to_obs()
{
    ASSERT_(obs);
    ASSERT_EQUAL_(edBoxes.size(), sliders.size());
    ASSERT_EQUAL_(edBoxes.size(), 12U);

    auto& p = obs->cameraParams;
    p.distortion = mrpt::img::DistortionModel::plumb_bob;

    p.cx(std::stod(edBoxes[0]->value()));
    p.cy(std::stod(edBoxes[1]->value()));
    p.fx(std::stod(edBoxes[2]->value()));
    p.fy(std::stod(edBoxes[3]->value()));

    for (int i = 0; i < 8; i++)
        p.dist.at(i) = std::stod(edBoxes[4 + i]->value());

    p.distortion = mrpt::typemeta::str2enum<mrpt::img::DistortionModel>(
        cbDistType->items().at((cbDistType->selectedIndex())));

    // Extrinsics:
    app.obs->sensorPose = mrpt::poses::CPose3D::FromXYZYawPitchRoll(
        std::stod(edBoxesExtrinsics[0]->value()),  // x
        std::stod(edBoxesExtrinsics[1]->value()),  // y
        std::stod(edBoxesExtrinsics[2]->value()),  // z
        mrpt::DEG2RAD(std::stod(edBoxesExtrinsics[3]->value())),  // yaw
        mrpt::DEG2RAD(std::stod(edBoxesExtrinsics[4]->value())),  // pitch
        mrpt::DEG2RAD(std::stod(edBoxesExtrinsics[5]->value()))  // roll
    );
}

static void AppDepthCamDemo()
{
    nanogui::init();

    // Create main window:
    mrpt::gui::CDisplayWindowGUI_Params cp;
    // cp.fullscreen = true;

    // Init values:
    app.gl_corner_reference = mrpt::opengl::CAxis::Create(
        -5.0f, -5.0f, .0f, 5.0f, 5.0f, 2.0f, 0.2f, 1.0f, true);
    app.gl_corner_reference->setTextScale(0.04);

    app.gl_pts = mrpt::opengl::CPointCloudColoured::Create();
    app.gl_sensorPoseCorner =
        mrpt::opengl::stock_objects::CornerXYZSimple(0.10f);

    // Create GUI:
    app.win = mrpt::gui::CDisplayWindowGUI::Create(
        "Depth camera distortion demo", 1000, 700, cp);

    // Add main window:
    // -----------------------------
    nanogui::Window* w = new nanogui::Window(&(*app.win), "Camera parameters");
    w->setPosition(nanogui::Vector2i(10, 50));
    w->setLayout(new nanogui::GridLayout(
        nanogui::Orientation::Horizontal, 1, nanogui::Alignment::Fill, 5, 0));
    w->setFixedWidth(350);

    // Load button:
    w->add<nanogui::Label>("Operations")->setFontSize(LARGE_FONT_SIZE);

    w->add<nanogui::Button>("Load depth image from rawlog...")
        ->setCallback([]() {
            try
            {
                const std::string loadFile = nanogui::file_dialog(
                    {{"rawlog", "MRPT rawlog dataset files"},
                     {"*", "Any file"}},
                    false);
                if (loadFile.empty()) return;

                // Enable MRPT to find externalized rawlogs:
                mrpt::io::setLazyLoadPathBase(
                    mrpt::obs::CRawlog::detectImagesDirectory(loadFile));

                mrpt::obs::CRawlog rawlog;
                if (!rawlog.loadFromRawLogFile(loadFile)) return;

                ASSERT_(!rawlog.empty());

                mrpt::obs::CObservation3DRangeScan::Ptr obs;

                // Obs-only format:
                if (rawlog.getType(0) == mrpt::obs::CRawlog::etObservation)
                {
                    obs = std::dynamic_pointer_cast<
                        mrpt::obs::CObservation3DRangeScan>(
                        rawlog.getAsObservation(0));
                    ASSERTMSG_(
                        obs,
                        "First observation in rawlog is not "
                        "CObservation3DRangeScan");
                }
                else
                {
                    // SF format:
                    ASSERT_(
                        rawlog.getType(0) ==
                        mrpt::obs::CRawlog::etSensoryFrame);

                    obs = rawlog.getAsObservations(0)
                              ->getObservationByClass<
                                  mrpt::obs::CObservation3DRangeScan>();
                    ASSERT_(obs);
                }

                app.obs = obs;
                app.originalCalib = obs->cameraParams;
                app.originalSensorPose = obs->sensorPose;
                app.obs_params_to_gui();
                recalcAll();
            }
            catch (const std::exception& e)
            {
                std::cerr << e.what() << std::endl;
                // auto dlg =
                new nanogui::MessageDialog(
                    app.win->screen(), nanogui::MessageDialog::Type::Warning,
                    "Exception", e.what());
            }
        });

    w->add<nanogui::Button>("Revert to original calibration")
        ->setCallback([]() {
            if (!app.obs) return;
            app.obs->cameraParams = app.originalCalib;
            app.obs->sensorPose = app.originalSensorPose;
            app.obs_params_to_gui();
            recalcAll();
        });

    // Sliders and value controls:
    w->add<nanogui::Label>("Intrinsics")->setFontSize(LARGE_FONT_SIZE);
    ;
    {
        auto pn = w->add<nanogui::Widget>();
        pn->setLayout(new nanogui::GridLayout(
            nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill, 5,
            0));

        pn->add<nanogui::Label>("Distortion model");
        app.cbDistType = pn->add<nanogui::ComboBox>(
            std::vector<std::string>({"none", "plumb_bob", "kannala_brandt"}));
        pn->add<nanogui::Label>(" ");

        app.cbDistType->setCallback([](int) {
            app.gui_params_to_obs();
            recalcAll();
        });

        std::vector<std::string> lb = {
            "cx=",           "cy=",           "fx=",           "fy=",
            "dist[0] (k1)=", "dist[1] (k2)=", "dist[2] (t1)=", "dist[3] (t2)=",
            "dist[4] (k3)=", "dist[5] (k4)=", "dist[6] (k5)=", "dist[7] (k6)=",
        };

        for (size_t i = 0; i < lb.size(); i++)
        {
            pn->add<nanogui::Label>(lb[i])->setFontSize(SMALL_FONT_SIZE);

            nanogui::TextBox* ed = pn->add<nanogui::TextBox>(" ");
            app.edBoxes.push_back(ed);

            ed->setEditable(true);
            ed->setFormat("[-+]?[0-9.e+-]*");
            ed->setCallback([](const std::string&) {
                app.gui_params_to_obs();
                recalcAll();
                return true;
            });
            nanogui::Slider* sl = pn->add<nanogui::Slider>();
            app.sliders.push_back(sl);

            sl->setRange({-1.0f, 1.0f});
            sl->setCallback([&, ed](float val) {
                ed->setValue(mrpt::format("%.03g", val));
                ed->callback()(ed->value());
            });

            ed->setFontSize(SMALL_FONT_SIZE);
        }
    }

    // Extrinsics:
    w->add<nanogui::Label>("Extrinsics")->setFontSize(LARGE_FONT_SIZE);
    {
        auto pn = w->add<nanogui::Widget>();
        pn->setLayout(new nanogui::GridLayout(
            nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill, 5,
            0));

        std::vector<std::string> lb = {
            "x=", "y=", "z=", "yaw=", "pitch=", "roll="};

        for (size_t i = 0; i < lb.size(); i++)
        {
            pn->add<nanogui::Label>(lb[i])->setFontSize(SMALL_FONT_SIZE);

            nanogui::TextBox* ed = pn->add<nanogui::TextBox>(" ");
            app.edBoxesExtrinsics.push_back(ed);

            ed->setEditable(true);
            ed->setFormat("[-+]?[0-9.e+-]*");
            ed->setCallback([](const std::string&) {
                app.gui_params_to_obs();
                recalcAll();
                return true;
            });
            ed->setFontSize(SMALL_FONT_SIZE);

            nanogui::Slider* sl = pn->add<nanogui::Slider>();
            app.slidersExtrinsics.push_back(sl);

            sl->setRange({-1.0f, 1.0f});
            sl->setCallback([&, ed](float val) {
                ed->setValue(mrpt::format("%.03g", val));
                ed->callback()(ed->value());
            });
        }
    }

    // Add top menu subwindow:
    // -----------------------------
    {
        nanogui::ref<nanogui::Window> winMenu =
            new nanogui::Window(&(*app.win), "");
        winMenu->setPosition(nanogui::Vector2i(0, 0));
        winMenu->setLayout(new nanogui::BoxLayout(
            nanogui::Orientation::Horizontal, nanogui::Alignment::Middle, 5));
        nanogui::Theme* modTheme =
            new nanogui::Theme(app.win->screen()->nvgContext());
        modTheme->mWindowHeaderHeight = 1;
        winMenu->setTheme(modTheme);

#ifndef __EMSCRIPTEN__  // are we actually in a Wasm JS instance?
        winMenu->add<nanogui::Button>("Quit", ENTYPO_ICON_ARROW_BOLD_LEFT)
            ->setCallback([]() { app.win->setVisible(false); });
#endif

        winMenu->add<nanogui::Label>("      ");  // separator

        winMenu
            ->add<nanogui::CheckBox>(
                "Show reference frame",
                [&](bool b) { app.gl_corner_reference->setVisibility(b); })
            ->setChecked(true);

        winMenu
            ->add<nanogui::CheckBox>(
                "Ortho. view",
                [&](bool b) { app.win->camera().setCameraProjective(!b); })
            ->setChecked(false);
    }

    // Add a background scene:
    // -----------------------------
    {
        auto scene = mrpt::opengl::COpenGLScene::Create();
        scene->insert(mrpt::opengl::CGridPlaneXY::Create());

        scene->insert(app.gl_corner_reference);
        scene->insert(app.gl_pts);
        scene->insert(app.gl_sensorPoseCorner);

        auto lck = mrpt::lockHelper(app.win->background_scene_mtx);
        app.win->background_scene = std::move(scene);
    }

    app.win->performLayout();

    app.win->camera().setZoomDistance(7.0f);

    // Update view and process events:
    app.win->drawAll();
    app.win->setVisible(true);

#if !defined(__EMSCRIPTEN__)  // are we in a Wasm JS instance?
    // No: regular procedure:

    nanogui::mainloop();
    nanogui::shutdown();
#else

    // Yes, we are in a web browser running on JS:
    loop = [] {
        // Check if any events have been activated (key pressed, mouse moved
        // etc.) and call corresponding response functions
        glfwPollEvents();

        glClearColor(0.2f, 0.25f, 0.3f, 1.0f);
        glClear(GL_COLOR_BUFFER_BIT);

        // Draw nanogui
        app.win->drawContents();
        app.win->drawWidgets();

        glfwSwapBuffers(app.win->glfwWindow());
    };
    emscripten_set_main_loop(main_loop, 0, true);

    glfwDestroyWindow(app.win->nanogui_screen()->glfwWindow());

    // Terminate GLFW, clearing any resources allocated by GLFW.
    glfwTerminate();

#endif
}

#endif  // MRPT_HAS_NANOGUI

int main()
{
    try
    {
#if MRPT_HAS_NANOGUI
        AppDepthCamDemo();
#else
        std::cerr << "This example requires MRPT built with NANOGUI.\n";
#endif
        return 0;
    }
    catch (const std::exception& e)
    {
        std::cerr << mrpt::exception_to_str(e) << std::endl;
        return -1;
    }
}