void Panoramic::ShowStitch(IplImage *profile,IplImage *center)
{
double profileFaceDate = 0.;
double Alpha = 0.5;//allocate middle of stich image a half of center and lateral image
double Blendling = 0.;
for(int j = 0;j < m_height;j++)
{
for(int i = 0;i < m_width;i++)
{
double linePosition = m_slope*i+m_intercept-j;
profileFaceDate = cvGetReal2D(profile,j,i);
if(linePosition <= -m_lineT)
{
cvSetReal2D(m_PanoramicFace,j,i,profileFaceDate);
}
else if(linePosition > -m_lineT && linePosition < m_lineT)
{
double getAlpha = LinearBlending(m_lineT,m_slope*i+m_intercept-j,Alpha);
double getBeta = 1-getAlpha;
Blendling = getAlpha*cvGetReal2D(center,j,i) + getBeta*profileFaceDate;
if(Blendling > 255) Blendling = 255;
else if(Blendling < 0)Blendling = 0;
cvSetReal2D(m_PanoramicFace,j,i,Blendling) ;
}
}
}
}
void findExtrinsic(std::vector<CvPoint2D32f>& corners, int rows, int cols)
{
CvMat *image_points_ex = cvCreateMat( corners.size(), 2, CV_64FC1);
for (uint j = 0; j < corners.size(); j++){
cvSetReal2D( image_points_ex, j, 0, corners[j].x);
cvSetReal2D( image_points_ex, j, 1, corners[j].y);
}
//int views = 1;
CvMat *object_points_ex = cvCreateMat( corners.size(), 3, CV_64FC1);
for (uint j = 0; j < corners.size(); j++){
cvSetReal2D( object_points_ex, j, 0, ( j % rows) * GRID_SIZE );
cvSetReal2D( object_points_ex, j, 1, ( j / rows) * GRID_SIZE );
cvSetReal2D( object_points_ex, j, 2, 0.0 );
}
//cvSetReal2D( itsCameraTranslation, 0, 2, 782.319961 );
cvFindExtrinsicCameraParams2( object_points_ex,
image_points_ex,
itsIntrinsicMatrix,
itsDistortionCoeffs,
itsCameraRotation,
itsCameraTranslation);
cvReleaseMat( &image_points_ex);
cvReleaseMat( &object_points_ex);
}
/** @Specifically designed for this program
*so it is include m_lineT to reduce unnecessary operation*/
void Panoramic::AffineTransform(IplImage *src,IplImage *dst,CvMat *Matrix)
{
for(int j = 0; j < m_height;j++)
{
for(int i = 0 ; i < m_width;i++)
{
double linePosition = m_slope*i + m_intercept-j;
if(linePosition <= m_lineT ) //
{
int x = i*cvGetReal2D(Matrix,0,0) + j*cvGetReal2D(Matrix,0,1) + cvGetReal2D(Matrix,0,2)*1.0;
int y = i*cvGetReal2D(Matrix,1,0) + j*cvGetReal2D(Matrix,1,1) + cvGetReal2D(Matrix,1,2)*1.0;
if(x > 0 && y > 0 && x < m_width && y < m_height)
{
double pixel = cvGetReal2D(src,y,x);
cvSetReal2D(dst,j,i,pixel) ;
}
else
{
cvSetReal2D(dst,j,i,0) ;
}
}
}
}
}
void mitk::UndistortCameraImage::SetUndistortImageFastInfo(float in_dF1, float in_dF2,
float in_dPrincipalX, float in_dPrincipalY,
float in_Dist[4], float ImageSizeX, float ImageSizeY)
{
//create new matrix
m_DistortionCoeffs = cvCreateMat(4, 1, CV_64FC1);
m_CameraMatrix = cvCreateMat(3, 3, CV_64FC1);
//set the camera matrix [fx 0 cx; 0 fy cy; 0 0 1].
cvSetReal2D(m_CameraMatrix, 0, 0, in_dF1);
cvSetReal2D(m_CameraMatrix, 0, 1, 0.0);
cvSetReal2D(m_CameraMatrix, 0, 2, in_dPrincipalX);
cvSetReal2D(m_CameraMatrix, 1, 0, 0.0);
cvSetReal2D(m_CameraMatrix, 1, 1, in_dF2);
cvSetReal2D(m_CameraMatrix, 1, 2, in_dPrincipalY);
cvSetReal2D(m_CameraMatrix, 2, 0, 0.0);
cvSetReal2D(m_CameraMatrix, 2, 1, 0.0);
cvSetReal2D(m_CameraMatrix, 2, 2, 1.0);
//set distortions coefficients
cvSetReal1D(m_DistortionCoeffs, 0, in_Dist[0]);
cvSetReal1D(m_DistortionCoeffs, 1, in_Dist[1]);
cvSetReal1D(m_DistortionCoeffs, 2, in_Dist[2]);
cvSetReal1D(m_DistortionCoeffs, 3, in_Dist[3]);
m_mapX = cvCreateMat(ImageSizeY, ImageSizeX, CV_32FC1);
m_mapY = cvCreateMat(ImageSizeY, ImageSizeX, CV_32FC1);
//cv::initUndistortRectifyMap(m_CameraMatrix, m_DistortionCoeffs, m_mapX, m_mapY);
cvInitUndistortMap(m_CameraMatrix, m_DistortionCoeffs, m_mapX, m_mapY);
}
/* This is the measurement mapping used by the UKF, i.e.
* z(t) = h(x(t), n(t))
*
* In this case, z is a vector with (x,y) position and heading and
* noise is additive. */
void beluga_measurement(const CvMat* x_k,
const CvMat* n_k,
CvMat* z_k)
{
unsigned int nrows = z_k->rows;
unsigned int nmeas = (nrows-1)/3;
if( (3*nmeas) != (nrows-1))
{
fprintf(stderr, "beluga_measurement error: "
"Number of rows in z is incorrect. Must be 3*n+1.\n");
return;
}
/* measurement should be (x, y, theta) repeated nmeas times then z */
for(unsigned int i = 0; i < nmeas; i++)
{
double x = cvGetReal2D(x_k, BELUGA_STATE_X, 0);
double y = cvGetReal2D(x_k, BELUGA_STATE_Y, 0);
double theta = cvGetReal2D(x_k, BELUGA_STATE_THETA, 0);
double x_noise = 0;
double y_noise = 0;
double theta_noise = 0;
if(n_k)
{
x_noise = cvGetReal2D(n_k, (BELUGA_NUM_MEAS-1)*i + BELUGA_MEAS_X, 0);
y_noise = cvGetReal2D(n_k, (BELUGA_NUM_MEAS-1)*i + BELUGA_MEAS_Y, 0);
theta_noise = cvGetReal2D(n_k, (BELUGA_NUM_MEAS-1)*i + BELUGA_MEAS_THETA, 0);
}
x += x_noise;
y += y_noise;
theta += theta_noise;
cvSetReal2D(z_k, (BELUGA_NUM_MEAS-1)*i+BELUGA_MEAS_X, 0, x);
cvSetReal2D(z_k, (BELUGA_NUM_MEAS-1)*i+BELUGA_MEAS_Y, 0, y);
cvSetReal2D(z_k, (BELUGA_NUM_MEAS-1)*i+BELUGA_MEAS_THETA, 0, theta);
}
double z = cvGetReal2D(x_k, BELUGA_STATE_Z, 0);
double z_noise = 0;
if(n_k)
{
/* z noise will be the last element */
z_noise = cvGetReal2D(n_k, n_k->rows - 1, 0);
}
z += z_noise;
/* NOTE: Specifically not constraining z here, b/c the constraint
* induces an artificial disturbance near the boundaries. The constraint
* is applied afterwords. */
cvSetReal2D(z_k, nrows-1, 0, z);
}
/*
* ofxNMPTFaceTracker
*
* @param width int Input frame width
* @param height int Input frame height
* @param numScales int Number of Patches in the Image Patch Pyramid.
* @param gridXSize SubImage size x
* @param gridYSize SubImage size y
* @param haarDetectorXMLFile string The Haar detector xml file
*
*/
ofxNMPTFaceTracker::ofxNMPTFaceTracker(int width, int height, int numScales, int gridXSize, int gridYSize, string haarDetectorXMLFile) {
int scaleGridCells[numScales];
int ngrid_pts = 1;
for (int i = 0; i < numScales; i++) {
int inc = ngrid_pts+2;
int halfwidth = (numScales - i - 1)*inc + ngrid_pts;
scaleGridCells[i] = halfwidth*2+1; //,9,15,21
}
int numGridPoints = scaleGridCells[0];
// The size of the images that will be given to the IPP to turn into MIPOMDP observations.
// This allocates memory for underlying data, but it can be changed easily later if needed
// without recreating the object by calling the changeInputSize() functions.
CvSize inputImageSize = cvSize(width, height);
// The common size to which all image patches will be reduced, creating the foveation effect.
// The smaller subImageSize is, the faster search is, and the more extreme the effect of foveation.
// This allocates memory for underlying data, but it can be changed easily later if needed without
// recreating the object by calling the changeInputSize() functions.
CvSize subImageSize = cvSize(gridXSize, gridYSize);
// Size of the discretization of the image. The number of POMDP states is the product of the
// demensions of this size (e.g. 21x21).
CvSize gridSize = cvSize(numGridPoints, numGridPoints);
// A matrix that describes the size and shape of each level (patch) of the IP Pyramid.
// This must be a matrix with size [numSubImages x 2]. Each row contains the width and height
// of the corresponding levels. These should be in order of *decreasing* size, so that the largest
// Image Patch is first. For example, in Butko and Movellan CVPR 2009, we used [21 21; 15 15; 9 9; 3 3].
// Finaly, note that it is not necessary that the largest patch cover the entire image. However, when the
// largest patch is the same size as grid-cell-matrix, special optimizations become available that reduce
// the complexity of the algorithm when the same image, or same frame of video, is fixated multiple times.
CvMat* subImageGridPoints = cvCreateMat(numScales, 2, CV_32SC1);
for (int i = 0; i < numScales; i++) {
cvSetReal2D(subImageGridPoints, i, 0, scaleGridCells[i]);
cvSetReal2D(subImageGridPoints, i, 1, scaleGridCells[i]);
}
// Instatiate MOPOMDP as tracker
faceTracker = new MIPOMDP(
cvSize(width, height),
subImageSize,
gridSize,
numScales,
subImageGridPoints,
ofToDataPath(haarDetectorXMLFile).c_str()
);
}
static void dance_measurement(const CvMat* x_k,
const CvMat* n_k,
CvMat* z_k)
{
cvSetReal2D(z_k, 0, 0, cvGetReal2D(x_k, 0, 0));
cvSetReal2D(z_k, 1, 0, cvGetReal2D(x_k, 1, 0));
/* as above, skip this step when n_k is null */
if(n_k)
{
cvAdd(z_k, n_k, z_k);
}
}
void GaborImage::CalculateKernel(int Orientation, int Frequency) {
Init();
float real, img;
for (int x = -(GaborWidth - 1) / 2; x < (GaborWidth - 1) / 2 + 1; x++)
for (int y = -(GaborHeight - 1) / 2; y < (GaborHeight - 1) / 2 + 1; y++)
{
real = KernelRealPart(x, y, Orientation, Frequency);
img = KernelImgPart(x, y, Orientation, Frequency);
cvSetReal2D(KernelRealData, x + (GaborWidth - 1) / 2, y + (GaborHeight - 1) / 2, real);
cvSetReal2D(KernelImgData, x + (GaborWidth - 1) / 2, y + (GaborHeight - 1) / 2, img);
}
}
void adjustRMatrixAndZForMeasurementSize(CvMat*& R, CvMat*& z, unsigned int nmeas)
{
if(nmeas == 0)
{
return;
}
unsigned int nrows_prev = (R)->rows;
unsigned int nrows_z_prev = (z)->rows;
if(nrows_prev < 4)
{
fprintf(stderr, "adjustRMatrixForMeasurementSize Error: Existing R is too small.\n");
return;
}
/* x, y, th each meas + z */
unsigned int nrows_now = 3*nmeas + 1;
/* same number of measurements -> do nothing */
if(nrows_now == nrows_prev && nrows_now == nrows_z_prev)
{
return;
}
// printf("adjR a %d %d\n", nrows_prev, nrows_now);
double sx = cvGetReal2D(R, 0, 0);
double sy = cvGetReal2D(R, 1, 1);
double sth = cvGetReal2D(R, 2, 2);
double sz = cvGetReal2D(R, nrows_prev-1, nrows_prev-1);
// printf("adjR b\n");
cvReleaseMat(&R);
cvReleaseMat(&z);
// printf("adjR c\n");
R = cvCreateMat(nrows_now, nrows_now, CV_64FC1);
z = cvCreateMat(nrows_now, 1, CV_64FC1);
// printf("adjR d\n");
cvZero(R);
cvZero(z);
// printf("adjR e\n");
for(unsigned int i = 0; i < nmeas; i++)
{
cvSetReal2D(R, i*3 + 0, i*3 + 0, sx);
cvSetReal2D(R, i*3 + 1, i*3 + 1, sy);
cvSetReal2D(R, i*3 + 2, i*3 + 2, sth);
}
// printf("adjR f\n");
cvSetReal2D(R, nrows_now-1, nrows_now-1, sz);
}
void radial_sample(int width, int height, char* data, IplImage *unwrapped, int slice)
{
IplImage *cvcast = cvCreateImageHeader(cvSize(width, height),
IPL_DEPTH_8U, 1);
cvcast->imageData = data;
// cvSaveImage("slice.png",cvcast);
CvPoint center = cvPoint(cx,cy);
unsigned char* linebuf;
for(int sample = 0; sample < RADIAL_SAMPLES; sample++) {
float theta = ((float)sample)*((2.0*PI)/(float)RADIAL_SAMPLES);
CvPoint outer = calc_ray_outer(theta, center);
// printf("%g:\t%d,%d\n", theta*(180.0/PI), outer.x, outer.y);
cvClipLine(cvSize(width, height), &outer, ¢er);
int linesize = abs(center.x-outer.x)+abs(center.y-outer.y)+1;
linebuf = (unsigned char*)malloc(linesize);
cvSampleLine(cvcast,outer,center,linebuf,4);
IplImage *castline = cvCreateImageHeader(cvSize(linesize,1), IPL_DEPTH_8U, 1);
castline->imageData = (char*)linebuf;
IplImage *sobel = cvCreateImage(cvSize(linesize,1), IPL_DEPTH_8U, 1);
cvSobel(castline, sobel, 1, 0, 3);
int layer = 0;
for(int i = 0; (i < linesize) && (layer < MAX_LAYERS); i++) {
// printf(" %d,", (int)cvGetReal1D(sobel,i));
if((int)cvGetReal1D(sobel,i) > SOBEL_THRESH) {
int max = 0, max_i = 0;
for(; i < linesize; i++) {
int curval = (int)cvGetReal1D(sobel,i);
if(curval == 0) break;
if(curval > max) {
max = curval;
max_i = i;
}
}
cvSetReal2D(unwrapped,slice,(layer*RADIAL_SAMPLES)+sample,cvGetReal1D(castline,max_i));
// printf("%d\t",max);
layer++;
}
}
// printf("\n");
/*
char filename[] = "line000.png";
sprintf(filename,"line%03d.png",(int)(theta*(180.0/PI)));
cvSaveImage(filename,sobel);
*/
cvReleaseImageHeader(&castline);
cvReleaseImage(&sobel);
free(linebuf);
}
}
void GS_rotate( IplImage* srcImg, IplImage *srcImgRotated, int angle )
{
int i, j;
double new_x, new_y, var;
int height = srcImg->height;
int width = srcImg->width;
double radius = -angle*(M_PI/180.0);
double sin_value = sin(radius);
double cos_value = cos(radius);
int centerX = height/2;
int centerY = width/2;
for(i=0; i<height; i++)
{
for(j=0; j<width; j++)
{
new_x = (i-centerX)*cos_value - (j-centerY)*sin_value + centerX;
new_y = (i-centerX)*sin_value + (j-centerY)*cos_value + centerY;
if(new_x < 0 || new_x > height)
{
var = 0.0;
} else if(new_y < 0 || new_y > width) {
var = 0.0;
} else {
var = cvGetReal2D( srcImg, (int)new_x, (int)new_y );
}
cvSetReal2D( srcImgRotated, i, j, var );
}
}
}
// Histogram Contrast Stretching
void Filterling ::contrastStretching (IplImage *srcImage, IplImage *dstImage) {
// histogram연산을 위해 사용할 배열메모리를 할당
unsigned int *histogramArray = new unsigned int[256],
*sumHistogramArray = new unsigned int[256] ;
memset (histogramArray, 0, sizeof (unsigned int) * 256) ;
memset (sumHistogramArray, 0, sizeof (unsigned int) * 256) ;
// 영상의 histogram을 계산
for (size_t i = 0 ; i < srcImage ->height ; i++)
for (size_t j = 0 ; j < srcImage ->width ; j++)
histogramArray[(unsigned int)cvGetReal2D (srcImage, i, j)]++ ;
// histogram의 정규화된 합을 계산
unsigned int sum = 0 ;
const float SCALE_FACTOR = 128.0f / (float)(srcImage ->height * srcImage ->width) ;
for (size_t i = 0 ; i < 256 ; i++) {
sum += histogramArray[i] ;
sumHistogramArray[i] = (unsigned int)((sum * SCALE_FACTOR) + 0.5) ;
}
// LUT로써 sumHistogramArray 배열을 사용하여 영상을 변환
for (size_t i = 0 ; i < srcImage ->height ; i++)
for (size_t j = 0 ; j < srcImage ->width ; j++)
// pixel indexing
cvSetReal2D (dstImage, i, j, (double) sumHistogramArray[(unsigned int)cvGetReal2D (srcImage, i, j)]) ;
delete [] (histogramArray) ;
delete [] (sumHistogramArray) ;
}
int main(int argc, const char * argv[]) {
CvMat *cmatrix = cvCreateMat(5,5,CV_32FC1);
float element_3_2 = 7.7;
*((float*)CV_MAT_ELEM_PTR( *cmatrix, 3,2) ) = element_3_2;
cvmSet(cmatrix,4,4,0.5000);
cvSetReal2D(cmatrix,3,3,0.5000);
CvFileStorage* fs1 = cvOpenFileStorage("cfg.xml", 0, CV_STORAGE_WRITE);
cvWriteInt( fs1, "frame_count", 10 );
cvStartWriteStruct( fs1, "frame_size", CV_NODE_SEQ );
cvWriteInt( fs1 , 0, 320 );
cvWriteInt( fs1 , 0, 200 );
cvEndWriteStruct( fs1 );
cvWrite( fs1, "color_cvt_matrix", cmatrix );
cvReleaseFileStorage( &fs1 );
CvFileStorage* fs2 = cvOpenFileStorage("cfg.xml", 0, CV_STORAGE_READ);
int frame_count = cvReadIntByName( fs2 , 0, "frame_count");
CvSeq* s = cvGetFileNodeByName( fs2,0,"frame_size" )->data.seq;
int frame_width = cvReadInt( (CvFileNode*)cvGetSeqElem(s,0) );
int frame_height = cvReadInt( (CvFileNode*)cvGetSeqElem(s,1) );
CvMat* color_cvt_matrix = (CvMat*) cvReadByName( fs2, 0 , "color_cvt_matrix");
printf("color_cvt_matrix: width=%d, height=%d\n",color_cvt_matrix->width, color_cvt_matrix->height );
printf("frame_count=%d, frame_width=%d, frame_height=%d\n",frame_count,frame_width,frame_height);
cvReleaseFileStorage( &fs2 );
return 0;
}
void CueTemplate::drawgaussianblob(const CvPoint& pos, const CvSize& size) {
float temp = 1.0;
temp = (mp_boundedoutputimg->width) / 160.0;
float K = 2.0 * size.width * temp; // smaller K, smaller circle
int windowSizeX = cvRound(size.width * temp);
int windowSizeY = cvRound(size.height * temp);
int posx = pos.x;
int posy = pos.y;
int width = mp_boundedoutputimg->width;
int height = mp_boundedoutputimg->height;
cvSetZero(mp_boundedoutputimg);
for(int x = -windowSizeX; x <= windowSizeX; ++x) {
for(int y = -windowSizeY; y <= windowSizeY; ++y) {
int tempx = posx + x;
int tempy = posy + y;
if(tempx >= 0 && tempy >= 0 && tempx < width && tempy < height) {
double val = exp(-0.5/K*((float)x*(float)x + (float)y*(float)y));
cvSetReal2D(mp_boundedoutputimg, tempy, tempx, val);
}
}
}
}
void ClassifierMLP::Train(std::vector< fvec > samples, ivec labels)
{
u32 sampleCnt = samples.size();
if(!sampleCnt) return;
DEL(mlp);
dim = samples[0].size();
CvMat *layers;
// if(neuronCount == 3) neuronCount = 2; // don't ask me why but 3 neurons mess up everything...
if(!layerCount || neuronCount < 2)
{
layers = cvCreateMat(2,1,CV_32SC1);
cvSet1D(layers, 0, cvScalar(dim));
cvSet1D(layers, 1, cvScalar(1));
}
else
{
layers = cvCreateMat(2+layerCount,1,CV_32SC1);
cvSet1D(layers, 0, cvScalar(dim));
cvSet1D(layers, layerCount+1, cvScalar(1));
FOR(i, layerCount) cvSet1D(layers, i+1, cvScalar(neuronCount));
}
u32 *perm = randPerm(sampleCnt);
CvMat *trainSamples = cvCreateMat(sampleCnt, dim, CV_32FC1);
CvMat *trainLabels = cvCreateMat(labels.size(), 1, CV_32FC1);
CvMat *sampleWeights = cvCreateMat(samples.size(), 1, CV_32FC1);
FOR(i, sampleCnt)
{
FOR(d, dim) cvSetReal2D(trainSamples, i, d, samples[perm[i]][d]);
cvSet1D(trainLabels, i, cvScalar(labels[perm[i]]));
cvSet1D(sampleWeights, i, cvScalar(1));
}
CvScalar myCvPointANNF(IplImage* img, CvPoint p, int size, double l_channel_weight) {
assert(size%2 == 1);
CvMat* mat = cvCreateMat(size, size, CV_32FC3);
CvMat* d = cvCreateMat(size, size, CV_32FC1);
// CvMat* w = cvCreateMat(size, size, CV_32FC1);
int rad = (size - 1)/2;
for (int r = -rad; r <= rad; r++) { // y-axis
CvPoint center = cvPoint(p.x, p.y + r);
int y = rad + r;
for (int rr = -rad; rr <= rad; rr++) { // x-axis
CvPoint this = myCvHandleEdgePoint(
cvPoint(center.x + rr, center.y),
img->width,
img->height
);
int x = rad + r;
cvSet2D(mat, x, y, cvGet2D(img, this.y, this.x)); // inversed xy
}
}
double max_d = .0;
double sum = .0;
for (int i = 0; i < size; i++) {
for (int j = 0; j < size; j++) {
double dd = .0;
// compute d(i, j)
for (int k = 0; k < size; k++) {
for (int l = 0; l < size; l++) {
if (!(k == i && l == j)) {
dd += euDist(cvGet2D(mat, i, j), cvGet2D(mat, k, l), l_channel_weight);
}
}
}
if (dd > max_d)
max_d = dd;
sum += dd;
cvSetReal2D(d, i, j, dd);
}
}
double denom = max_d*size*size - sum;
CvScalar re = cvScalar(0, 0, 0, 0);
for (int i = 0; i < size; i++) {
for (int j = 0; j < size; j++) {
// compute weight
double ww = (max_d - cvGetReal2D(d, i, j))/denom;
for (int k = 0; k < 4; k++) {
re.val[k] += ww*cvGet2D(mat, i, j).val[k];
}
}
}
cvReleaseMat(&mat);
cvReleaseMat(&d);
return re;
}
/*!
\fn CvGabor::creat_kernel()
Create gabor kernel
Parameters:
None
Returns:
None
Create 2 gabor kernels - REAL and IMAG, with an orientation and a scale
*/
void CvGabor::creat_kernel()
{
if (IsInit() == false) {perror("Error: The Object has not been initilised in creat_kernel()!\n");}
else {
CvMat *mReal, *mImag;
mReal = cvCreateMat( Width, Width, CV_32FC1);
mImag = cvCreateMat( Width, Width, CV_32FC1);
/**************************** Gabor Function ****************************/
int x, y;
double dReal;
double dImag;
double dTemp1, dTemp2, dTemp3;
for (int i = 0; i < Width; i++)
{
for (int j = 0; j < Width; j++)
{
x = i-(Width-1)/2; // (width-1)/2 = anchor
y = j-(Width-1)/2;
dTemp1 = (pow(K,2)/pow(Sigma,2))*exp(-(pow((double)x,2)+pow((double)y,2))*pow(K,2)/(2*pow(Sigma,2)));
dTemp2 = cos(K*cos(Phi)*x + K*sin(Phi)*y) - exp(-(pow(Sigma,2)/2));
dTemp3 = sin(K*cos(Phi)*x + K*sin(Phi)*y);
dReal = dTemp1*dTemp2;
dImag = dTemp1*dTemp3;
//gan_mat_set_el(pmReal, i, j, dReal);
//cvmSet( (CvMat*)mReal, i, j, dReal );
cvSetReal2D((CvMat*)mReal, i, j, dReal );
//gan_mat_set_el(pmImag, i, j, dImag);
//cvmSet( (CvMat*)mImag, i, j, dImag );
cvSetReal2D((CvMat*)mImag, i, j, dImag );
}
}
/**************************** Gabor Function ****************************/
bKernel = true;
cvCopy(mReal, Real, NULL);
cvCopy(mImag, Imag, NULL);
//printf("A %d x %d Gabor kernel with %f PI in arc is created.\n", Width, Width, Phi/PI);
cvReleaseMat( &mReal );
cvReleaseMat( &mImag );
}
}
void ColorDistribution::FeedFrame(IplImage *image)
{
if(_fReadyToCompute)
return;
if(_nWidth == 0 && _nHeight == 0) {
// Initialize
_nWidth = image->width;
_nHeight = image->height;
}
// Sampling
for(int x = 0; x < _nWidth; x++) {
for(int y = 0; y < _nHeight; y++) {
CvScalar color = cvGet2D(image, y, x);
for(int c = 1; c <= 2; c++)
// Sample only Cr, Cb channels. (or G, R channels.)
_Sample[x][y][c-1][_nIndex] = color.val[c];
}
}
// Increase index
_nIndex++;
if(_nIndex == MAX_HISTORY) {
_nIndex = 0;
_fReadyToCompute = true;
}
// Compute mean and variance per-pixel
if(_fReadyToCompute) {
for(int x = 0; x < _nWidth; x++) {
for(int y = 0; y < _nHeight; y++) {
for(int c = 0; c < 2; c++) {
// Mean
_Mean[x][y][c] = 0;
for(int h = 0; h < MAX_HISTORY; h++)
_Mean[x][y][c] += _Sample[x][y][c][h];
_Mean[x][y][c] /= ((float)MAX_HISTORY);
// Variance
_Var[x][y][c] = 0;
for(int h = 0; h < MAX_HISTORY; h++)
_Var[x][y][c] += (pow(_Sample[x][y][c][h] - _Mean[x][y][c], 2));
_Var[x][y][c] /= ((float)MAX_HISTORY);
}
}
}
if(!_pMask)
_pMask = cvCreateImage(cvSize(_nWidth, _nHeight), 8, 1);
// threshold by variance
for(int x = 0; x < _nWidth; x++)
for(int y = 0; y < _nHeight; y++)
cvSetReal2D(_pMask, y, x, 255 * (_Var[x][y][0]+_Var[x][y][1]) / 10000.0);
}
}
// use k-means to reduce color number
MyQuantifiedImage* kmeansQuantification(IplImage* img, int tableSize) {
// step 1: transfer image to kmeans samples
int sample_count = img->height * img->width;
CvMat* samples = cvCreateMat(sample_count, 1, CV_32FC3);
CvRNG rng = cvRNG(0xffffffff);
int idx = 0;
for (int i = 0; i < img->height; i++) {
for (int j = 0; j < img->width; j++) {
cvSet1D(samples, idx++, cvGet2D(img, i, j));
}
}
// step 2: apply kmeans;
CvMat* labels = cvCreateMat(sample_count, 1, CV_32SC1);
CvMat* centers = cvCreateMat(tableSize, 1, CV_32FC3);
cvSetZero(labels);
cvSetZero(centers);
cvKMeans2(samples, tableSize, labels,
cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS,
10, CV_KMEANS_ACC),
CV_KMEANS_ATTEMPTS, &rng,
CV_KMEANS_PP_CENTERS, centers, 0); // flag = KMEANS_PP_CENTERS
// step 3: rebuild the image
IplImage* quantImg = cvCreateImage(cvGetSize(img), IPL_DEPTH_32F, 3);
CvMat* labelImg = cvCreateMat(img->height, img->width, CV_32SC1);
cvSetZero(quantImg);
cvSetZero(labelImg);
idx = 0;
for (int i = 0; i < img->height; i++) {
for (int j = 0; j < img->width; j++) {
int cluster_idx = labels->data.i[idx++];
CvScalar color = cvGet1D(centers, cluster_idx);
cvSet2D(quantImg, i, j, color);
cvSetReal2D(labelImg, i, j, (double) cluster_idx);
}
}
MyQuantifiedImage* re = malloc(sizeof(MyQuantifiedImage));
re->labelMat = labelImg;
re->qImg = quantImg;
re->tableSize = tableSize;
CvScalar* colorTable = calloc(tableSize, sizeof(CvScalar));
for (int i = 0; i < tableSize; i++) {
colorTable[i] = cvGet1D(centers, i);
}
re->colorTable = colorTable;
return re;
}
CvMat *
get_camera_matrix(stereo_util instance)
{
CvMat *matrix = cvCreateMat(3, 3, CV_32F);
cvSetReal2D(matrix, 0, 1, 0.0);
cvSetReal2D(matrix, 1, 0, 0.0);
cvSetReal2D(matrix, 2, 0, 0.0);
cvSetReal2D(matrix, 2, 1, 0.0);
cvSetReal2D(matrix, 0, 0, instance.fx);
cvSetReal2D(matrix, 1, 1, instance.fy);
cvSetReal2D(matrix, 0, 2, instance.xc);
cvSetReal2D(matrix, 1, 2, instance.yc);
cvSetReal2D(matrix, 2, 2, 1.0);
return matrix;
}
int main(int argc, char** argv)
{
CvMat *mat = cvCreateMat(5,5,CV_32FC1);
float element_3_2 = 7.7;
*((float*)CV_MAT_ELEM_PTR( *mat, 3,2) ) = element_3_2;
cvmSet(mat,4,4,0.5000);
cvSetReal2D(mat,3,3,0.5000);
float s = sum(mat);
printf("%f\n",s);
return 0;
}
void showfilter(double *filter,int width,int height){
IplImage *show=cvCreateImage(cvSize(width, height),8,1);
for(int i=0;i<width;i++)
for(int j=0;j<height;j++){
cvSetReal2D(show, j, i, filter[i*width+j]*255.0);
}
cvNamedWindow("Filter", 1);
cvShowImage("Filter", show);
cvWaitKey(0);
cvReleaseImage(&show);
}
PABOD_EXPORT float makeDetection (CvMat **results, IplImage *img, Model * model, float thresh, double iouNms)
{
CvMat *dets = NULL;
CvMat *boxes = NULL;
CvMatND *info = NULL;
if (thresh == POSITIVE_INF)
thresh = (float)model->getThresh();
bool found = imgDetect (img, model, thresh, NULL, NEGATIVE_INF, &dets, &boxes, &info);
int detected = 0;
if (found)
{
int *pick;
int pickDim;
nms (&pick, &pickDim, dets, iouNms);
(*results) = cvCreateMat (pickDim, 6, CV_32FC1);
for (int i = 0; i < pickDim; i++)
{
cvSetReal2D ((*results), i, 0, cvGetReal2D (dets, pick[i], 0));
cvSetReal2D ((*results), i, 1, cvGetReal2D (dets, pick[i], 1));
cvSetReal2D ((*results), i, 2, cvGetReal2D (dets, pick[i], 2));
cvSetReal2D ((*results), i, 3, cvGetReal2D (dets, pick[i], 3));
//cout << cvGetReal2D (dets, pick[i], 4) << "+ " << cvGetReal2D (dets, pick[i], 5) << " |";
cvSetReal2D ((*results), i, 4, cvGetReal2D (dets, pick[i], 5));
cvSetReal2D ((*results), i, 5, cvGetReal2D (dets, pick[i], 4));
}
detected = pickDim;
delete[] pick;
}
else
(*results) = NULL;
if (dets != NULL)
{
cvReleaseMat(&dets);
dets = NULL;
}
if (boxes != NULL)
{
cvReleaseMat(&boxes);
boxes = NULL;
}
if (info != NULL)
{
cvReleaseMatND(&info);
info = NULL;
}
return thresh;
}
//------------------------------------------------------------------------------
// Color Similarity Matrix Calculation
//------------------------------------------------------------------------------
CvMat *colorsim(int nbins, double sigma) {
CvMat *xc=cvCreateMat(1,nbins, CV_32FC1);
CvMat *yr=cvCreateMat(nbins,1, CV_32FC1);
CvMat *x=cvCreateMat(nbins,nbins, CV_32FC1);
CvMat *y=cvCreateMat(nbins,nbins, CV_32FC1);
CvMat *m=cvCreateMat(x->rows,x->rows, CV_32FC1);
// Set x,y directions
for (int j=0;j<nbins;j++) {
cvSetReal2D(xc,0,j,(j+1-0.5)/nbins);
cvSetReal2D(yr,j,0,(j+1-0.5)/nbins);
}
// Set u,v, meshgrids
for (int i=0;i<x->rows;i++) {
cvRepeat(xc,x);
cvRepeat(yr,y);
}
CvMat *sub = cvCreateMat(x->rows,y->cols,CV_32FC1);
cvSub(x,y,sub);
cvAbs(sub,sub);
cvMul(sub,sub,sub);
cvConvertScale(sub,sub,-1.0/(2*sigma*sigma));
cvExp(sub,sub);
cvSubRS(sub,cvScalar(1.0),m);
cvReleaseMat(&xc);
cvReleaseMat(&yr);
cvReleaseMat(&x);
cvReleaseMat(&y);
cvReleaseMat(&sub);
return m;
}
static void dance_dynamics(const CvMat* x_k,
const CvMat* u_k,
const CvMat* v_k,
CvMat* x_kplus1)
{
/* Assuming that the time-step is one frame rather than e.g.
* 33 msec - I take the actual time step into account in
* analysis. */
double dT = 1.0;
/* cvGetReal2D is useful to get an element of a 2D matrix
* cvGetReal2D(x_k, i, j) gets the (i,k)^th element
* Note that x_k is a 4x1 matrix here */
double x = cvGetReal2D(x_k, 0, 0);
double y = cvGetReal2D(x_k, 1, 0);
double vx = cvGetReal2D(x_k, 2, 0); /* heading [rad] */
double vy = cvGetReal2D(x_k, 3, 0); /* speed */
/* position(t + 1) = position(t) + dT*velocity */
x += dT*vx;
y += dT*vy;
/* works just like cvGetReal2D */
cvSetReal2D(x_kplus1, 0, 0, x);
cvSetReal2D(x_kplus1, 1, 0, y);
cvSetReal2D(x_kplus1, 2, 0, vx);
cvSetReal2D(x_kplus1, 3, 0, vy);
/* this allows v_k to be a NULL pointer, in which case
* this step is skipped */
if(v_k)
{
/* simple additive noise: x(t+1) <- x(t+1) + noise */
cvAdd(x_kplus1, v_k, x_kplus1);
}
}
/* Using this function below to constrain the state estimate. The
* constraint is applied after the UKF correction step.
*
* Constraints applied:
* 0 <= x <= frame (image) width
* 0 <= y <= frame height
* 0 <= speed <= 100 (px/frame)
* If x, y, heading, or speed are NaN, then
* a) if the corresponding estimate is valid, that number is used
* b) if the estimate is also NaN, x, y, and heading are set to
* 0, speed is set to 0.1
*/
void constrain_state(CvMat* x_k,
CvMat* X_p,
double tank_radius,
double water_depth)
{
double x = cvGetReal2D(x_k, BELUGA_STATE_X, 0);
double y = cvGetReal2D(x_k, BELUGA_STATE_Y, 0);
double z = cvGetReal2D(x_k, BELUGA_STATE_Z, 0);
double zdot = cvGetReal2D(x_k, BELUGA_STATE_ZDOT, 0);
double speed = cvGetReal2D(x_k, BELUGA_STATE_SPEED, 0);
double theta = cvGetReal2D(x_k, BELUGA_STATE_THETA, 0);
double omega = cvGetReal2D(x_k, BELUGA_STATE_OMEGA, 0);
double x_p = cvGetReal2D(X_p, BELUGA_STATE_X, 0);
double y_p = cvGetReal2D(X_p, BELUGA_STATE_Y, 0);
double z_p = cvGetReal2D(X_p, BELUGA_STATE_Z, 0);
double zdot_p = cvGetReal2D(X_p, BELUGA_STATE_ZDOT, 0);
double speed_p = cvGetReal2D(X_p, BELUGA_STATE_SPEED, 0);
double theta_p = cvGetReal2D(X_p, BELUGA_STATE_THETA, 0);
double omega_p = cvGetReal2D(X_p, BELUGA_STATE_OMEGA, 0);
x = check_nan(x, x_p, 0);
y = check_nan(y, y_p, 0);
z = check_nan(z, z_p, water_depth);
zdot = check_nan(zdot, zdot_p, 0);
speed = check_nan(speed, speed_p, 0.1);
theta = check_nan(theta, theta_p, 0);
omega = check_nan(omega, omega_p, 0);
if(x*x + y*y > tank_radius)
{
double phi = atan2(y, x);
x = 0.95*tank_radius*cos(phi);
y = 0.95*tank_radius*sin(phi);
}
z = MT_CLAMP(z, 0, water_depth);
zdot = MT_CLAMP(zdot, -BELUGA_CONSTRAINT_MAX_VERTICAL_SPEED,
BELUGA_CONSTRAINT_MAX_VERTICAL_SPEED);
speed = MT_CLAMP(speed, 0, BELUGA_CONSTRAINT_MAX_SPEED);
omega = MT_CLAMP(omega, -BELUGA_CONSTRAINT_MAX_TURN_RATE,
BELUGA_CONSTRAINT_MAX_TURN_RATE);
cvSetReal2D(x_k, BELUGA_STATE_X, 0, x);
cvSetReal2D(x_k, BELUGA_STATE_Y, 0, y);
cvSetReal2D(x_k, BELUGA_STATE_Z, 0, z);
cvSetReal2D(x_k, BELUGA_STATE_ZDOT, 0, zdot);
cvSetReal2D(x_k, BELUGA_STATE_SPEED, 0, speed);
cvSetReal2D(x_k, BELUGA_STATE_THETA, 0, theta);
cvSetReal2D(x_k, BELUGA_STATE_OMEGA, 0, omega);
}
void FrequencyFilter::ChangePosition(IplImage *pImage)
{
int x, y;
float fValue;
for(y = 0; y < pImage->height; y++)
for(x = 0; x < pImage->width; x++)
{
fValue = (float)cvGetReal2D(pImage, y, x);
if((x+y)%2 == 1)
fValue = - fValue;
cvSetReal2D(pImage, y, x, fValue);
}
}
void DblMatrix::SaveAsBMP(std::string Name)
{
double Min=DBL_MAX, Max=-DBL_MAX;
MinMax(Min, Max);
IplImage* Output = cvCreateImage(cvSize(GetWidth(), GetHeight()), IPL_DEPTH_8U, 1);
for(int j=0; j<GetHeight(); j++)
for(int i=0; i<GetWidth(); i++)
{
double u = (*this)[j][i];
double v = 255.0 * (u-Min)/(Max-Min);
cvSetReal2D(Output, j, i, v);
}
cvSaveImage(Name.c_str(), Output);
cvReleaseImage(&Output);
}
void CTWithWater::setCalibrationAndWaterDepth(const CalibrationData& calibrationData,
double water_depth)
{
calibrationDataToOpenCVCalibration(calibrationData,
m_CameraMatrix,
m_DistCoeffs,
m_Rv,
m_T,
m_R);
/* CameraWorld = -R^T T */
cvGEMM(m_R, m_T, -1.0, NULL, 0, m_CameraWorld, CV_GEMM_A_T);
cvSetIdentity(m_CameraMatrixNorm);
/* TODO: These should really be read in from the calibration */
cvSetReal2D(m_CameraMatrixNorm, 0, 0, 472.0);
cvSetReal2D(m_CameraMatrixNorm, 1, 1, 472.0);
cvSetReal2D(m_CameraMatrixNorm, 0, 2, 320.0);
cvSetReal2D(m_CameraMatrixNorm, 1, 2, 240.0);
cvZero(m_DistCoeffsNorm);
setWaterDepth(water_depth);
}
// This function actually applies the specified noise to the given image
// in the given amounts
IplImage* GenerateNoise(IplImage* img, int noiseType, float amount=255)
{
CvSize imgSize = cvGetSize(img);
IplImage* imgTemp = cvCloneImage(img); // This will hold the noisy image
// Go through each pixel
for(int y=0;y<imgSize.height;y++)
{
for(int x=0;x<imgSize.width;x++)
{
int randomValue=0; // Our noise is additive.. this holds
switch(noiseType) // the amount to add/subtract
{
case NOISE_UNIFORM: // I chose UNIFORM, so give me a uniform random number
randomValue = (char)(uniform()*amount);
break;
case NOISE_EXPONENTIAL: // I chose EXPONENTIAL... so exp random number please
randomValue = (int)(exponential()*amount);
break;
case NOISE_GAUSSIAN: // same here
randomValue = (int)(gaussian()*amount);
break;
case NOISE_RAYLEIGH: // ... guess!!
randomValue = (int)(rayleigh()*amount);
break;
case NOISE_GAMMA: // I chose gamma... give me a gamma random number
randomValue = (int)(gamma()*amount);
break;
case NOISE_IMPULSE: // I need salt and pepper.. pass the shakers please
randomValue = (int)(impulse((float)amount/256)*amount);
}
// Here we "apply" the noise to the current pixel
int pixelValue = cvGetReal2D(imgTemp, y, x)+randomValue;
// And set this value in our noisy image
cvSetReal2D(imgTemp, y, x, pixelValue);
}
}
// return
return imgTemp;
}