vcglib/wrap/tsai/tsaimethods.cpp

/***************************************************************************
 *   Copyright (C) 2008 by Luca Baronti   				   *
 *   lbaronti@gmail.com   						   *
 *                                                                         *
 *   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, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

/***************************************************************************
  History
Modified by sottile on July 2009
****************************************************************************/

/****************************************************************************
TSAI METHODS
  Interface class between the tsai.lib and the project structure.
  It is sufficient calling <alignImage> with correct parameters, to
  get a calibrated shot.
  -
  Comment:
  Sometimes the Tsai Lib returns a irregular shifted translation values.
  Since now no solution found.
*****************************************************************************/
extern "C" {
#include <../meshlab/src/external/tsai-30b3/cal_main.h>
}

#include "tsaimethods.h"		//base header

#include <vcg/math/camera.h>

/*********************************************
...
*********************************************/
bool TsaiMethods::calibrate( vcg::Shot<double>* shot,std::list<TsaiCorrelation>* corr, bool p_foc)
{
  bool my_ret_val=false;

  if(corr->size() >= MIN_POINTS_FOR_CALIBRATE){

    //initialize_photometrics_parms ();
    Shot2Tsai(shot);

    if(createDataSet(corr,shot))
    {
      if (p_foc)
        //noncoplanar_calibration();
        noncoplanar_calibration_with_full_optimization ();
      else
        noncoplanar_extrinsic_parameter_estimation ();

      Tsai2Shot(shot);
      my_ret_val = true;
    }
    else my_ret_val = false;
  }
  return my_ret_val;
}

/*********************************************
CREATE DATA SET
*********************************************/
//TOGLIERE SHOT DAI PARAMETRI!
bool TsaiMethods::createDataSet(std::list<TsaiCorrelation>* corr,vcg::Shot<double>* s)
{
  bool my_ret_val=false;

  vcg::Point3d *p1;
  vcg::Point2d *p2;
        int count=0;

        std::list<TsaiCorrelation>::iterator it_c;
  TsaiCorrelation* c;

        double ratio = s->Intrinsics.ViewportPx.X()/(double) s->Intrinsics.ViewportPx.Y();

        for ( it_c= corr->begin() ; it_c !=corr->end(); it_c++ ){
                c=&*it_c;
                p1=&(c->point3d);
    p2=&(c->point2d);

    if(p1!=NULL && p2!=NULL)
    {
                        cd.xw[count] = p1->X();
                        cd.yw[count] = p1->Y();
                        cd.zw[count] = p1->Z();
//                        cd.Xf[count] = (p2->X()+1)/2.0 * cp.Cx*2.0;
//                        cd.Yf[count] = ((-p2->Y())+1)/2.0 * cp.Cy*2.0;

                        cd.Xf[count] = ((p2->X()/ratio) +1)/2.0 * cp.Cx*2.0;
                        cd.Yf[count] = ((-p2->Y())+1)/2.0 * cp.Cy*2.0;
      count++;
    }
    if(count>=MAX_POINTS) break;

  }//all corrs
        assert(count==corr->size());
        cd.point_count = count;
        //qDebug("all points: %i",cd.point_count);
        if(count>0 && count<MAX_POINTS && count>10) my_ret_val = true;

        return my_ret_val;
}


/*********************************************
SHOT 2 TSAI
Transformation of the camera data between tsai structure and vcg structure
*********************************************/
void TsaiMethods::Shot2Tsai(vcg::Shot<double>* shot){
  vcg::Matrix44d mat = shot->Extrinsics.Rot();

  vcg::Point3d view_p = shot->Extrinsics.Tra();

  cc.r1 = mat[0][0];  cc.r2 = mat[0][1]; cc.r3 = mat[0][2];
  cc.r4 = -mat[1][0];  cc.r5 = -mat[1][1]; cc.r6 = -mat[1][2];
  cc.r7 = -mat[2][0];  cc.r8 = -mat[2][1]; cc.r9 = -mat[2][2];


  vcg::Point3<vcg::Shot<double>::ScalarType> tl;//(-cc.Tx,cc.Ty,cc.Tz);
  tl = mat* shot->Extrinsics.Tra();
  cc.Tx = -tl[0];
  cc.Ty = tl[1];
  cc.Tz = tl[2];

  //cc.Tx = view_p.X();
  //cc.Ty = view_p.Y();
  //cc.Tz = view_p.Z();

  Cam2Tsai(shot);
}

