Use iridescence_viewer (#177)

stella-cv · Mar 31, 2024 · 7efb502 · 7efb502
1 parent 58bcca5
commit 7efb502
Show file tree

Hide file tree

Showing 5 changed files with 177 additions and 15 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -185,6 +185,11 @@ find_package(socket_publisher QUIET)
 if(socket_publisher_FOUND)
     message(STATUS "Viewer for examples: SocketPublisher")
 endif()
+find_package(iridescence_viewer QUIET)
+if(iridescence_viewer_FOUND)
+    message(STATUS "Viewer for examples: IridescenceViewer")
+endif()
+
 
 #########
 # Build #
@@ -211,6 +216,15 @@ if(socket_publisher_FOUND)
     target_link_libraries(stella_vslam_ros_system PRIVATE
         socket_publisher::socket_publisher ${SIOCLIENT_LIBRARY} ${PROTOBUF_LIBRARIES})
 endif()
+if(iridescence_viewer_FOUND)
+    # Set macro flag
+    target_compile_definitions(stella_vslam_ros_system
+        PRIVATE HAVE_IRIDESCENCE_VIEWER)
+    # Link viewer
+    target_link_libraries(stella_vslam_ros_system PRIVATE
+        iridescence_viewer::iridescence_viewer)
+endif()
+
 # TODO: Move heder files to include directory
 target_include_directories(stella_vslam_ros_system PRIVATE
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
@@ -1,10 +1,3 @@
-if(pangolin_viewer_FOUND)
-    message(STATUS "Viewer for examples: pangolin viewer")
-endif()
-if(socket_publisher_FOUND)
-    message(STATUS "Viewer for examples: socket publisher")
-endif()
-
 set(EXECUTABLE_TARGETS "")
 
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
@@ -30,6 +23,14 @@ foreach(EXECUTABLE_TARGET IN LISTS EXECUTABLE_TARGETS)
         target_link_libraries(${EXECUTABLE_TARGET} PRIVATE
                 socket_publisher::socket_publisher ${SIOCLIENT_LIBRARY} ${PROTOBUF_LIBRARIES})
     endif()
+    if(iridescence_viewer_FOUND)
+        # Set macro flag
+        target_compile_definitions(${EXECUTABLE_TARGET}
+                PRIVATE HAVE_IRIDESCENCE_VIEWER)
+        # Link viewer
+        target_link_libraries(${EXECUTABLE_TARGET} PRIVATE
+                iridescence_viewer::iridescence_viewer)
+    endif()
 
     # setup stack trace logger
     if(USE_STACK_TRACE_LOGGER)

diff --git a/src/run_slam.cc b/src/run_slam.cc
@@ -1,6 +1,9 @@
 #ifdef HAVE_PANGOLIN_VIEWER
 #include "pangolin_viewer/viewer.h"
 #endif
+#ifdef HAVE_IRIDESCENCE_VIEWER
+#include "iridescence_viewer/viewer.h"
+#endif
 #ifdef HAVE_SOCKET_PUBLISHER
 #include "socket_publisher/publisher.h"
 #endif
@@ -51,6 +54,29 @@ void tracking(const std::shared_ptr<stella_vslam_ros::system>& slam_ros,
             SLAM->get_map_publisher());
     }
 #endif
+#ifdef HAVE_IRIDESCENCE_VIEWER
+    std::shared_ptr<iridescence_viewer::viewer> iridescence_viewer;
+    std::mutex mtx_terminate;
+    bool terminate_is_requested = false;
+    if (viewer_string == "iridescence_viewer") {
+        iridescence_viewer = std::make_shared<iridescence_viewer::viewer>(
+            stella_vslam::util::yaml_optional_ref(cfg->yaml_node_, "IridescenceViewer"),
+            SLAM->get_frame_publisher(),
+            SLAM->get_map_publisher());
+        iridescence_viewer->add_button("Reset", [&SLAM] {
+            SLAM->request_reset();
+        });
+        iridescence_viewer->add_button("Save and exit", [&terminate_is_requested, &mtx_terminate, &iridescence_viewer] {
+            std::lock_guard<std::mutex> lock(mtx_terminate);
+            terminate_is_requested = true;
+            iridescence_viewer->request_terminate();
+        });
+        iridescence_viewer->add_close_callback([&terminate_is_requested, &mtx_terminate] {
+            std::lock_guard<std::mutex> lock(mtx_terminate);
+            terminate_is_requested = true;
+        });
+    }
+#endif
 #ifdef HAVE_SOCKET_PUBLISHER
     std::shared_ptr<socket_publisher::publisher> publisher;
     if (viewer_string == "socket_publisher") {
@@ -70,6 +96,11 @@ void tracking(const std::shared_ptr<stella_vslam_ros::system>& slam_ros,
             if (viewer_string == "pangolin_viewer") {
 #ifdef HAVE_PANGOLIN_VIEWER
                 viewer->run();
+#endif
+            }
+            if (viewer_string == "iridescence_viewer") {
+#ifdef HAVE_IRIDESCENCE_VIEWER
+                iridescence_viewer->run();
 #endif
             }
             if (viewer_string == "socket_publisher") {
@@ -93,6 +124,11 @@ void tracking(const std::shared_ptr<stella_vslam_ros::system>& slam_ros,
     if (viewer_string == "pangolin_viewer") {
 #ifdef HAVE_PANGOLIN_VIEWER
         viewer->request_terminate();
+#endif
+    }
+    if (viewer_string == "iridescence_viewer") {
+#ifdef HAVE_IRIDESCENCE_VIEWER
+        iridescence_viewer->request_terminate();
 #endif
     }
     if (viewer_string == "socket_publisher") {
@@ -155,7 +191,7 @@ int main(int argc, char* argv[]) {
     auto disable_mapping = op.add<popl::Switch>("", "disable-mapping", "disable mapping");
     auto temporal_mapping = op.add<popl::Switch>("", "temporal-mapping", "enable temporal mapping");
     auto rectify = op.add<popl::Switch>("r", "rectify", "rectify stereo image");
-    auto viewer = op.add<popl::Value<std::string>>("", "viewer", "viewer [pangolin_viewer, socket_publisher, none]");
+    auto viewer = op.add<popl::Value<std::string>>("", "viewer", "viewer [iridescence_viewer, pangolin_viewer, socket_publisher, none]");
     try {
         op.parse(argc, argv);
     }
@@ -182,7 +218,10 @@ int main(int argc, char* argv[]) {
     std::string viewer_string;
     if (viewer->is_set()) {
         viewer_string = viewer->value();
-        if (viewer_string != "pangolin_viewer" && viewer_string != "socket_publisher" && viewer_string != "none") {
+        if (viewer_string != "pangolin_viewer"
+            && viewer_string != "socket_publisher"
+            && viewer_string != "iridescence_viewer"
+            && viewer_string != "none") {
             std::cerr << "invalid arguments (--viewer)" << std::endl
                       << std::endl
                       << op << std::endl;
@@ -196,6 +235,14 @@ int main(int argc, char* argv[]) {
             return EXIT_FAILURE;
         }
 #endif
+#ifndef HAVE_IRIDESCENCE_VIEWER
+        if (viewer_string == "iridescence_viewer") {
+            std::cerr << "iridescence_viewer not linked" << std::endl
+                      << std::endl
+                      << op << std::endl;
+            return EXIT_FAILURE;
+        }
+#endif
 #ifndef HAVE_SOCKET_PUBLISHER
         if (viewer_string == "socket_publisher") {
             std::cerr << "socket_publisher not linked" << std::endl
@@ -206,7 +253,9 @@ int main(int argc, char* argv[]) {
 #endif
     }
     else {
-#ifdef HAVE_PANGOLIN_VIEWER
+#ifdef HAVE_IRIDESCENCE_VIEWER
+        viewer_string = "iridescence_viewer";
+#elif defined(HAVE_PANGOLIN_VIEWER)
         viewer_string = "pangolin_viewer";
 #elif defined(HAVE_SOCKET_PUBLISHER)
         viewer_string = "socket_publisher";

diff --git a/src/run_slam_offline.cc b/src/run_slam_offline.cc
@@ -1,6 +1,9 @@
 #ifdef HAVE_PANGOLIN_VIEWER
 #include "pangolin_viewer/viewer.h"
 #endif
+#ifdef HAVE_IRIDESCENCE_VIEWER
+#include "iridescence_viewer/viewer.h"
+#endif
 #ifdef HAVE_SOCKET_PUBLISHER
 #include "socket_publisher/publisher.h"
 #endif
@@ -66,6 +69,29 @@ void tracking(const std::shared_ptr<stella_vslam_ros::system>& slam_ros,
             SLAM->get_map_publisher());
     }
 #endif
+#ifdef HAVE_IRIDESCENCE_VIEWER
+    std::shared_ptr<iridescence_viewer::viewer> iridescence_viewer;
+    std::mutex mtx_terminate;
+    bool terminate_is_requested = false;
+    if (viewer_string == "iridescence_viewer") {
+        iridescence_viewer = std::make_shared<iridescence_viewer::viewer>(
+            stella_vslam::util::yaml_optional_ref(cfg->yaml_node_, "IridescenceViewer"),
+            SLAM->get_frame_publisher(),
+            SLAM->get_map_publisher());
+        iridescence_viewer->add_button("Reset", [&SLAM] {
+            SLAM->request_reset();
+        });
+        iridescence_viewer->add_button("Save and exit", [&terminate_is_requested, &mtx_terminate, &iridescence_viewer] {
+            std::lock_guard<std::mutex> lock(mtx_terminate);
+            terminate_is_requested = true;
+            iridescence_viewer->request_terminate();
+        });
+        iridescence_viewer->add_close_callback([&terminate_is_requested, &mtx_terminate] {
+            std::lock_guard<std::mutex> lock(mtx_terminate);
+            terminate_is_requested = true;
+        });
+    }
+#endif
 #ifdef HAVE_SOCKET_PUBLISHER
     std::shared_ptr<socket_publisher::publisher> publisher;
     if (viewer_string == "socket_publisher") {
@@ -85,6 +111,11 @@ void tracking(const std::shared_ptr<stella_vslam_ros::system>& slam_ros,
             if (viewer_string == "pangolin_viewer") {
 #ifdef HAVE_PANGOLIN_VIEWER
                 viewer->run();
+#endif
+            }
+            if (viewer_string == "iridescence_viewer") {
+#ifdef HAVE_IRIDESCENCE_VIEWER
+                iridescence_viewer->run();
 #endif
             }
             if (viewer_string == "socket_publisher") {
@@ -238,6 +269,11 @@ void tracking(const std::shared_ptr<stella_vslam_ros::system>& slam_ros,
     if (viewer_string == "pangolin_viewer") {
 #ifdef HAVE_PANGOLIN_VIEWER
         viewer->request_terminate();
+#endif
+    }
+    if (viewer_string == "iridescence_viewer") {
+#ifdef HAVE_IRIDESCENCE_VIEWER
+        iridescence_viewer->request_terminate();
 #endif
     }
     if (viewer_string == "socket_publisher") {
@@ -309,7 +345,7 @@ int main(int argc, char* argv[]) {
     auto disable_mapping = op.add<popl::Switch>("", "disable-mapping", "disable mapping");
     auto temporal_mapping = op.add<popl::Switch>("", "temporal-mapping", "enable temporal mapping");
     auto rectify = op.add<popl::Switch>("r", "rectify", "rectify stereo image");
-    auto viewer = op.add<popl::Value<std::string>>("", "viewer", "viewer [pangolin_viewer, socket_publisher, none]");
+    auto viewer = op.add<popl::Value<std::string>>("", "viewer", "viewer [iridescence_viewer, pangolin_viewer, socket_publisher, none]");
     try {
         op.parse(argc, argv);
     }
@@ -336,7 +372,10 @@ int main(int argc, char* argv[]) {
     std::string viewer_string;
     if (viewer->is_set()) {
         viewer_string = viewer->value();
-        if (viewer_string != "pangolin_viewer" && viewer_string != "socket_publisher" && viewer_string != "none") {
+        if (viewer_string != "pangolin_viewer"
+            && viewer_string != "socket_publisher"
+            && viewer_string != "iridescence_viewer"
+            && viewer_string != "none") {
             std::cerr << "invalid arguments (--viewer)" << std::endl
                       << std::endl
                       << op << std::endl;
@@ -350,6 +389,14 @@ int main(int argc, char* argv[]) {
             return EXIT_FAILURE;
         }
 #endif
+#ifndef HAVE_IRIDESCENCE_VIEWER
+        if (viewer_string == "iridescence_viewer") {
+            std::cerr << "iridescence_viewer not linked" << std::endl
+                      << std::endl
+                      << op << std::endl;
+            return EXIT_FAILURE;
+        }
+#endif
 #ifndef HAVE_SOCKET_PUBLISHER
         if (viewer_string == "socket_publisher") {
             std::cerr << "socket_publisher not linked" << std::endl
@@ -360,7 +407,9 @@ int main(int argc, char* argv[]) {
 #endif
     }
     else {
-#ifdef HAVE_PANGOLIN_VIEWER
+#ifdef HAVE_IRIDESCENCE_VIEWER
+        viewer_string = "iridescence_viewer";
+#elif defined(HAVE_PANGOLIN_VIEWER)
         viewer_string = "pangolin_viewer";
 #elif defined(HAVE_SOCKET_PUBLISHER)
         viewer_string = "socket_publisher";

diff --git a/src/system.cc b/src/system.cc
@@ -1,6 +1,9 @@
 #ifdef HAVE_PANGOLIN_VIEWER
 #include "pangolin_viewer/viewer.h"
 #endif
+#ifdef HAVE_IRIDESCENCE_VIEWER
+#include "iridescence_viewer/viewer.h"
+#endif
 #ifdef HAVE_SOCKET_PUBLISHER
 #include "socket_publisher/publisher.h"
 #endif
@@ -43,6 +46,9 @@ class System : public rclcpp::Node {
 #ifdef HAVE_PANGOLIN_VIEWER
     std::shared_ptr<pangolin_viewer::viewer> viewer_;
 #endif
+#ifdef HAVE_IRIDESCENCE_VIEWER
+    std::shared_ptr<iridescence_viewer::viewer> iridescence_viewer_;
+#endif
 #ifdef HAVE_SOCKET_PUBLISHER
     std::shared_ptr<socket_publisher::publisher> publisher_;
 #endif
@@ -73,7 +79,10 @@ System::System(
     // viewer
     if (!viewer.empty()) {
         viewer_string_ = viewer;
-        if (viewer_string_ != "pangolin_viewer" && viewer_string_ != "socket_publisher" && viewer_string_ != "none") {
+        if (viewer_string_ != "pangolin_viewer"
+            && viewer_string_ != "socket_publisher"
+            && viewer_string_ != "iridescence_viewer"
+            && viewer_string_ != "none") {
             RCLCPP_FATAL(get_logger(), "invalid arguments (--viewer)");
             return;
         }
@@ -83,6 +92,12 @@ System::System(
             return;
         }
 #endif
+#ifndef HAVE_IRIDESCENCE_VIEWER
+        if (viewer_string_ == "iridescence_viewer") {
+            RCLCPP_FATAL(get_logger(), "iridescence_viewer not linked");
+            return;
+        }
+#endif
 #ifndef HAVE_SOCKET_PUBLISHER
         if (viewer_string_ == "socket_publisher") {
             RCLCPP_FATAL(get_logger(), "socket_publisher not linked");
@@ -91,7 +106,9 @@ System::System(
 #endif
     }
     else {
-#ifdef HAVE_PANGOLIN_VIEWER
+#ifdef HAVE_IRIDESCENCE_VIEWER
+        viewer_string_ = "iridescence_viewer";
+#elif defined(HAVE_PANGOLIN_VIEWER)
         viewer_string_ = "pangolin_viewer";
 #elif defined(HAVE_SOCKET_PUBLISHER)
         viewer_string_ = "socket_publisher";
@@ -155,6 +172,28 @@ System::System(
             slam_->get_map_publisher());
     }
 #endif
+#ifdef HAVE_IRIDESCENCE_VIEWER
+    std::mutex mtx_terminate;
+    bool terminate_is_requested = false;
+    if (viewer_string_ == "iridescence_viewer") {
+        iridescence_viewer_ = std::make_shared<iridescence_viewer::viewer>(
+            stella_vslam::util::yaml_optional_ref(cfg_->yaml_node_, "IridescenceViewer"),
+            slam_->get_frame_publisher(),
+            slam_->get_map_publisher());
+        iridescence_viewer_->add_button("Reset", [this] {
+            slam_->request_reset();
+        });
+        iridescence_viewer_->add_button("Save and exit", [this, &terminate_is_requested, &mtx_terminate] {
+            std::lock_guard<std::mutex> lock(mtx_terminate);
+            terminate_is_requested = true;
+            iridescence_viewer_->request_terminate();
+        });
+        iridescence_viewer_->add_close_callback([&terminate_is_requested, &mtx_terminate] {
+            std::lock_guard<std::mutex> lock(mtx_terminate);
+            terminate_is_requested = true;
+        });
+    }
+#endif
 #ifdef HAVE_SOCKET_PUBLISHER
     if (viewer_string_ == "socket_publisher") {
         publisher_ = std::make_shared<socket_publisher::publisher>(
@@ -172,6 +211,11 @@ System::System(
             if (viewer_string_ == "pangolin_viewer") {
 #ifdef HAVE_PANGOLIN_VIEWER
                 viewer_->run();
+#endif
+            }
+            if (viewer_string_ == "iridescence_viewer") {
+#ifdef HAVE_IRIDESCENCE_VIEWER
+                iridescence_viewer_->run();
 #endif
             }
             if (viewer_string_ == "socket_publisher") {
@@ -195,6 +239,11 @@ System::~System() {
     if (viewer_string_ == "pangolin_viewer") {
 #ifdef HAVE_PANGOLIN_VIEWER
         viewer_->request_terminate();
+#endif
+    }
+    if (viewer_string_ == "iridescence_viewer") {
+#ifdef HAVE_IRIDESCENCE_VIEWER
+        iridescence_viewer_->request_terminate();
 #endif
     }
     if (viewer_string_ == "socket_publisher") {