a_shape.cxx

Go to the documentation of this file.

/*
         Copyright 2009-2020 Pierre SMARS (smars@yuntech.edu.tw)
         This program is free software: you can redistribute it and/or modify
         it under the terms of the GNU General Public License as published by
         the Free Software Foundation, either version 2 of the License, or
         (at your option) any later version.
 
         This program is distributed in the hope that it will be useful,
         but WITHOUT ANY WARRANTY; without even the implied warranty of
         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
         GNU General Public License for more details.
 
         You should have received a copy of the GNU General Public License
         along with this program.  If not, see <http://www.gnu.org/licenses/>.
         */
#include "a_shape.h"
#include <iostream>
#include <sstream>
#include <vnl/algo/vnl_levenberg_marquardt.h>
//---------------------------------------------------------------------------
const std::string a_shape::help()
{
        std::ostringstream o;
        o << "********" << std::endl;
        o << "a_shape:" << std::endl;
        o << "********" << std::endl;
        o << "This library provides classes to fit shapes to point clouds." << std::endl;
        o << "classes: a_shape_line, a_shape_circle, a_shape_sphere, a_shape_plane, a_shape_quadric, a_shape_cylinder" << std::endl;
        o << "use a_shape_line_help for more explanations." << std::endl;
        o << a_base::help();
        return o.str();
}
//---------------------------------------------------------------------------
a_shape::~a_shape()
{
        para_.clear();
}
//---------------------------------------------------------------------------
double a_shape::dist_cloud(const std::vector<a_point>& pts)
{
        double d=0;
        this->init_dist();
        for (auto pt:pts)
                d += this->dist_point(pt);
        return d;
}
//---------------------------------------------------------------------------
double a_shape::average_dist_cloud(const std::vector<a_point>& pts)
{
        return this->dist_cloud(pts)/pts.size();
}
//---------------------------------------------------------------------------
double a_shape::rms_dist_cloud(const std::vector<a_point>& pts)
{
        double d=0;
        this->init_dist();
        for (auto pt:pts)
        {
                double di = this->dist_point(pt);
                d += di*di;
        }
        return sqrt(d/pts.size());
}
//---------------------------------------------------------------------------
int a_shape::threshold_cloud(const std::vector<a_point>& pts, std::vector<a_point>& pts2)
{
        int iter = 0;
        pts2.clear();
        this->init_dist();
        for (auto pt:pts)
        {
                double d = this->dist_point(pt);
                if (d<verysmall_) 
                {
                        pts2.push_back(pt);
                        iter += 1;
                }
        }
        return iter;
}
//---------------------------------------------------------------------------
int a_shape::best_fitting_cloud(const std::vector<a_point>& pts, std::vector<a_point>& pts2)
{
        const double P = log(1.-P_);
        int niter = 1000000; // very big number
        int nc0 = pts.size();
        vnl_vector<double> bpara;
        int npara=this->npara();
        std::vector<a_point> bfit;
        int nf0 = 0;
        double w = 0.;
        int i = 0;
        while (i<niter)
        {
                this->random_hint(pts);
                int nf = threshold_cloud(pts,bfit);
                if (nf>nf0)
                { 
                        nf0 = nf;
                        pts2 = bfit;
                        bpara = para_;
                        w = nf/double(nc0); //proportion of inliers
                        std::cerr << "proportion of inliers: " << w*100 << "%" << std::endl;
                        //new estimate of the number of iterations necessary to achieve 
                        //a probability P_ that we got a sample with 100% inliers
                        double niterd = P/log(1.-exp(npara*log(w)));
                        //if it is too big, probably a bad sample anyway, do not change value!
                        if (niterd<1000000)
                                niter = int(niterd+.5);
                        if (niter<0)
                                niter = 1000000;
                        std::cerr << i << " " << niterd << std::endl;
                }
                i++;
        }
        if (verbose_)
        {
                std::cerr << "RANSAC: number of random tests: " << i << std::endl;
                std::cerr << nf0 << " inliers in a cloud of " << pts.size() 
                        << " points (RANSAC sample)" <<std::endl;
        }
        para_ = bpara;
        return nf0;
}
//---------------------------------------------------------------------------
void a_shape::fit_cloud(const std::vector<a_point>& pts, vnl_least_squares_function& f)
{
        if (verbose_)
                std::cerr << "rms dist (RANSAC sample, before LSF): " << this->rms_dist_cloud(pts) << std::endl;
        vnl_vector<double> x0 = this->getparameters();
        vnl_vector<double> x = x0;
        vnl_levenberg_marquardt levmarq(f);
        levmarq.minimize(x);
        if (verbose_)
        {
                std::cerr << "Least Squares Fitting (Levenberg Marquardt)" << std::endl
                        << "min: " << levmarq.get_end_error() << " at " << x << std::endl;
                levmarq.diagnose_outcome();
                std::cerr << "rms dist (RANSAC sample, after LSF): " << this->rms_dist_cloud(pts) << std::endl;
        }
}
//---------------------------------------------------------------------------
void a_shape::export_inliers(const std::vector<a_point>& pts)
{
        int n = 0;
        this->init_dist();
        for (auto pt:pts)
        {
                double d = this->dist_point(pt);
                if (d<=verysmall_)
                        n++;
        }
        if (n>0)
        {
                std::cout << n << std::endl;
                for (auto pt:pts)
                {
                        double d = this->dist_point(pt);
                        if (d<=verysmall_)
                                std::cout << pt << std::endl;
                }       
        }
}
//---------------------------------------------------------------------------
void a_shape::export_outliers(const std::vector<a_point>& pts)
{
        int n = 0;
        this->init_dist();
        for (auto pt:pts)
        {
                double d = this->dist_point(pt);
                if (d>verysmall_)
                        n++;
        }
        if (n>0)
        {
                std::cout << n << std::endl;
                for (auto pt:pts)
                {
                        double d = this->dist_point(pt);
                        if (d>verysmall_)
                                std::cout << pt << std::endl;
                }       
        }
}
//***************************************************************************
//-operator>>----------------------------------------------------------------
/*std::istream& operator>> (std::istream& i, a_shape& v)
        {
        return i;
        }*/
//-operator<<----------------------------------------------------------------
std::ostream& operator<< (std::ostream& o, const a_shape& v)
{
        o << v.classname() << std::endl;
        o << v.small() << std::endl;
        o << v.getparameters() << std::endl;
        return o;
}