unwarp.cxx

Go to the documentation of this file.

/*
         Copyright 2008-2022 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_image_defo.h"
#include "SimpleOpt.h"
#include <cmath>
#include <iostream>
#include <sstream>
enum { OPT_HELP, OPT_XC, OPT_YC, OPT_K1, OPT_K2, OPT_K3, OPT_SCALE, OPT_POINTS};
 
CSimpleOpt::SOption g_rgOptions[] = {
        { OPT_XC,  _T("-xc"),     SO_REQ_SEP },
        { OPT_YC,  _T("-yc"),     SO_REQ_SEP },
        { OPT_K1,  _T("-k1"),     SO_REQ_SEP },
        { OPT_K2,  _T("-k2"),     SO_REQ_SEP },
        { OPT_K3,  _T("-k3"),     SO_REQ_SEP },
        { OPT_SCALE, _T("-s"),           SO_NONE    },
        { OPT_SCALE, _T("--scaled"),     SO_NONE    },
        { OPT_POINTS, _T("-r"),           SO_NONE    },
        { OPT_POINTS, _T("--reference-points"),           SO_NONE    },
        { OPT_HELP, _T("-?"),     SO_NONE    },
        { OPT_HELP, _T("-h"),     SO_NONE    },
        { OPT_HELP, _T("--help"), SO_NONE    },
        SO_END_OF_OPTIONS
};
 
//----------------------------------------------------------------------------
// k1, k2... : radial distorsion parametre(s)
// xo, yo : point in the original image
// cx, cy : centre of deformation
// xn, yn : point in the new image
//
 
class a_radial_distorsion: public a_image_defo
{
        public:
                void read(const std::string& name) {a_image_defo::read(name); r_max_=this->diag();}
                void box_out();
                double diag() const {return sqrt(box_in_[1]*box_in_[1]+box_in_[3]*box_in_[3])/2.;}
                void centre() {xc_=(box_in_[1]-box_in_[0])/2.; yc_=(box_in_[3]-box_in_[2])/2.;}
                void centre(double xc, double yc) {xc_=yc; yc_=yc;}
                void k(double k1, double k2, double k3) {k1_=k1; k2_=k2; k3_=k3;}
                void xc(double xc) {xc_=xc;}
                void yc(double yc) {yc_=yc;}
                void new_coordinates(double xo, double yo, double& xn, double& yn);
                bool old_coordinates(double xn, double yn, double& xo, double& yo);
                double rdefo(double rexact);
        protected:
                double r_max_;
                double xc_;
                double yc_;
                double k0_;
                double k1_;
                double k2_;
                double k3_;
};
 
//---------------------------------------------------------------------------
void a_radial_distorsion::box_out()
{
        k0_=1.-k1_-k2_-k3_;
        a_image_defo::box_out();
}
//---------------------------------------------------------------------------
double a_radial_distorsion::rdefo(double rexact)
{
        const int n_max = 50;
        double rest = rexact;
        double rn;
        int n = 0;
        do 
        {
                n++;
                rn = k0_*rest+k1_*rest*rest+k2_*rest*rest*rest+k3_*rest*rest*rest*rest;
                double rnd = k0_+2*k1_*rest+3*k2_*rest*rest+4*k3_*rest*rest*rest;
                double dr = (rexact-rn)/rnd;
                rest += dr;
        } while ((fabs(rexact-rn)>1.e-5)&&(n<n_max));
        if (n>=n_max)
        {
                std::cerr << "convergence problem!" << std::endl;
                std::cerr << "are the factors large? check result carefully." << std::endl;
                std::cerr << "rest: " << fabs(rexact-rn) << std::endl;
                return -1.;
        }
        return rest;
}
//---------------------------------------------------------------------------
void a_radial_distorsion::new_coordinates(double xo, double yo, double& xn, double& yn)
{
        double dx = xo-xc_;
        double dy = yo-yc_;
        double r = sqrt(dx*dx+dy*dy)/r_max_;
        double L = k0_+k1_*r+k2_*r*r+k3_*r*r*r;
        xn = xc_+L*dx;
        yn = yc_+L*dy;
}
//---------------------------------------------------------------------------
bool a_radial_distorsion::old_coordinates(double xn, double yn, double& xo, double& yo)
{
        double dx = xn-xc_;
        double dy = yn-yc_;
        double r = sqrt(dx*dx+dy*dy)/r_max_;
        double L; 
        if (r != 0.)
        {
                L = this->rdefo(r);
                if (L == -1.) return false;
                L /= r;
        }
        else
                L = 0.;
        xo = xc_+L*dx;
        yo = yc_+L*dy;
        if (xo<(double)box_in_[0]) return false;
        if (xo>(double)box_in_[1]) return false;
        if (yo<(double)box_in_[2]) return false;
        if (yo>(double)box_in_[3]) return false;
        return true;
}
//---------------------------------------------------------------------------
int error(int val)
{
#include "unwarp.help"
        return val;
}
//---------------------------------------------------------------------------
int main(int argc, char ** argv)
{
        double k1 = 0;
        double k2 = 0;
        double k3 = 0;
        double xc = 0;
        double yc = 0;
        bool is_scaled=false;
        bool given_xc=false;
        bool given_yc=false;
        bool with_points = 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_SCALE)
                                is_scaled = true;
                        else if (args.OptionId() == OPT_POINTS)
                                with_points = true;
                        else if (args.OptionId() == OPT_XC)
                        {
                                std::ostringstream o;
                                o <<  args.OptionArg();
                                std::istringstream in(o.str().c_str());
                                in >> xc;
                                given_xc = true;
                        } else if (args.OptionId() == OPT_YC)
                        {
                                std::ostringstream o;
                                o <<  args.OptionArg();
                                std::istringstream in(o.str().c_str());
                                in >> yc;
                                given_yc = true;
                        } else if (args.OptionId() == OPT_K1)
                        {
                                std::ostringstream o;
                                o <<  args.OptionArg();
                                std::istringstream in(o.str().c_str());
                                in >> k1;
                        } else if (args.OptionId() == OPT_K2)
                        {
                                std::ostringstream o;
                                o <<  args.OptionArg();
                                std::istringstream in(o.str().c_str());
                                in >> k2;
                        } else if (args.OptionId() == OPT_K3)
                        {
                                std::ostringstream o;
                                o <<  args.OptionArg();
                                std::istringstream in(o.str().c_str());
                                in >> k3;
                        } 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() != 2) return error(-2);
        std::ifstream in(args.File(0));
        if (!in)
        {
                std::cerr << "cannot open file: '" << args.File(0) << "'" << std::endl;
                return error(-3);
        }
        in.close();
        a_radial_distorsion disto;
        disto.read(args.File(0));
        if (!is_scaled)
        {
                double d = disto.diag();
                k1 *= d;
                k2 *= d*d;
                k3 *= d*d*d;
        }
        disto.centre();
        if (given_xc) disto.xc(xc);
        if (given_yc) disto.yc(yc);
        disto.k(k1,k2,k3);
        disto.box_out();
        disto.deform();
        disto.write(args.File(1));
        if (with_points)
        {
                unsigned int npts;
                std::cin >> npts;
                std::cout << npts << std::endl; 
                for (unsigned int i=0; i<npts; i++)
                {
                        double u,v;
                        double un,vn;
                        std::cin >> u >> v;
                        disto.new_coordinates(u,v,un,vn);
                        std::cout << un << " " << vn << std::endl; 
                } 
        }
        return 0;
}