galaxis_polyfit.h

Go to the documentation of this file.

#pragma once
 
#ifndef GALAXIS_POLYFIT_H
#define GALAXIS_POLYFIT_H
 
#include "polyfit.h"
#include "utils/poly.hpp"
#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <exception>
#include <galaxis/linatypes.hpp>
 
 
constexpr int GALAXIS_MAXIMUM_DEGREE = 3;
 
namespace {
 
std::shared_ptr<std::vector<double>> galaxis_polyfit(std::vector<double>& pointsX,
                                                     std::vector<double>& pointsY,
                                                     int p_amountOfRandomPointsForFitting,
                                                     int p_iterationAmount,
                                                     float p_epsilon,
                                                     PolyFit<double>::SolverType solver = PolyFit<double>::SolverType::EIGEN_JACOBI_SVD,
                                                     int p_degree                       = GALAXIS_MAXIMUM_DEGREE) {
  try {
    if(pointsX.size() < (p_degree + 1) || pointsX.size() != pointsY.size()) {
      std::shared_ptr<std::vector<double>>
          coeff = std::make_shared<std::vector<double>>();
      coeff->push_back(0);
      coeff->push_back(0);
      coeff->push_back(0);
      coeff->push_back(0);
 
      return coeff;
    }
 
    if(pointsX.size() <= p_amountOfRandomPointsForFitting) {
      if(!(((ssize_t)pointsX.size() - 2) > 1)) {
        std::shared_ptr<std::vector<double>>
            coeff = std::make_shared<std::vector<double>>();
        coeff->push_back(0);
        coeff->push_back(0);
        coeff->push_back(0);
        coeff->push_back(0);
 
        return coeff;
      } else {
        p_amountOfRandomPointsForFitting = pointsX.size() - 2;
      }
    }
 
    // Create options for fitting
    PolyFit<double>::Options options;
 
    options.polyDeg   = p_degree;
    options.solver    = solver; // SVD solver
    options.maxMPts   = p_amountOfRandomPointsForFitting; // #Pts to use for fitting
    options.maxTrial  = p_iterationAmount; // Max. nr. of trials
    options.tolerance = p_epsilon; // Distance tolerance for inlier
 
    PolyFit<double> polyfit(pointsX, pointsY, options);
 
    // Solve using RANSAC
    polyfit.solveRLS();
 
    auto polynomVecType = polyfit.getCoefficients();
 
    std::shared_ptr<std::vector<double>>
        coeff = std::make_shared<std::vector<double>>();
 
    //std::cout << "Eigen PlynomVecTypes size: " << std::to_string(polynomVecType.size()) << std::endl;
 
    if(polynomVecType.size() != (p_degree + 1)) {
      std::shared_ptr<std::vector<double>>
          coeff = std::make_shared<std::vector<double>>();
      coeff->push_back(0);
      coeff->push_back(0);
      coeff->push_back(0);
      coeff->push_back(0);
 
      return coeff;
    }
 
    for(ssize_t i = p_degree; i >= 0; i--) {
      coeff->push_back(polynomVecType(i, 0));
    }
    for(ssize_t i = 0; i < (GALAXIS_MAXIMUM_DEGREE - p_degree); i++) {
      coeff->push_back(0);
    }
 
    //std::cout << "[POLYFIT] Degree: " << p_degree << "; [" << coeff->at(0) << ", " << coeff->at(1) << ", " << coeff->at(2) << ", " << coeff->at(3) << "]" << std::endl;
 
    return coeff;
  } catch(std::exception& e) {
    std::cout << "Exception in Polyfit" << std::endl;
    std::shared_ptr<std::vector<double>> coeff = std::make_shared<std::vector<double>>();
    coeff->push_back(0);
    coeff->push_back(0);
    coeff->push_back(0);
    coeff->push_back(0);
 
    return coeff;
  }
}
 
std::shared_ptr<std::vector<double>> galaxis_polyfit(std::vector<galaxis::lina::Vector2f> const& p_pointCloud,
                                                     int p_amountOfRandomPointsForFitting,
                                                     int p_iterationAmount,
                                                     float p_epsilon,
                                                     bool p_switchxy = false,
                                                     double p_pointsDivisionFactor = 1,
                                                     PolyFit<double>::SolverType solver = PolyFit<double>::SolverType::EIGEN_JACOBI_SVD,
                                                     int p_degree                       = GALAXIS_MAXIMUM_DEGREE) {
 
  throw "[GALAXIS POLYFIT] Function deprecated. Update your Code!";
  return std::make_shared<std::vector<double>>();
}
 
}
#endif