h_compute.cxx

Go to the documentation of this file.

/*
         Copyright 2008-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 <iostream>
#include <vector>
#include <mvl/HMatrix2D.h>
#include <mvl/HMatrix2DComputeLinear.h>
#include <vnl/vnl_least_squares_function.h>
#include <vnl/algo/vnl_levenberg_marquardt.h>
#include <vgl/vgl_distance.h>
#include "a_point.h"
#include "SimpleOpt.h"
 
enum { OPT_HELP, OPT_NONLIN, OPT_HOMG, OPT_VERB};
 
CSimpleOpt::SOption g_rgOptions[] = {
        { OPT_NONLIN, _T("-nl"),     SO_NONE    },
        { OPT_NONLIN, _T("--non-linear"),     SO_NONE    },
        { OPT_HOMG, _T("-hg"),     SO_NONE    },
        { OPT_HOMG, _T("--homogeneous"),     SO_NONE    },
        { OPT_VERB, _T("-v"),     SO_NONE    },
        { OPT_VERB, _T("--verbose"),     SO_NONE    },
        { OPT_HELP, _T("-?"),     SO_NONE    },
        { OPT_HELP, _T("-h"),     SO_NONE    },
        { OPT_HELP, _T("--help"), SO_NONE    },
        SO_END_OF_OPTIONS
};
bool verbose = false;
//---------------------------------------------------------------------------
int error(int val)
{
#include "h_compute.help"
        return val;
}
//***************************************************************************
class a_homography_function : public vnl_least_squares_function
{
        public:
                a_homography_function( std::vector<vgl_homg_point_2d<double> > & points_im1,
                                std::vector<vgl_homg_point_2d<double> > & points_im2) :
                        vnl_least_squares_function(9, points_im1.size()*2, no_gradient),
                        points_im1_(points_im1),
                        points_im2_(points_im2)
        {};
                void f(const vnl_vector<double>& x, vnl_vector<double>& fx);
                void scale1(const double s) {scale1_=s;}
        protected:
                std::vector<vgl_homg_point_2d<double> > & points_im1_;
                std::vector<vgl_homg_point_2d<double> > & points_im2_;
                double scale1_;
};
//---------------------------------------------------------------------------
void normalise_h(vnl_double_3x3 & h)
{
        double no = 0.;
        for (int i=0;i<3;i++)
                no += h(i,0)*h(i,0)+h(i,1)*h(i,1)+h(i,2)*h(i,2);
        h /= sqrt(no);
}
//---------------------------------------------------------------------------
void a_homography_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];
        normalise_h(h0);
        HMatrix2D H(h0);
        double d = 0;
        for (int i = 0; i< points_im1_.size(); i++)
        {
                vgl_homg_point_2d<double> pt_project = H.transform_to_plane2(points_im1_[i]);
                vgl_homg_point_2d<double> pt_true = points_im2_[i];
                fx[i*2] = pt_project.x()/pt_project.w()-pt_true.x();
                fx[i*2+1] = pt_project.y()/pt_project.w()-pt_true.y();
                if (verbose)
                {
                        double dist = vgl_distance(pt_project,pt_true);
                        d += dist*dist;
                }
        }
        if (verbose)
                std::cerr << sqrt(d/points_im1_.size())*scale1_ << " ";
}
//---------------------------------------------------------------------------
int main(int argc, char**argv)
{
        //parse the options
        bool nonlinear = false;
        bool homogeneous = false;
        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_NONLIN)
                                nonlinear = true;
                        else if (args.OptionId() == OPT_HOMG)
                                homogeneous = true;
                        else if (args.OptionId() == OPT_VERB)
                                verbose = true;
                        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() != 0) return error(-2);
 
        //read the data from the standard input
        std::vector<HomgPoint2D > points1;
        std::vector<HomgPoint2D > points2;
        while (std::cin.good()) 
        {
                double x,y,w=1;
                std::cin >> x >> y;
                if (homogeneous)
                        std::cin >> w;
                points1.push_back(HomgPoint2D(x,y,w));
                std::cin >> x >> y; 
                if (homogeneous)
                        std::cin >> w;
                points2.push_back(HomgPoint2D(x,y,w));
                std::cin >> std::ws; // Eat whitespace
        }
        if (nonlinear && points1.size()<5)
        {
                std::cerr << "option nonlinear is only active when the number of points is higher than 4" << std::endl;
                nonlinear = false;
        }
        //normalisation
        int npts = points1.size();
        //1. calculate centre of point cloud
        double um1 = 0; double vm1 = 0;
        double um2 = 0; double vm2 = 0;
        for (int i=0; i< npts; i++)
        {
                um1 += points1[i].x();
                vm1 += points1[i].y();
                um2 += points2[i].x();
                vm2 += points2[i].y();
        }
        um1 /= npts; vm1 /= npts;
        um2 /= npts; vm2 /= npts;
        //2. compute average size of point cloud
        a_point im10(um1,vm1,0.);
        a_point im20(um2,vm2,0.);
        double di1m = 0.;
        double di2m = 0.;
        for (int i=0; i< npts; i++)
        {
                a_point im1(points1[i].x(),points1[i].y(),0.);
                double di = dist(im1,im10);
                di1m += di*di;
                a_point im2(points2[i].x(),points2[i].y(),0.);
                di = dist(im2,im20);
                di2m += di*di;
        }
        di1m = sqrt(di1m/npts);
        di2m = sqrt(di2m/npts);
        //3. scale and translate point cloud
        std::vector<vgl_homg_point_2d<double> > points_im1;
        std::vector<vgl_homg_point_2d<double> > points_im2;
        for (int i=0; i< npts; i++)
        {
                double u1 = (points1[i].x()-um1)/di1m;
                double v1 = (points1[i].y()-vm1)/di1m;
                points_im1.push_back(vgl_homg_point_2d<double>(u1,v1,1.0));
                points1[i].set(points_im1[i].x(),points_im1[i].y(),points_im1[i].w());
                double u2 = (points2[i].x()-um2)/di2m;
                double v2 = (points2[i].y()-vm2)/di2m;
                points_im2.push_back(vgl_homg_point_2d<double>(u2,v2,1.0));
                points2[i].set(points_im2[i].x(),points_im2[i].y(),points_im2[i].w());
        }
        //4. prepare the transformation matrices
        vnl_double_3x3 hk1(0.);
        hk1.fill_diagonal(1./di1m);
        hk1(0,2) = -um1/di1m;
        hk1(1,2) = -vm1/di1m;
        hk1(2,2) = 1.;
        vnl_double_3x3 hk2(0.);
        hk2.fill_diagonal(di2m);
        hk2(0,2) = um2;
        hk2(1,2) = vm2;
        hk2(2,2) = 1.;
        //end of normalisation
 
        //compute linear estimate
        HMatrix2DComputeLinear computor;
        HMatrix2D h = computor.compute(points1, points2);
        //normalise matrix ||h|| = 1
        vnl_double_3x3 h0 = h.get_matrix();
        normalise_h(h0);
        if (nonlinear)
        {
                vnl_vector<double> x(9);
                unsigned int vv = 0;
                for (int i = 0; i< 3; i++)
                {
                        for (int j = 0; j< 3; j++)
                        {
                                x[vv] = h0(i,j);
                                vv++;
                        }
                }
                a_homography_function f(points_im1,points_im2);
                f.scale1(di1m);
                vnl_levenberg_marquardt levmarq(f);
                if (verbose)
                        std::cerr << "Errors (in the unit of ur, vr, wr)" << std::endl;
                levmarq.minimize(x);
                if (verbose)
                        std::cerr << std::endl;
                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];
                if (verbose)
                {
                        std::cerr << "Levenberg Marquardt" << std::endl
                                << "min: " << levmarq.get_end_error() << " at " << x << " (" << levmarq.get_end_error()*di1m*sqrt(2.) << " in the unit of ur, vr, wr)" << std::endl
                                << "iterations: " << levmarq.get_num_iterations() << "    "
                                << "evaluations: " << levmarq.get_num_evaluations() << std::endl;
                        levmarq.diagnose_outcome();
                }
        }
        //denormalisation
        vnl_double_3x3 hmat2 = hk2*h0*hk1;
        normalise_h(hmat2);
        //output results
        std::cout << "3 3" << std::endl;
        std::cout << hmat2 << std::endl;
        return 0;
}