CV_IMPL const CvMat*
cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement )
{
if( !kalman || !measurement )
CV_Error( CV_StsNullPtr, "" );
/* temp2 = H*P'(k) */
cvMatMulAdd( kalman->measurement_matrix, kalman->error_cov_pre, 0, kalman->temp2 );
/* temp3 = temp2*Ht + R */
cvGEMM( kalman->temp2, kalman->measurement_matrix, 1,
kalman->measurement_noise_cov, 1, kalman->temp3, CV_GEMM_B_T );
/* temp4 = inv(temp3)*temp2 = Kt(k) */
cvSolve( kalman->temp3, kalman->temp2, kalman->temp4, CV_SVD );
/* K(k) */
cvTranspose( kalman->temp4, kalman->gain );
/* temp5 = z(k) - H*x'(k) */
cvGEMM( kalman->measurement_matrix, kalman->state_pre, -1, measurement, 1, kalman->temp5 );
/* x(k) = x'(k) + K(k)*temp5 */
cvMatMulAdd( kalman->gain, kalman->temp5, kalman->state_pre, kalman->state_post );
/* P(k) = P'(k) - K(k)*temp2 */
cvGEMM( kalman->gain, kalman->temp2, -1, kalman->error_cov_pre, 1,
kalman->error_cov_post, 0 );
return kalman->state_post;
}
void cvSoftmaxDer(CvMat * X, CvMat * dE_dY, CvMat * dE_dY_afder) {
CV_FUNCNAME("cvSoftmaxDer");
__BEGIN__;
const int nr = X->rows, nc = X->cols, dtype = CV_MAT_TYPE(X->type);
CvMat * Y = cvCreateMat(nr, nc, dtype);
CvMat * dE_dY_transpose = cvCreateMat(nr, nc, dtype);
CvMat * sum = cvCreateMat(nr, 1, dtype);
CvMat * sum_repeat = cvCreateMat(nr, nc, dtype);
cvSoftmax(X, Y);
if (dE_dY->rows==nc && dE_dY->cols==nr){
cvTranspose(dE_dY,dE_dY_transpose);
cvMul(Y,dE_dY_transpose,dE_dY_afder);
}else{
cvMul(Y,dE_dY,dE_dY_afder);
}
cvReduce(dE_dY_afder,sum,-1,CV_REDUCE_SUM);
cvRepeat(sum,sum_repeat);
cvMul(Y,sum_repeat,sum_repeat);
cvSub(dE_dY_afder,sum_repeat,dE_dY_afder);
cvReleaseMat(&dE_dY_transpose);
cvReleaseMat(&sum);
cvReleaseMat(&sum_repeat);
cvReleaseMat(&Y);
__END__;
}
void Kalman::predict_P() {
// P_pred = F*P*trans(F) + Q
cvTranspose(F, F_trans);
cvMatMul(P, F_trans, P_pred);
cvMatMul(F, P_pred, P_pred);
cvScaleAdd(P_pred, cvScalar(1), Q, P_pred);
}
/**
* Method for transposing a matrix.
*
* @param src A Pointer to the matrix.
*
* @return Returns a pointer to the transposed matrix.
*/
CvMat* LibFaceUtils::transpose(CvMat* src)
{
CvMat* result = cvCreateMat(src->cols, src->rows, src->type);
cvTranspose(src, result);
return result;
}
void BazARTracker::show_result(CamAugmentation &augment, IplImage *video, IplImage **dst)
{
if (getDebugMode()){
if (*dst==0) *dst=cvCloneImage(video);
else cvCopy(video, *dst);
}
CvMat *m = augment.GetProjectionMatrix(0);
// Flip...(This occured from OpenGL origin / camera origin)
CvMat *coordinateTrans = cvCreateMat(3, 3, CV_64F);
cvmSetIdentity(coordinateTrans);
cvmSet(coordinateTrans, 1, 1, -1);
cvmSet(coordinateTrans, 1, 2, m_cparam->cparam.ysize);
cvMatMul(coordinateTrans, m, m);
// extract intrinsic camera parameters from bazar's projection matrix..
GetARToolKitRTfromBAZARProjMat(g_matIntrinsic, m, matCameraRT4_4);
cvTranspose(matCameraRT4_4, matCameraRT4_4);
cvReleaseMat(&coordinateTrans);
// Debug
if (getDebugMode()) {
// draw the coordinate system axes
double w =video->width/2.0;
double h =video->height/2.0;
// 3D coordinates of an object
double pts[4][4] = {
{w,h,0, 1}, // 0,0,0,1
{w*2,h,0, 1}, // w, 0
{w,h*2,0, 1}, // 0, h
{w,h,-w-h, 1} // 0, 0, -
};
CvMat ptsMat, projectedMat;
cvInitMatHeader(&ptsMat, 4, 4, CV_64FC1, pts);
cvInitMatHeader(&projectedMat, 3, 4, CV_64FC1, projected);
cvGEMM(m, &ptsMat, 1, 0, 0, &projectedMat, CV_GEMM_B_T );
for (int i=0; i<4; i++)
{
projected[0][i] /= projected[2][i];
projected[1][i] /= projected[2][i];
}
// draw the projected lines
cvLine(*dst, cvPoint((int)projected[0][0], (int)projected[1][0]),
cvPoint((int)projected[0][1], (int)projected[1][1]), CV_RGB(255,0,0), 2);
cvLine(*dst, cvPoint((int)projected[0][0], (int)projected[1][0]),
cvPoint((int)projected[0][2], (int)projected[1][2]), CV_RGB(0,255,0), 2);
cvLine(*dst, cvPoint((int)projected[0][0], (int)projected[1][0]),
cvPoint((int)projected[0][3], (int)projected[1][3]), CV_RGB(0,0,255), 2);
}
}
void KalmanSensor::update_K(CvMat *P_pred) {
// K = P * trans(H) * inv(H*P*trans(H) + R)
cvTranspose(H, H_trans);
cvMatMul(P_pred, H_trans, K);
cvMatMul(H, K, R_tmp);
cvScaleAdd(R_tmp, cvScalar(1), R, R_tmp);
cvInvert(R_tmp, R_tmp);
cvMatMul(H_trans, R_tmp, K);
cvMatMul(P_pred, K, K);
}
//////////////////////////////////////////////////////////////////////////
// 内部对psrc进行了转置操作
//////////////////////////////////////////////////////////////////////////
void MakeImage(const CvMat *psrc, const char *filename)
{
CvMat *pmat = cvCreateMat(psrc->cols, psrc->rows, psrc->type);
cvTranspose(psrc, pmat);
IplImage *pimage = cvCreateImage(cvGetSize(pmat), IPL_DEPTH_32F, 3);
cvCvtColor(pmat, pimage, CV_GRAY2BGR);
cvSaveImage(filename, pimage);
cvReleaseImage(&pimage);
cvReleaseMat(&pmat);
}
int contoursGetOutline(IplImage *src, IplImage **dst)
{
CvBox2D box;
CvMemStorage *storage = cvCreateMemStorage(0);
IplImage *gray = cvCreateImage(cvGetSize(src), src->depth, 1);
IplImage *temp = cvCreateImage(cvGetSize(src), src->depth, 1);
cvCvtColor(src, gray, CV_RGB2GRAY);
IplImage *mop = contoursGetOutlineMorh(gray, temp, 1);
if ((contorsFindBox(mop, storage, &box)) == 1) {
cvReleaseImage(&mop);
mop = contoursGetOutlineMorh(gray, temp, 0);
}
if ((contorsFindBox(mop, storage, &box)) != 0) {
perror("contoursFindBox: Box not found.");
exit(EXIT_FAILURE);
}
IplImage *rotated = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 3);
skewRotate(src, rotated, box.center, box.angle);
CvRect rect = contoursGetRect(&box);
cvSetImageROI(rotated, rect);
IplImage *crop = cvCreateImage(cvGetSize(rotated), IPL_DEPTH_8U, 3);
cvCopy(rotated, crop, NULL);
cvResetImageROI(rotated);
// delete this before refactor contoursGetRect()
if (crop->width > crop->height) {
cvReleaseImage(&rotated);
rotated = cvCreateImage(cvSize(crop->height, crop->width), crop->depth, crop->nChannels);
cvTranspose(crop, rotated);
cvFlip(rotated, rotated, 1);
*dst = cvCloneImage(rotated);
cvReleaseImage(&rotated);
} else {
*dst = cvCloneImage(crop);
}
cvReleaseImage(&crop);
cvReleaseImage(&mop);
cvReleaseMemStorage(&storage);
return 0;
}
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;
}
ImgAli_common::ImgAli_common()
{
x_kernel=cvCreateMat(3,3,CV_64FC1);
y_kernel=cvCreateMat(3,3,CV_64FC1);
//CV_MAT_ELEM(*x_kernel,double,0,0)=CV_MAT_ELEM(*x_kernel,double,2,0)=-1;
//CV_MAT_ELEM(*x_kernel,double,0,2)=CV_MAT_ELEM(*x_kernel,double,2,2)=1;
//CV_MAT_ELEM(*x_kernel,double,1,0)=-2;CV_MAT_ELEM(*x_kernel,double,1,2)=2;
//CV_MAT_ELEM(*x_kernel,double,0,1)=CV_MAT_ELEM(*x_kernel,double,1,1)=CV_MAT_ELEM(*x_kernel,double,2,1)=0;
CV_MAT_ELEM(*x_kernel,double,0,0)=CV_MAT_ELEM(*x_kernel,double,2,0)=0;
CV_MAT_ELEM(*x_kernel,double,0,2)=CV_MAT_ELEM(*x_kernel,double,2,2)=0;
CV_MAT_ELEM(*x_kernel,double,1,0)=-0.5;CV_MAT_ELEM(*x_kernel,double,1,2)=0.5;
CV_MAT_ELEM(*x_kernel,double,0,1)=CV_MAT_ELEM(*x_kernel,double,1,1)=CV_MAT_ELEM(*x_kernel,double,2,1)=0;
cvTranspose(x_kernel,y_kernel);
//for(int i=0;i<3;i++)
//{
// for(int j=0;j<3;j++)
// {
// CV_MAT_ELEM(*x_kernel,double,i,j)/=8;
// CV_MAT_ELEM(*y_kernel,double,i,j)/=8;
// }
//}
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
CvMat *tgso (CvMat &tmap, int ntex, double sigma, double theta, CvMat &tsim, int useChi2) {
CvMat *roundTmap=cvCreateMat(tmap.rows,tmap.cols,CV_32FC1);
CvMat *comp=cvCreateMat(tmap.rows,tmap.cols,CV_32FC1);
for (int i=0;i<tmap.rows;i++)
for (int j=0;j<tmap.cols;j++)
cvSetReal2D(roundTmap,i,j,cvRound(cvGetReal2D(&tmap,i,j)));
cvSub(&tmap,roundTmap,comp);
if (cvCountNonZero(comp)) {
printf("texton labels not integral");
cvReleaseMat(&roundTmap);
cvReleaseMat(&comp);
exit(1);
}
double min,max;
cvMinMaxLoc(&tmap,&min,&max);
if (min<1 && max>ntex) {
char *msg=new char[50];
printf(msg,"texton labels out of range [1,%d]",ntex);
cvReleaseMat(&roundTmap);
cvReleaseMat(&comp);
exit(1);
}
cvReleaseMat(&roundTmap);
cvReleaseMat(&comp);
double wr=floor(sigma); //sigma=radius (Leo)
CvMat *x=cvCreateMat(1,wr-(-wr)+1, CV_64FC1);
CvMat *y=cvCreateMat(wr-(-wr)+1,1, CV_64FC1);
CvMat *u=cvCreateMat(wr-(-wr)+1,wr-(-wr)+1, CV_64FC1);
CvMat *v=cvCreateMat(wr-(-wr)+1,wr-(-wr)+1, CV_64FC1);
CvMat *gamma=cvCreateMat(u->rows,v->rows, CV_64FC1);
// Set x,y directions
for (int j=-wr;j<=wr;j++) {
cvSetReal2D(x,0,(j+wr),j);
cvSetReal2D(y,(j+wr),0,j);
}
// Set u,v, meshgrids
for (int i=0;i<u->rows;i++) {
cvRepeat(x,u);
cvRepeat(y,v);
}
// Compute the gamma matrix from the grid
for (int i=0;i<u->rows;i++)
for (int j=0;j<u->cols;j++)
cvSetReal2D(gamma,i,j,atan2(cvGetReal2D(v,i,j),cvGetReal2D(u,i,j)));
cvReleaseMat(&x);
cvReleaseMat(&y);
CvMat *sum=cvCreateMat(u->rows,u->cols, CV_64FC1);
cvMul(u,u,u);
cvMul(v,v,v);
cvAdd(u,v,sum);
CvMat *mask=cvCreateMat(u->rows,u->cols, CV_8UC1);
cvCmpS(sum,sigma*sigma,mask,CV_CMP_LE);
cvConvertScale(mask,mask,1.0/255);
cvSetReal2D(mask,wr,wr,0);
int count=cvCountNonZero(mask);
cvReleaseMat(&u);
cvReleaseMat(&v);
cvReleaseMat(&sum);
CvMat *sub=cvCreateMat(mask->rows,mask->cols, CV_64FC1);
CvMat *side=cvCreateMat(mask->rows,mask->cols, CV_8UC1);
cvSubS(gamma,cvScalar(theta),sub);
cvReleaseMat(&gamma);
for (int i=0;i<mask->rows;i++){
for (int j=0;j<mask->cols;j++) {
double n=cvmGet(sub,i,j);
double n_mod = n-floor(n/(2*M_PI))*2*M_PI;
cvSetReal2D(side,i,j, 1 + int(n_mod < M_PI));
}
}
cvMul(side,mask,side);
cvReleaseMat(&sub);
cvReleaseMat(&mask);
CvMat *lmask=cvCreateMat(side->rows,side->cols, CV_8UC1);
CvMat *rmask=cvCreateMat(side->rows,side->cols, CV_8UC1);
cvCmpS(side,1,lmask,CV_CMP_EQ);
cvCmpS(side,2,rmask,CV_CMP_EQ);
int count1=cvCountNonZero(lmask), count2=cvCountNonZero(rmask);
if (count1 != count2) {
printf("Bug: imbalance\n");
}
CvMat *rlmask=cvCreateMat(side->rows,side->cols, CV_32FC1);
CvMat *rrmask=cvCreateMat(side->rows,side->cols, CV_32FC1);
cvConvertScale(lmask,rlmask,1.0/(255*count)*2);
cvConvertScale(rmask,rrmask,1.0/(255*count)*2);
cvReleaseMat(&lmask);
cvReleaseMat(&rmask);
cvReleaseMat(&side);
int h=tmap.rows;
int w=tmap.cols;
CvMat *d = cvCreateMat(h*w,ntex,CV_32FC1);
CvMat *coltemp = cvCreateMat(h*w,1,CV_32FC1);
CvMat *tgL = cvCreateMat(h,w, CV_32FC1);
CvMat *tgR = cvCreateMat(h,w, CV_32FC1);
CvMat *temp = cvCreateMat(h,w,CV_8UC1);
CvMat *im = cvCreateMat(h,w, CV_32FC1);
CvMat *sub2 = cvCreateMat(h,w,CV_32FC1);
CvMat *sub2t = cvCreateMat(w,h,CV_32FC1);
CvMat *prod = cvCreateMat(h*w,ntex,CV_32FC1);
CvMat reshapehdr,*reshape;
CvMat* tgL_pad = cvCreateMat(h+rlmask->rows-1,w+rlmask->cols-1,CV_32FC1);
CvMat* tgR_pad = cvCreateMat(h+rlmask->rows-1,w+rlmask->cols-1,CV_32FC1);
CvMat* im_pad = cvCreateMat(h+rlmask->rows-1,w+rlmask->cols-1,CV_32FC1);
CvMat *tg=cvCreateMat(h,w,CV_32FC1);
cvZero(tg);
if (useChi2 == 1){
CvMat* temp_add1 = cvCreateMat(h,w,CV_32FC1);
for (int i=0;i<ntex;i++) {
cvCmpS(&tmap,i+1,temp,CV_CMP_EQ);
cvConvertScale(temp,im,1.0/255);
cvCopyMakeBorder(tgL,tgL_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvCopyMakeBorder(tgR,tgR_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvCopyMakeBorder(im,im_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvFilter2D(im_pad,tgL_pad,rlmask,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2));
cvFilter2D(im_pad,tgR_pad,rrmask,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2));
cvGetSubRect(tgL_pad,tgL,cvRect((rlmask->cols-1)/2,(rlmask->rows-1)/2,tgL->cols,tgL->rows));
cvGetSubRect(tgR_pad,tgR,cvRect((rlmask->cols-1)/2,(rlmask->rows-1)/2,tgR->cols,tgR->rows));
cvSub(tgL,tgR,sub2);
cvPow(sub2,sub2,2.0);
cvAdd(tgL,tgR,temp_add1);
cvAddS(temp_add1,cvScalar(0.0000000001),temp_add1);
cvDiv(sub2,temp_add1,sub2);
cvAdd(tg,sub2,tg);
}
cvScale(tg,tg,0.5);
cvReleaseMat(&temp_add1);
}
else{// if not chi^2
for (int i=0;i<ntex;i++) {
cvCmpS(&tmap,i+1,temp,CV_CMP_EQ);
cvConvertScale(temp,im,1.0/255);
cvCopyMakeBorder(tgL,tgL_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvCopyMakeBorder(tgR,tgR_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvCopyMakeBorder(im,im_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvFilter2D(im_pad,tgL_pad,rlmask,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2));
cvFilter2D(im_pad,tgR_pad,rrmask,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2));
cvGetSubRect(tgL_pad,tgL,cvRect((rlmask->cols-1)/2,(rlmask->rows-1)/2,tgL->cols,tgL->rows));
cvGetSubRect(tgR_pad,tgR,cvRect((rlmask->cols-1)/2,(rlmask->rows-1)/2,tgR->cols,tgR->rows));
cvSub(tgL,tgR,sub2);
cvAbs(sub2,sub2);
cvTranspose(sub2,sub2t);
reshape=cvReshape(sub2t,&reshapehdr,0,h*w);
cvGetCol(d,coltemp,i);
cvCopy(reshape,coltemp);
}
cvMatMul(d,&tsim,prod);
cvMul(prod,d,prod);
CvMat *sumcols=cvCreateMat(h*w,1,CV_32FC1);
cvSetZero(sumcols);
for (int i=0;i<prod->cols;i++) {
cvGetCol(prod,coltemp,i);
cvAdd(sumcols,coltemp,sumcols);
}
reshape=cvReshape(sumcols,&reshapehdr,0,w);
cvTranspose(reshape,tg);
cvReleaseMat(&sumcols);
}
//Smooth the gradient now!!
tg=fitparab(*tg,sigma,sigma/4,theta);
cvMaxS(tg,0,tg);
cvReleaseMat(&im_pad);
cvReleaseMat(&tgL_pad);
cvReleaseMat(&tgR_pad);
cvReleaseMat(&rlmask);
cvReleaseMat(&rrmask);
cvReleaseMat(&im);
cvReleaseMat(&tgL);
cvReleaseMat(&tgR);
cvReleaseMat(&temp);
cvReleaseMat(&coltemp);
cvReleaseMat(&sub2);
cvReleaseMat(&sub2t);
cvReleaseMat(&d);
cvReleaseMat(&prod);
return tg;
}
CV_IMPL void
cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ,
CvMat *matrixQx, CvMat *matrixQy, CvMat *matrixQz,
CvPoint3D64f *eulerAngles)
{
CV_FUNCNAME("cvRQDecomp3x3");
__BEGIN__;
double _M[3][3], _R[3][3], _Q[3][3];
CvMat M = cvMat(3, 3, CV_64F, _M);
CvMat R = cvMat(3, 3, CV_64F, _R);
CvMat Q = cvMat(3, 3, CV_64F, _Q);
double z, c, s;
/* Validate parameters. */
CV_ASSERT( CV_IS_MAT(matrixM) && CV_IS_MAT(matrixR) && CV_IS_MAT(matrixQ) &&
matrixM->cols == 3 && matrixM->rows == 3 &&
CV_ARE_SIZES_EQ(matrixM, matrixR) && CV_ARE_SIZES_EQ(matrixM, matrixQ));
cvConvert(matrixM, &M);
{
/* Find Givens rotation Q_x for x axis (left multiplication). */
/*
( 1 0 0 )
Qx = ( 0 c s ), c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2)
( 0 -s c )
*/
s = _M[2][1];
c = _M[2][2];
z = 1./sqrt(c * c + s * s + DBL_EPSILON);
c *= z;
s *= z;
double _Qx[3][3] = { {1, 0, 0}, {0, c, s}, {0, -s, c} };
CvMat Qx = cvMat(3, 3, CV_64F, _Qx);
cvMatMul(&M, &Qx, &R);
assert(fabs(_R[2][1]) < FLT_EPSILON);
_R[2][1] = 0;
/* Find Givens rotation for y axis. */
/*
( c 0 s )
Qy = ( 0 1 0 ), c = m33/sqrt(m31^2 + m33^2), s = m31/sqrt(m31^2 + m33^2)
(-s 0 c )
*/
s = _R[2][0];
c = _R[2][2];
z = 1./sqrt(c * c + s * s + DBL_EPSILON);
c *= z;
s *= z;
double _Qy[3][3] = { {c, 0, s}, {0, 1, 0}, {-s, 0, c} };
CvMat Qy = cvMat(3, 3, CV_64F, _Qy);
cvMatMul(&R, &Qy, &M);
assert(fabs(_M[2][0]) < FLT_EPSILON);
_M[2][0] = 0;
/* Find Givens rotation for z axis. */
/*
( c s 0 )
Qz = (-s c 0 ), c = m22/sqrt(m21^2 + m22^2), s = m21/sqrt(m21^2 + m22^2)
( 0 0 1 )
*/
s = _M[1][0];
c = _M[1][1];
z = 1./sqrt(c * c + s * s + DBL_EPSILON);
c *= z;
s *= z;
double _Qz[3][3] = { {c, s, 0}, {-s, c, 0}, {0, 0, 1} };
CvMat Qz = cvMat(3, 3, CV_64F, _Qz);
cvMatMul(&M, &Qz, &R);
assert(fabs(_R[1][0]) < FLT_EPSILON);
_R[1][0] = 0;
// Solve the decomposition ambiguity.
// Diagonal entries of R, except the last one, shall be positive.
// Further rotate R by 180 degree if necessary
if( _R[0][0] < 0 )
{
if( _R[1][1] < 0 )
{
// rotate around z for 180 degree, i.e. a rotation matrix of
// [-1, 0, 0],
// [ 0, -1, 0],
// [ 0, 0, 1]
_R[0][0] *= -1;
_R[0][1] *= -1;
_R[1][1] *= -1;
_Qz[0][0] *= -1;
_Qz[0][1] *= -1;
_Qz[1][0] *= -1;
_Qz[1][1] *= -1;
}
else
{
// rotate around y for 180 degree, i.e. a rotation matrix of
// [-1, 0, 0],
// [ 0, 1, 0],
// [ 0, 0, -1]
_R[0][0] *= -1;
_R[0][2] *= -1;
_R[1][2] *= -1;
_R[2][2] *= -1;
cvTranspose( &Qz, &Qz );
_Qy[0][0] *= -1;
_Qy[0][2] *= -1;
_Qy[2][0] *= -1;
_Qy[2][2] *= -1;
}
}
else if( _R[1][1] < 0 )
{
// ??? for some reason, we never get here ???
// rotate around x for 180 degree, i.e. a rotation matrix of
// [ 1, 0, 0],
// [ 0, -1, 0],
// [ 0, 0, -1]
_R[0][1] *= -1;
_R[0][2] *= -1;
_R[1][1] *= -1;
_R[1][2] *= -1;
_R[2][2] *= -1;
cvTranspose( &Qz, &Qz );
cvTranspose( &Qy, &Qy );
_Qx[1][1] *= -1;
_Qx[1][2] *= -1;
_Qx[2][1] *= -1;
_Qx[2][2] *= -1;
}
// calculate the euler angle
if( eulerAngles )
{
eulerAngles->x = acos(_Qx[1][1]) * (_Qx[1][2] >= 0 ? 1 : -1) * (180.0 / CV_PI);
eulerAngles->y = acos(_Qy[0][0]) * (_Qy[0][2] >= 0 ? 1 : -1) * (180.0 / CV_PI);
eulerAngles->z = acos(_Qz[0][0]) * (_Qz[0][1] >= 0 ? 1 : -1) * (180.0 / CV_PI);
}
/* Calulate orthogonal matrix. */
/*
Q = QzT * QyT * QxT
*/
cvGEMM( &Qz, &Qy, 1, 0, 0, &M, CV_GEMM_A_T + CV_GEMM_B_T );
cvGEMM( &M, &Qx, 1, 0, 0, &Q, CV_GEMM_B_T );
/* Save R and Q matrices. */
cvConvert( &R, matrixR );
cvConvert( &Q, matrixQ );
if( matrixQx )
cvConvert(&Qx, matrixQx);
if( matrixQy )
cvConvert(&Qy, matrixQy);
if( matrixQz )
cvConvert(&Qz, matrixQz);
}
__END__;
}
void reconstructSurface(
const char* dirName, slParams* sl_params, slCalib* sl_calib)
{
IplImage** proj_gray_codes = NULL;
int gray_ncols, gray_nrows;
int gray_colshift, gray_rowshift;
generateGrayCodes(sl_params->proj_w, sl_params->proj_h, proj_gray_codes,
gray_ncols, gray_nrows, gray_colshift, gray_rowshift,
sl_params->scan_cols, sl_params->scan_rows);
IplImage **cam_gray_codes = new IplImage*[22];
int numImages = getLatestImages(dirName, cam_gray_codes, 22);
IplImage* gray_decoded_cols = cvCreateImage(cvSize(sl_params->cam_w, sl_params->cam_h), IPL_DEPTH_16U, 1);
IplImage* gray_decoded_rows = cvCreateImage(cvSize(sl_params->cam_w, sl_params->cam_h), IPL_DEPTH_16U, 1);
IplImage* gray_mask = cvCreateImage(cvSize(sl_params->cam_w, sl_params->cam_h), IPL_DEPTH_8U, 1);
decodeGrayCodes(sl_params->proj_w, sl_params->proj_h,
cam_gray_codes,
gray_decoded_cols, gray_decoded_rows, gray_mask,
gray_ncols, gray_nrows,
gray_colshift, gray_rowshift,
sl_params->thresh);
char str[1024], outputDir[1024];
mkdir(sl_params->outdir, 0755);
std::string baseNameBuilder(dirName);
size_t last_slash_position = baseNameBuilder.find_last_of("/");
baseNameBuilder = baseNameBuilder.substr(last_slash_position+1);
const char* baseName = baseNameBuilder.c_str();
//sprintf(outputDir, "3D/%s", baseName);
sprintf(outputDir, "%s/%s", sl_params->outdir, baseName);
//mkdir("3D", 0755);
mkdir(outputDir, 0755);
// Display and save the correspondences.
if(sl_params->display)
displayDecodingResults(gray_decoded_cols, gray_decoded_rows, gray_mask, sl_params);
// Reconstruct the point cloud and depth map.
//printf("Reconstructing the point cloud and the depth map...\n");
CvMat *points = cvCreateMat(3, sl_params->cam_h*sl_params->cam_w, CV_32FC1);
CvMat *colors = cvCreateMat(3, sl_params->cam_h*sl_params->cam_w, CV_32FC1);
CvMat *depth_map = cvCreateMat(sl_params->cam_h, sl_params->cam_w, CV_32FC1);
CvMat *mask = cvCreateMat(1, sl_params->cam_h*sl_params->cam_w, CV_32FC1);
CvMat *resampled_points = cvCreateMat(3, sl_params->cam_h*sl_params->cam_w, CV_32FC1);
reconstructStructuredLight(sl_params, sl_calib,
cam_gray_codes[0],
gray_decoded_cols, gray_decoded_rows, gray_mask,
points, colors, depth_map, mask);
// cvSave("points.xml",points);
CvMat *points_trans = cvCreateMat(sl_params->cam_h*sl_params->cam_w, 3, CV_32FC1);
cvTranspose(points, points_trans);
downsamplePoints(sl_params, sl_calib, points_trans, mask, resampled_points, depth_map);
double min_val, max_val;
cvMinMaxLoc(depth_map, &min_val, &max_val);
// Display and save the depth map.
if(sl_params->display)
displayDepthMap(depth_map, gray_mask, sl_params);
//printf("Saving the depth map...\n");
IplImage* depth_map_image = cvCreateImage(cvSize(sl_params->cam_w, sl_params->cam_h), IPL_DEPTH_8U, 1);
for(int r=0; r<sl_params->cam_h; r++){
for(int c=0; c<sl_params->cam_w; c++){
char* depth_map_image_data = (char*)(depth_map_image->imageData + r*depth_map_image->widthStep);
if(mask->data.fl[sl_params->cam_w*r+c])
depth_map_image_data[c] =
255-int(255*(depth_map->data.fl[sl_params->cam_w*r+c]-sl_params->dist_range[0])/
(sl_params->dist_range[1]-sl_params->dist_range[0]));
else
depth_map_image_data[c] = 0;
}
}
CvMat* dist_range = cvCreateMat(1, 2, CV_32FC1);
cvmSet(dist_range, 0, 0, sl_params->dist_range[0]);
cvmSet(dist_range, 0, 1, sl_params->dist_range[1]);
sprintf(str, "%s/depth_map.png", outputDir);
printf("%s\n",str);
cvSaveImage(str, depth_map_image);
sprintf(str, "%s/depth_map_range.xml", outputDir);
cvSave(str, dist_range);
cvReleaseImage(&depth_map_image);
cvReleaseMat(&dist_range);
// Save the texture map.
//printf("Saving the texture map...\n");
sprintf(str, "%s/%s.png", outputDir, baseName);
cvSaveImage(str, cam_gray_codes[0]);
// Save the point cloud.
//printf("Saving the point cloud...\n");
sprintf(str, "%s/%s.ply", outputDir, baseName);
//if(savePointsPLY(str, resampled_points, NULL, NULL, mask, sl_params->proj_w, sl_params->proj_h)){
//if(savePointsPLY(str, resampled_points, NULL, NULL, mask, sl_params->cam_w, sl_params->cam_h)){
if(savePointsPLY(str, points, NULL, NULL, mask, sl_params->cam_w, sl_params->cam_h)){
fprintf(stderr, "Saving the reconstructed point cloud failed!\n");
return;
}
sprintf(str,"%s/proj_intrinsic.xml", outputDir);
cvSave(str, sl_calib->proj_intrinsic);
sprintf(str,"%s/proj_distortion.xml", outputDir);
cvSave(str, sl_calib->proj_distortion);
sprintf(str,"%s/cam_intrinsic.xml", outputDir);
cvSave(str, sl_calib->cam_intrinsic);
sprintf(str,"%s/cam_distortion.xml", outputDir);
cvSave(str, sl_calib->cam_distortion);
sprintf(str, "%s/cam_extrinsic.xml", outputDir);
cvSave(str, sl_calib->cam_extrinsic);
sprintf(str, "%s/proj_extrinsic.xml", outputDir);
cvSave(str, sl_calib->proj_extrinsic);
// Free allocated resources.
cvReleaseImage(&gray_decoded_cols);
cvReleaseImage(&gray_decoded_rows);
cvReleaseImage(&gray_mask);
cvReleaseMat(&points);
cvReleaseMat(&colors);
cvReleaseMat(&depth_map);
cvReleaseMat(&mask);
cvReleaseMat(&resampled_points);
for(int i=0; i<(gray_ncols+gray_nrows+1); i++)
cvReleaseImage(&proj_gray_codes[i]);
delete[] proj_gray_codes;
for(int i=0; i<2*(gray_ncols+gray_nrows+1); i++)
cvReleaseImage(&cam_gray_codes[i]);
delete[] cam_gray_codes;
return;
}
/* Optimization using Levenberg-Marquardt */
void cvLevenbergMarquardtOptimization(pointer_LMJac JacobianFunction,
pointer_LMFunc function,
/*pointer_Err error_function,*/
CvMat *X0,CvMat *observRes,CvMat *resultX,
int maxIter,double epsilon)
{
/* This is not sparse method */
/* Make optimization using */
/* func - function to compute */
/* uses function to compute jacobian */
/* Allocate memory */
CvMat *vectX = 0;
CvMat *vectNewX = 0;
CvMat *resFunc = 0;
CvMat *resNewFunc = 0;
CvMat *error = 0;
CvMat *errorNew = 0;
CvMat *Jac = 0;
CvMat *delta = 0;
CvMat *matrJtJ = 0;
CvMat *matrJtJN = 0;
CvMat *matrJt = 0;
CvMat *vectB = 0;
CV_FUNCNAME( "cvLevenbegrMarquardtOptimization" );
__BEGIN__;
if( JacobianFunction == 0 || function == 0 || X0 == 0 || observRes == 0 || resultX == 0 )
{
CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
}
if( !CV_IS_MAT(X0) || !CV_IS_MAT(observRes) || !CV_IS_MAT(resultX) )
{
CV_ERROR( CV_StsUnsupportedFormat, "Some of input parameters must be a matrices" );
}
int numVal;
int numFunc;
double valError;
double valNewError;
numVal = X0->rows;
numFunc = observRes->rows;
/* test input data */
if( X0->cols != 1 )
{
CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector X0 must be 1" );
}
if( observRes->cols != 1 )
{
CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector observed rusult must be 1" );
}
if( resultX->cols != 1 || resultX->rows != numVal )
{
CV_ERROR( CV_StsUnmatchedSizes, "Size of result vector X must be equals to X0" );
}
if( maxIter <= 0 )
{
CV_ERROR( CV_StsUnmatchedSizes, "Number of maximum iteration must be > 0" );
}
if( epsilon < 0 )
{
CV_ERROR( CV_StsUnmatchedSizes, "Epsilon must be >= 0" );
}
/* copy x0 to current value of x */
CV_CALL( vectX = cvCreateMat(numVal, 1, CV_64F) );
CV_CALL( vectNewX = cvCreateMat(numVal, 1, CV_64F) );
CV_CALL( resFunc = cvCreateMat(numFunc,1, CV_64F) );
CV_CALL( resNewFunc = cvCreateMat(numFunc,1, CV_64F) );
CV_CALL( error = cvCreateMat(numFunc,1, CV_64F) );
CV_CALL( errorNew = cvCreateMat(numFunc,1, CV_64F) );
CV_CALL( Jac = cvCreateMat(numFunc,numVal, CV_64F) );
CV_CALL( delta = cvCreateMat(numVal, 1, CV_64F) );
CV_CALL( matrJtJ = cvCreateMat(numVal, numVal, CV_64F) );
CV_CALL( matrJtJN = cvCreateMat(numVal, numVal, CV_64F) );
CV_CALL( matrJt = cvCreateMat(numVal, numFunc,CV_64F) );
CV_CALL( vectB = cvCreateMat(numVal, 1, CV_64F) );
cvCopy(X0,vectX);
/* ========== Main optimization loop ============ */
double change;
int currIter;
double alpha;
change = 1;
currIter = 0;
alpha = 0.001;
do {
/* Compute value of function */
function(vectX,resFunc);
/* Print result of function to file */
/* Compute error */
cvSub(observRes,resFunc,error);
//valError = error_function(observRes,resFunc);
/* Need to use new version of computing error (norm) */
valError = cvNorm(observRes,resFunc);
/* Compute Jacobian for given point vectX */
JacobianFunction(vectX,Jac);
/* Define optimal delta for J'*J*delta=J'*error */
/* compute J'J */
cvMulTransposed(Jac,matrJtJ,1);
cvCopy(matrJtJ,matrJtJN);
/* compute J'*error */
cvTranspose(Jac,matrJt);
cvmMul(matrJt,error,vectB);
/* Solve normal equation for given alpha and Jacobian */
do
{
/* Increase diagonal elements by alpha */
for( int i = 0; i < numVal; i++ )
{
double val;
val = cvmGet(matrJtJ,i,i);
cvmSet(matrJtJN,i,i,(1+alpha)*val);
}
/* Solve system to define delta */
cvSolve(matrJtJN,vectB,delta,CV_SVD);
/* We know delta and we can define new value of vector X */
cvAdd(vectX,delta,vectNewX);
/* Compute result of function for new vector X */
function(vectNewX,resNewFunc);
cvSub(observRes,resNewFunc,errorNew);
valNewError = cvNorm(observRes,resNewFunc);
currIter++;
if( valNewError < valError )
{/* accept new value */
valError = valNewError;
/* Compute relative change of required parameter vectorX. change = norm(curr-prev) / norm(curr) ) */
change = cvNorm(vectX, vectNewX, CV_RELATIVE_L2);
alpha /= 10;
cvCopy(vectNewX,vectX);
break;
}
else
{
alpha *= 10;
}
} while ( currIter < maxIter );
/* new value of X and alpha were accepted */
} while ( change > epsilon && currIter < maxIter );
/* result was computed */
cvCopy(vectX,resultX);
__END__;
cvReleaseMat(&vectX);
cvReleaseMat(&vectNewX);
cvReleaseMat(&resFunc);
cvReleaseMat(&resNewFunc);
cvReleaseMat(&error);
cvReleaseMat(&errorNew);
cvReleaseMat(&Jac);
cvReleaseMat(&delta);
cvReleaseMat(&matrJtJ);
cvReleaseMat(&matrJtJN);
cvReleaseMat(&matrJt);
cvReleaseMat(&vectB);
return;
}
void LKInverseComp::preCompute()
{
IplImage * imgMask = cvCreateImage(cvSize(meanAppearance->width,meanAppearance->height), IPL_DEPTH_8U, 1);
GradientX = cvCreateImage(cvSize(meanAppearance->width,meanAppearance->height), IPL_DEPTH_32F, 1);
GradientY = cvCreateImage(cvSize(meanAppearance->width,meanAppearance->height), IPL_DEPTH_32F, 1);
IplImage* GradientXTemp = cvCreateImage(cvSize(meanAppearance->width,meanAppearance->height), IPL_DEPTH_32F, 1);
IplImage* GradientYTemp = cvCreateImage(cvSize(meanAppearance->width,meanAppearance->height), IPL_DEPTH_32F, 1);
cvZero(GradientX);
cvZero(GradientY);
cvZero(GradientXTemp);
cvZero(GradientYTemp);
cvZero(imgMask);
int t1=newDelaunay->numberOfPixels();
for (int i=0;i<t1;i++)
{
pixel * pix = newDelaunay->getpixel(i);
CvScalar s;
s.val[0]=1;
cvSet2D(imgMask,pix->y+pix->ty,pix->x+pix->tx,s);
}
cvSobel(meanAppearance, GradientXTemp, 1, 0);
cvSobel(meanAppearance, GradientYTemp, 0, 1);
//
// for (int i=0;i<meanAppearance->width;i++)
// {
// for (int j=0;j<meanAppearance->height;j++)
// {
//
// CvScalar im1;
// CvScalar ip1;
//
// im1.val[0]=0;
// ip1.val[0]=0;
// // printf("%d %d",i,j);
// if ((i-1)>=0)
// im1=cvGet2D(meanAppearance,j,i-1);
// if ((i+1)<meanAppearance->width)
// ip1=cvGet2D(meanAppearance,j,i+1);
//
// CvScalar k;
// k.val[0]=(ip1.val[0]-im1.val[0])/2;
//
// cvSet2D(GradientXTemp,j,i,k);
//
// im1.val[0]=0;
// ip1.val[0]=0;
//
// if ((j-1)>=0)
// im1=cvGet2D(meanAppearance,j-1,i);
//
// if ((j+1)<meanAppearance->height)
// ip1=cvGet2D(meanAppearance,j+1,i);
//
//
// k.val[0]=(ip1.val[0]-im1.val[0])/2;
//
// cvSet2D(GradientYTemp,j,i,k);
//
// }
// }
//////
//cvConvertScale(GradientXTemp,GradientXTemp,2);
//cvConvertScale(GradientYTemp,GradientYTemp,2);
//
cvAnd(GradientXTemp,GradientXTemp,GradientX,imgMask);
cvAnd(GradientYTemp,GradientYTemp,GradientY,imgMask);
// cvNamedWindow("mean",1);
// cvShowImage("mean",GradientX);
// cvWaitKey(-1);
HessianMatrix = cvCreateMat(nS+4, nS+4, CV_64FC1);
HessianMatrixInverse = cvCreateMat(nS+4, nS+4, CV_64FC1);
HessianMatrixInverse_steepestDescentImageTranspose = cvCreateMat( nS+4,(meanAppearance->width*meanAppearance->height), CV_64FC1);
steepestDescentImage = cvCreateMat((meanAppearance->width*meanAppearance->height), nS+4, CV_64FC1);
steepestDescentImageTranspose = cvCreateMat( nS+4,(meanAppearance->width*meanAppearance->height), CV_64FC1);
CvMat * steepestDescentCoeffX = cvCreateMat((meanAppearance->width*meanAppearance->height),nS+4, CV_64FC1);
CvMat * steepestDescentCoeffY = cvCreateMat((meanAppearance->width*meanAppearance->height),nS+4, CV_64FC1);
CvMat * app = cvCreateMat((meanAppearance->width*meanAppearance->height),1, CV_64FC1);
cvZero(steepestDescentCoeffX);
cvZero(steepestDescentCoeffY);
CvMat * dw_xi_yi = cvCreateMat((meanAppearance->width*meanAppearance->height),numberofpoints, CV_64FC1);
cvZero(dw_xi_yi);
for (int m=0;m<numberofpoints;m++)
{
int t=newDelaunay->numberOfPixels();
for (int i=0;i<t;i++)
{
pixel * pix = newDelaunay->getpixel(i);
int typ=pix->type;
int flag=0;
int k=0;
for ( k=0;k<newDelaunay->ithNodeCode[m].count;k++)
{
if (newDelaunay->ithNodeCode[m].triangles[k].colorcode==typ)
{
flag=1;
break;
}
}
if (flag==1)
{
CvScalar s1 = cvGet2D(meanShape,0,2*m);
CvScalar s2 = cvGet2D(meanShape,0,2*m +1);
double alphatop= ((((pix->x ) - newDelaunay->ithNodeCode[m].triangles[k].xi )*newDelaunay->ithNodeCode[m].triangles[k].diff_yk_yi) - (((pix->y) - newDelaunay->ithNodeCode[m].triangles[k].yi )*newDelaunay->ithNodeCode[m].triangles[k].diff_xk_xi));
double alphabottom = ((( newDelaunay->ithNodeCode[m].triangles[k].diff_xj_xi )*newDelaunay->ithNodeCode[m].triangles[k].diff_yk_yi) - (( newDelaunay->ithNodeCode[m].triangles[k].diff_yj_yi )*newDelaunay->ithNodeCode[m].triangles[k].diff_xk_xi));
double alpha= alphatop/alphabottom;
double betatop= ((((pix->y ) - newDelaunay->ithNodeCode[m].triangles[k].yi )*newDelaunay->ithNodeCode[m].triangles[k].diff_xj_xi) - (((pix->x) - newDelaunay->ithNodeCode[m].triangles[k].xi )*newDelaunay->ithNodeCode[m].triangles[k].diff_yj_yi));
double betabottom = ((( newDelaunay->ithNodeCode[m].triangles[k].diff_xj_xi )*newDelaunay->ithNodeCode[m].triangles[k].diff_yk_yi) - (( newDelaunay->ithNodeCode[m].triangles[k].diff_yj_yi )*newDelaunay->ithNodeCode[m].triangles[k].diff_xk_xi));
double beta= betatop/betabottom;
double val=1-alpha -beta;
CvScalar s;
if (val<0)
{
//printf("val %e \n",val);
val=0;
}
s.val[0]=val;
if ((pix->y+pix->ty)>=0 && (pix->x+pix->tx)>=0)
{
CvScalar s1= cvGet2D(dw_xi_yi, int( pix->y+pix->ty) * int(pix->width) + int(pix->x + pix->tx ) ,m);
s.val[0]+=s1.val[0];
cvSet2D(dw_xi_yi, int( pix->y+pix->ty) * int(pix->width) + int(pix->x + pix->tx ) ,m ,s );
}
}
}
}
int t=meanAppearance->width*meanAppearance->height;
for (int u=0;u<t;u++)
{
double GradT_x;
double GradT_y;
CvScalar s;
s=cvGet2D(GradientX,int(floor(u/meanAppearance->width)),(u%meanAppearance->width));
GradT_x=s.val[0];
s=cvGet2D(GradientY,int(floor(u/meanAppearance->width)),(u%meanAppearance->width));
GradT_y=s.val[0];
for (int i=0;i<numberofpoints;i++)
{
for (int m=0;m<(nS+4);m++)
{
CvScalar currentX = cvGet2D(steepestDescentCoeffX,u,m);
CvScalar currentY = cvGet2D(steepestDescentCoeffY,u,m);
CvScalar s1,s2,s3;
s1 =cvGet2D(dw_xi_yi,u,i);
s2 =cvGet2D(combineshapevectors[m],0,2*i);
s3 =cvGet2D(combineshapevectors[m],0,2*i + 1);
double temp;
temp = s1.val[0]*s2.val[0];
currentX.val[0]+= (GradT_x*(temp));
temp = s1.val[0]*s3.val[0];
currentY.val[0]+= (GradT_y* (temp));
cvSet2D(steepestDescentCoeffX,u,m,currentX);
cvSet2D(steepestDescentCoeffY,u,m,currentY);
}
}
}
cvAdd(steepestDescentCoeffX, steepestDescentCoeffY, steepestDescentImage);
for (int m=0;m<(nS+4);m++)
{
cvZero(app);
for (int l=0;l<(nA);l++)
{
double total=0;
for (int h=0;h<t;h++)
{
int wid;
int hei;
hei =floor(h/meanAppearance->width);
wid =(h%meanAppearance->width);
CvScalar s2;
s2=cvGet2D(appearanceVectors[l],hei,wid);
CvScalar s;
s=cvGet2D(steepestDescentImage,h,m);
total+= s2.val[0]*s.val[0];
}
// cvDotProduct(appearanceVectors[i],destShapeTemp);
for (int h=0;h<t;h++)
{
int wid;
int hei;
hei =(h/meanAppearance->width);
wid =(h%meanAppearance->width);
CvScalar s;
s=cvGet2D(app,h,0);
CvScalar s2;
s2=cvGet2D(appearanceVectors[l],hei,wid);
s.val[0]+= (s2.val[0]*total);
cvSet2D(app,h,0,s);
}
}
for (int h=0;h<t;h++)
{
CvScalar s;
s=cvGet2D(steepestDescentImage,h,m);
CvScalar s2;
s2=cvGet2D(app,h,0);
s.val[0]-=s2.val[0];
cvSet2D(steepestDescentImage,h,m,s);
}
}
cvTranspose(steepestDescentImage, steepestDescentImageTranspose);
cvMatMul(steepestDescentImageTranspose,steepestDescentImage,HessianMatrix);
cvInvert(HessianMatrix, HessianMatrixInverse);
cvMatMul(HessianMatrixInverse,steepestDescentImageTranspose,HessianMatrixInverse_steepestDescentImageTranspose);
}
// perform autocalibration using absolute quadric
bool mvg_autocalibration(CvMat ** Ps, double * principal_points, const size_t n, CvMat ** Xs, const size_t m)
{
if (n < 3)
{
return false;
}
printf("*****************************************\n");
opencv_debug("First camera before transformation", Ps[0]);
// move the principal point to the origin for every camera
// and use canonical first camera
CvMat * T = opencv_create_matrix(3, 3);
CvMat * S = opencv_create_matrix(3, 3);
CvMat * G = opencv_create_matrix(4, 4);
CvMat * H = opencv_create_matrix(4, 4);
for (size_t i = 0; i < n; i++)
{
// set up translation matrix
cvZero(T);
OPENCV_ELEM(T, 0, 0) = 1;
OPENCV_ELEM(T, 1, 1) = 1;
OPENCV_ELEM(T, 2, 2) = 1;
OPENCV_ELEM(T, 0, 2) = -principal_points[2 * i + 0];
OPENCV_ELEM(T, 1, 2) = -principal_points[2 * i + 1];
// apply it to the projection matrix
cvMatMul(T, Ps[i], Ps[i]);
// also scale
cvZero(S);
OPENCV_ELEM(S, 0, 0) = 0.001;
OPENCV_ELEM(S, 1, 1) = 0.001;
OPENCV_ELEM(S, 2, 2) = 1;
cvMatMul(S, Ps[i], Ps[i]);
// calculate the world-space homography which transforms P_1 to [I_3x3 | 0]
if (i == 0)
{
cvZero(G);
OPENCV_ELEM(G, 3, 3) = 1;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 4; j++)
{
OPENCV_ELEM(G, i, j) = OPENCV_ELEM(Ps[0], i, j);
}
}
cvInvert(G, H, CV_SVD);
}
// apply the homography to every camera
cvMatMul(Ps[i], H, Ps[i]);
}
// also apply inverse homography to all the points
for (size_t i = 0; i < m; i++)
{
cvMatMul(G, Xs[i], Xs[i]);
}
// debug
opencv_debug("First camera", Ps[0]);
opencv_debug("Transformed using this transformation", H);
printf("*****************************************\n");
printf("List of all cameras:\n");
for (size_t i = 0; i < n; i++)
{
opencv_debug("Camera", Ps[i]);
}
cvReleaseMat(&S);
cvReleaseMat(&T);
cvReleaseMat(&H);
cvReleaseMat(&G);
// construct system of linear equations
CvMat * W = opencv_create_matrix(4 * (n - 1), 5), * b = opencv_create_matrix(4 * (n - 1), 1);
for (size_t i = 1; i < n; i++)
{
const double
p11 = OPENCV_ELEM(Ps[i], 0, 0),
p12 = OPENCV_ELEM(Ps[i], 0, 1),
p13 = OPENCV_ELEM(Ps[i], 0, 2),
p14 = OPENCV_ELEM(Ps[i], 0, 3),
p21 = OPENCV_ELEM(Ps[i], 1, 0),
p22 = OPENCV_ELEM(Ps[i], 1, 1),
p23 = OPENCV_ELEM(Ps[i], 1, 2),
p24 = OPENCV_ELEM(Ps[i], 1, 3),
p31 = OPENCV_ELEM(Ps[i], 2, 0),
p32 = OPENCV_ELEM(Ps[i], 2, 1),
p33 = OPENCV_ELEM(Ps[i], 2, 2),
p34 = OPENCV_ELEM(Ps[i], 2, 3)
;
const double
p11_2 = sq(p11),
p12_2 = sq(p12),
p21_2 = sq(p21),
p22_2 = sq(p22),
p14_2 = sq(p14),
p24_2 = sq(p24),
p13_2 = sq(p13),
p23_2 = sq(p23)
;
OPENCV_ELEM(W, (i - 1) * 4 + 0, 0) = p11_2 + p12_2 - p21_2 - p22_2;
OPENCV_ELEM(W, (i - 1) * 4 + 0, 1) = 2 * p11 * p14 - 2 * p21 * p24;
OPENCV_ELEM(W, (i - 1) * 4 + 0, 2) = 2 * p12 * p14 - 2 * p22 * p24;
OPENCV_ELEM(W, (i - 1) * 4 + 0, 3) = 2 * p13 * p14 - 2 * p23 * p24;
OPENCV_ELEM(W, (i - 1) * 4 + 0, 4) = p14_2 - p24_2;
OPENCV_ELEM(b, (i - 1) * 4 + 0, 0) = -(p13_2 - p23_2);
OPENCV_ELEM(W, (i - 1) * 4 + 1, 0) = p11 * p21 + p12 * p22;
OPENCV_ELEM(W, (i - 1) * 4 + 1, 1) = p14 * p21 + p11 * p24;
OPENCV_ELEM(W, (i - 1) * 4 + 1, 2) = p14 * p22 + p12 * p24;
OPENCV_ELEM(W, (i - 1) * 4 + 1, 3) = p14 * p23 + p13 * p24;
OPENCV_ELEM(W, (i - 1) * 4 + 1, 4) = p14 * p24;
OPENCV_ELEM(b, (i - 1) * 4 + 1, 0) = -(p13 * p23);
OPENCV_ELEM(W, (i - 1) * 4 + 2, 0) = p11 * p31 + p12 * p32;
OPENCV_ELEM(W, (i - 1) * 4 + 2, 1) = p14 * p31 + p11 * p34;
OPENCV_ELEM(W, (i - 1) * 4 + 2, 2) = p14 * p32 + p12 * p34;
OPENCV_ELEM(W, (i - 1) * 4 + 2, 3) = p14 * p33 + p13 * p34;
OPENCV_ELEM(W, (i - 1) * 4 + 2, 4) = p14 * p34;
OPENCV_ELEM(b, (i - 1) * 4 + 2, 0) = -(p13 * p33);
OPENCV_ELEM(W, (i - 1) * 4 + 3, 0) = p21 * p31 + p22 * p32;
OPENCV_ELEM(W, (i - 1) * 4 + 3, 1) = p24 * p31 + p21 * p34;
OPENCV_ELEM(W, (i - 1) * 4 + 3, 2) = p24 * p32 + p22 * p34;
OPENCV_ELEM(W, (i - 1) * 4 + 3, 3) = p24 * p33 + p23 * p34;
OPENCV_ELEM(W, (i - 1) * 4 + 3, 4) = p24 * p34;
OPENCV_ELEM(b, (i - 1) * 4 + 3, 0) = -(p23 * p33);
}
opencv_debug("Autocalibrating equations", W);
CvMat * solution = opencv_create_matrix(5, 1);
cvSolve(W, b, solution, CV_SVD);
// if first row in solution is not positive, replace W by -W
/*if (OPENCV_ELEM(solution, 0, 0) <= 0)
{
printf("--- !!! --- Multiplying the matrix by -1 --- !!! ---\n");
for (int i = 0; i < W->rows; i++)
{
for (int j = 0; j < W->cols; j++)
{
OPENCV_ELEM(W, i, j) *= -1;
}
}
cvSolve(W, b, solution, CV_SVD);
}*/
opencv_debug("Solution", solution);
cvReleaseMat(&W);
cvReleaseMat(&b);
// compute f_1, K_1 and w (the focal length of the first camera, calibration matrix
// of the first camera and the plane at infinity)
double f_1 = OPENCV_ELEM(solution, 0, 0);
if (f_1 < 0)
{
printf("--- Multiplying f^2 by -1 ---\n");
f_1 *= -1;
}
f_1 = sqrt(f_1);
// f_1 *= 1000.0;
printf("f_1 = %f\n\n", f_1);
CvMat * K_1 = opencv_create_I_matrix(3);
OPENCV_ELEM(K_1, 0, 0) = f_1;
OPENCV_ELEM(K_1, 1, 1) = f_1;
CvMat * w = opencv_create_matrix(3, 1);
OPENCV_ELEM(w, 0, 0) = OPENCV_ELEM(solution, 1, 0) / f_1;
OPENCV_ELEM(w, 1, 0) = OPENCV_ELEM(solution, 2, 0) / f_1;
OPENCV_ELEM(w, 2, 0) = OPENCV_ELEM(solution, 3, 0);
// check with the last value of the solution vector, which contains
// the scalar product w_transposed * w
const double w_Tw = sq(OPENCV_ELEM(w, 0, 0)) + sq(OPENCV_ELEM(w, 1, 0)) + sq(OPENCV_ELEM(w, 2, 0));
// debug
opencv_debug("K_1", K_1);
opencv_debug("w", w);
printf("difference between calculated and recomputed w_T * w = %f - %f = %f\n", OPENCV_ELEM(solution, 4, 0), w_Tw, OPENCV_ELEM(solution, 4, 0) - w_Tw);
// contruct rectifying homography
CvMat * H_metric = opencv_create_matrix(4, 4);
cvZero(H_metric);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
OPENCV_ELEM(H_metric, i, j) = OPENCV_ELEM(K_1, i, j);
}
}
CvMat * H_metric_4_prime = opencv_create_matrix(1, 3);
cvTranspose(w, w);
cvMatMul(w, K_1, H_metric_4_prime);
for (int j = 0; j < 3; j++)
{
OPENCV_ELEM(H_metric, 3, j) = OPENCV_ELEM(H_metric_4_prime, 0, j);
}
OPENCV_ELEM(H_metric, 3, 3) = 1;
cvInvert(H_metric, H_metric);
// apply rectifying homography to all points
for (size_t i = 0; i < m; i++)
{
cvMatMul(H_metric, Xs[i], Xs[i]);
}
// release resources
cvReleaseMat(&K_1);
cvReleaseMat(&w);
cvReleaseMat(&solution);
return true;
}
/* log_weight_div_det[k] = -2*log(weights_k) + log(det(Sigma_k)))
covs[k] = cov_rotate_mats[k] * cov_eigen_values[k] * (cov_rotate_mats[k])'
cov_rotate_mats[k] are orthogonal matrices of eigenvectors and
cov_eigen_values[k] are diagonal matrices (represented by 1D vectors) of eigen values.
The <alpha_ik> is the probability of the vector x_i to belong to the k-th cluster:
<alpha_ik> ~ weights_k * exp{ -0.5[ln(det(Sigma_k)) + (x_i - mu_k)' Sigma_k^(-1) (x_i - mu_k)] }
We calculate these probabilities here by the equivalent formulae:
Denote
S_ik = -0.5(log(det(Sigma_k)) + (x_i - mu_k)' Sigma_k^(-1) (x_i - mu_k)) + log(weights_k),
M_i = max_k S_ik = S_qi, so that the q-th class is the one where maximum reaches. Then
alpha_ik = exp{ S_ik - M_i } / ( 1 + sum_j!=q exp{ S_ji - M_i })
*/
double CvEM::run_em( const CvVectors& train_data )
{
CvMat* centered_sample = 0;
CvMat* covs_item = 0;
CvMat* log_det = 0;
CvMat* log_weights = 0;
CvMat* cov_eigen_values = 0;
CvMat* samples = 0;
CvMat* sum_probs = 0;
log_likelihood = -DBL_MAX;
CV_FUNCNAME( "CvEM::run_em" );
__BEGIN__;
int nsamples = train_data.count, dims = train_data.dims, nclusters = params.nclusters;
double min_variation = FLT_EPSILON;
double min_det_value = MAX( DBL_MIN, pow( min_variation, dims ));
double likelihood_bias = -CV_LOG2PI * (double)nsamples * (double)dims / 2., _log_likelihood = -DBL_MAX;
int start_step = params.start_step;
int i, j, k, n;
int is_general = 0, is_diagonal = 0, is_spherical = 0;
double prev_log_likelihood = -DBL_MAX / 1000., det, d;
CvMat whdr, iwhdr, diag, *w, *iw;
double* w_data;
double* sp_data;
if( nclusters == 1 )
{
double log_weight;
CV_CALL( cvSet( probs, cvScalar(1.)) );
if( params.cov_mat_type == COV_MAT_SPHERICAL )
{
d = cvTrace(*covs).val[0]/dims;
d = MAX( d, FLT_EPSILON );
inv_eigen_values->data.db[0] = 1./d;
log_weight = pow( d, dims*0.5 );
}
else
{
w_data = inv_eigen_values->data.db;
if( params.cov_mat_type == COV_MAT_GENERIC )
cvSVD( *covs, inv_eigen_values, *cov_rotate_mats, 0, CV_SVD_U_T );
else
cvTranspose( cvGetDiag(*covs, &diag), inv_eigen_values );
cvMaxS( inv_eigen_values, FLT_EPSILON, inv_eigen_values );
for( j = 0, det = 1.; j < dims; j++ )
det *= w_data[j];
log_weight = sqrt(det);
cvDiv( 0, inv_eigen_values, inv_eigen_values );
}
log_weight_div_det->data.db[0] = -2*log(weights->data.db[0]/log_weight);
log_likelihood = DBL_MAX/1000.;
EXIT;
}
if( params.cov_mat_type == COV_MAT_GENERIC )
is_general = 1;
else if( params.cov_mat_type == COV_MAT_DIAGONAL )
is_diagonal = 1;
else if( params.cov_mat_type == COV_MAT_SPHERICAL )
is_spherical = 1;
/* In the case of <cov_mat_type> == COV_MAT_DIAGONAL, the k-th row of cov_eigen_values
contains the diagonal elements (variations). In the case of
<cov_mat_type> == COV_MAT_SPHERICAL - the 0-ths elements of the vectors cov_eigen_values[k]
are to be equal to the mean of the variations over all the dimensions. */
CV_CALL( log_det = cvCreateMat( 1, nclusters, CV_64FC1 ));
CV_CALL( log_weights = cvCreateMat( 1, nclusters, CV_64FC1 ));
CV_CALL( covs_item = cvCreateMat( dims, dims, CV_64FC1 ));
CV_CALL( centered_sample = cvCreateMat( 1, dims, CV_64FC1 ));
CV_CALL( cov_eigen_values = cvCreateMat( inv_eigen_values->rows, inv_eigen_values->cols, CV_64FC1 ));
CV_CALL( samples = cvCreateMat( nsamples, dims, CV_64FC1 ));
CV_CALL( sum_probs = cvCreateMat( 1, nclusters, CV_64FC1 ));
sp_data = sum_probs->data.db;
// copy the training data into double-precision matrix
for( i = 0; i < nsamples; i++ )
{
const float* src = train_data.data.fl[i];
double* dst = (double*)(samples->data.ptr + samples->step*i);
for( j = 0; j < dims; j++ )
dst[j] = src[j];
}
if( start_step != START_M_STEP )
{
for( k = 0; k < nclusters; k++ )
{
if( is_general || is_diagonal )
{
w = cvGetRow( cov_eigen_values, &whdr, k );
if( is_general )
cvSVD( covs[k], w, cov_rotate_mats[k], 0, CV_SVD_U_T );
else
cvTranspose( cvGetDiag( covs[k], &diag ), w );
w_data = w->data.db;
for( j = 0, det = 1.; j < dims; j++ )
det *= w_data[j];
if( det < min_det_value )
{
if( start_step == START_AUTO_STEP )
det = min_det_value;
else
EXIT;
}
log_det->data.db[k] = det;
}
else
{
d = cvTrace(covs[k]).val[0]/(double)dims;
if( d < min_variation )
{
if( start_step == START_AUTO_STEP )
d = min_variation;
else
EXIT;
}
cov_eigen_values->data.db[k] = d;
log_det->data.db[k] = d;
}
}
cvLog( log_det, log_det );
if( is_spherical )
cvScale( log_det, log_det, dims );
}
for( n = 0; n < params.term_crit.max_iter; n++ )
{
if( n > 0 || start_step != START_M_STEP )
{
// e-step: compute probs_ik from means_k, covs_k and weights_k.
CV_CALL(cvLog( weights, log_weights ));
// S_ik = -0.5[log(det(Sigma_k)) + (x_i - mu_k)' Sigma_k^(-1) (x_i - mu_k)] + log(weights_k)
for( k = 0; k < nclusters; k++ )
{
CvMat* u = cov_rotate_mats[k];
const double* mean = (double*)(means->data.ptr + means->step*k);
w = cvGetRow( cov_eigen_values, &whdr, k );
iw = cvGetRow( inv_eigen_values, &iwhdr, k );
cvDiv( 0, w, iw );
w_data = (double*)(inv_eigen_values->data.ptr + inv_eigen_values->step*k);
for( i = 0; i < nsamples; i++ )
{
double *csample = centered_sample->data.db, p = log_det->data.db[k];
const double* sample = (double*)(samples->data.ptr + samples->step*i);
double* pp = (double*)(probs->data.ptr + probs->step*i);
for( j = 0; j < dims; j++ )
csample[j] = sample[j] - mean[j];
if( is_general )
cvGEMM( centered_sample, u, 1, 0, 0, centered_sample, CV_GEMM_B_T );
for( j = 0; j < dims; j++ )
p += csample[j]*csample[j]*w_data[is_spherical ? 0 : j];
pp[k] = -0.5*p + log_weights->data.db[k];
// S_ik <- S_ik - max_j S_ij
if( k == nclusters - 1 )
{
double max_val = 0;
for( j = 0; j < nclusters; j++ )
max_val = MAX( max_val, pp[j] );
for( j = 0; j < nclusters; j++ )
pp[j] -= max_val;
}
}
}
CV_CALL(cvExp( probs, probs )); // exp( S_ik )
cvZero( sum_probs );
// alpha_ik = exp( S_ik ) / sum_j exp( S_ij ),
// log_likelihood = sum_i log (sum_j exp(S_ij))
for( i = 0, _log_likelihood = likelihood_bias; i < nsamples; i++ )
{
double* pp = (double*)(probs->data.ptr + probs->step*i), sum = 0;
for( j = 0; j < nclusters; j++ )
sum += pp[j];
sum = 1./MAX( sum, DBL_EPSILON );
for( j = 0; j < nclusters; j++ )
{
double p = pp[j] *= sum;
sp_data[j] += p;
}
_log_likelihood -= log( sum );
}
// check termination criteria
if( fabs( (_log_likelihood - prev_log_likelihood) / prev_log_likelihood ) < params.term_crit.epsilon )
break;
prev_log_likelihood = _log_likelihood;
}
// m-step: update means_k, covs_k and weights_k from probs_ik
cvGEMM( probs, samples, 1, 0, 0, means, CV_GEMM_A_T );
for( k = 0; k < nclusters; k++ )
{
double sum = sp_data[k], inv_sum = 1./sum;
CvMat* cov = covs[k], _mean, _sample;
w = cvGetRow( cov_eigen_values, &whdr, k );
w_data = w->data.db;
cvGetRow( means, &_mean, k );
cvGetRow( samples, &_sample, k );
// update weights_k
weights->data.db[k] = sum;
// update means_k
cvScale( &_mean, &_mean, inv_sum );
// compute covs_k
cvZero( cov );
cvZero( w );
for( i = 0; i < nsamples; i++ )
{
double p = probs->data.db[i*nclusters + k]*inv_sum;
_sample.data.db = (double*)(samples->data.ptr + samples->step*i);
if( is_general )
{
cvMulTransposed( &_sample, covs_item, 1, &_mean );
cvScaleAdd( covs_item, cvRealScalar(p), cov, cov );
}
else
for( j = 0; j < dims; j++ )
{
double val = _sample.data.db[j] - _mean.data.db[j];
w_data[is_spherical ? 0 : j] += p*val*val;
}
}
if( is_spherical )
{
d = w_data[0]/(double)dims;
d = MAX( d, min_variation );
w->data.db[0] = d;
log_det->data.db[k] = d;
}
else
{
if( is_general )
cvSVD( cov, w, cov_rotate_mats[k], 0, CV_SVD_U_T );
cvMaxS( w, min_variation, w );
for( j = 0, det = 1.; j < dims; j++ )
det *= w_data[j];
log_det->data.db[k] = det;
}
}
cvConvertScale( weights, weights, 1./(double)nsamples, 0 );
cvMaxS( weights, DBL_MIN, weights );
cvLog( log_det, log_det );
if( is_spherical )
cvScale( log_det, log_det, dims );
} // end of iteration process
//log_weight_div_det[k] = -2*log(weights_k/det(Sigma_k))^0.5) = -2*log(weights_k) + log(det(Sigma_k)))
if( log_weight_div_det )
{
cvScale( log_weights, log_weight_div_det, -2 );
cvAdd( log_weight_div_det, log_det, log_weight_div_det );
}
/* Now finalize all the covariation matrices:
1) if <cov_mat_type> == COV_MAT_DIAGONAL we used array of <w> as diagonals.
Now w[k] should be copied back to the diagonals of covs[k];
2) if <cov_mat_type> == COV_MAT_SPHERICAL we used the 0-th element of w[k]
as an average variation in each cluster. The value of the 0-th element of w[k]
should be copied to the all of the diagonal elements of covs[k]. */
if( is_spherical )
{
for( k = 0; k < nclusters; k++ )
cvSetIdentity( covs[k], cvScalar(cov_eigen_values->data.db[k]));
}
else if( is_diagonal )
{
for( k = 0; k < nclusters; k++ )
cvTranspose( cvGetRow( cov_eigen_values, &whdr, k ),
cvGetDiag( covs[k], &diag ));
}
cvDiv( 0, cov_eigen_values, inv_eigen_values );
log_likelihood = _log_likelihood;
__END__;
cvReleaseMat( &log_det );
cvReleaseMat( &log_weights );
cvReleaseMat( &covs_item );
cvReleaseMat( ¢ered_sample );
cvReleaseMat( &cov_eigen_values );
cvReleaseMat( &samples );
cvReleaseMat( &sum_probs );
return log_likelihood;
}
CvMat* vgg_X_from_xP_nonlin(CvMat* u1, CvMat** P1, CvMat* imsize1, int K)
{
CvMat* u;
CvMat** P=new CvMat* [K];
CvMat* imsize;
u=cvCreateMat(u1->rows,u1->cols,u1->type);
cvCopy(u1,u);
int kp;
for(kp=0;kp<K;kp++)
{
P[kp]=cvCreateMat(P1[kp]->rows,P1[kp]->cols,P1[kp]->type);
cvCopy(P1[kp],P[kp]);
}
imsize=cvCreateMat(imsize1->rows,imsize1->cols,imsize1->type);
cvCopy(imsize1,imsize);
CvMat* X;
CvMat* H;
CvMat* u_2_rows;
CvMat* W;
CvMat* U;
CvMat* T;
CvMat* Y;
CvMat** Q=new CvMat*[K];//Q is a variable not well defined
CvMat* J;
CvMat* e;
CvMat* J_tr;
CvMat* eprev;
CvMat* JtJ;
CvMat* Je;
CvMat* Y_new;
CvMat* T_2_cols;
CvMat* T_rest_cols;
CvMat* X_T;
CvScalar f, inf;
int i, mat_id;
int u_rows = u->rows;
int u_cols = u->cols;
double lambda_min, lambda_max;
CvMat* imsize_col = cvCreateMat(2, 1, CV_64FC1);
/* K is the number of images */
if(K < 2)
{
printf("\n Cannot reconstruct 3D from 1 image");
return 0;
}
/* Create temporary matrix for the linear function */
u_2_rows = cvCreateMat(2, u_cols, CV_64FC1);
/* Initialize the temporary matrix by extracting the first two rows */
u_2_rows = cvGetRows(u, u_2_rows, 0, 2, 1);
/* Call the linear function */
X = vgg_X_from_xP_lin(u_2_rows, P, K, imsize);
imsize_col = cvGetCol(imsize, imsize_col, 0);
f = cvSum(imsize_col);
f.val[0] = 4 / f.val[0];
/* Create and initialize H matrix */
H = cvCreateMat(3, 3, CV_64FC1);
H->data.db[0] = f.val[0];
H->data.db[1] = 0;
H->data.db[2] = ((-1) * f.val[0] * cvmGet(imsize, 0, 0)) / 2;
H->data.db[3] = 0;
H->data.db[4] = f.val[0];
H->data.db[5] = ((-1) * f.val[0] * cvmGet(imsize, 1, 0)) / 2;
H->data.db[6] = 0;
H->data.db[7] = 0;
H->data.db[8] = 1;
for(mat_id = 0; mat_id < K ; mat_id++)
{
cvMatMul(H, P[mat_id], P[mat_id]);
}
/* u = H * u; */
cvMatMul(H, u, u);
/*
//debug
printf("....H\n");
CvMat_printdb(stdout,"%7.3f ",H);
//debug
printf("....u\n");
CvMat_printdb(stdout,"%7.3f ",u);
*/
/* Parametrize X such that X = T*[Y;1]; thus x = P*T*[Y;1] = Q*[Y;1] */
/* Create the SVD matrices X = U*W*V'*/
X_T = cvCreateMat(X->cols, X->rows, CV_64FC1); /* M * N */
W = cvCreateMat(X_T->rows, X_T->cols, CV_64FC1); /* M * N */
U = cvCreateMat(X_T->rows, X_T->rows, CV_64FC1); /* M * M */
T = cvCreateMat(X_T->cols, X_T->cols, CV_64FC1); /* N * N */
cvTranspose(X, X_T);
cvSVD(X_T, W, U, T);
cvReleaseMat(&W);
/* T = T(:,[2:end 1]); */
/* Initialize the temporary matrix by extracting the first two columns */
/* Create temporary matrix for the linear function */
T_2_cols = cvCreateMat(T->rows, 2, CV_64FC1);
T_rest_cols = cvCreateMat(T->rows, (T->cols - 2), CV_64FC1);
/* Initialize the temporary matrix by extracting the first two columns */
T_2_cols= sfmGetCols(T,0,0);
T_rest_cols=sfmGetCols(T,1,T->cols-1);
T=sfmAlignMatH(T_rest_cols,T_2_cols);
for(mat_id = 0; mat_id < K ; mat_id++)
{
/* Create temporary matrix for the linear function */
Q[mat_id] = cvCreateMat(P[mat_id]->rows, T->cols, CV_64FC1);
cvMatMul(P[mat_id], T, Q[mat_id]);
}
/*
//debug
printf("....Q0\n");
CvMat_printdb(stdout,"%7.3f ",Q[0]);
//debug
printf("....Q1\n");
CvMat_printdb(stdout,"%7.3f ",Q[1]);
*/
/* Newton estimation */
/* Create the required Y matrix for the Newton process */
Y = cvCreateMat(3, 1, CV_64FC1);
cvSetZero(Y); /* Y = [0;0;0] */
/* Initialize the infinite array */
inf.val[0] = INF;
inf.val[1] = INF;
inf.val[2] = INF;
inf.val[3] = INF;
eprev = cvCreateMat(1, 1, CV_64FC1);
cvSet(eprev, inf, 0);
for(i = 0 ; i < 10 ; i++)
{
// printf("i=%d....\n",i);
int pass;
double RCondVal;
//initialize e,J before using.
e=cvCreateMat(2*K,1,CV_64FC1);
J=cvCreateMat(2*K,3,CV_64FC1);
pass = resid(Y, u, Q, K, e, J);
J_tr = cvCreateMat(J->cols, J->rows, CV_64FC1);
cvTranspose(J, J_tr);
JtJ = cvCreateMat(J->cols, J->cols, CV_64FC1);
cvMatMul(J_tr, J, JtJ);
//prevent memory leak;
cvReleaseMat(&W);
/* Create the SVD matrices JtJ = U*W*V'*/
W = cvCreateMat(J->cols, J->cols, CV_64FC1);
cvSVD(JtJ, W);
/*
//debug
printf("....W\n");
CvMat_printdb(stdout,"%7.3f ",W);
*/
lambda_max = W->data.db[0];
lambda_min = W->data.db[((W->rows * W->cols) - 1)];
RCondVal = lambda_min / lambda_max;
if(1 - (cvNorm(e, 0, CV_L2, 0) / cvNorm(eprev, 0, CV_L2, 0)) < 1000 * EPS)
{
cvReleaseMat(&J);
cvReleaseMat(&e);
cvReleaseMat(&J_tr);
cvReleaseMat(&JtJ);
cvReleaseMat(&W);
break;
}
if(RCondVal < 10 * EPS)
{
cvReleaseMat(&J);
cvReleaseMat(&e);
cvReleaseMat(&J_tr);
cvReleaseMat(&JtJ);
cvReleaseMat(&W);
break;
}
cvReleaseMat(&eprev);
eprev = cvCreateMat(e->rows, e->cols, CV_64FC1);
cvCopy(e, eprev);
Je = cvCreateMat(J->cols, e->cols, CV_64FC1);
cvMatMul(J_tr, e, Je); /* (J'*e) */
/*
//debug
printf("....J_tr\n");
CvMat_printdb(stdout,"%7.3f ",J_tr);
//debug
printf("....e\n");
CvMat_printdb(stdout,"%7.3f ",e);
//debug
printf("....JtJ\n");
CvMat_printdb(stdout,"%7.3f ",JtJ);
//debug
printf("....Je\n");
CvMat_printdb(stdout,"%7.3f ",Je);
//debug
printf("....JtJ\n");
CvMat_printdb(stdout,"%7.3f ",JtJ);
*/
cvInvert(JtJ,JtJ);
/* (J'*J)\(J'*e) */
Je=sfmMatMul(JtJ, Je);
/*
//debug
printf("....Je\n");
CvMat_printdb(stdout,"%7.3f ",Je);
*/
/* Y = Y - (J'*J)\(J'*e) */
cvSub(Y, Je, Y, 0);
/*
//debug
printf("....Y\n");
CvMat_printdb(stdout,"%7.3f ",Y);
*/
cvReleaseMat(&J);
cvReleaseMat(&e);
cvReleaseMat(&J_tr);
cvReleaseMat(&JtJ);
cvReleaseMat(&Je);
cvReleaseMat(&W);
}
Y_new = cvCreateMat(4, 1, CV_64FC1);
PutMatV(Y,Y_new,0);
Y_new->data.db[3]=1;
/*
//debug
printf("....Y_new\n");
CvMat_printdb(stdout,"%7.3f ",Y_new);
printf("....T\n");
CvMat_printdb(stdout,"%7.3f ",T);
*/
/* Obtain the new X */
cvMatMul(T, Y_new, X);
cvReleaseMat(&H);
cvReleaseMat(&u_2_rows);
cvReleaseMat(&U);
cvReleaseMat(&T);
cvReleaseMat(&Y);
cvReleaseMat(&Y_new);
cvReleaseMat(&T_2_cols);
cvReleaseMat(&T_rest_cols);
for(kp=0;kp<K;kp++)
{
cvReleaseMat(&P[kp]);
}
cvReleaseMat(&u);
cvReleaseMat(&imsize);
cvReleaseMatGrp(Q,K);
return X;
}
int main( int argc, char * argv[] )
{
const char * WINDOW_NAME = "Original Image vs. Box Filter vs. Gaussian";
const int QUIT_KEY_CODE = 113;
int box_filter_width = 3;
float sigma = 1.0;
std::string filename = "cameraman.tif";
ImageRAII original_image( filename );
CvSize image_dimensions = { original_image.image->width, original_image.image->height };
ImageRAII box_filter_image( cvCreateImage( image_dimensions, original_image.image->depth, 3 ) );
ImageRAII gaussian_image( cvCreateImage( image_dimensions, original_image.image->depth, 3 ) );
ImageRAII combined_image( cvCreateImage( cvSize( original_image.image->width * 3, original_image.image->height ), original_image.image->depth, 3 ) );
MatrixRAII box_filter = makeBoxFilter( box_filter_width );
MatrixRAII gaussian_filter_x = make1DGaussianFilter( sigma );
MatrixRAII gaussian_filter_y = cvCreateMat( sigma * 5, 1, CV_64FC1 );
cvTranspose( gaussian_filter_x.matrix, gaussian_filter_y.matrix );
std::vector<ImageRAII> original_image_channels( 3 );
std::vector<ImageRAII> box_filter_channels( 3 );
std::vector<ImageRAII> gaussian_filter_channels( 3 );
std::vector<ImageRAII> gaussian_filter_2_channels( 3 );
// initialize image channel vectors
for( int i = 0; i < original_image.image->nChannels; i++ )
{
original_image_channels[i].image = cvCreateImage( image_dimensions, original_image.image->depth, 1 );
box_filter_channels[i].image = cvCreateImage( image_dimensions, original_image.image->depth, 1 );
gaussian_filter_channels[i].image = cvCreateImage( image_dimensions, original_image.image->depth, 1 );
gaussian_filter_2_channels[i].image = cvCreateImage( image_dimensions, original_image.image->depth, 1 );
}
// split image channels
cvSplit( original_image.image, original_image_channels[0].image, original_image_channels[1].image, original_image_channels[2].image, NULL );
// apply filters
for( int i = 0; i < original_image.image->nChannels; i++ )
{
cvFilter2D( original_image_channels[i].image, box_filter_channels[i].image, box_filter.matrix );
cvFilter2D( original_image_channels[i].image, gaussian_filter_channels[i].image, gaussian_filter_x.matrix );
cvFilter2D( gaussian_filter_channels[i].image, gaussian_filter_2_channels[i].image, gaussian_filter_y.matrix );
}
// Merge channels back
cvMerge( box_filter_channels[0].image, box_filter_channels[1].image, box_filter_channels[2].image, NULL, box_filter_image.image );
cvMerge( gaussian_filter_2_channels[0].image, gaussian_filter_2_channels[1].image, gaussian_filter_2_channels[2].image, NULL, gaussian_image.image );
// Combine images side by side
int step = original_image.image->widthStep;
int step_destination = combined_image.image->widthStep;
int nChan = original_image.image->nChannels;
char *buf = combined_image.image->imageData;
char *original_buf = original_image.image->imageData;
char *box_filter_buf = box_filter_image.image->imageData;
char *gaussian_filter_buf = gaussian_image.image->imageData;
for( int row = 0; row < original_image.image->width; row++ )
{
for( int col = 0; col < original_image.image->height; col++ )
{
int width_adjust = 0;
//original image
// blue
*( buf + row * step_destination + nChan * col + width_adjust ) = *( original_buf + row * step + nChan * col );
// green
*( buf + row * step_destination + nChan * col + 1 + width_adjust ) = *( original_buf + row * step + nChan * col );
// red
*( buf + row * step_destination + nChan * col + 2 + width_adjust ) = *( original_buf + row * step + nChan * col );
// box filter
width_adjust = original_image.image->height * nChan;
*( buf + row * step_destination + nChan * col + width_adjust ) = *( box_filter_buf + row * step + nChan * col );
*( buf + row * step_destination + nChan * col + 1 + width_adjust ) = *( box_filter_buf + row * step + nChan * col );
*( buf + row * step_destination + nChan * col + 2 + width_adjust ) = *( box_filter_buf + row * step + nChan * col );
// gaussian filter
width_adjust = original_image.image->height * 2 * nChan;
*( buf + row * step_destination + nChan * col + width_adjust ) = *( gaussian_filter_buf + row * step + nChan * col );
*( buf + row * step_destination + nChan * col + 1 + width_adjust ) = *( gaussian_filter_buf + row * step + nChan * col );
*( buf + row * step_destination + nChan * col + 2 + width_adjust ) = *( gaussian_filter_buf + row * step + nChan * col );
}
}
// create windows
cvNamedWindow( WINDOW_NAME, CV_WINDOW_AUTOSIZE );
cvShowImage( WINDOW_NAME, combined_image.image );
// wait for keyboard input
int key_code = 0;
while( key_code != QUIT_KEY_CODE )
{
key_code = cvWaitKey( 0 );
}
return 0;
}
bool MT_Cholesky(const CvMat* src, CvMat* dst, unsigned int orientation)
{
if(!check_size_type(src, "Input"))
{
return false;
}
if(!dst)
{
fprintf(stderr, "MT_Cholesky Error: Output is not allocated.\n");
return false;
}
else
{
if((dst->rows != src->rows) || (dst->cols != src->cols) ||
(cvGetElemType(dst) != cvGetElemType(src)))
{
fprintf(stderr,
"MT_Cholesky Error: Output matrix must be same size"
" and type as input.\n");
return false;
}
}
if((orientation != MT_CHOLESKY_UPPER_TRIANGULAR) &&
(orientation != MT_CHOLESKY_LOWER_TRIANGULAR))
{
fprintf(stderr,
"MT_Cholesky Error: Orientation must be either "
"MT_CHOLESKY_UPPER_TRIANGULAR (%d) or "
"MT_CHOLESKY_LOWER_TRIANGULAR (%d).\n",
MT_CHOLESKY_UPPER_TRIANGULAR,
MT_CHOLESKY_LOWER_TRIANGULAR);
return false;
}
double p;
unsigned int R = src->rows;
cvSet(dst, cvScalar(0));
double* src_data = src->data.db;
double* dst_data = dst->data.db;
for(int i = 0; i < R; i++)
{
for(int j = 0; j < R; j++)
{
if(i < j)
{
continue;
}
p = 0;
if(i==j)
{
p = src_data[i*R + i];
for(int k = 0; k <= j-1; k++)
{
p -= dst_data[i*R+k]*dst_data[i*R+k];
}
if( p < 0 ){ // NOTE these may not be numerically correct, but it is a safeguard
p = fabs(p);
}
if( p == 0 ){
p = 1e-6;
}
dst_data[i*R+i] = sqrt(p);
} else {
p = src_data[i*R+j];
for(int k = 0; k <= j-1; k++)
{
p -= dst_data[i*R+k]*dst_data[j*R+k];
}
dst_data[i*R+j] = p/dst_data[j*R+j];
}
}
}
if(orientation == MT_CHOLESKY_UPPER_TRIANGULAR)
{
cvTranspose(dst, dst);
}
return true;
}
void pca(IplImage *image)
{
int i, j, k;
int width, height, step, channels;
unsigned char *src;
float *u;
unsigned char *b;
unsigned char *g;
height = image->height;
width = image->width;
step = image->widthStep;
channels = image->nChannels;
src = (unsigned char *)image->imageData;
u = (float *)malloc(sizeof(float) * height * channels);
b = (unsigned char *)malloc(sizeof(unsigned char) * width * height * channels);
g = (unsigned char *)malloc(sizeof(unsigned char) * height * channels);
//bg = (unsigned char *)malloc(sizeof(unsigned char) * width * height);
//bb = (unsigned char *)malloc(sizeof(unsigned char) * width * height);
init_floatMat(u, height * channels);
init_charMat(b, height * width * channels);
//init_mat(bg, height * width);
//init_mat(bb, height * widht);
init_charMat(g, height * channels);
//begin by calculating the empirical mean
//u[1..height] = 1/n sum(src[i,j])
for(i = 0; i < height; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
//need to fix floating point arithmetic
u[i*channels + k] += (float)src[i*step+j*channels+k] / (float)width;
}
}
}
printf("empirical means working\n");
//we next calculate the deviation from the mean
//b = src[i,j] - u;
for(i = 0; i < height; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
b[i*step+j*channels+k] = src[i*step+j*channels+k] - (int)u[i*channels+k];
}
}
}
printf("deviation working\n");
//we now need to find the covariance matrix
//b in opencv matrix form
CvMat bMat = cvMat(height, width, CV_8UC3, b);
//CvMat bgMat = cvMat(height, width, CV_8U3, bg);
//CvMat bbMat = cvMat(height, width, CV_8U3, bb);
CvMat *bMatb = cvCreateMat(height, width, CV_8UC1);
CvMat *bMatg = cvCreateMat(height, width, CV_8UC1);
CvMat *bMatr = cvCreateMat(height, width, CV_8UC1);
cvSplit(&bMat, bMatb, bMatg, bMatr, 0);
//covariance matrix
CvMat *cb = cvCreateMat(height, height, CV_32FC1);
CvMat *cg = cvCreateMat(height, height, CV_32FC1);
CvMat *cr = cvCreateMat(height, height, CV_32FC1);
CvMat *c = cvCreateMat(height, height, CV_32FC3);
cvMulTransposed(bMatb, cb, 0, NULL, 1.0/(double)width);
cvMulTransposed(bMatg, cg, 0, NULL, 1.0/(double)width);
cvMulTransposed(bMatr, cr, 0, NULL, 1.0/(double)width);
printf("Covariance Matrix working\n");
//eigenvector and values
CvMat *eMatb = cvCreateMat(height, height, CV_32FC1);
CvMat *lMatb = cvCreateMat(height, 1, CV_32FC1);
CvMat *eMatr = cvCreateMat(height, height, CV_32FC1);
CvMat *lMatr = cvCreateMat(height, 1, CV_32FC1);
CvMat *eMatg = cvCreateMat(height, height, CV_32FC1);
CvMat *lMatg = cvCreateMat(height, 1, CV_32FC1);
printf("Successfully created eigen matrices\n");
//cvEigenVV(cb, eMatb, lMatb, 1e-10, -1, -1);
//cvEigenVV(cg, eMatg, lMatg, 1e-10, -1, -1);
//cvEigenVV(cr, eMatr, lMatr, 1e-10, -1, -1);
cvSVD(cb, lMatb, eMatb, NULL, CV_SVD_U_T & CV_SVD_MODIFY_A);
cvSVD(cg, lMatg, eMatg, NULL, CV_SVD_U_T & CV_SVD_MODIFY_A);
cvSVD(cr, lMatr, eMatr, NULL, CV_SVD_U_T & CV_SVD_MODIFY_A);
printf("Eigentvectors and Eigenvalues passes\n");
unsigned char *lb = lMatb->data.ptr;
unsigned char *eb = eMatb->data.ptr;
unsigned char *lr = lMatr->data.ptr;
unsigned char *er = eMatr->data.ptr;
unsigned char *lg = lMatg->data.ptr;
unsigned char *eg = eMatg->data.ptr;
for(i = 0; i < height; i++)
{
for(j = 0; j < i+1; j++)
{
for(k = 0; k < channels; k++)
{
g[i*channels] += lb[i*channels+k];
}
}
}
printf("Successfully computed cumulative energy\n");
int L = 0;
float currVal = 0.0;
/*for(i = 0; i < height; i++)
{
if(currVal >= 0.9)
{
L = i;
break;
}
currVal = 0.0;
for(k = 0; k < channels; k++)
{
currVal += (float)g[i*channels+k] / (float)g[height - 3 + k];
}
}*/
L = 2;
printf("Successfully computed L with value of %d\n", L);
unsigned char *w;
w = (unsigned char *)malloc(sizeof(unsigned char) * height * L * channels);
for(i = 0; i < height; i++)
{
for(j = 0; j < L; j++)
{
//for(k = 0; k < channels; k++)
//{
w[i*(L*channels)+j*channels] = eb[i*height+j];
w[i*(L*channels)+j*channels+1] = eg[i*height+j];
w[i*(L*channels)+j*channels+2] = er[i*height+j];
//}
}
}
printf("Successfully created basis vectors\n");
unsigned char *s;
s = (unsigned char *)malloc(sizeof(unsigned char) * height * channels);
for(i = 0; i < height; i++)
{
s[i*channels] = sqrt(cb->data.ptr[i*cb->cols+i]);
s[i*channels+1] = sqrt(cg->data.ptr[i*cg->cols+i]);
s[i*channels+2] = sqrt(cr->data.ptr[i*cr->cols+i]);
}
printf("Successfully convertd source data to z-scores\n");
unsigned char *z;
z = (unsigned char *)malloc(sizeof(unsigned char) * height * width * channels);
for(i = 0; i < height; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
z[i*step+j*channels+k] = (float)b[i*step+j*channels+k] / (float)s[i*channels+k];
}
}
}
printf("Successfully calculated Z\n");
//Projecting the z-scores of the data onto the new basis
//CvMat wMatb = cvMat(height, L, CV_32FC1, eb);
//CvMat wMatr = cvMat(height, L, CV_32FC1, er);
//CvMat wMatg = cvMat(height, L, CV_32FC1, eg);
CvMat wMat = cvMat(L, height, CV_32FC3, w);
//cvMerge(&wMatb, &wMatr, &wMatg, 0, wMat);
CvMat *wMatT = cvCreateMat(height, L, CV_32FC3);
cvTranspose(&wMat, wMatT);
//char *dat = wMatT->data.ptr;
/*for(i = 0; i < L; i++)
{
for(j = 0; j < height; j++)
{
for(k = 0; k < channels; k++)
{
printf("%d ", dat[i*L+j*channels+k]);
}
printf("\n");
}
}*/
//CvMat *wMatTb = cvCreateMat(height, height, CV_8UC1);
//CvMat *wMatTg = cvCreateMat(height, height, CV_8UC1);
//CvMat *wMatTr = cvCreateMat(height, height, CV_8UC1);
//cvSplit(wMatT, wMatTb, wMatTg, wMatTr, 0);
printf("Transpose of W\n");
CvMat zMat = cvMat(height, width, CV_32FC3, z);
//CvMat *zMatb = cvCreateMat(height, width, CV_8UC1);
//CvMat *zMatg = cvCreateMat(height, width, CV_8UC1);
//CvMat *zMatr = cvCreateMat(height, width, CV_8UC1);
//cvSplit(&zMat, zMatb, zMatg, zMatr, 0);
printf("created z matrix\n");
CvMat *yMat = cvCreateMat(height, width, CV_32FC3);
//CvMat *yMatb = cvCreateMat(height, width, CV_8UC1);
//CvMat *yMatg = cvCreateMat(height, width, CV_8UC1);
//CvMat *yMatr = cvCreateMat(height, width, CV_8UC1);
printf("computed y matrix\n");
char *wdat = wMatT->data.ptr;
char *zdat = zMat.data.ptr;
char *ydat = (char *)malloc(sizeof(char) * L * width * channels);
init_charMat(ydat, L*width*channels);
int r = 0;
for(i = 0; i < L; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
for(r = 0; r < width; r++)
{
ydat[i*step+j*channels+k] += wdat[i*step+r*channels+k] * zdat[r*step+j*channels+k];
}
}
}
}
//char *fnorm = (char *)malloc(sizeof(char) * width);
/*for(i = 0; i < width * channels; i++)
{
printf("%d\n", ydat[i]);
}*/
/*float adotb = cvDotProduct(wMatT, &zMat);
float bdota = cvDotProduct(&zMat, wMatT);
float div = adotb/bdota;
cvDiv(NULL, &zMat, yMat, div);*/
//cvMul(wMatT, &zMat, yMat, 1.0);
//cvMul(wMatTg, zMatg, yMatg, 1.0);
//cvMul(wMatTr, zMatr, yMatr, 1.0);
printf("Matrix Multiply Successful\n");
//char *output = yMat->data.ptr;
//printf("height: %d width: %d channels: %d", height, width, channels);
for(i = 0; i < height; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
src[i*step+j*channels+k] = src[i*step+j*channels+k] * ydat[(i*channels)+width+k];
}
}
}
printf("Successfully multiplied\n");
//cvMerge(yMatb, yMatg, yMatr, 0, yMat);
}
//coord1:校正用之點 coord2:被校正之點
//可執行四個以上的轉換
CvMat* Morphining::GetProjectionMatrix(vector<Coordinate> adjust,vector<Coordinate> adjusted,bool debug)
{
//spec://cvSetReal2D(cvMat,row,col);
if(m_numFeature < 4)
{
printf("Not enough tiles points to execute perspective transform!\n");
exit(0);
}
//Element of PerspectiveTransformation
CvMat *b = cvCreateMat( 8 , 1 , CV_32FC1);
CvMat *MUL_8_2N = cvCreateMat( 8 , 2*m_numFeature , CV_32FC1);
CvMat *b_temp = cvCreateMat( 3 , 3 , CV_32FC1);
CvMat *b_temp_inv = cvCreateMat( 3 , 3 , CV_32FC1);
//Size:2n*1
for(int i = 0;i < m_numFeature;i++)
{
cvSetReal2D(U, i, 0, adjusted[i].x);
cvSetReal2D(U, i + m_numFeature, 0, adjusted[i].y);
}
//Size:2n*8
for(int i = 0; i < m_numFeature;i++)
{
double col_6 = -1.0 * adjust[i].x * cvGetReal2D(U,i,0);
double col_7 = -1.0 * adjust[i].y * cvGetReal2D(U,i,0);
double col_6_pair = -1.0 * adjust[i].x * cvGetReal2D(U,i + m_numFeature,0);
double col_7_pair = -1.0 * adjust[i].y * cvGetReal2D(U,i + m_numFeature,0);
cvSetReal2D(W, i, 0, adjust[i].x);
cvSetReal2D(W, i, 1, adjust[i].y);
cvSetReal2D(W, i, 2, 1);
cvSetReal2D(W, i, 3, 0);
cvSetReal2D(W, i, 4, 0);
cvSetReal2D(W, i, 5, 0);
cvSetReal2D(W, i, 6, col_6);
cvSetReal2D(W, i, 7, col_7);
cvSetReal2D(W, i + m_numFeature, 0, 0);
cvSetReal2D(W, i + m_numFeature, 1, 0);
cvSetReal2D(W, i + m_numFeature, 2, 0);
cvSetReal2D(W, i + m_numFeature, 3, adjust[i].x);
cvSetReal2D(W, i + m_numFeature, 4, adjust[i].y);
cvSetReal2D(W, i + m_numFeature, 5, 1);
cvSetReal2D(W, i + m_numFeature, 6, col_6_pair);
cvSetReal2D(W, i + m_numFeature, 7, col_7_pair);
}
if(debug)
{
std::cout << "U:\n";
PrintMatrix(U, 2*m_numFeature,1,1);
std::cout << "W:\n";
PrintMatrix(W, 2*m_numFeature,8,1);
}
if(W->cols == W->rows )cvInv(W,MUL_8_2N);
else { //pseudo inverse
cvTranspose(W,T);
cvmMul(T,W,MulResult);
cvInv(MulResult,Inv);
cvmMul(Inv,T,MUL_8_2N);
/*printf("T\n");
PrintMatrix(T, 8,2*m_numFeature,1);*/
/*printf("MulResult\n");
PrintMatrix(MulResult, 8,2*m_numFeature,1);*/
/*printf("Inv\n");
PrintMatrix(Inv,2*m_numFeature, 8,1);*/
}
cvmMul(MUL_8_2N,U,b);
cvSetReal2D(b_temp, 0, 0, cvGetReal2D(b,0,0)); cvSetReal2D(b_temp, 0, 1, cvGetReal2D(b,1,0)); cvSetReal2D(b_temp, 0, 2, cvGetReal2D(b,2,0));
cvSetReal2D(b_temp, 1, 0, cvGetReal2D(b,3,0)); cvSetReal2D(b_temp, 1, 1, cvGetReal2D(b,4,0)); cvSetReal2D(b_temp, 1, 2, cvGetReal2D(b,5,0));
cvSetReal2D(b_temp, 2, 0, cvGetReal2D(b,6,0)); cvSetReal2D(b_temp, 2, 1, cvGetReal2D(b,7,0)); cvSetReal2D(b_temp, 2, 2, 1);
cvInv(b_temp,b_temp_inv);
if(debug)
{
std::cout << "b_temp\n";
PrintMatrix(b_temp,3, 3,1);
std::cout << "b_temp_inv\n";
PrintMatrix(b_temp_inv,3, 3,1);
std::cout << "......................................................................\n";
std::cout << "......................................................................\n";
}
cvReleaseMat(&b);
cvReleaseMat(&MUL_8_2N);
cvReleaseMat(&b_temp);
//return b_temp;
return b_temp_inv;
}
UINT WINAPI
//DWORD WINAPI
#elif defined(POSIX_SYS)
// using pthread
void *
#endif
ChessRecognition::HoughLineThread(
#if defined(WINDOWS_SYS)
LPVOID
#elif defined(POSIX_SYS)
void *
#endif
Param) {
// 실제로 뒤에서 동작하는 windows용 thread함수.
// 함수 인자로 클래스를 받아옴.
ChessRecognition *_TChessRecognition = (ChessRecognition *)Param;
_TChessRecognition->_HoughLineBased = new HoughLineBased();
CvSeq *_TLineX, *_TLineY;
double _TH[] = { -1, -7, -15, 0, 15, 7, 1 };
CvMat _TDoGX = cvMat(1, 7, CV_64FC1, _TH);
CvMat* _TDoGY = cvCreateMat(7, 1, CV_64FC1);
cvTranspose(&_TDoGX, _TDoGY); // transpose(&DoGx) -> DoGy
double _TMinValX, _TMaxValX, _TMinValY, _TMaxValY, _TMinValT, _TMaxValT;
int _TKernel = 1;
// Hough 사용되는 Image에 대한 Initialize.
IplImage *iplTemp = cvCreateImage(cvSize(_TChessRecognition->_Width, _TChessRecognition->_Height), IPL_DEPTH_32F, 1);
IplImage *iplDoGx = cvCreateImage(cvGetSize(iplTemp), IPL_DEPTH_32F, 1);
IplImage *iplDoGy = cvCreateImage(cvGetSize(iplTemp), IPL_DEPTH_32F, 1);
IplImage *iplDoGyClone = cvCloneImage(iplDoGy);
IplImage *iplDoGxClone = cvCloneImage(iplDoGx);
IplImage *iplEdgeX = cvCreateImage(cvGetSize(iplTemp), 8, 1);
IplImage *iplEdgeY = cvCreateImage(cvGetSize(iplTemp), 8, 1);
CvMemStorage* _TStorageX = cvCreateMemStorage(0), *_TStorageY = cvCreateMemStorage(0);
while (_TChessRecognition->_EnableThread != false) {
// 이미지를 받아옴. main루프와 동기를 맞추기 위해서 critical section 사용.
_TChessRecognition->_ChessBoardDetectionInternalImageProtectMutex.lock();
//EnterCriticalSection(&(_TChessRecognition->cs));
cvConvert(_TChessRecognition->_ChessBoardDetectionInternalImage, iplTemp);
//LeaveCriticalSection(&_TChessRecognition->cs);
_TChessRecognition->_ChessBoardDetectionInternalImageProtectMutex.unlock();
// 각 X축 Y축 라인을 검출해 내기 위해서 filter 적용.
cvFilter2D(iplTemp, iplDoGx, &_TDoGX); // 라인만 축출해내고.
cvFilter2D(iplTemp, iplDoGy, _TDoGY);
cvAbs(iplDoGx, iplDoGx);
cvAbs(iplDoGy, iplDoGy);
// 이미지 내부에서 최댓값과 최소값을 구하여 정규화.
cvMinMaxLoc(iplDoGx, &_TMinValX, &_TMaxValX);
cvMinMaxLoc(iplDoGy, &_TMinValY, &_TMaxValY);
cvMinMaxLoc(iplTemp, &_TMinValT, &_TMaxValT);
cvScale(iplDoGx, iplDoGx, 2.0 / _TMaxValX); // 정규화.
cvScale(iplDoGy, iplDoGy, 2.0 / _TMaxValY);
cvScale(iplTemp, iplTemp, 2.0 / _TMaxValT);
cvCopy(iplDoGy, iplDoGyClone);
cvCopy(iplDoGx, iplDoGxClone);
// NMS진행후 추가 작업
_TChessRecognition->_HoughLineBased->NonMaximumSuppression(iplDoGx, iplDoGyClone, _TKernel);
_TChessRecognition->_HoughLineBased->NonMaximumSuppression(iplDoGy, iplDoGxClone, _TKernel);
cvConvert(iplDoGx, iplEdgeY); // IPL_DEPTH_8U로 다시 재변환.
cvConvert(iplDoGy, iplEdgeX);
double rho = 1.0; // distance resolution in pixel-related units.
double theta = 1.0; // angle resolution measured in radians.
int threshold = 20;
if (threshold == 0)
threshold = 1;
// detecting 해낸 edge에서 hough line 검출.
_TLineX = cvHoughLines2(iplEdgeX, _TStorageX, CV_HOUGH_STANDARD, 1.0 * rho, CV_PI / 180 * theta, threshold, 0, 0);
_TLineY = cvHoughLines2(iplEdgeY, _TStorageY, CV_HOUGH_STANDARD, 1.0 * rho, CV_PI / 180 * theta, threshold, 0, 0);
// cvSeq를 vector로 바꾸기 위한 연산.
_TChessRecognition->_Vec_ProtectionMutex.lock();
_TChessRecognition->_HoughLineBased->CastSequence(_TLineX, _TLineY);
_TChessRecognition->_Vec_ProtectionMutex.unlock();
Sleep(2);
}
// mat 할당 해제.
cvReleaseMat(&_TDoGY);
// 내부 연산에 사용된 이미지 할당 해제.
cvReleaseImage(&iplTemp);
cvReleaseImage(&iplDoGx);
cvReleaseImage(&iplDoGy);
cvReleaseImage(&iplDoGyClone);
cvReleaseImage(&iplDoGxClone);
cvReleaseImage(&iplEdgeX);
cvReleaseImage(&iplEdgeY);
// houghline2에 사용된 opencv 메모리 할당 해제.
cvReleaseMemStorage(&_TStorageX);
cvReleaseMemStorage(&_TStorageY);
delete _TChessRecognition->_HoughLineBased;
#if defined(WINDOWS_SYS)
_endthread();
#elif defined(POSIX_SYS)
#endif
_TChessRecognition->_EndThread = true;
return 0;
}
int ImageReranker::cvFindHomography( const CvMat* objectPoints, const CvMat* imagePoints,
CvMat* __H, int method, double ransacReprojThreshold,
CvMat* mask )
{
const double confidence = 0.995;
const int maxIters = 400;
const double defaultRANSACReprojThreshold = 3;
bool result = false;
Ptr<CvMat> m, M, tempMask;
double H[9];
CvMat matH = cvMat( 3, 3, CV_64FC1, H );
int count;
CV_Assert( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) );
count = MAX(imagePoints->cols, imagePoints->rows);
CV_Assert( count >= 4 );
if( ransacReprojThreshold <= 0 )
ransacReprojThreshold = defaultRANSACReprojThreshold;
m = cvCreateMat( 1, count, CV_64FC2 );
cvConvertPointsHomogeneous( imagePoints, m );
M = cvCreateMat( 1, count, CV_64FC2 );
cvConvertPointsHomogeneous( objectPoints, M );
if( mask )
{
CV_Assert( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
(mask->rows == 1 || mask->cols == 1) &&
mask->rows*mask->cols == count );
}
if( mask || count > 4 )
tempMask = cvCreateMat( 1, count, CV_8U );
if( !tempMask.empty() )
cvSet( tempMask, cvScalarAll(1.) );
CvHomographyEstimator estimator(4);
if( count == 4 )
method = 0;
assert(method == CV_RANSAC);
result = estimator.runRANSAC( M, m, &matH, tempMask, ransacReprojThreshold, confidence, maxIters);
if( result && count > 4 )
{
icvCompressPoints( (CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, count );
count = icvCompressPoints( (CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, count );
M->cols = m->cols = count;
if( method == CV_RANSAC )
estimator.runKernel( M, m, &matH );
estimator.refine( M, m, &matH, 10 );
}
if( result )
cvConvert( &matH, __H );
if( mask && tempMask )
{
if( CV_ARE_SIZES_EQ(mask, tempMask) )
cvCopy( tempMask, mask );
else
cvTranspose( tempMask, mask );
}
return (int)result;
}
void CvEM::init_em( const CvVectors& train_data )
{
CvMat *w = 0, *u = 0, *tcov = 0;
CV_FUNCNAME( "CvEM::init_em" );
__BEGIN__;
double maxval = 0;
int i, force_symm_plus = 0;
int nclusters = params.nclusters, nsamples = train_data.count, dims = train_data.dims;
if( params.start_step == START_AUTO_STEP || nclusters == 1 || nclusters == nsamples )
init_auto( train_data );
else if( params.start_step == START_M_STEP )
{
for( i = 0; i < nsamples; i++ )
{
CvMat prob;
cvGetRow( params.probs, &prob, i );
cvMaxS( &prob, 0., &prob );
cvMinMaxLoc( &prob, 0, &maxval );
if( maxval < FLT_EPSILON )
cvSet( &prob, cvScalar(1./nclusters) );
else
cvNormalize( &prob, &prob, 1., 0, CV_L1 );
}
EXIT; // do not preprocess covariation matrices,
// as in this case they are initialized at the first iteration of EM
}
else
{
CV_ASSERT( params.start_step == START_E_STEP && params.means );
if( params.weights && params.covs )
{
cvConvert( params.means, means );
cvReshape( weights, weights, 1, params.weights->rows );
cvConvert( params.weights, weights );
cvReshape( weights, weights, 1, 1 );
cvMaxS( weights, 0., weights );
cvMinMaxLoc( weights, 0, &maxval );
if( maxval < FLT_EPSILON )
cvSet( weights, cvScalar(1./nclusters) );
cvNormalize( weights, weights, 1., 0, CV_L1 );
for( i = 0; i < nclusters; i++ )
CV_CALL( cvConvert( params.covs[i], covs[i] ));
force_symm_plus = 1;
}
else
init_auto( train_data );
}
CV_CALL( tcov = cvCreateMat( dims, dims, CV_64FC1 ));
CV_CALL( w = cvCreateMat( dims, dims, CV_64FC1 ));
if( params.cov_mat_type == COV_MAT_GENERIC )
CV_CALL( u = cvCreateMat( dims, dims, CV_64FC1 ));
for( i = 0; i < nclusters; i++ )
{
if( force_symm_plus )
{
cvTranspose( covs[i], tcov );
cvAddWeighted( covs[i], 0.5, tcov, 0.5, 0, tcov );
}
else
cvCopy( covs[i], tcov );
cvSVD( tcov, w, u, 0, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
if( params.cov_mat_type == COV_MAT_SPHERICAL )
cvSetIdentity( covs[i], cvScalar(cvTrace(w).val[0]/dims) );
else if( params.cov_mat_type == COV_MAT_DIAGONAL )
cvCopy( w, covs[i] );
else
{
// generic case: covs[i] = (u')'*max(w,0)*u'
cvGEMM( u, w, 1, 0, 0, tcov, CV_GEMM_A_T );
cvGEMM( tcov, u, 1, 0, 0, covs[i], 0 );
}
}
__END__;
cvReleaseMat( &w );
cvReleaseMat( &u );
cvReleaseMat( &tcov );
}
void icvConvertPointsHomogenious( const CvMat* src, CvMat* dst )
{
CvMat* temp = 0;
CvMat* denom = 0;
CV_FUNCNAME( "cvConvertPointsHomogenious" );
__BEGIN__;
int i, s_count, s_dims, d_count, d_dims;
CvMat _src, _dst, _ones;
CvMat* ones = 0;
if( !CV_IS_MAT(src) )
CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg,
"The input parameter is not a valid matrix" );
if( !CV_IS_MAT(dst) )
CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg,
"The output parameter is not a valid matrix" );
if( src == dst || src->data.ptr == dst->data.ptr )
{
if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) ||
!CV_ARE_SIZES_EQ(src,dst)) )
CV_ERROR( CV_StsBadArg, "Invalid inplace operation" );
EXIT;
}
if( src->rows > src->cols )
{
if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )
CV_ERROR( CV_StsBadSize,
"Either the number of channels or columns or "
"rows must be =1" );
s_dims = CV_MAT_CN(src->type)*src->cols;
s_count = src->rows;
}
else
{
if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )
CV_ERROR( CV_StsBadSize,
"Either the number of channels or columns or "
"rows must be =1" );
s_dims = CV_MAT_CN(src->type)*src->rows;
s_count = src->cols;
}
if( src->rows == 1 || src->cols == 1 )
src = cvReshape( src, &_src, 1, s_count );
if( dst->rows > dst->cols )
{
if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
CV_ERROR( CV_StsBadSize,
"Either the number of channels or columns or "
"rows in the input matrix must be =1" );
d_dims = CV_MAT_CN(dst->type)*dst->cols;
d_count = dst->rows;
}
else
{
if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
CV_ERROR( CV_StsBadSize,
"Either the number of channels or columns or "
"rows in the output matrix must be =1" );
d_dims = CV_MAT_CN(dst->type)*dst->rows;
d_count = dst->cols;
}
if( dst->rows == 1 || dst->cols == 1 )
dst = cvReshape( dst, &_dst, 1, d_count );
if( s_count != d_count )
CV_ERROR( CV_StsUnmatchedSizes, "Both matrices must have the "
"same number of points" );
if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )
CV_ERROR( CV_StsUnsupportedFormat,
"Both matrices must be floating-point "
"(single or double precision)" );
if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )
CV_ERROR( CV_StsOutOfRange,
"Both input and output point dimensionality "
"must be 2, 3 or 4" );
if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )
CV_ERROR( CV_StsUnmatchedSizes,
"The dimensionalities of input and output "
"point sets differ too much" );
if( s_dims == d_dims - 1 )
{
if( d_count == dst->rows )
{
ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count ));
dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count ));
}
else
{
ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 ));
dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims ));
}
}
if( s_dims <= d_dims )
{
if( src->rows == dst->rows && src->cols == dst->cols )
{
if( CV_ARE_TYPES_EQ( src, dst ) )
cvCopy( src, dst );
else
cvConvert( src, dst );
}
else
{
if( !CV_ARE_TYPES_EQ( src, dst ))
{
CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
cvConvert( src, temp );
src = temp;
}
cvTranspose( src, dst );
}
if( ones )
cvSet( ones, cvRealScalar(1.) );
}
else
{
int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size;
if( !CV_ARE_TYPES_EQ( src, dst ))
{
CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
cvConvert( src, temp );
src = temp;
}
elem_size = CV_ELEM_SIZE(src->type);
if( s_count == src->cols )
s_plane_stride = src->step / elem_size, s_stride = 1;
else
s_stride = src->step / elem_size, s_plane_stride = 1;
if( d_count == dst->cols )
d_plane_stride = dst->step / elem_size, d_stride = 1;
else
d_stride = dst->step / elem_size, d_plane_stride = 1;
CV_CALL( denom = cvCreateMat( 1, d_count, dst->type ));
if( CV_MAT_DEPTH(dst->type) == CV_32F )
{
const float* xs = src->data.fl;
const float* ys = xs + s_plane_stride;
const float* zs = 0;
const float* ws = xs + (s_dims - 1)*s_plane_stride;
float* iw = denom->data.fl;
float* xd = dst->data.fl;
float* yd = xd + d_plane_stride;
float* zd = 0;
if( d_dims == 3 )
{
zs = ys + s_plane_stride;
zd = yd + d_plane_stride;
}
for( i = 0; i < d_count; i++, ws += s_stride )
{
float t = *ws;
iw[i] = t ? t : 1.f;
}
cvDiv( 0, denom, denom );
if( d_dims == 3 )
for( i = 0; i < d_count; i++ )
{
float w = iw[i];
float x = *xs * w, y = *ys * w, z = *zs * w;
xs += s_stride; ys += s_stride; zs += s_stride;
*xd = x; *yd = y; *zd = z;
xd += d_stride; yd += d_stride; zd += d_stride;
}
else
for( i = 0; i < d_count; i++ )
{
float w = iw[i];
float x = *xs * w, y = *ys * w;
xs += s_stride; ys += s_stride;
*xd = x; *yd = y;
xd += d_stride; yd += d_stride;
}
}
else
{
const double* xs = src->data.db;
const double* ys = xs + s_plane_stride;
const double* zs = 0;
const double* ws = xs + (s_dims - 1)*s_plane_stride;
double* iw = denom->data.db;
double* xd = dst->data.db;
double* yd = xd + d_plane_stride;
double* zd = 0;
if( d_dims == 3 )
{
zs = ys + s_plane_stride;
zd = yd + d_plane_stride;
}
for( i = 0; i < d_count; i++, ws += s_stride )
{
double t = *ws;
iw[i] = t ? t : 1.;
}
cvDiv( 0, denom, denom );
if( d_dims == 3 )
for( i = 0; i < d_count; i++ )
{
double w = iw[i];
double x = *xs * w, y = *ys * w, z = *zs * w;
xs += s_stride; ys += s_stride; zs += s_stride;
*xd = x; *yd = y; *zd = z;
xd += d_stride; yd += d_stride; zd += d_stride;
}
else
for( i = 0; i < d_count; i++ )
{
double w = iw[i];
double x = *xs * w, y = *ys * w;
xs += s_stride; ys += s_stride;
*xd = x; *yd = y;
xd += d_stride; yd += d_stride;
}
}
}
__END__;
cvReleaseMat( &denom );
cvReleaseMat( &temp );
}
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);
}
void TargetObject::updateJPDAstate(CvMat* trans_mat){
/// set the transition matrix
cvCopy(trans_mat, kalman->transition_matrix,0);
///printMatrix(kalman->transition_matrix, "transition matrix");
//cvCopy(control_mat, kalman->control_matrix,0);
/// as in Shashtry paper
//std::cout << " shastry ok 1 " << std::endl;
/// compute the event probs total sum
float allProbSum = 0;
//std::cout << "Nr of events = " << event_probs->size() << std::endl;
for (int i = 0; i < event_probs->size(); i ++){
//std::cout << "event Nr = " << i << " prob = " << event_probs->at(i) << std::endl;
allProbSum = allProbSum + event_probs->at(i);
}
if (allProbSum > 0){
//std::cout << " ok 2 " << std::endl;combined_innov
///printMatrix(kalman->measurement_noise_cov, "mes-noice-cov");
///printMatrix(kalman->error_cov_pre, "error-cov-pre");
/// compute the covariance of combined innovation
//std::cout << "targobj pos 1" << std::endl;
//printMatrix(kalman->error_cov_pre, "error cov pre");
cvMatMul(kalman->measurement_matrix,kalman->error_cov_pre, tempCov1);
//std::cout << "targobj pos 2" << std::endl;
cvTranspose(kalman->measurement_matrix, tempCov2);
//std::cout << "targobj pos 3" << std::endl;
cvMatMul(tempCov1, tempCov2, tempCov3);
//std::cout << "targobj pos 4" << std::endl;
cvAdd(kalman->measurement_noise_cov, tempCov3, combInnovCov);
//std::cout << "targobj pos 5" << std::endl;
///printMatrix(combInnovCov, "comb innov cov");
/// compute the combined Kalman Gain
cvTranspose(kalman->measurement_matrix, tempCov2);
///printMatrix(tempCov2, "tempCov2 1");
///printMatrix(kalman->error_cov_post, "kalman->error_cov_post");
//std::cout << "targobj pos 5.5" << std::endl;
cvMatMul(kalman->error_cov_post, tempCov2, tempCov2);
///printMatrix(tempCov2, "tempCov2 2");
//std::cout << "targobj pos 6" << std::endl;
cvInvert(combInnovCov, tempCov3, CV_LU);
///printMatrix(tempCov3, "tempCov3");
//std::cout << "targobj pos 7" << std::endl;
cvMatMul(tempCov2, tempCov3, kalman->gain);
//std::cout << "targobj pos 8" << std::endl;
///printMatrix(kalman->gain, "comb kalman gain");
///---------------- upate the state estimate-------------------------
///printMatrix(combined_innov, "targ: update state: combined_innov");
cvMatMulAdd( kalman->gain, combined_innov, 0, tempo2 );
//std::cout << " ok 4.1 " << std::endl;
cvAdd( tempo2, kalman->state_post, tempo2, 0 );
//std::cout << " ok 4.2 " << std::endl;
cvCopy(tempo2, kalman->state_post, 0);
///printMatrix(kalman->state_post, "kalman->state_post");
//std::cout << " ok 5 " << std::endl;
/// --------------- compute the state estimation covariance ---------
cvTranspose(kalman->gain,tempCov1 );
//std::cout << " ok 5.1 " << std::endl;
cvMatMul(kalman->gain, combInnovCov, tempCov2);
//std::cout << " ok 5.2 " << std::endl;
cvMatMul(tempCov2, tempCov1, tempCov4);
//std::cout << " ok 5.3 " << std::endl;
cvSet(tempCov5,cvScalarAll(0),0);
//std::cout << " ok 5.4 " << std::endl;
CvScalar prob_scal = cvScalarAll(allProbSum);
//std::cout << " targobj: ok 5.5 " << std::endl;
cvScaleAdd( tempCov4, prob_scal, tempCov5, tempCov5);
//std::cout << " targobj: ok 5.6 " << std::endl;
cvSub(kalman->error_cov_post, tempCov5, tempCov4, 0);
// so until now tempCov4 has the result
//std::cout << " ok 6 " << std::endl;
// now compute the middle bracket
cvSet(tempCov2,cvScalarAll(0),0);
cvSet(tempCov3,cvScalarAll(0),0); // just temp
cvSet(tempCov6,cvScalarAll(0),0);
//std::cout << " ok 6.1..events_prob size = " << event_probs->size() << std::endl;
for (int i = 0; i < event_probs->size(); i ++){
//std::cout << " ok 6.n2 " << std::endl;
CvScalar tmpSca = cvScalarAll(event_probs->at(i));
//std::cout << " ok 6.n3 " << std::endl;
cvTranspose(all_innovs->at(i), tempoTrans1);
//std::cout << " ok 6.n4 " << std::endl;
cvMatMul(all_innovs->at(i), tempoTrans1, tempCov3);
//std::cout << " ok 6.n5 " << std::endl;
cvScaleAdd( tempCov3, tmpSca, tempCov6, tempCov6);
//std::cout << " ok 6.n6 " << std::endl;
//cvCopy(tempCov1, tempCov2);
//std::cout << " ok 6.n7 " << std::endl;
}
/// tempCov6 has the result (which is the sum in the middle bracket)
//std::cout << " ok 6.2 " << std::endl;
cvTranspose(combined_innov, tempoTrans1);
//std::cout << " ok 6.3 " << std::endl;
cvMatMul(combined_innov, tempoTrans1, tempCov3);
//std::cout << " ok 6.4 " << std::endl;
cvSub(tempCov6, tempCov3, tempCov6);
//std::cout << " ok 7 " << std::endl;
// the last sum in the equation in paper
cvTranspose(kalman->gain, tempCov1);
cvMatMul(kalman->gain, tempCov6, tempCov2);
cvMatMul(tempCov2, tempCov1, tempCov5);
//std::cout << " ok 8 " << std::endl;
// now add with tempCov4
cvAdd( tempCov5, tempCov4, kalman->error_cov_post, 0 );
//std::cout << " ok 9 " << std::endl;
//printMatrix(kalman->gain, "gain");
//printMatrix(combInnovCov, "combInnovCov");
//printMatrix(kalman->error_cov_post, "estimate cov ");
//printMatrix(kalman->state_post, "kalman->state_post");
/// set x,y,z
CvMat* Me = kalman->state_post;
int stepMe = Me->step/sizeof(float);
float *dataMe = Me->data.fl;
/// save old state
xOld = x;
yOld = y;
zOld = z;
vxOld = vx;
vyOld = vy;
vzOld = vz;
hueOld = hue;
weightOld = weight;
x = (dataMe+0*stepMe)[0];
y = (dataMe+1*stepMe)[0];
z = (dataMe+2*stepMe)[0];
vx = (dataMe+3*stepMe)[0];
vy = (dataMe+4*stepMe)[0];
vz = (dataMe+5*stepMe)[0];
hue = (dataMe+9*stepMe)[0];
weight = (dataMe+10*stepMe)[0];
if (isnan(x)){
x = xOld;
}
if (isnan(y)){
y = yOld;
}
if (isnan(z)){
z = zOld;
}
if (isnan(vx)){
vx = vxOld;
}
if (isnan(vy)){
vy = vyOld;
}
if (isnan(vz)){
vz = vzOld;
}
if (isnan(hue)){
std::cout << "hue is nan in updateJPDAstate" << std::endl;
hue = hueOld;
}
if (isnan(weight)){
std::cout << "weight is nan in updateJPDAstate" << std::endl;
weight = weightOld;
}
/// ------------------------------------------------------------------
timer.setActiveTime();
/// error_cov_post is increased manually (24 July 2007 ZM)
//cvAdd( kalman->error_cov_post, extra_error , kalman->error_cov_post);
//std::cout << "x,y,vx,vy = " << x << ", " << y << ", " << vx << ", " << vy << std::endl;
}
}
