201 lines
7.4 KiB
C++
201 lines
7.4 KiB
C++
|
/***************************************************************************
|
||
|
|
* 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];
|
||
|
|
}
|