a_shape_quadric.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_quadric.h"
#include <math.h>
#include <cstdlib>
#include <vnl/vnl_least_squares_function.h>
#include <vnl/algo/vnl_svd.h>
#include <vnl/algo/vnl_rpoly_roots.h>
#include <vnl/algo/vnl_matrix_inverse.h>
#include <vnl/algo/vnl_matrix_inverse.h>
#include <vnl/algo/vnl_matrix_inverse.h>
#include <vnl/algo/vnl_matrix_inverse.h>
#include <vnl/algo/vnl_matrix_inverse.h>
#include <vnl/algo/vnl_matrix_inverse.h>
#include <vnl/algo/vnl_matrix_inverse.h>
#include <vnl/algo/vnl_matrix_inverse.h>
#include <vnl/algo/vnl_matrix_inverse.h>
#include <vnl/algo/vnl_matrix_inverse.h>
 
//***************************************************************************
class a_shape_quadric_function : public vnl_least_squares_function
{
        public:
                a_shape_quadric_function( a_shape_quadric& shape,
                                const std::vector<a_point> & cloud) :  
                        vnl_least_squares_function(9, cloud.size(), no_gradient),       
                        shape_(shape),
                        cloud_(cloud) {};
                void f(const vnl_vector<double>& x, vnl_vector<double>& fx);
        protected:
                a_shape_quadric &                 shape_;
                const std::vector<a_point> &    cloud_;
};
//---------------------------------------------------------------------------
void a_shape_quadric_function::f(const vnl_vector<double>& x, vnl_vector<double>& fx)
{
        for (int i = 0; i<9; i++)
                shape_.para(i,x[i]);
        shape_.init_dist();
        for (int i = 0; i< fx.size(); i++)
                fx[i] = shape_.dist_point(cloud_[i]);
}
//***************************************************************************
//---------------------------------------------------------------------------
const std::string a_shape_quadric::help()
{    
  std::ostringstream o;
  o << "****************" << std::endl;
  o << "a_shape_quadric:" << std::endl;
  o << "****************" << std::endl;
  o << "This is a 'quadric' fitting class" << std::endl;
  o << std::endl;                       
  o << a_shape::help();                                                        
  return o.str();                 
}
//---------------------------------------------------------------------------
a_shape_quadric::a_shape_quadric(): a_shape(9)
{
        for (short i=0;i<3;i++) para_[i] = 1.;
        for (short i=3;i<9;i++) para_[i] = 0.;
        this->init_dist();
}
//---------------------------------------------------------------------------
vnl_double_3x3 a_shape_quadric::A() const
{
        vnl_double_3x3 a;
        a(0,0) = para_[0];
        a(1,1) = para_[1];
        a(2,2) = para_[2];
        a(0,1) = para_[3];
        a(0,2) = para_[5];
        a(1,0) = para_[3];
        a(1,2) = para_[4];
        a(2,0) = para_[5];
        a(2,1) = para_[4];
        return a;
}
//---------------------------------------------------------------------------
void a_shape_quadric::Ap()
{
        a_point c = a_shape_quadric::center();
        vnl_double_3 k(c.x(),c.y(),c.z());
        double cp = dot_product(b_,k)+dot_product(A_*k,k)-1.;
        Ap_ = -A_/cp;
}
//---------------------------------------------------------------------------
vnl_double_3 a_shape_quadric::b() const
{
        vnl_double_3 b;
        b(0) = para_[6];
        b(1) = para_[7];
        b(2) = para_[8];
        return b;
}
//---------------------------------------------------------------------------
void a_shape_quadric::p9pts(const a_point & p1, const a_point & p2, const a_point & p3,
                const a_point & p4, const a_point & p5, const a_point & p6,
                const a_point & p7, const a_point & p8, const a_point & p9)
{
        a_point p[9];
        p[0] = p1; p[1] = p2; p[2] = p3;
        p[3] = p4; p[4] = p5; p[5] = p6;
        p[6] = p7; p[7] = p8; p[8] = p9;
        this->p9pts(p);
}
//---------------------------------------------------------------------------
void a_shape_quadric::p9pts(const a_point * pt)
{
        vnl_matrix<double> mat(9,9);
        vnl_vector<double> b(9);
        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) = 1.;
        }
        vnl_svd<double> svd(mat);
        vnl_vector<double> x = svd.solve(b);
        for (int i=0;i<9;i++)
                para_[i] = x(i);
        this->init_dist();
}
//---------------------------------------------------------------------------
vnl_vector<double> clean_poly(vnl_vector<double>& k)
{
        int n = k.size();
        if (n==1)
                return k;
        vnl_vector<double> kn(n-1);
        double kr = k[0];
        //if first factor is null, it can be eliminated directly
        //cannot be threshold at this stage because all factors may be very small
        if (kr==0.)
        {
                for (int i=1; i<n; i++)
                        kn[i-1] = k[i];
                return clean_poly(kn);
        }
        //divide all factors so that first one = 1.
        for (int i=0; i<n; i++)
        {
                k[i] /= kr;
                //eliminate very small factors
                if (k[i] < 1.e-6) k[i] = 0.;
        }
        //if 2d factor is very big, first one can be eliminated
        if (fabs(k[1])>1.e6)
        {
                for (int i=1; i<n; i++)
                        kn[i-1] = k[i];
                return clean_poly(kn);
        }
        return k;
}
//---------------------------------------------------------------------------
void a_shape_quadric::init_dist()
{
        A_ = this->A();
        b_ = this->b();
        vnl_symmetric_eigensystem<double> eig(A_.as_matrix());
        R_ = eig.V;
        d0_ = eig.get_eigenvalue(0);
        d1_ = eig.get_eigenvalue(1);
        d2_ = eig.get_eigenvalue(2);
        this->Ap();
}
//---------------------------------------------------------------------------
a_point a_shape_quadric::closest_point(const a_point pt) const
{
        // get parameters of the equation: xT.A.x + bT.x + c = 0
        double c = -1.;
        //check whether point is on quadric
        vnl_double_3 pt0(pt.x(),pt.y(),pt.z());
        //      double on = fabs(dot_product(A*pt0,pt0) + dot_product(b,pt0)-1.);
        //      if (on<1.e-6)
        //              return pt;
        // decompose matrix A: A = RT.D.R
        //
        // 1. compute coefficients first term
        double a00 = 1.;
        double a01 = 4*(d1_+d2_);
        double a02 = 4*(d1_*d1_+d2_*d2_+4*d1_*d2_);
        double a03 = 16*d1_*d2_*(d1_+d2_);
        double a04 = 16*d1_*d1_*d2_*d2_;
        double a10 = 1.;
        double a11 = 4*(d0_+d2_);
        double a12 = 4*(d0_*d0_+d2_*d2_+4*d0_*d2_);
        double a13 = 16*d0_*d2_*(d0_+d2_);
        double a14 = 16*d0_*d0_*d2_*d2_;
        double a20 = 1.;
        double a21 = 4*(d0_+d1_);
        double a22 = 4*(d0_*d0_+d1_*d1_+4*d0_*d1_);
        double a23 = 16*d0_*d1_*(d0_+d1_);
        double a24 = 16*d0_*d0_*d1_*d1_;
        //
        vnl_double_3 alpha = R_.transpose()*pt0;
        vnl_double_3 beta = R_.transpose()*b_;
        //
        double a0 = alpha[0]; 
        double a1 = alpha[1]; 
        double a2 = alpha[2]; 
        double b0 = beta[0]; 
        double b1 = beta[1]; 
        double b2 = beta[2]; 
        //
        double c00 = a0*a0*d0_;
        double c01 = -2*a0*b0*d0_;
        double c02 = b0*b0*d0_;
        double c10 = a1*a1*d1_;
        double c11 = -2*a1*b1*d1_;
        double c12 = b1*b1*d1_;
        double c20 = a2*a2*d2_;
        double c21 = -2*a2*b2*d2_;
        double c22 = b2*b2*d2_;
        //
        double d00 = c00*a00;
        double d01 = c00*a01+c01*a00;
        double d02 = c00*a02+c01*a01+c02*a00;
        double d03 = c00*a03+c01*a02+c02*a01;
        double d04 = c00*a04+c01*a03+c02*a02;
        double d05 = c01*a04+c02*a03;
        double d06 = c02*a04;
        //
        double d10 = c10*a10;
        double d11 = c10*a11+c11*a10;
        double d12 = c10*a12+c11*a11+c12*a10;
        double d13 = c10*a13+c11*a12+c12*a11;
        double d14 = c10*a14+c11*a13+c12*a12;
        double d15 = c11*a14+c12*a13;
        double d16 = c12*a14;
        //
        double d20 = c20*a20;
        double d21 = c20*a21+c21*a20;
        double d22 = c20*a22+c21*a21+c22*a20;
        double d23 = c20*a23+c21*a22+c22*a21;
        double d24 = c20*a24+c21*a23+c22*a22;
        double d25 = c21*a24+c22*a23;
        double d26 = c22*a24;
        // 1.n results
        double dd0 = d00+d10+d20;
        double dd1 = d01+d11+d21;
        double dd2 = d02+d12+d22;
        double dd3 = d03+d13+d23;
        double dd4 = d04+d14+d24;
        double dd5 = d05+d15+d25;
        double dd6 = d06+d16+d26;
        // 2. compute coefficients 2d term
        double e00 = a0*b0;
        double e01 = 2*a0*b0*d0_-b0*b0;
        double e02 = -2*b0*b0*d0_;
        //
        double e10 = a1*b1;
        double e11 = 2*a1*b1*d1_-b1*b1;
        double e12 = -2*b1*b1*d1_;
        //
        double e20 = a2*b2;
        double e21 = 2*a2*b2*d2_-b2*b2;
        double e22 = -2*b2*b2*d2_;
        //
        double f00 = e00*a00;
        double f01 = e00*a01+e01*a00;
        double f02 = e00*a02+e01*a01+e02*a00;
        double f03 = e00*a03+e01*a02+e02*a01;
        double f04 = e00*a04+e01*a03+e02*a02;
        double f05 = e01*a04+e02*a03;
        double f06 = e02*a04;
        //
        double f10 = e10*a10;
        double f11 = e10*a11+e11*a10;
        double f12 = e10*a12+e11*a11+e12*a10;
        double f13 = e10*a13+e11*a12+e12*a11;
        double f14 = e10*a14+e11*a13+e12*a12;
        double f15 = e11*a14+e12*a13;
        double f16 = e12*a14;
        //
        double f20 = e20*a20;
        double f21 = e20*a21+e21*a20;
        double f22 = e20*a22+e21*a21+e22*a20;
        double f23 = e20*a23+e21*a22+e22*a21;
        double f24 = e20*a24+e21*a23+e22*a22;
        double f25 = e21*a24+e22*a23;
        double f26 = e22*a24;
        // 2.n results
        double f0 = f00+f10+f20;
        double f1 = f01+f11+f21;
        double f2 = f02+f12+f22;
        double f3 = f03+f13+f23;
        double f4 = f04+f14+f24;
        double f5 = f05+f15+f25;
        double f6 = f06+f16+f26;
        // 3. compute coefficients 3d term
        double g0 = c;
        double g1 = 4*d0_*c;
        double g2 = g1*d0_;
        // 3.n results
        double h0 = g0*a00;
        double h1 = g0*a01+g1*a00;
        double h2 = g0*a02+g1*a01+g2*a00;
        double h3 = g0*a03+g1*a02+g2*a01;
        double h4 = g0*a04+g1*a03+g2*a02;
        double h5 = g1*a04+g2*a03;
        double h6 = g2*a04;
        // total of the 3 terms : coefficients of the polynomial
        vnl_vector<double> k(7);
        k[6] = dd0+f0+h0;
        k[5] = dd1+f1+h1;
        k[4] = dd2+f2+h2;
        k[3] = dd3+f3+h3;
        k[2] = dd4+f4+h4;
        k[1] = dd5+f5+h5;
        k[0] = dd6+f6+h6;
        vnl_vector<double> roots;
        vnl_vector<double> k2 = clean_poly(k);
        vnl_rpoly_roots poly(k2);
        roots = poly.realroots();
        vnl_double_3x3 I(0.);
        I(0,0) = 1.;
        I(1,1) = 1.;
        I(2,2) = 1.;
        vnl_double_3 x0;
        double li0 = 1.e30;
        for (int i=0; i<roots.size(); i++)
        {
                vnl_svd<double> svd(I+2*roots(i)*A_.as_matrix());
                svd.zero_out_relative(1.e-6);
                vnl_double_3 x = svd.solve(pt0-roots(i)*b_.as_vector());
                double li = (x-pt0).magnitude();
                if (li<li0)
                {
                        li0 = li;
                        x0 = x;
                }
        }
        return a_point(x0(0),x0(1),x0(2));
}
//---------------------------------------------------------------------------
a_point a_shape_quadric::center() const
{
        vnl_svd<double> svd(A_.as_matrix());
        svd.zero_out_relative(1.e-6);
        vnl_double_3 x = svd.solve(b_.as_vector())/(-2.);
        return a_point(x(0),x(1),x(2));
}
//---------------------------------------------------------------------------
a_point a_shape_quadric::principals() const
{
        vnl_symmetric_eigensystem<double> eig(Ap_.as_matrix());
        double d0 = eig.get_eigenvalue(0);
        double d1 = eig.get_eigenvalue(1);
        double d2 = eig.get_eigenvalue(2);
        return a_point(d2,d1,d0);
}
//---------------------------------------------------------------------------
a_point a_shape_quadric::n1() const
{
        vnl_symmetric_eigensystem<double> eig(Ap_.as_matrix());
        vnl_double_3 n = eig.get_eigenvector(2);
        return a_point(n(0),n(1),n(2));
}
//---------------------------------------------------------------------------
a_point a_shape_quadric::n2() const
{
        vnl_symmetric_eigensystem<double> eig(Ap_.as_matrix());
        vnl_double_3 n = eig.get_eigenvector(1);
        return a_point(n(0),n(1),n(2));
}
//---------------------------------------------------------------------------
a_point a_shape_quadric::n3() const
{
        vnl_symmetric_eigensystem<double> eig(Ap_.as_matrix());
        vnl_double_3 n = eig.get_eigenvector(0);
        return a_point(n(0),n(1),n(2));
}
//---------------------------------------------------------------------------
double a_shape_quadric::dist_point(const a_point pt) const
{
        a_point proj = this->closest_point(pt);
        return (pt-proj).norm();
}
//---------------------------------------------------------------------------
void a_shape_quadric::random_hint(const std::vector<a_point>& pts)
{
        int n = pts.size();
        int i[9];
        a_point pt[9];
        std::cerr << "hint: ";
        for (int j=0; j<9; j++)
        {
                bool passed;
                int it;
                do {
                        passed = true;
                        it = rand()%n;
                        int k=0;
                        while ((k<j)&&(passed))
                        {
                                if (it==i[k])
                                        passed = false;
                                k++;
                        }
                } while (!passed);
                i[j] = it;
                pt[j] = pts[it];
                std::cerr << it << " ";
        }
        std::cerr << std::endl;
        this->p9pts(pt);
}
//---------------------------------------------------------------------------
void a_shape_quadric::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_quadric_function f(*this,pts2);
                a_shape::fit_cloud(pts2,f);
        }
        pts = pts2;
}
//---------------------------------------------------------------------------
void a_shape_quadric::export_matrices() const
{
        std::cout << "A:" << std::endl;
        std::cout << A_;
        std::cout << "b:" << std::endl;
        std::cout << b_ << std::endl;
        std::cout << "center:" << std::endl;
        std::cout << this->center() << std::endl;
        std::cout << "Ap:" << std::endl;
        std::cout << Ap_;
        std::cout << "eigen values:" << std::endl;
        std::cout << this->principals() << std::endl;
        std::cout << "eigen vectors:" << std::endl;
        std::cout << "n1: " << this->n1() << std::endl;
        std::cout << "n2: " << this->n2() << std::endl;
        std::cout << "n3: " << this->n3() << std::endl;
}
//---------------------------------------------------------------------------
void a_shape_quadric::export_points(const unsigned int nseg, const std::vector<a_point>& pts) const
{
}
//---------------------------------------------------------------------------
void a_shape_quadric::export_triangles(const unsigned int nseg, const std::vector<a_point>& pts) const
{
}