diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2c027b32..2f1eff56 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -135,6 +135,7 @@ add_subdirectory(apps/mandeye_single_session_viewer)
 add_subdirectory(apps/multi_view_tls_registration)
 add_subdirectory(apps/multi_view_tls_registration_legacy)
 add_subdirectory(apps/multi_session_registration)
+add_subdirectory(apps/multi_session_registration_legacy)
 
 if(BUILD_WITH_UTILITY_APPLICATION)
     add_subdirectory(apps/manual_color)
diff --git a/apps/multi_session_registration/CMakeLists.txt b/apps/multi_session_registration/CMakeLists.txt
index bd9be085..89be3fdf 100644
--- a/apps/multi_session_registration/CMakeLists.txt
+++ b/apps/multi_session_registration/CMakeLists.txt
@@ -6,7 +6,7 @@ project(multi_session_registration_step_3)
 set(SOURCES
     multi_session_registration.cpp
     multi_session_factor_graph.cpp
-    "../../core/src/utils.cpp"
+    raylib_utils.cpp
 )
 
 # Windows: add resource file
@@ -41,13 +41,14 @@ target_include_directories(
 target_link_libraries(
   multi_session_registration_step_3
   # PRIVATE ${THIRDPARTY_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib
-  ${FREEGLUT_LIBRARY}
-  ${OPENGL_gl_LIBRARY}
-  OpenGL::GLU
+  # raylib-based like step 2; core_raylib brings in core, raylib and ScanRenderer.
+  core_raylib
+  raylib_widgets
+  imgui_raylib
+  rlimgui
+  imguizmo_raylib
   ${PLATFORM_LASZIP_LIB}
-  ${PLATFORM_MISCELLANEOUS_LIBS}
-  ${CORE_LIBRARIES}
-  ${GUI_LIBRARIES})
+  ${PLATFORM_MISCELLANEOUS_LIBS})
 
 if(WIN32)
   add_custom_command(
diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp
index ca6a4bbf..dcb78e46 100644
--- a/apps/multi_session_registration/multi_session_registration.cpp
+++ b/apps/multi_session_registration/multi_session_registration.cpp
@@ -2,13 +2,11 @@
 #include <filesystem>
 
 #include <imgui.h>
-#include <imgui_impl_glut.h>
-#include <imgui_impl_opengl2.h>
 #include <imgui_internal.h>
+#include <rlImGui.h>
 
 #include <ImGuizmo.h>
 
-#include <GL/freeglut.h>
 #include <glm/glm.hpp>
 #include <glm/gtc/matrix_transform.hpp>
 #include <nlohmann/json.hpp>
@@ -23,19 +21,27 @@
 #include <Core/pfd_wrapper.hpp>
 #include <Core/registration_plane_feature.h>
 #include <Core/session.h>
-#include <Core/utils.hpp>
 
+#ifdef _WIN32
+// windows.h (pulled in by portable-file-dialogs.h) declares CloseWindow/ShowCursor like raylib.h does.
+#define CloseWindow CloseWindow_win32
+#define ShowCursor ShowCursor_win32
+#endif
 #include <portable-file-dialogs.h>
+#ifdef _WIN32
+#undef CloseWindow
+#undef ShowCursor
+#endif
 
 #include <HDMapping/Version.hpp>
 
 #ifdef _WIN32
 #include "resource.h"
-#include <windows.h>
 
 #endif
 
 #include "multi_session_factor_graph.h"
+#include "raylib_utils.h"
 
 std::string winTitle = std::string("Step 3 (Multi session registration) ") + HDMAPPING_VERSION_STRING;
 
@@ -87,7 +93,7 @@ static const std::vector<ShortcutEntry> appShortcuts = { { "Normal keys", "A", "
                                                          { "", "Shift+R", "" },
                                                          { "", "S", "" },
                                                          { "", "Ctrl+S", "Save project" },
-                                                         { "", "Ctrl+Shift+S", "" },
+                                                         { "", "Ctrl+Shift+S", "Save project as" },
                                                          { "", "T", "" },
                                                          { "", "Ctrl+T", "" },
                                                          { "", "U", "" },
@@ -167,6 +173,9 @@ struct ProjectSettings
     std::vector<std::string> session_file_names;
 };
 
+// Project file opened or last saved; empty until then (Save project falls back to Save project as).
+std::string project_file_name;
+
 std::vector<Edge> edges;
 int index_active_edge = -1;
 bool manipulate_active_edge = false;
@@ -364,6 +373,35 @@ void ndt_gui()
 #endif
 }
 
+// Places the edge gizmo at the active edge's target pose.
+void setGizmoFromActiveEdge()
+{
+    const Edge& e = edges[index_active_edge];
+    const Eigen::Affine3d m_to = sessions[e.index_session_from].point_clouds_container.point_clouds[e.index_from].m_pose *
+        affine_matrix_from_pose_tait_bryan(e.relative_pose_tb);
+    Eigen::Map<Eigen::Matrix4f> gizmo(m_gizmo);
+    gizmo = m_to.matrix().cast<float>();
+}
+
+// Makes edge `i` active. The manipulate-edge view draws the scans picked by first/second_session_index and
+// index_loop_closure_source/target, so these follow the edge; a gizmo that is on is moved to the new edge,
+// otherwise it would write the previous edge's pose into this one.
+void setActiveEdge(int i)
+{
+    if (i < 0 || i >= (int)edges.size())
+        return;
+
+    index_active_edge = i;
+    const Edge& e = edges[i];
+    first_session_index = e.index_session_from;
+    second_session_index = e.index_session_to;
+    index_loop_closure_source = e.index_from;
+    index_loop_closure_target = e.index_to;
+
+    if (edge_gizmo)
+        setGizmoFromActiveEdge();
+}
+
 void loop_closure_gui()
 {
     if (ImGui::Begin("Manual Pose Graph Loop Closure Mode", &is_loop_closure_gui, ImGuiWindowFlags_AlwaysAutoResize))
@@ -462,9 +500,39 @@ void loop_closure_gui()
 
         std::string number_active_edges = "number_edges: " + std::to_string(edges.size());
         ImGui::Text(number_active_edges.c_str());
+
+        // Switching edges is blocked while a session gizmo is on, as for the index_active_edge input below.
+        bool is_session_gizmo = false;
+        for (const auto& s : sessions)
+            is_session_gizmo |= s.is_gizmo;
+
+        ImGui::BeginDisabled(is_session_gizmo);
+        for (int i = 0; i < (int)edges.size(); i++)
+        {
+            const auto& e = edges[i];
+            // session:scan -> session:scan
+            ImGui::Text(
+                "%s edge %d: %d:%d -> %d:%d",
+                (manipulate_active_edge && i == index_active_edge) ? ">" : " ",
+                i,
+                e.index_session_from,
+                e.index_from,
+                e.index_session_to,
+                e.index_to);
+            ImGui::SameLine();
+            char buttonLabel[128];
+            snprintf(buttonLabel, sizeof(buttonLabel), "Set Active##%d", i);
+            if (ImGui::Button(buttonLabel))
+            {
+                setActiveEdge(i);
+                manipulate_active_edge = true;
+            }
+        }
+        ImGui::EndDisabled();
         if (edges.size() > 0)
         {
-            ImGui::Checkbox("manipulate_active_edge", &manipulate_active_edge);
+            if (ImGui::Checkbox("manipulate_active_edge", &manipulate_active_edge) && manipulate_active_edge)
+                setActiveEdge(std::clamp(index_active_edge, 0, (int)edges.size() - 1));
             if (manipulate_active_edge)
             {
                 int remove_edge_index = -1;
@@ -494,6 +562,8 @@ void loop_closure_gui()
                             index_active_edge = 0;
                         if (index_active_edge >= (int)edges.size())
                             index_active_edge = (int)edges.size() - 1;
+                        if (index_active_edge != prev_index_active_edge)
+                            setActiveEdge(index_active_edge);
                     }
                 }
 
@@ -516,37 +586,15 @@ void loop_closure_gui()
                     }
                     edges = new_edges;
 
-                    index_active_edge = remove_edge_index - 1;
+                    index_active_edge = edges.empty() ? -1 : std::clamp(remove_edge_index - 1, 0, (int)edges.size() - 1);
                     manipulate_active_edge = false;
                 }
 
-                bool prev_gizmo = edge_gizmo;
+                const bool prev_gizmo = edge_gizmo;
                 ImGui::Checkbox("gizmo", &edge_gizmo);
 
                 if (prev_gizmo != edge_gizmo)
-                {
-                    auto m_to = sessions[edges[index_active_edge].index_session_from]
-                                    .point_clouds_container.point_clouds[edges[index_active_edge].index_from]
-                                    .m_pose *
-                        affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
-
-                    m_gizmo[0] = (float)m_to(0, 0);
-                    m_gizmo[1] = (float)m_to(1, 0);
-                    m_gizmo[2] = (float)m_to(2, 0);
-                    m_gizmo[3] = (float)m_to(3, 0);
-                    m_gizmo[4] = (float)m_to(0, 1);
-                    m_gizmo[5] = (float)m_to(1, 1);
-                    m_gizmo[6] = (float)m_to(2, 1);
-                    m_gizmo[7] = (float)m_to(3, 1);
-                    m_gizmo[8] = (float)m_to(0, 2);
-                    m_gizmo[9] = (float)m_to(1, 2);
-                    m_gizmo[10] = (float)m_to(2, 2);
-                    m_gizmo[11] = (float)m_to(3, 2);
-                    m_gizmo[12] = (float)m_to(0, 3);
-                    m_gizmo[13] = (float)m_to(1, 3);
-                    m_gizmo[14] = (float)m_to(2, 3);
-                    m_gizmo[15] = (float)m_to(3, 3);
-                }
+                    setGizmoFromActiveEdge();
                 if (!edge_gizmo)
                 {
                     if (ImGui::Button("ICP"))
@@ -1787,16 +1835,15 @@ bool loadProject(const std::string& file_name, ProjectSettings& _project_setting
         }
 
         std::cout << "Found " << edges.size() << "edges\nOpening done\n";
-
-        return true;
     } catch (std::exception& e)
     {
         std::cout << "can't load project settings: " << e.what() << std::endl;
         return false;
     }
 
+    project_file_name = file_name;
     std::string newTitle = winTitle + " - " + truncPath(file_name);
-    glutSetWindowTitle(newTitle.c_str());
+    SetWindowTitle(newTitle.c_str());
 
     loaded_sessions = false;
     time_stamp_offset = 0.0;
@@ -1815,17 +1862,31 @@ void openProject()
     }
 }
 
-void saveProject()
+void saveProjectTo(const std::string& output_file_name)
+{
+    if (save_project_settings(fs::path(output_file_name).string(), project_settings))
+    {
+        project_file_name = output_file_name;
+        std::string newTitle = winTitle + " - " + truncPath(output_file_name);
+        SetWindowTitle(newTitle.c_str());
+    }
+}
+
+void saveProjectAs()
 {
     std::string output_file_name = "";
     output_file_name = mandeye::fd::SaveFileDialog("Save project file", mandeye::fd::Project_filter, ".mjp", "project");
 
     if (output_file_name.size() > 0)
-        if (save_project_settings(fs::path(output_file_name).string(), project_settings))
-        {
-            std::string newTitle = winTitle + " - " + truncPath(output_file_name);
-            glutSetWindowTitle(newTitle.c_str());
-        }
+        saveProjectTo(output_file_name);
+}
+
+void saveProject()
+{
+    if (project_file_name.empty())
+        saveProjectAs();
+    else
+        saveProjectTo(project_file_name);
 }
 
 void addSession()
@@ -1845,10 +1906,82 @@ void addSession()
     }
 }
 
-void loadSessions()
+void appendSession(const std::string& ps);
+void finishLoadingSessions();
+void loadSessions();
+
+// Adds a session file to the project unless it is already there; returns whether it was added.
+bool addSessionFile(const std::string& path)
+{
+    const auto& names = project_settings.session_file_names;
+    if (std::find(names.begin(), names.end(), path) != names.end())
+    {
+        std::cout << "Session file already in project: '" << path << "'" << std::endl;
+        return false;
+    }
+    std::cout << "Adding session file: '" << path << "'" << std::endl;
+    project_settings.session_file_names.push_back(path);
+    return true;
+}
+
+// Drag & drop: a project (*.mjp) replaces the current one; session files (*.mjs, *.json) are added and loaded
+// right away (appended to already loaded sessions, which keep their in-memory state).
+void loadDroppedFiles()
+{
+    FilePathList dropped_files = LoadDroppedFiles();
+    std::vector<fs::path> paths(dropped_files.paths, dropped_files.paths + dropped_files.count);
+    UnloadDroppedFiles(dropped_files);
+
+    auto ext_of = [](const fs::path& p)
+    {
+        std::string ext = p.extension().string();
+        std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower);
+        return ext;
+    };
+
+    for (const auto& path : paths)
+    {
+        if (ext_of(path) == ".mjp")
+        {
+            loadProject(path.string(), project_settings);
+            return;
+        }
+    }
+
+    std::vector<std::string> added;
+    for (const auto& path : paths)
+    {
+        const std::string ext = ext_of(path);
+        if (ext != ".mjs" && ext != ".json")
+        {
+            std::cout << "Ignoring dropped file: '" << path.string() << "'" << std::endl;
+            continue;
+        }
+        if (addSessionFile(path.string()))
+            added.push_back(path.string());
+    }
+
+    if (added.empty())
+    {
+        pfd::message("Unsupported file", "Drop a project (*.mjp) or session files (*.mjs, *.json).", pfd::choice::ok, pfd::icon::warning);
+        return;
+    }
+
+    if (loaded_sessions)
+    {
+        for (const auto& ps : added)
+            appendSession(ps);
+        finishLoadingSessions();
+    }
+    else
+    {
+        time_stamp_offset = 0.0;
+        loadSessions();
+    }
+}
+
+void appendSession(const std::string& ps)
 {
-    sessions.clear();
-    for (const auto& ps : project_settings.session_file_names)
     {
         Session session;
         session.load(fs::path(ps).string(), is_decimate, bucket_x, bucket_y, bucket_z, calculate_offset);
@@ -1871,8 +2004,11 @@ void loadSessions()
         if (session.is_ground_truth)
             index_gt = sessions.size() - 1;
     }
-    loaded_sessions = true;
+}
 
+// Reorders (ground truth first), recolors and re-uploads the loaded sessions.
+void finishLoadingSessions()
+{
     // reorder
     std::vector<Session> sessions_reorder;
     std::vector<std::string> session_file_names_reordered;
@@ -1913,6 +2049,9 @@ void loadSessions()
         std::cout << "session: '" << s.session_file_name << "' ground truth [" << int(s.is_ground_truth) << "]" << std::endl;
     }
 
+    assignDistinctSessionColors(sessions);
+    invalidateSessionRenderers();
+
     // update time_stamp_offset
     std::cout << "update time_stamp_offset" << std::endl;
     for (const auto& s : sessions)
@@ -1930,6 +2069,16 @@ void loadSessions()
     }
 }
 
+void loadSessions()
+{
+    sessions.clear();
+    for (const auto& ps : project_settings.session_file_names)
+        appendSession(ps);
+    loaded_sessions = true;
+
+    finishLoadingSessions();
+}
+
 void generate_loop_closures(const std::vector<Session>& sessions, std::vector<Edge>& edges)
 {
     edges.clear();
@@ -2539,15 +2688,16 @@ void settings_gui()
 
 void display()
 {
+    syncSessionRenderers(sessions);
+
     ImGuiIO& io = ImGui::GetIO();
-    glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y);
+    rlViewport(0, 0, GetRenderWidth(), GetRenderHeight());
 
-    glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w);
-    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-    glEnable(GL_DEPTH_TEST);
+    ClearBackground(ColorFromNormalized(Vector4{ bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w }));
+    rlEnableDepthTest();
 
-    glMatrixMode(GL_PROJECTION);
-    glLoadIdentity();
+    rlMatrixMode(RL_PROJECTION);
+    rlLoadIdentity();
     float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y);
 
     updateCameraTransition();
@@ -2564,8 +2714,7 @@ void display()
 
     if (!is_ortho)
     {
-        reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y);
-        glTranslatef(translate_x, translate_y, translate_z);
+        reshape((int)io.DisplaySize.x, (int)io.DisplaySize.y);
 
         // janusz
         if (is_loop_closure_gui)
@@ -2642,10 +2791,11 @@ void display()
 
         viewLocal.translate(-rotation_center);
 
-        glLoadMatrixf(viewLocal.matrix().data());
+        rlMultMatrixf(viewLocal.matrix().data());
     }
     else
         updateOrthoView();
+    captureFrameMatrices();
 
     showAxes();
 
@@ -2698,7 +2848,9 @@ void display()
 
                         // sessions[first_session_index].point_clouds_container.point_clouds.at(i).point_size = gui_point_size;
 
-                        sessions[first_session_index].point_clouds_container.point_clouds.at(i).render(
+                        renderScanAtPose(
+                            first_session_index,
+                            i,
                             m_src,
                             viewer_decimate_point_cloud,
                             viewer_reduce_rendered_trajectory,
@@ -2723,7 +2875,9 @@ void display()
                         Eigen::Affine3d m_trg = _m_trg * (m_trg_0.inverse() * m_trg_curr);
 
                         // sessions[second_session_index].point_clouds_container.point_clouds.at(i).point_size = gui_point_size;
-                        sessions[second_session_index].point_clouds_container.point_clouds.at(i).render(
+                        renderScanAtPose(
+                            second_session_index,
+                            i,
                             m_trg,
                             viewer_decimate_point_cloud,
                             viewer_reduce_rendered_trajectory,
@@ -2760,7 +2914,9 @@ void display()
                     // false);
                     Eigen::Affine3d m_src = sessions[first_session_index].point_clouds_container.point_clouds.at(i).m_pose;
 
-                    sessions[first_session_index].point_clouds_container.point_clouds.at(i).render(
+                    renderScan(
+                        first_session_index,
+                        i,
                         false,
                         observation_picking,
                         viewer_decimate_point_cloud,
@@ -2797,7 +2953,9 @@ void display()
                     // false);
                     Eigen::Affine3d m_src = sessions[second_session_index].point_clouds_container.point_clouds.at(i).m_pose;
 
-                    sessions[second_session_index].point_clouds_container.point_clouds.at(i).render(
+                    renderScan(
+                        second_session_index,
+                        i,
                         false,
                         observation_picking,
                         viewer_decimate_point_cloud,
@@ -2813,37 +2971,37 @@ void display()
 
         // sessions[first_session_index].point_clouds_container.render();
 
-        glBegin(GL_LINE_STRIP);
+        beginLineStrip();
         for (auto& pc : sessions[first_session_index].point_clouds_container.point_clouds)
         {
-            glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]);
-            glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3));
+            color3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]);
+            lineStripVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3));
         }
-        glEnd();
+        endLineStrip();
 
         int i = 0;
         for (auto& pc : sessions[first_session_index].point_clouds_container.point_clouds)
         {
-            glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]);
-            glRasterPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1);
-            glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str());
+            color3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]);
+            labelPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1);
+            labelText(std::to_string(i).c_str());
             i++;
         }
 
-        glBegin(GL_LINE_STRIP);
+        beginLineStrip();
         for (auto& pc : sessions[second_session_index].point_clouds_container.point_clouds)
         {
-            glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]);
-            glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3));
+            color3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]);
+            lineStripVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3));
         }
-        glEnd();
+        endLineStrip();
 
         i = 0;
         for (auto& pc : sessions[second_session_index].point_clouds_container.point_clouds)
         {
-            glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]);
-            glRasterPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1);
-            glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str());
+            color3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]);
+            labelPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1);
+            labelText(std::to_string(i).c_str());
             i++;
         }
 
@@ -2854,19 +3012,19 @@ void display()
                 int index_src = sessions[i].pose_graph_loop_closure.edges[j].index_from;
                 int index_trg = sessions[i].pose_graph_loop_closure.edges[j].index_to;
 
-                glColor3f(0.0f, 0.0f, 1.0f);
-                glBegin(GL_LINES);
+                color3f(0.0f, 0.0f, 1.0f);
+                rlBegin(RL_LINES);
                 auto v1 = sessions[i].point_clouds_container.point_clouds.at(index_src).m_pose.translation();
                 auto v2 = sessions[i].point_clouds_container.point_clouds.at(index_trg).m_pose.translation();
-                glVertex3f(v1.x(), v1.y(), v1.z());
-                glVertex3f(v2.x(), v2.y(), v2.z());
+                rlVertex3f(v1.x(), v1.y(), v1.z());
+                rlVertex3f(v2.x(), v2.y(), v2.z());
 
-                glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5);
-                glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10);
-                glEnd();
+                rlVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5);
+                rlVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10);
+                rlEnd();
 
-                glRasterPos3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10 + 0.1);
-                glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(j).c_str());
+                labelPos3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10 + 0.1);
+                labelText(std::to_string(j).c_str());
             }
         }
 
@@ -2879,22 +3037,22 @@ void display()
             int index_session_to = edges[i].index_session_to;
 
             if (sessions[index_session_from].is_ground_truth || sessions[index_session_to].is_ground_truth)
-                glColor3f(0.0f, 1.0f, 1.0f);
+                color3f(0.0f, 1.0f, 1.0f);
             else
-                glColor3f(1.0f, 1.0f, 0.0f);
+                color3f(1.0f, 1.0f, 0.0f);
 
-            glBegin(GL_LINES);
+            rlBegin(RL_LINES);
             auto v1 = sessions[index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose.translation();
             auto v2 = sessions[index_session_to].point_clouds_container.point_clouds.at(index_trg).m_pose.translation();
-            glVertex3f(v1.x(), v1.y(), v1.z());
-            glVertex3f(v2.x(), v2.y(), v2.z());
+            rlVertex3f(v1.x(), v1.y(), v1.z());
+            rlVertex3f(v2.x(), v2.y(), v2.z());
 
-            glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5);
-            glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10);
-            glEnd();
+            rlVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5);
+            rlVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10);
+            rlEnd();
 
-            glRasterPos3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10 + 0.1);
-            glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str());
+            labelPos3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10 + 0.1);
+            labelText(std::to_string(i).c_str());
         }
     }
     else
@@ -2903,9 +3061,9 @@ void display()
         {
             if (session.visible)
             {
-                session.point_clouds_container.render(observation_picking, viewer_decimate_point_cloud, viewer_reduce_rendered_trajectory);
-                session.ground_control_points.render(session.point_clouds_container);
-                session.control_points.render(session.point_clouds_container, false);
+                renderSession(session, observation_picking, viewer_decimate_point_cloud, viewer_reduce_rendered_trajectory);
+                renderGroundControlPoints(session.ground_control_points, session.point_clouds_container);
+                renderControlPoints(session.control_points, session.point_clouds_container);
 
                 ////
                 int index_point_clouds = -1;
@@ -2932,11 +3090,11 @@ void display()
                 {
                     if (index_local_trajectory < session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory.size())
                     {
-                        glColor3f(
+                        color3f(
                             session.point_clouds_container.point_clouds[index_point_clouds].render_color[0],
                             session.point_clouds_container.point_clouds[index_point_clouds].render_color[1],
                             session.point_clouds_container.point_clouds[index_point_clouds].render_color[2]);
-                        glBegin(GL_LINES);
+                        rlBegin(RL_LINES);
 
                         auto m1 = session.point_clouds_container.point_clouds[index_point_clouds].m_pose;
                         auto m2 =
@@ -2944,16 +3102,16 @@ void display()
 
                         auto v1 = (m1 * m2).translation();
 
-                        glVertex3f(v1.x() - 5.0, v1.y(), v1.z());
-                        glVertex3f(v1.x() + 5.0, v1.y(), v1.z());
+                        rlVertex3f(v1.x() - 5.0, v1.y(), v1.z());
+                        rlVertex3f(v1.x() + 5.0, v1.y(), v1.z());
 
-                        glVertex3f(v1.x(), v1.y() - 5.0, v1.z());
-                        glVertex3f(v1.x(), v1.y() + 5.0, v1.z());
+                        rlVertex3f(v1.x(), v1.y() - 5.0, v1.z());
+                        rlVertex3f(v1.x(), v1.y() + 5.0, v1.z());
 
-                        glVertex3f(v1.x(), v1.y(), v1.z() - 5.0);
-                        glVertex3f(v1.x(), v1.y(), v1.z() + 5.0);
+                        rlVertex3f(v1.x(), v1.y(), v1.z() - 5.0);
+                        rlVertex3f(v1.x(), v1.y(), v1.z() + 5.0);
 
-                        glEnd();
+                        rlEnd();
                     }
                 }
             }
@@ -3023,9 +3181,7 @@ void display()
 
     // gnss.render(session.point_clouds_container);
 
-    ImGui_ImplOpenGL2_NewFrame();
-    ImGui_ImplGLUT_NewFrame();
-    ImGui::NewFrame();
+    rlImGuiBegin();
 
     ShowMainDockSpace();
 
@@ -3054,11 +3210,11 @@ void display()
 
                     if (!is_ortho)
                     {
-                        GLfloat projection[16];
-                        glGetFloatv(GL_PROJECTION_MATRIX, projection);
+                        float projection[16];
+                        getProjectionMatrix(projection);
 
-                        GLfloat modelview[16];
-                        glGetFloatv(GL_MODELVIEW_MATRIX, modelview);
+                        float modelview[16];
+                        getModelviewMatrix(modelview);
 
                         ImGuizmo::Manipulate(
                             modelview,
@@ -3192,11 +3348,11 @@ void display()
 
             if (!is_ortho)
             {
-                GLfloat projection[16];
-                glGetFloatv(GL_PROJECTION_MATRIX, projection);
+                float projection[16];
+                getProjectionMatrix(projection);
 
-                GLfloat modelview[16];
-                glGetFloatv(GL_MODELVIEW_MATRIX, modelview);
+                float modelview[16];
+                getModelviewMatrix(modelview);
 
                 ImGuizmo::Manipulate(
                     &modelview[0],
@@ -3438,6 +3594,9 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g);
 
     view_kbd_shortcuts();
 
+    if (IsFileDropped())
+        loadDroppedFiles();
+
     if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_A, false))
     {
         addSession();
@@ -3477,9 +3636,13 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g);
     if (sessions.size() > 0)
         if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_S, false))
         {
-            saveProject();
+            if (io.KeyShift)
+                saveProjectAs();
+            else
+                saveProject();
 
             // workaround
+            io.AddKeyEvent(ImGuiMod_Shift, false);
             io.AddKeyEvent(ImGuiKey_S, false);
             io.AddKeyEvent(ImGuiMod_Ctrl, false);
         }
@@ -3492,6 +3655,10 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g);
                 openProject();
             if (ImGui::MenuItem("Save project", "Ctrl+S", nullptr, project_settings.session_file_names.size() > 0))
                 saveProject();
+            if (ImGui::MenuItem("Save project as...", "Ctrl+Shift+S", nullptr, project_settings.session_file_names.size() > 0))
+                saveProjectAs();
+            if (ImGui::IsItemHovered() && !project_file_name.empty())
+                ImGui::SetTooltip("Current project: %s", project_file_name.c_str());
 
             ImGui::Separator();
 
@@ -4088,6 +4255,7 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g);
             ImGui::Text("Colors:");
 
             ImGui::ColorEdit3("Background", (float*)&bg_color, ImGuiColorEditFlags_NoInputs);
+            pointsColorMenu();
 
             ImGui::Separator();
 
@@ -4228,6 +4396,8 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g);
 
     info_window(infoLines, appShortcuts);
 
+    end3DAndDrawLabels();
+
     if (compass_ruler)
         drawMiniCompassWithRuler();
 
@@ -4248,32 +4418,12 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g);
     if (is_settings_gui)
         settings_gui();
 
-    ImGui::Render();
-    ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData());
-
-    glutSwapBuffers();
-    glutPostRedisplay();
+    rlImGuiEnd();
 }
 
 void mouse(int glut_button, int state, int x, int y)
 {
     ImGuiIO& io = ImGui::GetIO();
-    io.MousePos = ImVec2((float)x, (float)y);
-    int button = -1;
-    if (glut_button == GLUT_LEFT_BUTTON)
-        button = 0;
-    if (glut_button == GLUT_RIGHT_BUTTON)
-        button = 1;
-    if (glut_button == GLUT_MIDDLE_BUTTON)
-        button = 2;
-    if (button != -1 && state == GLUT_DOWN)
-        io.MouseDown[button] = true;
-    if (button != -1 && state == GLUT_UP)
-        io.MouseDown[button] = false;
-
-    static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000;
-    if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && glutMajorVersion < 3)
-        wheel(glut_button, glut_button == 3 ? 1 : -1, x, y);
 
     if (!io.WantCaptureMouse)
     {
@@ -4344,9 +4494,12 @@ int main(int argc, char* argv[])
         {
             std::cout << winTitle << "\n\n"
                       << "USAGE:\n"
-                      << std::filesystem::path(argv[0]).stem().string() << " <input_file> /?\n\n"
+                      << std::filesystem::path(argv[0]).stem().string()
+                      << " [--mjp <project.mjp>] [--mjs <session.mjs> ...] [<input_file> ...] /?\n\n"
                       << "where\n"
-                      << "   <input_file>         Path to Mandeye JSON Project file (*.mjp)\n"
+                      << "   --mjp <project.mjp>  Mandeye JSON Project file to open\n"
+                      << "   --mjs <file> [...]   Session file(s) (*.mjs, *.json) to add to the project and load\n"
+                      << "   <input_file>         *.mjp opens a project, *.mjs / *.json adds a session\n"
                       << "   -h, /h, --help, /?   Show this help and exit\n\n";
 
             return 0;
@@ -4354,27 +4507,54 @@ int main(int argc, char* argv[])
 
         initGL(&argc, argv, winTitle, display, mouse);
 
-        if (argc > 1)
+        // --mjp / --mjs take the following non-flag arguments; bare arguments are classified by extension.
+        std::string project_file;
+        std::vector<std::string> session_files;
+        std::string current_flag;
+        for (int i = 1; i < argc; i++)
         {
-            for (int i = 1; i < argc; i++)
+            const std::string arg(argv[i]);
+            if (arg == "--mjp" || arg == "--mjs")
+            {
+                current_flag = arg;
+                continue;
+            }
+            if (arg.rfind("--", 0) == 0)
             {
-                std::string ext = fs::path(argv[i]).extension().string();
-                std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower);
+                std::cerr << "Unknown option: '" << arg << "'" << std::endl;
+                current_flag.clear();
+                continue;
+            }
 
-                if (ext == ".mjp")
-                {
-                    loadProject(argv[i], project_settings);
+            std::string ext = fs::path(arg).extension().string();
+            std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower);
 
-                    break;
-                }
+            if (current_flag == "--mjp" || (current_flag.empty() && ext == ".mjp"))
+            {
+                if (project_file.empty())
+                    project_file = arg;
+                else
+                    std::cerr << "Only one project can be opened, ignoring: '" << arg << "'" << std::endl;
+                current_flag.clear();
             }
+            else if (current_flag == "--mjs" || ext == ".mjs" || ext == ".json")
+                session_files.push_back(arg);
+            else
+                std::cerr << "Ignoring argument: '" << arg << "'" << std::endl;
         }
 
-        glutMainLoop();
+        if (!project_file.empty())
+            loadProject(project_file, project_settings);
+
+        bool added = false;
+        for (const auto& session_file : session_files)
+            added |= addSessionFile(session_file);
+        if (added)
+            loadSessions();
+
+        mainLoop();
 
