From 655ae4ef6644e3cb466f160706f3c767be1a72cf Mon Sep 17 00:00:00 2001 From: Kee Jin Date: Sun, 9 Mar 2025 17:48:57 +0800 Subject: [PATCH 1/5] pose_graph_ros: removed old unused pose_graph_node impl file --- pose_graph_ros/src/old/pose_graph_node.cpp | 768 --------------------- 1 file changed, 768 deletions(-) delete mode 100644 pose_graph_ros/src/old/pose_graph_node.cpp diff --git a/pose_graph_ros/src/old/pose_graph_node.cpp b/pose_graph_ros/src/old/pose_graph_node.cpp deleted file mode 100644 index c794a5e77..000000000 --- a/pose_graph_ros/src/old/pose_graph_node.cpp +++ /dev/null @@ -1,768 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "pose_graph/details/keyframe.h" -#include "pose_graph/parameters.h" -#include "pose_graph/pose_graph.h" -#include "pose_graph/utility/tic_toc.h" -#include "pose_graph_ros/utility/CameraPoseVisualization.h" - -#define SKIP_FIRST_CNT 10 -#define SAVE_LOOP_PATH true -using namespace std; - -queue image_buf; -queue point_buf; -queue pose_buf; -queue odometry_buf; -std::mutex m_buf; -std::mutex m_process; -int frame_index = 0; -int sequence = 1; -PoseGraph posegraph; -int skip_first_cnt = 0; -int SKIP_CNT; -int skip_cnt = 0; -bool load_flag = 0; -bool start_flag = 0; -double SKIP_DIS = 0; -nav_msgs::Path path[10]; -nav_msgs::Path base_path; -CameraPoseVisualization* posegraph_visualization; - -int VISUALIZATION_SHIFT_X; -int VISUALIZATION_SHIFT_Y; -int ROW; -int COL; -int DEBUG_IMAGE; -int VISUALIZE_IMU_FORWARD; -int LOOP_CLOSURE; -int FAST_RELOCALIZATION; - -camodocal::CameraPtr m_camera; -Eigen::Vector3d tic; -Eigen::Matrix3d qic; -ros::Publisher pub_match_points; -ros::Publisher pub_camera_pose_visual; -ros::Publisher pub_key_odometrys; -ros::Publisher pub_vio_path; -ros::Publisher pub_pg_path; -ros::Publisher pub_base_path; -ros::Publisher pub_pose_graph; -ros::Publisher pub_path[10]; -ros::Publisher pub_match_img; -nav_msgs::Path no_loop_path; - -std::string BRIEF_PATTERN_FILE; -std::string POSE_GRAPH_SAVE_PATH; -std::string VINS_RESULT_PATH; -CameraPoseVisualization cameraposevisual(1, 0, 0, 1); -Eigen::Vector3d last_t(-100, -100, -100); -double last_image_time = -1; - -void publish() { - int sequence_cnt = posegraph.getCurrentSequenceCount(); - for (int i = 1; i <= sequence_cnt; i++) { - // if (sequence_loop[i] == true || i == base_sequence) - if (true) { - pub_pg_path.publish(path[i]); - pub_path[i].publish(path[i]); - posegraph_visualization->publish_by(pub_pose_graph, - path[sequence_cnt].header); - } - } - base_path.header.frame_id = "world"; - pub_base_path.publish(base_path); - // posegraph_visualization->publish_by(pub_pose_graph, - // path[sequence_cnt].header); -} - -void new_sequence() { - printf("new sequence\n"); - sequence++; - printf("sequence cnt %d \n", sequence); - if (sequence > 5) { - ROS_WARN( - "only support 5 sequences since it's boring to copy code for more " - "sequences."); - ROS_BREAK(); - } - posegraph_visualization->reset(); - publish(); - m_buf.lock(); - while (!image_buf.empty()) image_buf.pop(); - while (!point_buf.empty()) point_buf.pop(); - while (!pose_buf.empty()) pose_buf.pop(); - while (!odometry_buf.empty()) odometry_buf.pop(); - m_buf.unlock(); -} - -void on_keyframe_loaded_callback(KeyFrame* keyframe, - int pose_graph_load_count) { - Vector3d P; - Matrix3d R; - keyframe->getPose(P, R); - Quaterniond Q{R}; - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header.stamp = ros::Time(keyframe->time_stamp); - pose_stamped.header.frame_id = "world"; - pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X; - pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y; - pose_stamped.pose.position.z = P.z(); - pose_stamped.pose.orientation.x = Q.x(); - pose_stamped.pose.orientation.y = Q.y(); - pose_stamped.pose.orientation.z = Q.z(); - pose_stamped.pose.orientation.w = Q.w(); - base_path.poses.push_back(pose_stamped); - base_path.header = pose_stamped.header; - - if (pose_graph_load_count % 20 == 0) { - publish(); - } -} - -void on_optimization_step_completed_callback( - std::list keyframelist) { - list::iterator it; - int sequence_cnt = posegraph.getCurrentSequenceCount(); - for (int i = 1; i <= sequence_cnt; i++) { - path[i].poses.clear(); - } - base_path.poses.clear(); - posegraph_visualization->reset(); - - if (SAVE_LOOP_PATH) { - ofstream loop_path_file_tmp(VINS_RESULT_PATH, ios::out); - loop_path_file_tmp.close(); - } - - for (it = keyframelist.begin(); it != keyframelist.end(); it++) { - Vector3d P; - Matrix3d R; - (*it)->getPose(P, R); - Quaterniond Q; - Q = R; - // printf("path p: %f, %f, %f\n", P.x(), P.z(), P.y() ); - - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header.stamp = ros::Time((*it)->time_stamp); - pose_stamped.header.frame_id = "world"; - pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X; - pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y; - pose_stamped.pose.position.z = P.z(); - pose_stamped.pose.orientation.x = Q.x(); - pose_stamped.pose.orientation.y = Q.y(); - pose_stamped.pose.orientation.z = Q.z(); - pose_stamped.pose.orientation.w = Q.w(); - if ((*it)->sequence == 0) { - base_path.poses.push_back(pose_stamped); - base_path.header = pose_stamped.header; - } else { - path[(*it)->sequence].poses.push_back(pose_stamped); - path[(*it)->sequence].header = pose_stamped.header; - } - - if (SAVE_LOOP_PATH) { - ofstream loop_path_file(VINS_RESULT_PATH, ios::app); - loop_path_file.setf(ios::fixed, ios::floatfield); - loop_path_file.precision(0); - loop_path_file << (*it)->time_stamp * 1e9 << ","; - loop_path_file.precision(5); - loop_path_file << P.x() << "," << P.y() << "," << P.z() << "," << Q.w() - << "," << Q.x() << "," << Q.y() << "," << Q.z() << "," - << endl; - loop_path_file.close(); - } - - if (SHOW_S_EDGE) { - list::reverse_iterator rit = keyframelist.rbegin(); - list::reverse_iterator lrit; - for (; rit != keyframelist.rend(); rit++) { - if ((*rit)->index == (*it)->index) { - lrit = rit; - lrit++; - for (int i = 0; i < 4; i++) { - if (lrit == keyframelist.rend()) break; - if ((*lrit)->sequence == (*it)->sequence) { - Vector3d conncected_P; - Matrix3d connected_R; - (*lrit)->getPose(conncected_P, connected_R); - posegraph_visualization->add_edge(P, conncected_P); - } - lrit++; - } - break; - } - } - } - if (SHOW_L_EDGE) { - if ((*it)->has_loop && (*it)->sequence == sequence_cnt) { - KeyFrame* connected_KF = posegraph.getKeyFrame((*it)->loop_index); - Vector3d connected_P; - Matrix3d connected_R; - connected_KF->getPose(connected_P, connected_R); - //(*it)->getVioPose(P, R); - (*it)->getPose(P, R); - if ((*it)->sequence > 0) { - posegraph_visualization->add_loopedge( - P, connected_P + - Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0)); - } - } - } - } - - publish(); -} - -void on_new_edge_callback(Vector3d p1, Vector3d p2) { - posegraph_visualization->add_edge(p1, p2); -} - -void on_new_loopedge_callback(Vector3d p1, Vector3d p2) { - posegraph_visualization->add_loopedge(p1, p2); -} - -void on_keyframe_connection_found_callback( - KeyFrame* cur_kf, KeyFrame* old_kf, - vector& matched_2d_old_norm, vector& matched_id) { - { - sensor_msgs::ImagePtr msg = - cv_bridge::CvImage(std_msgs::Header(), "bgr8", cur_kf->getThumbImage()) - .toImageMsg(); - msg->header.stamp = ros::Time(cur_kf->time_stamp); - pub_match_img.publish(msg); - } - if (FAST_RELOCALIZATION) { - sensor_msgs::PointCloud msg_match_points; - msg_match_points.header.stamp = ros::Time(cur_kf->time_stamp); - for (int i = 0; i < (int)matched_2d_old_norm.size(); i++) { - geometry_msgs::Point32 p; - p.x = matched_2d_old_norm[i].x; - p.y = matched_2d_old_norm[i].y; - p.z = matched_id[i]; - msg_match_points.points.push_back(p); - } - Eigen::Vector3d T = old_kf->T_w_i; - Eigen::Matrix3d R = old_kf->R_w_i; - Quaterniond Q(R); - sensor_msgs::ChannelFloat32 t_q_index; - t_q_index.values.push_back(T.x()); - t_q_index.values.push_back(T.y()); - t_q_index.values.push_back(T.z()); - t_q_index.values.push_back(Q.w()); - t_q_index.values.push_back(Q.x()); - t_q_index.values.push_back(Q.y()); - t_q_index.values.push_back(Q.z()); - t_q_index.values.push_back(cur_kf->index); - msg_match_points.channels.push_back(t_q_index); - pub_match_points.publish(msg_match_points); - } -} - -void image_callback(const sensor_msgs::ImageConstPtr& image_msg) { - // ROS_INFO("image_callback!"); - if (!LOOP_CLOSURE) return; - m_buf.lock(); - image_buf.push(image_msg); - m_buf.unlock(); - // printf(" image time %f \n", image_msg->header.stamp.toSec()); - - // detect unstable camera stream - if (last_image_time == -1) - last_image_time = image_msg->header.stamp.toSec(); - else if (image_msg->header.stamp.toSec() - last_image_time > 1.0 || - image_msg->header.stamp.toSec() < last_image_time) { - ROS_WARN("image discontinue! detect a new sequence!"); - new_sequence(); - } - last_image_time = image_msg->header.stamp.toSec(); -} - -void point_callback(const sensor_msgs::PointCloudConstPtr& point_msg) { - // ROS_INFO("point_callback!"); - if (!LOOP_CLOSURE) return; - m_buf.lock(); - point_buf.push(point_msg); - m_buf.unlock(); - /* - for (unsigned int i = 0; i < point_msg->points.size(); i++) - { - printf("%d, 3D point: %f, %f, %f 2D point %f, %f \n",i , - point_msg->points[i].x, point_msg->points[i].y, point_msg->points[i].z, - point_msg->channels[i].values[0], - point_msg->channels[i].values[1]); - } - */ -} - -void pose_callback(const nav_msgs::Odometry::ConstPtr& pose_msg) { - // ROS_INFO("pose_callback!"); - if (!LOOP_CLOSURE) return; - m_buf.lock(); - pose_buf.push(pose_msg); - m_buf.unlock(); - /* - printf("pose t: %f, %f, %f q: %f, %f, %f %f \n", - pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, - pose_msg->pose.pose.position.z, - pose_msg->pose.pose.orientation.w, - pose_msg->pose.pose.orientation.x, - pose_msg->pose.pose.orientation.y, - pose_msg->pose.pose.orientation.z); - */ -} - -void imu_forward_callback(const nav_msgs::Odometry::ConstPtr& forward_msg) { - if (VISUALIZE_IMU_FORWARD) { - Vector3d vio_t(forward_msg->pose.pose.position.x, - forward_msg->pose.pose.position.y, - forward_msg->pose.pose.position.z); - Quaterniond vio_q; - vio_q.w() = forward_msg->pose.pose.orientation.w; - vio_q.x() = forward_msg->pose.pose.orientation.x; - vio_q.y() = forward_msg->pose.pose.orientation.y; - vio_q.z() = forward_msg->pose.pose.orientation.z; - - vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio; - vio_q = posegraph.w_r_vio * vio_q; - - vio_t = posegraph.r_drift * vio_t + posegraph.t_drift; - vio_q = posegraph.r_drift * vio_q; - - Vector3d vio_t_cam; - Quaterniond vio_q_cam; - vio_t_cam = vio_t + vio_q * tic; - vio_q_cam = vio_q * qic; - - cameraposevisual.reset(); - cameraposevisual.add_pose(vio_t_cam, vio_q_cam); - cameraposevisual.publish_by(pub_camera_pose_visual, forward_msg->header); - } -} -void relo_relative_pose_callback(const nav_msgs::Odometry::ConstPtr& pose_msg) { - Vector3d relative_t = - Vector3d(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, - pose_msg->pose.pose.position.z); - Quaterniond relative_q; - relative_q.w() = pose_msg->pose.pose.orientation.w; - relative_q.x() = pose_msg->pose.pose.orientation.x; - relative_q.y() = pose_msg->pose.pose.orientation.y; - relative_q.z() = pose_msg->pose.pose.orientation.z; - double relative_yaw = pose_msg->twist.twist.linear.x; - int index = pose_msg->twist.twist.linear.y; - // printf("receive index %d \n", index ); - Eigen::Matrix loop_info; - loop_info << relative_t.x(), relative_t.y(), relative_t.z(), relative_q.w(), - relative_q.x(), relative_q.y(), relative_q.z(), relative_yaw; - posegraph.updateKeyFrameLoop(index, loop_info); -} - -void vio_callback(const nav_msgs::Odometry::ConstPtr& pose_msg) { - // ROS_INFO("vio_callback!"); - Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, - pose_msg->pose.pose.position.z); - Quaterniond vio_q; - vio_q.w() = pose_msg->pose.pose.orientation.w; - vio_q.x() = pose_msg->pose.pose.orientation.x; - vio_q.y() = pose_msg->pose.pose.orientation.y; - vio_q.z() = pose_msg->pose.pose.orientation.z; - - vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio; - vio_q = posegraph.w_r_vio * vio_q; - - vio_t = posegraph.r_drift * vio_t + posegraph.t_drift; - vio_q = posegraph.r_drift * vio_q; - - Vector3d vio_t_cam; - Quaterniond vio_q_cam; - vio_t_cam = vio_t + vio_q * tic; - vio_q_cam = vio_q * qic; - - if (!VISUALIZE_IMU_FORWARD) { - cameraposevisual.reset(); - cameraposevisual.add_pose(vio_t_cam, vio_q_cam); - cameraposevisual.publish_by(pub_camera_pose_visual, pose_msg->header); - } - - odometry_buf.push(vio_t_cam); - if (odometry_buf.size() > 10) { - odometry_buf.pop(); - } - - visualization_msgs::Marker key_odometrys; - key_odometrys.header = pose_msg->header; - key_odometrys.header.frame_id = "world"; - key_odometrys.ns = "key_odometrys"; - key_odometrys.type = visualization_msgs::Marker::SPHERE_LIST; - key_odometrys.action = visualization_msgs::Marker::ADD; - key_odometrys.pose.orientation.w = 1.0; - key_odometrys.lifetime = ros::Duration(); - - // static int key_odometrys_id = 0; - key_odometrys.id = 0; // key_odometrys_id++; - key_odometrys.scale.x = 0.1; - key_odometrys.scale.y = 0.1; - key_odometrys.scale.z = 0.1; - key_odometrys.color.r = 1.0; - key_odometrys.color.a = 1.0; - - for (unsigned int i = 0; i < odometry_buf.size(); i++) { - geometry_msgs::Point pose_marker; - Vector3d vio_t; - vio_t = odometry_buf.front(); - odometry_buf.pop(); - pose_marker.x = vio_t.x(); - pose_marker.y = vio_t.y(); - pose_marker.z = vio_t.z(); - key_odometrys.points.push_back(pose_marker); - odometry_buf.push(vio_t); - } - pub_key_odometrys.publish(key_odometrys); - - if (!LOOP_CLOSURE) { - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header = pose_msg->header; - pose_stamped.header.frame_id = "world"; - pose_stamped.pose.position.x = vio_t.x(); - pose_stamped.pose.position.y = vio_t.y(); - pose_stamped.pose.position.z = vio_t.z(); - no_loop_path.header = pose_msg->header; - no_loop_path.header.frame_id = "world"; - no_loop_path.poses.push_back(pose_stamped); - pub_vio_path.publish(no_loop_path); - } -} - -void extrinsic_callback(const nav_msgs::Odometry::ConstPtr& pose_msg) { - m_process.lock(); - tic = Vector3d(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, - pose_msg->pose.pose.position.z); - qic = Quaterniond(pose_msg->pose.pose.orientation.w, - pose_msg->pose.pose.orientation.x, - pose_msg->pose.pose.orientation.y, - pose_msg->pose.pose.orientation.z) - .toRotationMatrix(); - m_process.unlock(); -} - -void process() { - if (!LOOP_CLOSURE) return; - while (true) { - sensor_msgs::ImageConstPtr image_msg = NULL; - sensor_msgs::PointCloudConstPtr point_msg = NULL; - nav_msgs::Odometry::ConstPtr pose_msg = NULL; - - // find out the messages with same time stamp - m_buf.lock(); - if (!image_buf.empty() && !point_buf.empty() && !pose_buf.empty()) { - if (image_buf.front()->header.stamp.toSec() > - pose_buf.front()->header.stamp.toSec()) { - pose_buf.pop(); - printf("throw pose at beginning\n"); - } else if (image_buf.front()->header.stamp.toSec() > - point_buf.front()->header.stamp.toSec()) { - point_buf.pop(); - printf("throw point at beginning\n"); - } else if (image_buf.back()->header.stamp.toSec() >= - pose_buf.front()->header.stamp.toSec() && - point_buf.back()->header.stamp.toSec() >= - pose_buf.front()->header.stamp.toSec()) { - pose_msg = pose_buf.front(); - pose_buf.pop(); - while (!pose_buf.empty()) pose_buf.pop(); - while (image_buf.front()->header.stamp.toSec() < - pose_msg->header.stamp.toSec()) - image_buf.pop(); - image_msg = image_buf.front(); - image_buf.pop(); - - while (point_buf.front()->header.stamp.toSec() < - pose_msg->header.stamp.toSec()) - point_buf.pop(); - point_msg = point_buf.front(); - point_buf.pop(); - } - } - m_buf.unlock(); - - if (pose_msg != NULL) { - // printf(" pose time %f \n", pose_msg->header.stamp.toSec()); - // printf(" point time %f \n", point_msg->header.stamp.toSec()); - // printf(" image time %f \n", image_msg->header.stamp.toSec()); - // skip fisrt few - if (skip_first_cnt < SKIP_FIRST_CNT) { - skip_first_cnt++; - continue; - } - - if (skip_cnt < SKIP_CNT) { - skip_cnt++; - continue; - } else { - skip_cnt = 0; - } - - cv_bridge::CvImageConstPtr ptr; - if (image_msg->encoding == "8UC1") { - sensor_msgs::Image img; - img.header = image_msg->header; - img.height = image_msg->height; - img.width = image_msg->width; - img.is_bigendian = image_msg->is_bigendian; - img.step = image_msg->step; - img.data = image_msg->data; - img.encoding = "mono8"; - ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8); - } else - ptr = - cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8); - - cv::Mat image = ptr->image; - // build keyframe - Vector3d T = Vector3d(pose_msg->pose.pose.position.x, - pose_msg->pose.pose.position.y, - pose_msg->pose.pose.position.z); - Matrix3d R = Quaterniond(pose_msg->pose.pose.orientation.w, - pose_msg->pose.pose.orientation.x, - pose_msg->pose.pose.orientation.y, - pose_msg->pose.pose.orientation.z) - .toRotationMatrix(); - if ((T - last_t).norm() > SKIP_DIS) { - vector point_3d; - vector point_2d_uv; - vector point_2d_normal; - vector point_id; - - for (unsigned int i = 0; i < point_msg->points.size(); i++) { - cv::Point3f p_3d; - p_3d.x = point_msg->points[i].x; - p_3d.y = point_msg->points[i].y; - p_3d.z = point_msg->points[i].z; - point_3d.push_back(p_3d); - - cv::Point2f p_2d_uv, p_2d_normal; - double p_id; - p_2d_normal.x = point_msg->channels[i].values[0]; - p_2d_normal.y = point_msg->channels[i].values[1]; - p_2d_uv.x = point_msg->channels[i].values[2]; - p_2d_uv.y = point_msg->channels[i].values[3]; - p_id = point_msg->channels[i].values[4]; - point_2d_normal.push_back(p_2d_normal); - point_2d_uv.push_back(p_2d_uv); - point_id.push_back(p_id); - - // printf("u %f, v %f \n", p_2d_uv.x, p_2d_uv.y); - } - - KeyFrame* keyframe = new KeyFrame( - pose_msg->header.stamp.toSec(), frame_index, T, R, image, point_3d, - point_2d_uv, point_2d_normal, point_id, sequence); - m_process.lock(); - start_flag = 1; - posegraph.addKeyFrame(keyframe, 1); - { - int sequence_cnt = posegraph.getCurrentSequenceCount(); - Vector3d P; - Matrix3d R; - keyframe->getPose(P, R); - Quaterniond Q{R}; - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header.stamp = ros::Time(keyframe->time_stamp); - pose_stamped.header.frame_id = "world"; - pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X; - pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y; - pose_stamped.pose.position.z = P.z(); - pose_stamped.pose.orientation.x = Q.x(); - pose_stamped.pose.orientation.y = Q.y(); - pose_stamped.pose.orientation.z = Q.z(); - pose_stamped.pose.orientation.w = Q.w(); - path[sequence_cnt].poses.push_back(pose_stamped); - path[sequence_cnt].header = pose_stamped.header; - - if (SAVE_LOOP_PATH) { - ofstream loop_path_file(VINS_RESULT_PATH, ios::app); - loop_path_file.setf(ios::fixed, ios::floatfield); - loop_path_file.precision(0); - loop_path_file << keyframe->time_stamp * 1e9 << ","; - loop_path_file.precision(5); - loop_path_file << P.x() << "," << P.y() << "," << P.z() << "," - << Q.w() << "," << Q.x() << "," << Q.y() << "," - << Q.z() << "," << endl; - loop_path_file.close(); - } - } - publish(); - m_process.unlock(); - frame_index++; - last_t = T; - } - } - - std::chrono::milliseconds dura(5); - std::this_thread::sleep_for(dura); - } -} - -void command() { - if (!LOOP_CLOSURE) return; - while (1) { - char c = getchar(); - if (c == 's') { - m_process.lock(); - posegraph.savePoseGraph(); - m_process.unlock(); - printf( - "save pose graph finish\nyou can set 'load_previous_pose_graph' to 1 " - "in the config file to reuse it next time\n"); - // printf("program shutting down...\n"); - // ros::shutdown(); - } - if (c == 'n') new_sequence(); - - std::chrono::milliseconds dura(5); - std::this_thread::sleep_for(dura); - } -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "pose_graph_ros"); - ros::NodeHandle n("~"); - // posegraph.registerPub(n); - - // OnPoseGraphLoaded - posegraph.setOnKeyFrameLoadedCallback(on_keyframe_loaded_callback); - - // OnPoseGraphOptimization - posegraph.setOnOptimizationStepCompletedCallback( - on_optimization_step_completed_callback); - - // OnKeyFrameAdded - posegraph.setOnNewEdgeCallback(on_new_edge_callback); - posegraph.setOnNewLoopEdgeCallback(on_new_loopedge_callback); - posegraph.setOnKeyFrameConnectionFoundCallback( - on_keyframe_connection_found_callback); - - posegraph_visualization = new CameraPoseVisualization(1.0, 0.0, 1.0, 1.0); - posegraph_visualization->setScale(0.1); - posegraph_visualization->setLineWidth(0.01); - - // read param - n.getParam("visualization_shift_x", VISUALIZATION_SHIFT_X); - n.getParam("visualization_shift_y", VISUALIZATION_SHIFT_Y); - n.getParam("skip_cnt", SKIP_CNT); - n.getParam("skip_dis", SKIP_DIS); - std::string config_file; - n.getParam("config_file", config_file); - cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); - if (!fsSettings.isOpened()) { - std::cerr << "ERROR: Wrong path to settings" << std::endl; - } - - double camera_visual_size = fsSettings["visualize_camera_size"]; - cameraposevisual.setScale(camera_visual_size); - cameraposevisual.setLineWidth(camera_visual_size / 10.0); - - LOOP_CLOSURE = fsSettings["loop_closure"]; - std::string IMAGE_TOPIC; - int LOAD_PREVIOUS_POSE_GRAPH; - if (LOOP_CLOSURE) { - ROW = fsSettings["image_height"]; - COL = fsSettings["image_width"]; - std::string pkg_path = ros::package::getPath("pose_graph_ros"); - string vocabulary_file = pkg_path + "/../support_files/brief_k10L6.bin"; - cout << "vocabulary_file" << vocabulary_file << endl; - posegraph.loadVocabulary(vocabulary_file); - - BRIEF_PATTERN_FILE = pkg_path + "/../support_files/brief_pattern.yml"; - cout << "BRIEF_PATTERN_FILE" << BRIEF_PATTERN_FILE << endl; - m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile( - config_file.c_str()); - - fsSettings["image_topic"] >> IMAGE_TOPIC; - fsSettings["pose_graph_save_path"] >> POSE_GRAPH_SAVE_PATH; - fsSettings["output_path"] >> VINS_RESULT_PATH; - fsSettings["save_image"] >> DEBUG_IMAGE; - - // create folder if not exists - FileSystemHelper::createDirectoryIfNotExists(POSE_GRAPH_SAVE_PATH.c_str()); - FileSystemHelper::createDirectoryIfNotExists(VINS_RESULT_PATH.c_str()); - - VISUALIZE_IMU_FORWARD = fsSettings["visualize_imu_forward"]; - LOAD_PREVIOUS_POSE_GRAPH = fsSettings["load_previous_pose_graph"]; - FAST_RELOCALIZATION = fsSettings["fast_relocalization"]; - VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.csv"; - std::ofstream fout(VINS_RESULT_PATH, std::ios::out); - fout.close(); - fsSettings.release(); - - if (LOAD_PREVIOUS_POSE_GRAPH) { - printf("load pose graph\n"); - m_process.lock(); - posegraph.loadPoseGraph(); - m_process.unlock(); - printf("load pose graph finish\n"); - load_flag = 1; - } else { - printf("no previous pose graph\n"); - load_flag = 1; - } - } - - fsSettings.release(); - - ros::Subscriber sub_imu_forward = - n.subscribe("/vins_estimator/imu_propagate", 2000, imu_forward_callback); - ros::Subscriber sub_vio = - n.subscribe("/vins_estimator/odometry", 2000, vio_callback); - ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 2000, image_callback); - ros::Subscriber sub_pose = - n.subscribe("/vins_estimator/keyframe_pose", 2000, pose_callback); - ros::Subscriber sub_extrinsic = - n.subscribe("/vins_estimator/extrinsic", 2000, extrinsic_callback); - ros::Subscriber sub_point = - n.subscribe("/vins_estimator/keyframe_point", 2000, point_callback); - ros::Subscriber sub_relo_relative_pose = n.subscribe( - "/vins_estimator/relo_relative_pose", 2000, relo_relative_pose_callback); - - pub_match_img = n.advertise("match_image", 1000); - pub_camera_pose_visual = - n.advertise("camera_pose_visual", 1000); - pub_key_odometrys = - n.advertise("key_odometrys", 1000); - pub_vio_path = n.advertise("no_loop_path", 1000); - pub_match_points = n.advertise("match_points", 100); - pub_pg_path = n.advertise("pose_graph_path", 1000); - pub_base_path = n.advertise("base_path", 1000); - pub_pose_graph = - n.advertise("pose_graph", 1000); - for (int i = 1; i < 10; i++) - pub_path[i] = n.advertise("path_" + to_string(i), 1000); - - std::thread measurement_process; - std::thread keyboard_command_process; - - measurement_process = std::thread(process); - keyboard_command_process = std::thread(command); - - ros::spin(); - - return 0; -} From 12e428089ff5e92078c207f36633ccef85a9ce11 Mon Sep 17 00:00:00 2001 From: Kee Jin Date: Sun, 9 Mar 2025 21:55:08 +0800 Subject: [PATCH 2/5] [#37] pose_graph: updated PoseGraphEventObserver interface --- .../details/pose_graph_event_observer.hpp | 10 +-- pose_graph/src/pose_graph.cpp | 9 ++- .../pose_graph_ros/pose_graph_node.hpp | 74 +++++++------------ 3 files changed, 36 insertions(+), 57 deletions(-) diff --git a/pose_graph/include/pose_graph/details/pose_graph_event_observer.hpp b/pose_graph/include/pose_graph/details/pose_graph_event_observer.hpp index f43ee37d5..cb56642ed 100644 --- a/pose_graph/include/pose_graph/details/pose_graph_event_observer.hpp +++ b/pose_graph/include/pose_graph/details/pose_graph_event_observer.hpp @@ -17,24 +17,24 @@ #include "pose_graph/details/keyframe.h" namespace pose_graph { -class PoseGraphEventObserver - : public std::enable_shared_from_this { +class PoseGraphEventObserver { public: PoseGraphEventObserver() = default; virtual ~PoseGraphEventObserver() = default; virtual void OnPoseGraphLoaded() = 0; virtual void OnPoseGraphSaved() = 0; - virtual void OnKeyFrameAdded(KeyFrame::Attributes kf_attribute) = 0; + virtual void OnKeyFrameAdded(KeyFrame::Attributes kf_attribute, + int sequence_count) = 0; virtual void OnKeyFrameLoaded(KeyFrame::Attributes kf_attribute, - int count) = 0; + int count, int sequence_count) = 0; virtual void OnKeyFrameConnectionFound( KeyFrame::Attributes current_kf_attribute, KeyFrame::Attributes old_kf_attribute, std::vector matched_2d_old_norm, std::vector matched_id, cv::Mat& thumb_image) = 0; virtual void OnPoseGraphOptimization( - std::vector kf_attributes) = 0; + std::vector kf_attributes, int sequence_count) = 0; virtual void OnNewSequentialEdge(Eigen::Vector3d p1, Eigen::Vector3d p2) = 0; virtual void OnNewLoopEdge(Eigen::Vector3d p1, Eigen::Vector3d p2) = 0; }; diff --git a/pose_graph/src/pose_graph.cpp b/pose_graph/src/pose_graph.cpp index 1b4b5f1e3..ae4b24135 100644 --- a/pose_graph/src/pose_graph.cpp +++ b/pose_graph/src/pose_graph.cpp @@ -241,7 +241,8 @@ bool PoseGraph::Load() { } } if (event_observer_) { - event_observer_->OnKeyFrameLoaded(current_kf_attribute, count); + event_observer_->OnKeyFrameLoaded(current_kf_attribute, count, + GetCurrentSequenceCount()); } count++; } @@ -489,7 +490,8 @@ void PoseGraph::AddKeyFrame(std::shared_ptr current_keyframe) { } // Generic callback - event_observer_->OnKeyFrameAdded(current_keyframe->getAttributes()); + event_observer_->OnKeyFrameAdded(current_keyframe->getAttributes(), + GetCurrentSequenceCount()); } void PoseGraph::AddKeyFrame(std::shared_ptr current_keyframe, @@ -1008,7 +1010,8 @@ void PoseGraph::StartOptimizationThread() { Optimize4DoF(); auto kf_attributes = GetKeyFrameAttributes(); if (event_observer_) { - event_observer_->OnPoseGraphOptimization(kf_attributes); + event_observer_->OnPoseGraphOptimization(kf_attributes, + GetCurrentSequenceCount()); } std::this_thread::sleep_for(std::chrono::milliseconds(2000)); } diff --git a/pose_graph_ros/include/pose_graph_ros/pose_graph_node.hpp b/pose_graph_ros/include/pose_graph_ros/pose_graph_node.hpp index 8413c285f..e0a033b66 100644 --- a/pose_graph_ros/include/pose_graph_ros/pose_graph_node.hpp +++ b/pose_graph_ros/include/pose_graph_ros/pose_graph_node.hpp @@ -16,7 +16,6 @@ #include #include -#include #include #include #include @@ -24,11 +23,11 @@ #include "camodocal/camera_models/CameraFactory.h" #include "pose_graph/pose_graph.hpp" -#include "pose_graph/details/pose_graph_event_observer.hpp" + #include "pose_graph_ros/utility/CameraPoseVisualization.h" namespace pose_graph { -class PoseGraphNode : public PoseGraphEventObserver { +class PoseGraphNode { public: ~PoseGraphNode(); @@ -40,24 +39,8 @@ class PoseGraphNode : public PoseGraphEventObserver { void StartCommandAndProcessingThreads(); void Process(); void Command(); - void Publish(); void NewSequence(); - // PoseGraphEventObserver callbacks - void OnPoseGraphLoaded() final; - void OnPoseGraphSaved() final; - void OnKeyFrameAdded(KeyFrame::Attributes kf_attribute) final; - void OnKeyFrameLoaded(KeyFrame::Attributes kf_attribute, int count) final; - void OnKeyFrameConnectionFound(KeyFrame::Attributes current_kf_attribute, - KeyFrame::Attributes old_kf_attribute, - std::vector matched_2d_old_norm, - std::vector matched_id, - cv::Mat& thumb_image) final; - void OnPoseGraphOptimization( - std::vector kf_attributes) final; - void OnNewSequentialEdge(Vector3d p1, Vector3d p2) final; - void OnNewLoopEdge(Vector3d p1, Vector3d p2) final; - // ros callbacks void ImuForwardCallback(const nav_msgs::OdometryConstPtr& forward_msg); void VioCallback(const nav_msgs::OdometryConstPtr& pose_msg); @@ -69,35 +52,19 @@ class PoseGraphNode : public PoseGraphEventObserver { private: ros::NodeHandle nh_{"~"}; + + // pose graph PoseGraphConfig config_; PoseGraph pose_graph_ = PoseGraph(); - std::unique_ptr camera_pose_vis_; - bool loop_closure_; - camodocal::CameraPtr camera_; // Note: internally it uses a shared pointer. - bool visualise_imu_forward_; - std::queue image_buffer_; - std::queue point_buffer_; - std::queue pose_buffer_; - std::queue odometry_buffer_; - std::mutex buffer_mutex_; - std::mutex process_mutex_; - Eigen::Vector3d last_t_ = {-100, -100, -100}; - std::string vins_result_path_; - std::string image_topic_; - nav_msgs::Path path_[10]; - nav_msgs::Path base_path_; - nav_msgs::Path no_loop_path_; - std::unique_ptr posegraph_visualization_; + class PoseGraphEventObserverImpl; // forward declaration + std::shared_ptr pose_graph_event_observer_; // ros parameters std::string config_file_path_; - int visualization_shift_x_ = 0; - int visualization_shift_y_ = 0; int skip_cnt_threshold_ = 0; int skip_cnt_ = 0; double skip_distance_ = 0.0; static const int skip_first_cnt_threshold = 10; - const bool save_loop_path = true; int skip_first_cnt_ = 0; int frame_index_ = 0; int sequence_index_ = 1; @@ -106,6 +73,11 @@ class PoseGraphNode : public PoseGraphEventObserver { std::thread measurement_thread_; std::thread keyboard_command_thread_; + // ros publishers + ros::Publisher pub_camera_pose_visual_; + ros::Publisher pub_key_odometrys_; + ros::Publisher pub_vio_path_; + // ros subscribers ros::Subscriber sub_imu_forward_; ros::Subscriber sub_vio_; @@ -115,16 +87,20 @@ class PoseGraphNode : public PoseGraphEventObserver { ros::Subscriber sub_point_; ros::Subscriber sub_relo_relative_pose_; - // ros publishers - ros::Publisher pub_match_points_; - ros::Publisher pub_camera_pose_visual_; - ros::Publisher pub_key_odometrys_; - ros::Publisher pub_vio_path_; - ros::Publisher pub_pg_path_; - ros::Publisher pub_base_path_; - ros::Publisher pub_pose_graph_; - ros::Publisher pub_path_[10]; - ros::Publisher pub_match_img_; + // others + std::unique_ptr camera_pose_vis_; + bool loop_closure_; + camodocal::CameraPtr camera_; // Note: internally it uses a shared pointer. + bool visualise_imu_forward_; + std::queue image_buffer_; + std::queue point_buffer_; + std::queue pose_buffer_; + std::queue odometry_buffer_; + std::mutex buffer_mutex_; + std::mutex process_mutex_; + Eigen::Vector3d last_t_ = {-100, -100, -100}; + std::string image_topic_; + nav_msgs::Path no_loop_path_; }; } // namespace pose_graph From 018d7844395be56f275c4a0db9e2308c42af7eb9 Mon Sep 17 00:00:00 2001 From: Kee Jin Date: Sun, 9 Mar 2025 21:55:45 +0800 Subject: [PATCH 3/5] config: updated rviz config to enable all topics by default --- config/vins_rviz_config.rviz | 235 ++++++++++++++++++++--------------- 1 file changed, 132 insertions(+), 103 deletions(-) diff --git a/config/vins_rviz_config.rviz b/config/vins_rviz_config.rviz index 6ed63a93d..0ffefd1b3 100644 --- a/config/vins_rviz_config.rviz +++ b/config/vins_rviz_config.rviz @@ -4,8 +4,8 @@ Panels: Name: Displays Property Tree Widget: Expanded: ~ - Splitter Ratio: 0.465115994 - Tree Height: 221 + Splitter Ratio: 0.4651159942150116 + Tree Height: 168 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -14,14 +14,13 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: tracked image @@ -31,7 +30,11 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 359 + Tree Height: 365 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -41,7 +44,7 @@ Visualization Manager: Color: 130; 130; 130 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -53,21 +56,23 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: rviz/Axes + - Alpha: 1 + Class: rviz/Axes Enabled: true Length: 1 Name: Axes - Radius: 0.100000001 + Radius: 0.10000000149011612 Reference Frame: + Show Trail: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines Line Width: 0.5 Name: ground_truth_path @@ -77,9 +82,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /benchmark_publisher/path Unreliable: false Value: true @@ -96,7 +102,7 @@ Visualization Manager: Unreliable: false Value: true - Class: rviz/Image - Enabled: false + Enabled: true Image Topic: /cam0/image_raw Max Value: 1 Median window: 5 @@ -106,7 +112,7 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: false + Value: true - Class: rviz/Group Displays: - Alpha: 1 @@ -114,11 +120,11 @@ Visualization Manager: Class: rviz/Path Color: 0; 255; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Name: Path Offset: X: 0 @@ -126,9 +132,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /vins_estimator/path Unreliable: false Value: true @@ -137,7 +144,7 @@ Visualization Manager: Marker Topic: /vins_estimator/camera_pose_visual Name: camera_visual Namespaces: - {} + CameraPoseVisualization: true Queue Size: 100 Value: true - Alpha: 1 @@ -155,15 +162,13 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: current_point Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 1 - Size (m): 0.00999999978 + Size (m): 0.009999999776482582 Style: Points Topic: /vins_estimator/point_cloud Unreliable: false @@ -202,19 +207,31 @@ Visualization Manager: Value: false - Class: rviz/TF Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: true + body: + Value: true + camera: + Value: true + world: + Value: true + Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: - {} + world: + body: + camera: + {} Update Interval: 0 Value: true - Enabled: false + Enabled: true Name: VIO - Class: rviz/Group Displays: @@ -223,11 +240,11 @@ Visualization Manager: Class: rviz/Path Color: 0; 255; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Name: pose_graph_path Offset: X: 0 @@ -235,9 +252,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/pose_graph_path Unreliable: false Value: true @@ -246,11 +264,11 @@ Visualization Manager: Class: rviz/Path Color: 255; 170; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Name: base_path Offset: X: 0 @@ -258,9 +276,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/base_path Unreliable: false Value: true @@ -269,7 +288,7 @@ Visualization Manager: Marker Topic: /pose_graph/pose_graph Name: loop_visual Namespaces: - {} + CameraPoseVisualization: true Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -277,7 +296,7 @@ Visualization Manager: Marker Topic: /pose_graph/camera_pose_visual Name: camera_visual Namespaces: - {} + CameraPoseVisualization: true Queue Size: 100 Value: true - Class: rviz/Image @@ -297,7 +316,7 @@ Visualization Manager: Marker Topic: /pose_graph/key_odometrys Name: Marker Namespaces: - {} + key_odometrys: true Queue Size: 100 Value: true - Alpha: 1 @@ -305,11 +324,11 @@ Visualization Manager: Class: rviz/Path Color: 25; 255; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.300000012 + Line Width: 0.30000001192092896 Name: Sequence1 Offset: X: 0 @@ -317,9 +336,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_1 Unreliable: false Value: true @@ -328,11 +348,11 @@ Visualization Manager: Class: rviz/Path Color: 255; 0; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.300000012 + Line Width: 0.30000001192092896 Name: Sequence2 Offset: X: 0 @@ -340,9 +360,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_2 Unreliable: false Value: true @@ -351,11 +372,11 @@ Visualization Manager: Class: rviz/Path Color: 0; 85; 255 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.300000012 + Line Width: 0.30000001192092896 Name: Sequence3 Offset: X: 0 @@ -363,9 +384,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_3 Unreliable: false Value: true @@ -374,11 +396,11 @@ Visualization Manager: Class: rviz/Path Color: 255; 170; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.300000012 + Line Width: 0.30000001192092896 Name: Sequence4 Offset: X: 0 @@ -386,9 +408,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_4 Unreliable: false Value: true @@ -397,11 +420,11 @@ Visualization Manager: Class: rviz/Path Color: 255; 170; 255 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Name: Sequence5 Offset: X: 0 @@ -409,9 +432,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/path_5 Unreliable: false Value: true @@ -420,11 +444,11 @@ Visualization Manager: Class: rviz/Path Color: 25; 255; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Name: no_loop_path Offset: X: 0 @@ -432,9 +456,10 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /pose_graph/no_loop_path Unreliable: false Value: true @@ -443,6 +468,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 0; 0; 0 + Default Light: true Fixed Frame: world Frame Rate: 30 Name: root @@ -452,7 +478,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -462,33 +491,33 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 20.9684067 + Distance: 14.08361530303955 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: - X: -2.7069304 - Y: -1.26974416 - Z: 2.1410624e-05 + X: -1.4666130542755127 + Y: -0.9299832582473755 + Z: 2.141062395821791e-05 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 1.0797962 + Near Clip Distance: 0.009999999776482582 + Pitch: 1.1397961378097534 Target Frame: - Value: XYOrbit (rviz) - Yaw: 3.08722663 + Yaw: 3.0522265434265137 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 912 + Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -497,9 +526,9 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1390 - X: 2127 - Y: 109 + Width: 1862 + X: 1138 + Y: 453 loop_match_image: collapsed: false raw_image: From 91ad8d44a2d74ae9334dedfc58f0240bfecd42a2 Mon Sep 17 00:00:00 2001 From: Kee Jin Date: Sun, 9 Mar 2025 21:57:45 +0800 Subject: [PATCH 4/5] [#37] pose_graph_ros: Implemented PoseGraphEventObserverImpl using pimpl pattern and updated observer usage in PoseGraphNode. PoseGraphNode class no longer inherits from std::enable_shared_from_this. --- pose_graph_ros/CMakeLists.txt | 1 + .../src/pose_graph_event_observer_impl.cpp | 303 ++++++++++++++++++ .../src/pose_graph_event_observer_impl.hpp | 76 +++++ pose_graph_ros/src/pose_graph_node.cpp | 284 ++-------------- 4 files changed, 400 insertions(+), 264 deletions(-) create mode 100644 pose_graph_ros/src/pose_graph_event_observer_impl.cpp create mode 100644 pose_graph_ros/src/pose_graph_event_observer_impl.hpp diff --git a/pose_graph_ros/CMakeLists.txt b/pose_graph_ros/CMakeLists.txt index 31f5986a1..8e1c509c6 100644 --- a/pose_graph_ros/CMakeLists.txt +++ b/pose_graph_ros/CMakeLists.txt @@ -22,6 +22,7 @@ catkin_package() add_executable(pose_graph_node src/pose_graph_node.cpp + src/pose_graph_event_observer_impl.cpp src/utility/CameraPoseVisualization.cpp ) diff --git a/pose_graph_ros/src/pose_graph_event_observer_impl.cpp b/pose_graph_ros/src/pose_graph_event_observer_impl.cpp new file mode 100644 index 000000000..2b17cd1d3 --- /dev/null +++ b/pose_graph_ros/src/pose_graph_event_observer_impl.cpp @@ -0,0 +1,303 @@ +/** + * @file pose_graph_event_observer_impl.cpp + * @brief + * @date 09-03-2025 + * + * @copyright Copyright (c) 2025 Cheo Kee Jin. + */ + +#include "pose_graph_event_observer_impl.hpp" + +#include + +#include +#include +#include + +#define SHOW_S_EDGE false +#define SHOW_L_EDGE true + +namespace pose_graph { +PoseGraphNode::PoseGraphEventObserverImpl::PoseGraphEventObserverImpl( + ros::NodeHandle& nh, const std::string config_file_path) + : nh_(nh) { + posegraph_visualization_ = + std::make_unique(1.0, 0.0, 1.0, 1.0); + posegraph_visualization_->setScale(0.1); + posegraph_visualization_->setLineWidth(0.01); + + // Load config file + cv::FileStorage fs(config_file_path, cv::FileStorage::READ); + if (!fs.isOpened()) { + ROS_ERROR("PoseGraphEventObserver::Failed to open config file: %s", + config_file_path.c_str()); + ros::shutdown(); + } + + fast_relocalization_ = (int)fs["fast_relocalization"]; + + vins_result_path_ = std::string(fs["output_path"]); + FileSystemHelper::createDirectoryIfNotExists(vins_result_path_.c_str()); + vins_result_path_ = vins_result_path_ + "/vins_result_loop.csv"; + + fs.release(); + + if (!ReadParameters()) { + ROS_ERROR("PoseGraphEventObserver::Failed to read parameters"); + ros::shutdown(); + } + + StartPublishers(); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::Publish(int sequence_count) { + for (int i = 1; i <= sequence_count; i++) { + pub_pg_path_.publish(path_[i]); + pub_path_[i].publish(path_[i]); + posegraph_visualization_->publish_by(pub_pose_graph_, + path_[sequence_count].header); + } + base_path_.header.frame_id = "world"; + pub_base_path_.publish(base_path_); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::ResetPoseGraphVisualisation() { + posegraph_visualization_->reset(); +} + +bool PoseGraphNode::PoseGraphEventObserverImpl::ReadParameters() { + nh_.getParam("visualization_shift_x", visualization_shift_x_); + nh_.getParam("visualization_shift_y", visualization_shift_y_); + + ROS_INFO( + "Loaded parameters: visualization_shift_x: %d, " + "visualization_shift_y: %d", + visualization_shift_x_, visualization_shift_y_); + + return true; +} + +void PoseGraphNode::PoseGraphEventObserverImpl::StartPublishers() { + pub_match_img_ = nh_.advertise("match_image", 2000); + pub_match_points_ = + nh_.advertise("match_points", 100); + pub_pg_path_ = nh_.advertise("pose_graph_path", 1000); + pub_base_path_ = nh_.advertise("base_path", 1000); + pub_pose_graph_ = + nh_.advertise("pose_graph", 1000); + for (int i = 1; i < 10; i++) + pub_path_[i] = nh_.advertise("path_" + to_string(i), 1000); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnPoseGraphLoaded() { + ROS_DEBUG("Pose graph loaded"); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnPoseGraphSaved() { + ROS_DEBUG("Pose graph saved"); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnKeyFrameAdded( + KeyFrame::Attributes kf_attribute, int sequence_count) { + ROS_DEBUG("On Keyframe added"); + + Eigen::Quaterniond quarternion{kf_attribute.rotation}; + geometry_msgs::PoseStamped pose_stamped; + pose_stamped.header.stamp = ros::Time(kf_attribute.time_stamp); + pose_stamped.header.frame_id = "world"; + pose_stamped.pose.position.x = + kf_attribute.position.x() + visualization_shift_x_; + pose_stamped.pose.position.y = + kf_attribute.position.y() + visualization_shift_y_; + pose_stamped.pose.position.z = kf_attribute.position.z(); + pose_stamped.pose.orientation.x = quarternion.x(); + pose_stamped.pose.orientation.y = quarternion.y(); + pose_stamped.pose.orientation.z = quarternion.z(); + pose_stamped.pose.orientation.w = quarternion.w(); + path_[sequence_count].poses.push_back(pose_stamped); + path_[sequence_count].header = pose_stamped.header; + + if (save_loop_path) { + std::ofstream loop_path_file(vins_result_path_, ios::app); + loop_path_file.setf(ios::fixed, ios::floatfield); + loop_path_file.precision(0); + loop_path_file << kf_attribute.time_stamp * 1e9 << ","; + loop_path_file.precision(5); + loop_path_file << kf_attribute.position.x() << "," + << kf_attribute.position.y() << "," + << kf_attribute.position.z() << "," << quarternion.w() << "," + << quarternion.x() << "," << quarternion.y() << "," + << quarternion.z() << "," << endl; + loop_path_file.close(); + } + + Publish(sequence_count); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnKeyFrameLoaded( + KeyFrame::Attributes kf_attribute, int count, int sequence_count) { + ROS_INFO("On Keyframe loaded"); + Eigen::Quaterniond Q{kf_attribute.rotation}; + geometry_msgs::PoseStamped pose_stamped; + pose_stamped.header.stamp = ros::Time(kf_attribute.time_stamp); + pose_stamped.header.frame_id = "world"; + pose_stamped.pose.position.x = + kf_attribute.position.x() + visualization_shift_x_; + pose_stamped.pose.position.y = + kf_attribute.position.y() + visualization_shift_y_; + pose_stamped.pose.position.z = kf_attribute.position.z(); + pose_stamped.pose.orientation.x = Q.x(); + pose_stamped.pose.orientation.y = Q.y(); + pose_stamped.pose.orientation.z = Q.z(); + pose_stamped.pose.orientation.w = Q.w(); + base_path_.poses.push_back(pose_stamped); + base_path_.header = pose_stamped.header; + + if (count % 20 == 0) { + Publish(sequence_count); + } +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnKeyFrameConnectionFound( + KeyFrame::Attributes current_kf_attribute, + KeyFrame::Attributes old_kf_attribute, + std::vector matched_2d_old_norm, + std::vector matched_id, cv::Mat& thumb_image) { + ROS_DEBUG("On Keyframe connection found"); + { + sensor_msgs::ImagePtr msg = + cv_bridge::CvImage(std_msgs::Header(), "bgr8", thumb_image) + .toImageMsg(); + msg->header.stamp = ros::Time(current_kf_attribute.time_stamp); + pub_match_img_.publish(msg); + } + if (fast_relocalization_) { + sensor_msgs::PointCloud msg_match_points; + msg_match_points.header.stamp = ros::Time(current_kf_attribute.time_stamp); + for (int i = 0; i < (int)matched_2d_old_norm.size(); i++) { + geometry_msgs::Point32 p; + p.x = matched_2d_old_norm[i].x; + p.y = matched_2d_old_norm[i].y; + p.z = matched_id[i]; + msg_match_points.points.push_back(p); + } + Eigen::Vector3d T = old_kf_attribute.position; + Eigen::Matrix3d R = old_kf_attribute.rotation; + Quaterniond Q(R); + sensor_msgs::ChannelFloat32 t_q_index; + t_q_index.values.push_back(T.x()); + t_q_index.values.push_back(T.y()); + t_q_index.values.push_back(T.z()); + t_q_index.values.push_back(Q.w()); + t_q_index.values.push_back(Q.x()); + t_q_index.values.push_back(Q.y()); + t_q_index.values.push_back(Q.z()); + t_q_index.values.push_back(current_kf_attribute.index); + msg_match_points.channels.push_back(t_q_index); + pub_match_points_.publish(msg_match_points); + } +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnPoseGraphOptimization( + std::vector kf_attributes, int sequence_count) { + // ROS_INFO("On Pose graph optimization"); + std::vector::iterator it; + for (int i = 1; i <= sequence_count; i++) { + path_[i].poses.clear(); + } + base_path_.poses.clear(); + posegraph_visualization_->reset(); + + if (save_loop_path) { + std::ofstream loop_path_file_tmp(vins_result_path_, ios::out); + loop_path_file_tmp.close(); + } + + for (it = kf_attributes.begin(); it != kf_attributes.end(); it++) { + Eigen::Quaterniond Q; + Q = it->rotation; + // printf("path p: %f, %f, %f\n", P.x(), P.z(), P.y() ); + + geometry_msgs::PoseStamped pose_stamped; + pose_stamped.header.stamp = ros::Time(it->time_stamp); + pose_stamped.header.frame_id = "world"; + pose_stamped.pose.position.x = it->position.x() + visualization_shift_x_; + pose_stamped.pose.position.y = it->position.y() + visualization_shift_y_; + pose_stamped.pose.position.z = it->position.z(); + pose_stamped.pose.orientation.x = Q.x(); + pose_stamped.pose.orientation.y = Q.y(); + pose_stamped.pose.orientation.z = Q.z(); + pose_stamped.pose.orientation.w = Q.w(); + if (it->sequence == 0) { + base_path_.poses.push_back(pose_stamped); + base_path_.header = pose_stamped.header; + } else { + path_[it->sequence].poses.push_back(pose_stamped); + path_[it->sequence].header = pose_stamped.header; + } + + if (save_loop_path && !vins_result_path_.empty()) { + std::ofstream loop_path_file(vins_result_path_, ios::app); + loop_path_file.setf(ios::fixed, ios::floatfield); + loop_path_file.precision(0); + loop_path_file << it->time_stamp * 1e9 << ","; + loop_path_file.precision(5); + loop_path_file << it->position.x() << "," << it->position.y() << "," + << it->position.z() << "," << Q.w() << "," << Q.x() << "," + << Q.y() << "," << Q.z() << "," << endl; + loop_path_file.close(); + } + + if (SHOW_S_EDGE) { + std::vector::reverse_iterator rit = + kf_attributes.rbegin(); + std::vector::reverse_iterator lrit; + for (; rit != kf_attributes.rend(); rit++) { + if (rit->index == it->index) { + lrit = rit; + lrit++; + for (int i = 0; i < 4; i++) { + if (lrit == kf_attributes.rend()) break; + if (lrit->sequence == it->sequence) { + posegraph_visualization_->add_edge(it->position, lrit->position); + } + lrit++; + } + break; + } + } + } + if (SHOW_L_EDGE) { + if (it->has_loop && it->sequence == sequence_count) { + std::find_if( + kf_attributes.begin(), kf_attributes.end(), + [&](KeyFrame::Attributes& attr) { + if (attr.index == it->loop_index && it->sequence > 0) { + posegraph_visualization_->add_loopedge( + it->position, + attr.position + Vector3d(visualization_shift_x_, + visualization_shift_y_, 0)); + return true; + } + return false; + }); + } + } + } + + Publish(sequence_count); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnNewSequentialEdge( + Vector3d p1, Vector3d p2) { + if (!SHOW_S_EDGE) return; + posegraph_visualization_->add_edge(p1, p2); +} + +void PoseGraphNode::PoseGraphEventObserverImpl::OnNewLoopEdge(Vector3d p1, + Vector3d p2) { + if (!SHOW_L_EDGE) return; + p2 += Vector3d(visualization_shift_x_, visualization_shift_y_, 0); + posegraph_visualization_->add_loopedge(p1, p2); +} +} // namespace pose_graph \ No newline at end of file diff --git a/pose_graph_ros/src/pose_graph_event_observer_impl.hpp b/pose_graph_ros/src/pose_graph_event_observer_impl.hpp new file mode 100644 index 000000000..d5c4bb671 --- /dev/null +++ b/pose_graph_ros/src/pose_graph_event_observer_impl.hpp @@ -0,0 +1,76 @@ +/** + * @file pose_graph_event_observer_impl.cpp + * @brief + * @date 09-03-2025 + * + * @copyright Copyright (c) 2025 Cheo Kee Jin. + */ + +#ifndef SRC_POSE_GRAPH_EVENT_OBSERVER_IMPL_HPP +#define SRC_POSE_GRAPH_EVENT_OBSERVER_IMPL_HPP + +#include + +#include + +#include +#include + +#include "pose_graph/details/pose_graph_event_observer.hpp" + +#include "pose_graph_ros/pose_graph_node.hpp" +#include "pose_graph_ros/utility/CameraPoseVisualization.h" + +namespace pose_graph { +class PoseGraphNode::PoseGraphEventObserverImpl + : public PoseGraphEventObserver { + public: + PoseGraphEventObserverImpl(ros::NodeHandle& nh, std::string config_file_path); + ~PoseGraphEventObserverImpl() = default; + + void Publish(int sequence_count); + void ResetPoseGraphVisualisation(); + + private: + bool ReadParameters(); + void StartPublishers(); + + // PoseGraphEventObserver callbacks + void OnPoseGraphLoaded() final; + void OnPoseGraphSaved() final; + void OnKeyFrameAdded(KeyFrame::Attributes kf_attribute, + int sequence_count) final; + void OnKeyFrameLoaded(KeyFrame::Attributes kf_attribute, int count, + int sequence_count) final; + void OnKeyFrameConnectionFound(KeyFrame::Attributes current_kf_attribute, + KeyFrame::Attributes old_kf_attribute, + std::vector matched_2d_old_norm, + std::vector matched_id, + cv::Mat& thumb_image) final; + void OnPoseGraphOptimization(std::vector kf_attributes, + int sequence_count) final; + void OnNewSequentialEdge(Vector3d p1, Vector3d p2) final; + void OnNewLoopEdge(Vector3d p1, Vector3d p2) final; + + private: + ros::NodeHandle& nh_; + nav_msgs::Path path_[10]; + nav_msgs::Path base_path_; + std::string vins_result_path_; + bool fast_relocalization_{false}; + const bool save_loop_path = true; + int visualization_shift_x_ = 0; + int visualization_shift_y_ = 0; + + // ros publishers + ros::Publisher pub_match_points_; + ros::Publisher pub_pg_path_; + ros::Publisher pub_base_path_; + ros::Publisher pub_pose_graph_; + ros::Publisher pub_path_[10]; + ros::Publisher pub_match_img_; + std::unique_ptr posegraph_visualization_; +}; +} // namespace pose_graph + +#endif /* SRC_POSE_GRAPH_EVENT_OBSERVER_IMPL_HPP */ diff --git a/pose_graph_ros/src/pose_graph_node.cpp b/pose_graph_ros/src/pose_graph_node.cpp index 1cd529df1..5a0b09bfc 100644 --- a/pose_graph_ros/src/pose_graph_node.cpp +++ b/pose_graph_ros/src/pose_graph_node.cpp @@ -14,13 +14,11 @@ #include #include -#define SHOW_S_EDGE false -#define SHOW_L_EDGE true +#include "pose_graph_event_observer_impl.hpp" namespace pose_graph { PoseGraphNode::~PoseGraphNode() { - ros::shutdown(); if (measurement_thread_.joinable()) { measurement_thread_.join(); } @@ -54,11 +52,6 @@ bool PoseGraphNode::Start() { camera_pose_vis_->setScale(camera_visual_size); camera_pose_vis_->setLineWidth(camera_visual_size / 10.0); - posegraph_visualization_ = - std::make_unique(1.0, 0.0, 1.0, 1.0); - posegraph_visualization_->setScale(0.1); - posegraph_visualization_->setLineWidth(0.01); - loop_closure_ = (int)fs["loop_closure"]; bool load_previous_pose_graph = false; @@ -75,19 +68,13 @@ bool PoseGraphNode::Start() { config_file_path_.c_str()); image_topic_ = std::string(fs["image_topic"]); config_.saved_pose_graph_dir = std::string(fs["pose_graph_save_path"]); - config_.save_debug_image = (int)fs["save_image"]; - - vins_result_path_ = std::string(fs["output_path"]); FileSystemHelper::createDirectoryIfNotExists( config_.saved_pose_graph_dir.c_str()); - FileSystemHelper::createDirectoryIfNotExists(vins_result_path_.c_str()); - vins_result_path_ = vins_result_path_ + "/vins_result_loop.csv"; + config_.save_debug_image = (int)fs["save_image"]; + config_.fast_relocalization = (int)fs["fast_relocalization"]; visualise_imu_forward_ = (int)fs["visualise_imu_forward"]; load_previous_pose_graph = (int)fs["load_previous_pose_graph"]; - config_.fast_relocalization = (int)fs["fast_relocalization"]; - - std::ofstream fout(vins_result_path_, std::ios::out); if (load_previous_pose_graph) { ROS_INFO("Loading previous pose graph"); @@ -102,8 +89,10 @@ bool PoseGraphNode::Start() { StartPublishersAndSubscribers(); + pose_graph_event_observer_ = + std::make_shared(nh_, config_file_path_); pose_graph_.Initialize(config_, camera_); - pose_graph_.RegisterEventObserver(PoseGraphEventObserver::shared_from_this()); + pose_graph_.RegisterEventObserver(pose_graph_event_observer_); StartCommandAndProcessingThreads(); return true; @@ -115,21 +104,23 @@ bool PoseGraphNode::ReadParameters() { return false; } - nh_.getParam("visualization_shift_x", visualization_shift_x_); - nh_.getParam("visualization_shift_y", visualization_shift_y_); nh_.getParam("skip_cnt", skip_cnt_threshold_); nh_.getParam("skip_dis", skip_distance_); ROS_INFO( - "Loaded parameters: config_file: %s, visualization_shift_x: %d, " - "visualization_shift_y: %d, skip_cnt: %d, skip_dis: %f", - config_file_path_.c_str(), visualization_shift_x_, visualization_shift_y_, - skip_cnt_threshold_, skip_distance_); + "Loaded parameters for PoseGraphNode: config_file: %s, skip_cnt: %d, " + "skip_dis: %f", + config_file_path_.c_str(), skip_cnt_threshold_, skip_distance_); return true; } void PoseGraphNode::StartPublishersAndSubscribers() { + pub_camera_pose_visual_ = nh_.advertise( + "camera_pose_visual", 1000); + pub_key_odometrys_ = + nh_.advertise("key_odometrys", 1000); + pub_vio_path_ = nh_.advertise("no_loop_path", 1000); sub_imu_forward_ = nh_.subscribe( "/vins_estimator/imu_propagate", 2000, &PoseGraphNode::ImuForwardCallback, this); @@ -149,21 +140,6 @@ void PoseGraphNode::StartPublishersAndSubscribers() { sub_relo_relative_pose_ = nh_.subscribe( "/vins_estimator/relo_relative_pose", 2000, &PoseGraphNode::ReloRelativePoseCallback, this); - - pub_match_img_ = nh_.advertise("match_image", 2000); - pub_camera_pose_visual_ = nh_.advertise( - "camera_pose_visual", 1000); - pub_key_odometrys_ = - nh_.advertise("key_odometrys", 1000); - pub_vio_path_ = nh_.advertise("no_loop_path", 1000); - pub_match_points_ = - nh_.advertise("match_points", 100); - pub_pg_path_ = nh_.advertise("pose_graph_path", 1000); - pub_base_path_ = nh_.advertise("base_path", 1000); - pub_pose_graph_ = - nh_.advertise("pose_graph", 1000); - for (int i = 1; i < 10; i++) - pub_path_[i] = nh_.advertise("path_" + to_string(i), 1000); } void PoseGraphNode::StartCommandAndProcessingThreads() { @@ -294,41 +270,6 @@ void PoseGraphNode::Process() { { std::lock_guard lock(process_mutex_); pose_graph_.AddKeyFrame(keyframe); - { - int sequence_cnt = pose_graph_.GetCurrentSequenceCount(); - auto kf_attribute = keyframe->getAttributes(); - Eigen::Quaterniond quarternion{kf_attribute.rotation}; - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header.stamp = ros::Time(keyframe->time_stamp); - pose_stamped.header.frame_id = "world"; - pose_stamped.pose.position.x = - kf_attribute.position.x() + visualization_shift_x_; - pose_stamped.pose.position.y = - kf_attribute.position.y() + visualization_shift_y_; - pose_stamped.pose.position.z = kf_attribute.position.z(); - pose_stamped.pose.orientation.x = quarternion.x(); - pose_stamped.pose.orientation.y = quarternion.y(); - pose_stamped.pose.orientation.z = quarternion.z(); - pose_stamped.pose.orientation.w = quarternion.w(); - path_[sequence_cnt].poses.push_back(pose_stamped); - path_[sequence_cnt].header = pose_stamped.header; - - if (save_loop_path) { - std::ofstream loop_path_file(vins_result_path_, ios::app); - loop_path_file.setf(ios::fixed, ios::floatfield); - loop_path_file.precision(0); - loop_path_file << kf_attribute.time_stamp * 1e9 << ","; - loop_path_file.precision(5); - loop_path_file << kf_attribute.position.x() << "," - << kf_attribute.position.y() << "," - << kf_attribute.position.z() << "," - << quarternion.w() << "," << quarternion.x() << "," - << quarternion.y() << "," << quarternion.z() << "," - << endl; - loop_path_file.close(); - } - } - Publish(); } frame_index_++; last_t_ = T; @@ -342,6 +283,8 @@ void PoseGraphNode::Process() { void PoseGraphNode::Command() { if (!loop_closure_) return; while (ros::ok()) { + // Note: Since getchar() is a blocking call, a SIGINT signal will not be + // able to interrupt this thread. This is a known issue. char c = getchar(); if (c == 's') { { @@ -363,18 +306,6 @@ void PoseGraphNode::Command() { } } -void PoseGraphNode::Publish() { - int sequence_cnt = pose_graph_.GetCurrentSequenceCount(); - for (int i = 1; i <= sequence_cnt; i++) { - pub_pg_path_.publish(path_[i]); - pub_path_[i].publish(path_[i]); - posegraph_visualization_->publish_by(pub_pose_graph_, - path_[sequence_cnt].header); - } - base_path_.header.frame_id = "world"; - pub_base_path_.publish(base_path_); -} - void PoseGraphNode::NewSequence() { ROS_INFO("new sequence"); sequence_index_++; @@ -385,8 +316,8 @@ void PoseGraphNode::NewSequence() { "sequences."); ROS_BREAK(); } - posegraph_visualization_->reset(); - Publish(); + pose_graph_event_observer_->ResetPoseGraphVisualisation(); + pose_graph_event_observer_->Publish(pose_graph_.GetCurrentSequenceCount()); { std::lock_guard lock(buffer_mutex_); while (!image_buffer_.empty()) image_buffer_.pop(); @@ -396,180 +327,6 @@ void PoseGraphNode::NewSequence() { } } -void PoseGraphNode::OnPoseGraphLoaded() { ROS_DEBUG("Pose graph loaded"); } - -void PoseGraphNode::OnPoseGraphSaved() { ROS_DEBUG("Pose graph saved"); } - -void PoseGraphNode::OnKeyFrameAdded(KeyFrame::Attributes kf_attribute) { - ROS_DEBUG("On Keyframe added"); -} - -void PoseGraphNode::OnKeyFrameLoaded(KeyFrame::Attributes kf_attribute, - int count) { - ROS_INFO("On Keyframe loaded"); - Eigen::Quaterniond Q{kf_attribute.rotation}; - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header.stamp = ros::Time(kf_attribute.time_stamp); - pose_stamped.header.frame_id = "world"; - pose_stamped.pose.position.x = - kf_attribute.position.x() + visualization_shift_x_; - pose_stamped.pose.position.y = - kf_attribute.position.y() + visualization_shift_y_; - pose_stamped.pose.position.z = kf_attribute.position.z(); - pose_stamped.pose.orientation.x = Q.x(); - pose_stamped.pose.orientation.y = Q.y(); - pose_stamped.pose.orientation.z = Q.z(); - pose_stamped.pose.orientation.w = Q.w(); - base_path_.poses.push_back(pose_stamped); - base_path_.header = pose_stamped.header; - - if (count % 20 == 0) { - Publish(); - } -} - -void PoseGraphNode::OnKeyFrameConnectionFound( - KeyFrame::Attributes current_kf_attribute, - KeyFrame::Attributes old_kf_attribute, - std::vector matched_2d_old_norm, - std::vector matched_id, cv::Mat& thumb_image) { - ROS_DEBUG("On Keyframe connection found"); - { - sensor_msgs::ImagePtr msg = - cv_bridge::CvImage(std_msgs::Header(), "bgr8", thumb_image) - .toImageMsg(); - msg->header.stamp = ros::Time(current_kf_attribute.time_stamp); - pub_match_img_.publish(msg); - } - if (config_.fast_relocalization) { - sensor_msgs::PointCloud msg_match_points; - msg_match_points.header.stamp = ros::Time(current_kf_attribute.time_stamp); - for (int i = 0; i < (int)matched_2d_old_norm.size(); i++) { - geometry_msgs::Point32 p; - p.x = matched_2d_old_norm[i].x; - p.y = matched_2d_old_norm[i].y; - p.z = matched_id[i]; - msg_match_points.points.push_back(p); - } - Eigen::Vector3d T = old_kf_attribute.position; - Eigen::Matrix3d R = old_kf_attribute.rotation; - Quaterniond Q(R); - sensor_msgs::ChannelFloat32 t_q_index; - t_q_index.values.push_back(T.x()); - t_q_index.values.push_back(T.y()); - t_q_index.values.push_back(T.z()); - t_q_index.values.push_back(Q.w()); - t_q_index.values.push_back(Q.x()); - t_q_index.values.push_back(Q.y()); - t_q_index.values.push_back(Q.z()); - t_q_index.values.push_back(current_kf_attribute.index); - msg_match_points.channels.push_back(t_q_index); - pub_match_points_.publish(msg_match_points); - } -} - -void PoseGraphNode::OnPoseGraphOptimization( - std::vector kf_attributes) { - // ROS_INFO("On Pose graph optimization"); - std::vector::iterator it; - int sequence_cnt = pose_graph_.GetCurrentSequenceCount(); - for (int i = 1; i <= sequence_cnt; i++) { - path_[i].poses.clear(); - } - base_path_.poses.clear(); - posegraph_visualization_->reset(); - - if (save_loop_path) { - std::ofstream loop_path_file_tmp(vins_result_path_, ios::out); - loop_path_file_tmp.close(); - } - - for (it = kf_attributes.begin(); it != kf_attributes.end(); it++) { - Eigen::Quaterniond Q; - Q = it->rotation; - // printf("path p: %f, %f, %f\n", P.x(), P.z(), P.y() ); - - geometry_msgs::PoseStamped pose_stamped; - pose_stamped.header.stamp = ros::Time(it->time_stamp); - pose_stamped.header.frame_id = "world"; - pose_stamped.pose.position.x = it->position.x() + visualization_shift_x_; - pose_stamped.pose.position.y = it->position.y() + visualization_shift_y_; - pose_stamped.pose.position.z = it->position.z(); - pose_stamped.pose.orientation.x = Q.x(); - pose_stamped.pose.orientation.y = Q.y(); - pose_stamped.pose.orientation.z = Q.z(); - pose_stamped.pose.orientation.w = Q.w(); - if (it->sequence == 0) { - base_path_.poses.push_back(pose_stamped); - base_path_.header = pose_stamped.header; - } else { - path_[it->sequence].poses.push_back(pose_stamped); - path_[it->sequence].header = pose_stamped.header; - } - - if (save_loop_path && !vins_result_path_.empty()) { - std::ofstream loop_path_file(vins_result_path_, ios::app); - loop_path_file.setf(ios::fixed, ios::floatfield); - loop_path_file.precision(0); - loop_path_file << it->time_stamp * 1e9 << ","; - loop_path_file.precision(5); - loop_path_file << it->position.x() << "," << it->position.y() << "," - << it->position.z() << "," << Q.w() << "," << Q.x() << "," - << Q.y() << "," << Q.z() << "," << endl; - loop_path_file.close(); - } - - if (SHOW_S_EDGE) { - std::vector::reverse_iterator rit = - kf_attributes.rbegin(); - std::vector::reverse_iterator lrit; - for (; rit != kf_attributes.rend(); rit++) { - if (rit->index == it->index) { - lrit = rit; - lrit++; - for (int i = 0; i < 4; i++) { - if (lrit == kf_attributes.rend()) break; - if (lrit->sequence == it->sequence) { - posegraph_visualization_->add_edge(it->position, lrit->position); - } - lrit++; - } - break; - } - } - } - if (SHOW_L_EDGE) { - if (it->has_loop && it->sequence == sequence_cnt) { - std::find_if( - kf_attributes.begin(), kf_attributes.end(), - [&](KeyFrame::Attributes& attr) { - if (attr.index == it->loop_index && it->sequence > 0) { - posegraph_visualization_->add_loopedge( - it->position, - attr.position + Vector3d(visualization_shift_x_, - visualization_shift_y_, 0)); - return true; - } - return false; - }); - } - } - } - - Publish(); -} - -void PoseGraphNode::OnNewSequentialEdge(Vector3d p1, Vector3d p2) { - if (!SHOW_S_EDGE) return; - posegraph_visualization_->add_edge(p1, p2); -} - -void PoseGraphNode::OnNewLoopEdge(Vector3d p1, Vector3d p2) { - if (!SHOW_L_EDGE) return; - p2 += Vector3d(visualization_shift_x_, visualization_shift_y_, 0); - posegraph_visualization_->add_loopedge(p1, p2); -} - void PoseGraphNode::ImuForwardCallback( const nav_msgs::OdometryConstPtr& forward_msg) { if (!visualise_imu_forward_) return; @@ -777,9 +534,8 @@ void PoseGraphNode::ReloRelativePoseCallback( int main(int argc, char** argv) { ros::init(argc, argv, "pose_graph_node"); - std::shared_ptr pose_graph_node = - std::make_shared(); - if (!pose_graph_node->Start()) { + pose_graph::PoseGraphNode pose_graph_node; + if (!pose_graph_node.Start()) { ROS_ERROR("Failed to start PoseGraphNode"); ros::shutdown(); } From 33929122a85ac5abf28c7b85936ad55be715281d Mon Sep 17 00:00:00 2001 From: Kee Jin Date: Sun, 9 Mar 2025 22:20:58 +0800 Subject: [PATCH 5/5] [#37] pose_graph_ros: docs: added class design diagram for PoseGraphNode --- pose_graph_ros/docs/pose_graph_node_design.png | Bin 0 -> 48203 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 pose_graph_ros/docs/pose_graph_node_design.png diff --git a/pose_graph_ros/docs/pose_graph_node_design.png b/pose_graph_ros/docs/pose_graph_node_design.png new file mode 100644 index 0000000000000000000000000000000000000000..05a086e1ff8ff3d4b69fedcb83f73cd7043e9e13 GIT binary patch literal 48203 zcmeFZ1zeO{yEhEu07DOmfKmetDcwkSN(mAoJ#=?BN+=AWs347mqJ$`lgn@(sDkvR- zA|)Z6?;0K4xc3wLob#T2_W8coJ->&Ub+3EHwXXPIv7Q7y9W_#-V?;POIHVft$_6+% zhoCq(xM47S(DG2@&J*wl*T+Cj5$E=6`Y9Y7UNc`66JNJLdnZ>E4x6yjk8f;30*)R& zzHGwEY(hfTo}PSmPSy_I)^0w0?kHc-1fE+vpxm(?PI?48xw=}j38{+m34lkOCW3-& z!iwOVwv)SGAov?7A!aEi!Ueu5czU{`j8QgPPGB5WQDJdDVKK}rTIz-;wb_J}z;{z`Ua5=CZuL)Eom!ZWFT(uW}+x!C}D`1;NWQcW4Z;Zd0Trrp7gLoxq^Z10)Gr4Bq9PR zfLWzo(2o`gN#P$24&M9oV5h}2oN@}@-wJkj4t`E{D4!qiVmtbJc)0pHdH&jH>*4N> zvc(=6wxhMTw@1LQ-RwPFvAe_e@dO+HWpqr#sXveu(`xKw=j({wg^>7i%f(@eSJNRc{YJcRLj3Ffj5I;OOLwI^}6? zi|HN!SP8T_`ntJdA7h6@dHbTUob?YD{JrU4Q|&j}siA<}`FaNd`e8@p702!v%c+8* z*xv!aoxAvcAIAe{E-ZoVXpQAHho56(i12651N6Zj=f7S*Mu+|G)Uo2^e~{E-u7G_M zZPj}g1lTRb-}-2KV9xEk;&4X!`UYXmhP9us2k3V|Zvam54l==Z{rj#b+W`QE?@d3g!EfsG zKdX(rQLfg$PX52n^^Z>MUu%kk76YR-^gNut3qMxbiHQBmL_*@CKeo9)laGg=w=D|$ z)}K>jzfP&(?QMS|+c>+)Se{Q^}ma`zoz0ERwbS^h1w z_z6P1Q2<$4+kD5p7~6tXgn+`Lifp3Fpap|7uqF&J_`g6ASSJTN7&sv;KO7W?@0#OZ z>zm&g;HN744FwDE35kjQ$_Kny+x^#=CLt`M#WLakv zu-H$~{6lpeQ0c$Hn?K%aqEn&7^kJcxexJ-2;6`b(WOi9ekEUue$v4fuDn%qK1+0?zHv z8T6kp`Nyl0KY#y*1uy{nS1_Q5@^%8Q_4feOcVrFV!tdLDkmC=K`k?j4YJp#@Kep|l zy8Bzf?EhBl|I??#a=wTFR@eMlE%+<){ew#&whylUYvccYtv@y}cR)3R5QepzClCjB z8z0OcLk~|UTYv`i{A^sEd>ozJ9e&^AAIkCnCV22&6aDAm0SHiw2w`CqmQS#1|E~cD zyh2!2ik*BvX1%Y~|K{jm-!L5j0e_((_`l)TeK#iGmHt2W>rn1?Kns44bGv)EV}fu# zK!y9Nf)E?Vvjo5YOl;+#A=p<-zgPk+3;v~+0HZ(ueP|u@`Ok)OL1yWXVe?-#l!m(_ zFt3<6%CGJJ>-EwP$@niRrr+r$UTm!3uTf0DqEhTZ{%_GrSV|qRj6a>0?+&uAjStEj z;Cc-=PmplJ1Yiy*u>a@90DAn>4<`2dM=x7QP~v-T7(+^|B@^CvP!2jWlHz<~Sm*n1 z7pny+MSQ7jDH^=gqAoD+PArAzeB_+i%uVYZjkMV>gB=#GA8}Q$I zsy|NdUwWnc%zqG!{N26MKYDs1*z@0KS_uih|LggmKg{}vl#|%+tmF@x{#{c}V*5g~ zPc(5X2L1P-@Q>!35*T{%;sV=iLuOU{#s$ zzVbf^S|mmPA@tm@8`wYazch@#pPxM7ef$;S6AW_r0bC>{CHH}a>38HJ4wi(49{&-f z@n>Qn|EnQ}2*&0e@NPr|u-bp0Z~o@>C4#@kIDanxKW6Oz$Bps#1^kn0((liHsoOnh zl)gXRuN40A0Q&+^aKMMd6sOKG71!h7AaFF46$}F{X0H>3Ui`4vD#r|CeySdiSmkj( zXI5J-W|U!8d#2~Dv&+4V%kOWS*4SSxFV{bFCS?93i$7dLJvMfGZ{%sZWvQ2kcWy{- z$oynb!%~`8j!)y{P8iFiPvgjZkh#^Kv3w#GSO3=LroH+lZX*S8@%tkB`ue+N2F033 z85wgVrKJZ4c1zlhqHzd+eoV3=@81sy3=Iw4E>uhTC~Vi}IW#o1qv`9r+)y7d|2+2m z`8yCeiB(eVT=jAw-{r4*le-YmTejo6)O`;}N5{Clyu8r9YE+Wbz&*DMSy@@e^74_# zWEY$ICr=EteAS4As_%d3FUWgV-dba_TnhSl=fmx@bbg!5Qv}S+%(tt+KhtWvzL}37 zt2bX>6+STa<@*`WY=lpO(Dh;2x ziLG#e=o+}x3!*Vm;=@?ioWS(UPWZXJBlX3I;6NjS`*nj;nP zH5GLHXxp01D`H@lN&!ZQg5cda9~XCNX7E9cRwm!sx5D!y-D(%+4vsJz7a1Ru8?h_t z+<(_8MaceT-OTdTyX(1B>;t|;_o$$_myR1VhEI2wZ4N#3^P!Ut^wi8s8y)>^FJNC* zcn}s6`&ZXi96K_&)|6?prGu&qdE=sZ!5TJ}J~a&i(rm@xlO}@KD{mgD^MO$!aDV#< zP(^V(86BHwPoMwPo<6yDw%(t#I!>Un9MP7^Kk^KaGQ9V`$4KdX&Unf5V zCckkqi&_TNk!evN-&vyIndi^UlaPcbD?qqNu!)BuV`n zuc9Mm;BadJ_rbas!Va%}Ll#CA`mS8Lf^g}rs7I8VKUm6(*xLzM`1~qsV;ILJFvIpI zmxC|8qPUVyJ|CYY1PD32el!YpFtnU=AtxGv2w9!IIX*uA&2``&FKHnpxIS$s{QzT0 z5aA|JKU#cFw6=$$?@QT((xTq%1Zz8! zG7`2nB$bns)07R^##T#rupk;?eB1oyqlShT3=*D*nqpE^#4l7Hy#+;@!D(q}=VoT= z7Z%2zUgv3lY!i)$*xjCymX_}LI#mDRMH+rJ|8K|o{fOn26>98XJvn>!tnt_W+M5@P zub3#qsd!90V@gUK9)OcZRc2Vvp8rjrF}ykI1Yse%ChifAIlBn=fqNCTZ0pD~a6J-} zC#176zjW4SMHJ+zBvG>vopv93SbNDSprOvF8X8qZ7!(phAS8q~7heefWl;jxBdNL6 zQ(0P;!1T4EJFoZdX;&lAkdHt|7~8#)_g?sB@yiS9$8aldAo%#)`|_2}pJ1<`L1;wF zCsVI1FT1wCi%m?Xa=;#JEz+!H`oa=K{7@eC73jw;&B6RA?gVMg7Uv|ID zEC85&`RkC>_Uhcg@aJT!%ZRodnXN_txqdtnIx(J#2ZyR;4sbi*!N_nZJ_#+n!KNj# zK`oiS%=tAZJ8^W!rQ_re1LlVv0Ou|WIN;o4`-ST$yK3=@)*d(hI*+;7QvbCMD&={+GK>hL-n zLUrf{0!8;L6&P^KIMyf9b=_4|Rqe%E5{-$XLpPRAF0ZbJN34Av(mo;-$2o9N2+@%j z2}Rucmp=o>zkjwNxOVZTc2F?_2j0o5Y8jU*$MMTX#^LDh{I|tgYOz2uLgCv&FD$rW zOOu^Vw_W?HFHLsl&f8~Cy`lVNO^7IC3df;`D-A>S0Uk1d;B!Sp(Kt8?7swtFiALxg zQ0(%M=d1|*Xzx!QLk(Wz&t3SFC?5$Z>Xqu{p9E5RM>=r9%6R-_3iU4xgG2ZzvH6y1 z4P!FBTak*TMe;FHVX^k z|FUBN3EVjpO59`Eb^Q_N;|OR}Q`26qQMqB6AQ1Sw__=hiSbw1^r9hNCu#g4d4C+4t z>aOIv?K1sf-M!r%3tU_S)#jvMI9I)S7_$q*GJ`Ro+gHQO5DSZoQZ|p-8GGO zzltp)YL9~R%stm<7wN=@>pjPwTzh7olL)8cHgp`%v<#rJ993M zJw5Mn!f)E}A`KA)XEBuG>YW%>2;B9Byz=>c7utcnm2SojJ|;WZ8&I!!nfSjICSnj-ehUZ?RPiNiM}f|igv zxw01l2}<`uUtBs)An9!+`(amDXZt~+3{^JycP@ymf;rOZb?%0I9h7Wan^2043I!y9 z9pN?&7gsNUZNC=w(gB62s@-tptH=s<_SwPs-C696LN5=0R1JPP{8V%Zov z9Ycd_o3~WQZvUm+r*JY_7}{0{x&Wp1TTIwf9Q%HyygSPz6y-%Ml|0va}p* z>HLmbQ11bF_O#sSW(F*9rsBRKGID>oUMXA(7z+`AScL@yl!E9s+*3)jLp67Eq06ib zn=^MtP5?;|o?}C4!htfGOmA*(o@bat*(DU>qF!7!Vv*N6^p;#OWZ&`)z~KsrCssiG z%fCkI9~xMXDol%fLugoTIQ1Cd6IpU1qD>7~qv+;0(*#Hs0hjkYm#CjPM!sRv=>NeH zR!)$FLVUnz=eh}mY^cWEna$!Op)sN^?_Ulzgaiyg6-K9~xU6F?tnT{T@QOON zCNrSp<*%RcoBeR=ZZ?*AdE`g$H0>JH;(KfAKRrjv+r?TB>|A941aPLp`16ZTSy)(v z*kF*Ly~qX{syilCiaoG+7`hbar(uk0g;JJKYFxyP!_6h3^mK<9n3&&l?UMq+w%i3( zs0Grq*BCHw!5^ZWE+O-!Fh$npVx-K8R52{0#YMtm;)rP!-^R;s%?kUSpumhgiPXEH zeRTndfN1Iyo6HQZQ~rT#LjkS)=5+!M4P)qWCAAdBSEZ2+lF6&P=jHEMG*o(wG{(-? zKYYldbdIDPQ3*JsT%yYi9B#cp#BQjcb31_UXi=Bbk9JlE9Q*Hb0l;5;^C-}M&mPAR zEWY4xLIivr+&ii@Ob1p>Q>2xt!|M$2gHaYhn)yaIPjdpX0V0uZM_0Qz`6}ytwxkdH z1Eyhhh$;udN!cKjrxq_w^BGVI%^R>sYr`S>;5~*TG&F0qT7Vp?ep7FY?^DtAzGEXk zwDFW*AGcAdIY(OUOs<;zVJLv@>RDpE1%-ta$Bva>>ku9;P$9k<3C`f?^NVyP7Oyi; z_&L|r*WbLZ{J{DBtptS_{3+-2Wzq5%&>81D<@U0p<&9G+ebm?yZ=wNuF1OXZsyNY> z^LbExbOUNPCl|K4ESSk}`E@Q02;-#~0`l7qx*nXDgK@d&!{-~;IPuprq@cJb;UrA5 z(qk=()mpN)&q-=e@#W5hoqZIjKSwPax(?3`-z+mj6se~w?%XD&mw=NqO6!#bw7=M1 ze)y6CYLoTIO`lB3DpsC9rP5N3LtcOM3*+Z)_0bmSgvXZ;mJtoyNMuo?7QpcD*X9Q9 z-)MkEr?RS*4{7y@eo}!XD~<+5j{=bJrMIe3lOZiS=s_~0Od^o+qm;~A>FlKCrz;Z4 z<+k;TyV`2qhfch_E=6N}$HHjjxMibs@Y>gk0y1w6v$R>5MV%)u0FescKEE>JMw`F8 zaq`sKd*DVL!Y2tNntNYyrYEo^@yMyLiwsiKlqc6mW5^d;nIf8By&?t19o!TbXi?!)$DA1${niw;)+q6bT75nuth9w6A0p zG4Pt*g$W7@I(FsZ>SRkqJv~pZq;>S6_ou`}Wkv~4`Ndln4JQDIxoxt*f~&@y*K?$8 z-sC(P13k(tn}87Ad>q!Mp~QRUp0xMQ=E_IDz8VKPL66}_iaq5ixYe3AczqG}*So2aw9lA7fTf4vQ$=UuSTDbl~wbo&1wE(|C5e$H$8$?tp z=IXKe04zAa;ZJf2V$R$C>KrZqHE3t$gORTXun+M`NvCWSm1Edpc8k1g?b)a`OX?ZG zoEmUwf2SQYtDmyLhv+4}mEG947oL)rdr%-*M3i5~H#`~d zuXQQE2#cQWtGOAUAV8Uuc7kn%)I0I&U~3YTJ|WVdD7E`YD9{+mBBHP300PH@D#PzO zz56^-td&Vr9~c-oc*6G!dTc~NUa)zxFO?8^n;DJgY-eYu__eWUzD@DW(%Pp?WB*dN zvpNQW%7}P4TE*Sakjjo?`#GIQ>HG_pv()QLLM#dR>Tb;*O*`yk3bi@Z6HWI)@bh}{hW;IVYzm&KrM?BwL+ zjBsudLQL1SkczR7Kei+ZT0eoJS^;ZjBVczWwTkiqxF;-7PlC5W&bnpo#!u)tR>yZ%cGjNa@5%{o{y^80b?|2vo0AFoJ-99=}k=2B9$pROnuj zrl`|fBr^+_dGUPDO=Aw_v-BGeCI+)pngLBI7#UxyM}2#cBkZpTslM^0EC1=$(a})| zpM`~mH31p@g4mK)dy&1o4wcO{^kLZw9idMSx<}KZEw}_Ow>SdNh$=2%IEX;peNeA{ zx_X8;bM}^QuI$-4JFPQ>YcF(i&8ulI8)mPKL|7JoZes}nSZa@%I|*ev>P!LQwLy@g z6nLgos^!rs4tP<6@9Mn9V`WkLyu|ZL=gG=afN#esr5x`Vji`#32;Y`?=r?m2y;-ON zLxyR@Cc;=Ho!Uo6m;g>ukB7%2;ESwWMlA8lByH{L{ai|EHljAq;!B89*jm(y3O|jU z%n~aL;j?VKAh)-DD&fjAh&uE!K>bFft>5@GR6bhc>ny#PvU}J4_WU}fFo%zH$xH^+ zE@o!sgJlx~h!eeO85r1F-QC%0xWEmwQ`7Tbx3hi%SnpEvH-mFI*Kf`iSOWaWrI2@o zF$s<)ryya;LoMnOWa4o zw^rReBnJm8J-FTv3pW?4Fzn^~t{%O{0s|>_%wR1q;cx<3_f2EnPitDvXliCOi<8swJ?FTW27^3Ln#~mu=xbGI+_;)~}PGjE4i4a#&aHu__TkG-S|t@D>~g25eNX%MOmYx~;N4Rcg!U3$SUHEgC|HOLw*&qqOGP;|V0iUq%@&Nw#C z++YA9wN)+u-Wf(o4AFR+Dtz(VWapj4%wqmBYxGb+U!1PFArG-%g67xZhBLgkif_ml z;Al`lSzMLdV42VO+26%JsZnVWlC!>{LKB-=#HJw=6_0bZ9)?jI73Y-CPGqlct29Ow6jRWz-jKycEk-YIu@0*5K8l4o~PO6J68D7i>~f)yMheGVgC|o>c_#!`vswZQIBi z=^t~&byN#nE{&PZaoLg9g|{i15oNOVs8VlurxZ=7o1u$%oH&lPN1N%KcuRyjU zOAy!FQvE{1DCAFUjB5+;8rDS+z}sGs1p&uH0w2?l3wb%Z30O%Io-k4T8jRF$>t@u;Nt6Ylyv2duVmf$_xKT_$ygUUdl^7G09YGv@ZPf%k1-! z`A}S4YVRC^6yYBcl1dMV%ZJ-W{9EG*N~?2fX1Wl?X1(Y<0I$pa5w_1SER*ef?~zf2 z3k^_wf3lOTxWa_As_?eH2@aj?(GY@{zKASrco0OK)xErcYoT9hrEunIvllo39{Fa0 z=*X65)C=qwZ{z#tIHi?W)e&nSPRg#-NNcD90PY(sLJSPAZe!zp&2NEA`(?nRsinK@ z6z`De7Vy|BS#XtrZ?e|BINo}d&rYQky#8arN+@n)dNUS$6 z+uVB@U+rfMMIfo!GGYhM*x1-e3**r@%M#Km!Yjj@WtkYwPGU)ai*;039XFgSWY<;GH4qfI zS;UaWj0>Ht*x`HSqAVJMt%0zv%2B+Igl400@S+Z%xPEHiQPu*RtQ65-RfD`|l~=Hm zc~76PFEE!u*QXCJANGMjsGvHdb~>rpC(G>e3VGtztGbX)iabPD3hz(1yM$R@VN~Sy zXJrP=JcmwPI^``5p-?;b_>(@5jpBWl&(FzPv+{5qCa%3^y8TshbUKIFsrqzGNRAl( z-8A;pNR0T?w$j{hO_g zAYQjk7aml*DINCp{Q{?;`rczn2w^WX&^J=P$4;_?FbyYcof8uQi@klPxBwE)hRq;(Feydm>Bn~#xeBkUhNw5+08_Xo=EkHSILa4?(QWeAM6RMMo_)AHpDkk+ z+ia^}c`X%^)Mdw!tOyQFU%EX=f)%XOVfnn)ph zYDlX)VE?l{K6kUAI7ARxJfsyIlE#;D+>wHR`=3-qS% zZ^+J$!HAb9flu%%X0_p=#iku6AY0##^1e?2yc6!)CrtR|Bv`%2li6HwF7WDRhPdfD z(JIlbqjAmcMksWRQHK7@dcZJRKUw-MUrG-UWo}|AUbIGPep0+ zkKN^h-wp zJ=O1f8)zWp70^(}M{!UR%<2%yc}J1Wu&6NEL;rk@IIK{z4HB@7qBD|ZNI(~Lh{&)a z1Ruz=60gwiUuSvD8LddqYb_oTs>7s99%q!lmN?t7M3UYHf0$E=Y4ZAG>rC0G{{ z_m~GWZVVVoAfD9`pHM*2Dz_8tCv5ZL=`%Ut%crZ=YLu1KU?`W=&k~pwDq&VgHg#Hu zn-Fu3I2Z2&xdlVfKBEwc0Ti$7bO8I2-BG>Cs|&s?Y4@=bnk0dtYSn${F)L)otnj?= zagIw+ur7_r=V#QL5tuXl2)m{Lxd09U5c!`Tf`H;E8nZ&BhnLbdA-F`*NcnMI3^!9j z@x7q05P-Q2J9&XfYF;FW~D-V0C?j2@2E$=7(TA+M_d4_zek z&h)r~BZlgrYP-fJ+_+gM6YT4Fn{J^^N}pDG(}Cyg1Bwrd9?U+f{Ev((AdT}M4mFwz znfVS*5nnF+HmOtjI1BjwKp|ej^_CW@(T&Lhww{fK|qE%i56J>m6gE}Mdyu&ToefF zmRGTQ{zR^8s>Vamx|e3XRLRG+uaVr6grS6buCHbzFJ!>}%TR6YW{i~GvWWKedJ^%c zPsZjaW5Q+RBA3}Rc^1wmUytg_y08B*kom&as!s-|0V03TLv|+mj@(R|eAvrQVtBAH zNtmo_(l;8u{MiAwMV*G}-bZfy*B6tewiVJf{yNdS__fQ=PwGJ*>d8K zh>Vn4gjyCC##m>218CWjsGnVUb0}3VK*up!-qoUNm1!_JUitiG(#7#+UEh_!k1z!r z=Fv}|;sEBb0SV*nL$_`c^ghss0Usb9P7CtlX0Xm{qAv<=bUYCQk$JRWT5w!k+!%

