a_shape_line.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_line.h"
#include <math.h>
#include <time.h>
#include <vnl/vnl_least_squares_function.h>
 
class a_shape_line_function : public vnl_least_squares_function
{
        public:
                a_shape_line_function( a_shape_line& shape,
                                const std::vector<a_point> & cloud) :  
                        vnl_least_squares_function(6, cloud.size(), no_gradient),       
                        shape_(shape),
                        cloud_(cloud) {};
                void f(const vnl_vector<double>& x, vnl_vector<double>& fx);
        protected:
                a_shape_line &                 shape_;
                const std::vector<a_point> &    cloud_;
};
//***************************************************************************
//---------------------------------------------------------------------------
void a_shape_line_function::f(const vnl_vector<double>& x, vnl_vector<double>& fx)
{
        a_point dir(x[0],x[1],x[2]);
        a_point orig(x[3],x[4],x[5]);
        dir.normalise();
        shape_.dir(dir);
        shape_.orig(orig-(dir*orig)*dir);
        for (int i = 0; i< fx.size(); i++)
                fx[i] = shape_.dist_point(cloud_[i]);
}
//***************************************************************************
//---------------------------------------------------------------------------
const std::string a_shape_line::help()
{    
  std::ostringstream o;
  o << "*************" << std::endl;
  o << "a_shape_line:" << std::endl;
  o << "*************" << std::endl;
  o << "This is a 'line' fitting class" << std::endl;
  o << std::endl;                       
  o << a_shape::help();                                                        
  return o.str();                 
}
//---------------------------------------------------------------------------
void a_shape_line::p2pts(const a_point p1, const a_point p2)
{
        a_point dir = p2-p1;
        if (dir.norm()==0) return;
        this->dir(dir.normalise());
        this->orig(p1-(p1*this->dir())*this->dir());
        //std::cerr << orig_ << std::endl << dir_ << std::endl;
}
//---------------------------------------------------------------------------
a_point a_shape_line::closest_point(const a_point pt) const
{
        a_point d = this->orig()-pt;
        a_point dist = (this->dir()*d)*this->dir()-d;
        return pt-dist;
}
//---------------------------------------------------------------------------
double a_shape_line::dist_point(const a_point pt) const
{
        a_point d = this->orig()-pt;
        a_point dist = (this->dir()*d)*this->dir()-d;
        return dist.norm();
}
//---------------------------------------------------------------------------
void a_shape_line::random_hint(const std::vector<a_point>& pts)
{
        //srand(time(NULL));
        int n = pts.size();
        int i1 = rand()%n;
        int i2;
        do
        {
                i2 = rand()%n;
        } while (i1==i2);
        a_point p1 = pts[i1];
        a_point p2 = pts[i2];
        this->p2pts(p1,p2);
}
//---------------------------------------------------------------------------
void a_shape_line::fit_cloud(std::vector<a_point>& pts, short nl)
{
        std::vector<a_point> pts2;
        int cs0 = pts.size();
        this->best_fitting_cloud(pts,pts2);
        if (nl==1)
        {
                a_shape_line_function f(*this,pts2);
                a_shape::fit_cloud(pts2,f);
        }
        pts = pts2;
}
//---------------------------------------------------------------------------
void a_shape_line::export_line(const std::vector<a_point>& pts) const
{
        a_point x0 = this->closest_point(pts[0]);
        double vmin, vmax;
        for (int i=1; i< pts.size(); i++)
        {
                a_point x = this->closest_point(pts[i]);
                double v = this->dir()*(x-x0);
                if (v<vmin) vmin=v;
                if (v>vmax) vmax=v;
        }
        std::cout << "1" << std::endl;
        std::cout << x0+vmin*this->dir() << " " << x0+vmax*this->dir() << std::endl;
}