double pkmGaussianMixtureModel::multinormalDistribution(const CvMat *pts, const CvMat *mean, const CvMat *covar)
{
int dimensions = 2;
// add a tiny bit because of small samples
CvMat *covarShifted = cvCreateMat(2, 2, CV_64FC1);
cvAddS( covar, cvScalarAll(0.001), covarShifted);
// calculate the determinant
double det = cvDet(covarShifted);
// invert covariance
CvMat *covarInverted = cvCreateMat(2, 2, CV_64FC1);
cvInvert(covarShifted, covarInverted);
double ff = (1.0/(2.0*(double)PI))*(pow(det,-0.5));
CvMat *centered = cvCreateMat(2, 1, CV_64FC1);
cvSub(pts, mean, centered);
CvMat *invxmean = cvCreateMat(2, 1, CV_64FC1);
//cvGEMM(covarInverted, centered, 1., NULL, 1., invxmean);
cvMatMul(covarInverted, centered, invxmean);
cvMul(centered, invxmean, invxmean);
CvScalar sum = cvSum(invxmean);
/*
printf("covar: %f %f %f %f\n", cvmGet(covar, 0, 0), cvmGet(covar, 0, 1), cvmGet(covar, 1, 0), cvmGet(covar, 1, 1));
printf("covarShifted: %f %f %f %f\n", cvmGet(covarShifted, 0, 0), cvmGet(covarShifted, 0, 1), cvmGet(covarShifted, 1, 0), cvmGet(covarShifted, 1, 1));
printf("det: %f\n", det);
printf("covarInverted: %f %f %f %f\n", cvmGet(covarInverted, 0, 0), cvmGet(covarInverted, 0, 1), cvmGet(covarInverted, 1, 0), cvmGet(covarShifted, 1, 1));
printf("ff: %f\n", ff);
printf("pts: %f %f)\n", cvmGet(pts, 0, 0), cvmGet(pts, 1, 0));
printf("mean: %f %f)\n", cvmGet(mean, 0, 0), cvmGet(mean, 1, 0));
printf("centered: %f %f)\n", cvmGet(centered, 0, 0), cvmGet(centered, 1, 0));
printf("invxmean: %f %f)\n", cvmGet(invxmean, 0, 0), cvmGet(invxmean, 1, 0));
printf("scalar: %f %f %f %f\n", sum.val[0], sum.val[1], sum.val[2], sum.val[3]);
*/
cvReleaseMat(&covarShifted);
cvReleaseMat(&covarInverted);
cvReleaseMat(¢ered);
cvReleaseMat(&invxmean);
return ff * exp(-0.5*sum.val[0]);
}
SimilarityTransform * AffineTransform::getSimilarityTransform() const
{
double subMatData[4];
CvMat subMat = cvMat(2, 2, CV_64FC1, subMatData);
subMatData[0] = data_[0];
subMatData[1] = data_[1];
subMatData[2] = data_[3];
subMatData[3] = data_[4];
double scale = sqrt(cvDet(&subMat));
//double theta = acos((*(const Transform *)this)(0,0)/scale); //Todo: do a LS fit?
double theta = asin((*(const Transform *)this)(0,1)/scale); //Todo: do a LS fit?
CvPoint2D64f translation = cvPoint2D64f((*(const Transform *)this)(0,2), (*(const Transform *)this)(1,2));
SimilarityTransform * pm = new SimilarityTransform(theta, scale, translation);
//for(int x=0; x<2; x++)
// for(int y=0; y<2; y++)
// m(x,y) = scale_ * cvmGet(*this, x, y);
return pm;
}
float MixGaussian::GetProbability(CvMat * Sample)
{
double P = 0.0;
CvMat *diffMat = cvCloneMat(Sample);
CvMat *diffMatT = cvCreateMat(1, _nDim, CV_64FC1);
double expo;
CvMat expoMat = cvMat(1, 1, CV_64FC1, &expo);
for(int k = 0; k < _nMixture ; k++) {
cvSub(Sample, _MeanMat[k], diffMat);
cvTranspose(diffMat, diffMatT);
cvMatMul(_CovMatI[k], diffMat, diffMat);
cvMatMul(diffMatT, diffMat, &expoMat);
expo *= (-0.5);
P += (_Weight[k] * 1.0 / (pow(2 * CV_PI, 1.5) * sqrt(cvDet(_CovMat[k]))) * exp(expo));
}
cvReleaseMat(&diffMat);
cvReleaseMat(&diffMatT);
return P;
}
CV_IMPL void
cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr,
CvMat *rotMatr, CvMat *posVect,
CvMat *rotMatrX, CvMat *rotMatrY,
CvMat *rotMatrZ, CvPoint3D64f *eulerAngles)
{
CvMat *tmpProjMatr = 0;
CvMat *tmpMatrixD = 0;
CvMat *tmpMatrixV = 0;
CvMat *tmpMatrixM = 0;
CV_FUNCNAME("cvDecomposeProjectionMatrix");
__BEGIN__;
/* Validate parameters. */
if(projMatr == 0 || calibMatr == 0 || rotMatr == 0 || posVect == 0)
CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
if(!CV_IS_MAT(projMatr) || !CV_IS_MAT(calibMatr) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(posVect))
CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
if(projMatr->cols != 4 || projMatr->rows != 3)
CV_ERROR(CV_StsUnmatchedSizes, "Size of projection matrix must be 3x4!");
if(calibMatr->cols != 3 || calibMatr->rows != 3 || rotMatr->cols != 3 || rotMatr->rows != 3)
CV_ERROR(CV_StsUnmatchedSizes, "Size of calibration and rotation matrices must be 3x3!");
if(posVect->cols != 1 || posVect->rows != 4)
CV_ERROR(CV_StsUnmatchedSizes, "Size of position vector must be 4x1!");
CV_CALL(tmpProjMatr = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixD = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixV = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixM = cvCreateMat(3, 3, CV_64F));
/* Compute position vector. */
cvSetZero(tmpProjMatr); // Add zero row to make matrix square.
int i, k;
for(i = 0; i < 3; i++)
for(k = 0; k < 4; k++)
cvmSet(tmpProjMatr, i, k, cvmGet(projMatr, i, k));
CV_CALL(cvSVD(tmpProjMatr, tmpMatrixD, NULL, tmpMatrixV, CV_SVD_MODIFY_A + CV_SVD_V_T));
/* Save position vector. */
for(i = 0; i < 4; i++)
cvmSet(posVect, i, 0, cvmGet(tmpMatrixV, 3, i)); // Solution is last row of V.
/* Compute calibration and rotation matrices via RQ decomposition. */
cvGetCols(projMatr, tmpMatrixM, 0, 3); // M is first square matrix of P.
assert(cvDet(tmpMatrixM) != 0.0); // So far only finite cameras could be decomposed, so M has to be nonsingular [det(M) != 0].
CV_CALL(cvRQDecomp3x3(tmpMatrixM, calibMatr, rotMatr, rotMatrX, rotMatrY, rotMatrZ, eulerAngles));
__END__;
cvReleaseMat(&tmpProjMatr);
cvReleaseMat(&tmpMatrixD);
cvReleaseMat(&tmpMatrixV);
cvReleaseMat(&tmpMatrixM);
}
bool SparseRec2View::estimateRelativePose()
{
double U[9], S[9], VT[9];
double W[9] =
{ 0.0, -1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, 1.0 };
double WT[9] =
{ 0.0, 1.0, 0.0,
-1.0, 0.0, 0.0,
0.0, 0.0, 1.0 };
double E[9];
double tmp9[9];
double u3[3], Ra[9], Rb[9];
CreateCvMatHead(_U,3,3,U);
CreateCvMatHead(_S,3,3,S);
CreateCvMatHead(_VT,3,3,VT);
CreateCvMatHead(_K,3,3,K);
CreateCvMatHead(_E,3,3,E);
CreateCvMatHead(_F,3,3,F);
CreateCvMatHead(_tmp9,3,3,tmp9);
CreateCvMatHead(_u3,3,1,u3);
CreateCvMatHead(_Ra,3,3,Ra);
CreateCvMatHead(_Rb,3,3,Rb);
CreateCvMatHead(_W,3,3,W);
CreateCvMatHead(_WT,3,3,WT);
//get essential matrix
cvGEMM(&_K,&_F,1,0,0,&_tmp9, CV_GEMM_A_T);
cvMatMul(&_tmp9, &_K, &_E); //E = K2' * F * K1; % K2==K1 currently
cvSVD(&_E, &_S, &_U, &_VT, CV_SVD_V_T);
/* Now find R and t */
u3[0] = U[2]; u3[1] = U[5]; u3[2] = U[8]; //u3 = U*[0;0;1];
//two possible R UDVT, UDTVT
cvMatMul(&_U, &_W, &_tmp9);
cvMatMul(&_tmp9, &_VT, &_Ra); //Ra = U*W*V';
cvMatMul(&_U, &_WT, &_tmp9);
cvMatMul(&_tmp9, &_VT, &_Rb); //Rb = U*W'*V';
if( cvDet(&_Ra) <0 )
cvScale(&_Ra, &_Ra, -1);
if( cvDet(&_Rb) <0 )
cvScale(&_Rb, &_Rb, -1);
double P1[12] = {
K[0],K[1],K[2],0,
K[3],K[4],K[5],0,
K[6],K[7],K[8],0
};
double P2[12];
CreateCvMatHead(_P2,3,4,P2);
// test Ra
P2[0]=Ra[0]; P2[1]=Ra[1]; P2[2]=Ra[2]; P2[3]=u3[0];
P2[4]=Ra[3]; P2[5]=Ra[4]; P2[6]=Ra[5]; P2[7]=u3[1];
P2[8]=Ra[6]; P2[9]=Ra[7]; P2[10]=Ra[8]; P2[11]=u3[2];
cvMatMul(&_K,&_P2,&_P2); //P2 = K*[Ra,u3];
int c1_pos = 0, c1_neg = 0;
int c2_pos = 0, c2_neg = 0;
for(int i=0; i<(int)p1.size(); ++i) {
if(!inliers[i]) continue;
double X[3];
helper::triangulate(p1[i].x, p1[i].y,
p2[i].x, p2[i].y, P1, P2, X);
double X2[3]={X[0],X[1],X[2]};
CreateCvMatHead(_X2,3,1,X2);
cvMatMul(&_Ra, &_X2, &_X2);
cvAdd(&_X2, &_u3, &_X2);
if (X[2] > 0) c1_pos++;
else c1_neg++;
if (X2[2] > 0) c2_pos++;
else c2_neg++;
}
//cout<<"Test Ra"<<endl;
//cout<<"+c1="<<c1_pos<<"\t-c1="<<c1_neg<<endl;
//cout<<"+c2="<<c2_pos<<"\t-c2="<<c2_neg<<endl;
if (c1_pos > c1_neg && c2_pos > c2_neg) {
memcpy(R, Ra, 9 * sizeof(double));
t[0] = u3[0]; t[1] = u3[1]; t[2] = u3[2];
} else if (c1_pos < c1_neg && c2_pos < c2_neg) {
memcpy(R, Ra, 9 * sizeof(double));
t[0] = -u3[0]; t[1] = -u3[1]; t[2] = -u3[2];
} else {
//test Rb
P2[0]=Rb[0]; P2[1]=Rb[1]; P2[2]=Rb[2]; P2[3]=u3[0];
P2[4]=Rb[3]; P2[5]=Rb[4]; P2[6]=Rb[5]; P2[7]=u3[1];
P2[8]=Rb[6]; P2[9]=Rb[7]; P2[10]=Rb[8]; P2[11]=u3[2];
cvMatMul(&_K,&_P2,&_P2); //P2 = K2*[Rb,u3];
c1_pos = c1_neg = c2_pos = c2_neg = 0;
for(int i=0; i<(int)p1.size(); ++i) {
if(!inliers[i]) continue;
double X[3];
helper::triangulate(p1[i].x, p1[i].y,
p2[i].x, p2[i].y, P1, P2, X);
double X2[3]={X[0],X[1],X[2]};
CreateCvMatHead(_X2,3,1,X2);
cvMatMul(&_Rb, &_X2, &_X2);
cvAdd(&_X2, &_u3, &_X2);
if (X[2] > 0) c1_pos++;
else c1_neg++;
if (X2[2] > 0) c2_pos++;
else c2_neg++;
}
//cout<<"Test Rb"<<endl;
//cout<<"+c1="<<c1_pos<<"\t-c1="<<c1_neg<<endl;
//cout<<"+c2="<<c2_pos<<"\t-c2="<<c2_neg<<endl;
if (c1_pos > c1_neg && c2_pos > c2_neg) {
memcpy(R, Rb, 9 * sizeof(double));
t[0] = u3[0]; t[1] = u3[1]; t[2] = u3[2];
} else if (c1_pos < c1_neg && c2_pos < c2_neg) {
memcpy(R, Rb, 9 * sizeof(double));
t[0] = -u3[0]; t[1] = -u3[1]; t[2] = -u3[2];
} else {
TagE("no case was found!\n");
return false;
}
};
//final triangulate
double tulen = u3[0]*u3[0]+u3[1]*u3[1]+u3[2]*u3[2];
tulen = sqrt(tulen);
u3[0]*=lamda/tulen; u3[1]*=lamda/tulen; u3[2]*=lamda/tulen;
P2[0]=R[0]; P2[1]=R[1]; P2[2]=R[2]; P2[3]=t[0];
P2[4]=R[3]; P2[5]=R[4]; P2[6]=R[5]; P2[7]=t[1];
P2[8]=R[6]; P2[9]=R[7]; P2[10]=R[8]; P2[11]=t[2];
cvMatMul(&_K,&_P2,&_P2);
if(Log::level>=Log::LOG_DEBUG) helper::print(3,4,P2,"Final P2");
double maxu1=-DBL_MAX,minu1=DBL_MAX,maxv1=-DBL_MAX,minv1=DBL_MAX;
double maxu2=-DBL_MAX,minu2=DBL_MAX,maxv2=-DBL_MAX,minv2=DBL_MAX;
for(int i=0; i<(int)p1.size(); ++i) {
if(!inliers[i]) continue;
double X[3];
helper::triangulate(p1[i].x, p1[i].y,
p2[i].x, p2[i].y, P1, P2, X);
if(X[2]>0)
results.push_back(cv::Point3d(X[0],X[1],X[2]));
double u,v;
helper::project(P1,X[0],X[1],X[2], u,v);
double du1=u-p1[i].x, dv1=v-p1[i].y;
LogD(">P1\t%lf\t%lf\n",du1,dv1);
helper::project(P2,X[0],X[1],X[2], u,v);
double du2=u-p2[i].x, dv2=v-p2[i].y;
LogD(">P2\t%lf\t%lf\n",du2,dv2);
maxu1 = std::max(maxu1,du1); minu1 = std::min(minu1,du1);
maxv1 = std::max(maxv1,dv1); minv1 = std::min(minv1,dv1);
maxu2 = std::max(maxu2,du2); minu2 = std::min(minu2,du2);
maxv2 = std::max(maxv2,dv2); minv2 = std::min(minv2,dv2);
}
TagI("reproject error for img1 = (%g ~ %g, %g ~ %g)\n", minu1, maxu1, minv1, maxv1);
TagI("reproject error for img2 = (%g ~ %g, %g ~ %g)\n", minu2, maxu2, minv2, maxv2);
return true;
}
bool
cvFindExtrinsicCameraParams3( const CvMat* obj_points,
const CvMat* img_points, const CvMat* A,
const CvMat* dist_coeffs,
CvMat* r_vec, CvMat* t_vec )
{
bool fGood = true;
const int max_iter = 20;
CvMat *_M = 0, *_Mxy = 0, *_m = 0, *_mn = 0, *_L = 0, *_J = 0;
CV_FUNCNAME( "cvFindExtrinsicCameraParams3" );
__BEGIN__;
int i, j, count;
double a[9], k[4] = { 0, 0, 0, 0 }, R[9], ifx, ify, cx, cy;
double Mc[3] = {0, 0, 0}, MM[9], U[9], V[9], W[3];
double JtJ[6*6], JtErr[6], JtJW[6], JtJV[6*6], delta[6], param[6];
CvPoint3D64f* M = 0;
CvPoint2D64f *m = 0, *mn = 0;
CvMat _a = cvMat( 3, 3, CV_64F, a );
CvMat _R = cvMat( 3, 3, CV_64F, R );
CvMat _r = cvMat( 3, 1, CV_64F, param );
CvMat _t = cvMat( 3, 1, CV_64F, param + 3 );
CvMat _Mc = cvMat( 1, 3, CV_64F, Mc );
CvMat _MM = cvMat( 3, 3, CV_64F, MM );
CvMat _U = cvMat( 3, 3, CV_64F, U );
CvMat _V = cvMat( 3, 3, CV_64F, V );
CvMat _W = cvMat( 3, 1, CV_64F, W );
CvMat _JtJ = cvMat( 6, 6, CV_64F, JtJ );
CvMat _JtErr = cvMat( 6, 1, CV_64F, JtErr );
CvMat _JtJW = cvMat( 6, 1, CV_64F, JtJW );
CvMat _JtJV = cvMat( 6, 6, CV_64F, JtJV );
CvMat _delta = cvMat( 6, 1, CV_64F, delta );
CvMat _param = cvMat( 6, 1, CV_64F, param );
CvMat _dpdr, _dpdt;
if( !CV_IS_MAT(obj_points) || !CV_IS_MAT(img_points) ||
!CV_IS_MAT(A) || !CV_IS_MAT(r_vec) || !CV_IS_MAT(t_vec) )
CV_ERROR( CV_StsBadArg, "One of required arguments is not a valid matrix" );
count = MAX(obj_points->cols, obj_points->rows);
CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
CV_CALL( _Mxy = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( _mn = cvCreateMat( 1, count, CV_64FC2 ));
M = (CvPoint3D64f*)_M->data.db;
m = (CvPoint2D64f*)_m->data.db;
mn = (CvPoint2D64f*)_mn->data.db;
CV_CALL( cvConvertPointsHomogenious( obj_points, _M ));
CV_CALL( cvConvertPointsHomogenious( img_points, _m ));
CV_CALL( cvConvert( A, &_a ));
if( dist_coeffs )
{
CvMat _k;
if( !CV_IS_MAT(dist_coeffs) ||
CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
CV_MAT_DEPTH(dist_coeffs->type) != CV_32F ||
dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 )
CV_ERROR( CV_StsBadArg,
"Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
_k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(dist_coeffs->type)), k );
CV_CALL( cvConvert( dist_coeffs, &_k ));
}
if( CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F ||
r_vec->rows != 1 && r_vec->cols != 1 ||
r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3 )
CV_ERROR( CV_StsBadArg, "Rotation vector must be 1x3 or 3x1 floating-point vector" );
if( CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F ||
t_vec->rows != 1 && t_vec->cols != 1 ||
t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
CV_ERROR( CV_StsBadArg,
"Translation vector must be 1x3 or 3x1 floating-point vector" );
ifx = 1./a[0]; ify = 1./a[4];
cx = a[2]; cy = a[5];
// normalize image points
// (unapply the intrinsic matrix transformation and distortion)
for( i = 0; i < count; i++ )
{
double x = (m[i].x - cx)*ifx, y = (m[i].y - cy)*ify, x0 = x, y0 = y;
// compensate distortion iteratively
if( dist_coeffs )
for( j = 0; j < 5; j++ )
{
double r2 = x*x + y*y;
double icdist = 1./(1 + k[0]*r2 + k[1]*r2*r2);
double delta_x = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
double delta_y = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
x = (x0 - delta_x)*icdist;
y = (y0 - delta_y)*icdist;
}
mn[i].x = x; mn[i].y = y;
// calc mean(M)
Mc[0] += M[i].x;
Mc[1] += M[i].y;
Mc[2] += M[i].z;
}
Mc[0] /= count;
Mc[1] /= count;
Mc[2] /= count;
cvReshape( _M, _M, 1, count );
cvMulTransposed( _M, &_MM, 1, &_Mc );
cvSVD( &_MM, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );
// initialize extrinsic parameters
if( W[2]/W[1] < 1e-3 || count < 4 )
{
// a planar structure case (all M's lie in the same plane)
double tt[3], h[9], h1_norm, h2_norm;
CvMat* R_transform = &_V;
CvMat T_transform = cvMat( 3, 1, CV_64F, tt );
CvMat _H = cvMat( 3, 3, CV_64F, h );
CvMat _h1, _h2, _h3;
if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
cvSetIdentity( R_transform );
if( cvDet(R_transform) < 0 )
cvScale( R_transform, R_transform, -1 );
cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
for( i = 0; i < count; i++ )
{
const double* Rp = R_transform->data.db;
const double* Tp = T_transform.data.db;
const double* src = _M->data.db + i*3;
double* dst = _Mxy->data.db + i*2;
dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0];
dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1];
}
cvFindHomography( _Mxy, _mn, &_H );
cvGetCol( &_H, &_h1, 0 );
_h2 = _h1; _h2.data.db++;
_h3 = _h2; _h3.data.db++;
h1_norm = sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]);
h2_norm = sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]);
cvScale( &_h1, &_h1, 1./h1_norm );
cvScale( &_h2, &_h2, 1./h2_norm );
cvScale( &_h3, &_t, 2./(h1_norm + h2_norm));
cvCrossProduct( &_h1, &_h2, &_h3 );
cvRodrigues2( &_H, &_r );
cvRodrigues2( &_r, &_H );
cvMatMulAdd( &_H, &T_transform, &_t, &_t );
cvMatMul( &_H, R_transform, &_R );
cvRodrigues2( &_R, &_r );
}
else
{
// non-planar structure. Use DLT method
double* L;
double LL[12*12], LW[12], LV[12*12], sc;
CvMat _LL = cvMat( 12, 12, CV_64F, LL );
CvMat _LW = cvMat( 12, 1, CV_64F, LW );
CvMat _LV = cvMat( 12, 12, CV_64F, LV );
CvMat _RRt, _RR, _tt;
CV_CALL( _L = cvCreateMat( 2*count, 12, CV_64F ));
L = _L->data.db;
for( i = 0; i < count; i++, L += 24 )
{
double x = -mn[i].x, y = -mn[i].y;
L[0] = L[16] = M[i].x;
L[1] = L[17] = M[i].y;
L[2] = L[18] = M[i].z;
L[3] = L[19] = 1.;
L[4] = L[5] = L[6] = L[7] = 0.;
L[12] = L[13] = L[14] = L[15] = 0.;
L[8] = x*M[i].x;
L[9] = x*M[i].y;
L[10] = x*M[i].z;
L[11] = x;
L[20] = y*M[i].x;
L[21] = y*M[i].y;
L[22] = y*M[i].z;
L[23] = y;
}
cvMulTransposed( _L, &_LL, 1 );
cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
_RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
cvGetCols( &_RRt, &_RR, 0, 3 );
cvGetCol( &_RRt, &_tt, 3 );
if( cvDet(&_RR) < 0 )
cvScale( &_RRt, &_RRt, -1 );
sc = cvNorm(&_RR);
cvSVD( &_RR, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T );
cvScale( &_tt, &_t, cvNorm(&_R)/sc );
cvRodrigues2( &_R, &_r );
cvReleaseMat( &_L );
}
//
// Check if new r and t are good
//
if ( cvGetReal1D( r_vec, 0 ) ||
cvGetReal1D( r_vec, 1 ) ||
cvGetReal1D( r_vec, 2 ) ||
cvGetReal1D( t_vec, 0 ) ||
cvGetReal1D( t_vec, 1 ) ||
cvGetReal1D( t_vec, 2 ) )
{
//
// perfom this only when the old r and t exist.
//
CvMat * R_inv = cvCreateMat( 3, 3, CV_64FC1 );
CvMat * r_old = cvCreateMat( 3, 1, CV_64FC1 );
CvMat * R_old = cvCreateMat( 3, 3, CV_64FC1 );
CvMat * t_old = cvCreateMat( 3, 1, CV_64FC1 );
// get new center
cvInvert( &_R, R_inv );
double new_center[3];
CvMat newCenter = cvMat( 3, 1, CV_64FC1, new_center );
cvMatMul( R_inv, &_t, &newCenter );
cvScale( &newCenter, &newCenter, -1 );
// get old center
cvConvert( r_vec, r_old );
cvConvert( t_vec, t_old );
cvRodrigues2( r_old, R_old );
cvInvert( R_old, R_inv );
double old_center[3];
CvMat oldCenter = cvMat( 3, 1, CV_64FC1, old_center );
cvMatMul( R_inv, t_old, &oldCenter );
cvScale( &oldCenter, &oldCenter, -1 );
// get difference
double diff_center = 0;
for ( int i = 0; i < 3 ; i ++ )
diff_center += pow( new_center[i] - old_center[i], 2 );
diff_center = sqrt( diff_center );
if ( diff_center > 300 )
{
printf("diff_center = %.2f --> set initial r and t as same as the previous\n", diff_center);
cvConvert(r_vec, &_r);
cvConvert(t_vec, &_t);
fGood = false;
}
// else printf("diff_center = %.2f\n", diff_center );
cvReleaseMat( &R_inv );
cvReleaseMat( &r_old );
cvReleaseMat( &R_old );
cvReleaseMat( &t_old );
}
if ( fGood )
{
CV_CALL( _J = cvCreateMat( 2*count, 6, CV_64FC1 ));
cvGetCols( _J, &_dpdr, 0, 3 );
cvGetCols( _J, &_dpdt, 3, 6 );
// refine extrinsic parameters using iterative algorithm
for( i = 0; i < max_iter; i++ )
{
double n1, n2;
cvReshape( _mn, _mn, 2, 1 );
cvProjectPoints2( _M, &_r, &_t, &_a, dist_coeffs,
_mn, &_dpdr, &_dpdt, 0, 0, 0 );
cvSub( _m, _mn, _mn );
cvReshape( _mn, _mn, 1, 2*count );
cvMulTransposed( _J, &_JtJ, 1 );
cvGEMM( _J, _mn, 1, 0, 0, &_JtErr, CV_GEMM_A_T );
cvSVD( &_JtJ, &_JtJW, 0, &_JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
if( JtJW[5]/JtJW[0] < 1e-12 )
break;
cvSVBkSb( &_JtJW, &_JtJV, &_JtJV, &_JtErr,
&_delta, CV_SVD_U_T + CV_SVD_V_T );
cvAdd( &_delta, &_param, &_param );
n1 = cvNorm( &_delta );
n2 = cvNorm( &_param );
if( n1/n2 < 1e-10 )
break;
}
_r = cvMat( r_vec->rows, r_vec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), param );
_t = cvMat( t_vec->rows, t_vec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), param + 3 );
cvConvert( &_r, r_vec );
cvConvert( &_t, t_vec );
}
__END__;
cvReleaseMat( &_M );
cvReleaseMat( &_Mxy );
cvReleaseMat( &_m );
cvReleaseMat( &_mn );
cvReleaseMat( &_L );
cvReleaseMat( &_J );
return fGood;
}
int DoubleSidesModelFitting::FullFit(const vector<CvPoint> & list_left, const vector<CvPoint> & list_right, int horizon, double& belief)
{
int max_counter = 0;
//double max_weight = 0;
for(int cycle_counter = 0; cycle_counter < DSMF_RANSAC_MAX_TIMES; ++cycle_counter)
{
int det_times = 0;
//Randomly find a sample of points that are good for solving the equation.
do
{
int rndind[4];
rndind[0] = cvFloor(cvRandReal(&rng) * total_length);
do
{
rndind[1] = cvFloor(cvRandReal(&rng) * total_length);
}
while(rndind[1] == rndind[0]);
do
{
rndind[2] = cvFloor(cvRandReal(&rng) * total_length);
}
while(rndind[2] == rndind[0] || rndind[2] == rndind[1]);
do
{
rndind[3] = cvFloor(cvRandReal(&rng) * total_length);
}
while(rndind[3] == rndind[2] || rndind[3] == rndind[1] || rndind[3] == rndind[0]);
for(int i = 0; i < 4; i++)
{
if(rndind[i] < static_cast<int>(list_left.size()))
{
const CvPoint & p = list_left.at(rndind[i]);
cvmSet(matA, i, 0, 1.0 / (p.y - horizon));
cvmSet(matA, i, 1, p.y - horizon);
cvmSet(matA, i, 2, 0.0);
cvmSet(matA, i, 3, 1.0);
cvmSet(matB, i, 0, p.x);
}
else
{
const CvPoint & p = list_right.at(rndind[i] - list_left.size());
cvmSet(matA, i, 0, 1.0 / (p.y - horizon));
cvmSet(matA, i, 1, 0.0);
cvmSet(matA, i, 2, p.y - horizon);
cvmSet(matA, i, 3, 1.0);
cvmSet(matB, i, 0, p.x);
}
}
}
while(fabs(cvDet(matA)) < DSMF_RANSAC_DET_THRESHOLD && ++det_times < DSMF_RANSAC_MAX_DET_TIMES);
if(det_times >= DSMF_RANSAC_MAX_DET_TIMES) continue;
//Get a model as candidate.
if(cvSolve(matA, matB, matX) == 0) continue;
//Calculate the difference between the model and the actual points.
cvGEMM(matFullA, matX, 1, matFullB, -1, matFullDiff);
//Check whether the candidate is a good fit.
int counter = 0;
for(int i = 0; i < (int)total_length; i++)
{
if(fabs(cvmGet(matFullDiff, i, 0)) < DSMF_RANSAC_TOL)
{
counter++;
}
}
//Check whether the model is the best one ever. If so, remember the points that are close to it.
if(counter > max_counter)
{
cvCopy(matX, matOptX);
max_counter = counter;
filtered_index.clear();
for(int i = 0; i < (int)total_length; i++)
{
if(fabs(cvmGet(matFullDiff, i, 0)) < DSMF_RANSAC_TOL_STRICT)
{
filtered_index.push_back(i);
}
}
}
}
//Check whether the best found model is a good fit (acceptable).
if(filtered_index.size() > max(4, DSMF_RANSAC_WEIGHT_ACPT_RATE * max_counter))
{
belief = (double)filtered_index.size() / max_counter;
FilteredFit();
return DSMF_SUCCESS;
}
else
{
return DSMF_FAIL_NO_ACCEPTED_RESULT;
}
return DSMF_SUCCESS;
}
void cvComputeRQDecomposition(CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ, CvMat *matrixQx, CvMat *matrixQy, CvMat *matrixQz, CvPoint3D64f *eulerAngles)
{
CvMat *tmpMatrix1 = 0;
CvMat *tmpMatrix2 = 0;
CvMat *tmpMatrixM = 0;
CvMat *tmpMatrixR = 0;
CvMat *tmpMatrixQ = 0;
CvMat *tmpMatrixQx = 0;
CvMat *tmpMatrixQy = 0;
CvMat *tmpMatrixQz = 0;
double tmpEulerAngleX, tmpEulerAngleY, tmpEulerAngleZ;
CV_FUNCNAME("cvRQDecomp3x3");
__CV_BEGIN__;
/* Validate parameters. */
if(matrixM == 0 || matrixR == 0 || matrixQ == 0)
CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
if(!CV_IS_MAT(matrixM) || !CV_IS_MAT(matrixR) || !CV_IS_MAT(matrixQ))
CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
if(matrixM->cols != 3 || matrixM->rows != 3 || matrixR->cols != 3 || matrixR->rows != 3 || matrixQ->cols != 3 || matrixQ->rows != 3)
CV_ERROR(CV_StsUnmatchedSizes, "Size of matrices must be 3x3!");
CV_CALL(tmpMatrix1 = cvCreateMat(3, 3, CV_64F));
CV_CALL(tmpMatrix2 = cvCreateMat(3, 3, CV_64F));
CV_CALL(tmpMatrixM = cvCreateMat(3, 3, CV_64F));
CV_CALL(tmpMatrixR = cvCreateMat(3, 3, CV_64F));
CV_CALL(tmpMatrixQ = cvCreateMat(3, 3, CV_64F));
CV_CALL(tmpMatrixQx = cvCreateMat(3, 3, CV_64F));
CV_CALL(tmpMatrixQy = cvCreateMat(3, 3, CV_64F));
CV_CALL(tmpMatrixQz = cvCreateMat(3, 3, CV_64F));
cvCopy(matrixM, tmpMatrixM);
/* Find Givens rotation Q_x for x axis. */
/*
( 1 0 0 )
Qx = ( 0 c -s ), cos = -m33/sqrt(m32^2 + m33^2), cos = m32/sqrt(m32^2 + m33^2)
( 0 s c )
*/
double x, y, z, c, s;
x = cvmGet(tmpMatrixM, 2, 1);
y = cvmGet(tmpMatrixM, 2, 2);
z = x * x + y * y;
assert(z != 0); // Prevent division by zero.
c = -y / sqrt(z);
s = x / sqrt(z);
cvSetIdentity(tmpMatrixQx);
cvmSet(tmpMatrixQx, 1, 1, c);
cvmSet(tmpMatrixQx, 1, 2, -s);
cvmSet(tmpMatrixQx, 2, 1, s);
cvmSet(tmpMatrixQx, 2, 2, c);
tmpEulerAngleX = acos(c) * 180.0 / CV_PI;
/* Multiply M on the right by Q_x. */
cvMatMul(tmpMatrixM, tmpMatrixQx, tmpMatrixR);
cvCopy(tmpMatrixR, tmpMatrixM);
assert(cvmGet(tmpMatrixM, 2, 1) < CV_VERYSMALLDOUBLE && cvmGet(tmpMatrixM, 2, 1) > -CV_VERYSMALLDOUBLE); // Should actually be zero.
if(cvmGet(tmpMatrixM, 2, 1) != 0.0)
cvmSet(tmpMatrixM, 2, 1, 0.0); // Rectify arithmetic precision error.
/* Find Givens rotation for y axis. */
/*
( c 0 s )
Qy = ( 0 1 0 ), cos = m33/sqrt(m31^2 + m33^2), cos = m31/sqrt(m31^2 + m33^2)
(-s 0 c )
*/
x = cvmGet(tmpMatrixM, 2, 0);
y = cvmGet(tmpMatrixM, 2, 2);
z = x * x + y * y;
assert(z != 0); // Prevent division by zero.
c = y / sqrt(z);
s = x / sqrt(z);
cvSetIdentity(tmpMatrixQy);
cvmSet(tmpMatrixQy, 0, 0, c);
cvmSet(tmpMatrixQy, 0, 2, s);
cvmSet(tmpMatrixQy, 2, 0, -s);
cvmSet(tmpMatrixQy, 2, 2, c);
tmpEulerAngleY = acos(c) * 180.0 / CV_PI;
/* Multiply M*Q_x on the right by Q_y. */
cvMatMul(tmpMatrixM, tmpMatrixQy, tmpMatrixR);
cvCopy(tmpMatrixR, tmpMatrixM);
assert(cvmGet(tmpMatrixM, 2, 0) < CV_VERYSMALLDOUBLE && cvmGet(tmpMatrixM, 2, 0) > -CV_VERYSMALLDOUBLE); // Should actually be zero.
if(cvmGet(tmpMatrixM, 2, 0) != 0.0)
cvmSet(tmpMatrixM, 2, 0, 0.0); // Rectify arithmetic precision error.
/* Find Givens rotation for z axis. */
/*
( c -s 0 )
Qz = ( s c 0 ), cos = -m22/sqrt(m21^2 + m22^2), cos = m21/sqrt(m21^2 + m22^2)
( 0 0 1 )
*/
x = cvmGet(tmpMatrixM, 1, 0);
y = cvmGet(tmpMatrixM, 1, 1);
z = x * x + y * y;
assert(z != 0); // Prevent division by zero.
c = -y / sqrt(z);
s = x / sqrt(z);
cvSetIdentity(tmpMatrixQz);
cvmSet(tmpMatrixQz, 0, 0, c);
cvmSet(tmpMatrixQz, 0, 1, -s);
cvmSet(tmpMatrixQz, 1, 0, s);
cvmSet(tmpMatrixQz, 1, 1, c);
tmpEulerAngleZ = acos(c) * 180.0 / CV_PI;
/* Multiply M*Q_x*Q_y on the right by Q_z. */
cvMatMul(tmpMatrixM, tmpMatrixQz, tmpMatrixR);
assert(cvmGet(tmpMatrixR, 1, 0) < CV_VERYSMALLDOUBLE && cvmGet(tmpMatrixR, 1, 0) > -CV_VERYSMALLDOUBLE); // Should actually be zero.
if(cvmGet(tmpMatrixR, 1, 0) != 0.0)
cvmSet(tmpMatrixR, 1, 0, 0.0); // Rectify arithmetic precision error.
/* Calulate orthogonal matrix. */
/*
Q = QzT * QyT * QxT
*/
cvTranspose(tmpMatrixQz, tmpMatrix1);
cvTranspose(tmpMatrixQy, tmpMatrix2);
cvMatMul(tmpMatrix1, tmpMatrix2, tmpMatrixQ);
cvCopy(tmpMatrixQ, tmpMatrix1);
cvTranspose(tmpMatrixQx, tmpMatrix2);
cvMatMul(tmpMatrix1, tmpMatrix2, tmpMatrixQ);
/* Solve decomposition ambiguity. */
/*
Diagonal entries of R should be positive, so swap signs if necessary.
*/
if(cvmGet(tmpMatrixR, 0, 0) < 0.0) {
cvmSet(tmpMatrixR, 0, 0, -1.0 * cvmGet(tmpMatrixR, 0, 0));
cvmSet(tmpMatrixQ, 0, 0, -1.0 * cvmGet(tmpMatrixQ, 0, 0));
cvmSet(tmpMatrixQ, 0, 1, -1.0 * cvmGet(tmpMatrixQ, 0, 1));
cvmSet(tmpMatrixQ, 0, 2, -1.0 * cvmGet(tmpMatrixQ, 0, 2));
}
if(cvmGet(tmpMatrixR, 1, 1) < 0.0) {
cvmSet(tmpMatrixR, 0, 1, -1.0 * cvmGet(tmpMatrixR, 0, 1));
cvmSet(tmpMatrixR, 1, 1, -1.0 * cvmGet(tmpMatrixR, 1, 1));
cvmSet(tmpMatrixQ, 1, 0, -1.0 * cvmGet(tmpMatrixQ, 1, 0));
cvmSet(tmpMatrixQ, 1, 1, -1.0 * cvmGet(tmpMatrixQ, 1, 1));
cvmSet(tmpMatrixQ, 1, 2, -1.0 * cvmGet(tmpMatrixQ, 1, 2));
}
/* Enforce det(Q) = 1 */
if (cvDet(tmpMatrixQ) < 0)
{
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
cvmSet(tmpMatrixQ, j, i, -cvmGet(tmpMatrixQ, j, i));
}
}
}
/* Save R and Q matrices. */
cvCopy(tmpMatrixR, matrixR);
cvCopy(tmpMatrixQ, matrixQ);
if(matrixQx && CV_IS_MAT(matrixQx) && matrixQx->cols == 3 || matrixQx->rows == 3)
cvCopy(tmpMatrixQx, matrixQx);
if(matrixQy && CV_IS_MAT(matrixQy) && matrixQy->cols == 3 || matrixQy->rows == 3)
cvCopy(tmpMatrixQy, matrixQy);
if(matrixQz && CV_IS_MAT(matrixQz) && matrixQz->cols == 3 || matrixQz->rows == 3)
cvCopy(tmpMatrixQz, matrixQz);
/* Save Euler angles. */
if(eulerAngles)
*eulerAngles = cvPoint3D64f(tmpEulerAngleX, tmpEulerAngleY, tmpEulerAngleZ);
__CV_END__;
cvReleaseMat(&tmpMatrix1);
cvReleaseMat(&tmpMatrix2);
cvReleaseMat(&tmpMatrixM);
cvReleaseMat(&tmpMatrixR);
cvReleaseMat(&tmpMatrixQ);
cvReleaseMat(&tmpMatrixQx);
cvReleaseMat(&tmpMatrixQy);
cvReleaseMat(&tmpMatrixQz);
}