-        ImGui_ImplOpenGL2_Shutdown();
-        ImGui_ImplGLUT_Shutdown();
-        ImGui::DestroyContext();
+        shutdownGL();
     } catch (const std::bad_alloc& e)
     {
         std::cerr << "System is out of memory : " << e.what() << std::endl;
diff --git a/apps/multi_session_registration/raylib_utils.cpp b/apps/multi_session_registration/raylib_utils.cpp
new file mode 100644
index 00000000..c7b8a55c
--- /dev/null
+++ b/apps/multi_session_registration/raylib_utils.cpp
@@ -0,0 +1,759 @@
+#include "raylib_utils.h"
+
+#include "raymath.h"
+#include "rlImGui.h"
+
+#include <imgui_internal.h>
+
+#include <RaylibWidgets/CenterOfRotationWindow.h>
+#include <RaylibWidgets/CompassRuler.h>
+#include <RaylibWidgets/PointPicking.h>
+#include <RaylibWidgets/RayPlaneD.h>
+#include <RaylibWidgets/WindowFit.h>
+
+#include <HDMapping/Version.hpp>
+
+#include <algorithm>
+#include <filesystem>
+#include <iostream>
+#include <memory>
+#include <random>
+
+raylib_widgets::OrbitCamera camera;
+
+// GLUT step 3 used 1000 (every point was a glVertex call); GPU buffers draw full density, as in step 2.
+int viewer_decimate_point_cloud = 2;
+int mouse_old_x = 0, mouse_old_y = 0;
+int mouse_buttons = 0;
+bool& is_ortho = camera.isOrtho;
+bool& lock_z = camera.lockZ;
+bool show_axes = true;
+ImVec4 bg_color = ImVec4(0.65f, 0.65f, 0.65f, 1.00f);
+int point_size = 1;
+bool info_gui = false;
+bool compass_ruler = true;
+bool cor_gui = false;
+Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity();
+
+Eigen::Map<Eigen::Vector3f> rotation_center(&camera.euler.rotationCenter.x);
+float& rotate_x = camera.euler.rotateX;
+float& rotate_y = camera.euler.rotateY;
+float& translate_x = camera.euler.translate.x;
+float& translate_y = camera.euler.translate.y;
+float& translate_z = camera.euler.translate.z;
+Eigen::Map<Eigen::Vector3f> new_rotation_center(&camera.eulerGoal.rotationCenter.x);
+float& new_rotate_x = camera.eulerGoal.rotateX;
+float& new_rotate_y = camera.eulerGoal.rotateY;
+float& new_translate_x = camera.eulerGoal.translate.x;
+float& new_translate_y = camera.eulerGoal.translate.y;
+float& new_translate_z = camera.eulerGoal.translate.z;
+bool& camera_transition_active = camera.eulerTransitionActive;
+float* const m_ortho_projection = camera.orthoProjection;
+float* const m_ortho_gizmo_view = camera.orthoGizmoView;
+
+namespace
+{
+    void (*display_cb)() = nullptr;
+    void (*mouse_cb)(int, int, int, int) = nullptr;
+
+    Matrix frame_mvp{};
+    float current_color[3] = { 1.f, 1.f, 1.f };
+
+    struct StripVertex
+    {
+        Vector3 p;
+        float c[3];
+    };
+    std::vector<StripVertex> strip;
+
+    struct Label
+    {
+        Vector3 p;
+        std::string text;
+        Color color;
+    };
+    Vector3 label_pos{};
+    float label_color[3] = { 1.f, 1.f, 1.f };
+    std::vector<Label> labels;
+
+    std::vector<std::unique_ptr<ScanRenderer>> renderers;
+    bool renderers_valid = false;
+    const Session* renderers_base = nullptr;
+    PointClouds::PointCloudDimensions scene_dims{ 0, 0, 0, 0, 0, 1, 1, 1, 1 };
+    ScanColorMode color_mode = ScanColorMode::FlatIntensity;
+
+    Vector3 toVec3(const Eigen::Vector3d& v)
+    {
+        return Vector3{ static_cast<float>(v.x()), static_cast<float>(v.y()), static_cast<float>(v.z()) };
+    }
+
+    Vector2 worldToScreen(const Vector3& w)
+    {
+        const ImGuiIO& io = ImGui::GetIO();
+        const Matrix& m = frame_mvp;
+        float cx = m.m0 * w.x + m.m4 * w.y + m.m8 * w.z + m.m12;
+        float cy = m.m1 * w.x + m.m5 * w.y + m.m9 * w.z + m.m13;
+        float cw = m.m3 * w.x + m.m7 * w.y + m.m11 * w.z + m.m15;
+        if (cw < 1e-6f) // behind the camera
+            return Vector2{ -1000.f, -1000.f };
+        return Vector2{ (cx / cw * 0.5f + 0.5f) * io.DisplaySize.x, (0.5f - cy / cw * 0.5f) * io.DisplaySize.y };
+    }
+
+    void copyMatrix(const Matrix& m, float out[16])
+    {
+        const float v[16] = { m.m0, m.m1, m.m2, m.m3, m.m4, m.m5, m.m6, m.m7, m.m8, m.m9, m.m10, m.m11, m.m12, m.m13, m.m14, m.m15 };
+        std::copy(v, v + 16, out);
+    }
+
+    ScanRenderer* rendererOf(int session_index)
+    {
+        return session_index >= 0 && session_index < static_cast<int>(renderers.size()) ? renderers[session_index].get() : nullptr;
+    }
+} // namespace
+
+std::string truncPath(const std::string& fullPath)
+{
+    std::filesystem::path path(fullPath);
+    return "..\\" + path.parent_path().parent_path().filename().string() + "\\" + path.parent_path().filename().string() + "\\" +
+        path.filename().string();
+}
+
+void wheel(int, int, int, int)
+{
+    if (!ImGui::IsWindowHovered(ImGuiHoveredFlags_AnyWindow))
+        camera.zoom(GetMouseWheelMove(), ImGui::GetIO().KeyShift);
+}
+
+void motion(int x, int y)
+{
+    ImGuiIO& io = ImGui::GetIO();
+    if (io.WantCaptureMouse)
+        return;
+
+    float dx = static_cast<float>(x - mouse_old_x);
+    float dy = static_cast<float>(y - mouse_old_y);
+
+    // Ctrl/Shift+click picks a rotation center; don't let a stray drag break that transition.
+    if (!io.KeyCtrl && !io.KeyShift)
+    {
+        if (mouse_buttons & 1)
+            camera.dragOrbit(dx, dy);
+        if (mouse_buttons & 4)
+        {
+            if (camera.isOrtho)
+                camera.dragPanOrtho(dx, dy, io.DisplaySize.x, io.DisplaySize.y);
+            else
+                camera.dragPanPerspective(dx, dy);
+        }
+    }
+
+    mouse_old_x = x;
+    mouse_old_y = y;
+}
+
+void reshape(int w, int h)
+{
+    camera.applyPerspectiveProjection(w, h);
+}
+
+bool initGL(int*, char**, const std::string& winTitle, void (*display)(), void (*mouse)(int, int, int, int))
+{
+    display_cb = display;
+    mouse_cb = mouse;
+
+    // HiDPI breaks ImGui scaling on Windows, so only macOS/Linux enable it (as in step 2).
+    unsigned int flags = FLAG_WINDOW_RESIZABLE;
+#if defined(__APPLE__) || defined(__linux__)
+    flags |= FLAG_WINDOW_HIGHDPI;
+#endif
+    SetConfigFlags(flags);
+    InitWindow(1600, 900, winTitle.c_str());
+    SetExitKey(KEY_NULL); // Esc must not close the window
+    SetTargetFPS(60);
+    raylib_widgets::fitWindowToScreen(100, 100, true);
+
+    rlImGuiSetup(true);
+    ImGuiIO& io = ImGui::GetIO();
+    io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard | ImGuiConfigFlags_NavEnableGamepad | ImGuiConfigFlags_DockingEnable;
+    io.ConfigDockingWithShift = true;
+    return true;
+}
+
+void mainLoop()
+{
+    const int buttons[][2] = { { MOUSE_BUTTON_LEFT, GLUT_LEFT_BUTTON },
+                               { MOUSE_BUTTON_MIDDLE, GLUT_MIDDLE_BUTTON },
+                               { MOUSE_BUTTON_RIGHT, GLUT_RIGHT_BUTTON } };
+    while (!WindowShouldClose())
+    {
+        int x = GetMouseX();
+        int y = GetMouseY();
+        for (const auto& b : buttons)
+        {
+            if (IsMouseButtonPressed(b[0]))
+                mouse_cb(b[1], GLUT_DOWN, x, y);
+            if (IsMouseButtonReleased(b[0]))
+                mouse_cb(b[1], GLUT_UP, x, y);
+        }
+        motion(x, y);
+        if (GetMouseWheelMove() != 0.0f)
+            wheel(0, GetMouseWheelMove() > 0.0f ? 1 : -1, x, y);
+
+        BeginDrawing();
+        display_cb();
+        EndDrawing();
+    }
+}
+
+void shutdownGL()
+{
+    renderers.clear(); // GPU buffers must go while the GL context exists
+    rlImGuiShutdown();
+    CloseWindow();
+}
+
+void showAxes()
+{
+    if (!show_axes && !ImGui::GetIO().KeyCtrl)
+        return;
+
+    const Vector3 rc = camera.euler.rotationCenter;
+    rlBegin(RL_LINES);
+    rlColor3f(1.f, 1.f, 1.f);
+    for (int axis = 0; axis < 3; axis++)
+    {
+        Vector3 d{ axis == 0 ? 1.f : 0.f, axis == 1 ? 1.f : 0.f, axis == 2 ? 1.f : 0.f };
+        rlVertex3f(rc.x - d.x, rc.y - d.y, rc.z - d.z);
+        rlVertex3f(rc.x + d.x, rc.y + d.y, rc.z + d.z);
+    }
+    rlColor3f(1.f, 0.f, 0.f);
+    rlVertex3f(0, 0, 0);
+    rlVertex3f(100, 0, 0);
+    rlColor3f(0.f, 1.f, 0.f);
+    rlVertex3f(0, 0, 0);
+    rlVertex3f(0, 100, 0);
+    rlColor3f(0.f, 0.f, 1.f);
+    rlVertex3f(0, 0, 0);
+    rlVertex3f(0, 0, 100);
+    rlEnd();
+}
+
+void updateCameraTransition()
+{
+    camera.updateEulerTransition(GetFrameTime());
+}
+
+void updateOrthoView()
+{
+    const ImGuiIO& io = ImGui::GetIO();
+    viewLocal.rotate(Eigen::AngleAxisf((camera.euler.rotateX + camera.euler.rotateY) * DEG_TO_RAD, Eigen::Vector3f::UnitZ()));
+    camera.updateOrtho(io.DisplaySize.x / io.DisplaySize.y);
+}
+
+void camMenu()
+{
+    using P = raylib_widgets::OrbitCamera::EulerPreset;
+    if (ImGui::BeginMenu("Camera"))
+    {
+        const std::pair<const char*, P> items[] = { { "Front (yz view)", P::Front }, { "Back", P::Back },
+                                                    { "Left (xz view)", P::Left },   { "Right", P::Right },
+                                                    { "Top (xy view)", P::Top },     { "Bottom", P::Bottom },
+                                                    { "Isometric", P::Iso } };
+        const char* keys[] = { "key F", "key B", "key L", "key R", "key T", "key U", "key I" };
+        for (size_t i = 0; i < 7; i++)
+            if (ImGui::MenuItem(items[i].first, keys[i]))
+                camera.setEulerPreset(items[i].second);
+        ImGui::Separator();
+        if (ImGui::MenuItem("Reset", "key Z"))
+        {
+            camera.setEulerPreset(P::Reset);
+            viewer_decimate_point_cloud = 2;
+        }
+        ImGui::EndMenu();
+    }
+    if (ImGui::IsItemHovered())
+        ImGui::SetTooltip("Change camera view to fixed positions");
+}
+
+void view_kbd_shortcuts()
+{
+    using P = raylib_widgets::OrbitCamera::EulerPreset;
+    ImGuiIO& io = ImGui::GetIO();
+    if (io.WantCaptureKeyboard)
+        return;
+
+    const float step = 0.5f * camera.eulerMouseSensitivity;
+    const std::pair<ImGuiKey, Vector2> arrows[] = { { ImGuiKey_RightArrow, { 1, 0 } },
+                                                    { ImGuiKey_LeftArrow, { -1, 0 } },
+                                                    { ImGuiKey_UpArrow, { 0, 1 } },
+                                                    { ImGuiKey_DownArrow, { 0, -1 } } };
+    for (const auto& [key, d] : arrows)
+    {
+        if (!ImGui::IsKeyPressed(key, true))
+            continue;
+        if (io.KeyShift)
+        {
+            camera.euler.translate.x += d.x * step;
+            camera.euler.translate.y += d.y * step;
+            camera.breakEulerTransition();
+        }
+        else if (io.KeyCtrl)
+        {
+            camera.euler.rotateY -= d.x * 0.6f;
+            camera.euler.rotateX -= d.y * 0.6f;
+            camera.breakEulerTransition();
+        }
+    }
+
+    if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_R, false))
+        cor_gui = true;
+    if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_Z, false) && !camera.isOrtho)
+        camera.lockZ = !camera.lockZ;
+
+    if (io.KeyCtrl || io.KeyAlt || io.KeyShift)
+        return;
+
+    const std::pair<ImGuiKey, P> presets[] = { { ImGuiKey_B, P::Back },  { ImGuiKey_F, P::Front }, { ImGuiKey_I, P::Iso },
+                                               { ImGuiKey_L, P::Left },  { ImGuiKey_R, P::Right }, { ImGuiKey_T, P::Top },
+                                               { ImGuiKey_U, P::Bottom } };
+    for (const auto& [key, preset] : presets)
+        if (ImGui::IsKeyPressed(key))
+            camera.setEulerPreset(preset);
+    if (ImGui::IsKeyPressed(ImGuiKey_Z))
+    {
+        camera.setEulerPreset(P::Reset);
+        viewer_decimate_point_cloud = 2;
+    }
+
+    if (ImGui::IsKeyPressed(ImGuiKey_C, false))
+        compass_ruler = !compass_ruler;
+    if (ImGui::IsKeyPressed(ImGuiKey_O, false))
+        camera.isOrtho = !camera.isOrtho;
+    if (ImGui::IsKeyPressed(ImGuiKey_X, false))
+        show_axes = !show_axes;
+    for (int k = 1; k <= 9; k++)
+        if (ImGui::IsKeyPressed(static_cast<ImGuiKey>(ImGuiKey_0 + k)))
+            point_size = k;
+}
+
+void cor_window()
+{
+    raylib_widgets::showEulerCenterOfRotationWindow(cor_gui, camera, xText, yText, zText);
+}
+
+void info_window(const std::vector<std::string>& infoLines, const std::vector<ShortcutEntry>& appShortcuts)
+{
+    raylib_widgets::ShowInfoWindow(info_gui, infoLines, appShortcuts, HDMAPPING_VERSION_STRING, __DATE__);
+}
+
+void drawMiniCompassWithRuler()
+{
+    // Rows 0/1 of the world-to-eye rotation are the world directions of screen right/up.
+    const Eigen::Matrix3f& R = viewLocal.rotation();
+    Color rulerColor = ColorFromNormalized(Vector4{ 1.0f - bg_color.x, 1.0f - bg_color.y, 1.0f - bg_color.z, 1.0f });
+    raylib_widgets::drawCompassRuler(
+        Vector3{ R(0, 0), R(0, 1), R(0, 2) },
+        Vector3{ R(1, 0), R(1, 1), R(1, 2) },
+        camera.euler.translate.z,
+        rulerColor,
+        raylib_widgets::CompassAxisLabels{ "X (long.)", "Y (lat.)", "Z (vert.)" });
+}
+
+Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane)
+{
+    Eigen::Vector3d hit = laser_beam.position;
+    raylib_widgets::intersectPlane(laser_beam.position, laser_beam.direction, plane.a, plane.b, plane.c, plane.d, hit);
+    return hit;
+}
+
+LaserBeam GetLaserBeam(int x, int y)
+{
+    Ray ray = camera.eulerScreenRay(x, y, GetScreenWidth(), GetScreenHeight());
+    LaserBeam laser_beam;
+    laser_beam.position = Eigen::Vector3d(ray.position.x, ray.position.y, ray.position.z);
+    laser_beam.direction = Eigen::Vector3d(ray.direction.x, ray.direction.y, ray.direction.z);
+    return laser_beam;
+}
+
+double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line)
+{
+    return raylib_widgets::distancePointToLine(point, line.position, line.direction);
+}
+
+// Same behavior as utils.cpp: Ctrl picks the loop closure source (and time_stamp_offset), Shift the target;
+// with more than two visible sessions all visible ones are searched. Moves the rotation center to the pick.
+void getClosestTrajectoriesPoint(
+    std::vector<Session>& sessions,
+    int x,
+    int y,
+    const int first_session_index,
+    const int second_session_index,
+    const int number_visible_sessions,
+    int& index_loop_closure_source,
+    int& index_loop_closure_target,
+    bool KeyShift,
+    double& time_stamp_offset)
+{
+    const auto laser_beam = GetLaserBeam(x, y);
+    double min_distance = std::numeric_limits<double>::max();
+    Vector3 center = camera.eulerGoal.rotationCenter;
+
+    auto visit = [&](int s, bool source_target)
+    {
+        if (s < 0 || s >= static_cast<int>(sessions.size()))
+            return;
+        const auto& pcs = sessions[s].point_clouds_container.point_clouds;
+        for (size_t i = 0; i < pcs.size(); i++)
+            for (size_t j = 0; j < pcs[i].local_trajectory.size(); j++)
+            {
+                Eigen::Vector3d vp = pcs[i].m_pose * pcs[i].local_trajectory[j].m_pose.translation();
+                double dist = distance_point_to_line(vp, laser_beam);
+                if (dist >= min_distance)
+                    continue;
+                min_distance = dist;
+                if (source_target && KeyShift)
+                {
+                    index_loop_closure_target = static_cast<int>(i);
+                    continue;
+                }
+                if (source_target)
+                    index_loop_closure_source = static_cast<int>(i);
+                center = toVec3(vp);
+                time_stamp_offset = pcs[i].local_trajectory[j].timestamps.first;
+            }
+    };
+
+    if (number_visible_sessions == 1)
+        visit(first_session_index, true);
+    else if (number_visible_sessions == 2)
+        visit(KeyShift ? second_session_index : first_session_index, true);
+    else
+        for (size_t s = 0; s < sessions.size(); s++)
+            if (sessions[s].visible)
+                visit(static_cast<int>(s), false);
+
+    camera.moveEulerRotationCenterTo(center);
+}
+
+void setNewRotationCenter(int x, int y)
+{
+    RegistrationPlaneFeature::Plane pl;
+    pl.a = 0;
+    pl.b = 0;
+    pl.c = 1;
+    pl.d = 0;
+    Eigen::Vector3d c = rayIntersection(GetLaserBeam(x, y), pl);
+    std::cout << "Setting new rotation center to:\n" << c << std::endl;
+    camera.moveEulerRotationCenterTo(toVec3(c));
+}
+
+bool checkClHelp(int argc, char** argv)
+{
+    for (int i = 1; i < argc; ++i)
+    {
+        std::string arg(argv[i]);
+        if (arg == "-h" || arg == "/h" || arg == "--help" || arg == "/?")
+            return true;
+    }
+    return false;
+}
+
+void getProjectionMatrix(float m[16])
+{
+    copyMatrix(rlGetMatrixProjection(), m);
+}
+
+void getModelviewMatrix(float m[16])
+{
+    copyMatrix(rlGetMatrixModelview(), m);
+}
+
+void captureFrameMatrices()
+{
+    camera.captureFrameMatrices();
+    frame_mvp = MatrixMultiply(camera.frameView3D, camera.frameProj3D);
+}
+
+void color3f(float r, float g, float b)
+{
+    current_color[0] = r;
+    current_color[1] = g;
+    current_color[2] = b;
+    rlColor3f(r, g, b);
+}
+
+void beginLineStrip()
+{
+    strip.clear();
+}
+
+void lineStripVertex3f(float x, float y, float z)
+{
+    strip.push_back({ Vector3{ x, y, z }, { current_color[0], current_color[1], current_color[2] } });
+}
+
+void endLineStrip()
+{
+    rlBegin(RL_LINES);
+    for (size_t i = 1; i < strip.size(); i++)
+        for (const auto* v : { &strip[i - 1], &strip[i] })
+        {
+            rlColor3f(v->c[0], v->c[1], v->c[2]);
+            rlVertex3f(v->p.x, v->p.y, v->p.z);
+        }
+    rlEnd();
+    strip.clear();
+}
+
+void labelPos3f(float x, float y, float z)
+{
+    label_pos = Vector3{ x, y, z };
+    std::copy(current_color, current_color + 3, label_color);
+}
+
+void labelText(const std::string& text)
+{
+    labels.push_back({ label_pos, text, ColorFromNormalized(Vector4{ label_color[0], label_color[1], label_color[2], 1.f }) });
+}
+
+void end3DAndDrawLabels()
+{
+    const ImGuiIO& io = ImGui::GetIO();
+    raylib_widgets::end3DMatrixStack(io.DisplaySize.x, io.DisplaySize.y);
+
+    for (const auto& l : labels)
+    {
+        // Outlined, so labels stay readable over geometry of the same color.
+        Vector2 s = worldToScreen(l.p);
+        int x = static_cast<int>(s.x) + 6;
+        int y = static_cast<int>(s.y) - 26;
+        for (int dx = -1; dx <= 1; ++dx)
+            for (int dy = -1; dy <= 1; ++dy)
+                if (dx != 0 || dy != 0)
+                    DrawText(l.text.c_str(), x + dx, y + dy, 20, BLACK);
+        DrawText(l.text.c_str(), x, y, 20, l.color);
+    }
+    labels.clear();
+}
+
+void syncSessionRenderers(const std::vector<Session>& sessions)
+{
+    if (renderers_valid && renderers.size() == sessions.size() && renderers_base == sessions.data())
+    {
+        for (size_t i = 0; i < sessions.size(); i++)
+            renderers[i]->syncPoses(sessions[i].point_clouds_container.point_clouds);
+        return;
+    }
+
+    renderers.clear();
+    bool first = true;
+    for (const auto& s : sessions)
+    {
+        auto renderer = std::make_unique<ScanRenderer>();
+        renderer->init();
+        renderer->rebuildAll(s.point_clouds_container.point_clouds);
+        renderers.push_back(std::move(renderer));
+
+        if (s.point_clouds_container.point_clouds.empty())
+            continue;
+        const auto d = s.point_clouds_container.compute_point_cloud_dimension();
+        if (first)
+            scene_dims = d;
+        scene_dims.x_min = std::min(scene_dims.x_min, d.x_min);
+        scene_dims.x_max = std::max(scene_dims.x_max, d.x_max);
+        scene_dims.y_min = std::min(scene_dims.y_min, d.y_min);
+        scene_dims.y_max = std::max(scene_dims.y_max, d.y_max);
+        scene_dims.z_min = std::min(scene_dims.z_min, d.z_min);
+        scene_dims.z_max = std::max(scene_dims.z_max, d.z_max);
+        first = false;
+    }
+    scene_dims.length = scene_dims.x_max - scene_dims.x_min;
+    scene_dims.width = scene_dims.y_max - scene_dims.y_min;
+    scene_dims.height = scene_dims.z_max - scene_dims.z_min;
+    renderers_base = sessions.data();
+    renderers_valid = true;
+}
+
+void invalidateSessionRenderers()
+{
+    renderers_valid = false;
+}
+
+void renderSession(const Session& session, const ObservationPicking&, int decimate, int reduce_trajectory)
+{
+    ScanRenderer* r = rendererOf(static_cast<int>(&session - renderers_base));
+    const auto& pcc = session.point_clouds_container;
+    if (!r || pcc.point_clouds.empty())
+        return;
+
+    const Vector3 rc = camera.euler.rotationCenter;
+    r->draw(
+        pcc.point_clouds,
+        static_cast<float>(pcc.point_clouds[0].point_size),
+        color_mode,
+        static_cast<float>(scene_dims.z_min),
+        static_cast<float>(scene_dims.z_max),
+        Eigen::Vector3d(rc.x, rc.y, rc.z),
+        static_cast<float>(std::max({ scene_dims.length, scene_dims.width, scene_dims.height, 1.0 })),
+        decimate,
+        pcc.xz_intersection,
+        pcc.yz_intersection,
+        pcc.xy_intersection,
+        static_cast<float>(pcc.intersection_width),
+        pcc.show_with_initial_pose);
+    r->drawTrajectories(
+        pcc.point_clouds,
+        reduce_trajectory,
+        pcc.show_imu_to_lio_diff,
+        pcc.xz_intersection,
+        pcc.yz_intersection,
+        pcc.xy_intersection,
+        pcc.show_with_initial_pose,
+        pcc.imu_to_lio_diff_scale);
+}
+
+void renderScan(
+    int session_index,
+    int index,
+    bool show_with_initial_pose,
+    const ObservationPicking&,
+    int decimate,
+    int reduce_trajectory,
+    bool,
+    bool,
+    bool,
+    double,
+    bool)
+{
+    ScanRenderer* r = rendererOf(session_index);
+    if (!r || index < 0 || index >= static_cast<int>(renderers_base[session_index].point_clouds_container.point_clouds.size()))
+        return;
+    const PointCloud& pc = renderers_base[session_index].point_clouds_container.point_clouds[index];
+    renderScanAtPose(
+        session_index, index, show_with_initial_pose ? pc.m_initial_pose : pc.m_pose, decimate, reduce_trajectory, pc.render_color);
+}
+
+void renderScanAtPose(int session_index, int index, const Eigen::Affine3d& pose, int, int reduce_trajectory, const float color[3])
+{
+    ScanRenderer* r = rendererOf(session_index);
+    if (!r || index < 0 || index >= static_cast<int>(renderers_base[session_index].point_clouds_container.point_clouds.size()))
+        return;
+    const PointCloud& pc = renderers_base[session_index].point_clouds_container.point_clouds[index];
+    if (!pc.visible)
+        return;
+
+    r->drawCachedWithTransform(
+        static_cast<size_t>(index),
+        pose * pc.m_pose.inverse(),
+        ColorFromNormalized(Vector4{ color[0], color[1], color[2], 1.f }),
+        static_cast<float>(pc.point_size),
+        false);
+
+    const size_t stride = std::max(1, reduce_trajectory);
+    rlBegin(RL_LINES);
+    rlColor3f(color[0], color[1], color[2]);
+    for (size_t i = stride; i < pc.local_trajectory.size(); i += stride)
+        for (size_t k : { i - stride, i })
+        {
+            Eigen::Vector3d p = (pc.m_pose * pc.local_trajectory[k].m_pose).translation();
+            rlVertex3f(static_cast<float>(p.x()), static_cast<float>(p.y()), static_cast<float>(p.z()));
+        }
+    rlEnd();
+}
+
+void renderGroundControlPoints(const GroundControlPoints& ground_control_points, const PointClouds& point_clouds_container)
+{
+    const Color mark{ 179, 77, 128, 255 };
+    const Color connector{ 0, 77, 153, 255 };
+    for (const auto& gcp : ground_control_points.gpcs)
+    {
+        if (gcp.index_to_node_inner < 0 || gcp.index_to_node_inner >= static_cast<int>(point_clouds_container.point_clouds.size()))
+            continue;
+        const auto& pc = point_clouds_container.point_clouds[gcp.index_to_node_inner];
+        if (gcp.index_to_node_outer < 0 || gcp.index_to_node_outer >= static_cast<int>(pc.local_trajectory.size()))
+            continue;
+
+        Vector3 c = toVec3(pc.m_pose * pc.local_trajectory[gcp.index_to_node_outer].m_pose.translation());
+        float h = static_cast<float>(gcp.lidar_height_above_ground);
+        Vector3 g{ static_cast<float>(gcp.x), static_cast<float>(gcp.y), static_cast<float>(gcp.z) };
+        DrawLine3D(Vector3{ g.x - 0.05f, g.y, g.z }, Vector3{ g.x + 0.05f, g.y, g.z }, mark);
+        DrawLine3D(Vector3{ g.x, g.y - 0.05f, g.z }, Vector3{ g.x, g.y + 0.05f, g.z }, mark);
+        DrawLine3D(g, Vector3{ g.x, g.y, g.z + h }, mark);
+        DrawLine3D(c, Vector3{ g.x, g.y, g.z + h }, connector);
+        labels.push_back({ Vector3{ g.x, g.y, g.z + h + 0.1f }, gcp.name, WHITE });
+    }
+}
+
+void renderControlPoints(const ControlPoints& control_points, const PointClouds& point_clouds_container)
+{
+    const Color mark{ 179, 77, 128, 255 };
+    const Color connector{ 0, 77, 153, 255 };
+    const auto& pcs = point_clouds_container.point_clouds;
+    for (const auto& cp : control_points.cps)
+    {
+        if (cp.index_to_pose < 0 || cp.index_to_pose >= static_cast<int>(pcs.size()))
+            continue;
+        Vector3 c = toVec3(pcs[cp.index_to_pose].m_pose * Eigen::Vector3d(cp.x_source_local, cp.y_source_local, cp.z_source_local));
+        Vector3 g{ static_cast<float>(cp.x_target_global), static_cast<float>(cp.y_target_global), static_cast<float>(cp.z_target_global) };
+        DrawLine3D(Vector3{ g.x - 0.05f, g.y, g.z }, Vector3{ g.x + 0.05f, g.y, g.z }, mark);
+        DrawLine3D(Vector3{ g.x, g.y - 0.05f, g.z }, Vector3{ g.x, g.y + 0.05f, g.z }, mark);
+        DrawLine3D(c, g, connector);
+        labels.push_back({ Vector3{ g.x, g.y, g.z + 0.1f }, cp.name, WHITE });
+    }
+}
+
+void pointsColorMenu()
+{
+    if (!ImGui::BeginMenu("Points color"))
+        return;
+    const std::tuple<const char*, ScanColorMode, const char*> items[] = {
+        { "> Session color", ScanColorMode::FlatIntensity, "Each session in its own color (Settings window), shaded by intensity" },
+        { "> By intensity (gradient)", ScanColorMode::Intensity, "Per-point jet colormap from LAS/LAZ intensity" },
+        { "> By height (gradient)", ScanColorMode::Elevation, "Per-point jet colormap from world Z, over all sessions" },
+        { "> By distance (gradient)", ScanColorMode::Distance, "Per-point jet colormap from distance to the rotation center" },
+    };
+    for (const auto& [label, mode, tooltip] : items)
+    {
+        if (ImGui::MenuItem(label, nullptr, color_mode == mode))
+            color_mode = mode;
+        if (ImGui::IsItemHovered())
+            ImGui::SetTooltip("%s", tooltip);
+    }
+    ImGui::EndMenu();
+}
+
+void assignDistinctSessionColors(std::vector<Session>& sessions)
+{
+    static const float palette[][3] = {
+        { 0.90f, 0.10f, 0.10f }, // red
+        { 0.15f, 0.45f, 0.95f }, // blue
+        { 1.00f, 0.55f, 0.05f }, // orange
+        { 0.15f, 0.75f, 0.20f }, // green
+        { 0.85f, 0.20f, 0.85f }, // magenta
+        { 0.10f, 0.85f, 0.85f }, // cyan
+        { 0.95f, 0.90f, 0.10f }, // yellow
+        { 0.55f, 0.30f, 0.95f }, // violet
+    };
+    const size_t n = sizeof(palette) / sizeof(palette[0]);
+
+    for (size_t i = 0; i < sessions.size(); i++)
+    {
+        float c[3];
+        if (i < n)
+            std::copy(palette[i], palette[i] + 3, c);
+        else
+        {
+            std::mt19937 rng(static_cast<unsigned>(i));
+            Color rgb = ColorFromHSV(std::uniform_real_distribution<float>(0.f, 360.f)(rng), 0.85f, 0.95f);
+            c[0] = rgb.r / 255.f;
+            c[1] = rgb.g / 255.f;
+            c[2] = rgb.b / 255.f;
+        }
+        std::copy(c, c + 3, sessions[i].render_color);
+        for (auto& pc : sessions[i].point_clouds_container.point_clouds)
+        {
+            std::copy(c, c + 3, pc.render_color);
+            std::copy(c, c + 3, pc.traj_color);
+        }
+    }
+}
diff --git a/apps/multi_session_registration/raylib_utils.h b/apps/multi_session_registration/raylib_utils.h
new file mode 100644
index 00000000..f796f9cc
--- /dev/null
+++ b/apps/multi_session_registration/raylib_utils.h
@@ -0,0 +1,156 @@
+#pragma once
+
+// raylib replacement for the parts of <Core/utils.hpp> (GLUT + legacy OpenGL) that step 3 uses:
+// same names, so multi_session_registration.cpp keeps its code and only its GL calls change.
+// Camera state lives in raylib_widgets::OrbitCamera; the old globals are references into it.
+
+#include "raylib.h"
+#include "rlgl.h"
+
+#include <imgui.h>
+
+#include <Eigen/Eigen>
+
+#include <Core/raylib_render.hpp>
+#include <Core/registration_plane_feature.h>
+#include <Core/session.h>
+#include <Core/structures.h>
+
+#include <RaylibWidgets/AppShell.h>
+#include <RaylibWidgets/OrbitCamera.h>
+#include <RaylibWidgets/ShortcutsTable.h>
+
+#include <string>
+#include <vector>
+
+using raylib_widgets::ShortcutEntry;
+using raylib_widgets::ShowMainDockSpace;
+
+constexpr float DEG_TO_RAD = M_PI / 180.0f;
+constexpr float RAD_TO_DEG = 180.0f / M_PI;
+constexpr float ImGuiNumberWidth = 120.0f;
+constexpr const char* xText = "Longitudinal (forward/backward)";
+constexpr const char* yText = "Lateral (left/right)";
+constexpr const char* zText = "Vertical (up/down)";
+
+// GLUT mouse codes, so mouse() keeps its GLUT-callback shape.
+constexpr int GLUT_LEFT_BUTTON = 0;
+constexpr int GLUT_MIDDLE_BUTTON = 1;
+constexpr int GLUT_RIGHT_BUTTON = 2;
+constexpr int GLUT_DOWN = 0;
+constexpr int GLUT_UP = 1;
+
+extern raylib_widgets::OrbitCamera camera;
+
+extern int viewer_decimate_point_cloud;
+extern int mouse_old_x, mouse_old_y;
+extern int mouse_buttons;
+extern bool& is_ortho;
+extern bool& lock_z;
+extern bool show_axes;
+extern ImVec4 bg_color;
+extern int point_size;
+extern bool info_gui;
+extern bool compass_ruler;
+extern bool cor_gui;
+extern Eigen::Affine3f viewLocal;
+
+extern Eigen::Map<Eigen::Vector3f> rotation_center;
+extern float& rotate_x;
+extern float& rotate_y;
+extern float& translate_x;
+extern float& translate_y;
+extern float& translate_z;
+extern Eigen::Map<Eigen::Vector3f> new_rotation_center;
+extern float& new_rotate_x;
+extern float& new_rotate_y;
+extern float& new_translate_x;
+extern float& new_translate_y;
+extern float& new_translate_z;
+extern bool& camera_transition_active;
+extern float* const m_ortho_projection;
+extern float* const m_ortho_gizmo_view;
+
+//! Same as Core/utils.hpp; the GL/GLUT ones are implemented with raylib/rlgl.
+std::string truncPath(const std::string& fullPath);
+void wheel(int button, int dir, int x, int y);
+void motion(int x, int y);
+void reshape(int w, int h);
+bool initGL(int* argc, char** argv, const std::string& winTitle, void (*display)(), void (*mouse)(int, int, int, int));
+void showAxes();
+void updateCameraTransition();
+void updateOrthoView();
+void camMenu();
+void view_kbd_shortcuts();
+void cor_window();
+void info_window(const std::vector<std::string>& infoLines, const std::vector<ShortcutEntry>& appShortcuts);
+void drawMiniCompassWithRuler();
+Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane);
+LaserBeam GetLaserBeam(int x, int y);
+double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line);
+void getClosestTrajectoriesPoint(
+    std::vector<Session>& sessions,
+    int x,
+    int y,
+    const int first_session_index,
+    const int second_session_index,
+    const int number_visible_sessions,
+    int& index_loop_closure_source,
+    int& index_loop_closure_target,
+    bool KeyShift,
+    double& time_stamp_offset);
+void setNewRotationCenter(int x, int y);
+bool checkClHelp(int argc, char** argv);
+
+//! Replaces glutMainLoop(): polls raylib input into the mouse()/motion()/wheel() callbacks, calls display() per frame.
+void mainLoop();
+//! Releases GPU buffers and closes the window (replaces the ImGui GLUT/OpenGL2 backend shutdown).
+void shutdownGL();
+
+//! Copies the current rlgl projection/modelview (column-major, for ImGuizmo); replaces glGetFloatv(GL_*_MATRIX).
+void getProjectionMatrix(float m[16]);
+void getModelviewMatrix(float m[16]);
+//! Stores this frame's 3D matrices for picking and labels; call once the camera transform is set.
+void captureFrameMatrices();
+
+//! GL-style immediate helpers: color3f() also sets the color labels and line strips pick up (like glColor3f).
+void color3f(float r, float g, float b);
+//! rlgl has no line strips; these collect a strip and emit it as RL_LINES segments.
+void beginLineStrip();
+void lineStripVertex3f(float x, float y, float z);
+void endLineStrip();
+//! Replace glRasterPos3f + glutBitmapString: queue a text label at a world position, in the current color3f().
+void labelPos3f(float x, float y, float z);
+void labelText(const std::string& text);
+//! Switches to 2D and draws the queued labels; call after all 3D drawing, before the compass.
+void end3DAndDrawLabels();
+
+//! Per-session GPU buffers (ScanRenderer). syncSessionRenderers() runs once per frame; it re-uploads after
+//! invalidateSessionRenderers() or when the number of sessions changes, else only scans whose pose changed.
+void syncSessionRenderers(const std::vector<Session>& sessions);
+void invalidateSessionRenderers();
+//! Replaces PointClouds::render(): session.visible is checked by the caller.
+void renderSession(const Session& session, const ObservationPicking& observation_picking, int decimate, int reduce_trajectory);
+//! Replaces PointCloud::render(show_with_initial_pose, ...) for scan `index` of session `session_index`.
+void renderScan(
+    int session_index,
+    int index,
+    bool show_with_initial_pose,
+    const ObservationPicking& observation_picking,
+    int decimate,
+    int reduce_trajectory,
+    bool xz_intersection,
+    bool yz_intersection,
+    bool xy_intersection,
+    double intersection_width,
+    bool visible_imu_diff);
+//! Replaces PointCloud::render(pose, ...): points drawn at `pose`, trajectory at the scan's m_pose.
+void renderScanAtPose(int session_index, int index, const Eigen::Affine3d& pose, int decimate, int reduce_trajectory, const float color[3]);
+//! Replace GroundControlPoints::render() / ControlPoints::render(pcs, false), which use legacy GL.
+void renderGroundControlPoints(const GroundControlPoints& ground_control_points, const PointClouds& point_clouds_container);
+void renderControlPoints(const ControlPoints& control_points, const PointClouds& point_clouds_container);
+
+//! View > Points color: ScanRenderer's shader color modes (default: session color shaded by intensity).
+void pointsColorMenu();
+//! After loading: red, blue, orange, ... for the first 8 sessions, then a random vivid hue (seeded by index).
+void assignDistinctSessionColors(std::vector<Session>& sessions);
diff --git a/apps/multi_session_registration_legacy/CMakeLists.txt b/apps/multi_session_registration_legacy/CMakeLists.txt
new file mode 100644
index 00000000..01c03fbe
--- /dev/null
+++ b/apps/multi_session_registration_legacy/CMakeLists.txt
@@ -0,0 +1,73 @@
+cmake_minimum_required(VERSION 4.0.0)
+
+# Legacy GLUT + legacy immediate-mode OpenGL build of step3, kept side by
+# side with apps/multi_session_registration (raylib-based since it was
+# ported off GLUT), the same way step2 keeps multi_view_tls_registration_legacy.
+# This is a full, independent copy of the app as it was before that port --
+# not sharing translation units with apps/multi_session_registration -- so
+# the two can diverge or be retired independently.
+project(multi_session_registration_step_3_legacy)
+
+# Source files
+set(SOURCES
+    multi_session_registration.cpp
+    multi_session_factor_graph.cpp
+    "../../core/src/utils.cpp"
+)
+
+# Windows: add resource file
+if(WIN32)
+    list(APPEND SOURCES "resource.rc")
+endif()
+
+add_executable(
+  multi_session_registration_step_3_legacy ${SOURCES}
+  )
+
+target_compile_definitions(multi_session_registration_step_3_legacy PRIVATE -DWITH_GUI=1)
+
+target_include_directories(
+  multi_session_registration_step_3_legacy
+  PRIVATE include
+          ${REPOSITORY_DIRECTORY}/core/include
+          ${REPOSITORY_DIRECTORY}/core_hd_mapping/include
+          ${THIRDPARTY_DIRECTORY}
+          ${THIRDPARTY_DIRECTORY}/glm
+          ${EIGEN3_INCLUDE_DIR}
+          ${THIRDPARTY_DIRECTORY}/imgui
+          ${THIRDPARTY_DIRECTORY}/imgui/backends
+          ${THIRDPARTY_DIRECTORY}/ImGuizmo
+          ${THIRDPARTY_DIRECTORY}/json/include
+          ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master
+          ${THIRDPARTY_DIRECTORY}/glew-cmake/include
+          ${THIRDPARTY_DIRECTORY}/observation_equations/codes
+          ${THIRDPARTY_DIRECTORY}/freeglut/include
+          ${LASZIP_INCLUDE_DIR}/LASzip/include)
+
+target_link_libraries(
+  multi_session_registration_step_3_legacy
+  # PRIVATE ${THIRDPARTY_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib
+  ${FREEGLUT_LIBRARY}
+  ${OPENGL_gl_LIBRARY}
+  OpenGL::GLU
+  ${PLATFORM_LASZIP_LIB}
+  ${PLATFORM_MISCELLANEOUS_LIBS}
+  ${CORE_LIBRARIES}
+  ${GUI_LIBRARIES})
+
+if(WIN32)
+  add_custom_command(
+    TARGET multi_session_registration_step_3_legacy
+    POST_BUILD
+    COMMAND
+      ${CMAKE_COMMAND} -E copy
+      $<TARGET_RUNTIME_DLLS:multi_session_registration_step_3_legacy>
+      $<TARGET_FILE_DIR:multi_session_registration_step_3_legacy>
+    COMMAND_EXPAND_LISTS)
+endif()
+
+if (MSVC)
+    target_compile_options(multi_session_registration_step_3_legacy PRIVATE /bigobj)
+endif()
+
+hdmapping_install_app(multi_session_registration_step_3_legacy)
\ No newline at end of file
diff --git a/apps/multi_session_registration_legacy/icon.ico b/apps/multi_session_registration_legacy/icon.ico
new file mode 100644
index 00000000..f8a2cb27
Binary files /dev/null and b/apps/multi_session_registration_legacy/icon.ico differ
diff --git a/apps/multi_session_registration_legacy/multi_session_factor_graph.cpp b/apps/multi_session_registration_legacy/multi_session_factor_graph.cpp
new file mode 100644
index 00000000..e4afff84
--- /dev/null
+++ b/apps/multi_session_registration_legacy/multi_session_factor_graph.cpp
@@ -0,0 +1,928 @@
+#include "multi_session_factor_graph.h"
+
+#include <Core/m_estimators.h>
+#include <Core/utils.hpp>
+
+#include <python-scripts/constraints/relative_pose_tait_bryan_cw_jacobian.h>
+#include <python-scripts/constraints/relative_pose_tait_bryan_wc_jacobian.h>
+#include <python-scripts/point-to-feature-metrics/point_to_line_tait_bryan_wc_jacobian.h>
+#include <python-scripts/point-to-feature-metrics/point_to_plane_tait_bryan_wc_jacobian.h>
+#include <python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_wc_jacobian.h>
+
+bool optimize(std::vector<Session>& sessions, const std::vector<Edge>& edges, TaitBryanPose motion_model_weights)
+{
+    for (auto& session : sessions)
+    {
+        for (auto& pc : session.point_clouds_container.point_clouds)
+            pc.m_pose_temp = pc.m_pose;
+    }
+
+    std::vector<int> sums;
+    sums.push_back(0);
+    int sum = 0;
+    std::vector<bool> vfixed_x;
+    std::vector<bool> vfixed_y;
+    std::vector<bool> vfixed_z;
+    std::vector<bool> vfixed_om;
+    std::vector<bool> vfixed_fi;
+    std::vector<bool> vfixed_ka;
+
+    for (size_t i = 0; i < sessions.size(); i++)
+    {
+        sum += sessions[i].point_clouds_container.point_clouds.size();
+        sums.push_back(sum);
+
+        for (size_t j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++)
+        {
+            vfixed_x.push_back(sessions[i].point_clouds_container.point_clouds[j].fixed_x);
+            vfixed_y.push_back(sessions[i].point_clouds_container.point_clouds[j].fixed_y);
+            vfixed_z.push_back(sessions[i].point_clouds_container.point_clouds[j].fixed_z);
+
+            vfixed_om.push_back(sessions[i].point_clouds_container.point_clouds[j].fixed_om);
+            vfixed_fi.push_back(sessions[i].point_clouds_container.point_clouds[j].fixed_fi);
+            vfixed_ka.push_back(sessions[i].point_clouds_container.point_clouds[j].fixed_ka);
+        }
+    }
+
+    bool is_ok = true;
+    std::vector<Eigen::Affine3d> m_poses;
+    std::vector<Eigen::Affine3d> poses_motion_model;
+    std::vector<int> index_trajectory;
+
+    for (size_t j = 0; j < sessions.size(); j++)
+    {
+        for (size_t i = 0; i < sessions[j].point_clouds_container.point_clouds.size(); i++)
+        {
+            m_poses.push_back(sessions[j].point_clouds_container.point_clouds[i].m_pose);
+            poses_motion_model.push_back(sessions[j].point_clouds_container.point_clouds[i].m_initial_pose);
+            // poses_motion_model.push_back(sessions[j].point_clouds_container.point_clouds[i].m_pose);
+            index_trajectory.push_back(j);
+        }
+    }
+
+    std::vector<TaitBryanPose> poses;
+
+    bool is_wc = true;
+    bool is_cw = false;
+    int iterations = 1;
+    bool is_fix_first_node = true;
+    // std::vector<int> indexes_ground_truth;
+    for (size_t j = 0; j < sessions.size(); j++)
+    {
+        for (size_t i = 0; i < sessions[j].point_clouds_container.point_clouds.size(); i++)
+        {
+            if (is_wc)
+            {
+                poses.push_back(pose_tait_bryan_from_affine_matrix(sessions[j].point_clouds_container.point_clouds[i].m_pose));
+            }
+            else if (is_cw)
+            {
+                poses.push_back(pose_tait_bryan_from_affine_matrix(sessions[j].point_clouds_container.point_clouds[i].m_pose.inverse()));
+            }
+            // if (sessions[j].is_ground_truth)
+            //{
+            //     indexes_ground_truth.push_back(poses.size() - 1);
+            // }
+        }
+    }
+
+    std::vector<Edge> all_edges;
+
+    // motion model edges;
+    // double angle =  * DEG_TO_RAD;
+    // double wangle = 1.0 / (angle * angle);
+
+    for (size_t i = 1; i < poses_motion_model.size(); i++)
+    {
+        if (index_trajectory[i - 1] == index_trajectory[i])
+        {
+            Eigen::Affine3d m_rel = poses_motion_model[i - 1].inverse() * poses_motion_model[i];
+            Edge edge;
+            edge.index_from = i - 1;
+            edge.index_to = i;
+            edge.index_session_from = -1;
+            edge.index_session_to = -1;
+            edge.relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_rel);
+            edge.relative_pose_tb_weights.om = 1.0 / (motion_model_weights.om * DEG_TO_RAD * motion_model_weights.om * DEG_TO_RAD);
+            edge.relative_pose_tb_weights.fi = 1.0 / (motion_model_weights.fi * DEG_TO_RAD * motion_model_weights.fi * DEG_TO_RAD);
+            edge.relative_pose_tb_weights.ka = 1.0 / (motion_model_weights.ka * DEG_TO_RAD * motion_model_weights.ka * DEG_TO_RAD);
+            edge.relative_pose_tb_weights.px =
+                motion_model_weights.px > 0 ? 1.0 / (motion_model_weights.px * motion_model_weights.px) : 1000000.0;
+            edge.relative_pose_tb_weights.py =
+                motion_model_weights.py > 0 ? 1.0 / (motion_model_weights.py * motion_model_weights.py) : 1000000.0;
+            edge.relative_pose_tb_weights.pz =
+                motion_model_weights.pz > 0 ? 1.0 / (motion_model_weights.pz * motion_model_weights.pz) : 1000000.0;
+            all_edges.push_back(edge);
+        }
+    }
+
+    for (size_t i = 0; i < sessions.size(); i++)
+    {
+        for (size_t j = 0; j < sessions[i].pose_graph_loop_closure.edges.size(); j++)
+        {
+            Edge edge;
+            edge.index_from = sessions[i].pose_graph_loop_closure.edges[j].index_from + sums[i];
+            edge.index_to = sessions[i].pose_graph_loop_closure.edges[j].index_to + sums[i];
+            edge.relative_pose_tb = sessions[i].pose_graph_loop_closure.edges[j].relative_pose_tb;
+            edge.relative_pose_tb_weights = sessions[i].pose_graph_loop_closure.edges[j].relative_pose_tb_weights;
+            // edge.is_fixed_fi = ToDo
+            all_edges.push_back(edge);
+        }
+    }
+
+    for (size_t i = 0; i < edges.size(); i++)
+    {
+        Edge edge;
+        edge.index_from = edges[i].index_from + sums[edges[i].index_session_from];
+        edge.index_to = edges[i].index_to + sums[edges[i].index_session_to];
+        edge.relative_pose_tb = edges[i].relative_pose_tb;
+        edge.relative_pose_tb_weights = edges[i].relative_pose_tb_weights;
+        // edge.is_fixed_fi = ToDo
+        all_edges.push_back(edge);
+    }
+
+    for (int iter = 0; iter < iterations; iter++)
+    {
+        std::vector<Eigen::Triplet<double>> tripletListA;
+        std::vector<Eigen::Triplet<double>> tripletListP;
+        std::vector<Eigen::Triplet<double>> tripletListB;
+
+        for (size_t i = 0; i < all_edges.size(); i++)
+        {
+            Eigen::Matrix<double, 6, 1> delta;
+            Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
+            // auto relative_pose = pose_tait_bryan_from_affine_matrix(all_edges[i].relative_pose_tb);
+            if (is_wc)
+            {
+                relative_pose_obs_eq_tait_bryan_wc_case1(
+                    delta,
+                    poses[all_edges[i].index_from].px,
+                    poses[all_edges[i].index_from].py,
+                    poses[all_edges[i].index_from].pz,
+                    normalize_angle(poses[all_edges[i].index_from].om),
+                    normalize_angle(poses[all_edges[i].index_from].fi),
+                    normalize_angle(poses[all_edges[i].index_from].ka),
+                    poses[all_edges[i].index_to].px,
+                    poses[all_edges[i].index_to].py,
+                    poses[all_edges[i].index_to].pz,
+                    normalize_angle(poses[all_edges[i].index_to].om),
+                    normalize_angle(poses[all_edges[i].index_to].fi),
+                    normalize_angle(poses[all_edges[i].index_to].ka),
+                    all_edges[i].relative_pose_tb.px,
+                    all_edges[i].relative_pose_tb.py,
+                    all_edges[i].relative_pose_tb.pz,
+                    normalize_angle(all_edges[i].relative_pose_tb.om),
+                    normalize_angle(all_edges[i].relative_pose_tb.fi),
+                    normalize_angle(all_edges[i].relative_pose_tb.ka));
+                relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(
+                    jacobian,
+                    poses[all_edges[i].index_from].px,
+                    poses[all_edges[i].index_from].py,
+                    poses[all_edges[i].index_from].pz,
+                    normalize_angle(poses[all_edges[i].index_from].om),
+                    normalize_angle(poses[all_edges[i].index_from].fi),
+                    normalize_angle(poses[all_edges[i].index_from].ka),
+                    poses[all_edges[i].index_to].px,
+                    poses[all_edges[i].index_to].py,
+                    poses[all_edges[i].index_to].pz,
+                    normalize_angle(poses[all_edges[i].index_to].om),
+                    normalize_angle(poses[all_edges[i].index_to].fi),
+                    normalize_angle(poses[all_edges[i].index_to].ka));
+            }
+            else if (is_cw)
+            {
+                relative_pose_obs_eq_tait_bryan_cw_case1(
+                    delta,
+                    poses[all_edges[i].index_from].px,
+                    poses[all_edges[i].index_from].py,
+                    poses[all_edges[i].index_from].pz,
+                    normalize_angle(poses[all_edges[i].index_from].om),
+                    normalize_angle(poses[all_edges[i].index_from].fi),
+                    normalize_angle(poses[all_edges[i].index_from].ka),
+                    poses[all_edges[i].index_to].px,
+                    poses[all_edges[i].index_to].py,
+                    poses[all_edges[i].index_to].pz,
+                    normalize_angle(poses[all_edges[i].index_to].om),
+                    normalize_angle(poses[all_edges[i].index_to].fi),
+                    normalize_angle(poses[all_edges[i].index_to].ka),
+                    all_edges[i].relative_pose_tb.px,
+                    all_edges[i].relative_pose_tb.py,
+                    all_edges[i].relative_pose_tb.pz,
+                    normalize_angle(all_edges[i].relative_pose_tb.om),
+                    normalize_angle(all_edges[i].relative_pose_tb.fi),
+                    normalize_angle(all_edges[i].relative_pose_tb.ka));
+                relative_pose_obs_eq_tait_bryan_cw_case1_jacobian(
+                    jacobian,
+                    poses[all_edges[i].index_from].px,
+                    poses[all_edges[i].index_from].py,
+                    poses[all_edges[i].index_from].pz,
+                    normalize_angle(poses[all_edges[i].index_from].om),
+                    normalize_angle(poses[all_edges[i].index_from].fi),
+                    normalize_angle(poses[all_edges[i].index_from].ka),
+                    poses[all_edges[i].index_to].px,
+                    poses[all_edges[i].index_to].py,
+                    poses[all_edges[i].index_to].pz,
+                    normalize_angle(poses[all_edges[i].index_to].om),
+                    normalize_angle(poses[all_edges[i].index_to].fi),
+                    normalize_angle(poses[all_edges[i].index_to].ka));
+            }
+
+            int ir = tripletListB.size();
+
+            int ic_1 = all_edges[i].index_from * 6;
+            int ic_2 = all_edges[i].index_to * 6;
+
+            for (size_t row = 0; row < 6; row++)
+            {
+                tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0));
+                tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1));
+                tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2));
+                tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row, 3));
+                tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row, 4));
+                tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row, 5));
+
+                tripletListA.emplace_back(ir + row, ic_2, -jacobian(row, 6));
+                tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row, 7));
+                tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row, 8));
+                tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row, 9));
+                tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row, 10));
+                tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row, 11));
+            }
+
+            tripletListB.emplace_back(ir, 0, delta(0, 0));
+            tripletListB.emplace_back(ir + 1, 0, delta(1, 0));
+            tripletListB.emplace_back(ir + 2, 0, delta(2, 0));
+            tripletListB.emplace_back(ir + 3, 0, normalize_angle(delta(3, 0)));
+            tripletListB.emplace_back(ir + 4, 0, normalize_angle(delta(4, 0)));
+            tripletListB.emplace_back(ir + 5, 0, normalize_angle(delta(5, 0)));
+
+            tripletListP.emplace_back(ir, ir, all_edges[i].relative_pose_tb_weights.px * get_cauchy_w(delta(0, 0), 1));
+            tripletListP.emplace_back(ir + 1, ir + 1, all_edges[i].relative_pose_tb_weights.py * get_cauchy_w(delta(1, 0), 1));
+            tripletListP.emplace_back(ir + 2, ir + 2, all_edges[i].relative_pose_tb_weights.pz * get_cauchy_w(delta(2, 0), 1));
+            tripletListP.emplace_back(ir + 3, ir + 3, all_edges[i].relative_pose_tb_weights.om * get_cauchy_w(delta(3, 0), 1));
+            tripletListP.emplace_back(ir + 4, ir + 4, all_edges[i].relative_pose_tb_weights.fi * get_cauchy_w(delta(4, 0), 1));
+            tripletListP.emplace_back(ir + 5, ir + 5, all_edges[i].relative_pose_tb_weights.ka * get_cauchy_w(delta(5, 0), 1));
+        }
+
+        if (is_fix_first_node)
+        {
+            int ir = tripletListB.size();
+            tripletListA.emplace_back(ir, 0, 1);
+            tripletListA.emplace_back(ir + 1, 1, 1);
+            tripletListA.emplace_back(ir + 2, 2, 1);
+            tripletListA.emplace_back(ir + 3, 3, 1);
+            tripletListA.emplace_back(ir + 4, 4, 1);
+            tripletListA.emplace_back(ir + 5, 5, 1);
+
+            tripletListP.emplace_back(ir, ir, 0.0001);
+            tripletListP.emplace_back(ir + 1, ir + 1, 0.0001);
+            tripletListP.emplace_back(ir + 2, ir + 2, 0.0001);
+            tripletListP.emplace_back(ir + 3, ir + 3, 0.0001);
+            tripletListP.emplace_back(ir + 4, ir + 4, 0.0001);
+            tripletListP.emplace_back(ir + 5, ir + 5, 0.0001);
+
+            tripletListB.emplace_back(ir, 0, 0);
+            tripletListB.emplace_back(ir + 1, 0, 0);
+            tripletListB.emplace_back(ir + 2, 0, 0);
+            tripletListB.emplace_back(ir + 3, 0, 0);
+            tripletListB.emplace_back(ir + 4, 0, 0);
+            tripletListB.emplace_back(ir + 5, 0, 0);
+
+            for (size_t i = 1; i < index_trajectory.size(); i++)
+            {
+                if (index_trajectory[i - 1] != index_trajectory[i])
+                {
+                    ir = tripletListB.size();
+                    tripletListA.emplace_back(ir, i * 6 + 0, 1);
+                    tripletListA.emplace_back(ir + 1, i * 6 + 1, 1);
+                    tripletListA.emplace_back(ir + 2, i * 6 + 2, 1);
+                    tripletListA.emplace_back(ir + 3, i * 6 + 3, 1);
+                    tripletListA.emplace_back(ir + 4, i * 6 + 4, 1);
+                    tripletListA.emplace_back(ir + 5, i * 6 + 5, 1);
+
+                    tripletListP.emplace_back(ir, ir, 0.0001);
+                    tripletListP.emplace_back(ir + 1, ir + 1, 0.0001);
+                    tripletListP.emplace_back(ir + 2, ir + 2, 0.0001);
+                    tripletListP.emplace_back(ir + 3, ir + 3, 0.0001);
+                    tripletListP.emplace_back(ir + 4, ir + 4, 0.0001);
+                    tripletListP.emplace_back(ir + 5, ir + 5, 0.0001);
+
+                    tripletListB.emplace_back(ir, 0, 0);
+                    tripletListB.emplace_back(ir + 1, 0, 0);
+                    tripletListB.emplace_back(ir + 2, 0, 0);
+                    tripletListB.emplace_back(ir + 3, 0, 0);
+                    tripletListB.emplace_back(ir + 4, 0, 0);
+                    tripletListB.emplace_back(ir + 5, 0, 0);
+                }
+            }
+        }
+
+        /*double angle = 1.0 * DEG_TO_RAD;
+        double wangle = 1.0 / (angle * angle);
+
+        for (size_t index = 0; index < indexes_ground_truth.size(); index++)
+        {
+            int ir = tripletListB.size();
+            int ic = indexes_ground_truth[index] * 6;
+            tripletListA.emplace_back(ir, ic + 0, 1);
+            tripletListA.emplace_back(ir + 1, ic + 1, 1);
+            tripletListA.emplace_back(ir + 2, ic + 2, 1);
+            tripletListA.emplace_back(ir + 3, ic + 3, 1);
+            tripletListA.emplace_back(ir + 4, ic + 4, 1);
+            tripletListA.emplace_back(ir + 5, ic + 5, 1);
+
+            tripletListP.emplace_back(ir    , ir    , 10000.0);
+            tripletListP.emplace_back(ir + 1, ir + 1, 10000.0);
+            tripletListP.emplace_back(ir + 2, ir + 2, 10000.0);
+            tripletListP.emplace_back(ir + 3, ir + 3, wangle);
+            tripletListP.emplace_back(ir + 4, ir + 4, wangle);
+            tripletListP.emplace_back(ir + 5, ir + 5, wangle);
+
+            tripletListB.emplace_back(ir, 0, 0);
+            tripletListB.emplace_back(ir + 1, 0, 0);
+            tripletListB.emplace_back(ir + 2, 0, 0);
+            tripletListB.emplace_back(ir + 3, 0, 0);
+            tripletListB.emplace_back(ir + 4, 0, 0);
+            tripletListB.emplace_back(ir + 5, 0, 0);
+        }*/
+
+        // gnss
+        // for (const auto &pc : point_clouds_container.point_clouds)
+        /*for (size_t index_pose = 0; index_pose < point_clouds_container.point_clouds.size(); index_pose++)
+        {
+            const auto &pc = point_clouds_container.point_clouds[index_pose];
+            for (size_t i = 0; i < gnss.gnss_poses.size(); i++)
+            {
+                double time_stamp = gnss.gnss_poses[i].timestamp;
+
+                auto it = std::lower_bound(pc.local_trajectory.begin(), pc.local_trajectory.end(),
+                                           time_stamp, [](const PointCloud::LocalTrajectoryNode &lhs, const double &time) -> bool
+                                           { return lhs.timestamp < time; });
+
+                int index = it - pc.local_trajectory.begin();
+
+                if (index > 0 && index < pc.local_trajectory.size())
+                {
+
+                    if (fabs(time_stamp - pc.local_trajectory[index].timestamp) < 10e12)
+                    {
+
+                        Eigen::Matrix<double, 3, 6, Eigen::RowMajor> jacobian;
+                        TaitBryanPose pose_s;
+                        pose_s = pose_tait_bryan_from_affine_matrix(m_poses[index_pose]);
+                        Eigen::Vector3d p_s = pc.local_trajectory[index].m_pose.translation();
+                        point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om,
+        pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z());
+
+                        double delta_x;
+                        double delta_y;
+                        double delta_z;
+                        Eigen::Vector3d p_t(gnss.gnss_poses[i].x - gnss.offset_x, gnss.gnss_poses[i].y - gnss.offset_y,
+        gnss.gnss_poses[i].alt - gnss.offset_alt); point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, pose_s.px,
+        pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z());
+
+                        std::cout << " delta_x " << delta_x << " delta_y " << delta_y << " delta_z " << delta_z << std::endl;
+
+                        int ir = tripletListB.size();
+                        int ic = index_pose * 6;
+                        for (int row = 0; row < 3; row++)
+                        {
+                            for (int col = 0; col < 6; col++)
+                            {
+                                if (jacobian(row, col) != 0.0)
+                                {
+                                    tripletListA.emplace_back(ir + row, ic + col, -jacobian(row, col));
+                                }
+                            }
+                        }
+                        tripletListP.emplace_back(ir, ir, get_cauchy_w(delta_x, 1));
+                        tripletListP.emplace_back(ir + 1, ir + 1, get_cauchy_w(delta_y, 1));
+                        tripletListP.emplace_back(ir + 2, ir + 2, get_cauchy_w(delta_z, 1));
+
+                        tripletListB.emplace_back(ir, 0, delta_x);
+                        tripletListB.emplace_back(ir + 1, 0, delta_y);
+                        tripletListB.emplace_back(ir + 2, 0, delta_z);
+
+                        // jacobian3x6 = get_point_to_point_jacobian_tait_bryan(pose_convention,
+        point_clouds_container.point_clouds[i].m_pose, p_s, p_t);
+
+                        // auto m = pc.m_pose * pc.local_trajectory[index].m_pose;
+                        // glVertex3f(m(0, 3), m(1, 3), m(2, 3));
+                        // glVertex3f(gnss_poses[i].x - offset_x, gnss_poses[i].y - offset_y, gnss_poses[i].alt - offset_alt);
+                    }
+                }
+            }
+        }*/
+        //
+
+        for (size_t j = 0; j < sessions.size(); j++)
+        {
+            for (size_t jj = 0; jj < sessions[j].ground_control_points.gpcs.size(); jj++)
+            {
+                Eigen::Vector3d p_s =
+                    sessions[j]
+                        .point_clouds_container.point_clouds[sessions[j].ground_control_points.gpcs[jj].index_to_node_inner]
+                        .local_trajectory[sessions[j].ground_control_points.gpcs[jj].index_to_node_outer]
+                        .m_pose.translation();
+                Eigen::Matrix<double, 3, 6, Eigen::RowMajor> jacobian;
+                TaitBryanPose pose_s;
+                pose_s = pose_tait_bryan_from_affine_matrix(
+                    sessions[j].point_clouds_container.point_clouds[sessions[j].ground_control_points.gpcs[jj].index_to_node_inner].m_pose);
+                point_to_point_source_to_target_tait_bryan_wc_jacobian(
+                    jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z());
+
+                double delta_x;
+                double delta_y;
+                double delta_z;
+                Eigen::Vector3d p_t(
+                    sessions[j].ground_control_points.gpcs[jj].x,
+                    sessions[j].ground_control_points.gpcs[jj].y,
+                    sessions[j].ground_control_points.gpcs[jj].z + sessions[j].ground_control_points.gpcs[jj].lidar_height_above_ground);
+                point_to_point_source_to_target_tait_bryan_wc(
+                    delta_x,
+                    delta_y,
+                    delta_z,
+                    pose_s.px,
+                    pose_s.py,
+                    pose_s.pz,
+                    pose_s.om,
+                    pose_s.fi,
+                    pose_s.ka,
+                    p_s.x(),
+                    p_s.y(),
+                    p_s.z(),
+                    p_t.x(),
+                    p_t.y(),
+                    p_t.z());
+
+                int ir = tripletListB.size();
+                int ic = sessions[j].ground_control_points.gpcs[jj].index_to_node_inner * 6 + sums[j] * 6;
+
+                for (int row = 0; row < 3; row++)
+                {
+                    for (int col = 0; col < 6; col++)
+                    {
+                        if (jacobian(row, col) != 0.0)
+                        {
+                            tripletListA.emplace_back(ir + row, ic + col, -jacobian(row, col));
+                        }
+                    }
+                }
+                tripletListP.emplace_back(
+                    ir + 0,
+                    ir + 0,
+                    (1.0 / (sessions[j].ground_control_points.gpcs[jj].sigma_x * sessions[j].ground_control_points.gpcs[jj].sigma_x)) *
+                        get_cauchy_w(delta_x, 1));
+                tripletListP.emplace_back(
+                    ir + 1,
+                    ir + 1,
+                    (1.0 / (sessions[j].ground_control_points.gpcs[jj].sigma_y * sessions[j].ground_control_points.gpcs[jj].sigma_y)) *
+                        get_cauchy_w(delta_y, 1));
+                tripletListP.emplace_back(
+                    ir + 2,
+                    ir + 2,
+                    (1.0 / (sessions[j].ground_control_points.gpcs[jj].sigma_z * sessions[j].ground_control_points.gpcs[jj].sigma_z)) *
+                        get_cauchy_w(delta_z, 1));
+
+                tripletListB.emplace_back(ir, 0, delta_x);
+                tripletListB.emplace_back(ir + 1, 0, delta_y);
+                tripletListB.emplace_back(ir + 2, 0, delta_z);
+
+                std::cout << "gcp: delta_x " << delta_x << " delta_y " << delta_y << " delta_z " << delta_z << std::endl;
+            }
+        }
+
+        ///////////////////////////////////////////////////////
+        for (size_t i = 0; i < sessions.size(); i++)
+        {
+            for (size_t index_pose = 0; index_pose < sessions[i].point_clouds_container.point_clouds.size(); index_pose++)
+            {
+                if (sessions[i].point_clouds_container.point_clouds[index_pose].fixed_x || sessions[i].is_ground_truth)
+                {
+                    int ir = tripletListB.size();
+                    int ic = (index_pose + sums[i]) * 6;
+                    tripletListA.emplace_back(ir + 0, ic, 1000000000000);
+                    tripletListP.emplace_back(ir, ir, 1);
+                    tripletListB.emplace_back(ir, 0, 0);
+                }
+
+                if (sessions[i].point_clouds_container.point_clouds[index_pose].fixed_y || sessions[i].is_ground_truth)
+                {
+                    int ir = tripletListB.size();
+                    int ic = (index_pose + sums[i]) * 6 + 1;
+                    tripletListA.emplace_back(ir + 0, ic, 1000000000000);
+                    tripletListP.emplace_back(ir, ir, 1);
+                    tripletListB.emplace_back(ir, 0, 0);
+                }
+
+                if (sessions[i].point_clouds_container.point_clouds[index_pose].fixed_z || sessions[i].is_ground_truth)
+                {
+                    int ir = tripletListB.size();
+                    int ic = (index_pose + sums[i]) * 6 + 2;
+                    tripletListA.emplace_back(ir + 0, ic, 1000000000000);
+                    tripletListP.emplace_back(ir, ir, 1);
+                    tripletListB.emplace_back(ir, 0, 0);
+                }
+
+                if (sessions[i].point_clouds_container.point_clouds[index_pose].fixed_om || sessions[i].is_ground_truth)
+                {
+                    int ir = tripletListB.size();
+                    int ic = (index_pose + sums[i]) * 6 + 3;
+                    tripletListA.emplace_back(ir + 0, ic, 1000000000000);
+                    tripletListP.emplace_back(ir, ir, 1);
+                    tripletListB.emplace_back(ir, 0, 0);
+                }
+
+                if (sessions[i].point_clouds_container.point_clouds[index_pose].fixed_fi || sessions[i].is_ground_truth)
+                {
+                    int ir = tripletListB.size();
+                    int ic = (index_pose + sums[i]) * 6 + 4;
+                    tripletListA.emplace_back(ir + 0, ic, 1000000000000);
+                    tripletListP.emplace_back(ir, ir, 1);
+                    tripletListB.emplace_back(ir, 0, 0);
+                }
+
+                if (sessions[i].point_clouds_container.point_clouds[index_pose].fixed_ka || sessions[i].is_ground_truth)
+                {
+                    int ir = tripletListB.size();
+                    int ic = (index_pose + sums[i]) * 6 + 5;
+                    tripletListA.emplace_back(ir + 0, ic, 1000000000000);
+                    tripletListP.emplace_back(ir, ir, 1);
+                    tripletListB.emplace_back(ir, 0, 0);
+                }
+            }
+        }
+
+        double error_imu = 0;
+        double error_imu_sum = 0;
+
+        for (size_t j = 0; j < sessions.size(); j++)
+        {
+            for (size_t index_pose = 0; index_pose < sessions[j].point_clouds_container.point_clouds.size(); index_pose++)
+            {
+                const auto& pc = sessions[j].point_clouds_container.point_clouds[index_pose];
+                if (!pc.fuse_inclination_from_IMU)
+                {
+                    continue;
+                }
+                if (pc.local_trajectory.size() == 0)
+                {
+                    continue;
+                }
+
+                TaitBryanPose target_pose;
+                target_pose.om = pc.local_trajectory[0].imu_om_fi_ka.x();
+                target_pose.fi = pc.local_trajectory[0].imu_om_fi_ka.y();
+                target_pose.ka = pc.local_trajectory[0].imu_om_fi_ka.z();
+                target_pose.px = pc.m_pose(0, 3);
+                target_pose.py = pc.m_pose(1, 3);
+                target_pose.pz = pc.m_pose(2, 3);
+
+                Eigen::Affine3d target_mpose = affine_matrix_from_pose_tait_bryan(target_pose);
+
+                double xtg = target_mpose(0, 3);
+                double ytg = target_mpose(1, 3);
+                double ztg = target_mpose(2, 3);
+                double vzx = target_mpose(0, 2);
+                double vzy = target_mpose(1, 2);
+                double vzz = target_mpose(2, 2);
+
+                TaitBryanPose current_pose = pose_tait_bryan_from_affine_matrix(pc.m_pose);
+
+                Eigen::Matrix<double, 1, 1> delta;
+                point_to_plane_tait_bryan_wc(
+                    delta,
+                    current_pose.px,
+                    current_pose.py,
+                    current_pose.pz,
+                    current_pose.om,
+                    current_pose.fi,
+                    current_pose.ka,
+                    1,
+                    0,
+                    0, // add 0,1,0
+                    xtg,
+                    ytg,
+                    ztg,
+                    vzx,
+                    vzy,
+                    vzz);
+
+                Eigen::Matrix<double, 1, 6> delta_jacobian;
+                point_to_plane_tait_bryan_wc_jacobian(
+                    delta_jacobian,
+                    current_pose.px,
+                    current_pose.py,
+                    current_pose.pz,
+                    current_pose.om,
+                    current_pose.fi,
+                    current_pose.ka,
+                    1,
+                    0,
+                    0,
+                    xtg,
+                    ytg,
+                    ztg,
+                    vzx,
+                    vzy,
+                    vzz);
+
+                int ir = tripletListB.size();
+                int ic = (index_pose + sums[j]) * 6;
+                tripletListA.emplace_back(ir + 0, ic + 3, -delta_jacobian(0, 3));
+                tripletListA.emplace_back(ir + 0, ic + 4, -delta_jacobian(0, 4));
+
+                tripletListP.emplace_back(ir, ir, /*get_cauchy_w(delta(0, 0), 1) * 10000*/ 1);
+
+                tripletListB.emplace_back(ir, 0, delta(0, 0));
+
+                ///////////////////////////////
+                point_to_plane_tait_bryan_wc(
+                    delta,
+                    current_pose.px,
+                    current_pose.py,
+                    current_pose.pz,
+                    current_pose.om,
+                    current_pose.fi,
+                    current_pose.ka,
+                    0,
+                    1,
+                    0,
+                    xtg,
+                    ytg,
+                    ztg,
+                    vzx,
+                    vzy,
+                    vzz);
+
+                point_to_plane_tait_bryan_wc_jacobian(
+                    delta_jacobian,
+                    current_pose.px,
+                    current_pose.py,
+                    current_pose.pz,
+                    current_pose.om,
+                    current_pose.fi,
+                    current_pose.ka,
+                    0,
+                    1,
+                    0,
+                    xtg,
+                    ytg,
+                    ztg,
+                    vzx,
+                    vzy,
+                    vzz);
+
+                ir = tripletListB.size();
+                // ic = index_pose * 6;
+                ic = (index_pose + sums[j]) * 6;
+
+                tripletListA.emplace_back(ir + 0, ic + 3, -delta_jacobian(0, 3));
+                tripletListA.emplace_back(ir + 0, ic + 4, -delta_jacobian(0, 4));
+
+                tripletListP.emplace_back(ir, ir, /*get_cauchy_w(delta(0, 0), 1) * 10000*/ 1);
+
+                tripletListB.emplace_back(ir, 0, delta(0, 0));
+            }
+        }
+
+        // fuse control points
+        for (size_t j = 0; j < sessions.size(); j++)
+        {
+            // CPs
+            auto& cps = sessions[j].control_points;
+            auto& point_clouds_container = sessions[j].point_clouds_container;
+
+            for (int i = 0; i < cps.cps.size(); i++)
+            {
+                if (!cps.cps[i].is_z_0)
+                {
+                    Eigen::Vector3d p_s(cps.cps[i].x_source_local, cps.cps[i].y_source_local, cps.cps[i].z_source_local);
+
+                    Eigen::Matrix<double, 3, 6, Eigen::RowMajor> jacobian;
+                    TaitBryanPose pose_s;
+                    pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[cps.cps[i].index_to_pose].m_pose);
+
+                    point_to_point_source_to_target_tait_bryan_wc_jacobian(
+                        jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z());
+
+                    double delta_x;
+                    double delta_y;
+                    double delta_z;
+                    Eigen::Vector3d p_t(cps.cps[i].x_target_global, cps.cps[i].y_target_global, cps.cps[i].z_target_global);
+                    point_to_point_source_to_target_tait_bryan_wc(
+                        delta_x,
+                        delta_y,
+                        delta_z,
+                        pose_s.px,
+                        pose_s.py,
+                        pose_s.pz,
+                        pose_s.om,
+                        pose_s.fi,
+                        pose_s.ka,
+                        p_s.x(),
+                        p_s.y(),
+                        p_s.z(),
+                        p_t.x(),
+                        p_t.y(),
+                        p_t.z());
+
+                    int ir = tripletListB.size();
+
+                    //(index_pose + sums[j]) * 6;
+                    int ic = (cps.cps[i].index_to_pose + sums[j]) * 6;
+
+                    for (int row = 0; row < 3; row++)
+                    {
+                        for (int col = 0; col < 6; col++)
+                        {
+                            if (jacobian(row, col) != 0.0)
+                            {
+                                tripletListA.emplace_back(ir + row, ic + col, -jacobian(row, col));
+                            }
+                        }
+                    }
+                    tripletListP.emplace_back(ir + 0, ir + 0, (1.0 / (cps.cps[i].sigma_x * cps.cps[i].sigma_x)) * get_cauchy_w(delta_x, 1));
+                    tripletListP.emplace_back(ir + 1, ir + 1, (1.0 / (cps.cps[i].sigma_y * cps.cps[i].sigma_y)) * get_cauchy_w(delta_y, 1));
+                    tripletListP.emplace_back(ir + 2, ir + 2, (1.0 / (cps.cps[i].sigma_z * cps.cps[i].sigma_z)) * get_cauchy_w(delta_z, 1));
+
+                    tripletListB.emplace_back(ir, 0, delta_x);
+                    tripletListB.emplace_back(ir + 1, 0, delta_y);
+                    tripletListB.emplace_back(ir + 2, 0, delta_z);
+
+                    std::cout << "cp [not z == 0]: delta_x " << delta_x << " delta_y " << delta_y << " delta_z " << delta_z << std::endl;
+                }
+                else
+                {
+                    Eigen::Vector3d p_s(cps.cps[i].x_source_local, cps.cps[i].y_source_local, cps.cps[i].z_source_local);
+
+                    Eigen::Matrix<double, 3, 6, Eigen::RowMajor> jacobian;
+                    TaitBryanPose pose_s;
+                    pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[cps.cps[i].index_to_pose].m_pose);
+
+                    point_to_point_source_to_target_tait_bryan_wc_jacobian(
+                        jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z());
+
+                    double delta_x;
+                    double delta_y;
+                    double delta_z;
+                    Eigen::Vector3d p_t(cps.cps[i].x_target_global, cps.cps[i].y_target_global, 0.0 /*cps.cps[i].z_target_global*/);
+                    point_to_point_source_to_target_tait_bryan_wc(
+                        delta_x,
+                        delta_y,
+                        delta_z,
+                        pose_s.px,
+                        pose_s.py,
+                        pose_s.pz,
+                        pose_s.om,
+                        pose_s.fi,
+                        pose_s.ka,
+                        p_s.x(),
+                        p_s.y(),
+                        p_s.z(),
+                        p_t.x(),
+                        p_t.y(),
+                        p_t.z());
+
+                    int ir = tripletListB.size();
+                    // int ic = cps.cps[i].index_to_pose * 6;
+                    int ic = (cps.cps[i].index_to_pose + sums[j]) * 6;
+
+                    for (int row = 2; row < 3; row++)
+                    {
+                        for (int col = 0; col < 6; col++)
+                        {
+                            if (jacobian(row, col) != 0.0)
+                            {
+                                tripletListA.emplace_back(ir, ic + col, -jacobian(row, col));
+                            }
+                        }
+                    }
+                    // tripletListP.emplace_back(ir + 0, ir + 0, (1.0 / (cps.cps[i].sigma_x * cps.cps[i].sigma_x)) * get_cauchy_w(delta_x,
+                    // 1)); tripletListP.emplace_back(ir + 1, ir + 1, (1.0 / (cps.cps[i].sigma_y * cps.cps[i].sigma_y)) *
+                    // get_cauchy_w(delta_y, 1));
+                    tripletListP.emplace_back(ir, ir, (1.0 / (cps.cps[i].sigma_z * cps.cps[i].sigma_z)));
+
+                    // tripletListB.emplace_back(ir, 0, delta_x);
+                    // tripletListB.emplace_back(ir + 1, 0, delta_y);
+                    tripletListB.emplace_back(ir, 0, delta_z);
+
+                    std::cout << "cp [not z == 0]: delta_z " << delta_z << std::endl;
+                }
+            }
+        }
+
+        //////////////////////////////////////////////////////
+        // for (size_t i = 0; i < gcps.gpcs.size(); i++)
+        //{
+
+        Eigen::SparseMatrix<double> matA(tripletListB.size(), poses.size() * 6);
+        Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
+        Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);
+
+        matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
+        matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
+        matB.setFromTriplets(tripletListB.begin(), tripletListB.end());
+
+        Eigen::SparseMatrix<double> AtPA(poses.size() * 6, poses.size() * 6);
+        Eigen::SparseMatrix<double> AtPB(poses.size() * 6, 1);
+
+        {
+            Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
+            AtPA = (AtP)*matA;
+            AtPB = (AtP)*matB;
+        }
+
+        tripletListA.clear();
+        tripletListP.clear();
+        tripletListB.clear();
+
+        Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);
+
+        Eigen::SparseMatrix<double> x = solver.solve(AtPB);
+
+        std::vector<double> h_x;
+
+        for (int k = 0; k < x.outerSize(); ++k)
+        {
+            for (Eigen::SparseMatrix<double>::InnerIterator it(x, k); it; ++it)
+            {
+                h_x.push_back(it.value());
+            }
+        }
+
+        if (h_x.size() == 6 * poses.size())
+        {
+            int counter = 0;
+
+            for (size_t i = 0; i < poses.size(); i++)
+            {
+                TaitBryanPose pose = poses[i];
+                pose.px += h_x[counter++] * 0.1;
+                pose.py += h_x[counter++] * 0.1;
+                pose.pz += h_x[counter++] * 0.1;
+                pose.om += h_x[counter++] * 0.1;
+                pose.fi += h_x[counter++] * 0.1;
+                pose.ka += h_x[counter++] * 0.1;
+
+                poses[i].px = pose.px;
+                poses[i].py = pose.py;
+                poses[i].pz = pose.pz;
+                poses[i].om = pose.om;
+                poses[i].fi = pose.fi;
+                poses[i].ka = pose.ka;
+            }
+        }
+        else
+        {
+            std::cout << "optimizing with tait bryan FAILED" << std::endl;
+            std::cout << "h_x.size(): " << h_x.size() << " should be: " << 6 * poses.size() << std::endl;
+            is_ok = false;
+            break;
+        }
+
+        if (is_ok)
+        {
+            for (size_t i = 0; i < m_poses.size(); i++)
+            {
+                if (is_wc)
+                    m_poses[i] = affine_matrix_from_pose_tait_bryan(poses[i]);
+                else if (is_cw)
+                    m_poses[i] = affine_matrix_from_pose_tait_bryan(poses[i]).inverse();
+            }
+        }
+
+        if (is_ok)
+        {
+            int index = 0;
+            for (size_t i = 0; i < m_poses.size(); i++)
+            {
+                if (i > 0)
+                {
+                    if (index_trajectory[i - 1] != index_trajectory[i])
+                        index = 0;
+                }
+                // if (!sessions[index_trajectory[i]].is_ground_truth)
+                //{
+                sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].m_pose = m_poses[i];
+                sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose =
+                    pose_tait_bryan_from_affine_matrix(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].m_pose);
+                sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_translation[0] =
+                    sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.px;
+                sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_translation[1] =
+                    sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.py;
+                sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_translation[2] =
+                    sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.pz;
+                sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_rotation[0] =
+                    rad2deg(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.om);
+                sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_rotation[1] =
+                    rad2deg(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.fi);
+                sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_rotation[2] =
+                    rad2deg(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.ka);
+                //}
+                index++;
+            }
+        }
+    }
+    return true;
+}
\ No newline at end of file
diff --git a/apps/multi_session_registration_legacy/multi_session_factor_graph.h b/apps/multi_session_registration_legacy/multi_session_factor_graph.h
new file mode 100644
index 00000000..b173f04e
--- /dev/null
+++ b/apps/multi_session_registration_legacy/multi_session_factor_graph.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <Core/session.h>
+
+struct Edge
+{
+    TaitBryanPose relative_pose_tb;
+    TaitBryanPose relative_pose_tb_weights;
+    int index_session_from;
+    int index_session_to;
+    int index_from;
+    int index_to;
+    bool is_fixed_px = false;
+    bool is_fixed_py = false;
+    bool is_fixed_pz = false;
+    bool is_fixed_om = false;
+    bool is_fixed_fi = false;
+    bool is_fixed_ka = false;
+};
+
+bool optimize(std::vector<Session>& sessions, const std::vector<Edge>& edges, TaitBryanPose motion_model_weights);
diff --git a/apps/multi_session_registration_legacy/multi_session_registration.cpp b/apps/multi_session_registration_legacy/multi_session_registration.cpp
new file mode 100644
index 00000000..ca6a4bbf
--- /dev/null
+++ b/apps/multi_session_registration_legacy/multi_session_registration.cpp
@@ -0,0 +1,4391 @@
+#include <cmath>
+#include <filesystem>
+
+#include <imgui.h>
+#include <imgui_impl_glut.h>
+#include <imgui_impl_opengl2.h>
+#include <imgui_internal.h>
+
+#include <ImGuizmo.h>
+
+#include <GL/freeglut.h>
+#include <glm/glm.hpp>
+#include <glm/gtc/matrix_transform.hpp>
+#include <nlohmann/json.hpp>
+
+#include <Eigen/Eigen>
+
+#include <Core/export_laz.h>
+#include <Core/icp.h>
+#include <Core/ndt.h>
+#include <Core/observation_picking.h>
+#include <Core/pair_wise_iterative_closest_point.h>
+#include <Core/pfd_wrapper.hpp>
+#include <Core/registration_plane_feature.h>
+#include <Core/session.h>
+#include <Core/utils.hpp>
+
+#include <portable-file-dialogs.h>
+
+#include <HDMapping/Version.hpp>
+
+#ifdef _WIN32
+#include "resource.h"
+#include <windows.h>
+
+#endif
+
+#include "multi_session_factor_graph.h"
+
+std::string winTitle = std::string("Step 3 (Multi session registration) ") + HDMAPPING_VERSION_STRING;
+
+std::vector<std::string> infoLines = {
+    "This program is third/final step in MANDEYE process",
+    "",
+    "First step: create project by adding sessions (result of 'multi_view_tls_registration_step_2' program)",
+    "Last step: save project",
+    "To produce map use 'multi_view_tls_registration_step_2' export functionality"
+};
+
+// App specific shortcuts (Type and Shortcut are just for easy reference)
+static const std::vector<ShortcutEntry> appShortcuts = { { "Normal keys", "A", "" },
+                                                         { "", "Ctrl+A", "Add session(s)" },
+                                                         { "", "B", "" },
+                                                         { "", "Ctrl+B", "" },
+                                                         { "", "C", "" },
+                                                         { "", "Ctrl+C", "" },
+                                                         { "", "D", "" },
+                                                         { "", "Ctrl+D", "" },
+                                                         { "", "E", "" },
+                                                         { "", "Ctrl+E", "" },
+                                                         { "", "F", "" },
+                                                         { "", "Ctrl+F", "" },
+                                                         { "", "G", "" },
+                                                         { "", "Ctrl+G", "" },
+                                                         { "", "H", "" },
+                                                         { "", "Ctrl+H", "" },
+                                                         { "", "I", "" },
+                                                         { "", "Ctrl+I", "" },
+                                                         { "", "J", "" },
+                                                         { "", "Ctrl+K", "" },
+                                                         { "", "K", "" },
+                                                         { "", "Ctrl+K", "" },
+                                                         { "", "L", "" },
+                                                         { "", "Ctrl+L", "Load sessions" },
+                                                         { "", "M", "" },
+                                                         { "", "Ctrl+M", "" },
+                                                         { "", "N", "" },
+                                                         { "", "Ctrl+N", "" },
+                                                         { "", "O", "" },
+                                                         { "", "Ctrl+O", "Open project" },
+                                                         { "", "P", "" },
+                                                         { "", "Ctrl+P", "" },
+                                                         { "", "Q", "" },
+                                                         { "", "Ctrl+Q", "" },
+                                                         { "", "R", "" },
+                                                         { "", "Ctrl+R", "Remove session(s)" },
+                                                         { "", "Shift+R", "" },
+                                                         { "", "S", "" },
+                                                         { "", "Ctrl+S", "Save project" },
+                                                         { "", "Ctrl+Shift+S", "" },
+                                                         { "", "T", "" },
+                                                         { "", "Ctrl+T", "" },
+                                                         { "", "U", "" },
+                                                         { "", "Ctrl+U", "" },
+                                                         { "", "V", "" },
+                                                         { "", "Ctrl+V", "" },
+                                                         { "", "W", "" },
+                                                         { "", "Ctrl+W", "" },
+                                                         { "", "X", "" },
+                                                         { "", "Ctrl+X", "" },
+                                                         { "", "Y", "" },
+                                                         { "", "Ctrl+Y", "" },
+                                                         { "", "Z", "" },
+                                                         { "", "Ctrl+Z", "" },
+                                                         { "", "Shift+Z", "" },
+                                                         { "", "1-9", "" },
+                                                         { "Special keys", "Up arrow", "" },
+                                                         { "", "Shift + up arrow", "" },
+                                                         { "", "Ctrl + up arrow", "" },
+                                                         { "", "Down arrow", "" },
+                                                         { "", "Shift + down arrow", "" },
+                                                         { "", "Ctrl + down arrow", "" },
+                                                         { "", "Left arrow", "" },
+                                                         { "", "Shift + left arrow", "" },
+                                                         { "", "Ctrl + left arrow", "" },
+                                                         { "", "Right arrow", "" },
+                                                         { "", "Shift + right arrow", "" },
+                                                         { "", "Ctrl + right arrow", "" },
+                                                         { "", "Pg down", "" },
+                                                         { "", "Pg up", "" },
+                                                         { "", "- key", "" },
+                                                         { "", "+ key", "" },
+                                                         { "Mouse related", "Left click + drag", "" },
+                                                         { "", "Right click + drag", "n" },
+                                                         { "", "Scroll", "" },
+                                                         { "", "Shift + scroll", "" },
+                                                         { "", "Shift + drag", "" },
+                                                         { "", "Ctrl + left click", "" },
+                                                         { "", "Ctrl + right click", "" },
+                                                         { "", "Ctrl + middle click", "" } };
+
+float m_gizmo[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 };
+
+bool is_decimate = true;
+double bucket_x = 0.1;
+double bucket_y = 0.1;
+double bucket_z = 0.1;
+bool calculate_offset = false;
+ObservationPicking observation_picking;
+int index_loop_closure_source = -1;
+int index_loop_closure_target = -1;
+int first_session_index = -1;
+int second_session_index = -1;
+double search_radius = 0.3;
+bool loaded_sessions = false;
+bool optimized = false;
+bool gizmo_all_sessions = false;
+bool is_ndt_gui = false;
+bool is_loop_closure_gui = false;
+bool remove_gui = false;
+NDT ndt;
+
+bool update_rotation_center = false;
+
+bool is_settings_gui = true;
+
+int number_visible_sessions = 0;
+int index_gt = -1;
+int old_index_gt = -1;
+int index_gizmo = -1;
+int old_index_gizmo = -1;
+
+double time_stamp_offset = 0.0;
+
+struct ProjectSettings
+{
+    std::vector<std::string> session_file_names;
+};
+
+std::vector<Edge> edges;
+int index_active_edge = -1;
+bool manipulate_active_edge = false;
+bool edge_gizmo = false;
+
+ProjectSettings project_settings;
+std::vector<Session> sessions;
+
+int viewer_reduce_rendered_trajectory = 1;
+namespace fs = std::filesystem;
+
+int num_edge_extended_before = 0;
+int num_edge_extended_after = 0;
+
+int gui_point_size = 2;
+
+TaitBryanPose motion_model_weights = { 0.01, 0.01, 0.01, 0.1, 0.1, 0.1 };
+///////////////////////////////////////////////////////////////////////////////////
+
+void ndt_gui()
+{
+    static bool compute_mean_and_cov_for_bucket = false;
+    if (ImGui::Begin("Normal Distributions Transform", &is_ndt_gui, ImGuiWindowFlags_AlwaysAutoResize))
+    {
+        ImGui::InputFloat3("Bucket size [m] (x, y,z)", ndt.bucket_size);
+        if (ndt.bucket_size[0] < 0.01)
+            ndt.bucket_size[0] = 0.01f;
+        if (ndt.bucket_size[1] < 0.01)
+            ndt.bucket_size[1] = 0.01f;
+        if (ndt.bucket_size[2] < 0.01)
+            ndt.bucket_size[2] = 0.01f;
+
+        ImGui::PushItemWidth(ImGuiNumberWidth);
+        ImGui::InputInt("Number of threads", &ndt.number_of_threads);
+        if (ndt.number_of_threads < 1)
+            ndt.number_of_threads = 1;
+        ImGui::SameLine();
+        ImGui::InputInt("Number of iterations", &ndt.number_of_iterations);
+        if (ndt.number_of_iterations < 1)
+            ndt.number_of_iterations = 1;
+        ImGui::PopItemWidth();
+
+        if (ImGui::Button("NDT optimization"))
+        {
+            for (auto& s : sessions)
+            {
+                s.is_gizmo = false;
+            }
+
+            double rms_initial = 0.0;
+            double rms_final = 0.0;
+            double mui = 0.0;
+
+            ndt.optimize(sessions, false, compute_mean_and_cov_for_bucket);
+        }
+        ImGui::End();
+    }
+
+#if 0
+    ImGui::Checkbox("ndt fix_first_node (add I to first pose in Hessian)", &ndt.is_fix_first_node);
+
+    ImGui::Checkbox("ndt Gauss-Newton", &ndt.is_gauss_newton);
+    if (ndt.is_gauss_newton)
+    {
+        ndt.is_levenberg_marguardt = false;
+    }
+
+    ImGui::SameLine();
+    ImGui::Checkbox("ndt Levenberg-Marguardt", &ndt.is_levenberg_marguardt);
+    if (ndt.is_levenberg_marguardt)
+    {
+        ndt.is_gauss_newton = false;
+    }
+
+    ImGui::Checkbox("ndt poses expressed as camera<-world (cw)", &ndt.is_cw);
+    if (ndt.is_cw)
+    {
+        ndt.is_wc = false;
+    }
+    ImGui::SameLine();
+    ImGui::Checkbox("ndt poses expressed as camera->world (wc)", &ndt.is_wc);
+    if (ndt.is_wc)
+    {
+        ndt.is_cw = false;
+    }
+
+    ImGui::Checkbox("ndt Tait-Bryan angles (om fi ka: RxRyRz)", &ndt.is_tait_bryan_angles);
+    if (ndt.is_tait_bryan_angles)
+    {
+        ndt.is_quaternion = false;
+        ndt.is_rodrigues = false;
+    }
+
+    ImGui::SameLine();
+    ImGui::Checkbox("ndt Quaternion (q0 q1 q2 q3)", &ndt.is_quaternion);
+    if (ndt.is_quaternion)
+    {
+        ndt.is_tait_bryan_angles = false;
+        ndt.is_rodrigues = false;
+    }
+
+    ImGui::SameLine();
+    ImGui::Checkbox("ndt Rodrigues (sx sy sz)", &ndt.is_rodrigues);
+    if (ndt.is_rodrigues)
+    {
+        ndt.is_tait_bryan_angles = false;
+        ndt.is_quaternion = false;
+    }
+
+    if (ImGui::Button("compute mean mahalanobis distance"))
+    {
+        double rms_initial = 0.0;
+        double rms_final = 0.0;
+        double mui = 0.0;
+        ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket);
+    }
+
+    ImGui::Text("--------------------------------------------------------------------------------------------------------");
+
+    if (ImGui::Button("ndt_optimization(Lie-algebra left Jacobian)"))
+    {
+        // icp.optimize_source_to_target_lie_algebra_left_jacobian(point_clouds_container);
+        ndt.optimize_lie_algebra_left_jacobian(session.point_clouds_container.point_clouds, compute_mean_and_cov_for_bucket);
+    }
+    if (ImGui::Button("ndt_optimization(Lie-algebra right Jacobian)"))
+    {
+        // icp.optimize_source_to_target_lie_algebra_right_jacobian(point_clouds_container);
+        ndt.optimize_lie_algebra_right_jacobian(session.point_clouds_container.point_clouds, compute_mean_and_cov_for_bucket);
+    }
+
+    ImGui::Text("--------------------------------------------------------------------------------------------------------");
+
+    ImGui::Checkbox("generalized", &ndt.is_generalized);
+
+    if (ndt.is_generalized)
+    {
+        ImGui::InputDouble("sigma_r", &ndt.sigma_r, 0.01, 0.01);
+        ImGui::InputDouble("sigma_polar_angle_rad", &ndt.sigma_polar_angle, 0.0001, 0.0001);
+        ImGui::InputDouble("sigma_azimuthal_angle_rad", &ndt.sigma_azimuthal_angle, 0.0001, 0.0001);
+        ImGui::InputInt("num_extended_points", &ndt.num_extended_points, 1, 1);
+
+        ImGui::Checkbox("compute_mean_and_cov_for_bucket", &compute_mean_and_cov_for_bucket);
+    }
+
+    if (ImGui::Button("Set Zoller+Fröhlich TLS Imager 5006i errors"))
+    {
+        ndt.sigma_r = 0.0068;
+        ndt.sigma_polar_angle = 0.007 * DEG_TO_RAD;
+        ndt.sigma_azimuthal_angle = 0.007 * DEG_TO_RAD;
+    }
+
+    if (ImGui::Button("Set Zoller+Fröhlich TLS Imager 5010C errors"))
+    {
+        ndt.sigma_r = 0.01;
+        ndt.sigma_polar_angle = 0.007 * DEG_TO_RAD;
+        ndt.sigma_azimuthal_angle = 0.007 * DEG_TO_RAD;
+    }
+
+    if (ImGui::Button("Set Zoller+Fröhlich TLS Imager 5016 errors"))
+    {
+        ndt.sigma_r = 0.00025;
+        ndt.sigma_polar_angle = 0.004 * DEG_TO_RAD;
+        ndt.sigma_azimuthal_angle = 0.004 * DEG_TO_RAD;
+    }
+    if (ImGui::Button("Set Faro Focus3D errors"))
+    {
+        ndt.sigma_r = 0.001;
+        ndt.sigma_polar_angle = 19.0 * (1.0 / 3600.0) * DEG_TO_RAD;
+        ndt.sigma_azimuthal_angle = 19.0 * (1.0 / 3600.0) * DEG_TO_RAD;
+    }
+    if (ImGui::Button("Set Leica ScanStation C5 C10 errors"))
+    {
+        ndt.sigma_r = 0.006;
+        ndt.sigma_polar_angle = 0.00006;
+        ndt.sigma_azimuthal_angle = 0.00006;
+    }
+    if (ImGui::Button("Set Riegl VZ400 errors"))
+    {
+        ndt.sigma_r = 0.005;
+        ndt.sigma_polar_angle = 0.0005 * DEG_TO_RAD + 0.0003;     // Laser Beam Dicvergence
+        ndt.sigma_azimuthal_angle = 0.0005 * DEG_TO_RAD + 0.0003; // Laser Beam Dicvergence
+    }
+    if (ImGui::Button("Set Leica HDS6100 errors"))
+    {
+        ndt.sigma_r = 0.009;
+        ndt.sigma_polar_angle = 0.000125;
+        ndt.sigma_azimuthal_angle = 0.000125;
+    }
+    if (ImGui::Button("Set Leica P40 errors"))
+    {
+        ndt.sigma_r = 0.0012;
+        ndt.sigma_polar_angle = 8.0 / 3600;
+        ndt.sigma_azimuthal_angle = 8.0 / 3600;
+    }
+#endif
+}
+
+void loop_closure_gui()
+{
+    if (ImGui::Begin("Manual Pose Graph Loop Closure Mode", &is_loop_closure_gui, ImGuiWindowFlags_AlwaysAutoResize))
+    {
+        if (ImGui::Button("Optimize GRAPH"))
+        {
+            for (int i = 0; i < 100; i++)
+            {
+                std::cout << "Iteration [" << i + 1 << "] of: " << 100 << std::endl;
+                optimize(sessions, edges, motion_model_weights);
+            }
+            optimized = true;
+        }
+
+        ImGui::Checkbox("update_rotation_center", &update_rotation_center);
+
+        //
+        auto point_cloud_upper = sessions[first_session_index].point_clouds_container.point_clouds.size() - 1;
+
+        ImGui::InputInt("gui_point_size", &gui_point_size);
+        if (gui_point_size < 1)
+            gui_point_size = 1;
+
+        ImGui::Text("Num edge extended:");
+
+        ImGui::Text("before: ");
+        ImGui::SameLine();
+        ImGui::PushItemWidth(ImGuiNumberWidth);
+        ImGui::SliderInt("##fs", &num_edge_extended_before, 0, point_cloud_upper);
+        if (ImGui::IsItemHovered())
+            ImGui::SetTooltip("min 0; max %zu", point_cloud_upper);
+        ImGui::SameLine();
+        ImGui::InputInt("##fi", &num_edge_extended_before, 1, 5);
+        if (ImGui::IsItemHovered())
+            ImGui::SetTooltip("min 0; max %zu", point_cloud_upper);
+        if (num_edge_extended_before < 0)
+            num_edge_extended_before = 0;
+        if (num_edge_extended_before >= point_cloud_upper)
+            num_edge_extended_before = point_cloud_upper;
+
+        point_cloud_upper = sessions[second_session_index].point_clouds_container.point_clouds.size() - 1;
+
+        ImGui::Text(" after: ");
+        ImGui::SameLine();
+
+        ImGui::SliderInt("##ts", &num_edge_extended_after, index_loop_closure_target, static_cast<int>(point_cloud_upper));
+        if (ImGui::IsItemHovered())
+            ImGui::SetTooltip("min 0; max %zu", point_cloud_upper);
+        ImGui::SameLine();
+        ImGui::InputInt("##ti", &num_edge_extended_after, 1, 5);
+        if (ImGui::IsItemHovered())
+            ImGui::SetTooltip("min 0; max %zu", point_cloud_upper);
+        if (num_edge_extended_after < 0)
+            num_edge_extended_after = 0;
+        if (num_edge_extended_after >= point_cloud_upper)
+            num_edge_extended_after = point_cloud_upper;
+        ImGui::PopItemWidth();
+        //
+
+        if (!manipulate_active_edge)
+        {
+            ImGui::InputInt("index_loop_closure_source", &index_loop_closure_source);
+            if (index_loop_closure_source < 0)
+                index_loop_closure_source = 0;
+            if (index_loop_closure_source >= sessions[first_session_index].point_clouds_container.point_clouds.size() - 1)
+                index_loop_closure_source = sessions[first_session_index].point_clouds_container.point_clouds.size() - 1;
+            ImGui::InputInt("index_loop_closure_target", &index_loop_closure_target);
+            if (index_loop_closure_target < 0)
+                index_loop_closure_target = 0;
+            if (index_loop_closure_target >= sessions[second_session_index].point_clouds_container.point_clouds.size() - 1)
+                index_loop_closure_target = sessions[second_session_index].point_clouds_container.point_clouds.size() - 1;
+        }
+        if (ImGui::Button("Add edge"))
+        {
+            Edge edge;
+            edge.index_from = index_loop_closure_source;
+            edge.index_to = index_loop_closure_target;
+            edge.index_session_from = first_session_index;
+            edge.index_session_to = second_session_index;
+
+            edge.relative_pose_tb = pose_tait_bryan_from_affine_matrix(
+                sessions[first_session_index].point_clouds_container.point_clouds[index_loop_closure_source].m_pose.inverse() *
+                sessions[second_session_index].point_clouds_container.point_clouds[index_loop_closure_target].m_pose);
+
+            edge.relative_pose_tb_weights.px = 1000000.0;
+            edge.relative_pose_tb_weights.py = 1000000.0;
+            edge.relative_pose_tb_weights.pz = 1000000.0;
+            edge.relative_pose_tb_weights.om = 1000000.0;
+            edge.relative_pose_tb_weights.fi = 1000000.0;
+            edge.relative_pose_tb_weights.ka = 1000000.0;
+
+            edges.push_back(edge);
+
+            index_active_edge = edges.size() - 1;
+        }
+
+        std::string number_active_edges = "number_edges: " + std::to_string(edges.size());
+        ImGui::Text(number_active_edges.c_str());
+        if (edges.size() > 0)
+        {
+            ImGui::Checkbox("manipulate_active_edge", &manipulate_active_edge);
+            if (manipulate_active_edge)
+            {
+                int remove_edge_index = -1;
+                if (ImGui::Button("remove active edge"))
+                {
+                    edge_gizmo = false;
+                    remove_edge_index = index_active_edge;
+                }
+
+                int prev_index_active_edge = index_active_edge;
+
+                if (!edge_gizmo)
+                {
+                    bool is_gizmo = false;
+
+                    for (const auto& s : sessions)
+                    {
+                        if (s.is_gizmo)
+                            is_gizmo = true;
+                    }
+
+                    if (!is_gizmo)
+                    {
+                        ImGui::InputInt("index_active_edge", &index_active_edge);
+
+                        if (index_active_edge < 0)
+                            index_active_edge = 0;
+                        if (index_active_edge >= (int)edges.size())
+                            index_active_edge = (int)edges.size() - 1;
+                    }
+                }
+
+                std::string txt = "index_session_from: " + std::to_string(edges[index_active_edge].index_session_from);
+                ImGui::Text(txt.c_str());
+                txt = "index_session_to: " + std::to_string(edges[index_active_edge].index_session_to);
+                ImGui::Text(txt.c_str());
+                txt = "index_from: " + std::to_string(edges[index_active_edge].index_from);
+                ImGui::Text(txt.c_str());
+                txt = "index_to: " + std::to_string(edges[index_active_edge].index_to);
+                ImGui::Text(txt.c_str());
+
+                if (remove_edge_index != -1)
+                {
+                    std::vector<Edge> new_edges;
+                    for (size_t i = 0; i < edges.size(); i++)
+                    {
+                        if (remove_edge_index != i)
+                            new_edges.push_back(edges[i]);
+                    }
+                    edges = new_edges;
+
+                    index_active_edge = remove_edge_index - 1;
+                    manipulate_active_edge = false;
+                }
+
+                bool prev_gizmo = edge_gizmo;
+                ImGui::Checkbox("gizmo", &edge_gizmo);
+
+                if (prev_gizmo != edge_gizmo)
+                {
+                    auto m_to = sessions[edges[index_active_edge].index_session_from]
+                                    .point_clouds_container.point_clouds[edges[index_active_edge].index_from]
+                                    .m_pose *
+                        affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                    m_gizmo[0] = (float)m_to(0, 0);
+                    m_gizmo[1] = (float)m_to(1, 0);
+                    m_gizmo[2] = (float)m_to(2, 0);
+                    m_gizmo[3] = (float)m_to(3, 0);
+                    m_gizmo[4] = (float)m_to(0, 1);
+                    m_gizmo[5] = (float)m_to(1, 1);
+                    m_gizmo[6] = (float)m_to(2, 1);
+                    m_gizmo[7] = (float)m_to(3, 1);
+                    m_gizmo[8] = (float)m_to(0, 2);
+                    m_gizmo[9] = (float)m_to(1, 2);
+                    m_gizmo[10] = (float)m_to(2, 2);
+                    m_gizmo[11] = (float)m_to(3, 2);
+                    m_gizmo[12] = (float)m_to(0, 3);
+                    m_gizmo[13] = (float)m_to(1, 3);
+                    m_gizmo[14] = (float)m_to(2, 3);
+                    m_gizmo[15] = (float)m_to(3, 3);
+                }
+                if (!edge_gizmo)
+                {
+                    if (ImGui::Button("ICP"))
+                    {
+                        std::cout << "Iterative Closest Point" << std::endl;
+                        if (sessions[edges[index_active_edge].index_session_from].is_ground_truth &&
+                            sessions[edges[index_active_edge].index_session_to].is_ground_truth)
+                        {
+                            std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl;
+                        }
+                        else
+                        {
+                            bool is_with_ground_truth = false;
+                            if (sessions[edges[index_active_edge].index_session_from].is_ground_truth ||
+                                sessions[edges[index_active_edge].index_session_to].is_ground_truth)
+                            {
+                                is_with_ground_truth = true;
+                            }
+
+                            if (is_with_ground_truth)
+                            {
+                                int index_session_from = -1;
+                                int index_session_to = -1;
+                                int index_from = -1;
+                                int index_to = -1;
+
+                                if (sessions[edges[index_active_edge].index_session_from].is_ground_truth)
+                                {
+                                    index_session_from = edges[index_active_edge].index_session_from;
+                                    index_session_to = edges[index_active_edge].index_session_to;
+                                    index_from = edges[index_active_edge].index_from;
+                                    index_to = edges[index_active_edge].index_to;
+                                }
+                                else
+                                {
+                                    index_session_from = edges[index_active_edge].index_session_to;
+                                    index_session_to = edges[index_active_edge].index_session_from;
+                                    index_from = edges[index_active_edge].index_to;
+                                    index_to = edges[index_active_edge].index_from;
+                                }
+
+                                double x_min = 1000000000000.0;
+                                double y_min = 1000000000000.0;
+                                double z_min = 1000000000000.0;
+                                double x_max = -1000000000000.0;
+                                double y_max = -1000000000000.0;
+                                double z_max = -1000000000000.0;
+
+                                auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to];
+
+                                for (const auto& p : points_to.points_local)
+                                {
+                                    auto pg = points_to.m_pose * p;
+                                    if (pg.x() < x_min)
+                                        x_min = pg.x();
+                                    if (pg.y() < y_min)
+                                        y_min = pg.y();
+                                    if (pg.z() < z_min)
+                                        z_min = pg.z();
+
+                                    if (pg.x() > x_max)
+                                        x_max = pg.x();
+                                    if (pg.y() > y_max)
+                                        y_max = pg.y();
+                                    if (pg.z() > z_max)
+                                        z_max = pg.z();
+                                }
+                                auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from];
+                                std::vector<Eigen::Vector3d> ground_truth;
+                                for (const auto& p : points_from.points_local)
+                                {
+                                    auto pg = points_from.m_pose * p;
+                                    if (pg.x() > x_min && pg.x() < x_max)
+                                    {
+                                        if (pg.y() > y_min && pg.y() < y_max)
+                                        {
+                                            if (pg.z() > z_min && pg.z() < z_max)
+                                                ground_truth.push_back(p);
+                                        }
+                                    }
+                                }
+
+                                int number_of_iterations = 10;
+                                PairWiseICP icp;
+                                auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                                std::vector<Eigen::Vector3d> source =
+                                    sessions[edges[index_active_edge].index_session_to]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_to]
+                                        .points_local;
+                                std::vector<Eigen::Vector3d> target =
+                                    ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local;
+
+                                if (icp.compute(source, target, search_radius, number_of_iterations, m_pose))
+                                    edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose);
+                            }
+                            else
+                            {
+                                int number_of_iterations = 10;
+                                PairWiseICP icp;
+                                auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                                std::vector<Eigen::Vector3d> source;
+                                auto& e = edges[index_active_edge];
+                                for (int i = -num_edge_extended_before; i <= num_edge_extended_after; i++)
+                                {
+                                    int index_src = e.index_to + i;
+                                    if (index_src >= 0 &&
+                                        index_src <
+                                            sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.size())
+                                    {
+                                        Eigen::Affine3d m_src = sessions[edges[index_active_edge].index_session_to]
+                                                                    .point_clouds_container.point_clouds.at(index_src)
+                                                                    .m_pose;
+                                        for (int k = 0; k < sessions[edges[index_active_edge].index_session_to]
+                                                                .point_clouds_container.point_clouds[index_src]
+                                                                .points_local.size();
+                                             k++)
+                                        {
+                                            Eigen::Vector3d p_g = m_src *
+                                                sessions[edges[index_active_edge].index_session_to]
+                                                    .point_clouds_container.point_clouds[index_src]
+                                                    .points_local[k];
+                                            source.push_back(p_g);
+                                        }
+                                        // point_clouds_container.point_clouds.at(index_src).render(m_src, 1);
+                                    }
+                                }
+                                std::vector<Eigen::Vector3d> target;
+
+                                for (int i = -num_edge_extended_before; i <= num_edge_extended_after; i++)
+                                {
+                                    int index_trg = e.index_from + i;
+                                    if (index_trg >= 0 &&
+                                        index_trg < sessions[edges[index_active_edge].index_session_from]
+                                                        .point_clouds_container.point_clouds.size())
+                                    {
+                                        Eigen::Affine3d m_trg = sessions[edges[index_active_edge].index_session_from]
+                                                                    .point_clouds_container.point_clouds.at(index_trg)
+                                                                    .m_pose;
+                                        for (int k = 0; k < sessions[edges[index_active_edge].index_session_from]
+                                                                .point_clouds_container.point_clouds[index_trg]
+                                                                .points_local.size();
+                                             k++)
+                                        {
+                                            Eigen::Vector3d p_g = m_trg *
+                                                sessions[edges[index_active_edge].index_session_from]
+                                                    .point_clouds_container.point_clouds[index_trg]
+                                                    .points_local[k];
+                                            target.push_back(p_g);
+                                        }
+                                        // point_clouds_container.point_clouds.at(index_src).render(m_src, 1);
+                                    }
+                                }
+
+                                Eigen::Affine3d m_src_inv = sessions[edges[index_active_edge].index_session_to]
+                                                                .point_clouds_container.point_clouds[e.index_to]
+                                                                .m_pose.inverse();
+
+                                for (auto& p : source)
+                                {
+                                    p = m_src_inv * p;
+                                }
+
+                                Eigen::Affine3d m_trg_inv = sessions[edges[index_active_edge].index_session_from]
+                                                                .point_clouds_container.point_clouds[e.index_from]
+                                                                .m_pose.inverse();
+
+                                for (auto& p : target)
+                                {
+                                    p = m_trg_inv * p;
+                                }
+
+                                if (icp.compute(source, target, search_radius, number_of_iterations, m_pose))
+                                    edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose);
+                            }
+                        }
+                    }
+                    ImGui::SameLine();
+                    ImGui::InputDouble("search_radius", &search_radius);
+                    if (search_radius < 0.01)
+                        search_radius = 0.01;
+
+                    /////////////////////////////////
+                    if (ImGui::Button("ICP [search radius 2m]"))
+                    {
+                        float sr = 2.0;
+                        std::cout << "Iterative Closest Point" << std::endl;
+                        if (sessions[edges[index_active_edge].index_session_from].is_ground_truth &&
+                            sessions[edges[index_active_edge].index_session_to].is_ground_truth)
+                        {
+                            std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl;
+                        }
+                        else
+                        {
+                            bool is_with_ground_truth = false;
+                            if (sessions[edges[index_active_edge].index_session_from].is_ground_truth ||
+                                sessions[edges[index_active_edge].index_session_to].is_ground_truth)
+                            {
+                                is_with_ground_truth = true;
+                            }
+
+                            if (is_with_ground_truth)
+                            {
+                                int index_session_from = -1;
+                                int index_session_to = -1;
+                                int index_from = -1;
+                                int index_to = -1;
+
+                                if (sessions[edges[index_active_edge].index_session_from].is_ground_truth)
+                                {
+                                    index_session_from = edges[index_active_edge].index_session_from;
+                                    index_session_to = edges[index_active_edge].index_session_to;
+                                    index_from = edges[index_active_edge].index_from;
+                                    index_to = edges[index_active_edge].index_to;
+                                }
+                                else
+                                {
+                                    index_session_from = edges[index_active_edge].index_session_to;
+                                    index_session_to = edges[index_active_edge].index_session_from;
+                                    index_from = edges[index_active_edge].index_to;
+                                    index_to = edges[index_active_edge].index_from;
+                                }
+
+                                double x_min = 1000000000000.0;
+                                double y_min = 1000000000000.0;
+                                double z_min = 1000000000000.0;
+                                double x_max = -1000000000000.0;
+                                double y_max = -1000000000000.0;
+                                double z_max = -1000000000000.0;
+
+                                auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to];
+
+                                for (const auto& p : points_to.points_local)
+                                {
+                                    auto pg = points_to.m_pose * p;
+                                    if (pg.x() < x_min)
+                                        x_min = pg.x();
+                                    if (pg.y() < y_min)
+                                        y_min = pg.y();
+                                    if (pg.z() < z_min)
+                                        z_min = pg.z();
+
+                                    if (pg.x() > x_max)
+                                        x_max = pg.x();
+                                    if (pg.y() > y_max)
+                                        y_max = pg.y();
+                                    if (pg.z() > z_max)
+                                        z_max = pg.z();
+                                }
+                                auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from];
+                                std::vector<Eigen::Vector3d> ground_truth;
+                                for (const auto& p : points_from.points_local)
+                                {
+                                    auto pg = points_from.m_pose * p;
+                                    if (pg.x() > x_min && pg.x() < x_max)
+                                    {
+                                        if (pg.y() > y_min && pg.y() < y_max)
+                                        {
+                                            if (pg.z() > z_min && pg.z() < z_max)
+                                                ground_truth.push_back(p);
+                                        }
+                                    }
+                                }
+
+                                int number_of_iterations = 30;
+                                PairWiseICP icp;
+                                auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                                const std::vector<Eigen::Vector3d>& source =
+                                    sessions[edges[index_active_edge].index_session_to]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_to]
+                                        .points_local;
+                                const std::vector<Eigen::Vector3d>& target =
+                                    ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local;
+
+                                if (icp.compute(source, target, sr, number_of_iterations, m_pose))
+                                    edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose);
+                            }
+                            else
+                            {
+                                int number_of_iterations = 30;
+                                PairWiseICP icp;
+                                auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                                const std::vector<Eigen::Vector3d>& source =
+                                    sessions[edges[index_active_edge].index_session_to]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_to]
+                                        .points_local;
+                                const std::vector<Eigen::Vector3d>& target =
+                                    sessions[edges[index_active_edge].index_session_from]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_from]
+                                        .points_local;
+
+                                if (icp.compute(source, target, sr, number_of_iterations, m_pose))
+                                    edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose);
+                            }
+                        }
+                    }
+
+                    ImGui::SameLine();
+                    if (ImGui::Button("ICP [search radius 1m]"))
+                    {
+                        float sr = 1.0;
+                        std::cout << "Iterative Closest Point" << std::endl;
+                        if (sessions[edges[index_active_edge].index_session_from].is_ground_truth &&
+                            sessions[edges[index_active_edge].index_session_to].is_ground_truth)
+                        {
+                            std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl;
+                        }
+                        else
+                        {
+                            bool is_with_ground_truth = false;
+                            if (sessions[edges[index_active_edge].index_session_from].is_ground_truth ||
+                                sessions[edges[index_active_edge].index_session_to].is_ground_truth)
+                            {
+                                is_with_ground_truth = true;
+                            }
+
+                            if (is_with_ground_truth)
+                            {
+                                int index_session_from = -1;
+                                int index_session_to = -1;
+                                int index_from = -1;
+                                int index_to = -1;
+
+                                if (sessions[edges[index_active_edge].index_session_from].is_ground_truth)
+                                {
+                                    index_session_from = edges[index_active_edge].index_session_from;
+                                    index_session_to = edges[index_active_edge].index_session_to;
+                                    index_from = edges[index_active_edge].index_from;
+                                    index_to = edges[index_active_edge].index_to;
+                                }
+                                else
+                                {
+                                    index_session_from = edges[index_active_edge].index_session_to;
+                                    index_session_to = edges[index_active_edge].index_session_from;
+                                    index_from = edges[index_active_edge].index_to;
+                                    index_to = edges[index_active_edge].index_from;
+                                }
+
+                                double x_min = 1000000000000.0;
+                                double y_min = 1000000000000.0;
+                                double z_min = 1000000000000.0;
+                                double x_max = -1000000000000.0;
+                                double y_max = -1000000000000.0;
+                                double z_max = -1000000000000.0;
+
+                                auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to];
+
+                                for (const auto& p : points_to.points_local)
+                                {
+                                    auto pg = points_to.m_pose * p;
+                                    if (pg.x() < x_min)
+                                        x_min = pg.x();
+                                    if (pg.y() < y_min)
+                                        y_min = pg.y();
+                                    if (pg.z() < z_min)
+                                        z_min = pg.z();
+
+                                    if (pg.x() > x_max)
+                                        x_max = pg.x();
+                                    if (pg.y() > y_max)
+                                        y_max = pg.y();
+                                    if (pg.z() > z_max)
+                                        z_max = pg.z();
+                                }
+                                auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from];
+                                std::vector<Eigen::Vector3d> ground_truth;
+                                for (const auto& p : points_from.points_local)
+                                {
+                                    auto pg = points_from.m_pose * p;
+                                    if (pg.x() > x_min && pg.x() < x_max)
+                                    {
+                                        if (pg.y() > y_min && pg.y() < y_max)
+                                        {
+                                            if (pg.z() > z_min && pg.z() < z_max)
+                                                ground_truth.push_back(p);
+                                        }
+                                    }
+                                }
+
+                                int number_of_iterations = 30;
+                                PairWiseICP icp;
+                                auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                                const std::vector<Eigen::Vector3d>& source =
+                                    sessions[edges[index_active_edge].index_session_to]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_to]
+                                        .points_local;
+                                const std::vector<Eigen::Vector3d>& target =
+                                    ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local;
+
+                                if (icp.compute(source, target, sr, number_of_iterations, m_pose))
+                                    edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose);
+                            }
+                            else
+                            {
+                                int number_of_iterations = 30;
+                                PairWiseICP icp;
+                                auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                                const std::vector<Eigen::Vector3d>& source =
+                                    sessions[edges[index_active_edge].index_session_to]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_to]
+                                        .points_local;
+                                const std::vector<Eigen::Vector3d>& target =
+                                    sessions[edges[index_active_edge].index_session_from]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_from]
+                                        .points_local;
+
+                                if (icp.compute(source, target, sr, number_of_iterations, m_pose))
+                                    edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose);
+                            }
+                        }
+                    }
+                    ImGui::SameLine();
+                    if (ImGui::Button("ICP [search radius 0.5m]"))
+                    {
+                        float sr = 0.5;
+                        std::cout << "Iterative Closest Point" << std::endl;
+                        if (sessions[edges[index_active_edge].index_session_from].is_ground_truth &&
+                            sessions[edges[index_active_edge].index_session_to].is_ground_truth)
+                        {
+                            std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl;
+                        }
+                        else
+                        {
+                            bool is_with_ground_truth = false;
+                            if (sessions[edges[index_active_edge].index_session_from].is_ground_truth ||
+                                sessions[edges[index_active_edge].index_session_to].is_ground_truth)
+                            {
+                                is_with_ground_truth = true;
+                            }
+
+                            if (is_with_ground_truth)
+                            {
+                                int index_session_from = -1;
+                                int index_session_to = -1;
+                                int index_from = -1;
+                                int index_to = -1;
+
+                                if (sessions[edges[index_active_edge].index_session_from].is_ground_truth)
+                                {
+                                    index_session_from = edges[index_active_edge].index_session_from;
+                                    index_session_to = edges[index_active_edge].index_session_to;
+                                    index_from = edges[index_active_edge].index_from;
+                                    index_to = edges[index_active_edge].index_to;
+                                }
+                                else
+                                {
+                                    index_session_from = edges[index_active_edge].index_session_to;
+                                    index_session_to = edges[index_active_edge].index_session_from;
+                                    index_from = edges[index_active_edge].index_to;
+                                    index_to = edges[index_active_edge].index_from;
+                                }
+
+                                double x_min = 1000000000000.0;
+                                double y_min = 1000000000000.0;
+                                double z_min = 1000000000000.0;
+                                double x_max = -1000000000000.0;
+                                double y_max = -1000000000000.0;
+                                double z_max = -1000000000000.0;
+
+                                auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to];
+
+                                for (const auto& p : points_to.points_local)
+                                {
+                                    auto pg = points_to.m_pose * p;
+                                    if (pg.x() < x_min)
+                                        x_min = pg.x();
+                                    if (pg.y() < y_min)
+                                        y_min = pg.y();
+                                    if (pg.z() < z_min)
+                                        z_min = pg.z();
+
+                                    if (pg.x() > x_max)
+                                        x_max = pg.x();
+                                    if (pg.y() > y_max)
+                                        y_max = pg.y();
+                                    if (pg.z() > z_max)
+                                        z_max = pg.z();
+                                }
+                                auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from];
+                                std::vector<Eigen::Vector3d> ground_truth;
+                                for (const auto& p : points_from.points_local)
+                                {
+                                    auto pg = points_from.m_pose * p;
+                                    if (pg.x() > x_min && pg.x() < x_max)
+                                    {
+                                        if (pg.y() > y_min && pg.y() < y_max)
+                                        {
+                                            if (pg.z() > z_min && pg.z() < z_max)
+                                                ground_truth.push_back(p);
+                                        }
+                                    }
+                                }
+
+                                int number_of_iterations = 30;
+                                PairWiseICP icp;
+                                auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                                const std::vector<Eigen::Vector3d>& source =
+                                    sessions[edges[index_active_edge].index_session_to]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_to]
+                                        .points_local;
+                                const std::vector<Eigen::Vector3d>& target =
+                                    ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local;
+
+                                if (icp.compute(source, target, sr, number_of_iterations, m_pose))
+                                {
+                                    edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose);
+                                }
+                            }
+                            else
+                            {
+                                int number_of_iterations = 30;
+                                PairWiseICP icp;
+                                auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                                const std::vector<Eigen::Vector3d>& source =
+                                    sessions[edges[index_active_edge].index_session_to]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_to]
+                                        .points_local;
+                                const std::vector<Eigen::Vector3d>& target =
+                                    sessions[edges[index_active_edge].index_session_from]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_from]
+                                        .points_local;
+
+                                if (icp.compute(source, target, sr, number_of_iterations, m_pose))
+                                {
+                                    edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose);
+                                }
+                            }
+                        }
+                    }
+                    ImGui::SameLine();
+                    if (ImGui::Button("ICP [search radius 0.25m]"))
+                    {
+                        float sr = 0.25;
+                        std::cout << "Iterative Closest Point" << std::endl;
+                        if (sessions[edges[index_active_edge].index_session_from].is_ground_truth &&
+                            sessions[edges[index_active_edge].index_session_to].is_ground_truth)
+                        {
+                            std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl;
+                        }
+                        else
+                        {
+                            bool is_with_ground_truth = false;
+                            if (sessions[edges[index_active_edge].index_session_from].is_ground_truth ||
+                                sessions[edges[index_active_edge].index_session_to].is_ground_truth)
+                            {
+                                is_with_ground_truth = true;
+                            }
+
+                            if (is_with_ground_truth)
+                            {
+                                int index_session_from = -1;
+                                int index_session_to = -1;
+                                int index_from = -1;
+                                int index_to = -1;
+
+                                if (sessions[edges[index_active_edge].index_session_from].is_ground_truth)
+                                {
+                                    index_session_from = edges[index_active_edge].index_session_from;
+                                    index_session_to = edges[index_active_edge].index_session_to;
+                                    index_from = edges[index_active_edge].index_from;
+                                    index_to = edges[index_active_edge].index_to;
+                                }
+                                else
+                                {
+                                    index_session_from = edges[index_active_edge].index_session_to;
+                                    index_session_to = edges[index_active_edge].index_session_from;
+                                    index_from = edges[index_active_edge].index_to;
+                                    index_to = edges[index_active_edge].index_from;
+                                }
+
+                                double x_min = 1000000000000.0;
+                                double y_min = 1000000000000.0;
+                                double z_min = 1000000000000.0;
+                                double x_max = -1000000000000.0;
+                                double y_max = -1000000000000.0;
+                                double z_max = -1000000000000.0;
+
+                                auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to];
+
+                                for (const auto& p : points_to.points_local)
+                                {
+                                    auto pg = points_to.m_pose * p;
+                                    if (pg.x() < x_min)
+                                        x_min = pg.x();
+                                    if (pg.y() < y_min)
+                                        y_min = pg.y();
+                                    if (pg.z() < z_min)
+                                        z_min = pg.z();
+
+                                    if (pg.x() > x_max)
+                                        x_max = pg.x();
+                                    if (pg.y() > y_max)
+                                        y_max = pg.y();
+                                    if (pg.z() > z_max)
+                                        z_max = pg.z();
+                                }
+                                auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from];
+                                std::vector<Eigen::Vector3d> ground_truth;
+                                for (const auto& p : points_from.points_local)
+                                {
+                                    auto pg = points_from.m_pose * p;
+                                    if (pg.x() > x_min && pg.x() < x_max)
+                                    {
+                                        if (pg.y() > y_min && pg.y() < y_max)
+                                        {
+                                            if (pg.z() > z_min && pg.z() < z_max)
+                                                ground_truth.push_back(p);
+                                        }
+                                    }
+                                }
+
+                                int number_of_iterations = 30;
+                                PairWiseICP icp;
+                                auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                                const std::vector<Eigen::Vector3d>& source =
+                                    sessions[edges[index_active_edge].index_session_to]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_to]
+                                        .points_local;
+                                const std::vector<Eigen::Vector3d>& target =
+                                    ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local;
+
+                                if (icp.compute(source, target, sr, number_of_iterations, m_pose))
+                                {
+                                    edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose);
+                                }
+                            }
+                            else
+                            {
+                                int number_of_iterations = 30;
+                                PairWiseICP icp;
+                                auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                                const std::vector<Eigen::Vector3d>& source =
+                                    sessions[edges[index_active_edge].index_session_to]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_to]
+                                        .points_local;
+                                const std::vector<Eigen::Vector3d>& target =
+                                    sessions[edges[index_active_edge].index_session_from]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_from]
+                                        .points_local;
+
+                                if (icp.compute(source, target, sr, number_of_iterations, m_pose))
+                                {
+                                    edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose);
+                                }
+                            }
+                        }
+                    }
+                    ImGui::SameLine();
+                    if (ImGui::Button("ICP [search radius 0.1m]"))
+                    {
+                        float sr = 0.1;
+                        std::cout << "Iterative Closest Point" << std::endl;
+                        if (sessions[edges[index_active_edge].index_session_from].is_ground_truth &&
+                            sessions[edges[index_active_edge].index_session_to].is_ground_truth)
+                        {
+                            std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl;
+                        }
+                        else
+                        {
+                            bool is_with_ground_truth = false;
+                            if (sessions[edges[index_active_edge].index_session_from].is_ground_truth ||
+                                sessions[edges[index_active_edge].index_session_to].is_ground_truth)
+                            {
+                                is_with_ground_truth = true;
+                            }
+
+                            if (is_with_ground_truth)
+                            {
+                                int index_session_from = -1;
+                                int index_session_to = -1;
+                                int index_from = -1;
+                                int index_to = -1;
+
+                                if (sessions[edges[index_active_edge].index_session_from].is_ground_truth)
+                                {
+                                    index_session_from = edges[index_active_edge].index_session_from;
+                                    index_session_to = edges[index_active_edge].index_session_to;
+                                    index_from = edges[index_active_edge].index_from;
+                                    index_to = edges[index_active_edge].index_to;
+                                }
+                                else
+                                {
+                                    index_session_from = edges[index_active_edge].index_session_to;
+                                    index_session_to = edges[index_active_edge].index_session_from;
+                                    index_from = edges[index_active_edge].index_to;
+                                    index_to = edges[index_active_edge].index_from;
+                                }
+
+                                double x_min = 1000000000000.0;
+                                double y_min = 1000000000000.0;
+                                double z_min = 1000000000000.0;
+                                double x_max = -1000000000000.0;
+                                double y_max = -1000000000000.0;
+                                double z_max = -1000000000000.0;
+
+                                auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to];
+
+                                for (const auto& p : points_to.points_local)
+                                {
+                                    auto pg = points_to.m_pose * p;
+                                    if (pg.x() < x_min)
+                                        x_min = pg.x();
+                                    if (pg.y() < y_min)
+                                        y_min = pg.y();
+                                    if (pg.z() < z_min)
+                                        z_min = pg.z();
+
+                                    if (pg.x() > x_max)
+                                        x_max = pg.x();
+                                    if (pg.y() > y_max)
+                                        y_max = pg.y();
+                                    if (pg.z() > z_max)
+                                        z_max = pg.z();
+                                }
+                                auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from];
+                                std::vector<Eigen::Vector3d> ground_truth;
+                                for (const auto& p : points_from.points_local)
+                                {
+                                    auto pg = points_from.m_pose * p;
+                                    if (pg.x() > x_min && pg.x() < x_max)
+                                    {
+                                        if (pg.y() > y_min && pg.y() < y_max)
+                                        {
+                                            if (pg.z() > z_min && pg.z() < z_max)
+                                                ground_truth.push_back(p);
+                                        }
+                                    }
+                                }
+
+                                int number_of_iterations = 30;
+                                PairWiseICP icp;
+                                auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                                const std::vector<Eigen::Vector3d>& source =
+                                    sessions[edges[index_active_edge].index_session_to]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_to]
+                                        .points_local;
+                                const std::vector<Eigen::Vector3d>& target =
+                                    ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local;
+
+                                if (icp.compute(source, target, sr, number_of_iterations, m_pose))
+                                {
+                                    edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose);
+                                }
+                            }
+                            else
+                            {
+                                int number_of_iterations = 30;
+                                PairWiseICP icp;
+                                auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                                const std::vector<Eigen::Vector3d>& source =
+                                    sessions[edges[index_active_edge].index_session_to]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_to]
+                                        .points_local;
+                                const std::vector<Eigen::Vector3d>& target =
+                                    sessions[edges[index_active_edge].index_session_from]
+                                        .point_clouds_container.point_clouds[edges[index_active_edge].index_from]
+                                        .points_local;
+
+                                if (icp.compute(source, target, sr, number_of_iterations, m_pose))
+                                {
+                                    edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose);
+                                }
+                            }
+                        }
+                    }
+#if 0
+                    if (ImGui::Button("Save src"))
+                    {
+                        const auto output_file_name = mandeye::fd::SaveFileDialog("Output file name", mandeye::fd::LAS_LAZ_filter, ".laz");
+                        std::cout << "laz file to save: '" << output_file_name << "'" << std::endl;
+
+                        if (output_file_name.size() > 0)
+                        {
+                            std::vector<Eigen::Vector3d> source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local;
+                            std::vector<Eigen::Vector3d> pointcloud;
+                            std::vector<unsigned short> intensity;
+                            std::vector<double> timestamps;
+
+                            for (size_t i = 0; i < source.size(); i++)
+                            {
+                                pointcloud.push_back(source[i]);
+                                intensity.push_back(0);
+                                timestamps.push_back(0.0);
+                            }
+
+                            exportLaz(
+                                output_file_name[0],
+                                pointcloud,
+                                intensity,
+                                timestamps);
+                        }
+
+                        /*int number_of_iterations = 30;
+                        PairWiseICP icp;
+                        auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                        std::vector<Eigen::Vector3d> source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local;
+                        std::vector<Eigen::Vector3d> target = sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local;
+
+                        if (icp.compute(source, target, sr, number_of_iterations, m_pose))
+                        {
+                            edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose);
+                        }*/
+                        //save
+                    }
+                    ImGui::SameLine();
+                    if (ImGui::Button("Save trg (transfromed only by rotation)"))
+                    {
+                    }
+#endif
+                    //////////////////////////////////
+                }
+            }
+        }
+
+        ImGui::End();
+    }
+}
+
+void save_trajectories_to_laz(
+    const Session& session,
+    const std::string& output_file_name,
+    float curve_consecutive_distance_meters,
+    float not_curve_consecutive_distance_meters,
+    bool is_trajectory_export_downsampling)
+{
+    std::vector<Eigen::Vector3d> pointcloud;
+    std::vector<unsigned short> intensity;
+    std::vector<double> timestamps;
+
+    float consecutive_distance = 0;
+    for (auto& p : session.point_clouds_container.point_clouds)
+    {
+        if (p.visible)
+        {
+            for (size_t i = 0; i < p.local_trajectory.size(); i++)
+            {
+                const auto& pp = p.local_trajectory[i].m_pose.translation();
+                Eigen::Vector3d vp;
+                vp = p.m_pose * pp; // + session.point_clouds_container.offset;
+
+                if (i > 0)
+                {
+                    double dist = (p.local_trajectory[i].m_pose.translation() - p.local_trajectory[i - 1].m_pose.translation()).norm();
+                    consecutive_distance += dist;
+                }
+
+                bool is_curve = false;
+
+                if (i > 100 && i < p.local_trajectory.size() - 100)
+                {
+                    Eigen::Vector3d position_prev = p.local_trajectory[i - 100].m_pose.translation();
+                    Eigen::Vector3d position_curr = p.local_trajectory[i].m_pose.translation();
+                    Eigen::Vector3d position_next = p.local_trajectory[i + 100].m_pose.translation();
+
+                    Eigen::Vector3d v1 = position_curr - position_prev;
+                    Eigen::Vector3d v2 = position_next - position_curr;
+
+                    if (v1.norm() > 0 && v2.norm() > 0)
+                    {
+                        double angle_deg = fabs(acos(v1.dot(v2) / (v1.norm() * v2.norm())) * RAD_TO_DEG);
+
+                        if (angle_deg > 10.0)
+                        {
+                            is_curve = true;
+                        }
+                    }
+                }
+                double tol = not_curve_consecutive_distance_meters;
+
+                if (is_curve)
+                {
+                    tol = curve_consecutive_distance_meters;
+                }
+
+                if (!is_trajectory_export_downsampling)
+                {
+                    pointcloud.push_back(vp);
+                    intensity.push_back(0);
+                    timestamps.push_back(p.local_trajectory[i].timestamps.first);
+                }
+                else
+                {
+                    if (consecutive_distance >= tol)
+                    {
+                        consecutive_distance = 0;
+                        pointcloud.push_back(vp);
+                        intensity.push_back(0);
+                        timestamps.push_back(p.local_trajectory[i].timestamps.first);
+                    }
+                }
+            }
+        }
+    }
+    // if (!exportLaz(output_file_name, pointcloud, intensity, gnss.offset_x, gnss.offset_y, gnss.offset_alt))
+    if (!exportLaz(
+            output_file_name,
+            pointcloud,
+            intensity,
+            timestamps,
+            session.point_clouds_container.offset.x(),
+            session.point_clouds_container.offset.y(),
+            session.point_clouds_container.offset.z()))
+    {
+        std::cout << "problem with saving file: " << output_file_name << std::endl;
+    }
+}
+
+void createDXFPolyline(const std::string& filename, const std::vector<Eigen::Vector3d>& points)
+{
+    std::ofstream dxfFile(filename);
+    dxfFile << std::setprecision(20);
+    if (!dxfFile.is_open())
+    {
+        std::cerr << "Failed to open file: " << filename << std::endl;
+        return;
+    }
+
+    // DXF header
+    dxfFile << "0\nSECTION\n2\nHEADER\n0\nENDSEC\n";
+    dxfFile << "0\nSECTION\n2\nTABLES\n0\nENDSEC\n";
+
+    // Start the ENTITIES section
+    dxfFile << "0\nSECTION\n2\nENTITIES\n";
+
+    // Start the POLYLINE entity
+    dxfFile << "0\nPOLYLINE\n";
+    dxfFile << "8\n0\n"; // Layer 0
+    dxfFile << "66\n1\n"; // Indicates the presence of vertices
+    dxfFile << "70\n8\n"; // 1 = Open polyline
+
+    // Write the VERTEX entities
+    for (const auto& point : points)
+    {
+        dxfFile << "0\nVERTEX\n";
+        dxfFile << "8\n0\n"; // Layer 0
+        dxfFile << "10\n" << point.x() << "\n"; // X coordinate
+        dxfFile << "20\n" << point.y() << "\n"; // Y coordinate
+        dxfFile << "30\n" << point.z() << "\n"; // Z coordinate
+    }
+
+    // End the POLYLINE
+    dxfFile << "0\nSEQEND\n";
+
+    // End the ENTITIES section
+    dxfFile << "0\nENDSEC\n";
+
+    // End the DXF file
+    dxfFile << "0\nEOF\n";
+
+    dxfFile.close();
+    std::cout << "DXF file created: " << filename << std::endl;
+}
+
+void save_trajectories(
+    Session& session,
+    const std::string& output_file_name,
+    float curve_consecutive_distance_meters,
+    float not_curve_consecutive_distance_meters,
+    bool is_trajectory_export_downsampling,
+    bool write_lidar_timestamp,
+    bool write_unix_timestamp,
+    bool use_quaternions,
+    bool save_to_dxf)
+{
+    std::ofstream outfile;
+    if (!save_to_dxf)
+    {
+        outfile.open(output_file_name);
+    }
+    if (save_to_dxf || outfile.good())
+    {
+        float consecutive_distance = 0;
+        std::vector<Eigen::Vector3d> polylinePoints;
+        for (auto& p : session.point_clouds_container.point_clouds)
+        {
+            if (p.visible)
+            {
+                for (size_t i = 0; i < p.local_trajectory.size(); i++)
+                {
+                    const auto& m = p.local_trajectory[i].m_pose;
+                    Eigen::Affine3d pose = p.m_pose * m;
+                    pose.translation() += session.point_clouds_container.offset;
+
+                    if (i > 0)
+                    {
+                        double dist = (p.local_trajectory[i].m_pose.translation() - p.local_trajectory[i - 1].m_pose.translation()).norm();
+                        consecutive_distance += dist;
+                    }
+
+                    bool is_curve = false;
+
+                    if (i > 100 && i < p.local_trajectory.size() - 100)
+                    {
+                        Eigen::Vector3d position_prev = p.local_trajectory[i - 100].m_pose.translation();
+                        Eigen::Vector3d position_curr = p.local_trajectory[i].m_pose.translation();
+                        Eigen::Vector3d position_next = p.local_trajectory[i + 100].m_pose.translation();
+
+                        Eigen::Vector3d v1 = position_curr - position_prev;
+                        Eigen::Vector3d v2 = position_next - position_curr;
+
+                        if (v1.norm() > 0 && v2.norm() > 0)
+                        {
+                            double angle_deg = fabs(acos(v1.dot(v2) / (v1.norm() * v2.norm())) * RAD_TO_DEG);
+
+                            if (angle_deg > 10.0)
+                                is_curve = true;
+                        }
+                    }
+                    double tol = not_curve_consecutive_distance_meters;
+
+                    if (is_curve)
+                        tol = curve_consecutive_distance_meters;
+
+                    if (!is_trajectory_export_downsampling || (is_trajectory_export_downsampling && consecutive_distance >= tol))
+                    {
+                        if (is_trajectory_export_downsampling)
+                            consecutive_distance = 0;
+                        if (save_to_dxf)
+                            polylinePoints.push_back(pose.translation());
+                        else
+                        {
+                            outfile << std::setprecision(20);
+
+                            if (write_lidar_timestamp)
+                                outfile << p.local_trajectory[i].timestamps.first << ",";
+                            if (write_unix_timestamp)
+                                outfile << p.local_trajectory[i].timestamps.second << ",";
+
+                            outfile << pose(0, 3) << "," << pose(1, 3) << "," << pose(2, 3) << ",";
+                            if (use_quaternions)
+                            {
+                                Eigen::Quaterniond q(pose.rotation());
+                                outfile << q.x() << "," << q.y() << "," << q.z() << "," << q.w() << std::endl;
+                            }
+                            else
+                                outfile << pose(0, 0) << "," << pose(0, 1) << "," << pose(0, 2) << "," << pose(1, 0) << "," << pose(1, 1)
+                                        << "," << pose(1, 2) << "," << pose(2, 0) << "," << pose(2, 1) << "," << pose(2, 2) << std::endl;
+                        }
+                    }
+                }
+            }
+        }
+        if (!save_to_dxf)
+            outfile.close();
+        else
+            createDXFPolyline(output_file_name, polylinePoints);
+    }
+}
+
+bool save_project_settings(const std::string& file_name, const ProjectSettings& _project_settings)
+{
+    std::cout << "saving file: '" << file_name << "'" << std::endl;
+
+    nlohmann::json jj;
+
+    nlohmann::json jsession_file_names;
+    for (const auto& pc : _project_settings.session_file_names)
+    {
+        nlohmann::json jfn{ { "session_file_name", pc } };
+        jsession_file_names.push_back(jfn);
+    }
+    jj["session_file_names"] = jsession_file_names;
+
+    nlohmann::json jloop_closure_edges;
+    for (const auto& edge : edges)
+    {
+        nlohmann::json jloop_closure_edge{
+            { "px", edge.relative_pose_tb.px },
+            { "py", edge.relative_pose_tb.py },
+            { "pz", edge.relative_pose_tb.pz },
+            { "om", edge.relative_pose_tb.om },
+            { "fi", edge.relative_pose_tb.fi },
+            { "ka", edge.relative_pose_tb.ka },
+            { "w_px", edge.relative_pose_tb_weights.px },
+            { "w_py", edge.relative_pose_tb_weights.py },
+            { "w_pz", edge.relative_pose_tb_weights.pz },
+            { "w_om", edge.relative_pose_tb_weights.om },
+            { "w_fi", edge.relative_pose_tb_weights.fi },
+            { "w_ka", edge.relative_pose_tb_weights.ka },
+            { "index_from", edge.index_from },
+            { "index_to", edge.index_to },
+            { "is_fixed_px", edge.is_fixed_px },
+            { "is_fixed_py", edge.is_fixed_py },
+            { "is_fixed_pz", edge.is_fixed_pz },
+            { "is_fixed_om", edge.is_fixed_om },
+            { "is_fixed_fi", edge.is_fixed_fi },
+            { "is_fixed_ka", edge.is_fixed_ka },
+            { "index_session_from", edge.index_session_from },
+            { "index_session_to", edge.index_session_to },
+        };
+        jloop_closure_edges.push_back(jloop_closure_edge);
+    }
+    jj["loop_closure_edges"] = jloop_closure_edges;
+
+    std::ofstream fs(file_name);
+    if (!fs.good())
+        return false;
+    fs << jj.dump(2);
+    fs.close();
+
+    return true;
+}
+
+void update_timestamp_offset()
+{
+    std::cout << "update_timestamp" << std::endl;
+    time_stamp_offset = std::numeric_limits<double>::max();
+
+    for (const auto& s : sessions)
+    {
+        if (!s.point_clouds_container.point_clouds.empty() && !s.point_clouds_container.point_clouds[0].local_trajectory.empty())
+        {
+            double ts = s.point_clouds_container.point_clouds[0].local_trajectory[0].timestamps.first;
+            if (ts < time_stamp_offset)
+                time_stamp_offset = ts;
+        }
+    }
+
+    std::cout << "new time_stamp_offset = " << time_stamp_offset << std::endl;
+}
+
+bool revert(std::vector<Session>& sessions)
+{
+    for (auto& session : sessions)
+    {
+        for (auto& pc : session.point_clouds_container.point_clouds)
+            pc.m_pose = pc.m_pose_temp;
+    }
+    return true;
+}
+
+bool revert_to_initial(std::vector<Session>& sessions)
+{
+    for (auto& session : sessions)
+    {
+        for (auto& pc : session.point_clouds_container.point_clouds)
+            pc.m_pose = pc.m_initial_pose;
+    }
+    return true;
+}
+
+bool save_results(std::vector<Session>& sessions)
+{
+    for (auto& session : sessions)
+    {
+        if (!session.is_ground_truth)
+        {
+            std::cout << "saving result to: " << session.point_clouds_container.poses_file_name << std::endl;
+            session.point_clouds_container.save_poses(fs::path(session.point_clouds_container.poses_file_name).string(), false);
+        }
+    }
+    return true;
+}
+
+Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, const ObservationPicking& observation_picking)
+{
+    const auto laser_beam = GetLaserBeam(x, y);
+
+    RegistrationPlaneFeature::Plane pl;
+
+    pl.a = 0;
+    pl.b = 0;
+    pl.c = 1;
+    pl.d = -observation_picking.picking_plane_height;
+
+    Eigen::Vector3d pos = rayIntersection(laser_beam, pl);
+
+    std::cout << "intersection: " << pos.x() << " " << pos.y() << " " << pos.z() << std::endl;
+
+    return pos;
+}
+
+bool loadProject(const std::string& file_name, ProjectSettings& _project_settings)
+{
+    std::cout << "Opening project file: '" << file_name << "'\n";
+
+    try
+    {
+        std::ifstream fs(file_name);
+        if (!fs.good())
+            return false;
+        nlohmann::json data = nlohmann::json::parse(fs);
+        fs.close();
+
+        _project_settings.session_file_names.clear();
+
+        std::cout << "Contained sessions:\n";
+
+        for (const auto& fn_json : data["session_file_names"])
+        {
+            const std::string fn = fn_json["session_file_name"];
+            _project_settings.session_file_names.push_back(fn);
+            std::cout << "'" << fn << "'";
+            if (!fs::exists(fn))
+                std::cout << "  (WARNING: session file does not exist! Please manually adapt path)";
+            std::cout << "\n";
+        }
+
+        edges.clear();
+        for (const auto& edge_json : data["loop_closure_edges"])
+        {
+            Edge edge;
+            edge.index_from = edge_json["index_from"];
+            edge.index_to = edge_json["index_to"];
+            edge.is_fixed_fi = edge_json["is_fixed_fi"];
+            edge.is_fixed_ka = edge_json["is_fixed_ka"];
+            edge.is_fixed_om = edge_json["is_fixed_om"];
+            edge.is_fixed_px = edge_json["is_fixed_px"];
+            edge.is_fixed_py = edge_json["is_fixed_py"];
+            edge.is_fixed_pz = edge_json["is_fixed_pz"];
+            edge.relative_pose_tb.fi = edge_json["fi"];
+            edge.relative_pose_tb.ka = edge_json["ka"];
+            edge.relative_pose_tb.om = edge_json["om"];
+            edge.relative_pose_tb.px = edge_json["px"];
+            edge.relative_pose_tb.py = edge_json["py"];
+            edge.relative_pose_tb.pz = edge_json["pz"];
+            edge.relative_pose_tb_weights.fi = edge_json["w_fi"];
+            edge.relative_pose_tb_weights.ka = edge_json["w_ka"];
+            edge.relative_pose_tb_weights.om = edge_json["w_om"];
+            edge.relative_pose_tb_weights.px = edge_json["w_px"];
+            edge.relative_pose_tb_weights.py = edge_json["w_py"];
+            edge.relative_pose_tb_weights.pz = edge_json["w_pz"];
+            edge.index_session_from = edge_json["index_session_from"];
+            edge.index_session_to = edge_json["index_session_to"];
+            edges.push_back(edge);
+        }
+
+        std::cout << "Found " << edges.size() << "edges\nOpening done\n";
+
+        return true;
+    } catch (std::exception& e)
+    {
+        std::cout << "can't load project settings: " << e.what() << std::endl;
+        return false;
+    }
+
+    std::string newTitle = winTitle + " - " + truncPath(file_name);
+    glutSetWindowTitle(newTitle.c_str());
+
+    loaded_sessions = false;
+    time_stamp_offset = 0.0;
+
+    return true;
+}
+
+void openProject()
+{
+    std::string input_file_name = "";
+    input_file_name = mandeye::fd::OpenFileDialogOneFile("Open project", mandeye::fd::Project_filter);
+
+    if (input_file_name.size() > 0)
+    {
+        loadProject(fs::path(input_file_name).string(), project_settings);
+    }
+}
+
+void saveProject()
+{
+    std::string output_file_name = "";
+    output_file_name = mandeye::fd::SaveFileDialog("Save project file", mandeye::fd::Project_filter, ".mjp", "project");
+
+    if (output_file_name.size() > 0)
+        if (save_project_settings(fs::path(output_file_name).string(), project_settings))
+        {
+            std::string newTitle = winTitle + " - " + truncPath(output_file_name);
+            glutSetWindowTitle(newTitle.c_str());
+        }
+}
+
+void addSession()
+{
+    auto input_file_names = mandeye::fd::OpenFileDialog("Add session(s)", mandeye::fd::Session_filter, true);
+
+    if (input_file_names.size() > 0)
+    {
+        for (const auto& input_file_name : input_file_names)
+        {
+            std::cout << "Adding session file: '" << input_file_name << "'" << std::endl;
+            project_settings.session_file_names.push_back(input_file_name);
+        }
+
+        loaded_sessions = false;
+        time_stamp_offset = 0.0;
+    }
+}
+
+void loadSessions()
+{
+    sessions.clear();
+    for (const auto& ps : project_settings.session_file_names)
+    {
+        Session session;
+        session.load(fs::path(ps).string(), is_decimate, bucket_x, bucket_y, bucket_z, calculate_offset);
+
+        // making sure irelevant session specific settings that could affect rendering are off
+        session.point_clouds_container.xz_intersection = false;
+        session.point_clouds_container.yz_intersection = false;
+        session.point_clouds_container.xy_intersection = false;
+        session.point_clouds_container.xz_grid_10x10 = false;
+        session.point_clouds_container.xz_grid_1x1 = false;
+        session.point_clouds_container.xz_grid_01x01 = false;
+        session.point_clouds_container.yz_grid_10x10 = false;
+        session.point_clouds_container.yz_grid_1x1 = false;
+        session.point_clouds_container.yz_grid_01x01 = false;
+        session.point_clouds_container.xy_grid_10x10 = false;
+        session.point_clouds_container.xy_grid_1x1 = false;
+        session.point_clouds_container.xy_grid_01x01 = false;
+
+        sessions.push_back(session);
+        if (session.is_ground_truth)
+            index_gt = sessions.size() - 1;
+    }
+    loaded_sessions = true;
+
+    // reorder
+    std::vector<Session> sessions_reorder;
+    std::vector<std::string> session_file_names_reordered;
+
+    std::map<int, int> map_reorder;
+    // project_settings.session_file_names.
+    int new_index = 0;
+    for (size_t i = 0; i < sessions.size(); i++)
+    {
+        if (sessions[i].is_ground_truth)
+        {
+            sessions_reorder.push_back(sessions[i]);
+            session_file_names_reordered.push_back(project_settings.session_file_names[i]);
+            map_reorder[i] = new_index++;
+        }
+    }
+    for (size_t i = 0; i < sessions.size(); i++)
+    {
+        if (!sessions[i].is_ground_truth)
+        {
+            sessions_reorder.push_back(sessions[i]);
+            session_file_names_reordered.push_back(project_settings.session_file_names[i]);
+            map_reorder[i] = new_index++;
+        }
+    }
+    sessions = sessions_reorder;
+    project_settings.session_file_names = session_file_names_reordered;
+
+    for (auto& e : edges)
+    {
+        e.index_session_from = map_reorder[e.index_session_from];
+        e.index_session_to = map_reorder[e.index_session_to];
+    }
+
+    std::cout << "sessions reordered, ground truth should be in front" << std::endl;
+    for (const auto& s : sessions)
+    {
+        std::cout << "session: '" << s.session_file_name << "' ground truth [" << int(s.is_ground_truth) << "]" << std::endl;
+    }
+
+    // update time_stamp_offset
+    std::cout << "update time_stamp_offset" << std::endl;
+    for (const auto& s : sessions)
+    {
+        if (s.point_clouds_container.point_clouds.size() > 0)
+        {
+            if (s.point_clouds_container.point_clouds[0].local_trajectory.size() > 0)
+            {
+                if (s.point_clouds_container.point_clouds[0].local_trajectory[0].timestamps.first > time_stamp_offset)
+                {
+                    time_stamp_offset = s.point_clouds_container.point_clouds[0].local_trajectory[0].timestamps.first;
+                }
+            }
+        }
+    }
+}
+
+void generate_loop_closures(const std::vector<Session>& sessions, std::vector<Edge>& edges)
+{
+    edges.clear();
+    // Implementation for generating loop closures
+
+    // bool found_edge = false;
+
+    for (int i1 = 0; i1 < sessions.size(); i1++)
+    {
+        for (int j1 = 0; j1 < sessions[i1].point_clouds_container.point_clouds.size(); j1++)
+        {
+            Eigen::Affine3d pose_i = sessions[i1].point_clouds_container.point_clouds[j1].m_pose;
+
+            for (int i2 = i1 + 1; i2 < sessions.size(); i2++)
+            {
+                for (int j2 = 0; j2 < sessions[i2].point_clouds_container.point_clouds.size(); j2++)
+                {
+                    Eigen::Affine3d pose_j = sessions[i2].point_clouds_container.point_clouds[j2].m_pose;
+
+                    if ((pose_i.translation() - pose_j.translation()).norm() < 10.0) // Example threshold for proximity
+                    {
+                        // found_edge = true;
+                        //  Create a loop closure edge between pose_i and pose_j
+                        /*Edge edge;
+                        edge.index_session_from = i1;
+                        edge.index_from = j1;
+                        edge.index_session_to = i2;
+                        edge.index_to = j2;
+
+                            // Initialize other edge parameters as needed
+                            edges.push_back(edge);*/
+
+                        Edge edge;
+
+                        edge.index_session_from = i1;
+                        edge.index_session_to = i2;
+
+                        edge.index_from = j1;
+                        edge.index_to = j2;
+
+                        std::cout << "Found loop closure edge between session " << i1 << " (pose " << j1 << ") and session " << i2
+                                  << " (pose " << j2 << ")" << std::endl;
+
+                        edge.relative_pose_tb = pose_tait_bryan_from_affine_matrix(
+                            sessions[edge.index_session_from].point_clouds_container.point_clouds[edge.index_from].m_pose.inverse() *
+                            sessions[edge.index_session_to].point_clouds_container.point_clouds[edge.index_to].m_pose);
+
+                        edge.relative_pose_tb_weights.px = 1.0;
+                        edge.relative_pose_tb_weights.py = 1.0;
+                        edge.relative_pose_tb_weights.pz = 1.0;
+                        edge.relative_pose_tb_weights.om = 1.0;
+                        edge.relative_pose_tb_weights.fi = 1.0;
+                        edge.relative_pose_tb_weights.ka = 1.0;
+
+                        edges.push_back(edge);
+
+                        // j1 += 20;
+                        // j2 += 20;
+                    }
+                }
+            } // for(int i2 = i1 + 1; i2 < sessions.size(); i2++)
+        } // for(int j1 = 0; j1 < sessions[i1].point_clouds_container.point_clouds.size(); j1++)
+    } // for(int i1 = 0; i1 < sessions.size(); i1++)
+}
+
+void icp_all_edges(std::vector<Session>& sessions, std::vector<Edge>& edges, float sr)
+{
+    std::cout << "icp_all_edges" << std::endl;
+
+    int number_of_iterations = 10;
+
+    for (size_t index_active_edge = 0; index_active_edge < edges.size(); index_active_edge++)
+    {
+        PairWiseICP icp;
+        auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+        std::vector<Eigen::Vector3d> source;
+        auto& e = edges[index_active_edge];
+        for (int i = -num_edge_extended_before; i <= num_edge_extended_after; i++)
+        {
+            int index_src = e.index_to + i;
+            if (index_src >= 0 &&
+                index_src < sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.size())
+            {
+                Eigen::Affine3d m_src =
+                    sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(index_src).m_pose;
+                for (int k = 0; k <
+                     sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[index_src].points_local.size();
+                     k++)
+                {
+                    Eigen::Vector3d p_g = m_src *
+                        sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[index_src].points_local[k];
+                    source.push_back(p_g);
+                }
+                // point_clouds_container.point_clouds.at(index_src).render(m_src, 1);
+            }
+        }
+        std::vector<Eigen::Vector3d> target;
+
+        for (int i = -num_edge_extended_before; i <= num_edge_extended_after; i++)
+        {
+            int index_trg = e.index_from + i;
+            if (index_trg >= 0 &&
+                index_trg < sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.size())
+            {
+                Eigen::Affine3d m_trg =
+                    sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_trg).m_pose;
+                for (int k = 0; k < sessions[edges[index_active_edge].index_session_from]
+                                        .point_clouds_container.point_clouds[index_trg]
+                                        .points_local.size();
+                     k++)
+                {
+                    Eigen::Vector3d p_g = m_trg *
+                        sessions[edges[index_active_edge].index_session_from]
+                            .point_clouds_container.point_clouds[index_trg]
+                            .points_local[k];
+                    target.push_back(p_g);
+                }
+                // point_clouds_container.point_clouds.at(index_src).render(m_src, 1);
+            }
+        }
+
+        Eigen::Affine3d m_src_inv =
+            sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[e.index_to].m_pose.inverse();
+
+        for (auto& p : source)
+        {
+            p = m_src_inv * p;
+        }
+
+        Eigen::Affine3d m_trg_inv =
+            sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[e.index_from].m_pose.inverse();
+
+        for (auto& p : target)
+        {
+            p = m_trg_inv * p;
+        }
+
+        if (icp.compute(source, target, sr, number_of_iterations, m_pose))
+            edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose);
+    }
+}
+
+void settings_gui()
+{
+    if (ImGui::Begin("Settings", &is_settings_gui))
+    {
+        ImGui::Checkbox("Downsample during load", &is_decimate);
+        ImGui::SameLine();
+        ImGui::Text("Bucket [m]:");
+        ImGui::PushItemWidth(ImGuiNumberWidth);
+        ImGui::InputDouble("X##b", &bucket_x, 0.0, 0.0, "%.3f");
+        if (ImGui::IsItemHovered())
+            ImGui::SetTooltip(xText);
+        ImGui::SameLine();
+        ImGui::InputDouble("Y##b", &bucket_y, 0.0, 0.0, "%.3f");
+        if (ImGui::IsItemHovered())
+            ImGui::SetTooltip(yText);
+        ImGui::SameLine();
+        ImGui::InputDouble("Z##b", &bucket_z, 0.0, 0.0, "%.3f");
+        if (ImGui::IsItemHovered())
+            ImGui::SetTooltip(zText);
+        ImGui::PopItemWidth();
+
+        ImGui::NewLine();
+
+        ImGui::Separator();
+
+        ImGui::Text("Benchmark settings:");
+
+        ImGui::PushItemWidth(ImGuiNumberWidth * 2);
+
+        static double fast_plus = 100000000.0;
+        static double fast_plus_plus = 1000000000.0;
+
+        ImGui::InputDouble("Increment", &fast_plus);
+        ImGui::InputDouble("Fast increment", &fast_plus_plus);
+        ImGui::InputDouble("Timestamp offset", &time_stamp_offset, fast_plus, fast_plus_plus);
+        ImGui::PopItemWidth();
+        ImGui::SameLine();
+        if (ImGui::Button("Set to origin"))
+        {
+            bool is_first_gt = false;
+
+            if (sessions.size() > 0)
+            {
+                if (sessions[0].is_ground_truth)
+                    is_first_gt = true;
+            }
+
+            Eigen::Affine3d m_gt = Eigen::Affine3d::Identity();
+            if (sessions.size() > 0)
+            {
+                int index_point_clouds = -1;
+                int index_local_trajectory = -1;
+                bool found = false;
+                for (size_t a = 0; a < sessions[0].point_clouds_container.point_clouds.size(); a++)
+                {
+                    for (size_t b = 0; b < sessions[0].point_clouds_container.point_clouds[a].local_trajectory.size(); b++)
+                    {
+                        if (sessions[0].point_clouds_container.point_clouds[a].local_trajectory[b].timestamps.first > time_stamp_offset)
+                        {
+                            if (!found)
+                            {
+                                found = true;
+                                index_point_clouds = a;
+                                index_local_trajectory = b;
+                                break;
+                            }
+                        }
+                    }
+                }
+
+                if (index_point_clouds != -1 && index_local_trajectory != -1)
+                {
+                    m_gt = sessions[0].point_clouds_container.point_clouds[index_point_clouds].m_pose *
+                        sessions[0].point_clouds_container.point_clouds[index_point_clouds].local_trajectory[index_local_trajectory].m_pose;
+                }
+            }
+
+            // for (auto& session : sessions)
+            for (auto& session : sessions)
+            {
+                if (is_first_gt)
+                {
+                    if (session.is_ground_truth)
+                        continue;
+                }
+
+                int index_point_clouds = -1;
+                int index_local_trajectory = -1;
+                bool found = false;
+                for (size_t a = 0; a < session.point_clouds_container.point_clouds.size(); a++)
+                {
+                    for (size_t b = 0; b < session.point_clouds_container.point_clouds[a].local_trajectory.size(); b++)
+                    {
+                        if (session.point_clouds_container.point_clouds[a].local_trajectory[b].timestamps.first > time_stamp_offset)
+                        {
+                            if (!found)
+                            {
+                                found = true;
+                                index_point_clouds = a;
+                                index_local_trajectory = b;
+                                break;
+                            }
+                        }
+                    }
+                }
+
+                if (index_point_clouds != -1 && index_local_trajectory != -1)
+                {
+                    auto m1 = session.point_clouds_container.point_clouds[index_point_clouds].m_pose;
+                    auto m2 =
+                        session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory[index_local_trajectory].m_pose;
+
+                    auto inv = Eigen::Affine3d::Identity();
+                    inv = (m1 * m2).inverse();
+                    for (size_t index = 0; index < session.point_clouds_container.point_clouds.size(); index++)
+                        session.point_clouds_container.point_clouds[index].m_pose =
+                            inv * session.point_clouds_container.point_clouds[index].m_pose;
+
+                    for (size_t index = 0; index < session.point_clouds_container.point_clouds.size(); index++)
+                        session.point_clouds_container.point_clouds[index].m_pose =
+                            m_gt * session.point_clouds_container.point_clouds[index].m_pose;
+                }
+            }
+        }
+
+        static bool open_import_popup = false;
+        static bool show_instruction = false;
+
+        ImGui::Dummy(ImVec2(0, 10));
+
+        if (ImGui::Button("Import Benchmark Output Folders"))
+        {
+            open_import_popup = true;
+            ImGui::OpenPopup("Import Benchmark");
+        }
+
+        if (ImGui::BeginPopupModal("Import Benchmark", NULL, ImGuiWindowFlags_AlwaysAutoResize))
+        {
+            ImGui::Text("Import benchmark sessions");
+            ImGui::Separator();
+            ImGui::SetNextWindowSize(ImVec2(740, 340), ImGuiCond_Once);
+            if (ImGui::Button("Instruction"))
+            {
+                show_instruction = !show_instruction;
+            }
+
+            ImGui::Dummy(ImVec2(0, 10));
+            if (ImGui::Button("Select Folder"))
+            {
+                const std::string algorithms[] = { "ct-icp",    "glim",      "super-lio", "dlio",
+                                                   "i2ekf-lo",  "superOdom", "lego-loam", "faster-lio",
+                                                   "kiss-icp",  "fast-lio",  "lio-ekf",   "genz-icp",
+                                                   "point-lio", "ig-lio",    "dlo",       "lidar_odometry_ros_wrapper" };
+
+                std::vector<fs::path> missing;
+                std::unordered_map<std::string, std::string> algo_map;
+
+                for (const auto& algo : algorithms)
+                {
+                    if (algo == "kiss-icp")
+                    {
+                        algo_map[algo] = "output_hdmapping-kiss";
+                    }
+                    else if (algo == "genz-icp")
+                    {
+                        algo_map[algo] = "output_hdmapping-genz";
+                    }
+                    else if (algo == "lidar_odometry_ros_wrapper")
+                    {
+                        algo_map[algo] = "output_hdmapping-lidar-odometry-ros";
+                    }
+                    else
+                    {
+                        algo_map[algo] = "output_hdmapping-" + algo;
+                    }
+                }
+
+                fs::path path = fs::path(mandeye::fd::SelectFolder("Add sessions"));
+
+                for (const auto& algo : algorithms)
+                {
+                    fs::path output_folder = path / algo / algo_map[algo];
+                    fs::path session_file = output_folder / "session.json";
+
+                    if (fs::is_directory(output_folder))
+                    {
+                        auto it =
+                            std::find(project_settings.session_file_names.begin(), project_settings.session_file_names.end(), session_file);
+
+                        if (it == project_settings.session_file_names.end())
+                        {
+                            std::cout << "Adding session file: '" << session_file << "'" << std::endl;
+                            project_settings.session_file_names.push_back(session_file.string());
+                        }
+                    }
+                    else
+                    {
+                        missing.push_back(output_folder);
+                    }
+                }
+
+                for (const auto& miss : missing)
+                {
+                    std::cout << miss << " doesn't exist" << std::endl;
+                }
+            }
+
+            ImGui::SameLine();
+
+            if (ImGui::Button("Close"))
+            {
+                ImGui::CloseCurrentPopup();
+                show_instruction = false;
+            }
+
+            if (show_instruction)
+            {
+                ImGui::BeginChild("InstructionChild", ImVec2(720, 300), true, ImGuiWindowFlags_HorizontalScrollbar);
+                ImGui::Separator();
+
+                ImGui::TextWrapped("Required folder structure:");
+
+                ImGui::Spacing();
+
+                ImGui::TextWrapped("Folders must follow the structure generated in benchmark-HDMapping-Orchestration (step 3):");
+                ImGui::TextWrapped("https://github.com/MapsHD/benchmark-HDMapping-Orchestration");
+
+                ImGui::Spacing();
+                ImGui::Separator();
+
+                ImGui::BulletText("chosen_folder/");
+                ImGui::BulletText("  ct-icp/output_hdmapping-ct-icp/session.json");
+                ImGui::BulletText("  glim/output_hdmapping-glim/session.json");
+                ImGui::BulletText("  kiss-icp/output_hdmapping-kiss/session.json");
+                ImGui::BulletText("  fast-lio/output_hdmapping-fast-lio/session.json");
+                ImGui::BulletText("  ... (same pattern for other algorithms)");
+
+                ImGui::Spacing();
+                ImGui::EndChild();
+            }
+
+            ImGui::EndPopup();
+        }
+        if (project_settings.session_file_names.size() > 0)
+        {
+            ImGui::Separator();
+
+            ImGui::Text("Sessions:");
+
+            for (size_t i = 0; i < project_settings.session_file_names.size(); i++)
+            {
+                ImGui::Text(truncPath(project_settings.session_file_names[i]).c_str());
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip(project_settings.session_file_names[i].c_str());
+
+                if (project_settings.session_file_names.size() == sessions.size())
+                {
+                    ImGui::BeginDisabled(is_loop_closure_gui);
+                    {
+                        ImGui::SameLine();
+                        ImGui::Checkbox(("Visible##" + std::to_string(i)).c_str(), &sessions[i].visible);
+                    }
+                    ImGui::EndDisabled();
+
+                    ImGui::SameLine();
+                    if (ImGui::RadioButton(("Ground truth##" + std::to_string(i)).c_str(), &index_gt, i))
+                        if (old_index_gt == i)
+                            index_gt = -1; // unselect
+                    if (ImGui::IsItemHovered())
+                        ImGui::SetTooltip("Select session as unmovable reference");
+
+                    ImGui::BeginDisabled(!sessions[i].visible);
+                    {
+                        ImGui::BeginDisabled(sessions[i].is_ground_truth);
+                        {
+                            ImGui::SameLine();
+                            if (ImGui::RadioButton(("Gizmo##" + std::to_string(i)).c_str(), &index_gizmo, i))
+                                if (old_index_gizmo == i)
+                                    index_gizmo = -1; // unselect
+                        }
+                        ImGui::EndDisabled();
+
+                        ImGui::SameLine();
+                        ImGui::ColorEdit3(
+                            ("Color##" + std::to_string(i)).c_str(), (float*)&sessions[i].render_color, ImGuiColorEditFlags_NoInputs);
+                        for (auto& pc : sessions[i].point_clouds_container.point_clouds)
+                        {
+                            pc.traj_color[0] = sessions[i].render_color[0];
+                            pc.traj_color[1] = sessions[i].render_color[1];
+                            pc.traj_color[2] = sessions[i].render_color[2];
+                            pc.render_color[0] = sessions[i].render_color[0];
+                            pc.render_color[1] = sessions[i].render_color[1];
+                            pc.render_color[2] = sessions[i].render_color[2];
+                        }
+                    }
+                    ImGui::EndDisabled();
+
+                    //
+                    if (sessions[i].point_clouds_container.point_clouds.size() > 0)
+                    {
+                        if (sessions[i].point_clouds_container.point_clouds[0].local_trajectory.size() > 0)
+                        {
+                            if (sessions[i]
+                                    .point_clouds_container.point_clouds[sessions[i].point_clouds_container.point_clouds.size() - 1]
+                                    .local_trajectory.size() > 0)
+                            {
+                                ImGui::SameLine();
+
+                                int index_last = sessions[i].point_clouds_container.point_clouds.size() - 1;
+                                int index_last2 = sessions[i].point_clouds_container.point_clouds[index_last].local_trajectory.size() - 1;
+
+                                ImGui::Text(
+                                    "Timestamp range: <%.0f, %.0f>",
+                                    sessions[i].point_clouds_container.point_clouds[0].local_trajectory[0].timestamps.first,
+                                    sessions[i]
+                                        .point_clouds_container.point_clouds[index_last]
+                                        .local_trajectory[index_last2]
+                                        .timestamps.first);
+                            }
+                        }
+                    }
+                }
+            }
+
+            if (project_settings.session_file_names.size() == sessions.size())
+            {
+                if ((old_index_gt != index_gt) || (old_index_gizmo != index_gizmo))
+                {
+                    for (size_t i = 0; i < sessions.size(); i++)
+                    {
+                        sessions[i].is_ground_truth = (i == index_gt);
+                        sessions[i].is_gizmo = (i == index_gizmo);
+                    }
+
+                    old_index_gt = index_gt;
+                    old_index_gizmo = index_gizmo;
+                }
+
+                if (index_gizmo != -1 && index_gizmo < sessions.size())
+                {
+                    // sessions[index_gizmo].is_gizmo = true;
+                    m_gizmo[0] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 0);
+                    m_gizmo[1] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 0);
+                    m_gizmo[2] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 0);
+                    m_gizmo[3] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 0);
+                    m_gizmo[4] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 1);
+                    m_gizmo[5] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 1);
+                    m_gizmo[6] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 1);
+                    m_gizmo[7] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 1);
+                    m_gizmo[8] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 2);
+                    m_gizmo[9] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 2);
+                    m_gizmo[10] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 2);
+                    m_gizmo[11] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 2);
+                    m_gizmo[12] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 3);
+                    m_gizmo[13] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 3);
+                    m_gizmo[14] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 3);
+                    m_gizmo[15] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 3);
+                }
+            }
+
+            ImGui::BeginDisabled((project_settings.session_file_names.size() < 2) || (index_gizmo == -1));
+            {
+                ImGui::Checkbox("Gizmo all sessions", &gizmo_all_sessions);
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("Gizmo will move all sessions except ground truth one");
+            }
+            ImGui::EndDisabled();
+
+            ImGui::Separator();
+
+            ImGui::NewLine();
+
+            if (project_settings.session_file_names.size() == sessions.size())
+            {
+                number_visible_sessions = 0;
+
+                bool first_session_index_found = false;
+                for (size_t index = 0; index < sessions.size(); index++)
+                {
+                    if (sessions[index].visible)
+                    {
+                        number_visible_sessions++;
+                        if (!first_session_index_found)
+                        {
+                            first_session_index = index;
+                            second_session_index = index;
+                            first_session_index_found = true;
+                        }
+                        else
+                        {
+                            second_session_index = index;
+                        }
+                    }
+                }
+
+                if (!is_loop_closure_gui)
+                {
+                    static int nr_iter = 100;
+                    ImGui::SetNextItemWidth(ImGuiNumberWidth);
+                    ImGui::InputInt("Number of iterations", &nr_iter);
+                    if (nr_iter < 1)
+                        nr_iter = 1;
+
+                    ImGui::InputDouble("Motion Model Weight (1 sigma m): position x [px]", &motion_model_weights.px, 0.0, 0.0, "%.3f");
+                    ImGui::InputDouble("Motion Model Weight (1 sigma m): position y [py]", &motion_model_weights.py, 0.0, 0.0, "%.3f");
+                    ImGui::InputDouble("Motion Model Weight (1 sigma m): position z [pz]", &motion_model_weights.pz, 0.0, 0.0, "%.3f");
+                    ImGui::InputDouble(
+                        "Motion Model Weight (1 sigma degree): orientation om [om]", &motion_model_weights.om, 0.0, 0.0, "%.3f");
+                    ImGui::InputDouble(
+                        "Motion Model Weight (1 sigma degree): orientation fi [fi]", &motion_model_weights.fi, 0.0, 0.0, "%.3f");
+                    ImGui::InputDouble(
+                        "Motion Model Weight (1 sigma degree): orientation ka [ka]", &motion_model_weights.ka, 0.0, 0.0, "%.3f");
+
+                    std::string bn = "Optimize (number of iterations: " + std::to_string(nr_iter) + ")";
+
+                    if (ImGui::Button(bn.c_str()))
+                    {
+                        for (int i = 0; i < nr_iter; i++)
+                        {
+                            std::cout << "Iteration [" << i + 1 << "] of: " << nr_iter << std::endl;
+                            optimize(sessions, edges, motion_model_weights);
+                        }
+                        optimized = true;
+                    }
+
+                    // if (optimized)
+                    //{
+                    ImGui::SameLine();
+                    if (ImGui::Button("Revert"))
+                        revert(sessions);
+                    ImGui::SameLine();
+                    if (ImGui::Button("Save results"))
+                        save_results(sessions);
+                    ImGui::SameLine();
+                    if (ImGui::Button("Revert to initial"))
+                        revert_to_initial(sessions);
+                    //}
+
+                    if (ImGui::Button("Generate loop closures"))
+                    {
+                        generate_loop_closures(sessions, edges);
+                    }
+
+                    if (ImGui::Button("ICP all edges"))
+                    {
+                        icp_all_edges(sessions, edges, search_radius);
+                    }
+                    ImGui::SameLine();
+                    ImGui::InputDouble("search_radius", &search_radius);
+                    if (search_radius < 0.01)
+                        search_radius = 0.01;
+                }
+
+                // if (!is_loop_closure_gui && prev_is_loop_closure_gui)
+                //{
+                //     exit(1);
+                // }
+            }
+        }
+    }
+
+    ImGui::End();
+}
+
+void display()
+{
+    ImGuiIO& io = ImGui::GetIO();
+    glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y);
+
+    glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w);
+    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+    glEnable(GL_DEPTH_TEST);
+
+    glMatrixMode(GL_PROJECTION);
+    glLoadIdentity();
+    float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y);
+
+    updateCameraTransition();
+
+    viewLocal = Eigen::Affine3f::Identity();
+
+    for (auto& s : sessions)
+    {
+        for (auto& pc : s.point_clouds_container.point_clouds)
+        {
+            pc.point_size = gui_point_size;
+        }
+    }
+
+    if (!is_ortho)
+    {
+        reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y);
+        glTranslatef(translate_x, translate_y, translate_z);
+
+        // janusz
+        if (is_loop_closure_gui)
+        {
+            // sessions[first_session_index].point_clouds_container.point_clouds.at(index_loop_closure_source).render(false,
+            // observation_picking, viewer_decmiate_point_cloud, false, false, false, false, false, false, false, false, false, false,
+            // false, false, 100000);
+            // sessions[second_session_index].point_clouds_container.point_clouds.at(index_loop_closure_target).render(false,
+            // observation_picking, viewer_decmiate_point_cloud, false, false, false, false, false, false, false, false, false, false,
+            // false, false, 100000);
+
+            if (first_session_index < sessions[first_session_index].point_clouds_container.point_clouds.size())
+            {
+                if (update_rotation_center)
+                {
+                    rotation_center.x() = sessions[first_session_index]
+                                              .point_clouds_container.point_clouds[index_loop_closure_source]
+                                              .m_pose.translation()
+                                              .x();
+                    rotation_center.y() = sessions[first_session_index]
+                                              .point_clouds_container.point_clouds[index_loop_closure_source]
+                                              .m_pose.translation()
+                                              .y();
+                    rotation_center.z() = sessions[first_session_index]
+                                              .point_clouds_container.point_clouds[index_loop_closure_source]
+                                              .m_pose.translation()
+                                              .z();
+                }
+            }
+
+            if (manipulate_active_edge)
+            {
+                if (edges.size() > 0)
+                {
+                    int index_src = edges[index_active_edge].index_from;
+                    Eigen::Affine3d m_src =
+                        sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose;
+
+                    if (update_rotation_center)
+                    {
+                        rotation_center.x() = m_src(0, 3);
+                        rotation_center.y() = m_src(1, 3);
+                        rotation_center.z() = m_src(2, 3);
+                    }
+                }
+            }
+
+            /*if (session.pose_graph_loop_closure.manipulate_active_edge)
+            {
+                if (session.pose_graph_loop_closure.edges.size() > 0)
+                {
+                    if (session.pose_graph_loop_closure.index_active_edge < session.pose_graph_loop_closure.edges.size())
+                    {
+                        rotation_center.x() =
+            session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(0,
+            3); rotation_center.y() =
+            session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(1,
+            3); rotation_center.z() =
+            session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(2,
+            3);
+                    }
+                }
+            }*/
+        }
+
+        viewLocal.translate(rotation_center);
+
+        viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z));
+        if (!lock_z)
+            viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX()));
+        else
+            viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX()));
+        viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ()));
+
+        viewLocal.translate(-rotation_center);
+
+        glLoadMatrixf(viewLocal.matrix().data());
+    }
+    else
+        updateOrthoView();
+
+    showAxes();
+
+    if (is_loop_closure_gui)
+    {
+        if (manipulate_active_edge)
+        {
+            if (edges.size() > 0)
+            {
+                /*int index_src = edges[index_active_edge].index_from;
+                int index_trg = edges[index_active_edge].index_to;
+
+                Eigen::Affine3d m_src =
+                    sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose;
+                Eigen::Affine3d m_trg = m_src * affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).render(
+                    m_src,
+                    viewer_decimate_point_cloud,
+                    viewer_reduce_rendered_trajectory,
+                    sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).render_color);
+                sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(index_trg).render(
+                    m_trg,
+                    viewer_decimate_point_cloud,
+                    viewer_reduce_rendered_trajectory,
+                    sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(index_trg).render_color);*/
+
+                int index_src = edges[index_active_edge].index_from;
+                int index_trg = edges[index_active_edge].index_to;
+
+                Eigen::Affine3d _m_src =
+                    sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose;
+                Eigen::Affine3d _m_trg = _m_src * affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb);
+
+                Eigen::Affine3d m_src_0 =
+                    sessions[first_session_index].point_clouds_container.point_clouds.at(index_loop_closure_source).m_pose; // Todo
+
+                for (int i = index_loop_closure_source - num_edge_extended_before; i <= index_loop_closure_source + num_edge_extended_after;
+                     i++)
+                {
+                    if (i >= 0 && i < sessions[first_session_index].point_clouds_container.point_clouds.size() &&
+                        sessions[first_session_index].point_clouds_container.point_clouds.size() > 0)
+                    {
+                        // ObservationPicking observation_picking;
+                        // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000,
+                        // false);
+
+                        Eigen::Affine3d m_src_curr = sessions[first_session_index].point_clouds_container.point_clouds.at(i).m_pose; // Todo
+                        Eigen::Affine3d m_src = _m_src * (m_src_0.inverse() * m_src_curr);
+
+                        // sessions[first_session_index].point_clouds_container.point_clouds.at(i).point_size = gui_point_size;
+
+                        sessions[first_session_index].point_clouds_container.point_clouds.at(i).render(
+                            m_src,
+                            viewer_decimate_point_cloud,
+                            viewer_reduce_rendered_trajectory,
+                            sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(i).render_color);
+                    }
+                }
+
+                Eigen::Affine3d m_trg_0 =
+                    sessions[second_session_index].point_clouds_container.point_clouds.at(index_loop_closure_target).m_pose; // Todo
+
+                for (int i = index_loop_closure_target - num_edge_extended_before; i <= index_loop_closure_target + num_edge_extended_after;
+                     i++)
+                {
+                    if (i >= 0 && i < sessions[second_session_index].point_clouds_container.point_clouds.size() &&
+                        sessions[second_session_index].point_clouds_container.point_clouds.size() > 0)
+                    {
+                        // ObservationPicking observation_picking;
+                        // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000,
+                        // false);
+                        Eigen::Affine3d m_trg_curr =
+                            sessions[second_session_index].point_clouds_container.point_clouds.at(i).m_pose; // Todo
+                        Eigen::Affine3d m_trg = _m_trg * (m_trg_0.inverse() * m_trg_curr);
+
+                        // sessions[second_session_index].point_clouds_container.point_clouds.at(i).point_size = gui_point_size;
+                        sessions[second_session_index].point_clouds_container.point_clouds.at(i).render(
+                            m_trg,
+                            viewer_decimate_point_cloud,
+                            viewer_reduce_rendered_trajectory,
+                            sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(i).render_color);
+                    }
+                }
+            }
+        }
+        else
+        {
+            ObservationPicking observation_picking;
+
+            /*sessions[first_session_index]
+                .point_clouds_container.point_clouds.at(index_loop_closure_source)
+                .render(
+                    false,
+                    observation_picking,
+                    viewer_decimate_point_cloud,
+                    viewer_reduce_rendered_trajectory,
+                    false,
+                    false,
+                    false,
+                    100000,
+                    false);*/
+
+            for (int i = index_loop_closure_source - num_edge_extended_before; i <= index_loop_closure_source + num_edge_extended_after;
+                 i++)
+            {
+                if (i >= 0 && i < sessions[first_session_index].point_clouds_container.point_clouds.size() &&
+                    sessions[first_session_index].point_clouds_container.point_clouds.size() > 0)
+                {
+                    // ObservationPicking observation_picking;
+                    // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000,
+                    // false);
+                    Eigen::Affine3d m_src = sessions[first_session_index].point_clouds_container.point_clouds.at(i).m_pose;
+
+                    sessions[first_session_index].point_clouds_container.point_clouds.at(i).render(
+                        false,
+                        observation_picking,
+                        viewer_decimate_point_cloud,
+                        viewer_reduce_rendered_trajectory,
+                        false,
+                        false,
+                        false,
+                        100000,
+                        false);
+                }
+            }
+
+            /*sessions[second_session_index]
+                .point_clouds_container.point_clouds.at(index_loop_closure_target)
+                .render(
+                    false,
+                    observation_picking,
+                    viewer_decimate_point_cloud,
+                    viewer_reduce_rendered_trajectory,
+                    false,
+                    false,
+                    false,
+                    100000,
+                    false);*/
+
+            for (int i = index_loop_closure_target - num_edge_extended_before; i <= index_loop_closure_target + num_edge_extended_after;
+                 i++)
+            {
+                if (i >= 0 && i < sessions[second_session_index].point_clouds_container.point_clouds.size() &&
+                    sessions[second_session_index].point_clouds_container.point_clouds.size() > 0)
+                {
+                    // ObservationPicking observation_picking;
+                    // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000,
+                    // false);
+                    Eigen::Affine3d m_src = sessions[second_session_index].point_clouds_container.point_clouds.at(i).m_pose;
+
+                    sessions[second_session_index].point_clouds_container.point_clouds.at(i).render(
+                        false,
+                        observation_picking,
+                        viewer_decimate_point_cloud,
+                        viewer_reduce_rendered_trajectory,
+                        false,
+                        false,
+                        false,
+                        100000,
+                        false);
+                }
+            }
+        }
+
+        // sessions[first_session_index].point_clouds_container.render();
+
+        glBegin(GL_LINE_STRIP);
+        for (auto& pc : sessions[first_session_index].point_clouds_container.point_clouds)
+        {
+            glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]);
+            glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3));
+        }
+        glEnd();
+
+        int i = 0;
+        for (auto& pc : sessions[first_session_index].point_clouds_container.point_clouds)
+        {
+            glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]);
+            glRasterPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1);
+            glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str());
+            i++;
+        }
+
+        glBegin(GL_LINE_STRIP);
+        for (auto& pc : sessions[second_session_index].point_clouds_container.point_clouds)
+        {
+            glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]);
+            glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3));
+        }
+        glEnd();
+
+        i = 0;
+        for (auto& pc : sessions[second_session_index].point_clouds_container.point_clouds)
+        {
+            glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]);
+            glRasterPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1);
+            glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str());
+            i++;
+        }
+
+        for (size_t i = 0; i < sessions.size(); i++)
+        {
+            for (size_t j = 0; j < sessions[i].pose_graph_loop_closure.edges.size(); j++)
+            {
+                int index_src = sessions[i].pose_graph_loop_closure.edges[j].index_from;
+                int index_trg = sessions[i].pose_graph_loop_closure.edges[j].index_to;
+
+                glColor3f(0.0f, 0.0f, 1.0f);
+                glBegin(GL_LINES);
+                auto v1 = sessions[i].point_clouds_container.point_clouds.at(index_src).m_pose.translation();
+                auto v2 = sessions[i].point_clouds_container.point_clouds.at(index_trg).m_pose.translation();
+                glVertex3f(v1.x(), v1.y(), v1.z());
+                glVertex3f(v2.x(), v2.y(), v2.z());
+
+                glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5);
+                glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10);
+                glEnd();
+
+                glRasterPos3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10 + 0.1);
+                glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(j).c_str());
+            }
+        }
+
+        for (size_t i = 0; i < edges.size(); i++)
+        {
+            int index_src = edges[i].index_from;
+            int index_trg = edges[i].index_to;
+
+            int index_session_from = edges[i].index_session_from;
+            int index_session_to = edges[i].index_session_to;
+
+            if (sessions[index_session_from].is_ground_truth || sessions[index_session_to].is_ground_truth)
+                glColor3f(0.0f, 1.0f, 1.0f);
+            else
+                glColor3f(1.0f, 1.0f, 0.0f);
+
+            glBegin(GL_LINES);
+            auto v1 = sessions[index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose.translation();
+            auto v2 = sessions[index_session_to].point_clouds_container.point_clouds.at(index_trg).m_pose.translation();
+            glVertex3f(v1.x(), v1.y(), v1.z());
+            glVertex3f(v2.x(), v2.y(), v2.z());
+
+            glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5);
+            glVertex3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10);
+            glEnd();
+
+            glRasterPos3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10 + 0.1);
+            glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str());
+        }
+    }
+    else
+    {
+        for (auto& session : sessions)
+        {
+            if (session.visible)
+            {
+                session.point_clouds_container.render(observation_picking, viewer_decimate_point_cloud, viewer_reduce_rendered_trajectory);
+                session.ground_control_points.render(session.point_clouds_container);
+                session.control_points.render(session.point_clouds_container, false);
+
+                ////
+                int index_point_clouds = -1;
+                int index_local_trajectory = -1;
+                bool found = false;
+                for (size_t a = 0; a < session.point_clouds_container.point_clouds.size(); a++)
+                {
+                    for (size_t b = 0; b < session.point_clouds_container.point_clouds[a].local_trajectory.size(); b++)
+                    {
+                        if (session.point_clouds_container.point_clouds[a].local_trajectory[b].timestamps.first > time_stamp_offset)
+                        {
+                            if (!found)
+                            {
+                                found = true;
+                                index_point_clouds = a;
+                                index_local_trajectory = b;
+                                break;
+                            }
+                        }
+                    }
+                }
+
+                if (index_point_clouds != -1 && index_local_trajectory != -1)
+                {
+                    if (index_local_trajectory < session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory.size())
+                    {
+                        glColor3f(
+                            session.point_clouds_container.point_clouds[index_point_clouds].render_color[0],
+                            session.point_clouds_container.point_clouds[index_point_clouds].render_color[1],
+                            session.point_clouds_container.point_clouds[index_point_clouds].render_color[2]);
+                        glBegin(GL_LINES);
+
+                        auto m1 = session.point_clouds_container.point_clouds[index_point_clouds].m_pose;
+                        auto m2 =
+                            session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory[index_local_trajectory].m_pose;
+
+                        auto v1 = (m1 * m2).translation();
+
+                        glVertex3f(v1.x() - 5.0, v1.y(), v1.z());
+                        glVertex3f(v1.x() + 5.0, v1.y(), v1.z());
+
+                        glVertex3f(v1.x(), v1.y() - 5.0, v1.z());
+                        glVertex3f(v1.x(), v1.y() + 5.0, v1.z());
+
+                        glVertex3f(v1.x(), v1.y(), v1.z() - 5.0);
+                        glVertex3f(v1.x(), v1.y(), v1.z() + 5.0);
+
+                        glEnd();
+                    }
+                }
+            }
+        }
+    }
+
+    /*if (is_loop_closure_gui)
+    {
+        session.manual_pose_graph_loop_closure.Render(session.point_clouds_container, index_loop_closure_source, index_loop_closure_target);
+    }
+    else
+    {
+        for (const auto &g : available_geo_points)
+        {
+            glBegin(GL_LINES);
+            glColor3f(1.0f, 0.0f, 0.0f);
+            auto c = g.coordinates - session.point_clouds_container.offset;
+            glVertex3f(c.x() - 0.5, c.y(), c.z());
+            glVertex3f(c.x() + 0.5, c.y(), c.z());
+
+            glVertex3f(c.x(), c.y() - 0.5, c.z());
+            glVertex3f(c.x(), c.y() + 0.5, c.z());
+
+            glVertex3f(c.x(), c.y(), c.z() - 0.5);
+            glVertex3f(c.x(), c.y(), c.z() + 0.5);
+            glEnd();
+        }
+
+        //
+        for (const auto &pc : session.point_clouds_container.point_clouds)
+        {
+            for (const auto &gp : pc.available_geo_points)
+            {
+                if (gp.choosen)
+                {
+                    auto c = pc.m_pose * gp.coordinates;
+                    glBegin(GL_LINES);
+                    glColor3f(1.0f, 0.0f, 0.0f);
+                    glVertex3f(c.x() - 0.5, c.y(), c.z());
+                    glVertex3f(c.x() + 0.5, c.y(), c.z());
+
+                    glVertex3f(c.x(), c.y() - 0.5, c.z());
+                    glVertex3f(c.x(), c.y() + 0.5, c.z());
+
+                    glVertex3f(c.x(), c.y(), c.z() - 0.5);
+                    glVertex3f(c.x(), c.y(), c.z() + 0.5);
+                    glEnd();
+
+                    glBegin(GL_LINES);
+                    glColor3f(0.0f, 1.0f, 0.0f);
+                    glVertex3f(c.x(), c.y(), c.z());
+                    glVertex3f(gp.coordinates.x(), gp.coordinates.y(), gp.coordinates.z());
+                    glEnd();
+
+                    glColor3f(0.0f, 0.0f, 0.0f);
+                    glBegin(GL_LINES);
+                    glVertex3f(c.x(), c.y(), c.z());
+                    glVertex3f(c.x() + 10, c.y(), c.z());
+                    glEnd();
+
+                    glRasterPos3f(c.x() + 10, c.y(), c.z());
+                    glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)gp.name.c_str());
+                }
+            }
+        }
+    }*/
+
+    // gnss.render(session.point_clouds_container);
+
+    ImGui_ImplOpenGL2_NewFrame();
+    ImGui_ImplGLUT_NewFrame();
+    ImGui::NewFrame();
+
+    ShowMainDockSpace();
+
+    if (!is_loop_closure_gui)
+    {
+        Eigen::Affine3d prev_pose_manipulated = Eigen::Affine3d::Identity();
+        Eigen::Affine3d prev_pose_after_gismo = Eigen::Affine3d::Identity();
+
+        for (size_t i = 0; i < sessions.size(); i++)
+        {
+            // guizmo_all_sessions;
+            if (sessions[i].is_gizmo && !sessions[i].is_ground_truth)
+            {
+                if (sessions[i].point_clouds_container.point_clouds.size() > 0)
+                {
+                    prev_pose_manipulated = sessions[i].point_clouds_container.point_clouds[0].m_pose;
+                    std::vector<Eigen::Affine3d> all_m_poses;
+                    for (size_t j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++)
+                        all_m_poses.push_back(sessions[i].point_clouds_container.point_clouds[j].m_pose);
+
+                    ImGuiIO& io = ImGui::GetIO();
+
+                    ImGuizmo::BeginFrame();
+                    ImGuizmo::Enable(true);
+                    ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y);
+
+                    if (!is_ortho)
+                    {
+                        GLfloat projection[16];
+                        glGetFloatv(GL_PROJECTION_MATRIX, projection);
+
+                        GLfloat modelview[16];
+                        glGetFloatv(GL_MODELVIEW_MATRIX, modelview);
+
+                        ImGuizmo::Manipulate(
+                            modelview,
+                            projection,
+                            ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y,
+                            ImGuizmo::WORLD,
+                            m_gizmo,
+                            NULL);
+                    }
+                    else
+                        ImGuizmo::Manipulate(
+                            m_ortho_gizmo_view,
+                            m_ortho_projection,
+                            ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z,
+                            ImGuizmo::WORLD,
+                            m_gizmo,
+                            NULL);
+
+                    sessions[i].point_clouds_container.point_clouds[0].m_pose = Eigen::Map<const Eigen::Matrix4f>(m_gizmo).cast<double>();
+                    prev_pose_after_gismo = sessions[i].point_clouds_container.point_clouds[0].m_pose;
+                    sessions[i].point_clouds_container.point_clouds[0].pose =
+                        pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[0].m_pose);
+
+                    sessions[i].point_clouds_container.point_clouds[0].gui_translation[0] =
+                        (float)sessions[i].point_clouds_container.point_clouds[0].pose.px;
+                    sessions[i].point_clouds_container.point_clouds[0].gui_translation[1] =
+                        (float)sessions[i].point_clouds_container.point_clouds[0].pose.py;
+                    sessions[i].point_clouds_container.point_clouds[0].gui_translation[2] =
+                        (float)sessions[i].point_clouds_container.point_clouds[0].pose.pz;
+
+                    sessions[i].point_clouds_container.point_clouds[0].gui_rotation[0] =
+                        (float)(sessions[i].point_clouds_container.point_clouds[0].pose.om * RAD_TO_DEG);
+                    sessions[i].point_clouds_container.point_clouds[0].gui_rotation[1] =
+                        (float)(sessions[i].point_clouds_container.point_clouds[0].pose.fi * RAD_TO_DEG);
+                    sessions[i].point_clouds_container.point_clouds[0].gui_rotation[2] =
+                        (float)(sessions[i].point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG);
+
+                    Eigen::Affine3d curr_m_pose = sessions[i].point_clouds_container.point_clouds[0].m_pose;
+                    for (size_t j = 1; j < sessions[i].point_clouds_container.point_clouds.size(); j++)
+                    {
+                        curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]);
+                        sessions[i].point_clouds_container.point_clouds[j].m_pose = curr_m_pose;
+                        sessions[i].point_clouds_container.point_clouds[j].pose =
+                            pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[j].m_pose);
+
+                        sessions[i].point_clouds_container.point_clouds[j].gui_translation[0] =
+                            (float)sessions[i].point_clouds_container.point_clouds[j].pose.px;
+                        sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] =
+                            (float)sessions[i].point_clouds_container.point_clouds[j].pose.py;
+                        sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] =
+                            (float)sessions[i].point_clouds_container.point_clouds[j].pose.pz;
+
+                        sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] =
+                            (float)(sessions[i].point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG);
+                        sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] =
+                            (float)(sessions[i].point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG);
+                        sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] =
+                            (float)(sessions[i].point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG);
+                    }
+                    //}
+                }
+            }
+        }
+        if (gizmo_all_sessions)
+        {
+            for (size_t i = 0; i < sessions.size(); i++)
+            {
+                // guizmo_all_sessions;
+                if (!sessions[i].is_gizmo && !sessions[i].is_ground_truth)
+                {
+                    std::vector<Eigen::Affine3d> all_m_poses;
+                    for (size_t j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++)
+                        all_m_poses.push_back(sessions[i].point_clouds_container.point_clouds[j].m_pose);
+
+                    Eigen::Affine3d m_rel_org = prev_pose_manipulated.inverse() * sessions[i].point_clouds_container.point_clouds[0].m_pose;
+
+                    Eigen::Affine3d m_new = prev_pose_after_gismo * m_rel_org;
+
+                    sessions[i].point_clouds_container.point_clouds[0].m_pose = m_new;
+                    sessions[i].point_clouds_container.point_clouds[0].pose =
+                        pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[0].m_pose);
+
+                    sessions[i].point_clouds_container.point_clouds[i].gui_translation[0] =
+                        (float)sessions[i].point_clouds_container.point_clouds[0].pose.px;
+                    sessions[i].point_clouds_container.point_clouds[i].gui_translation[1] =
+                        (float)sessions[i].point_clouds_container.point_clouds[0].pose.py;
+                    sessions[i].point_clouds_container.point_clouds[i].gui_translation[2] =
+                        (float)sessions[i].point_clouds_container.point_clouds[0].pose.pz;
+
+                    sessions[i].point_clouds_container.point_clouds[i].gui_rotation[0] =
+                        (float)(sessions[i].point_clouds_container.point_clouds[0].pose.om * RAD_TO_DEG);
+                    sessions[i].point_clouds_container.point_clouds[i].gui_rotation[1] =
+                        (float)(sessions[i].point_clouds_container.point_clouds[0].pose.fi * RAD_TO_DEG);
+                    sessions[i].point_clouds_container.point_clouds[i].gui_rotation[2] =
+                        (float)(sessions[i].point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG);
+
+                    Eigen::Affine3d curr_m_pose = sessions[i].point_clouds_container.point_clouds[0].m_pose;
+                    for (size_t j = 1; j < sessions[i].point_clouds_container.point_clouds.size(); j++)
+                    {
+                        curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]);
+                        sessions[i].point_clouds_container.point_clouds[j].m_pose = curr_m_pose;
+                        sessions[i].point_clouds_container.point_clouds[j].pose =
+                            pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[j].m_pose);
+
+                        sessions[i].point_clouds_container.point_clouds[j].gui_translation[0] =
+                            (float)sessions[i].point_clouds_container.point_clouds[j].pose.px;
+                        sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] =
+                            (float)sessions[i].point_clouds_container.point_clouds[j].pose.py;
+                        sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] =
+                            (float)sessions[i].point_clouds_container.point_clouds[j].pose.pz;
+
+                        sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] =
+                            (float)(sessions[i].point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG);
+                        sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] =
+                            (float)(sessions[i].point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG);
+                        sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] =
+                            (float)(sessions[i].point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG);
+                    }
+                }
+            }
+        }
+    }
+    else
+    {
+        // ImGuizmo -----------------------------------------------
+        if (edge_gizmo && edges.size() > 0)
+        {
+            ImGuizmo::BeginFrame();
+            ImGuizmo::Enable(true);
+            ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y);
+
+            if (!is_ortho)
+            {
+                GLfloat projection[16];
+                glGetFloatv(GL_PROJECTION_MATRIX, projection);
+
+                GLfloat modelview[16];
+                glGetFloatv(GL_MODELVIEW_MATRIX, modelview);
+
+                ImGuizmo::Manipulate(
+                    &modelview[0],
+                    &projection[0],
+                    ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y,
+                    ImGuizmo::WORLD,
+                    m_gizmo,
+                    NULL);
+            }
+            else
+                ImGuizmo::Manipulate(
+                    m_ortho_gizmo_view,
+                    m_ortho_projection,
+                    ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z,
+                    ImGuizmo::WORLD,
+                    m_gizmo,
+                    NULL);
+
+            Eigen::Affine3d m_g = Eigen::Affine3d::Identity();
+
+            m_g.matrix() = Eigen::Map<const Eigen::Matrix4f>(m_gizmo).cast<double>();
+
+            const int& index_src = edges[index_active_edge].index_from;
+
+            const Eigen::Affine3d& m_src =
+                sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose;
+            edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g);
+        }
+    }
+
+    /*if (!is_loop_closure_gui)
+{
+    for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++)
+    {
+        if (session.point_clouds_container.point_clouds[i].gizmo)
+        {
+            std::vector<Eigen::Affine3d> all_m_poses;
+            for (size_t j = 0; j < session.point_clouds_container.point_clouds.size(); j++)
+                all_m_poses.push_back(session.point_clouds_container.point_clouds[j].m_pose);
+
+            ImGuiIO &io = ImGui::GetIO();
+            // ImGuizmo -----------------------------------------------
+            ImGuizmo::BeginFrame();
+            ImGuizmo::Enable(true);
+            ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y);
+
+            if (!is_ortho)
+            {
+                GLfloat projection[16];
+                glGetFloatv(GL_PROJECTION_MATRIX, projection);
+
+                GLfloat modelview[16];
+                glGetFloatv(GL_MODELVIEW_MATRIX, modelview);
+
+                ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X |
+ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL);
+            }
+            else
+                ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y |
+ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL);
+
+            session.point_clouds_container.point_clouds[i].m_pose(0, 0) = m_gizmo[0];
+            session.point_clouds_container.point_clouds[i].m_pose(1, 0) = m_gizmo[1];
+            session.point_clouds_container.point_clouds[i].m_pose(2, 0) = m_gizmo[2];
+            session.point_clouds_container.point_clouds[i].m_pose(3, 0) = m_gizmo[3];
+            session.point_clouds_container.point_clouds[i].m_pose(0, 1) = m_gizmo[4];
+            session.point_clouds_container.point_clouds[i].m_pose(1, 1) = m_gizmo[5];
+            session.point_clouds_container.point_clouds[i].m_pose(2, 1) = m_gizmo[6];
+            session.point_clouds_container.point_clouds[i].m_pose(3, 1) = m_gizmo[7];
+            session.point_clouds_container.point_clouds[i].m_pose(0, 2) = m_gizmo[8];
+            session.point_clouds_container.point_clouds[i].m_pose(1, 2) = m_gizmo[9];
+            session.point_clouds_container.point_clouds[i].m_pose(2, 2) = m_gizmo[10];
+            session.point_clouds_container.point_clouds[i].m_pose(3, 2) = m_gizmo[11];
+            session.point_clouds_container.point_clouds[i].m_pose(0, 3) = m_gizmo[12];
+            session.point_clouds_container.point_clouds[i].m_pose(1, 3) = m_gizmo[13];
+            session.point_clouds_container.point_clouds[i].m_pose(2, 3) = m_gizmo[14];
+            session.point_clouds_container.point_clouds[i].m_pose(3, 3) = m_gizmo[15];
+            session.point_clouds_container.point_clouds[i].pose =
+pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[i].m_pose);
+
+            session.point_clouds_container.point_clouds[i].gui_translation[0] =
+(float)session.point_clouds_container.point_clouds[i].pose.px; session.point_clouds_container.point_clouds[i].gui_translation[1] =
+(float)session.point_clouds_container.point_clouds[i].pose.py; session.point_clouds_container.point_clouds[i].gui_translation[2] =
+(float)session.point_clouds_container.point_clouds[i].pose.pz;
+
+            session.point_clouds_container.point_clouds[i].gui_rotation[0] = (float)(session.point_clouds_container.point_clouds[i].pose.om
+* RAD_TO_DEG); session.point_clouds_container.point_clouds[i].gui_rotation[1] =
+(float)(session.point_clouds_container.point_clouds[i].pose.fi * RAD_TO_DEG); session.point_clouds_container.point_clouds[i].gui_rotation[2]
+= (float)(session.point_clouds_container.point_clouds[i].pose.ka * RAD_TO_DEG);
+
+            if (!manipulate_only_marked_gizmo)
+            {
+                Eigen::Affine3d curr_m_pose = session.point_clouds_container.point_clouds[i].m_pose;
+                for (size_t j = i + 1; j < session.point_clouds_container.point_clouds.size(); j++)
+                {
+                    curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]);
+                    session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose;
+                    session.point_clouds_container.point_clouds[j].pose =
+pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[j].m_pose);
+
+                    session.point_clouds_container.point_clouds[j].gui_translation[0] =
+(float)session.point_clouds_container.point_clouds[j].pose.px; session.point_clouds_container.point_clouds[j].gui_translation[1] =
+(float)session.point_clouds_container.point_clouds[j].pose.py; session.point_clouds_container.point_clouds[j].gui_translation[2] =
+(float)session.point_clouds_container.point_clouds[j].pose.pz;
+
+                    session.point_clouds_container.point_clouds[j].gui_rotation[0] =
+(float)(session.point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); session.point_clouds_container.point_clouds[j].gui_rotation[1]
+= (float)(session.point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG);
+                    session.point_clouds_container.point_clouds[j].gui_rotation[2] =
+(float)(session.point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG);
+                }
+            }
+        }
+    }
+
+    session.point_clouds_container.render(observation_picking, viewer_decmiate_point_cloud);
+    observation_picking.render();
+
+    glPushAttrib(GL_ALL_ATTRIB_BITS);
+    glPointSize(5);
+    for (const auto &obs : observation_picking.observations)
+    {
+        for (const auto &[key1, value1] : obs)
+        {
+            for (const auto &[key2, value2] : obs)
+            {
+                if (key1 != key2)
+                {
+                    Eigen::Vector3d p1, p2;
+                    if (session.point_clouds_container.show_with_initial_pose)
+                    {
+                        p1 = session.point_clouds_container.point_clouds[key1].m_initial_pose * value1;
+                        p2 = session.point_clouds_container.point_clouds[key2].m_initial_pose * value2;
+                    }
+                    else
+                    {
+                        p1 = session.point_clouds_container.point_clouds[key1].m_pose * value1;
+                        p2 = session.point_clouds_container.point_clouds[key2].m_pose * value2;
+                    }
+                    glColor3f(0, 1, 0);
+                    glBegin(GL_POINTS);
+                    glVertex3f(p1.x(), p1.y(), p1.z());
+                    glVertex3f(p2.x(), p2.y(), p2.z());
+                    glEnd();
+                    glColor3f(1, 0, 0);
+                    glBegin(GL_LINES);
+                    glVertex3f(p1.x(), p1.y(), p1.z());
+                    glVertex3f(p2.x(), p2.y(), p2.z());
+                    glEnd();
+                }
+            }
+        }
+    }
+    glPopAttrib();
+
+    for (const auto &obs : observation_picking.observations)
+    {
+        Eigen::Vector3d mean(0, 0, 0);
+        int counter = 0;
+        for (const auto &[key1, value1] : obs)
+        {
+            mean += session.point_clouds_container.point_clouds[key1].m_initial_pose * value1;
+            counter++;
+        }
+        if (counter > 0)
+        {
+            mean /= counter;
+
+            glColor3f(1, 0, 0);
+            glBegin(GL_LINE_STRIP);
+            glVertex3f(mean.x() - 1, mean.y() - 1, mean.z());
+            glVertex3f(mean.x() + 1, mean.y() - 1, mean.z());
+            glVertex3f(mean.x() + 1, mean.y() + 1, mean.z());
+            glVertex3f(mean.x() - 1, mean.y() + 1, mean.z());
+            glVertex3f(mean.x() - 1, mean.y() - 1, mean.z());
+            glEnd();F
+        }
+    }
+
+    glColor3f(1, 0, 1);
+    glBegin(GL_POINTS);
+    for (auto p : picked_points)
+    {
+        glVertex3f(p.x(), p.y(), p.z());
+    }
+    glEnd();
+}
+else
+{
+    // ImGuizmo -----------------------------------------------
+    if (session.manual_pose_graph_loop_closure.gizmo && session.manual_pose_graph_loop_closure.edges.size() > 0)
+    {
+        ImGuizmo::BeginFrame();
+        ImGuizmo::Enable(true);
+        ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y);
+
+        if (!is_ortho)
+        {
+            GLfloat projection[16];
+            glGetFloatv(GL_PROJECTION_MATRIX, projection);
+
+            GLfloat modelview[16];
+            glGetFloatv(GL_MODELVIEW_MATRIX, modelview);
+
+            ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X |
+ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL);
+        }
+        else
+            ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z,
+ImGuizmo::WORLD, m_gizmo, NULL);
+
+        Eigen::Affine3d m_g = Eigen::Affine3d::Identity();
+
+        m_g(0, 0) = m_gizmo[0];
+        m_g(1, 0) = m_gizmo[1];
+        m_g(2, 0) = m_gizmo[2];
+        m_g(3, 0) = m_gizmo[3];
+        m_g(0, 1) = m_gizmo[4];
+        m_g(1, 1) = m_gizmo[5];
+        m_g(2, 1) = m_gizmo[6];
+        m_g(3, 1) = m_gizmo[7];
+        m_g(0, 2) = m_gizmo[8];
+        m_g(1, 2) = m_gizmo[9];
+        m_g(2, 2) = m_gizmo[10];
+        m_g(3, 2) = m_gizmo[11];
+        m_g(0, 3) = m_gizmo[12];
+        m_g(1, 3) = m_gizmo[13];
+        m_g(2, 3) = m_gizmo[14];
+        m_g(3, 3) = m_gizmo[15];
+
+        const int &index_src =
+session.manual_pose_graph_loop_closure.edges[session.manual_pose_graph_loop_closure.index_active_edge].index_from;
+
+        const Eigen::Affine3d &m_src = session.point_clouds_container.point_clouds.at(index_src).m_pose;
+        session.manual_pose_graph_loop_closure.edges[session.manual_pose_graph_loop_closure.index_active_edge].relative_pose_tb =
+pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g);
+    }
+}*/
+
+    view_kbd_shortcuts();
+
+    if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_A, false))
+    {
+        addSession();
+
+        // workaround
+        io.AddKeyEvent(ImGuiKey_A, false);
+        io.AddKeyEvent(ImGuiMod_Ctrl, false);
+    }
+    if ((project_settings.session_file_names.size() > 0) && !loaded_sessions)
+        if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_L, false))
+        {
+            loadSessions();
+
+            // workaround
+            io.AddKeyEvent(ImGuiKey_L, false);
+            io.AddKeyEvent(ImGuiMod_Ctrl, false);
+        }
+    if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_O, false))
+    {
+        openProject();
+
+        // workaround
+        io.AddKeyEvent(ImGuiKey_O, false);
+        io.AddKeyEvent(ImGuiMod_Ctrl, false);
+    }
+
+    if (project_settings.session_file_names.size() > 0)
+        if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_R, false))
+        {
+            remove_gui = true;
+
+            // workaround
+            io.AddKeyEvent(ImGuiKey_R, false);
+            io.AddKeyEvent(ImGuiMod_Ctrl, false);
+        }
+
+    if (sessions.size() > 0)
+        if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_S, false))
+        {
+            saveProject();
+
+            // workaround
+            io.AddKeyEvent(ImGuiKey_S, false);
+            io.AddKeyEvent(ImGuiMod_Ctrl, false);
+        }
+
+    if (ImGui::BeginMainMenuBar())
+    {
+        if (ImGui::BeginMenu("File"))
+        {
+            if (ImGui::MenuItem("Open project", "Ctrl+O"))
+                openProject();
+            if (ImGui::MenuItem("Save project", "Ctrl+S", nullptr, project_settings.session_file_names.size() > 0))
+                saveProject();
+
+            ImGui::Separator();
+
+            if (ImGui::MenuItem("Add session(s)", "Ctrl+A"))
+                addSession();
+            if (ImGui::MenuItem("Remove session(s)", "Ctrl+R", nullptr, project_settings.session_file_names.size() > 0))
+                remove_gui = true;
+
+            if (ImGui::MenuItem("Load sessions", "Ctrl+L", nullptr, (project_settings.session_file_names.size() > 0) && !loaded_sessions))
+                loadSessions();
+
+            ImGui::Separator();
+
+            if (ImGui::BeginMenu("Save all marked trajectories", sessions.size() > 0))
+            {
+                if (ImGui::MenuItem("Save all as las/laz files"))
+                {
+                    for (size_t i = 0; i < project_settings.session_file_names.size(); ++i)
+                    {
+                        const auto& session_path = project_settings.session_file_names[i];
+
+                        if (i >= sessions.size())
+                        {
+                            std::cerr << "No loaded session for: " << session_path << std::endl;
+                            continue;
+                        }
+
+                        Session& session = sessions[i];
+
+                        std::filesystem::path dir = std::filesystem::path(session_path).parent_path();
+                        std::string folder_name = dir.filename().string();
+                        std::string laz_path = (dir / (folder_name + "_trajectory_laz.laz")).string();
+
+                        std::cout << "Saving trajectory to LAZ: " << laz_path << std::endl;
+
+                        save_trajectories_to_laz(session, laz_path, 0.0f, 0.0f, false);
+                    }
+
+                    std::cout << "Finished saving all trajectories to .laz files." << std::endl;
+                }
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("As one global scan");
+
+                ImGui::Separator();
+
+                ImGui::Text("(x,y,z,r00,r01,r02,r10,r11,r12,r20,r21,r22)");
+                if (ImGui::MenuItem("Save all as csv (timestamp Lidar)##1"))
+                {
+                    for (size_t i = 0; i < project_settings.session_file_names.size(); ++i)
+                    {
+                        const auto& session_path = project_settings.session_file_names[i];
+
+                        if (i >= sessions.size())
+                        {
+                            std::cerr << "No loaded session for: " << session_path << std::endl;
+                            continue;
+                        }
+
+                        Session& session = sessions[i];
+                        std::filesystem::path dir = std::filesystem::path(session_path).parent_path();
+                        std::string folder_name = dir.filename().string();
+                        std::string csv_path = (dir / (folder_name + "_trajectory_timestampLidar_r.csv")).string();
+
+                        std::cout << "Saving trajectory to CSV: " << csv_path << std::endl;
+
+                        try
+                        {
+                            std::ofstream outfile(csv_path);
+                            if (!outfile.is_open())
+                            {
+                                std::cerr << "Failed to create file: " << csv_path << std::endl;
+                                continue;
+                            }
+
+                            outfile << "timestampLidar,x,y,z," << "r00,r01,r02," << "r10,r11,r12," << "r20,r21,r22\n";
+
+                            for (const auto& pc : session.point_clouds_container.point_clouds)
+                            {
+                                if (!pc.visible)
+                                    continue;
+
+                                for (const auto& traj : pc.local_trajectory)
+                                {
+                                    Eigen::Affine3d pose = pc.m_pose * traj.m_pose;
+                                    Eigen::Vector3d pos = pose.translation();
+                                    Eigen::Matrix3d rot = pose.rotation();
+
+                                    outfile << std::fixed << std::setprecision(0) << traj.timestamps.first << "," << std::setprecision(10)
+                                            << pos.x() << "," << pos.y() << "," << pos.z() << "," << rot(0, 0) << "," << rot(0, 1) << ","
+                                            << rot(0, 2) << "," << rot(1, 0) << "," << rot(1, 1) << "," << rot(1, 2) << "," << rot(2, 0)
+                                            << "," << rot(2, 1) << "," << rot(2, 2) << "\n";
+                                }
+                            }
+
+                            outfile.close();
+                            std::cout << "Saved: " << csv_path << std::endl;
+                        } catch (const std::exception& e)
+                        {
+                            std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl;
+                        }
+                    }
+
+                    std::cout << "Finished saving all trajectories to CSV files." << std::endl;
+                }
+                if (ImGui::MenuItem("Save all as csv (timestamp Unix)##1"))
+                {
+                    for (size_t i = 0; i < project_settings.session_file_names.size(); ++i)
+                    {
+                        const auto& session_path = project_settings.session_file_names[i];
+                        if (i >= sessions.size())
+                        {
+                            std::cerr << "No loaded session for: " << session_path << std::endl;
+                            continue;
+                        }
+
+                        Session& session = sessions[i];
+                        std::filesystem::path dir = std::filesystem::path(session_path).parent_path();
+                        std::string folder_name = dir.filename().string();
+                        std::string csv_path = (dir / (folder_name + "_trajectory_timestampUnix_r.csv")).string();
+
+                        try
+                        {
+                            std::ofstream outfile(csv_path);
+                            if (!outfile.is_open())
+                            {
+                                std::cerr << "Failed to create file: " << csv_path << std::endl;
+                                continue;
+                            }
+
+                            outfile << "timestampUnix,x,y,z," << "r00,r01,r02,r10,r11,r12,r20,r21,r22\n";
+
+                            for (const auto& pc : session.point_clouds_container.point_clouds)
+                            {
+                                if (!pc.visible)
+                                    continue;
+                                for (const auto& traj : pc.local_trajectory)
+                                {
+                                    Eigen::Affine3d pose = pc.m_pose * traj.m_pose;
+                                    Eigen::Vector3d pos = pose.translation();
+                                    Eigen::Matrix3d rot = pose.rotation();
+                                    outfile << std::fixed << std::setprecision(0) << traj.timestamps.second << "," // Unix timestamp
+                                            << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << rot(0, 0)
+                                            << "," << rot(0, 1) << "," << rot(0, 2) << "," << rot(1, 0) << "," << rot(1, 1) << ","
+                                            << rot(1, 2) << "," << rot(2, 0) << "," << rot(2, 1) << "," << rot(2, 2) << "\n";
+                                }
+                            }
+
+                            outfile.close();
+                            std::cout << "Saved: " << csv_path << std::endl;
+                        } catch (const std::exception& e)
+                        {
+                            std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl;
+                        }
+                    }
+                }
+                if (ImGui::MenuItem("Save all as csv (timestamp Lidar, Unix)##1"))
+                {
+                    for (size_t i = 0; i < project_settings.session_file_names.size(); ++i)
+                    {
+                        const auto& session_path = project_settings.session_file_names[i];
+                        if (i >= sessions.size())
+                        {
+                            std::cerr << "No loaded session for: " << session_path << std::endl;
+                            continue;
+                        }
+
+                        Session& session = sessions[i];
+                        std::filesystem::path dir = std::filesystem::path(session_path).parent_path();
+                        std::string folder_name = dir.filename().string();
+                        std::string csv_path = (dir / (folder_name + "_trajectory_timestampLidarUnix_r.csv")).string();
+
+                        try
+                        {
+                            std::ofstream outfile(csv_path);
+                            if (!outfile.is_open())
+                            {
+                                std::cerr << "Failed to create file: " << csv_path << std::endl;
+                                continue;
+                            }
+
+                            outfile << "timestampLidar,timestampUnix,x,y,z," << "r00,r01,r02,r10,r11,r12,r20,r21,r22\n";
+
+                            for (const auto& pc : session.point_clouds_container.point_clouds)
+                            {
+                                if (!pc.visible)
+                                    continue;
+                                for (const auto& traj : pc.local_trajectory)
+                                {
+                                    Eigen::Affine3d pose = pc.m_pose * traj.m_pose;
+                                    Eigen::Vector3d pos = pose.translation();
+                                    Eigen::Matrix3d rot = pose.rotation();
+                                    outfile << std::fixed << std::setprecision(0) << traj.timestamps.first << "," // Lidar timestamp
+                                            << traj.timestamps.second << "," // Unix timestamp
+                                            << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << rot(0, 0)
+                                            << "," << rot(0, 1) << "," << rot(0, 2) << "," << rot(1, 0) << "," << rot(1, 1) << ","
+                                            << rot(1, 2) << "," << rot(2, 0) << "," << rot(2, 1) << "," << rot(2, 2) << "\n";
+                                }
+                            }
+
+                            outfile.close();
+                            std::cout << "Saved: " << csv_path << std::endl;
+                        } catch (const std::exception& e)
+                        {
+                            std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl;
+                        }
+                    }
+                }
+
+                ImGui::Separator();
+                ImGui::Text("(x,y,z,qx,qy,qz,qw)");
+
+                if (ImGui::MenuItem("Save all as csv (timestamp Lidar)##2"))
+                {
+                    for (size_t i = 0; i < project_settings.session_file_names.size(); ++i)
+                    {
+                        const auto& session_path = project_settings.session_file_names[i];
+
+                        if (i >= sessions.size())
+                        {
+                            std::cerr << "No loaded session for: " << session_path << std::endl;
+                            continue;
+                        }
+
+                        Session& session = sessions[i];
+                        std::filesystem::path dir = std::filesystem::path(session_path).parent_path();
+                        std::string folder_name = dir.filename().string();
+                        std::string csv_path = (dir / (folder_name + "_trajectory_timestampLidar_q.csv")).string();
+
+                        std::cout << "Saving trajectory to CSV: " << csv_path << std::endl;
+
+                        try
+                        {
+                            std::ofstream outfile(csv_path);
+                            if (!outfile.is_open())
+                            {
+                                std::cerr << "Failed to create file: " << csv_path << std::endl;
+                                continue;
+                            }
+
+                            outfile << "timestampLidar,x,y,z,qx,qy,qz,qw\n";
+
+                            for (const auto& pc : session.point_clouds_container.point_clouds)
+                            {
+                                if (!pc.visible)
+                                    continue;
+
+                                for (const auto& traj : pc.local_trajectory)
+                                {
+                                    Eigen::Affine3d pose = pc.m_pose * traj.m_pose;
+                                    Eigen::Vector3d pos = pose.translation();
+                                    Eigen::Quaterniond q(pose.rotation());
+
+                                    outfile << std::fixed << std::setprecision(0) << traj.timestamps.first << "," // Lidar timestamp
+                                            << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << q.x() << ","
+                                            << q.y() << "," << q.z() << "," << q.w() << "\n";
+                                }
+                            }
+
+                            outfile.close();
+                            std::cout << "Saved: " << csv_path << std::endl;
+                        } catch (const std::exception& e)
+                        {
+                            std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl;
+                        }
+                    }
+
+                    std::cout << "Finished saving all trajectories to CSV files." << std::endl;
+                }
+                if (ImGui::MenuItem("Save all as csv (timestamp Unix)##2"))
+                {
+                    for (size_t i = 0; i < project_settings.session_file_names.size(); ++i)
+                    {
+                        const auto& session_path = project_settings.session_file_names[i];
+
+                        if (i >= sessions.size())
+                        {
+                            std::cerr << "No loaded session for: " << session_path << std::endl;
+                            continue;
+                        }
+
+                        Session& session = sessions[i];
+                        std::filesystem::path dir = std::filesystem::path(session_path).parent_path();
+                        std::string folder_name = dir.filename().string();
+                        std::string csv_path = (dir / (folder_name + "_trajectory_timestampUnix_q.csv")).string();
+
+                        std::cout << "Saving trajectory to CSV: " << csv_path << std::endl;
+
+                        try
+                        {
+                            std::ofstream outfile(csv_path);
+                            if (!outfile.is_open())
+                            {
+                                std::cerr << "Failed to create file: " << csv_path << std::endl;
+                                continue;
+                            }
+
+                            outfile << "timestampUnix,x,y,z,qx,qy,qz,qw\n";
+
+                            for (const auto& pc : session.point_clouds_container.point_clouds)
+                            {
+                                if (!pc.visible)
+                                    continue;
+
+                                for (const auto& traj : pc.local_trajectory)
+                                {
+                                    Eigen::Affine3d pose = pc.m_pose * traj.m_pose;
+                                    Eigen::Vector3d pos = pose.translation();
+                                    Eigen::Quaterniond q(pose.rotation());
+
+                                    outfile << std::fixed << std::setprecision(0) << traj.timestamps.second << "," // Unix timestamp
+                                            << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << q.x() << ","
+                                            << q.y() << "," << q.z() << "," << q.w() << "\n";
+                                }
+                            }
+
+                            outfile.close();
+                            std::cout << "Saved: " << csv_path << std::endl;
+                        } catch (const std::exception& e)
+                        {
+                            std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl;
+                        }
+                    }
+
+                    std::cout << "Finished saving all trajectories to CSV files." << std::endl;
+                }
+                if (ImGui::MenuItem("Save all as csv (timestamp Lidar, Unix)##2"))
+                {
+                    for (size_t i = 0; i < project_settings.session_file_names.size(); ++i)
+                    {
+                        const auto& session_path = project_settings.session_file_names[i];
+
+                        if (i >= sessions.size())
+                        {
+                            std::cerr << "No loaded session for: " << session_path << std::endl;
+                            continue;
+                        }
+
+                        Session& session = sessions[i];
+                        std::filesystem::path dir = std::filesystem::path(session_path).parent_path();
+                        std::string folder_name = dir.filename().string();
+                        std::string csv_path = (dir / (folder_name + "_trajectory_timestampLidarUnix_q.csv")).string();
+
+                        std::cout << "Saving trajectory to CSV: " << csv_path << std::endl;
+
+                        try
+                        {
+                            std::ofstream outfile(csv_path);
+                            if (!outfile.is_open())
+                            {
+                                std::cerr << "Failed to create file: " << csv_path << std::endl;
+                                continue;
+                            }
+
+                            outfile << "timestampLidar,timestampUnix,x,y,z,qx,qy,qz,qw\n";
+
+                            for (const auto& pc : session.point_clouds_container.point_clouds)
+                            {
+                                if (!pc.visible)
+                                    continue;
+
+                                for (const auto& traj : pc.local_trajectory)
+                                {
+                                    Eigen::Affine3d pose = pc.m_pose * traj.m_pose;
+                                    Eigen::Vector3d pos = pose.translation();
+                                    Eigen::Quaterniond q(pose.rotation());
+
+                                    outfile << std::fixed << std::setprecision(0) << traj.timestamps.first << "," // Lidar timestamp
+                                            << traj.timestamps.second << "," // Unix timestamp
+                                            << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << q.x() << ","
+                                            << q.y() << "," << q.z() << "," << q.w() << "\n";
+                                }
+                            }
+
+                            outfile.close();
+                            std::cout << "Saved: " << csv_path << std::endl;
+                        } catch (const std::exception& e)
+                        {
+                            std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl;
+                        }
+                    }
+
+                    std::cout << "Finished saving all trajectories to CSV files." << std::endl;
+                }
+                if (ImGui::MenuItem("Save all as TUM TXT"))
+                {
+                    for (size_t i = 0; i < project_settings.session_file_names.size(); ++i)
+                    {
+                        const auto& session_path = project_settings.session_file_names[i];
+                        if (i >= sessions.size())
+                        {
+                            std::cerr << "No loaded session for: " << session_path << std::endl;
+                            continue;
+                        }
+                        Session& session = sessions[i];
+                        std::filesystem::path dir = std::filesystem::path(session_path).parent_path();
+                        std::string folder_name = dir.filename().string();
+                        std::string txt_path = (dir / (folder_name + "_trajectory_tum.txt")).string();
+
+                        std::cout << "Saving trajectory to TUM TXT: " << txt_path << std::endl;
+                        try
+                        {
+                            std::ofstream outfile(txt_path);
+                            if (!outfile.is_open())
+                            {
+                                std::cerr << "Failed to create file: " << txt_path << std::endl;
+                                continue;
+                            }
+
+                            for (const auto& pc : session.point_clouds_container.point_clouds)
+                            {
+                                if (!pc.visible)
+                                    continue;
+                                for (const auto& traj : pc.local_trajectory)
+                                {
+                                    Eigen::Affine3d pose = pc.m_pose * traj.m_pose;
+                                    Eigen::Vector3d pos = pose.translation();
+                                    Eigen::Quaterniond q(pose.rotation());
+
+                                    double t_s = static_cast<double>(traj.timestamps.first) / 1e9;
+
+                                    outfile << std::fixed << std::setprecision(9) << t_s << " " << std::setprecision(10) << pos.x() << " "
+                                            << pos.y() << " " << pos.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w()
+                                            << "\n";
+                                }
+                            }
+
+                            outfile.close();
+                            std::cout << "Saved: " << txt_path << std::endl;
+                        } catch (const std::exception& e)
+                        {
+                            std::cerr << "Error creating " << txt_path << ": " << e.what() << std::endl;
+                        }
+                    }
+                    std::cout << "Finished saving all trajectories to TUM TXT files." << std::endl;
+                }
+                if (ImGui::MenuItem("Save all as TUM TXT (single folder)"))
+                {
+                    for (size_t i = 0; i < project_settings.session_file_names.size(); ++i)
+                    {
+                        const auto& session_path = project_settings.session_file_names[i];
+
+                        if (i >= sessions.size())
+                        {
+                            std::cerr << "No loaded session for: " << session_path << std::endl;
+                            continue;
+                        }
+
+                        Session& session = sessions[i];
+
+                        std::filesystem::path session_dir = std::filesystem::path(session_path).parent_path();
+
+                        std::filesystem::path base_dir = session_dir.parent_path();
+
+                        std::filesystem::path output_dir = base_dir / "all_tum_files";
+
+                        try
+                        {
+                            std::filesystem::create_directories(output_dir);
+                        } catch (const std::exception& e)
+                        {
+                            std::cerr << "Failed to create export directory: " << e.what() << std::endl;
+                            continue;
+                        }
+
+                        std::string folder_name = session_dir.filename().string();
+
+                        std::filesystem::path txt_path = output_dir / (folder_name + "_trajectory_tum.txt");
+
+                        std::cout << "Saving trajectory to TUM TXT: " << txt_path << std::endl;
+
+                        try
+                        {
+                            std::ofstream outfile(txt_path);
+
+                            if (!outfile.is_open())
+                            {
+                                std::cerr << "Failed to create file: " << txt_path << std::endl;
+                                continue;
+                            }
+
+                            for (const auto& pc : session.point_clouds_container.point_clouds)
+                            {
+                                if (!pc.visible)
+                                    continue;
+
+                                for (const auto& traj : pc.local_trajectory)
+                                {
+                                    Eigen::Affine3d pose = pc.m_pose * traj.m_pose;
+
+                                    Eigen::Vector3d pos = pose.translation();
+
+                                    Eigen::Quaterniond q(pose.rotation());
+
+                                    double t_s = static_cast<double>(traj.timestamps.first) / 1e9;
+
+                                    outfile << std::fixed << std::setprecision(9) << t_s << " " << std::setprecision(10) << pos.x() << " "
+                                            << pos.y() << " " << pos.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w()
+                                            << "\n";
+                                }
+                            }
+
+                            outfile.close();
+
+                            std::cout << "Saved: " << txt_path << std::endl;
+                        } catch (const std::exception& e)
+                        {
+                            std::cerr << "Error creating " << txt_path << ": " << e.what() << std::endl;
+                        }
+                    }
+                    std::cout << "Finished saving all trajectories to single folder." << std::endl;
+                }
+
+                ImGui::EndMenu();
+            }
+
+            ImGui::EndMenu();
+        }
+
+        if (ImGui::BeginMenu("Tools"))
+        {
+            ImGui::MenuItem("Normal Distributions Transform", nullptr, &is_ndt_gui, !is_loop_closure_gui && (sessions.size() > 0));
+            if (ImGui::IsItemHovered())
+            {
+                ImGui::BeginTooltip();
+                ImGui::Text("Point cloud alignment (registration) algorithm");
+                ImGui::Text(
+                    "Probabilistic alternative to ICP that models one cloud (the target)\nas a set of Gaussian distributions "
+                    "rather than raw points");
+                ImGui::Text(
+                    "Robust for rough initial poses but can converge to a local optimum\nif the initial misalignment is very large");
+                ImGui::Text(
+                    "Known for being faster and smoother in optimization because\nit replaces discrete point-point correspondences "
+                    "with continuous probability density functions.");
+                ImGui::EndTooltip();
+            }
+
+            // bool prev_is_loop_closure_gui
+            ImGui::MenuItem(
+                "Manual Loop Closure", "Ctrl+L", &is_loop_closure_gui, (number_visible_sessions == 1 || number_visible_sessions == 2));
+            if (ImGui::IsItemHovered())
+                ImGui::SetTooltip("Manually connect overlapping scan sections");
+
+            ImGui::EndMenu();
+        }
+
+        if (ImGui::BeginMenu("View"))
+        {
+            ImGui::BeginDisabled(!(sessions.size() > 0));
+            {
+                auto tmp = point_size;
+                ImGui::SetNextItemWidth(ImGuiNumberWidth);
+                ImGui::InputInt("Points size", &point_size);
+                if (ImGui::IsItemHovered())
+                    ImGui::SetTooltip("keyboard 1-9 keys");
+                if (point_size < 1)
+                    point_size = 1;
+                else if (point_size > 10)
+                    point_size = 10;
+
+                if (tmp != point_size)
+                    for (auto& session : sessions)
+                        for (auto& point_cloud : session.point_clouds_container.point_clouds)
+                            point_cloud.point_size = point_size;
+
+                ImGui::Separator();
+            }
+            ImGui::EndDisabled();
+
+            if (ImGui::MenuItem("Orthographic", "key O", &is_ortho))
+            {
+                if (is_ortho)
+                {
+                    new_rotation_center = rotation_center;
+                    new_rotate_x = 0.0;
+                    new_rotate_y = 0.0;
+                    new_translate_x = translate_x;
+                    new_translate_y = translate_y;
+                    new_translate_z = translate_z;
+                    camera_transition_active = true;
+                }
+            }
+            if (ImGui::IsItemHovered())
+                ImGui::SetTooltip("Switch between perspective view (3D) and orthographic view (2D/flat)");
+
+            ImGui::MenuItem("Show axes", "key X", &show_axes);
+            ImGui::MenuItem("Show compass/ruler", "key C", &compass_ruler);
+
+            ImGui::MenuItem("Lock Z", "Shift + Z", &lock_z, !is_ortho);
+
+            // ImGui::MenuItem("show_covs", nullptr, &show_covs);
+
+            ImGui::Separator();
+
+            ImGui::Text("Colors:");
+
+            ImGui::ColorEdit3("Background", (float*)&bg_color, ImGuiColorEditFlags_NoInputs);
+
+            ImGui::Separator();
+
+            ImGui::MenuItem("Settings", nullptr, &is_settings_gui);
+            if (ImGui::IsItemHovered())
+                ImGui::SetTooltip("Show power user settings window with more parameters");
+
+            ImGui::EndMenu();
+        }
+        if (ImGui::IsItemHovered())
+            ImGui::SetTooltip("Scene view relevant parameters");
+
+        camMenu();
+
+        ImGui::BeginDisabled(sessions.size() <= 0);
+        {
+            ImGui::SameLine();
+            ImGui::Dummy(ImVec2(20, 0));
+            ImGui::SameLine();
+
+            ImGui::SetNextItemWidth(ImGuiNumberWidth);
+            ImGui::InputInt("Points render downsampling", &viewer_decimate_point_cloud, 10, 100);
+            if (ImGui::IsItemHovered())
+                ImGui::SetTooltip("increase for better performance, decrease for rendering more points");
+            // ImGui::SameLine();
+
+            if (viewer_decimate_point_cloud < 1)
+                viewer_decimate_point_cloud = 1;
+
+            ImGui::SameLine();
+
+            ImGui::InputInt("Trajectory reduce render", &viewer_reduce_rendered_trajectory, 10, 100);
+            if (ImGui::IsItemHovered())
+                ImGui::SetTooltip("increase for better performance, decrease for rendering more nodes in the trajectory");
+            // ImGui::SameLine();
+
+            if (viewer_reduce_rendered_trajectory < 1)
+                viewer_reduce_rendered_trajectory = 1;
+
+            ImGui::SameLine();
+
+            ImGui::Text("(%.1f FPS)", ImGui::GetIO().Framerate);
+        }
+        ImGui::EndDisabled();
+
+        ImGui::SameLine();
+        ImGui::Dummy(ImVec2(20, 0));
+        ImGui::SameLine();
+
+        ImGui::SameLine(
+            ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 -
+            ImGui::GetStyle().FramePadding.x * 2);
+
+        ImGui::PushStyleVar(ImGuiStyleVar_FrameBorderSize, 0.0f);
+        ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(4, 2));
+        ImGui::PushStyleColor(ImGuiCol_Button, ImVec4(0, 0, 0, 0));
+        ImGui::PushStyleColor(ImGuiCol_ButtonHovered, ImGui::GetStyleColorVec4(ImGuiCol_HeaderHovered));
+        ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImGui::GetStyleColorVec4(ImGuiCol_Header));
+        if (ImGui::SmallButton("Info"))
+            info_gui = !info_gui;
+
+        ImGui::PopStyleVar(2);
+        ImGui::PopStyleColor(3);
+
+        ImGui::EndMainMenuBar();
+    }
+
+    if (remove_gui)
+    {
+        ImGui::OpenPopup("Remove session(s)");
+        remove_gui = false;
+    }
+
+    if (ImGui::BeginPopupModal("Remove session(s)", NULL, ImGuiWindowFlags_AlwaysAutoResize))
+    {
+        static std::vector<bool> session_marked_for_removal;
+        if (session_marked_for_removal.size() != project_settings.session_file_names.size())
+            session_marked_for_removal.resize(project_settings.session_file_names.size(), false);
+
+        ImGui::Text("Select session(s) to remove:");
+        ImGui::Separator();
+
+        for (size_t i = 0; i < project_settings.session_file_names.size(); i++)
+        {
+            bool checked = session_marked_for_removal[i];
+            if (ImGui::Checkbox(project_settings.session_file_names[i].c_str(), &checked))
+                session_marked_for_removal[i] = checked;
+        }
+
+        ImGui::Separator();
+
+        if (ImGui::Button("Remove"))
+        {
+            for (size_t i = project_settings.session_file_names.size(); i > 0; --i)
+            {
+                size_t idx = i - 1;
+
+                if (session_marked_for_removal[idx])
+                {
+                    std::cout << "Removing session: " << project_settings.session_file_names[idx] << std::endl;
+
+                    project_settings.session_file_names.erase(project_settings.session_file_names.begin() + idx);
+
+                    if (idx < sessions.size())
+                        sessions.erase(sessions.begin() + idx);
+                }
+            }
+            session_marked_for_removal.clear();
+
+            if (!sessions.empty())
+                update_timestamp_offset();
+            else
+            {
+                loaded_sessions = false;
+                time_stamp_offset = 0.0;
+            }
+
+            ImGui::CloseCurrentPopup();
+        }
+
+        ImGui::SameLine();
+        if (ImGui::Button("Cancel"))
+        {
+            session_marked_for_removal.clear();
+            ImGui::CloseCurrentPopup();
+        }
+
+        ImGui::EndPopup();
+    }
+
+    if (is_ndt_gui)
+        ndt_gui();
+
+    if (is_loop_closure_gui)
+        loop_closure_gui();
+
+    cor_window();
+
+    info_window(infoLines, appShortcuts);
+
+    if (compass_ruler)
+        drawMiniCompassWithRuler();
+
+    // my_display_code();
+    /*if (is_ndt_gui)
+        ndt_gui();
+    if (is_icp_gui)
+        icp_gui();
+    if (is_pose_graph_slam)
+        pose_graph_slam_gui();
+    if (is_registration_plane_feature)
+        registration_plane_feature_gui();
+    if (is_manual_analisys)
+        observation_picking_gui();*/
+    // if (is_loop_closure_gui)
+    //     manual_pose_graph_loop_closure.Gui();
+
+    if (is_settings_gui)
+        settings_gui();
+
+    ImGui::Render();
+    ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData());
+
+    glutSwapBuffers();
+    glutPostRedisplay();
+}
+
+void mouse(int glut_button, int state, int x, int y)
+{
+    ImGuiIO& io = ImGui::GetIO();
+    io.MousePos = ImVec2((float)x, (float)y);
+    int button = -1;
+    if (glut_button == GLUT_LEFT_BUTTON)
+        button = 0;
+    if (glut_button == GLUT_RIGHT_BUTTON)
+        button = 1;
+    if (glut_button == GLUT_MIDDLE_BUTTON)
+        button = 2;
+    if (button != -1 && state == GLUT_DOWN)
+        io.MouseDown[button] = true;
+    if (button != -1 && state == GLUT_UP)
+        io.MouseDown[button] = false;
+
+    static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000;
+    if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && glutMajorVersion < 3)
+        wheel(glut_button, glut_button == 3 ? 1 : -1, x, y);
+
+    if (!io.WantCaptureMouse)
+    {
+        if ((glut_button == GLUT_MIDDLE_BUTTON || glut_button == GLUT_RIGHT_BUTTON) && state == GLUT_DOWN && (io.KeyCtrl || io.KeyShift) &&
+            !manipulate_active_edge)
+        {
+            // if (s_loop_closure_gui)
+            if ((sessions.size() > 0) && (number_visible_sessions > 0) && update_rotation_center)
+            {
+                getClosestTrajectoriesPoint(
+                    sessions,
+                    x,
+                    y,
+                    first_session_index,
+                    second_session_index,
+                    number_visible_sessions,
+                    index_loop_closure_source,
+                    index_loop_closure_target,
+                    io.KeyShift,
+                    time_stamp_offset);
+            }
+            else
+            {
+                if (update_rotation_center)
+                {
+                    setNewRotationCenter(x, y);
+                }
+            }
+        }
+
+        if (state == GLUT_DOWN)
+        {
+            mouse_buttons |= 1 << glut_button;
+
+            /*if (observation_picking.is_observation_picking_mode)
+            {
+                Eigen::Vector3d p = GLWidgetGetOGLPos(x, y, observation_picking);
+                int number_active_pcs = 0;
+                int index_picked = -1;
+                for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++)
+                {
+                    if (session.point_clouds_container.point_clouds[i].visible)
+                    {
+                        number_active_pcs++;
+                        index_picked = i;
+                    }
+                }
+                if (number_active_pcs == 1)
+                {
+                    observation_picking.add_picked_to_current_observation(index_picked, p);
+                }
+            }*/
+        }
+        else if (state == GLUT_UP)
+        {
+            mouse_buttons = 0;
+        }
+        mouse_old_x = x;
+        mouse_old_y = y;
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    try
+    {
+        if (checkClHelp(argc, argv))
+        {
+            std::cout << winTitle << "\n\n"
+                      << "USAGE:\n"
+                      << std::filesystem::path(argv[0]).stem().string() << " <input_file> /?\n\n"
+                      << "where\n"
+                      << "   <input_file>         Path to Mandeye JSON Project file (*.mjp)\n"
+                      << "   -h, /h, --help, /?   Show this help and exit\n\n";
+
+            return 0;
+        }
+
+        initGL(&argc, argv, winTitle, display, mouse);
+
+        if (argc > 1)
+        {
+            for (int i = 1; i < argc; i++)
+            {
+                std::string ext = fs::path(argv[i]).extension().string();
+                std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower);
+
+                if (ext == ".mjp")
+                {
+                    loadProject(argv[i], project_settings);
+
+                    break;
+                }
+            }
+        }
+
+        glutMainLoop();
+
+        ImGui_ImplOpenGL2_Shutdown();
+        ImGui_ImplGLUT_Shutdown();
+        ImGui::DestroyContext();
+    } catch (const std::bad_alloc& e)
+    {
+        std::cerr << "System is out of memory : " << e.what() << std::endl;
+        mandeye::fd::OutOfMemMessage();
+    } catch (const std::exception& e)
+    {
+        std::cout << e.what();
+    } catch (...)
+    {
+        std::cerr << "Unknown fatal error occurred." << std::endl;
+    }
+
+    return 0;
+}
\ No newline at end of file
diff --git a/apps/multi_session_registration_legacy/resource.h b/apps/multi_session_registration_legacy/resource.h
new file mode 100644
index 00000000..4f3204ca
--- /dev/null
+++ b/apps/multi_session_registration_legacy/resource.h
@@ -0,0 +1,17 @@
+//{{NO_DEPENDENCIES}}
+// Microsoft Visual C++ generated include file.
+// Used by resource.rc
+//
+#define IDI_ICON1 101 // application icon
+#define VS_VERSION_INFO 1 // version info
+
+// Next default values for new objects
+//
+#ifdef APSTUDIO_INVOKED
+#ifndef APSTUDIO_READONLY_SYMBOLS
+#define _APS_NEXT_RESOURCE_VALUE 106
+#define _APS_NEXT_COMMAND_VALUE 40001
+#define _APS_NEXT_CONTROL_VALUE 1000
+#define _APS_NEXT_SYMED_VALUE 101
+#endif
+#endif
diff --git a/apps/multi_session_registration_legacy/resource.rc b/apps/multi_session_registration_legacy/resource.rc
new file mode 100644
index 00000000..e8821486
--- /dev/null
+++ b/apps/multi_session_registration_legacy/resource.rc
@@ -0,0 +1,49 @@
+// Microsoft Visual C++ generated resource script.
+//
+#include "resource.h"
+/////////////////////////////////////////////////////////////////////////////
+// English (United States) resources
+
+/////////////////////////////////////////////////////////////////////////////
+//
+// Icon
+//
+
+// Icon with lowest ID value placed first to ensure application icon
+// remains consistent on all systems.
+IDI_ICON1               ICON                    "icon.ico"
+
+
+/////////////////////////////////////////////////////////////////////////////
+//
+// Version
+//
+
+VS_VERSION_INFO VERSIONINFO
+FILEVERSION 0, 0, 100, 1
+PRODUCTVERSION 0, 0, 100, 1
+FILEFLAGSMASK 0x3fL
+FILEOS 0x40004
+FILETYPE 0x1
+BEGIN
+BLOCK "StringFileInfo"
+BEGIN
+BLOCK "040904B0"
+BEGIN
+VALUE "CompanyName", "Mandeye\0"
+VALUE "FileDescription", "HDMapping Step 3\0"
+VALUE "FileVersion", "0.100.1\0"
+VALUE "InternalName", "Multi session registration\0"
+VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0"
+VALUE "OriginalFilename", "multi_session_registration_step_3.exe\0"
+VALUE "ProductVersion", "0.100.1\0"
+VALUE "ProgramID", "github.com/MapsHD/HDMapping\0"
+VALUE "ProductName", "HDMapping\0"
+END
+END
+BLOCK "VarFileInfo"
+BEGIN
+VALUE "Translation", 0x409, 0x04B0
+END
+END
+/////////////////////////////////////////////////////////////////////////////
\ No newline at end of file
diff --git a/core/include/Core/control_points.h b/core/include/Core/control_points.h
index c6385bf3..b06c0878 100644
--- a/core/include/Core/control_points.h
+++ b/core/include/Core/control_points.h
@@ -30,7 +30,7 @@ class ControlPoints
     bool is_imgui = false;
     std::vector<ControlPoint> cps;
 
