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); }