a_persp_mes_list.cxx

Go to the documentation of this file.

/*
         Copyright 2000-19 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_persp_mes_list.h"
 
const double sd_pos = 0.01;
const double sd_ang = 0.001;
const double sd_sxy = 0.1;
const double sd_st = 0.1;
const double sd_uv0 = 0.1;
const double sd_K1 = 0.0001;
const double sd_K2 = 0.00001;
const double sd_P1 = 0.0001;
const double sd_P2 = 0.00001;
 
//---------------------------------------------------------------------------
a_persp_mes_list::a_persp_mes_list(const std::string file)
{
        file_ = file;
        std::ifstream in(file.c_str());
        if (in)
                in >> *this;
        else
                throw file_error();
}
//---------------------------------------------------------------------------
a_persp_mes_list::~a_persp_mes_list()
{
        for (std::vector<a_unwarp *>::iterator w = cam_unwarp_.begin(); w < cam_unwarp_.end(); w++)
                delete (*w);
        cam_unwarp_.clear();
        for (std::vector<a_intern *>::iterator i = cam_intern_.begin(); i < cam_intern_.end(); i++)
                delete (*i);
        cam_intern_.clear();
        for (std::vector<a_pos2 *>::iterator p = pos_.begin(); p < pos_.end(); p++)
                delete (*p);
        pos_.clear();
        for (std::vector<a_persp_mes *>::iterator m = measures_.begin(); m < measures_.end(); m++)
                delete (*m);
        measures_.clear();
}
//---------------------------------------------------------------------------
double a_persp_mes_list::getX(int ai) const
{
        int nc = cam_unwarp_.size();
        int np = pos_.size();
        if (ai<6*np)
        {
                a_pos2 * pos = pos_[ai/6];
                switch (ai%6)
                {
                        case 0:                 return pos->ox()/sd_pos;
                        case 1:                 return pos->oy()/sd_pos;
                        case 2:                 return pos->oz()/sd_pos;
                        case 3:                 return pos->ax()/sd_ang;
                        case 4:                 return pos->ay()/sd_ang;
                        case 5:                 return pos->az()/sd_ang;
                }
        }
        else if (ai<6*np+5*nc)
        {
                ai -= 6*np;
                a_intern * intern = cam_intern_[ai/5];
                a_unwarp * unwarp;
                switch (ai%5)
                {
                        case 0:                 return intern->sx()/sd_sxy;
                        case 1:                 return intern->sy()/sd_sxy;
                        case 2:                 return intern->stheta()/sd_st;
                        case 3:                 return intern->u0()/sd_uv0;
                        case 4:                 return intern->v0()/sd_uv0;
                }
        }
        else
        {
                ai -= 6*np+5*nc;
                a_unwarp * unwarp = cam_unwarp_[ai/4];
                switch (ai%4)
                {
                        case 0:                 return unwarp->K1()/sd_K1;      
                        case 1:                 return unwarp->K2()/sd_K2;
                        case 2:                 return unwarp->P1()/sd_P1;
                        case 3:                 return unwarp->P2()/sd_P2;
                }
        }
        return 0.; // just for the sake of eliminating compiler warning
}
//---------------------------------------------------------------------------
void a_persp_mes_list::setX(int ai, double val)
{
        int nc = cam_unwarp_.size();
        int np = pos_.size();
        if (ai<6*np)
        {
                a_pos2 * pos = pos_[ai/6];
                switch (ai%6)
                {
                        case 0:                 pos->ox(val*sd_pos);                    break;
                        case 1:                 pos->oy(val*sd_pos);                    break;
                        case 2:                 pos->oz(val*sd_pos);                    break;
                        case 3:                 pos->ax(val*sd_ang);                    break;
                        case 4:                 pos->ay(val*sd_ang);                    break;
                        case 5:                 pos->az(val*sd_ang);
                }
        }
        else if (ai<6*np+5*nc)
        {
                ai -= 6*np;
                a_intern * intern = cam_intern_[ai/5];
                a_unwarp * unwarp;
                switch (ai%5)
                {
                        case 0:                 intern->sx(val*sd_sxy);         break;
                        case 1:                 intern->sy(val*sd_sxy);         break;
                        case 2:                 intern->stheta(val*sd_st);      break;
                        case 3:                 
                                                                        intern->u0(val*sd_uv0);
                                                                        unwarp = cam_unwarp_[ai/5];
                                                                        unwarp->u0(val*sd_uv0);
                                                                        break;
                        case 4:
                                                                        intern->v0(val*sd_uv0);
                                                                        unwarp = cam_unwarp_[ai/5];
                                                                        unwarp->v0(val*sd_uv0);
                }
        }
        else
        {
                ai -= 6*np+5*nc;
                a_unwarp * unwarp = cam_unwarp_[ai/4];
                switch (ai%4)
                {
                        case 0:                 unwarp->K1(val*sd_K1);          break;
                        case 1:                 unwarp->K2(val*sd_K2);          break;
                        case 2:                 unwarp->P1(val*sd_P1);          break;
                        case 3:                 unwarp->P2(val*sd_P2);
                }
        }
}
//---------------------------------------------------------------------------
double a_persp_mes_list::residual()
{
        double d = 0;
        for (int i = 0; i < measures_.size(); i++)
        {
                a_persp_mes * meas = measures_[i];
                a_unwarp * unwarp = cam_unwarp_[meas->cam()];
                a_intern * intern = cam_intern_[meas->cam()];
                a_pos2 * pos = pos_[meas->pos()];
                d +=  meas->residual(unwarp,intern,pos);
        }
        return d;
}
//---------------------------------------------------------------------------
std::istream& operator>> (std::istream& i, a_persp_mes_list& l)
{
        int n_cam,n_pos,n_meas;
        i >> n_cam >> n_pos >> n_meas;
        for (int j = 0; j < n_cam; j++)
        {
                auto unwarp = new a_unwarp;
                i >> *unwarp;
                l.cam_unwarp_.push_back(unwarp);
                auto intern = new a_intern;
                i >> *intern;
                l.cam_intern_.push_back(intern);
        }
        for (int j = 0; j < n_pos; j++)
        {
                auto pos = new a_pos2;
                i >> *pos;
                l.pos_.push_back(pos);
        }
        for (int j = 0; j < n_meas; j++)
        {
                auto meas = new a_persp_mes;
                l.measures_.push_back(meas);
                i >> *meas;
        }
        return i;
}
//---------------------------------------------------------------------------
std::ostream& operator<< (std::ostream& o, const a_persp_mes_list& l)
{
        o << l.cam_unwarp_.size() << " " << l.pos_.size() << " " << l.measures_.size() << std::endl;
        for (int i = 0; i < l.cam_unwarp_.size(); i++)
        {
                o << *(l.cam_unwarp_[i]);
                o << *(l.cam_intern_[i]);
        }
        for (int i = 0; i < l.pos_.size(); i++)
                o << *(l.pos_[i]);
        for (int i = 0; i < l.measures_.size(); i++)
                o << *(l.measures_[i]);
        return o;
}