From d39cff4ab28efaedc0850d3c86517f13454347eb Mon Sep 17 00:00:00 2001 From: Erick Fuentes Date: Wed, 22 Jan 2025 15:57:13 -0500 Subject: [PATCH 1/5] simple example of using the opencv viewer --- experimental/learn_descriptors/BUILD | 8 ++++ .../learn_descriptors/opencv_viz_example.cc | 40 +++++++++++++++++++ 2 files changed, 48 insertions(+) create mode 100644 experimental/learn_descriptors/opencv_viz_example.cc diff --git a/experimental/learn_descriptors/BUILD b/experimental/learn_descriptors/BUILD index e0f417bd..141f869a 100644 --- a/experimental/learn_descriptors/BUILD +++ b/experimental/learn_descriptors/BUILD @@ -68,3 +68,11 @@ cc_test( ":learn_descriptors" ] ) + +cc_binary( + name = "opencv_viz_example", + srcs = ["opencv_viz_example.cc"], + deps = [ + "@opencv//:opencv" + ] +) diff --git a/experimental/learn_descriptors/opencv_viz_example.cc b/experimental/learn_descriptors/opencv_viz_example.cc new file mode 100644 index 00000000..f760e983 --- /dev/null +++ b/experimental/learn_descriptors/opencv_viz_example.cc @@ -0,0 +1,40 @@ + +#include "opencv2/viz.hpp" + +int main() { + cv::viz::Viz3d window("My Window"); + + window.showWidget("world_frame", cv::viz::WCoordinateSystem()); + window.showWidget("text_overlay", cv::viz::WText("hello world overlay!", cv::Point(20, 50))); + constexpr bool ALWAYS_FACE_CAMERA = true; + constexpr double TEXT_SCALE = 0.1; + window.showWidget("text_3d", cv::viz::WText3D("hello world 3d!", cv::Point3d(0.1, 0.2, 0.3), + TEXT_SCALE, ALWAYS_FACE_CAMERA)); + + constexpr bool FIXED_TEXT = false; + cv::Affine3d world_from_fixed_text( + cv::Affine3d::Vec3{M_PI_2, 0.0, 0.0}, // rotation + {0.2, 0.4, 0.6} // translation + ); + + window.showWidget( + "text_3d_fixed", + cv::viz::WText3D("hello world fixed!", cv::Point3d(0.0, 0.0, 0.0), TEXT_SCALE, FIXED_TEXT), + world_from_fixed_text + ); + constexpr double COORD_SCALE = 0.2; + window.showWidget("text_3d_fixed_frame", cv::viz::WCoordinateSystem(COORD_SCALE), world_from_fixed_text); + + constexpr double CIRCLE_RADIUS_M = 0.5; + cv::Affine3d world_from_circle( + cv::Affine3d::Vec3{0.0, 0.0, 0.0}, // rotation + {0.0, 0.0, 1.5} // translation + ); + window.showWidget( + "circle", + cv::viz::WCircle(CIRCLE_RADIUS_M), + world_from_circle + ); + + window.spin(); +} From 7ddc579d3fec7cc105829ee1aca0d282512e9fd5 Mon Sep 17 00:00:00 2001 From: Nicolaniello Buono Date: Mon, 3 Feb 2025 21:17:44 -0500 Subject: [PATCH 2/5] uploading progress --- common/geometry/BUILD | 28 +++++ common/geometry/opencv_viz.cc | 114 +++++++++++++++++ common/geometry/opencv_viz.hh | 9 ++ common/geometry/opencv_viz_example.cc | 52 ++++++++ common/geometry/opencv_viz_test.cc | 89 +++++++++++++ experimental/learn_descriptors/BUILD | 8 ++ experimental/learn_descriptors/opencv_viz.cc | 119 ++++++++++++++++++ .../learn_descriptors/opencv_viz_example.cc | 49 +++++++- 8 files changed, 465 insertions(+), 3 deletions(-) create mode 100644 common/geometry/opencv_viz.cc create mode 100644 common/geometry/opencv_viz.hh create mode 100644 common/geometry/opencv_viz_example.cc create mode 100644 common/geometry/opencv_viz_test.cc create mode 100644 experimental/learn_descriptors/opencv_viz.cc diff --git a/common/geometry/BUILD b/common/geometry/BUILD index 8468ebe1..0a28b1e2 100644 --- a/common/geometry/BUILD +++ b/common/geometry/BUILD @@ -18,3 +18,31 @@ cc_test( "@com_google_googletest//:gtest_main", ] ) + +cc_library( + name = "opencv_viz", + hdrs = ["opencv_viz.hh"], + srcs = ["opencv_viz.cc"], + visibility = ["//visibility:public"], + deps = [ + "@eigen", + "@opencv//:opencv" + ] +) + +cc_test( + name = "opencv_viz_test", + srcs = ["opencv_viz_test.cc"], + deps = [ + ":opencv_viz", + "@com_google_googletest//:gtest_main", + ] +) + +cc_binary( + name = "opencv_viz_example", + srcs = ["opencv_viz_example.cc"], + deps = [ + "@opencv//:opencv" + ] +) diff --git a/common/geometry/opencv_viz.cc b/common/geometry/opencv_viz.cc new file mode 100644 index 00000000..1649291e --- /dev/null +++ b/common/geometry/opencv_viz.cc @@ -0,0 +1,114 @@ +#include "common/geometry/opencv_viz.hh" + +namespace robot::geometry::opencv_viz { +template +cv::Mat eigen_to_cv_mat(const Eigen::MatrixBase& eigenMatrix) { + int cvType; + if constexpr (std::is_same_v) { + cvType = CV_64F; // 64-bit floating point + } else if constexpr (std::is_same_v) { + cvType = CV_32F; // 32-bit floating point + } else if constexpr (std::is_same_v) { + cvType = CV_32S; // 32-bit signed integer + } else { + static_assert(!std::is_same_v, "Unsupported data type in Eigen matrix"); + } + return cv::Mat(eigenMatrix.rows(), eigenMatrix.cols(), cvType, const_cast(static_cast(eigenMatrix.derived().data()))).clone(); +} +cv::Vec3d rotation_matrix_to_axis_angle(const cv::Matx33d &R) { + // Ensure R is a valid rotation matrix + CV_Assert(cv::determinant(R) > 0.999 && cv::determinant(R) < 1.001); + + double trace = R(0, 0) + R(1, 1) + R(2, 2); + double theta = std::acos((trace - 1) / 2.0); + + if (std::abs(theta) < 1e-6) { + return cv::Vec3d(0, 0, 0); + } + + double sinTheta = std::sin(theta); + cv::Vec3d axis( + (R(2, 1) - R(1, 2)) / (2.0 * sinTheta), + (R(0, 2) - R(2, 0)) / (2.0 * sinTheta), + (R(1, 0) - R(0, 1)) / (2.0 * sinTheta) + ); + + return axis * theta; +} + +void viz_scene(const std::vector &poses, const std::vector &points) { + cv::viz::Viz3d window("Viz Scene"); + + constexpr double pose_size = 0.2; + for (unsigned int i = 0; i < poses.size(); i++) { + cv::Affine3d cv_pose( + eigen_to_cv_mat(poses[i].matrix().block(0,0,3,3)) , + cv::Affine3d::Vec3(poses[i].translation()[0], poses[i].translation()[1], poses[i].translation()[3]) + ); + window.showWidget( + "pose_" + std::to_string(i), + cv::viz::WCoordinateSystem(pose_size), + cv_pose + ); + } + constexpr double point_radius = 0.2; + constexpr int sphere_res = 10; + const cv::viz::Color point_color = cv::viz::Color::celestial_blue(); + for (unsigned int i = 0; i < points.size(); i++) { + const Eigen::Vector3d &point = points[i]; + window.showWidget( + "translation_" + std::to_string(i), + cv::viz::WSphere( + cv::Point3d(point[0], point[1], point[2]), + point_radius, + sphere_res, + point_color + ) + ); + } + + window.showWidget("world_frame", cv::viz::WCoordinateSystem()); + + constexpr unsigned int num_cells = 6; + constexpr double units = 1.0; + const cv::viz::Color color = cv::viz::Color::white(); + const cv::Vec2i cells = cv::Vec2i::all(num_cells); + const cv::Vec2d cell_sizes = cv::Vec2d::all(units); + window.showWidget("xy_grid", cv::viz::WGrid(cells, cell_sizes, color)); + window.showWidget("yz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(1,0,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); + window.showWidget("xz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(0,1,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); + window.showWidget("text_overlay", cv::viz::WText("hello world overlay!", cv::Point(20, 50))); + + constexpr bool ALWAYS_FACE_CAMERA = true; + constexpr double TEXT_SCALE = 0.1; + window.showWidget("text_3d", cv::viz::WText3D("hello world 3d!", cv::Point3d(0.1, 0.2, 0.3), + TEXT_SCALE, ALWAYS_FACE_CAMERA)); + + constexpr bool FIXED_TEXT = false; + cv::Affine3d world_from_fixed_text( + cv::Affine3d::Vec3{M_PI_2, 0.0, 0.0}, // rotation + {0.2, 0.4, 0.6} // translation + ); + + window.showWidget( + "text_3d_fixed", + cv::viz::WText3D("hello world fixed!", cv::Point3d(0.0, 0.0, 0.0), TEXT_SCALE, FIXED_TEXT), + world_from_fixed_text + ); + constexpr double COORD_SCALE = 0.2; + window.showWidget("text_3d_fixed_frame", cv::viz::WCoordinateSystem(COORD_SCALE), world_from_fixed_text); + + constexpr double CIRCLE_RADIUS_M = 0.5; + cv::Affine3d world_from_circle( + cv::Affine3d::Vec3{0.0, 0.0, 0.0}, // rotation + {0.0, 0.0, 1.5} // translation + ); + window.showWidget( + "circle", + cv::viz::WCircle(CIRCLE_RADIUS_M), + world_from_circle + ); + + window.spin(); +} +} diff --git a/common/geometry/opencv_viz.hh b/common/geometry/opencv_viz.hh new file mode 100644 index 00000000..a64d1d8d --- /dev/null +++ b/common/geometry/opencv_viz.hh @@ -0,0 +1,9 @@ +#pragma once +#include +#include "opencv2/viz.hpp" +#include "Eigen/Core" +#include "Eigen/Geometry" + +namespace robot::geometry::opencv_viz { +void viz_scene(const std::vector &poses, const std::vector &points); +} \ No newline at end of file diff --git a/common/geometry/opencv_viz_example.cc b/common/geometry/opencv_viz_example.cc new file mode 100644 index 00000000..614b8292 --- /dev/null +++ b/common/geometry/opencv_viz_example.cc @@ -0,0 +1,52 @@ +#include "opencv2/viz.hpp" + +void demo() { + cv::viz::Viz3d window("My Window"); + + window.showWidget("world_frame", cv::viz::WCoordinateSystem()); + + constexpr unsigned int num_cells = 6; + constexpr double units = 1.0; + const cv::viz::Color color = cv::viz::Color::white(); + const cv::Vec2i cells = cv::Vec2i::all(num_cells); + const cv::Vec2d cell_sizes = cv::Vec2d::all(units); + window.showWidget("xy_grid", cv::viz::WGrid(cells, cell_sizes, color)); + window.showWidget("yz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(1,0,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); + window.showWidget("xz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(0,1,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); + window.showWidget("text_overlay", cv::viz::WText("hello world overlay!", cv::Point(20, 50))); + constexpr bool ALWAYS_FACE_CAMERA = true; + constexpr double TEXT_SCALE = 0.1; + window.showWidget("text_3d", cv::viz::WText3D("hello world 3d!", cv::Point3d(0.1, 0.2, 0.3), + TEXT_SCALE, ALWAYS_FACE_CAMERA)); + + constexpr bool FIXED_TEXT = false; + cv::Affine3d world_from_fixed_text( + cv::Affine3d::Vec3{M_PI_2, 0.0, 0.0}, // rotation + {0.2, 0.4, 0.6} // translation + ); + + window.showWidget( + "text_3d_fixed", + cv::viz::WText3D("hello world fixed!", cv::Point3d(0.0, 0.0, 0.0), TEXT_SCALE, FIXED_TEXT), + world_from_fixed_text + ); + constexpr double COORD_SCALE = 0.2; + window.showWidget("text_3d_fixed_frame", cv::viz::WCoordinateSystem(COORD_SCALE), world_from_fixed_text); + + constexpr double CIRCLE_RADIUS_M = 0.5; + cv::Affine3d world_from_circle( + cv::Affine3d::Vec3{0.0, 0.0, 0.0}, // rotation + {0.0, 0.0, 1.5} // translation + ); + window.showWidget( + "circle", + cv::viz::WCircle(CIRCLE_RADIUS_M), + world_from_circle + ); + + window.spin(); +} + +int main() { + demo(); +} diff --git a/common/geometry/opencv_viz_test.cc b/common/geometry/opencv_viz_test.cc new file mode 100644 index 00000000..bbe872e4 --- /dev/null +++ b/common/geometry/opencv_viz_test.cc @@ -0,0 +1,89 @@ +#include "common/geometry/opencv_viz.hh" +#include "gtest/gtest.h" + +namespace robot::geometry::opencv_viz { +namespace { +bool is_test() { return std::getenv("BAZEL_TEST") != nullptr && + std::getenv("BUILD_WORKSPACE_DIRECTORY") == nullptr; } +} // namespace + +TEST(OpencvVizTest, demo) { + cv::viz::Viz3d window("My Window"); + + window.showWidget("world_frame", cv::viz::WCoordinateSystem()); + + constexpr unsigned int num_cells = 6; + constexpr double units = 1.0; + const cv::viz::Color color = cv::viz::Color::white(); + const cv::Vec2i cells = cv::Vec2i::all(num_cells); + const cv::Vec2d cell_sizes = cv::Vec2d::all(units); + window.showWidget("xy_grid", cv::viz::WGrid(cells, cell_sizes, color)); + window.showWidget("yz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(1,0,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); + window.showWidget("xz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(0,1,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); + window.showWidget("text_overlay", cv::viz::WText("hello world overlay!", cv::Point(20, 50))); + constexpr bool ALWAYS_FACE_CAMERA = true; + constexpr double TEXT_SCALE = 0.1; + window.showWidget("text_3d", cv::viz::WText3D("hello world 3d!", cv::Point3d(0.1, 0.2, 0.3), + TEXT_SCALE, ALWAYS_FACE_CAMERA)); + + constexpr bool FIXED_TEXT = false; + cv::Affine3d world_from_fixed_text( + cv::Affine3d::Vec3{M_PI_2, 0.0, 0.0}, // rotation + {0.2, 0.4, 0.6} // translation + ); + + window.showWidget( + "text_3d_fixed", + cv::viz::WText3D("hello world fixed!", cv::Point3d(0.0, 0.0, 0.0), TEXT_SCALE, FIXED_TEXT), + world_from_fixed_text + ); + constexpr double COORD_SCALE = 0.2; + window.showWidget("text_3d_fixed_frame", cv::viz::WCoordinateSystem(COORD_SCALE), world_from_fixed_text); + + constexpr double CIRCLE_RADIUS_M = 0.5; + cv::Affine3d world_from_circle( + cv::Affine3d::Vec3{0.0, 0.0, 0.0}, // rotation + {0.0, 0.0, 1.5} // translation + ); + window.showWidget( + "circle", + cv::viz::WCircle(CIRCLE_RADIUS_M), + world_from_circle + ); + if (!is_test()) { + window.spin(); + } +} + +TEST(OpencvVizTest, cube_test) { + std::vector cube_W; + float cube_size = 1.0f; + cube_W.push_back(Eigen::Vector3d(0, 0, 0)); + cube_W.push_back(Eigen::Vector3d(cube_size, 0, 0)); + cube_W.push_back(Eigen::Vector3d(cube_size, cube_size, 0)); + cube_W.push_back(Eigen::Vector3d(0, cube_size, 0)); + cube_W.push_back(Eigen::Vector3d(0, 0, cube_size)); + cube_W.push_back(Eigen::Vector3d(cube_size, 0, cube_size)); + cube_W.push_back(Eigen::Vector3d(cube_size, cube_size, cube_size)); + cube_W.push_back(Eigen::Vector3d(0, cube_size, cube_size)); + + std::vector poses; + + Eigen::Matrix3d rotation0( + Eigen::AngleAxis(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix() * + Eigen::AngleAxis(-M_PI / 2, Eigen::Vector3d(1, 0, 0)).toRotationMatrix()); + Eigen::Isometry3d pose0; + pose0.translation() = Eigen::Vector3d(4,0,0); + pose0.linear() = rotation0; + poses.push_back(pose0); + + Eigen::Isometry3d pose1; + pose1.linear() = Eigen::AngleAxis(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix() * rotation0; + pose1.translation() = Eigen::Vector3d(0,4,0); + poses.push_back(pose1); + + if (!is_test()) { + viz_scene(poses, cube_W); + } +} +} \ No newline at end of file diff --git a/experimental/learn_descriptors/BUILD b/experimental/learn_descriptors/BUILD index 141f869a..9728193b 100644 --- a/experimental/learn_descriptors/BUILD +++ b/experimental/learn_descriptors/BUILD @@ -76,3 +76,11 @@ cc_binary( "@opencv//:opencv" ] ) + +cc_binary( + name = "opencv_viz_test", + srcs = ["opencv_viz_example.cc"], + deps = [ + "@opencv//:opencv" + ] +) diff --git a/experimental/learn_descriptors/opencv_viz.cc b/experimental/learn_descriptors/opencv_viz.cc new file mode 100644 index 00000000..0e6d5102 --- /dev/null +++ b/experimental/learn_descriptors/opencv_viz.cc @@ -0,0 +1,119 @@ +#include + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Geometry" +#include "opencv2/viz.hpp" + +namespace opencv_viz { +template +cv::Mat eigen_to_cv_mat(const Eigen::MatrixBase& eigenMatrix) { + int cvType; + if constexpr (std::is_same_v) { + cvType = CV_64F; // 64-bit floating point + } else if constexpr (std::is_same_v) { + cvType = CV_32F; // 32-bit floating point + } else if constexpr (std::is_same_v) { + cvType = CV_32S; // 32-bit signed integer + } else { + static_assert(!std::is_same_v, "Unsupported data type in Eigen matrix"); + } + return cv::Mat(eigenMatrix.rows(), eigenMatrix.cols(), cvType, const_cast(static_cast(eigenMatrix.derived().data()))).clone(); +} +cv::Vec3d rotation_matrix_to_axis_angle(const cv::Matx33d &R) { + // Ensure R is a valid rotation matrix + CV_Assert(cv::determinant(R) > 0.999 && cv::determinant(R) < 1.001); + + double trace = R(0, 0) + R(1, 1) + R(2, 2); + double theta = std::acos((trace - 1) / 2.0); + + if (std::abs(theta) < 1e-6) { + return cv::Vec3d(0, 0, 0); + } + + double sinTheta = std::sin(theta); + cv::Vec3d axis( + (R(2, 1) - R(1, 2)) / (2.0 * sinTheta), + (R(0, 2) - R(2, 0)) / (2.0 * sinTheta), + (R(1, 0) - R(0, 1)) / (2.0 * sinTheta) + ); + + return axis * theta; +} +void viz_scene(const std::vector &poses, const std::vector &points) { + cv::viz::Viz3d window("Viz Scene"); + + std::vector cv_poses; + constexpr double pose_size = 0.2; + for (unsigned int i = 0; i < poses.size(); i++) { + cv::Affine3d cv_pose( + eigen_to_cv_mat(poses[i].matrix().block(0,0,3,3)) , + cv::Affine3d::Vec3(poses[i].translation()[0], poses[i].translation()[1], poses[i].translation()[3]) + ); + window.showWidget( + "pose_" + std::to_string(i), + cv::viz::WCoordinateSystem(pose_size), + cv_pose + ); + } + constexpr double point_radius = 0.2; + constexpr int sphere_res = 10; + const cv::viz::Color point_color = cv::viz::Color::celestial_blue(); + for (unsigned int i = 0; i < points.size(); i++) { + const Eigen::Vector3d &point = points[i]; + window.showWidget( + "translation_" + std::to_string(i), + cv::viz::WSphere( + cv::Point3d(point[0], point[1], point[2]), + point_radius, + sphere_res, + point_color + ) + ); + } + + window.showWidget("world_frame", cv::viz::WCoordinateSystem()); + + constexpr unsigned int num_cells = 6; + constexpr double units = 1.0; + const cv::viz::Color color = cv::viz::Color::white(); + const cv::Vec2i cells = cv::Vec2i::all(num_cells); + const cv::Vec2d cell_sizes = cv::Vec2d::all(units); + window.showWidget("xy_grid", cv::viz::WGrid(cells, cell_sizes, color)); + window.showWidget("yz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(1,0,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); + window.showWidget("xz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(0,1,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); + window.showWidget("text_overlay", cv::viz::WText("hello world overlay!", cv::Point(20, 50))); + + constexpr bool ALWAYS_FACE_CAMERA = true; + constexpr double TEXT_SCALE = 0.1; + window.showWidget("text_3d", cv::viz::WText3D("hello world 3d!", cv::Point3d(0.1, 0.2, 0.3), + TEXT_SCALE, ALWAYS_FACE_CAMERA)); + + constexpr bool FIXED_TEXT = false; + cv::Affine3d world_from_fixed_text( + cv::Affine3d::Vec3{M_PI_2, 0.0, 0.0}, // rotation + {0.2, 0.4, 0.6} // translation + ); + + window.showWidget( + "text_3d_fixed", + cv::viz::WText3D("hello world fixed!", cv::Point3d(0.0, 0.0, 0.0), TEXT_SCALE, FIXED_TEXT), + world_from_fixed_text + ); + constexpr double COORD_SCALE = 0.2; + window.showWidget("text_3d_fixed_frame", cv::viz::WCoordinateSystem(COORD_SCALE), world_from_fixed_text); + + constexpr double CIRCLE_RADIUS_M = 0.5; + cv::Affine3d world_from_circle( + cv::Affine3d::Vec3{0.0, 0.0, 0.0}, // rotation + {0.0, 0.0, 1.5} // translation + ); + window.showWidget( + "circle", + cv::viz::WCircle(CIRCLE_RADIUS_M), + world_from_circle + ); + + window.spin(); +} +} + diff --git a/experimental/learn_descriptors/opencv_viz_example.cc b/experimental/learn_descriptors/opencv_viz_example.cc index f760e983..c37c3ba4 100644 --- a/experimental/learn_descriptors/opencv_viz_example.cc +++ b/experimental/learn_descriptors/opencv_viz_example.cc @@ -1,10 +1,20 @@ - #include "opencv2/viz.hpp" +#include "opencv_viz.cc" +#include "eigen/Eigen/Geometry" -int main() { - cv::viz::Viz3d window("My Window"); +void demo() { + cv::viz::Viz3d window("My Window"); window.showWidget("world_frame", cv::viz::WCoordinateSystem()); + + constexpr unsigned int num_cells = 6; + constexpr double units = 1.0; + const cv::viz::Color color = cv::viz::Color::white(); + const cv::Vec2i cells = cv::Vec2i::all(num_cells); + const cv::Vec2d cell_sizes = cv::Vec2d::all(units); + window.showWidget("xy_grid", cv::viz::WGrid(cells, cell_sizes, color)); + window.showWidget("yz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(1,0,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); + window.showWidget("xz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(0,1,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); window.showWidget("text_overlay", cv::viz::WText("hello world overlay!", cv::Point(20, 50))); constexpr bool ALWAYS_FACE_CAMERA = true; constexpr double TEXT_SCALE = 0.1; @@ -38,3 +48,36 @@ int main() { window.spin(); } + +void demo_cube() { + std::vector cube_W; + float cube_size = 1.0f; + cube_W.push_back(Eigen::Vector3d(0, 0, 0)); + cube_W.push_back(Eigen::Vector3d(cube_size, 0, 0)); + cube_W.push_back(Eigen::Vector3d(cube_size, cube_size, 0)); + cube_W.push_back(Eigen::Vector3d(0, cube_size, 0)); + cube_W.push_back(Eigen::Vector3d(0, 0, cube_size)); + cube_W.push_back(Eigen::Vector3d(cube_size, 0, cube_size)); + cube_W.push_back(Eigen::Vector3d(cube_size, cube_size, cube_size)); + cube_W.push_back(Eigen::Vector3d(0, cube_size, cube_size)); + + std::vector poses; + + Eigen::Matrix3d rotation0( + Eigen::AngleAxis(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix() * + Eigen::AngleAxis(-M_PI / 2, Eigen::Vector3d(1, 0, 0)).toRotationMatrix()); + cv::Affine3d pose0(opencv_viz::eigen_to_cv_mat(rotation0), cv::Affine3d::Vec3{4, 0, 0}); + + // Eigen::Matrix3d + // cv::Affine3d pose1( + // opencv_viz::eigen_to_cv_mat(Eigen::AngleAxis(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix() * rotation0), + // Eigen::Vector3d(0, 4, 0)); + // gtsam::PinholeCamera camera1(pose1, *K); + // graph.emplace_shared>(gtsam::Symbol('x', 1), pose1, poseNoise); + // poses.push_back(pose1); + // cameras.push_back(camera1); +} + +int main() { + demo(); +} From d35dcc86272b24acba4ef9cc03e414fe0596bf02 Mon Sep 17 00:00:00 2001 From: Nicolaniello Buono Date: Tue, 4 Feb 2025 01:04:32 -0500 Subject: [PATCH 3/5] progress --- common/geometry/opencv_viz.cc | 55 ++++++++++------------------------- 1 file changed, 16 insertions(+), 39 deletions(-) diff --git a/common/geometry/opencv_viz.cc b/common/geometry/opencv_viz.cc index 1649291e..a7ccc0ae 100644 --- a/common/geometry/opencv_viz.cc +++ b/common/geometry/opencv_viz.cc @@ -1,4 +1,5 @@ #include "common/geometry/opencv_viz.hh" +#include namespace robot::geometry::opencv_viz { template @@ -37,21 +38,28 @@ cv::Vec3d rotation_matrix_to_axis_angle(const cv::Matx33d &R) { } void viz_scene(const std::vector &poses, const std::vector &points) { + // Eigen::Matrix4d test_mat = poses.front().matrix(); + // cv::Mat cv_mat = eigen_to_cv_mat(test_mat); + // std::cout << "viz_scene called! " << std::endl; + // (void)poses; + // (void)points; cv::viz::Viz3d window("Viz Scene"); - constexpr double pose_size = 0.2; - for (unsigned int i = 0; i < poses.size(); i++) { - cv::Affine3d cv_pose( - eigen_to_cv_mat(poses[i].matrix().block(0,0,3,3)) , - cv::Affine3d::Vec3(poses[i].translation()[0], poses[i].translation()[1], poses[i].translation()[3]) - ); + constexpr double pose_size = 2; + for (unsigned int i = 0; i < poses.size(); i++) { + // Eigen::Matrix4d mat = poses[i].matrix(); + // std::cout << "mat " << i << ": " << mat << std::endl; + cv::Affine3d cv_pose(eigen_to_cv_mat(Eigen::Matrix4d(poses[i].matrix()))); + cv_pose = cv_pose.inv(); + // std::string matString = cv::format(cv::Mat(cv_pose.matrix), cv::Formatter::FMT_DEFAULT); + // std::cout << "matString: " << matString << std::endl; window.showWidget( "pose_" + std::to_string(i), cv::viz::WCoordinateSystem(pose_size), cv_pose - ); + ); } - constexpr double point_radius = 0.2; + constexpr double point_radius = 0.08; constexpr int sphere_res = 10; const cv::viz::Color point_color = cv::viz::Color::celestial_blue(); for (unsigned int i = 0; i < points.size(); i++) { @@ -77,37 +85,6 @@ void viz_scene(const std::vector &poses, const std::vector Date: Tue, 4 Feb 2025 20:21:16 -0500 Subject: [PATCH 4/5] MVP complete --- common/geometry/opencv_viz.cc | 84 ++++++------- common/geometry/opencv_viz.hh | 6 +- common/geometry/opencv_viz_example.cc | 34 +++-- common/geometry/opencv_viz_test.cc | 50 ++++---- experimental/learn_descriptors/BUILD | 8 -- experimental/learn_descriptors/opencv_viz.cc | 119 ------------------ .../learn_descriptors/opencv_viz_example.cc | 83 ------------ 7 files changed, 79 insertions(+), 305 deletions(-) delete mode 100644 experimental/learn_descriptors/opencv_viz.cc delete mode 100644 experimental/learn_descriptors/opencv_viz_example.cc diff --git a/common/geometry/opencv_viz.cc b/common/geometry/opencv_viz.cc index a7ccc0ae..48675e23 100644 --- a/common/geometry/opencv_viz.cc +++ b/common/geometry/opencv_viz.cc @@ -1,25 +1,28 @@ #include "common/geometry/opencv_viz.hh" + #include namespace robot::geometry::opencv_viz { template -cv::Mat eigen_to_cv_mat(const Eigen::MatrixBase& eigenMatrix) { +cv::Mat eigen_to_cv_mat(const Eigen::MatrixBase &eigenMatrix) { int cvType; if constexpr (std::is_same_v) { - cvType = CV_64F; // 64-bit floating point + cvType = CV_64F; } else if constexpr (std::is_same_v) { - cvType = CV_32F; // 32-bit floating point + cvType = CV_32F; } else if constexpr (std::is_same_v) { - cvType = CV_32S; // 32-bit signed integer + cvType = CV_32S; } else { static_assert(!std::is_same_v, "Unsupported data type in Eigen matrix"); } - return cv::Mat(eigenMatrix.rows(), eigenMatrix.cols(), cvType, const_cast(static_cast(eigenMatrix.derived().data()))).clone(); + return cv::Mat(eigenMatrix.rows(), eigenMatrix.cols(), cvType, + const_cast(static_cast(eigenMatrix.derived().data()))) + .clone(); } cv::Vec3d rotation_matrix_to_axis_angle(const cv::Matx33d &R) { // Ensure R is a valid rotation matrix CV_Assert(cv::determinant(R) > 0.999 && cv::determinant(R) < 1.001); - + double trace = R(0, 0) + R(1, 1) + R(2, 2); double theta = std::acos((trace - 1) / 2.0); @@ -28,64 +31,49 @@ cv::Vec3d rotation_matrix_to_axis_angle(const cv::Matx33d &R) { } double sinTheta = std::sin(theta); - cv::Vec3d axis( - (R(2, 1) - R(1, 2)) / (2.0 * sinTheta), - (R(0, 2) - R(2, 0)) / (2.0 * sinTheta), - (R(1, 0) - R(0, 1)) / (2.0 * sinTheta) - ); + cv::Vec3d axis((R(2, 1) - R(1, 2)) / (2.0 * sinTheta), (R(0, 2) - R(2, 0)) / (2.0 * sinTheta), + (R(1, 0) - R(0, 1)) / (2.0 * sinTheta)); return axis * theta; } -void viz_scene(const std::vector &poses, const std::vector &points) { - // Eigen::Matrix4d test_mat = poses.front().matrix(); - // cv::Mat cv_mat = eigen_to_cv_mat(test_mat); - // std::cout << "viz_scene called! " << std::endl; - // (void)poses; - // (void)points; +void viz_scene(const std::vector &poses, + const std::vector &points, const bool show_grid) { cv::viz::Viz3d window("Viz Scene"); - constexpr double pose_size = 2; - for (unsigned int i = 0; i < poses.size(); i++) { - // Eigen::Matrix4d mat = poses[i].matrix(); - // std::cout << "mat " << i << ": " << mat << std::endl; - cv::Affine3d cv_pose(eigen_to_cv_mat(Eigen::Matrix4d(poses[i].matrix()))); - cv_pose = cv_pose.inv(); - // std::string matString = cv::format(cv::Mat(cv_pose.matrix), cv::Formatter::FMT_DEFAULT); - // std::cout << "matString: " << matString << std::endl; - window.showWidget( - "pose_" + std::to_string(i), - cv::viz::WCoordinateSystem(pose_size), - cv_pose - ); + constexpr double pose_size = .5; + for (unsigned int i = 0; i < poses.size(); i++) { + cv::Affine3d cv_pose(eigen_to_cv_mat(Eigen::Matrix4d(poses[i].matrix().transpose()))); + window.showWidget("rigid_transform_" + std::to_string(i), + cv::viz::WCoordinateSystem(pose_size), cv_pose); } constexpr double point_radius = 0.08; constexpr int sphere_res = 10; const cv::viz::Color point_color = cv::viz::Color::celestial_blue(); for (unsigned int i = 0; i < points.size(); i++) { const Eigen::Vector3d &point = points[i]; - window.showWidget( - "translation_" + std::to_string(i), - cv::viz::WSphere( - cv::Point3d(point[0], point[1], point[2]), - point_radius, - sphere_res, - point_color - ) - ); + window.showWidget("point_" + std::to_string(i), + cv::viz::WSphere(cv::Point3d(point[0], point[1], point[2]), point_radius, + sphere_res, point_color)); } window.showWidget("world_frame", cv::viz::WCoordinateSystem()); - constexpr unsigned int num_cells = 6; - constexpr double units = 1.0; - const cv::viz::Color color = cv::viz::Color::white(); - const cv::Vec2i cells = cv::Vec2i::all(num_cells); - const cv::Vec2d cell_sizes = cv::Vec2d::all(units); - window.showWidget("xy_grid", cv::viz::WGrid(cells, cell_sizes, color)); - window.showWidget("yz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(1,0,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); - window.showWidget("xz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(0,1,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); + if (show_grid) { + constexpr unsigned int num_cells = 6; + constexpr double units = 1.0; + const cv::viz::Color color = cv::viz::Color::gray(); + const cv::Vec2i cells = cv::Vec2i::all(num_cells); + const cv::Vec2d cell_sizes = cv::Vec2d::all(units); + window.showWidget("xy_grid", cv::viz::WGrid(cells, cell_sizes, color)); + window.showWidget("yz_grid", + cv::viz::WGrid(cv::Point3d(0., 0., 0.), cv::Point3d(1, 0, 0), + cv::Point3d(0, 0, 1), cells, cell_sizes, color)); + window.showWidget("xz_grid", + cv::viz::WGrid(cv::Point3d(0., 0., 0.), cv::Point3d(0, 1, 0), + cv::Point3d(0, 0, 1), cells, cell_sizes, color)); + } window.spin(); } -} +} // namespace robot::geometry::opencv_viz diff --git a/common/geometry/opencv_viz.hh b/common/geometry/opencv_viz.hh index a64d1d8d..d5530ece 100644 --- a/common/geometry/opencv_viz.hh +++ b/common/geometry/opencv_viz.hh @@ -1,9 +1,11 @@ #pragma once #include -#include "opencv2/viz.hpp" + #include "Eigen/Core" #include "Eigen/Geometry" +#include "opencv2/viz.hpp" namespace robot::geometry::opencv_viz { -void viz_scene(const std::vector &poses, const std::vector &points); +void viz_scene(const std::vector &poses, + const std::vector &points, const bool show_grid = true); } \ No newline at end of file diff --git a/common/geometry/opencv_viz_example.cc b/common/geometry/opencv_viz_example.cc index 614b8292..6a045b0b 100644 --- a/common/geometry/opencv_viz_example.cc +++ b/common/geometry/opencv_viz_example.cc @@ -1,7 +1,7 @@ #include "opencv2/viz.hpp" void demo() { - cv::viz::Viz3d window("My Window"); + cv::viz::Viz3d window("My Window"); window.showWidget("world_frame", cv::viz::WCoordinateSystem()); @@ -11,8 +11,10 @@ void demo() { const cv::Vec2i cells = cv::Vec2i::all(num_cells); const cv::Vec2d cell_sizes = cv::Vec2d::all(units); window.showWidget("xy_grid", cv::viz::WGrid(cells, cell_sizes, color)); - window.showWidget("yz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(1,0,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); - window.showWidget("xz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(0,1,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); + window.showWidget("yz_grid", cv::viz::WGrid(cv::Point3d(0., 0., 0.), cv::Point3d(1, 0, 0), + cv::Point3d(0, 0, 1), cells, cell_sizes, color)); + window.showWidget("xz_grid", cv::viz::WGrid(cv::Point3d(0., 0., 0.), cv::Point3d(0, 1, 0), + cv::Point3d(0, 0, 1), cells, cell_sizes, color)); window.showWidget("text_overlay", cv::viz::WText("hello world overlay!", cv::Point(20, 50))); constexpr bool ALWAYS_FACE_CAMERA = true; constexpr double TEXT_SCALE = 0.1; @@ -20,33 +22,25 @@ void demo() { TEXT_SCALE, ALWAYS_FACE_CAMERA)); constexpr bool FIXED_TEXT = false; - cv::Affine3d world_from_fixed_text( - cv::Affine3d::Vec3{M_PI_2, 0.0, 0.0}, // rotation - {0.2, 0.4, 0.6} // translation + cv::Affine3d world_from_fixed_text(cv::Affine3d::Vec3{M_PI_2, 0.0, 0.0}, // rotation + {0.2, 0.4, 0.6} // translation ); window.showWidget( "text_3d_fixed", cv::viz::WText3D("hello world fixed!", cv::Point3d(0.0, 0.0, 0.0), TEXT_SCALE, FIXED_TEXT), - world_from_fixed_text - ); + world_from_fixed_text); constexpr double COORD_SCALE = 0.2; - window.showWidget("text_3d_fixed_frame", cv::viz::WCoordinateSystem(COORD_SCALE), world_from_fixed_text); + window.showWidget("text_3d_fixed_frame", cv::viz::WCoordinateSystem(COORD_SCALE), + world_from_fixed_text); constexpr double CIRCLE_RADIUS_M = 0.5; - cv::Affine3d world_from_circle( - cv::Affine3d::Vec3{0.0, 0.0, 0.0}, // rotation - {0.0, 0.0, 1.5} // translation - ); - window.showWidget( - "circle", - cv::viz::WCircle(CIRCLE_RADIUS_M), - world_from_circle + cv::Affine3d world_from_circle(cv::Affine3d::Vec3{0.0, 0.0, 0.0}, // rotation + {0.0, 0.0, 1.5} // translation ); + window.showWidget("circle", cv::viz::WCircle(CIRCLE_RADIUS_M), world_from_circle); window.spin(); } -int main() { - demo(); -} +int main() { demo(); } diff --git a/common/geometry/opencv_viz_test.cc b/common/geometry/opencv_viz_test.cc index bbe872e4..e5e8265a 100644 --- a/common/geometry/opencv_viz_test.cc +++ b/common/geometry/opencv_viz_test.cc @@ -1,10 +1,13 @@ #include "common/geometry/opencv_viz.hh" + #include "gtest/gtest.h" namespace robot::geometry::opencv_viz { namespace { -bool is_test() { return std::getenv("BAZEL_TEST") != nullptr && - std::getenv("BUILD_WORKSPACE_DIRECTORY") == nullptr; } +bool is_test() { + return std::getenv("BAZEL_TEST") != nullptr && + std::getenv("BUILD_WORKSPACE_DIRECTORY") == nullptr; +} } // namespace TEST(OpencvVizTest, demo) { @@ -18,8 +21,10 @@ TEST(OpencvVizTest, demo) { const cv::Vec2i cells = cv::Vec2i::all(num_cells); const cv::Vec2d cell_sizes = cv::Vec2d::all(units); window.showWidget("xy_grid", cv::viz::WGrid(cells, cell_sizes, color)); - window.showWidget("yz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(1,0,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); - window.showWidget("xz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(0,1,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); + window.showWidget("yz_grid", cv::viz::WGrid(cv::Point3d(0., 0., 0.), cv::Point3d(1, 0, 0), + cv::Point3d(0, 0, 1), cells, cell_sizes, color)); + window.showWidget("xz_grid", cv::viz::WGrid(cv::Point3d(0., 0., 0.), cv::Point3d(0, 1, 0), + cv::Point3d(0, 0, 1), cells, cell_sizes, color)); window.showWidget("text_overlay", cv::viz::WText("hello world overlay!", cv::Point(20, 50))); constexpr bool ALWAYS_FACE_CAMERA = true; constexpr double TEXT_SCALE = 0.1; @@ -27,32 +32,26 @@ TEST(OpencvVizTest, demo) { TEXT_SCALE, ALWAYS_FACE_CAMERA)); constexpr bool FIXED_TEXT = false; - cv::Affine3d world_from_fixed_text( - cv::Affine3d::Vec3{M_PI_2, 0.0, 0.0}, // rotation - {0.2, 0.4, 0.6} // translation + cv::Affine3d world_to_fixed_text(cv::Affine3d::Vec3{M_PI_2, 0.0, 0.0}, // rotation + {0.2, 0.4, 0.6} // translation ); window.showWidget( "text_3d_fixed", cv::viz::WText3D("hello world fixed!", cv::Point3d(0.0, 0.0, 0.0), TEXT_SCALE, FIXED_TEXT), - world_from_fixed_text - ); + world_to_fixed_text); constexpr double COORD_SCALE = 0.2; - window.showWidget("text_3d_fixed_frame", cv::viz::WCoordinateSystem(COORD_SCALE), world_from_fixed_text); + window.showWidget("text_3d_fixed_frame", cv::viz::WCoordinateSystem(COORD_SCALE), + world_to_fixed_text); constexpr double CIRCLE_RADIUS_M = 0.5; - cv::Affine3d world_from_circle( - cv::Affine3d::Vec3{0.0, 0.0, 0.0}, // rotation - {0.0, 0.0, 1.5} // translation - ); - window.showWidget( - "circle", - cv::viz::WCircle(CIRCLE_RADIUS_M), - world_from_circle + cv::Affine3d world_to_circle(cv::Affine3d::Vec3{0.0, 0.0, 0.0}, // rotation + {0.0, 0.0, 1.5} // translation ); + window.showWidget("circle", cv::viz::WCircle(CIRCLE_RADIUS_M), world_to_circle); if (!is_test()) { - window.spin(); - } + window.spin(); + } } TEST(OpencvVizTest, cube_test) { @@ -73,17 +72,18 @@ TEST(OpencvVizTest, cube_test) { Eigen::AngleAxis(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix() * Eigen::AngleAxis(-M_PI / 2, Eigen::Vector3d(1, 0, 0)).toRotationMatrix()); Eigen::Isometry3d pose0; - pose0.translation() = Eigen::Vector3d(4,0,0); + pose0.translation() = Eigen::Vector3d(4, 0, 0); pose0.linear() = rotation0; poses.push_back(pose0); - + Eigen::Isometry3d pose1; - pose1.linear() = Eigen::AngleAxis(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix() * rotation0; - pose1.translation() = Eigen::Vector3d(0,4,0); + pose1.linear() = + Eigen::AngleAxis(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix() * rotation0; + pose1.translation() = Eigen::Vector3d(0, 4, 0); poses.push_back(pose1); if (!is_test()) { viz_scene(poses, cube_W); } } -} \ No newline at end of file +} // namespace robot::geometry::opencv_viz \ No newline at end of file diff --git a/experimental/learn_descriptors/BUILD b/experimental/learn_descriptors/BUILD index 9728193b..141f869a 100644 --- a/experimental/learn_descriptors/BUILD +++ b/experimental/learn_descriptors/BUILD @@ -76,11 +76,3 @@ cc_binary( "@opencv//:opencv" ] ) - -cc_binary( - name = "opencv_viz_test", - srcs = ["opencv_viz_example.cc"], - deps = [ - "@opencv//:opencv" - ] -) diff --git a/experimental/learn_descriptors/opencv_viz.cc b/experimental/learn_descriptors/opencv_viz.cc deleted file mode 100644 index 0e6d5102..00000000 --- a/experimental/learn_descriptors/opencv_viz.cc +++ /dev/null @@ -1,119 +0,0 @@ -#include - -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Geometry" -#include "opencv2/viz.hpp" - -namespace opencv_viz { -template -cv::Mat eigen_to_cv_mat(const Eigen::MatrixBase& eigenMatrix) { - int cvType; - if constexpr (std::is_same_v) { - cvType = CV_64F; // 64-bit floating point - } else if constexpr (std::is_same_v) { - cvType = CV_32F; // 32-bit floating point - } else if constexpr (std::is_same_v) { - cvType = CV_32S; // 32-bit signed integer - } else { - static_assert(!std::is_same_v, "Unsupported data type in Eigen matrix"); - } - return cv::Mat(eigenMatrix.rows(), eigenMatrix.cols(), cvType, const_cast(static_cast(eigenMatrix.derived().data()))).clone(); -} -cv::Vec3d rotation_matrix_to_axis_angle(const cv::Matx33d &R) { - // Ensure R is a valid rotation matrix - CV_Assert(cv::determinant(R) > 0.999 && cv::determinant(R) < 1.001); - - double trace = R(0, 0) + R(1, 1) + R(2, 2); - double theta = std::acos((trace - 1) / 2.0); - - if (std::abs(theta) < 1e-6) { - return cv::Vec3d(0, 0, 0); - } - - double sinTheta = std::sin(theta); - cv::Vec3d axis( - (R(2, 1) - R(1, 2)) / (2.0 * sinTheta), - (R(0, 2) - R(2, 0)) / (2.0 * sinTheta), - (R(1, 0) - R(0, 1)) / (2.0 * sinTheta) - ); - - return axis * theta; -} -void viz_scene(const std::vector &poses, const std::vector &points) { - cv::viz::Viz3d window("Viz Scene"); - - std::vector cv_poses; - constexpr double pose_size = 0.2; - for (unsigned int i = 0; i < poses.size(); i++) { - cv::Affine3d cv_pose( - eigen_to_cv_mat(poses[i].matrix().block(0,0,3,3)) , - cv::Affine3d::Vec3(poses[i].translation()[0], poses[i].translation()[1], poses[i].translation()[3]) - ); - window.showWidget( - "pose_" + std::to_string(i), - cv::viz::WCoordinateSystem(pose_size), - cv_pose - ); - } - constexpr double point_radius = 0.2; - constexpr int sphere_res = 10; - const cv::viz::Color point_color = cv::viz::Color::celestial_blue(); - for (unsigned int i = 0; i < points.size(); i++) { - const Eigen::Vector3d &point = points[i]; - window.showWidget( - "translation_" + std::to_string(i), - cv::viz::WSphere( - cv::Point3d(point[0], point[1], point[2]), - point_radius, - sphere_res, - point_color - ) - ); - } - - window.showWidget("world_frame", cv::viz::WCoordinateSystem()); - - constexpr unsigned int num_cells = 6; - constexpr double units = 1.0; - const cv::viz::Color color = cv::viz::Color::white(); - const cv::Vec2i cells = cv::Vec2i::all(num_cells); - const cv::Vec2d cell_sizes = cv::Vec2d::all(units); - window.showWidget("xy_grid", cv::viz::WGrid(cells, cell_sizes, color)); - window.showWidget("yz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(1,0,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); - window.showWidget("xz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(0,1,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); - window.showWidget("text_overlay", cv::viz::WText("hello world overlay!", cv::Point(20, 50))); - - constexpr bool ALWAYS_FACE_CAMERA = true; - constexpr double TEXT_SCALE = 0.1; - window.showWidget("text_3d", cv::viz::WText3D("hello world 3d!", cv::Point3d(0.1, 0.2, 0.3), - TEXT_SCALE, ALWAYS_FACE_CAMERA)); - - constexpr bool FIXED_TEXT = false; - cv::Affine3d world_from_fixed_text( - cv::Affine3d::Vec3{M_PI_2, 0.0, 0.0}, // rotation - {0.2, 0.4, 0.6} // translation - ); - - window.showWidget( - "text_3d_fixed", - cv::viz::WText3D("hello world fixed!", cv::Point3d(0.0, 0.0, 0.0), TEXT_SCALE, FIXED_TEXT), - world_from_fixed_text - ); - constexpr double COORD_SCALE = 0.2; - window.showWidget("text_3d_fixed_frame", cv::viz::WCoordinateSystem(COORD_SCALE), world_from_fixed_text); - - constexpr double CIRCLE_RADIUS_M = 0.5; - cv::Affine3d world_from_circle( - cv::Affine3d::Vec3{0.0, 0.0, 0.0}, // rotation - {0.0, 0.0, 1.5} // translation - ); - window.showWidget( - "circle", - cv::viz::WCircle(CIRCLE_RADIUS_M), - world_from_circle - ); - - window.spin(); -} -} - diff --git a/experimental/learn_descriptors/opencv_viz_example.cc b/experimental/learn_descriptors/opencv_viz_example.cc deleted file mode 100644 index c37c3ba4..00000000 --- a/experimental/learn_descriptors/opencv_viz_example.cc +++ /dev/null @@ -1,83 +0,0 @@ -#include "opencv2/viz.hpp" -#include "opencv_viz.cc" -#include "eigen/Eigen/Geometry" - -void demo() { - cv::viz::Viz3d window("My Window"); - - window.showWidget("world_frame", cv::viz::WCoordinateSystem()); - - constexpr unsigned int num_cells = 6; - constexpr double units = 1.0; - const cv::viz::Color color = cv::viz::Color::white(); - const cv::Vec2i cells = cv::Vec2i::all(num_cells); - const cv::Vec2d cell_sizes = cv::Vec2d::all(units); - window.showWidget("xy_grid", cv::viz::WGrid(cells, cell_sizes, color)); - window.showWidget("yz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(1,0,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); - window.showWidget("xz_grid", cv::viz::WGrid(cv::Point3d(0.,0.,0.), cv::Point3d(0,1,0), cv::Point3d(0,0,1), cells, cell_sizes, color)); - window.showWidget("text_overlay", cv::viz::WText("hello world overlay!", cv::Point(20, 50))); - constexpr bool ALWAYS_FACE_CAMERA = true; - constexpr double TEXT_SCALE = 0.1; - window.showWidget("text_3d", cv::viz::WText3D("hello world 3d!", cv::Point3d(0.1, 0.2, 0.3), - TEXT_SCALE, ALWAYS_FACE_CAMERA)); - - constexpr bool FIXED_TEXT = false; - cv::Affine3d world_from_fixed_text( - cv::Affine3d::Vec3{M_PI_2, 0.0, 0.0}, // rotation - {0.2, 0.4, 0.6} // translation - ); - - window.showWidget( - "text_3d_fixed", - cv::viz::WText3D("hello world fixed!", cv::Point3d(0.0, 0.0, 0.0), TEXT_SCALE, FIXED_TEXT), - world_from_fixed_text - ); - constexpr double COORD_SCALE = 0.2; - window.showWidget("text_3d_fixed_frame", cv::viz::WCoordinateSystem(COORD_SCALE), world_from_fixed_text); - - constexpr double CIRCLE_RADIUS_M = 0.5; - cv::Affine3d world_from_circle( - cv::Affine3d::Vec3{0.0, 0.0, 0.0}, // rotation - {0.0, 0.0, 1.5} // translation - ); - window.showWidget( - "circle", - cv::viz::WCircle(CIRCLE_RADIUS_M), - world_from_circle - ); - - window.spin(); -} - -void demo_cube() { - std::vector cube_W; - float cube_size = 1.0f; - cube_W.push_back(Eigen::Vector3d(0, 0, 0)); - cube_W.push_back(Eigen::Vector3d(cube_size, 0, 0)); - cube_W.push_back(Eigen::Vector3d(cube_size, cube_size, 0)); - cube_W.push_back(Eigen::Vector3d(0, cube_size, 0)); - cube_W.push_back(Eigen::Vector3d(0, 0, cube_size)); - cube_W.push_back(Eigen::Vector3d(cube_size, 0, cube_size)); - cube_W.push_back(Eigen::Vector3d(cube_size, cube_size, cube_size)); - cube_W.push_back(Eigen::Vector3d(0, cube_size, cube_size)); - - std::vector poses; - - Eigen::Matrix3d rotation0( - Eigen::AngleAxis(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix() * - Eigen::AngleAxis(-M_PI / 2, Eigen::Vector3d(1, 0, 0)).toRotationMatrix()); - cv::Affine3d pose0(opencv_viz::eigen_to_cv_mat(rotation0), cv::Affine3d::Vec3{4, 0, 0}); - - // Eigen::Matrix3d - // cv::Affine3d pose1( - // opencv_viz::eigen_to_cv_mat(Eigen::AngleAxis(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix() * rotation0), - // Eigen::Vector3d(0, 4, 0)); - // gtsam::PinholeCamera camera1(pose1, *K); - // graph.emplace_shared>(gtsam::Symbol('x', 1), pose1, poseNoise); - // poses.push_back(pose1); - // cameras.push_back(camera1); -} - -int main() { - demo(); -} From 270376dc3b18a0e56bd006cd3e9ab2d558e28163 Mon Sep 17 00:00:00 2001 From: Nicolaniello Buono Date: Tue, 4 Feb 2025 20:25:47 -0500 Subject: [PATCH 5/5] clean up experimental/learn_descriptors/BUILD --- experimental/learn_descriptors/BUILD | 8 -------- 1 file changed, 8 deletions(-) diff --git a/experimental/learn_descriptors/BUILD b/experimental/learn_descriptors/BUILD index 141f869a..e0f417bd 100644 --- a/experimental/learn_descriptors/BUILD +++ b/experimental/learn_descriptors/BUILD @@ -68,11 +68,3 @@ cc_test( ":learn_descriptors" ] ) - -cc_binary( - name = "opencv_viz_example", - srcs = ["opencv_viz_example.cc"], - deps = [ - "@opencv//:opencv" - ] -)