/*************************************************************************** * 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 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 /********************************************* ... *********************************************/ bool TsaiMethods::calibrate( vcg::Shot* shot,std::list* 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* corr,vcg::Shot* s) { bool my_ret_val=false; vcg::Point3d *p1; vcg::Point2d *p2; int count=0; std::list::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 && count10) 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* 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::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* 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::ScalarType> mat; vcg::Matrix44::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 *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]; }