diff --git a/config/config_viewer.json b/config/config_viewer.json index b93e94d..76cbe2f 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/viewer/interactive_viewer.hpp b/include/glim/viewer/interactive_viewer.hpp index bafc4a0..7842b85 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 0bb4607..3b67094 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/util/trajectory_manager.cpp b/src/glim/util/trajectory_manager.cpp index 8a2c5f9..381c2df 100644 --- a/src/glim/util/trajectory_manager.cpp +++ b/src/glim/util/trajectory_manager.cpp @@ -35,7 +35,7 @@ void TrajectoryManager::update_anchor(double stamp, const Eigen::Isometry3d& T_w const double t0 = odom_stamps[idx - 1]; const double t1 = odom_stamps[idx]; if (t0 > stamp || t1 < stamp) { - T_world_odom = T_world_sensor * T_odom_sensor[idx].inverse(); + // T_world_odom = T_world_sensor * T_odom_sensor[idx].inverse(); return; } diff --git a/src/glim/viewer/interactive_viewer.cpp b/src/glim/viewer/interactive_viewer.cpp index a2e3852..39201a8 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 062020a..4806d3d 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);