void CvBlobTrackPredictKalman::ParamUpdate() { cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) ); cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) ); CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize; CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize; }
CvBlobTrackPredictKalman::CvBlobTrackPredictKalman() { m_ModelNoise = 1e-6f; m_DataNoisePos = 1e-6f; m_DataNoiseSize = 1e-1f; #if STATE_NUM>6 m_DataNoiseSize *= (float)pow(20.,2.); #else m_DataNoiseSize /= (float)pow(20.,2.); #endif AddParam("ModelNoise",&m_ModelNoise); AddParam("DataNoisePos",&m_DataNoisePos); AddParam("DataNoiseSize",&m_DataNoiseSize); m_Frame = 0; m_pKalman = cvCreateKalman(STATE_NUM,4); memcpy( m_pKalman->transition_matrix->data.fl, A, sizeof(A)); memcpy( m_pKalman->measurement_matrix->data.fl, H, sizeof(H)); cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) ); cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) ); CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize; CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize; cvSetIdentity( m_pKalman->error_cov_post, cvRealScalar(1)); cvZero(m_pKalman->state_post); cvZero(m_pKalman->state_pre); SetModuleName("Kalman"); }
/// <summary> /// Finds min and max Y of the data present in given image. /// </summary> /// <params name="imsSrc"> /// Source image for which min and max Y has to be found. /// </params> /// <params name="min"> /// Int pointer where the min Y has to saved. /// </params> /// <params name="max"> /// Int pointer where the max Y has to saved. /// </params> /// <returns> Nothing. </returns> void OCR::findY(IplImage* imgSrc,int* min, int* max) { int i; int minFound=0; CvMat data; CvScalar maxVal=cvRealScalar(imgSrc->width * 255); CvScalar val=cvRealScalar(0); //For each col sum, if sum < width*255 then we find the min //then continue to end to search the max, if sum< width*255 then is new max for (i=0; i< imgSrc->height; i++) { val = cvRealScalar(0); cvGetRow(imgSrc, &data, i); val= cvSum(&data); if(val.val[0] < maxVal.val[0]) { *max=i; if(!minFound) { *min= i; minFound= 1; } } } }
void Kalman_CV::init() { Logger::log("Kalman_CV", "init", Logger::LOGLEVEL_DEBUG); // state transition matrix const float F[] = {1.0, 0.02, 0.0, \ 0.0, 1.0, 0.004, \ 0.0, 0.0, 1.0}; memcpy( cvkal->transition_matrix->data.fl, F, sizeof(F)); // H - measurement transform const float H[] = {1.0, 0.0, 0.0, \ 1.0, 0.0, 0.0, \ 0.0, 0.0, 1.0, \ 1.0, 0.0, 0.0, \ 1.0, 0.0, 0.0 }; memcpy(cvkal->measurement_matrix->data.fl, H, sizeof(H)); // Q - process noise covariance cvSetIdentity( cvkal->process_noise_cov, cvRealScalar(5e-3)); // R - measurement covariance const float R[] = {0.3, 0.0, 0.0, 0.0, 0.0, \ 0.0, 2.0, 0.0, 0.0, 0.0, \ 0.0, 0.0, 0.01, 0.0, 0.0, \ 0.0, 0.0, 0.0, 0.3, 0.0, \ 0.0, 0.0, 0.0, 0.0, 0.3 }; // 7.86737928e+04 memcpy(cvkal->measurement_noise_cov->data.fl, R, sizeof(R)); // P - a posteriori error estimate cvSetIdentity( cvkal->error_cov_post, cvRealScalar(1)); // measurement vector meas = cvCreateMat(5,1, CV_32FC1); }
ofxCvKalman::ofxCvKalman(float initial) { kalman = cvCreateKalman( 1, 1, 0 ); measurement = cvCreateMat( 1, 1, CV_32FC1 ); cvZero( measurement ); //kalman->transition_matrix = cvCreateMat( 4, 1, CV_32FC1 ); //cvSetIdentity( kalman->transition_matrix, cvRealScalar(1) ); memcpy( kalman->transition_matrix->data.fl, A, sizeof(A)); /* cvSetIdentity( kalman->measurement_matrix, cvRealScalar(1) ); cvSetIdentity( kalman->process_noise_cov, cvRealScalar(1e-8) ); cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1e-8) ); cvSetIdentity( kalman->error_cov_post, cvRealScalar(1e-5)); */ /*cvSetIdentity(kalman->measurement_matrix, cvRealScalar(1)); cvSetIdentity(kalman->process_noise_cov, cvRealScalar(0.4)); cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(3)); cvSetIdentity(kalman->error_cov_post, cvRealScalar(1)); */ cvSetIdentity(kalman->measurement_matrix, cvRealScalar(1)); cvSetIdentity(kalman->process_noise_cov, cvRealScalar(0.4)); cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(8)); cvSetIdentity(kalman->error_cov_post, cvRealScalar(3)); kalman->state_post->data.fl[0]=initial; }
int main( int argc, char** argv ) { int i, j; CvMemStorage* storage = cvCreateMemStorage(0); IplImage* img = cvCreateImage( cvSize(w,w), 8, 1 ); cvZero( img ); for( i=0; i < 6; i++ ) { int dx = (i%2)*250 - 30; int dy = (i/2)*150; CvScalar white = cvRealScalar(255); CvScalar black = cvRealScalar(0); if( i == 0 ) { for( j = 0; j <= 10; j++ ) { double angle = (j+5)*CV_PI/21; cvLine(img, cvPoint(cvRound(dx+100+j*10-80*cos(angle)), cvRound(dy+100-90*sin(angle))), cvPoint(cvRound(dx+100+j*10-30*cos(angle)), cvRound(dy+100-30*sin(angle))), white, 1, 8, 0); } } cvEllipse( img, cvPoint(dx+150, dy+100), cvSize(100,70), 0, 0, 360, white, -1, 8, 0 ); cvEllipse( img, cvPoint(dx+115, dy+70), cvSize(30,20), 0, 0, 360, black, -1, 8, 0 ); cvEllipse( img, cvPoint(dx+185, dy+70), cvSize(30,20), 0, 0, 360, black, -1, 8, 0 ); cvEllipse( img, cvPoint(dx+115, dy+70), cvSize(15,15), 0, 0, 360, white, -1, 8, 0 ); cvEllipse( img, cvPoint(dx+185, dy+70), cvSize(15,15), 0, 0, 360, white, -1, 8, 0 ); cvEllipse( img, cvPoint(dx+115, dy+70), cvSize(5,5), 0, 0, 360, black, -1, 8, 0 ); cvEllipse( img, cvPoint(dx+185, dy+70), cvSize(5,5), 0, 0, 360, black, -1, 8, 0 ); cvEllipse( img, cvPoint(dx+150, dy+100), cvSize(10,5), 0, 0, 360, black, -1, 8, 0 ); cvEllipse( img, cvPoint(dx+150, dy+150), cvSize(40,10), 0, 0, 360, black, -1, 8, 0 ); cvEllipse( img, cvPoint(dx+27, dy+100), cvSize(20,35), 0, 0, 360, white, -1, 8, 0 ); cvEllipse( img, cvPoint(dx+273, dy+100), cvSize(20,35), 0, 0, 360, white, -1, 8, 0 ); } cvNamedWindow( "image", 1 ); cvShowImage( "image", img ); cvFindContours( img, storage, &contours, sizeof(CvContour), CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0) ); // comment this out if you do not want approximation contours = cvApproxPoly( contours, sizeof(CvContour), storage, CV_POLY_APPROX_DP, 3, 1 ); cvNamedWindow( "contours", 1 ); cvCreateTrackbar( "levels+3", "contours", &levels, 7, on_trackbar ); on_trackbar(0); cvWaitKey(0); cvReleaseMemStorage( &storage ); cvReleaseImage( &img ); return 0; }
void GaussianDiag3D::AddNoise(double* vin, double* vout) { /* Tirage aleatoire du bruit des parametres continus (m=0 sd=1) */ cvRandArr(&this->rng_state,this->RandVectorMat,CV_RAND_NORMAL,cvRealScalar(0),cvRealScalar(1)); vout[0] = vin[0] + this->cholcov[0]*this->RandVector[0]; vout[1] = vin[1] + this->cholcov[4]*this->RandVector[1]; vout[2] = vin[2] + this->cholcov[8]*this->RandVector[2]; }
CvBlobTrackerOneKalman() { m_Frame = 0; m_pKalman = cvCreateKalman(STATE_NUM,4); memcpy( m_pKalman->transition_matrix->data.fl, A, sizeof(A)); memcpy( m_pKalman->measurement_matrix->data.fl, H, sizeof(H)); cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(1e-5) ); cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(1e-1) ); // CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) *= (float)pow(20,2); // CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) *= (float)pow(20,2); cvSetIdentity( m_pKalman->error_cov_post, cvRealScalar(1)); cvZero(m_pKalman->state_post); cvZero(m_pKalman->state_pre); }
void blur_function(const IplImage *latent_image, IplImage *blur_image, const CvMat *hom1, const CvMat *hom2) { const int T = 20; const int tau = 10; CvMat *id_mat = cvCreateMat(3, 3, CV_32FC1); cvSetIdentity(id_mat, cvRealScalar(1)); CvMat *invhom1 = cvCreateMat(3, 3, CV_32FC1); cvInvert(hom1, invhom1, CV_LU); CvMat *h1 = cvCreateMat(3, 3, CV_32FC1); CvMat *h2 = cvCreateMat(3, 3, CV_32FC1); CvSize size = cvSize(latent_image->width, latent_image->height); IplImage *temp = cvCreateImage(size, latent_image->depth, latent_image->nChannels); IplImage *blur = cvCreateImage(size, IPL_DEPTH_32F, latent_image->nChannels); cvSetZero(blur); for (int i = 1; i <= tau; ++i) { cvAddWeighted(id_mat, (double)(T-i)/T, invhom1, (double)i/T, 0, h1); cvAddWeighted(id_mat, (double)(T-i)/T, hom2, (double)i/T, 0, h2); cvWarpPerspective(latent_image, temp, h1, CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS, cvScalarAll(0)); cvAdd(blur, temp, blur, NULL); cvWarpPerspective(latent_image, temp, h2, CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS, cvScalarAll(0)); cvAdd(blur, temp, blur, NULL); } cvAdd(blur, latent_image, blur, NULL); cvConvertScale(blur, blur_image, 1.0/(2*tau+1), 0); cvReleaseMat(&id_mat); cvReleaseMat(&invhom1); cvReleaseMat(&h1); cvReleaseMat(&h2); cvReleaseImage(&temp); cvReleaseImage(&blur); }
void Ellipse::draw(Image& img, double intensity, int thickness, int line_type) const { CV_FUNCNAME( "Ellipse::draw" ); __BEGIN__; int shift = 0; double fixpoint_factor = static_cast<double>(1 << shift); CvPoint center = cvPoint( cvRound(this->params_.center.x * fixpoint_factor), cvRound(this->params_.center.y * fixpoint_factor) ); CvSize axes = cvSize(cvRound(this->params_.size.width * fixpoint_factor * 0.5) , cvRound(this->params_.size.height * fixpoint_factor * 0.5) ); double angle = static_cast<double>( - this->params_.angle ) * 180 / CV_PI; CV_CALL( cvEllipse(img.get_ipl_image(), center, axes, angle, 0, 360, cvRealScalar(intensity), thickness, line_type) ); __END__; __ISL_CHECK_ERROR__; }
void GaussianND::Draw(double* mm, double* vout) { int i,j; double* ptcholcov; /* Tirage aleatoire du bruit des parametres continus (m=0 sd=1) */ cvRandArr(&this->rng_state,this->RandVectorMat,CV_RAND_NORMAL,cvRealScalar(0),cvRealScalar(1)); /* Tir autour de la moyenne mean avec une covariance cov */ for(i=0,ptcholcov=this->cholcov;i<this->dim;i++,ptcholcov+=this->dim) { vout[i] = mm[i]; for(j=0;j<this->dim;j++) vout[i]+=ptcholcov[j]*this->RandVector[j]; } }
IplImage* BouyObject::ShapeMask(const IplImage * imgIn) const { IplImage * hsv = cvCloneImage(imgIn); IplImage * imgOut = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); //cvEqualizeHist( imgIn, hsv); IplImage * chan0 = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); IplImage * chan1 = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); IplImage * chan2 = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); IplImage * chan3 = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); cvCvtColor(imgIn, hsv, CV_BGR2YCrCb); cvSplit(hsv,chan0,chan1,chan2, NULL); CvScalar white = cvRealScalar(255); //imgOut = SegmentationMask(imgIn); imgOut = cvCloneImage(chan2); //invert black and white cvAbsDiffS(imgOut, imgOut, white); // cvShowImage("hue",chan0); // cvShowImage("sat",chan1); // cvShowImage("val",chan2); // cvShowImage("inv",imgOut); //cvWaitKey(0); cvReleaseImage(&hsv); cvReleaseImage(&chan0); cvReleaseImage(&chan1); cvReleaseImage(&chan2); cvReleaseImage(&chan3); CvSize imageSize = cvSize(imgIn->width & -2, imgIn->height & -2 ); IplImage* imgSmallCopy = cvCreateImage( cvSize(imageSize.width/2, imageSize.height/2), IPL_DEPTH_8U, 1 ); cvPyrDown( imgOut, imgSmallCopy); cvPyrUp( imgSmallCopy, imgOut); cvSmooth(imgOut, imgOut, CV_GAUSSIAN, 5); cvReleaseImage(&imgSmallCopy); CvMemStorage* storage = cvCreateMemStorage(0); CvSeq* results = cvHoughCircles( imgOut, storage, CV_HOUGH_GRADIENT, 2, imgOut->width/10, 100, 100); cvZero(imgOut); for( int i = 0; i < results->total; i++ ) { float* p = (float*) cvGetSeqElem( results, i ); CvPoint pt = cvPoint( cvRound( p[0] ), cvRound( p[1] ) ); cvCircle( imgOut, pt, cvRound( p[2] ), CV_RGB(0xff,0xff,0xff),CV_FILLED); } cvReleaseMemStorage(&storage); return imgOut; }
void on_mouse( int event, int x, int y, int flags, void* param ) { if( !color_img ) return; switch( event ) { case CV_EVENT_LBUTTONDOWN: { CvPoint seed = cvPoint(x,y); int lo = ffill_case == 0 ? 0 : lo_diff; int up = ffill_case == 0 ? 0 : up_diff; int flags = connectivity + (new_mask_val << 8) + (ffill_case == 1 ? CV_FLOODFILL_FIXED_RANGE : 0); int b = rand() & 255, g = rand() & 255, r = rand() & 255; CvConnectedComp comp; if( is_mask ) cvThreshold( mask, mask, 1, 128, CV_THRESH_BINARY ); if( is_color ) { CvScalar color = CV_RGB( r, g, b ); cvFloodFill( color_img, seed, color, CV_RGB( lo, lo, lo ), CV_RGB( up, up, up ), &comp, flags, is_mask ? mask : NULL ); cvShowImage( "image", color_img ); } else { CvScalar brightness = cvRealScalar((r*2 + g*7 + b + 5)/10); cvFloodFill( gray_img, seed, brightness, cvRealScalar(lo), cvRealScalar(up), &comp, flags, is_mask ? mask : NULL ); cvShowImage( "image", gray_img ); } printf("%g pixels were repainted\n", comp.area ); if( is_mask ) cvShowImage( "mask", mask ); } break; } }
/// <summary> /// Reads the sample images and associated charaters into trainClasses and trainData respectively. /// </summary> /// <returns> Nothing. </returns> void OCR::getData() { IplImage* src_image; IplImage prs_image; CvMat row,data; char file[255]; char dataFile[255]; std::ifstream labelStream; std::ostringstream outStringStream; char ch; int i,j; for(i = 0; i < classes; i++) { //26 //Read the corresponding character for current sample being processed into ch. sprintf(dataFile,"%s%d/data.txt",file_path, i); labelStream.open(dataFile); labelStream >> ch; labelStream.close(); for( j = 0; j< train_samples; j++) { //3 //Load file //get the path of image for training into file. if(j<10) sprintf(file,"%s%d/%d0%d.pbm",file_path, i, i, j); else sprintf(file,"%s%d/%d%d.pbm",file_path, i, i, j); src_image = cvLoadImage(file,0); if(!src_image) { printf("Error: Cant load image %s\n", file); //exit(-1); } //process file prs_image = preprocessing(src_image, size, size); //Set class label cvGetRow(trainClasses, &row, i*train_samples + j); cvSet(&row, cvRealScalar(ch)); //Set data cvGetRow(trainData, &row, i*train_samples + j); IplImage* img = cvCreateImage( cvSize( size, size ), IPL_DEPTH_32F, 1 ); //convert 8 bits image to 32 float image cvConvertScale(&prs_image, img, 0.0039215, 0); cvGetSubRect(img, &data, cvRect(0,0, size,size)); CvMat row_header, *row1; //convert data matrix sizexsize to vecor row1 = cvReshape( &data, &row_header, 0, 1 ); cvCopy(row1, &row, NULL); } } }
void img_from_labels(CvMat* labels, CvMat **classes, CvMat* color_dst, CvScalar *color_tab) { for (int row = 0; row < color_dst->rows; row++) { for (int col = 0; col < color_dst->cols; col++) { int i = CV_MAT_ELEM(*labels, int, row * color_dst->cols + col, 0); cvSet2D(color_dst, row, col, color_tab[i]); cvSet2D(classes[i], row, col, cvRealScalar(255)); } } }
void DrawImage(IplImage* pImg) { if (pImg == NULL) return; cvZero(pImg); for (int i = 0; i < 6; i++) { int dx = (i % 2) * 250 - 30; int dy = (i / 2) * 150; CvScalar white = cvRealScalar(255); CvScalar black = cvRealScalar(0); if (i == 0) { for (int j = 0; j <= 10; j++) { double angle = (j + 5)*CV_PI / 21; cvLine(pImg, cvPoint(cvRound(dx + 100 + j * 10 - 80 * cos(angle)), cvRound(dy + 100 - 90 * sin(angle))), cvPoint(cvRound(dx + 100 + j * 10 - 30 * cos(angle)), cvRound(dy + 100 - 30 * sin(angle))), white, 1, 8, 0); } } cvEllipse(pImg, cvPoint(dx + 150, dy + 100), cvSize(100, 70), 0, 0, 360, white, -1, 8, 0); cvEllipse(pImg, cvPoint(dx + 115, dy + 70), cvSize(30, 20), 0, 0, 360, black, -1, 8, 0); cvEllipse(pImg, cvPoint(dx + 185, dy + 70), cvSize(30, 20), 0, 0, 360, black, -1, 8, 0); cvEllipse(pImg, cvPoint(dx + 115, dy + 70), cvSize(15, 15), 0, 0, 360, white, -1, 8, 0); cvEllipse(pImg, cvPoint(dx + 185, dy + 70), cvSize(15, 15), 0, 0, 360, white, -1, 8, 0); cvEllipse(pImg, cvPoint(dx + 115, dy + 70), cvSize(5, 5), 0, 0, 360, black, -1, 8, 0); cvEllipse(pImg, cvPoint(dx + 185, dy + 70), cvSize(5, 5), 0, 0, 360, black, -1, 8, 0); cvEllipse(pImg, cvPoint(dx + 150, dy + 100), cvSize(10, 5), 0, 0, 360, black, -1, 8, 0); cvEllipse(pImg, cvPoint(dx + 150, dy + 150), cvSize(40, 10), 0, 0, 360, black, -1, 8, 0); cvEllipse(pImg, cvPoint(dx + 27, dy + 100), cvSize(20, 35), 0, 0, 360, white, -1, 8, 0); cvEllipse(pImg, cvPoint(dx + 273, dy + 100), cvSize(20, 35), 0, 0, 360, white, -1, 8, 0); } }
kalman::kalman(int winwidth, int winheight,CvRect rect) { cvkalman = cvCreateKalman(4, 2, 0); process_noise = cvCreateMat(4, 1, CV_32FC1); measurement = cvCreateMat(2, 1, CV_32FC1); rng = cvRNG(-1); float A[4][4] = { 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1 }; this->rect = rect; memcpy(cvkalman->transition_matrix->data.fl,A,sizeof(A)); cvSetIdentity(cvkalman->measurement_matrix,cvRealScalar(1)); cvSetIdentity(cvkalman->process_noise_cov,cvRealScalar(1e-5)); cvSetIdentity(cvkalman->measurement_noise_cov, cvRealScalar(1e-1)); cvSetIdentity(cvkalman->error_cov_post, cvRealScalar(1)); cvRandArr(&rng, cvkalman->state_post, CV_RAND_UNI, cvRealScalar(0), cvRealScalar(winheight>winwidth ? winwidth : winheight)); //cvNamedWindow("kalman",1); img = cvCreateImage(cvSize(winwidth, winheight), 8, 3); cvSet(img, cvScalar(255, 255, 255, 0)); cvRectangle(img, cvPoint(rect.x, rect.y), cvPoint(rect.x+rect.width, rect.y+rect.height), cvScalar(255, 0, 0)); //cvShowImage("kalman",img); initlinklist(nl); sum = 0; num = 0; }
void SensorFusion::showGridMap(const string &windowName) { for (int x = 0; x < mapWidth / gridWidth; x++) { for (int y = 0; y < mapHeight / gridHeight; y++) { double grayValue = (double)((1 - p[x][y]) * 255); CvScalar pixel = cvRealScalar(grayValue); cvSet2D(gridMapImage, y, x, pixel); } } cvNamedWindow(windowName.c_str(), 0); cvShowImage(windowName.c_str(), gridMapImage); }
void Ellipse::draw_center(Image& img, int nb_pixels, double intensity, int thickness, int line_type) const { CV_FUNCNAME( "BeamBox::draw_box_center" ); __BEGIN__; CvPoint center = cvPoint(cvRound(this->params_.center.x), cvRound(this->params_.center.y)); CV_CALL(cvLine(img.get_ipl_image() , cvPoint(center.x - nb_pixels, center.y), cvPoint(center.x - 1, center.y), cvRealScalar(intensity), thickness, line_type)); CV_CALL(cvLine(img.get_ipl_image() , cvPoint(center.x + 1, center.y), cvPoint(center.x + nb_pixels, center.y), cvRealScalar(intensity), thickness, line_type)); CV_CALL(cvLine(img.get_ipl_image() , cvPoint(center.x, center.y - nb_pixels), cvPoint(center.x, center.y - 1), cvRealScalar(intensity), thickness, line_type)); CV_CALL(cvLine(img.get_ipl_image() , cvPoint(center.x, center.y + 1), cvPoint(center.x, center.y + nb_pixels), cvRealScalar(intensity), thickness, line_type)); __END__; __ISL_CHECK_ERROR__; }
CVAPI(void) cvParticleStateConfig( CvParticle * p, const CvSize imsize, CvBox2D std ) { // config dynamics model CvMat * dynamicsmat = cvCreateMat( p->num_states, p->num_states, CV_64F ); cvSetIdentity(dynamicsmat, cvRealScalar(1)); // config random noise standard deviation // CvRNG rng = cvRNG( time( NULL ) ); double stdarr[] = { // std.x, std.y, // std.width, std.height, std.center.x, std.center.y, std.size.width, std.size.height, std.angle }; CvMat stdmat = cvMat( p->num_states, 1, CV_64FC1, stdarr ); // config minimum and maximum values of states // lowerbound, upperbound, circular flag (useful for degree) // lowerbound == upperbound to express no bounding // const CvSize tsize = cvSize(24.*(imsize.width /160.), // 30.*(imsize.height/120.)); const CvSize tsize = cvSize(30.*(imsize.width /160.), 36.*(imsize.height/120.)); const float space = // space for rotation of box (10.*(imsize.width /160.)+ // additional bound MAX(tsize.width, tsize.height))*0.1415+ // rotated bound 1.0; // additional pixel double boundarr[] = { space, imsize.width - (tsize.width +1.) - space, false, // position.x space, imsize.height -(tsize.height+1.) - space, false, // position.y 12.*(imsize.width/160.), tsize.width , false, // winsize.x 15.*(imsize.height/160.), tsize.height, false, // winsize.y // 0, 360, true // dtheta + circular flag -30, 30, false // dtheta }; CvMat boundmat = cvMat( p->num_states, 3, CV_64FC1, boundarr ); //cvParticleSetDynamics( p, dynamicsmat ); cvConvert(dynamicsmat, p->dynamics); // cvParticleSetNoise( p, rng, &stdmat ); p->rng = cvRNG( time( NULL ) ); cvConvert(&stdmat,p->std); // cvParticleSetBound( p, &boundmat ); cvConvert(&boundmat,p->bound); cvReleaseMat(&dynamicsmat); }
void basicOCR::getData() { IplImage* src_image; IplImage prs_image; CvMat row,data; char file[255]; int i,j; //for(i =0; i<classes; i++) for (i = 32; i < 32 + classes; i++) { for ( j = 0; j < train_samples; j++) { //加载pbm格式图像,作为训练 /*if(j < 10) sprintf(file,"%s%d/%d0%d.pbm",file_path, i - 48, i - 48 , j); else sprintf(file,"%s%d/%d%d.pbm",file_path, i - 48, i - 48 , j);*/ if (i >= 48 && i <= 57) sprintf(file,"%s%d/%d.pbm",file_path, i, j); else sprintf(file,"%s%d/%d.bmp",file_path, i, j); src_image = cvLoadImage(file,0); if(!src_image) { //printf("Error: Cant load image %s\n", file); continue; //exit(-1); } //process file prs_image = preprocessing(src_image, size, size); //Set class label cvGetRow(trainClasses, &row, (i - 32)*train_samples + j); cvSet(&row, cvRealScalar(i)); //Set data cvGetRow(trainData, &row, (i - 32)*train_samples + j); IplImage* img = cvCreateImage( cvSize( size, size ), IPL_DEPTH_32F, 1 ); //convert 8 bits image to 32 float image cvConvertScale(&prs_image, img, 0.0039215, 0); cvGetSubRect(img, &data, cvRect(0,0, size,size)); CvMat row_header, *row1; //convert data matrix sizexsize to vecor row1 = cvReshape( &data, &row_header, 0, 1 ); cvCopy(row1, &row, NULL); } } }
KalmanFilter::KalmanFilter() { int dynam_params = 8; // x,y,width,height,dx,dy,dw,dh int measure_params = 4;//x,y,width,height m_pKalmanFilter = cvCreateKalman(dynam_params, measure_params,0); cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI ); cvRandSetRange( &rng, 0, 1, 0 ); rng.disttype = CV_RAND_NORMAL; measurement = cvCreateMat( measure_params, 1, CV_32FC1 ); cvZero(measurement); // F matrix data // F is transition matrix. It relates how the states interact const float F[] = { 1, 0, 0, 0, 1, 0, 0, 0, //x + dx 0, 1, 0, 0, 0, 1, 0, 0,//y + dy 0, 0, 1, 0, 0, 0, 1, 0,//width + dw 0, 0, 0, 1, 0, 0, 0, 1,//height + dh 0, 0, 0, 0, 1, 0, 0, 0,//dx 0, 0, 0, 0, 0, 1, 0, 0,//dy 0, 0, 0, 0, 0, 0, 1, 0,//dw 0, 0, 0, 0, 0, 0, 0, 1 //dh }; memcpy( m_pKalmanFilter->transition_matrix->data.fl, F, sizeof(F)); cvSetIdentity( m_pKalmanFilter->measurement_matrix, cvRealScalar(1) ); // (H) cvSetIdentity( m_pKalmanFilter->process_noise_cov, cvRealScalar(1e-5) ); // (Q) cvSetIdentity( m_pKalmanFilter->measurement_noise_cov, cvRealScalar(1e-1) ); //(R) cvSetIdentity( m_pKalmanFilter->error_cov_post, cvRealScalar(1)); // choose random initial state cvRand( &rng, m_pKalmanFilter->state_post ); }
IplImage* PathObject::ChannelMask2(const IplImage * imgIn) const { if(imgIn == NULL) return NULL; IplImage * hsv = cvCloneImage(imgIn); IplImage * imgOut = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); //cvEqualizeHist( imgIn, hsv); IplImage * chan0 = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); IplImage * chan1 = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); IplImage * chan2 = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); IplImage * chan3 = cvCreateImage(cvGetSize(imgIn),IPL_DEPTH_8U, 1); cvCvtColor(imgIn, hsv, CV_BGR2YCrCb); cvSplit(hsv,chan0,chan1,chan2, NULL); CvScalar white = cvRealScalar(255); imgOut = cvCloneImage(chan2); //invert black and white cvAbsDiffS(imgOut, imgOut, white); cvShowImage("hue",chan0); cvShowImage("sat",chan1); cvShowImage("val",chan2); cvShowImage("inv",imgOut); cvWaitKey(0); cvReleaseImage(&hsv); cvReleaseImage(&chan0); cvReleaseImage(&chan1); cvReleaseImage(&chan2); cvReleaseImage(&chan3); CvSize imageSize = cvSize(imgOut->width & -2, imgOut->height & -2 ); IplImage* imgSmallCopy = cvCreateImage( cvSize(imageSize.width/2, imageSize.height/2), IPL_DEPTH_8U, 1 ); cvPyrDown( imgOut, imgSmallCopy); cvPyrUp( imgSmallCopy, imgOut); cvReleaseImage(&imgSmallCopy); //cvThreshold(imgOut,imgOut,200, 255,CV_THRESH_TOZERO); return imgOut; }
void boxtrackproperties:: initkalman(const cv::Mat_<float> &init) { //init 5x1 cvSetIdentity( k->transition_matrix, cvRealScalar(1.) ); //rates of change #if(0) //center cvmSet(k->transition_matrix,0,5,k_dt*5.); cvmSet(k->transition_matrix,1,6,k_dt*5.); //size cvmSet(k->transition_matrix,2,7,k_dt*0.1); cvmSet(k->transition_matrix,3,8,k_dt*0.1); //angle cvmSet(k->transition_matrix,4,9,k_dt*0.05); #else //center cvmSet(k->transition_matrix,0,5,k_dt*10.); cvmSet(k->transition_matrix,1,6,k_dt*10.); //size cvmSet(k->transition_matrix,2,7,k_dt); cvmSet(k->transition_matrix,3,8,k_dt); //angle cvmSet(k->transition_matrix,4,9,k_dt*0.5); #endif cvSetIdentity( k->measurement_matrix, cvRealScalar(1.) ); cvSetIdentity( k->process_noise_cov, cvRealScalar(1e-4) ); //1. //how believable is the model? //0.5 cvSetIdentity( k->measurement_noise_cov, cvRealScalar(1e-1)); //how believable are the measurements? 5. //5. cvSetIdentity( k->error_cov_post, cvRealScalar(1.) ); cvSetIdentity( k->error_cov_pre, cvRealScalar(1.) ); cvZero(k->state_pre); cvZero(k->state_post); for (size_t i=0; i<nmeasure; i++) k->state_pre->data.fl[i]=k->state_post->data.fl[i]=init(i,0); cvZero(k->gain); if (k->control_matrix) cvZero(k->control_matrix); cvZero(k->temp1); cvZero(k->temp2); cvZero(k->temp3); cvZero(k->temp4); cvZero(k->temp5); uid_tag=boost::uuids::random_generator(ran)(); }
CV_IMPL CvSeq* cvSegmentMotion( const CvArr* mhiimg, CvArr* segmask, CvMemStorage* storage, double timestamp, double seg_thresh ) { CvSeq* components = 0; cv::Ptr<CvMat> mask8u; CvMat mhistub, *mhi = cvGetMat(mhiimg, &mhistub); CvMat maskstub, *mask = cvGetMat(segmask, &maskstub); Cv32suf v, comp_idx; int stub_val, ts; int x, y; if( !storage ) CV_Error( CV_StsNullPtr, "NULL memory storage" ); mhi = cvGetMat( mhi, &mhistub ); mask = cvGetMat( mask, &maskstub ); if( CV_MAT_TYPE( mhi->type ) != CV_32FC1 || CV_MAT_TYPE( mask->type ) != CV_32FC1 ) CV_Error( CV_BadDepth, "Both MHI and the destination mask" ); if( !CV_ARE_SIZES_EQ( mhi, mask )) CV_Error( CV_StsUnmatchedSizes, "" ); mask8u = cvCreateMat( mhi->rows + 2, mhi->cols + 2, CV_8UC1 ); cvZero( mask8u ); cvZero( mask ); components = cvCreateSeq( CV_SEQ_KIND_GENERIC, sizeof(CvSeq), sizeof(CvConnectedComp), storage ); v.f = (float)timestamp; ts = v.i; v.f = FLT_MAX*0.1f; stub_val = v.i; comp_idx.f = 1; for( y = 0; y < mhi->rows; y++ ) { int* mhi_row = (int*)(mhi->data.ptr + y*mhi->step); for( x = 0; x < mhi->cols; x++ ) { if( mhi_row[x] == 0 ) mhi_row[x] = stub_val; } } for( y = 0; y < mhi->rows; y++ ) { int* mhi_row = (int*)(mhi->data.ptr + y*mhi->step); uchar* mask8u_row = mask8u->data.ptr + (y+1)*mask8u->step + 1; for( x = 0; x < mhi->cols; x++ ) { if( mhi_row[x] == ts && mask8u_row[x] == 0 ) { CvConnectedComp comp; int x1, y1; CvScalar _seg_thresh = cvRealScalar(seg_thresh); CvPoint seed = cvPoint(x,y); cvFloodFill( mhi, seed, cvRealScalar(0), _seg_thresh, _seg_thresh, &comp, CV_FLOODFILL_MASK_ONLY + 2*256 + 4, mask8u ); for( y1 = 0; y1 < comp.rect.height; y1++ ) { int* mask_row1 = (int*)(mask->data.ptr + (comp.rect.y + y1)*mask->step) + comp.rect.x; uchar* mask8u_row1 = mask8u->data.ptr + (comp.rect.y + y1+1)*mask8u->step + comp.rect.x+1; for( x1 = 0; x1 < comp.rect.width; x1++ ) { if( mask8u_row1[x1] > 1 ) { mask8u_row1[x1] = 1; mask_row1[x1] = comp_idx.i; } } } comp_idx.f++; cvSeqPush( components, &comp ); } } } for( y = 0; y < mhi->rows; y++ ) { int* mhi_row = (int*)(mhi->data.ptr + y*mhi->step); for( x = 0; x < mhi->cols; x++ ) { if( mhi_row[x] == stub_val ) mhi_row[x] = 0; } } return components; }
static int build_rtrees_classifier( char* data_filename, char* filename_to_save, char* filename_to_load ) { CvMat* data = 0; CvMat* responses = 0; CvMat* var_type = 0; CvMat* sample_idx = 0; int ok = read_num_class_data( data_filename, 16, &data, &responses ); int nsamples_all = 0, ntrain_samples = 0; int i = 0; double train_hr = 0, test_hr = 0; CvRTrees forest; CvMat* var_importance = 0; if( !ok ) { printf( "Could not read the database %s\n", data_filename ); return -1; } printf( "The database %s is loaded.\n", data_filename ); nsamples_all = data->rows; ntrain_samples = (int)(nsamples_all*0.8); // Create or load Random Trees classifier if( filename_to_load ) { // load classifier from the specified file forest.load( filename_to_load ); ntrain_samples = 0; if( forest.get_tree_count() == 0 ) { printf( "Could not read the classifier %s\n", filename_to_load ); return -1; } printf( "The classifier %s is loaded.\n", data_filename ); } else { // create classifier by using <data> and <responses> printf( "Training the classifier ...\n"); // 1. create type mask var_type = cvCreateMat( data->cols + 1, 1, CV_8U ); cvSet( var_type, cvScalarAll(CV_VAR_ORDERED) ); cvSetReal1D( var_type, data->cols, CV_VAR_CATEGORICAL ); // 2. create sample_idx sample_idx = cvCreateMat( 1, nsamples_all, CV_8UC1 ); { CvMat mat; cvGetCols( sample_idx, &mat, 0, ntrain_samples ); cvSet( &mat, cvRealScalar(1) ); cvGetCols( sample_idx, &mat, ntrain_samples, nsamples_all ); cvSetZero( &mat ); } // 3. train classifier forest.train( data, CV_ROW_SAMPLE, responses, 0, sample_idx, var_type, 0, CvRTParams(10,10,0,false,15,0,true,4,100,0.01f,CV_TERMCRIT_ITER)); printf( "\n"); } // compute prediction error on train and test data for( i = 0; i < nsamples_all; i++ ) { double r; CvMat sample; cvGetRow( data, &sample, i ); r = forest.predict( &sample ); r = fabs((double)r - responses->data.fl[i]) <= FLT_EPSILON ? 1 : 0; if( i < ntrain_samples ) train_hr += r; else test_hr += r; } test_hr /= (double)(nsamples_all-ntrain_samples); train_hr /= (double)ntrain_samples; printf( "Recognition rate: train = %.1f%%, test = %.1f%%\n", train_hr*100., test_hr*100. ); printf( "Number of trees: %d\n", forest.get_tree_count() ); // Print variable importance var_importance = (CvMat*)forest.get_var_importance(); if( var_importance ) { double rt_imp_sum = cvSum( var_importance ).val[0]; printf("var#\timportance (in %%):\n"); for( i = 0; i < var_importance->cols; i++ ) printf( "%-2d\t%-4.1f\n", i, 100.f*var_importance->data.fl[i]/rt_imp_sum); } //Print some proximitites printf( "Proximities between some samples corresponding to the letter 'T':\n" ); { CvMat sample1, sample2; const int pairs[][2] = {{0,103}, {0,106}, {106,103}, {-1,-1}}; for( i = 0; pairs[i][0] >= 0; i++ ) { cvGetRow( data, &sample1, pairs[i][0] ); cvGetRow( data, &sample2, pairs[i][1] ); printf( "proximity(%d,%d) = %.1f%%\n", pairs[i][0], pairs[i][1], forest.get_proximity( &sample1, &sample2 )*100. ); } } // Save Random Trees classifier to file if needed if( filename_to_save ) forest.save( filename_to_save ); cvReleaseMat( &sample_idx ); cvReleaseMat( &var_type ); cvReleaseMat( &data ); cvReleaseMat( &responses ); return 0; }
/* log_weight_div_det[k] = -2*log(weights_k) + log(det(Sigma_k))) covs[k] = cov_rotate_mats[k] * cov_eigen_values[k] * (cov_rotate_mats[k])' cov_rotate_mats[k] are orthogonal matrices of eigenvectors and cov_eigen_values[k] are diagonal matrices (represented by 1D vectors) of eigen values. The <alpha_ik> is the probability of the vector x_i to belong to the k-th cluster: <alpha_ik> ~ weights_k * exp{ -0.5[ln(det(Sigma_k)) + (x_i - mu_k)' Sigma_k^(-1) (x_i - mu_k)] } We calculate these probabilities here by the equivalent formulae: Denote S_ik = -0.5(log(det(Sigma_k)) + (x_i - mu_k)' Sigma_k^(-1) (x_i - mu_k)) + log(weights_k), M_i = max_k S_ik = S_qi, so that the q-th class is the one where maximum reaches. Then alpha_ik = exp{ S_ik - M_i } / ( 1 + sum_j!=q exp{ S_ji - M_i }) */ double CvEM::run_em( const CvVectors& train_data ) { CvMat* centered_sample = 0; CvMat* covs_item = 0; CvMat* log_det = 0; CvMat* log_weights = 0; CvMat* cov_eigen_values = 0; CvMat* samples = 0; CvMat* sum_probs = 0; log_likelihood = -DBL_MAX; CV_FUNCNAME( "CvEM::run_em" ); __BEGIN__; int nsamples = train_data.count, dims = train_data.dims, nclusters = params.nclusters; double min_variation = FLT_EPSILON; double min_det_value = MAX( DBL_MIN, pow( min_variation, dims )); double likelihood_bias = -CV_LOG2PI * (double)nsamples * (double)dims / 2., _log_likelihood = -DBL_MAX; int start_step = params.start_step; int i, j, k, n; int is_general = 0, is_diagonal = 0, is_spherical = 0; double prev_log_likelihood = -DBL_MAX / 1000., det, d; CvMat whdr, iwhdr, diag, *w, *iw; double* w_data; double* sp_data; if( nclusters == 1 ) { double log_weight; CV_CALL( cvSet( probs, cvScalar(1.)) ); if( params.cov_mat_type == COV_MAT_SPHERICAL ) { d = cvTrace(*covs).val[0]/dims; d = MAX( d, FLT_EPSILON ); inv_eigen_values->data.db[0] = 1./d; log_weight = pow( d, dims*0.5 ); } else { w_data = inv_eigen_values->data.db; if( params.cov_mat_type == COV_MAT_GENERIC ) cvSVD( *covs, inv_eigen_values, *cov_rotate_mats, 0, CV_SVD_U_T ); else cvTranspose( cvGetDiag(*covs, &diag), inv_eigen_values ); cvMaxS( inv_eigen_values, FLT_EPSILON, inv_eigen_values ); for( j = 0, det = 1.; j < dims; j++ ) det *= w_data[j]; log_weight = sqrt(det); cvDiv( 0, inv_eigen_values, inv_eigen_values ); } log_weight_div_det->data.db[0] = -2*log(weights->data.db[0]/log_weight); log_likelihood = DBL_MAX/1000.; EXIT; } if( params.cov_mat_type == COV_MAT_GENERIC ) is_general = 1; else if( params.cov_mat_type == COV_MAT_DIAGONAL ) is_diagonal = 1; else if( params.cov_mat_type == COV_MAT_SPHERICAL ) is_spherical = 1; /* In the case of <cov_mat_type> == COV_MAT_DIAGONAL, the k-th row of cov_eigen_values contains the diagonal elements (variations). In the case of <cov_mat_type> == COV_MAT_SPHERICAL - the 0-ths elements of the vectors cov_eigen_values[k] are to be equal to the mean of the variations over all the dimensions. */ CV_CALL( log_det = cvCreateMat( 1, nclusters, CV_64FC1 )); CV_CALL( log_weights = cvCreateMat( 1, nclusters, CV_64FC1 )); CV_CALL( covs_item = cvCreateMat( dims, dims, CV_64FC1 )); CV_CALL( centered_sample = cvCreateMat( 1, dims, CV_64FC1 )); CV_CALL( cov_eigen_values = cvCreateMat( inv_eigen_values->rows, inv_eigen_values->cols, CV_64FC1 )); CV_CALL( samples = cvCreateMat( nsamples, dims, CV_64FC1 )); CV_CALL( sum_probs = cvCreateMat( 1, nclusters, CV_64FC1 )); sp_data = sum_probs->data.db; // copy the training data into double-precision matrix for( i = 0; i < nsamples; i++ ) { const float* src = train_data.data.fl[i]; double* dst = (double*)(samples->data.ptr + samples->step*i); for( j = 0; j < dims; j++ ) dst[j] = src[j]; } if( start_step != START_M_STEP ) { for( k = 0; k < nclusters; k++ ) { if( is_general || is_diagonal ) { w = cvGetRow( cov_eigen_values, &whdr, k ); if( is_general ) cvSVD( covs[k], w, cov_rotate_mats[k], 0, CV_SVD_U_T ); else cvTranspose( cvGetDiag( covs[k], &diag ), w ); w_data = w->data.db; for( j = 0, det = 1.; j < dims; j++ ) det *= w_data[j]; if( det < min_det_value ) { if( start_step == START_AUTO_STEP ) det = min_det_value; else EXIT; } log_det->data.db[k] = det; } else { d = cvTrace(covs[k]).val[0]/(double)dims; if( d < min_variation ) { if( start_step == START_AUTO_STEP ) d = min_variation; else EXIT; } cov_eigen_values->data.db[k] = d; log_det->data.db[k] = d; } } cvLog( log_det, log_det ); if( is_spherical ) cvScale( log_det, log_det, dims ); } for( n = 0; n < params.term_crit.max_iter; n++ ) { if( n > 0 || start_step != START_M_STEP ) { // e-step: compute probs_ik from means_k, covs_k and weights_k. CV_CALL(cvLog( weights, log_weights )); // S_ik = -0.5[log(det(Sigma_k)) + (x_i - mu_k)' Sigma_k^(-1) (x_i - mu_k)] + log(weights_k) for( k = 0; k < nclusters; k++ ) { CvMat* u = cov_rotate_mats[k]; const double* mean = (double*)(means->data.ptr + means->step*k); w = cvGetRow( cov_eigen_values, &whdr, k ); iw = cvGetRow( inv_eigen_values, &iwhdr, k ); cvDiv( 0, w, iw ); w_data = (double*)(inv_eigen_values->data.ptr + inv_eigen_values->step*k); for( i = 0; i < nsamples; i++ ) { double *csample = centered_sample->data.db, p = log_det->data.db[k]; const double* sample = (double*)(samples->data.ptr + samples->step*i); double* pp = (double*)(probs->data.ptr + probs->step*i); for( j = 0; j < dims; j++ ) csample[j] = sample[j] - mean[j]; if( is_general ) cvGEMM( centered_sample, u, 1, 0, 0, centered_sample, CV_GEMM_B_T ); for( j = 0; j < dims; j++ ) p += csample[j]*csample[j]*w_data[is_spherical ? 0 : j]; pp[k] = -0.5*p + log_weights->data.db[k]; // S_ik <- S_ik - max_j S_ij if( k == nclusters - 1 ) { double max_val = 0; for( j = 0; j < nclusters; j++ ) max_val = MAX( max_val, pp[j] ); for( j = 0; j < nclusters; j++ ) pp[j] -= max_val; } } } CV_CALL(cvExp( probs, probs )); // exp( S_ik ) cvZero( sum_probs ); // alpha_ik = exp( S_ik ) / sum_j exp( S_ij ), // log_likelihood = sum_i log (sum_j exp(S_ij)) for( i = 0, _log_likelihood = likelihood_bias; i < nsamples; i++ ) { double* pp = (double*)(probs->data.ptr + probs->step*i), sum = 0; for( j = 0; j < nclusters; j++ ) sum += pp[j]; sum = 1./MAX( sum, DBL_EPSILON ); for( j = 0; j < nclusters; j++ ) { double p = pp[j] *= sum; sp_data[j] += p; } _log_likelihood -= log( sum ); } // check termination criteria if( fabs( (_log_likelihood - prev_log_likelihood) / prev_log_likelihood ) < params.term_crit.epsilon ) break; prev_log_likelihood = _log_likelihood; } // m-step: update means_k, covs_k and weights_k from probs_ik cvGEMM( probs, samples, 1, 0, 0, means, CV_GEMM_A_T ); for( k = 0; k < nclusters; k++ ) { double sum = sp_data[k], inv_sum = 1./sum; CvMat* cov = covs[k], _mean, _sample; w = cvGetRow( cov_eigen_values, &whdr, k ); w_data = w->data.db; cvGetRow( means, &_mean, k ); cvGetRow( samples, &_sample, k ); // update weights_k weights->data.db[k] = sum; // update means_k cvScale( &_mean, &_mean, inv_sum ); // compute covs_k cvZero( cov ); cvZero( w ); for( i = 0; i < nsamples; i++ ) { double p = probs->data.db[i*nclusters + k]*inv_sum; _sample.data.db = (double*)(samples->data.ptr + samples->step*i); if( is_general ) { cvMulTransposed( &_sample, covs_item, 1, &_mean ); cvScaleAdd( covs_item, cvRealScalar(p), cov, cov ); } else for( j = 0; j < dims; j++ ) { double val = _sample.data.db[j] - _mean.data.db[j]; w_data[is_spherical ? 0 : j] += p*val*val; } } if( is_spherical ) { d = w_data[0]/(double)dims; d = MAX( d, min_variation ); w->data.db[0] = d; log_det->data.db[k] = d; } else { if( is_general ) cvSVD( cov, w, cov_rotate_mats[k], 0, CV_SVD_U_T ); cvMaxS( w, min_variation, w ); for( j = 0, det = 1.; j < dims; j++ ) det *= w_data[j]; log_det->data.db[k] = det; } } cvConvertScale( weights, weights, 1./(double)nsamples, 0 ); cvMaxS( weights, DBL_MIN, weights ); cvLog( log_det, log_det ); if( is_spherical ) cvScale( log_det, log_det, dims ); } // end of iteration process //log_weight_div_det[k] = -2*log(weights_k/det(Sigma_k))^0.5) = -2*log(weights_k) + log(det(Sigma_k))) if( log_weight_div_det ) { cvScale( log_weights, log_weight_div_det, -2 ); cvAdd( log_weight_div_det, log_det, log_weight_div_det ); } /* Now finalize all the covariation matrices: 1) if <cov_mat_type> == COV_MAT_DIAGONAL we used array of <w> as diagonals. Now w[k] should be copied back to the diagonals of covs[k]; 2) if <cov_mat_type> == COV_MAT_SPHERICAL we used the 0-th element of w[k] as an average variation in each cluster. The value of the 0-th element of w[k] should be copied to the all of the diagonal elements of covs[k]. */ if( is_spherical ) { for( k = 0; k < nclusters; k++ ) cvSetIdentity( covs[k], cvScalar(cov_eigen_values->data.db[k])); } else if( is_diagonal ) { for( k = 0; k < nclusters; k++ ) cvTranspose( cvGetRow( cov_eigen_values, &whdr, k ), cvGetDiag( covs[k], &diag )); } cvDiv( 0, cov_eigen_values, inv_eigen_values ); log_likelihood = _log_likelihood; __END__; cvReleaseMat( &log_det ); cvReleaseMat( &log_weights ); cvReleaseMat( &covs_item ); cvReleaseMat( ¢ered_sample ); cvReleaseMat( &cov_eigen_values ); cvReleaseMat( &samples ); cvReleaseMat( &sum_probs ); return log_likelihood; }
// -------------------------------------------------------------------------- // main(Number of arguments, Argument values) // Description : This is the entry point of the program. // Return value : SUCCESS:0 ERROR:-1 // -------------------------------------------------------------------------- int main(int argc, char **argv) { // AR.Drone class ARDrone ardrone; // Initialize if (!ardrone.open()) { printf("Failed to initialize.\n"); return -1; } // Kalman filter CvKalman *kalman = cvCreateKalman(4, 2); // Setup cvSetIdentity(kalman->measurement_matrix, cvRealScalar(1.0)); cvSetIdentity(kalman->process_noise_cov, cvRealScalar(1e-5)); cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(0.1)); cvSetIdentity(kalman->error_cov_post, cvRealScalar(1.0)); // Linear system kalman->DynamMatr[0] = 1.0; kalman->DynamMatr[1] = 0.0; kalman->DynamMatr[2] = 1.0; kalman->DynamMatr[3] = 0.0; kalman->DynamMatr[4] = 0.0; kalman->DynamMatr[5] = 1.0; kalman->DynamMatr[6] = 0.0; kalman->DynamMatr[7] = 1.0; kalman->DynamMatr[8] = 0.0; kalman->DynamMatr[9] = 0.0; kalman->DynamMatr[10] = 1.0; kalman->DynamMatr[11] = 0.0; kalman->DynamMatr[12] = 0.0; kalman->DynamMatr[13] = 0.0; kalman->DynamMatr[14] = 0.0; kalman->DynamMatr[15] = 1.0; // Thresholds int minH = 0, maxH = 255; int minS = 0, maxS = 255; int minV = 0, maxV = 255; // Create a window cvNamedWindow("binalized"); cvCreateTrackbar("H max", "binalized", &maxH, 255); cvCreateTrackbar("H min", "binalized", &minH, 255); cvCreateTrackbar("S max", "binalized", &maxS, 255); cvCreateTrackbar("S min", "binalized", &minS, 255); cvCreateTrackbar("V max", "binalized", &maxV, 255); cvCreateTrackbar("V min", "binalized", &minV, 255); cvResizeWindow("binalized", 0, 0); // Main loop while (1) { // Key input int key = cvWaitKey(1); if (key == 0x1b) break; // Update if (!ardrone.update()) break; // Get an image IplImage *image = ardrone.getImage(); // HSV image IplImage *hsv = cvCloneImage(image); cvCvtColor(image, hsv, CV_RGB2HSV_FULL); // Binalized image IplImage *binalized = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1); // Binalize CvScalar lower = cvScalar(minH, minS, minV); CvScalar upper = cvScalar(maxH, maxS, maxV); cvInRangeS(image, lower, upper, binalized); // Show result cvShowImage("binalized", binalized); // De-noising cvMorphologyEx(binalized, binalized, NULL, NULL, CV_MOP_CLOSE); // Detect contours CvSeq *contour = NULL, *maxContour = NULL; CvMemStorage *contourStorage = cvCreateMemStorage(); cvFindContours(binalized, contourStorage, &contour, sizeof(CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE); // Find largest contour double max_area = 0.0; while (contour) { double area = fabs(cvContourArea(contour)); if ( area > max_area) { maxContour = contour; max_area = area; } contour = contour->h_next; } // Object detected if (maxContour) { // Draw a contour cvZero(binalized); cvDrawContours(binalized, maxContour, cvScalarAll(255), cvScalarAll(255), 0, CV_FILLED); // Calculate the moments CvMoments moments; cvMoments(binalized, &moments, 1); int my = (int)(moments.m01/moments.m00); int mx = (int)(moments.m10/moments.m00); // Measurements float m[] = {mx, my}; CvMat measurement = cvMat(2, 1, CV_32FC1, m); // Correct phase const CvMat *correction = cvKalmanCorrect(kalman, &measurement); } // Prediction phase const CvMat *prediction = cvKalmanPredict(kalman); // Display the image cvCircle(image, cvPointFrom32f(cvPoint2D32f(prediction->data.fl[0], prediction->data.fl[1])), 10, CV_RGB(0,255,0)); cvShowImage("camera", image); // Release the memories cvReleaseImage(&hsv); cvReleaseImage(&binalized); cvReleaseMemStorage(&contourStorage); } // Release the kalman filter cvReleaseKalman(&kalman); // See you ardrone.close(); return 0; }
static void filterPlane(IplImage * ap_depth, std::vector<IplImage *> & a_masks, std::vector<CvPoint> & a_chain, double f) { const int l_num_cost_pts = 200; float l_thres = 4; IplImage * lp_mask = cvCreateImage(cvGetSize(ap_depth), IPL_DEPTH_8U, 1); cvSet(lp_mask, cvRealScalar(0)); std::vector<CvPoint> l_chain_vector; float l_chain_length = 0; float * lp_seg_length = new float[a_chain.size()]; for (int l_i = 0; l_i < (int)a_chain.size(); ++l_i) { float x_diff = (float)(a_chain[(l_i + 1) % a_chain.size()].x - a_chain[l_i].x); float y_diff = (float)(a_chain[(l_i + 1) % a_chain.size()].y - a_chain[l_i].y); lp_seg_length[l_i] = sqrt(x_diff*x_diff + y_diff*y_diff); l_chain_length += lp_seg_length[l_i]; } for (int l_i = 0; l_i < (int)a_chain.size(); ++l_i) { if (lp_seg_length[l_i] > 0) { int l_cur_num = cvRound(l_num_cost_pts * lp_seg_length[l_i] / l_chain_length); float l_cur_len = lp_seg_length[l_i] / l_cur_num; for (int l_j = 0; l_j < l_cur_num; ++l_j) { float l_ratio = (l_cur_len * l_j / lp_seg_length[l_i]); CvPoint l_pts; l_pts.x = cvRound(l_ratio * (a_chain[(l_i + 1) % a_chain.size()].x - a_chain[l_i].x) + a_chain[l_i].x); l_pts.y = cvRound(l_ratio * (a_chain[(l_i + 1) % a_chain.size()].y - a_chain[l_i].y) + a_chain[l_i].y); l_chain_vector.push_back(l_pts); } } } std::vector<cv::Point3d> lp_src_3Dpts(l_chain_vector.size()); for (int l_i = 0; l_i < (int)l_chain_vector.size(); ++l_i) { lp_src_3Dpts[l_i].x = l_chain_vector[l_i].x; lp_src_3Dpts[l_i].y = l_chain_vector[l_i].y; lp_src_3Dpts[l_i].z = CV_IMAGE_ELEM(ap_depth, unsigned short, cvRound(lp_src_3Dpts[l_i].y), cvRound(lp_src_3Dpts[l_i].x)); //CV_IMAGE_ELEM(lp_mask,unsigned char,(int)lp_src_3Dpts[l_i].Y,(int)lp_src_3Dpts[l_i].X)=255; } //cv_show_image(lp_mask,"hallo2"); //cv::imshow("hallo2",lp_mask); reprojectPoints(lp_src_3Dpts, lp_src_3Dpts, f); CvMat * lp_pts = cvCreateMat((int)l_chain_vector.size(), 4, CV_32F); CvMat * lp_v = cvCreateMat(4, 4, CV_32F); CvMat * lp_w = cvCreateMat(4, 1, CV_32F); for (int l_i = 0; l_i < (int)l_chain_vector.size(); ++l_i) { CV_MAT_ELEM(*lp_pts, float, l_i, 0) = (float)lp_src_3Dpts[l_i].x; CV_MAT_ELEM(*lp_pts, float, l_i, 1) = (float)lp_src_3Dpts[l_i].y; CV_MAT_ELEM(*lp_pts, float, l_i, 2) = (float)lp_src_3Dpts[l_i].z; CV_MAT_ELEM(*lp_pts, float, l_i, 3) = 1.0f; } cvSVD(lp_pts, lp_w, 0, lp_v); float l_n[4] = {CV_MAT_ELEM(*lp_v, float, 0, 3), CV_MAT_ELEM(*lp_v, float, 1, 3), CV_MAT_ELEM(*lp_v, float, 2, 3), CV_MAT_ELEM(*lp_v, float, 3, 3)};
void VisualTracker::initTracker(Dims imageDims) { #ifdef HAVE_OPENCV itsMaxNumPoints = 1; itsCurrentNumPoints = 0; itsCurrentPoints = (CvPoint2D32f*)cvAlloc(itsMaxNumPoints*sizeof(itsCurrentPoints)); itsPreviousPoints = (CvPoint2D32f*)cvAlloc(itsMaxNumPoints*sizeof(itsPreviousPoints)); itsPreviousGreyImg = Image<byte>(imageDims, ZEROS); itsCurrentPyramid = cvCreateImage( cvSize(imageDims.w(), imageDims.h()), 8, 1 ); itsPreviousPyramid = cvCreateImage( cvSize(imageDims.w(), imageDims.h()), 8, 1 ); itsTrackStatus = (char*)cvAlloc(itsMaxNumPoints); itsTrackError = (float*)cvAlloc(itsMaxNumPoints); if (itsUseKalman) { itsKalman = cvCreateKalman(4, //Dim of state vector x,y,dx,dy 2); //dim of mesurment vector x,y //State transition matrix //x y dx dy const float A[] = { 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1}; //Observation matrix const float H[] = { 1, 0, 0, 0, 0, 1, 0, 0}; //set the transition and mesurment matrix memcpy( itsKalman->transition_matrix->data.fl, A, sizeof(A)); memcpy( itsKalman->measurement_matrix->data.fl, H, sizeof(H)); //Set the process and measurment noise cvSetIdentity( itsKalman->process_noise_cov, cvRealScalar(1e-5) ); cvSetIdentity( itsKalman->measurement_noise_cov, cvRealScalar(1e-1) ); /*posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k) */ cvSetIdentity( itsKalman->error_cov_post, cvRealScalar(1)); cvZero(itsKalman->state_post); /* corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) */ cvZero(itsKalman->state_pre); /* predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k) */ } // //camshift // int hdims = 16; // float hranges_arr[] = {0,180}; // float* hranges = hranges_arr; // // itsObjectHist = cvCreateHist( 1, &hdims, CV_HIST_ARRAY, &hranges, 1 ); // itsBackproject = cvCreateImage( cvSize(imageDims.w(), imageDims.h()), 8, 1 ); itsTrackFlags = 0; #endif itsInitTracker = false; }