/*********************************************
TSAI 2 SHOT
Transformation of the camera data between tsai structure and vcg structure
*********************************************/
void TsaiMethods::Tsai2Shot(vcg::Shot<double>* shot, bool p_foc ) {

  if(p_foc)
    shot->Intrinsics.FocalMm =	cc.f;//*cp.sx;// *SCALE_FACTOR;
  /* old ones
  shot->Intrinsics.DistorCenterPx[0] = cc.p1;
  shot->Intrinsics.DistorCenterPx[1] = cc.p2;

  shot->Intrinsics.DistorCenterPx[0] = shot->Intrinsics.CenterPx.X()+(cc.p1/shot->Intrinsics.PixelSizeMm.X());
  shot->Intrinsics.DistorCenterPx[1] = shot->Intrinsics.CenterPx.Y()+(cc.p2/shot->Intrinsics.PixelSizeMm.Y());
  */
  shot->Intrinsics.DistorCenterPx[0] = cp.Cx;
  shot->Intrinsics.DistorCenterPx[1] = cp.Cy;

  shot->Intrinsics.k[0]=cc.kappa1;

  /* ROTATION */
  vcg::Matrix44<vcg::Shot<double>::ScalarType> mat;
  vcg::Matrix44<vcg::Shot<double>::ScalarType> s_mat=shot->Extrinsics.Rot();
  mat.SetIdentity();
  mat[0][0]=cc.r1; 	mat[0][1]=cc.r2; 	mat[0][2]=cc.r3;
  mat[1][0]=-cc.r4; 	mat[1][1]=-cc.r5; 	mat[1][2]=-cc.r6;
  mat[2][0]=-cc.r7; 	mat[2][1]=-cc.r8; 	mat[2][2]=-cc.r9;

  shot->Extrinsics.SetRot(mat);

  /* TRANSLATION */
  vcg::Point3d tl = shot->Extrinsics.Tra();

  tl = vcg::Inverse(shot->Extrinsics.Rot())* vcg::Point3d(-cc.Tx,cc.Ty,cc.Tz);

        shot->Extrinsics.SetTra(tl);
}

void TsaiMethods::Cam2Tsai(vcg::Shot<double> *s){

  cp.Ncx = s->Intrinsics.ViewportPx.X();	// [sel]     Number of sensor elements in camera's x direction //
  cp.Nfx = s->Intrinsics.ViewportPx.X();	// [pix]     Number of pixels in frame grabber's x direction   //
  cp.dx  = s->Intrinsics.PixelSizeMm.X();//*SCALE_FACTOR;	// [mm/sel]  X dimension of camera's sensor element (in mm)    //
  cp.dy  = s->Intrinsics.PixelSizeMm.Y();//*SCALE_FACTOR;	// [mm/sel]  Y dimension of camera's sensor element (in mm)    //

  cp.dpx = cp.dx * cp.Nfx/cp.Ncx;	// [mm/pix]  Effective X dimension of pixel in frame grabber   //
  cp.dpy = cp.dy;	// [mm/pix]  Effective Y dimension of pixel in frame grabber   //

  cp.Cx  = s->Intrinsics.CenterPx.X();	// [pix]     Z axis intercept of camera coordinate system      //
  cp.Cy  = s->Intrinsics.CenterPx.Y();	// [pix]     Z axis intercept of camera coordinate system      //

  cp.sx  = 1.0;	// []        Scale factor to compensate for any error in dpx   //

  cc.f = s->Intrinsics.FocalMm;// *SCALE_FACTOR;
  cc.kappa1 = s->Intrinsics.k[0];
}