-#if WITH_GUI == 1
+    // Data members stay outside WITH_GUI so the class layout is identical in GUI and non-GUI builds.
     bool picking_mode = false;
     bool draw_uncertainty = false;
     int index_picked_point = -1;
@@ -38,6 +38,7 @@ class ControlPoints
 
     int index_pose = 0;
 
+#if WITH_GUI == 1
     void imgui(PointClouds& point_clouds_container, const Eigen::Vector3f& rotation_center);
     void render(const PointClouds& point_clouds_container, bool show_pc);
     void draw_ellipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, const Eigen::Vector3f& color, float nstd = 1);
diff --git a/core/include/Core/ground_control_points.h b/core/include/Core/ground_control_points.h
index 4eb73a0a..9f22eaef 100644
--- a/core/include/Core/ground_control_points.h
+++ b/core/include/Core/ground_control_points.h
@@ -27,13 +27,14 @@ class GroundControlPoints
 
     std::vector<GroundControlPoint> gpcs;
     double default_lidar_height_above_ground = 0.15;
-#if WITH_GUI == 1
+    // Data members stay outside WITH_GUI so the class layout is identical in GUI and non-GUI builds.
     bool is_imgui = false;
     bool picking_mode = false;
     int picking_mode_index_to_node_inner = -1;
     int picking_mode_index_to_node_outer = -1;
     bool draw_uncertainty = false;
 
