From 7797d940a52ea850947758887cd7d9f3a54ac548 Mon Sep 17 00:00:00 2001 From: zz990099 <771647586@qq.com> Date: Thu, 18 Sep 2025 01:16:26 +0800 Subject: [PATCH 1/3] fix[assimp_mesh_loader]: fix obb computation in assimp mesh loader Signed-off-by: zz990099 <771647586@qq.com> --- .../src/mesh_loader/assimp_mesh_loader.cpp | 33 +++++++++++-------- 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/detection_6d_foundationpose/src/mesh_loader/assimp_mesh_loader.cpp b/detection_6d_foundationpose/src/mesh_loader/assimp_mesh_loader.cpp index 8f9e86b..fefe2b7 100644 --- a/detection_6d_foundationpose/src/mesh_loader/assimp_mesh_loader.cpp +++ b/detection_6d_foundationpose/src/mesh_loader/assimp_mesh_loader.cpp @@ -88,28 +88,33 @@ static void ComputeOBB(const aiMesh *mesh, // 特征分解 Eigen::SelfAdjointEigenSolver solver(cov); - Eigen::Matrix3f rotation = solver.eigenvectors(); - Eigen::Vector3f extents = solver.eigenvalues().cwiseSqrt(); - // 生成变换矩阵 - Eigen::Matrix4f transform = Eigen::Matrix4f::Identity(); - transform.block<3, 3>(0, 0) = rotation; - transform.block<3, 1>(0, 3) = mean; - + Eigen::Matrix3f rotation = solver.eigenvectors(); + Eigen::Vector3f extents = solver.eigenvalues().cwiseSqrt(); + + // 将顶点变换到旋转后的坐标系 Eigen::MatrixXf transformed(vertices.size(), 3); for (int i = 0; i < vertices.size(); ++i) { - Eigen::Vector3f proj = rotation.transpose() * vertices[i]; - transformed(i, 0) = proj(0); - transformed(i, 1) = proj(1); - transformed(i, 2) = proj(2); + Eigen::Vector3f proj = rotation.transpose() * (vertices[i] - mean); + transformed(i, 0) = proj(0); + transformed(i, 1) = proj(1); + transformed(i, 2) = proj(2); } + // 计算边界框 Eigen::Vector3f minBound = transformed.colwise().minCoeff(); Eigen::Vector3f maxBound = transformed.colwise().maxCoeff(); - Eigen::Vector3f dimension = maxBound - minBound; - - out_orient_bbox = transform; + + // 计算OBB中心（在旋转后的坐标系中） + Eigen::Vector3f center = (minBound + maxBound) / 2.0f; + + // 构建变换矩阵：从OBB坐标系到模型坐标系 + Eigen::Matrix4f to_origin = Eigen::Matrix4f::Identity(); + to_origin.block<3, 3>(0, 0) = rotation; + to_origin.block<3, 1>(0, 3) = rotation * center + mean; + + out_orient_bbox = to_origin; out_dimension = dimension; } From 75906a2dcb8c1c31d2e3b3c5859d49ec415bac9e Mon Sep 17 00:00:00 2001 From: zz990099 <771647586@qq.com> Date: Thu, 18 Sep 2025 01:17:39 +0800 Subject: [PATCH 2/3] improve[simple_tests]: Add axies drawing in draw3DBoundingBox function Signed-off-by: zz990099 <771647586@qq.com> --- simple_tests/include/tests/help_func.hpp | 46 +++++++++++++++++++++--- 1 file changed, 42 insertions(+), 4 deletions(-) diff --git a/simple_tests/include/tests/help_func.hpp b/simple_tests/include/tests/help_func.hpp index 6bd7018..50772fa 100644 --- a/simple_tests/include/tests/help_func.hpp +++ b/simple_tests/include/tests/help_func.hpp @@ -64,8 +64,10 @@ inline void draw3DBoundingBox(const Eigen::Matrix3f &intrinsic, float h = dimension(2) / 2; // 目标的八个顶点在物体坐标系中的位置 - Eigen::Vector3f points[8] = {{-l, -w, h}, {l, -w, h}, {l, w, h}, {-l, w, h}, - {-l, -w, -h}, {l, -w, -h}, {l, w, -h}, {-l, w, -h}}; + Eigen::Vector3f points[8] = { + {-l, -w, h}, {l, -w, h}, {l, w, h}, {-l, w, h}, + {-l, -w, -h}, {l, -w, -h}, {l, w, -h}, {-l, w, -h} + }; // 变换到世界坐标系 Eigen::Vector4f transformed_points[8]; @@ -99,10 +101,46 @@ inline void draw3DBoundingBox(const Eigen::Matrix3f &intrinsic, { if (edge.first < image_points.size() && edge.second < image_points.size()) { - cv::line(image, image_points[edge.first], image_points[edge.second], cv::Scalar(0, 255, 0), - 2); // 绿色边框 + cv::line(image, image_points[edge.first], image_points[edge.second], cv::Scalar(0, 255, 0), 2); // 绿色边框 } } + + // 计算物体中心在世界坐标系中的位置 + Eigen::Vector4f center_obj(0, 0, 0, 1); // 物体坐标系中心原点 + Eigen::Vector4f center_world = pose * center_obj; + + // 定义三个轴的终点 + float axis_length = (dimension(0) + dimension(1) + dimension(2)) / 6; + Eigen::Vector4f x_end_obj(axis_length, 0, 0, 1); + Eigen::Vector4f y_end_obj(0, axis_length, 0, 1); + Eigen::Vector4f z_end_obj(0, 0, axis_length, 1); + + Eigen::Vector4f x_end_world = pose * x_end_obj; + Eigen::Vector4f y_end_world = pose * y_end_obj; + Eigen::Vector4f z_end_world = pose * z_end_obj; + + std::vector axis_end_points = {x_end_world, y_end_world, z_end_world}; + std::vector axis_colors = { + cv::Scalar(0, 0, 255), // X轴: 红色 + cv::Scalar(0, 255, 0), // Y轴: 绿色 + cv::Scalar(255, 0, 0) // Z轴: 蓝色 + }; + + float cx = center_world(0) / center_world(2); + float cy = center_world(1) / center_world(2); + float cu = intrinsic(0, 0) * cx + intrinsic(0, 2); + float cv = intrinsic(1, 1) * cy + intrinsic(1, 2); + cv::Point center_pt(cu, cv); + + for (int k = 0; k < 3; ++k) + { + float ex = axis_end_points[k](0) / axis_end_points[k](2); + float ey = axis_end_points[k](1) / axis_end_points[k](2); + float eu = intrinsic(0, 0) * ex + intrinsic(0, 2); + float ev = intrinsic(1, 1) * ey + intrinsic(1, 2); + + cv::line(image, center_pt, cv::Point(eu, ev), axis_colors[k], 3); + } } inline Eigen::Matrix3f ReadCamK(const std::string &cam_K_path) From 8b39573f7f28e60fada8a83ecf757b5b58e8adbd Mon Sep 17 00:00:00 2001 From: zz990099 <771647586@qq.com> Date: Thu, 18 Sep 2025 01:53:32 +0800 Subject: [PATCH 3/3] fix precommit Signed-off-by: zz990099 <771647586@qq.com> --- .../src/mesh_loader/assimp_mesh_loader.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/detection_6d_foundationpose/src/mesh_loader/assimp_mesh_loader.cpp b/detection_6d_foundationpose/src/mesh_loader/assimp_mesh_loader.cpp index fefe2b7..9fb25f2 100644 --- a/detection_6d_foundationpose/src/mesh_loader/assimp_mesh_loader.cpp +++ b/detection_6d_foundationpose/src/mesh_loader/assimp_mesh_loader.cpp @@ -90,7 +90,7 @@ static void ComputeOBB(const aiMesh *mesh, Eigen::SelfAdjointEigenSolver solver(cov); Eigen::Matrix3f rotation = solver.eigenvectors(); Eigen::Vector3f extents = solver.eigenvalues().cwiseSqrt(); - + // 将顶点变换到旋转后的坐标系 Eigen::MatrixXf transformed(vertices.size(), 3); for (int i = 0; i < vertices.size(); ++i) @@ -105,10 +105,10 @@ static void ComputeOBB(const aiMesh *mesh, Eigen::Vector3f minBound = transformed.colwise().minCoeff(); Eigen::Vector3f maxBound = transformed.colwise().maxCoeff(); Eigen::Vector3f dimension = maxBound - minBound; - + // 计算OBB中心（在旋转后的坐标系中） Eigen::Vector3f center = (minBound + maxBound) / 2.0f; - + // 构建变换矩阵：从OBB坐标系到模型坐标系 Eigen::Matrix4f to_origin = Eigen::Matrix4f::Identity(); to_origin.block<3, 3>(0, 0) = rotation;