radial.cxx

Go to the documentation of this file.

/*
         Copyright 2002-2019 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 <osl/osl_fit_circle.h>
#include <osl/osl_easy_canny.h>
#include <osl/osl_save_topology.h>
#include <vil1/vil1_load.h>
#include <list>
#include <vector>
#include <iostream>
#include <vnl/vnl_least_squares_function.h>
#include <vnl/algo/vnl_levenberg_marquardt.h>
#include <mvl/HMatrix2D.h>
#include <mvl/HMatrix2DComputeLinear.h>
#include <math.h>
#include <stdlib.h>
 
#include "a_point.h"
#include "SimpleOpt.h"
 
enum { OPT_HELP, OPT_NX, OPT_NY, OPT_DATA, OPT_RATIO, OPT_TOL};
 
CSimpleOpt::SOption g_rgOptions[] = {
        { OPT_NX,  _T("-nx"),     SO_REQ_SEP },
        { OPT_NY,  _T("-ny"),     SO_REQ_SEP },
        { OPT_RATIO,  _T("-r"),     SO_REQ_SEP },
        { OPT_RATIO,  _T("--ratio"),     SO_REQ_SEP },
        { OPT_TOL,  _T("-t"),     SO_REQ_SEP },
        { OPT_TOL,  _T("--tolerance"),     SO_REQ_SEP },
        { OPT_DATA, _T("-d"),     SO_NONE    },
        { OPT_DATA, _T("--data"),     SO_NONE    },
        { OPT_HELP, _T("-?"),     SO_NONE    },
        { OPT_HELP, _T("-h"),     SO_NONE    },
        { OPT_HELP, _T("--help"), SO_NONE    },
        SO_END_OF_OPTIONS
};
 
//***************************************************************************
class a_radial_function : public vnl_least_squares_function
{
        public:
                a_radial_function( std::vector<HomgPoint2D> & points_image,
                                std::vector<HomgPoint2D> & points_world) :
                        vnl_least_squares_function(12, points_image.size()*2, no_gradient),
                        points_image_(points_image),
                        points_world_(points_world)
        {};
                void f(const vnl_vector<double>& x, vnl_vector<double>& fx);
        protected:
                std::vector<HomgPoint2D> & points_image_;
                std::vector<HomgPoint2D> & points_world_;
};
//---------------------------------------------------------------------------
void a_radial_function::f(const vnl_vector<double>& x, vnl_vector<double>& fx)
{
        vnl_double_3x3 h0;
        h0(0,0) = x[0]; h0(0,1) = x[1]; h0(0,2) = x[2];
        h0(1,0) = x[3]; h0(1,1) = x[4]; h0(1,2) = x[5];
        h0(2,0) = x[6]; h0(2,1) = x[7]; h0(2,2) = x[8];
        HMatrix2D H(h0);
        double k1 = x[9];
        double k2 = x[10];
        double k3 = x[11];
        double k0 = 1.-k1-k2-k3;
        double d = 0.;
        double u0 = 0.;
        double v0 = 0.;
        for (int i = 0; i< points_image_.size(); i++)
        {
                HomgPoint2D im = H.transform_to_plane2(points_world_[i]); 
                double u = points_image_[i].x();
                double v = points_image_[i].y();
                double r = sqrt((u-u0)*(u-u0)+(v-v0)*(v-v0));
                double L = k0+k1*r+k2*r*r+k3*r*r*r;
                double un = u0+L*(u-u0);
                double vn = v0+L*(v-v0);
                fx[i*2] = un-im.x()/im.w();
                fx[i*2+1] = vn-im.y()/im.w();
                //              double dn = sqrt(fx[i*2]*fx[i*2]+fx[i*2+1]*fx[i*2+1]);
                //              d += dn;
        }
        //      std::cerr << "error: " << d/points_image_.size()*999.3 << std::endl;
}
//---------------------------------------------------------------------------
int error(int val)
{
#include "radial.help"
        return val;
}
//---------------------------------------------------------------------------
int main(int argc, char *argv[])
{
        int nrow = 10;
        int ncol = 14;
        double poss_var = 0.25;
        bool with_data = false;
        double ratio = 1.;
        CSimpleOpt args(argc, argv, g_rgOptions);
        while (args.Next())
        {
                if (args.LastError() == SO_SUCCESS)
                {
                        if (args.OptionId() == OPT_HELP)
                                return error(0);
                        else if (args.OptionId() == OPT_DATA)
                                with_data = true;
                        else if (args.OptionId() == OPT_NX)
                        {
                                std::ostringstream o;
                                o <<  args.OptionArg();
                                std::istringstream in(o.str().c_str());
                                in >> ncol;
                        } else if (args.OptionId() == OPT_NY)
                        {
                                std::ostringstream o;
                                o <<  args.OptionArg();
                                std::istringstream in(o.str().c_str());
                                in >> nrow;
                        } else if (args.OptionId() == OPT_RATIO)
                        {
                                std::ostringstream o;
                                o <<  args.OptionArg();
                                std::istringstream in(o.str().c_str());
                                in >> ratio;
                        } else if (args.OptionId() == OPT_TOL)
                        {
                                std::ostringstream o;
                                o <<  args.OptionArg();
                                std::istringstream in(o.str().c_str());
                                in >> poss_var;
                        } else
                                return error(-1);
                        // handle option, using OptionId(), OptionText() and OptionArg()
                } else {
                        std::cerr << "Invalid argument: " << args.OptionText() << std::endl;
                        return error(args.LastError());
                        // handle error, one of: SO_OPT_INVALID, SO_OPT_MULTIPLE,
                        // SO_ARG_INVALID, SO_ARG_INVALID_TYPE, SO_ARG_MISSING
                }
        }
 
        if (args.FileCount() != 1) return error(-2);
 
        const int npts = nrow*ncol;
        const double cx_w = (ncol-1)/2.*ratio;
        const double cy_w = (nrow-1)/2.;
        const double s_w = sqrt(cx_w*cx_w+cy_w*cy_w);
        std::vector<a_point> centers_w;
        std::vector<HomgPoint2D> centers_w2;
        for (int i=0; i< nrow; i++)
        {
                for (int j=0; j<ncol; j++)
                {
                        double x = ratio*(j-cx_w)/s_w;
                        double y = (i-cy_w)/s_w;
                        centers_w.push_back(a_point(x,y,0.));
                        centers_w2.push_back(HomgPoint2D(x,y,1.));
                }
        }
        // input image
        vil1_image image = vil1_load(args.File(0));
        if (!image) return 2;
        double width_i = image.width();
        double height_i = image.height();
        double cx_i = (width_i-1)/2.;
        double cy_i = (height_i-1)/2.;
        double s_i = sqrt(cx_i*cx_i+cy_i*cy_i);
        // Canny edgel extraction
        std::list<osl_edge*> edgels;
        osl_easy_canny(0, image, &edgels);
        double rm=0;
        for (std::list<osl_edge*>::iterator it= edgels.begin(); it!= edgels.end(); it++)
        {
                osl_edge const *e = *it;
                osl_fit_circle fit(*e);
                rm += fit.radius();
        }
        rm /= edgels.size();
        std::vector<a_point> centers_i;
        if (with_data)
                std::cout << npts << std::endl;
        for (std::list<osl_edge*>::iterator it= edgels.begin(); it!= edgels.end(); it++)
        {
                osl_edge const *e = *it;
                osl_fit_circle fit(*e);
                double rf = fit.radius()/rm;
                if ((rf>1.-poss_var)&&(rf<1.+poss_var))
                {
                        double x = (fit.center().y()-cx_i)/s_i;
                        double y = (height_i-1-fit.center().x()-cy_i)/s_i;
                        a_point p(x,y,0.);
                        //std::cerr << p << std::endl;
                        centers_i.push_back(p);
                }
        }
        if (centers_i.size() != npts)
        {
                std::cerr << "Problem" << std::endl;
                std::cerr << "Number of circle identified is " << centers_i.size() << " and should be " << npts << std::endl;
                std::cerr << "Did you try to blur the image? Is contrast high enough? Do you have borders?" << std::endl;
                exit(1);
        }
        //1. estimate the transformation H between image and world
        //1.1 estimate the position of 4 corners...
        //1.1.1 estimate the extremes on the diagonal
        int i_h[4];
        double pm = 100000;
        double pM = -100000;
        for (int i =0; i< centers_i.size(); i++)
        {
                a_point p = centers_i[i];
                double ptax = p.x()+p.y();
                if (ptax<pm)
                { 
                        pm = ptax;
                        i_h[0] = i;
                }
                if (ptax>pM)
                { 
                        pM = ptax;
                        i_h[3] = i;
                }
        }
        double x_h[4], y_h[4];
        a_point p_h[4];
        p_h[0] = centers_i[i_h[0]];
        p_h[3] = centers_i[i_h[3]];
        //1.1.2 estimate the other 2 extremes (maximising the surface of triangles formed with diagonal)
        pm = 100000;
        pM = -100000;
        for (int i =0; i< centers_i.size(); i++)
        {
                a_point p = centers_i[i];
                a_point pn = cross(p_h[3]-p_h[0],p-p_h[0]);
                double ptax = pn.norm();
                if (pn.z() < 0) ptax = -ptax;
                if (ptax<pm)
                { 
                        pm = ptax;
                        i_h[1] = i;
                }
                if (ptax>pM)
                { 
                        pM = ptax;
                        i_h[2] = i;
                }
        }       
        p_h[1] = centers_i[i_h[1]];
        p_h[2] = centers_i[i_h[2]];
        std::cerr << p_h[0] << " " << p_h[1] << " " << p_h[2] << " " << p_h[3] << std::endl;
        //1.2 estimate H
        //HMatrix2DCompute4Point computor;
        HMatrix2DComputeLinear computor(false);
        std::vector<HomgPoint2D> corners_w,corners_i;
        a_point pp = centers_w[0];
        corners_w.push_back(HomgPoint2D(pp.x(),pp.y(),1.));
        pp = centers_w[ncol-1];
        corners_w.push_back(HomgPoint2D(pp.x(),pp.y(),1.));
        pp = centers_w[ncol*(nrow-1)];
        corners_w.push_back(HomgPoint2D(pp.x(),pp.y(),1.));
        pp = centers_w[ncol*nrow-1];
        corners_w.push_back(HomgPoint2D(pp.x(),pp.y(),1.));
        for (int i=0;i<4;i++)
        {
                pp = centers_i[i_h[i]];
                corners_i.push_back(HomgPoint2D(pp.x(),pp.y(),1.));
                std::cerr << corners_w[i] << " " << corners_i[i] << std::endl;
        }
        HMatrix2D H = computor.compute(corners_w,corners_i);
        std::cerr << H << std::endl;
        //find the corresponding points 
        std::vector<HomgPoint2D> centers_i2;
        for (int i =0; i< centers_i.size(); i++)
        {
                HomgPoint2D p_w(centers_w[i].x(),centers_w[i].y(),1.);
                HomgPoint2D p_ie = H.transform_to_plane2(p_w);
                double d0 = 1.e30;
                int j0;
                for (int j=0; j<centers_i.size(); j++)
                {
                        HomgPoint2D p_i(centers_i[j].x(),centers_i[j].y(),1.);
                        double d = sqrt((p_i.x()/p_i.w()-p_ie.x()/p_ie.w())*(p_i.x()/p_i.w()-p_ie.x()/p_ie.w())+
                                        (p_i.y()/p_i.w()-p_ie.y()/p_ie.w())*(p_i.y()/p_i.w()-p_ie.y()/p_ie.w()));
                        if (d<d0)
                        {
                                d0 = d;
                                j0 = j;
                        }
                }
                centers_i2.push_back(HomgPoint2D(centers_i[j0].x(),centers_i[j0].y(),1.));
        }
        HMatrix2D H2 = computor.compute(centers_w2,centers_i2);
        std::cerr << H2 << std::endl;
        for (int i =0; i< centers_i.size(); i++)
        {
                HomgPoint2D pi = centers_i2[i];
                HomgPoint2D pw = centers_w2[i];
                HomgPoint2D pi2 = H2.transform_to_plane2(pw);
                double d = sqrt((pi.x()/pi.w()-pi2.x()/pi2.w())*(pi.x()/pi.w()-pi2.x()/pi2.w())+
                                (pi.y()/pi.w()-pi2.y()/pi2.w())*(pi.y()/pi.w()-pi2.y()/pi2.w()));
                //              std::cerr << "i: " << i << " d: " << d*s_i << std::endl;
                if (with_data)
                        std::cout << centers_i2[i].x() << " " << centers_i2[i].y() << " " << d*10 << std::endl;
        }
        vnl_vector<double> x(12);
        int vv = 0;
        for (int i = 0; i< 3; i++)
        {
                for (int j = 0; j< 3; j++)
                {
                        x[vv] = H2.get(i,j);
                        vv++;
                }
        }
        x[9] = 0.; x[10] = 0.; x[11] = 0.;
        a_radial_function f(centers_i2,centers_w2);
        vnl_levenberg_marquardt levmarq(f);
        //      levmarq.set_max_function_evals(10000);
        //      levmarq.set_g_tolerance(levmarq.get_g_tolerance()/10.);
        levmarq.minimize(x);
        std::cerr << "Levenberg Marquardt" << std::endl
                << "min: " << levmarq.get_end_error() << " at " << x << std::endl
                << "iterations: " << levmarq.get_num_iterations() << "    "
                << "evaluations: " << levmarq.get_num_evaluations() << std::endl;
        levmarq.diagnose_outcome();
        if (!with_data)
                std::cout << " -k1=" << x[9] << " -k2=" << x[10] << " -k3=" << x[11] << " -s" << std::endl;
        return 0;
}