+#if WITH_GUI == 1
     void imgui(PointClouds& point_clouds_container);
     void render(const PointClouds& point_clouds_container);
     void draw_ellipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, const Eigen::Vector3f& color, float nstd = 1);
diff --git a/core/include/Core/manual_pose_graph_loop_closure.h b/core/include/Core/manual_pose_graph_loop_closure.h
index d43bc92a..58e51d42 100644
--- a/core/include/Core/manual_pose_graph_loop_closure.h
+++ b/core/include/Core/manual_pose_graph_loop_closure.h
@@ -1,12 +1,15 @@
 #pragma once
 
-#if WITH_GUI == 1
 #include <Core/gnss.h>
-#include <Core/observation_picking.h>
 #include <Core/point_clouds.h>
 #include <Core/pose_graph_loop_closure.h>
 #include <Core/tum.h>
 
+#if WITH_GUI == 1
+#include <Core/observation_picking.h>
+#endif
+
+// Defined in every build (not only WITH_GUI) so Session has one layout; only the GUI methods are conditional.
 class ManualPoseGraphLoopClosure : public PoseGraphLoopClosure
 {
 public:
@@ -18,6 +21,7 @@ class ManualPoseGraphLoopClosure : public PoseGraphLoopClosure
     ManualPoseGraphLoopClosure() = default;
     ~ManualPoseGraphLoopClosure() = default;
 
+#if WITH_GUI == 1
     void Gui(
         PointClouds& point_clouds_container,
         int& index_loop_closure_source,
@@ -35,6 +39,5 @@ class ManualPoseGraphLoopClosure : public PoseGraphLoopClosure
         int index_loop_closure_target,
         int num_edge_extended_before,
         int num_edge_extended_after);
-};
-
 #endif
