From 7a6f2ddebf2d9899559e61602075bc9e60b64698 Mon Sep 17 00:00:00 2001 From: liangheming <2605400720@qq.com> Date: Thu, 27 Jun 2024 14:43:32 +0800 Subject: [PATCH] update for color render --- fastlio2/CMakeLists.txt | 25 +- fastlio2/config/lio.yaml | 23 +- fastlio2/package.xml | 1 + fastlio2/rviz/fastlio2.rviz | 83 +++++-- fastlio2/src/lio_node.cpp | 163 +++++++++++-- fastlio2/src/map_builder/commons.h | 16 +- fastlio2/src/map_builder/image_processor.cpp | 91 +++++++ fastlio2/src/map_builder/image_processor.h | 31 +++ fastlio2/src/map_builder/imu_processor.cpp | 67 +++--- fastlio2/src/map_builder/imu_processor.h | 1 + fastlio2/src/map_builder/map_builder.cpp | 28 ++- fastlio2/src/map_builder/map_builder.h | 14 +- fastlio2/src/map_builder/pinhole_camera.cpp | 241 +++++++++++++++++++ fastlio2/src/map_builder/pinhole_camera.h | 63 +++++ 14 files changed, 765 insertions(+), 82 deletions(-) create mode 100644 fastlio2/src/map_builder/image_processor.cpp create mode 100644 fastlio2/src/map_builder/image_processor.h create mode 100644 fastlio2/src/map_builder/pinhole_camera.cpp create mode 100644 fastlio2/src/map_builder/pinhole_camera.h diff --git a/fastlio2/CMakeLists.txt b/fastlio2/CMakeLists.txt index 7fd37a7..61e4562 100644 --- a/fastlio2/CMakeLists.txt +++ b/fastlio2/CMakeLists.txt @@ -30,30 +30,45 @@ find_package(sensor_msgs REQUIRED) find_package(livox_ros_driver2 REQUIRED) find_package(pcl_conversions REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(cv_bridge REQUIRED) find_package(OpenMP QUIET) find_package(PCL REQUIRED) find_package(Eigen3 REQUIRED) find_package(Sophus REQUIRED) find_package(yaml-cpp REQUIRED) +find_package(OpenCV REQUIRED) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") -set(SRC_LIST src/map_builder/commons.cpp - src/map_builder/ieskf.cpp - src/map_builder/imu_processor.cpp + +include_directories( + ${EIGEN3_INCLUDE_DIRS} + ${Sophus_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + + +set(SRC_LIST src/map_builder/ieskf.cpp src/map_builder/ikd_Tree.cpp + src/map_builder/commons.cpp + src/utils.cpp + src/map_builder/pinhole_camera.cpp + src/map_builder/imu_processor.cpp src/map_builder/lidar_processor.cpp + src/map_builder/image_processor.cpp src/map_builder/map_builder.cpp - src/utils.cpp) + ) add_executable(lio_node src/lio_node.cpp ${SRC_LIST}) -ament_target_dependencies(lio_node rclcpp std_msgs tf2 tf2_ros nav_msgs sensor_msgs livox_ros_driver2 pcl_conversions geometry_msgs) +ament_target_dependencies(lio_node rclcpp std_msgs tf2 tf2_ros nav_msgs sensor_msgs livox_ros_driver2 pcl_conversions geometry_msgs cv_bridge) target_link_libraries(lio_node yaml-cpp ${PCL_LIBRARIES} + ${OpenCV_LIBRARIES} ) diff --git a/fastlio2/config/lio.yaml b/fastlio2/config/lio.yaml index 1648f05..4ae9323 100644 --- a/fastlio2/config/lio.yaml +++ b/fastlio2/config/lio.yaml @@ -4,7 +4,7 @@ body_frame: body world_frame: lidar print_time_cost: false -lidar_filter_num: 6 +lidar_filter_num: 3 lidar_min_range: 0.5 lidar_max_range: 30.0 scan_resolution: 0.15 @@ -26,7 +26,24 @@ ieskf_max_iter: 5 gravity_align: true esti_il: false -r_il: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] -t_il: [-0.011, -0.02329, 0.04412] +ext_il: [-0.011, -0.02329, 0.04412, 0.0, 0.0, 0.0, 1.0] # x y z qx qy qz qw +ext_lc: + [ + 0.055301417087128005, + -0.023940681099107203, + -0.036510243108364156, + 0.5040772962309031, + -0.5018066026651014, + 0.49854633904149553, + -0.4955277598425481, + ] # x y z qx qy qz qw + +cam_width: 1280 +cam_height: 720 +cam_fx: 641.976318359375 +cam_fy: 641.0658569335938 +cam_cx: 641.5721435546875 +cam_cy: 368.21832275390625 +cam_d: [-0.05588280409574509, 0.06619580090045929, 0.0002959131670650095, 0.0008284428040497005, -0.021595485508441925] lidar_cov_inv: 1000.0 diff --git a/fastlio2/package.xml b/fastlio2/package.xml index 4bd70f5..8e70b70 100644 --- a/fastlio2/package.xml +++ b/fastlio2/package.xml @@ -18,6 +18,7 @@ pcl_conversions livox_ros_driver2 geometry_msgs + cv_bridge ament_lint_auto diff --git a/fastlio2/rviz/fastlio2.rviz b/fastlio2/rviz/fastlio2.rviz index 0b3e4a9..485dbc4 100644 --- a/fastlio2/rviz/fastlio2.rviz +++ b/fastlio2/rviz/fastlio2.rviz @@ -6,13 +6,8 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /PointCloud21 - - /PointCloud22 - - /Axes1 - - /Axes2 - - /Path1 Splitter Ratio: 0.5 - Tree Height: 1079 + Tree Height: 667 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -30,7 +25,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: PointCloud2 Visualization Manager: Class: "" Displays: @@ -67,13 +62,13 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 237 + Max Intensity: 157 Min Color: 0; 0; 0 Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Selectable: true - Size (Pixels): 2 + Size (Pixels): 1 Size (m): 0.009999999776482582 Style: Points Topic: @@ -98,7 +93,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 200 - Enabled: true + Enabled: false Invert Rainbow: true Max Color: 255; 255; 255 Max Intensity: 255 @@ -119,7 +114,7 @@ Visualization Manager: Value: /fastlio2/world_cloud Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Class: rviz_default_plugins/Axes Enabled: true Length: 1 @@ -162,6 +157,54 @@ Visualization Manager: Reliability Policy: Reliable Value: /fastlio2/lio_path Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 500 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/color_world_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/camera/color/image_raw + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -208,25 +251,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 7.265907287597656 + Distance: 24.62679672241211 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 2.841027021408081 + Y: -3.6961326599121094 + Z: 4.53550910949707 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.8947967290878296 + Pitch: 0.6097959280014038 Target Frame: - Value: Orbit (rviz) - Yaw: 4.220390319824219 + Value: Orbit (rviz_default_plugins) + Yaw: 4.207400798797607 Saved: ~ Window Geometry: Displays: @@ -234,7 +277,9 @@ Window Geometry: Height: 1376 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000749000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000264000004c2fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000326000000c900fffffffb0000000a0049006d0061006700650100000369000001960000002800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002e50000014b0000000000000000000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000009ba000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000063b000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/fastlio2/src/lio_node.cpp b/fastlio2/src/lio_node.cpp index fdc8f20..84396bf 100644 --- a/fastlio2/src/lio_node.cpp +++ b/fastlio2/src/lio_node.cpp @@ -7,6 +7,7 @@ // #include #include #include +#include #include #include "utils.h" @@ -20,12 +21,14 @@ #include #include #include +#include using namespace std::chrono_literals; struct NodeConfig { std::string imu_topic = "/livox/imu"; std::string lidar_topic = "/livox/lidar"; + std::string image_topic = "/camera/camera/color/image_raw"; std::string body_frame = "body"; std::string world_frame = "lidar"; bool print_time_cost = false; @@ -35,10 +38,13 @@ struct StateData bool lidar_pushed = false; std::mutex imu_mutex; std::mutex lidar_mutex; + std::mutex image_mutex; double last_lidar_time = -1.0; double last_imu_time = -1.0; + double last_image_time = -1.0; std::deque imu_buffer; std::deque::Ptr>> lidar_buffer; + std::deque> image_buffer; nav_msgs::msg::Path path; }; @@ -52,9 +58,11 @@ class LIONode : public rclcpp::Node m_imu_sub = this->create_subscription(m_node_config.imu_topic, 10, std::bind(&LIONode::imuCB, this, std::placeholders::_1)); m_lidar_sub = this->create_subscription(m_node_config.lidar_topic, 10, std::bind(&LIONode::lidarCB, this, std::placeholders::_1)); + m_image_sub = this->create_subscription(m_node_config.image_topic, 10, std::bind(&LIONode::imageCB, this, std::placeholders::_1)); m_body_cloud_pub = this->create_publisher("body_cloud", 10000); m_world_cloud_pub = this->create_publisher("world_cloud", 10000); + m_color_world_cloud_pub = this->create_publisher("color_world_cloud", 10000); m_path_pub = this->create_publisher("lio_path", 10000); m_odom_pub = this->create_publisher("lio_odom", 10000); m_tf_broadcaster = std::make_shared(*this); @@ -106,10 +114,26 @@ class LIONode : public rclcpp::Node m_builder_config.ieskf_max_iter = config["ieskf_max_iter"].as(); m_builder_config.gravity_align = config["gravity_align"].as(); m_builder_config.esti_il = config["esti_il"].as(); - std::vector t_il_vec = config["t_il"].as>(); - std::vector r_il_vec = config["r_il"].as>(); - m_builder_config.t_il << t_il_vec[0], t_il_vec[1], t_il_vec[2]; - m_builder_config.r_il << r_il_vec[0], r_il_vec[1], r_il_vec[2], r_il_vec[3], r_il_vec[4], r_il_vec[5], r_il_vec[6], r_il_vec[7], r_il_vec[8]; + std::vector ext_il_vec = config["ext_il"].as>(); + std::vector ext_lc_vec = config["ext_lc"].as>(); + V3D t_il(ext_il_vec[0], ext_il_vec[1], ext_il_vec[2]); + Eigen::Quaterniond q_il(ext_il_vec[6], ext_il_vec[3], ext_il_vec[4], ext_il_vec[5]); + m_builder_config.r_il = q_il.toRotationMatrix(); + m_builder_config.t_il = t_il; + + V3D t_lc(ext_lc_vec[0], ext_lc_vec[1], ext_lc_vec[2]); + Eigen::Quaterniond q_lc(ext_lc_vec[6], ext_lc_vec[3], ext_lc_vec[4], ext_lc_vec[5]); + m_builder_config.r_cl = q_lc.toRotationMatrix().transpose(); + m_builder_config.t_cl = -m_builder_config.r_cl * t_lc; + + m_builder_config.cam_width = config["cam_width"].as(); + m_builder_config.cam_height = config["cam_height"].as(); + m_builder_config.cam_fx = config["cam_fx"].as(); + m_builder_config.cam_fy = config["cam_fy"].as(); + m_builder_config.cam_cx = config["cam_cx"].as(); + m_builder_config.cam_cy = config["cam_cy"].as(); + m_builder_config.cam_d = Vec(config["cam_d"].as>()); + m_builder_config.lidar_cov_inv = config["lidar_cov_inv"].as(); } @@ -140,10 +164,23 @@ class LIONode : public rclcpp::Node m_state_data.lidar_buffer.emplace_back(timestamp, cloud); m_state_data.last_lidar_time = timestamp; } + void imageCB(const sensor_msgs::msg::Image::SharedPtr msg) + { + std::lock_guard lock(m_state_data.image_mutex); + double timestamp = Utils::getSec(msg->header); + if (timestamp < m_state_data.last_image_time) + { + RCLCPP_WARN(this->get_logger(), "Image Message is out of order"); + std::deque>().swap(m_state_data.image_buffer); + } + cv::Mat image_mat = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8)->image; + m_state_data.image_buffer.emplace_back(timestamp, image_mat); + m_state_data.last_image_time = timestamp; + } bool syncPackage() { - if (m_state_data.imu_buffer.empty() || m_state_data.lidar_buffer.empty()) + if (m_state_data.lidar_buffer.empty()) return false; if (!m_state_data.lidar_pushed) { @@ -151,20 +188,64 @@ class LIONode : public rclcpp::Node std::sort(m_package.cloud->points.begin(), m_package.cloud->points.end(), [](PointType &p1, PointType &p2) { return p1.curvature < p2.curvature; }); m_package.cloud_start_time = m_state_data.lidar_buffer.front().first; - m_package.cloud_end_time = m_package.cloud_start_time + m_package.cloud->points.back().curvature / 1000.0; + m_package.cloud_end_time = m_package.cloud_start_time + m_package.cloud->points.back().curvature / double(1000.0); + m_package.lidar_end = false; m_state_data.lidar_pushed = true; } - if (m_state_data.last_imu_time < m_package.cloud_end_time) + + if (m_state_data.image_buffer.empty() || m_state_data.image_buffer.front().first > m_package.cloud_end_time) + { + if (m_state_data.last_imu_time < m_package.cloud_end_time) + return false; + double imu_time = m_state_data.imu_buffer.front().time; + m_package.imus.clear(); + { + std::lock_guard lock(m_state_data.imu_mutex); + while ((!m_state_data.imu_buffer.empty() && (imu_time < m_package.cloud_end_time))) + { + m_package.imus.push_back(m_state_data.imu_buffer.front()); + m_state_data.imu_buffer.pop_front(); + imu_time = m_state_data.imu_buffer.front().time; + } + } + { + std::lock_guard lock(m_state_data.lidar_mutex); + m_state_data.lidar_buffer.pop_front(); + } + m_package.lidar_end = true; + m_state_data.lidar_pushed = false; + return true; + } + double image_start_time = m_state_data.image_buffer.front().first; + if (image_start_time < m_package.cloud_start_time) + { + std::lock_guard lock(m_state_data.image_mutex); + m_state_data.image_buffer.pop_front(); + return false; + } + + if (m_state_data.last_imu_time < image_start_time) return false; - Vec().swap(m_package.imus); - while (!m_state_data.imu_buffer.empty() && m_state_data.imu_buffer.front().time < m_package.cloud_end_time) + double imu_time = m_state_data.imu_buffer.front().time; + m_package.imus.clear(); + + m_package.image_time = image_start_time; + m_package.image = m_state_data.image_buffer.front().second; + { + std::lock_guard lock(m_state_data.imu_mutex); + while ((!m_state_data.imu_buffer.empty() && (imu_time < image_start_time))) + { + m_package.imus.push_back(m_state_data.imu_buffer.front()); + m_state_data.imu_buffer.pop_front(); + imu_time = m_state_data.imu_buffer.front().time; + } + } { - m_package.imus.emplace_back(m_state_data.imu_buffer.front()); - m_state_data.imu_buffer.pop_front(); + std::lock_guard lock(m_state_data.image_mutex); + m_state_data.image_buffer.pop_front(); } - m_state_data.lidar_buffer.pop_front(); - m_state_data.lidar_pushed = false; + m_package.lidar_end = false; return true; } @@ -179,6 +260,19 @@ class LIONode : public rclcpp::Node pub->publish(cloud_msg); } + void publishCloud(rclcpp::Publisher::SharedPtr pub, pcl::PointCloud::Ptr cloud, std::string frame_id, const double &time) + { + if (pub->get_subscription_count() <= 0) + return; + if (cloud == nullptr || cloud->empty()) + return; + sensor_msgs::msg::PointCloud2 cloud_msg; + pcl::toROSMsg(*cloud, cloud_msg); + cloud_msg.header.frame_id = frame_id; + cloud_msg.header.stamp = Utils::getTime(time); + pub->publish(cloud_msg); + } + void publishOdometry(rclcpp::Publisher::SharedPtr odom_pub, std::string frame_id, std::string child_frame, const double &time) { if (odom_pub->get_subscription_count() <= 0) @@ -244,6 +338,7 @@ class LIONode : public rclcpp::Node { if (!syncPackage()) return; + auto t1 = std::chrono::high_resolution_clock::now(); m_builder->process(m_package); auto t2 = std::chrono::high_resolution_clock::now(); @@ -257,27 +352,55 @@ class LIONode : public rclcpp::Node if (m_builder->status() != BuilderStatus::MAPPING) return; - broadCastTF(m_tf_broadcaster, m_node_config.world_frame, m_node_config.body_frame, m_package.cloud_end_time); + broadCastTF(m_tf_broadcaster, m_node_config.world_frame, m_node_config.body_frame, m_package.lidar_end ? m_package.cloud_end_time : m_package.image_time); + + publishOdometry(m_odom_pub, m_node_config.world_frame, m_node_config.body_frame, m_package.lidar_end ? m_package.cloud_end_time : m_package.image_time); - publishOdometry(m_odom_pub, m_node_config.world_frame, m_node_config.body_frame, m_package.cloud_end_time); + if (m_package.lidar_end) + { + CloudType::Ptr body_cloud = m_builder->lidar_processor()->transformCloud(m_package.cloud, m_kf->x().r_il, m_kf->x().t_il); - CloudType::Ptr body_cloud = m_builder->lidar_processor()->transformCloud(m_package.cloud, m_kf->x().r_il, m_kf->x().t_il); + publishCloud(m_body_cloud_pub, body_cloud, m_node_config.body_frame, m_package.cloud_end_time); - publishCloud(m_body_cloud_pub, body_cloud, m_node_config.body_frame, m_package.cloud_end_time); + CloudType::Ptr world_cloud = m_builder->lidar_processor()->transformCloud(m_package.cloud, m_builder->lidar_processor()->r_wl(), m_builder->lidar_processor()->t_wl()); - CloudType::Ptr world_cloud = m_builder->lidar_processor()->transformCloud(m_package.cloud, m_builder->lidar_processor()->r_wl(), m_builder->lidar_processor()->t_wl()); + publishCloud(m_world_cloud_pub, world_cloud, m_node_config.world_frame, m_package.cloud_end_time); - publishCloud(m_world_cloud_pub, world_cloud, m_node_config.world_frame, m_package.cloud_end_time); + publishPath(m_path_pub, m_node_config.world_frame, m_package.cloud_end_time); + } + else + { + if (m_builder->colorRendered()) + { + publishCloud(m_color_world_cloud_pub, m_builder->coloredCloud(), m_node_config.world_frame, m_package.image_time); + m_builder->colorRendered() = false; + } + } - publishPath(m_path_pub, m_node_config.world_frame, m_package.cloud_end_time); + // if (m_package.lidar_end) + // { + // RCLCPP_INFO(this->get_logger(), "lidar begin: %.8f | lidar end: %.8f ;", m_package.cloud_start_time, m_package.cloud_end_time); + // } + // else + // { + // RCLCPP_INFO(this->get_logger(), "lidar begin: %.8f | image end: %.8f ;", m_package.cloud_start_time, m_package.image_time); + // } + + // if (!m_package.imus.empty()) + // { + // RCLCPP_INFO(this->get_logger(), "imu begin: %.8f | imu end: %.8f ;", m_package.imus.front().time, m_package.imus.back().time); + // } + // RCLCPP_WARN(this->get_logger(), "===================================================="); } private: rclcpp::Subscription::SharedPtr m_lidar_sub; rclcpp::Subscription::SharedPtr m_imu_sub; + rclcpp::Subscription::SharedPtr m_image_sub; rclcpp::Publisher::SharedPtr m_body_cloud_pub; rclcpp::Publisher::SharedPtr m_world_cloud_pub; + rclcpp::Publisher::SharedPtr m_color_world_cloud_pub; rclcpp::Publisher::SharedPtr m_path_pub; rclcpp::Publisher::SharedPtr m_odom_pub; diff --git a/fastlio2/src/map_builder/commons.h b/fastlio2/src/map_builder/commons.h index 419f2d5..006492e 100644 --- a/fastlio2/src/map_builder/commons.h +++ b/fastlio2/src/map_builder/commons.h @@ -2,6 +2,7 @@ #include #include #include +#include using PointType = pcl::PointXYZINormal; using CloudType = pcl::PointCloud; @@ -18,11 +19,9 @@ using V2F = Eigen::Vector2f; using M4D = Eigen::Matrix4d; using V4D = Eigen::Vector4d; - template using Vec = std::vector; - bool esti_plane(PointVec &points, const double &thresh, V4D &out); float sq_dist(const PointType &p1, const PointType &p2); @@ -50,6 +49,16 @@ struct Config bool esti_il = false; M3D r_il = M3D::Identity(); V3D t_il = V3D::Zero(); + M3D r_cl = M3D::Identity(); + V3D t_cl = V3D::Zero(); + + double cam_width = 1280; + double cam_height = 720; + double cam_fx = 641.976318359375; + double cam_fy = 641.0658569335938; + double cam_cx = 641.5721435546875; + double cam_cy = 368.21832275390625; + Vec cam_d{-0.05588280409574509, 0.06619580090045929, 0.0002959131670650095, 0.0008284428040497005, -0.021595485508441925}; double lidar_cov_inv = 1000.0; }; @@ -83,4 +92,7 @@ struct SyncPackage CloudType::Ptr cloud; double cloud_start_time = 0.0; double cloud_end_time = 0.0; + double image_time = 0.0; + bool lidar_end; + cv::Mat image; }; \ No newline at end of file diff --git a/fastlio2/src/map_builder/image_processor.cpp b/fastlio2/src/map_builder/image_processor.cpp new file mode 100644 index 0000000..ee4db69 --- /dev/null +++ b/fastlio2/src/map_builder/image_processor.cpp @@ -0,0 +1,91 @@ +#include "image_processor.h" + +ImageProcessor::ImageProcessor(Config &config, std::shared_ptr kf) + : m_kf(kf), m_config(config) +{ + m_frame_count = 0; + m_camera = std::make_shared(m_config.cam_width, m_config.cam_height, + m_config.cam_fx, m_config.cam_fy, + m_config.cam_cx, m_config.cam_cy, + m_config.cam_d[0], m_config.cam_d[1], + m_config.cam_d[2], m_config.cam_d[3], + m_config.cam_d[4]); +} + +void ImageProcessor::process(cv::Mat &img, CloudType::Ptr cloud, bool is_new_cloud) +{ + m_frame_count++; + if (img.cols != m_camera->width() || img.rows != m_camera->height()) + cv::resize(img, m_cur_img_color, cv::Size2i(m_camera->width(), m_camera->height())); + else + m_cur_img_color = img; + cv::cvtColor(m_cur_img_color, m_cur_img_gray, cv::COLOR_BGR2GRAY); + m_cur_cloud = cloud; +} + +M3D ImageProcessor::r_cw() +{ + const State &s = m_kf->x(); + // return s.r_cl * s.r_il.transpose() * s.r_wi.transpose(); + return m_config.r_cl * s.r_il.transpose() * s.r_wi.transpose(); +} + +V3D ImageProcessor::t_cw() +{ + const State &s = m_kf->x(); + // return -s.r_cl * s.r_il.transpose() * (s.r_wi.transpose() * s.t_wi + s.t_il) + s.t_cl; + return -m_config.r_cl * s.r_il.transpose() * (s.r_wi.transpose() * s.t_wi + s.t_il) + m_config.t_cl; +} + +M3D ImageProcessor::r_ci() +{ + const State &s = m_kf->x(); + // return s.r_cl * s.r_il.transpose(); + return m_config.r_cl * s.r_il.transpose(); +} + +V3D ImageProcessor::t_ci() +{ + const State &s = m_kf->x(); + // return -s.r_cl * s.r_il.transpose() * s.t_il + s.t_cl; + return -m_config.r_cl * s.r_il.transpose() * s.t_il + m_config.t_cl; +} + +M3D ImageProcessor::r_wc() +{ + return r_cw().transpose(); +} + +V3D ImageProcessor::t_wc() +{ + return -r_cw().transpose() * t_cw(); +} + +pcl::PointCloud::Ptr ImageProcessor::getLastestColoredCloud() +{ + pcl::PointCloud::Ptr ret(new pcl::PointCloud); + if (m_cur_cloud == nullptr || m_cur_cloud->size() <= 0) + return ret; + ret->reserve(m_cur_cloud->size()); + for (auto p : m_cur_cloud->points) + { + V3D pw = V3D(p.x, p.y, p.z); + V3D pc = r_cw() * pw + t_cw(); + + if (pc(2) <= 0) + continue; + V2D px = m_camera->cam2Img(pc); + if (!m_camera->isInImg(px.cast(), 1)) + continue; + Eigen::Vector3f bgr = CVUtils::interpolateMat_color(m_cur_img_color, px(0), px(1)); + pcl::PointXYZRGB p_color; + p_color.x = p.x; + p_color.y = p.y; + p_color.z = p.z; + p_color.b = bgr(0); + p_color.g = bgr(1); + p_color.r = bgr(2); + ret->push_back(p_color); + } + return ret; +} \ No newline at end of file diff --git a/fastlio2/src/map_builder/image_processor.h b/fastlio2/src/map_builder/image_processor.h new file mode 100644 index 0000000..28c7070 --- /dev/null +++ b/fastlio2/src/map_builder/image_processor.h @@ -0,0 +1,31 @@ +#pragma once +#include "commons.h" +#include "ieskf.h" +#include "pinhole_camera.h" + +class ImageProcessor +{ +public: + ImageProcessor(Config &config, std::shared_ptr kf); + + void process(cv::Mat &img, CloudType::Ptr cloud, bool is_new_cloud); + + pcl::PointCloud::Ptr getLastestColoredCloud(); + + + M3D r_cw(); + V3D t_cw(); + M3D r_ci(); + V3D t_ci(); + M3D r_wc(); + V3D t_wc(); + +private: + Config m_config; + std::shared_ptr m_kf; + std::shared_ptr m_camera; + cv::Mat m_cur_img_color; + cv::Mat m_cur_img_gray; + CloudType::Ptr m_cur_cloud; + u_int64_t m_frame_count; +}; \ No newline at end of file diff --git a/fastlio2/src/map_builder/imu_processor.cpp b/fastlio2/src/map_builder/imu_processor.cpp index d5bc04a..9cc11c9 100644 --- a/fastlio2/src/map_builder/imu_processor.cpp +++ b/fastlio2/src/map_builder/imu_processor.cpp @@ -11,6 +11,7 @@ IMUProcessor::IMUProcessor(Config &config, std::shared_ptr kf) : m_config m_last_gyro.setZero(); m_imu_cache.clear(); m_poses_cache.clear(); + m_pushed = true; } bool IMUProcessor::initialize(SyncPackage &package) @@ -59,10 +60,14 @@ void IMUProcessor::undistort(SyncPackage &package) const double imu_time_end = m_imu_cache.back().time; const double cloud_time_begin = package.cloud_start_time; - const double propagate_time_end = package.cloud_end_time; + const double propagate_time_end = package.lidar_end ? package.cloud_end_time : package.image_time; - m_poses_cache.clear(); - m_poses_cache.emplace_back(0.0, m_last_acc, m_last_gyro, m_kf->x().v, m_kf->x().t_wi, m_kf->x().r_wi); + if (m_pushed) + { + m_poses_cache.clear(); + m_poses_cache.emplace_back(0.0, m_last_acc, m_last_gyro, m_kf->x().v, m_kf->x().t_wi, m_kf->x().r_wi); + m_pushed = false; + } V3D acc_val, gyro_val; double dt = 0.0; @@ -98,35 +103,39 @@ void IMUProcessor::undistort(SyncPackage &package) m_last_imu = m_imu_cache.back(); m_last_propagate_end_time = propagate_time_end; - M3D cur_r_wi = m_kf->x().r_wi; - V3D cur_t_wi = m_kf->x().t_wi; - M3D cur_r_il = m_kf->x().r_il; - V3D cur_t_il = m_kf->x().t_il; - auto it_pcl = package.cloud->points.end() - 1; - - for (auto it_kp = m_poses_cache.end() - 1; it_kp != m_poses_cache.begin(); it_kp--) + if (package.lidar_end) { - auto head = it_kp - 1; - auto tail = it_kp; - - M3D imu_r_wi = head->rot; - V3D imu_t_wi = head->trans; - V3D imu_vel = head->vel; - V3D imu_acc = tail->acc; - V3D imu_gyro = tail->gyro; + M3D cur_r_wi = m_kf->x().r_wi; + V3D cur_t_wi = m_kf->x().t_wi; + M3D cur_r_il = m_kf->x().r_il; + V3D cur_t_il = m_kf->x().t_il; + auto it_pcl = package.cloud->points.end() - 1; - for (; it_pcl->curvature / double(1000) > head->offset; it_pcl--) + for (auto it_kp = m_poses_cache.end() - 1; it_kp != m_poses_cache.begin(); it_kp--) { - dt = it_pcl->curvature / double(1000) - head->offset; - V3D point(it_pcl->x, it_pcl->y, it_pcl->z); - M3D point_rot = imu_r_wi * Sophus::SO3d::exp(imu_gyro * dt).matrix(); - V3D point_pos = imu_t_wi + imu_vel * dt + 0.5 * imu_acc * dt * dt; - V3D p_compensate = cur_r_il.transpose() * (cur_r_wi.transpose() * (point_rot * (cur_r_il * point + cur_t_il) + point_pos - cur_t_wi) - cur_t_il); - it_pcl->x = p_compensate(0); - it_pcl->y = p_compensate(1); - it_pcl->z = p_compensate(2); - if (it_pcl == package.cloud->points.begin()) - break; + auto head = it_kp - 1; + auto tail = it_kp; + + M3D imu_r_wi = head->rot; + V3D imu_t_wi = head->trans; + V3D imu_vel = head->vel; + V3D imu_acc = tail->acc; + V3D imu_gyro = tail->gyro; + + for (; it_pcl->curvature / double(1000) > head->offset; it_pcl--) + { + dt = it_pcl->curvature / double(1000) - head->offset; + V3D point(it_pcl->x, it_pcl->y, it_pcl->z); + M3D point_rot = imu_r_wi * Sophus::SO3d::exp(imu_gyro * dt).matrix(); + V3D point_pos = imu_t_wi + imu_vel * dt + 0.5 * imu_acc * dt * dt; + V3D p_compensate = cur_r_il.transpose() * (cur_r_wi.transpose() * (point_rot * (cur_r_il * point + cur_t_il) + point_pos - cur_t_wi) - cur_t_il); + it_pcl->x = p_compensate(0); + it_pcl->y = p_compensate(1); + it_pcl->z = p_compensate(2); + if (it_pcl == package.cloud->points.begin()) + break; + } } + m_pushed = true; } } \ No newline at end of file diff --git a/fastlio2/src/map_builder/imu_processor.h b/fastlio2/src/map_builder/imu_processor.h index 0751dfb..7640aaa 100644 --- a/fastlio2/src/map_builder/imu_processor.h +++ b/fastlio2/src/map_builder/imu_processor.h @@ -13,6 +13,7 @@ class IMUProcessor private: Config m_config; + bool m_pushed; std::shared_ptr m_kf; double m_last_propagate_end_time; Vec m_imu_cache; diff --git a/fastlio2/src/map_builder/map_builder.cpp b/fastlio2/src/map_builder/map_builder.cpp index 74a9329..0ad9a51 100644 --- a/fastlio2/src/map_builder/map_builder.cpp +++ b/fastlio2/src/map_builder/map_builder.cpp @@ -3,6 +3,10 @@ MapBuilder::MapBuilder(Config &config, std::shared_ptr kf) : m_config(con { m_imu_processor = std::make_shared(config, kf); m_lidar_processor = std::make_shared(config, kf); + m_image_processor = std::make_shared(config, kf); + m_lastest_cloud = nullptr; + m_is_new_cloud = false; + m_colored_points_rendered = false; m_status = BuilderStatus::IMU_INIT; } @@ -21,9 +25,29 @@ void MapBuilder::process(SyncPackage &package) { CloudType::Ptr cloud_world = LidarProcessor::transformCloud(package.cloud, m_lidar_processor->r_wl(), m_lidar_processor->t_wl()); m_lidar_processor->initCloudMap(cloud_world->points); + m_lastest_cloud = cloud_world; + m_is_new_cloud = true; m_status = BuilderStatus::MAPPING; return; } - - m_lidar_processor->process(package); + + if (package.lidar_end) + { + m_lidar_processor->process(package); + m_lastest_cloud = LidarProcessor::transformCloud(package.cloud, m_lidar_processor->r_wl(), m_lidar_processor->t_wl()); + m_is_new_cloud = true; + } + else + { + if (!m_lastest_cloud) + return; + m_image_processor->process(package.image, m_lastest_cloud, m_is_new_cloud); + + if (m_is_new_cloud) + { + m_lastest_colored_cloud = m_image_processor->getLastestColoredCloud(); + m_colored_points_rendered = true; + } + m_is_new_cloud = false; + } } \ No newline at end of file diff --git a/fastlio2/src/map_builder/map_builder.h b/fastlio2/src/map_builder/map_builder.h index baa0fba..97439bc 100644 --- a/fastlio2/src/map_builder/map_builder.h +++ b/fastlio2/src/map_builder/map_builder.h @@ -1,6 +1,7 @@ #pragma once #include "imu_processor.h" #include "lidar_processor.h" +#include "image_processor.h" enum BuilderStatus { @@ -14,9 +15,13 @@ class MapBuilder public: MapBuilder(Config &config, std::shared_ptr kf); + bool &colorRendered() { return m_colored_points_rendered; } + + pcl::PointCloud::Ptr coloredCloud() { return m_lastest_colored_cloud; } + void process(SyncPackage &package); - BuilderStatus status() { return m_status; } - std::shared_ptr lidar_processor(){return m_lidar_processor;} + BuilderStatus status() { return m_status; } + std::shared_ptr lidar_processor() { return m_lidar_processor; } private: Config m_config; @@ -24,4 +29,9 @@ class MapBuilder std::shared_ptr m_kf; std::shared_ptr m_imu_processor; std::shared_ptr m_lidar_processor; + std::shared_ptr m_image_processor; + CloudType::Ptr m_lastest_cloud; + pcl::PointCloud::Ptr m_lastest_colored_cloud; + bool m_colored_points_rendered; + bool m_is_new_cloud; }; diff --git a/fastlio2/src/map_builder/pinhole_camera.cpp b/fastlio2/src/map_builder/pinhole_camera.cpp new file mode 100644 index 0000000..0e401ee --- /dev/null +++ b/fastlio2/src/map_builder/pinhole_camera.cpp @@ -0,0 +1,241 @@ +#include "pinhole_camera.h" + +PinholeCamera::PinholeCamera(double width, double height, + double fx, double fy, + double cx, double cy, + double d0, double d1, double d2, double d3, double d4) + : m_width(width), m_height(height), + m_fx(fx), m_fy(fy), m_cx(cx), m_cy(cy), + m_distort(fabs(d0) > 0.0000001) +{ + m_d[0] = d0; + m_d[1] = d1; + m_d[2] = d2; + m_d[3] = d3; + m_d[4] = d4; + m_cvK = (cv::Mat_(3, 3) << m_fx, 0.0, m_cx, 0.0, m_fy, m_cy, 0.0, 0.0, 1.0); + m_cvD = (cv::Mat_(1, 5) << m_d[0], m_d[1], m_d[2], m_d[3], m_d[4]); +} + +V3D PinholeCamera::img2Cam(const double &u, const double &v) const +{ + V3D xyz; + if (!m_distort) + { + xyz[0] = (u - m_cx) / m_fx; + xyz[1] = (v - m_cy) / m_fy; + xyz[2] = 1.0; + } + else + { + cv::Point2f uv(u, v), px; + const cv::Mat src_pt(1, 1, CV_32FC2, &uv.x); + cv::Mat dst_pt(1, 1, CV_32FC2, &px.x); + cv::undistortPoints(src_pt, dst_pt, m_cvK, m_cvD); + xyz[0] = px.x; + xyz[1] = px.y; + xyz[2] = 1.0; + } + return xyz.normalized(); +} + +V3D PinholeCamera::img2Cam(const V2D &uv) const +{ + return img2Cam(uv(0), uv(1)); +} + +V2D PinholeCamera::cam2Img(const V2D &uv) const +{ + V2D px; + if (!m_distort) + { + px[0] = m_fx * uv[0] + m_cx; + px[1] = m_fy * uv[1] + m_cy; + } + else + { + double x, y, r2, r4, r6, a1, a2, a3, cdist, xd, yd; + x = uv[0]; + y = uv[1]; + r2 = x * x + y * y; + r4 = r2 * r2; + r6 = r4 * r2; + a1 = 2 * x * y; + a2 = r2 + 2 * x * x; + a3 = r2 + 2 * y * y; + cdist = 1 + m_d[0] * r2 + m_d[1] * r4 + m_d[4] * r6; + xd = x * cdist + m_d[2] * a1 + m_d[3] * a2; + yd = y * cdist + m_d[2] * a3 + m_d[3] * a1; + px[0] = xd * m_fx + m_cx; + px[1] = yd * m_fy + m_cy; + } + return px; +} + +V2D PinholeCamera::cam2Img(const V3D &xyz) const +{ + return cam2Img(project2d(xyz)); +} + +bool PinholeCamera::isInImg(const Eigen::Vector2i &obs, int boundary) const +{ + if (obs[0] >= boundary && obs[0] < width() - boundary && obs[1] >= boundary && obs[1] < height() - boundary) + return true; + return false; +} + +Eigen::Matrix PinholeCamera::dpi(V3D pc) +{ + Eigen::Matrix J; + const double x = pc[0]; + const double y = pc[1]; + const double z_inv = 1. / pc[2]; + const double z_inv_2 = z_inv * z_inv; + J(0, 0) = m_fx * z_inv; + J(0, 1) = 0.0; + J(0, 2) = -m_fx * x * z_inv_2; + J(1, 0) = 0.0; + J(1, 1) = m_fy * z_inv; + J(1, 2) = -m_fy * y * z_inv_2; + return J; +} + +float CVUtils::interpolateMat_8u(const cv::Mat &mat, float u, float v) +{ + assert(mat.type() == CV_8U); + int x = floor(u); + int y = floor(v); + float subpix_x = u - x; + float subpix_y = v - y; + + float wtl = (1.0f - subpix_x) * (1.0f - subpix_y); + float wtr = subpix_x * (1.0f - subpix_y); + float wbl = (1.0f - subpix_x) * subpix_y; + float wbr = subpix_x * subpix_y; + float ptl = static_cast(mat.ptr(y)[x]); + float ptr = static_cast(mat.ptr(y)[x + 1]); + float pbl = static_cast(mat.ptr(y + 1)[x]); + float pbr = static_cast(mat.ptr(y + 1)[x + 1]); + + return wtl * ptl + wtr * ptr + wbl * pbl + wbr * pbr; +} + +float CVUtils::shiTomasiScore(const cv::Mat &img, int u, int v) +{ + assert(img.type() == CV_8UC1); + + float dXX = 0.0; + float dYY = 0.0; + float dXY = 0.0; + const int halfbox_size = 4; + const int box_size = 2 * halfbox_size; + const int box_area = box_size * box_size; + const int x_min = u - halfbox_size; + const int x_max = u + halfbox_size; + const int y_min = v - halfbox_size; + const int y_max = v + halfbox_size; + + if (x_min < 1 || x_max >= img.cols - 1 || y_min < 1 || y_max >= img.rows - 1) + return 0.0; // patch is too close to the boundary + + const int stride = img.step.p[0]; + for (int y = y_min; y < y_max; ++y) + { + const uint8_t *ptr_left = img.data + stride * y + x_min - 1; + const uint8_t *ptr_right = img.data + stride * y + x_min + 1; + const uint8_t *ptr_top = img.data + stride * (y - 1) + x_min; + const uint8_t *ptr_bottom = img.data + stride * (y + 1) + x_min; + for (int x = 0; x < box_size; ++x, ++ptr_left, ++ptr_right, ++ptr_top, ++ptr_bottom) + { + float dx = *ptr_right - *ptr_left; + float dy = *ptr_bottom - *ptr_top; + dXX += dx * dx; + dYY += dy * dy; + dXY += dx * dy; + } + } + + // Find and return smaller eigenvalue: + dXX = dXX / (2.0 * box_area); + dYY = dYY / (2.0 * box_area); + dXY = dXY / (2.0 * box_area); + return 0.5 * (dXX + dYY - sqrt((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY))); +} + +Eigen::Vector3f CVUtils::interpolateMat_color(const cv::Mat &mat, float u, float v) +{ + assert(mat.type() == CV_8UC3); + + int x = floor(u); + int y = floor(v); + float subpix_x = u - x; + float subpix_y = v - y; + float wtl = (1.0f - subpix_x) * (1.0f - subpix_y); + float wtr = subpix_x * (1.0f - subpix_y); + float wbl = (1.0f - subpix_x) * subpix_y; + float wbr = subpix_x * subpix_y; + + cv::Vec3b ptl = mat.ptr(y)[x]; + cv::Vec3b ptr = mat.ptr(y)[x + 1]; + cv::Vec3b pbl = mat.ptr(y + 1)[x]; + cv::Vec3b pbr = mat.ptr(y + 1)[x + 1]; + + Eigen::Vector3f vtl(ptl[0], ptl[1], ptl[2]); + Eigen::Vector3f vtr(ptr[0], ptr[1], ptr[2]); + Eigen::Vector3f vbl(pbl[0], pbl[1], pbl[2]); + Eigen::Vector3f vbr(pbr[0], pbr[1], pbr[2]); + + return wtl * vtl + wtr * vtr + wbl * vbl + wbr * vbr; +} + +bool CVUtils::getPatch(cv::Mat img, const V2D px, cv::Mat &patch, int half_patch, int level) +{ + + int height = img.rows; + int width = img.cols; + assert(img.type() == CV_8U); + assert(patch.type() == CV_32F); + assert(patch.cols == 2 * half_patch + 1 && patch.rows == 2 * half_patch + 1); + const float u_ref = px[0]; + const float v_ref = px[1]; + const int scale = (1 << level); + const int u_ref_i = floorf(px[0] / scale) * scale; + const int v_ref_i = floorf(px[1] / scale) * scale; + const float subpix_u_ref = (u_ref - u_ref_i) / scale; + const float subpix_v_ref = (v_ref - v_ref_i) / scale; + + const float w_tl = (1.0 - subpix_u_ref) * (1.0 - subpix_v_ref); + const float w_tr = subpix_u_ref * (1.0 - subpix_v_ref); + const float w_bl = (1.0 - subpix_u_ref) * subpix_v_ref; + const float w_br = subpix_u_ref * subpix_v_ref; + if (u_ref_i - half_patch < 0 || u_ref_i + half_patch >= width || v_ref_i - half_patch < 0 || v_ref_i + half_patch >= height) + return false; + + for (int y = 0; y <= half_patch * 2; y++) + { + for (int x = 0; x <= half_patch * 2; x++) + { + V2D px_patch(x - half_patch, y - half_patch); + px_patch *= (1 << level); + int tl_x = u_ref_i + px_patch(0), tl_y = v_ref_i + px_patch(1); + uint8_t tl = img.ptr(tl_y)[tl_x]; + uint8_t tr = img.ptr(tl_y)[tl_x + 1]; + uint8_t bl = img.ptr(tl_y + 1)[tl_x]; + uint8_t br = img.ptr(tl_y + 1)[tl_x + 1]; + patch.ptr(y)[x] = w_tl * tl + w_tr * tr + w_bl * bl + w_br * br; + } + } + return true; +} + +float CVUtils::weightPixel(cv::Mat img, Eigen::Vector4f &weight, Eigen::Vector2i &tl_xy) +{ + assert(img.type() == CV_8U); + int tl_x = tl_xy(0), tl_y = tl_xy(1); + float w_tl = weight(0), w_tr = weight(1), w_bl = weight(2), w_br = weight(3); + float tl = static_cast(img.ptr(tl_y)[tl_x]); + float tr = static_cast(img.ptr(tl_y)[tl_x + 1]); + float bl = static_cast(img.ptr(tl_y + 1)[tl_x]); + float br = static_cast(img.ptr(tl_y + 1)[tl_x + 1]); + return w_tl * tl + w_tr * tr + w_bl * bl + w_br * br; +} \ No newline at end of file diff --git a/fastlio2/src/map_builder/pinhole_camera.h b/fastlio2/src/map_builder/pinhole_camera.h new file mode 100644 index 0000000..692bf85 --- /dev/null +++ b/fastlio2/src/map_builder/pinhole_camera.h @@ -0,0 +1,63 @@ +#pragma once +#include "commons.h" + +class CVUtils +{ +public: + static float interpolateMat_8u(const cv::Mat &mat, float u, float v); + + static float shiTomasiScore(const cv::Mat &img, int u, int v); + + static Eigen::Vector3f interpolateMat_color(const cv::Mat &mat, float u, float v); + + static bool getPatch(cv::Mat img, const V2D px, cv::Mat &patch, int half_patch, int level = 0); + + static float weightPixel(cv::Mat img, Eigen::Vector4f &weight, Eigen::Vector2i &tl_xy); +}; + +class PinholeCamera +{ +public: + PinholeCamera(double width, double height, + double fx, double fy, + double cx, double cy, + double d0, double d1, double d2, double d3, double d4); + + V3D img2Cam(const double &u, const double &v) const; + + V3D img2Cam(const V2D &uv) const; + + V2D cam2Img(const V2D &uv) const; + + V2D cam2Img(const V3D &xyz) const; + + inline V2D project2d(const V3D &v) const { return v.head<2>() / v[2]; } + + inline int width() const { return static_cast(m_width); } + + inline int height() const { return static_cast(m_height); } + + inline double fx() const { return m_fx; } + + inline double fy() const { return m_fy; } + + inline double cx() const { return m_cx; } + + inline double cy() const { return m_cy; } + + bool isInImg(const Eigen::Vector2i &obs, int boundary = 0) const; + + Eigen::Matrix dpi(V3D pc); + +private: + double m_width; + double m_height; + double m_fx; + double m_fy; + double m_cx; + double m_cy; + double m_d[5]; + bool m_distort; + cv::Mat m_cvK; + cv::Mat m_cvD; +}; \ No newline at end of file