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;
}
