/**
* Save the current JPG into a file.
*
* @param filename The name of the file.
*
* @throw FileException If the image cannot be saved.
*/
void JPGImage::saveToFile(string filename) throw (artemis::FileException*){
CvScalar s;
IplImage *image = cvCreateImage(cvSize(getWidth(), getHeight()), IPL_DEPTH_8U, 0);
int div;
if ((getBitsPerPixel() > 8) && (getBitsPerPixel() <= 16))
div = 16;
else
div = 1;
for(int x = 0; x < getWidth(); x++){
for(int y = 0; y < getHeight(); y++){
s.val[0] = (getPixel(x, y).getGrayPixelValue()/div);
cvSet2D(image, y, x, s);
}
}
filename.append(".jpg");
if (!cvSaveImage(filename.c_str(), image)){
throw new artemis::FileException(0, "This file cannot been created!", filename);
}
}
IplImage* Zooming::SobelEnergy::GetEnergyImage(IplImage* input)
{
IplImage* dstX = cvCloneImage(input);
IplImage* dstY = cvCloneImage(input);
cvSobel(input, dstY, 0, _yOrder);
cvSobel(input, dstX, 1, _xOrder);
int width = input->width;
int height = input->height;
// average 3 channels to 1
IplImage* energy = cvCreateImage(cvSize(width, height), IPL_DEPTH_16U, 1);
//
//char* dstData = dstX->imageData;
//short* energyData = (short*)energy->imageData;
for(int i = 0; i < height; i++)
{
for(int j = 0; j < width; j++)
{
CvScalar inputPixelX = cvGet2D(dstX, i, j);
CvScalar inputPixelY = cvGet2D(dstY, i, j);
CvScalar outputX = cvScalar( abs(inputPixelX.val[0] + inputPixelX.val[1] + inputPixelX.val[2]) / 3);
CvScalar outputY = cvScalar( abs(inputPixelY.val[0] + inputPixelY.val[1] + inputPixelY.val[2]) / 3);
CvScalar output;
output.val[0] = outputX.val[0] + outputY.val[0];
cvSet2D(energy, i, j, output);
}
//dstData += dst->widthStep;
//energyData += energy->width;
}
return energy;
}
void hue_horiz_project(IplImage* src, IplImage* proj_image)
{
IplImage* hsv = cvCreateImage( cvGetSize(src), 8, 3 );
cvCvtColor( src, hsv, CV_BGR2HSV );
IplImage* h_plane = cvCreateImage( cvGetSize(src), 8, 1 );
IplImage* s_plane = cvCreateImage( cvGetSize(src), 8, 1 );
IplImage* v_plane = cvCreateImage( cvGetSize(src), 8, 1 );
IplImage* planes[] = { h_plane, s_plane };
cvCvtPixToPlane( hsv, h_plane, s_plane, v_plane, 0 );
int* v=new int[src->width];
memset(v,0,src->width*4);
int x,y;
CvScalar s,t;
for(x=0;x<src->width;x++)
{
for(y=0;y<src->height;y++)
{
s=cvGet2D(src,y,x);
if(s.val[0]==0)
v[x]++;
}
}
for(x=0;x<src->width;x++)
{
for(y=0;y<v[x];y++)
{
t.val[0]=0;
cvSet2D(proj_image,y,x,t);
}
}
}
//void DlIntegral(IplImage *source, CvMat *result) {
// for (int i = 0; i < 10; i++) {
// for (int j = 0; j < 10; j++) {
// printf("%.0f ",cvGet2D(source, i, j).val[0]);
// }
// printf("\n");
// }
// printf("///////////\n");
//
// cvSet2D(result, 0, 0, cvScalarAll(cvGet2D(source, 0, 0).val[0]));
//
// for (int x = 1; x < source->width; x++) {
// double sum = cvGet2D(source, x, 0).val[0] + cvGet2D(result, x-1, 0).val[0];
// cvSet2D(result, x, 0, cvScalarAll(sum));
// }
//
// for (int y = 1; y < 360/*img_hsv->height*/; y++) {
// double sum = cvGet2D(source, 0, y).val[0] + cvGet2D(result, 0, y-1).val[0];
// cvSet2D(result, 0, y, cvScalarAll(sum));
// }
//
// for (int y = 1; y <360/*img_hsv->height*/; y++) {
// for (int x = 1; x<source->width; x++) {
// double sum = cvGet2D(source, x, y).val[0] + cvGet2D(result, x-1, y).val[0] +
// cvGet2D(source, x, y-1).val[0] - cvGet2D(result, x-1, y-1).val[0];
// cvSet2D(result, x, y, cvScalarAll(sum));
// }
// }
//
// for (int i = 0; i < 10; i++) {
// for (int j = 0; j < 10; j++) {
// printf("%.0f ",cvGet2D(result, i, j).val[0]);
// }
// printf("\n");
// }
//
//}
void AdaptiveThreshold(IplImage *source, IplImage *result, int size) {
CvScalar pixel;
IplImage *img_gray = cvCreateImage(cvGetSize(source),IPL_DEPTH_8U,1);
cvCvtColor(source, img_gray, CV_RGB2GRAY);
for (int i = 0; i < img_gray->height; i++) {
for (int j = 0; j < img_gray->width; j++) {
int threshold = 0;
for (int n = i - size/2; n < i + size/2; n++) {
for (int m = j - size/2; m < j + size/2; m++) {
if (n>=0 && m>=0 && m < img_gray->width - size/2 && n < img_gray->height - size/2) {
pixel = cvGet2D(img_gray, n, m);
threshold += pixel.val[0];
}
}
}
threshold /= size*size;
pixel = cvGet2D(img_gray, i, j);
pixel.val[0] = pixel.val[0] > threshold ? 255 : 0;
cvSet2D(result, i, j, pixel);
}
}
}
void on_tracker(int thresh){
int mosaic_num = 0;
IplImage *pColor = cvCreateImage(cvSize(pImage->width, pImage->height), 8, 3);
cvCvtColor(pImage, pColor, CV_GRAY2BGR);
for (int i = 0; i < h16; i++){
for (int j = 0; j < w16; j++){
int n_edges = 0;
for (int k = 0; k < 4; k++){
if (mb[i*w16+j][k] >= thresh){
n_edges ++;
}
}
int top = i*16;
int left = j*16;
int bottom = top+15;
int right = left+15;
if (n_edges >= 2){
mosaic_num ++;
cvRectangle(pColor, cvPoint(left, top), cvPoint(left+15, top+15), CV_RGB(255, 0, 0), 1, CV_AA, 0);
}else{
cvSet2D(pColor, top, left, CV_RGB(255, 0, 0));
}
}
}
printf("mosaic number = %d\n" , mosaic_num);
fflush(stdin);
cvShowImage("image", pColor);
char filename[64];
sprintf(filename, "mosaic-%d-%d.jpg", thresh, mosaic_num);
cvSaveImage(filename, pColor);
cvReleaseImage(&pColor);
}
void CvFunctionPlot::PlotFunction(const std::vector<unsigned int>& FunctionData,const CvScalar& PlotColour,const CvRect& ROI,float ScaleFactor)
{
if (Canvas == 0) return; // No canvas
const unsigned int WidthFunction = (unsigned int) FunctionData.size();
if (WidthFunction == 0) return; // nothing to plot ...
CvSize SizeCanvas = cvGetSize(Canvas);
// Check location of ROI
if (ROI.x < 0) return; // invalid ROI
if ((ROI.x + ROI.width) >= SizeCanvas.width) return; // invalid ROI
if (ROI.width < 1) return; // invalid ROI
if (ROI.y < 0) return; // invalid ROI
if (ROI.height < 1) return; // invalid ROI
if ((ROI.y + ROI.height) >= SizeCanvas.height) return; // invalid ROI
// Scale-factor
unsigned int PlotWidth = WidthFunction;
if (PlotWidth > (unsigned int)ROI.width) PlotWidth = ROI.width;
// Draw the function
for (unsigned int i=0; i < PlotWidth;++i)
{
unsigned int Value = FunctionData[i];
unsigned int ValuePixels = cvRound(ScaleFactor*Value);
if (ValuePixels > (unsigned int)ROI.height) ValuePixels = ROI.height;
CvPoint PlotPoint;
PlotPoint.x = ROI.x + i;
PlotPoint.y = ROI.y + ROI.height - 1 - ValuePixels;
cvSet2D(Canvas,PlotPoint.y,PlotPoint.x,PlotColour);
}
}
int hist2sig(CvHistogram* hist1,CvHistogram* hist2,int h_bins, int s_bins) {
int numrows = h_bins*s_bins;
// Create matrices to store the signature in
CvMat* sig1 = cvCreateMat(numrows, 3, CV_32FC1);//1 count + 2 coords = 3
CvMat* sig2 = cvCreateMat(numrows, 3, CV_32FC1);//sigs are of type float
// Fill signatures for the two histograms
for (int h = 0; h < h_bins; h++) {
for (int s = 0; s < s_bins; s++) {
float bin_val = cvQueryHistValue_2D(hist1, h, s);
cvSet2D(sig1, h*s_bins + s, 0, cvScalar(bin_val));// bin value
cvSet2D(sig1, h*s_bins + s, 1, cvScalar(h));//Coord 1
cvSet2D(sig1, h*s_bins + s, 2, cvScalar(s));
bin_val = cvQueryHistValue_2D(hist2, h, s);
cvSet2D(sig2, h*s_bins + s, 0, cvScalar(bin_val));//bin value
cvSet2D(sig2, h*s_bins + s, 1, cvScalar(h));//Coord 1
cvSet2D(sig2, h*s_bins + s, 2, cvScalar(s));//Coord 2
}
}
return 0;
}
void display()
{
glClearColor(0.0, 0.0, 0.0, 0.0);
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
/* glPushMatrix();
glTranslatef(xavg,yavg,0);
glutSolidCube(200);
glPopMatrix();
/*
glBegin(GL_QUADS);
glVertex3f(xr,xb,0);
glVertex3f(xb,yb,0);
glVertex3f(xl,yl,0);
glVertex3f(xt,yt,0);
glEnd();
*/
///////////////////////////////////////////////////////////nishanthprakash20///////////////////////////////////////////////////
captured=cvQueryFrame(video1);
disp=cvCreateImage(cvGetSize(captured),IPL_DEPTH_8U,3);
eroded=cvCreateImage(cvGetSize(captured),IPL_DEPTH_8U,3);
dilated=cvCreateImage(cvGetSize(captured),IPL_DEPTH_8U,3);
// data=cvGet2D(captured,240,320);
// printf("%f,%f,%f\n",data.val[0],data.val[1],data.val[2]);
thresh1=150;
thresh2=100;
thresh3=100;
for(i=0;i<disp->height;i++)
for(j=0;j<disp->width;j++)
{
data=cvGet2D(captured,i,j);
if(data.val[1]>thresh1&&data.val[2]<thresh2&&data.val[0]<thresh3)
{
cvSet2D(disp,i,j,data);
}
}
cvErode(disp,eroded,NULL,1);
cvDilate(eroded,dilated,NULL,4);
for(i=0;i<disp->height;i++)
for(j=0;j<disp->width;j++)
{
data=cvGet2D(dilated,i,j);
if(data.val[1]>thresh1&&data.val[2]<thresh2&&data.val[0]<thresh3)
{ goto donetop;
}
}
donetop:
xt=j;
yt=i;
for(i=479;i>0;i--)
for(j=0;j<disp->width;j++)
{
data=cvGet2D(dilated,i,j);
if(data.val[1]>thresh1&&data.val[2]<thresh2&&data.val[0]<thresh3)
{ goto doneleft;
}
}
doneleft:
xb=j;
yb=i;
inclination=((float)atan((yt-yb)/(xt-xb))-(float)atan(10.0/21))*180/3.14;
if(inclination<0) inclination+=60;
printf("%f\n",inclination);
cvNamedWindow("Cap");
cvShowImage("Cap",dilated);
cvWaitKey(3);
//*/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
glColor3f(1.0, 1.0, 1.0);
glPushMatrix();
glTranslatef(0,0,-5);
glRotatef(inclination,0,0,1);
glScalef(100,100,100);
glColor3f(0.0f,0.0f,0.0f);
drawmodel_box();
glColor3f(1.0f,1.0f,1.0f);
drawmodel_box2();
glColor3f(1.0f,1.0f,1.0f);
drawmodel_box3();
glColor3f(1.0f,1.0f,1.0f);
drawmodel_box4();
glColor3f(0.2f,0.2f,1.0f);
drawmodel_box5(); //remove this
//glScalef(0.01,0.01,0.01);
//glTranslatef(0,0,5);
glPopMatrix();
glutSwapBuffers();
}
void CreateMask(IplImage* input, IplImage* mask, CvPoint shift, CvSize outputSize, IplImage* outputMask, IplImage* outputData)
{
// check size
if(outputMask->width != outputSize.width || outputMask->height != outputSize.height)
return;
if(outputData->width != outputSize.width || outputData->height != outputSize.height)
return;
for(int i = 0; i < outputMask->width; i++)
for(int j = 0; j < outputMask->height; j++)
{
CvPoint inputPixel;
inputPixel.x = shift.x + i;
inputPixel.y = shift.y + j;
if(!IsOutside(inputPixel, cvSize(input->width, input->height)))
{
cvSet2D(outputMask, j, i, cvGet2D(mask, inputPixel.y, inputPixel.x));
}
else
{
cvSet2D(outputMask, j, i, cvScalar(255));
}
}
for(int i = 0; i < outputSize.width; i++)
for(int j = 0; j < outputSize.height; j++)
{
cvSet2D(outputData,j,i, cvScalar(231,233,233));
}
for(int i = 0; i < outputSize.width; i++)
for(int j = 0; j < outputSize.height; j++)
{
if(IsMaskedPixel(i, j, outputMask))
{
// check neighbor
CvPoint inputPixel;
inputPixel.x = shift.x + i;
inputPixel.y = shift.y + j;
CvScalar value;
CvSize inputSize = cvSize(input->width, input->height);
if(!IsOutside(cvPoint(inputPixel.x, inputPixel.y), inputSize) &&
!IsOutside(cvPoint(i,j), outputSize))
{
value = cvGet2D(input, inputPixel.y, inputPixel.x);
cvSet2D(outputData, j, i, value);
}
if(!IsOutside(cvPoint(inputPixel.x+1, inputPixel.y), inputSize) &&
!IsOutside(cvPoint(i+1,j), outputSize))
{
value = cvGet2D(input, inputPixel.y, inputPixel.x + 1);
cvSet2D(outputData, j, i+1, value);
}
if(!IsOutside(cvPoint(inputPixel.x - 1, inputPixel.y), inputSize) &&
!IsOutside(cvPoint(i-1,j), outputSize))
{
value = cvGet2D(input, inputPixel.y, inputPixel.x - 1);
cvSet2D(outputData, j, i-1, value);
}
if(!IsOutside(cvPoint(inputPixel.x, inputPixel.y+1), inputSize) &&
!IsOutside(cvPoint(i,j+1), outputSize))
{
value = cvGet2D(input, inputPixel.y + 1, inputPixel.x);
cvSet2D(outputData, j+1, i, value);
}
if(!IsOutside(cvPoint(inputPixel.x, inputPixel.y-1), inputSize) &&
!IsOutside(cvPoint(i,j-1), outputSize))
{
value = cvGet2D(input, inputPixel.y - 1, inputPixel.x);
cvSet2D(outputData, j-1, i, value);
}
}
}
for(int i = 0; i < outputSize.width - 1; i++)
for(int j = 0; j < outputSize.height - 1; j++)
{
if(IsMaskedPixel(i, j, outputMask))
{
// check neighbor
CvScalar value;
value = cvGet2D(outputData, j, i);
if(value.val[0] == 231 && value.val[1] == 233 && value.val[2] == 233)
printf("Test");
value = cvGet2D(outputData, j+1, i);
if(value.val[0] == 231 && value.val[1] == 233 && value.val[2] == 233)
printf("Test");
value = cvGet2D(outputData, j-1, i);
if(value.val[0] == 231 && value.val[1] == 233 && value.val[2] == 233)
printf("Test");
value = cvGet2D(outputData, j, i+1);
if(value.val[0] == 231 && value.val[1] == 233 && value.val[2] == 233)
printf("Test");
value = cvGet2D(outputData, j, i-1);
if(value.val[0] == 231 && value.val[1] == 233 && value.val[2] == 233)
printf("Test");
}
}
}
// mask is of the same size as input
MaskShift* CreateMaskShift(IplImage* input, IplImage* mask, CvPoint shift, CvSize outputSize)
{
IplImage* outputMask = cvCreateImage(outputSize, IPL_DEPTH_8U, 3);
IplImage* outputTest = cvCreateImage(outputSize, IPL_DEPTH_8U, 3);
IplImage* outputData = cvCreateImage(outputSize, IPL_DEPTH_8U, 3);
for(int i = 0; i < outputMask->width; i++)
for(int j = 0; j < outputMask->height; j++)
{
CvPoint inputPixel;
inputPixel.x = shift.x + i;
inputPixel.y = shift.y + j;
if(!IsOutside(inputPixel, cvSize(input->width, input->height)))
{
cvSet2D(outputMask, j, i, cvGet2D(mask, inputPixel.y, inputPixel.x));
}
else
{
cvSet2D(outputMask, j, i, cvScalar(255));
}
}
for(int i = 0; i < outputSize.width; i++)
for(int j = 0; j < outputSize.height; j++)
{
cvSet2D(outputData,j,i, cvScalar(231,233,233));
}
for(int i = 0; i < outputSize.width; i++)
for(int j = 0; j < outputSize.height; j++)
{
if(IsMaskedPixel(i, j, outputMask))
{
// check neighbor
CvPoint inputPixel;
inputPixel.x = shift.x + i;
inputPixel.y = shift.y + j;
CvScalar value;
CvSize inputSize = cvSize(input->width, input->height);
if(!IsOutside(cvPoint(inputPixel.x, inputPixel.y), inputSize) &&
!IsOutside(cvPoint(i,j), outputSize))
{
value = cvGet2D(input, inputPixel.y, inputPixel.x);
cvSet2D(outputData, j, i, value);
}
if(!IsOutside(cvPoint(inputPixel.x+1, inputPixel.y), inputSize) &&
!IsOutside(cvPoint(i+1,j), outputSize))
{
value = cvGet2D(input, inputPixel.y, inputPixel.x + 1);
cvSet2D(outputData, j, i+1, value);
}
if(!IsOutside(cvPoint(inputPixel.x - 1, inputPixel.y), inputSize) &&
!IsOutside(cvPoint(i-1,j), outputSize))
{
value = cvGet2D(input, inputPixel.y, inputPixel.x - 1);
cvSet2D(outputData, j, i-1, value);
}
if(!IsOutside(cvPoint(inputPixel.x, inputPixel.y+1), inputSize) &&
!IsOutside(cvPoint(i,j+1), outputSize))
{
value = cvGet2D(input, inputPixel.y + 1, inputPixel.x);
cvSet2D(outputData, j+1, i, value);
}
if(!IsOutside(cvPoint(inputPixel.x, inputPixel.y-1), inputSize) &&
!IsOutside(cvPoint(i,j-1), outputSize))
{
value = cvGet2D(input, inputPixel.y - 1, inputPixel.x);
cvSet2D(outputData, j-1, i, value);
}
}
}
int j2 =21;
int i2 = 5;
CvScalar value2 = cvGet2D(outputData, j2+1, i2);
for(int i = 0; i < outputSize.width; i++)
for(int j = 0; j < outputSize.height; j++)
{
if(IsMaskedPixel(i, j, outputMask))
{
// check neighbor
CvScalar value;
value = cvGet2D(outputData, j, i);
if(value.val[0] == 231 && value.val[1] == 233 && value.val[2] == 233)
printf("Test");
value = cvGet2D(outputData, j+1, i);
if(value.val[0] == 231 && value.val[1] == 233 && value.val[2] == 233)
printf("Test");
value = cvGet2D(outputData, j-1, i);
if(value.val[0] == 231 && value.val[1] == 233 && value.val[2] == 233)
printf("Test");
value = cvGet2D(outputData, j, i+1);
if(value.val[0] == 231 && value.val[1] == 233 && value.val[2] == 233)
printf("Test");
value = cvGet2D(outputData, j, i-1);
if(value.val[0] == 231 && value.val[1] == 233 && value.val[2] == 233)
printf("Test");
}
}
//cvNamedWindow("Test");
//while(1)
//{
// cvShowImage("Test", outputData);
// cvWaitKey(100);
//}
MaskShift* maskShift = new MaskShift(outputMask, input);
maskShift->SetMaskData(outputData);
return maskShift;
}
void BlobDetectionEngine::pixelClassEvaluate(bool **mask, int ***bins)
{
float shellRad;
int r,g,b, cInd;
set<int>::iterator it;
for(int row = 0; row < preHypNum; row++){
//CvScalar color = CV_RGB( rand()&255, rand()&255, rand()&255 );
int roi = ( hyp_t[row].innerShellRadius * 1.25 ) ;
int xSum = 0, ySum = 0, numPt = 0;
int yLim, xLim, yStart, xStart;
float dist;
float prior,likelihood, posterior;
//Boundary Checks
if ( hyp_t[row].y0 - roi <= 0 )
yStart = 0;
else
yStart = hyp_t[row].y0 - roi;
if ( hyp_t[row].y0 + roi >= Hypothesis::img_height - 1)
yLim = Hypothesis::img_height - 1;
else
yLim = hyp_t[row].y0 + roi;
if ( hyp_t[row].x0 - roi < 0 )
xStart = 0;
else
xStart = hyp_t[row].x0 - roi;
if ( hyp_t[row].x0 + roi >= Hypothesis::img_width - 1 )
xLim = Hypothesis::img_width - 1;
else
xLim = hyp_t[row].x0 + roi;
//Boundary Checks
//Implementation of argmax c
hyp_t[row].classUnionSet.insert( hyp_t[row].intEvtSet.begin(), hyp_t[row].intEvtSet.end() );
hyp_t[row].classUnionSet.insert( hyp_t[row].merEvtSet.begin(), hyp_t[row].merEvtSet.end() );
//cout << "Gonna search from {"<<yStart<<","<<xStart<<"} to {"<<yLim << "," << xLim << "} . " << endl;
//cout << "ActualImageLimits {"<<0<<","<<0<<"} to {"<<Hypothesis::img_height << "," << Hypothesis::img_width << "} . " << endl;
//cvRectangle(visualImg, cvPoint( xStart, yStart ), cvPoint( xLim , yLim ), CV_RGB(0,255,0), 1);
/*int step = visualImg->widthStep;
int channels = visualImg->nChannels;
uchar *data = (uchar *)visualImg->imageData;*/
for(int y = yStart ; y < yLim; y++){
for(int x = xStart ; x < xLim ; x++){
if ( mask[y][x] ){
//Ask whether inside shell?
dist = find_eu_distance( cvPoint( hyp_t[row].x0, hyp_t[row].y0), cvPoint(x,y) ) / (roi);
//Ask whether inside shell?
if ( dist > 1 )
continue;
b = bins[y][x][0];
g = bins[y][x][1];
r = bins[y][x][2];
cInd = b + g*16 + r*256;
//Find max matched class
int biggestIdx = hyp_t[row].id;
//float priorDef = (float)(1.00 - (find_eu_distance( cvPoint( hyp_t[row].x0, hyp_t[row].y0), cvPoint(x,y) ) / (roi))) ;
float priorDef = (float)(1 - pow( find_eu_distance( cvPoint( hyp_t[row].x0, hyp_t[row].y0), cvPoint(x,y) ) / (roi) , 2.0 ));
float LikelihoodDef = (hyp_t[row].histogram[ cInd ]+ 1e-6) / hyp_t[row].pxInHistogram ;
float biggestPost;
//useposterior.
biggestPost = LikelihoodDef * priorDef;
for(int clas = 0; clas < preHypNum; clas++ ){
it = hyp_t[row].classUnionSet.find( hyp_t[clas].id );
if ( it != hyp_t[row].classUnionSet.end() ){ //Implementation of Index and C_i
//Get pixel likelihood
//cout <<"lulz " << endl;
shellRad = hyp_t[clas].innerShellRadius * 1.25;
likelihood = (hyp_t[clas].histogram[ cInd ]+ 1e-6) / hyp_t[clas].pxInHistogram;
//prior = (float)(1.00 - find_eu_distance( cvPoint( hyp_t[clas].x0, hyp_t[clas].y0), cvPoint(x,y) ) / (shellRad)) ;
prior = (float)(1 - pow( find_eu_distance( cvPoint( hyp_t[clas].x0, hyp_t[clas].y0), cvPoint(x,y) ) / (shellRad) , 2.0 ));
//if (prior > 2.00 )
//cout << "prior: " << prior << endl;
if ( prior < 0)
prior = 0;
//posterior = likelihood ;
//cout << "likelihood: " << likelihood << endl;
posterior = likelihood * prior;
if ( posterior > biggestPost ){
biggestPost = posterior;
biggestIdx = hyp_t[clas].id;
}
}
}
if ( biggestIdx == hyp_t[row].id ){
//cout << "pix : {"<<y<<","<<x<<"} is class " << biggestIdx << " prob: " << LikelihoodDef << endl;
//cvSet2D( visualImg, y , x, color);
xSum += (x); // multiply with mask to prevent if check.
ySum += (y);
numPt += (1);
}
if (biggestIdx == 0)
cvSet2D( visualImg, y , x, cvScalar(255,0,0,0));
if (biggestIdx == 1)
cvSet2D( visualImg, y , x, cvScalar(0,0,255,0));
if (biggestIdx == 2)
cvSet2D( visualImg, y , x, cvScalar(0,255,0,0));
if (biggestIdx == 3)
cvSet2D( visualImg, y , x, cvScalar(255,0,255,0));
if (biggestIdx == 4)
cvSet2D( visualImg, y , x, cvScalar(255,255,0,0));
if (biggestIdx == 5)
cvSet2D( visualImg, y , x, cvScalar(0,255,255,0));
}
}
}
hyp_t[row].ex_x0 = hyp_t[row].x0;
hyp_t[row].ex_y0 = hyp_t[row].y0;
if ( numPt != 0){
hyp_t[row].x0 = xSum / numPt;
hyp_t[row].y0 = ySum / numPt;
}
hyp_t[row].classUnionSet.clear();
}
}
bool ColorImageSegmentByKMeans2 ( const IplImage * img , IplImage * pResult, int nClusters, int sortFlag )
{
assert ( img != NULL && pResult != NULL );
assert ( img -> nChannels == 3 && pResult -> nChannels == 1);
int i , j ;
CvMat * samples = cvCreateMat (( img -> width )*( img -> height ),1, CV_32FC3 ); // 创建样本矩阵， CV_32FC3 代表位浮点通道（彩色图像）
CvMat * clusters = cvCreateMat (( img -> width )*( img -> height ),1, CV_32SC1 ); // 创建类别标记矩阵， CV_32SF1 代表位整型通道
int k =0;
for ( i =0; i < img -> width ; i ++)
{
for ( j =0; j < img -> height ; j ++)
{
CvScalar s ;
// 获取图像各个像素点的三通道值（ RGB ）
s . val [0]=( float ) cvGet2D ( img , j , i ). val [0];
s . val [1]=( float ) cvGet2D ( img , j , i ). val [1];
s . val [2]=( float ) cvGet2D ( img , j , i ). val [2];
cvSet2D ( samples , k ++,0, s ); // 将像素点三通道的值按顺序排入样本矩阵
}
}
cvKMeans2 ( samples , nClusters , clusters , cvTermCriteria ( CV_TERMCRIT_ITER ,50,1.0)); // 开始聚类，迭代次，终止误差 .0
k =0;
int val =0;
float step =255/( nClusters -1);
for ( i =0; i < img -> width ; i ++)
{
for ( j =0; j < img -> height ; j ++)
{
val =( int ) clusters -> data . i [ k ++];
CvScalar s ;
s . val [0]=255- val * step ; // 这个是将不同类别取不同的像素值，
cvSet2D ( pResult , j , i , s ); // 将每个像素点赋值
}
}
cvReleaseMat (& samples );
cvReleaseMat (& clusters );
return true ;
}
bool GrayImageSegmentByKMeans2 ( const IplImage * pImg , IplImage * pResult , int nClusters, int sortFlag )
{
assert ( pImg != NULL && pImg -> nChannels == 1);
// 创建样本矩阵， CV_32FC1 代表位浮点通道（灰度图像）
CvMat * samples = cvCreateMat (( pImg -> width )* ( pImg -> height ),1, CV_32FC1 );
// 创建类别标记矩阵， CV_32SF1 代表位整型通道
CvMat * clusters = cvCreateMat (( pImg -> width )* ( pImg -> height ),1, CV_32SC1 );
// 创建类别中心矩阵
CvMat * centers = cvCreateMat ( nClusters , 1, CV_32FC1 );
// 将原始图像转换到样本矩阵
{
int k = 0;
CvScalar s ;
for ( int i = 0; i < pImg -> width ; i ++)
{
for ( int j =0; j < pImg -> height ; j ++)
{
s . val [0] = ( float ) cvGet2D ( pImg , j , i ). val [0];
cvSet2D ( samples , k ++, 0, s );
}
}
}
// 开始聚类，迭代次，终止误差 .0
cvKMeans2 ( samples , nClusters , clusters , cvTermCriteria ( CV_TERMCRIT_ITER + CV_TERMCRIT_EPS ,50, 1.0), 1, 0, 0, centers );
// 无需排序直接输出时
if ( sortFlag == 0)
{
int k = 0;
int val = 0;
float step = 255 / (( float ) nClusters - 1);
CvScalar s ;
for ( int i = 0; i < pImg -> width ; i ++)
{
for ( int j = 0; j < pImg -> height ; j ++)
{
val = ( int ) clusters -> data . i [ k ++];
s . val [0] = 255- val * step ; // 这个是将不同类别取不同的像素值，
cvSet2D ( pResult , j , i , s ); // 将每个像素点赋值
}
}
return true ;
}
}
lfError cLightFieldViewGenerator_AllInFocus::generate(void *raw_image, void *depth_image, lfCalibrationParameter_t* cparams, lfViewGeneratorParameter_t* vparams)
{
IplImage* img = static_cast<IplImage*> (raw_image);
IplImage* depthImg = static_cast<IplImage*> (depth_image);
IplImage* view = CREATE_IMAGE(img);
_lens_mask = cparams->createLensMask();
IplImage* mask = (IplImage*) _lens_mask;
RETURN_IF_NULL(img && view && depthImg && mask);
cvZero(view);
unsigned int numLenses; // number of lenses in the neighborhood
unsigned int numUsedLenses = 0; // number of lenses used for the projection
int xLens, yLens, xImg, yImg; // projected point in lens coordinates
double depth; // virtual depth at current image point
lfPoint2Dd_t lens_centers[100]; // storage for the lens centers of the neighboring lenses
lfPoint2Dd_t image_point; // projected point in image coordinates
lfPoint2Dd_t lens_center; // current used lens center
CvScalar s = cvScalar(0); // color value of the final pixel
const int r = (int) (cparams->diameter / 2 - cparams->lens_border); // radius with integer resolution
//showImage(mask,"Lens mask");
for (int y=0; y < img->height; y++) {
for (int x=0; x < img->width; x++) {
depth = cvGetReal2D(depthImg, y, x);
numLenses = sizeof(lens_centers) / sizeof(*lens_centers);
cparams->findCloseLensCenters(lfPoint2D(x, y), 2, depth, &numLenses, lens_centers);
//IplImage *temp_img = cvCloneImage(img);
//for(unsigned int l = 0; l < numLenses; l++) {
// lens_center = lens_centers[l];
// cvCircle(temp_img, cvPoint((int) lens_center.x, (int) lens_center.y), r, gColors[0], 1);
//}
//cvCircle(temp_img, cvPoint(x,y), 5, gColors[1], 2);
//showImage(temp_img, "Debug Image");
//cvReleaseImage(&temp_img);
for(unsigned int l = 0; l < numLenses; l++) {
lens_center = lens_centers[l];
image_point = lens_center - (1/depth * (lfPoint2D(x,y) - lens_center));
// point in image coordinate system
xImg = cvFloor(image_point.x);
yImg = cvFloor(image_point.y);
// point in corresponding lens coordinate system
xLens = (int) (mask->width / 2 - lens_center.x + image_point.x);
yLens = (int) (mask->height / 2 - lens_center.y + image_point.y);
// only if projected image point is at a valid lense position
if(IS_POINT_IN_IMAGE(mask, xLens, yLens)) {
if(cvGetReal2D(mask,yLens,xLens)) {
if(IS_POINT_IN_IMAGE(img, xImg, yImg) && IS_POINT_IN_IMAGE(img, xImg+1, yImg+1)) {
s = s + get2DInterpolated(img, image_point.x, image_point.y);
numUsedLenses++;
}
}
}
}
if(numUsedLenses == 0)
continue;
// compute mean value over all found pixels
s = (1/(double)numUsedLenses) * s;
cvSet2D(view, y, x, s);
// reset variables
numUsedLenses = 0;
s = cvScalar(0);
}
}
_view = view;
RETURN_NO_ERR;
}
/*
* Extract the DC terms from the specified component.
*/
IplImage *
extract_dc(j_decompress_ptr cinfo, jvirt_barray_ptr *coeffs, int ci)
{
jpeg_component_info *ci_ptr = &cinfo->comp_info[ci];
CvSize size = cvSize(ci_ptr->width_in_blocks, ci_ptr->height_in_blocks);
IplImage *dc = cvCreateImage(size, IPL_DEPTH_8U, 1);
assert(dc != NULL);
JQUANT_TBL *tbl = ci_ptr->quant_table;
UINT16 dc_quant = tbl->quantval[0];
#if DEBUG
printf("DCT method: %x\n", cinfo->dct_method);
printf
(
"component: %d (%d x %d blocks) sampling: (%d x %d)\n",
ci,
ci_ptr->width_in_blocks,
ci_ptr->height_in_blocks,
ci_ptr->h_samp_factor,
ci_ptr->v_samp_factor
);
printf("quantization table: %d\n", ci);
for (int i = 0; i < DCTSIZE2; ++i)
{
printf("% 4d ", (int)(tbl->quantval[i]));
if ((i + 1) % 8 == 0)
printf("\n");
}
printf("raw DC coefficients:\n");
#endif
JBLOCKARRAY buf =
(cinfo->mem->access_virt_barray)
(
(j_common_ptr)cinfo,
coeffs[ci],
0,
ci_ptr->v_samp_factor,
FALSE
);
for (int sf = 0; (JDIMENSION)sf < ci_ptr->height_in_blocks; ++sf)
{
for (JDIMENSION b = 0; b < ci_ptr->width_in_blocks; ++b)
{
int intensity = 0;
intensity = buf[sf][b][0]*dc_quant/DCTSIZE + 128;
intensity = MAX(0, intensity);
intensity = MIN(255, intensity);
cvSet2D(dc, sf, (int)b, cvScalar(intensity));
#if DEBUG
printf("% 2d ", buf[sf][b][0]);
#endif
}
#if DEBUG
printf("\n");
#endif
}
return dc;
}
// Convert Python lists to CvMat *
CvArr * PySequence_to_CvArr (PyObject * obj)
{
int dims [CV_MAX_DIM] = { 1, 1, 1};
PyObject * container[CV_MAX_DIM+1] = {NULL, NULL, NULL, NULL};
int ndim = 0;
PyObject * item = Py_None;
// TODO: implement type detection - currently we create CV_64F only
// scan full array to
// - figure out dimensions
// - check consistency of dimensions
// - find appropriate data-type and signedness
// enum NEEDED_DATATYPE { NEEDS_CHAR, NEEDS_INTEGER, NEEDS_FLOAT, NEEDS_DOUBLE };
// NEEDED_DATATYPE needed_datatype = NEEDS_CHAR;
// bool needs_sign = false;
// scan first entries to find out dimensions
for (item = obj, ndim = 0; PySequence_Check (item) && ndim <= CV_MAX_DIM; ndim++)
{
dims [ndim] = PySequence_Size (item);
container [ndim] = PySequence_GetItem (item, 0);
item = container[ndim];
}
// in contrast to PyTuple_GetItem, PySequence_GetItame returns a NEW reference
if (container[0])
{
Py_DECREF (container[0]);
}
if (container[1])
{
Py_DECREF (container[1]);
}
if (container[2])
{
Py_DECREF (container[2]);
}
if (container[3])
{
Py_DECREF (container[3]);
}
// it only makes sense to support 2 and 3 dimensional data at this time
if (ndim < 2 || ndim > 3)
{
PyErr_SetString (PyExc_TypeError, "Nested sequences should have 2 or 3 dimensions");
return NULL;
}
// also, the number of channels should match what's typical for OpenCV
if (ndim == 3 && (dims[2] < 1 || dims[2] > 4))
{
PyErr_SetString (PyExc_TypeError, "Currently, the third dimension of CvMat only supports 1 to 4 channels");
return NULL;
}
// CvMat
CvMat * matrix = cvCreateMat (dims[0], dims[1], CV_MAKETYPE (CV_64F, dims[2]));
for (int y = 0; y < dims[0]; y++)
{
PyObject * rowobj = PySequence_GetItem (obj, y);
// double check size
if (PySequence_Check (rowobj) && PySequence_Size (rowobj) == dims[1])
{
for (int x = 0; x < dims[1]; x++)
{
PyObject * colobj = PySequence_GetItem (rowobj, x);
if (dims [2] > 1)
{
if (PySequence_Check (colobj) && PySequence_Size (colobj) == dims[2])
{
PyObject * tuple = PySequence_Tuple (colobj);
double a, b, c, d;
if (PyArg_ParseTuple (colobj, "d|d|d|d", &a, &b, &c, &d))
{
cvSet2D (matrix, y, x, cvScalar (a, b, c, d));
}
else
{
PyErr_SetString (PyExc_TypeError, "OpenCV only accepts numbers that can be converted to float");
cvReleaseMat (& matrix);
Py_DECREF (tuple);
Py_DECREF (colobj);
Py_DECREF (rowobj);
return NULL;
}
Py_DECREF (tuple);
}
else
{
PyErr_SetString (PyExc_TypeError, "All sub-sequences must have the same number of entries");
cvReleaseMat (& matrix);
Py_DECREF (colobj);
Py_DECREF (rowobj);
return NULL;
}
}
else
{
if (PyFloat_Check (colobj) || PyInt_Check (colobj))
{
cvmSet (matrix, y, x, PyFloat_AsDouble (colobj));
}
else
{
PyErr_SetString (PyExc_TypeError, "OpenCV only accepts numbers that can be converted to float");
cvReleaseMat (& matrix);
Py_DECREF (colobj);
Py_DECREF (rowobj);
return NULL;
}
}
Py_DECREF (colobj);
}
}
else
{
PyErr_SetString (PyExc_TypeError, "All sub-sequences must have the same number of entries");
cvReleaseMat (& matrix);
Py_DECREF (rowobj);
return NULL;
}
Py_DECREF (rowobj);
}
return matrix;
}
int main()
{
pImage = cvLoadImage("./frame.jpg", 0);
if (pImage == NULL)
return -1;
int width = pImage->width;
int height = pImage->height;
w16 = width/BLOCK_SIZE;
h16 = height/BLOCK_SIZE;
mb = new microblock[w16*h16];
// 计算图像的x-梯度和y-梯度值(1阶, 2阶)
// fx'(x,y) = abs(f(x+1, y) - f(x, y))
// fx"(x,y) = min{fx'(x,y)-f'(x-1,y), fx'(x,y)-f'(x+1,y)}
// = min{abs(f(x+1,y) - f(x,y)) - abs(f(x,y) - f(x-1,y)),
// abs(f(x+1,y) - f(x,y)) - abs(f(x+2,y) - f(x+1,y))}
//
// fy'(x,y) = abs(f(x, y+1) - f(x, y))
// fy"(x,y) = min(fy'(x, y)-fy'(x, y-1), fy'(x,y)-fy'(x,y-1)
// = min{abs(f(x,y+1) - f(x,y)) - abs(f(x,y) - f(x,y-1)),
// abs(f(x,y+1) - f(x,y)) - abs(f(x,y+2) - f(x,y+1))}
IplImage *pDiff = cvCreateImage(cvSize(pImage->width, pImage->height), 8, 3);
IplImage *p2Diff = cvCreateImage(cvSize(pImage->width, pImage->height), 8, 3);
for (int i = 0; i < h16; i++){
for (int j = 0; j < w16; j++){
int top = i ? (i*16-1) : 0;
int left = j ? (j*16-1) : 0;
double ddy = 0;
for (int k = 0; i > 0 && k < 16; k++){
double y[4];
y[0] = cvGetReal2D(pImage, top-1, left+k);
y[1] = cvGetReal2D(pImage, top, left+k);
y[2] = cvGetReal2D(pImage, top+1, left+k);
y[3] = cvGetReal2D(pImage, top+2, left+k);
double dy[3];
dy[0] = abs(y[1] - y[0]);
dy[1] = abs(y[2] - y[1]);
dy[2] = abs(y[3] - y[2]);
cvSet2D(pDiff, top-1,left+k, CV_RGB(0, dy[0]*5, 0));
cvSet2D(pDiff, top,left+k, CV_RGB(0, dy[1]*5, 0));
cvSet2D(pDiff,top+1, left+k, CV_RGB(0, dy[2]*5, 0));
ddy += min(dy[1] - dy[0], dy[1] - dy[2]);
}
ddy /= 16;
mb[i*w16+j][TOP] = ddy;
if (i > 0){
mb[(i-1)*w16+j][BOTTOM] = ddy;
}
double ddx = 0;
for (int k = 0; j > 0 && k < 16; k++){
double x[4];
x[0] = cvGetReal2D(pImage, top+k, left-1);
x[1] = cvGetReal2D(pImage, top+k, left);
x[2] = cvGetReal2D(pImage, top+k, left+1);
x[3] = cvGetReal2D(pImage, top+k, left+2);
double dx[3];
dx[0] = abs(x[1] - x[0]);
dx[1] = abs(x[2] - x[1]);
dx[2] = abs(x[3] - x[2]);
cvSet2D(pDiff, top+k,left-1, CV_RGB(0, dx[0]*5, 0));
cvSet2D(pDiff, top+k,left, CV_RGB(0, dx[0]*5, 0));
cvSet2D(pDiff, top+k,left+1, CV_RGB(0, dx[0]*5, 0));
ddx += min(dx[1] - dx[0], dx[1] - dx[2]);
}
ddx /= 16;
mb[i*w16+j][LEFT] = ddx;
if (j > 0){
mb[i*w16+j-1][RIGHT] = ddx;
}
cvLine(p2Diff, cvPoint(left, top), cvPoint(left+15, top), CV_RGB(0, ddy*10, 0), 1, CV_AA, 0);
cvLine(p2Diff, cvPoint(left, top), cvPoint(left, top+15), CV_RGB(0, ddx*10, 0), 1, CV_AA, 0);
}
}
cvShowImage("1-degree gradient", pDiff);
cvShowImage("2-degree gradient", p2Diff);
cvSaveImage("1-degree-gradient.jpg", pDiff);
cvSaveImage("2-degree-gradient.jpg", p2Diff);
cvNamedWindow("image");
int val = 10;
cvCreateTrackbar("tracker", "image", &val, 50, on_tracker);
on_tracker(val);
cvWaitKey(0);
cvReleaseImage(&pImage);
cvReleaseImage(&pDiff);
cvReleaseImage(&p2Diff);
return 0;
}
int image_analysis(const int argc, const char * argv[])
{
#define NUM 5
char fname[4096];
unsigned u, v, q, vec_per_class, vec_num, cl_ind, yes, res, vec_class[5], max_iter[NUM], * d, * test_d;
double C[NUM], tau[NUM], epsilon[NUM], ** x, ** test_x;
double (* K[NUM])(const double *, const double *, unsigned);
int (* selector[NUM])(const fmll_svm *, const double **, const char *, const unsigned, int *, int *, const double, const double, const double, const double *, const double *, const double **);
IplImage * src_teach, * src_test, * dst;
CvSize size_teach, size_test;
CvScalar pixel, pixel_white, pixel_red, pixel_green, pixel_blue, pixel_violet, pixel_black, pixel_yellow;
fmll_svm_net * svm_net;
/* ############################################################################ */
if(argc != 3)
{
/* У C90 "проблемы" с максимальной длиной строки кода */
printf("\nДемонстрация нейронной сети, состоящей из нескольких машин опорных векторов.\n\n");
printf("Запуск программы:\n\n");
printf("ex_svm_net DIR IMAGE_1\n\n");
printf("Где:\n\n");
printf("\tDIR - путь и имя каталога, в котором должны содержаться следующие файлы:\n\n");
printf("\t\tteach.png - файл с изображением, на основании которого будет составлена обучающая выборка;\n");
printf("\t\ttest.png - файл с изображением, классификация пикселей которого будет выполнена;\n\n");
printf("\tIMAGE_1 - путь и имя файла, в который будет сохранено результирующее изображение ");
printf("(белый, красный, зеленый, синий, фиолетовый - правильно классифицированные пиксели; черный - неправильно классифицированные пиксели; ");
printf("желтый - неклассифицированные пиксели).\n\n");
return -1;
}
/* ############################################################################ */
memset(vec_class, 0, sizeof(unsigned) * 5);
sprintf(fname, "%s/teach.png", argv[1]);
src_teach = cvLoadImage(fname, CV_LOAD_IMAGE_COLOR);
size_teach = cvGetSize(src_teach);
sprintf(fname, "%s/test.png", argv[1]);
src_test = cvLoadImage(fname, CV_LOAD_IMAGE_COLOR);
size_test = cvGetSize(src_test);
dst = cvCreateImage(size_test, IPL_DEPTH_8U, 3);
pixel_white.val[0] = 255;
pixel_white.val[1] = 255;
pixel_white.val[2] = 255;
pixel_white.val[3] = 0;
pixel_red.val[0] = 0;
pixel_red.val[1] = 0;
pixel_red.val[2] = 255;
pixel_red.val[3] = 0;
pixel_green.val[0] = 0;
pixel_green.val[1] = 255;
pixel_green.val[2] = 0;
pixel_green.val[3] = 0;
pixel_black.val[0] = 0;
pixel_black.val[1] = 0;
pixel_black.val[2] = 0;
pixel_black.val[3] = 0;
pixel_blue.val[0] = 255;
pixel_blue.val[1] = 0;
pixel_blue.val[2] = 0;
pixel_blue.val[3] = 0;
pixel_violet.val[0] = 255;
pixel_violet.val[1] = 0;
pixel_violet.val[2] = 255;
pixel_violet.val[3] = 0;
pixel_yellow.val[0] = 0;
pixel_yellow.val[1] = 255;
pixel_yellow.val[2] = 255;
pixel_yellow.val[3] = 0;
vec_per_class = size_teach.height * size_teach.width / 2000;
vec_num = vec_per_class * 5;
x = (double **) fmll_alloc(sizeof(double), 2, vec_num, 3);
d = fmll_alloc(sizeof(unsigned), 1, vec_num);
test_x = (double **) fmll_alloc(sizeof(double), 2, size_test.height * size_test.width, 3);
test_d = fmll_alloc(sizeof(unsigned), 1, size_test.height * size_test.width);
for(v = 0, q = 0; v < size_teach.height; v++)
for(u = 0; u < size_teach.width; u++)
{
pixel = cvGet2D(src_teach, v, u);
if(pixel.val[0] == 0 && pixel.val[1] == 237 && pixel.val[2] == 95)
cl_ind = 0;
else if(pixel.val[0] == 10 && pixel.val[1] == 169 && pixel.val[2] == 203)
cl_ind = 1;
else if(pixel.val[0] == 0 && pixel.val[1] == 255 && pixel.val[2] == 255)
cl_ind = 2;
else if(pixel.val[0] == 255 && pixel.val[1] == 0 && pixel.val[2] == 12)
cl_ind = 3;
else
cl_ind = 4;
if(vec_class[cl_ind] < vec_per_class)
{
x[q][0] = pixel.val[0];
x[q][1] = pixel.val[1];
x[q][2] = pixel.val[2];
d[q] = cl_ind;
vec_class[cl_ind]++;
q++;
}
}
for(v = 0, q = 0; v < size_test.height; v++)
for(u = 0; u < size_test.width; u++, q++)
{
pixel = cvGet2D(src_test, v, u);
if(pixel.val[0] == 0 && pixel.val[1] == 237 && pixel.val[2] == 95)
test_d[q] = 0;
else if(pixel.val[0] == 10 && pixel.val[1] == 169 && pixel.val[2] == 203)
test_d[q] = 1;
else if(pixel.val[0] == 0 && pixel.val[1] == 255 && pixel.val[2] == 255)
test_d[q] = 2;
else if(pixel.val[0] == 255 && pixel.val[1] == 0 && pixel.val[2] == 12)
test_d[q] = 3;
else
test_d[q] = 4;
test_x[q][0] = pixel.val[0];
test_x[q][1] = pixel.val[1];
test_x[q][2] = pixel.val[2];
}
cvReleaseImage(& src_teach);
cvReleaseImage(& src_test);
/* ############################################################################ */
for(u = 0; u < NUM; u++)
K[u] = & fmll_kernel_radial;
svm_net = fmll_svm_net_init(NUM, 3, K);
/* ############################################################################ */
for(u = 0; u < NUM; u++)
{
C[u] = 1;
tau[u] = 1E-12;
selector[u] = & fmll_svm_teach_smo_selector_fan_chen_lin;
max_iter[u] = 10000;
epsilon[u] = 1E-3;
}
fmll_svm_net_teach_smo(svm_net, (const double **) x, d, vec_num, C, tau, selector, max_iter, epsilon);
/* ############################################################################ */
yes = fmll_svm_net_test(svm_net, (const double **) test_x, test_d, size_test.width * size_test.height, NULL, NULL);
printf("\nВерно классифицированных пикселей: %u из %u (%.7f %%)\n",
yes, (size_test.width * size_test.height), (100.0 * yes) / (size_test.width * size_test.height));
/* ############################################################################ */
fmll_svm_net_save(svm_net, "svm_net");
fmll_svm_net_destroy(svm_net);
svm_net = fmll_svm_net_load("svm_net", K);
for(v = 0, q = 0, yes = 0; v < size_test.height; v++)
for(u = 0; u < size_test.width; u++, q++)
{
res = fmll_svm_net_run(svm_net, test_x[q], NULL);
if(res >= 0)
{
if(res == test_d[q])
{
yes++;
switch(res)
{
case 0:
{
cvSet2D(dst, v, u, pixel_white);
break;
}
case 1:
{
cvSet2D(dst, v, u, pixel_red);
break;
}
case 2:
{
cvSet2D(dst, v, u, pixel_green);
break;
}
case 3:
{
cvSet2D(dst, v, u, pixel_blue);
break;
}
case 4:
{
cvSet2D(dst, v, u, pixel_violet);
break;
}
}
}
else
cvSet2D(dst, v, u, pixel_black);
}
else if(res == -1)
cvSet2D(dst, v, u, pixel_yellow);
else
cvSet2D(dst, v, u, pixel_black);
}
printf("Верно классифицированных пикселей: %u из %u (%.7f %%)\n",
yes, (size_test.width * size_test.height), (100.0 * yes) / (size_test.width * size_test.height));
/* ############################################################################ */
fmll_free(x);
fmll_free(d);
fmll_free(test_x);
fmll_free(test_d);
fmll_svm_net_destroy(svm_net);
cvSaveImage(argv[2], dst, NULL);
cvReleaseImage(& dst);
return 0;
}
ImageRAII hysteresis( IplImage * image, IplImage * orientation, std::pair< int, int > thresh )
{
const char * WINDOW_NAME = "Hysteresis Threshold";
CvSize image_size = cvGetSize( image );
ImageRAII hysteresis_image( cvCreateImage( image_size, image->depth, image->nChannels ) );
// key: pixel position
// value: visited = true, unvisited = false
std::map< CvPoint, bool, classcomp > pixels;
std::map< CvPoint, bool, classcomp >::iterator it;
std::vector< std::vector< CvPoint > > edges;
// initialize map
for( int i = 0; i < image_size.width; i++ )
{
for( int j = 0; j < image_size.height; j++ )
{
pixels[cvPoint( i, j )] = false;
}
}
// visit all pixels
for( it = pixels.begin(); it != pixels.end(); it++ )
{
std::vector< CvPoint > edge;
// find next unvisited edge pixel
bool run = true;
while( run )
{
if( it->second == false && check_boundaries( image, it->first ) && cvGet2D( image, it->first.y, it->first.x ).val[0] > thresh.second )
run = false;
if( it == pixels.end() )
run = false;
it++;
}
// mark pixel as visited
CvPoint current_pixel = it->first;
it->second = true;
edge.push_back( current_pixel );
// follow links forward
std::pair< CvPoint, CvPoint > positions = get_edge_positions( orientation, current_pixel.x, current_pixel.y );
// go forward
CvPoint forward = positions.first;
while( check_boundaries( image, forward ) && cvGet2D( image, forward.y, forward.x ).val[0] > thresh.first )
{
// mark pixel as visited
edge.push_back( forward );
pixels.find( forward )->second = true;
std::pair< CvPoint, CvPoint > forward_positions = get_edge_positions( orientation, forward.x, forward.y );
forward = forward_positions.first;
}
// go backward
CvPoint backward = positions.second;
while( check_boundaries( image, backward ) && cvGet2D( image, backward.y, backward.x ).val[0] > thresh.first )
{
// mark pixel as visited
edge.push_back( backward );
pixels.find( backward )->second = true;
std::pair< CvPoint, CvPoint > backward_positions = get_edge_positions( orientation, backward.x, backward.y );
backward = backward_positions.second;
}
// store this edge
edges.push_back( edge );
}
int size = 0;
// set the edges in the image
std::vector< std::vector< CvPoint > >::iterator edges_iterator;
for( edges_iterator = edges.begin(); edges_iterator < edges.end(); edges_iterator++ )
{
std::vector< CvPoint >::iterator edge_iterator;
std::vector< CvPoint > edge = *edges_iterator;
for( edge_iterator = edge.begin(); edge_iterator < edge.end(); edge_iterator++ )
{
size++;
CvPoint pixel = *edge_iterator;
CvScalar e;
e.val[0] = GRAY;
cvSet2D( hysteresis_image.image, pixel.y, pixel.x, e );
}
}
cvNamedWindow( WINDOW_NAME );
cvShowImage( WINDOW_NAME, hysteresis_image.image );
cvMoveWindow( WINDOW_NAME, image_size.width * 3, 0 );
return hysteresis_image;
}
int main(int argc, char* argv[])
{
int height,width,n;
int len;
if(argc!=2)
{
cerr<<"No file name found. Please pass a file name as a parameter."<<endl;
exit(1);
}
IplImage *img,*scrambled;
CvScalar pix;
img=cvLoadImage(argv[1]);
if(img==0)
{
cerr<<argv[1]<<": File not found."<<endl;
exit(1);
}
cout<<"Image dimensions: "<<img->height<<" "<<img->width<<endl;
cout<<"Enter side length of square piece: ";
scanf("%d",&len);
if(len<1)
{
cerr<<"Invalid side length."<<endl;
exit(1);
}
n = min(img->height/len,img->width/len);
height = width = len;
assignMemory(height,width,n*n);
generateImages(img,n,height,width);
vector<Block> permuted = permute(n*n);
scrambled=cvCreateImage(cvSize(width,height),IPL_DEPTH_8U,3);
for(int i=0;i<n*n;i++)
{
char filename[50]="./generated_pieces/",fileid[10];
sprintf(fileid,"%d",i+1);
strcat(fileid,".jpg");
for(int j=0;j<height;j++)
{
for(int k=0;k<width;k++)
{
for(int h=0;h<3;h++)
{
pix.val[h]=permuted[i].image[j][k].val[h];
}
cvSet2D(scrambled,j,k,pix);
}
}
strcat(filename,fileid);
cvSaveImage(filename,scrambled);
printf("Generated %s\n",fileid);
}
cout<<"Picture Broken into total "<<n*n<<" pieces."<<endl<<endl;
cout<<"N = "<<n<<endl<<endl;
return 0;
}
//参数说明：nCuster为聚类的类数
int color_cluster(char *filename,int nCuster )
{
IplImage* img=cvLoadImage(filename);
int i,j;
CvMat *samples=cvCreateMat((img->width)*(img->height),1,CV_32FC3);//创建样本矩阵，CV_32FC3代表32位浮点3通道（彩色图像）
CvMat *clusters=cvCreateMat((img->width)*(img->height),1,CV_32SC1);//创建类别标记矩阵，CV_32SF1代表32位整型1通道
int k=0;
for (i=0;i<img->width;i++)
{
for (j=0;j<img->height;j++)
{
CvScalar s;
//获取图像各个像素点的三通道值（BGR）
s.val[0]=(float)cvGet2D(img,j,i).val[0];//B
s.val[1]=(float)cvGet2D(img,j,i).val[1];//G
s.val[2]=(float)cvGet2D(img,j,i).val[2];//R
cvSet2D(samples,k++,0,s);//将像素点三通道的值按顺序排入样本矩阵
}
}
//聚类类别数，后期可以通过学习确定分类数。
cvKMeans2(samples,nCuster,clusters,cvTermCriteria(CV_TERMCRIT_ITER,100,1.0));//开始聚类，迭代100次，终止误差1.0
//创建用于显示的图像，二值图像
IplImage *binimg=cvCreateImage(cvSize(img->width,img->height),IPL_DEPTH_8U,1);
//创建用于单独显示每个聚类结果的图像
IplImage *cluster_img0=cvCreateImage(cvSize(img->width,img->height),IPL_DEPTH_8U,1);
IplImage *cluster_img1=cvCreateImage(cvSize(img->width,img->height),IPL_DEPTH_8U,1);
IplImage *cluster_img2=cvCreateImage(cvSize(img->width,img->height),IPL_DEPTH_8U,1);
k=0;
int val=0;
float step=255/(nCuster-1);
CvScalar bg={255,0,0,0};//背景设置为白色
for (i=0;i<img->width;i++)
{
for (j=0;j<img->height;j++)
{
cvSet2D(cluster_img0,j,i,bg);
cvSet2D(cluster_img1,j,i,bg);
cvSet2D(cluster_img2,j,i,bg);
}
}
for (i=0;i<img->width;i++)
{
for (j=0;j<img->height;j++)
{
val=(int)clusters->data.i[k++];
CvScalar s;
s.val[0]=255-val*step;//这个是将不同类别取不同的像素值，
cvSet2D(binimg,j,i,s);
//将每个聚类进行分离
switch(val)
{
case 0:
cvSet2D(cluster_img0,j,i,s);break;//白色类
case 1:
cvSet2D(cluster_img1,j,i,s);break;//灰色类
case 2:
cvSet2D(cluster_img2,j,i,s);break;//黑色类
default:
break;
}
}
}
cvSaveImage("PicVideo//cluster_img0.png",cluster_img0);
cvSaveImage("PicVideo//cluster_img1.png",cluster_img1);
cvSaveImage("PicVideo//cluster_img2.png",cluster_img2);
cvNamedWindow( "原始图像", 1 );
cvShowImage( "原始图像", img );
cvNamedWindow( "聚类图像", 1 );
cvShowImage( "聚类图像", binimg );
cvSaveImage("PicVideo//clusterimg.png",binimg);
cvWaitKey(0); //等待按键
cvDestroyWindow( "原始图像" );
cvDestroyWindow( "聚类图像" );
cvReleaseImage( &img );
cvReleaseImage( &binimg );
cvReleaseImage(&cluster_img0);
cvReleaseImage(&cluster_img1);
cvReleaseImage(&cluster_img0);
return 0;
}
void adjust_bodybbox_w_clusters(CvMat* mask, IplImage* cluster, int numclusters, CvRect facebox )
{
double a=1.15; //extra growing of body box region
double A=1.00; // body bbox is lower than face bbox (neck)
double b=1+a;
int bodyx0 = ((facebox.x-facebox.width* a) < 0 ) ? 0 : (facebox.x-facebox.width* a);
int bodyy0 = ((facebox.y+facebox.height * A) > cluster->height) ? cluster->height : (facebox.y+facebox.height*A );
int bodyx1 = ((facebox.x+facebox.width* b) > cluster->width) ? cluster->width : (facebox.x+facebox.width*b);
int bodyy1 = (cluster->height);
int x,y, i=0;
int *accu;
int eqclass_1st; // equivalence class chosen as most populated
int tmp_eqclass; // temp, to hold the equivalence class associated to a pixel
int eqclass_2nd; // equivalence class chosen as 2nd most populated
accu = (int*)malloc( numclusters*sizeof(int));
bzero( accu, numclusters*sizeof(int));
eqclass_1st = 0;
eqclass_2nd = numclusters-1; // just initialised to sth != eqclass_1st
// 1st: get to know the amount of pixels per equivalence class ("cluster")
// but not blindly: only those in the FG mask already
for( x = 0; x < cluster->width; x++ ){
for( y = 0; y < cluster->height; y++ ){
// filter the equ_classes using the mask
if( ( x >= bodyx0) && ( x <= bodyx1) && ( y >= bodyy0) && ( y <= bodyy1)){
tmp_eqclass = (int) ( round(cvGet2D( cluster, y, x).val[1]*numclusters/255.0) -1);
accu[ tmp_eqclass ] ++;
}
}
}
// 2nd: get the most populated and the 2nd most populated cluster
for( i = 0; i< numclusters; i++ ){
eqclass_1st = ( accu[i] > accu[eqclass_1st] ) ? i : eqclass_1st;
eqclass_2nd = ( accu[i] > accu[eqclass_2nd] ) ? ((accu[i] < accu[eqclass_1st]) ? i : eqclass_2nd ): eqclass_2nd;
printf(" %.8d ", accu[i]);
}
printf(" (eqclass_1st %d 2nd %d) \n", eqclass_1st, eqclass_2nd);
// 3rd: Using the pixels inside of a seed of the body bbox, calculated from the face box,
// we calculate the (minx,miny)-(maxx,maxy) bounding rectangle due to the largest cluster
int minx=10000, miny=10000, maxx=0, maxy=0;
for( x = 0; x < cluster->width; x++ ){
for( y = 0; y < cluster->height; y++ ){
if(!( ( x >= bodyx0) && ( x <= bodyx1) && ( y >= bodyy0) && ( y <= bodyy1) )){
cvSet2D(cluster, y, x, cvScalar(0, 0, 0 ) );
continue;
}
tmp_eqclass = (int) ( round(cvGet2D( cluster, y, x).val[1]*numclusters/255.0) -1);
if(tmp_eqclass == eqclass_1st){
cvSet2D(cluster, y, x, cvScalar(255, 0, 0 ) ); // for display purposes
maxx = ( maxx > x ) ? maxx : x;
maxy = ( maxy > y ) ? maxy : y;
minx = ( minx < x ) ? minx : x;
miny = ( miny < y ) ? miny : y;
}
else if (tmp_eqclass == eqclass_2nd) {
cvSet2D(cluster, y, x, cvScalar(100, 0, 0 ) ); // for display purposes
}
else{
cvSet2D(cluster, y, x, cvScalar(10, 0, 0 ) ); // for display purposes
}
}
}
cvRectangle(cluster, cvPoint(minx, miny), cvPoint(maxx, maxy), cvScalar(255, 0, 255), 1);
// Last: compose in the mask a body-box based on the largest cluster bbox
// the rectangle is needed otherwise the largest cluster has loads of holes
cvRectangle(mask, cvPoint(minx, miny), cvPoint(maxx, cluster->height), cvScalar(GC_PR_FGD, 0, 0), CV_FILLED);
free( accu );
}
void detectAndDisplay(IplImage *frame)
{
IplImage *frame_gray;
int ForCount1, ForCount2;
ForCount1 = ForCount2 = 0;
memset(pointXY, 0, sizeof(char)*column*row);
frame_gray = cvCreateImage( cvGetSize(frame),IPL_DEPTH_8U,1);
//frame_gray = cvCreateImage( cvSize(column, row),IPL_DEPTH_8U,1);
dst = cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U,1);
//dst = cvCreateImage(cvSize(column,row),IPL_DEPTH_8U,1);
#ifdef showXY
showSumX = cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U,1);
showSumY = cvCreateImage(cvSize(row,column),IPL_DEPTH_8U,1);
#endif
cvCvtColor(frame, frame_gray, CV_BGR2GRAY);
//cvCanny(frame_gray, dst, 40, 40*3, 3);
cvThreshold(frame_gray, dst, Threshold, 255, CV_THRESH_BINARY);
for(ForCount1 = 0; ForCount1 < column; ForCount1++)
{
for(ForCount2 = 0; ForCount2 < row; ForCount2++)
{
CvScalar s = cvGet2D(dst,ForCount2,ForCount1);
//char s = ((uchar *)(dst->imageData + ForCount1*dst->widthStep))[ForCount2];
#ifdef Detail
printf("%3d %3d %f\n",ForCount1,ForCount2, s.val[0]);
#endif
/*if( s.val[0] <= Threshold)
{
pointXY[ForCount1][ForCount2] = 1;
}*/
if(s.val[0] <= Threshold)
{
pointXY[ForCount1][ForCount2] = 0;
}
else
{
pointXY[ForCount1][ForCount2] = 1;
}
}
}
Integral(row);
Integral(column);
Error1();
Error2();
cvShowImage("Webcam",dst);
cvShowImage("Webcam1",frame_gray);
#ifdef Detail
for(ForCount1 = 0; ForCount1 < column; ForCount1++)
{
printf("x[%3d]:%d\n", ForCount1, SumX[ForCount1]);
}
printf("\n");
for(ForCount1 = 0; ForCount1 < row; ForCount1++)
{
printf("y[%3d]:%d\n", ForCount1, SumY[ForCount1]);
}
printf("\n");
#endif
#ifdef showXY
for(ForCount1 = 0; ForCount1 < column; ForCount1++)
{
for(ForCount2 = 0; ForCount2 < (int)SumX[ForCount1]; ForCount2++)
{
CvScalar s = cvGet2D(showSumX,ForCount2,ForCount1);
s.val[0] = 255;
cvSet2D(showSumX, ForCount2, ForCount1, s);
}
}
cvShowImage("SumX", showSumX);
for(ForCount1 = 0; ForCount1 < row; ForCount1++)
{
for(ForCount2 = 0; ForCount2 < (int)SumY[ForCount1]; ForCount2++)
{
CvScalar s = cvGet2D(showSumY,ForCount2,ForCount1);
s.val[0] = 255;
cvSet2D(showSumY, ForCount2, ForCount1, s);
}
}
#endif
//cvShowImage("SumY", showSumY);
cvReleaseImage( &dst );
cvReleaseImage( &frame_gray );
#ifdef showXY
cvReleaseImage( &showSumX );
cvReleaseImage( &showSumY );
#endif
}
void cveSet2D(CvArr* arr, int idx0, int idx1, CvScalar* value)
{
cvSet2D(arr, idx0, idx1, *value);
}
int _tmain(int argc, _TCHAR* argv[])
{
int i, j, n, m;
CvScalar tempValue;
double S= 0.0;
double Mask[3][3] = {{255., 255., 255.}, {255., 255., 255.}, {255., 255., 255.}};
//침식 연산을 위한 마스크
IplImage *inputImage = cvLoadImage("lena.bmp", CV_LOAD_IMAGE_GRAYSCALE);
IplImage *binaryImage = cvCreateImage(cvGetSize(inputImage), 8, 1);
IplImage *tempImage = cvCreateImage(cvSize(inputImage->width+2, inputImage->height+2), 8, 1);
IplImage *outputImage = cvCreateImage(cvGetSize(inputImage), 8, 1);
for(i=0; i<inputImage->height; i++){ //원본영상의 이진화
for(j=0; j<inputImage->width; j++){
tempValue = cvGet2D(inputImage, i, j);
if(tempValue.val[0] > THRESHOLD) {
cvSet2D(binaryImage, i, j, cvScalar(255));
} else {
cvSet2D(binaryImage, i, j, cvScalar(0));
}
}
}
for(i=0; i<binaryImage->height; i++) {
for(j=0; j<binaryImage->width; j++){
cvSet2D(tempImage, i+1, j+1, cvGet2D(binaryImage, i, j));
}
}
for(i=0; i<binaryImage->height; i++){
for(j=0; j<binaryImage->width; j++){
for(n=0; n<3; n++){
for(m=0; m<3; m++){
tempValue = cvGet2D(tempImage, i+n, j+m);
if(Mask[n][m] == tempValue.val[0]){
//마스크와 같은 값이 있는지 조사
S += 1.0;
}
}
}
if(S==9.0){
cvSet2D(outputImage, i, j, cvScalar(255));
//값이 모두 일치하면 출력 값은 255
} else {
cvSet2D(outputImage, i, j, cvScalar(0));
//모두 일치하지 않으면 출력 값은 0
}
S =0.0; // reset
}
}
cvShowImage("Input Image", inputImage);
cvShowImage("Binary Image", binaryImage);
cvShowImage("Output Image", outputImage);
cvWaitKey();
cvReleaseImage(&inputImage);
cvReleaseImage(&binaryImage);
cvReleaseImage(&tempImage);
cvReleaseImage(&outputImage);
return 0;
}
void Error1()
{
int ForCount1 = 0;
int ForCount2 = 0;
memset(SumX1, 0, sizeof(short)*column);
/* for(ForCount1 = 0; ForCount1 < column; ForCount1++)
{
for(ForCount2 = YUpperBoundPos -1; ForCount2 >= (YUpperBoundPos/2); ForCount2--)
{
SumX1[ForCount1] = SumX1[ForCount1] + pointXY[ForCount1][ForCount1];
}
if(ForCount1 > 3 || SumX1[ForCount1] > 4 && SumX1[ForCount1-1] > 4 && SumX1[ForCount1-2] > 4 && SumX1[ForCount1-3] >4 )
{
for(ForCount2 = YUpperBoundPos -1; ForCount2 >= (YUpperBoundPos/2); ForCount2--)
{
if(pointXY[ForCount1][ForCount2] == 1)
{
CvScalar s = cvGet2D(dst,ForCount2,ForCount1);
s.val[0] = 150;
cvSet2D(dst, ForCount2, ForCount1, s);
}
if(pointXY[ForCount1-1][ForCount2] == 1)
{
CvScalar s = cvGet2D(dst,ForCount2,ForCount1-1);
s.val[0] = 150;
cvSet2D(dst, ForCount2, ForCount1-1, s);
}
if(pointXY[ForCount1-2][ForCount2] == 1)
{
CvScalar s = cvGet2D(dst,ForCount2,ForCount1-2);
s.val[0] = 150;
cvSet2D(dst, ForCount2, ForCount1-2, s);
}
if(pointXY[ForCount1-3][ForCount2] == 1)
{
CvScalar s = cvGet2D(dst,ForCount2,ForCount1-3);
s.val[0] = 150;
cvSet2D(dst, ForCount2, ForCount1-3, s);
}
}
printf("Error1\n");
break;
}
}
*/
for(ForCount1 = YUpperBoundPos-1; ForCount1 >= (YUpperBoundPos/2); ForCount1--)
{
if(SumY[ForCount1] > Error1YThreshold && SumY[ForCount1-1] > Error1YThreshold && SumY[ForCount1-2] > Error1YThreshold )
{
for(ForCount2 = 0; ForCount2 < column; ForCount2++)
{
if(pointXY[ForCount2][ForCount1] == 1)
{
CvScalar s = cvGet2D(dst,ForCount1,ForCount2);
s.val[0] = 150;
cvSet2D(dst, ForCount1, ForCount2, s);
cvSet2D(dst, ForCount1-1, ForCount2, s);
cvSet2D(dst, ForCount1-2, ForCount2, s);
}
}
printf("Error1\n\n");
break;
}
}
}
int opticaltri( CvMat * &clean_texture, int verts )
{
char * im1fname = "conhull-dirty-thresh.jpg";
char * im2fname = "conhull-clean-thresh.jpg";
int count = MAX_COUNT;
char * status;
CvPoint2D32f * source_points;
CvPoint2D32f * dest_points;
CvPoint2D32f * delaunay_points = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(CvPoint2D32f));
// count = opticalflow( im1fname, im2fname, source_points, dest_points, status );
count = findsiftpoints( "conhull-dirty.jpg", "conhull-clean.jpg", source_points, dest_points, status );
IplImage * image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_COLOR);
CvMemStorage * storage = cvCreateMemStorage(0);
CvSubdiv2D * delaunay = cvCreateSubdivDelaunay2D( cvRect(0,0,image1->width,image1->height), storage);
IplImage * image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_COLOR);
cvSet( image1, cvScalarAll(255) );
std::map<CvPoint, CvPoint> point_lookup_map;
std::vector<std::pair<CvPoint, CvPoint> > point_lookup;
int num_matches = 0;
int num_out_matches = 0;
int max_dist = 50;
int offset = 200;
// put corners in the point lookup as going to themselves
point_lookup_map[cvPoint(0,0)] = cvPoint(0,0);
point_lookup_map[cvPoint(0,image1->height-1)] = cvPoint(0,image1->height-1);
point_lookup_map[cvPoint(image1->width-1,0)] = cvPoint(image1->width-1,0);
point_lookup_map[cvPoint(image1->width-1,image1->height-1)] = cvPoint(image1->width-1,image1->height-1);
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(0,0), cvPoint(0,0)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(0,image1->height-1), cvPoint(0,image1->height-1)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(image1->width-1,0), cvPoint(image1->width-1,0)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(image1->width-1,image1->height-1), cvPoint(image1->width-1,image1->height-1)));
printf("Inserting corners...");
// put corners in the Delaunay subdivision
for(unsigned int i = 0; i < point_lookup.size(); i++) {
cvSubdivDelaunay2DInsert( delaunay, cvPointTo32f(point_lookup[i].first) );
}
printf("done.\n");
CvSubdiv2DEdge proxy_edge;
for(int i = 0; i < count; i++) {
if(status[i]) {
CvPoint source = cvPointFrom32f(source_points[i]);
CvPoint dest = cvPointFrom32f(dest_points[i]);
if((((int)fabs((double)(source.x - dest.x))) > max_dist) ||
(((int)fabs((double)(source.y - dest.y))) > max_dist)) {
num_out_matches++;
}
else if((dest.x >= 0) && (dest.y >= 0) && (dest.x < (image1->width)) && (dest.y < (image1->height))) {
if(point_lookup_map.find(source) == point_lookup_map.end()) {
num_matches++;
point_lookup_map[source] = dest;
point_lookup.push_back(std::pair<CvPoint,CvPoint>(source,dest));
// delaunay_points[i] =
(cvSubdivDelaunay2DInsert( delaunay, cvPointTo32f(source) ))->pt;
cvSetImageROI( image1, cvRect(source.x-8,source.y-8,8*2,8*2) );
cvResetImageROI( image2 );
cvGetRectSubPix( image2, image1, dest_points[i] );
}
/*
cvSet2D( image1, source.y, source.x, cvGet2D( image2, dest.y, dest.x ) );
cvSet2D( image1, source.y, source.x+1, cvGet2D( image2, dest.y, dest.x+1 ) );
cvSet2D( image1, source.y, source.x-1, cvGet2D( image2, dest.y, dest.x-1 ) );
cvSet2D( image1, source.y+1, source.x, cvGet2D( image2, dest.y+1, dest.x ) );
cvSet2D( image1, source.y-1, source.x, cvGet2D( image2, dest.y-1, dest.x ) );
cvSet2D( image1, source.y+1, source.x+1, cvGet2D( image2, dest.y+1, dest.x+1 ) );
cvSet2D( image1, source.y-1, source.x-1, cvGet2D( image2, dest.y-1, dest.x-1 ) );
cvSet2D( image1, source.y+1, source.x-1, cvGet2D( image2, dest.y+1, dest.x-1 ) );
cvSet2D( image1, source.y-1, source.x+1, cvGet2D( image2, dest.y-1, dest.x+1 ) );
*/
// cvCircle( image1, source, 4, CV_RGB(255,0,0), 2, CV_AA );
// cvCircle( image2, dest, 4, CV_RGB(255,0,0), 2, CV_AA );
}
/*
cvSetImageROI( image1, cvRect(source.x-offset,source.y-offset,offset*2,offset*2) );
cvSetImageROI( image2, cvRect(dest.x-offset,dest.y-offset,offset*2,offset*2) );
cvNamedWindow("image1",0);
cvNamedWindow("image2",0);
cvShowImage("image1",image1);
cvShowImage("image2",image2);
printf("%d,%d -> %d,%d\n",source.x,source.y,dest.x,dest.y);
cvWaitKey(0);
cvDestroyAllWindows();
*/
}
}
printf("%d %d\n",num_matches,num_out_matches);
printf("%d lookups\n",point_lookup_map.size());
cvResetImageROI( image1 );
cvSaveImage("sparse.jpg", image1);
cvReleaseImage(&image1);
image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_COLOR);
cvSet( image1, cvScalarAll(255) );
printf("Warping image...");
CvSeqReader reader;
int total = delaunay->edges->total;
int elem_size = delaunay->edges->elem_size;
std::vector<Triangle> trivec;
std::vector<CvMat *> baryinvvec;
for( int i = 0; i < total*2; i++ ) {
if((i == 0) || (i == total)) {
cvStartReadSeq( (CvSeq*)(delaunay->edges), &reader, 0 );
}
CvQuadEdge2D* edge = (CvQuadEdge2D*)(reader.ptr);
if( CV_IS_SET_ELEM( edge )) {
CvSubdiv2DEdge curedge = (CvSubdiv2DEdge)edge;
CvSubdiv2DEdge t = curedge;
Triangle temptri;
int count = 0;
// construct a triangle from this edge
do {
CvSubdiv2DPoint* pt = cvSubdiv2DEdgeOrg( t );
if(count < 3) {
pt->pt.x = pt->pt.x >= image1->width ? image1->width-1 : pt->pt.x;
pt->pt.y = pt->pt.y >= image1->height ? image1->height-1 : pt->pt.y;
pt->pt.x = pt->pt.x < 0 ? 0 : pt->pt.x;
pt->pt.y = pt->pt.y < 0 ? 0 : pt->pt.y;
temptri.points[count] = cvPointFrom32f( pt->pt );
}
else {
printf("More than 3 edges\n");
}
count++;
if(i < total)
t = cvSubdiv2DGetEdge( t, CV_NEXT_AROUND_LEFT );
else
t = cvSubdiv2DGetEdge( t, CV_NEXT_AROUND_RIGHT );
} while( t != curedge );
// check that triangle is not already in
if( std::find(trivec.begin(), trivec.end(), temptri) == trivec.end() ) {
// push triangle in and draw
trivec.push_back(temptri);
cvLine( image1, temptri.points[0], temptri.points[1], CV_RGB(255,0,0), 1, CV_AA, 0 );
cvLine( image1, temptri.points[1], temptri.points[2], CV_RGB(255,0,0), 1, CV_AA, 0 );
cvLine( image1, temptri.points[2], temptri.points[0], CV_RGB(255,0,0), 1, CV_AA, 0 );
// compute barycentric computation vector for this triangle
CvMat * barycen = cvCreateMat( 3, 3, CV_32FC1 );
CvMat * baryceninv = cvCreateMat( 3, 3, CV_32FC1 );
barycen->data.fl[3*0+0] = temptri.points[0].x;
barycen->data.fl[3*0+1] = temptri.points[1].x;
barycen->data.fl[3*0+2] = temptri.points[2].x;
barycen->data.fl[3*1+0] = temptri.points[0].y;
barycen->data.fl[3*1+1] = temptri.points[1].y;
barycen->data.fl[3*1+2] = temptri.points[2].y;
barycen->data.fl[3*2+0] = 1;
barycen->data.fl[3*2+1] = 1;
barycen->data.fl[3*2+2] = 1;
cvInvert( barycen, baryceninv, CV_LU );
baryinvvec.push_back(baryceninv);
cvReleaseMat( &barycen );
}
}
CV_NEXT_SEQ_ELEM( elem_size, reader );
}
printf("%d triangles...", trivec.size());
cvSaveImage("triangles.jpg", image1);
cvSet( image1, cvScalarAll(255) );
IplImage * clean_nonthresh = cvLoadImage( "conhull-clean.jpg", CV_LOAD_IMAGE_COLOR );
// for each triangle
for(unsigned int i = 0; i < trivec.size(); i++) {
Triangle curtri = trivec[i];
CvMat * curpoints = cvCreateMat( 1, 3, CV_32SC2 );
Triangle target;
std::map<CvPoint,CvPoint>::iterator piter[3];
printf("Triangle %d / %d\n",i,trivec.size());
int is_corner = 0;
for(int j = 0; j < 3; j++) {
/*
curpoints->data.i[2*j+0] = curtri.points[j].x;
curpoints->data.i[2*j+1] = curtri.points[j].y;
*/
CV_MAT_ELEM( *curpoints, CvPoint, 0, j ) = curtri.points[j];
printf("%d,%d\n",curtri.points[j].x,curtri.points[j].y);
if((curtri.points[j] == cvPoint(0,0)) || (curtri.points[j] == cvPoint(0,image1->height - 1)) ||(curtri.points[j] == cvPoint(image1->width - 1,0)) ||(curtri.points[j] == cvPoint(image1->width - 1,image1->height - 1))) {
is_corner++;
}
for(unsigned int k = 0; k < point_lookup.size(); k++) {
std::pair<CvPoint,CvPoint> thispair = point_lookup[k];
if(thispair.first == curtri.points[j]) {
target.points[j] = thispair.second;
break;
}
}
/*
piter[j] = point_lookup_map.find(curtri.points[j]);
if(piter[j] != point_lookup_map.end() ) {
target.points[j] = piter[j]->second;
}
*/
}
// if((piter[0] != point_lookup_map.end()) && (piter[1] != point_lookup_map.end()) && (piter[2] != point_lookup_map.end())) {
if(is_corner < 3) {
CvMat * newcorners = cvCreateMat( 3, 3, CV_32FC1 );
newcorners->data.fl[3*0+0] = target.points[0].x;
newcorners->data.fl[3*0+1] = target.points[1].x;
newcorners->data.fl[3*0+2] = target.points[2].x;
newcorners->data.fl[3*1+0] = target.points[0].y;
newcorners->data.fl[3*1+1] = target.points[1].y;
newcorners->data.fl[3*1+2] = target.points[2].y;
newcorners->data.fl[3*2+0] = 1;
newcorners->data.fl[3*2+1] = 1;
newcorners->data.fl[3*2+2] = 1;
CvContour hdr;
CvSeqBlock blk;
CvRect trianglebound = cvBoundingRect( cvPointSeqFromMat(CV_SEQ_KIND_CURVE+CV_SEQ_FLAG_CLOSED, curpoints, &hdr, &blk), 1 );
printf("Bounding box: %d,%d,%d,%d\n",trianglebound.x,trianglebound.y,trianglebound.width,trianglebound.height);
for(int y = trianglebound.y; (y < (trianglebound.y + trianglebound.height)) && ( y < image1->height); y++) {
for(int x = trianglebound.x; (x < (trianglebound.x + trianglebound.width)) && (x < image1->width); x++) {
// check to see if we're inside this triangle
/*
CvPoint v0 = cvPoint( curtri.points[2].x - curtri.points[0].x, curtri.points[2].y - curtri.points[0].y );
CvPoint v1 = cvPoint( curtri.points[1].x - curtri.points[0].x, curtri.points[1].y - curtri.points[0].y );
CvPoint v2 = cvPoint( x - curtri.points[0].x, y - curtri.points[0].y );
int dot00 = v0.x * v0.x + v0.y * v0. y;
int dot01 = v0.x * v1.x + v0.y * v1. y;
int dot02 = v0.x * v2.x + v0.y * v2. y;
int dot11 = v1.x * v1.x + v1.y * v1. y;
int dot12 = v1.x * v2.x + v1.y * v2. y;
double invDenom = 1.0 / (double)(dot00 * dot11 - dot01 * dot01);
double u = (double)(dot11 * dot02 - dot01 * dot12) * invDenom;
double v = (double)(dot00 * dot12 - dot01 * dot02) * invDenom;
*/
CvMat * curp = cvCreateMat(3, 1, CV_32FC1);
CvMat * result = cvCreateMat(3, 1, CV_32FC1);
curp->data.fl[0] = x;
curp->data.fl[1] = y;
curp->data.fl[2] = 1;
cvMatMul( baryinvvec[i], curp, result );
// double u = result->data.fl[0]/result->data.fl[2];
// double v = result->data.fl[1]/result->data.fl[2];
/*
if((i == 3019) && (y == 1329) && (x > 2505) && (x < 2584)) {
printf("Range %d: %f, %f, %f\t%f, %f, %f\n",x,result->data.fl[0],result->data.fl[1],result->data.fl[2],
sourcepoint->data.fl[0],sourcepoint->data.fl[1],sourcepoint->data.fl[2]);
}
*/
if( (result->data.fl[0] > MIN_VAL) && (result->data.fl[1] > MIN_VAL) && (result->data.fl[2] > MIN_VAL) && (fabs(1.0 - (result->data.fl[0]+result->data.fl[1]+result->data.fl[2])) <= 0.01) ) {
// if((u > 0) || (v > 0) /*&& ((u +v) < 1)*/ )
// printf("Barycentric: %f %f %f\n", result->data.fl[0], result->data.fl[1], result->data.fl[2]);
// this point is inside this triangle
// printf("Point %d,%d inside %d,%d %d,%d %d,%d\n",x,y,trivec[i].points[0].x,trivec[i].points[0].y,
// trivec[i].points[1].x,trivec[i].points[1].y,trivec[i].points[2].x,trivec[i].points[2].y);
CvMat * sourcepoint = cvCreateMat(3, 1, CV_32FC1);
cvMatMul( newcorners, result, sourcepoint );
double sourcex = sourcepoint->data.fl[0]/*/sourcepoint->data.fl[2]*/;
double sourcey = sourcepoint->data.fl[1]/*/sourcepoint->data.fl[2]*/;
if((sourcex >= 0) && (sourcey >= 0) && (sourcex < (image1->width)) && (sourcey < (image1->height))) {
// printf("%d,%d %d,%d\n",x,y,(int)sourcex,(int)sourcey);
cvSet2D( image1, y, x, cvGet2D( clean_nonthresh, (int)sourcey, (int)sourcex ) );
}
// printf("Point %d,%d inside %d,%d %d,%d %d,%d\n",x,y,trivec[i].points[0].x,trivec[i].points[0].y,
// trivec[i].points[1].x,trivec[i].points[1].y,trivec[i].points[2].x,trivec[i].points[2].y);
cvReleaseMat( &sourcepoint );
}
cvReleaseMat( &result );
cvReleaseMat( &curp );
}
}
for(int k = 0; k < verts; k++) {
double x = clean_texture->data.fl[2*k+0];
double y = clean_texture->data.fl[2*k+1];
// check to see if we're inside this triangle
CvMat * curp = cvCreateMat(3, 1, CV_32FC1);
CvMat * result = cvCreateMat(3, 1, CV_32FC1);
curp->data.fl[0] = x;
curp->data.fl[1] = y;
curp->data.fl[2] = 1;
cvMatMul( baryinvvec[i], curp, result );
if( (result->data.fl[0] > MIN_VAL) && (result->data.fl[1] > MIN_VAL) && (result->data.fl[2] > MIN_VAL) && (fabs(1.0 - (result->data.fl[0]+result->data.fl[1]+result->data.fl[2])) <= 0.01) ) {
CvMat * sourcepoint = cvCreateMat(3, 1, CV_32FC1);
cvMatMul( newcorners, result, sourcepoint );
double sourcex = sourcepoint->data.fl[0]/*/sourcepoint->data.fl[2]*/;
double sourcey = sourcepoint->data.fl[1]/*/sourcepoint->data.fl[2]*/;
if((sourcex >= 0) && (sourcey >= 0) && (sourcex < (image1->width)) && (sourcey < (image1->height))) {
clean_texture->data.fl[2*k+0] = sourcex;
clean_texture->data.fl[2*k+1] = sourcey;
// cvSet2D( image1, y, x, cvGet2D( clean_nonthresh, (int)sourcey, (int)sourcex ) );
}
cvReleaseMat( &sourcepoint );
}
cvReleaseMat( &result );
cvReleaseMat( &curp );
}
cvReleaseMat( &newcorners );
}
cvReleaseMat( &curpoints );
}
cvReleaseImage( &clean_nonthresh );
printf("done.\n");
cvSaveImage("fullwarp.jpg", image1);
printf("Drawing subdivisions on warped image...");
draw_subdiv( image1, delaunay, NULL, NULL, 0, NULL );
// draw_subdiv( image1, delaunay, delaunay_points, source_points, count, status );
printf("done.\n");
cvSaveImage("edgeswarp.jpg", image1);
cvReleaseImage(&image2);
image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_COLOR);
// cvCreateImage( cvGetSize(image2), IPL_DEPTH_8U, 3 );
// cvCalcSubdivVoronoi2D( delaunay );
printf("Drawing subdivisions on unwarped image...");
// draw_subdiv( image2, delaunay, delaunay_points, dest_points, count, status );
// draw_subdiv( image2, delaunay, NULL, NULL, 0, NULL );
printf("done.\n");
cvSaveImage("edges.jpg",image2);
cvReleaseImage(&image1);
cvFree(&source_points);
cvFree(&dest_points);
cvFree(&status);
cvReleaseMemStorage(&storage);
cvFree(&delaunay_points);
cvReleaseImage(&image2);
return 0;
}
void compresie_decompresie_cu_DCT( IplImage* image )
{
int w, h;
int i, j;
int k, l;
double pi = 3.1415926;
CvScalar pixel;
CvScalar valoare;
IplImage *imagine_rezultat;
IplImage *imagine_diferenta;
IplImage *cosinus;
int N, nivel;
double max = 0, min = 10000;
double value;
CvMat *C; //matricea transformarii cosinus
CvMat *U; //matricea imaginii in spatiul original
CvMat *V; //matricea imaginii in spatiul transformatei
CvMat *AUX; //matrice auxiliara pentru rezultatul partial
double m, M;
double prag;
int nr; //numarul de coeficienti anulati in spatiul transformatei
w = image->width;
h = image->height;
if( w == h )
{
imagine_rezultat = cvCreateImage( cvSize( w, h ), IPL_DEPTH_8U, 1 );
N = w;
C = cvCreateMat( N, N, CV_32FC1 );
U = cvCreateMat( N, N, CV_32FC1 );
V = cvCreateMat( N, N, CV_32FC1 );
AUX = cvCreateMat( N, N, CV_32FC1 );
//formarea matricii C a transformarii cosinus discreta
for( i = 0; i < N; i++ )
cvmSet( C, 0, i, 1. / sqrt( (float)N ) );
for( i = 1; i < N; i++ )
for( j = 0; j < N; j++ )
{
value = sqrt( 2./N ) * cos( pi * ( 2*j + 1 ) * i / ( 2*N ) );
cvmSet( C, i, j, value );
if( value > max )
max = value;
}
//popularea matricei U cu valorile pixelilor imaginii
for( i = 0; i < N; i++ )
for( j = 0; j < N; j++ )
{
pixel = cvGet2D( image, i, j );
cvmSet( U, i, j, pixel.val[ 0 ] );
}
//V = C*U*Ct
//mai intii vom calcula AUX = C * U
for( i = 0; i < N; i++ )
for( j = 0; j < N; j++ )
{
value = 0;
for( k = 0; k < N; k++ )
value += cvmGet( C, i, k ) * cvmGet( U, k, j );
cvmSet( AUX, i, j, value );
}
//apoi V = AUX * Ct
max = 0;
for( i = 0; i < N; i++ )
for( j = 0; j < N; j++ )
{
value = 0;
for( k = 0; k < N; k++ )
value += cvmGet( AUX, i, k ) * cvmGet( C, j, k );
cvmSet( V, i, j, value );
if( value > max )
max = value;
else
if( value < min )
min = value;
}
// /*
//anularea coeficientilor de energie mica
prag = 800;
nr = 0;
for( i = 0; i < N; i++ )
for( j = 0; j < N; j++ )
{
value = fabs( cvmGet( V, i, j ) );
if( value < prag )
{
cvmSet( V, i, j, 0 );
nr++;
}
}
printf( "Procentul de valori retinute in spatiul transformatei = %5.2lf%%\n", 100.*(N*N - nr) / (N*N) );
if( nr != 0 )
printf( "Factorul de compresie obtinut = %5.2lf\n", 1. * (N * N) / (N*N - nr) );
// */
// DECOMPRESIA IMAGINII
//U = Ct * V * C
//AUX = Ct * V
for( i = 0; i < N; i++ )
for( j = 0; j < N; j++ )
{
value = 0;
for( k = 0; k < N; k++ )
value += cvmGet( C, k, i ) * cvmGet( V, k, j );
cvmSet( AUX, i, j, value );
}
//apoi U = AUX * C
for( i = 0; i < N; i++ )
for( j = 0; j < N; j++ )
{
value = 0;
for( k = 0; k < N; k++ )
value += cvmGet( AUX, i, k ) * cvmGet( C, k, j );
cvmSet( U, i, j, value );
pixel.val[ 0 ] = value;
cvSet2D( imagine_rezultat, i, j, pixel );
}
cvNamedWindow( "Imaginea dupa decompresie", 1 );
cvShowImage( "Imaginea dupa decompresie", imagine_rezultat );
//calcul pseudo-imagine diferenta (zgomot cauzat de compresia cu pierderi)
imagine_diferenta = cvCreateImage( cvSize( w, h ), IPL_DEPTH_8U, 1 );
for( i = 0; i < N; i++ )
for( j = 0; j < N; j++ )
{
pixel = cvGet2D( image, i, j );
value = (int)( fabs( pixel.val[ 0 ] - cvmGet( U, i, j ) ) );
valoare.val[ 0 ] = value;
cvSet2D( imagine_diferenta, i, j, valoare );
}
cvNamedWindow( "Pseudo-imaginea diferenta", 1 );
cvShowImage( "Pseudo-imaginea diferenta", imagine_diferenta );
cvWaitKey(0);
cvReleaseImage( &imagine_rezultat );
cvReleaseImage( &imagine_diferenta );
cvDestroyWindow( "Imaginea dupa decompresie" );
cvDestroyWindow( "Pseudo-imaginea diferenta" );
cvReleaseMat( &C );
cvReleaseMat( &U );
cvReleaseMat( &V );
cvReleaseMat( &AUX );
}
else
printf( "Imaginea de intrare nu este patrata! (w != h)\n" );
}
void Breadth::buildInitialMap(){
cout<<"Started Breadth"<<endl;
//grab the position of the robot
CvPoint robot;
robot.x = (int)(model->state.crioPosition.x*(1.0/model->meterToPixel)) + model->translation.x;
robot.y = (int)(model->state.crioPosition.y*-(1.0/model->meterToPixel)) + model->translation.y;
cout<<"Grabbed robot position\n";
//stores the current point to check
CvPoint current, next;
int values[4];
//create the queue of points to check
queue <CvPoint> points;
//create the cost map
model->costMap = cvCreateImage(cvGetSize(model->worldModel), IPL_DEPTH_32F, 1);
cvSet(model->costMap, cvScalar(10000));
cvSet2D(model->costMap,robot.x,robot.y,cvScalar(0));
int floatstep = model->costMap->widthStep/sizeof(float);
int floatchannels = model->costMap->nChannels;
cout<<"Created cost map\n";
//store the pointer to the data
int* costdata = (int *)model->costMap->imageData;
//set the region of interest
CvRect roi = cvRect(robot.x - width/2., robot.y - height/2., width, height);
roi = model->correctROI(roi);
cvSetImageROI(model->costMap, roi);
//grab a copy of the combined image
open = cvCreateImage(cvGetSize(model->worldModel), 8, 1);
model->copyCompletePlanWorldmap(open);
int intstep = open->widthStep/sizeof(int);
int intchannels = open->nChannels/sizeof(int);
int* opendata = (int*)open->imageData;
//push the first point into the queue
points.push(robot);
cout<<"Initialize everything and starting loop\n";
while(points.size()!=0){
cout<<"Queue Size: "<< points.size()<<endl;
waitKey(0);
//read in the current front point
current = points.front();
//pop the point from the list
points.pop();
//check four neighbors for the lowest value
values[0] = costdata[(current.y-1)*floatstep+(current.x)*floatchannels+0];
values[1] = costdata[(current.y)*floatstep+(current.x-1)*floatchannels+0];
values[2] = costdata[(current.y+1)*floatstep+(current.x)*floatchannels+0];
values[3] = costdata[(current.y)*floatstep+(current.x+1)*floatchannels+0];
for(int i=0; i<4; i++){
//check to see if the neighbor is less than the current
if(values[i]+1<costdata[(current.y)*floatstep+(current.x)*floatchannels+0]){
costdata[(current.y)*floatstep+(current.x)*floatchannels+0] = values[i];
}
//check to see if neighbor should be added to list
switch(i){
case 0:
next.x = current.x;
next.y = current.y-1;
break;
case 1:
next.x = current.x-1;
next.y = current.y;
break;
case 2:
next.x = current.x;
next.y = current.y+1;
break;
case 3:
next.x = current.x+1;
next.y = current.y;
}
if(opendata[next.y*intstep+next.x+1*intchannels+0] < model->neutral_knowledge){
//push value into queue
points.push(next);
//close the value on the image
opendata[next.y*intstep+next.x+1*floatchannels+0] = 255;
}
}
}
}
void CTWithWater::imageAndDepthToWorld(double u, double v, double d,
double* x, double* y, double* z,
bool undistort)
{
double xx, yy, t;
CvMat* r = cvCreateMat(3, 1, CV_32FC1);
if(undistort)
{
CvMat* I = cvCreateMat(1, 1, CV_32FC2);
CvMat* Io = cvCreateMat(1, 1, CV_32FC2);
cvSet2D(I, 0, 0, cvScalar(u,v));
cvUndistortPoints(I, Io, m_CameraMatrix, m_DistCoeffs, NULL, m_CameraMatrixNorm);
CvScalar s = cvGet2D(Io, 0, 0);
xx = s.val[0];//cvGetReal1D(Io, 0);
yy = s.val[1];//cvGetReal1D(Io, 1);
cvReleaseMat(&I);
cvReleaseMat(&Io);
}
else
{
xx = u;
yy = v;
}
xx = (xx - cvGetReal2D(m_CameraMatrixNorm, 0, 2))/cvGetReal2D(m_CameraMatrixNorm, 0, 0);
yy = (yy - cvGetReal2D(m_CameraMatrixNorm, 1, 2))/cvGetReal2D(m_CameraMatrixNorm, 1, 1);
cvSetReal1D(r, 0, xx);
cvSetReal1D(r, 1, yy);
cvSetReal1D(r, 2, 1.0);
/* Rt_(3,:)*r = sum of third column of R times elements of r */
t = xx*cvGetReal2D(m_R, 0, 2) + yy*cvGetReal2D(m_R, 1, 2) + cvGetReal2D(m_R, 2, 2);
if(t == 0)
{
t = 1.0;
}
if(d <= 0)
{
/* d<= 0 => above water surface */
t = (-m_dCameraHeightAboveWater-d)/t;
/* r = t*R'*r + C */
cvGEMM(m_R, r, t, m_CameraWorld, 1.0, r, CV_GEMM_A_T);
}
else
{
/* d > 0 => below water surface */
t = -m_dCameraHeightAboveWater/t;
/* S = t*R'*r */
cvGEMM(m_R, r, t, NULL, 0, m_S, CV_GEMM_A_T);
double Sx = cvGetReal1D(m_S, 0);
double Sy = cvGetReal1D(m_S, 1);
double phi = atan2(Sy, Sx);
double rS = sqrt(Sx*Sx + Sy*Sy);
double rP = calculateRpFromRs(rS, d, m_dCameraHeightAboveWater);
cvSetReal1D(r, 0, rP*cos(phi));
cvSetReal1D(r, 1, rP*sin(phi));
cvSetReal1D(r, 2, -m_dCameraHeightAboveWater-d);
cvAdd(r, m_CameraWorld, r);
}
*x = cvGetReal1D(r, 0);
*y = cvGetReal1D(r, 1);
*z = cvGetReal1D(r, 2);
cvReleaseMat(&r);
}