CV_IMPL const CvMat*
cvKalmanPredict( CvKalman* kalman, const CvMat* control )
{
if( !kalman )
CV_Error( CV_StsNullPtr, "" );
/* update the state */
/* x'(k) = A*x(k) */
cvMatMulAdd( kalman->transition_matrix, kalman->state_post, 0, kalman->state_pre );
if( control && kalman->CP > 0 )
/* x'(k) = x'(k) + B*u(k) */
cvMatMulAdd( kalman->control_matrix, control, kalman->state_pre, kalman->state_pre );
/* update error covariance matrices */
/* temp1 = A*P(k) */
cvMatMulAdd( kalman->transition_matrix, kalman->error_cov_post, 0, kalman->temp1 );
/* P'(k) = temp1*At + Q */
cvGEMM( kalman->temp1, kalman->transition_matrix, 1, kalman->process_noise_cov, 1,
kalman->error_cov_pre, CV_GEMM_B_T );
/* handle the case when there will be measurement before the next predict */
cvCopy(kalman->state_pre, kalman->state_post);
return kalman->state_pre;
}
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;
}
virtual CvBlob* Process(CvBlob* pBlob, IplImage* /*pImg*/, IplImage* /*pImgFG*/ = NULL)
{
CvBlob* pBlobRes = &m_Blob;
float Z[4];
CvMat Zmat = cvMat(4,1,CV_32F,Z);
m_Blob = pBlob[0];
if(m_Frame < 2)
{/* first call */
m_pKalman->state_post->data.fl[0+4] = CV_BLOB_X(pBlob)-m_pKalman->state_post->data.fl[0];
m_pKalman->state_post->data.fl[1+4] = CV_BLOB_Y(pBlob)-m_pKalman->state_post->data.fl[1];
if(m_pKalman->DP>6)
{
m_pKalman->state_post->data.fl[2+4] = CV_BLOB_WX(pBlob)-m_pKalman->state_post->data.fl[2];
m_pKalman->state_post->data.fl[3+4] = CV_BLOB_WY(pBlob)-m_pKalman->state_post->data.fl[3];
}
m_pKalman->state_post->data.fl[0] = CV_BLOB_X(pBlob);
m_pKalman->state_post->data.fl[1] = CV_BLOB_Y(pBlob);
m_pKalman->state_post->data.fl[2] = CV_BLOB_WX(pBlob);
m_pKalman->state_post->data.fl[3] = CV_BLOB_WY(pBlob);
memcpy(m_pKalman->state_pre->data.fl,m_pKalman->state_post->data.fl,sizeof(float)*STATE_NUM);
}
else
{/* another call */
Z[0] = CV_BLOB_X(pBlob);
Z[1] = CV_BLOB_Y(pBlob);
Z[2] = CV_BLOB_WX(pBlob);
Z[3] = CV_BLOB_WY(pBlob);
cvKalmanCorrect(m_pKalman,&Zmat);
cvKalmanPredict(m_pKalman,0);
cvMatMulAdd(m_pKalman->measurement_matrix, m_pKalman->state_pre, NULL, &Zmat);
CV_BLOB_X(pBlobRes) = Z[0];
CV_BLOB_Y(pBlobRes) = Z[1];
CV_BLOB_WX(pBlobRes) = Z[2];
CV_BLOB_WY(pBlobRes) = Z[3];
}
m_Frame++;
return pBlobRes;
}
CvBlob* CvBlobTrackPostProcKalman::Process(CvBlob* pBlob)
{
CvBlob* pBlobRes = &m_Blob;
float Z[4];
CvMat Zmat = cvMat(4,1,CV_32F,Z);
m_Blob = pBlob[0];
if(m_Frame < 2)
{ /* First call: */
m_pKalman->state_post->data.fl[0+4] = CV_BLOB_X(pBlob)-m_pKalman->state_post->data.fl[0];
m_pKalman->state_post->data.fl[1+4] = CV_BLOB_Y(pBlob)-m_pKalman->state_post->data.fl[1];
if(m_pKalman->DP>6)
{
m_pKalman->state_post->data.fl[2+4] = CV_BLOB_WX(pBlob)-m_pKalman->state_post->data.fl[2];
m_pKalman->state_post->data.fl[3+4] = CV_BLOB_WY(pBlob)-m_pKalman->state_post->data.fl[3];
}
m_pKalman->state_post->data.fl[0] = CV_BLOB_X(pBlob);
m_pKalman->state_post->data.fl[1] = CV_BLOB_Y(pBlob);
m_pKalman->state_post->data.fl[2] = CV_BLOB_WX(pBlob);
m_pKalman->state_post->data.fl[3] = CV_BLOB_WY(pBlob);
}
else
{ /* Nonfirst call: */
cvKalmanPredict(m_pKalman,0);
Z[0] = CV_BLOB_X(pBlob);
Z[1] = CV_BLOB_Y(pBlob);
Z[2] = CV_BLOB_WX(pBlob);
Z[3] = CV_BLOB_WY(pBlob);
cvKalmanCorrect(m_pKalman,&Zmat);
cvMatMulAdd(m_pKalman->measurement_matrix, m_pKalman->state_post, NULL, &Zmat);
CV_BLOB_X(pBlobRes) = Z[0];
CV_BLOB_Y(pBlobRes) = Z[1];
// CV_BLOB_WX(pBlobRes) = Z[2];
// CV_BLOB_WY(pBlobRes) = Z[3];
}
m_Frame++;
return pBlobRes;
}
//=========================================
CvRect camKalTrack(IplImage* frame, camshift_kalman_tracker& camKalTrk) {
//=========================================
if (!frame)
printf("Input frame empty!\n");
cvCopy(frame, camKalTrk.image, 0);
cvCvtColor(camKalTrk.image, camKalTrk.hsv, CV_BGR2HSV); // BGR to HSV
if (camKalTrk.trackObject) {
int _vmin = vmin, _vmax = vmax;
cvInRangeS(camKalTrk.hsv, cvScalar(0, smin, MIN(_vmin,_vmax), 0), cvScalar(180, 256, MAX(_vmin,_vmax), 0), camKalTrk.mask); // MASK
cvSplit(camKalTrk.hsv, camKalTrk.hue, 0, 0, 0); // HUE
if (camKalTrk.trackObject < 0) {
float max_val = 0.f;
boundaryCheck(camKalTrk.originBox, frame->width, frame->height);
cvSetImageROI(camKalTrk.hue, camKalTrk.originBox); // for ROI
cvSetImageROI(camKalTrk.mask, camKalTrk.originBox); // for camKalTrk.mask
cvCalcHist(&camKalTrk.hue, camKalTrk.hist, 0, camKalTrk.mask); //
cvGetMinMaxHistValue(camKalTrk.hist, 0, &max_val, 0, 0);
cvConvertScale(camKalTrk.hist->bins, camKalTrk.hist->bins, max_val ? 255. / max_val : 0., 0); // bin [0,255]
cvResetImageROI(camKalTrk.hue); // remove ROI
cvResetImageROI(camKalTrk.mask);
camKalTrk.trackWindow = camKalTrk.originBox;
camKalTrk.trackObject = 1;
camKalTrk.lastpoint = camKalTrk.predictpoint = cvPoint(camKalTrk.trackWindow.x + camKalTrk.trackWindow.width / 2,
camKalTrk.trackWindow.y + camKalTrk.trackWindow.height / 2);
getCurrState(camKalTrk.kalman, camKalTrk.lastpoint, camKalTrk.predictpoint);//input curent state
}
//(x,y,vx,vy),
camKalTrk.prediction = cvKalmanPredict(camKalTrk.kalman, 0);//predicton=kalman->state_post
camKalTrk.predictpoint = cvPoint(cvRound(camKalTrk.prediction->data.fl[0]), cvRound(camKalTrk.prediction->data.fl[1]));
camKalTrk.trackWindow = cvRect(camKalTrk.predictpoint.x - camKalTrk.trackWindow.width / 2, camKalTrk.predictpoint.y
- camKalTrk.trackWindow.height / 2, camKalTrk.trackWindow.width, camKalTrk.trackWindow.height);
camKalTrk.trackWindow = checkRectBoundary(cvRect(0, 0, frame->width, frame->height), camKalTrk.trackWindow);
camKalTrk.searchWindow = cvRect(camKalTrk.trackWindow.x - region, camKalTrk.trackWindow.y - region, camKalTrk.trackWindow.width + 2
* region, camKalTrk.trackWindow.height + 2 * region);
camKalTrk.searchWindow = checkRectBoundary(cvRect(0, 0, frame->width, frame->height), camKalTrk.searchWindow);
cvSetImageROI(camKalTrk.hue, camKalTrk.searchWindow);
cvSetImageROI(camKalTrk.mask, camKalTrk.searchWindow);
cvSetImageROI(camKalTrk.backproject, camKalTrk.searchWindow);
cvCalcBackProject( &camKalTrk.hue, camKalTrk.backproject, camKalTrk.hist ); // back project
cvAnd(camKalTrk.backproject, camKalTrk.mask, camKalTrk.backproject, 0);
camKalTrk.trackWindow = cvRect(region, region, camKalTrk.trackWindow.width, camKalTrk.trackWindow.height);
if (camKalTrk.trackWindow.height > 5 && camKalTrk.trackWindow.width > 5) {
// calling CAMSHIFT
cvCamShift(camKalTrk.backproject, camKalTrk.trackWindow, cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1),
&camKalTrk.trackComp, &camKalTrk.trackBox);
/*cvMeanShift( camKalTrk.backproject, camKalTrk.trackWindow,
cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
&camKalTrk.trackComp);*/
}
else {
camKalTrk.trackComp.rect.x = 0;
camKalTrk.trackComp.rect.y = 0;
camKalTrk.trackComp.rect.width = 0;
camKalTrk.trackComp.rect.height = 0;
}
cvResetImageROI(camKalTrk.hue);
cvResetImageROI(camKalTrk.mask);
cvResetImageROI(camKalTrk.backproject);
camKalTrk.trackWindow = camKalTrk.trackComp.rect;
camKalTrk.trackWindow = cvRect(camKalTrk.trackWindow.x + camKalTrk.searchWindow.x, camKalTrk.trackWindow.y
+ camKalTrk.searchWindow.y, camKalTrk.trackWindow.width, camKalTrk.trackWindow.height);
camKalTrk.measurepoint = cvPoint(camKalTrk.trackWindow.x + camKalTrk.trackWindow.width / 2, camKalTrk.trackWindow.y
+ camKalTrk.trackWindow.height / 2);
camKalTrk.realposition->data.fl[0] = camKalTrk.measurepoint.x;
camKalTrk.realposition->data.fl[1] = camKalTrk.measurepoint.y;
camKalTrk.realposition->data.fl[2] = camKalTrk.measurepoint.x - camKalTrk.lastpoint.x;
camKalTrk.realposition->data.fl[3] = camKalTrk.measurepoint.y - camKalTrk.lastpoint.y;
camKalTrk.lastpoint = camKalTrk.measurepoint;//keep the current real position
//measurement x,y
cvMatMulAdd( camKalTrk.kalman->measurement_matrix/*2x4*/, camKalTrk.realposition/*4x1*/,/*measurementstate*/0, camKalTrk.measurement );
cvKalmanCorrect(camKalTrk.kalman, camKalTrk.measurement);
cvRectangle(frame, cvPoint(camKalTrk.trackWindow.x, camKalTrk.trackWindow.y), cvPoint(camKalTrk.trackWindow.x
+ camKalTrk.trackWindow.width, camKalTrk.trackWindow.y + camKalTrk.trackWindow.height), CV_RGB(255,128,0), 4, 8, 0);
}
// set new selection if it exists
if (camKalTrk.selectObject && camKalTrk.selection.width > 0 && camKalTrk.selection.height > 0) {
cvSetImageROI(camKalTrk.image, camKalTrk.selection);
cvXorS(camKalTrk.image, cvScalarAll(255), camKalTrk.image, 0);
cvResetImageROI(camKalTrk.image);
}
return camKalTrk.trackWindow;
}
bool
cvFindExtrinsicCameraParams3( const CvMat* obj_points,
const CvMat* img_points, const CvMat* A,
const CvMat* dist_coeffs,
CvMat* r_vec, CvMat* t_vec )
{
bool fGood = true;
const int max_iter = 20;
CvMat *_M = 0, *_Mxy = 0, *_m = 0, *_mn = 0, *_L = 0, *_J = 0;
CV_FUNCNAME( "cvFindExtrinsicCameraParams3" );
__BEGIN__;
int i, j, count;
double a[9], k[4] = { 0, 0, 0, 0 }, R[9], ifx, ify, cx, cy;
double Mc[3] = {0, 0, 0}, MM[9], U[9], V[9], W[3];
double JtJ[6*6], JtErr[6], JtJW[6], JtJV[6*6], delta[6], param[6];
CvPoint3D64f* M = 0;
CvPoint2D64f *m = 0, *mn = 0;
CvMat _a = cvMat( 3, 3, CV_64F, a );
CvMat _R = cvMat( 3, 3, CV_64F, R );
CvMat _r = cvMat( 3, 1, CV_64F, param );
CvMat _t = cvMat( 3, 1, CV_64F, param + 3 );
CvMat _Mc = cvMat( 1, 3, CV_64F, Mc );
CvMat _MM = cvMat( 3, 3, CV_64F, MM );
CvMat _U = cvMat( 3, 3, CV_64F, U );
CvMat _V = cvMat( 3, 3, CV_64F, V );
CvMat _W = cvMat( 3, 1, CV_64F, W );
CvMat _JtJ = cvMat( 6, 6, CV_64F, JtJ );
CvMat _JtErr = cvMat( 6, 1, CV_64F, JtErr );
CvMat _JtJW = cvMat( 6, 1, CV_64F, JtJW );
CvMat _JtJV = cvMat( 6, 6, CV_64F, JtJV );
CvMat _delta = cvMat( 6, 1, CV_64F, delta );
CvMat _param = cvMat( 6, 1, CV_64F, param );
CvMat _dpdr, _dpdt;
if( !CV_IS_MAT(obj_points) || !CV_IS_MAT(img_points) ||
!CV_IS_MAT(A) || !CV_IS_MAT(r_vec) || !CV_IS_MAT(t_vec) )
CV_ERROR( CV_StsBadArg, "One of required arguments is not a valid matrix" );
count = MAX(obj_points->cols, obj_points->rows);
CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
CV_CALL( _Mxy = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( _mn = cvCreateMat( 1, count, CV_64FC2 ));
M = (CvPoint3D64f*)_M->data.db;
m = (CvPoint2D64f*)_m->data.db;
mn = (CvPoint2D64f*)_mn->data.db;
CV_CALL( cvConvertPointsHomogenious( obj_points, _M ));
CV_CALL( cvConvertPointsHomogenious( img_points, _m ));
CV_CALL( cvConvert( A, &_a ));
if( dist_coeffs )
{
CvMat _k;
if( !CV_IS_MAT(dist_coeffs) ||
CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
CV_MAT_DEPTH(dist_coeffs->type) != CV_32F ||
dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 )
CV_ERROR( CV_StsBadArg,
"Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
_k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(dist_coeffs->type)), k );
CV_CALL( cvConvert( dist_coeffs, &_k ));
}
if( CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F ||
r_vec->rows != 1 && r_vec->cols != 1 ||
r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3 )
CV_ERROR( CV_StsBadArg, "Rotation vector must be 1x3 or 3x1 floating-point vector" );
if( CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F ||
t_vec->rows != 1 && t_vec->cols != 1 ||
t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
CV_ERROR( CV_StsBadArg,
"Translation vector must be 1x3 or 3x1 floating-point vector" );
ifx = 1./a[0]; ify = 1./a[4];
cx = a[2]; cy = a[5];
// normalize image points
// (unapply the intrinsic matrix transformation and distortion)
for( i = 0; i < count; i++ )
{
double x = (m[i].x - cx)*ifx, y = (m[i].y - cy)*ify, x0 = x, y0 = y;
// compensate distortion iteratively
if( dist_coeffs )
for( j = 0; j < 5; j++ )
{
double r2 = x*x + y*y;
double icdist = 1./(1 + k[0]*r2 + k[1]*r2*r2);
double delta_x = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
double delta_y = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
x = (x0 - delta_x)*icdist;
y = (y0 - delta_y)*icdist;
}
mn[i].x = x; mn[i].y = y;
// calc mean(M)
Mc[0] += M[i].x;
Mc[1] += M[i].y;
Mc[2] += M[i].z;
}
Mc[0] /= count;
Mc[1] /= count;
Mc[2] /= count;
cvReshape( _M, _M, 1, count );
cvMulTransposed( _M, &_MM, 1, &_Mc );
cvSVD( &_MM, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );
// initialize extrinsic parameters
if( W[2]/W[1] < 1e-3 || count < 4 )
{
// a planar structure case (all M's lie in the same plane)
double tt[3], h[9], h1_norm, h2_norm;
CvMat* R_transform = &_V;
CvMat T_transform = cvMat( 3, 1, CV_64F, tt );
CvMat _H = cvMat( 3, 3, CV_64F, h );
CvMat _h1, _h2, _h3;
if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
cvSetIdentity( R_transform );
if( cvDet(R_transform) < 0 )
cvScale( R_transform, R_transform, -1 );
cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
for( i = 0; i < count; i++ )
{
const double* Rp = R_transform->data.db;
const double* Tp = T_transform.data.db;
const double* src = _M->data.db + i*3;
double* dst = _Mxy->data.db + i*2;
dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0];
dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1];
}
cvFindHomography( _Mxy, _mn, &_H );
cvGetCol( &_H, &_h1, 0 );
_h2 = _h1; _h2.data.db++;
_h3 = _h2; _h3.data.db++;
h1_norm = sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]);
h2_norm = sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]);
cvScale( &_h1, &_h1, 1./h1_norm );
cvScale( &_h2, &_h2, 1./h2_norm );
cvScale( &_h3, &_t, 2./(h1_norm + h2_norm));
cvCrossProduct( &_h1, &_h2, &_h3 );
cvRodrigues2( &_H, &_r );
cvRodrigues2( &_r, &_H );
cvMatMulAdd( &_H, &T_transform, &_t, &_t );
cvMatMul( &_H, R_transform, &_R );
cvRodrigues2( &_R, &_r );
}
else
{
// non-planar structure. Use DLT method
double* L;
double LL[12*12], LW[12], LV[12*12], sc;
CvMat _LL = cvMat( 12, 12, CV_64F, LL );
CvMat _LW = cvMat( 12, 1, CV_64F, LW );
CvMat _LV = cvMat( 12, 12, CV_64F, LV );
CvMat _RRt, _RR, _tt;
CV_CALL( _L = cvCreateMat( 2*count, 12, CV_64F ));
L = _L->data.db;
for( i = 0; i < count; i++, L += 24 )
{
double x = -mn[i].x, y = -mn[i].y;
L[0] = L[16] = M[i].x;
L[1] = L[17] = M[i].y;
L[2] = L[18] = M[i].z;
L[3] = L[19] = 1.;
L[4] = L[5] = L[6] = L[7] = 0.;
L[12] = L[13] = L[14] = L[15] = 0.;
L[8] = x*M[i].x;
L[9] = x*M[i].y;
L[10] = x*M[i].z;
L[11] = x;
L[20] = y*M[i].x;
L[21] = y*M[i].y;
L[22] = y*M[i].z;
L[23] = y;
}
cvMulTransposed( _L, &_LL, 1 );
cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
_RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
cvGetCols( &_RRt, &_RR, 0, 3 );
cvGetCol( &_RRt, &_tt, 3 );
if( cvDet(&_RR) < 0 )
cvScale( &_RRt, &_RRt, -1 );
sc = cvNorm(&_RR);
cvSVD( &_RR, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T );
cvScale( &_tt, &_t, cvNorm(&_R)/sc );
cvRodrigues2( &_R, &_r );
cvReleaseMat( &_L );
}
//
// Check if new r and t are good
//
if ( cvGetReal1D( r_vec, 0 ) ||
cvGetReal1D( r_vec, 1 ) ||
cvGetReal1D( r_vec, 2 ) ||
cvGetReal1D( t_vec, 0 ) ||
cvGetReal1D( t_vec, 1 ) ||
cvGetReal1D( t_vec, 2 ) )
{
//
// perfom this only when the old r and t exist.
//
CvMat * R_inv = cvCreateMat( 3, 3, CV_64FC1 );
CvMat * r_old = cvCreateMat( 3, 1, CV_64FC1 );
CvMat * R_old = cvCreateMat( 3, 3, CV_64FC1 );
CvMat * t_old = cvCreateMat( 3, 1, CV_64FC1 );
// get new center
cvInvert( &_R, R_inv );
double new_center[3];
CvMat newCenter = cvMat( 3, 1, CV_64FC1, new_center );
cvMatMul( R_inv, &_t, &newCenter );
cvScale( &newCenter, &newCenter, -1 );
// get old center
cvConvert( r_vec, r_old );
cvConvert( t_vec, t_old );
cvRodrigues2( r_old, R_old );
cvInvert( R_old, R_inv );
double old_center[3];
CvMat oldCenter = cvMat( 3, 1, CV_64FC1, old_center );
cvMatMul( R_inv, t_old, &oldCenter );
cvScale( &oldCenter, &oldCenter, -1 );
// get difference
double diff_center = 0;
for ( int i = 0; i < 3 ; i ++ )
diff_center += pow( new_center[i] - old_center[i], 2 );
diff_center = sqrt( diff_center );
if ( diff_center > 300 )
{
printf("diff_center = %.2f --> set initial r and t as same as the previous\n", diff_center);
cvConvert(r_vec, &_r);
cvConvert(t_vec, &_t);
fGood = false;
}
// else printf("diff_center = %.2f\n", diff_center );
cvReleaseMat( &R_inv );
cvReleaseMat( &r_old );
cvReleaseMat( &R_old );
cvReleaseMat( &t_old );
}
if ( fGood )
{
CV_CALL( _J = cvCreateMat( 2*count, 6, CV_64FC1 ));
cvGetCols( _J, &_dpdr, 0, 3 );
cvGetCols( _J, &_dpdt, 3, 6 );
// refine extrinsic parameters using iterative algorithm
for( i = 0; i < max_iter; i++ )
{
double n1, n2;
cvReshape( _mn, _mn, 2, 1 );
cvProjectPoints2( _M, &_r, &_t, &_a, dist_coeffs,
_mn, &_dpdr, &_dpdt, 0, 0, 0 );
cvSub( _m, _mn, _mn );
cvReshape( _mn, _mn, 1, 2*count );
cvMulTransposed( _J, &_JtJ, 1 );
cvGEMM( _J, _mn, 1, 0, 0, &_JtErr, CV_GEMM_A_T );
cvSVD( &_JtJ, &_JtJW, 0, &_JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
if( JtJW[5]/JtJW[0] < 1e-12 )
break;
cvSVBkSb( &_JtJW, &_JtJV, &_JtJV, &_JtErr,
&_delta, CV_SVD_U_T + CV_SVD_V_T );
cvAdd( &_delta, &_param, &_param );
n1 = cvNorm( &_delta );
n2 = cvNorm( &_param );
if( n1/n2 < 1e-10 )
break;
}
_r = cvMat( r_vec->rows, r_vec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), param );
_t = cvMat( t_vec->rows, t_vec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), param + 3 );
cvConvert( &_r, r_vec );
cvConvert( &_t, t_vec );
}
__END__;
cvReleaseMat( &_M );
cvReleaseMat( &_Mxy );
cvReleaseMat( &_m );
cvReleaseMat( &_mn );
cvReleaseMat( &_L );
cvReleaseMat( &_J );
return fGood;
}
// Render more than one image per frame -- mosaic
void mosaic(int index, IplImage** images, CvMat** matches, int* bestMatchedIndex,
IplImage* viewport, CvMat* initHomography)
{
int image_count = myScene->max_image;
int baseIndex = myScene->currentIndex;
int history[image_count];
int j;
CvMat *ident = create3DIdentity();
for (j = 0; j < image_count; j++) {
history[j] = -1;
}
CvMat* topHomography = copyMatrix(matches[index*image_count + baseIndex]);
double topConfidence = topConfidence = cvNorm(topHomography, ident, CV_L2, 0);
CvMat* currHomography = copyMatrix(matches[index*image_count + bestMatchedIndex[index]]);;
int currIndex = bestMatchedIndex[index];
while (currIndex != baseIndex) {
if (!matches[currIndex*image_count + baseIndex]) {
//printf("FIX ME!\n");
currIndex = -1;
break;
}
CvMat* tempHomography = cvCreateMat(3, 3, CV_64F);
cvMatMul(matches[currIndex*image_count + baseIndex],
currHomography, tempHomography);
double currConfidence = cvNorm(tempHomography, ident, CV_L2, 0);
if (currConfidence > topConfidence) {
break;
} else {
cvMatMulAdd(matches[currIndex*image_count + bestMatchedIndex[currIndex]],
currHomography, 0, currHomography);
currIndex = bestMatchedIndex[currIndex];
if (history[currIndex] == 1) {
currIndex = -1;
break;
}
else history[currIndex] = 1;
}
}
if (currIndex == baseIndex) {
if (DEBUG) {
printf("Index to Match: bestBaseImageIndex = %d\n", currIndex);
printf("Chosen homography: ");
printMatrix(currHomography);
}
// multiply initial homography
cvMatMulAdd(initHomography, currHomography, 0, currHomography);
cvWarpPerspective(images[index], viewport, currHomography, CV_INTER_LINEAR, cvScalarAll(0));
} else {
//if (DEBUG) {
printf("Index to Match: currIndex = %d\n", currIndex);
printf("Chosen homography: ");
printMatrix(topHomography);
//}
// multiply initial homography
cvMatMulAdd(initHomography, topHomography, 0, topHomography);
cvWarpPerspective(images[index], viewport, topHomography, CV_INTER_LINEAR, cvScalarAll(0));
}
if (DEBUG){
cvShowImage("Scene", viewport);
cvWaitKey(0);
}
cvReleaseMat(&topHomography);
cvReleaseMat(&currHomography);
cvReleaseMat(&ident);
}
static
void icvRandomQuad( int width, int height, double quad[4][2],
double maxxangle,
double maxyangle,
double maxzangle )
{
double distfactor = 3.0;
double distfactor2 = 1.0;
double halfw, halfh;
int i;
double rotVectData[3];
double vectData[3];
double rotMatData[9];
CvMat rotVect;
CvMat rotMat;
CvMat vect;
double d;
rotVect = cvMat( 3, 1, CV_64FC1, &rotVectData[0] );
rotMat = cvMat( 3, 3, CV_64FC1, &rotMatData[0] );
vect = cvMat( 3, 1, CV_64FC1, &vectData[0] );
rotVectData[0] = maxxangle * (2.0 * rand() / RAND_MAX - 1.0);
rotVectData[1] = ( maxyangle - fabs( rotVectData[0] ) )
* (2.0 * rand() / RAND_MAX - 1.0);
rotVectData[2] = maxzangle * (2.0 * rand() / RAND_MAX - 1.0);
d = (distfactor + distfactor2 * (2.0 * rand() / RAND_MAX - 1.0)) * width;
/*
rotVectData[0] = maxxangle;
rotVectData[1] = maxyangle;
rotVectData[2] = maxzangle;
d = distfactor * width;
*/
cvRodrigues2( &rotVect, &rotMat );
halfw = 0.5 * width;
halfh = 0.5 * height;
quad[0][0] = -halfw;
quad[0][1] = -halfh;
quad[1][0] = halfw;
quad[1][1] = -halfh;
quad[2][0] = halfw;
quad[2][1] = halfh;
quad[3][0] = -halfw;
quad[3][1] = halfh;
for( i = 0; i < 4; i++ )
{
rotVectData[0] = quad[i][0];
rotVectData[1] = quad[i][1];
rotVectData[2] = 0.0;
cvMatMulAdd( &rotMat, &rotVect, 0, &vect );
quad[i][0] = vectData[0] * d / (d + vectData[2]) + halfw;
quad[i][1] = vectData[1] * d / (d + vectData[2]) + halfh;
/*
quad[i][0] += halfw;
quad[i][1] += halfh;
*/
}
}
// Calculate cameraHomography -- go from current position to camera position
CvMat* modelViewMatrix(int baseIndex, struct pData* poses)
{
// Initial Homography
CvMat* initHomography = create3DIdentity();
// Find Forward and Up Vector
CvScalar forward = cvScalarAll(0);
createForwardVector(&forward, myScene->pose, poses[baseIndex]);
CvScalar up = poses[baseIndex].up;
//printf("forward vector: [%.2lf %.2lf %.2lf]\n", forward.val[0], forward.val[1], forward.val[2]);
//printf("up vector: [%.2lf %.2lf %.2lf]\n", up.val[0], up.val[1], up.val[2]);
// the z-axis
double forwardAngle = atan2(forward.val[1], forward.val[0]);
if (forwardAngle < 0)
forwardAngle += PI;
//printf("forwardAngle: %.2lf\n", forwardAngle * 180 / PI);
// the y-axis
double upAngleY = atan2(up.val[0], up.val[2]);
// if (upAngleY < 0)
// upAngleY += PI;
//printf("upAngleY: %.2lf\n", upAngleY * 180 / PI);
// the x-axis
double upAngleX = atan2(up.val[1], up.val[2]);
//if (upAngleX < 0)
// upAngleX += PI;
//printf("upAngleX: %.2lf\n", upAngleX * 180 / PI);
CvMat* rotateXHomography = cvCreateMat(3, 3, CV_64F);
makeXAxisRotation(rotateXHomography, upAngleX);
//printf("X: \n");
//printMatrix(rotateXHomography);
CvMat* rotateYHomography = cvCreateMat(3, 3, CV_64F);
makeYAxisRotation(rotateYHomography, upAngleY);
//printf("Y: \n");
//printMatrix(rotateYHomography);
CvMat* rotateZHomography = cvCreateMat(3, 3, CV_64F);
makeZAxisRotation(rotateZHomography, forwardAngle);
//printf("Z: \n");
//printMatrix(rotateZHomography);
// Apply all transformations
cvMatMulAdd(rotateXHomography, initHomography, 0, initHomography);
//cvMatMulAdd(rotateYHomography, initHomography, 0, initHomography);
//cvMatMulAdd(rotateZHomography, initHomography, 0, initHomography);
projectTransform(initHomography);
//printf("final: \n");
//printMatrix(initHomography);
cvReleaseMat(&rotateXHomography);
cvReleaseMat(&rotateYHomography);
cvReleaseMat(&rotateZHomography);
return initHomography;
}
int main(int argc, char **argv)
{
// Initialize, create Kalman Filter object, window, random number
// generator etc.
//
cvNamedWindow("Kalman", 1);
CvRandState rng;
cvRandInit(&rng, 0, 1, -1, CV_RAND_UNI);
IplImage *img = cvCreateImage(cvSize(500, 500), 8, 3);
CvKalman *kalman = cvCreateKalman(2, 1, 0);
// state is (phi, delta_phi) - angle and angular velocity
// Initialize with random guess.
//
CvMat *x_k = cvCreateMat(2, 1, CV_32FC1);
cvRandSetRange(&rng, 0, 0.1, 0);
rng.disttype = CV_RAND_NORMAL;
cvRand(&rng, x_k);
// process noise
//
CvMat *w_k = cvCreateMat(2, 1, CV_32FC1);
// measurements, only one parameter for angle
//
CvMat *z_k = cvCreateMat(1, 1, CV_32FC1);
cvZero(z_k);
// Transition matrix 'F' describes relationship between
// model parameters at step k and at step k+1 (this is
// the "dynamics" in our model.
//
const float F[] = { 1, 1, 0, 1 };
memcpy(kalman->transition_matrix->data.fl, F, sizeof(F));
// Initialize other Kalman filter parameters.
//
cvSetIdentity(kalman->measurement_matrix, cvRealScalar(1));
cvSetIdentity(kalman->process_noise_cov, cvRealScalar(1e-5));
cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(1e-1));
cvSetIdentity(kalman->error_cov_post, cvRealScalar(1));
// choose random initial state
//
cvRand(&rng, kalman->state_post);
while (1) {
// predict point position
const CvMat *y_k = cvKalmanPredict(kalman, 0);
// generate measurement (z_k)
//
cvRandSetRange(&rng,
0, sqrt(kalman->measurement_noise_cov->data.fl[0]), 0);
cvRand(&rng, z_k);
cvMatMulAdd(kalman->measurement_matrix, x_k, z_k, z_k);
// plot points (eg convert to planar co-ordinates and draw)
//
cvZero(img);
cvCircle(img, phi2xy(z_k), 4, CVX_YELLOW); // observed state
cvCircle(img, phi2xy(y_k), 4, CVX_WHITE, 2); // "predicted" state
cvCircle(img, phi2xy(x_k), 4, CVX_RED); // real state
cvShowImage("Kalman", img);
// adjust Kalman filter state
//
cvKalmanCorrect(kalman, z_k);
// Apply the transition matrix 'F' (eg, step time forward)
// and also apply the "process" noise w_k.
//
cvRandSetRange(&rng, 0, sqrt(kalman->process_noise_cov->data.fl[0]), 0);
cvRand(&rng, w_k);
cvMatMulAdd(kalman->transition_matrix, x_k, w_k, x_k);
// exit if user hits 'Esc'
if (cvWaitKey(100) == 27)
break;
}
return 0;
}
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;
}
}
int main(int argc, char** argv)
{
const float A[] = { 1, 1, 0, 1 };//状态转移矩阵
IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 );//创建显示所用的图像
CvKalman* kalman = cvCreateKalman( 2, 1, 0 );//创建cvKalman数据结构，状态向量为2维，观测向量为1维，无激励输入维
CvMat* state = cvCreateMat( 2, 1, CV_32FC1 ); //(phi, delta_phi) 定义了状态变量
CvMat* process_noise = cvCreateMat( 2, 1, CV_32FC1 );// 创建两行一列CV_32FC1的单通道浮点型矩阵
CvMat* measurement = cvCreateMat( 1, 1, CV_32FC1 ); //定义观测变量
CvRNG rng = cvRNG(-1);//初始化一个随机序列函数
char code = -1;
cvZero( measurement );//观测变量矩阵置零
cvNamedWindow( "Kalman", 1 );
for(;;)
{ //用均匀分布或者正态分布的随机数填充输出数组state
cvRandArr( &rng, state, CV_RAND_NORMAL, cvRealScalar(0), cvRealScalar(0.1) );//状态state
memcpy( kalman->transition_matrix->data.fl, A, sizeof(A));//初始化状态转移F矩阵
//cvSetIdentity()用法：把数组中除了行数与列数相等以外的所有元素的值都设置为0；行数与列数相等的元素的值都设置为1
//我们将(第一个前假象阶段的)后验状态初始化为一个随机值
cvSetIdentity( kalman->measurement_matrix, cvRealScalar(1) );//观测矩阵H
cvSetIdentity( kalman->process_noise_cov, cvRealScalar(1e-5) );//过程噪声Q
cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1e-1) );//观测噪声R
cvSetIdentity( kalman->error_cov_post, cvRealScalar(1));//后验误差协方差
cvRandArr( &rng, kalman->state_post, CV_RAND_NORMAL, cvRealScalar(0), cvRealScalar(0.1) );//校正状态
//在时机动态系统上开始预测
for(;;)
{
#define calc_point(angle) \
cvPoint( cvRound(img->width/2 + img->width/3*cos(angle)), \
cvRound(img->height/2 - img->width/3*sin(angle)))
float state_angle = state->data.fl[0];
CvPoint state_pt = calc_point(state_angle);
const CvMat* prediction = cvKalmanPredict( kalman, 0 );//计算下一个时间点的预期值，激励项输入为0
float predict_angle = prediction->data.fl[0];
CvPoint predict_pt = calc_point(predict_angle);
float measurement_angle;
CvPoint measurement_pt;
cvRandArr( &rng, measurement, CV_RAND_NORMAL, cvRealScalar(0),
cvRealScalar(sqrt(kalman->measurement_noise_cov->data.fl[0])) );
/* generate measurement */
cvMatMulAdd( kalman->measurement_matrix, state, measurement, measurement );
//cvMatMulAdd(src1,src2,src3,dst)就是实现dist=src1*src2+src3;
measurement_angle = measurement->data.fl[0];
measurement_pt = calc_point(measurement_angle);
//调用Kalman滤波器并赋予其最新的测量值，接下来就是产生过程噪声，然后对状态乘以传递矩阵F完成一次迭代并加上我们产生的过程噪声
/* plot points */
#define draw_cross( center, color, d ) \
cvLine( img, cvPoint( center.x - d, center.y - d ), \
cvPoint( center.x + d, center.y + d ), color, 1, CV_AA, 0); \
cvLine( img, cvPoint( center.x + d, center.y - d ), \
cvPoint( center.x - d, center.y + d ), color, 1, CV_AA, 0 )
cvZero( img );
//使用上面宏定义的函数
draw_cross( state_pt, CV_RGB(255,255,255), 3 );//白色，状态点
draw_cross( measurement_pt, CV_RGB(255,0,0), 3 );//红色，测量点
draw_cross( predict_pt, CV_RGB(0,255,0), 3 );//绿色，估计点
cvLine( img, state_pt, measurement_pt, CV_RGB(255,0,0), 3, CV_AA, 0 );
cvLine( img, state_pt, predict_pt, CV_RGB(255,255,0), 3, CV_AA, 0 );
cvKalmanCorrect( kalman, measurement );//校正新的测量值
cvRandArr( &rng, process_noise, CV_RAND_NORMAL, cvRealScalar(0),
cvRealScalar(sqrt(kalman->process_noise_cov->data.fl[0])));//设置正态分布过程噪声
cvMatMulAdd( kalman->transition_matrix, state, process_noise, state );
cvShowImage( "Kalman", img );
//当按键按下时，开始新的循环，初始矩阵可能会改变，所以移动速率会改变
code = (char) cvWaitKey( 100 );
if( code > 0 )
break;
}
if( code == 27 || code == 'q' || code == 'Q' )
break;
}
cvDestroyWindow("Kalman");
return 0;
}
CvMat *change(CvMat *original,int rti)
{
CvMat *mat_k,*mat_k_inv,*mat_rt,*mat_inv_e;
mat_rt = cvCreateMat(3,4,CV_64FC1);
mat_inv_e = cvCreateMat(4,4,CV_64FC1);
double _e[] = {-1,0,0,0,
0,-1,0,0,
0,0,-1,0,
0,0,0,-1};
cvInitMatHeader(mat_inv_e,4,4,CV_64FC1,_e);
mat_k = cvCreateMat( 3, 3, CV_64FC1);
double _k[] = {-800,0,599.5,0,800,399.5,0,0,1};
cvInitMatHeader( mat_k, 3, 3, CV_64FC1, _k);
mat_k_inv = cvCreateMat( 3, 3, CV_64FC1);
cvInvert(mat_k, mat_k_inv, CV_SVD);
cvmSet(mat_k,0,0,800);
cvMatMulAdd( mat_k_inv, original, 0, mat_rt);
Print_Mat(mat_rt);
cvMatMulAdd( mat_rt, mat_inv_e, 0, mat_rt);
Print_Mat(mat_rt);
//if (rti == 1)
//{
// CvMat* ad, *mul ,*e ,*inv_rt ,*rt_change , *rt_e;
// ad = cvCreateMat(3,4,CV_64FC1);
// mul = cvCreateMat(4,4,CV_64FC1);
// e = cvCreateMat(4,4,CV_64FC1);
// inv_rt = cvCreateMat(4,3,CV_64FC1);
// rt_change = cvCreateMat(4,4,CV_64FC1);
// rt_e = cvCreateMat(3,4,CV_64FC1);
// double _add[] = {0,0,0,0.12,
// 0,0,0,-0.43,
// 0,0,0,-0.17};
// cvInitMatHeader(ad,3,4,CV_64FC1,_add);
// double _rt_e[] = {1,0,0,0,
// 0,1,0,0,
// 0,0,1,0};
// cvInitMatHeader(rt_e,3,4,CV_64FC1,_rt_e);
// double _e[] ={1,0,0,0,
// 0,1,0,0,
// 0,0,1,0,
// 0,0,0,1};
// cvInitMatHeader(e,4,4,CV_64FC1,_e);
// cvMatMulAdd(mat_rt,e,ad,mat_rt);
// cout<<"mat_rt:";
// Print_Mat(mat_rt);
// cvInvert(mat_rt, inv_rt, CV_SVD);
// cout<<"inv_rt:";
// Print_Mat(inv_rt);
// cvMatMulAdd(inv_rt,rt_e,0,rt_change);
// cout<<"rt_change:";
// Print_Mat(rt_change);
// CvMat *test;
// test = cvCreateMat(3,4,CV_64FC1);
// cvMatMulAdd(mat_rt,rt_change,0,test);
// cout<<"test:";
// Print_Mat(test);
//}
//CvMat *rt_change,*rt_add,*rt_e;
//rt_change = cvCreateMat(4,4,CV_64FC1);
//rt_add = cvCreateMat(3,4,CV_64FC1);
//rt_e = cvCreateMat(4,4,CV_64FC1);
//double _change[] = {1.64,-0.24,-0.52,0,
// 0.27,1.59,-0.02,0,
// -0.1,0.42,1.08,0,
// 0,-0.01,0,0};
//cvInitMatHeader(rt_change,4,4,CV_64FC1,_change);
//double _add[] = {0,0,0,0.12,
// 0,0,0,-0.43,
// 0,0,0,-0.17};
//cvInitMatHeader(rt_add,3,4,CV_64FC1,_add);
//double _rt_e[] ={1,0,0,0,
// 0,1,0,0,
// 0,0,1,0,
// 0,0,0,1};
//cvInitMatHeader(rt_e,4,4,CV_64FC1,_rt_e);
//cvMatMulAdd(mat_rt,rt_e,rt_add,mat_rt);
//cvMatMulAdd(mat_rt,rt_change,0,mat_rt);
//Print_Mat(mat_rt);
cvMatMulAdd(mat_k,mat_rt,0,original);
return original;
}
CV_IMPL double
cvPseudoInv( CvArr* srcarr, CvArr* dstarr, int flags )
{
uchar* buffer = 0;
int local_alloc = 0;
double condition_number = 0;
CV_FUNCNAME( "cvPseudoInv" );
__BEGIN__;
CvMat astub, *a = (CvMat*)srcarr;
CvMat bstub, *b = (CvMat*)dstarr;
CvMat ustub, *u = &ustub;
CvMat vstub, *v = &vstub;
CvMat tmat;
uchar* tw = 0;
int type, n, m, nm, mn;
int buf_size, pix_size;
if( !CV_IS_ARR( a ))
CV_CALL( a = cvGetMat( a, &astub ));
if( !CV_IS_ARR( b ))
CV_CALL( b = cvGetMat( b, &bstub ));
if( !CV_ARE_TYPES_EQ( a, b ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
n = a->width;
m = a->height;
nm = MIN( n, m );
mn = MAX( n, m );
if( n != b->height || m != b->width )
CV_ERROR( CV_StsUnmatchedSizes, "" );
type = CV_ARR_TYPE( a->type );
pix_size = icvPixSize[type];
buf_size = nm*2 + mn + m*mn + n*n;
if( !(flags & CV_SVD_MODIFY_A) )
buf_size += m*n;
buf_size *= pix_size;
if( buf_size <= CV_MAX_LOCAL_SIZE )
{
buffer = (uchar*)alloca( buf_size );
local_alloc = 1;
}
else
{
CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
}
if( !(flags & CV_SVD_MODIFY_A) )
{
cvInitMatHeader( &tmat, a->height, a->width, type,
buffer + buf_size - n*m*pix_size );
cvCopy( a, &tmat );
a = &tmat;
}
tw = buffer + (nm + mn)*pix_size;
cvInitMatHeader( u, m, m, type, tw + nm*pix_size );
cvInitMatHeader( v, n, n, type, u->data.ptr + m*mn*pix_size );
if( type == CV_32FC1 )
{
IPPI_CALL( icvSVD_32f( a->data.fl, a->step/sizeof(float), (float*)tw,
u->data.fl, u->step/sizeof(float),
v->data.fl, v->step/sizeof(float),
icvGetMatSize(a), (float*)buffer ));
}
else if( type == CV_64FC1 )
{
IPPI_CALL( icvSVD_64f( a->data.db, a->step/sizeof(double), (double*)tw,
u->data.db, u->step/sizeof(double),
v->data.db, v->step/sizeof(double),
icvGetMatSize(a), (double*)buffer ));
}
else
{
CV_ERROR( CV_StsUnsupportedFormat, "" );
}
cvT( v, v );
cvGetRow( u, &tmat, 0 );
if( type == CV_32FC1 )
{
for( int i = 0; i < nm; i++ )
{
double t = ((float*)tw)[i];
tmat.data.ptr = u->data.ptr + i*u->step;
t = t > FLT_EPSILON ? 1./t : 0;
if( i == mn - 1 )
condition_number = t != 0 ? ((float*)tw)[0]*t : DBL_MAX;
cvScale( &tmat, &tmat, t );
}
}
else
{
for( int i = 0; i < nm; i++ )
{
double t = ((double*)tw)[i];
tmat.data.ptr = u->data.ptr + i*u->step;
t = t > DBL_EPSILON ? 1./t : 0;
if( i == mn - 1 )
condition_number = t != 0 ? ((double*)tw)[0]*t : DBL_MAX;
cvScale( &tmat, &tmat, t );
}
}
u->height = n;
if( n > m )
{
cvGetSubArr( u, &tmat, cvRect( 0, m, m, n - m ));
cvSetZero( &tmat );
}
cvMatMulAdd( v, u, 0, b );
CV_CHECK_NANS( b );
__END__;
if( buffer && !local_alloc )
cvFree( (void**)&buffer );
return condition_number;
}
// Render Scene Struct
void renderScene(IplImage** images, CvMat** matches, int* bestMatchedIndex,
IplImage* viewport, struct pData* poses)
{
if (myScene->currentIndex != myScene->previousIndex) {
//printf("CURRENT POSE: \n");
//printPose(myScene->pose);
}
int i, j, k;
int image_count = myScene->max_image;
int baseIndex = closestImageIndex(poses);
CvMat* transform = modelViewMatrix(baseIndex, poses);
// Translation?
cvmSet(transform, 0, 2, -1*(myScene->pose.center.val[1] - poses[baseIndex].center.val[1]));
cvmSet(transform, 1, 2, 1*(myScene->pose.center.val[2] - poses[baseIndex].center.val[2]));
// Rotation?
CvScalar diff = cvScalar(myScene->pose.center.val[0] - myScene->pose.eye.val[0],
myScene->pose.center.val[1] - myScene->pose.eye.val[1],
myScene->pose.center.val[2] - myScene->pose.eye.val[2], 0.0);
//printf("diff is: [%.2lf %.2lf %.2lf %.2lf]\n", diff.val[0], diff.val[1], diff.val[2], diff.val[3]);
double radius = norm(diff);
double angle1 = acos(diff.val[0] / radius) - PI;
double angle2 = asin(diff.val[1] / radius);
//printf("angle1: %.2lf\n", angle1);
//printf("angle2: %.2lf\n", angle2);
CvMat* zRotation = cvCreateMat(3, 3, CV_64F);
makeZAxisRotation(zRotation, (angle1+angle2) / 2);
//cvmSet(zRotation, 0, 2, 200*angle1);
//cvmSet(zRotation, 1, 2, 200*angle1);
cvMatMulAdd(zRotation, transform, 0, transform);
cvReleaseMat(&zRotation);
// Zoom?
double zoom = radius;
CvMat* zoomTransform = create3DIdentity();
cvmSet(zoomTransform, 0, 0, zoom);
cvmSet(zoomTransform, 1, 1, zoom);
cvMatMulAdd(zoomTransform, transform, 0, transform);
cvReleaseMat(&zoomTransform);
for (k = 0; k < MATCH_RANGE; k++) {
i = (baseIndex + k) % image_count;
if (i < 0) i += image_count;
//printf("displaying image %d\n", i);
if (i == baseIndex) {
cvWarpPerspective(images[i], viewport, transform, CV_INTER_LINEAR, cvScalarAll(0));
continue;
}
//mosaic other images
mosaic(i, images, matches, bestMatchedIndex, viewport, transform);
}
cvReleaseMat(&transform);
}
// ######################################################################
Point2D<int> VisualTracker::trackObjects(const Image<byte>& grey)
{
Point2D<int> targetLoc(-1,-1);
if (!itsTracking)
return targetLoc;
#ifdef HAVE_OPENCV
if (itsCurrentNumPoints > 0)
{
IplImage* pGrey = img2ipl(itsPreviousGreyImg);
IplImage* cGrey = img2ipl(grey);
//flags = CV_LKFLOW_INITIAL_GUESSES;
cvCalcOpticalFlowPyrLK(pGrey, cGrey, itsPreviousPyramid, itsCurrentPyramid,
itsPreviousPoints, itsCurrentPoints,
itsCurrentNumPoints, //number of feature points
cvSize(itsTrackWindowSize.getVal(),itsTrackWindowSize.getVal()), //search window size in each pyramid
3, // maximal pyramid level nummber
itsTrackStatus,
itsTrackError,
cvTermCriteria(CV_TERMCRIT_ITER
|CV_TERMCRIT_EPS,
20,0.03), itsTrackFlags);
itsTrackFlags = CV_LKFLOW_PYR_A_READY | CV_LKFLOW_PYR_B_READY;
cvReleaseImageHeader(&pGrey);
cvReleaseImageHeader(&cGrey);
//show track points
int k, i;
for(i = k = 0; i<itsCurrentNumPoints; i++)
{
if (!itsTrackStatus[i])
continue;
itsCurrentPoints[k++] = itsCurrentPoints[i];
//LINFO("Error %i: %f", i, itsTrackError[i]);
if (itsTrackError[i] < 2000)
{
targetLoc.i = std::min(grey.getWidth()-1, std::max(0, (int)itsCurrentPoints[i].x));
targetLoc.j = std::min(grey.getHeight()-1, std::max(0, (int)itsCurrentPoints[i].y));
ASSERT(grey.coordsOk(targetLoc));
}
}
itsCurrentNumPoints = k;
}
IplImage *swap_temp;
CV_SWAP( itsPreviousPyramid, itsCurrentPyramid, swap_temp );
CvPoint2D32f* swap_points;
CV_SWAP( itsPreviousPoints, itsCurrentPoints, swap_points );
itsPreviousGreyImg = grey;
if (itsUseKalman && grey.coordsOk(targetLoc))
{
float Z[2];
CvMat Zmat = cvMat(2,1,CV_32F, Z);
Z[0] = targetLoc.i;
Z[1] = targetLoc.j;
cvKalmanCorrect(itsKalman, &Zmat);
const CvMat* prediction = cvKalmanPredict(itsKalman, 0);
//generate measurement
cvMatMulAdd(itsKalman->measurement_matrix, itsKalman->state_pre, NULL, &Zmat);
targetLoc.i = (int)prediction->data.fl[0];
targetLoc.j = (int)prediction->data.fl[1];
}
#endif
return targetLoc;
}
int main(int argc, char** argv)
{
/* A matrix data */
const float A[] = { 1, 1, 0, 1 };
IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 );
CvKalman* kalman = cvCreateKalman( 2, 1, 0 );
/* state is (phi, delta_phi) - angle and angle increment */
CvMat* state = cvCreateMat( 2, 1, CV_32FC1 );
CvMat* process_noise = cvCreateMat( 2, 1, CV_32FC1 );
/* only phi (angle) is measured */
CvMat* measurement = cvCreateMat( 1, 1, CV_32FC1 );
CvRandState rng;
int code = -1;
cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );
cvZero( measurement );
cvNamedWindow( "Kalman", 1 );
for(;;)
{
cvRandSetRange( &rng, 0, 0.1, 0 );
rng.disttype = CV_RAND_NORMAL;
cvRand( &rng, state );
memcpy( kalman->transition_matrix->data.fl, A, sizeof(A));
cvSetIdentity( kalman->measurement_matrix, cvRealScalar(1) );
cvSetIdentity( kalman->process_noise_cov, cvRealScalar(1e-5) );
cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1e-1) );
cvSetIdentity( kalman->error_cov_post, cvRealScalar(1));
/* choose random initial state */
cvRand( &rng, kalman->state_post );
rng.disttype = CV_RAND_NORMAL;
for(;;)
{
#define calc_point(angle) \
cvPoint( cvRound(img->width/2 + img->width/3*cos(angle)), \
cvRound(img->height/2 - img->width/3*sin(angle)))
float state_angle = state->data.fl[0];
CvPoint state_pt = calc_point(state_angle);
/* predict point position */
const CvMat* prediction = cvKalmanPredict( kalman, 0 );
float predict_angle = prediction->data.fl[0];
CvPoint predict_pt = calc_point(predict_angle);
float measurement_angle;
CvPoint measurement_pt;
cvRandSetRange( &rng,
0,
sqrt(kalman->measurement_noise_cov->data.fl[0]),
0 );
cvRand( &rng, measurement );
/* generate measurement */
cvMatMulAdd( kalman->measurement_matrix, state, measurement, measurement );
measurement_angle = measurement->data.fl[0];
measurement_pt = calc_point(measurement_angle);
/* plot points */
#define draw_cross( center, color, d ) \
cvLine( img, cvPoint( center.x - d, center.y - d ), \
cvPoint( center.x + d, center.y + d ), \
color, 1, 0 ); \
cvLine( img, cvPoint( center.x + d, center.y - d ), \
cvPoint( center.x - d, center.y + d ), \
color, 1, 0 )
cvZero( img );
draw_cross( state_pt, CV_RGB(255,255,255), 3 );
draw_cross( measurement_pt, CV_RGB(255,0,0), 3 );
draw_cross( predict_pt, CV_RGB(0,255,0), 3 );
cvLine( img, state_pt, predict_pt, CV_RGB(255,255,0), 3, 0 );
/* adjust Kalman filter state */
cvKalmanCorrect( kalman, measurement );
cvRandSetRange( &rng,
0,
sqrt(kalman->process_noise_cov->data.fl[0]),
0 );
cvRand( &rng, process_noise );
cvMatMulAdd( kalman->transition_matrix,
state,
process_noise,
state );
cvShowImage( "Kalman", img );
code = cvWaitKey( 100 );
if( code > 0 ) /* break current simulation by pressing a key */
break;
}
if( code == 27 ) /* exit by ESCAPE */
break;
}
return 0;
}
