diff --git a/config/config_preprocess.json b/config/config_preprocess.json index 08612260..c89fb328 100644 --- a/config/config_preprocess.json +++ b/config/config_preprocess.json @@ -10,6 +10,7 @@ // outlier_removal_k : Number of neighbors for outlier removal // outlier_std_mul_factor : Standard deviation multiplier for outlier removal // k_correspondences : Number of neighbors for covariance estimation + // plane_eps : Epsilon for covariance regularization (plane thickness) // num_threads : Number of threads for preprocessing */ "preprocess": { @@ -23,6 +24,7 @@ "outlier_removal_k": 10, "outlier_std_mul_factor": 1.0, "k_correspondences": 10, + "plane_eps": 1e-3, "num_threads": 2 } } \ No newline at end of file diff --git a/config/config_viewer.json b/config/config_viewer.json index b93e94d7..76cbe2fd 100644 --- a/config/config_viewer.json +++ b/config/config_viewer.json @@ -13,7 +13,12 @@ 4.0 ], "enable_partial_rendering": false, - "partial_rendering_budget": 1024 + "partial_rendering_budget": 1024, + "point_shape_circle": true, + "point_size_metric": false, + "point_size": 10.0, + "points_alpha": 0.5, + "factors_alpha": 0.5 }, "interactive_viewer": { "viewer_width": 2560, @@ -23,6 +28,11 @@ 4.0 ], "enable_partial_rendering": false, - "partial_rendering_budget": 1024 + "partial_rendering_budget": 1024, + "point_shape_circle": true, + "point_size_metric": false, + "point_size": 10.0, + "points_alpha": 0.5, + "factors_alpha": 0.5 } } \ No newline at end of file diff --git a/include/glim/common/cloud_covariance_estimation.hpp b/include/glim/common/cloud_covariance_estimation.hpp index 19ac3599..96d88464 100644 --- a/include/glim/common/cloud_covariance_estimation.hpp +++ b/include/glim/common/cloud_covariance_estimation.hpp @@ -15,7 +15,7 @@ enum class RegularizationMethod { NONE, PLANE, NORMALIZED_MIN_EIG, FROBENIUS }; */ class CloudCovarianceEstimation { public: - CloudCovarianceEstimation(const int num_threads = 1); + CloudCovarianceEstimation(const int num_threads = 1, const double plane_eps = 1e-3); ~CloudCovarianceEstimation(); /** @@ -60,6 +60,7 @@ class CloudCovarianceEstimation { private: const RegularizationMethod regularization_method; const int num_threads; + const double plane_eps; }; } // namespace glim diff --git a/include/glim/odometry/odometry_estimation_gpu.hpp b/include/glim/odometry/odometry_estimation_gpu.hpp index 2d631619..05192bfd 100644 --- a/include/glim/odometry/odometry_estimation_gpu.hpp +++ b/include/glim/odometry/odometry_estimation_gpu.hpp @@ -20,7 +20,7 @@ struct OdometryEstimationGPUParams : public OdometryEstimationIMUParams { OdometryEstimationGPUParams(); virtual ~OdometryEstimationGPUParams(); - enum class KeyframeUpdateStrategy { OVERLAP, DISPLACEMENT, ENTROPY }; + enum class KeyframeUpdateStrategy { OVERLAP, OVERLAP2, DISPLACEMENT, ENTROPY }; public: // Registration params @@ -57,6 +57,7 @@ class OdometryEstimationGPU : public OdometryEstimationIMU { virtual void update_frames(const int current, const gtsam::NonlinearFactorGraph& new_factors) override; void update_keyframes_overlap(int current); + void update_keyframes_overlap2(int current); void update_keyframes_displacement(int current); void update_keyframes_entropy(const gtsam::NonlinearFactorGraph& matching_cost_factors, int current); diff --git a/include/glim/odometry/odometry_estimation_imu.hpp b/include/glim/odometry/odometry_estimation_imu.hpp index 4246f3d3..00a88ee2 100644 --- a/include/glim/odometry/odometry_estimation_imu.hpp +++ b/include/glim/odometry/odometry_estimation_imu.hpp @@ -37,6 +37,9 @@ struct OdometryEstimationIMUParams { virtual ~OdometryEstimationIMUParams(); public: + // Preprocessing params + double plane_eps; + // Sensor params; bool fix_imu_bias; double imu_bias_noise; diff --git a/include/glim/viewer/interactive_viewer.hpp b/include/glim/viewer/interactive_viewer.hpp index bafc4a01..7842b857 100644 --- a/include/glim/viewer/interactive_viewer.hpp +++ b/include/glim/viewer/interactive_viewer.hpp @@ -94,8 +94,15 @@ class InteractiveViewer : public ExtensionModule { bool enable_partial_rendering; int partial_rendering_budget; + double point_size; + bool point_size_metric; + bool point_shape_circle; + Eigen::Vector2f z_range; + double points_alpha; + double factors_alpha; + // Click information Eigen::Vector4i right_clicked_info; Eigen::Vector3f right_clicked_pos; diff --git a/include/glim/viewer/standard_viewer.hpp b/include/glim/viewer/standard_viewer.hpp index 0bb4607b..3b67094a 100644 --- a/include/glim/viewer/standard_viewer.hpp +++ b/include/glim/viewer/standard_viewer.hpp @@ -79,8 +79,14 @@ class StandardViewer : public ExtensionModule { std::vector, FactorLineGetter>> odometry_factor_lines; std::unordered_map odometry_poses; + double point_size; + bool point_size_metric; + bool point_shape_circle; + Eigen::Vector2f z_range; Eigen::Vector2f auto_z_range; + double points_alpha; + double factors_alpha; std::unique_ptr trajectory; std::vector submap_keyframes; diff --git a/src/glim/common/cloud_covariance_estimation.cpp b/src/glim/common/cloud_covariance_estimation.cpp index dfe24ba6..c1e32f07 100644 --- a/src/glim/common/cloud_covariance_estimation.cpp +++ b/src/glim/common/cloud_covariance_estimation.cpp @@ -10,7 +10,10 @@ namespace glim { -CloudCovarianceEstimation::CloudCovarianceEstimation(const int num_threads) : regularization_method(RegularizationMethod::PLANE), num_threads(num_threads) {} +CloudCovarianceEstimation::CloudCovarianceEstimation(const int num_threads, const double plane_eps) +: regularization_method(RegularizationMethod::PLANE), + num_threads(num_threads), + plane_eps(plane_eps) {} CloudCovarianceEstimation::~CloudCovarianceEstimation() {} @@ -151,7 +154,7 @@ Eigen::Matrix4d CloudCovarianceEstimation::regularize(const Eigen::Matrix4d& cov *eigenvectors = eig.eigenvectors(); } - Eigen::Vector3d values(1e-3, 1.0, 1.0); + Eigen::Vector3d values(plane_eps, 1.0, 1.0); Eigen::Matrix4d c = Eigen::Matrix4d::Zero(); c.block<3, 3>(0, 0) = eig.eigenvectors() * values.asDiagonal() * eig.eigenvectors().transpose(); return c; @@ -169,7 +172,7 @@ Eigen::Matrix4d CloudCovarianceEstimation::regularize(const Eigen::Matrix4d& cov } Eigen::Vector3d values = eig.eigenvalues() / eig.eigenvalues()[2]; - values = values.array().max(1e-3); + values = values.array().max(plane_eps); Eigen::Matrix4d c = Eigen::Matrix4d::Zero(); c.block<3, 3>(0, 0) = eig.eigenvectors() * values.asDiagonal() * eig.eigenvectors().transpose(); @@ -177,7 +180,7 @@ Eigen::Matrix4d CloudCovarianceEstimation::regularize(const Eigen::Matrix4d& cov } case RegularizationMethod::FROBENIUS: { - const double lambda = 1e-3; + const double lambda = plane_eps; Eigen::Matrix3d C = cov.block<3, 3>(0, 0) + lambda * Eigen::Matrix3d::Identity(); Eigen::Matrix3d C_inv = C.inverse(); Eigen::Matrix4d C_ = Eigen::Matrix4d::Zero(); diff --git a/src/glim/odometry/odometry_estimation_gpu.cpp b/src/glim/odometry/odometry_estimation_gpu.cpp index 6b4cb1ed..e0f38907 100644 --- a/src/glim/odometry/odometry_estimation_gpu.cpp +++ b/src/glim/odometry/odometry_estimation_gpu.cpp @@ -48,6 +48,8 @@ OdometryEstimationGPUParams::OdometryEstimationGPUParams() : OdometryEstimationI const std::string strategy = config.param("odometry_estimation", "keyframe_update_strategy", "OVERLAP"); if (strategy == "OVERLAP") { keyframe_strategy = KeyframeUpdateStrategy::OVERLAP; + } else if (strategy == "OVERLAP2") { + keyframe_strategy = KeyframeUpdateStrategy::OVERLAP2; } else if (strategy == "DISPLACEMENT") { keyframe_strategy = KeyframeUpdateStrategy::DISPLACEMENT; } else if (strategy == "ENTROPY") { @@ -104,6 +106,9 @@ void OdometryEstimationGPU::update_frames(const int current, const gtsam::Nonlin case OdometryEstimationGPUParams::KeyframeUpdateStrategy::OVERLAP: update_keyframes_overlap(current); break; + case OdometryEstimationGPUParams::KeyframeUpdateStrategy::OVERLAP2: + update_keyframes_overlap2(current); + break; case OdometryEstimationGPUParams::KeyframeUpdateStrategy::DISPLACEMENT: update_keyframes_displacement(current); break; @@ -170,12 +175,27 @@ gtsam::NonlinearFactorGraph OdometryEstimationGPU::create_factors(const int curr create_binary_factor(factors, X(target), X(current), frames[target], frames[current]); } - for (const auto& keyframe : keyframes) { + // Compute overlaps to keyframes + std::vector keyframe_voxelmaps(keyframes.size()); + std::vector Ts_keyframe_current(keyframes.size()); + for (int i = 0; i < keyframes.size(); i++) { + keyframe_voxelmaps[i] = keyframes[i]->voxelmaps.back(); + Ts_keyframe_current[i] = keyframes[i]->T_world_imu.inverse() * frames[current]->T_world_imu; + } + std::vector current_points(keyframes.size(), frames[current]->frame); + std::vector overlaps = gtsam_points::overlap_gpu(keyframe_voxelmaps, current_points, Ts_keyframe_current, *stream); + + for (int i = 0; i < keyframes.size(); i++) { + const auto& keyframe = keyframes[i]; if (keyframe->id >= current - params->full_connection_window_size) { // There already exists a factor continue; } + if (overlaps[i] < 0.01) { + continue; + } + auto stream_buffer = stream_buffer_roundrobin->get_stream_buffer(); const auto& stream = stream_buffer.first; const auto& buffer = stream_buffer.second; @@ -284,6 +304,86 @@ void OdometryEstimationGPU::update_keyframes_overlap(int current) { Callbacks::on_marginalized_keyframes(marginalized_keyframes); } +/** + * @brief Keyframe management based on an overlap metric + * @ref Koide et al., "Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping", ICRA2022 + */ +void OdometryEstimationGPU::update_keyframes_overlap2(int current) { + const auto params = static_cast(this->params.get()); + + if (!frames[current]->frame->size()) { + return; + } + + if (keyframes.empty()) { + keyframes.push_back(frames[current]); + return; + } + + std::vector keyframes_(keyframes.size()); + std::vector delta_from_keyframes(keyframes.size()); + for (int i = 0; i < keyframes.size(); i++) { + keyframes_[i] = keyframes[i]->voxelmaps.back(); + delta_from_keyframes[i] = keyframes[i]->T_world_imu.inverse() * frames[current]->T_world_imu; + } + + const double overlap = gtsam_points::overlap_gpu(keyframes_, frames[current]->frame, delta_from_keyframes, *stream); + if (overlap > params->keyframe_max_overlap) { + return; + } + + const auto& new_keyframe = frames[current]; + keyframes.push_back(new_keyframe); + + if (keyframes.size() <= params->max_num_keyframes) { + return; + } + + std::vector marginalized_keyframes; + + std::vector overlap_targets; + std::vector overlap_sources; + std::vector Ts_target_source; + + for (int i = 0; i < keyframes.size(); i++) { + for (int j = i + 1; j < keyframes.size(); j++) { + overlap_targets.emplace_back(keyframes[i]->voxelmaps.back()); + overlap_sources.emplace_back(keyframes[j]->frame); + Ts_target_source.emplace_back(keyframes[i]->T_world_imu.inverse() * keyframes[j]->T_world_imu); + } + } + + std::vector overlaps = gtsam_points::overlap_gpu(overlap_targets, overlap_sources, Ts_target_source, *stream); + + Eigen::MatrixXd overlap_map = Eigen::MatrixXd::Ones(keyframes.size(), keyframes.size()); + int count = 0; + for (int i = 0; i < keyframes.size(); i++) { + for (int j = i + 1; j < keyframes.size(); j++) { + overlap_map(i, j) = overlaps[count]; + count++; + } + } + + constexpr int power = 3; + std::vector scores(keyframes.size() - 1, 0.0); + for (int i = 0; i < keyframes.size() - 1; i++) { + Eigen::MatrixXd overlap_map_i = overlap_map.topLeftCorner(keyframes.size() - 1, keyframes.size() - 1); + overlap_map_i.row(i).setZero(); + overlap_map_i.col(i).setZero(); + + const double sum_overlap = overlap_map_i.array().pow(power).sum(); + const double overlap_latest = overlap_map(i, keyframes.size() - 1); + scores[i] = std::pow(overlap_latest, 1.0 / power) * sum_overlap; + } + + auto min_loc = std::min_element(scores.begin(), scores.end()); + int frame_to_eliminate = std::distance(scores.begin(), min_loc); + + marginalized_keyframes.push_back(keyframes[frame_to_eliminate]); + keyframes.erase(keyframes.begin() + frame_to_eliminate); + Callbacks::on_marginalized_keyframes(marginalized_keyframes); +} + /** * @brief Keyframe management based on displacement criteria * @ref Engel et al., "Direct Sparse Odometry", IEEE Trans. PAMI, 2018 diff --git a/src/glim/odometry/odometry_estimation_imu.cpp b/src/glim/odometry/odometry_estimation_imu.cpp index 783648ac..5f36b98c 100644 --- a/src/glim/odometry/odometry_estimation_imu.cpp +++ b/src/glim/odometry/odometry_estimation_imu.cpp @@ -32,6 +32,9 @@ using gtsam::symbol_shorthand::V; // IMU velocity (v_world_imu) using gtsam::symbol_shorthand::X; // IMU pose (T_world_imu) OdometryEstimationIMUParams::OdometryEstimationIMUParams() { + Config preprocess_config(GlobalConfig::get_config_path("config_preprocess")); + plane_eps = preprocess_config.param("preprocess", "plane_eps", 1e-3); + // sensor config Config sensor_config(GlobalConfig::get_config_path("config_sensors")); T_lidar_imu = sensor_config.param("sensors", "T_lidar_imu", Eigen::Isometry3d::Identity()); @@ -89,7 +92,7 @@ OdometryEstimationIMU::OdometryEstimationIMU(std::unique_ptrnum_threads)); + covariance_estimation.reset(new CloudCovarianceEstimation(params->num_threads, params->plane_eps)); gtsam::ISAM2Params isam2_params; if (params->use_isam2_dogleg) { diff --git a/src/glim/viewer/interactive_viewer.cpp b/src/glim/viewer/interactive_viewer.cpp index a2e38526..39201a86 100644 --- a/src/glim/viewer/interactive_viewer.cpp +++ b/src/glim/viewer/interactive_viewer.cpp @@ -57,6 +57,13 @@ InteractiveViewer::InteractiveViewer() : logger(create_module_logger("viewer")) enable_partial_rendering = config.param("interactive_viewer", "enable_partial_rendering", false); partial_rendering_budget = config.param("interactive_viewer", "partial_rendering_budget", 1024); + points_alpha = config.param("interactive_viewer", "points_alpha", 1.0); + factors_alpha = config.param("interactive_viewer", "factors_alpha", 1.0); + + point_size = config.param("interactive_viewer", "point_size", 1.0); + point_size_metric = config.param("interactive_viewer", "point_size_metric", false); + point_shape_circle = config.param("interactive_viewer", "point_shape_circle", true); + z_range = config.param("interactive_viewer", "default_z_range", Eigen::Vector2d(-2.0, 4.0)).cast(); trajectory.reset(new TrajectoryManager); @@ -90,6 +97,16 @@ void InteractiveViewer::viewer_loop() { viewer->enable_vsync(); viewer->shader_setting().add("z_range", z_range); + viewer->shader_setting().set_point_size(point_size); + + if (point_size_metric) { + viewer->shader_setting().set_point_scale_metric(); + } + + if (point_shape_circle) { + viewer->shader_setting().set_point_shape_circle(); + } + if (enable_partial_rendering) { viewer->enable_partial_rendering(0.1); viewer->shader_setting().add("dynamic_object", 1); @@ -289,7 +306,7 @@ void InteractiveViewer::update_viewer() { } else { const Eigen::Vector4i info(static_cast(PickType::POINTS), 0, 0, submap->id); auto cloud_buffer = std::make_shared(submap->frame->points, submap->frame->size()); - auto shader_setting = guik::Rainbow(submap_pose).add("info_values", info); + auto shader_setting = guik::Rainbow(submap_pose).add("info_values", info).set_alpha(points_alpha); if (enable_partial_rendering) { cloud_buffer->enable_partial_rendering(partial_rendering_budget); @@ -348,13 +365,13 @@ void InteractiveViewer::update_viewer() { Eigen::Vector4f color; switch (type) { case FactorType::MATCHING_COST: - color = Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f); + color = Eigen::Vector4f(0.0f, 1.0f, 0.0f, factors_alpha); break; case FactorType::BETWEEN: - color = Eigen::Vector4f(0.0f, 0.0f, 1.0f, 1.0f); + color = Eigen::Vector4f(0.0f, 0.0f, 1.0f, factors_alpha); break; case FactorType::IMU: - color = Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f); + color = Eigen::Vector4f(1.0f, 0.0f, 0.0f, factors_alpha); break; } @@ -362,7 +379,7 @@ void InteractiveViewer::update_viewer() { factor_colors.push_back(color); } - viewer->update_drawable("factors", std::make_shared(factor_lines, factor_colors), guik::VertexColor()); + viewer->update_drawable("factors", std::make_shared(factor_lines, factor_colors), guik::VertexColor().set_alpha(factors_alpha)); std::vector traj; for (const auto& submap : submaps) { diff --git a/src/glim/viewer/standard_viewer.cpp b/src/glim/viewer/standard_viewer.cpp index 062020af..4806d3d6 100644 --- a/src/glim/viewer/standard_viewer.cpp +++ b/src/glim/viewer/standard_viewer.cpp @@ -58,6 +58,13 @@ StandardViewer::StandardViewer() : logger(create_module_logger("viewer")) { z_range = config.param("standard_viewer", "default_z_range", Eigen::Vector2d(-2.0, 4.0)).cast(); auto_z_range << 0.0f, 0.0f; + points_alpha = config.param("standard_viewer", "points_alpha", 1.0); + factors_alpha = config.param("standard_viewer", "factors_alpha", 1.0); + + point_size = config.param("standard_viewer", "point_size", 1.0); + point_size_metric = config.param("standard_viewer", "point_size_metric", false); + point_shape_circle = config.param("standard_viewer", "point_shape_circle", true); + trajectory.reset(new TrajectoryManager); enable_partial_rendering = config.param("standard_viewer", "enable_partial_rendering", false); @@ -265,7 +272,11 @@ void StandardViewer::set_callbacks() { } const Eigen::Vector3d pt1 = static_cast(factor)->get_fixed_target_pose().translation(); - return std::make_tuple(found0->second.translation(), pt1.cast(), Eigen::Vector4f(0.0f, 1.0f, 0.0f, 0.5f), Eigen::Vector4f(1.0f, 0.0f, 0.0f, 0.5f)); + return std::make_tuple( + found0->second.translation(), + pt1.cast(), + Eigen::Vector4f(0.0f, 1.0f, 0.0f, factors_alpha), + Eigen::Vector4f(1.0f, 0.0f, 0.0f, factors_alpha)); }; new_factor_lines.emplace_back(factor, l); @@ -282,7 +293,7 @@ void StandardViewer::set_callbacks() { } const Eigen::Vector3f pt1 = static_cast(factor)->get_fixed_target_pose().translation(); - return std::make_tuple(found0->second.translation(), pt1, Eigen::Vector4f(0.0f, 1.0f, 0.0f, 0.5f), Eigen::Vector4f(1.0f, 0.0f, 0.0f, 0.5f)); + return std::make_tuple(found0->second.translation(), pt1, Eigen::Vector4f(0.0f, 1.0f, 0.0f, factors_alpha), Eigen::Vector4f(1.0f, 0.0f, 0.0f, factors_alpha)); }; new_factor_lines.emplace_back(factor, l); @@ -306,7 +317,11 @@ void StandardViewer::set_callbacks() { return std::nullopt; } - return std::make_tuple(found0->second.translation(), found1->second.translation(), Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f), Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f)); + return std::make_tuple( + found0->second.translation(), + found1->second.translation(), + Eigen::Vector4f(0.0f, 1.0f, 0.0f, factors_alpha), + Eigen::Vector4f(0.0f, 1.0f, 0.0f, factors_alpha)); }; new_factor_lines.emplace_back(factor, l); @@ -342,7 +357,7 @@ void StandardViewer::set_callbacks() { } auto viewer = guik::viewer(); - viewer->update_drawable("odometry_factors", std::make_shared(line_vertices, line_colors), guik::VertexColor().make_transparent()); + viewer->update_drawable("odometry_factors", std::make_shared(line_vertices, line_colors), guik::VertexColor().set_alpha(factors_alpha)); }); }); @@ -461,7 +476,7 @@ void StandardViewer::set_callbacks() { lines.push_back(submap_keyframes[factor.second].translation()); } - sub_viewer->update_drawable("factors", std::make_shared(lines), guik::FlatGreen()); + sub_viewer->update_drawable("factors", std::make_shared(lines), guik::FlatGreen().set_alpha(factors_alpha)); }); }); @@ -486,7 +501,7 @@ void StandardViewer::set_callbacks() { auto viewer = guik::LightViewer::instance(); auto cloud_buffer = std::make_shared(submap->frame->points, submap->frame->size()); auto shader_setting = guik::Rainbow(T_world_origin->matrix().cast()); - shader_setting.add("point_scale", 0.1f); + shader_setting.set_alpha(points_alpha); if (enable_partial_rendering) { cloud_buffer->enable_partial_rendering(partial_rendering_budget); @@ -535,7 +550,7 @@ void StandardViewer::set_callbacks() { } } - viewer->update_drawable("factors", std::make_shared(submap_positions, indices), guik::FlatGreen()); + viewer->update_drawable("factors", std::make_shared(submap_positions, indices), guik::FlatGreen().set_alpha(factors_alpha)); viewer->shader_setting().add("z_range", auto_z_range + z_range); }); }); @@ -573,6 +588,15 @@ void StandardViewer::viewer_loop() { auto viewer = guik::LightViewer::instance(Eigen::Vector2i(config.param("standard_viewer", "viewer_width", 2560), config.param("standard_viewer", "viewer_height", 1440))); viewer->enable_vsync(); viewer->shader_setting().add("z_range", z_range); + viewer->shader_setting().set_point_size(point_size); + + if (point_size_metric) { + viewer->shader_setting().set_point_scale_metric(); + } + + if (point_shape_circle) { + viewer->shader_setting().set_point_shape_circle(); + } if (enable_partial_rendering) { viewer->enable_partial_rendering(1e-1);