I'm trying to get the pose (translation, rotation) of the (closest to origin on x then z) left point (furthest on y from origin) in OrientedBoundingBox, my problem is the bounding box in different runs have different rotations, as defined by Open3D I was expecting the following cube:
/// ------- x
/// /|
/// / |
/// / | z
/// y
/// 0 ------------------- 1
/// /| /|
/// / | / |
/// / | / |
/// / | / |
/// 2 ------------------- 7 |
/// | |____________|____| 6
/// | /3 | /
/// | / | /
/// | / | /
/// |/ |/
/// 5 ------------------- 4
so my target was Point 2, but getting the point from GetBoxPoints() with rotations as normal vectors gave me different poses! Can you please tell me how can I get the desired pose? thanks in advance.
open3d::geometry::OrientedBoundingBox bbox_ws
std::vector<Eigen::Vector3d> points = bbox_ws.GetBoxPoints();
Eigen::Vector3d P2 = points[2];
for (const auto& point : points) {
if (point.x() < P2.x() && point.y() > P2.y() && point.z() < P2.z()) {
P2 = point;
}
}
Eigen::Vector3d x_axis = points[7] - P2; // P2P7
Eigen::Vector3d y_axis = points[0] - P2; // P2P0
Eigen::Vector3d z_axis = points[5] - P2; // P2P5
x_axis.normalize();
y_axis.normalize();
z_axis.normalize();
Eigen::Matrix3d rotation_matrix;
rotation_matrix.col(0) = x_axis;
rotation_matrix.col(1) = y_axis;
rotation_matrix.col(2) = z_axis;
According to the extracted values, P5 is my desired point
point number 0 Has values: 1.20885 0.52103 0.172856
point number 1 Has values: 0.939225 -0.536148 0.205281
point number 2 Has values: 1.21036 0.52761 0.399916
point number 3 Has values: 0.810032 0.622739 0.17256
point number 4 Has values: 0.541912 -0.42786 0.432043
point number 5 Has values: 0.811541 0.629318 0.399619
point number 6 Has values: 0.540403 -0.434439 0.204984
point number 7 Has values: 0.940734 -0.529568 0.43234
## Here are other values:
0.945674 -0.514864 0.407497
1.18082 0.423109 0.415139
0.945228 -0.513155 0.211485
0.546889 -0.414905 0.409276
0.781587 0.524777 0.220907
0.546443 -0.413196 0.213265
0.782034 0.523068 0.416919
1.18037 0.424818 0.219128
1.21301 0.515485 0.172203
0.945833 -0.534461 0.226136
1.21074 0.527857 0.401783
0.811611 0.617145 0.162747
0.542156 -0.42043 0.446261
0.809336 0.629517 0.392328
0.54443 -0.432802 0.21668
0.943558 -0.52209 0.455717
Simple stupid solution:
std::vector<Eigen::Vector3d> sortBoxPoints(std::vector<Eigen::Vector3d>& points, bool verbose){
/// 4 ------------------- 6
/// /. /|
/// / . / |
/// / . / |
/// / . / |
/// 5 ------------------- 3 |
/// | . . . . . . .| . .| 1
/// | .7 | /
/// | . | /
/// | . | /
/// |. |/
/// 2 ------------------- 0
///
/// z x
/// | /
/// | /
/// |/
/// y -------
std::vector<Eigen::Vector3d> sorted_points(8);
/**
* ---------- Z ------------------- min
*/
// Sort the vector based on z-values
std::sort(points.begin(), points.end(), [](const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
return a(2) > b(2); // Sort in descending order based on z-values
});
// Get the first half of the sorted vector
size_t halfSize = points.size() / 2;
/** 3, 4, 5, 6 */
std::vector<Eigen::Vector3d> ZP_(points.begin(), points.begin() + halfSize);
/** 0, 1, 2, 7 */
std::vector<Eigen::Vector3d> ZM_ (points.begin() + halfSize, points.end());
// Sort the vector based on x-values
std::sort(ZP_.begin(), ZP_.end(), [](const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
return a(0) > b(0); // Sort in descending order based on z-values
});
std::sort(ZM_.begin(), ZM_.end(), [](const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
return a(0) > b(0); // Sort in descending order based on z-values
});
/**
* ---------- X ------------------- min
*/
size_t quarterSize = points.size() / 4;
/** 4, 6 */
std::vector<Eigen::Vector3d> ZP_XP (ZP_.begin(), ZP_.begin() + quarterSize);
/** 5, 3 */
std::vector<Eigen::Vector3d> ZP_XM (ZP_.begin() + quarterSize, ZP_.end());
/** 7, 1 */
std::vector<Eigen::Vector3d> ZM_XP (ZM_.begin(), ZM_.begin() + quarterSize);
/** 2, 0 */
std::vector<Eigen::Vector3d> ZM_XM (ZM_.begin() + quarterSize, ZM_.end());
/**
* ---------- Y ------------------- max
*/
if(ZP_XP[0](1) > ZP_XP[1](1)){
sorted_points[4] = ZP_XP[0];
sorted_points[6] = ZP_XP[1];
}
else{
sorted_points[4] = ZP_XP[1];
sorted_points[6] = ZP_XP[0];
}
if(ZP_XM[0](1) > ZP_XM[1](1)){
sorted_points[5] = ZP_XM[0];
sorted_points[3] = ZP_XM[1];
}
else{
sorted_points[5] = ZP_XM[1];
sorted_points[3] = ZP_XM[0];
}
if(ZM_XP[0](1) > ZM_XP[1](1)){
sorted_points[7] = ZM_XP[0];
sorted_points[1] = ZM_XP[1];
}
else{
sorted_points[7] = ZM_XP[1];
sorted_points[1] = ZM_XP[0];
}
if(ZM_XM[0](1) > ZM_XM[1](1)){
sorted_points[2] = ZM_XM[0];
sorted_points[0] = ZM_XM[1];
}
else{
sorted_points[2] = ZM_XM[1];
sorted_points[0] = ZM_XM[0];
}
if(verbose){
int count = 0;
for (const auto& point : sorted_points) {
std::cout << "Point " << count << " values: "<< point.transpose() << std::endl;
count++;
}
}
return sorted_points;
}