+};
diff --git a/core/include/Core/raylib_render.hpp b/core/include/Core/raylib_render.hpp
index b0d0c3f3..959db519 100644
--- a/core/include/Core/raylib_render.hpp
+++ b/core/include/Core/raylib_render.hpp
@@ -46,6 +46,7 @@ enum class ScanColorMode
     Intensity, // jet colormap by normalized LAS/LAZ intensity
     Elevation, // jet colormap by world-space Z, normalized over [elevationMin, elevationMax]
     Distance, // jet colormap by distance from distanceCenter, normalized over [0, distanceMax]
+    FlatIntensity, // pc.render_color shaded by normalized intensity (keeps per-scan colors distinguishable)
 };
 
 class ScanRenderer
diff --git a/core/include/Core/session.h b/core/include/Core/session.h
index 57f13717..35e22c26 100644
--- a/core/include/Core/session.h
+++ b/core/include/Core/session.h
@@ -5,15 +5,10 @@
 #include <string>
 #include <vector>
 
-#if WITH_GUI == 1
 #include <Core/control_points.h>
 #include <Core/ground_control_points.h>
 #include <Core/manual_pose_graph_loop_closure.h>
 
-#else
-#include <Core/pose_graph_loop_closure.h>
-#endif
-
 class Session
 {
 public:
@@ -30,13 +25,10 @@ class Session
     // bool show_rgb = true;
     bool load_cache_mode = false;
 
-#if WITH_GUI == 1
+    // No WITH_GUI branches here: Session is shared between core_math (WITH_GUI=0) and GUI apps, so its layout must not depend on it.
     ManualPoseGraphLoopClosure pose_graph_loop_closure;
     GroundControlPoints ground_control_points;
     ControlPoints control_points;
-#else
-    PoseGraphLoopClosure pose_graph_loop_closure;
-#endif
 
     bool load(const std::string& file_name, bool is_decimate, double bucket_x, double bucket_y, double bucket_z, bool calculate_offset);
     bool save(
diff --git a/core/src/raylib_render.cpp b/core/src/raylib_render.cpp
index 010c18e6..3413a7a7 100644
--- a/core/src/raylib_render.cpp
+++ b/core/src/raylib_render.cpp
@@ -317,7 +317,7 @@ void ScanRenderer::draw(
             color[2] = pc.render_color[2];
             color[3] = 1.0f;
             // Matches the shader's colorMode branches (0=flat, 1=intensity,
-            // 2=elevation, 3=distance) exactly, since ScanColorMode's
+            // 2=elevation, 3=distance, 4=flat shaded by intensity) exactly, since ScanColorMode's
             // enumerator order was chosen to match.
             colorModeInt = static_cast<int>(colorMode);
         }
diff --git a/core/src/raylib_render_shaders.hpp b/core/src/raylib_render_shaders.hpp
index b0995e97..b6ee6c29 100644
--- a/core/src/raylib_render_shaders.hpp
+++ b/core/src/raylib_render_shaders.hpp
@@ -85,6 +85,12 @@ void main()
         float d = length(fragWorldPos - distCenter);
         finalColor = vec4(jet(d / max(distMax, 1e-6)), pointColor.a);
     }
+    else if (colorMode == 4)
+    {
+        // Flat color shaded by intensity. sqrt lifts the typically low-skewed intensity distribution;
+        // the 0.35 floor keeps low-intensity points' hue visible.
+        finalColor = vec4(pointColor.rgb * mix(0.35, 1.0, sqrt(fragIntensity)), pointColor.a);
+    }
     else
     {
         finalColor = pointColor;
diff --git a/deploy_mandeye.bat b/deploy_mandeye.bat
index a7e2d178..ab24d95c 100644
--- a/deploy_mandeye.bat
+++ b/deploy_mandeye.bat
@@ -60,7 +60,7 @@ rem This list has to be kept in sync by hand with the hdmapping_install_app()
 rem calls in apps/*/CMakeLists.txt. hd_mapper is deliberately absent: it is
 rem gated behind BUILD_WITH_HD_MAPPER_APPLICATION, which defaults to OFF, so
 rem listing it would warn on every normal build.
-set "EXPECTED=camera_lidar_calibration.exe camera_lidar_intrinsics_calib.exe camera_lidar_trajectory_viewer.exe concatenate_multi_livox.exe drag_folder_with_mandeye_data_and_drop_here-precision_forestry.exe laz_to_mcap.exe laz_to_pcd.exe laz_to_ply.exe laz_to_txt.exe lidar_odometry_step_1.exe livox_mid_360_intrinsic_calibration.exe mandeye_compare_trajectories.exe mandeye_mission_recorder_calibration.exe mandeye_raw_data_viewer.exe mandeye_single_session_viewer.exe mandeye_with_360_camera_manual_coloring.exe matrix_mul.exe mcap_to_laz.exe multiply_timestamps_session_point_cloud_laz.exe multiply_timestamps_session_trajectory_csv.exe multi_session_registration_step_3.exe multi_view_tls_registration_step_2.exe multi_view_tls_registration_step_2_legacy.exe pcd_to_laz.exe precision_forestry_tools.exe single_session_manual_coloring.exe split_multi_livox.exe freeglut.dll laszip3.dll opencv_world4130.dll proj_9_3.dll tbb12.dll z.dll"
+set "EXPECTED=camera_lidar_calibration.exe camera_lidar_intrinsics_calib.exe camera_lidar_trajectory_viewer.exe concatenate_multi_livox.exe drag_folder_with_mandeye_data_and_drop_here-precision_forestry.exe laz_to_mcap.exe laz_to_pcd.exe laz_to_ply.exe laz_to_txt.exe lidar_odometry_step_1.exe livox_mid_360_intrinsic_calibration.exe mandeye_compare_trajectories.exe mandeye_mission_recorder_calibration.exe mandeye_raw_data_viewer.exe mandeye_single_session_viewer.exe mandeye_with_360_camera_manual_coloring.exe matrix_mul.exe mcap_to_laz.exe multiply_timestamps_session_point_cloud_laz.exe multiply_timestamps_session_trajectory_csv.exe multi_session_registration_step_3.exe multi_session_registration_step_3_legacy.exe multi_view_tls_registration_step_2.exe multi_view_tls_registration_step_2_legacy.exe pcd_to_laz.exe precision_forestry_tools.exe single_session_manual_coloring.exe split_multi_livox.exe freeglut.dll laszip3.dll opencv_world4130.dll proj_9_3.dll tbb12.dll z.dll"
 
 set "MISSING="
 for %%F in (%EXPECTED%) do (