a_shape_cylinder.cxx

Go to the documentation of this file.

/*
         Copyright 2015-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_cylinder.h"
#include <math.h>
#include <cstdlib>
#include <vnl/algo/vnl_svd.h>
#include <vnl/algo/vnl_matrix_inverse.h>
#include <vnl/algo/vnl_symmetric_eigensystem.h>
#include <vnl/algo/vnl_rpoly_roots.h>
#include <vnl/algo/vnl_levenberg_marquardt.h>
//***************************************************************************
class a_shape_cylinder_function : public vnl_least_squares_function
{
        public:
                a_shape_cylinder_function(const std::vector<a_point> & cloud):
                        vnl_least_squares_function(7, cloud.size(), no_gradient),
                        cloud_(cloud) {};
                //void radius(const double& r) {x[0]=r;};
                //              void center(const a_point& c) {x[1]=c.x();x[2]=c.y();x[3]=c.z();};
                //              void dir(const a_point& dir) {x[4]=dir.x();x[5]=dir.y();x[6]=dir.z();};
                void f(const vnl_vector<double>& x, vnl_vector<double>& fx);
        protected:
                const std::vector<a_point> &    cloud_;
};
//---------------------------------------------------------------------------
void a_shape_cylinder_function::f(const vnl_vector<double>& x, vnl_vector<double>& fx)
{
        double r = x[0];
        a_point c(x[1],x[2],x[3]);
        a_point d(x[4],x[5],x[6]);
        for (int i = 0; i< fx.size(); i++)
        {
                a_point v=cloud_[i]-c;
                a_point v0=c+(v*d)*d;
                fx[i]=(cloud_[i]-v0).norm()-r;
        }
}
//***************************************************************************
//---------------------------------------------------------------------------
const std::string a_shape_cylinder::help()
{    
  std::ostringstream o;
  o << "****************" << std::endl;
  o << "a_shape_cylinder:" << std::endl;
  o << "****************" << std::endl;
  o << "This is a 'cylinder' fitting class" << std::endl;
  o << std::endl;                       
  o << a_shape::help();                                                        
  return o.str();                 
}
//---------------------------------------------------------------------------
void a_shape_cylinder::center(a_point c)
{
        if (c.norm() != 0)
                c.normalise();
}
//---------------------------------------------------------------------------
a_point a_shape_cylinder::center() const
{
        return center_;
}
//---------------------------------------------------------------------------
void a_shape_cylinder::dir(a_point d)
{
        if (d.norm() != 0)
        {
                d.normalise();
        }
}
//---------------------------------------------------------------------------
void a_shape_cylinder::radius(const double r)
{
}
//---------------------------------------------------------------------------
double a_shape_cylinder::radius() const
{
        return radius_;
}
//---------------------------------------------------------------------------
a_point a_shape_cylinder::dir() const
{
        return dir_;
}
//---------------------------------------------------------------------------
void a_shape_cylinder::p9pts(const a_point * pt)
{
        vnl_matrix<double> mat(9,9);
        vnl_matrix<double> b(9,1);
        for (int i=0;i<9;i++)
        {
                double x = pt[i].x();
                double y = pt[i].y();
                double z = pt[i].z();
                mat(i,0) = x*x;
                mat(i,1) = y*y;
                mat(i,2) = z*z;
                mat(i,3) = 2*x*y;
                mat(i,4) = 2*y*z;
                mat(i,5) = 2*x*z;
                mat(i,6) = x;
                mat(i,7) = y;
                mat(i,8) = z;
                b(i,0) = 1.;
        }
        vnl_matrix<double> inv_mat = vnl_matrix_inverse<double>(mat).as_matrix();
        vnl_matrix<double> x = inv_mat*b;
        for (int i=0;i<9;i++)
                para_[i] = x(i,0);
        vnl_double_3x3 A = this->A();
        //std::cerr << "*****************" << std::endl << A;
        vnl_double_3 b2 = this->b();
        //compute center: c
        vnl_double_3 b3 = -b2/2.;
        vnl_svd<double> svd(A.as_matrix());
        svd.zero_out_relative(1.e-6);
        vnl_double_3 c = svd.solve(b3.as_vector());
        double d = 1.-dot_product(b2,c)-dot_product(c*A,c);
        A /= d;
        //check whether it is a good candidate, close to cylinder
        double n1 = svd.W(0);
        double n2 = svd.W(1);
        double n3 = svd.W(2);
        if (n1!=0)
        {
                n2=fabs(1-n2/n1);
                n3=fabs(n3/n1);
                n1=1.;
          std::cerr << "svd " << n1 << " " << n2 << " " << n3 << std::endl;
                if ((n2>0.1)&&(n3>0.01))
                {
                        std::cerr << "failed! not a cylinder!" << std::endl;
                        for (short i=0;i<9;i++) para_[i]=0.;
                        radius_=0;
                        dir_.set(1.,0.,0.);
                        center_.set(0.,0.,0.);
                        return;
                }
                else
                        std::cerr << "passed! this is a cylinder" << std::endl;
        }
        //compute radius
        double r = (svd.W(0)+svd.W(1))/2.;
        svd.W(0) = r;
        svd.W(1) = r;
        svd.W(2) = 0;
        r = sqrt(fabs(1./r));
        radius_ = sqrt(fabs(1./r));
        vnl_double_3x3 A2 = svd.recompose();
        //      A2 *= d;
        para_[0] = A2(0,0);
        para_[1] = A2(1,1);
        para_[2] = A2(2,2);
        para_[3] = A2(0,1);
        para_[4] = A2(1,2);
        para_[5] = A2(0,2);
        center_ = a_shape_quadric::center();
        dir_ = a_shape_quadric::n3();
        std::cerr << "dddir " << dir_ << std::endl;
        //std::cerr << A2 << "--------------------" << std::endl;
}
//---------------------------------------------------------------------------
a_point a_shape_cylinder::closest_point(const a_point pt) const
{
        double r = this->radius();
        a_point c = this->center();
        a_point n = this->dir();
        a_point v = pt-c;
        a_point c1 = c+(n*v)*n;
        a_point v2 = (pt-c1).normalise();
        return c1+v2*r;
}
//---------------------------------------------------------------------------
int a_shape_cylinder::threshold_cloud(const std::vector<a_point>& pts, std::vector<a_point>& pts2)
{
        int iter = 0;
        pts2.clear();
        this->init_dist();
        std::cerr << "rr " << radius_ << " " << center_ << " " << dir_ << std::endl;
        for (auto pt:pts)
        {
                a_point pt0 = this->closest_point(pt);
                //a_point pt0 = a_shape_quadric::closest_point(*pt);
                double d = (pt-pt0).norm();
                if (d<verysmall_) 
                {
                        pts2.push_back(pt);
                        iter += 1;
                }
        }
        std::cerr << "thres: " << iter << std::endl;
        return iter;
}
//---------------------------------------------------------------------------
void a_shape_cylinder::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_cylinder_function f(pts2);
                vnl_svd<double> svd(Ap_.as_matrix());
                double r = (svd.W(0)+svd.W(1))/2.;
                r = sqrt(fabs(1./r));
                a_point c = a_shape_quadric::center();
                a_point d = a_shape_quadric::n3();
                vnl_vector<double> x(7);
                x[0] = r;
                x[1] = c.x();
                x[2] = c.y();
                x[3] = c.z();
                x[4] = d.x();
                x[5] = d.y();
                x[6] = d.z();
                vnl_levenberg_marquardt levmarq(f);
                levmarq.minimize(x);
                radius_ = x[0];
                center_ = a_point(x[1],x[2],x[3]);
                dir_ = a_point(x[4],x[5],x[6]);
                if (verbose_)
                {
                        std::cerr << "Least Squares Fitting (Levenberg Marquardt)" << std::endl
                                << "min: " << levmarq.get_end_error() << " at " << x << std::endl;
                        levmarq.diagnose_outcome();
                }
        }
        pts = pts2;
}
//---------------------------------------------------------------------------
void a_shape_cylinder::export_matrices() const
{
        std::cout << "radius: " << radius_ << std::endl;
        std::cout << "center: " << center_ << std::endl;
        a_point dir=dir_;
        if (dir[2]<0.)
        {
                for (short i=0;i<3;i++) dir[i]=-dir[i];
        }
        std::cout << "direction: " << dir << std::endl;
        std::cout << "azimuth: " << atan2(dir[1],dir[0])*180./3.1415926535 << "°" << std::endl;
        double b = sqrt(dir[0]*dir[0]+dir[1]*dir[1]);
        std::cout << "elevation: " << atan2(dir[2],b)*180./3.1415926535 << "°" << std::endl;
}
//---------------------------------------------------------------------------
void a_shape_cylinder::export_points(const unsigned int nseg, const std::vector<a_point>& pts) const
{
}
//---------------------------------------------------------------------------
void a_shape_cylinder::export_triangles(const unsigned int nseg, const std::vector<a_point>& pts) const
{
}