//
// Constructor
//
CCondens::CCondens(TCHAR *tszName,LPUNKNOWN punk,HRESULT *phr) :
CTransInPlaceFilter(tszName, punk, CLSID_CCondens,phr)
{
m_params.nSamples = 64;
m_params.x = 0.4f;
m_params.y = 0.3f;
m_params.width = 0.2f;
m_params.height = 0.3f;
m_params.Smin = 20;
m_params.Vmin = 40;
m_params.view = 0;
m_Indicat1.x = 0;
m_Indicat1.y = 0;
m_Indicat1.width = 100;
m_Indicat1.height = 60;
IsTracking = false;
IsInit =false;
ConDens=cvCreateConDensation(4,4,m_params.nSamples);
CvMat Dyn = cvMat(4,4,CV_MAT4x4_32F,ConDens->DynamMatr);
cvmSetIdentity(&Dyn);
cvmSet(&Dyn,0,1,0.9);
cvmSet(&Dyn,2,3,0.9);
ASSERT(tszName);
ASSERT(phr);
} // CCondens
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);
}
}
/** Initialize the Condensation data structure and state dynamics
*/
void OpticalFlow::InitCondensation(int condens_num_samples)
{
// initialize Condensation data structure and set the
// system dynamics
m_sample_confidences.resize(condens_num_samples);
if (m_pConDens) {
cvReleaseConDensation(&m_pConDens);
}
m_pConDens = cvCreateConDensation(OF_CONDENS_DIMS, OF_CONDENS_DIMS, condens_num_samples);
CvMat dyn = cvMat(OF_CONDENS_DIMS, OF_CONDENS_DIMS, CV_32FC1, m_pConDens->DynamMatr);
// CvMat dyn = cvMat(OF_CONDENS_DIMS, OF_CONDENS_DIMS, CV_MAT3x3_32F, m_pConDens->DynamMatr);
cvmSetIdentity(&dyn);
cvmSet(&dyn, 0, 1, 0.0);
cvmSet(&dyn, 2, 3, 0.0);
// initialize bounds for state
float lower_bound[OF_CONDENS_DIMS];
float upper_bound[OF_CONDENS_DIMS];
// velocity bounds highly depend on the frame rate that we will achieve,
// increase the factor for lower frame rates;
// it states how much the center can move in either direction in a single
// frame, measured in terms of the width or height of the initial match size
double velocity_factor = .25;
double cx = (m_condens_init_rect.left+m_condens_init_rect.right)/2.0;
double cy = (m_condens_init_rect.top+m_condens_init_rect.bottom)/2.0;
double width = (m_condens_init_rect.right-m_condens_init_rect.left)*velocity_factor;
double height = (m_condens_init_rect.bottom-m_condens_init_rect.top)*velocity_factor;
lower_bound[0] = (float) (cx-width);
upper_bound[0] = (float) (cx+width);
lower_bound[1] = (float) (-width);
upper_bound[1] = (float) (+width);
lower_bound[2] = (float) (cy-height);
upper_bound[2] = (float) (cy+height);
lower_bound[3] = (float) (-height);
upper_bound[3] = (float) (+height);
lower_bound[4] = (float) (-10.0*velocity_factor*M_PI/180.0);
upper_bound[4] = (float) (+10.0*velocity_factor*M_PI/180.0);
CvMat lb = cvMat(OF_CONDENS_DIMS, 1, CV_MAT3x1_32F, lower_bound);
CvMat ub = cvMat(OF_CONDENS_DIMS, 1, CV_MAT3x1_32F, upper_bound);
cvConDensInitSampleSet(m_pConDens, &lb, &ub);
// set the state that will later be computed by condensation to
// the currently observed state
m_condens_state.x = cx;
m_condens_state.y = cy;
m_condens_state.vx = 0;
m_condens_state.vy = 0;
m_condens_state.angle = 0;
// debugging:
// DbgSetModuleLevel(LOG_CUSTOM1, 3);
}
//
// Constructor
//
CKalmTrack::CKalmTrack(TCHAR *tszName,LPUNKNOWN punk,HRESULT *phr) :
CTransInPlaceFilter(tszName, punk, CLSID_CKalmTrack,phr)
{
m_params.x = 0.4f;
m_params.y = 0.3f;
m_params.width = 0.2f;
m_params.height = 0.3f;
m_params.Smin = 20;
m_params.Vmin = 40;
m_params.view = 0;
IsTracking = false;
IsInit =false;
Kalman = cvCreateKalman(4,4);
CvMat Dyn = cvMat(4,4,CV_MAT4x4_32F,Kalman->DynamMatr);
CvMat Mes = cvMat(4,4,CV_MAT4x4_32F,Kalman->MeasurementMatr);
CvMat PNC = cvMat(4,4,CV_MAT4x4_32F,Kalman->PNCovariance);
CvMat MNC = cvMat(4,4,CV_MAT4x4_32F,Kalman->MNCovariance);
CvMat PriErr = cvMat(4,4,CV_MAT4x4_32F,Kalman->PriorErrorCovariance);
CvMat PostErr = cvMat(4,4,CV_MAT4x4_32F,Kalman->PosterErrorCovariance);
CvMat PriState = cvMat(4,1,CV_MAT4x1_32F,Kalman->PriorState);
cvmSetIdentity(&PNC);
cvmSetIdentity(&PriErr);
cvmSetIdentity(&PostErr);
cvmSetZero(&MNC);
cvmSetZero(&PriState);
cvmSetIdentity(&Dyn);
cvmSet(&Dyn,0,1,0.9f);
cvmSet(&Dyn,2,3,0.9f);
cvmSetIdentity(&Mes);
MVect = cvMat(4,1,CV_MAT4x1_32F,Measurement);
ASSERT(tszName);
ASSERT(phr);
} // CKalmTrack
/*
* \param Intrinsic : 3 by 3 camera intrinsic matrix
* \param bazProjMat : 3 by 4 projection matrix = K[RT] obtained by calling "augment.GetProjectionMatrix(0)" function
* \arRT : 4 by 4 [R t] matrix for rendering
*/
void BazARTracker::GetARToolKitRTfromBAZARProjMat(CvMat *Intrinsic, CvMat *bazProjMat, CvMat *arRT)
{
int i, j;
CvMat *temps = cvCreateMat(3, 4, CV_64F);
// inverse matrix with intrinsic camera parameters
CvMat *invK = cvCloneMat(Intrinsic);
cvInv(Intrinsic, invK);
// multiply bazProjMat with inverted intrinsic camera parameter matrix -> extract intrinsic parameters
cvMatMul(invK, bazProjMat, temps);
cvmSetIdentity(arRT);
for(i=0; i<temps->rows; i++)
{
for(j=0; j<temps->cols;j++) cvmSet(arRT, i, j, cvmGet(temps, i,j));
}
cvReleaseMat(&temps);
cvReleaseMat(&invK);
}
