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