CV_IMPL void
cvCalcImageHomography( float* line, CvPoint3D32f* _center,
float* _intrinsic, float* _homography )
{
double norm_xy, norm_xz, xy_sina, xy_cosa, xz_sina, xz_cosa, nx1, plane_dist;
float _ry[3], _rz[3], _r_trans[9];
CvMat rx = cvMat( 1, 3, CV_32F, line );
CvMat ry = cvMat( 1, 3, CV_32F, _ry );
CvMat rz = cvMat( 1, 3, CV_32F, _rz );
CvMat r_trans = cvMat( 3, 3, CV_32F, _r_trans );
CvMat center = cvMat( 3, 1, CV_32F, _center );
float _sub[9];
CvMat sub = cvMat( 3, 3, CV_32F, _sub );
float _t_trans[3];
CvMat t_trans = cvMat( 3, 1, CV_32F, _t_trans );
CvMat intrinsic = cvMat( 3, 3, CV_32F, _intrinsic );
CvMat homography = cvMat( 3, 3, CV_32F, _homography );
if( !line || !_center || !_intrinsic || !_homography )
CV_Error( CV_StsNullPtr, "" );
norm_xy = cvSqrt( line[0] * line[0] + line[1] * line[1] );
xy_cosa = line[0] / norm_xy;
xy_sina = line[1] / norm_xy;
norm_xz = cvSqrt( line[0] * line[0] + line[2] * line[2] );
xz_cosa = line[0] / norm_xz;
xz_sina = line[2] / norm_xz;
nx1 = -xz_sina;
_rz[0] = (float)(xy_cosa * nx1);
_rz[1] = (float)(xy_sina * nx1);
_rz[2] = (float)xz_cosa;
cvScale( &rz, &rz, 1./cvNorm(&rz,0,CV_L2) );
/* new axe y */
cvCrossProduct( &rz, &rx, &ry );
cvScale( &ry, &ry, 1./cvNorm( &ry, 0, CV_L2 ) );
/* transpone rotation matrix */
memcpy( &_r_trans[0], line, 3*sizeof(float));
memcpy( &_r_trans[3], _ry, 3*sizeof(float));
memcpy( &_r_trans[6], _rz, 3*sizeof(float));
/* calculate center distanse from arm plane */
plane_dist = cvDotProduct( ¢er, &rz );
/* calculate (I - r_trans)*center */
cvSetIdentity( &sub );
cvSub( &sub, &r_trans, &sub );
cvMatMul( &sub, ¢er, &t_trans );
cvMatMul( &t_trans, &rz, &sub );
cvScaleAdd( &sub, cvRealScalar(1./plane_dist), &r_trans, &sub ); /* ? */
cvMatMul( &intrinsic, &sub, &r_trans );
cvInvert( &intrinsic, &sub, CV_SVD );
cvMatMul( &r_trans, &sub, &homography );
}
CV_IMPL CvKalman*
cvCreateKalman( int DP, int MP, int CP )
{
CvKalman *kalman = 0;
if( DP <= 0 || MP <= 0 )
CV_Error( CV_StsOutOfRange,
"state and measurement vectors must have positive number of dimensions" );
if( CP < 0 )
CP = DP;
/* allocating memory for the structure */
kalman = (CvKalman *)cvAlloc( sizeof( CvKalman ));
memset( kalman, 0, sizeof(*kalman));
kalman->DP = DP;
kalman->MP = MP;
kalman->CP = CP;
kalman->state_pre = cvCreateMat( DP, 1, CV_32FC1 );
cvZero( kalman->state_pre );
kalman->state_post = cvCreateMat( DP, 1, CV_32FC1 );
cvZero( kalman->state_post );
kalman->transition_matrix = cvCreateMat( DP, DP, CV_32FC1 );
cvSetIdentity( kalman->transition_matrix );
kalman->process_noise_cov = cvCreateMat( DP, DP, CV_32FC1 );
cvSetIdentity( kalman->process_noise_cov );
kalman->measurement_matrix = cvCreateMat( MP, DP, CV_32FC1 );
cvZero( kalman->measurement_matrix );
kalman->measurement_noise_cov = cvCreateMat( MP, MP, CV_32FC1 );
cvSetIdentity( kalman->measurement_noise_cov );
kalman->error_cov_pre = cvCreateMat( DP, DP, CV_32FC1 );
kalman->error_cov_post = cvCreateMat( DP, DP, CV_32FC1 );
cvZero( kalman->error_cov_post );
kalman->gain = cvCreateMat( DP, MP, CV_32FC1 );
if( CP > 0 )
{
kalman->control_matrix = cvCreateMat( DP, CP, CV_32FC1 );
cvZero( kalman->control_matrix );
}
kalman->temp1 = cvCreateMat( DP, DP, CV_32FC1 );
kalman->temp2 = cvCreateMat( MP, DP, CV_32FC1 );
kalman->temp3 = cvCreateMat( MP, MP, CV_32FC1 );
kalman->temp4 = cvCreateMat( MP, DP, CV_32FC1 );
kalman->temp5 = cvCreateMat( MP, 1, CV_32FC1 );
#if 1
kalman->PosterState = kalman->state_pre->data.fl;
kalman->PriorState = kalman->state_post->data.fl;
kalman->DynamMatr = kalman->transition_matrix->data.fl;
kalman->MeasurementMatr = kalman->measurement_matrix->data.fl;
kalman->MNCovariance = kalman->measurement_noise_cov->data.fl;
kalman->PNCovariance = kalman->process_noise_cov->data.fl;
kalman->KalmGainMatr = kalman->gain->data.fl;
kalman->PriorErrorCovariance = kalman->error_cov_pre->data.fl;
kalman->PosterErrorCovariance = kalman->error_cov_post->data.fl;
#endif
return kalman;
}
int StereoVision::calibrationEnd() {
calibrationStarted = false;
// ARRAY AND VECTOR STORAGE:
double M1[3][3], M2[3][3], D1[5], D2[5];
double R[3][3], T[3], E[3][3], F[3][3];
CvMat _M1,_M2,_D1,_D2,_R,_T,_E,_F;
_M1 = cvMat(3, 3, CV_64F, M1 );
_M2 = cvMat(3, 3, CV_64F, M2 );
_D1 = cvMat(1, 5, CV_64F, D1 );
_D2 = cvMat(1, 5, CV_64F, D2 );
_R = cvMat(3, 3, CV_64F, R );
_T = cvMat(3, 1, CV_64F, T );
_E = cvMat(3, 3, CV_64F, E );
_F = cvMat(3, 3, CV_64F, F );
// HARVEST CHESSBOARD 3D OBJECT POINT LIST:
objectPoints.resize(sampleCount*cornersN);
for(int k=0; k<sampleCount; k++)
for(int i = 0; i < cornersY; i++ )
for(int j = 0; j < cornersX; j++ )
objectPoints[k*cornersY*cornersX + i*cornersX + j] = cvPoint3D32f(i, j, 0);
npoints.resize(sampleCount,cornersN);
int N = sampleCount * cornersN;
CvMat _objectPoints = cvMat(1, N, CV_32FC3, &objectPoints[0] );
CvMat _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] );
CvMat _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] );
CvMat _npoints = cvMat(1, npoints.size(), CV_32S, &npoints[0] );
cvSetIdentity(&_M1);
cvSetIdentity(&_M2);
cvZero(&_D1);
cvZero(&_D2);
//CALIBRATE THE STEREO CAMERAS
cvStereoCalibrate( &_objectPoints, &_imagePoints1,
&_imagePoints2, &_npoints,
&_M1, &_D1, &_M2, &_D2,
imageSize, &_R, &_T, &_E, &_F,
cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5),
CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_ZERO_TANGENT_DIST + CV_CALIB_SAME_FOCAL_LENGTH
);
//Always work in undistorted space
cvUndistortPoints( &_imagePoints1, &_imagePoints1,&_M1, &_D1, 0, &_M1 );
cvUndistortPoints( &_imagePoints2, &_imagePoints2,&_M2, &_D2, 0, &_M2 );
//COMPUTE AND DISPLAY RECTIFICATION
double R1[3][3], R2[3][3];
CvMat _R1 = cvMat(3, 3, CV_64F, R1);
CvMat _R2 = cvMat(3, 3, CV_64F, R2);
//HARTLEY'S RECTIFICATION METHOD
double H1[3][3], H2[3][3], iM[3][3];
CvMat _H1 = cvMat(3, 3, CV_64F, H1);
CvMat _H2 = cvMat(3, 3, CV_64F, H2);
CvMat _iM = cvMat(3, 3, CV_64F, iM);
cvStereoRectifyUncalibrated(
&_imagePoints1,&_imagePoints2, &_F,
imageSize,
&_H1, &_H2, 3
);
cvInvert(&_M1, &_iM);
cvMatMul(&_H1, &_M1, &_R1);
cvMatMul(&_iM, &_R1, &_R1);
cvInvert(&_M2, &_iM);
cvMatMul(&_H2, &_M2, &_R2);
cvMatMul(&_iM, &_R2, &_R2);
//Precompute map for cvRemap()
cvReleaseMat(&mx1);
cvReleaseMat(&my1);
cvReleaseMat(&mx2);
cvReleaseMat(&my2);
mx1 = cvCreateMat( imageSize.height,imageSize.width, CV_32F );
my1 = cvCreateMat( imageSize.height,imageSize.width, CV_32F );
mx2 = cvCreateMat( imageSize.height,imageSize.width, CV_32F );
my2 = cvCreateMat( imageSize.height,imageSize.width, CV_32F );
cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_M1,mx1,my1);
cvInitUndistortRectifyMap(&_M2,&_D2,&_R2,&_M2,mx2,my2);
calibrationDone = true;
return RESULT_OK;
}
void cv_SetIdentity (CvArr* mat, CvScalar* scalar) {
cvSetIdentity(mat, *scalar);
}
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;
}
KalmanPoint *NewKalman(PointSeq *Point_New, int ID, int frame_count, int contourArea) {
const float A[] = { 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1 };
const float H[] = { 1, 0, 0, 0, 0, 1, 0, 0 };
KalmanPoint *Kalmanfilter;
Kalmanfilter = (KalmanPoint *) malloc(sizeof(KalmanPoint));
CvKalman *Kalman = cvCreateKalman(4, 2, 0);
float measure[2] =
{ (float) Point_New->Point.x, (float) Point_New->Point.y };
CvMat measurement = cvMat(2, 1, CV_32FC1, measure);
// initialize the Kalman filter
memcpy(Kalman->transition_matrix->data.fl, A, sizeof(A));
memcpy(Kalman->measurement_matrix->data.fl, H, sizeof(H));
cvSetIdentity(Kalman->error_cov_post, cvRealScalar(10));
cvSetIdentity(Kalman->process_noise_cov, cvRealScalar(1e-5));
cvSetIdentity(Kalman->measurement_noise_cov, cvRealScalar(1e-1));
// for the first time, use the measured point as the state_post
cvZero(Kalman->state_post);
cvmSet(Kalman->state_post, 0, 0, Point_New->Point.x);
cvmSet(Kalman->state_post, 1, 0, Point_New->Point.y);
// use Kalman filter to predict the position of the centroid
const CvMat *prediction = cvKalmanPredict(Kalman, 0);
// use the measurement to correct the position
const CvMat *correction = cvKalmanCorrect(Kalman, &measurement);
/*
// test code
printf("measurement %f,%f\n",cvmGet(&measurement,0,0),cvmGet(&measurement, 1, 0));
printf("prediction %f,%f\n",cvmGet(prediction, 0, 0),cvmGet(prediction, 1, 0));
printf("correction %f,%f\ndelta %f,%f\n",cvmGet(correction, 0, 0),cvmGet(correction, 1, 0),
cvmGet(correction, 2, 0), cvmGet(correction, 3, 0));
*/
Kalmanfilter->Point_now = Point_New->Point;
Kalmanfilter->Point_pre = Point_New->Point;
Kalmanfilter->firstPoint = Point_New->Point;
Kalmanfilter->firstFrame = frame_count; //tambahan 25 januari 2016 speed measurement
Kalmanfilter->lastFrame = frame_count; //tambahan 25 januari 2016 speed measurement
Kalmanfilter->Kalman = Kalman;
Kalmanfilter->ID = Point_New->ID = ID;
Kalmanfilter->contourArea=contourArea;
/*
//nilai 1 = mobil nilai 2= truk nilai 3 = undefined
if (Kalmanfilter->contourArea >= 4800
&& Kalmanfilter->contourArea <= 19900) {
Kalmanfilter->jenis = 1;
} else if (Kalmanfilter->contourArea > 19900) {
Kalmanfilter->jenis = 2;
} else {
Kalmanfilter->jenis = 3;
}
*/
//nilai 1 = motor;nilai 2 = mobil;nilai 3 = truk sedang;nilai 4 = truk besar;nilai 0 = undefined;
if (Kalmanfilter->contourArea >= 4000
&& Kalmanfilter->contourArea <= 7000) {
Kalmanfilter->jenis = 2;
} else if (Kalmanfilter->contourArea > 7000
&& Kalmanfilter->contourArea <= 12000) {
Kalmanfilter->jenis = 3;
} else if (Kalmanfilter->contourArea > 12000) {
Kalmanfilter->jenis = 4;
} else {
Kalmanfilter->jenis = 0;
}
//Kalmanfilter->jenis= 0;//tambahan 26 januari 2016
Kalmanfilter->Loss = 0;
return Kalmanfilter;
}
TargetObject::TargetObject(int idd, std::string namee, float xx, float yy, float zz, float vxx, float vyy, float vzz, int r, int g, int b, int inacTime):
id(idd),
name(namee),
x(xx),
y(yy),
z (zz),
vx(vxx),
vz(vzz),
vy(vyy),
red(r),
green(g),
blue(b),
//associated_to_cam_data(false),
//associated_to_any_data(false),
hue(NOF_HUEBINS/2.0),
weight(-1),
hueDataAvailable(false),
currentHueValue(1.0),
minMahaDist(999.),
attentionalPriority(20. + ((float)rand()/(float)RAND_MAX)),
sendx(xx),
sendy(yy),
sendz(zz),
tapx(-0.5),
tapy(0.0)
{
predX = x;
predY = y;
weightReset = false;
curWeight = 0.0;
prevWeight = 0.0;
speedVec.clear();
speedVecx.clear();
speedVecy.clear();
meanSpeed = 0.0;
meanSpeedx = 0.0;
meanSpeedy = 0.0;
computeSpecialID();
//tempTimer.setActiveTime(); // this is testing timer..delete later
//newHue = false;
// set timer
timer.setActiveTime();
//std::std::cout << "targets previousActive time at creation = " << timer->previousActiveTime << std::std::endl;
// initialize measurement matrtimer.setActiveTime();ix
measurement = cvCreateMat( MES_SIZE, 1, CV_32FC1 );
// initialize measurement_prediction matrix
measurement_pred = cvCreateMat( MES_SIZE, 1, CV_32FC1 );
// temporary matrices
combined_innov = cvCreateMat( MES_SIZE, 1, CV_32FC1 );
tempo1 = cvCreateMat( MES_SIZE, 1, CV_32FC1 );
tempo2 = cvCreateMat( KALMAN_SIZE, 1, CV_32FC1 );
tempoTrans1 = cvCreateMat( 1, MES_SIZE, CV_32FC1 );
tempCov1 = cvCreateMat(MES_SIZE,KALMAN_SIZE,CV_32FC1);
tempCov2 = cvCreateMat(KALMAN_SIZE,MES_SIZE,CV_32FC1);
tempCov3 = cvCreateMat(MES_SIZE,MES_SIZE,CV_32FC1);
tempCov4 = cvCreateMat(KALMAN_SIZE,KALMAN_SIZE,CV_32FC1);
tempCov5 = cvCreateMat(KALMAN_SIZE,KALMAN_SIZE,CV_32FC1);
tempCov6 = cvCreateMat(MES_SIZE,MES_SIZE,CV_32FC1);
combInnovCov = cvCreateMat(MES_SIZE,MES_SIZE,CV_32FC1);
/// extra error
extra_error = cvCreateMat(KALMAN_SIZE,KALMAN_SIZE,CV_32FC1);
// JPDA event probabilities
event_probs = new std::vector<float>;
all_innovs = new std::vector<CvMat*>;
all_measurements = new std::vector<high_dim_data>;
// CvKalman* cvCreateKalman( int dynam_params, int measure_params, int control_params=0 );
kalman = cvCreateKalman( KALMAN_SIZE, MES_SIZE, CONTROL_SIZE ); // state is x,y,vx,vy and control vector is ax,ay (=acceleration)
// measurement is x,y,z
//std::cout << "kalman state size = " << kalman->DP << std::endl;
//std::cout << "kalman mes size = " << kalman->MP << std::endl;
// set the current state
CvMat* M = kalman->state_post;
int step = M->step/sizeof(float);
float *data = M->data.fl;
(data+0*step)[0] = xx;
(data+1*step)[0] = yy;
(data+2*step)[0] = zz;
(data+3*step)[0] = vxx;
(data+4*step)[0] = vyy;
(data+5*step)[0] = vzz;
(data+6*step)[0] = 0.0; // acceleration
(data+7*step)[0] = 0.0;
(data+8*step)[0] = hue; // hue
(data+9*step)[0] = weight; // weight
/// set the prediction to the same as x, y (for validation gate)
cvmSet(kalman->state_pre,0,0, 300.);
cvmSet(kalman->state_pre,1,0, 300.);
notAssociated = false;
lostTrack = false;
//cvSetIdentity( extra_error, cvRealScalar(60) ); /// 3500 until april 2008
// initialize the Kalman vectors and matrices
//const float A[] = { 1.0, 0.0, 0.0, 0.0, 1.0 ,0.0,0.0,0.0,1.0 }; // transition matrix
// const float A[] = { 1.0, 0.0, 1.0, 0.0, 1.0, 0.0 ,
// 0.0, 1.0, 0.0, 1.0, 0.0, 1.0,
// 0.0, 0.0, 1.0, 0.0, 1.0, 0.0,
// 0.0, 0.0, 0.0, 1.0, 0.0, 1.0,
// 0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
// 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 }; // transition matrix
// X, Y, Z, VX, VY, VZ, AX, AY, AZ, HUE WEI
const float A[] = { 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
}; // transition matrix
memcpy( kalman->transition_matrix->data.fl, A, sizeof(A));
//printMatrix(kalman->transition_matrix, "transition matrix");
///std::cout << "ok 1" << std::endl;
// H = measurement matrix
//const float MES[] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 ,
// 0.0, 1.0, 0.0, 0.0, 0.0, 0.0 }; //measurement matrix
const float MES[] = {
1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0
}; //
memcpy( kalman->measurement_matrix->data.fl, MES, sizeof(MES));
//printMatrix(kalman->measurement_matrix, "measurement matrix");
/// process noice covariance matrix
const float B[] = { 0.01, 0.0, 0.0, 0.001, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0,
0.0, 0.01, 0.0, 0.0, 0.001, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0,
0.0, 0.0, 0.001, 0.0, 0.0, 0.001, 0.0, 0.0, 0.001, 0.0, 0.0,
0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.001, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01
};
///memcpy( kalman->process_noise_cov->data.fl, B, sizeof(B));
cvSetIdentity( kalman->process_noise_cov, cvRealScalar(1.0) );
cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(0.01) );
/// meaurement noice covariance
///cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1.0));
/// estimation error covariance (P-Schlange)
cvSetIdentity( kalman->error_cov_post, cvRealScalar(1.0));
estimationAdvanced = false;
/// testing
//std::cout << "target created: id, x,y,hue,weight = " << id << ", " << x << ", " << y << ", " << hue << ", " << weight << std::endl;
/// for computation of the validation gate
xaxis = 100.;
yaxis = 100.;
growth_factor = 0.1;
}
// do the Kalman estimation..given the measurement (x,y)
void TargetObject::kalmanEstimateState(float xx, float yy, float zz, float huee, float weightt){
/// attention: make sure that the covariance min and max are kept (otherwise we run into numerical instability problems)
//std::cout << " --------------kalmanEstimateState with " << xx << ", " << yy << ", " << huee << ", " << weightt << std::endl;
/// set timer
//timer.setActiveTime();
// save old state
xOld = x;
yOld = y;
zOld = z;
vxOld = vx;
vyOld = vy;
vzOld = vz;
hueOld = hue;
weightOld = weight;
// pointer to the state data
CvMat* M = kalman->state_post;
int step = M->step/sizeof(float);
float *data = M->data.fl;
// measurement
/* x,y is measured */
// pointer to the measurement data
float* mesdata = measurement->data.fl;
int stepm = measurement->step/sizeof(float);
(mesdata+0*stepm)[0] = xx;
(mesdata+1*stepm)[0] = yy;
(mesdata+2*stepm)[0] = zz;
(mesdata+3*stepm)[0] = huee;
(mesdata+4*stepm)[0] = weightt;
cvKalmanCorrect( kalman, measurement );
float* postdata = kalman->state_post->data.fl;
x = (postdata+0*step)[0];
y = (postdata+1*step)[0];
z = (postdata+2*step)[0];
vx = (postdata+3*step)[0];
vy = (postdata+4*step)[0];
vz = (postdata+5*step)[0];
hue = (postdata+9*step)[0];
weight = (postdata+10*step)[0];
if (isnan(x)){
x = xOld;
std::cout << "kalmanUpdateState: x is nan" << std::endl;
}
if (isnan(y)){
y = yOld;
std::cout << "kalmanUpdateState: y is nan" << std::endl;
}
if (isnan(z)){
z = zOld;
std::cout << "kalmanUpdateState: z is nan" << std::endl;
}
if (isnan(vx)){
vx = vxOld;
std::cout << "kalmanUpdateState: vx is nan" << std::endl;
}
if (isnan(vy)){
vy = vyOld;
std::cout << "kalmanUpdateState: vy is nan" << std::endl;
}
if (isnan(vz)){
vz = vzOld;
std::cout << "kalmanUpdateState: vz is nan" << std::endl;
}
if (isnan(hue)){
hue = hueOld;
std::cout << "kalmanUpdateState: hue is nan" << std::endl;
}
if (isnan(weight)){
std::cout << "kalmanUpdateState: weight is nan" << std::endl;
weight = weightOld;
}
//std::cout << "id: " << id << ", " << x << ", " << y << ", " << hue << ", " << weight << std::endl;
//std::cout << "id = " << id << ", norm cov = " << cvNorm(kalman->error_cov_post) << std::endl;
double curNorm = cvNorm(kalman->error_cov_post);
if (curNorm > 17.0){
estimationAdvanced = true;
}
// 22.0 and 17.0 before
if ((curNorm > 22.0 ) || ((estimationAdvanced) && (curNorm < 18.0)))
{
estimationAdvanced = false;
//std::cout << std::endl << std::endl << "************************************ resetting " << std::endl << std::endl;
/// set the new x and y to the data
// set the current state
CvMat* M = kalman->state_post;
int step = M->step/sizeof(float);
float *data = M->data.fl;
(data+0*step)[0] = xx;
(data+1*step)[0] = yy;
(data+2*step)[0] = zz;
(data+3*step)[0] = zz;
(data+4*step)[0] = 1.0;
(data+5*step)[0] = 1.0;
(data+6*step)[0] = 1.0;
(data+7*step)[0] = 1.0;
(data+8*step)[0] = 1.0;
(data+9*step)[0] = huee;
(data+10*step)[0] = weightt;
cvSetIdentity( kalman->error_cov_post, cvRealScalar(1.0));
cvCopy(kalman->state_post,kalman->state_pre );
cvSetIdentity( kalman->process_noise_cov, cvRealScalar(5) );
cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(5.0) );
/// for computation of the validation gate
xaxis = 100.;
yaxis = 100.;
growth_factor = 0.1;
}
/// testing ID from hues
// if ((hue < 12.0) && (newHue) && (id ==0)){
// std::cout << "recovery green = " << tempTimer.getInactiveTime() << std::endl;
// tempTimer.setActiveTime();
// newHue = false;
// }
// if ((hue > 15.0) && (newHue) && (id ==1)){
// std::cout << "recovery red = " << tempTimer.getInactiveTime() << std::endl;
// tempTimer.setActiveTime();
// newHue = false;
// }
}
int main(int argc, char** argv)
{
CvMat* M = cvCreateMat(3, 3, CV_64FC1);
cvSetIdentity(M);
CvMat* L = MT_CreateCholeskyResult(M);
MT_Cholesky(M, L);
disp_mat(M, "M");
disp_mat(L, "L = chol(M)");
cvmSet(M, 1, 1, 4.0);
cvmSet(M, 2, 2, 9.0);
MT_Cholesky(M, L);
disp_mat(M, "M");
disp_mat(L, "L = chol(M)");
cvmSet(M, 0, 1, 0.1);
cvmSet(M, 0, 2, 0.1);
cvmSet(M, 1, 0, 0.1);
cvmSet(M, 1, 2, 0.1);
cvmSet(M, 2, 0, 0.1);
cvmSet(M, 2, 1, 0.1);
MT_Cholesky(M, L);
disp_mat(M, "M");
disp_mat(L, "L = chol(M)");
cvReleaseMat(&M);
cvReleaseMat(&L);
M = cvCreateMat(5, 5, CV_64FC1);
cvSet(M, cvScalar(0.1));
for(unsigned int i = 0; i < 5; i++)
{
cvmSet(M, i, i, (double) (i+1));
}
L = MT_CreateCholeskyResult(M);
MT_Cholesky(M, L);
disp_mat(M, "M");
disp_mat(L, "L = chol(M)");
cvReleaseMat(&M);
cvReleaseMat(&L);
CvMat* C = cvCreateMat(4, 4, CV_64FC1);
CvMat* A = cvCreateMatHeader(2, 2, CV_64FC1);
CvMat* B = cvCreateMatHeader(2, 2, CV_64FC1);
cvGetSubRect(C, A, cvRect(0, 0, 2, 2));
cvGetSubRect(C, B, cvRect(2, 2, 2, 2));
cvSet(C, cvScalar(0));
cvSet(A, cvScalar(1));
cvSet(B, cvScalar(2));
cvReleaseMat(&A);
cvReleaseMat(&B);
disp_mat(C, "C (composited)");
cvReleaseMat(&C);
}
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;
}
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;
}
int main(int argc, char **argv)
{
std::string tracker_name;
std::string obj_name;
std::string input;
std::string intrinsic;
std::string distortion;
int width;
int height;
float sample_step;
bool dull_edge;
float th_cm;
int n; // number of particles
bool display;
bool net;
bool save_rslt_txt;
bool save_rslt_img;
std::string str_result_path;
CvMat* pose_init;
pose_init = cvCreateMat(4, 4, CV_32F);
cvSetIdentity(pose_init);
CV_MAT_ELEM(*pose_init, float, 2, 3) = 0.5f; // 0.5 meter in front of camera
std::string pose_init_str;
int min_keypoint_matches;
std::string ach_channel;
bool use_tracking;
po::options_description desc("Tracker options");
desc.add_options()
("help,h", "produce help message")
("tracker,t", po::value<std::string>(&tracker_name)->default_value("irls"), "name tracker (irls, pf, pf_textureless)")
("obj-name,o", po::value<std::string>(&obj_name), "name of traget object")
("input,i", po::value<std::string>(&input), "name of camera (e.g. flea) or image sequence path (e.g. dir/seq1/)")
("sample-step,s", po::value<float>(&sample_step)->default_value(0.005f), "sample step")
("num-particle,n", po::value<int>(&n)->default_value(1), "number of particles")
("save-txt", po::value<bool>(&save_rslt_txt)->default_value(false), "save results in text file")
("save-img", po::value<bool>(&save_rslt_img)->default_value(false), "save results in image files")
("save-path", po::value<std::string>(&str_result_path), "save results in image files")
("display", po::value<bool>(&display)->default_value(true), "display results or not")
("network", po::value<bool>(&net)->default_value(false), "use network mode or not")
("width", po::value<int>(&width)->default_value(640), "width")
("height", po::value<int>(&height)->default_value(480), "height")
("intrinsic", po::value<std::string>(&intrinsic)->default_value("Intrinsics_normal.xml"), "intrinsic parameters")
("distortion", po::value<std::string>(&distortion)->default_value("Distortion_normal.xml"), "distortion parameters")
("dull_edge", po::value<bool>(&dull_edge)->default_value(false), "consider dull edges")
("th_cm", po::value<float>(&th_cm)->default_value(0.2f), "threshold of chamfer matching")
("init_pose", po::value<std::string>(&pose_init_str), "init pose")
// AKAN
("min_keypoint_matches", po::value<int>(&min_keypoint_matches)->default_value(20), "min number of keypoint matches to start tracking")
("use_ach_channel", po::value<std::string>(&ach_channel)->default_value("none"), "Use specific ach channel with given name")
("use_tracking", po::value<bool>(&use_tracking)->default_value(true), "Enable tracking after detection of object")
;
po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc), vm);
po::notify(vm);
if(argc < 2 || vm.count("help"))
{
std::cout << desc << std::endl;
return 1;
}
if(input=="ach")
{
//sns_init();
}
if(obj_name.empty())
{
std::cerr << "obj-name should be specified." << std::endl;
return 1;
}
if(save_rslt_txt || save_rslt_img)
{
if(str_result_path.empty())
{
std::cerr << "No save-path specified when using either save-txt or save-img" << std::endl;
return 1;
}
}
if(vm.count("input"))
{
std::cout << "input: " << vm["input"].as<std::string>() << std::endl;
}
if(vm.count("obj-name"))
{
std::cout << "obj-name: " << vm["obj-name"].as<std::string>() << std::endl;
}
if(vm.count("n"))
{
std::cout << "n: " << vm["n"].as<int>() << std::endl;
}
if(vm.count("init_pose"))
{
std::cout << "init_pose: ";
std::vector<std::string> tokens;
boost::split(tokens, pose_init_str, boost::is_any_of(","), boost::token_compress_on);
for(int i = 0; i < tokens.size(); i++)
std::cout << tokens[i] << " ";
std::cout << std::endl;
if(tokens.size() != 16)
std::cerr << "Not enough number of data in pose. 16 values are required!" << std::endl;
{
// oeverwrite pose_init
for (int i = 0; i < 16; i++)
pose_init->data.fl[i] = std::atof(tokens[i].c_str());
}
}
TrackerBase* tracker;
if(tracker_name.compare("irls") == 0)
{
tracker = new IrlsTracker ();
tracker->setMinKeypointMatches(min_keypoint_matches);
}
else if(tracker_name.compare("pf") == 0)
{
tracker = new TextureParticleFilterTracker ();
((TextureParticleFilterTracker*)tracker)->setNumParticle(n);
((TextureParticleFilterTracker*)tracker)->initParticleFilter();
}
else if(tracker_name.compare("pf_textureless") == 0)
{
tracker = new TexturelessParticleFilterTracker ();
((TexturelessParticleFilterTracker*)tracker)->setNumParticle(n);
((TexturelessParticleFilterTracker*)tracker)->setThresholdCM(th_cm);
((TexturelessParticleFilterTracker*)tracker)->initParticleFilter();
}
else
{
std::cerr << "Unknown tracker method name: " << tracker_name << std::endl;
return 1;
}
tracker->setSampleStep(sample_step);
tracker->setDisplay(display);
tracker->setNetworkMode(net);
tracker->setSaveResultText(save_rslt_txt);
tracker->setSaveResultImage(save_rslt_img);
tracker->setSaveResultPath(str_result_path);
tracker->setConsideringDullEdges(dull_edge);
tracker->setTracking(use_tracking);
tracker->initTracker(obj_name, input, intrinsic, distortion, width, height, pose_init , ach_channel);
tracker->run();
delete tracker;
return (0);
}
void
cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
const CvMat* _distCoeffs,
const CvMat* _R, const CvMat* _P )
{
CV_FUNCNAME( "cvUndistortPoints" );
__BEGIN__;
double A[3][3], RR[3][3], k[5]={0,0,0,0,0}, fx, fy, ifx, ify, cx, cy;
CvMat _A=cvMat(3, 3, CV_64F, A), _Dk;
CvMat _RR=cvMat(3, 3, CV_64F, RR);
const CvPoint2D32f* srcf;
const CvPoint2D64f* srcd;
CvPoint2D32f* dstf;
CvPoint2D64f* dstd;
int stype, dtype;
int sstep, dstep;
int i, j, n;
CV_ASSERT( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
(_src->rows == 1 || _src->cols == 1) &&
(_dst->rows == 1 || _dst->cols == 1) &&
CV_ARE_SIZES_EQ(_src, _dst) &&
(CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) &&
(CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2));
CV_ASSERT( CV_IS_MAT(_cameraMatrix) && CV_IS_MAT(_distCoeffs) &&
_cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 &&
(_distCoeffs->rows == 1 || _distCoeffs->cols == 1) &&
(_distCoeffs->rows*_distCoeffs->cols == 4 ||
_distCoeffs->rows*_distCoeffs->cols == 5) );
_Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k);
cvConvert( _cameraMatrix, &_A );
cvConvert( _distCoeffs, &_Dk );
if( _R )
{
CV_ASSERT( CV_IS_MAT(_R) && _R->rows == 3 && _R->cols == 3 );
cvConvert( _R, &_RR );
}
else
cvSetIdentity(&_RR);
if( _P )
{
double PP[3][3];
CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP);
CV_ASSERT( CV_IS_MAT(_P) && _P->rows == 3 && (_P->cols == 3 || _P->cols == 4));
cvConvert( cvGetCols(_P, &_P3x3, 0, 3), &_PP );
cvMatMul( &_PP, &_RR, &_RR );
}
srcf = (const CvPoint2D32f*)_src->data.ptr;
srcd = (const CvPoint2D64f*)_src->data.ptr;
dstf = (CvPoint2D32f*)_dst->data.ptr;
dstd = (CvPoint2D64f*)_dst->data.ptr;
stype = CV_MAT_TYPE(_src->type);
dtype = CV_MAT_TYPE(_dst->type);
sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);
n = _src->rows + _src->cols - 1;
fx = A[0][0];
fy = A[1][1];
ifx = 1./fx;
ify = 1./fy;
cx = A[0][2];
cy = A[1][2];
for( i = 0; i < n; i++ )
{
double x, y, x0, y0;
if( stype == CV_32FC2 )
{
x = srcf[i*sstep].x;
y = srcf[i*sstep].y;
}
else
{
x = srcd[i*sstep].x;
y = srcd[i*sstep].y;
}
x0 = x = (x - cx)*ifx;
y0 = y = (y - cy)*ify;
// compensate distortion iteratively
for( j = 0; j < 5; j++ )
{
double r2 = x*x + y*y;
double icdist = 1./(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
x = (x0 - deltaX)*icdist;
y = (y0 - deltaY)*icdist;
}
double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2];
double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2];
double ww = 1./(RR[2][0]*x + RR[2][1]*y + RR[2][2]);
x = xx*ww;
y = yy*ww;
if( dtype == CV_32FC2 )
{
dstf[i*dstep].x = (float)x;
dstf[i*dstep].y = (float)y;
}
else
{
dstd[i*dstep].x = x;
dstd[i*dstep].y = y;
}
}
__END__;
}
void
cvInitUndistortRectifyMap( const CvMat* A, const CvMat* distCoeffs,
const CvMat *R, const CvMat* Ar, CvArr* mapxarr, CvArr* mapyarr )
{
CV_FUNCNAME( "cvInitUndistortMap" );
__BEGIN__;
double a[9], ar[9], r[9], ir[9], k[5]={0,0,0,0,0};
int coi1 = 0, coi2 = 0;
CvMat mapxstub, *_mapx = (CvMat*)mapxarr;
CvMat mapystub, *_mapy = (CvMat*)mapyarr;
CvMat _a = cvMat( 3, 3, CV_64F, a );
CvMat _k = cvMat( 4, 1, CV_64F, k );
CvMat _ar = cvMat( 3, 3, CV_64F, ar );
CvMat _r = cvMat( 3, 3, CV_64F, r );
CvMat _ir = cvMat( 3, 3, CV_64F, ir );
int i, j;
double fx, fy, u0, v0, k1, k2, k3, p1, p2;
CvSize size;
CV_CALL( _mapx = cvGetMat( _mapx, &mapxstub, &coi1 ));
CV_CALL( _mapy = cvGetMat( _mapy, &mapystub, &coi2 ));
if( coi1 != 0 || coi2 != 0 )
CV_ERROR( CV_BadCOI, "The function does not support COI" );
if( CV_MAT_TYPE(_mapx->type) != CV_32FC1 )
CV_ERROR( CV_StsUnsupportedFormat, "Both maps must have 32fC1 type" );
if( !CV_ARE_TYPES_EQ( _mapx, _mapy ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( !CV_ARE_SIZES_EQ( _mapx, _mapy ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
if( A )
{
if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 ||
(CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) )
CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" );
cvConvert( A, &_a );
}
else
cvSetIdentity( &_a );
if( Ar )
{
CvMat Ar33;
if( !CV_IS_MAT(Ar) || Ar->rows != 3 || (Ar->cols != 3 && Ar->cols != 4) ||
(CV_MAT_TYPE(Ar->type) != CV_32FC1 && CV_MAT_TYPE(Ar->type) != CV_64FC1) )
CV_ERROR( CV_StsBadArg, "The new intrinsic matrix must be a valid 3x3 floating-point matrix" );
cvGetCols( Ar, &Ar33, 0, 3 );
cvConvert( &Ar33, &_ar );
}
else
cvSetIdentity( &_ar );
if( !CV_IS_MAT(R) || R->rows != 3 || R->cols != 3 ||
(CV_MAT_TYPE(R->type) != CV_32FC1 && CV_MAT_TYPE(R->type) != CV_64FC1) )
CV_ERROR( CV_StsBadArg, "Rotaion/homography matrix must be a valid 3x3 floating-point matrix" );
if( distCoeffs )
{
CV_ASSERT( CV_IS_MAT(distCoeffs) &&
(distCoeffs->rows == 1 || distCoeffs->cols == 1) &&
(distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 4 ||
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 5) &&
(CV_MAT_DEPTH(distCoeffs->type) == CV_64F ||
CV_MAT_DEPTH(distCoeffs->type) == CV_32F) );
_k = cvMat( distCoeffs->rows, distCoeffs->cols,
CV_MAKETYPE(CV_64F, CV_MAT_CN(distCoeffs->type)), k );
cvConvert( distCoeffs, &_k );
}
else
cvZero( &_k );
cvConvert( R, &_r ); // rectification matrix
cvMatMul( &_ar, &_r, &_r ); // Ar*R
cvInvert( &_r, &_ir ); // inverse: R^-1*Ar^-1
u0 = a[2]; v0 = a[5];
fx = a[0]; fy = a[4];
k1 = k[0]; k2 = k[1]; k3 = k[4];
p1 = k[2]; p2 = k[3];
size = cvGetMatSize(_mapx);
for( i = 0; i < size.height; i++ )
{
float* mapx = (float*)(_mapx->data.ptr + _mapx->step*i);
float* mapy = (float*)(_mapy->data.ptr + _mapy->step*i);
double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8];
for( j = 0; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] )
{
double w = 1./_w, x = _x*w, y = _y*w;
double x2 = x*x, y2 = y*y;
double r2 = x2 + y2, _2xy = 2*x*y;
double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2;
double u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + u0;
double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + v0;
mapx[j] = (float)u;
mapy[j] = (float)v;
}
}
__END__;
}
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);
}
