From 7b8e39c026b467cec0c8a4d42bba6dba3c88bcc6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Szymu=C5=9B?= <151686324+dashin2004@users.noreply.github.com> Date: Mon, 17 Nov 2025 19:39:42 +0100 Subject: [PATCH 1/7] obraz z ov2slama Dodanie wizualizacji poprzez subskrypcje topicu image track --- GUI_OV2SLAM/CMakeLists.txt | 6 +- GUI_OV2SLAM/Dockerfile | 13 ++-- GUI_OV2SLAM/launch/visualizer.launch.py | 10 ++- GUI_OV2SLAM/src/main.cpp | 88 +++++++++++++++++++------ 4 files changed, 87 insertions(+), 30 deletions(-) diff --git a/GUI_OV2SLAM/CMakeLists.txt b/GUI_OV2SLAM/CMakeLists.txt index 4718dab..bafdfa8 100644 --- a/GUI_OV2SLAM/CMakeLists.txt +++ b/GUI_OV2SLAM/CMakeLists.txt @@ -15,7 +15,8 @@ find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(tf2_ros REQUIRED) - +find_package(cv_bridge REQUIRED) +find_package(OpenCV REQUIRED) # Znajdź OpenGL i GLFW find_package(OpenGL REQUIRED) find_package(glfw3 REQUIRED) @@ -54,6 +55,7 @@ target_include_directories(imgui_visualizer PUBLIC ${IMGUI_DIR} ${IMGUI_DIR}/backends ${IMPLOT_DIR} + ${OpenCV_INCLUDE_DIRS} ) # Link libraries @@ -61,6 +63,7 @@ target_link_libraries(imgui_visualizer OpenGL::GL glfw GLEW::GLEW + ${OpenCV_LIBS} ) # ROS 2 dependencies @@ -71,6 +74,7 @@ ament_target_dependencies(imgui_visualizer geometry_msgs nav_msgs tf2_ros + cv_bridge ) # Install executable diff --git a/GUI_OV2SLAM/Dockerfile b/GUI_OV2SLAM/Dockerfile index 67fdbe5..2e2a76e 100644 --- a/GUI_OV2SLAM/Dockerfile +++ b/GUI_OV2SLAM/Dockerfile @@ -40,14 +40,12 @@ WORKDIR /ws/src/imgui_app/Thirdparty RUN git clone https://github.com/ocornut/imgui.git WORKDIR /ws/src/imgui_app/Thirdparty/imgui RUN git checkout docking -# ImGui to header-only library, nie wymaga kompilacji ################## ### ImPlot ### ################## WORKDIR /ws/src/imgui_app/Thirdparty RUN git clone https://github.com/epezent/implot.git -# ImPlot również header-only ################### ### GLM (math) ### @@ -70,11 +68,14 @@ RUN apt update \ ros-${ROS_DISTRO}-tf2 \ ros-${ROS_DISTRO}-tf2-ros \ ros-${ROS_DISTRO}-tf2-geometry-msgs \ - ros-${ROS_DISTRO}-image-transport \ ros-${ROS_DISTRO}-cv-bridge \ - ros-${ROS_DISTRO}-pcl-ros \ - ros-${ROS_DISTRO}-pcl-conversions \ - ros-${ROS_DISTRO}-visualization-msgs \ + && rm -rf /var/lib/apt/lists/* +################### +### OpenCV ### +################### +RUN apt-get update \ + && DEBIAN_FRONTEND=noninteractive apt-get install -y \ + libopencv-dev \ && rm -rf /var/lib/apt/lists/* ############################ diff --git a/GUI_OV2SLAM/launch/visualizer.launch.py b/GUI_OV2SLAM/launch/visualizer.launch.py index 6ad4db3..08b5322 100644 --- a/GUI_OV2SLAM/launch/visualizer.launch.py +++ b/GUI_OV2SLAM/launch/visualizer.launch.py @@ -4,12 +4,17 @@ from launch.substitutions import LaunchConfiguration def generate_launch_description(): - # Deklaruj argumenty launch (opcjonalne - można je nadpisać z CLI) + # Deklaruj argumenty launch vo_pose_topic_arg = DeclareLaunchArgument( 'vo_pose_topic', default_value='/vo_pose', description='Topic name for VO pose' ) + image_track_topic_arg = DeclareLaunchArgument( + 'image_track_topic', + default_value='/image_track', + description='Topic name for image tracking' + ) @@ -21,12 +26,13 @@ def generate_launch_description(): output='screen', parameters=[{ 'vo_pose_topic': LaunchConfiguration('vo_pose_topic'), + 'image_track_topic': LaunchConfiguration('image_track_topic'), }] ) return LaunchDescription([ vo_pose_topic_arg, - + image_track_topic_arg, visualizer_node ]) diff --git a/GUI_OV2SLAM/src/main.cpp b/GUI_OV2SLAM/src/main.cpp index 56d8592..ed4d836 100644 --- a/GUI_OV2SLAM/src/main.cpp +++ b/GUI_OV2SLAM/src/main.cpp @@ -1,6 +1,8 @@ #include #include -#include +#include +#include +#include #include "imgui.h" #include "imgui_impl_glfw.h" @@ -18,16 +20,20 @@ class ImGuiVisualizerNode : public rclcpp::Node ImGuiVisualizerNode() : Node("imgui_visualizer") { this->declare_parameter("vo_pose_topic", "/vo_pose"); + this->declare_parameter("image_track_topic", "/image_track"); std::string vo_pose_topic = this->get_parameter("vo_pose_topic").as_string(); - + std::string image_track_topic = this->get_parameter("image_track_topic").as_string(); + pose_sub_ = this->create_subscription( - vo_pose_topic, 10, + vo_pose_topic, 10, std::bind(&ImGuiVisualizerNode::poseCallback, this, std::placeholders::_1)); - RCLCPP_INFO(this->get_logger(), "ImGui Visualizer Node started!"); - RCLCPP_INFO(this->get_logger(), "Subscribed to pose topic: %s",vo_pose_topic.c_str()); + image_sub_ = this->create_subscription( + image_track_topic, 10, + std::bind(&ImGuiVisualizerNode::imageCallback, this, std::placeholders::_1)); + RCLCPP_INFO(this->get_logger(), "ImGui Visualizer Node started!"); } float getCameraX() const { return camera_x_; } @@ -35,56 +41,89 @@ class ImGuiVisualizerNode : public rclcpp::Node float getCameraZ() const { return camera_z_; } float getVelocityLinear() const { return velocity_linear_; } float getDistanceLinear() const { return distance_linear_; } - + + bool hasImage() const { return has_image_; } + GLuint getImageTexture() const { return image_texture_; } + int getImageWidth() const { return image_width_; } + int getImageHeight() const { return image_height_; } private: void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - auto current_time = this->now(); if (last_pose_time_.seconds() > 0.0) { - dt = (current_time-last_pose_time_).seconds(); + double dt = (current_time - last_pose_time_).seconds(); - if (dt > 0.000001) { + if (dt > 0.0001) { float dx = msg->pose.position.x - last_x_; float dy = msg->pose.position.y - last_y_; float dz = msg->pose.position.z - last_z_; velocity_linear_ = std::sqrt(dx*dx + dy*dy + dz*dz) / dt; - distance_linear_ = distance_linear_ + std::sqrt(dx*dx + dy*dy + dz*dz); + distance_linear_ += std::sqrt(dx*dx + dy*dy + dz*dz); } - } - // Zapisz dla następnej iteracji last_pose_time_ = current_time; last_x_ = msg->pose.position.x; last_y_ = msg->pose.position.y; last_z_ = msg->pose.position.z; - // Aktualizuj pozycję camera_x_ = msg->pose.position.x; camera_y_ = msg->pose.position.y; camera_z_ = msg->pose.position.z; - } - + void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) + { + try { + cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8"); + cv::Mat rgb_image; + cv::cvtColor(cv_ptr->image, rgb_image, cv::COLOR_BGR2RGB); + + image_width_ = rgb_image.cols; + image_height_ = rgb_image.rows; + + // Utwórz teksturę OpenGL jeśli nie istnieje + if (image_texture_ == 0) { + glGenTextures(1, &image_texture_); + glBindTexture(GL_TEXTURE_2D, image_texture_); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + } + + // Aktualizuj teksturę + glBindTexture(GL_TEXTURE_2D, image_texture_); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, image_width_, image_height_, + 0, GL_RGB, GL_UNSIGNED_BYTE, rgb_image.data); + + has_image_ = true; + + } catch (cv_bridge::Exception& e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + } + } float camera_x_ = 0.0f; float camera_y_ = 0.0f; float camera_z_ = 0.0f; float velocity_linear_ = 0.0f; - float distance_linear_=0.0f; + float distance_linear_ = 0.0f; + rclcpp::Time last_pose_time_ = rclcpp::Time(0); float last_x_ = 0.0f; float last_y_ = 0.0f; float last_z_ = 0.0f; - double dt =0; - + + // Image + bool has_image_ = false; + GLuint image_texture_ = 0; + int image_width_ = 0; + int image_height_ = 0; rclcpp::Subscription::SharedPtr pose_sub_; + rclcpp::Subscription::SharedPtr image_sub_; }; int main(int argc, char** argv) @@ -101,7 +140,7 @@ int main(int argc, char** argv) glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3); glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); - GLFWwindow* window = glfwCreateWindow(1600, 900, "OV2SLAM Visualizer", NULL, NULL); + GLFWwindow* window = glfwCreateWindow(1920, 1080, "OV2SLAM Visualizer", NULL, NULL); if (window == NULL) { std::cerr << "Failed to create GLFW window" << std::endl; glfwTerminate(); @@ -147,9 +186,16 @@ int main(int argc, char** argv) ImGui::Text("Distance:"); ImGui::Text(" Linear: %.3f m", node->getDistanceLinear()); ImGui::End(); + // Nowe okno z obrazem + ImGui::Begin("Image Track"); + if (node->hasImage()) { + ImVec2 imageSize(node->getImageWidth(), node->getImageHeight()); + ImGui::Image((void*)(intptr_t)node->getImageTexture(), imageSize); + } else { + ImGui::Text("Waiting for image..."); + } + ImGui::End(); - - ImGui::Render(); int display_w, display_h; glfwGetFramebufferSize(window, &display_w, &display_h); From 269adcfa35932d371744b202730172ef756ba656 Mon Sep 17 00:00:00 2001 From: dashin2004 Date: Mon, 24 Nov 2025 19:12:38 +0100 Subject: [PATCH 2/7] Dodanie wizualizacji trajektorii na plaszczyznie xz --- GUI_OV2SLAM/Dockerfile | 11 ++++++++++ GUI_OV2SLAM/src/main.cpp | 46 ++++++++++++++++++++++++++++++++++++++-- 2 files changed, 55 insertions(+), 2 deletions(-) diff --git a/GUI_OV2SLAM/Dockerfile b/GUI_OV2SLAM/Dockerfile index 2e2a76e..1d66daa 100644 --- a/GUI_OV2SLAM/Dockerfile +++ b/GUI_OV2SLAM/Dockerfile @@ -70,6 +70,13 @@ RUN apt update \ ros-${ROS_DISTRO}-tf2-geometry-msgs \ ros-${ROS_DISTRO}-cv-bridge \ && rm -rf /var/lib/apt/lists/* +###################### +### Cyclone DDS ### +###################### +RUN apt update \ + && DEBIAN_FRONTEND=noninteractive apt-get install -y \ + ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ + && rm -rf /var/lib/apt/lists/* ################### ### OpenCV ### ################### @@ -94,6 +101,10 @@ ENV QT_X11_NO_MITSHM=1 RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc RUN echo "source /ws/install/setup.bash" >> ~/.bashrc +# Ustaw Cyclone DDS jako domyślny +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +ENV ROS_DOMAIN_ID=0 + WORKDIR /ws CMD ["/bin/bash"] diff --git a/GUI_OV2SLAM/src/main.cpp b/GUI_OV2SLAM/src/main.cpp index ed4d836..9750287 100644 --- a/GUI_OV2SLAM/src/main.cpp +++ b/GUI_OV2SLAM/src/main.cpp @@ -13,6 +13,7 @@ #include #include #include +#include class ImGuiVisualizerNode : public rclcpp::Node { @@ -46,6 +47,9 @@ class ImGuiVisualizerNode : public rclcpp::Node GLuint getImageTexture() const { return image_texture_; } int getImageWidth() const { return image_width_; } int getImageHeight() const { return image_height_; } + + const std::vector& getTrajectoryX() const { return trajectory_x_; } + const std::vector& getTrajectoryZ() const { return trajectory_z_; } private: void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) @@ -73,6 +77,10 @@ class ImGuiVisualizerNode : public rclcpp::Node camera_x_ = msg->pose.position.x; camera_y_ = msg->pose.position.y; camera_z_ = msg->pose.position.z; + + // Dodaj punkt do trajektorii + trajectory_x_.push_back(camera_x_); + trajectory_z_.push_back(camera_z_); } void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) @@ -122,6 +130,10 @@ class ImGuiVisualizerNode : public rclcpp::Node int image_width_ = 0; int image_height_ = 0; + // Trajectory + std::vector trajectory_x_; + std::vector trajectory_z_; + rclcpp::Subscription::SharedPtr pose_sub_; rclcpp::Subscription::SharedPtr image_sub_; }; @@ -174,6 +186,7 @@ int main(int argc, char** argv) ImGui::DockSpaceOverViewport(0, ImGui::GetMainViewport()); + // Okno z pozycją kamery ImGui::Begin("Camera Position"); ImGui::Text("Position:"); ImGui::Text(" X: %.3f m", node->getCameraX()); @@ -186,7 +199,8 @@ int main(int argc, char** argv) ImGui::Text("Distance:"); ImGui::Text(" Linear: %.3f m", node->getDistanceLinear()); ImGui::End(); - // Nowe okno z obrazem + + // Okno z obrazem ImGui::Begin("Image Track"); if (node->hasImage()) { ImVec2 imageSize(node->getImageWidth(), node->getImageHeight()); @@ -196,6 +210,34 @@ int main(int argc, char** argv) } ImGui::End(); + // Okno z trajektorią XZ + ImGui::Begin("Trajectory XZ"); + const auto& traj_x = node->getTrajectoryX(); + const auto& traj_z = node->getTrajectoryZ(); + + if (traj_x.size() > 1) { + if (ImPlot::BeginPlot("Camera Trajectory (Top View)", ImVec2(-1, -1))) { + ImPlot::SetupAxis(ImAxis_X1, "X [m]"); + ImPlot::SetupAxis(ImAxis_Y1, "Z [m]"); + ImPlot::SetupAxisLimits(ImAxis_X1, -50, 50, ImGuiCond_Once); + ImPlot::SetupAxisLimits(ImAxis_Y1, -50, 50, ImGuiCond_Once); + + ImPlot::PlotLine("Path", traj_x.data(), traj_z.data(), traj_x.size()); + + // Rysuj aktualną pozycję jako punkt + if (!traj_x.empty()) { + float current_x = traj_x.back(); + float current_z = traj_z.back(); + ImPlot::PlotScatter("Current", ¤t_x, ¤t_z, 1); + } + + ImPlot::EndPlot(); + } + } else { + ImGui::Text("Collecting trajectory data..."); + } + ImGui::End(); + ImGui::Render(); int display_w, display_h; glfwGetFramebufferSize(window, &display_w, &display_h); @@ -216,4 +258,4 @@ int main(int argc, char** argv) rclcpp::shutdown(); return 0; -} +}g From 85bfed6077fb7ec47dee2ab5578387011fec5d13 Mon Sep 17 00:00:00 2001 From: dashin2004 Date: Mon, 24 Nov 2025 22:05:59 +0100 Subject: [PATCH 3/7] =?UTF-8?q?Dodanie=20przycisku=20zapisuj=C4=85cego=20t?= =?UTF-8?q?rajektorie=20w=20formacie=20csv=20x,x,time=5Fstamp=20do=20/ws/t?= =?UTF-8?q?rajectories?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- GUI_OV2SLAM/Dockerfile | 3 +++ GUI_OV2SLAM/src/main.cpp | 42 +++++++++++++++++++++++++++++++++++----- 2 files changed, 40 insertions(+), 5 deletions(-) diff --git a/GUI_OV2SLAM/Dockerfile b/GUI_OV2SLAM/Dockerfile index 1d66daa..c304c55 100644 --- a/GUI_OV2SLAM/Dockerfile +++ b/GUI_OV2SLAM/Dockerfile @@ -91,6 +91,9 @@ RUN apt-get update \ ADD . /ws/src/imgui_app WORKDIR /ws + +RUN mkdir -p /ws/trajectories + RUN . /opt/ros/$ROS_DISTRO/setup.sh && colcon build --symlink-install # Ustaw zmienne środowiskowe dla X11 diff --git a/GUI_OV2SLAM/src/main.cpp b/GUI_OV2SLAM/src/main.cpp index 9750287..f328c83 100644 --- a/GUI_OV2SLAM/src/main.cpp +++ b/GUI_OV2SLAM/src/main.cpp @@ -13,7 +13,8 @@ #include #include #include -#include +#include +#include class ImGuiVisualizerNode : public rclcpp::Node { @@ -36,7 +37,7 @@ class ImGuiVisualizerNode : public rclcpp::Node RCLCPP_INFO(this->get_logger(), "ImGui Visualizer Node started!"); } - + float getCameraX() const { return camera_x_; } float getCameraY() const { return camera_y_; } float getCameraZ() const { return camera_z_; } @@ -50,6 +51,25 @@ class ImGuiVisualizerNode : public rclcpp::Node const std::vector& getTrajectoryX() const { return trajectory_x_; } const std::vector& getTrajectoryZ() const { return trajectory_z_; } + + + void saveTrajectoryToCSV(const std::string& filename) + { + std::ofstream file(filename); + if (!file.is_open()) { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", filename.c_str()); + return; + } + + file << "x,z,time_stamp\n"; + for (size_t i = 0; i < trajectory_x_.size(); ++i) { + file << trajectory_x_[i] << "," << trajectory_z_[i] <<","<< std::to_string(std::chrono::system_clock::now().time_since_epoch().count())<<"\n"; + } + + file.close(); + RCLCPP_INFO(this->get_logger(), "Trajectory saved to: %s (%zu points)", + filename.c_str(), trajectory_x_.size()); + } private: void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) @@ -133,7 +153,9 @@ class ImGuiVisualizerNode : public rclcpp::Node // Trajectory std::vector trajectory_x_; std::vector trajectory_z_; - + std::vector sec; + std::vector nanosec; + rclcpp::Subscription::SharedPtr pose_sub_; rclcpp::Subscription::SharedPtr image_sub_; }; @@ -152,7 +174,7 @@ int main(int argc, char** argv) glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3); glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); - GLFWwindow* window = glfwCreateWindow(1920, 1080, "OV2SLAM Visualizer", NULL, NULL); + GLFWwindow* window = glfwCreateWindow(1920, 1080, "OV2SLAM PowerVisualizer", NULL, NULL); if (window == NULL) { std::cerr << "Failed to create GLFW window" << std::endl; glfwTerminate(); @@ -215,6 +237,16 @@ int main(int argc, char** argv) const auto& traj_x = node->getTrajectoryX(); const auto& traj_z = node->getTrajectoryZ(); + // Przycisk zapisu trajektorii + if (ImGui::Button("Save Trajectory to CSV")) { + std::string filename = "/ws/trajectories/trajectory_" + + std::to_string(std::chrono::system_clock::now().time_since_epoch().count()) + + ".csv"; + node->saveTrajectoryToCSV(filename); + } + ImGui::SameLine(); + ImGui::Text("Points: %zu", traj_x.size()); + if (traj_x.size() > 1) { if (ImPlot::BeginPlot("Camera Trajectory (Top View)", ImVec2(-1, -1))) { ImPlot::SetupAxis(ImAxis_X1, "X [m]"); @@ -258,4 +290,4 @@ int main(int argc, char** argv) rclcpp::shutdown(); return 0; -}g +} From 283ac1c53e42627387c5a0ee9cb0b9342aee6c89 Mon Sep 17 00:00:00 2001 From: dashin2004 Date: Fri, 28 Nov 2025 00:15:25 +0100 Subject: [PATCH 4/7] =?UTF-8?q?dodanie=20logo=20PUTPowerTrain=20jako=20t?= =?UTF-8?q?=C5=82o?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- GUI_OV2SLAM/CMakeLists.txt => CMakeLists.txt | 0 GUI_OV2SLAM/Dockerfile => Dockerfile | 5 + GUI_OV2SLAM/src/main.cpp | 293 ---------- .../launch => launch}/visualizer.launch.py | 18 +- logo.png | Bin 0 -> 71114 bytes GUI_OV2SLAM/package.xml => package.xml | 0 src/main.cpp | 548 ++++++++++++++++++ 7 files changed, 564 insertions(+), 300 deletions(-) rename GUI_OV2SLAM/CMakeLists.txt => CMakeLists.txt (100%) rename GUI_OV2SLAM/Dockerfile => Dockerfile (97%) delete mode 100644 GUI_OV2SLAM/src/main.cpp rename {GUI_OV2SLAM/launch => launch}/visualizer.launch.py (69%) create mode 100644 logo.png rename GUI_OV2SLAM/package.xml => package.xml (100%) create mode 100644 src/main.cpp diff --git a/GUI_OV2SLAM/CMakeLists.txt b/CMakeLists.txt similarity index 100% rename from GUI_OV2SLAM/CMakeLists.txt rename to CMakeLists.txt diff --git a/GUI_OV2SLAM/Dockerfile b/Dockerfile similarity index 97% rename from GUI_OV2SLAM/Dockerfile rename to Dockerfile index c304c55..2e47d9d 100644 --- a/GUI_OV2SLAM/Dockerfile +++ b/Dockerfile @@ -40,6 +40,9 @@ WORKDIR /ws/src/imgui_app/Thirdparty RUN git clone https://github.com/ocornut/imgui.git WORKDIR /ws/src/imgui_app/Thirdparty/imgui RUN git checkout docking +############## +### Logo ##### +############## ################## ### ImPlot ### @@ -92,6 +95,8 @@ ADD . /ws/src/imgui_app WORKDIR /ws +COPY logo.png /ws/src/imgui_app/logo.png + RUN mkdir -p /ws/trajectories RUN . /opt/ros/$ROS_DISTRO/setup.sh && colcon build --symlink-install diff --git a/GUI_OV2SLAM/src/main.cpp b/GUI_OV2SLAM/src/main.cpp deleted file mode 100644 index f328c83..0000000 --- a/GUI_OV2SLAM/src/main.cpp +++ /dev/null @@ -1,293 +0,0 @@ -#include -#include -#include -#include -#include - -#include "imgui.h" -#include "imgui_impl_glfw.h" -#include "imgui_impl_opengl3.h" -#include "implot.h" -#include - -#include -#include -#include -#include -#include - -class ImGuiVisualizerNode : public rclcpp::Node -{ -public: - ImGuiVisualizerNode() : Node("imgui_visualizer") - { - this->declare_parameter("vo_pose_topic", "/vo_pose"); - this->declare_parameter("image_track_topic", "/image_track"); - - std::string vo_pose_topic = this->get_parameter("vo_pose_topic").as_string(); - std::string image_track_topic = this->get_parameter("image_track_topic").as_string(); - - pose_sub_ = this->create_subscription( - vo_pose_topic, 10, - std::bind(&ImGuiVisualizerNode::poseCallback, this, std::placeholders::_1)); - - image_sub_ = this->create_subscription( - image_track_topic, 10, - std::bind(&ImGuiVisualizerNode::imageCallback, this, std::placeholders::_1)); - - RCLCPP_INFO(this->get_logger(), "ImGui Visualizer Node started!"); - } - - float getCameraX() const { return camera_x_; } - float getCameraY() const { return camera_y_; } - float getCameraZ() const { return camera_z_; } - float getVelocityLinear() const { return velocity_linear_; } - float getDistanceLinear() const { return distance_linear_; } - - bool hasImage() const { return has_image_; } - GLuint getImageTexture() const { return image_texture_; } - int getImageWidth() const { return image_width_; } - int getImageHeight() const { return image_height_; } - - const std::vector& getTrajectoryX() const { return trajectory_x_; } - const std::vector& getTrajectoryZ() const { return trajectory_z_; } - - - void saveTrajectoryToCSV(const std::string& filename) - { - std::ofstream file(filename); - if (!file.is_open()) { - RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", filename.c_str()); - return; - } - - file << "x,z,time_stamp\n"; - for (size_t i = 0; i < trajectory_x_.size(); ++i) { - file << trajectory_x_[i] << "," << trajectory_z_[i] <<","<< std::to_string(std::chrono::system_clock::now().time_since_epoch().count())<<"\n"; - } - - file.close(); - RCLCPP_INFO(this->get_logger(), "Trajectory saved to: %s (%zu points)", - filename.c_str(), trajectory_x_.size()); - } - -private: - void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) - { - auto current_time = this->now(); - - if (last_pose_time_.seconds() > 0.0) { - double dt = (current_time - last_pose_time_).seconds(); - - if (dt > 0.0001) { - float dx = msg->pose.position.x - last_x_; - float dy = msg->pose.position.y - last_y_; - float dz = msg->pose.position.z - last_z_; - - velocity_linear_ = std::sqrt(dx*dx + dy*dy + dz*dz) / dt; - distance_linear_ += std::sqrt(dx*dx + dy*dy + dz*dz); - } - } - - last_pose_time_ = current_time; - last_x_ = msg->pose.position.x; - last_y_ = msg->pose.position.y; - last_z_ = msg->pose.position.z; - - camera_x_ = msg->pose.position.x; - camera_y_ = msg->pose.position.y; - camera_z_ = msg->pose.position.z; - - // Dodaj punkt do trajektorii - trajectory_x_.push_back(camera_x_); - trajectory_z_.push_back(camera_z_); - } - - void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) - { - try { - cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8"); - cv::Mat rgb_image; - cv::cvtColor(cv_ptr->image, rgb_image, cv::COLOR_BGR2RGB); - - image_width_ = rgb_image.cols; - image_height_ = rgb_image.rows; - - // Utwórz teksturę OpenGL jeśli nie istnieje - if (image_texture_ == 0) { - glGenTextures(1, &image_texture_); - glBindTexture(GL_TEXTURE_2D, image_texture_); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - } - - // Aktualizuj teksturę - glBindTexture(GL_TEXTURE_2D, image_texture_); - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, image_width_, image_height_, - 0, GL_RGB, GL_UNSIGNED_BYTE, rgb_image.data); - - has_image_ = true; - - } catch (cv_bridge::Exception& e) { - RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); - } - } - - float camera_x_ = 0.0f; - float camera_y_ = 0.0f; - float camera_z_ = 0.0f; - float velocity_linear_ = 0.0f; - float distance_linear_ = 0.0f; - - rclcpp::Time last_pose_time_ = rclcpp::Time(0); - float last_x_ = 0.0f; - float last_y_ = 0.0f; - float last_z_ = 0.0f; - - // Image - bool has_image_ = false; - GLuint image_texture_ = 0; - int image_width_ = 0; - int image_height_ = 0; - - // Trajectory - std::vector trajectory_x_; - std::vector trajectory_z_; - std::vector sec; - std::vector nanosec; - - rclcpp::Subscription::SharedPtr pose_sub_; - rclcpp::Subscription::SharedPtr image_sub_; -}; - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - - if (!glfwInit()) { - std::cerr << "Failed to initialize GLFW" << std::endl; - return -1; - } - - glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); - glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3); - glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); - - GLFWwindow* window = glfwCreateWindow(1920, 1080, "OV2SLAM PowerVisualizer", NULL, NULL); - if (window == NULL) { - std::cerr << "Failed to create GLFW window" << std::endl; - glfwTerminate(); - return -1; - } - glfwMakeContextCurrent(window); - glfwSwapInterval(1); - - IMGUI_CHECKVERSION(); - ImGui::CreateContext(); - ImPlot::CreateContext(); - - ImGuiIO& io = ImGui::GetIO(); - io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard; - io.ConfigFlags |= ImGuiConfigFlags_DockingEnable; - - ImGui::StyleColorsDark(); - ImGui_ImplGlfw_InitForOpenGL(window, true); - ImGui_ImplOpenGL3_Init("#version 330"); - - ImVec4 clear_color = ImVec4(0.1f, 0.1f, 0.12f, 1.00f); - - while (!glfwWindowShouldClose(window)) - { - rclcpp::spin_some(node); - glfwPollEvents(); - - ImGui_ImplOpenGL3_NewFrame(); - ImGui_ImplGlfw_NewFrame(); - ImGui::NewFrame(); - - ImGui::DockSpaceOverViewport(0, ImGui::GetMainViewport()); - - // Okno z pozycją kamery - ImGui::Begin("Camera Position"); - ImGui::Text("Position:"); - ImGui::Text(" X: %.3f m", node->getCameraX()); - ImGui::Text(" Y: %.3f m", node->getCameraY()); - ImGui::Text(" Z: %.3f m", node->getCameraZ()); - ImGui::Separator(); - ImGui::Text("Velocity:"); - ImGui::Text(" Linear: %.3f m/s", node->getVelocityLinear()); - ImGui::Separator(); - ImGui::Text("Distance:"); - ImGui::Text(" Linear: %.3f m", node->getDistanceLinear()); - ImGui::End(); - - // Okno z obrazem - ImGui::Begin("Image Track"); - if (node->hasImage()) { - ImVec2 imageSize(node->getImageWidth(), node->getImageHeight()); - ImGui::Image((void*)(intptr_t)node->getImageTexture(), imageSize); - } else { - ImGui::Text("Waiting for image..."); - } - ImGui::End(); - - // Okno z trajektorią XZ - ImGui::Begin("Trajectory XZ"); - const auto& traj_x = node->getTrajectoryX(); - const auto& traj_z = node->getTrajectoryZ(); - - // Przycisk zapisu trajektorii - if (ImGui::Button("Save Trajectory to CSV")) { - std::string filename = "/ws/trajectories/trajectory_" + - std::to_string(std::chrono::system_clock::now().time_since_epoch().count()) + - ".csv"; - node->saveTrajectoryToCSV(filename); - } - ImGui::SameLine(); - ImGui::Text("Points: %zu", traj_x.size()); - - if (traj_x.size() > 1) { - if (ImPlot::BeginPlot("Camera Trajectory (Top View)", ImVec2(-1, -1))) { - ImPlot::SetupAxis(ImAxis_X1, "X [m]"); - ImPlot::SetupAxis(ImAxis_Y1, "Z [m]"); - ImPlot::SetupAxisLimits(ImAxis_X1, -50, 50, ImGuiCond_Once); - ImPlot::SetupAxisLimits(ImAxis_Y1, -50, 50, ImGuiCond_Once); - - ImPlot::PlotLine("Path", traj_x.data(), traj_z.data(), traj_x.size()); - - // Rysuj aktualną pozycję jako punkt - if (!traj_x.empty()) { - float current_x = traj_x.back(); - float current_z = traj_z.back(); - ImPlot::PlotScatter("Current", ¤t_x, ¤t_z, 1); - } - - ImPlot::EndPlot(); - } - } else { - ImGui::Text("Collecting trajectory data..."); - } - ImGui::End(); - - ImGui::Render(); - int display_w, display_h; - glfwGetFramebufferSize(window, &display_w, &display_h); - glViewport(0, 0, display_w, display_h); - glClearColor(clear_color.x, clear_color.y, clear_color.z, clear_color.w); - glClear(GL_COLOR_BUFFER_BIT); - ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData()); - - glfwSwapBuffers(window); - } - - ImGui_ImplOpenGL3_Shutdown(); - ImGui_ImplGlfw_Shutdown(); - ImPlot::DestroyContext(); - ImGui::DestroyContext(); - glfwDestroyWindow(window); - glfwTerminate(); - - rclcpp::shutdown(); - return 0; -} diff --git a/GUI_OV2SLAM/launch/visualizer.launch.py b/launch/visualizer.launch.py similarity index 69% rename from GUI_OV2SLAM/launch/visualizer.launch.py rename to launch/visualizer.launch.py index 08b5322..4e2fad5 100644 --- a/GUI_OV2SLAM/launch/visualizer.launch.py +++ b/launch/visualizer.launch.py @@ -10,29 +10,33 @@ def generate_launch_description(): default_value='/vo_pose', description='Topic name for VO pose' ) - image_track_topic_arg = DeclareLaunchArgument( + + image_track_topic_arg = DeclareLaunchArgument( 'image_track_topic', default_value='/image_track', description='Topic name for image tracking' ) - - - + point_cloud_topic_arg = DeclareLaunchArgument( + 'point_cloud_topic', + default_value='/point_cloud', + description='Topic name for point cloud tracking' + ) # Node z parametrami visualizer_node = Node( - package='imgui_app', # Zmień na nazwę swojego pakietu - executable='imgui_visualizer', # Nazwa pliku wykonywalnego + package='imgui_app', + executable='imgui_visualizer', name='imgui_visualizer', output='screen', parameters=[{ 'vo_pose_topic': LaunchConfiguration('vo_pose_topic'), 'image_track_topic': LaunchConfiguration('image_track_topic'), - + 'point_cloud_topic': LaunchConfiguration('point_cloud_topic'), }] ) return LaunchDescription([ vo_pose_topic_arg, image_track_topic_arg, + point_cloud_topic_arg, visualizer_node ]) diff --git a/logo.png b/logo.png new file mode 100644 index 0000000000000000000000000000000000000000..c9c7bad9468be94718ef10342504c70157fd658d GIT binary patch literal 71114 zcmW(+1yCGK6D1)?a3{FCLvVM8;O+r}yIXL#;7)LNcX!tShdYNKa5(>c|5a^W?bg+8 zPtQ!x>({R%e<(>KBj6)IK|vwQ%1Ed}L4CUW?}CSgJegeCj)HuBa#58QgQ}S!JcZnR zu@qGlg@US&M|?Gbf!u#}l+kg4fZ4dzLR8(;@H_{`7<+VOR2llK ztc;jg4Aj?4$j9&(Vkp8==(EksdTmW@jY!HK}`3-woZ3@u0_URe*0>QEo=n1s@%J9z<)#2ZNVJmIM&O$-3ly4l{OiC&kK*0rwT ztDg)ndmf%7!kxn}7?DL0i9yVX|GweyM>uNO?{<;&Z^!}akauA}JsiNQj6B$znK1V2 zbp{I$DsJg_mWobYom)CbAG-%`P}!^eJDv>+%af(VARvP=AZNqG;Svz8`x6QUVw2!a z*R!+dY+v46i69u-h$VP`+l2K5x?Y1F39@|fEntnCjgD}}FDP3v0ZLB6J4XApVn|4y zJ~^!zkwZw?4dRJ1wx59NvRmp0nmORg8=O>kp4MBr%CVpjI>$(K+JX4f*KhXo0Mf^_ zCw_t14aC=dnXUN4uup*!bm%y zZ2Fm!_0U=y7?IL-Su{yU#*y#I@Rre;O1rXt~e=pA{#1{DW(vE)*xG+R5S^ zXvyQo&$HU`gDyvcY6PBgFR8{Ge}28aaiw8F^|*bYl`WP(w1bxO`P1t`cMGPMH1`vr z>)S}0NM)v)>0Ij{`V!4w<>|Ri;}m>|^4Dl|)d7{e9f!qZDu;l_0590Rj=T%?wY#9az6CHHA-$Gf3h48%jJ;1RUt~T zQ|{|)W7ezB?@)Pdz+-#^%@;k}t5@!;4+}EBPlv)Dd?Ebau^b*?FSyQ2+uiU~Iz?<~ zU)m{Q2fes>2Dm4cla#FJ9LopFUc!9OT>6>YhkNGHSv>L|vz=E_x$&1ie>S z?>b?SIe~@3h+H(?piv;Fqt+gpye;8vwE*l`Z6)ZrT>{Ww>^RtK3`*tc7-!10PT8YD z=3DV*fpKPmN;6$jbQx>OLmTQXO(r`06hgMUg9R*;slLWmEC>&vf*-;T^jBAd$k0$m z71}G|^w%PNlbbC`WQ+&b*}^j$X1<;Y8_f3M+JM@Iye73KL|4!Vn)s2As`une3y_qQ3gy6a=%r$8rEL9!ErM^ghL+J=VN(yWC%*0-eH3}dP4CJ7IlML8lcCxpa*!0p zz;u=NGfFtuP%!HL18FNgKu()}5AAp=8#rL|x>GEn>md-elG7%kGJfg`e6X9CK6D>_ zQ82~rtDl*^5ZblX;$}?XTdES-1WNvRLkK(oo|57FTYUBH-E^wqAG$!buY58T7x0C3 zKo*Ci#NZc2=on&F8JJji0*+L3wo(H!besAR?BbMr<-(b%oaBor(vnWp_32x&c-=1h zuKh&&ZTnXlaYfuF(u%zgq03b~>$J1LAhB-zXj!ww?a2*AzzPU>u^(2%jS*=8Srn#P zbqRy`B!d@p_UTIC-I+!5Bt+BhyE)#>-YcXF0bB-w`CSu#8VkKW{k~Har@OipJ#{*1 z)Rqd#H+&5$WsgbgP1?oVGAWPL4I^J8rNc05SI8ej{G?iCESJMQ79V$Ri+bx6a7l^I zZI6RyHRL?FK?vLd#u*h5S(-_Z<3s{KJ6)+)lHXhX(5V`vupl;Zr2@F}SD69DD1Ypc zL2M~eP?4Mto_&`LPwD$s>mFo*)gZlrM8%GJ*%%lc z@A5xXq4z>Heg@VW&Dk<`z{vuKJ-f&kg4Qj!_wJmo7s0DRJ;S5)G%P%b|0T#%E)nx9cq=t##?N z4?4NAd$!r`<2tSg+Dzn#Q}%wI=|n0*4!wFjKzX~%VDd8v@I$z(iB~Mw`5s?!-8KCk z;H_VzGOXS))k9rR1@8yCxy#S|`~JPfz`(oAji^!J=;^(8y49D@0R^;++tr($rC>o? z{}p1VW+*@N5<1$uzz%5rR=`3+j1T)(}F4E|2%q7Ng{FmZq%|u9nAm4 zgGr7^EHgW!x6thbZ26{OVkN$$uhp`rzrrau+o8n$n3*rdKk#|iH^L7quODZ6c?o{C z>WdLUa9h1OGtR(t_&1)Ou;f1BIXC|1VxJ`aa65e$`j04Rz%Cvc_WS3X~S^p0c-$s>qb&5lw08>|vp;9JE&hry@9 zv)Ej2nCrb#D-n#Zg*{r*x9x=Eu;QFGYWFuQOOJ0xp0eb>xP}428ZytfJgt#{0R?nt zN9tV!ziw|~CP0AH5{$H$iq>Zwuw%>77XDlm{o@OKeVE3@7%KVD3y$6?njhu0&UkQ> z2kvEl^mt9FY`UW81=z{9S$MIzz6`?>A*zl7b5rltRu+ygQdUMZ4GY189qd%u*O}86 zoH6#I=w-9i9V|N>62q9tPh@z8h??yr3&)LxU9Q)|==Z&f=-xqHgOH!2OdH()$Kbo! zu`FBYRn(U!&&cw}L?-K~wQr0m-w=h(+Fv8=cA56p&~L-`STQ1*A*;EgPSP-yd#o~! z-tttPeMlXuiru$Fc0}3I2eHHs|Z8JGw zPNToWJEyMUdy|eE{&sy}*^@mZp0{27!^xm89BsvP#)yh+Ne95{s5u9 zuyVITdcD5qFpuXVMd@Gc+*OSo-d|OdAG1NO0c_Yn>zxJVP53QJ&G+7}Swf->gFyQ)}_tyN>C zzSqjyRfK1dpTmFBnMLN^Kdv8UCiy;Y0NGYw04m8lD_I`GNB<2VXVX0=wouE$0S9=H z!Mhr&K~1)M%>^9yU+Q(zs1*2>J^(pIb5t8$XkoSvyA-D<_cdW>0|5F%tKSbE zFj_?cY~QeDqJI?ICC-~#69J3ZKgEz;*l41y@QN)ojg7U7mSQeqsq`q-pXy+xB~CL% zn|cZdg7-NZC5d0AfZU-nxY%PMuqt4Xt7Ra#=m!Z40ti&%++&D!22?!s+TLCiuR;%h z_Ea@*;k$>0asf*%Q1n*+(YRLc69M+GG@stYA(!bCqZRJ2VVit}=jr{-ei3XTe2Lm>6soUMH4MSh2cgW3uMmDZ~EuMn`mg%k%cl` z0*|XYvGBzY}k-nPYgh5YYtzQDjBc#~(*GBCJxp6m;;nXx}5@{~r5%|ND+y z8Sztmq;HnBYbWj6<@Hl5qL(FAO&FFdZZL^&NPf!k=2w8>(a|)~?@K?ZLe^NOvH#%K z+c3Y?M#i^9B839v6^X*$%zV6X!W{RMu(#$gcS&C5@2ZgF?W8iW`gEMj?K{)GktgZn z(l#)eA`+)9V11cZ`3l)yewrctPb>O(=wtiHog#g!N89w#C|_h&HlGVwQGO@dYZpZ) z!N6?7w-SF7F%;uKiu3dM>QO7u=_L7mX^~^c^4HSlS`@7~BrlVBlxP1*mD0&FoSj3_ zN}9XEqg~O7j8{^j?ga+5BA4vMN6!wmZCH&VSKr#_F#;pcy060fkIrDntf}}%1^e{W zv^J=c6vRI~jF31M`zUHO)|m%_-s=0tChSWW=^fDgJJ&ecdNY!i?udG>LN~p^5`JYQ zVB2qkZn3b$!l=~2E586n3_S3vC?9<`@zb!Aj2F|E20#~d ziTXMGb=Xm50T$?QM2;k2lz1v9_p#~hdYW+~YG>=-uI$htGULK-6x%l^<%xAyO1b^LtF>{VionR4?i?ZJ2 zN?zL>{;LJ*R|fW48`RIck5p{&Hrx*L9yX{y@@kJ4L5o~bVmwm60T?9L?wk%Zj-WAq zCMT|ptda`>Sy{X_B}Q(^V8h4M zMqGm7(#4k$UtxM543|C*A2!}#|1HOe4u7^Zi%UK#xy>`Y^VgihDkr;3lAhxS?$!!G zRBKG$(3y;=2l?Zt*qw*3S2c&>tpvn-U3q1}R$U}(7+2tXF>miPJ{5`30HddJ3G#YN z(sRqb+PUCf=29|Y#=l-KY%CE#MrSbe$K%Nt<${L*xHO}K5y=pUH;JP2M zw@|y#iRkfT_G7vfnTC1%Gd)IRbl@}8bKj^I9uBh^<&GEt-odQMCl&ORC6)4wiDH%V)E!xP=FXp>X%7?G7hCIXCiV4SP` zPut8znDumC!gHn2tzLlRt^vicR$h43xeLOW1f8jkn@d>xx(UA_RP~(z`D2X5lWDJw zUk9_3bnNd@j0NP0GGDa)H{6)x*8K#MdJMbsFczBOo&mE^Rn-9lZxg}*I8G6xWUyLf zH#y8^+%DW$I6+D8(aqp8tg`l&@=`|gag#Q~hwm`)#KQOzRicc3+EOL&-Fi0D_mj@y zrrQYrVvS2@G)@=cR&xNZ;sJHz)EXPaHnf-!FA4q7R<);VH~%s*kiRK>$?EQM)DWu~ zb#+lvJ>Bce1S%Cqqy)?q0Fpb&L-*=Q)K0B4Hi%n)5)Tq3_S6REId6X{)winNAcOoL zl6kc+(iQVz_%BxO>3asAuBmmusOnpoyU7uXJ#EM+V_+7qx2t$XC<1h(CQ4!*{;}6i+A$+O1+-i7^&A4a zB>87Y7UoD&=Zy5?BfZBZLrZ;R>9tCl$}EX=N3!I6+8BfrWhhhoW(8n8(+cqazN`0& zkuKMMlW3lR`B7Jko8rijY0r0f#_4VMfP+8chv=;JO9%LBC`#Iz*EX9uRo!(BFfBbisD-t2;aa1SDAign&eU?p@90FL&@l7K?*)SsuSH94hUR<7b-eW!gapg60(Nl?lEistQ$ z2Rv;XL)ASj@7{q1H_K!tBBra^ka$Y31Y`#TbmgK^a+#_|1Wi{0b{;svb|!m8MPrFU zhDdb@6pI^j_7qQ|b1}dntcRq4+QP2^HP~OxY-9Nm!2!?B`I57!1@#was(37$PGO(E z&9|0e$f*&ZiaICsP{>Xnz_LkPsoo zv7~9rUzCpvVb_R#3sDa)2z#D@&Wo?_@xzphV|i)w2a3`XIud$&7rROjUgU3%Lwo^h zSPNzx&+ArlUS^y!ok#uqgGi+=vsJlA4W`oJs~m&$Q(&BQvBm9*7dBB`*4K=6r)g~t zls%uR<+5)(^1=d3DRAFKaRhN6H%UoYyd4|~bnh2_oUPU&JbABkZ6n4+L*i%=Zi&Mj zC-YpX6L6xl46u^EvNbhBd9OwoDjtqW3*^nfUPGTzk zH(%ORgCm8QOCZ9Ld&00XRZZ1r*K|LBue5Gj~x?Ox~T%$MOfT+Vcd zMEsk0Mb>dCf$7e&<2wciUrkV{&)3ZFEd*{=8lt>ryl4+7oD6g4r7w#yG%u7 ztatZ4P{lClbefDjX1MiL*uq7uM0rZt!8rF7K4OY#*`zWs7#bn_!x|6W4PAC#^>lu8gSV1QSh=J=F8^qM*`5MB z*$i~%T~Egn=wZ8BgtJgB_(Z!e6hK?rT|TZf8d7<8J0PCvcbfh!@P(_&u@fMWpHz}i z(U;bK0O2Vn``fOXN2j%-P%;8RsL&JKq$f*my%p5%n} zeSU8lG|wKbwzb`ui5k6YJ6LN;`p{uFs`$)~E`}wgof@luCW=+UfC;@@u_8)jZXUFH zPB(@E;iL?;C-ZZg5{c36~xq;z%B)rhW z?8jPEeI2wK{L-J%$*q(H`h^t*!lL?*k{+_0o%~x({zBEds!WH_Hw<=(y&RryRGN>h|H;H6 zocM<7B>my{(qP0te^SJw*GwOOOTX0NH$}i-|MfEKRBMdt&*a7`!SI2kD_pr<2ds;g z@PdeDlZ9d;R=YWX3?`!a?a*SjqUAbp=q{qC1t)i+!e{0*#7#e+RV4l^?qLCN|{ZdB74K23Uj~HZo}zv zha5+Gpq>8R*Pg=VQRap4Xpi|V#2c=hFzK*PqjuC2?Z$%3hR@*WQZEoZSd_ngd=FDo z3ZVnpc=FY2i&Q7Z;(US0|KC=6qa;tpzlCU`o81qBwL}P$XX^S5pD)OqhIxJzh%)zn zLV2l;7;u@{m|mMXC*9g8dK~j7Hchn&4sIBE#aB-QvG6^%BI^F*_Vvw#4?U-`os|1Q z`qjX{<1(Mkt05<JKA$XvzoN7%dZmgPkKVAXwaaPKhC=-1T{J0XjYRN^Z~% zhLso8m!O2tX}x@3)t@jE9X%Y-?VGeMpT(y6KbeWW>w~RC>M^A6=8xpzqQ9=;?er{_ z*~z_NkqXO~7`EGgN07bsujBg&{isiSdZz40t2fECkY`*?25Z5n5y@s1pCbNzq5vl! ztM5|nqeCyRLzewsV!;cir$f$J6Q6uDV6eB4Fx z;Vp&ie8E1KG;9r?kuSxvfTG6il!PFAs(%hE#tNFCv#@2Z=W~rTE7CRwO|{wjT-Pr>v=Fu z658^)WsE2o%BIcF|C58K|G~zE^5)|=UEA6}}m~Gq^8GChZxTmj5 zCDvH@xyp#)%wX~n9#O52REKBPVES*0G)u&gf)Df1*UBE+-|26+jvn}{JvEodFYxmy zm0%r#EGRMuHM$UglC?tp6o#@TURi$0aYv}A2tALCrcokk!Q`@bLBqZTu z@a?D(kj>xH3=f$~GGB4cBd!dO&Ku80nxfkg@#mY z23B@+bTqfe5wg8+c**%F_?1lODrVwDpUw;`C+Qjm{ek+orEHAHHL<#C70Y)fMM5D#=Yj5>%Kv^SBs? zh+5kj?#hbtaJr-T)XS19Pl-4yQN79@t4$zd}`!k_kBba3Lc&r8c zHLqKP*MISS5k04N;G>7-U1`bP$q;HVSHuT2b=09uq_ElN%wu72)XrbZM>gI25?*v> z%haE)VdW)C%Id5}P2s!3qD6%Y!2Vhee%0O)FEd+awbmEdXTJX!hy%A_yo$=LXXbFb~BYRaxUH~rf|D8d7CYvv%l#CuCed@j(p&NB_o zMK2_#Z^3FfXZyfuvm7V7gAJD>1rA%-URWuwT390Em31SEJ(u{qjRN?ZY*XaXmWqD!6A0&d;*aA}wWuqO_K3f@4CI!qx;|{GY`RfEViNX>e5m_& z0GPtQ+Q=-}nb@&zzu_FbN4k9|B5tOi`^}e9rsJ8=mg`>(7#4J(Ycm{+t$WcYd~3L2 zwc7z^3~6>!eYJD^VDk3Qhu>KE>`!lIXGX zyjxo8BLC6iNIxYHvzb9`YoODc*NCV>vNV(xk^HSZohGlaSEC3iT1A{!;6l36ao0>xv~Ck|TSIx@#~9oCxg zf7VW^{0aYOF4*WM@b*RjDN(e@Jabo&H*9m(V$jQV>mu~vW8wCn562;X9U#DaT4gD+ z;4#4AkcKifgbd2Gkh(yMYNP@ zc)F?Y(FZ~7;FWWh2A?vA9C+`%&~B??(n3D(p{IblpSX&f{sYv0I4duC!1<7^^Hnh) zS#sqiZfBo3rh798Ao*34pCtpyPHDELOxacyz6DdVp}uxxLokIcCJ@yAtxPNaq18=c_n6)wBJM29Kpb&)lh_bG0kpmDPz1#m zwv7vXJR6OZtf+A$iy*qB3rbF54lZVhhX4IDawmN7pM$^`qIYs_4&t>o`dEA2oQe}3 zV|>J1ba*dyIat4DYZAo`uZ0u0!xfFeUx)JIu7gdfMDYbGA62cVRobF$@daqa=K*Z_ z*s=DSgJm;9(RqR88}Sh0l6zfZDN*sJoEbT^Ha^v|hx}}`9oa-B9*c%biVSA+i}n=% zE$s3F`R&X%jFfEASw%hNXw+P_8Ew=E%{0DU@)z}L=k?q|uyb;Ce5>a9Un;=g13?-| z#Huvq=a&5A_0>h}O%I%NG`2I{74(u>iqCIl8Y#|quB%ey9vxI_XOGmZZK8CG;`MY* zX9rZeZuFUd8jA|6DFV$+DLL^M^4Xg!_2VjFpBw(^U2-@DT}5uU5@LJJD~}qfU~>Zk zrs+eY=RX4v-%6^A%x=mjG74bGpML`R?o3CYrBMA>oC9tEO~|eY8lYsCmCtEM z=$EKM`0GmCZ~|bh4KL7_=;!$^jz0sC2u|ti;&%MNAMMrVJn=in((JUn9C%`^Uf?|9 zogeI7uEn8Z3HZk1&eQQWKygtEDT|gdz$dBs|3Lck;`3%&Mb#tD#CGMqe|`PTfbHX~-RxG%bX6P-e5p$1)i7%*B8EpEmpi}1u8P7Iu zXJqaYf(+q(#oL_-`a02EmV*X86>jE)AA!#32&Q&*Qt#t+4|l)9y`^|z{^GCmS3WEl z0V4mIu1^srT=|W<1c%%g!SJCYNhDw${AQ;OlH6)?j;OnvZth@BiQ~LkE@g?PWRd=N z*2ibzEoLlbUNRAxR#z0ut3Hv$chK(sfQ zGS&KhNAwKSrFDI}`$%shN9cSp2m*FY)PwSx&I!G}jw~`;Kp`4~=TJQzGtC31S2x{m z9A{3wj$tob=z#3PjwEEZt4nvXM+^L^3!k10^-V1@F?p|p_k}4%SL)p;=M`^tOZlDX zM56M5LHbZ?QPI-+2iNa@bq`Q>ooFmuS%OZ&dWuQXt_N{LuU#i2L09DLbU7=MQqfmG zC!cuGJy@Isjf=VINn|d(9RC$c!6ef3}l}2(JQS&Lt=a zv>2K&*>K^X;-bIay{!~B?J88kV>g(m&_(V;+2G??H;V{+7rTh#&jkOp3F$8n6o7&D zHS3|H7}zpHp1Nztb*enj21CUFU5s)0IYz-rscH9*9-bI5Jxh z*B+2;B$(_WrHV#(f?t>Jv4af3?$dxE_wYbLrTD zU{1JK#xuv{tiA$f@pbitpqe0z%m?o1_6lx(`-65LrA7HhF14l<4k0p1WHO@b&yt=> zCg;7`5|~H!f1&5cH3Rx=VHSM-w|c1rmzp{xQS7xHjGS)&pgH-zpEOmXfQV$yp7oT-=&@Y?or*s)$kTp{nt?b|CE!kvyvnRmyg&S@& zT#_`N?E30}W@*TBrQlCW4SJAVK!lXS*h5yna9@pzZ!G3hht{$TvcLHbl07>wn)ufd zb|$ly@R97y0vatOH_wf(>o)PQm2H%y5i4IEZKD(($?Ly*zSF}!svcXLEERiu?J9(y zJ_QlqI^t}Ql8a%}A&v7TCcOW9SjGv!*vnIKVxbxxjNf)iEl` zPq`95IR@Faf47ET!;f}H1^T%@Yw)FI`*Y9sa%?7T5mTKm&mf;N;XDjv!;+o#+6yQ^ z6a+V{_u+v+98@X?}xm5^6E-XQR3SG>brU5Whk_(Q@EK0eg^OYFG0v?`)BRO zE8W7bWdY#*@8&Q{%Cba%F1j51Z#uX{bbNZ!Ul5HDj?I5IAneJe+?%4}o zn~}KE!`C&JeO^W&1itq2_I~GL+hk^ozcueVy#rE|tIJM?Olh+Q2KAHvi6Rx>WZ6)T z$i9&ws*tRU$F275;^t(m&3Ndety7q{yhY%vah@eN+s?&>Rny54BOh^mn$S4oFfT3> z^9U!qXIMNB@1&D{=-)F_`F_q)RU|w0DTn;e?JI9Zl4I)P2{laHaT=ij4T9R=nU3Z@ z2J)w=E=#x{Ua{Xh8P7Zc{`(ICdS?$A5ry!ynOoB+yTEv;tJ(JZB@CAXi_wV1b<;TY zn#J4`y|Q!)x(NvN6Sb76I>iKy`wbYV4b2o4wi<*@cyz32mbF^M>Dx^ceH6`T-Jg~U-0!AHY7VaWjx0ig^kmQaCvw7#UEX57Fkr*p6#BJtJju;L)cBq z=p-oBTqTJ;c}6qD9$uZuc6Zce_mt{&8T#VvaJjm@%O%Bo=-if_4;p?Lo1XRy^VmKj z5OJ%=C`VS|yUs^m+42<>{3X+Pmf8IK6Go9hr!m1>+gqE0-kC&S$cbvT0J4L&hR2yI zdA)SP)!yzY!D+bN=yyqMpxyD?XdowSrkoYwUC9L5X}pf|u1t3&4L_N~)prnz8T1_M8-SX77WyXGRZ`y@ z3_9igc;6U!7wn5i+FETM*wzv?-Zkmdts?`s6G!0pCD-!yGSmyDbG}+ks-O#1N|x$k zk55azBenq{0tCqMyhx!9!s(wd!AKAmdZe|c8rZjZ z)tW+0HFZs=@rU@As)463koRY7USnv*7lrz$`MSo%>?GJ-w|rwu}bT z^C)i6K04ZvpF*;0jbTDkDL-gE&8PRbwRGPBeKc)RT3Mf~E8C;hxfdeN9WuX*BSp4v zZLL;1Iv%?=u5X&}+icGsR9t4t$u05du#JDsn=HwzMY*;ENSN0oA?Y5bW+yCB`&;-+ zzy?lVD`vLg+>_X|<4Xh+h!~)*Sn~8TGq+j^#*Ku=xA@$&11>+8CCnHbawX&P_WbwD zPlcS09zh;4y1%fx9YWC8PH%~I&JNRA-VPcFg}mz6*B%`m@%ed1s)6Yl6D|Q;`~qz9o`*8o<(;Q<&CYd0>u;2HCz3c zXSax_I{*0Si0gP73{Y2d8NLY(FnN~husxix3U%URmgES-=vpfOs~`YT8=T`X>JOYY zYE?%;c+8fZNc5-8=s(@Kdnd8&dM6c9F#^$i^wu*6C4v}Ej@hbnI#W)iZ{xT>IDhW0 zTx{G;HQhHIy1FJR#||XD<;QWzj#eC=yh~|B7ez!s3{IE(VWsR{El-j7_5&IIY46SQ zG&NB&vI<532q_CoLiS+^p6Yn!kLZ`zT5XCS$&;s!e4@wEq2(#FZRKI5{VMyyE-iE9 zJ-Z6Ah0U;5YpYh9H?$q)yE)P2j3IQ*^E!EuexpBoPz&5ENs& z8#u5Ie_R}jLw0rPk*9q4vbV6@(7tJ<)lhBpllG)Bu^k` zxuzsI-`#hPyUlgzOrXLyWZ?mdU1`0u_%Nymxb5JHT${il!TX-{!Zqp(f&SU@+xY6C zn&`-iTDMZcR^zN&7OK>p{}SAbRk38~l|5fmzSxW|F^*Ah70kR`DtZg*pTDHoLdBN6 z2+}=H9(fj%`I!}(yEb2UV0|3smH0~j@HMoF^gdibcvW^T&Z`iFx*6fCRk@UkLB zSGRj${%rL|wu}c&?72^NF;VaiIiE&HcVURu0@}J~S1t0L18)OeliKp5QL?dts}B;c z*3{iX9*D&!A@!n!@7#6Rp^y3W`9oX%MF056L*zhii~AK)#q^OwxwQSbuCsdQ~pjH_IW?U$-75Z z<5!U#dVwMfM@#vu@Q1XS%r10ieJpk0qTFp8X%_fnxR#q_S%Hx<@gMXxG~Py6$z{yWOrukQua$~= zc@yj$%v6|vvvr=~LRLLM-EA1%QC-wwl3=-jA-yHH3*%d{4FWWAGf$%83>#k5tf zlSDMT0-?Q5)*jpry*r>k#{O}DS)mZDgVl_2x50lshRb`D zvL9qT^lCqAaVL^=Hh~w}9c<%8PZm2e^H1hOrzgB2I$xG(0%T_WKhP$%cl8sm{-yW(ES)tYrUPcmf6V%m>p+naG{5TMWlAfhBbU_Lj# zOn0XEe-i=XfZIt^dC=bOtLc#Kw zktSTBeQ6-df{#q&$(OuV9uezJ2uNBm=qY<2XH1;z!%7f=iQ`U>lW@kAhrhosa8tf3 z^gjHY%n|1LW{zjagIP$Jp~UMS3lRq3o`x;Un8=sB)J~p?xc+e->BvVqp1ARYfG0@d zMx}P#)9*zV=`cp(@nx>!tKau4=(@czFYlApl;q6f9R5KST4a`Yw#^eV;-R+kyH8cz zX*xC#g>G4PgAd_|*f9%tI(KbZBrlI6Tj2Y;`xM;}A)}ygoW%uS`I8rGrhg<%d4rNn z^3r|P(~N+X%4hu>k5A9_?_FclWF=cfl5^;C ztU_0`!tfs%5D~JSked$gz5r?1d%uy^k02$@deM7&6&;6gFN_UY(gg|xiQhiX2 zLFq{rCtS7#?498cS|AA9=U!-|60m-7vCi$za>957PtvE}X}OgVu$wpU+B0u)#e30Y zB0^)PcG7sb&Yjh(MOXg-5noasEtL7@TTJmUH3);3b$?#~jcF z1yTG=bonA05Xa4V8+VJD@<-ID1^}!hvmR+Uy)&Qw%nWHSEfdRF_&+jASAvj-ILwFn zP;kuDXE+JYuxJyorPevL$)W(_*^OqSb6p5@*#I5k5tEhI7?EzhmLYofkQ+Jcrh4x} zd;apNFM~*hBioIU4G`~yYZBHR@8_fM?$6#J09rd3js1;G?^F>q^N+@z0WwB(K*=?( zRh`Jx@o?tM_J&e~pJjM2^z=N(oct^;WNe{-wJNt(CxemU!W~;1BKYG1I%kHj;6;(qeH`-b1a#J#RfjFJOhHv#H=*V-7u&r?S?LMJeAZrC7O8U4muhjQgjiPM!uTFlvTqlW#|-eCH!C$Vw{Q>}l6q3?|G z{_AuvPa-AcOO0;swGaI2D7S^D%6ESqb8H=Ll#NP|aG!)I&$PFOw~l#u^QpMVfFp{T z5Y>+J&xlrO*=JWzp9-y>D(8`Q(NCGEI9Cgcnf-u{8BgHXXigETpaR$`&P!HqDZsa$x+YAo~cRFS;j{&g&&%JXZi23sgh-I zi}9|st$gLLr02RrR(yL11)H1-nN84q8zB%*qnpSzjTTT74zo8D^3o4J#dV3 zdii`1seWnooc9O4M>II#{+m`H2w;nCzc@&Y@9+3RuXPXUX08@?lI7j+@~lrEWDh9v z6EvVn#=Oxg`?#t+cy3Sp0?9dRVVg?+P`wiku9(OdT7{BsiY~fd?p(Uzh~1>LYMraZ z*~a&#!A%ov1b1k%)Mm3m+RIcb?tj=K7;uvrerwh8>Rni>ai-NfT{n9H0p`KkP1!S) z83DCkooPk5d`qPr^cwpUFJ3@@As+vl_?W-Rw)fA=`^UB=HVq?P8X|ux%;WDdGWMJL z-olV_yg7$+BBlA43XO;_Yo24SJw7!%oF5(GWO$T#)+36}PERB6PMhL@G`%d}VRB9_ zC&6<8xVEcAynKi(54Lh7$aXYUu1r5tL4QvP{v+X?$(Lm4lW$5~9E3!O`3B69Xm{7{ zfKL;N7os$`|KsT_gW~F%u6+|cxH|+%a0u@1?(XhBxCIXoAOv@J3GNac26vZ0fB}LJ zE_n}6)%X2|DyTDO_U`VruGO16El|lU{-N8tv#xZ}ZrY!9bb{)risT0&a%fJ!xzpHP zUZjrr*g^C93x{IMj`*KZeVRp0p4U^Up}SC}{vCJg=liXkOG%UCFd~ipjqbYgY17kw z_4*baGc2{g8fVW|I|AZ49*gq!bgEY)OBO7Y2Y|g`CQ1T2^epYnA~YVB^-*_SZAvbi zK+)R;xGGl#zcRA4WXIyY5@Rp#&@p_U+?F2Ww^C>Gzt@d=g!*mcE9b2+P(Sg%bX3w!sb3Flr&}SLdZ$ zPJD|~K2O)mhO5VWoy_Nks%k9G5r-P$1NLy0S-?PzGo0Rt{bx@q)`U=YdtY*AKHf%aSx4%0)b0ZAtrPkqX=}C>AeOV>bt)t7{pZMme7yFtqW3b3;swOg$ z)dEbRhNz5(s36Y;3a(jWIe%UEHHCX@PM#y|NU|R(oz&^WwG~Ol3xd{84$}3>gaTpT zO1*60O8c=}VBG$nK5s3p7RFqz{$Ws#w2HADhj&iHF=~QP$m&AGG+HrCTonT_(An<8 zsCHs?WxQgtU7}zifeGFT=^{$@PR`q78-uhM+HCg_~xtSwA0ebp+B2U zz+bs@7kqs$Bothwn>+iC!;AvMp)9LmZMIJoe#l_ADNU5NZrD@xHa3}x018lwOof4? z1Y7yPSV|p*t+5n(D#`vqAf}toB7p{~%qidWyuWaOMV_W`Gb7c25lqZ`-E9ZzqN7## zbx;obZ_>Nfuebfbm-YC_(!>GsuyRVguH|=F@8EWt-=qVHdNFX&z-+b&O?T#9MRChs zcEr}qzHYM=($LnP3)S0bu!Y`$bWC_gUf7)e5&6Z)^k+20WUhW#w4K~x*93=>9%3KD zl6}rrxRH;LhhFa5oW#B|OWleK(auvWQxxi0Q!@Yb;Ai44CzPx*CYCpQy*Qu3I5B+h zg3#99?ldi+!HHXu-{=|dn{LH)fIx5D7v8%xldL3@OWOFN` zjaj5Bkt*IW+KT))3Wm-!p*8)vm)id}sJYYD_zOxXVVD zW*!E_#)Q>zJd!K?z1IB7;`xFdL>Vh_N>AzBfe;d*UQ+>PAD;p!SWCJOqXw5hVpZuT*(Z`qBT+OwCUshmx(Kz_<6z>&Pts$H9b$M=v>y7jiUT!upXS7Kpbg~L&|nviGJ-fMAWVGxF=Cq{G1Re@yh&@f zRN(P!L{IzCT8+9KRYVpXB3UR=mp~igo|HbFIX}zk)Z26I4EV<*#4PO8Y5lDLGLoMt z_M?Z^vHQt;v28ZFAQ zg^_WmIwx|FJO9_$!GN8xi)}oA+lMi_{cn_z2l5kD6==j1?^3+nWHDff(IQ)@J9u#S z{|etN`@R)-Y;%GFfz7wl7xvCXF<+^Sg;8b-zMp#wvi_qVOz0=J1;duJcj#OY8=xTz z6y-&3`)i(lc9+VD_FohBf}cE`rqim-ly*UP?@6F9k&0 zBD3N!yXQyB-hyxGjcB@M9V@xiShwbbPpK|SsxE@{2im4uB@;A9=Ki&&d-p|QKZh;u zvTDKy0Puh=?{!P@AB5JG|2$MO;o;$Nih^ZS#SM*bzIM@@K{Ix^Iw*odw@ukH0-4lPvZ@EeuO^!F=@=F z7B-57{ypLJyLzwhj!8eAg95#Y&83h$bx78kdPYIvXKGPnf;0jh3PRR<{#aim7q5m? zoiTxH)(S`Fo7e4twLoEIpugMcXOWW1J&q)}MIX~!F5kS1JxU(tj2je$!o8%dW*r4H z-qFj{N&%RZ-NPUyIPnj<`b-UqWZ+37Y5uQNEtXiPNTg6psQV^Au2fH| zPXI!&2F!>t6ZNO#;`1}js&_BmqKrl|W-@S}?g71vp8dD{7P3TU0}Hq6ty3s-&Mw$> zz;^J%5aYe)JQh~)nPYg`P02rW#~PyTnCP@%K-`z7^@Ibu)!NCW>r2jmqrOVLAF6wV z0nGlU(gY}q1@leEm#Y?RU>-b*;>YOlJ+nTF)RpA(94xW8@Q45{+U(W9RBNRAa3}7| ztE|e^VGVwwoE>za#UgnxT$vI;{NCl2^Q4^@46N0!_Pe3dYlH=4Ppe%|JB>(13Zju) zi2bboYe1HT#NTU%YxeL(h6aJ$Om-n5{AEg83Uyv4!Mj|A%z^07R6B|@DBL?cRpS)U z!lUKJzy-92&LxA2rxo5z3jG`#tEF27PA2s!K?*~wSzK6pds|k4O=D3lV-%B9i+^C~7T!tyRm>&z(PE0{uMZIA`U81D-P zgY=JLZY%;3#|6NOrZXZ0vU;I*K!Wo z55V2>kL8NcA4}(>0oTnDlec8tR1Aow>dG;$);d_`rS2V1P%EBR|oXKfGuw8Jh`C$Iqn@hfZ%DahVk+x{eaj*uj_a|g-a@JtnBE3eZ}PkVXwbej z6w{<$5+uY>I2%jwxd*8p9rbRQ;YT*jELJ?vqGoutBi(zslK8b42~FO(L+9V9I)Kgw z6P;i}_T;p}UMl!dy#GmSm3uaSUbHl!t8D;`dHO+rx@KZEbLs>%?)QJ6K_zVlWVyHl z%rY>%JUk3Cy@&aAwPqfaIy?mW^wAlGC;-5+;^?0ej>D(aIU1_)*94dEsaoPv@jLz5 z3>TSdV?<5o=7N)(IC3P)Mh*HEOUQu9=MKYj7F8n**06V5doH6G1YEnR0duYJ-LrTw z__cjHRd@5_96aVjh7j< zdehqqCBSz681TH%NnJRcSpDrHxGyH{NS4fF*>*~C^m>E(4tUOV0t1)87S$)7%@)DyrB&UGwD&VL!!sxZu$llGO}9d`r*rV?6y%Dk1w zBO;^>tt2CoHWPhFadt8-TGCI)rH9yp{CV!-`r z-7G6kC%`&f$X3dciCj zA{Dhz#dCh;fQ9UtosN#oOFTsT8d^#ym)+|cElu=*d$j8GzXC5>bO_T0Pl| zIclyU4hMEH^B;dpr?cNOESnrOZ{MG8UQS_PjoK|^wJ@oc7K`=XBbWzBUieFRiK%|Y|S;pLoIK+pV(i-V>%)Q zs(RkK?;zS6RYIyYlrAmDw7Uqx7_JwBf)ivi=Tl}5JUnhYZE>(1DRzlr0cde%mn)#PxCyhQj zxVxwOQwX_>$xI^YRxg=(^~;BjYE$XxsH1`Bf|6RHXo&2NNG3Z1~YmT&7F)S#X^@asuSuQ0+Tuh4Vf>=J;d=2N_ zD%q&OPxt^F@D7~%Cgg(w;Iky?LRZZ$LW=CfP!eLD#@V?n2A3Ab5F>+_3|mRvUD+6F z6n5;dmuEB*>pg?TB_oyykBwwz89NjbNj<@I=-q9xax>!$W)OPaCuiyx?T>ud#Ax@< zl&8EnRoQGg(Lz~sIz{~SGXMHJqOq=un6iRB8g-efue$3z{kheAoN5yU+l zT7&snu+lszxn!a__F9@#xXk72S-*G)vLRh7YaG^-WNtNqzTv)9!diUPiRn?epy)<5 z#x~R%-$K@iClbX2;xDc|(7yBxu3-}};RTPyd?SST-{xl#m#d|UZFJN8m&QFfp@%zu zFuTXY!pMA%au8=c16+BKHBGR~_@iC@bwYFYZe5)VCgyk%#-rb2P$S?mAg!_c=p+-J zn(h!?}E#W~F)EsJF0xSOT40g9UN2NELr*8k8T7(@bO)Z~o4%xz!6QFptl& znwv@jsdw(i0}^%{)7NKT>sM|tw>dICRs2U~g+Wqc7&1}k2bE=t;!h@%W%hX!g&ld* z1s52l^X0)y^F8BonJ0gI?0C&&lBJfkuSdJQ!hj$RN`?l!a8GQl71T*@jym0$hqzx6 z7x3gpgaLbXQv{^EfNn7Y;r1NW8E8j7AqhSeQaTOR6~`es%?FHgbnI+7+_`ZjtvY8V z+CZ-t()+V84S51y)nX7EGC>7k7M34;n=`BUC9L^z`H@>Ee!^ll z!~-Pt(a~UbDovMz$?Lv8GT(bY&OWm#MTRBj2GOGrdK*^bz$g=Gs5qJ5djHO7r-oRM z$9b*=m)5Yy?b-aIW zc`7dbnmK+}F6rlZYi)l!8Q|wLEPeJbVb*I2hS$qJaZo(AU(by{+KJqpS$q=RgAYGv zLb}%4toBni<(jz-4L-BHsU(okbRN%STR_cvUT%^)e>HHJ!ciI`ayJQ^19>=80u&;_ zURQy@X@YUmx_FK*Gp%Dop>LIlk7`o?=K2NJNL@KhlkcV~1I-6%>5c2oHa=8WBPCo$ zQkO`1`(Ex&-Tk3vwpVkAvnngpmyju69vl*wK;Nz>aNYtN6bcM#>o?&2Fa)G)jiZE? zYB?8>oubyeK&4#$k!naulj84IfQO2-rr?)JjJB)PSRcMnFGGM@{aea3zRvr(q5w>8 z7XONkX1y51%s7NEZlavfNt58{-31+3-%41k>5RA(GPEu}BcT2m%l@iVx7 zuUbS_fb|n^fXyK)Fc02EY`{a?J^31rjb6PkP02BofxhJ2nwwBEV&eh;1nj2{Zm?KC z!REqkK~{eC7J-#JWm|{a3hX4xDDl~4LnDRZy>nLN#d;RjoK(M}mPA%nEL$}8Ap11O zFN^PDxUYMVZ>~#|C=zfq?%~atjfS%jH#RmVrX;P^g3AVTr_IE8kQf3SUb!EAj-2;L zIbB1ryyKy@_R6+|WM=kHfGxX*+bE3n-5A`nyszq;{RuRGTqusYn;sGM@gM2W6b}v^ znwUOZ^rdm0yx1ghXGBxUxR4dVJ}~nVSE>I;a!(ehIxwKbn4v)$+V4-MI+A+cvTI^3 zRbN)?h-d2<7SWj{jL$wwZKL*DSGS(`B#SaC8)HY}KK?d*4dMEkgLpFOEwMHiuI;H= z1M=Cxcygm>7648CUCt3~5fIvv@S$F7+aV54A~=NMc*3k2fK#Fn-!6+eh3A zxMcD>2<1#+234k!eN!%N9BX0rdN&}Fi1^88hmFcG;F+pMa-TcMiT>bLYMcKzbfjE) zVP$BL^utl)U_fRR;_NmCsIzi1u^2$hwsJDR}8bIJr^+x`?`H z!jxXPBb&Vapr{)aJ`3Us9S_@qXobq)ynmnAr%mbCnI^D7R&Z=Afb41G3xyeh@es3L z3{k3Nkcs?zkcf61t6)f?7^Orso49o}t6A;ePFx;)G1Y(Cq$iA`U7#gdm%d-IW6~y( zI{2U$_{e@5Bd;2ddIZEXXuap0{vOG?1TG?wq)A7hUUB8BYDm_TK*fh{b!7|{A;aqY z%IH*{g^V#yfTtpPu4n7K-p2ct%1ZZCU?qj>%2;SfV*e;#P&}eL+!b7GVR->i z{Yw14qM-gf2fH=&o4+m*zt!gAyt-fV=}wF=@jWapB>M1^EP9miK|ySTxxxLNto8XM z>C$1nCW^7>>9lRKG>yk41q+hY2e-C{44yAm6{G*rxG7=A-}?U3F8l#Fe`BNm{u+XN zJP$o>+P&~iJI^l3C&6IiprJ{Ocdu1?affNL7dK%zKLpOC>82c7y@loUoKt79NjcU1bXsb-|bImQxnanSt>zYey|)wQJp_Wej&d2*odN?<=eELIwmU8 zLg}WS0o$hV_pGr;$EjUv^D)i&RzOFolj532=0&JZ$T9 z_8R~iD*inQVQ>BSmsie6I8TChPdyx`MAG~{P=07N=YNO-{-Ly&30NDhv;^3|F34{$ z&t&TP_Dqj3{J+c9p6uyaeqJPG6If6)!O+haq+-%-y)9W}7%D&!|CYnKKl=Teo~Y?G zpv#@c4exuO%hbT;k;o=-Y@4Q#bG=V*uMYuG>FBc=&?M@}9J(_W zbIlTUCVy8o=aIM~ef_DW8Ztxch7s!7-Z5^{C}M+xU?lrR(t&Yy&Y?Ly0L1T)f>gud z?Jk?{lSJ;dI2>l>NilTgCK(ZCv)MK)4C3BvHi(|>`G;TyMI#0b1nG+B)vQoU_h3ht z?2!M}W&V~bv@~A6wuya69rlE63Aadb$UuuyHc4c?^tUR2ZlDjE7-y2y21iYu)t&!4lTY(D>; zYdK#zhP<>VwqOHQ8dY~?+e3J(T9s>thAO=>%2r@tk{=YW-e85tq4PTe{pKk4wWcBz zOC#rNIpOUd_F5o|GAh$YY=fB=_j1=!$y&E)hmhj{KenSAMmTjojx?(skCuabDdtnj znk8%B&S5ja;3eDlujlE^q3B>UH$745$y2zF{Q0ijAqVMd(MJ{YycGh0Z$w1F9{Dd- zHM|$&MX*{ZaIUq4E~W_aEH;maw6I=7TTR?A6>6Ebf~(l;4FK@?B5Sb>h_VWsDWYUI z%#NRJ`7CwbU*TZsAG{mk<-NuaZ^mMQI_dhykxOM#LA2T;7^5C^J)u+|k8mTGVUGj^ zma0F@9aXq-^GOVcb^LcT0G`{Z-?S4ySqq?Mq~-d$k9BR)`?Da;+8I0Ag62nIVPJ~t zW1ywX`}xbeL|%f=_)F5#u{R(tRZA<=J$nSMsknVZabl;rsr!`>&t(W!F}=oFb36S0 z=_^Vvq0+XmIv;zEsfmmI6h<_Dl!ZFo)I?wC;Hwy3XOL&V-6qaUxvx*1y>_X0Vh)8JqMPnNWccW z6|Dw+io|9|9oW+vn$!6o9$GsvBi`l*?w?F7slUpMQ$lDteIIR);V7m2h>ekk>ML@AKgWpZq*a zx%eLFsvH-Aq3#Hm%l`}4O8G;81w4Q};G&kU|E8%sNH2{tF5o;wEcDXV15Sl80hJg> zA?(9KX;q@kS5=+=kwatVKiC_Oib^^mzv&t2^_B|KRPan$xbjmu{LGKZsxweJp2v=bd=JFQI`xBAksSfVO+m?-w2%i1B?{-L>ghjA*2 zQGk5AMpPyqr@+mo8ZvHDX^y+fuI`7+{+GC_7~QQk%GZwEzv-7DCejJOl>UnP@Te5m zBbqEsxLTq4J!i0?%F)Z&pAWkX%2qWb$<|5^Y9BQ@%&R_sSAeBo;If_6uRe7wHT{-9 zu%MG8^Xn_s_{3k=l(1(`l)-C6wOAUz%Pke#W-Y~G zqpK~4;MG@em3P_3O05SbHg(W)DNJObUW?zl!^>%uJ zq=s|lfU*xsmO|XjfA4QJFrM1{UOt_6XG`)}u@glhF3tuKZRHvtZRF38kflBVKFZl} zk>$C4+|wv&s(Ky3s*eM^JV$=ud5ryp&^&GCXfL(OSw+eqUO#E|Dm;1Ypexg=_%>s# zr&~a(y#`zlADN%{PX$S;09)I(v=-;`bmGCymzI&dL6~2hmPzkJs2Z2PKc-X^XT7el z0m0|FnH}-Mx?LL0(sh!%NIN~}YK`xh372yiz`UMimVIico*;lh00oZ&W6~qjXsocv z&^iQ*32_;rrAXh-2R-*4rrtVVjoq`4@?R(T_5XIHi_RV$=r||giiDl$ zU=L~SeAXmHEX20G05pUI=8@v-tU-E+e$hLn_FiiElRqopc|XardoUR{=D!zOD=;oV z@6|U&3miYP-5>oDv;i6j1(^VK=hp{gaS8)Zm3j;5H#|no#za$`{rrKQB1pP+yF-cy5W4lX zmFsEykLL5)ne%uh`F`NSxBa=y&;xOe%+Y^EHKW^Jd2=_XU^D&DvgDXPBJMXA4t=^p`%E zod}{(I&N@EjK9Z$Mn8|6&_{VkD|E_FZs#e_R%C#Q=F>S_1meyYL;)m4zGUekAl~`E z-l}(O!Ie%%em`|MVylEHosQo)n_Aox zyATA`^z-`>qj0KKY9DL0vZo;_0nIX76zm-cfCJ5-|_?IhX6--E(x(>7m`zhu4Z}x@#D=zO1aa3`Ne4D%J$N=#YFI&J2w5q(0irtfREdOC)KploXY! zRQ%9nmfif!AiFv9-<5moR(>%}f{$wPuvVXX<`zFdcd>|Bei5jzj7SOXRgOn4cK?*!WQU@eNC-BQ=H-?n|g;I2^y1W_Z~OihgtNc4>rUNzT` z-ZU~tIn-A(HXzn??N~bpF_Fq*B@f4en#V zOstd0i8%2VldfW<^N*Nr6MkPGSc5!l^9sny1%1#rKx9O?S541QLnjpMw_R$phU`=x zo)Q;&^MH1(FRp<-3?O-{m?grQBdTrY7W-eT5{xJbjGCT`MO*DpBV(Sgm5eZ z0l5f4t?BVnwCk2JEn1823mtyx?v}lv8XV`_FyjqJLC@$pqlH#0plL)G`aaecG;kcB zkrr$BFlx;ED{xgSPH$M0f|bqF`D})4s-Ku+aa3QE(PbG^OH4=c5nu2f=RKdGmr52r zq+Gt(Ml45VDt}OM`6j!I;Ohg9)A5W8wLoGTS?xB__`@LQyFaYP-~s;Z+-^gLU2hIN68{+rSKaP15WC~f z;lYg_c1Xgbh4?+4>t_{SLakMSMTrO}AZG$cGd;7xiv+GTkqgG=vHd#h;C!6na^}?h zyRChS8#^^yuVyq_R<$w}8;vIw>xve@LV~+By7cD|Evlku%3;e+X*)~WNjOtZq*|n0 z2lR`<)QR{3?JwEAsIAY>HKE#U>i_g@W9?1~sgjsM4o{zqye>cc_g3kDu&Watxo zt%l3=XXuP7#)YV4VHMslU~ujcgmAiU=H=S!x@gg#(|UYMR1I?&=466&My&ul4qrh< z61i$#tM85@-}&RCudTn@j3i2cOu9A2-8zH(glilYyLE=XHnYt77OC7@s(=c}{=;tN zZ&R-_DO=66Mj6Zw-*f*F(K`V1lVp$9Q@I)UC{2{^8jLZ0+0Ml0huchfIuXeKz5>i! zfx>gDnKAQ}kbo#pzCs5Ljr$D4ETKa{5Px6%m*`Nam=A>A+#bu z2GRUgHa3&d8R_lULwC&Cz| zI0>WTas&FzdjrwKj1)?)Ov45qgRd!Dxz zu6hzHu5#`V7TKtQ%_J%HZ#=*yoleiF)G`qk*ljz>I3W2PELOV>E&;3wF~?%?MDx`M z&8iP%7jL@pP)xUWf&(+s72f|e?$weIBdZ|B_dBw@sd>>voyz(jXMb?1_r?R+y``tT zKs74kqi2FuNooZn{N=u=ATiO>;t0cZMgS~hplBh}n6ibn z_dBxW(=+mfLM|2`@eR6V;jTJG$-=TT6JJ zS~XAqIdAtE^#KWwCR-+9!9$_1bSRH5;NM0-##sru$&`j?3F|lZtrA=2Sov#kWuhw- z#T%QBeaAi{Dw3<5YEUr$`#VepARg#4xSFl%YjkdYmVz2GzvO?)7B`$%9|Z&$6O8ji zwgKX)P1g;{uMtcs`2AdBQQ}$Z(ZK(|yT2vLsD4gU_?eBga}ZTd83NFtMBdXhU8QA7 z220n$xPn)G0WH}cLo&UR!zXA7e?erBeOo6`@Fr~?T2EmQNz^#Z7rO~*rkOu%k^Kto z(d5G|7qU7{b`t*FAaz6M0U#r#A1H3RTH5GUJ{h{TlfWAPl|8JaJ!p0Jc#Ej#OchXf z(4p~wQyrz<2+7{8M$0}VT{ICb-X&X?2tQqkO^4OxPkHxZYo1UVfnLlE6ZV<6W|yc1 z@$;thB+<-5^@-Q@YD|{o(y&jQm8xL3u8d50JRg7+lpRP}Sox;~s1P#>zsoZKIwaa? z7iC=L&xkY?7*I8e)qaHwka?!kT*w*+SygHgQqC;9v`3>?UMY~TvKKY7l}Z(JU|&$` zYZmR9O@tZ>e8zHLKb5YZdWz0{7Su&0tcfeH_U6}5p8k7%^G@FvT6mSTJEE`0%T^WA z7XpeIhgC>>@?4s&ib@7X?yN^zmR+^^Ji88wDg!9!IaoTSnW_hhRIhcu+MMhbc0z)B z%Puo|91_1tGiJmYE**jFVH_LBU9}#oR6Ztn2U@SOQIiW=|2thF53u=;zQ>EOKUOycIDa z?ZJQ4kA{V%!l{S;jCoqRGvYt{$jXleaT%C1P#Om80`s%8+LLWd_s>Tmo*(H<+5bch zC7VWwbG`AP7~V`v`sdOt|GEik?(S%?We}7rM;Gq@Y@d{Gtq0FTb7-Y#14fFTM@wij zThG6(usUP2*Ko`uQd6F0?j1C@!UHxaU6GWqcfazvyf40rv?~dOF?>4@sDQk$bkxH4 zTx`F55q!r+ph#q^H2q=Gu8xc^a|KqHCj`>~Ru}P{WkZZHLD%LO!*>*z@GnHDK%ATTbo?dkBE3b61!zXMyiU>nVLv*xK4y(sZ1)r z66C`g9KTed@OwY|@QhlUVWP(ISj2ljk$oRDX3XOy)!^5pKb+I3YWYMd6f6DOmHcLu zRI)&M$G4+ohjN!!@HS;~HkrT+gY`%=ropy(cT!k4Jm0W@xDA<``RVK5wl1aJOv5-C zZjP*?-2M-R-5=dSSE^B30-z&}T0bbAZJ}h&Ke$#+ALB|KE@<(X{__Jfj502efKd8S@VVKIg(m6HE;D5m3 zeQuX{sh9xlB7p+a_9pWH3jJ$IlgHCn<%L$|HbP`zX<@X`T$S<1M5!~tN&;z-kB=0v z0Tv1Uw6R;whGRtblb-TSoLkVkpgWQdw2zuk#v-#j#d)#CJDI{cUO0t-g#oBNvPz}Ms?!ZywaX7lk3WA($CDj>SyM}L5A0v zO_0P~&~7sp)VDX4s?`U{;OydC8t_>B|8s!I0N&8(-X(gQ1+fF(;%nyGW>B@iK~cUu zDvIXiB*zN*L%qwfcw`k&zD;a@%@$C3oL?{&kPzM#zOl7h%3P4o-Ajw`@C z-}JZMxanmz#2lHoe_ICfJ*KzqlS6_@v5L;$dVO`)@E*IPPrW}oz|KANcluywVRQAL zb7+jn{7VVe*JwgsVEGQ~;qAc%e~Fe23Ky-cPjQeBJr}-ven8|U)^mh{9q@F9BvSI( zp{iMH;6!eJpbl^0czAD#M~_6=lDq@a5`28^sUDE z<}*GDnY|zbsY`9r`wxnrIb@7x^g7SCeD0=e%ByE*87}>^bw^smy}Q5Fg5-BPTyt~kN5P&g7l^4 z+l<5DA4_yIyAXVS(%J%?ZFFY|n{86LtX!fGOdiRpQu3xAC9r(RJtqEN{GMHehx8r< zLOpW}$n;!wI?wT-k6>Ta?tlvZwltgHhvKk)fPFBVJJa6&Ygq)q`d_JTW2<-=Xw;LkyXZ@9>AP9Z&@3Uo( zXU%2)5q;q>7f}rIOMve_@}Gy)_^v}P_Gz%T%;dB`@=j37P43m$N25d~rh^KTWND&s_fP0N;y;&fZWX+0!(f zjjAir?eqKQ_5p<~tw3h-eUq&r>#=cBs8o-3x?FBA>ks4H<_v^7<>n_J{BEvb5|a^4 zZa=5IwL8!=o*7b=YRq)(Uukb9DAE>Tz}yLshZGsKa93G7=X{y0SO$2-lwQv%{CvO+ zYo091f^Y{r1LCiKY0{hpo=%g7oq(yt98GXjVJ<0$1lfjBNL9`a*xMvU-fwLOUaH!V z5l~NUBL+f3pD0hum2G>H52eG#8^f4gc-y)BUHIdyghGYI%9@UoR$_ojg+4!Is)oty zY{Qc{G)O^_8JEjvepD#_=oJWkL>cb}JM>0B?o=fh<9bFZAtPP@?n;-26m%D9z}Qm% zaq~o4^>FPs;PQm*sa_z(Yf43zD7#%c?%?T=c2}6{zij4CDpulz?r&Yo9X0*_^YKll z)I2}!E#H*kIQr={a^!9wIr9;XarNLsM8~j0QGTZEr(l^GH7g~f8IIE;?FXR8e6*en zB8I+D9Y5>`5Z`Z7JzC!cz5d`x2`<2pZ=&LaiM9id;X!TFj+afb8c&VTP+$XH(`)#? zotP5k$MJ?&Qc8z-%0o$&rV0sa5(}<&*PYIr>3AH@zfmRCtk>n|%P2=c%($eri2p;^ zj3^ohekjY2@eM2xDlHnE(4lqhef}*Rshlcnz)&Iz6#RE`VW&5E{(>UyD|_DPVDa6) z4wRMI{Q~@*8~9ihj#Ds-mlg+-shyh4W1S*OG|t>07G$EczeOU7KYs*0`Yn{p9P}Ut z8L?P@p?GlOV&?PFhKzFwC_0g(^y7DQgz>`(xqx$&d?tFMHp+e=AR2q#-uIpTckXX# z1XN9^XclVcca!3otvu@9@|LFE@{T10xg9FNYdTS)IfjjUTy=hGOltXOqs|s=h|~eQ z0tq|TDmBenT>&~Q=;(Qsq%Q`hJGC0!*VTyw z-Q{1}s3oR7=DBA|=V!rs5)Z*}MwUlPxqBEdP3@_XX8 z9~#$>IYA#eBc>o)-ina!&WemeW?svm+(*?_1S7*=U1MYahv}AiUs;I}&P?-v#1QuK zf8*t;H1Ee7XHH2d&h{LlZ2g;E%)&5~UHmxOZ{OQj*?uu(|G25U^5LM;V`dcl@M|@{ zpV%OReMrCsd#Dm%3M1xgWon$UP_vtyikPXgL{8+>jOFvEjurefp%1Zj*Xv>VXja<;QABjqkwW^;Clx~)(j~kdBoHLi zG*un)QRA$9AW?fzIp9FGVKA@|t`&du;-DNtbA$X#|1l6q+@E|f#iKxCZTGjko>feo zn!j*`Q~IC_DZ_=7H=eY8}+Yg!M@~Z_DcoTlw*1oNBxe<;BB z+yCoIkHVm3h_5c35f2Uq@qG1BdbU#i3R6%14f z_fM!&jO~g5-^N|W`ai47U+n8J*`uEyLRaCK-;wcx<;p*I|t(0hL=HXL1k{T%E9UxB}?5-hY83(RsqVsJ19XK?JF zo$re|tct4H;Y>~2=MNsGRl~%~?tc*Be|{yr+J6;~ z;IEs}dU?`dR>acHP6oiqAY+6ibZ@NOOmNR&J#XGFZ#$ME48Bw)jxvme>ifgBMr1p$ zS=H59O*S5a1sT?#W@uE>{Dsv&SIwPSs2nF1^KcYf1wwIX1B#&57vL27H9ahcP`$3| zlgvgMF-*|lPogh{F!9I8FOH3e~{GY$gpfOQ&~nQ*XLR#%nd#qlo#5e>!rK5 z?oAYUV{Y9mGeX+^D80pJv=nAPOlB8(<-lw)L5TNJ>ce+r_y4_}O!}sx*}j`yuhVKw zAW8^7-w9v}A#KT($&IKDdRdXh3$10_&rLe4-}?fW%u=HFBsi@x|0-_nh-WyR1#EWW+%-_WsYmWvT zK(c@1Eb*;99=*a1_}^g);TFt3hF=(5(EUyiji>h}LlGhjt<|rfnd}$8A3}D2uzxF8 zElJCtVP=LFq2OO%{l4 z&vpk&<#=?zw*0x0a{K(Ba+8h4zH&jk9Hg{7$bt`vJ~C)Bx$ZKlyft+Lz++L-g;Zu> zG)PuwS;%)JlnJ!k0rOuJBOo$CSXVos)JRBQsNkpEDX`{(6OCNxd+&hSmwbm}S*_9y zvSsmQ_v4JqGaFw2&;uDD{I>7TK&B>q3)nuNTF4o?fUjMT@ zi}_=)D&C^NVocWoOvWcKpUIYAkN8=d-VtoX#m8peOg1Y#Vl_t+XihBc@aGdyU1Lks zTAFy5yPAa8+TC1zew-o2@;N(UB8sK=UX;J!A*~zc{N*`^1G4q49q>a0eqU}jY0uOz zzZo!OYyKV$jn@UCZBRlGVv+9|NIkVPS@BY4E?g1s#X+^rgHyBPv*!@o62?>@(;?(L zS4|N4ZEo09+o4@!%NJxF5eYGZ!o&1RowkS|gL9O`6+W)e=VYP7cMb(G{CMvG^6K(e z%wi_U`w#HN{%C;)KaUOVO8i0{ilQYxG6x(w>N8}i=vw{d>R;^LC8FRpz7%C?d9PR! zz*k@$s{omK)vMsAtl*d2<2l(ImqO2KJ+TKJY#kZJb_dY4&lc&TI3kr<{G*MBI7*Iz z8Ggrt#|S)m2as(f{%Twa|4Q~l8;cTZ9)-|xS3^BptxgyCWJ)lp%`Dmct4J-tEf{gJ zH=4Ui4{hH>&XA04zxFQy+;BU!0feyHHh|w#Y&o~OF4#4v$$SO|fniSE+wg-Q2G38q zxBZc&c<+X8yfjsBj_OmJZTs5Dam_*&84_i$ifAGI;yiq=(?K)Qa}2&FECWR;0eR^4 zYnwgk3I@tDj5ef*$wOI!Er@Y;1~Pm}c`1N4vQ5tz8~BU81yChCG;M&Z^-YraEy7%q z#ghA9nP0hxRM6E|cS|G$YvhboDwoEh5H_2y32Eg5`B8^B+iFv2<3it_ILj5sY0=dH zLpAhI3a8Tu^1f`AnxJgPrKJfleUHzZ;_h@Adw$SksJYAu>^%6do7bYq-O@-O*tiYw zx8g7*T15d|dccP9j$l3&zQ;6vGb=lQ$)fM7{^>s46FoUvcy+F%5&t*uEdmxhr(r8H zh`wB7?lji7>BJC7$*8~Y?Q??k!sh`EFWAC&JA5N`83zw9cw{yD-Q z>)p{$1(q`$iv^)C1BDkJ!vS!(2|}?){xEm)pD{ThJH5Qg)C?NNTzIP)z8&JCItX~uKzM_rp}`#PEL#d1|y1a2Gg zRfm~(lc(#g5&2bqxP10=4ENl}dXS4RY14@wu6(Bqr`^f2fW#$WxZZav>-gU4Yh@(1 z4LrK8y{IdcU0MSJVq2 z;`kNF;Or{~wPYt02`A}%^*bMmhPtzoGP8Q)qCdK=yFEJH(f{5*`?~LXshE1}TK2X} zXF!;_Tmyiv_}Gs2b17u5-T!&+nfYCvdjcgEXBDnEqY1gZm~iC~%PBhW!*io5s!8Jc zxMxAMYb}~$8?`s<%kObHFwuyZ%C=9wJF>*!->!jYpKFOCq2YRy&yO7y9Q*N!SyMGk;f!JbX2L8k~q?jsFx%r`RDPS{HwgAcrE>7{@7M`95@ zfS{Zd>Zg&}-Ih%SQ?SvCkU)b5Y14R7vDk6CfTcZ32jaFPdbxj_V)=fpXWxYtuf5y< zF?G)EasAO2Z<@wz?4(h{#&*)!wr$&P+}O5l+qTu%c5+XCf82ZDz|1@|bI#e{wb%MA zcc^I~$Cc7{LagaLW#(h1Au^mrvm7HZEF5BQ%gC;}JQ|gmD6EBlLXb7#Lsb4mM66)G z^7cHg5u7#;szU#ZSVvH$3VNj z%mHF|bMPdG#ZMyHp9}}!xbsO1&i z?cz6}oj!48=AR6fC2u<4nMzIN$m`h}Gd9CXkH0xDlU#+STNkCpKnza-yGGCH)fURJ zsqoEt5APgb5}lFQ)(*vEiC`X!_Q&;BkkGAfyZlhxiz1nNH=f*<$Sfb&@K|$&X?9{y zT5e~iuR{Y)%KCo5V$T{vHF!Ue*spN;O+v1fZkV}US9+h=DqVK zGysUvu=;6X$)BtFc>x#sF0>oDv*a6+{P~gV%hMGL9cORG zl%@bWT8l$ytVn1WDUekJ5?i$?S{~KyHuTJa8ObdfQiFv6CxM~bcvg=oL8huNZ4to{ z7(f2tebYQ)&>LBoO*h+4N;=py8uENtPyEWK98r2XDCj(OCkov9RSz?$_VfQ?0b@or zh&V!eF5f0~0n;E2@QU)4`~-K!+Nx|6cq4Av)-35V5YxTT>GF)qaUS6!zezE&!wlc}D&9*Dgj7Vr0riCf z?n$?Lh<4$|d(CC_r1S}>a|L6(4JP?afI2+Nyq%$;+@)PiG(b3_6|+L#X0zgcX1cweMNAeg4*m-&N#=A!L+w&UK{iUM9u z2sU7uAcl12A^|Npp#43;P0dLvTXk@1aSn=7{IkTPrvEhcTlI|{%Vcm-oxA9F5`&Ab zqvr6)tU6aRr>L4 zTH&*7`nBO%&mTlTE2B>M(T!0Ms@oVu2V5B3epMV)*diGSUNGW6qrh*2>yM%+l8UZY z@F*O&j}%`Xpjm?@xAW+lL8*MD&OSOY0eApd@#m7W?$HvAJ(m^^zYC@R`f!9F{Ka?#}%sfrij z4#jZBqMN-tTfCoEhPah`%7$01Tvw<_I}?Z2@_T3Pu`2%F+h1)qB{BH(NIL^P9W*7j zQQ>Mx=1EtRYvw8`-{k}1y8Hro!Y;1OvwxqJBnHp$AiK$P;PEc$P3b_9oo36g37}i6 z1jEzrMVjwu@xQot#>9WV5qnBnPv#3+MOjH8{K#r|N>sCpt4S2`+1U?3k6ZwoFs{u! z7Ph{#7KH2d4~UOEO*HAYpj)g&{7rLj!q4!uiLqOxR9!~n$ykRgiM5(;bag@HZcBUS z{qRHV{hgfsWjVKQYZ>GdqA^!5#i|HJLDUv~7Al(X3CWN+ak%wh0p{ z7D%7smtmYuDSuu9rbh9|Q4kn8Rrjl&2;k-YDl|;iuoTh5XZ1@gZ)W8;J|YXZR8DMJ zjA+6(iuZgYwTuYQwlvlY7-aA2>VWu21&~fO4uA2L3K+TOftve^IiGWD*;t?%j*x!i zM*+5!kxHZEH-YXLiR29n;%>1n?4$0ObHr6HmUNu zH)+s@3%>+l@JEzW`W!jgQ#*{YKf1dwwPR?t+Q#NAo6LUor+buPB~-geg}Y%~uzMqa z7~%68$=LOYt4DTcoTuXTnO>a;y8_1HICxH#Qsm=G95#qUvP=ujLBrqu3v(3c1S`1j z%U*sBX_08j8&f*@mE8B%W(y^pA^~9M&?-A95Iuh<*C&h#XY8$}-hpIbZ58c(#>I4i z2d>SR^88_{{b8zOy%F3E{bbj|8-MKeXdnBLJll0sn_06SI{pZlVoV2iS*FCuJ`i5+ zqp0UP#O3$2z>4|aLsJ7pdmnOj;jNVj_1ot(>f5&#e@6F7(-ntjyN*Xkg%$qmVt3p4 zm=Qz=@r3do#Ou=2Rm!G?PteTqU0Z&-dy>7}0_f4EldT9dmqttui+`9iBXURC@h$-?E5c@$ zTfE7`jelQ*oez-ed|@@{mFQhvPo46!<-#qv3?xE~Pm=-&e!{>SIWO2tlaCT!TgEE} zLaa*Z-^)HcSz{SG>aK(i91ge8KH;}@kX!4$VQ~jqQdJ;BFiPi%rIc-H9hnzV8TFaF zm1jZ|=b=GpMRm#Bn4i-B7wrq2bJ}izV41Yb4OZq5i-e$AZv+11T*T2Ycv`N7W+y>F zOaIDLi6aI_2_BvJ9ZWgKRbk`#MbLp%v|i65`v1-NWcReMh51JP>+Hz$s+WX3uD?G& z0jY8gZr#*Z9#^kd8#FZVe@}hRalL4T`M0JrssReBJwUL>bmh}&&YY{Zkb`OQW-@&> zyQ~MjhC}b|r8GPA=rj)E7A@FD>$Xt&8@Q;wSyGqAGz(2&y_w6O5wZm1?dp#6f1v0H zGYAxsJ^i03hPl{A`bJk0YiGbN9wj{emuKBtj{^;?Lb>|Dw}ZC!G)!Uzj>b;4zB9q)0im*s zYo?TyIp~lJ&{IDP8@Sf%zt@+jO_0%bCcCEs2>kV5eHwbWv@t@K#wv~Qiymz_gawPs zT`SMlYtC!m?Oi%Q8up3=Amf<2%1TMLHXOgf;Ntv|%6U+_Ko1EI2Z8c&11LDw2quE| znMcwu8=fKo6f2`M_AaVC<$Lhq?7ySyIsEYY$G7D5i4j#=TnfHcPx#Z+php4C-QV|$ zr3a>glz-vRR>r*RckY~_*6IgTswQjT0Rtv{Bi#3*h@J;kMxI>Q6c~lfoZz__ zysWZK?+<>HZGU9F{#JI)K*q zross(`N8q+K!m$o?v1%SeZhUX)P279otSIoGb{o0xC}H2@d4nEU=PzUol1ExlY(0w z2%xcYK+xb8Iuko+la3P*P;d!7X#N@)Ek%h3Wd5bpJ&NP#qy9bDV}rA0e6w-vd3*a> zPB=1*l!>i!ez}abUI0(+;zS_mdBy6VyJPZ(c`lk|?WVY-|*ZoU>B41u( z8)YA*k%v7bvacW%TI1e1@UGh)^WT}MC1r4^qbPNryoQ&AW2k>p?`8prbC0uIq9c%H z2HLKEOWNz(q7^pnMLiU7*Z0N~-5hp-W|trA#t{wXfQRX6zrPA#D`YRk^N&f@$RjG) zsJq^NeY4)QIRw>ul)}D1fwD`39{J3S<+D3U^zwlfw zs*SW*$>`1#9w!KQe{k#7=|_;`{JGB%54j!8kgq#^BoP}LD>!kY`Xe_5BP>2uS?%Lp ze+1ds8`rN_h-7<&xYdDRAp416-fgxq*el}&$jho5*iP~F4#R{ld z-~G@KH$NBEFo2U$29tkFGNgcf@ZPM-lJ0xg z=XmIXg{yX$IwUQihf0oSk)b&r3snJQ1Tslo|2 zCF~f6ln#*#U?Nm1-PGnNE4w83k4lD?uA<|H;&7JKN4w1h*6v&1)o9Phk)D~0_<;kh zbwiTzzqPIDZB?wR2dv{^1Y@8aB!ubOGcguNXpCgvpUT_&lv~w4vw5wMsjlK&8rZ(E~`Ztq&bGWWD%=f2n}deE;K$%luFdvhzsmsEAS30bG|;|1^IdRWGL& z^+tuc{;Tsd!eOOkw^yN`L35Twg88@Qgf=CcXEZDVYkv9r0K6og@W-XvI4YU^NH0Zg zIqF;5D8Djj0ks1uJ-OysM!hi{P(nk}US$$Il`Srm2R(56_#xy(EKM2E(2f6lm%F{s zf4;R}^!yF|kX--T>1KA0|0ncRnH{9$7K9`MYxQR<9s5UHT+4e+c!qYBribf63_~9} zRW$2OO(;%*S}>nv38j0CJKrxGbHs2J2XstehLGBJ2dcMeI3j0v?V062S%%_a&T>LO z*kD^0;0l@NM<|8&<%Rn(r9fuSoD1R3wNRB@AzMmLOzqM1#eHVGA0y$4>n7$rfk+S* zgC}T2Mgw1&v5p()#@MH*1^oe3^gesFcjY-TcRwOB11=5=IGw#j+2nUeFdb>$wcOc? znZ?v7nObYKsfwa+lK#lHA4ME;Jv|_ZIX#R%S(~1B?%H6E(=-QX`-O^%qe2)%(W?b& zY}JwZ1O0OnEgu*4j4xAc+s)*FDz;y9_0RA#_bN(Fz50~MA;Z&!#XChUDYHxUn>2rV zx0WR=fXR&arezN5r#1`(@7)LlxjQ()8t**+Ag-sXs==gipwA47LH9Ufj~~3OGU(U$ z)$`aMaX>H!B)1<+Vi~XJ7M2OqmrYKBk^T8IYrO|diJ+wr>IqkjIb}3pB~*!3O4`Qj z9bJ<1{J}+;2N7mnuDv$n-}$p2uS_O{cfq@cTzZpLzAOK###Om5QvKxv&R8PWo3cn- z&Q!_=EbW&h_qc$sjlEk*O#of~TCQU4q)UsqRFJuFylvETyllyK+cdv4ftPBEMylnl<+V)c4) z3{2mt81Y2VJe338-g%0Yt=b&?)#BV>*$K<<9U^-l_(PXr{dBcCzD|K|a%U89CCBhm zB!!^1W__06QZ^Pw`|g)2m-($u>Zim8V3gm>kSr%W1{x7eJqp2fm@?Q(!+grcRy5IyO-aOxol1r5!vtYFUqcselfA8=nA8SLSIzU4w>UK%m>>X*9VA z3=2C1Lt+fshoA(93u%H523eZagnmq&QL%L7ah685wZ~-v5Txxy$IUD4kp)YzCk>8SFdtH6f{=18dyIZz9au_&AgUh~*jxey9fj<-& zQM1*-AM!5EJB9dmDM@xwm_C8Cr!bywSJ}N%6l;3!Q_~)nl4T8;zkQN&{i4})TDF0P z%K~+8+#iPtg@g9QIW9go=0D;~zdmt%*qCs*0mRF(46^@f#Tdohn_}IePVTBfEV5hG z8d#}i+!W34LFX5w2J=%C*^oWC31h|L=NO$3$X8o2G`<6#@1@m7YhfUR)eTvxnvYmp zs0wtTWNW}RzMJ`Wpd6pPKKWjNZT1ZUz)HVdbcfkwJ%{^Eka81;)Dav#Zla#u1L8Id}hb7(#o|%T^RJp^5@txf2?!H-1rV0!p&!w zd*z*}?z|)AZ#<8x{V#VZb!%DWyC;TGTcplPZS}%CNFjm*^2tnaUw@OvSHe^36*m;d@uxW9F9#HZ-TMAWrF4!kjW?|0@SIHWOxKy%Hr z5}Lr8^G6cH$-h$bBdYVJz0o4APMQkmPxcq`|s$&V#-CwV$Td=S|(m_0t3o zFz;S`Dyi}ptry(>clKG1f;&xnFe;q_!C0tqT(he7r#UsokZo(}<+;p(DtP@|04V0I z{K2p zgdCEuQ7?G=LRHK@WjUVrm5s`jS_xP1TJh$vC_Vm$F|5UGDQN*!bgO`!XQN#!U>R>Q z`l=YkUXa71JQb!QcM~S&;??*GPi+hoAD4%1o9y8n0Q~Y&o^XsTBmY@%OG>N2+4GfN z%{3BaznqbA`!vyN=i_{K0HVP!s0jtg(0J)|;w=U3N1pvCpoOXV(DSS4&HJrKh3H@& zl$x^%#lUbpoQ$zi6CcIDgKFP#rcq6+vRBs|r9jw)A|cE6(>}Jn7+LMNv%WE^DK>MVD_1QHHJ`` zuO5?=_TTi zK1}Mj*llKhxLs`uZr^>vc@p;0hokoA4S#HQVf(-4k)6vHdH@l1#;I(5ELlo(_?;(; zgy>mTf_TAaARKD)?8^ZB<~sd^)Yi1#&Sr2twGB7WX*YhJ^V%01N_1Vp19jyBFik`r zMUs}a?7i&=yQV$b=0_fS9Xb3`<5x)rKxHmzX-ISZq1;c6IU^?~8=#PFrR`-=3sKix z$?IeLePwxC!*?Imzqn2a!_sb_i5)XlFCl4{-mzFS`hsQNi^IQOm4PT(OFC+bDIEM7 z1jM(nH8@$GMMZvyV(ZWWf^~oiVR0hl#7tu!{;k+mU3G4f!Waj+Pwt(w0K0voF|hm3 zEXI}!J)rwI0Fh03j*uS~w%zZwa~G+Bb=34iZ00<3kXMC;EzI;U%g(>2khxad>EJBY zxGn;4eSd4JDN@1sd7GaZ4<={!_jD<^fHGUu^Z(HY0y8cQz4M4U;oR~9;M##@MMQ!fe0={_>Vy zBzszdawLQrnPK1=s@3!Wc~!hCf(-N*d^@=#Sub?WlPD<9ut`rtQD-L5_x;_=Ei* zpR~>&G!RW~u1fvj&m`J+eQ;4~GAMX^^$c629`f#5Ra+t4K)$7l_j-V(HvU#m`IV>w9_8mPTp%gh@N0MA$N;fMXwk!+AasAnRo!%oUzXVJ+H zDY(sKvn^i>s!w?vAKtPQXAgo!>`vM!39&Y2NK)%w0 z0QX%de1GnF_NH-Wa`g@-v58Qm?Pm22*Yu0Fjrd|Ot!BfK1?=7KNs&@1_aEV`U@bh- zSFAF)vNlWOoFiMj?OYpQwi5}Y*{X&t@o=IaH(0p4?rSNZbTx9Ia%iIH+xlq?7@zlQ zz8eP6K%8LZNJ~v?nmLqd+&z6$w>1RTa-QeUrti{d%<$ZqAsL_h+NE6f9lK397f!Am zyWib^b&A8H4K<+LeobWCBHNsO5ry=yn`)oZrBM|CC98TpwyB&jKmaWY2zU!)?2Shj zRE{5x&9hPms{BBl{+9pcnGr}xmUJQHv$epFdURLA+-?KaUflg%k~_aNOB2yWHPi$H zRG8NG;17^y4|(hxX8>CDzxoC`xv8*>SSaPgkY;t08@66d+K^p8)l{NN*j%;lci@lH zwOF|9J}ZG`u|7J{h@YjT2sb-ilQpTwg*PtAK;x|I7iobW`-Bkw2{g7Dfb@DtA89hu zQmmj^n(@eQ>}KWjDKky?QWJ|7t=L_*fKCd9y1XXvmPY7kmOxgcQyf{OVtR4QW^$v; zdAM;vcg>P554ix{`x$s6v?%mh_x;R*;g9o?=g2Ym0r=({I$RX4&19%zL(Fu4_ zF~h!Tuz8sq01k2-2dQ#=nef^n36$PmmfSPPUZ8DJ}g67+uvu3lis|ROoh-^V$0*kv=fc7!Bz75SDz~!lmDh> zQ>@pzi^v$BNLduhZ4T&0XJ$w}$e$332n z6LupLeiG5=yExz9FW``)9oPlkHZOTnn-)zPO%7lTl~R-jg}K;cj^{V1(19RLw?0h- zx_GZ@8zHSVxM+7T-~Te-j`psTPYrfy3J}_2*lhAN>TpP|xXQ5+*^-V_WzSTbu}OYZ z0(JA`Kh|Ggza!LD|3oD|}L!U_!0-{@>3mb{R>?o!-Y5~(6 z#kdGg@F{kq$$oe3?c7>9NG+EB|%?aF1i}f&;3=(-Qga>deja4ELr$7)ndGts9CoqzuI8zf2ni>H)o7LpmJl zO5pJ;bkr-2x~R3I*Fe$}wRyeXrApMX+USQZ{4dSnJdn=H0-lF7r3lYXbQkyccN1y1 zVq^L1`@iBeNH5=zrTz@R>x}#IUzmpIBWtXLxtT4Jp&zA0b4}y??JE-9LtIeX>*K1r z&(MKmrZ@_#_Jhwg69vwX;4H;L86@M<-A|)JXJ%YQdJI(=ZpDfr`dp%nkdIMpbw<{HCXfjI`8Vy2nsVVaJ=7+jJOu@#jea)DgEeS?+XqU6#O{vKcNUlsu zS2xN3|?KDSy7*AOMtM^g1^R9skSpQ^WUO zarCI5JEP$yPnPX&zKijt>HaQ`2=w0WL)qmH7fhbYp)6Y%$azAT9`}ruMTE(faHvex zcZ83Ca4XD%nbWpHx8@+J%q>pgZ5;cib~x7CF*5S92%RfbO+Ut%ie2(&g%pm`OsetXpV4hk8@XTPWj&llF*cdCjwy|q7MlVa zJ)8#^wv>=*!Qa5fm%reOR27KC)zX)+z|$lT0CLaVf*M2P?j<}tWOw3UNPd#ZH|V8( z^Zu}jQnR1adn7rk41ttjw7H~7UtmM%@XiUSodD$qn3;aXQHTPJ!OICGR zloGo}RgqdL919Keqp6x^mjKoQw>d2j2a~=<)e-vo{uBNMx$q-1qccKFt497G*%Z;# zV5}F$dVGg_6+`@0!h{{Ls2GmMOKZqz(Eg(g6vX(lXTQ(!uj)#U(f%~lDPDD`vf3!O z3RutqdD3^@JHrYVg&rdc)NiA+5g&9vlBp$pO2X6wFFkD>7iLUuQ=^=zkUSVTU*A;1B?A4o!>rH7Qk^(kir>5m*@)6Oml=NR4#39 zQV7E(chl)U#Y8t-rrSRFhx8ob!C8`*hf6M0VnL{ z*z+bXA9jhjC$cj};J6ct3^aLHGSxlA+{=yOu2dySV{fLaDbYmH|G}}epy6Es~OK*&2Ez*T2h~FG4`=EliwUnfUZlh z8gX&p4WSh&Iymhh|KwhSue0JVGdN4RRRd}Sjq&_6ObR@B zQO82Fnk(>SQvd;%tPe|eiHq`OZIe*RzEG*t<+uW=L&+Z<8%94lW14;;UP=QwT>gA)KK8$RvEdoG4pos*4(ZR*OwD?UvPP#qbiX$WR1L4EyB zjQ`!li!f=k5ihqC>&{@Bt#i6OI?c(M?{dpyBK@?Nfq7>5+ns6}hfb))XJ6vO-b69m!GwLo9cdlP1L0RKZ?7T&^ugFQ9`Z>)r; zS__v19P@rY3VO@JU-+qY^?GZFg(0LdjS``ZqKaIx)fh?whhd#|BBztf_jG~UD;=g*t)gJ4tFap^9H))B*)`t8*-+N#`K`x9$olrZ^be!p^rzEn|(zaF% z;>ng(Z)Y%s4NQW@qq>VJ@M&8eZJk8zQ@M0~>TIb|KRmnuC=gNtB)eU^)<1OkF<`!f zpdq&wKZH8CSaa83a$&q+nWF3{z@nj7w3V4`0zEBFEi`gDieWtqdyZR^mA4#-9iG`N6Iio(5$Y+?b395#@pq< zTPdRCXkfW1KF98(>!>jVO%F+k|~CpMlrfZP#G}?5&x0f%!-Jk!R#|2 z6K8pp%S~;fuxo~LevxZ9uNIO0y5a`b4b%gPg`lDAnJTfx9h=jDd6`!vfrfQpHq5wR zf^WEJeU_eGxdK5!i~YJ6&)a*Sgik9^$pZTG1v;7~x@v#fa`JBd-^RwYOqPf(J}A4P zMab(>^adYgfs~o$;ECxmf8(Ry9u)bSt-m&6^A5YWj{D)^4%agzhn4r85|bb^R(q?- z=N=kS_5PBP<82WKbvWq_g7;v;Rc}uDx`RP`zKyS~)_vH;T-vL+v!}!?sSRBJsdxC| zIdmJ`BKOhq9T?v8j5Kq#K2oLKCsGsN(KAiY8Qx5bMxHs<4~E-J2L`VD+ZYP;i?e$7 zJm5M-{Q*CM(&qcFsvg`dj#+SgKH%t>8q4t)6Q=IeBdgqqhnsz2A$R@1BCw-~;4!F&^a1VT&^IUi{ zU(^nS=31Rt|?5+ctTr33pW9 z7>LqUrGD$|F764X@wNZe(jyyyqtYyyxDWhl-e!MC>sBX7jdnvz(VM^)x|9kkCosY7 z5~-!pCcr=!+1Jn|SBbnM9fo&CGI}N!P6(779>3nmt+8km=}P%D@DS(9$z@#jtcqPkqX@$0h*~< z=05nanTl}r!VTl-A^zG}2R1d5Lzj{p#737Gi^%tf)r8#JQy%O?wI5J?b)WM!d^?Yj z#9)!{v}2=o0;<{&XjeD<3E>~Z3;EWZq=jO={z!azZu>{0lqQ#-{RN}Ntvfl)Vb7JX zQ#uWT&+H;|J}$Yi6ml(kL6UN}Tf-hjrf}8VR%ckaN#yJON-sIN=%%ZIF1F=B37aE? z<&^R*CVEi z5!nc850=NW+9E(vSy1yN@u837Q2P$=>g2P?0PIGXhsh2GM%!ulmK;C z9T3#>hPn?>6W;Y*>=zT6uG2UEbv3>MR zS#ckOdM_I}=>Uw+p%_<6Tni(wW&g`1?~b46u=edU16|Hgn7b&8nH;ywoa}{C`&ZZI zL@GJ+v)1y#!*~)7L%mSFZ-kMi<*!yu6(BgFN|J}-)QV=TL~}PsuIfjX?fcY$Y}}2_ zlS3yg#QmT~qKlVrn6e^xQ09K4o+nNffAo@^AHSAIh3n>02Ae^=%=);+yL`bZz&T}a?v zO+bAjIsR0buo{v|w%jv^raqK5+&(aihs_!0ty;LXCi}M@rH+Q@WWps8wK-N9(I(77 zzjT$NC&e$pu@appRQY+eW>E_$*ji_Fi$WTuGX{=EF;{PV_UCO3=*xSB&xWLDyM?&! zHL3|CgZOZqS0R}uH=<5De_a~C!(gV$Y@B5H>sId?k4D*rUnLel$bFdla9HN<8LAlp zU3u;>y?E|ES9R|3<30PuTT2M29gFkMmm)ymf1fRo7Fbf8NVC&#>9yx%nnV{^%|2c@PHh92E) zN(sX570t$qZGZAD--^`f>qG@eapjuogS z*z_ThLb)DK?4}XSW&I#P)6JCgzK@%S5>r=gjH+#sR6;Q7zT%sk0O@0ZNLY-qC9t}Q zCAdu?wM2CoU6#t#oc!_iK}Ql9i9`_r=yEmDAXn+zRPEsa#P&Q%VJa+=VqZ)jo(8pB{MVYG&|PT-KK}faZn0t(`K!RH>^FgZ23-mCUM8 zxe{LZso}bi4%~qyM>c-~1I$-yR>XLbPD?f+&<@StcN_0v8)=OZ_nH!j@9O)kqz7wF zg26N-)4a^*fX@U8-f@3ta3xsb5RbzAW5nSl(;{8n5=C!b6xz$5q{(uR#YCn6Ji(^+ zp&+crKH-x?inUqH4QD1N35x~|x^3CSt4@?FINS=T6i57WEs6y_w$%kZ(}^@`ZjWGc z$dQlk<>AipD42F5ntIki*YQzk|2m z2fOt07r?;#$?{^fw$8(#QV|2NttS0_Kh%=9m8zBQYt{K60PR}e^@gKU6twmhsD3`W zV@uMl!YR`i{`NfWUv4%F91x40lD6mW1OGXwxn1H&9>6%TGYkHE?H8;i1qj%6kAVCl zN%TO&o3b(hRA*Q)AfEyOHUcnBJ!uC6Sxq3ZR}jo@q>caP$uz#~zCviO0vC=yCER;y zu33(ASEejl)|p7NE6|Z~9@nMxdeo$_;&P2NzKkrID97-72g5v&X~L@|Q{{x_k6yYn zKxAA6+N#)>_@}mKI#R&e?EslGL~I!GX?>f?#PYRWT-FfnaV7%~C}|3UWN1xGDSn-6 zs4GSj8g)TI%ZeG=g~zYt;Jv6=p}A=nw}=FKOWJ)_pi7^$0-RgXRNir(caf^Yk0cn& zQTSIkaq^FWCd`f$79y3Y@yV6~zj<_L)uG87BhXG={~uBq+H2jCB}YM2Nu%*)IP5Ou z(G7w^v%XJ-4C0rW z8h>5i)dmWP?}w7wkSXGTj8|hTi>&0!&uA5xKd%0EhRgJF4<&)If0-yy#E1i@X{r7W zFwZ&#p+Tvmogv1vrAMS#wAGiR6`+a*hwB%4>)l1y-$@z<2@@3c+#_^uL&VEF^DQlZ z8ekXBxyWFm0Sb106v)tcPD{Xz)oq9a$5mzvZOZ+DK9wq;y- zqHV zznFUM8oSmeG=dkH+#z~zr=MHsKp!=GGynPhuBP+vSuHUr1?)B~EXKmuxeQCFH4|Tg z#@UQ_@2xGcKG+XTZY&*?_}`=1tS~eTN;0NW0}Q2I*fKUWk{LuZ>e$|wn{+(MV&m~pC*3!~ zwe!X8Gja(IuI#>UKDf(9rPKIkL}F%8+x<;gGl6l%k^7Aw5pWrmN*Bmin-kzh#6k^a zf_tNJ0B)};m1(-bG0c+mkhgkfb4NC$`_H$H4VpVM(gp0a2=g`54m#&j5t33cP9C0< z#TbfbbFEjx?TMG7Y-BY;Ta7J`W@h>ny!85z36&q>xqu4McbZK)f0SJ??-&sWbWtLM zM=>DvfKR_so5_iyO3E`{VlVto4N6+lkbD9y+bT%Gt1xbdJeK3oV6*#$7-bkskQde*@Fr(|< zbbKiFHBJpu?z(`NUm^I#0S>JOe#Ij_TO=+(_@{sK_!fdeRaMQK1nk}OrpL%wbyGv@ zx^L=BYjDd{lhd7pJ{^8qN5(#WhYVG=1v218vW93Rk&gcc>B1wT?oP)vThV(;{NZ;y zKzF2CccaHy&W0;Vt~#XqBYFvbQ||osbMy2&w*>gR|3&vaL=ZQ*DIS$#tAgDJ8Q*Zm z&w;!{Dz@=E+1cNBV!a4IBSZSHY}7l+baWdQ~!KPW+$5w zTr0S+so5DK06doDN8{ge`=2n1;Ezk%mp4hPlPVh5oH}_HUvwC;maNljfDSW@(Ye$|wew?L4 z=%CCm3{Jw#@QdUJ1$+~MY=1y=yC72(MClI#+f&Ve!+$PDK*gp|sUjfsBWQovFo&e( z<>&uO9>;5P#qrVFO54i8ungClx<3Ba?wqzfOnN%8=03^wkddRtu%Q0vI;%@3KmWD1 zbueXcYF8`C25?)spf9mxa#JOkouH)g*G+yToaZzd6z<8A+sODiYAgzsaLbjQY+>9d zl$P0NwM*@MdxQb6A*kx8y^6ssisuf&_Gx6-nTMefhsnGsR5#(FI|*HC-0&hDKbj!= z6a2CN-oRK=?AWe#IiA4kW;P+TO9|TD(O;}Cfe|l54pr1YzWO1v5^wIX(*qc$o6251 zz9UEdV|7e-w=AG3ioyfRv| zN?n3P7H?k3>fl`fy{2+@_*R_ILb7}Y>s+yRWTm>d;d=MJ)r*#C1T3_(cn`K@;4ZcC z=#-?_UNEi3iBcBg14%DSP*I?tCp9i!(p)+Z5<)@U1Ya?_!BxQF^xuFVVD>D8H(fVC0s^HdRb%iiC53SRR zjXz3S`v*!Zi{AyeE8veS(D&SpZ>`Gc^?wO>Tpy+SrG8x3-pKalJ(6sNK2EyuDzOf_ zD^h$o3ommlfA%P{GV%Nua8Bf_%{b0i=Xm?F8GOD;{e8l2f|v)oozDc4+C2v%{$^Im z$}9{1Rn)73hI&aXY-eIkHco6sp_mu{)=bEe5*!AGx72n=ndShnj}^`y9yO;Pexsx$ zv6!`^`Q|FA6SZd(hL4@&o1`=XTo9veM^TB4ME;xo0fF{%veyWbos9KWsCaKsKfDN;_g6u$hiCrRSvydQM4oup1? zs9}IBQ|||Uk4$I0+@H-C&H97{;pmw_7{#lN>fL3Po2K9MCJMZytWU&h1=QONDt%3DjIY1UK$ zLWpCl$Hl`}quTjV@F}2qN=`=f1pW)+^IZ!M3NdJz zr4;Ii2_4@QM;}+q!`Dz%hQIT9-R^ZM=`r=B`^s5;myPto-Qbg8Jp+mDTA?ToD@}w! zoCno}Tbb*_lk7XV2B)RcpNXJV<}&nRzJYGPKM<-^+9z=xa}EF5qjof^x>&Bqrsr`) zuTe4uGO2PIr@e(49{&U-_%2pikmC~8=%@NpIO=o*yP$(>_FbJi?tMHVw~-7L8IX5P zWw^{ChCmz%i$#+>(Wp^452N67jyLysbs zBJVeFPDiXZUc8T82gB!inb6kn45xDus1d8>TTpRUL7@GewKU8SA#jajNg`AE@oXf6 zy$%Jfr^u5!_KBL$65_z0uk zpP!rH($A}%QhU?1?rxF+)Ip}!y-txIFVcpNm{4i4bw_sB8_nwwCn~Sd{FL$Vo%o-t5WS|_uZfxhEfYDV=a{K=FgeUV(w$M+Tx z;sgBSTP*~5L&gmaUlzN+Wh2H@>4SYU%tj&OfE^Qi4OO)!;k_bDZaMpdu}1Z=A-l2pYxRc(Dq+jTfLpY9{e`N)I_dKkV!9l0G_h%u=Q+sPa z;@AyjNJ47QmqV5ruwj>Ix08%(Vf3AKcz53Z`Q@i@EWbP`^0exU)C_%xb=7hF){VN? z0)7KNYAgXshf~Gh+tS?ho+eU+js`21q;tbGL2B@}fxhpSe3ZB&lMIZ&b^tc6rOEAc!1&>xBZiY{+*4@)<+*@?K3}%U{DIhGLC1hC2fGLu6Fpv z4YyGxn((&YM@gYE$2fsWP`J~SF00h{!t|@#SG-BJwWfH=xUdkt2UC_^NlqJz?yQP6 zpm+D{aYyO&Ko1l=kkJx6=gwelviG1qCqj|-4X1MQ8ZRI=JBxc#Dr8;ttM~LxJmG8D zEG2E=1OtYI;m;bC@?u@2rA}EC$BOa}kA3PcI;#;N2REML>6~CT&-*pF5~4mEI^WP$ z-hDZU#5^#g%d1%~yK#S;Z)D4wX399R7Z!=8$%a zBRpUeRHa`U)Z&mY{zNm60GAz%eQ?&fCAFn>SB}a@Gq^DCC^I(J@hfH!M|_-K>q-lp zDY5{=a+f0GZbh34tl_upUjW7Vuj+qDx(4>Tx^A0>Z(|!x)9?h1ZQFLz*tYGaQDfU~ zY}>Y-6FYZ*&;18`J$qr!F$aiWV5h7r#tFv}yKrX&$HUw4G-~WV8bh$pd8sEG&G8ho ze2EwJTs$=LWQ`87xZIwrKFKjXB?r3Wcm?TdvM=;|Yowr$ILFA>kSHfAKfO!_v-ak1 zUklxS_3X^G+6Wo>!zt_#mdfzAUW&k1%1bb|`OTQTS9+7{!0@dgT%0PD^k(;D_V9|< zCY7QzG~Rqw2z)u58rP6bk4v|^a3^C0!OnGgu_$I@)*%PgPk_9)?j9#HWCCcM$^J85 zGvr{UjHa6dZ!Fm5u*Z9_h5;^>S#r8Rq#4tppUc|uJAK3&psZn?1I1A_J4q+m zYr@=f=ZX9eg#n}w#L+DfRiG4hKK@%qVqgDDuv5c>NZkq(Um|rcnJ&!7fy@B9v#PBd zuVt`T@DO*?6W{rQ%#S~_d(D+#7PBKpDwaL0D`?{$vp#MGkBhH(Wf4`3Sva^bRnyND zItHnh>wYzm(pN5hqqzDW49h{Mp95RYqu(VX(PgYrUGGd<yXuqi)T5kCY<)z(#5< zS-LhZm(3EqU#qPk7~XdyJ6_4&5Vr%Wp`{BUsELy%-L5+q*C=Knp`hS*OTJPNJbC!~ zlLQ=egfV0AR3yQRTrl@p5r6fRsY}y8x)0wjL_+a9xLSG9kjADny4TF*KwT>rnL3e^ zEQD~9_mvrM%9PBfE1rFNyM#tsoRosiXEE|KAhIv!Ax4|W4# zlKv@n%oL}VARbxQ1e3_4aF>#Vl{$45$;7ddAm?k!<9m@*S657OH_;9@yh=$ zE$&u)$+$=@VzYBWA5(eB`*HX!3imz8ItGWuN!+#erD8ZkZQ?r#ge7=QNF}^W^H9ir zv5(}7;+Hcy3W93<7v$taADv}8)Q1)kw7iIZ+V_ors8~X@egre|X#|uVmN^Tb9gonw zJ;n>H?5^-Gn54es$18G3!|GmVJM4?b_BZtQ>q62u6WtU?b3Aj5Bp!Iq9RV@dW zH+v_Iy?%EZ56CU0--v}2jh#>P%&huWqRyyseh-52Sde|4hET&X>ev0c>yboi@+9-u zg&iJbTr`b#OD8mknM`W5XUN*D6kNJJ)P~NmZ&mmund%0iMAA60HsqooSB+$3=*bUV|&u5W&=oHOHFe_;(H1Q`sb3mNgyvqp&xFB znyU6(Vc|3Q!q_8sCBq)zxHB^3P<)ybcW;MNx9J+H(Zu$=Sk!#fD#{JQ`373YcxJ@D z?Ao3!*K5WS$W282lD7DDwo0F=i-WoB^y)o8{Nm!2QH8Z!r6qb=o*>hg*?q&3Q3A9# zqRsT}%>Ga~8Ot&`PDUrn79B!p1`%%h@R+UO?-={q(KOE~Oio$CN;q$_@=#tA7_B$k zzq~Idx1SL$a&*59K)>NzCU>RRBJiPg@h?ZP@y)3Z-{sbtuDLv24#yr#p<={U1dX0s zJT6rW9lWoP?Y&1}vpRQN*J3^0F_fD(2lcUTVEJVYcWpGoz?i@CyiMe_s_H2GOLE-@ zGI@vi4pZY)T_H72;)WJ;#kd^$Hv3hZiuAJUBd{-}ryHQrikKH;C^-8}D~iOj#~c5w ze!eC^ZoQ?7O=y{63mwuhxeGL^bl39W!9R2m^g@>-$MTi@ms09(_g`1BvNL%-hmOj{ zXtj{|>hv*+X{ppjoM{xT_p{+!uZ`Mc2e{ZT;y|yEj8F*t+qU7`$@TsB@~PFAu`ba( zVhn|LI_qU6p;*_0rmr_w`PQIbG*XSt=IGo@VvNHZzT2}ZW7AGTcjS`UP;y;22+8(P z_GqTgI@pBk&yS86HbU8le@UfpfTuZ@Wsydzaxd2@4}V*=h8docTiPmJ$p=k3k6~q zd{?D+bv7kOI$MLjv4pA!|5p9)Yk`y~wd?`$#Yyy-nuVRs0sI!6H@2!|mq?1^ja#-H z*sQMos%=UyXQFsk#FQ69!FSaA1m=q+hKBFrg!MhZ$<9&1_E070cbfq@mBBc2{%buY zWP0f6O`xAU#iYQq1Kh}YJ~jgA*GcZ)*lUnDOeXj?u!QaMbS(5=x&&=1zB6EY57Z6V zpYr|77a9tGK%Vkfk;O{X@Tr%-723H+M$PB5IFXxpcmCm{fXy0T67-#Pw5P=JcecF( zU9DRQW}2h&JeHV;Kk+imqslKi?T8ao{+0Z z!dnd(CHN+Z#3|wfkF#rzUCZs`=F9yJXxd@$=Fq^{r&c$$;>uvs5AZLC(bJAr^6ZYU zRA7tzR)yfff1^Hr+-Ieqwf}V6v)U|0{}xbf=CxLFR(K#;7A5SAt%XZZqGn1VaSW&? zQXAg>LeVZC71ZT)Ce4j1)j(J-QaJ8*%m`v{XExWb{4RUE_!RpsvPU+?voYBP~d%RVqz%l$q z<3v+^fg3Y9u$F}y&ew;Q&Y7G_vTQuv_xt-F%|QmHq3pTP=08TGdRMD1bmMv3cYUV zHOfpuxh|~D#8>!wVN<$5oBmI~421$Fdd*U#({Q-_`R7!)iulxIBTZC8ji?UKn5%eB z$$Fhj3tG*zoF!rb*rH+@E9>#Ut2){Pi{0%OMCLdTamWKCQ4{pi(Z&D_^~}f~Qw-v< z)pv?%er)1Sf^=m3u+*g+H!KO)+tuB+mA!SCj+X#f#W}&%9Q5y1fRi|ZjM6$Kw=NNmSX1XVt~-hJ)j{>c>kdKp zN#lxySiGu#lvxIwlSV5RK*88w{iCB_t+m;!gDl18uc(#`Mh$HOLn}QdqL(kqe7V$P z{kPA5f3jFXbHXwCw+97o6n#1Yd>duhG1lmGA6ZJoR@lP4R>A3-h}YLmA(;_sNuD9~ zq{#wF!gy7r4$=5h#T2{>!BE-k^IPq>643nOV^_b>O)A>(C3v}W8%u55lI!E z2sET%?O!}=6fqksm3n9u|0^3LC8FIdmr1dNOt$m;V86cgZ6q6~P+Q(T#l`YGy8V9C zj2+%7fQxwjFyaEk!qAh+KSK1y>zv(3- zRaiKcLr;d$k@QR;-k%oF2 z>91?EB}Jz$Xl`;Kfe2Kfidv{fRgp(CxE0yC%j+j`=0e4S< zPNv=uvH#*rgtVGi>(}?Z5^iN7NyWjtsV!`|6851Pg*Q@toY?s2loero~%#H8U4 zO@HIhRT;oRtaGkPQ3LvMQ-w}021V^Ee+k>JcnSPmOJ3+Cyyn~3J>Ev5f&?x;c(YqZ zUtG8vIxw|ROX69nqmaE^QgZQUnK`fb=nZK?z=)Xy&( zkIp0xRiZ<7*b(13C=_bUdyF?qTnbv8uEqe<6@YyC&zn{lw&R@viK$#Y=LZ?LIA+|c z{XFXCiEK{H8%rxejJcw+%bOw4MZOeMj-l<5HxH1pxo}#~+FXdW|^e>6RCnT7jq(W5GXbR^>`*t0}L)`RSr{ zA<12{o(D+%c#PC~_!kJ>f~9t5O$n1iL%LfRkI=t{OQf9=$`&=lOtEQNJ0(Q@jYl-s z_*Z6cT`X3cDvJMZ`g9je2=%F|7nl8mSF5qlsV2LU6CO-ZPl|Q?zp)&927?jj^LDz1 z(`C(E=X*p<4TcjSeCMNQ?(Qx_)>(EZw+b$KmM(MSf!wTQ@QG|B>R^+Hr^spDJXK|P zMZ+`>Br&}~VaJohZD^xd8MthwMxlA*tff*ZfdSW`G+`B3=$d?`@tL#OB92%`eEaKZ z@FS{_E_-q>-6{zSa?)^aktD~=Pq**rFgb}bko+Np;jTFH%j#=l*A=f|zOTO7VXh4d zkXbsUWH@xJu#xwZN;ndTf%PZvDq;F!G=-MBPV3W=JDz*3>;Y*H%p=-pO=aF{c(er3 zn*FF)6CTQfb4jC%y<&cTWV!XJ*Pt6|wu>Spo`}3(czrR{7-q10S+cjYze=iIO8(o4 z@+Wy71*xgkPb!&2Ht$y5F}fPj00sATUrV}LYB5uSF=|Ua@FHMv$HY11ErnPz@2;HF zC|j3-m_VBdVQ(^=FP7Yr7R4$+a_O?5ZR)>I&%GKZ%=`pdG(W$d9cDLQxYs++#_sqR zw+rw-xAfhz@2QgR7AH>Xf3>`(l9e|BZg0fhw!W_7Pk7kD5B^>KOV%NA*3qNMr&Dz4 zR}04{pyNjyV#Au#K-I7n`NtyU^$(Wh)x9S^gHm3<=f4OL7n_HnxJfT#-YkMvPRg#OMZeWkx7&bKv$5MN8ovBkdMtAvH*J0|y{Vo>KmI8{`RB~bH*u%|FWKc+ z?&lZwyUnTNCgy>wLl^l>B=pO1k_A;ap@hE9cK6S~u^_ zzV3kG26gmqcJ89b7k;cYa5w-A{HvwSzB!TU+)-o4dp)^AvKFN)S7p%y^K6IkSV(YN z{uY;**hhjBxws63U?+CdmnIBZ-3nbtcyiJHOg`|sb#KE{>qzMq=AX@!kNh7u8@%_Roc+Q~dZ&KEBGA)mHD4(w1?@Lxr7_G9+ zEvn=1#DEXPJ#CbH9?H`BT!w*x_30K>WL9{kS^KW03LTEb=gFK-_4O!MGK$91z8yn7 z1ehZP+1uM2`Fe*eOG4A+Jb?IIwX1CS`;kSdAt)@dNIrB>i8}LLdOW8V@iIJZSK=sK zDOZaK$51^qV`()30r0fa?G|`<@Zz!Ia*=akg*32!-~M2muaqmmb>f8yR_q*vC5iMd z?b&*V1l>Gp3~>}21SNo%0B|3v6)!k=tV5ct===BQeij^$D z@gh%w0caF^zT6!0_`-63I z&x071PTMm@>bAD@Y-yiH76T~Vr)@d$_pfh2M0LTbMl59ghTBVHf#0KoojC+?X97E6 zfgi*Ap+Z=f)7 zk08lPS|&9~9-MI-Mt?&O#wIOM#bm4#8w{q6>d}<`4#8P+nFj(Dn*7tHAW+Qru^CZf z@un=6DXwF=eig(bY7`fAWFLIOL*LsU(jR@H>kx74!t6clBzjl&2JU0_18EP8hDQ3U zy5)7M*9@Y{#FtbzvsMm~aJyf}+~%+-0Wa-Fe!~IV>R~p@T39H0$5cPo8dlK|4HLO! z31`9C!U)jgb+Ce$BoV0r(2*BntAcQ=|Ka>y_I7K4=Is4Wj_$?b#h5NOt^v$?ClXMpOHsF_?Hpeno9An;e~IJTr=RAQ zy^d8dSJk(Vas5I@6xlP08`#5Y{pySR*YJX|oe3^zS)@t9Xrp5O^ctJBM4VVJFWZho zWUh&L=8nw;Q+ujl28zG3Kv$BuWB12QuGxgWA~e8M#lf8MFqv`Ps7Tbc694a@w24Ju zVVxnmWwk6j0*fHUuceE`=^s4D~5`DY35T5Iw+!?zKf z`o~PnM(hSLsC{?85>;9J`5&#_9hKlKGR`HlzO}QEFJyjnb+Mikvjwj*g@huuwwFJB z1@v!APeXS+)&2-~>d73jI4ZOS`u`a?jw|+x!mGrlI^(o|fnRmxT5um|OuPq4-h*J3 zH4LcXX%)x!j9w5x%M2p#_WKO#P-8{BUD`CX^nh$LZv{1#y{tSucE(6ti01y;nz#_# zVE9+x=)rL_BQMM2#EL{u_lrO9n_>H~uDS`ZxXO6E$8Rt}t{^&SY3JG5%)9KC^nMS) zpEVAc?{^w*H*=DiPb_|&cFp}B&T^g+X8~B7EbX-K_Yyc|$97mxq=X=nH=CFe70USi zu;nr=iyd>~E|xdOnzALU8y+o><)*s9SXfOVK(rU+xlkUMq`kM$R>LGNR5$c&reXW| zCZz}8K6$>2heN#BMqAzwgHS`u(kYIiDBST^Zs4bxVub`+yl!b?C{q`3lr__eY?4NG zjQGu&!!DxawhzC8X_Jx=0z@dS?qV%BfJD zAKWB|p6N_*dM&Y|QoJ!JC^s_J={u2}HAb5kTalM`=ogM~u@GBE^9@oS6NPAqgI5*H zbj?dh<^G5{CHY(b&YxO)#cp)CZC+3StS=x`RDD7&w zP+5f+zdXW73RKZ>{EevUvrjV3N|nTwCb!go)y(Zy2N5ev|Ib#~NR5!UoFH8f#M1A6 z=)F3ik~bypjEX#j-;Ll*cotM2nz9+u4048+{TOCa)KSRQT7h`6c88kBxnw8N*dJV_6-1I7HB2n;GnXoy{~ExNoIfX?e`ekHaA8+(j@?;? zAU}Qiu77IwCSz7tVk&A*pbOj&GpQS#(d`fCChmXxSKh{61GRs^h&Uu2*kDxI%L~~) zjPx0mN@?C(rw=ZyG;5-C=WqCw4{?MG>h&ksd4KxUk z5DF?Cn&@7+>~ADX+(=85?%n}Ud|6(+9B$(U+)}j^&~c89k*|8-7RArldQLdc$O2-SzOZm!W$L?806Cb99a$*UNF zDl;Xezbh6($0-5;l*j-JMTkah9RCr|7V~`1ldTHQsR~HPWnqqE@M@RQuVN+#go#;3 z&Bd?tgT^nKTv4Y6LbFNlVkI%_RAI|vOf<#OcDkp2^?CMnA+{jJ_{`E3GeShN)aWt7 zxk{SkN-+D*Lh#^C>JTqEv?&I6hhoRXeCGJ_e_7k}MN%*+VfIoy81rI&jX=J7L8+@Q z#IGpsvjTG>mboP__Iy61veU+u7}_9@aW_3x5PQM21(I(!_tW)v`E3=ecq6={&r3Vm zxaM-J_FWD7+K|pO6@1g*gP_Bw6e=k@@HkFVZGlqCall2c-Ibj*$%CyNz-h_1A@DQe z?i9Vs|KN#uyWN>&DKbIYQ;iTIlEJ0~_k)vS??aNR2HH1IUS0!)_EU%a0 z;~v1Y#QMk!dhq!|Q}R9myMY(=QAMy+JzTB4XrY_1fP z=K^oF{g^yRIg?+%cru794Ue-EPD!`tmaWu)G(JfQr;=W?d+J8_vIzeJ!2+yDyTiZHG}*X8sm?j=UJ(52*!sMM=c?Z8rUv^?A1a(5E$t8mi??k` zgpfZhI7G}}MLj!T4ASi_c)?w`8tj?BSU9s!%yz90)`qF##r!@+<6i zitja!y`JiUO&ZiWm@xKh`c6Elwp`1zf81de4*T%c@)=+o@~8+BpVxJV%iR#!k#UyV1I2tGFl0CKCGqy%8x|JdH+=&8l z?=``H81%3s*RX3SS8BzeCp_!2Pnix~)IRb}EAvymgiV^ja}Fd^R$;Lb+oH?zBiYz_yWnuNztmF!SDQO8p@gnV5Oorcga>$yOyeL(_+Tzh;=qR~op zY8<+E(@&Z&cxr z?!*wxsmzslfl~hEItm}AAIXt83+ks&gypf>g_NdtIl+iilc@^14GaLufz%}$z7X&y z|6ZkqbPiLj(@K;0It6f4uDs;o5$b+I4Agy`@Tt)Ca{=D5>B!SgU6lnOPWEW^b2!-! z2N937*Kr7V^^u>CCoe&Fvs>Em6Ke9m6fE#@v{Sf5!<3-2tr{i$JKgzS@h^5HKaL1c zX8w?w^|Q(sev#MdJl>KLQ4}aW4WP>e*Dua3Sv_()PZ2ikg13QEFE~34zhc}WrPy)J zN0sKbO(Mkh3+R5;`zez^lBf^M3i&qLds(5}q*^yaQA%7Ez zi@AFmieK#Ff{?{zy$?&u%9pd-18?`~p&>ZNBXqKmRcg7IuNV{=eyJ!v^N7Az8iNzbmb5x?mtG_#Pv<>1u+%2(L5AYm+$0`je1eu(4Hj;_h?EY_DwI(bB zg_w2qXf1a2Xf7q#IS>+Jj)H3*Mz=7q zq}awTy}xaSKHjI=^jTol{)SMu^Ab<`+IO9#wjKG1okRFg)LV*z7d$7W^=}GXdzjh6 zFHf?ODFoHX4+pvxee=%xWs+so{)5p-zno5_rcf^d#m}iv@m@`sS#GjjXoD|RI&6~5 zRsjYVS3%nS@Nbq1CGAyOFYvW9C~JEsg)rK@y-M#nX3D_En=%{_ta%CTxy9s z1J0+tNYVAo3)=eAc{@X)@vW_vLs7wQm@uM=eE=irSyf<(1xKR`Gh ztk7!rFxOEkcu@ma9A{^pzS?kHYpYT$^m}b`_Ba3AfUC>N z!NWa~)(OHQrX|BtIW8^X&_fqb_~yl;$Udby0{G^K0a1$DN}s3(zR)zG>|he1n$Y1Z z*j=q=mj|z*1F%$JX%J*hdz4UL<+lCCJJ*`5KpZPq8~qUe?#7YaV)bI$@0#ytjlZU@ zax^BLYRtVel{q95{XBAuLsT{n*7RgAks5nS-OnA5&-STMIb4+pse%HzBx)0lBUyCq zM6$AKM2vgZrP%M)d<#n>teJEObhTe#zSHX8dB`!yXi zeYq1$32pdI$pIR=E0}s|&TGZmaLdbw<70lnq6?S`W@f{=RyQ=%3M}-K?~kJtCQBEY zdsEc4c*furA@u9wLQDH9MF>NII@Me$>tiVnzyl~x*454>3^$Kt7BYY|+BHb;Z%eG$ z=f4ak1};7(MAEi7G1n2RDQ{AeQ1D+`V=9;HalKO`K-#5!vqPDMCq_`E5yWwirkpa} z`t&1=L9|E83*XDzCtVqjfL1Qp#i!1f->3C5=Ra;oxZf+*BYPVmsyQ&1+G4J;*QCpi z-$gmR7QTYW-mperQ@-mH*%|X*yVW*#zRTdt0-WbX$_xx&xc`_Puk#%_Brb1;`Z7zb zT%ZlsO$HbH?)v4WZ;8r^MDxRM8tw*qc>bknOZoU|hyf%~#!g55nlyheHS&Kfpqo>c zzjR$We+F)ONqhQFb{o$c_)j1wREu9RQiGH3brJM}F*=(bUgx+(CLEh8qby2a;Awnih>ZnsZC?NBllgico= z)QqNf+W2%$2fAkq1uYure_gz*)~?aIaz{;hp4oPtFLC5#hNEVkeqPLyD0)=Cz^bi_ z&HA+r)dX!op1;_J&2D=%E2Gw=3@CFZLq~b04>VeGF2VkTit`fWlIfU!0a0ngMC5^q?kv>lKW}Jq1Uui4~9NWFj6} zZg+I^mV4F7t(2@2jcmP>6_v*=Er&Z=r0m*O0jE7-kB3q$LEx&A*EsO-xHe8QRn__4 zF|fCyM$DhV=ox*n?!S59K&nuTi|Q9!=q(_7y9cb97g#ft+Uc}}#9Otd8Pn^Ftf$2y zS6b_Xe;@akhr>KT$TXGTrW|Dn(3$Q7Zt%P82`0AjTm`9ISZXM><_qLs8>cfv7&z@C zLw)Nd5>r&6{Dzv{6(gzk zo?c4dgD*hYlkVO}SK zc=!!DjZ*e~s6SKld!a?1pEW9PyW=h_PG0TGgegD6+xdqT>WRKu+(6b}khl>SqAY9i z$J=J^t)kcixhxuCtSY7zcIXgMq3bPa(}+l!bWtF%&nY>mfMCOyuobcKNFVm{%a z12ATQIz|QF#p3b;)9AK_70^9KsvN^2=0*;P-jHf+W#)qvQ%;izckjYK9N0kVu60;^ zR>`kT0p5eVu{rGH{7q?n!V%YDAbEs zV4E{;YNn*Av4m+0hssUdecqS}x@COS`z)13%lk+CVqKtkZ3c)(Q}_>#8KQqq z4V81S8|jV$b@J*#Q1JZ*FGWUKNaCT{F&WQ05u--WDUaU)e&vDL)5Pj~-Y)4&a1Yfu z|C6;apg@XS$Dw1sNbTlljCtLR^0RXo{Xt@Yj;T!lo8sPrel?ozf+Rux2|hZrR;+@a zE1#UK|8YpX?W;a+CKy5*a(LncGfL(t4TW$<9G-3+B1nS*eA2fLq3Xp5`y%=(Kl^WM zY3?iK#JC|APw3O7ZH#mMSgOBcAlWhwP_&Y~%5e>lBJObUqu9s?aA`vhv)ltZkKU-| z_#8s>_v^a(Qd5)DaVUw~GOS9e;7!oq3UX83Tx076(k>p$#VhI10&bSl7-zb*gCGWi%I~tb+>Z; zfJuN+R6LG`@`x3ee^sl)qLZ{7rE>fju2?F+Hy5yus(M8k8l(HQ^sajeA%qejs<~`t zaU&8Q(wr8^sstiB)oYga8p7?c#zH19UgE%Af2-`Ui@l2x-!MI zyaSncz05R=^X3*Ik)xtYrRl@h)Cz9ly5_>M+C5(g<>2eFUmJU_t#!cb?X4gikJ&zj zf9Iw*Pim(|V+z34LPxgce2dO73`i5$(tg<$D8tbkbRWQ%$!U|-{+>rm2&2a8ONbA8 z7`Ji37F}!xW{aqe)ESMv^E737C4bc#D;_ku_bBnfUbH07PfzZ4JrFoRV+gekdop(O zHJfjkOIJDllGiCK>)D0X?Z?r%V!-JE)Jb2@MTvCfsL}H*_GC@r^J(6NQFo_0Idqga zm-gYiGrHyWmiyZf7K_!8;yU-9>Jb#E!>og5zRM7Ep%ha{b(uz`9P08vC3^k*&&m~P z{#wVOS#Mu3c1Uah1xpTQjHQtfkRN30zD$*sA#AJT+NX_- zJUn>rBX&4q8Xy}uUlQN!?Pc{A8%P9|-DOWAiMDsp73h!Tu5ib}%ll$#I>7z}l71?M z^!b(pdOd_HVIKVwD@Ay3Q8eHuU+1NN!*2-M_tEy;&}MgzAt}$@KdcW}d)`mQuo#b_ z)oWOZ?`l_2@lLa(@p>k;$7DKpRfD8RQzt;u){?PV6(e^KVIO_t+tN*>ppt z>%;_MNocR_vHP?jFlSrr+$_{rXIeVxc2{Rkd9UtfuYWFdX3F1!&-*P(y&oUq0jjzmcDW%@HeR~H& z$$@x??TDq*ME|iXkR3autWtT*^dgYw=#F@VVMrv&srGg*Y(x}nv8tX zt_apYQifYwJ^&dIobmE{V=uj1Va!U^Be(F*-Kv4F!3TQ3w$GMI@Oh^TxKx2aGyr6c zcsqC_jJX;od77kLF(Ulw14A?}-l8k7e*DkJ1q2<%(hgu?gXPa~hB?Ek5JOD|aPl+l z8TYVa)Ov1NQjetl`?YmNgO655%+_w|TxE9?BkB$EnVv zHjqrrLEgS>n8bZ1M;iUmm^E=+cp2j|L`Y0I_XKg&+@-O8)Z@XSl*=6eIBgMo`zx_C z#1}#kJ*7-I+;M4_O-gJUK*~89uPzlHc|eq)8l*SICI!(I&zYYtlChZaXB^&S@u+{L zmc1o8k#bqln%;GOF*$vF_C_P#t1>u8LZFeo?;TU(ZK6@(K z3j6AcMrb|&|JgYtc6HT>yD-UjYI9t0-w$1VN_MS(e1bp*@cR~JUol_rB79> zBag)`Yrc!S3B)I*0HC>RcoceAd>5KjUet_I^;q#=A4?jS@oBdVE^!p^MhTTEN9^S( zp&Kkr0fA6X?fcn<^k(y!9T88%4^yed6)R!OCH+vhoR3B_m(K@j0AkobXtQY&gU6?` zrzJ_IQA?z`4KB3nvz#yXRV^dY3+*cQ(S_Sv)@hY3y{>Ru=U_;pSA5xi{zV`;PZ@*vif%^B?fu{TJ&DrQ||Vh(&?JwDa) z7P+LdRv{@4x2pLi?54Jaju>>EEW7&l-4>NQJTkt{Ul6RLxZX^KU0Xns@EU)+{&s$5 zg5G8zO_XK0Ro`$lSiYH$#{uSfzNOLfIF6osSojht#j^#(-&rcBf7cLPDNFnv%OiA&&-og^rv%TNvz% zB3>7|$L3s^X;q=VrsPUwvXDNJTNFqRY{`hQwtrgZycN^Uz#sNnGULt zy?F<&N&BU9 zrMP7{%(na>Snx~gVR9`iT1pyqU3X(7t$Yk9Dfjvt|3+v77rmlNUXXB3bATXzDSteN zvHjfXozc>ux9nQr>R)>~clNc@=c>YKMoDyR9B1M!Z{=023h3qNo5$<0r%!X_{~qi6 z^+ICn3d=Q8+2^RIB1CvGmOXX)F+{0cGRt5&*X~toS@%jcHNnHq|^3w9Il*VhO(fe6H+4iS2vv%3CzR{OK zgg}%(KT>)rHgln|O^x0{B_AjpHa_lq+^t0N_O@+cPkVqlNj-ep37Q>Ow3g+pVliW1 z;Q#*aNqBk}U1M!!1qYjVCamBGPH2Ayh)tMb{yup=(Kp(fRWwj|HUPWE zk8Nr{2-);(hdYO(GtZ}RgITmy=!d3OuDKXc4efc}Z!%Tm10i;zsQRox1Cf|$Au0+N z(lJj?sSSTA`Cy@{uKh#}&X7K_;0jno_@Xkm~r4oT}tzGiQ;-Mg^x# zkx{};Xllw8i^LQBw^@wFX^9l86=5edb1d@xCrOw?kp6hyNnUjD&p9!dq6jFhC~^n0 zKxhVS`2vNg2xh=ihu}oRa*=o92K?x|?jFvk+u7FrX9)Ahuq?mbqr)D^su%Lzx~co8 zpaA}g2{=Niv=>8*+ENNlQd$9P@}`4Aa8$ps7(q1Oo1_U&&P1oEnvJNa&-KS(K>e@Z za>#xfZMc$U9_2M9jD58v&7SZ$oo1txBP$}6iBCls z-0W!I%o?^I+%sVg3gK=wQu;62?I%{@aS1MZ{~XxZ-n9}~EfWX}{YRy1 z_J~}8D&lzLQ)41K<8e{R9DC69Ildv{cg1;f{hLM}GFFHm7`gxgQjV;jtWUpUb;p*6 zC!}v($wjt|EF8`#g^V>e$2>`Uo=jdhoC_Ci~FOd z*Pp2ltKc(}xefuq##!|k;iN7Kx0V3+X}+JwO_9ijpGdaibY-i=V(^m%`bF#?t0GYk zrA3nsG3_{6dE6IcY=e1`lB+5IX$*nGn8h)% z=RRV2Fj?_{GPWn}OcC119+S*`b;&D{chVssu&N|Q1Xa+xzFq2F@a=V6a|$RH$0eT^ zh+U26=rxImQr6#}SZU#jzv$b|$C&4}|$H`ZEQY zHLxOYIV1FCr7_L=Pm)SWP*29LOc!$kUoI_*vWYBo+AAPiECh9X{961d6J_S(5Q*KA>$Tk)0@*7*ZDb zs-Zr^M57|o?OcXe)yCVuhuv=BXY%YHgL&aY5%Is`8EIP%%z!NuKOK=>vcfW_Yf7V)sZ#>nPvEBqadqw2IQULa5V9)e(3=1h&&`80(dU#{`yyt9H7i`LR3qs+pXt9zkO6f2!ZbvzggRv=Ra7 zJyC7~BzArkm@UJTze82ybR0kH5k}sXN7IWEVFcdju?joeXuuQ=!Yxmk*5~+s)H)=+%`o?iic#Vy}GAZA_)4Oc1nAded9hteCod8#+f zl^s@SK6Hus;b?BQZ~lEq0z9)Frc3UlhW zI~>4blgPH?5sBKHoNf;)KJG~JIzF2Si{V#2Qkq2a+<$Dmdw+2M=nw&WO--!Hgy|&u zDC~NpdWNDl3@{+0!esqST?!gPEwIWQHHNQnEU;6ZlKSCbRnT*|gB#V}jqxFT8GK0H zN87E!&3!~cx0)?rY*s;?h*hl!wha$=3`NRL4=(Qj%f?E2D*-Hx-u zC}kW|y~ZHwA_|SDRMxpfmXD=kymt1HV+_7&)TTNCLUhAis8vd(*6Pm=&jTtOw~P_9Gv!_vWYDZ2oQ@@`4BU=SB&A z!*+^VOGH02b+3<`%xkNW$9sN{%>cHx-ZgG*0!ol3@qX>z-+xqK_X45E@BfVR zew-2Vg*1LZ0fS!3Q%sPO$h@)G#^nIRL-EUwi737VK&Jgjr1d0eEZ2)z>|*=dNjMX4 z83KF6$#}%L13C5y=0F>(OB+JOTiq6w2)Q(|r^E`;dwRm`3TEd?-f@Xa*gD!12G%iK zmjXLmENonAtASS=OgX||B2p~sdz$No{@MHKyc(xx<0HA5)8$%$C#OtuLfgKiEgEvT zQsj>KZ}f&c8+q647JEz;`}y1dn(+?I%L;alT85)Nd*5etw?j~9wz=}=mU=;u4x8<_ z_hvuo4OE;6K2PK@6X~S8Q@YsK$hWAsC@U{jGo2(cLtikRTH+?byw?6-+?hHGnAP8C ze_nlQNQh1GCX9PMi7^V`+ETt_wWjoCvPW@)alEzSxE~m3TWcj1Sg@d^XZ&7Rir=sJ ze$-9&tcc;CKR9sd))yf^0|X!4GIhYbfv*=*O3@kbqONm9(_@m39De$-ED0$`-IJ{_ zIF7i`@6lYSr%-AiFArLD5;AP%g$H)rFy2J2gOF1O=Q}VR+L_f0J3zwjPd{uiM6N!` zKVq_2ZG{3nTcEzTOa5UGhmw0bW`sw$Vb)_bTT5j*WwiNfqIX-xh4UA65^p8-WhsJ1 zueNCLY>rxQuzgLWWtRLTNl#XErjW)2(t5SZkUWI8 zCVGJ}r#|~gfQ(-)L0hUu?;pxuEr`NWDc1-JSw6SwiS$N!+_8HKFT8i&Gm#_}rnwVB zMuB*&4ZfyM-pO6QcM`!D1jk{N;+*Uqv>g}AhPiaKB7kb@)Y^F@M3Bq_Sk6y{~Vge4>U=nQYEN*Ox}l+cX{2Im4(v=Zx5bQOR%1EfIY zUkmWTzC<_?zDME|?mj*%K!6-8j?HTsz&tMR?(~JafAHYx1i20!Ht=WWi0~Dtwt%73 zGUVfiwD;fs_XjIJ(`!Z7ej(t|LH~k+2r$G4i$6^*hMi-HM8^Xkp?_VsO3Ux!ZQ2W0CHw?)09RaM|D{R?kUA-={ydf|9MHXa=gMR};!l3W3 z-MWQmDwO{yL7Rl!eI}J8CEs~kq(X+B#@PE-0y2? z>be6mE1eN2ZKP@KUB4=g?bLw6gKesUhUbbKn8LD(vKZA1D@$WLH=uqEs)?+OwloK$MUENr8kRS>FUGUT8s!ozq(YbI!WMfeKE$#- z$kLAolDrc|8_B$Soi-jjOMmS;PV@JiDAWTwV)EbVzB zeXw{3UGlP6ds_(&^bMzyIe93<_jlfeBA7Wm&RZ*x~WCKb8F7NkO#*aZ(X&_w>NtTxP z4^xTFNQExR7sjk}byYzVRb=I%O9->*MNsf8?*33H$z9_-7O zE-BJ@mM&3@DzI`#+Fc?3r6@GWk17WE@_T&6&kIuFV=o+ACMj9jVznjZ;uT8fb13i> zetqHcHSu#iN93_DOrGp`mNhE|)5EQ6Qn%U>6kR?j&syY`ur5c)*%y4A%P-@7WEC_? zK9CAsa!>F8RD=bo!QNu&&QgX4CY#frVc>_;+)yhaVZ%hB&#rnHbG$-TrDNey`72=vHlbAvI|4aN%sp1GbGh7F_uh}<@TONFi zuktx^2wc5!ORSbavRE-+ykIE+X<8Q79I6G!8;gZWw14J%7e zb*Lxy$B{tD9;ZjXZ&2FjSWxo#BYtM*S)@fS$v3vQK8ZvJ9-Oexjy?8XEd5wo@Ze;Z z_ZO7P-ZN>mHa>}#95_YWPo@Zud5OebfJ0mR8sXHbW@UC~WH}{%Ad%c7QlU#63J+FL;{JSW!N>Pl8t{N+S383hd6_N3#Ei@I&D#C+^QOav z){#!)9$;^LOl&=RxOFX&$qkC#&Ss6i0Y|rWEbp=)RqJJz!nU{}aUxQoOPmG|V2EY! z$)_V(kXfZN4`6l|1cGw^Yxi-Qv-6lJ?s0|{eU7a9CUGLzwulHRO${T;QU8Y3sC(Ti zB5R(#7?t=*mJKY^x&3cqk;_gJryv!&#OWBb&tab(R{Jw7&3I6=yY1b$bz3-MA-(K| z_#~RW^QhQ!$3Ct^a<8C4##9KV!OfznYwbv?Qr4f!`TOeTr`LiNIcA(|xtUMpBPtvw z^0`a~BXKHHp-Wr_4{~U4u-e=6@kJJ-m_^u|-9g!K&_eF=b*$KDY&}9-j;DyiC6`=W zyhP%;z=;s~>O=fWQs>&0MZ7pNxuFcKj}3LlE#Y>Jfcpz9$g%!8x9fd;yv0Y^ASEtA zDs)MKz=IzYdv!i-$I^qPBM*Fj_?f+6w9L4YLx+;nX=lPY+Ik{|ww^pE_TVKFOHc%* zRa7W7uUdf`RxU>kBg#@hN!#PaE#Y>IY;Mbqf^2R`1S6N#B`!xQbV-52gCV@*p~%Bn zupB=!zlt63-xA*P2)o*NEScu+Iwm~k3v0GAi2+#Q*b5)tB8F;(hKN0K-(qlVGrl1o za9cyj+2<_3v0UJGdzD36jy1z6s$3x5=Y#BcKKHbY4!B)h<3k3^7M35movr2L zb*aQ9?v7OG5|4rhTPXKPmd1SEmXDA+JlNVFB&-4U+Y>Haq!VdbA}lU0IfIV#`LWb2 zkqTAnch>;R8l8!m+llZfu#>`d(?w3>_8D?sT^_N z{w0fxFtMOg#YJVZTA{(BT3Ow&GNSUFv*5OIi4Tb^r&-ppEakRwh(%iA60blibct7j z>~uU(SL8vw0>6Zkhv$4-zIIRh?s;+ZcCJw5SFYcn%Q-j2Im(G1OUt6utn5#*oKC+) zNg0>DvKBlPH|o1bg!l{VI--ofSSGJlF3?ye*0BE#@#FLSWMQ)he*T$!Kz#U079@H} z<~ESVBCT+V*C7?U#OvWf9a;nK&al+m^XsmB#NK;Z7R0>!@&NC&z5w%&|4kOd5gAAR z$^C~iuIAiP@X481=xk>8ojT*FD~=_8WWBqcn=6jD6z5nS5TZOc{(XDEKD7`ZL%G%> z#@DzVa56)gP-IlYQM@kVLnVveCu+Bc2m6U*Sdifu$;!JbD{olh;&1TZ$KT=qi`UMC zzc+3xLdcL!2utUy_}^a0*LUy{GpE~pl$N-}t8s2GlSteYV}AK4mKc^=EH#YBnmq73 zZ{62Mpr8?A2XA|L+9O#GUYpL5*B+|@Q2Hn_i`R&^hr%~$t(+UT`P^KGL9rGuY6#^* z=SNsq(IVKNW`)g1!(O%F8fvMdAPEw%KuO~qR&_}vWD$R%U}0sA3~li&aV!UL&d(0# zr}-#7(j|ossn8{b9##yn;7jnaBrAddJ~roLH$EaAZMflGo(CocYB<|kb|(Bxpum^0 zppN)Hetm_{ukm>fi?qNcg&L{QB@!K2VU%HM#L5EYUmNi`lHDM|O)yIUUoXl>iG$$# zi}Zbn0mt5VI^W+EzP6vw5lXhp$H(V9A0-lr3R0mhX2#%S&v4?;G~L;dOqP{loyngyldt1-s_{CikKtOn@f^$NvKo#=;zXoEmq-c- zR(7!JQ5PO==l8L~!#R#caE*_p`8xLSq4Y5kFr7o$m*(@5d>+6eLho4c8jgLUIvxKU z{ks(Yp7HffKEIRNj$gxShV_hVa4y5~B8w^fOYCE_VBZ>II)@V)yvSjR-{QX`&iR^n i%|~fvOC&Cb=>Gvz=%ENrdwS*o0000 +#include +#include +#include +#include +#include + +#include "imgui.h" +#include "imgui_impl_glfw.h" +#include "imgui_impl_opengl3.h" +#include "implot.h" +#include + +#include +#include +#include +#include +#include +#include + +class ImGuiVisualizerNode : public rclcpp::Node +{ +public: + ImGuiVisualizerNode() : Node("imgui_visualizer") + { + this->declare_parameter("vo_pose_topic", "/vo_pose"); + this->declare_parameter("image_track_topic", "/image_track"); + this->declare_parameter("point_cloud_topic", "/point_cloud"); + + std::string vo_pose_topic = this->get_parameter("vo_pose_topic").as_string(); + std::string image_track_topic = this->get_parameter("image_track_topic").as_string(); + std::string point_cloud_topic = this->get_parameter("point_cloud_topic").as_string(); + + pose_sub_ = this->create_subscription( + vo_pose_topic, 10, + std::bind(&ImGuiVisualizerNode::poseCallback, this, std::placeholders::_1)); + + image_sub_ = this->create_subscription( + image_track_topic, 10, + std::bind(&ImGuiVisualizerNode::imageCallback, this, std::placeholders::_1)); + + cloud_sub_ = this->create_subscription( + point_cloud_topic, 10, + std::bind(&ImGuiVisualizerNode::pointCloudCallback, this, std::placeholders::_1)); + + RCLCPP_INFO(this->get_logger(), "ImGui Visualizer Node started!"); + } + + float getCameraX() const { return camera_x_; } + float getCameraY() const { return camera_y_; } + float getCameraZ() const { return camera_z_; } + float getVelocityLinear() const { return velocity_linear_; } + float getDistanceLinear() const { return distance_linear_; } + + bool hasImage() const { return has_image_; } + GLuint getImageTexture() const { return image_texture_; } + int getImageWidth() const { return image_width_; } + int getImageHeight() const { return image_height_; } + + const std::vector& getTrajectoryX() const { return trajectory_x_; } + const std::vector& getTrajectoryZ() const { return trajectory_z_; } + + bool loadLogo(const std::string& logo_path) + { + cv::Mat logo_img = cv::imread(logo_path, cv::IMREAD_UNCHANGED); + if (logo_img.empty()) { + RCLCPP_WARN(this->get_logger(), "Failed to load logo: %s", logo_path.c_str()); + return false; + } + + cv::Mat logo_converted; + bool has_alpha = (logo_img.channels() == 4); + + if (has_alpha) { + cv::cvtColor(logo_img, logo_converted, cv::COLOR_BGRA2RGBA); + } else { + cv::cvtColor(logo_img, logo_converted, cv::COLOR_BGR2RGB); + } + + logo_width_ = logo_converted.cols; + logo_height_ = logo_converted.rows; + + glGenTextures(1, &logo_texture_); + glBindTexture(GL_TEXTURE_2D, logo_texture_); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + + GLint format = has_alpha ? GL_RGBA : GL_RGB; + glTexImage2D(GL_TEXTURE_2D, 0, format, logo_width_, logo_height_, + 0, format, GL_UNSIGNED_BYTE, logo_converted.data); + + has_logo_ = true; + RCLCPP_INFO(this->get_logger(), "Logo loaded successfully (%s)", + has_alpha ? "with alpha" : "without alpha"); + return true; + } + + bool hasLogo() const { return has_logo_; } + GLuint getLogoTexture() const { return logo_texture_; } + int getLogoWidth() const { return logo_width_; } + int getLogoHeight() const { return logo_height_; } + float getLogoScale() const { return logo_scale_; } + float getLogoAlpha() const { return logo_alpha_; } + + const std::vector& getPointCloudX() const { return cloud_x_; } + const std::vector& getPointCloudY() const { return cloud_y_; } + const std::vector& getPointCloudZ() const { return cloud_z_; } + const std::vector& getPointCloudColors() const { return cloud_colors_; } + size_t getPointCloudSize() const { return cloud_x_.size(); } + + float& getCameraAngleH() { return camera_angle_h_; } + float& getCameraAngleV() { return camera_angle_v_; } + float& getCameraDistance() { return camera_distance_; } + + void saveTrajectoryToCSV(const std::string& filename) + { + std::ofstream file(filename); + if (!file.is_open()) { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", filename.c_str()); + return; + } + + file << "x,z\n"; + for (size_t i = 0; i < trajectory_x_.size(); ++i) { + file << trajectory_x_[i] << "," << trajectory_z_[i] << "\n"; + } + + file.close(); + RCLCPP_INFO(this->get_logger(), "Trajectory saved to: %s (%zu points)", + filename.c_str(), trajectory_x_.size()); + } + +private: + void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) + { + auto current_time = this->now(); + + if (last_pose_time_.seconds() > 0.0) { + double dt = (current_time - last_pose_time_).seconds(); + + if (dt > 0.0001) { + float dx = msg->pose.position.x - last_x_; + float dy = msg->pose.position.y - last_y_; + float dz = msg->pose.position.z - last_z_; + + velocity_linear_ = std::sqrt(dx*dx + dy*dy + dz*dz) / dt; + distance_linear_ += std::sqrt(dx*dx + dy*dy + dz*dz); + } + } + + last_pose_time_ = current_time; + last_x_ = msg->pose.position.x; + last_y_ = msg->pose.position.y; + last_z_ = msg->pose.position.z; + + camera_x_ = msg->pose.position.x; + camera_y_ = msg->pose.position.y; + camera_z_ = msg->pose.position.z; + + // Dodaj punkt do trajektorii + trajectory_x_.push_back(camera_x_); + trajectory_z_.push_back(camera_z_); + } + + void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) + { + try { + cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8"); + cv::Mat rgb_image; + cv::cvtColor(cv_ptr->image, rgb_image, cv::COLOR_BGR2RGB); + + image_width_ = rgb_image.cols; + image_height_ = rgb_image.rows; + + // Utwórz teksturę OpenGL jeśli nie istnieje + if (image_texture_ == 0) { + glGenTextures(1, &image_texture_); + glBindTexture(GL_TEXTURE_2D, image_texture_); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + } + + // Aktualizuj teksturę + glBindTexture(GL_TEXTURE_2D, image_texture_); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, image_width_, image_height_, + 0, GL_RGB, GL_UNSIGNED_BYTE, rgb_image.data); + + has_image_ = true; + + } catch (cv_bridge::Exception& e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + } + } + + void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { + cloud_x_.clear(); + cloud_y_.clear(); + cloud_z_.clear(); + cloud_colors_.clear(); + + // Znajdź offsety pól + int x_offset = -1, y_offset = -1, z_offset = -1, rgb_offset = -1; + + for (const auto& field : msg->fields) { + if (field.name == "x") x_offset = field.offset; + else if (field.name == "y") y_offset = field.offset; + else if (field.name == "z") z_offset = field.offset; + else if (field.name == "rgb") rgb_offset = field.offset; + } + + if (x_offset < 0 || y_offset < 0 || z_offset < 0) { + RCLCPP_WARN(this->get_logger(), "Point cloud missing x, y, or z fields"); + return; + } + + size_t num_points = msg->width * msg->height; + cloud_x_.reserve(num_points); + cloud_y_.reserve(num_points); + cloud_z_.reserve(num_points); + cloud_colors_.reserve(num_points); + + // Parsuj punkty + for (size_t i = 0; i < num_points; ++i) { + size_t point_offset = i * msg->point_step; + + float x, y, z; + memcpy(&x, &msg->data[point_offset + x_offset], sizeof(float)); + memcpy(&y, &msg->data[point_offset + y_offset], sizeof(float)); + memcpy(&z, &msg->data[point_offset + z_offset], sizeof(float)); + + // Sprawdź czy punkt jest poprawny + if (std::isfinite(x) && std::isfinite(y) && std::isfinite(z)) { + cloud_x_.push_back(x); + cloud_y_.push_back(y); + cloud_z_.push_back(z); + + // Odczytaj kolor jeśli dostępny + if (rgb_offset >= 0) { + uint32_t rgb; + memcpy(&rgb, &msg->data[point_offset + rgb_offset], sizeof(uint32_t)); + cloud_colors_.push_back(rgb); + } else { + cloud_colors_.push_back(0xFFFFFFFF); + } + } + } + + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Point cloud: %zu points", cloud_x_.size()); + } + + float camera_x_ = 0.0f; + float camera_y_ = 0.0f; + float camera_z_ = 0.0f; + float velocity_linear_ = 0.0f; + float distance_linear_ = 0.0f; + + rclcpp::Time last_pose_time_ = rclcpp::Time(0); + float last_x_ = 0.0f; + float last_y_ = 0.0f; + float last_z_ = 0.0f; + + // Image + bool has_image_ = false; + GLuint image_texture_ = 0; + int image_width_ = 0; + int image_height_ = 0; + + // Trajectory + std::vector trajectory_x_; + std::vector trajectory_z_; + + // Logo + bool has_logo_ = false; + GLuint logo_texture_ = 0; + int logo_width_ = 0; + int logo_height_ = 0; + float logo_scale_ = 0.3f; // Rozmiar logo (0.1 - 1.0, gdzie 1.0 = 100% wysokości ekranu) + float logo_alpha_ = 0.15f; // Przezroczystość (0.0 - 1.0, gdzie 1.0 = nieprzezroczyste) + + // Point Cloud + std::vector cloud_x_; + std::vector cloud_y_; + std::vector cloud_z_; + std::vector cloud_colors_; + + // Kamera 3D + float camera_angle_h_ = 45.0f; // Kąt poziomy (stopnie) + float camera_angle_v_ = 30.0f; // Kąt pionowy (stopnie) + float camera_distance_ = 20.0f; // Odległość od centrum + + rclcpp::Subscription::SharedPtr pose_sub_; + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Subscription::SharedPtr cloud_sub_; +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + if (!glfwInit()) { + std::cerr << "Failed to initialize GLFW" << std::endl; + return -1; + } + + glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); + glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3); + glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); + + GLFWwindow* window = glfwCreateWindow(1920, 1080, "OV2SLAM PowerViz", NULL, NULL); + if (window == NULL) { + std::cerr << "Failed to create GLFW window" << std::endl; + glfwTerminate(); + return -1; + } + glfwMakeContextCurrent(window); + glfwSwapInterval(1); + + IMGUI_CHECKVERSION(); + ImGui::CreateContext(); + ImPlot::CreateContext(); + + ImGuiIO& io = ImGui::GetIO(); + io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard; + io.ConfigFlags |= ImGuiConfigFlags_DockingEnable; + + ImGui::StyleColorsDark(); + ImGui_ImplGlfw_InitForOpenGL(window, true); + ImGui_ImplOpenGL3_Init("#version 330"); + + // Załaduj logo + node->loadLogo("/ws/src/imgui_app/logo.png"); + + // Ustaw przezroczyste tło dla okien ImGui + ImGuiStyle& style = ImGui::GetStyle(); + style.Colors[ImGuiCol_WindowBg].w = 0.85f; // 85% nieprzezroczystości okien + + ImVec4 clear_color = ImVec4(0.1f, 0.1f, 0.12f, 1.00f); + + while (!glfwWindowShouldClose(window)) + { + rclcpp::spin_some(node); + glfwPollEvents(); + + ImGui_ImplOpenGL3_NewFrame(); + ImGui_ImplGlfw_NewFrame(); + ImGui::NewFrame(); + + // Okno tła z logo (pełnoekranowe, bez ramek, nieinteraktywne) + if (node->hasLogo()) { + ImGui::SetNextWindowPos(ImVec2(0, 0)); + ImGui::SetNextWindowSize(ImGui::GetMainViewport()->Size); + ImGui::SetNextWindowBgAlpha(node->getLogoAlpha()); // Przezroczystość całego okna + ImGui::Begin("Background", nullptr, + ImGuiWindowFlags_NoTitleBar | + ImGuiWindowFlags_NoResize | + ImGuiWindowFlags_NoMove | + ImGuiWindowFlags_NoScrollbar | + ImGuiWindowFlags_NoScrollWithMouse | + ImGuiWindowFlags_NoCollapse | + ImGuiWindowFlags_NoBringToFrontOnFocus | + ImGuiWindowFlags_NoFocusOnAppearing | + ImGuiWindowFlags_NoNav | + ImGuiWindowFlags_NoBackground | + ImGuiWindowFlags_NoDocking); + + ImVec2 viewport_size = ImGui::GetWindowSize(); + float logo_height = viewport_size.y * node->getLogoScale(); + float aspect = (float)node->getLogoWidth() / (float)node->getLogoHeight(); + float logo_width = logo_height * aspect; + + ImVec2 pos( + (viewport_size.x - logo_width) * 0.5f, + (viewport_size.y - logo_height) * 0.5f + ); + + ImGui::SetCursorPos(pos); + ImGui::Image( + (void*)(intptr_t)node->getLogoTexture(), + ImVec2(logo_width, logo_height), + ImVec2(0, 0), + ImVec2(1, 1) + ); + + ImGui::End(); + } + + ImGui::DockSpaceOverViewport(0, ImGui::GetMainViewport()); + + // Okno z pozycją kamery + ImGui::Begin("Camera Position"); + ImGui::Text("Position:"); + ImGui::Text(" X: %.3f m", node->getCameraX()); + ImGui::Text(" Y: %.3f m", node->getCameraY()); + ImGui::Text(" Z: %.3f m", node->getCameraZ()); + ImGui::Separator(); + ImGui::Text("Velocity:"); + ImGui::Text(" Linear: %.3f m/s", node->getVelocityLinear()); + ImGui::Separator(); + ImGui::Text("Distance:"); + ImGui::Text(" Linear: %.3f m", node->getDistanceLinear()); + ImGui::End(); + + // Okno z obrazem + ImGui::Begin("Image Track"); + if (node->hasImage()) { + ImVec2 imageSize(node->getImageWidth(), node->getImageHeight()); + ImGui::Image((void*)(intptr_t)node->getImageTexture(), imageSize); + } else { + ImGui::Text("Waiting for image..."); + } + ImGui::End(); + + // Okno z trajektorią XZ + ImGui::Begin("Trajectory XZ"); + const auto& traj_x = node->getTrajectoryX(); + const auto& traj_z = node->getTrajectoryZ(); + + // Przycisk zapisu trajektorii + if (ImGui::Button("Save Trajectory to CSV")) { + std::string filename = "/ws/trajectories/trajectory_" + + std::to_string(std::chrono::system_clock::now().time_since_epoch().count()) + + ".csv"; + node->saveTrajectoryToCSV(filename); + } + ImGui::SameLine(); + ImGui::Text("Points: %zu", traj_x.size()); + + if (traj_x.size() > 1) { + if (ImPlot::BeginPlot("Camera Trajectory (Top View)", ImVec2(-1, -1))) { + ImPlot::SetupAxis(ImAxis_X1, "X [m]"); + ImPlot::SetupAxis(ImAxis_Y1, "Z [m]"); + ImPlot::SetupAxisLimits(ImAxis_X1, -10, 10, ImGuiCond_Once); + ImPlot::SetupAxisLimits(ImAxis_Y1, -10, 10, ImGuiCond_Once); + + ImPlot::PlotLine("Path", traj_x.data(), traj_z.data(), traj_x.size()); + + // Rysuj aktualną pozycję jako punkt + if (!traj_x.empty()) { + float current_x = traj_x.back(); + float current_z = traj_z.back(); + ImPlot::PlotScatter("Current", ¤t_x, ¤t_z, 1); + } + + ImPlot::EndPlot(); + } + } else { + ImGui::Text("Collecting trajectory data..."); + } + ImGui::End(); + + // Okno z Point Cloud (wizualizacja 3D) + ImGui::Begin("Point Cloud"); + size_t num_points = node->getPointCloudSize(); + ImGui::Text("Points: %zu", num_points); + + // Kontrolki kamery + ImGui::SliderFloat("Angle H", &node->getCameraAngleH(), 0.0f, 360.0f); + ImGui::SliderFloat("Angle V", &node->getCameraAngleV(), -89.0f, 89.0f); + ImGui::SliderFloat("Distance", &node->getCameraDistance(), 5.0f, 100.0f); + + if (num_points > 0) { + const auto& cloud_x = node->getPointCloudX(); + const auto& cloud_y = node->getPointCloudY(); + const auto& cloud_z = node->getPointCloudZ(); + + float angle_h_rad = node->getCameraAngleH() * 3.14159f / 180.0f; + float angle_v_rad = node->getCameraAngleV() * 3.14159f / 180.0f; + + // Projekcja 3D na 2D (izometryczna) + if (ImPlot::BeginPlot("Point Cloud 3D", ImVec2(-1, -1))) { + ImPlot::SetupAxis(ImAxis_X1, ""); + ImPlot::SetupAxis(ImAxis_Y1, ""); + + // Próbkowanie punktów + size_t step = std::max(size_t(1), num_points / 3000); + std::vector proj_x, proj_y; + + for (size_t i = 0; i < num_points; i += step) { + // Obrót kamery + float x = cloud_x[i]; + float y = cloud_y[i]; + float z = cloud_z[i]; + + // Obrót wokół osi Y (poziomy) + float x_rot = x * cos(angle_h_rad) - z * sin(angle_h_rad); + float z_rot = x * sin(angle_h_rad) + z * cos(angle_h_rad); + + // Obrót wokół osi X (pionowy) + float y_rot = y * cos(angle_v_rad) - z_rot * sin(angle_v_rad); + float z_final = y * sin(angle_v_rad) + z_rot * cos(angle_v_rad); + + // Projekcja perspektywiczna + float distance = node->getCameraDistance(); + float scale = distance / (distance + z_final); + + proj_x.push_back(x_rot * scale); + proj_y.push_back(z_final * scale); + } + + ImPlot::PlotScatter("Points", proj_x.data(), proj_y.data(), proj_x.size()); + + // Rysuj osie układu współrzędnych + std::vector axis_x = {0, 0, 0}; + std::vector axis_y = {0, 0, 0}; + std::vector axis_len_x = {1, 0, 0}; + std::vector axis_len_y = {0, 1, 0}; + + // Oś X (czerwona) + for (size_t i = 0; i < 3; ++i) { + float x_rot = axis_len_x[i] * cos(angle_h_rad); + float z_rot = axis_len_x[i] * sin(angle_h_rad); + float y_rot = axis_len_y[i] * cos(angle_v_rad); + + axis_x[i] = x_rot; + axis_y[i] = z_rot; + } + + ImPlot::EndPlot(); + } + } else { + ImGui::Text("Waiting for point cloud data..."); + } + ImGui::End(); + + ImGui::Render(); + int display_w, display_h; + glfwGetFramebufferSize(window, &display_w, &display_h); + glViewport(0, 0, display_w, display_h); + glClearColor(clear_color.x, clear_color.y, clear_color.z, clear_color.w); + glClear(GL_COLOR_BUFFER_BIT); + ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData()); + + glfwSwapBuffers(window); + } + + ImGui_ImplOpenGL3_Shutdown(); + ImGui_ImplGlfw_Shutdown(); + ImPlot::DestroyContext(); + ImGui::DestroyContext(); + glfwDestroyWindow(window); + glfwTerminate(); + + rclcpp::shutdown(); + return 0; +} From ff7f20ef1874c2cb34d1582681b5c369e3601af9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Szymu=C5=9B?= <151686324+dashin2004@users.noreply.github.com> Date: Thu, 4 Dec 2025 20:40:56 +0100 Subject: [PATCH 5/7] Add README with setup and usage instructions Added instructions for cloning the repository, building Docker, and running the application. --- README.md | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 0000000..571bf75 --- /dev/null +++ b/README.md @@ -0,0 +1,37 @@ +# GUI_OV2SLAM +PowerViz to aplikacja GUI pozwalająca na żywo wizualizować mi.n parametry takie jak pozycja, prędkość, obraz, trajektora czy chmura punktów z $OV^2SLAM$ +Screenshot from 2025-12-04 20-28-15 +## Uruchomienie +Na początku pobieramy repozytorium +```bash +git clone https://github.com/PUT-POWERTRAIN/GUI_OV2SLAM.git +``` +Następnie wchodzimy w folder repozytorium +```bash +cd GUI_OV2SLAM +``` +Budujemy dockera +```bash +docker build -t imgui_ros2_gui:latest . +``` +Następnie musimy dać dockerowi uprawnienia do wyświetlania okien na ekranie +```bash +xhost +local:docker +``` +I uruchamiamy dockera tą komendą +```bash +docker run -it --rm \ + --name imgui_gui \ + --network host \ + -e DISPLAY=$DISPLAY \ + -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ + imgui_ros2_gui:latest +``` +Będąc w dockerze musimy zsourcować środowisko +```bash +source /ws/install/setup.bash +``` +Ostatecznie gdy chcemy uruchomić aplikację włączamy plik launch +```bash +ros2 launch imgui_app visualizer.launch.py +``` From 986b2fa711b61b3695fd77968c41e6d2f3b35fb7 Mon Sep 17 00:00:00 2001 From: dashin2004 Date: Sun, 7 Dec 2025 16:45:15 +0100 Subject: [PATCH 6/7] Stworzenie 3D interaktywnego point_cloud --- README.md | 2 +- src/main.cpp | 639 +++++++++++++++++++++++++++------------------------ 2 files changed, 345 insertions(+), 296 deletions(-) diff --git a/README.md b/README.md index 571bf75..93d331a 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # GUI_OV2SLAM PowerViz to aplikacja GUI pozwalająca na żywo wizualizować mi.n parametry takie jak pozycja, prędkość, obraz, trajektora czy chmura punktów z $OV^2SLAM$ -Screenshot from 2025-12-04 20-28-15 +Screenshot from 2025-12-07 16-40-43 ## Uruchomienie Na początku pobieramy repozytorium ```bash diff --git a/src/main.cpp b/src/main.cpp index 2512b6f..dfd466e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,6 +5,12 @@ #include #include + +#include +#include +#include +#include + #include "imgui.h" #include "imgui_impl_glfw.h" #include "imgui_impl_opengl3.h" @@ -17,6 +23,60 @@ #include #include #include +#include + +// --- SHADERY DLA OKNA 3D --- +const char* viz_vertex_shader = R"( +#version 330 core +layout (location = 0) in vec3 aPos; +layout (location = 1) in vec3 aColor; +uniform mat4 projection; +uniform mat4 view; +out vec3 vertexColor; +void main() { + gl_Position = projection * view * vec4(aPos, 1.0); + gl_PointSize = 2.0; + vertexColor = aColor; +} +)"; + +const char* viz_fragment_shader = R"( +#version 330 core +in vec3 vertexColor; +out vec4 FragColor; +void main() { + FragColor = vec4(vertexColor, 1.0); +} +)"; + +// Shader dla siatki (Grid) +const char* grid_vertex_shader = R"( +#version 330 core +layout (location = 0) in vec3 aPos; +uniform mat4 projection; +uniform mat4 view; +void main() { + gl_Position = projection * view * vec4(aPos, 1.0); +} +)"; + +const char* grid_fragment_shader = R"( +#version 330 core +out vec4 FragColor; +void main() { + FragColor = vec4(0.4, 0.4, 0.4, 1.0); +} +)"; + + +GLuint createShaderProgram(const char* vSrc, const char* fSrc) { + GLuint v = glCreateShader(GL_VERTEX_SHADER); glShaderSource(v, 1, &vSrc, NULL); glCompileShader(v); + GLuint f = glCreateShader(GL_FRAGMENT_SHADER); glShaderSource(f, 1, &fSrc, NULL); glCompileShader(f); + GLuint p = glCreateProgram(); glAttachShader(p, v); glAttachShader(p, f); glLinkProgram(p); + glDeleteShader(v); glDeleteShader(f); + return p; +} + class ImGuiVisualizerNode : public rclcpp::Node { @@ -27,25 +87,26 @@ class ImGuiVisualizerNode : public rclcpp::Node this->declare_parameter("image_track_topic", "/image_track"); this->declare_parameter("point_cloud_topic", "/point_cloud"); - std::string vo_pose_topic = this->get_parameter("vo_pose_topic").as_string(); - std::string image_track_topic = this->get_parameter("image_track_topic").as_string(); - std::string point_cloud_topic = this->get_parameter("point_cloud_topic").as_string(); - + // QoS Best Effort dla PointCloud + rclcpp::QoS qos(10); + qos.keep_last(5); + qos.best_effort(); + qos.durability_volatile(); + pose_sub_ = this->create_subscription( - vo_pose_topic, 10, + this->get_parameter("vo_pose_topic").as_string(), 10, std::bind(&ImGuiVisualizerNode::poseCallback, this, std::placeholders::_1)); image_sub_ = this->create_subscription( - image_track_topic, 10, + this->get_parameter("image_track_topic").as_string(), 10, std::bind(&ImGuiVisualizerNode::imageCallback, this, std::placeholders::_1)); cloud_sub_ = this->create_subscription( - point_cloud_topic, 10, + this->get_parameter("point_cloud_topic").as_string(), qos, std::bind(&ImGuiVisualizerNode::pointCloudCallback, this, std::placeholders::_1)); - - RCLCPP_INFO(this->get_logger(), "ImGui Visualizer Node started!"); } + // Gettery float getCameraX() const { return camera_x_; } float getCameraY() const { return camera_y_; } float getCameraZ() const { return camera_z_; } @@ -53,6 +114,21 @@ class ImGuiVisualizerNode : public rclcpp::Node float getDistanceLinear() const { return distance_linear_; } bool hasImage() const { return has_image_; } + // Texture ID + void updateImageTexture() { + if(has_new_image_ && !current_image_mat_.empty()) { + if (image_texture_ == 0) { + glGenTextures(1, &image_texture_); + glBindTexture(GL_TEXTURE_2D, image_texture_); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + } + glBindTexture(GL_TEXTURE_2D, image_texture_); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, image_width_, image_height_, + 0, GL_RGB, GL_UNSIGNED_BYTE, current_image_mat_.data); + has_new_image_ = false; + } + } GLuint getImageTexture() const { return image_texture_; } int getImageWidth() const { return image_width_; } int getImageHeight() const { return image_height_; } @@ -60,22 +136,15 @@ class ImGuiVisualizerNode : public rclcpp::Node const std::vector& getTrajectoryX() const { return trajectory_x_; } const std::vector& getTrajectoryZ() const { return trajectory_z_; } - bool loadLogo(const std::string& logo_path) - { + // Logo + bool loadLogo(const std::string& logo_path) { cv::Mat logo_img = cv::imread(logo_path, cv::IMREAD_UNCHANGED); - if (logo_img.empty()) { - RCLCPP_WARN(this->get_logger(), "Failed to load logo: %s", logo_path.c_str()); - return false; - } + if (logo_img.empty()) return false; cv::Mat logo_converted; bool has_alpha = (logo_img.channels() == 4); - - if (has_alpha) { - cv::cvtColor(logo_img, logo_converted, cv::COLOR_BGRA2RGBA); - } else { - cv::cvtColor(logo_img, logo_converted, cv::COLOR_BGR2RGB); - } + if (has_alpha) cv::cvtColor(logo_img, logo_converted, cv::COLOR_BGRA2RGBA); + else cv::cvtColor(logo_img, logo_converted, cv::COLOR_BGR2RGB); logo_width_ = logo_converted.cols; logo_height_ = logo_converted.rows; @@ -86,12 +155,9 @@ class ImGuiVisualizerNode : public rclcpp::Node glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); GLint format = has_alpha ? GL_RGBA : GL_RGB; - glTexImage2D(GL_TEXTURE_2D, 0, format, logo_width_, logo_height_, - 0, format, GL_UNSIGNED_BYTE, logo_converted.data); + glTexImage2D(GL_TEXTURE_2D, 0, format, logo_width_, logo_height_, 0, format, GL_UNSIGNED_BYTE, logo_converted.data); has_logo_ = true; - RCLCPP_INFO(this->get_logger(), "Logo loaded successfully (%s)", - has_alpha ? "with alpha" : "without alpha"); return true; } @@ -102,288 +168,332 @@ class ImGuiVisualizerNode : public rclcpp::Node float getLogoScale() const { return logo_scale_; } float getLogoAlpha() const { return logo_alpha_; } - const std::vector& getPointCloudX() const { return cloud_x_; } - const std::vector& getPointCloudY() const { return cloud_y_; } - const std::vector& getPointCloudZ() const { return cloud_z_; } - const std::vector& getPointCloudColors() const { return cloud_colors_; } - size_t getPointCloudSize() const { return cloud_x_.size(); } - - float& getCameraAngleH() { return camera_angle_h_; } - float& getCameraAngleV() { return camera_angle_v_; } - float& getCameraDistance() { return camera_distance_; } - - void saveTrajectoryToCSV(const std::string& filename) - { + void saveTrajectoryToCSV(const std::string& filename) { std::ofstream file(filename); - if (!file.is_open()) { - RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", filename.c_str()); - return; + if (file.is_open()) { + file << "x,z\n"; + for (size_t i = 0; i < trajectory_x_.size(); ++i) + file << trajectory_x_[i] << "," << trajectory_z_[i] << "\n"; + file.close(); } - - file << "x,z\n"; - for (size_t i = 0; i < trajectory_x_.size(); ++i) { - file << trajectory_x_[i] << "," << trajectory_z_[i] << "\n"; - } - - file.close(); - RCLCPP_INFO(this->get_logger(), "Trajectory saved to: %s (%zu points)", - filename.c_str(), trajectory_x_.size()); } + // Point Cloud Data Access + std::vector cloud_points_buffer; // x,y,z flat + std::vector cloud_colors_buffer; // r,g,b flat + bool new_cloud_available = false; + size_t getCloudSize() const { return cloud_points_buffer.size() / 3; } + + float center_x=0, center_y=0, center_z=0; // Centrum chmury + private: - void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) - { + void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { auto current_time = this->now(); - if (last_pose_time_.seconds() > 0.0) { double dt = (current_time - last_pose_time_).seconds(); - if (dt > 0.0001) { float dx = msg->pose.position.x - last_x_; float dy = msg->pose.position.y - last_y_; float dz = msg->pose.position.z - last_z_; - velocity_linear_ = std::sqrt(dx*dx + dy*dy + dz*dz) / dt; distance_linear_ += std::sqrt(dx*dx + dy*dy + dz*dz); } } - last_pose_time_ = current_time; last_x_ = msg->pose.position.x; last_y_ = msg->pose.position.y; last_z_ = msg->pose.position.z; - camera_x_ = msg->pose.position.x; camera_y_ = msg->pose.position.y; camera_z_ = msg->pose.position.z; - - // Dodaj punkt do trajektorii trajectory_x_.push_back(camera_x_); trajectory_z_.push_back(camera_z_); } - void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) - { + void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) { try { cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8"); - cv::Mat rgb_image; - cv::cvtColor(cv_ptr->image, rgb_image, cv::COLOR_BGR2RGB); - - image_width_ = rgb_image.cols; - image_height_ = rgb_image.rows; - - // Utwórz teksturę OpenGL jeśli nie istnieje - if (image_texture_ == 0) { - glGenTextures(1, &image_texture_); - glBindTexture(GL_TEXTURE_2D, image_texture_); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - } - - // Aktualizuj teksturę - glBindTexture(GL_TEXTURE_2D, image_texture_); - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, image_width_, image_height_, - 0, GL_RGB, GL_UNSIGNED_BYTE, rgb_image.data); - + cv::cvtColor(cv_ptr->image, current_image_mat_, cv::COLOR_BGR2RGB); + image_width_ = current_image_mat_.cols; + image_height_ = current_image_mat_.rows; has_image_ = true; - - } catch (cv_bridge::Exception& e) { - RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); - } + has_new_image_ = true; + } catch (...) {} } - void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) - { - cloud_x_.clear(); - cloud_y_.clear(); - cloud_z_.clear(); - cloud_colors_.clear(); - - // Znajdź offsety pól - int x_offset = -1, y_offset = -1, z_offset = -1, rgb_offset = -1; - - for (const auto& field : msg->fields) { - if (field.name == "x") x_offset = field.offset; - else if (field.name == "y") y_offset = field.offset; - else if (field.name == "z") z_offset = field.offset; - else if (field.name == "rgb") rgb_offset = field.offset; - } - - if (x_offset < 0 || y_offset < 0 || z_offset < 0) { - RCLCPP_WARN(this->get_logger(), "Point cloud missing x, y, or z fields"); - return; + void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + // Parsing danych bezpośrednio do bufora dla OpenGL + int x_off=-1, y_off=-1, z_off=-1, rgb_off=-1; + for(const auto& f : msg->fields) { + if(f.name=="x") x_off=f.offset; + else if(f.name=="y") y_off=f.offset; + else if(f.name=="z") z_off=f.offset; + else if(f.name=="rgb") rgb_off=f.offset; } + if(x_off<0) return; + + size_t n = msg->width * msg->height; + std::vector pts; pts.reserve(n*3); + std::vector cols; cols.reserve(n*3); - size_t num_points = msg->width * msg->height; - cloud_x_.reserve(num_points); - cloud_y_.reserve(num_points); - cloud_z_.reserve(num_points); - cloud_colors_.reserve(num_points); - - // Parsuj punkty - for (size_t i = 0; i < num_points; ++i) { - size_t point_offset = i * msg->point_step; - - float x, y, z; - memcpy(&x, &msg->data[point_offset + x_offset], sizeof(float)); - memcpy(&y, &msg->data[point_offset + y_offset], sizeof(float)); - memcpy(&z, &msg->data[point_offset + z_offset], sizeof(float)); - - // Sprawdź czy punkt jest poprawny - if (std::isfinite(x) && std::isfinite(y) && std::isfinite(z)) { - cloud_x_.push_back(x); - cloud_y_.push_back(y); - cloud_z_.push_back(z); - - // Odczytaj kolor jeśli dostępny - if (rgb_offset >= 0) { - uint32_t rgb; - memcpy(&rgb, &msg->data[point_offset + rgb_offset], sizeof(uint32_t)); - cloud_colors_.push_back(rgb); + double sx=0, sy=0, sz=0; + int valid=0; + + for(size_t i=0; ipoint_step; + float x = *(float*)&msg->data[ptr + x_off]; + float y = *(float*)&msg->data[ptr + y_off]; + float z = *(float*)&msg->data[ptr + z_off]; + + if(std::isfinite(x) && std::isfinite(y) && std::isfinite(z)) { + pts.push_back(x); pts.push_back(y); pts.push_back(z); + sx+=x; sy+=y; sz+=z; valid++; + + if(rgb_off>=0) { + uint32_t c = *(uint32_t*)&msg->data[ptr + rgb_off]; + cols.push_back(((c>>16)&0xFF)/255.0f); + cols.push_back(((c>>8)&0xFF)/255.0f); + cols.push_back((c&0xFF)/255.0f); } else { - cloud_colors_.push_back(0xFFFFFFFF); + cols.push_back(1.0f); cols.push_back(1.0f); cols.push_back(1.0f); } } } + + if(valid > 0) { + center_x = sx/valid; center_y = sy/valid; center_z = sz/valid; + } - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Point cloud: %zu points", cloud_x_.size()); + cloud_points_buffer = std::move(pts); + cloud_colors_buffer = std::move(cols); + new_cloud_available = true; } - float camera_x_ = 0.0f; - float camera_y_ = 0.0f; - float camera_z_ = 0.0f; - float velocity_linear_ = 0.0f; - float distance_linear_ = 0.0f; - + // Zmienne + float camera_x_ = 0, camera_y_ = 0, camera_z_ = 0; + float velocity_linear_ = 0, distance_linear_ = 0; rclcpp::Time last_pose_time_ = rclcpp::Time(0); - float last_x_ = 0.0f; - float last_y_ = 0.0f; - float last_z_ = 0.0f; + float last_x_ = 0, last_y_ = 0, last_z_ = 0; // Image bool has_image_ = false; + bool has_new_image_ = false; + cv::Mat current_image_mat_; GLuint image_texture_ = 0; - int image_width_ = 0; - int image_height_ = 0; + int image_width_ = 0, image_height_ = 0; // Trajectory - std::vector trajectory_x_; - std::vector trajectory_z_; + std::vector trajectory_x_, trajectory_z_; // Logo bool has_logo_ = false; GLuint logo_texture_ = 0; - int logo_width_ = 0; - int logo_height_ = 0; - float logo_scale_ = 0.3f; // Rozmiar logo (0.1 - 1.0, gdzie 1.0 = 100% wysokości ekranu) - float logo_alpha_ = 0.15f; // Przezroczystość (0.0 - 1.0, gdzie 1.0 = nieprzezroczyste) + int logo_width_ = 0, logo_height_ = 0; + float logo_scale_ = 0.3f; + float logo_alpha_ = 0.15f; - // Point Cloud - std::vector cloud_x_; - std::vector cloud_y_; - std::vector cloud_z_; - std::vector cloud_colors_; - - // Kamera 3D - float camera_angle_h_ = 45.0f; // Kąt poziomy (stopnie) - float camera_angle_v_ = 30.0f; // Kąt pionowy (stopnie) - float camera_distance_ = 20.0f; // Odległość od centrum - rclcpp::Subscription::SharedPtr pose_sub_; rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Subscription::SharedPtr cloud_sub_; }; +// --- STRUKTURA KAMERY DLA OKNA 3D --- +struct Camera3D { + float yaw = -45.0f, pitch = 30.0f, dist = 50.0f; + float target[3] = {0,0,0}; + bool dragging = false, panning = false; + double lx=0, ly=0; + + void update(GLFWwindow* win) { + double mx, my; glfwGetCursorPos(win, &mx, &my); + + // Obrót (LPM) + if(glfwGetMouseButton(win, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS) { + if(!dragging) { dragging=true; lx=mx; ly=my; } + yaw += (float)(mx - lx) * 0.5f; + pitch += (float)(my - ly) * 0.5f; + pitch = std::max(-89.0f, std::min(89.0f, pitch)); + lx=mx; ly=my; + } else dragging=false; + + // Przesuwanie (PPM) + if(glfwGetMouseButton(win, GLFW_MOUSE_BUTTON_RIGHT) == GLFW_PRESS) { + if(!panning) { panning=true; lx=mx; ly=my; } + float dx = (float)(mx - lx) * 0.05f; + float dy = (float)(my - ly) * 0.05f; + float rad = glm::radians(yaw); + target[0] -= (dx * cos(rad) - dy * sin(rad)); + target[2] -= (dx * sin(rad) + dy * cos(rad)); + lx=mx; ly=my; + } else panning=false; + } + + glm::mat4 getView() { + float ry = glm::radians(yaw), rp = glm::radians(pitch); + float cx = target[0] + dist * cos(rp) * sin(ry); + float cy = target[1] + dist * sin(rp); + float cz = target[2] + dist * cos(rp) * cos(ry); + return glm::lookAt(glm::vec3(cx,cy,cz), glm::vec3(target[0],target[1],target[2]), glm::vec3(0,1,0)); + } +}; + +Camera3D cam3d; +void scroll_callback(GLFWwindow* w, double x, double y) { + cam3d.dist -= (float)y * 2.0f; + if(cam3d.dist < 0.1f) cam3d.dist = 0.1f; +} + + int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); - if (!glfwInit()) { - std::cerr << "Failed to initialize GLFW" << std::endl; - return -1; - } - + if (!glfwInit()) return -1; glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3); glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); + + // 1. OKNO GUI + GLFWwindow* winGUI = glfwCreateWindow(1280, 800, "OV2SLAM PowerViz", NULL, NULL); + + // 2. OKNO 3D + GLFWwindow* win3D = glfwCreateWindow(1200, 900, "3D Point Cloud PowerView", NULL, NULL); + + if (!winGUI || !win3D) return -1; - GLFWwindow* window = glfwCreateWindow(1920, 1080, "OV2SLAM PowerViz", NULL, NULL); - if (window == NULL) { - std::cerr << "Failed to create GLFW window" << std::endl; - glfwTerminate(); - return -1; + // --- KONFIGURACJA OKNA 3D --- + glfwMakeContextCurrent(win3D); + glewExperimental = GL_TRUE; + glewInit(); + glfwSetScrollCallback(win3D, scroll_callback); + + // Shadery dla 3D + GLuint progViz = createShaderProgram(viz_vertex_shader, viz_fragment_shader); + GLuint progGrid = createShaderProgram(grid_vertex_shader, grid_fragment_shader); + + // VAO/VBO dla chmury punktów + GLuint vaoPts, vboPts, cboPts; + glGenVertexArrays(1, &vaoPts); glGenBuffers(1, &vboPts); glGenBuffers(1, &cboPts); + + // VAO/VBO dla siatki + GLuint vaoGrid; glGenVertexArrays(1, &vaoGrid); + std::vector gridV; + int gSize=100; + for(int i=-gSize; i<=gSize; i+=5) { + gridV.push_back(i); gridV.push_back(0); gridV.push_back(-gSize); + gridV.push_back(i); gridV.push_back(0); gridV.push_back(gSize); + gridV.push_back(-gSize); gridV.push_back(0); gridV.push_back(i); + gridV.push_back(gSize); gridV.push_back(0); gridV.push_back(i); } - glfwMakeContextCurrent(window); - glfwSwapInterval(1); + GLuint vboGrid; glGenBuffers(1, &vboGrid); + glBindVertexArray(vaoGrid); + glBindBuffer(GL_ARRAY_BUFFER, vboGrid); + glBufferData(GL_ARRAY_BUFFER, gridV.size()*sizeof(float), gridV.data(), GL_STATIC_DRAW); + glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0); glEnableVertexAttribArray(0); + // --- KONFIGURACJA OKNA GUI --- + glfwMakeContextCurrent(winGUI); IMGUI_CHECKVERSION(); ImGui::CreateContext(); ImPlot::CreateContext(); - ImGuiIO& io = ImGui::GetIO(); - io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard; io.ConfigFlags |= ImGuiConfigFlags_DockingEnable; ImGui::StyleColorsDark(); - ImGui_ImplGlfw_InitForOpenGL(window, true); + ImGui_ImplGlfw_InitForOpenGL(winGUI, true); ImGui_ImplOpenGL3_Init("#version 330"); - // Załaduj logo + // Ładowanie logo node->loadLogo("/ws/src/imgui_app/logo.png"); - // Ustaw przezroczyste tło dla okien ImGui ImGuiStyle& style = ImGui::GetStyle(); - style.Colors[ImGuiCol_WindowBg].w = 0.85f; // 85% nieprzezroczystości okien + style.Colors[ImGuiCol_WindowBg].w = 0.85f; - ImVec4 clear_color = ImVec4(0.1f, 0.1f, 0.12f, 1.00f); - - while (!glfwWindowShouldClose(window)) + while (!glfwWindowShouldClose(winGUI) && !glfwWindowShouldClose(win3D)) { rclcpp::spin_some(node); glfwPollEvents(); + // ========================================================= + // 1. RENDEROWANIE OKNA POINT CLOUD + // ========================================================= + glfwMakeContextCurrent(win3D); + cam3d.update(win3D); + + int w3, h3; glfwGetFramebufferSize(win3D, &w3, &h3); + glViewport(0, 0, w3, h3); + glClearColor(0.15f, 0.15f, 0.15f, 1.0f); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_DEPTH_TEST); + glEnable(GL_PROGRAM_POINT_SIZE); + + glm::mat4 proj = glm::perspective(glm::radians(45.0f), (float)w3/h3, 0.1f, 10000.0f); + glm::mat4 view = cam3d.getView(); + + // Rysuj siatkę + glUseProgram(progGrid); + glUniformMatrix4fv(glGetUniformLocation(progGrid, "projection"), 1, GL_FALSE, glm::value_ptr(proj)); + glUniformMatrix4fv(glGetUniformLocation(progGrid, "view"), 1, GL_FALSE, glm::value_ptr(view)); + glBindVertexArray(vaoGrid); + glDrawArrays(GL_LINES, 0, gridV.size()/3); + + // Rysuj punkty + if(node->new_cloud_available) { + glBindVertexArray(vaoPts); + glBindBuffer(GL_ARRAY_BUFFER, vboPts); + glBufferData(GL_ARRAY_BUFFER, node->cloud_points_buffer.size()*4, node->cloud_points_buffer.data(), GL_DYNAMIC_DRAW); + glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0); glEnableVertexAttribArray(0); + + glBindBuffer(GL_ARRAY_BUFFER, cboPts); + glBufferData(GL_ARRAY_BUFFER, node->cloud_colors_buffer.size()*4, node->cloud_colors_buffer.data(), GL_DYNAMIC_DRAW); + glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 0, 0); glEnableVertexAttribArray(1); + node->new_cloud_available = false; + + // Auto-center + if(cam3d.target[0]==0 && cam3d.target[1]==0) { + cam3d.target[0]=node->center_x; cam3d.target[1]=node->center_y; cam3d.target[2]=node->center_z; + } + } + + if(node->getCloudSize() > 0) { + glUseProgram(progViz); + glUniformMatrix4fv(glGetUniformLocation(progViz, "projection"), 1, GL_FALSE, glm::value_ptr(proj)); + glUniformMatrix4fv(glGetUniformLocation(progViz, "view"), 1, GL_FALSE, glm::value_ptr(view)); + glBindVertexArray(vaoPts); + glDrawArrays(GL_POINTS, 0, node->getCloudSize()); + } + + glfwSwapBuffers(win3D); + + // ========================================================= + // 2. RENDEROWANIE OKNA IMGUI + // ========================================================= + glfwMakeContextCurrent(winGUI); + + // Aktualizuj teksturę obrazu (jeśli jest nowa klatka) + node->updateImageTexture(); + ImGui_ImplOpenGL3_NewFrame(); ImGui_ImplGlfw_NewFrame(); ImGui::NewFrame(); - // Okno tła z logo (pełnoekranowe, bez ramek, nieinteraktywne) + // LOGO W TLE if (node->hasLogo()) { ImGui::SetNextWindowPos(ImVec2(0, 0)); ImGui::SetNextWindowSize(ImGui::GetMainViewport()->Size); - ImGui::SetNextWindowBgAlpha(node->getLogoAlpha()); // Przezroczystość całego okna + ImGui::SetNextWindowBgAlpha(node->getLogoAlpha()); ImGui::Begin("Background", nullptr, - ImGuiWindowFlags_NoTitleBar | - ImGuiWindowFlags_NoResize | - ImGuiWindowFlags_NoMove | - ImGuiWindowFlags_NoScrollbar | - ImGuiWindowFlags_NoScrollWithMouse | - ImGuiWindowFlags_NoCollapse | - ImGuiWindowFlags_NoBringToFrontOnFocus | - ImGuiWindowFlags_NoFocusOnAppearing | - ImGuiWindowFlags_NoNav | - ImGuiWindowFlags_NoBackground | - ImGuiWindowFlags_NoDocking); + ImGuiWindowFlags_NoDecoration | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_NoBackground | ImGuiWindowFlags_NoBringToFrontOnFocus); - ImVec2 viewport_size = ImGui::GetWindowSize(); - float logo_height = viewport_size.y * node->getLogoScale(); + ImVec2 sz = ImGui::GetWindowSize(); + float lh = sz.y * node->getLogoScale(); float aspect = (float)node->getLogoWidth() / (float)node->getLogoHeight(); - float logo_width = logo_height * aspect; - - ImVec2 pos( - (viewport_size.x - logo_width) * 0.5f, - (viewport_size.y - logo_height) * 0.5f - ); - - ImGui::SetCursorPos(pos); - ImGui::Image( - (void*)(intptr_t)node->getLogoTexture(), - ImVec2(logo_width, logo_height), - ImVec2(0, 0), - ImVec2(1, 1) - ); + float lw = lh * aspect; + ImGui::SetCursorPos(ImVec2((sz.x - lw)*0.5f, (sz.y - lh)*0.5f)); + // Naprawiony Image call z 6 argumentami + ImGui::Image((void*)(intptr_t)node->getLogoTexture(), ImVec2(lw, lh), ImVec2(0,0), ImVec2(1,1), ImVec4(1,1,1,1.0f), ImVec4(0,0,0,0)); ImGui::End(); } @@ -401,19 +511,25 @@ int main(int argc, char** argv) ImGui::Separator(); ImGui::Text("Distance:"); ImGui::Text(" Linear: %.3f m", node->getDistanceLinear()); + ImGui::Separator(); + ImGui::Text("3D View Status:"); + ImGui::Text(" Points: %zu", node->getCloudSize()); + ImGui::Text(" Center: %.1f %.1f %.1f", node->center_x, node->center_y, node->center_z); ImGui::End(); - // Okno z obrazem + // OKNO 2: OBRAZ Z KAMERY ImGui::Begin("Image Track"); if (node->hasImage()) { - ImVec2 imageSize(node->getImageWidth(), node->getImageHeight()); - ImGui::Image((void*)(intptr_t)node->getImageTexture(), imageSize); + float avail = ImGui::GetContentRegionAvail().x; + float w = node->getImageWidth(); float h = node->getImageHeight(); + if(w > avail) { float s = avail/w; w*=s; h*=s; } + ImGui::Image((void*)(intptr_t)node->getImageTexture(), ImVec2(w, h)); } else { ImGui::Text("Waiting for image..."); } ImGui::End(); - // Okno z trajektorią XZ + // OKNO 3: TRAJEKTORIA ImGui::Begin("Trajectory XZ"); const auto& traj_x = node->getTrajectoryX(); const auto& traj_z = node->getTrajectoryZ(); @@ -450,97 +566,30 @@ int main(int argc, char** argv) ImGui::Text("Collecting trajectory data..."); } ImGui::End(); - - // Okno z Point Cloud (wizualizacja 3D) - ImGui::Begin("Point Cloud"); - size_t num_points = node->getPointCloudSize(); - ImGui::Text("Points: %zu", num_points); - - // Kontrolki kamery - ImGui::SliderFloat("Angle H", &node->getCameraAngleH(), 0.0f, 360.0f); - ImGui::SliderFloat("Angle V", &node->getCameraAngleV(), -89.0f, 89.0f); - ImGui::SliderFloat("Distance", &node->getCameraDistance(), 5.0f, 100.0f); - - if (num_points > 0) { - const auto& cloud_x = node->getPointCloudX(); - const auto& cloud_y = node->getPointCloudY(); - const auto& cloud_z = node->getPointCloudZ(); - - float angle_h_rad = node->getCameraAngleH() * 3.14159f / 180.0f; - float angle_v_rad = node->getCameraAngleV() * 3.14159f / 180.0f; - - // Projekcja 3D na 2D (izometryczna) - if (ImPlot::BeginPlot("Point Cloud 3D", ImVec2(-1, -1))) { - ImPlot::SetupAxis(ImAxis_X1, ""); - ImPlot::SetupAxis(ImAxis_Y1, ""); - - // Próbkowanie punktów - size_t step = std::max(size_t(1), num_points / 3000); - std::vector proj_x, proj_y; - - for (size_t i = 0; i < num_points; i += step) { - // Obrót kamery - float x = cloud_x[i]; - float y = cloud_y[i]; - float z = cloud_z[i]; - - // Obrót wokół osi Y (poziomy) - float x_rot = x * cos(angle_h_rad) - z * sin(angle_h_rad); - float z_rot = x * sin(angle_h_rad) + z * cos(angle_h_rad); - - // Obrót wokół osi X (pionowy) - float y_rot = y * cos(angle_v_rad) - z_rot * sin(angle_v_rad); - float z_final = y * sin(angle_v_rad) + z_rot * cos(angle_v_rad); - - // Projekcja perspektywiczna - float distance = node->getCameraDistance(); - float scale = distance / (distance + z_final); - - proj_x.push_back(x_rot * scale); - proj_y.push_back(z_final * scale); - } - - ImPlot::PlotScatter("Points", proj_x.data(), proj_y.data(), proj_x.size()); - - // Rysuj osie układu współrzędnych - std::vector axis_x = {0, 0, 0}; - std::vector axis_y = {0, 0, 0}; - std::vector axis_len_x = {1, 0, 0}; - std::vector axis_len_y = {0, 1, 0}; - - // Oś X (czerwona) - for (size_t i = 0; i < 3; ++i) { - float x_rot = axis_len_x[i] * cos(angle_h_rad); - float z_rot = axis_len_x[i] * sin(angle_h_rad); - float y_rot = axis_len_y[i] * cos(angle_v_rad); - - axis_x[i] = x_rot; - axis_y[i] = z_rot; - } - - ImPlot::EndPlot(); - } - } else { - ImGui::Text("Waiting for point cloud data..."); - } - ImGui::End(); + // RENDER GUI ImGui::Render(); - int display_w, display_h; - glfwGetFramebufferSize(window, &display_w, &display_h); - glViewport(0, 0, display_w, display_h); - glClearColor(clear_color.x, clear_color.y, clear_color.z, clear_color.w); + int wg, hg; glfwGetFramebufferSize(winGUI, &wg, &hg); + glViewport(0, 0, wg, hg); + glClearColor(0.1f, 0.1f, 0.12f, 1.0f); glClear(GL_COLOR_BUFFER_BIT); ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData()); - - glfwSwapBuffers(window); + + glfwSwapBuffers(winGUI); } ImGui_ImplOpenGL3_Shutdown(); ImGui_ImplGlfw_Shutdown(); ImPlot::DestroyContext(); ImGui::DestroyContext(); - glfwDestroyWindow(window); + + glDeleteVertexArrays(1, &vaoPts); + glDeleteBuffers(1, &vboPts); + glDeleteBuffers(1, &cboPts); + glDeleteProgram(progViz); + + glfwDestroyWindow(winGUI); + glfwDestroyWindow(win3D); glfwTerminate(); rclcpp::shutdown(); From 010a3453481df8d98602c8df578dbcf801fd897f Mon Sep 17 00:00:00 2001 From: dashin2004 Date: Sun, 14 Dec 2025 16:52:37 +0100 Subject: [PATCH 7/7] Merge okno point cloud i PowerViz --- src/main.cpp | 431 +++++++++++++++++++++++++++++++-------------------- 1 file changed, 265 insertions(+), 166 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index dfd466e..f112d61 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,7 +5,6 @@ #include #include - #include #include #include @@ -25,7 +24,7 @@ #include #include -// --- SHADERY DLA OKNA 3D --- +// --- SHADERY DLA POINT CLOUD --- const char* viz_vertex_shader = R"( #version 330 core layout (location = 0) in vec3 aPos; @@ -68,7 +67,6 @@ void main() { } )"; - GLuint createShaderProgram(const char* vSrc, const char* fSrc) { GLuint v = glCreateShader(GL_VERTEX_SHADER); glShaderSource(v, 1, &vSrc, NULL); glCompileShader(v); GLuint f = glCreateShader(GL_FRAGMENT_SHADER); glShaderSource(f, 1, &fSrc, NULL); glCompileShader(f); @@ -77,6 +75,48 @@ GLuint createShaderProgram(const char* vSrc, const char* fSrc) { return p; } +// --- STRUKTURA KAMERY 3D --- +struct Camera3D { + float yaw = 135.0f, pitch = 30.0f, dist = 50.0f; + float target[3] = {0,0,0}; + bool dragging = false, panning = false; + double lx=0, ly=0; + ImVec2 last_mouse_pos; + bool mouse_over_window = false; + + void update(ImVec2 mouse_pos, bool left_down, bool right_down, ImVec2 mouse_delta) { + // Obrót (LPM) + if(left_down && mouse_over_window) { + yaw += mouse_delta.x * 0.5f; + pitch += mouse_delta.y * 0.5f; + pitch = std::max(-89.0f, std::min(89.0f, pitch)); + } + + // Przesuwanie (PPM) + if(right_down && mouse_over_window) { + float dx = mouse_delta.x * 0.05f; + float dy = mouse_delta.y * 0.05f; + float rad = glm::radians(yaw); + target[0] -= (dx * cos(rad) - dy * sin(rad)); + target[2] -= (dx * sin(rad) + dy * cos(rad)); + } + } + + void scroll(float delta) { + if(mouse_over_window) { + dist -= delta * 2.0f; + if(dist < 0.1f) dist = 0.1f; + } + } + + glm::mat4 getView() { + float ry = glm::radians(yaw), rp = glm::radians(pitch); + float cx = target[0] + dist * cos(rp) * sin(ry); + float cy = target[1] + dist * sin(rp); + float cz = target[2] + dist * cos(rp) * cos(ry); + return glm::lookAt(glm::vec3(cx,cy,cz), glm::vec3(target[0],target[1],target[2]), glm::vec3(0,1,0)); + } +}; class ImGuiVisualizerNode : public rclcpp::Node { @@ -114,7 +154,7 @@ class ImGuiVisualizerNode : public rclcpp::Node float getDistanceLinear() const { return distance_linear_; } bool hasImage() const { return has_image_; } - // Texture ID + void updateImageTexture() { if(has_new_image_ && !current_image_mat_.empty()) { if (image_texture_ == 0) { @@ -129,6 +169,7 @@ class ImGuiVisualizerNode : public rclcpp::Node has_new_image_ = false; } } + GLuint getImageTexture() const { return image_texture_; } int getImageWidth() const { return image_width_; } int getImageHeight() const { return image_height_; } @@ -179,12 +220,12 @@ class ImGuiVisualizerNode : public rclcpp::Node } // Point Cloud Data Access - std::vector cloud_points_buffer; // x,y,z flat - std::vector cloud_colors_buffer; // r,g,b flat + std::vector cloud_points_buffer; + std::vector cloud_colors_buffer; bool new_cloud_available = false; size_t getCloudSize() const { return cloud_points_buffer.size() / 3; } - float center_x=0, center_y=0, center_z=0; // Centrum chmury + float center_x=0, center_y=0, center_z=0; private: void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { @@ -222,7 +263,6 @@ class ImGuiVisualizerNode : public rclcpp::Node } void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - // Parsing danych bezpośrednio do bufora dla OpenGL int x_off=-1, y_off=-1, z_off=-1, rgb_off=-1; for(const auto& f : msg->fields) { if(f.name=="x") x_off=f.offset; @@ -269,23 +309,19 @@ class ImGuiVisualizerNode : public rclcpp::Node new_cloud_available = true; } - // Zmienne float camera_x_ = 0, camera_y_ = 0, camera_z_ = 0; float velocity_linear_ = 0, distance_linear_ = 0; rclcpp::Time last_pose_time_ = rclcpp::Time(0); float last_x_ = 0, last_y_ = 0, last_z_ = 0; - // Image bool has_image_ = false; bool has_new_image_ = false; cv::Mat current_image_mat_; GLuint image_texture_ = 0; int image_width_ = 0, image_height_ = 0; - // Trajectory std::vector trajectory_x_, trajectory_z_; - // Logo bool has_logo_ = false; GLuint logo_texture_ = 0; int logo_width_ = 0, logo_height_ = 0; @@ -297,52 +333,152 @@ class ImGuiVisualizerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr cloud_sub_; }; -// --- STRUKTURA KAMERY DLA OKNA 3D --- -struct Camera3D { - float yaw = -45.0f, pitch = 30.0f, dist = 50.0f; - float target[3] = {0,0,0}; - bool dragging = false, panning = false; - double lx=0, ly=0; - - void update(GLFWwindow* win) { - double mx, my; glfwGetCursorPos(win, &mx, &my); +// --- KLASA DO RENDEROWANIA POINT CLOUD DO TEKSTURY --- +class PointCloudRenderer { +public: + PointCloudRenderer() {} + + bool init(int width, int height) { + width_ = width; + height_ = height; - // Obrót (LPM) - if(glfwGetMouseButton(win, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS) { - if(!dragging) { dragging=true; lx=mx; ly=my; } - yaw += (float)(mx - lx) * 0.5f; - pitch += (float)(my - ly) * 0.5f; - pitch = std::max(-89.0f, std::min(89.0f, pitch)); - lx=mx; ly=my; - } else dragging=false; - - // Przesuwanie (PPM) - if(glfwGetMouseButton(win, GLFW_MOUSE_BUTTON_RIGHT) == GLFW_PRESS) { - if(!panning) { panning=true; lx=mx; ly=my; } - float dx = (float)(mx - lx) * 0.05f; - float dy = (float)(my - ly) * 0.05f; - float rad = glm::radians(yaw); - target[0] -= (dx * cos(rad) - dy * sin(rad)); - target[2] -= (dx * sin(rad) + dy * cos(rad)); - lx=mx; ly=my; - } else panning=false; + // Shadery + prog_viz_ = createShaderProgram(viz_vertex_shader, viz_fragment_shader); + prog_grid_ = createShaderProgram(grid_vertex_shader, grid_fragment_shader); + + // VAO/VBO dla punktów + glGenVertexArrays(1, &vao_pts_); + glGenBuffers(1, &vbo_pts_); + glGenBuffers(1, &cbo_pts_); + + // VAO/VBO dla siatki + glGenVertexArrays(1, &vao_grid_); + std::vector gridV; + int gSize = 100; + for(int i=-gSize; i<=gSize; i+=5) { + gridV.push_back(i); gridV.push_back(0); gridV.push_back(-gSize); + gridV.push_back(i); gridV.push_back(0); gridV.push_back(gSize); + gridV.push_back(-gSize); gridV.push_back(0); gridV.push_back(i); + gridV.push_back(gSize); gridV.push_back(0); gridV.push_back(i); + } + glGenBuffers(1, &vbo_grid_); + glBindVertexArray(vao_grid_); + glBindBuffer(GL_ARRAY_BUFFER, vbo_grid_); + glBufferData(GL_ARRAY_BUFFER, gridV.size()*sizeof(float), gridV.data(), GL_STATIC_DRAW); + glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0); + glEnableVertexAttribArray(0); + grid_vertices_ = gridV.size() / 3; + + // Framebuffer i tekstura + glGenFramebuffers(1, &fbo_); + glGenTextures(1, &texture_); + glGenRenderbuffers(1, &rbo_depth_); + + resize(width, height); + + return true; } - glm::mat4 getView() { - float ry = glm::radians(yaw), rp = glm::radians(pitch); - float cx = target[0] + dist * cos(rp) * sin(ry); - float cy = target[1] + dist * sin(rp); - float cz = target[2] + dist * cos(rp) * cos(ry); - return glm::lookAt(glm::vec3(cx,cy,cz), glm::vec3(target[0],target[1],target[2]), glm::vec3(0,1,0)); + void resize(int width, int height) { + width_ = width; + height_ = height; + + // Tekstura koloru + glBindTexture(GL_TEXTURE_2D, texture_); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width_, height_, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + + // Renderbuffer głębi + glBindRenderbuffer(GL_RENDERBUFFER, rbo_depth_); + glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT, width_, height_); + + // Framebuffer + glBindFramebuffer(GL_FRAMEBUFFER, fbo_); + glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, texture_, 0); + glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, rbo_depth_); + glBindFramebuffer(GL_FRAMEBUFFER, 0); + } + + void render(Camera3D& cam, ImGuiVisualizerNode* node) { + // Renderuj do framebuffera + glBindFramebuffer(GL_FRAMEBUFFER, fbo_); + glViewport(0, 0, width_, height_); + glClearColor(0.15f, 0.15f, 0.15f, 1.0f); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_DEPTH_TEST); + glEnable(GL_PROGRAM_POINT_SIZE); + + glm::mat4 proj = glm::perspective(glm::radians(45.0f), (float)width_/height_, 0.1f, 10000.0f); + glm::mat4 view = cam.getView(); + + // Rysuj siatkę + glUseProgram(prog_grid_); + glUniformMatrix4fv(glGetUniformLocation(prog_grid_, "projection"), 1, GL_FALSE, glm::value_ptr(proj)); + glUniformMatrix4fv(glGetUniformLocation(prog_grid_, "view"), 1, GL_FALSE, glm::value_ptr(view)); + glBindVertexArray(vao_grid_); + glDrawArrays(GL_LINES, 0, grid_vertices_); + + // Aktualizuj dane punktów jeśli są nowe + if(node->new_cloud_available) { + glBindVertexArray(vao_pts_); + glBindBuffer(GL_ARRAY_BUFFER, vbo_pts_); + glBufferData(GL_ARRAY_BUFFER, node->cloud_points_buffer.size()*4, + node->cloud_points_buffer.data(), GL_DYNAMIC_DRAW); + glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0); + glEnableVertexAttribArray(0); + + glBindBuffer(GL_ARRAY_BUFFER, cbo_pts_); + glBufferData(GL_ARRAY_BUFFER, node->cloud_colors_buffer.size()*4, + node->cloud_colors_buffer.data(), GL_DYNAMIC_DRAW); + glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 0, 0); + glEnableVertexAttribArray(1); + + node->new_cloud_available = false; + + // Auto-center + if(cam.target[0]==0 && cam.target[1]==0) { + cam.target[0]=node->center_x; + cam.target[1]=node->center_y; + cam.target[2]=node->center_z; + } + } + + // Rysuj punkty + if(node->getCloudSize() > 0) { + glUseProgram(prog_viz_); + glUniformMatrix4fv(glGetUniformLocation(prog_viz_, "projection"), 1, GL_FALSE, glm::value_ptr(proj)); + glUniformMatrix4fv(glGetUniformLocation(prog_viz_, "view"), 1, GL_FALSE, glm::value_ptr(view)); + glBindVertexArray(vao_pts_); + glDrawArrays(GL_POINTS, 0, node->getCloudSize()); + } + + glBindFramebuffer(GL_FRAMEBUFFER, 0); + } + + GLuint getTexture() const { return texture_; } + + ~PointCloudRenderer() { + if(fbo_) glDeleteFramebuffers(1, &fbo_); + if(texture_) glDeleteTextures(1, &texture_); + if(rbo_depth_) glDeleteRenderbuffers(1, &rbo_depth_); + if(vao_pts_) glDeleteVertexArrays(1, &vao_pts_); + if(vbo_pts_) glDeleteBuffers(1, &vbo_pts_); + if(cbo_pts_) glDeleteBuffers(1, &cbo_pts_); + if(vao_grid_) glDeleteVertexArrays(1, &vao_grid_); + if(vbo_grid_) glDeleteBuffers(1, &vbo_grid_); + if(prog_viz_) glDeleteProgram(prog_viz_); + if(prog_grid_) glDeleteProgram(prog_grid_); } -}; - -Camera3D cam3d; -void scroll_callback(GLFWwindow* w, double x, double y) { - cam3d.dist -= (float)y * 2.0f; - if(cam3d.dist < 0.1f) cam3d.dist = 0.1f; -} +private: + GLuint fbo_ = 0, texture_ = 0, rbo_depth_ = 0; + GLuint vao_pts_ = 0, vbo_pts_ = 0, cbo_pts_ = 0; + GLuint vao_grid_ = 0, vbo_grid_ = 0; + GLuint prog_viz_ = 0, prog_grid_ = 0; + int width_ = 800, height_ = 600; + int grid_vertices_ = 0; +}; int main(int argc, char** argv) { @@ -354,46 +490,13 @@ int main(int argc, char** argv) glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3); glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); - // 1. OKNO GUI - GLFWwindow* winGUI = glfwCreateWindow(1280, 800, "OV2SLAM PowerViz", NULL, NULL); - - // 2. OKNO 3D - GLFWwindow* win3D = glfwCreateWindow(1200, 900, "3D Point Cloud PowerView", NULL, NULL); - - if (!winGUI || !win3D) return -1; + GLFWwindow* window = glfwCreateWindow(1600, 1000, "OV2SLAM PowerViz", NULL, NULL); + if (!window) return -1; - // --- KONFIGURACJA OKNA 3D --- - glfwMakeContextCurrent(win3D); + glfwMakeContextCurrent(window); glewExperimental = GL_TRUE; glewInit(); - glfwSetScrollCallback(win3D, scroll_callback); - - // Shadery dla 3D - GLuint progViz = createShaderProgram(viz_vertex_shader, viz_fragment_shader); - GLuint progGrid = createShaderProgram(grid_vertex_shader, grid_fragment_shader); - - // VAO/VBO dla chmury punktów - GLuint vaoPts, vboPts, cboPts; - glGenVertexArrays(1, &vaoPts); glGenBuffers(1, &vboPts); glGenBuffers(1, &cboPts); - // VAO/VBO dla siatki - GLuint vaoGrid; glGenVertexArrays(1, &vaoGrid); - std::vector gridV; - int gSize=100; - for(int i=-gSize; i<=gSize; i+=5) { - gridV.push_back(i); gridV.push_back(0); gridV.push_back(-gSize); - gridV.push_back(i); gridV.push_back(0); gridV.push_back(gSize); - gridV.push_back(-gSize); gridV.push_back(0); gridV.push_back(i); - gridV.push_back(gSize); gridV.push_back(0); gridV.push_back(i); - } - GLuint vboGrid; glGenBuffers(1, &vboGrid); - glBindVertexArray(vaoGrid); - glBindBuffer(GL_ARRAY_BUFFER, vboGrid); - glBufferData(GL_ARRAY_BUFFER, gridV.size()*sizeof(float), gridV.data(), GL_STATIC_DRAW); - glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0); glEnableVertexAttribArray(0); - - // --- KONFIGURACJA OKNA GUI --- - glfwMakeContextCurrent(winGUI); IMGUI_CHECKVERSION(); ImGui::CreateContext(); ImPlot::CreateContext(); @@ -401,77 +504,26 @@ int main(int argc, char** argv) io.ConfigFlags |= ImGuiConfigFlags_DockingEnable; ImGui::StyleColorsDark(); - ImGui_ImplGlfw_InitForOpenGL(winGUI, true); + ImGui_ImplGlfw_InitForOpenGL(window, true); ImGui_ImplOpenGL3_Init("#version 330"); - // Ładowanie logo node->loadLogo("/ws/src/imgui_app/logo.png"); ImGuiStyle& style = ImGui::GetStyle(); style.Colors[ImGuiCol_WindowBg].w = 0.85f; + + // Inicjalizacja renderera Point Cloud + PointCloudRenderer pc_renderer; + pc_renderer.init(800, 600); + + Camera3D cam3d; + ImVec2 last_mouse_pos(0, 0); - while (!glfwWindowShouldClose(winGUI) && !glfwWindowShouldClose(win3D)) + while (!glfwWindowShouldClose(window)) { rclcpp::spin_some(node); glfwPollEvents(); - // ========================================================= - // 1. RENDEROWANIE OKNA POINT CLOUD - // ========================================================= - glfwMakeContextCurrent(win3D); - cam3d.update(win3D); - - int w3, h3; glfwGetFramebufferSize(win3D, &w3, &h3); - glViewport(0, 0, w3, h3); - glClearColor(0.15f, 0.15f, 0.15f, 1.0f); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glEnable(GL_DEPTH_TEST); - glEnable(GL_PROGRAM_POINT_SIZE); - - glm::mat4 proj = glm::perspective(glm::radians(45.0f), (float)w3/h3, 0.1f, 10000.0f); - glm::mat4 view = cam3d.getView(); - - // Rysuj siatkę - glUseProgram(progGrid); - glUniformMatrix4fv(glGetUniformLocation(progGrid, "projection"), 1, GL_FALSE, glm::value_ptr(proj)); - glUniformMatrix4fv(glGetUniformLocation(progGrid, "view"), 1, GL_FALSE, glm::value_ptr(view)); - glBindVertexArray(vaoGrid); - glDrawArrays(GL_LINES, 0, gridV.size()/3); - - // Rysuj punkty - if(node->new_cloud_available) { - glBindVertexArray(vaoPts); - glBindBuffer(GL_ARRAY_BUFFER, vboPts); - glBufferData(GL_ARRAY_BUFFER, node->cloud_points_buffer.size()*4, node->cloud_points_buffer.data(), GL_DYNAMIC_DRAW); - glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0); glEnableVertexAttribArray(0); - - glBindBuffer(GL_ARRAY_BUFFER, cboPts); - glBufferData(GL_ARRAY_BUFFER, node->cloud_colors_buffer.size()*4, node->cloud_colors_buffer.data(), GL_DYNAMIC_DRAW); - glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 0, 0); glEnableVertexAttribArray(1); - node->new_cloud_available = false; - - // Auto-center - if(cam3d.target[0]==0 && cam3d.target[1]==0) { - cam3d.target[0]=node->center_x; cam3d.target[1]=node->center_y; cam3d.target[2]=node->center_z; - } - } - - if(node->getCloudSize() > 0) { - glUseProgram(progViz); - glUniformMatrix4fv(glGetUniformLocation(progViz, "projection"), 1, GL_FALSE, glm::value_ptr(proj)); - glUniformMatrix4fv(glGetUniformLocation(progViz, "view"), 1, GL_FALSE, glm::value_ptr(view)); - glBindVertexArray(vaoPts); - glDrawArrays(GL_POINTS, 0, node->getCloudSize()); - } - - glfwSwapBuffers(win3D); - - // ========================================================= - // 2. RENDEROWANIE OKNA IMGUI - // ========================================================= - glfwMakeContextCurrent(winGUI); - - // Aktualizuj teksturę obrazu (jeśli jest nowa klatka) node->updateImageTexture(); ImGui_ImplOpenGL3_NewFrame(); @@ -484,7 +536,8 @@ int main(int argc, char** argv) ImGui::SetNextWindowSize(ImGui::GetMainViewport()->Size); ImGui::SetNextWindowBgAlpha(node->getLogoAlpha()); ImGui::Begin("Background", nullptr, - ImGuiWindowFlags_NoDecoration | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_NoBackground | ImGuiWindowFlags_NoBringToFrontOnFocus); + ImGuiWindowFlags_NoDecoration | ImGuiWindowFlags_NoInputs | + ImGuiWindowFlags_NoBackground | ImGuiWindowFlags_NoBringToFrontOnFocus); ImVec2 sz = ImGui::GetWindowSize(); float lh = sz.y * node->getLogoScale(); @@ -492,14 +545,14 @@ int main(int argc, char** argv) float lw = lh * aspect; ImGui::SetCursorPos(ImVec2((sz.x - lw)*0.5f, (sz.y - lh)*0.5f)); - // Naprawiony Image call z 6 argumentami - ImGui::Image((void*)(intptr_t)node->getLogoTexture(), ImVec2(lw, lh), ImVec2(0,0), ImVec2(1,1), ImVec4(1,1,1,1.0f), ImVec4(0,0,0,0)); + ImGui::Image((void*)(intptr_t)node->getLogoTexture(), ImVec2(lw, lh), + ImVec2(0,0), ImVec2(1,1), ImVec4(1,1,1,1.0f), ImVec4(0,0,0,0)); ImGui::End(); } ImGui::DockSpaceOverViewport(0, ImGui::GetMainViewport()); - // Okno z pozycją kamery + // OKNO 1: POZYCJA KAMERY ImGui::Begin("Camera Position"); ImGui::Text("Position:"); ImGui::Text(" X: %.3f m", node->getCameraX()); @@ -512,7 +565,7 @@ int main(int argc, char** argv) ImGui::Text("Distance:"); ImGui::Text(" Linear: %.3f m", node->getDistanceLinear()); ImGui::Separator(); - ImGui::Text("3D View Status:"); + ImGui::Text("Point Cloud:"); ImGui::Text(" Points: %zu", node->getCloudSize()); ImGui::Text(" Center: %.1f %.1f %.1f", node->center_x, node->center_y, node->center_z); ImGui::End(); @@ -534,7 +587,6 @@ int main(int argc, char** argv) const auto& traj_x = node->getTrajectoryX(); const auto& traj_z = node->getTrajectoryZ(); - // Przycisk zapisu trajektorii if (ImGui::Button("Save Trajectory to CSV")) { std::string filename = "/ws/trajectories/trajectory_" + std::to_string(std::chrono::system_clock::now().time_since_epoch().count()) + @@ -553,7 +605,6 @@ int main(int argc, char** argv) ImPlot::PlotLine("Path", traj_x.data(), traj_z.data(), traj_x.size()); - // Rysuj aktualną pozycję jako punkt if (!traj_x.empty()) { float current_x = traj_x.back(); float current_z = traj_z.back(); @@ -567,15 +618,69 @@ int main(int argc, char** argv) } ImGui::End(); + // OKNO 4: POINT CLOUD 3D + ImGui::Begin("3D Point Cloud View"); + + ImVec2 region = ImGui::GetContentRegionAvail(); + + // Sprawdź czy region jest prawidłowy (nie zerowy) + if(region.x > 50 && region.y > 50) { + pc_renderer.resize((int)region.x, (int)region.y); + + // Renderuj Point Cloud + pc_renderer.render(cam3d, node.get()); + + // Wyświetl jako teksturę (INVISIBLE_BUTTON przechwytuje kliknięcia myszy) + ImGui::InvisibleButton("##pc_canvas", region); + ImVec2 p_min = ImGui::GetItemRectMin(); + ImVec2 p_max = ImGui::GetItemRectMax(); + ImDrawList* draw_list = ImGui::GetWindowDrawList(); + draw_list->AddImage((void*)(intptr_t)pc_renderer.getTexture(), + p_min, p_max, ImVec2(0,1), ImVec2(1,0)); + + // Sprawdź czy mysz jest nad obrazem + bool is_over_image = ImGui::IsItemHovered(); + + cam3d.mouse_over_window = is_over_image; + + // Obsługa myszy - tylko gdy jest nad obrazem + ImVec2 mouse_pos = ImGui::GetMousePos(); + if(is_over_image) { + ImVec2 mouse_delta(mouse_pos.x - last_mouse_pos.x, mouse_pos.y - last_mouse_pos.y); + bool left_down = ImGui::IsMouseDown(ImGuiMouseButton_Left); + bool right_down = ImGui::IsMouseDown(ImGuiMouseButton_Right); + + cam3d.update(mouse_pos, left_down, right_down, mouse_delta); + + // Scroll dla zoomu + float wheel = ImGui::GetIO().MouseWheel; + if(wheel != 0.0f) { + cam3d.scroll(wheel); + } + } + last_mouse_pos = mouse_pos; + } else { + // Okno jest za małe lub jeszcze się inicjalizuje + ImGui::Text("Resize window to view Point Cloud..."); + } + + ImGui::Text("Controls:"); + ImGui::BulletText("Left Mouse: Rotate"); + ImGui::BulletText("Right Mouse: Pan"); + ImGui::BulletText("Scroll: Zoom"); + + ImGui::End(); + // RENDER GUI ImGui::Render(); - int wg, hg; glfwGetFramebufferSize(winGUI, &wg, &hg); - glViewport(0, 0, wg, hg); + int display_w, display_h; + glfwGetFramebufferSize(window, &display_w, &display_h); + glViewport(0, 0, display_w, display_h); glClearColor(0.1f, 0.1f, 0.12f, 1.0f); glClear(GL_COLOR_BUFFER_BIT); ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData()); - glfwSwapBuffers(winGUI); + glfwSwapBuffers(window); } ImGui_ImplOpenGL3_Shutdown(); @@ -583,13 +688,7 @@ int main(int argc, char** argv) ImPlot::DestroyContext(); ImGui::DestroyContext(); - glDeleteVertexArrays(1, &vaoPts); - glDeleteBuffers(1, &vboPts); - glDeleteBuffers(1, &cboPts); - glDeleteProgram(progViz); - - glfwDestroyWindow(winGUI); - glfwDestroyWindow(win3D); + glfwDestroyWindow(window); glfwTerminate(); rclcpp::shutdown();