a_persp_mes.cxx

Go to the documentation of this file.

/*
         Copyright 2000-18 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.h"
#include "a_2dh.h"
#include "a_3dh.h"
 
//---------------------------------------------------------------------------
class a_mat_test : public a_mat
{
        public:
                a_mat_test() : a_mat(3,4) {for (int i = 0; i < 3; i++) (*this)(i,i) = 1;}
};
//---------------------------------------------------------------------------
a_mat_test * a_persp_mes::T_ = new a_mat_test();
//---------------------------------------------------------------------------
a_persp_mes::a_persp_mes()
{
        for (int i = 0; i < 2; i++)
                p2_[i] = p3_[i] = 0;
        p3_[2] = 0;
        ref_pos_ = 0;
        ref_cam_ = 0;
}
//---------------------------------------------------------------------------
a_persp_mes::a_persp_mes(double mx, double my, double mz, double mu, double mv, int pos, int cam)
{
        p3_[0] = mx; p3_[1] = my; p3_[2] = mz;
        p2_[0] = mu; p2_[1] = mv;
        ref_pos_ = pos;
        ref_cam_ = cam;
}
//---------------------------------------------------------------------------
double a_persp_mes::residual(a_unwarp * unwarp,a_intern * intern, a_pos2 * pos)
{
        double un,vn;
        unwarp->unwarp(p3_[0],p3_[1],un,vn);
        a_3dh p3d(p3_[0],p3_[1],p3_[2]);
        a_mat a = (*pos)*p3d;
        a_mat b = (*T_)*a;
        a_mat c = (*intern)*b;
        double rx = un-c(0,0)/c(2,0);
        double ry = vn-c(1,0)/c(2,0);
        return (rx*rx+ry*ry);
}
//---------------------------------------------------------------------------
std::istream& operator>> (std::istream& i, a_persp_mes& l)
{
        for (int k = 0; k < 3; k++)
                i >> l.p3_[k];
        for (int k = 0; k < 2; k++)
                i >> l.p2_[k];
        i >> l.ref_cam_ >> l.ref_pos_;
        return i;
}
//---------------------------------------------------------------------------
std::ostream& operator<< (std::ostream& o, const a_persp_mes& l)
{
        for (int k = 0; k < 3; k++)
                o << l.p3_[k] << " ";
        for (int k = 0; k < 2; k++)
                o << l.p2_[k] << " ";
        o << l.ref_cam_ << " " << l.ref_pos_ << std::endl;
        return o;
}