sphere.cpp

Go to the documentation of this file.

//=============================================================================================================
 
//=============================================================================================================
// INCLUDES
//=============================================================================================================
 
#include "sphere.h"
#include "simplex_algorithm.h"
 
#include <QDebug>
 
//=============================================================================================================
// EIGEN INCLUDES
//=============================================================================================================
 
#include <Eigen/Dense>
 
#include <iostream>
 
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================
 
using namespace UTILSLIB;
using namespace Eigen;
 
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================
 
Sphere::Sphere(const Vector3f& center, float radius)
: m_center(center)
, m_r(radius)
{
}
 
//=============================================================================================================
 
Sphere Sphere::fit_sphere(const MatrixX3f& points)
{
    const VectorXf& x = points.col(0);
    const VectorXf& y = points.col(1);
    const VectorXf& z = points.col(2);
 
    VectorXf point_means = points.colwise().mean();
 
    VectorXf x_mean_free = x.array() - point_means(0);
    VectorXf y_mean_free = y.array() - point_means(1);
    VectorXf z_mean_free = z.array() - point_means(2);
 
    Matrix3f A;
    A << (x.cwiseProduct(x_mean_free)).mean(), 2 * (x.cwiseProduct(y_mean_free)).mean(), 2 * (x.cwiseProduct(z_mean_free)).mean(),
        0, (y.cwiseProduct(y_mean_free)).mean(), 2 * (y.cwiseProduct(z_mean_free)).mean(),
        0, 0, (z.cwiseProduct(z_mean_free)).mean();
 
    Matrix3f A_T = A.transpose();
    A += A_T;
 
    Vector3f b;
    VectorXf sq_sum = x.array().pow(2) + y.array().pow(2) + z.array().pow(2);
    b << (sq_sum.cwiseProduct(x_mean_free)).mean(),
        (sq_sum.cwiseProduct(y_mean_free)).mean(),
        (sq_sum.cwiseProduct(z_mean_free)).mean();
 
    Vector3f center = A.ldlt().solve(b);
 
    MatrixX3f tmp(points.rows(), 3);
    tmp.col(0) = x.array() - center(0);
    tmp.col(1) = y.array() - center(1);
    tmp.col(2) = z.array() - center(2);
 
    float r = sqrt(tmp.array().pow(2).rowwise().sum().mean());
 
    return Sphere(center, r);
}
 
//=============================================================================================================
 
Sphere Sphere::fit_sphere_simplex(const MatrixX3f& points, double simplex_size)
{
    VectorXf center;
    float R;
    if (fit_sphere_to_points(points, simplex_size, center, R)) {
        return Sphere(center, R);
    }
 
    return Sphere(Vector3f(), 0.0f);
}
 
//=============================================================================================================
 
bool Sphere::fit_sphere_to_points(float** rr, int np, float simplex_size, float* r0, float* R)
{
    MatrixXf rr_eigen(np, 3);
    VectorXf r0_eigen(3);
 
    MatrixX3f dig_rr_eigen;
    for (int k = 0; k < np; k++) {
        rr_eigen(k, 0) = rr[k][0];
        rr_eigen(k, 1) = rr[k][1];
        rr_eigen(k, 2) = rr[k][2];
    }
 
    if (rr_eigen.rows() < 0) {
        std::cout << "Sphere::fit_sphere_to_points - No points were passed." << std::endl;
        return false;
    }
 
    r0_eigen(0) = r0[0];
    r0_eigen(1) = r0[1];
    r0_eigen(2) = r0[2];
 
    bool state = UTILSLIB::Sphere::fit_sphere_to_points(rr_eigen, simplex_size, r0_eigen, *R);
 
    r0[0] = r0_eigen(0);
    r0[1] = r0_eigen(1);
    r0[2] = r0_eigen(2);
 
    return state;
}
 
//=============================================================================================================
 
