Skip to content

Fix issue of mesh oriented bbox computation #31

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
zz990099 merged 3 commits into from
Sep 17, 2025
Merged

Fix issue of mesh oriented bbox computation #31

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7797d94
fix[assimp_mesh_loader]: fix obb computation in assimp mesh loader
zz990099 Sep 17, 2025
75906a2
improve[simple_tests]: Add axies drawing in draw3DBoundingBox function
zz990099 Sep 17, 2025
8b39573
fix precommit
zz990099 Sep 17, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
29 changes: 17 additions & 12 deletions detection_6d_foundationpose/src/mesh_loader/assimp_mesh_loader.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,28 +88,33 @@ static void ComputeOBB(const aiMesh *mesh,

// 特征分解
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> 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;
}

Expand Down
46 changes: 42 additions & 4 deletions simple_tests/include/tests/help_func.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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];
Expand Down Expand Up @@ -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<Eigen::Vector4f> axis_end_points = {x_end_world, y_end_world, z_end_world};
std::vector<cv::Scalar> 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)
Expand Down