如何获取OrientedBoundingBox中最近的左下点

Question

我正在尝试获取OrientedBoundingBox中（在x上最接近原点，然后是z）左点（

在y上距原点最远

）的姿势（平移，旋转），我的问题是边界框不同的运行有不同的旋转，正如 Open3D 定义的那样，我期待以下立方体：

///      ------- x
///     /|
///    / |
///   /  | z
///  y
///      0 ------------------- 1
///       /|                /|
///      / |               / |
///     /  |              /  |
///    /   |             /   |
/// 2 ------------------- 7  |
///   |    |____________|____| 6
///   |   /3            |   /
///   |  /              |  /
///   | /               | /
///   |/                |/
/// 5 ------------------- 4

所以我的目标是点 2，但是从 

GetBoxPoints()

获取点并将旋转作为法线向量给了我不同的姿势！你能告诉我怎样才能得到想要的姿势吗？预先感谢。

    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;

根据提取的值，P5就是我想要的点

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

Answer 1

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;
}