toys/gps/docs/source/math/obb.cpp

#include <cmath>

#include "Eigen/Dense"
#include "Eigen/Eigen"
#include "geo/private/math/obb.h"
#include "geo/private/coord/gauss.h"
#include "geo/private/coord/ct_convert.h"

namespace ns
{
namespace geo
{

// 根据物体的型点坐标计算出obb包围盒,这里只计算2D的
ObbData getObb(const std::vector<PointD>& points)
{
    ObbData obb;
    if (points.size() < 3)
    {
        return obb;
    }

    // 将原来的数据转换到Eigen中的vector
    Eigen::MatrixX2d eigPoints = Eigen::MatrixX2d::Zero(points.size(), 2);
    for (size_t i = 0; i < points.size(); i++)
    {
        eigPoints.row(i) = Eigen::Vector2d(points[i].x, points[i].y);
    }

    // 求协方差矩阵
    // Eigen::Vector2d centerV = eigPoints.colwise().mean();
    // Eigen::Matrix2d covariance;
    // covariance.setZero();
    // for (size_t i = 0; i < points.size(); i++)
    // {
    //     Eigen::Vector2d pAdg = Eigen::Vector2d(points[i].x, points[i].y) - centerV;
    //     covariance += pAdg * pAdg.transpose();
    // }
    // covariance /= (double)points.size();
    Eigen::Vector2d meanVec = eigPoints.colwise().mean();
    Eigen::RowVector2d meanVecRow(Eigen::RowVector2d::Map(meanVec.data(), 2));
    Eigen::MatrixX2d zeroMeanMat = eigPoints;
    zeroMeanMat.rowwise() -= meanVecRow;
    Eigen::Matrix2d covariance = (zeroMeanMat.adjoint() * zeroMeanMat) / double(eigPoints.rows());
    // LOG_DEBUG("covariance row: {}, col: {}", covariance.rows(), covariance.cols());

    // 计算矩阵的特征值和特征向量
    Eigen::EigenSolver<Eigen::Matrix2d> solver(covariance);
    Eigen::Matrix2d eigenVectors = solver.eigenvectors().real();
    Eigen::Matrix2d eigenVectorsT = eigenVectors.transpose();

    // 应该是旋转到特征向量坐标系下面求出来长宽/四个角点,然后再将角点转回原来坐标系
    Eigen::MatrixXd ar = eigPoints * eigenVectors;

    // 求出的应该是一个矩阵，然后求出这组矩阵中的每一行的最大值和最小值
    // 形成一个最大矩阵组和一个最小矩阵组
    Eigen::RowVector2d mina = Eigen::RowVector2d(ar.colwise().minCoeff()); // Eigen的部分归约
    Eigen::RowVector2d maxa = Eigen::RowVector2d(ar.colwise().maxCoeff());

    // 最后再求出中心点、四个角点、heading、宽和高等
    Eigen::RowVector2d diff = (maxa - mina) * 0.5;
    Eigen::RowVector2d center = mina + diff;
    Eigen::MatrixXd corners = Eigen::MatrixXd::Zero(4, 2);
    corners.row(0) = center + Eigen::RowVector2d(-diff.x(), -diff.y());
    corners.row(1) = center + Eigen::RowVector2d(diff.x(), -diff.y());
    corners.row(2) = center + Eigen::RowVector2d(diff.x(), diff.y());
    corners.row(3) = center + Eigen::RowVector2d(-diff.x(), diff.y());
    // 中心点
    center = center * eigenVectorsT;
    // 四个角点
    corners = corners * eigenVectorsT;
    // heading
    Eigen::RowVector2d d1 = corners.row(0);
    Eigen::RowVector2d d2 = corners.row(1);
    // LOG_DEBUG("d2: {:.8f}, {:.8f}", d2.x(), d2.y());
    // LOG_DEBUG("d1: {:.8f}, {:.8f}", d1.x(), d1.y());
    double heading = atan2((d2.y() - d1.y()), (d2.x() - d1.x())) * 180 / c_pi();
    if (heading < 0)
    {
        heading += 360;
    }
    else if (heading >= 360)
    {
        heading -= 360;
    }
    // 宽和高
    double h = diff.maxCoeff() * 2.0;
    double w = diff.minCoeff() * 2.0;

    // 求z值，暂时根据所有z的平均值
    double sumZ = 0.0;
    for (size_t i = 0; i < points.size(); i++)
    {
        sumZ += points[i].z;
    }
    double avgZ = sumZ / points.size();

    // 保存需要的center点、heading、宽和高
    obb.centerPoint = PointD{center.x(), center.y(), avgZ};
    obb.width = w;
    obb.height = h;
    obb.heading = heading;
    // 从右上角顺时针方向存储角点
    obb.cornerPoints.emplace_back(corners.row(2).x(), corners.row(2).y(), avgZ);
    obb.cornerPoints.emplace_back(corners.row(1).x(), corners.row(1).y(), avgZ);
    obb.cornerPoints.emplace_back(corners.row(0).x(), corners.row(0).y(), avgZ);
    obb.cornerPoints.emplace_back(corners.row(3).x(), corners.row(3).y(), avgZ);

    return obb;
}

} // namespace geo
} // namespace ns