bool Sphere::fit_sphere_to_points(const MatrixXf& rr, float simplex_size, VectorXf& r0, float& R)
{
    //    int   np = rr.rows();
 
    /*
     * Find the optimal sphere origin
     */
    FitUser user;
    float ftol = 1e-5f;
    int max_eval = 500;
    int report_interval = -1;
    int neval;
    MatrixXf init_simplex;
    VectorXf init_vals(4);
 
    VectorXf cm(3);
    float R0 = 0.1f;
 
    user.rr = rr;
 
    calculate_cm_ave_dist(rr, cm, R0); // [done]
 
    init_simplex = make_initial_simplex(cm, simplex_size);
 
    user.report = false;
 
    for (int k = 0; k < 4; k++) {
        init_vals[k] = fit_eval(static_cast<VectorXf>(init_simplex.row(k)), &user);
    }
 
    user.report = false;
 
    // Capture user data in type-safe lambda — no void* needed
    auto cost = [&user](const VectorXf& x) -> float {
        return fit_eval(x, &user);
    };
 
    //Start the minimization
    if (!SimplexAlgorithm::simplex_minimize<float>(init_simplex,    /* The initial simplex */
                                                   init_vals,       /* Function values at the vertices */
                                                   ftol,            /* Relative convergence tolerance */
                                                   0.0f,            /* No spatial convergence tolerance */
                                                   cost,            /* The cost function (captures user data) */
                                                   max_eval,        /* Maximum number of function evaluations */
                                                   neval,           /* Number of function evaluations */
                                                   report_interval, /* How often to report (-1 = no_reporting) */
                                                   report_func))    /* The function to be called when reporting */
    {
        return false;
    }
 
    r0 = init_simplex.row(0);
    R = opt_rad(r0, &user);
 
    return true;
}
 
//=============================================================================================================
 
bool Sphere::report_func(int loop, const VectorXf& fitpar, double fval_lo, double /*fval_hi*/, double /*par_diff*/)
{
    /*
     * Report periodically
     */
    const VectorXf& r0 = fitpar;
 
    std::cout << "loop: " << loop << "; r0: " << 1000 * r0[0] << ", r1: " << 1000 * r0[1] << ", r2: " << 1000 * r0[2] << "; fval: " << fval_lo << std::endl;
 
    return true;
}
 
//=============================================================================================================
 
void Sphere::calculate_cm_ave_dist(const MatrixXf& rr, VectorXf& cm, float& avep)
{
    cm = rr.colwise().mean();
    MatrixXf diff = rr.rowwise() - cm.transpose();
    avep = diff.rowwise().norm().mean();
}
 
//=============================================================================================================
 
MatrixXf Sphere::make_initial_simplex(const VectorXf& pars, float size)
{
    /*
     * Make the initial tetrahedron
     */
    int npar = pars.size();
 
    MatrixXf simplex = MatrixXf::Zero(npar + 1, npar);
 
    simplex.rowwise() += pars.transpose();
 
    for (int k = 1; k < npar + 1; k++) {
        simplex(k, k - 1) += size;
    }
 
    return simplex;
}
 
//=============================================================================================================
 
float Sphere::fit_eval(const VectorXf& fitpar, const void* user_data)
{
    /*
     * Calculate the cost function value
     * Optimize for the radius inside here
     */
    const FitUser* user = static_cast<const FitUser*>(user_data);
    const VectorXf& r0 = fitpar;
 
    float F;
 
    MatrixXf diff = user->rr.rowwise() - r0.transpose();
    VectorXf one = diff.rowwise().norm();
 
    float sum = one.sum();
    float sum2 = one.dot(one);
 
    F = sum2 - sum * sum / user->rr.rows();
 
    if (user->report)
        std::cout << "r0: " << 1000 * r0[0] << ", r1: " << 1000 * r0[1] << ", r2: " << 1000 * r0[2] << "; R: " << 1000 * sum / user->rr.rows() << "; fval: " << F << std::endl;
 
    return F;
}
 
//=============================================================================================================
 
float Sphere::opt_rad(const VectorXf& r0, const FitUser* user)
{
    MatrixXf diff = user->rr.rowwise() - r0.transpose();
    return diff.rowwise().norm().mean();
}