66n|-5(mdZ62uYF;zOvFy2~6i6kC*dA^`$SS3pO zfk0^hN3BGzfJ>G0mj6m3r@_kE(BX!Ql(aLV(z~T+F&MFc6Yy1)qw6(V_BtgK3(R|R`Dy&FOe%i> z8T|?WV^29=KCiqZt2o_#+p&9B*w^C&$>F9g1H$N#wXe2)7noAp2M&|=uPDls$<3My zb11+KEA)sg*S<#UpPIDpIafhAKJ&?*0=noyZ(OE8NdaeugBXJVec_?tREH*2RdK|&a0+gbrT z3i{8QWbmCen_G6Fh<=gDUkKp6a|x!pV10c&$xW8=su~Tx$MNp=-v)n0sym<=4YZ zGa5hz!1`24Zubrdzqf$!jUm;u4mde$(3TN&>?5W~kLhzn)h@3ua{ z7#fm{p;UPTLK7vpw9A?L&ntp1FF(+Vq8K+rrO4=LbX8Y0QARc9s2SG6OoCktG19o3 zB2Rm|vbcNHmc9*$+)R4gLOh<7KfX9#B)QNwdV3e${<>jyI=6o_E0H@M0Tc|bCN*zl zU(&Afd5f6C+E=#T!KpfE@}N=5^yk7>te!1L9mONur>qt+4-oP!&mWcEO4 z3Djp)J1@^S?UlzxneS|^seu?$_aqBD5R&Kf=`lmHLnZxf^EHDUA8vv;w`rA4oMr%} zRMYxL;#wF@ewFW(!I6qDAb-BuB=5hYVmCW*pFBJ~oL0~#c5^snzM8;`*1eZ;O4Sg1Us! z*{JCR7@Gxe$S~6Nn0RCzd-G9rR-nQ)^_#8Mv6I5Koj`;2*oG+K}ypni-Q`P zCFZ7>D;w7R(X`%|?|Q5FUz0|XI!t$X~9 zvuPtnN|-jACIiWad@+2xF^7VDc;YUGF_INwr!_6CljZ}_I^w&6$o85KH-_+oN6}?w15c6)|aqGXUe=j~S z-D@OxhId7ng67GTsltNY;ySnX!)j_e=r{+2Z(p!9l99hksnT`p8;#@XJNUt&U650gyEmwp&+J`^`Vt~oI<&+p z;$fn7{WMZ)xys8+qG2WAnaqW!bfi)?BV&iBC!>ljp4N6{nbd{y@>x7c{#2vN9P*xr zTT#H?(<8M1!6hLLqSTaJvL{n+FFe1jJ9WG6=5uRWNNh;40nwQ+H@ra~-uPvC<9_)) zc_=2p_MQqVc}9ls7>Lt}SR=|Ps|VKGIX2%5O00U|J2;R-C;Bo~$|uS=bgrhy1%NDR zPqcCUOJ1Jarz`q>D3H{cSgP7cV_46})Ae@NylWlq50#-vMMP#1>4WXumd>RUA{**^3^bz&z22#k(*}9PjJ{-)b zQcz8WR*yBOypCRGexSB{2d>Y`WI*3w^%znM&&*9SN!2pQM^uq1VyH$G!7=t^`vUfRe6v=ArsV2bV87Rl9d}R`4wv(sa zSn;BUWBcR?uc$X)j+WOpUt7~zW|AdT)6I#qe9yC98s>B4Bzh02s=bYu9${MUzfrSc z|GbsyZnMJ>BU2$)OM99amG{Q6M4I*#F4oO5D&_}8_M-z;@2Xr^*v%h>o-L*};x?va zx)(uHTb|K6tkk;43VZL~7v*jleg{P9c{K_sycvcbnGi;Y;z}A$*bL+-mB$A?FIC`v z3&KNr4^DV_RZZ-4e5L27^z?-oC?S%(O+{WjIe8eLP3v-~bHK0xJ&3U$y$PxY*XQn! z7;-1`%(4}$d5@v0c!|$yFs046+juX3dBcrj_aapQ0ji$PPgJa?hMAoD5SRD2^n;fB z*``FR3;7>xr}?u|q+S(vDE|__o%3RHi8(c@99^!xmvo^B(6Cbsv(M3>= zZbj-(X$9#f@V3{DRNb1~7pdNMJgEmcmOVk;L|@dTdB2AB9T>cCE;Fu1`H-EOMFE9D z2trYLn(S_Y&D^q}6Gtip1f}Wi=r#IMRYHNb5Cr*Rqq{sG6~A86+DyNr68MNmu@YqP zjAD6G;R)vP6_lTVXA&QnW;2aVd`*W_2%0@(Eo-dq4g)?Zi-tlTlg6qxq#cAf7Mvqj z*`mN-lnRB|c(EK_vOS2Sx*EZ?PXhssWDPmADrI7r8Jl_c0q=2>)TInApXqQZV4u z?a*VEe+Dey9)z3(h%>E^or2RoKO*w%IRGiHvp5{IInnZR9H-!4S})6_N1B4xmrqbv zo>M{3dEt>(34$@`q?!rOl_0u0+o_DE z7i`%FEd19EYAaaL8h63zFcGt=L)lh=U|HGP|8H6U|D)Dz9{zaqsB|=UMXu>uRh1hi zZgg2rO`Hty)A;%mecpV)Wi>y!EUIfobS(Y842WF1y$^GBm4dvvzskHk=(^tBZzaVl zA+#Uq8EdxJgMXPc2aMHQ?LaAzB;UJAj(%JNm)@bf;>ScckSzbKSk8-MiAw1773+g< zTH6XgVX^f*E~IM8ocqG>_{YU2Z`6{nk}A3QN>F$Nf6c98jMSI{>3aF3$wiedlmpT#1qjjX8<+O`rqnwFDHcz!YK=)m zNYdv_4UJxvCH*)awfr@OW|#4+6D9sbx-h2H09*J`KPHL*+tKS|URj~L7Mxe!5r0au z3(ME1g0`=HNDBT<*q^DKZJwKBLTC+>Y10WMWxE~Idf0X8>BtihRvn;i*4JpZ0<)8R zhBLgPps4f?5m?Qx!%tau=gb};VEhXHw0@Y?XDwt@Bj>YBX>QM((c~=h4B~-vQ@z+U zq>74ec74P?vLD6VAymGeb@5A0%~7^4CckPm^q0tx##Qyq`(f2vHU!8~7?_IwaT0%S z3_DcEl0+?kP?mGgj4w&o45~OWv{e_(N}s93>d7%J3rVs!Vmf|#ClO#mI*^U8UaxyS z%#0URBZr&aln+zSK)8l*)P*#ikvEkO%7sAPX2i3O4dSIZH@^D=jVWknCGC{ z<70}WD$B}(R)&+@|g^gN+@$%$e ze|Unj_T+U%vQz_Q1*Ja5azG8{U}JzVCrZOF{pb-DD} z?LubGD^In01bTg0DisGr{kI}WWEpGlPP$OtHz~ikb2w;) zgz|%cVdZ$C9v94F&IP~>N_BP+QniCj%pt7^z#=cqAth~2CAxhIrXn8<_MT1-4cpSW zaKf#`lhLx9h{tL+fwubu1^x>$#;Evcn(Kjfdhafof*AfPnHwuE0VdCcNS+W;A7#Ug zwGtSQgNBUJjR~R+^VS0q4ySdCw6au>UR&>GNLhC3{_rSGO(=fBHn8C)pC~&{M&oR`;S-b0GxS2A$?eo}!Vb76E*{h1YTd;K1 z8zcymN*p-|doi#=%AfHh2kv2Td{^a0=Adm%>HT^U|qKKAMD3M|%~Or`$*#V{yv zc#EZ9hXYM&0pjhEG z=1zE3qIaVhby9K(y+k?y{m0Zr2cDV(y2KqkUoa=qUjH^j?rdP~a|mODzr?|tqpUJu%aWy^#?xD0MQb79jg9N-Ak zFEgu%4JkRfO_B6jr?uw=O{$;|^&``pjE%Gs^i#rt(x}PWo0)o_R&XXr+LGkNndJNL zPIY%4Z@kRVqM7l$^V{i1z7}*Jb`#jXIaqaMe`?R!OFD5y(1}WwWP|d>`>zIKY?Eo@ zs+n%m7jlD*v~4C@!c&Y-z3>)B(~RmEN;0X|Md~xoAsghplb5z#|>e4@ElS2 zwMN?78PUhnN3<54GS~D;82=$9y{#Z#oePT$3)d!&3WGMZqVU}4wEuGTzTTKbFM&9P_C}{dY}?+AgkFsLup0SO{&&xB?i!Tn#E4xFwp#5Rul&|) zRQI4*r7zkP-6BqhRGIuTw&Q44PmiB+C4Q~nuVuS0T`*)?!n`T?XwWwr#XF571)3`- zB!V58t~DAldh~cCUO#g`&wuE7TlyU<#aP6|lc43V?~nN%6M0&y;gc$a(o7H9Yg`ma z{Ise%M1;#eVmkvaMBZD)Y30D>Kv>;IsAqol;=G#qiBMTh?M&mTRdookcDKIKy2=HU z8dzm#cjr5tI`_=`gJGFonKN%~O~lUhk+$T}@qm$5a=BOG&NdatjGhjkJyW&TeX>^l z>$=6+*^wHhgh-nft$edr4;|lC5lBnt${ML@9d+)zNBsVlN#cd8)mAMx!UAiwhhsDH zHEhpWchrr+^I5pm4sH3WwRH3>>d)hTS+|gRCL0);n*Nznf5CkykyaShsOhnp_i3Qx z`KyrEHhl$>=FDqDB z9zd3?Tbt-EA=Yp030&B-7AWs2Pg<1QEB|aiT^l~>G}siu)NqZi2`#ah4OG>NzyPq= z$g4Pb3Tbalw^fNCNvx>zx|lTZyuQf;8Ct27v!~{f3u`MPAyy~hp}jZdyg$88vOW~T z>T+*(J!m&~;YH5j)?|A5bTbqvn>G`>6}?k`|F{AoX?J}+kiNf(?#%sLPhVGLRXI#H zahgns@>n(*^FFKvWynksvg$n)%TsT3iaB-~19hSifh9VKWQJk#wdJGTZeCSY3oGAR zAG)<(lGn%idbN7p=EY_Y`b$e>zHOz|dpn?2o@I}S<-OPrYcvly~VL(X9d$#n*B-30ERc`0h}VUTZ9YDvWr98#);z;3-%Yi+hc2o7%n1INDS(w31O7 zbSYDXif14Y%5^ppAR1-Wl%f*<24^0v-R$V*G?6}Vv!>aH5UBzW-QAd^mGb8?&n4^; z^BATFW%yN`Q$CZg$3VgQa}Wj38JNeE-^`Av1?z6ye8^W0PZn6zQ1*JaBvV#Pk-T-X?g_JSx_GTrW&R72o&$L3xaA1!ozfI zQGiP+^Vq&omvOO$jm>4y%Bu_7PxzLKw89y4zySi!iHn=0>PVDAMvvR!cWsO3^hbf?joW;DJQVpgI zAFK;EN=S;2AK-okYK*_k6WDar%`a|5&Z9Xjnc4WJMcwn?UO@(XE-JzskM5>x;-+!N zDIf1v3?Fkghn~o9f&gq8z>r4p!G;hT zkhIq5K-&-1SU<(D9R)Gw$%wB~ULT>{M&+>}uU!ak;ArOYSUjR#y1W7|Z|8w~YW(y` zU>xzvs_Ph>cKwj3wI}z4nES{Baycels@HQ{ie3xS3JBum6PJ?8PNp>N$`FmX_Vq*u zzrLI6`MKMxR?Fh9GMDen#*y;(f!Q_uA8wV|F3w~-)p<`r42pFsba*$tyU029>Uu30 zmJ9Z40Zz&19Dyz=^^NU)9A71j1~&w@Xhm-|l?%pSATe&u(v*oRDY?&W#kg`dB5-?e zvDD=rqY;<9ktW{gYDHGB&-A$yJEf=N6k(x-*EDI~@^Q)Li2I~*bH6su69`_TyODUQ zw%{YUqbTWbIL-DtTl$=zluomht$hsx&y!KZE?aAXI@`RywKL!FK|#pd&nC#xA}9|p z40FFN@yPLyAXlb-{Y34Fi!9(8C^|0e^UJGmKvg!lBcvMEq+h6tNr)3kh6DxWgM_ez z_e6YuekQ{f-`R(GcI|0(fr~8DJvYyTLc|p=esH_x7`VWq|0eO-2TfZ$}cIQ6_Re1as?6CfOZb_Ibgu-p)iN)7a^9aoK64I(UOrn<0Mps@Q2Nby9 z3in@~H3dbWpfK_HRxRu?NX?zcTDh!Q&d)HR=WaK zmk$6{;x8W*UC}Z)7rr^gQFJnEwd@QaN&>jjUI0>h&iXuD5+J_*xYGJ*rQgiyl^RgS zUjWj@&QW}FH2MpV378PzMhLC2J;{lHuNR1E`0}hCN7sQ;|Nf~6{aDNeZRaZfFzNX; zL1Z+7cny?q+-wPL4BdbqJ(>V$-XwrhhsMBgDfm9U$ZU-|L{RDUj-6gM)TECyQyGc5 zrb4|cR3e1xuxdjy!-$DbMT48Le(T_RNe?}^P;?R8b+iWc49*<<;3kX-NQ;BxJ$K~z z=}}O1`sH0o%4vyJg8+KDaPy(gB_0TLN+Sif^>bZvdK{%&oy{RiqVwnsvaKvZe) zsGy-b0MV1E$tN=$QCO(XVD7`Syz5AD`nEY?*>UtxSRXUZA~Uh~+)KwkiioYb`#oe2 zV30C`F6#QF+HP|7XQL|7^2$%+DOkHeE}h2vq0eNp8T!RX3+i`(E{0Ou=^VP@`G~sU z6>iK;wOpH!Bx+uSehd)WvTn92txa%!fckj~)17IuiICmhUEMxr3djSs0^%hf3b=RJ zVhha`xroN;Gnxbx%VYaIpB;7Srv z?(7Y%4()R!M}hgwpsToIytVn|O(8!ObgsWv30$i?>^~=(OF}Q9$9s*9MZesTt5%CY zH5%N5Ij%qnuEb3!&jb?SMJ0Roi2{ZB7IuXNR^*^@w7(%lTr-pJqDDG< z*<)m_YHT#m^ZB9r^FU#iQZctTMWiAy7hf^O6!hSFv>bj?9r|6YU8V)~Tv~DsNYfdW zAh{1uAs`D2iUF?(+Mb)%okoEFe}^P+HL-NdE{eYvm(nlr z+bi*&PUKsJ)V9GPL@EizVK&VSJ*f@Lu$4;>)e4X-+REDM4>RJ^K)pvH$aujYvvR3Y zs=(R#>p`j7>dPg*#w#yRcjB}}RYH|rbU`{(F6dz8EP&fJ-2 zo|)$tEhP>T%<7jfg3}RckD|T@ED_!BT%MK-*1%6yhTgXNdN->hgp1}$e8dD?y*h)p zt=GdmB2hivA`Z~iI`rY{%L*T3ySlr_>uu%0#KJ`*&byQi6UuC#=1#DwH#QGztz)y1 zv4{dG06GRqKXp!SDgzdJpmKfXxC_!_@@4jU#Y#jd05spca+EGOjs2bS z^wkj?i{qjJPea5Z?rQvdmWGf|_9At8bf*_@GCrq1^8mq+LgPpYBy&Pw_fMsCT9^X% zbdDRFGDdK-D?A#J(Cf6u;Y5sEN4ug{8|1=U@$D&A5plT$v?hc1V632Qw17ja0AWi* zZ|}oKvC!q4{5Q_{kPTT(L#c-IIfO`^&9^ozUy($(pv5#8rYheCb-_6TuLfAQGeiov z691b#g6sBP9=K%Hsm-Gj;0GQCA3s`OuaX0(K>H;fG{k#88yYZ}=J@+F?TZpb1I~}D zWCaB~>+eq(lace+N_;4aj!bm5y6^n0im`$&TcuK4UV+m#kelKJ^6Y}m>iA0q^9|zR zabCswp3@CjYP|A{WM2RiCM%T6n@8#o;k+N9Ip=A@ntOS{{zuOms_B5=9#aQu3( za)EFUOreR1#UDJ5JR}GW#!jqRI@VIK3uST>V0W8LGk|P-()%4x#vuMchyrXRO2g#u zbioupUc8q=qyAB7%&&N!R;?&AKID0Em}`+lcz)09SqOFHy|rAq>O6jtvs9V2QxA>q zjwsMeA#!`fXk@*Y3wTAuzih0pt0@%me5dy3Z-Ks;%!}6JAbA|lmvt{`zLJA9O>xnQ zj)YDZz)G{1Wkp%gJcL#B!JYKmmj~KhFkYRBgG)|)ZD*(FPxe@vtko0=xd4LH{qUBT z75XNEoFJ)>lI$ZM2K`(6NSPwZ(g%)>6{7!=h86qeVc*g?yNvoewlZw_!`3rhiorGe z;a8e3Vo@+X!h3%>3V(x7H^&I^Av01s%I}U!C&AA4<^-p0LhAQ!R}d)tS$04A+Ii z_uWMN{PK77#BPJq&MAv|@*zCio`@BC2jyV&=e78O*OIbjf3X5y3)9T<0Z6S(ZWH>` zzlnX3)s-o}v%eeNnOdoy-6u2{K~vzP?soJx82Ho#j3T? zbc5cT8~a^*YOHt%4II>PvoD_a(1E+4`9K56PC(2Aq-ECM$J-=2@aX91%5Bq1eN2F+ zd60+a%h84&_C&SClKxZ%2nV~-M)-YU^x2xysXU1zh#*CSe*|q%DX@VOG&oBgaGe31 z3n=|0f}IRAKR+?Z3MY+7p;9?iIxWIr%<*8oTJU1Q@8g$UOZ%y0@uBB^kv`#13Kylp zFAO9W>2Y;SIUy(OK4k#@ua7`3S= z%O6;2y+d!oIr94XqmL;_1lL3WP{oM&3> zEvy${)&2BkPWHUnx_Y;?U(jUx5fZgN@XG_$BcSq#x&+%AFU~ngY#0+gh|tbE9G?VDmBH2>gfVa`z@E*QxxLu3az) zNo&y_bE3zGST{(9ROG0qC2)iVH-*%`Q8@K45#wr?4lHdtmwpsSXJr*JT#%|Ky0|F! zGnD{a;p5j`T@WksVakR1$vdTU?|6b`Zu5_+R2WS0;n73^tq3~jJ1mm!Oy{)uBC!S< z4ah%ighUPKTOdq^hQ5Qcr*M@F-BUCMKjcnF6xfrfL5@d8y+DjIC9jdT`#ph>O2GRpCq!iwQB>Ur|`@Ncre7a!>{h1XiwbHg$n+A9R8<;id` zTg!)bm(}>abPCD$q8@LhUS%#y*V=17IHj*n-gq?1(94t}o0^8bHDDQvy#b}49nYK7 z0<4@v=5PBimD)Of{d!0N%7m|nJd#42^o@^%HH!*o&>4%{T7grsKw^mb{BxD?4&Kcl z!7Lbz0$WbITRHsg7OCyBT+7<|BYJ$7M$d+-YcGt8E36U{Wm>jn2o{X7i!-#&O0*vC zG;6t4gz7!qQ-=4nw^l03oJx!S7G5k7P$Ituh*O2I1Pb{0e2%RA{o@nghq#t<4e4=j z6+(J6Zu9K6)aoppu-O?w_QIzCCurJw9RwsDCb~F!t3+aOBw_kN0PO$Jop%@AdDCr{ z_nfaT55q|xh{5=GQ{m)ol->r+cm$#5c@>(gaiD_RrNXy&^#Ga3n!l1^CM|s!IDE6G zy-eQyfbv`W>$3zeFAYC>JL59Ha9-MPAv`nF&xqw@OR3D-VbARsIQ5f zbHn2O#2n0wDRmujMG{j`h*g-gv3ZAs0_wlS-7a{m1PcJtl&q}snkE6Ios0`cLZSv( z4CY#P_5z^9LQ`oodsphxV0E?Y$GB3<43n>;l;N|Xx{rY$op@s=el}G%Gnk+3H%UkK zILE2QF`%M?O})2Q7%9!Y_5-oJG>UD6*|^jx@K<(ICFO9vvrKMS4@kSa|3X0@BgDn% zWLre1KJaqEHv5eI?JIfpv9Pcjy%rRQ3uG+6@+uYqM*md%0|6uwx#_@o_(HIy<2khywJlsaCc z`)`GkZdOv-OG&ausf7jn789(D5lMvBXrUN4U?8xcC5i9302LGkAeZ<%O{)Yu;fjcu zI5q_AFF&LUncerGfSUAPEWTqTh>&OvzbK5(ziNIuh-k?bpuWP{O_#XiG9JCwa1r0u z!gTC$H$7TY{gH7AOj}HI7zE6`io|>Q|8livwL#g&!^-JFECAMTdO-Z`1hFHN^A5}~ zhGUj5-KyjmFAt5q1?GlbSu57W-4}mD6IhgO&X>Ha(>>YCc0)h?WQ9>cC1pc4r#;ZM z^4blT))?@?jAqTZ4V1SEy}Mv6kdo06)ssf1g0d@-=-|E#?Tf||k|dGGPh2S|{b2xh zQ%%8o{H^w}yw^)wC_aQ$=4o>;C#W4IE#ghvyMgTKcQBxwsr9j7uwkz)u9Rwi35fAUCNT~BW6Z^O^ULiPa|ACm;hxU+tO_A3BDtU{LU4c z7$zUCX8>PAiSu*Qq{qe1d+e~3eYi!nbVn$%c+^YLMnS+=`p!Sx4;XdydlC8jBt4=EAt*J;9 zg{J!wx_q7YVJUAr5v28NSMH$R`>3&ldj9gWPW~u$OgAW{~nm&MxePdjOkrBC5%6jWw zgl$4(5>EE})DL`G@`<$Hq;FV1BX`2jtG0>0;|b7!ZvvB!ETX{Lev{LeFc@rniaN7E z)#ElVDL^ajr*YpiImx6^X0~Gy6MaD(+0%tc)?{C|GbP0Wn>Fd{^u2FEv!}8;U24P7 zhhuR7|_T;8X zzxI}=!V$0RnEJ{^KNUVisXeamYdp9r8Rcu%{pT&RC&8uTyuZ!b0Wu{tORaH$Qd}im zzza$?mH^huTZBI^W7>z1jL&3 zEHKB5OG3VCh7jpp;jA>=wj>i|i|iSP6k2Q$J_Uz_b^~g0s$bE2;A95W0ukU=%7>eR z_vk?X@(t@&++|{3jnf{*SDObOWRZY0jq|}6%|FDw z*bnHDunoaSUQg=w_v`5%@C8qA9==W=33XSNFTtkv4|5SW0H<`NxX5nW9B|*_S*4{K zP@9(zpQ33T7+0 zqeQAZ8o0uUJ@mnl#<@@Up-B?l z`XXJb77?G=7Qquvq40g3$4|n5v&ag|s@2apP*CL|#g6)R$_kO&LWO;3nxrwewe;EA zIIH%h6Ic201(9RRvoi6?y{V5@9KMuL!d{2yoEEA6@Hsh3+~Q2TUg7#k*4AmF z#)^2zIa#v~&p)uUjCl5K6QgX)mzjw$Uu=b)UY-H#xAw>nTt?;cUwEJHe(wJ@UaI6f zyFwOt9Y0F#D(M-I*rLT6-|dmLm{--t`lETCPm-t@9{fiBSUW$}cejzG{LK9%ta{sd zweTcv)XfvG+haCGzH|F}iAm<3i%Dnf`rhwkK|&fKyzsg!T>FLeft5$sbT&+Y;NWZ; zbExE|j32$zoJXQiRPfTLky>8+*t*c${1#Z3UqigTx*>_lrKjE(B^v8jw7&eMFeEsH zwFBViO2cRP+@^KsU0et2p9#QG_D$!9?phEI^q9djYNC|@??UpJV6pBz#=ZA2fp`FU zVly0?w00%5B^Tc-kZ9`A|F-+YUS99uwBcS;-cdYGavTxB7e&0*%VZn z?GuD7qj=L#Q|R)}i>cOf806k%0xq9<4FWKf?1ZG6Q@U8UQSD>7Yt!Xi>)2ZhiLEe_7^)-7w6$a*1`zb%OLPHBWg+wZ-7&Z3B7<}LyKB( z*=6DBx0WQm_$$42SZa;SqePt69fJPn5(IH6h2@9wy-~8JCpuXua z@FIy`!Z0)(Xw!0h=w+k@i{&|3kAU8^1T<>ymo zziRkS1EU`@h6sDHiF(XN9c=`xbjQaQ&7`IFlt z@%d*dMXM+#@6Cz878KKYpy}rd-Z6rX@ms&xJglV6FL2o>r}?BBd@efICfbBFDY%NL zP(N%0s{Mzpjmmq*KP2vaen2kh{GNNDMAG@FMzeWvI5}XD7E*@>Pwc!sarbhH>&>dZOK}P%}#J7Ly!142+tr)_gzx> zW{wvC-EX2qCRmio&vS zVm*CxWfdFt=3065q0{>p5U&F=9`xM3v)bO~u4SGv|MXLZ&r z>cT;RMJ9qFZ+-miGUE#m%e}-}pR7)LJEw!{nENshDzodyDq-8g zfx#?p3DbkIB2`BALn)!Ko#XCJ?>WQ+T{-sF7OliC4LOI=)!7-g^P1HNrrl4&D%oNS zmK4LeTDvae4WcQ967^w|KOQ)IJhrWZDH@&&`s`U9ocibep1lhFKs8ZE3p;%ydVKZB z`Rw$r3sZ>fZFj|J`O8#VQIGN<(H`n5E9+gSRDZLpV}AGPm$;|CtEi86Jl9jNA3r2R;zqq|!57laM)d=q#1Zr&aK!i$G`?WGWo`y#1 zXm>7aYRW9+U|bEY@b%JT_IDS*HYK#aY1Ijd;N>L*qJ#2ZYRq+>KD2HIF!nheT^`uq zcy~VyYO+`FA*S<#{7*L@>*c+nDJ>s1A|N}(`fYLeq?Ia&oVD|k0H%mcuZkfP<$8vEp z0ja+fnWyMRH5Hvw=XxF0QXw&I%(YV7g{UW2T72o)=F`%Y1~qoFnN$dAwifl3`GOH5 zh9Dx7M4<%q!~T)Z>Ro!!vc6jSj=LLILfQInDgluz`+qo~d3_6t-=l;XRK9nAK!E7+c*3*&7AZW3KN>Cd@8s7Q3v<7I7%WfbR=wYpVMn9n5fe; zzdLC1IMj_qKhZ26qa$T@OJ(~F-&wd+!S*@bY50xX@2isOD#|54_-6L9S!B>dFx3!N)-;F4-=Nc)LQ3u61?T%7%Y#N%Ap67J`E zmZp6^OB#l z%$WoX6!Go-1m4POwfVc(_DDl9qmOb%=P2Wk)e333Lfal>)Kd=|9KYG$4%{1QUC`8b zRKl`}stlFm^vpG}i>lL0HOa+QD!@0C7A%k1)_L50U3ZA?hU}Bw(pb);-o|cR0#i-} z3}$lahF8t*U-a+6C=U189>`crH9sj&J?PYfCozcXCu0|C;ZZ+gTkhs^T`C)jFxVR; zxYDO1TbUaFj*Yo_Hm=6>9tb+K@$u;Z9nNa1cW|755B>dW2P`lu0h=1Cb@%|#GTeKA zy$~E<>p-PefvDb#AnW}VKI^ty)u#0Yj%tkddNe##=Q|=HpzLhe?ypM{?4f`m3*Zo$ zMMVe=4UP1E#$au{azKxL{t73oPsXf_gho&vKpcQd-Gbq3AEzJ2CmXg`+<}3x-$%cA zMrDrkdiwgRZ>oSr{Br;TRuHt#McId)9*GPC(hVxfM*DEoCv-ZH@bFI`z(1RsQFLSpwJ1&CN5?y|sS% zx!reZY6($Qb@yW@YsOC_A0coM+ zO{ab_f**(1n1+H2L%G7MHmhaN8+;!I>26=Die7e@;vo-*WDN;i@AXqjZBIpt5nT%k ze}{if?l9^Mib-|Fe>k%WhyJpB_4tGBh3O$VQaw01s@J_OWNbL=pvqiM%;T<)0R6LVVF1isAFB|1M`?P^!m$iJ`$0-dg!}^22LkB z_twX?oneDjte)yonZBkR&m5}l1(Y*{MyLD#Nmh|tyc@$<#c0Ub3)0F#fF_Xbpd^e|r zyuH2CtBfi%!GWO#!i`xHXqJ%ic9AehmMNp!x8FXe`Ur=Wv%f-MJTkQnH6|k`cLInw zYk~dp z4xncKS`RMY5s9_#`n}^d83kM!&O3jr*5c;j1ppp#!SFBKeza24n84ZYg=)Ksz+O&! zmzu#HcN4}iIKA1J97r+eVc0>f{qUAg<#FO1A>46oQU>ezH8JioD_MLjdx_{~ znl|C{#Yg6!g&DQyu8IAw&oq{EPj^<;8z2>qj+r0vq^{NyQAneDt%a9WGOa*x_)b@( zL&4(b?(h)zFuu*TXvO^L)6PxnZylRYZDqcka7u>zJ$(F}fvxWH-IvPmHIiTkHE2nD zEF&mK7K|#M;LpjALyM~rm6O$3K}?q~onqLm+e$jLW{xkpvw5=W%Lf+tcs?WLI#1&Q z@r922vR9o(dK!H$9h&k8Q=cTuZ5#Z0iVrb5>-H5rmD_jPYzS9hJ*vGupXgbAJpEA5 zxiW@Lx##jebM+jYCVLWIc4hgBPtCCLXUOq!Z7DSJ?!YaNu@`dY+~lhv9B>U-V**;vm8iE!9{u()k$8d{*- zm-ooG^Pc`a$CZx0!DjXIMphA!_X^YEw_XKT&+fBWRi^C$g z0{#OUKms5$ZXO7qn1p`rLl&~Kuxyb-chse?&iB_PllH$l7iNx)=~w9&BhhE*Z^F(` zynoKNi(xBhd{_4mtGCBiSp4F#_;o(;IX?27s%Pqr+s9ol%9Wi$je5=BhjhYW>v66I zDPx$?)tfQ_w|*fUDmwO!N^kLARG+@+6=--hQEP3K0O{esY6y&r{`Fh+;vKEj?i}}d z@rUjY*o{MCo_7Y??zKzz)9YBW#t)A)a-6WcLvkK2((9GMLs0IHvJfOdHgKz4d@^=h;Wzrr}DqQbQ5B%t>0i1<5xYL{+v`-Vv|cEP>Z_*54`K4t8{@UueIqVqs?V%o zhm(q@*fusgeN*OE@PKJVC3gx#UAfiJjRZ2Oswo2%g$~g!IGi!FP-g#mKW(AdYN}3_ z&v%7Ii>py_FSlkR{rp}^=qX=(cj>6+{s`9R5&mc~|5Z7g4$YVccCyDzyKM*CpN(x8 zMfIt8&s6!=8rNbbIXX|X0;PCX8X`7q3 z427imb{bGKNNM_LVU~`2SHIyB{@jYY5%coj9sT%LI zoE+m%qJl@3Pbz%y`8f9$lef=kTWvcw0Za7tj?0g}Up0ECg-QbPAit)J zKV6Go?lRurZtHBMON3f}cwS^3HwKF)V#aJ#c@fJ!hxt7dDGe3ZfVH2&UZCoPsN}>c z@2sdLI{kQ%d&@f0+WIpwUy6Ni2?4frK>%UNeEBqAp9ASB6xPC-!muZGhm;wIX!_C` z%sjphdCt?XFGNz^lD>VP^w7J;?GhI7ro@+7A02BUwV- ze%{g{{a9YfN=h?qpQKZ11a&|nXPWrE^(1lq0zWcQMYzj!uZd3iXQkL3o|$esY|rT5 zSREH`?694;v8#~8yRh(Kxd2HZ-X9p+Hk!NtBq?Z%>yU?!V%$#%KQiLBU4VLBd3n{h zXQQicYZWO7$e`kW7!EjzLCYc$)VlY(${}5+f^|UC`T1zm-TG9+gx|Cz(FBo76Hk3V z3REVBNS+Z@En793`H%S@`4S*Y!qD+u1FB%}4-N;4@`a-X^S&v{=f(lqd(%Wax6Jd5 zSRL$l0-fg>Z?IC{I_whG<4`riAyeJ%<9sfJH3 z0Z(8=sHO}0u1F!OXJ*E-_a}J0Q&#}A%c&6IjZm%xb_q=Y-yR4Mw#IaUKi~)jn#2do zl0|mo^?^hn&T&^GK>5Pv z*>~>V7PfMak-fXHZ=uBRsCGABekhOPsg6UH=eO7`!GRr5u1T1~ zI}3B;yW|Z|7!LR7Av*x0S}|?Tzh@=T12xgXAqd zNc1)$fkpx|ej`qO_xO0!2#zSjJ7z9}Eoi_=)pKM+fBPj!Lw9Hp0+1!OHR=-tA>PFu zAr4Hi^U2QDWNwkQWzUuJM5gHw}vCKP$I7oa+Y5;;n`9T z5HPr56x6DenoVbWxHlppB(-f}(8FFQbZ!;i&h z3L9N2gaV8rCm$r18j6u0?!@2-jAFu!?8-TPt#$NS~Nrvx2lP@D79 zZ1Bzp*Ns)#Pw|m?$%B&0ani#OKZ-=alF)C~Zl7)OTREr(oGdTz?1LH)Dgoe0 z^wAQWKIytLqB5XeVa&+>_~P`Q2?2?DbS(g*xu8Bjl9_+rZb0(q|>kxYl_b zkC})8ZcvXxv|bCesZE4HN(Dj;aHR;Q7mMm2H`2A(`R+WM@DNJq&yZX`c(AF!t`fq} zwfaTlP;~#9!-tVJMqcd5x)0qWe^(A)n3D-20AE@$DQtgHz$b7{-8)0dFtH02;YoT% z=i_stsF7mEIdXG4!TD9KzYw5cw+oU>nGb>nYhDx21@g6kIwIJ8vO{&-YKot$-5N4}0_Y{vqkHf8oaBDP(02Fm|#i>Ti&N$`-46t|L3<40uVLkZ?*> zio+O9N&l+vn$HJJH|9a2o3+5o8 z`}p`3l{RP)mqTL^Dm!=X27ozD)G0>o(J^3SI?LjH10bb{7eE8Ns2RLgv-pEqqXfl? zD&+%OGZ^=C1L#%|mgvBgIs{;x$lreJmkCLjbBv4RCc6fJoK{NK~XE zDdjK72e&kD@3?Ort(JqP)$GO_-*goDZL9ppxtBi2vSW~@`FOmM6cy3R>-pf>(0U@b z4gVg|k))|$H&rzfNezTuBR&DR)L+k%elYtf!C$klH$Q`_U30KKK#OwQMw5Q$F$)j! z)u(DwoX26OJJxy;S`6O%bZnHa{-n<$iax9p6`1WHApbO`Bh=iq)xy%N3R)h{pxd&@>ik`BwB@F_WZozoXBQ}7T=X@ zknX|6S4fKGzmz z!+`DDb|#>!j>E`C*&#{CLQ=%`L<0nYK_@HHR8>|jL!xbV%s3EG=Ji*W?bPKb!L0GS zS0E>KTtU4+{X=5_O!!=lv&Ywqt)5LA78~k#S6#y2;F&a6{Opn&s!Foz3{m%BnqF}$NkT|zpWk)b!wFFVe~i)oO`e9J$an|&?dii-?R95^h09%^7OdIBKgVF7}wi3 z+3e<^h9k9^Zd~?@-#rp$38Mk}M!Pp90~HFIOYdiRn*ckr&`XV7V2(&elIQ8KDl;#(1-rJfN&i!O|CXWfpJ|BZRaKwL9K0V^>(!saw5cxwW?^dK z(|ffMr#Y=H?I+ErNyyJnV(n(yn7!^?xp?0_LAHJgHxUcvu3@Es#*i*<&lraVOd^c zDj60WLMg*!aOnNcv*qU<|6VZ`>LDh)nKF|hjXQJQ1R5w(JiAgBu)0-U9Dnk^DGlPN zFtwBxL;^$$FGUJ<)Gg`xu6D@c78yMwu8#gZrH*#xc76Y>^|QHllQZJ)2}u0KU0FKvk-}mGc192P)CkYqaKS~&}&HeUhDjYzJEd#d4(NyTsS52s4s%|=1St+nh7KdC9 zwNM4U7?pOL=ciTJ`rg z0TflCIcabBA(+d`?DYBk=1zl&V98NwHL}eD-!;UzNrXoc17^B`%dSNt2>%V7x^d5W zZ^)R`G30ZLksqsrsJ3W$u|R*_UgBGX1fm;0My%3$CfLoo-h5;7choaYf-~L93WVS= z#fto2vhlQ;O2AlyC4#)cFm-gm+r5r8In_~!5%bp$)OqyC@JsF6TX%O~D#?R20(Qd( zj)Rtp*YEb7LZgn;s3U=b<9lH0)!VrAwgIDEePDdZ?)@6j^?1a6~_qJ{k|DqS{BNE-wak z=ZAddAySY4dxKg*+5y5^ zMs_ymsPul)yD{HZbYqL;Akt3i;i8RkjG@3M>|^s?qq~!J`7(Z4p+M4NKm}Zeke0TB ziyi=y=9oCR#Dr~ranYf7y4La#Ft%!EFx*BtP-PCfv5W9*+0Zsk&N`sv>+P{B1AbSO z18pq<)TrN$mokOyL?#2(6*4b((2A6Mwpz(4NR%O4vN1^^P)H5-ZB9`Gcf(K*Jg8jK z;q_JRp@;Z#ns3npMf66$Vzr!Yc=iAMx zMy2mzXZOB#hIf7i>IuWDy#yLX6L^SXn&Lb6fX(pX%dZFkhIfLkDZ&w{80~4=yf2ds z%n9(8Rf+@^^jlnwugL^8qp6q-OyaH+oPj`n@pCY_9^zstRc<0G4!BCN0)fU1fE&>? zG|Xt@0PI+gxwm|P9hnIZ%kgE+=1BuU&B_VrmLos)wE6$4Uf=rsJgcX-x2nDpS3(*1 zUO(FVF{A~G*KBNT%D@)bhcFUZbg&`OrdO`a;wNZurvO{B+3nvP3a`KJ70tfo&bs42&AX4gIpMV_inq*CMEA#;M1dc(J2vU28mVZR z%;P%XtBo|fMe)`nkoYzA*Vn!<1WdO;noCwzb`a-8f}2|teb!&0n-3_+Pbw#wX?OOK z*aSVFKO3K4Tzp0{VN0C?f~TguJbYd<9=G=GHzWoLd&qLS8k>xa^#`o@M4;k3d(07$ z6}ew_@SVCLSCk~_FrjChDiei{2EYJtbszU3e#+#dh!NPDD%_<%RS|9%`3}SyJ9EfN z04z<~3LyD6v8BUQ_v-H&?#3~NaRPH0eI&+xhux_n1+4p{Ao>}v>HQeY%tW9@I;k#8 z*(q~_U|u8{IVAMr2)T=cN3H~n@qnoLgGO2@mRE|}j1JH2Ax0`)3x&oPCCE#5&@mW< zJ8%TYQ$4k@Neba;8$hVXR$v%72W^E?^QtplG>I358SFzx`c}cBCU_ zkYp3+*g@i}DFRV}xvM*?^w}1GNhq|+rsgyUP1Ck2*S>a5(yuaD|-o6&zA&QfwY z%Ds7_;I{AJ)7XOdB8-*1Ar_&ouATDy)CcEXJ^W(gp=?8DYH;YCB*{Gy{?9S@-oH6uXHja$eD@MO}M zFgeJ5J<=i^NyqXLs1)WDhGo*Sl$+z?v@>Yt{u`nxEHW<&h&spCSTR6?&~ods857uv?K1xyJzLG63VDCxc%vwzZGZ8d-)5&(Cl4;TS5sa+%MM>9@w zBLSo)5HRcOft>T_+4qx^Vj;kdl!z(xz`(j2$*HcXxwwz#caPma<*zR(z>_Dm>rc~T zed)~8w9RJ62=u7@5o(+)qM|R;AH8~YTm`0{*ynb{SVj_^hzmj{=s?i33=BVXccXip z7dT*j1&!y{ysF@5mivOUUD z2qHWe39JLcsWF&)4oEiHa4mmCWQ@ZB#Kb4L-?{($l*~)lt-I@Kr>V6a}0I z{A1G_@u%a!;^8!cQ31uHNy31>1Ae@dg@8+q7ftv^4s@S400cJj5aw{opXkfWM_3=` z=Yy=^&wI5bNBQC)a6op}|0&x29^p$ph$teWYuoM>8#M$17lz<=q?0 zBDZ!Q{uz%ikM~z_Hf` zUtz}uEGeQKY;03tUKTaqb_{WXkzSYl+Z1V8PkxBH6_|hqt(gJ)^aQjkRT0#6vMzKV zti(DhY*a|pWOw(HnW3SfQ}9Y7IIoll{oTAn!4mwEp#Z3gDsXK3JL>u%@`b-W5ZTNuQoYrP&+?{K1cpO}ir0G9^o%NpPTBDae5t zfK^gb0#!>;gf@d>30`+!9zOj|ylj;skfcuv*mRs=kUjK;K}Zwc4-N`HCkuW9>%vIr zM=#(uSOElS(S+#{;r~2a(F4d1WS)uYDhF01Cckasc%?3H3)LonsDm9jQE9L=(0O&S z{p9GNXECEF1_1~?S!HExhd0rZ9Z(onJd}d317>^8^%mK5N7i4n$O>Y%S(+WzdwhaL zOj;B=SFS^MC@-{vldbh+66h>NVC>=E8OeKnQ&_mET~+x9yy)cQ1R59^SS#x_A7RZT zXJKWegea1yMA|#;AM4Z2_VVF*S_?CwplMgWPNAonv zKu?OniYo^Uf*TS`>E#AB{`rl+FYwC3=!*FsdOTa$-PP5WBmABj1{d`Msn+)5(h?6D z{oj+|A5*&}_|DPM$iU#>I@&nm!zn)GyaYJK3jyf$fzwpl5=|lN{~Dinbj8TC$Hid? zNbjJq2mp5XMgYJI*7d0VS!e!z7`rPd2unUd0VRJ1h~__-`-BgQy6XH#J3Hb}9zVW4 z@F{k4?i)Q2Cj4hz03Vjw^uaiGd;k7DUvpgg(@RkS1qASa><1z06R)^N{WggZU3JqNsG83%?2VS`QWW=6A}d1$<7OM+<_Gq_=DKR1)mHGkeHq9f`Sc8Aj{1s8p-JX*B0+jIBaJT4A_~^ z1^cT`up>L-tX#sz2WR@>!Pejf<BF|FfGOiH3RGOHki8HJnXVH^MMT73N`_{j{w)2qo=>$w2boa#p|ynECPCe& zDzLK&z4x`rDRKB${#PUhC6K5JaRybI8D3z+fP77~^UtdNcV>XOL7pw{y_lSqmNo^* z;CEE+)~X{hPNBe)S?c`qY}?dHR>44U-{S8$``1+sjF`(r2OE>y>%jfJnzALc1RwJA z+v|O-{#2&9{t#qI$;#pXHy#XPyFiG=cX*kuBOYZF^*7q%By{{4sp- z#bf?jnE!q@lq56_{kyal9|f5|eK+q>Dl(n|vOvAm{8lIZU=6Vj2@S1Pifs9xsT}bR zHt#Ziy)Z=h6kw%_*)p=*8YgEdgU7{DM6wJxOO^iwAD}UUz+i@xk@+}-y}uLK zzC4MEjg@sy4Ew4GzfBOO#u#{8*UW5+90c)dbo+c>VxUC-XLJ-b<7pR2pB{n8Gw8oL z<$VZR{GB3w>f=__mjXU)g_nY$0plegm&Z+^5h~FJDT_5ajkesiYyWRk*z71FJ1~GI z;tWNvKIlaPq!Uy9PZZ_#kO)f}5fDYXUi+@%fNj1ht%T3*-uZbu`+`X06;_`Ajvh{I z$Z;1E0Tl+aU*4qvu6HE`46%4+V3;!aNcTtR1z$o+s`E)+-nYTIxz1tWKl=?qj280# z|3C8EP!UpK#?aSXSy?Fs;@K>ml9JK~y_^PZwlgZm;x8^*g24q!(2RO2%F2HGdwXr) zTii@5JZ9UBk*g8KDnWUMo8 zm9~+n$Ato6V0LU+1IwT5;M~Rf3QQ9luyGKW1A%u2oQe20|6kb-}HcGnync+Wk zCX@`NXv=eAb9T-4hOn^VE05Wr)v;nF~~8USFS^)(3&TPfN}-yGF);?*l$8!7G7h%jcS4 z0Jr2D2Y)X?@-OzHsfh(-BSu@_UPs}^We#?C*JxZ^Vb0NqWOvv(m|%b(4HaGGIz>C= F{{c#ZPPhO7 literal 0 HcmV?d00001