/***************************************************************************
* 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];
}