ColorImage* remap(const ColorImage& src, const FloatImage &px, const FloatImage &py, ColorImage* dst, int interpolation)
{
#ifdef NICE_USELIB_IPP
ColorImage ci(src);
ColorImage::Pixel* cursor = ci.getPixelPointerY(0);
*cursor++ = 0;
*cursor++ = 0;
*cursor = 0;
ColorImage * result = createResultBuffer(ci.width(), ci.height(), dst);
IppStatus ret = ippiRemap_8u_C3R(ci.getPixelPointer(), makeROIFullImage(ci), ci.getStepsize(),
makeRectFullImage(ci),
px.getPixelPointer(), px.getStepsize(),
py.getPixelPointer(), py.getStepsize(),
result->getPixelPointer(), result->getStepsize(),
makeROIFullImage(ci), interpolation);
if(ret!=ippStsNoErr)
fthrow(ImageException, ippGetStatusString(ret));
return result;
#else // NICE_USELIB_IPP
fthrow(ImageException,"Not yet supported without IPP.");
#endif // NICE_USELIB_IPP
}
void output(int sampling, string outputFileName[], Viewport &viewport)
{
ColorImage image;
Pixel p = { 0, 0, 0 };
image.init(viewport.width, viewport.width);
for (int i = 0; i < viewport.height; i++)
{
for (int j = 0; j < viewport.width; j++)
{
viewport.pixel[i][j] *= 255;
for (int k = 0; k < 3; k++)
{
if (viewport.pixel[i][j][k] > 255)
{
viewport.pixel[i][j][k] = 255;
}
}
p.R = viewport.pixel[i][j][0];
p.G = viewport.pixel[i][j][1];
p.B = viewport.pixel[i][j][2];
image.writePixel(j, i, p);
}
}
image.outputPPM(const_cast<char*>((outputFileName[sampling] + ".ppm").c_str()));
}
int main()
{
readFile();
setRay();
ColorImage image;
image.init(Rw, Rh);
Pixel pixelColor;
for (int i = 0; i < Rw; ++i) {
for (int j = 0; j < Rh; ++j) {
/// generate the ray.
vec3 ray = dir + i*hori - j*vert;
ray = ray.normalize();
bestDistance = 5;
/// test if it intersected with all triangles.
for (unsigned int t = 0; t < Triangle.size(); t+=3) {
if( isIntersectedTri(ray, Triangle[t], Triangle[t+1], Triangle[t+2]) ) {
pixelColor = {color, color, color};
image.writePixel(i, j, pixelColor);
}
}
/// test if it intersected with all spheres.
// for (unsigned int s = 0; s < Sphere.size(); ++s) {
// if( isIntersectedSph(ray, Sphere[s]) ) {
// image.writePixel(i, j, pixelColor);
// }
// }
}
}
char outputname[15] = "hw1_output.ppm";
image.outputPPM(outputname);
return 0;
}
void ImageFile::readerPNG(GrayColorImageCommonImplementationT<Ipp32f> *image)
{
switch(image->channels())
{
case 1:
{
Image buffer;
readerPNG(&buffer);
if(image->widthInline()!=buffer.width() || image->heightInline()!=buffer.height()) {
image->resize(buffer.width(), buffer.height());
}
grayToFloat(buffer,dynamic_cast<ImageT<Ipp32f>* >(image));
break;
}
case 3:
{
ColorImage buffer;
readerPNG(&buffer);
if(image->widthInline()!=buffer.width() || image->heightInline()!=buffer.height()) {
image->resize(buffer.width(), buffer.height());
}
convertBitDepth(buffer,dynamic_cast<ColorImageT<Ipp32f>* >(image));
break;
}
default: fthrow(ImageException,"No or invalid color image->channels()");
break;
}
}
ColorImage* rgbToHSV(const ColorImage& src, ColorImage* dst)
{
ColorImage* result = createResultBuffer(src, dst);
#ifdef NICE_USELIB_IPP
IppStatus ret = ippiRGBToHSV_8u_C3R(src.getPixelPointer(), src.getStepsize(),
result->getPixelPointer(), result->getStepsize(),
makeROIFullImage(src));
if(ret!=ippStsNoErr)
fthrow(ImageException, ippGetStatusString(ret));
#else // NICE_USELIB_IPP
double min,max,diff, r,g,b, h;
const Ipp8u* pSrcStart = src.getPixelPointer();
Ipp8u* pDstStart = result->getPixelPointer();
const Ipp8u* pSrc;
Ipp8u* pDst;
for(int y=0; y<src.height(); ++y) {
pSrc = pSrcStart;
pDst = pDstStart;
for(int x=0; x<src.width(); ++x) {
r = *pSrc/255.0; ++pSrc;
g = *pSrc/255.0; ++pSrc;
b = *pSrc/255.0; ++pSrc;
min = std::min(g,b);
min = std::min(r, min);
max = std::max(g,b);
max = std::max(r, max);
diff = max-min;
// H
h = 0;
if(diff!=0) {
if(max==r) { h = ((g-b)/diff )*60; }
else if(max==g) { h = ((b-r)/diff + 2)*60; }
else if(max==b) { h = ((r-g)/diff + 4)*60; }
}
h += (h<0)?360:0;
*pDst = static_cast<unsigned char>(h*17/24); ++pDst; // *255/360
*pDst = static_cast<unsigned char>((max==0)?0:(diff*255)/max); ++pDst;
*pDst = static_cast<unsigned char>(max*255); ++pDst; // v = max;
}
pSrcStart += src.getStepsize();
pDstStart += result->getStepsize();
}
#endif // NICE_USELIB_IPP
return result;
}
ColorImage* yuvToRGB(const ColorImage& src, ColorImage* dst)
{
ColorImage* result = createResultBuffer(src.width(), src.height(), dst);
#ifdef NICE_USELIB_IPP
IppStatus ret = ippiYUVToRGB_8u_C3R(src.getPixelPointer(), src.getStepsize(),
result->getPixelPointer(), result->getStepsize(),
makeROIFullImage(src));
if(ret!=ippStsNoErr)
fthrow(ImageException, ippGetStatusString(ret));
#else // NICE_USELIB_IPP
double y,u,v, r,g,b;
const Ipp8u* pSrcStart = src.getPixelPointer();
Ipp8u* pDstStart = result->getPixelPointer();
const ColorImage::Pixel* pSrc;
ColorImage::Pixel* pDst;
for(int ty=0; ty<src.height(); ++ty) {
pSrc = pSrcStart;
pDst = pDstStart;
for(int tx=0; tx<src.width(); ++tx) {
y = *pSrc/255.0; ++pSrc;
u = *pSrc/255.0-0.5; ++pSrc;
v = *pSrc/255.0-0.5; ++pSrc;
r = y + 1.140*v;
g = y - 0.394*u - 0.581*v;
b = y + 2.032*u;
r = std::min(1.0, r);
r = std::max(0.0, r);
g = std::min(1.0, g);
g = std::max(0.0, g);
b = std::min(1.0, b);
b = std::max(0.0, b);
*pDst = static_cast<Ipp8u>(r*255); ++pDst;
*pDst = static_cast<Ipp8u>(g*255); ++pDst;
*pDst = static_cast<Ipp8u>(b*255); ++pDst;
}
pSrcStart += src.getStepsize();
pDstStart += result->getStepsize();
}
#endif // NICE_USELIB_IPP
return result;
}
ColorImage* rgbToYUV(const ColorImage& src, ColorImage* dst)
{
ColorImage* result = createResultBuffer(src.width(), src.height(), dst);
#ifdef NICE_USELIB_IPP
IppStatus ret = ippiRGBToYUV_8u_C3R(src.getPixelPointer(), src.getStepsize(),
result->getPixelPointer(), result->getStepsize(),
makeROIFullImage(src));
if(ret!=ippStsNoErr)
fthrow(ImageException, ippGetStatusString(ret));
#else // NICE_USELIB_IPP
double r,g,b, y,u,v;
const Ipp8u* pSrcStart = src.getPixelPointer();
Ipp8u* pDstStart = result->getPixelPointer();
const ColorImage::Pixel* pSrc;
ColorImage::Pixel* pDst;
for(int ty=0; ty<src.height(); ++ty) {
pSrc = pSrcStart;
pDst = pDstStart;
for(int tx=0; tx<src.width(); ++tx) {
r = *pSrc/255.0; ++pSrc;
g = *pSrc/255.0; ++pSrc;
b = *pSrc/255.0; ++pSrc;
y = 0.299*r + 0.587*g + 0.114*b;
u = 0.492*(b-y);
v = 0.877*(r-y);
v+= 0.5;
v = std::max(0.0, v);
v = std::min(1.0, v);
*pDst = static_cast<Ipp8u>(y*255); ++pDst;
*pDst = static_cast<Ipp8u>((u+0.5)*255); ++pDst;
*pDst = static_cast<Ipp8u>(v*255); ++pDst;
}
pSrcStart += src.getStepsize();
pDstStart += result->getStepsize();
}
#endif // NICE_USELIB_IPP
return result;
}
int main ( int argc, char **argv )
{
cout << "converting and cropping (not resizing!) tool (c) Erik Rodner, 2011" << endl;
if ( (argc != 3) && (argc != 5) ) {
cerr << "usage: " << argv[0] << " <src> <dst> [width] [height]" << endl;
exit(-1);
}
ColorImage src ( argv[1] );
if ( argc == 3 )
{
src.write(argv[2]);
} else {
int xsize = std::min( atoi(argv[3]), src.width() );
int ysize = std::min( atoi(argv[4]), src.height() );
src.createSubImage(Rect(0,0,xsize,ysize))->write ( argv[2] );
}
}
std::vector<std::string > MDApp::process_frame(ColorImage& frame)
{
assert(has_been_initialized_ && "Must be initialized before calling process_frame!");
assert(frame.type()==CV_8UC3 && "Frame type must be RGB!");
clock_t beginTime;
clock_t endTime;
double elapsed_secs;
cv::Mat grayscaleImage;
convert_to_grayscale(frame, grayscaleImage);
std::vector<cv::Mat> digitPatches;
std::vector<std::vector<int> > numberIndices;
beginTime = clock();
seg_.segment(grayscaleImage, digitPatches, numberIndices);
endTime = clock();
elapsed_secs = double(endTime - beginTime) / CLOCKS_PER_SEC;
LOG_DEBUG(TAG, "segmentation %f sec.", elapsed_secs);
beginTime = clock();
std::vector<char> digits = net_.extract_digits(digitPatches, useBatch_);
endTime = clock();
elapsed_secs = double(endTime - beginTime) / CLOCKS_PER_SEC;
LOG_DEBUG(TAG, "recognition %f sec.", elapsed_secs);
beginTime = clock();
std::vector<std::string > result;
// back tracing the indices of digits to form the number strings
for(const auto& digitIndices : numberIndices) {
size_t stringLen = digitIndices.size();
std::string s(stringLen, 'x');
for(int i = 0; i < stringLen; ++i)
s[i] = digits[digitIndices[i]];
result.push_back(s);
}
endTime = clock();
elapsed_secs = double(endTime - beginTime) / CLOCKS_PER_SEC;
LOG_DEBUG(TAG, "post-processing %f sec.", elapsed_secs);
return result;
}
void MotionBlur::applyEffect(ColorImage& image, KinectData& kinectData, const GrayImage& handsMask, int timeElapsed) {
for (int y = 0; y < image.rows; y++) {
for (int x = 0; x < image.cols; x++) {
// Leave the area where hand currently is alone
if (handsMask.data[y][x] != kMaskUnoccupied)
continue;
unsigned int blendRed = 0;
unsigned int blendGreen = 0;
unsigned int blendBlue = 0;
unsigned int blendSamplesUsed = 0;
// Blend from each buffer
for (int buf = 0; buf < historyLength; buf++) {
// Include only pixels where hand was
if (maskHistory[buf].data[y][x] != kMaskUnoccupied) {
blendRed += imgHistory[buf].data[y][x].red;
blendGreen += imgHistory[buf].data[y][x].green;
blendBlue += imgHistory[buf].data[y][x].blue;
blendSamplesUsed++;
}
}
// If any blending to be done, average the current pixel and the old ones.
// Current frame makes up half the contribution, with the other half coming from all the other frames combined.
if (blendSamplesUsed > 0) {
blendRed /= blendSamplesUsed;
blendGreen /= blendSamplesUsed;
blendBlue /= blendSamplesUsed;
image.data[y][x].red = (image.data[y][x].red + blendRed) / 2;
image.data[y][x].green = (image.data[y][x].green + blendGreen) / 2;
image.data[y][x].blue = (image.data[y][x].blue + blendBlue) / 2;
}
}
}
// Store current frame in buffers and increment position in ring
image.CopyTo(imgHistory[curBuffer]);
handsMask.CopyTo(maskHistory[curBuffer]);
curBuffer++;
curBuffer = curBuffer % historyLength;
}
int Webcam::getFrame(ColorImage &image)
{
int ret = 0;
// Dequeue a buffer.
ret = ioctl(dev, VIDIOC_DQBUF, &buf);
if (ret < 0)
{
KError("Unable to dequeue buffer", errno);
return EXIT_FAILURE;
}
// Save the image.
//uchar jpegBuf1[buf.bytesused + 420];
if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG)
{
/*
if (mjpegToJpeg(mem[buf.index], jpegBuf1, (int) buf.bytesused) == EXIT_SUCCESS)
image.loadFromData(jpegBuf1, buf.bytesused+420);
*/
return EXIT_FAILURE;
}
if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV)
{
image.setImage(yuvToRGB(mem[buf.index], currentWidth(), currentHeight()), currentWidth(), currentHeight());
}
// Requeue the buffer.
ret = ioctl(dev, VIDIOC_QBUF, &buf);
if (ret < 0)
{
KError("Unable to requeue buffer", errno);
return EXIT_FAILURE;
}
if(!imageNotifier->isEnabled())
imageNotifier->setEnabled(true);
return EXIT_SUCCESS;
}
ColorImage* hsvToRGB(const ColorImage& src, ColorImage* dst)
{
ColorImage* result = createResultBuffer(src.width(), src.height(), dst);
#ifdef NICE_USELIB_IPP
IppStatus ret = ippiHSVToRGB_8u_C3R(src.getPixelPointer(), src.getStepsize(),
result->getPixelPointer(), result->getStepsize(),
makeROIFullImage(src));
if(ret!=ippStsNoErr)
fthrow(ImageException, ippGetStatusString(ret));
#else // NICE_USELIB_IPP
double h,s,v, r,g,b, k,m,n, f;
int i;
const Ipp8u* pSrcStart = src.getPixelPointer();
Ipp8u* pDstStart = result->getPixelPointer();
const ColorImage::Pixel* pSrc;
ColorImage::Pixel* pDst;
for(int y=0; y<src.height(); ++y) {
pSrc = pSrcStart;
pDst = pDstStart;
for(int x=0; x<src.width(); ++x) {
h =(*pSrc/255.0)*360; ++pSrc;
s = *pSrc/255.0; ++pSrc;
v = *pSrc/255.0; ++pSrc;
r = g = b = v; // default case if s==0
if(s!=0) {
h = (h==360)?0:h/60;
i = static_cast<int>(h);
f = h-i;
m = v*(1-s);
n = v*(1-s*f);
k = v*(1-s*(1-f));
switch(i) {
case 0: r = v; g = k; b = m; break;
case 1: r = n; g = v; b = m; break;
case 2: r = m; g = v; b = k; break;
case 3: r = m; g = n; b = v; break;
case 4: r = k; g = m; b = v; break;
case 5: r = v; g = m; b = n; break;
}
}
*pDst = static_cast<Ipp8u>(r*255); ++pDst;
*pDst = static_cast<Ipp8u>(g*255); ++pDst;
*pDst = static_cast<Ipp8u>(b*255); ++pDst;
}
pSrcStart += src.getStepsize();
pDstStart += result->getStepsize();
}
#endif // NICE_USELIB_IPP
return(result);
}
void mtdCameraCode(void)
{
Threshold threshold(0, 255, 0, 255, 221, 255);
ParticleFilterCriteria2 criteria[] = {{IMAQ_MT_AREA, AREA_MINIMUM, 65535, false, false}};
AxisCamera &camera = AxisCamera::GetInstance("10.26.3.11");
camera.WriteResolution(AxisCamera::kResolution_320x240);
camera.WriteCompression(20);
camera.WriteBrightness(50);
//SmartDashboard::PutNumber("Test", 3);
if(timerCamera.Get() > 0.1)
{
ColorImage *image;
//image = new RGBImage("/HybridLine_DoubleGreenBK3.jpg"); // get the sample image from the cRIO flash
image = camera.GetImage();
//camera.GetImage(image); //To get the images from the camera comment the line above and uncomment this one
//Wait(.1);
//SmartDashboard::PutNumber("Test", 4);
BinaryImage *thresholdImage = image->ThresholdHSV(threshold); // get just the green target pixels
// thresholdImage->Write("/threshold.bmp");
//SmartDashboard::PutNumber("Test", 5);
BinaryImage *convexHullImage = thresholdImage->ConvexHull(false); // fill in partial and full rectangles
// convexHullImage->Write("ConvexHull.bmp");
//SmartDashboard::PutNumber("Test", 6);
BinaryImage *filteredImage = convexHullImage->ParticleFilter(criteria, 1); //Remove small particles
// filteredImage->Write("/Filtered.bmp");
//SmartDashboard::PutNumber("Test", 7);
vector<ParticleAnalysisReport> *reports = filteredImage->GetOrderedParticleAnalysisReports(); //get a particle analysis report for each particle
//SmartDashboard::PutNumber("Test", 8);
int size = reports->size();
scores = new Scores[size];
//SmartDashboard::PutNumber("Test", 9);
//Iterate through each particle, scoring it and determining whether it is a target or not
for (unsigned i = 0; i < reports->size(); i++)
{
//SmartDashboard::PutNumber("Test", 10);
ParticleAnalysisReport *report = &(reports->at(i));
scores[i].rectangularity = scoreRectangularity(report);
scores[i].aspectRatioOuter = scoreAspectRatio(filteredImage, report, true);
scores[i].aspectRatioInner = scoreAspectRatio(filteredImage, report, false);
scores[i].xEdge = scoreXEdge(thresholdImage, report);
scores[i].yEdge = scoreYEdge(thresholdImage, report);
if(scoreCompare(scores[i], false))
{
//We hit this!! Note to self: changethe below printf statement
//To use SmartDashboard::PutString so wecan seevalues.
//printf("particle: %d is a High Goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
//string particle = ("particle: %d is a High Goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
SmartDashboard::PutNumber("CenterX", report->center_mass_x);
SmartDashboard::PutNumber("CenterY", report->center_mass_y);
SmartDashboard::PutNumber("Area", report->particleArea);
SmartDashboard::PutNumber("Distance",computeDistance(thresholdImage,report, false));
SmartDashboard::PutNumber("size", reports->size());
SmartDashboard::PutNumber("height", report->boundingRect.height);
SmartDashboard::PutNumber("Quality", report->particleQuality);
//SmartDashboard::PutNumber("Test",computeDistance(thresholdImage, report, false));
//SmartDashboard::PutNumber("Distance",computeDistance(thresholdImage,report, false));
SmartDashboard::PutString("high goal detected", "asdf");
}
else if (scoreCompare(scores[i], true))
{
printf("particle: %d is a Middle Goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
SmartDashboard::PutNumber("Test", computeDistance(thresholdImage, report, true));
SmartDashboard::PutNumber("CenterX", report->center_mass_x);
SmartDashboard::PutNumber("CenterY", report->center_mass_y);
SmartDashboard::PutNumber("height", report->boundingRect.height);
SmartDashboard::PutNumber("Distance",computeDistance(thresholdImage,report, false));
SmartDashboard::PutString("middle goal detected", "adsf");
}
else
{
printf("particle: %d is not a goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
SmartDashboard::PutNumber("CenterX", report->center_mass_x);
SmartDashboard::PutNumber("CenterY", report->center_mass_y);
SmartDashboard::PutNumber("height", report->boundingRect.height);
SmartDashboard::PutNumber("Distance",computeDistance(thresholdImage,report, false));
SmartDashboard::PutString("we areinelse", "else");
}
if(report->center_mass_x < 85.00)
{
SmartDashboard::PutString("Pausing", "paused");
//image->Write("C:\\testimg.bmp");
//Wait(10);
}
printf("rect: %f ARinner: %f \n", scores[i].rectangularity, scores[i].aspectRatioInner);
printf("ARouter: %f xEdge: %f yEdge: %f \n", scores[i].aspectRatioOuter, scores[i].xEdge, scores[i].yEdge);
}
printf("\n");
// be sure to delete images after using them
delete filteredImage;
delete convexHullImage;
delete thresholdImage;
delete image;
//delete allocated reports and Scores objects also
delete scores;
delete reports;
}
timerCamera.Reset();
}
void ImageTest::testColorImage()
{
ColorImage image(4,3);
CPPUNIT_ASSERT_EQUAL(4, image.width());
CPPUNIT_ASSERT_EQUAL(3, image.height());
CPPUNIT_ASSERT_THROW(image.setPixel(4, 2, 1, 11), ImageException);
CPPUNIT_ASSERT_THROW(image.setPixel(3, 3, 2, 12), ImageException);
CPPUNIT_ASSERT_NO_THROW(image.setPixelSave(4, 2, 11, 12, 13));
CPPUNIT_ASSERT_NO_THROW(image.setPixelSave(3, 3, 11, 12, 13));
CPPUNIT_ASSERT_THROW(image.getPixel(4, 2, 1), ImageException);
CPPUNIT_ASSERT_THROW(image.getPixel(3, 3, 2), ImageException);
image.setPixel(0, 0, 11, 12, 13);
image.setPixel(3, 2, 25, 26, 27);
CPPUNIT_ASSERT_EQUAL(11, (int) image.getPixel(0, 0, 0));
CPPUNIT_ASSERT_EQUAL(12, (int) image.getPixel(0, 0, 1));
CPPUNIT_ASSERT_EQUAL(13, (int) image.getPixel(0, 0, 2));
CPPUNIT_ASSERT_EQUAL(25, (int) image.getPixel(3, 2, 0));
CPPUNIT_ASSERT_EQUAL(26, (int) image.getPixel(3, 2, 1));
CPPUNIT_ASSERT_EQUAL(27, (int) image.getPixel(3, 2, 2));
CPPUNIT_ASSERT_EQUAL(25, (int) *image.getPixelPointerXY(3, 2));
ColorImage image2(image, GrayColorImageCommonImplementation::noAlignment);
CPPUNIT_ASSERT_EQUAL(26, (int) image.getPixel(3, 2, 1));
CPPUNIT_ASSERT_EQUAL(26, (int) image2.getPixel(3, 2, 1));
// set
ColorImage image3(10,10);
image3.set(15,64,253);
for(int y=0; y<image3.height(); ++y)
for(int x=0; x<image3.width(); ++x)
{
CPPUNIT_ASSERT_EQUAL( 15, static_cast<int>(image3.getPixelQuick(x,y,0)));
CPPUNIT_ASSERT_EQUAL( 64, static_cast<int>(image3.getPixelQuick(x,y,1)));
CPPUNIT_ASSERT_EQUAL(253, static_cast<int>(image3.getPixelQuick(x,y,2)));
}
// operator =, ==, !=
{
ColorImage img(2,2);
img.set(0,0,0);
img.setPixelQuick(1,1,0,133);
ColorImage fimg(3,2);
CPPUNIT_ASSERT_THROW(fimg==img, ImageException);
CPPUNIT_ASSERT_THROW(fimg!=img, ImageException);
ColorImage timg;
timg = img;
CPPUNIT_ASSERT_NO_THROW(img==timg);
CPPUNIT_ASSERT_EQUAL(true, img==timg);
CPPUNIT_ASSERT_EQUAL(false, img!=timg);
timg.setPixelQuick(0,0,3,99);
CPPUNIT_ASSERT_NO_THROW(img!=timg);
CPPUNIT_ASSERT_EQUAL(true, img!=timg);
CPPUNIT_ASSERT_EQUAL(false, img==timg);
}
// Kontruktor: create a ColorImageT from 3 Channel Images ImageT
{
// exception
{
Image chan1, chan2, chan3;
chan1 = Image(5,6);
chan2 = Image(6,7);
chan3 = Image(7,8);
CPPUNIT_ASSERT_THROW(ColorImage c(chan1,chan2,chan3), ImageException);
}
// <Ipp8u>
{
Image chan1(5,5), chan2(5,5), chan3(5,5);
chan1.set(100);
chan2.set(150);
chan3.set(200);
for(int y=0; y<chan1.height(); ++y)
for(int x=0; x<chan1.width(); ++x)
{
CPPUNIT_ASSERT_EQUAL(100, static_cast<int>(chan1.getPixelQuick(x,y)));
CPPUNIT_ASSERT_EQUAL(150, static_cast<int>(chan2.getPixelQuick(x,y)));
CPPUNIT_ASSERT_EQUAL(200, static_cast<int>(chan3.getPixelQuick(x,y)));
}
ColorImage c(chan1,chan2,chan3);
CPPUNIT_ASSERT_EQUAL(static_cast<int>(c.width()), static_cast<int>(chan1.width()));
CPPUNIT_ASSERT_EQUAL(static_cast<int>(c.height()), static_cast<int>(chan1.height()));
for(int y=0; y<c.height(); ++y)
for(int x=0; x<c.width(); ++x)
{
CPPUNIT_ASSERT_EQUAL(100, static_cast<int>(c.getPixelQuick(x,y,0)));
CPPUNIT_ASSERT_EQUAL(150, static_cast<int>(c.getPixelQuick(x,y,1)));
CPPUNIT_ASSERT_EQUAL(200, static_cast<int>(c.getPixelQuick(x,y,2)));
}
}
// FloatImage
{
FloatImage chan1(5,5), chan2(5,5), chan3(5,5);
chan1.set( -50.0);
chan2.set(1000.0);
chan3.set( 1.0);
for(int y=0; y<chan1.height(); ++y)
for(int x=0; x<chan1.width(); ++x)
{
CPPUNIT_ASSERT_EQUAL( -50.0, static_cast<double>(chan1.getPixelQuick(x,y)));
CPPUNIT_ASSERT_EQUAL(1000.0, static_cast<double>(chan2.getPixelQuick(x,y)));
CPPUNIT_ASSERT_EQUAL( 1.0, static_cast<double>(chan3.getPixelQuick(x,y)));
}
ColorImageT<float> c(chan1,chan2,chan3);
for(int y=0; y<c.height(); ++y)
for(int x=0; x<c.width(); ++x)
{
CPPUNIT_ASSERT_DOUBLES_EQUAL( -50.0, static_cast<double>(c.getPixelQuick(x,y,0)), 0.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1000.0, static_cast<double>(c.getPixelQuick(x,y,1)), 0.0);
CPPUNIT_ASSERT_DOUBLES_EQUAL( 1.0, static_cast<double>(c.getPixelQuick(x,y,2)), 0.0);
}
}
// Ipp16s
{
ImageT<Ipp16s> chan1(5,5), chan2(5,5), chan3(5,5);
chan1.set(-255);
chan2.set(+255);
chan3.set( 0);
for(int y=0; y<chan1.height(); ++y)
for(int x=0; x<chan1.width(); ++x)
{
CPPUNIT_ASSERT_EQUAL(-255, static_cast<int>(chan1.getPixelQuick(x,y)));
CPPUNIT_ASSERT_EQUAL(+255, static_cast<int>(chan2.getPixelQuick(x,y)));
CPPUNIT_ASSERT_EQUAL( 0, static_cast<int>(chan3.getPixelQuick(x,y)));
}
ColorImageT<Ipp16s> c(chan1,chan2,chan3);
for(int y=0; y<c.height(); ++y)
for(int x=0; x<c.width(); ++x)
{
CPPUNIT_ASSERT_EQUAL(-255, static_cast<int>(c.getPixelQuick(x,y,0)));
CPPUNIT_ASSERT_EQUAL(+255, static_cast<int>(c.getPixelQuick(x,y,1)));
CPPUNIT_ASSERT_EQUAL( 0, static_cast<int>(c.getPixelQuick(x,y,2)));
}
}
}
// getChannel
{
// exception
{
ColorImage c(5,5);
CPPUNIT_ASSERT_NO_THROW(c.getChannel(0));
CPPUNIT_ASSERT_NO_THROW(c.getChannel(1));
CPPUNIT_ASSERT_NO_THROW(c.getChannel(2));
CPPUNIT_ASSERT_THROW(c.getChannel( 3), ImageException);
CPPUNIT_ASSERT_THROW(c.getChannel( 4), ImageException);
}
// <Ipp8u>
{
ColorImage c(5,5);
for(int y=0; y<c.height(); ++y)
for(int x=0; x<c.width(); ++x)
c.setPixelQuick(x,y, 100,150,200);
Image* g0 = c.getChannel(0);
Image* g1 = c.getChannel(1);
Image* g2 = c.getChannel(2);
for(int y=0; y<c.height(); ++y)
for(int x=0; x<c.width(); ++x)
{
CPPUNIT_ASSERT_EQUAL(100, static_cast<int>(g0->getPixelQuick(x,y)));
CPPUNIT_ASSERT_EQUAL(150, static_cast<int>(g1->getPixelQuick(x,y)));
CPPUNIT_ASSERT_EQUAL(200, static_cast<int>(g2->getPixelQuick(x,y)));
}
}
// FloatImage
{
ColorImageT<float> c(5,5);
for(int y=0; y<c.height(); ++y)
for(int x=0; x<c.width(); ++x)
c.setPixelQuick(x,y, -1000.125, 1.32678, +999.999);
FloatImage* g0 = c.getChannel(0);
FloatImage* g1 = c.getChannel(1);
FloatImage* g2 = c.getChannel(2);
for(int y=0; y<c.height(); ++y)
for(int x=0; x<c.width(); ++x)
{
CPPUNIT_ASSERT_DOUBLES_EQUAL(-1000.125, static_cast<double>(g0->getPixelQuick(x,y)), 0.001);
CPPUNIT_ASSERT_DOUBLES_EQUAL(1.32678 , static_cast<double>(g1->getPixelQuick(x,y)), 0.00001);
CPPUNIT_ASSERT_DOUBLES_EQUAL(999.999 , static_cast<double>(g2->getPixelQuick(x,y)), 0.001);
}
}
// <Ipp16s>
{
ColorImageT<Ipp16s> c(5,5);
for(int y=0; y<c.height(); ++y)
for(int x=0; x<c.width(); ++x)
c.setPixelQuick(x,y, -255, 0, +255);
GrayImage16s* g0 = c.getChannel(0);
GrayImage16s* g1 = c.getChannel(1);
GrayImage16s* g2 = c.getChannel(2);
for(int y=0; y<c.height(); ++y)
for(int x=0; x<c.width(); ++x)
{
CPPUNIT_ASSERT_EQUAL(-255, static_cast<int>(g0->getPixelQuick(x,y)));
CPPUNIT_ASSERT_EQUAL( 0, static_cast<int>(g1->getPixelQuick(x,y)));
CPPUNIT_ASSERT_EQUAL(+255, static_cast<int>(g2->getPixelQuick(x,y)));
}
}
}
// mirror
ColorImage image4(5,5);
for(int y=0; y<image4.height(); ++y)
for(int x=0; x<image4.width(); ++x)
image4.setPixelQuick(x,y,1*(x+1+y*5),3*(x+1+y*5),6*(x+1+y*5));
image4.mirror(ippAxsHorizontal);
CPPUNIT_ASSERT_EQUAL( 21, static_cast<int>(image4.getPixelQuick(0,0,0)));
CPPUNIT_ASSERT_EQUAL( 16, static_cast<int>(image4.getPixelQuick(0,1,0)));
CPPUNIT_ASSERT_EQUAL( 11, static_cast<int>(image4.getPixelQuick(0,2,0)));
CPPUNIT_ASSERT_EQUAL( 6, static_cast<int>(image4.getPixelQuick(0,3,0)));
CPPUNIT_ASSERT_EQUAL( 1, static_cast<int>(image4.getPixelQuick(0,4,0)));
CPPUNIT_ASSERT_EQUAL( 22, static_cast<int>(image4.getPixelQuick(1,0,0)));
CPPUNIT_ASSERT_EQUAL( 17, static_cast<int>(image4.getPixelQuick(1,1,0)));
CPPUNIT_ASSERT_EQUAL( 12, static_cast<int>(image4.getPixelQuick(1,2,0)));
CPPUNIT_ASSERT_EQUAL( 7, static_cast<int>(image4.getPixelQuick(1,3,0)));
CPPUNIT_ASSERT_EQUAL( 2, static_cast<int>(image4.getPixelQuick(1,4,0)));
image4.mirror(ippAxsHorizontal);
image4.mirror(ippAxsVertical);
CPPUNIT_ASSERT_EQUAL( 5, static_cast<int>(image4.getPixelQuick(0,0,0)));
CPPUNIT_ASSERT_EQUAL( 10, static_cast<int>(image4.getPixelQuick(0,1,0)));
CPPUNIT_ASSERT_EQUAL( 15, static_cast<int>(image4.getPixelQuick(0,2,0)));
CPPUNIT_ASSERT_EQUAL( 20, static_cast<int>(image4.getPixelQuick(0,3,0)));
CPPUNIT_ASSERT_EQUAL( 25, static_cast<int>(image4.getPixelQuick(0,4,0)));
CPPUNIT_ASSERT_EQUAL( 4, static_cast<int>(image4.getPixelQuick(1,0,0)));
CPPUNIT_ASSERT_EQUAL( 9, static_cast<int>(image4.getPixelQuick(1,1,0)));
CPPUNIT_ASSERT_EQUAL( 14, static_cast<int>(image4.getPixelQuick(1,2,0)));
CPPUNIT_ASSERT_EQUAL( 19, static_cast<int>(image4.getPixelQuick(1,3,0)));
CPPUNIT_ASSERT_EQUAL( 24, static_cast<int>(image4.getPixelQuick(1,4,0)));
image4.mirror(ippAxsVertical);
image4.mirror(ippAxsBoth);
CPPUNIT_ASSERT_EQUAL( 25, static_cast<int>(image4.getPixelQuick(0,0,0)));
CPPUNIT_ASSERT_EQUAL( 19, static_cast<int>(image4.getPixelQuick(1,1,0)));
CPPUNIT_ASSERT_EQUAL( 13, static_cast<int>(image4.getPixelQuick(2,2,0)));
CPPUNIT_ASSERT_EQUAL( 7, static_cast<int>(image4.getPixelQuick(3,3,0)));
CPPUNIT_ASSERT_EQUAL( 1, static_cast<int>(image4.getPixelQuick(4,4,0)));
CPPUNIT_ASSERT_EQUAL( 21, static_cast<int>(image4.getPixelQuick(4,0,0)));
CPPUNIT_ASSERT_EQUAL( 17, static_cast<int>(image4.getPixelQuick(3,1,0)));
CPPUNIT_ASSERT_EQUAL( 13, static_cast<int>(image4.getPixelQuick(2,2,0)));
CPPUNIT_ASSERT_EQUAL( 9, static_cast<int>(image4.getPixelQuick(1,3,0)));
CPPUNIT_ASSERT_EQUAL( 5, static_cast<int>(image4.getPixelQuick(0,4,0)));
image4.mirror(ippAxsBoth);
// transpose
image4.transpose();
for(int y=0; y<image4.height(); ++y)
for(int x=0; x<image4.width(); ++x)
{
CPPUNIT_ASSERT_EQUAL( 1*(y+1+x*5), static_cast<int>(image4.getPixelQuick(x,y,0)));
CPPUNIT_ASSERT_EQUAL( 3*(y+1+x*5), static_cast<int>(image4.getPixelQuick(x,y,1)));
CPPUNIT_ASSERT_EQUAL( 6*(y+1+x*5), static_cast<int>(image4.getPixelQuick(x,y,2)));
}
// copyRect
image4 = ColorImage(10,10);
for(int y=0; y<image4.width(); ++y)
for(int x=0; x<image4.width(); ++x)
image4.setPixelQuick(x,y,x+1+y*image4.width(),(x+1+y*image4.width())/2,(x+1+y*image4.width())/3);
ColorImage* imgRect = image4.copyRect(Rect(2,3,4,3));
CPPUNIT_ASSERT_EQUAL(4, static_cast<int>(imgRect->width()));
CPPUNIT_ASSERT_EQUAL(3, static_cast<int>(imgRect->height()));
for(int y=0; y<imgRect->height(); ++y)
for(int x=0; x<imgRect->width(); ++x)
{
CPPUNIT_ASSERT_EQUAL(((x+3)+(y+3)*image4.width())/1, static_cast<int>(imgRect->getPixelQuick(x,y,0)));
CPPUNIT_ASSERT_EQUAL(((x+3)+(y+3)*image4.width())/2, static_cast<int>(imgRect->getPixelQuick(x,y,1)));
CPPUNIT_ASSERT_EQUAL(((x+3)+(y+3)*image4.width())/3, static_cast<int>(imgRect->getPixelQuick(x,y,2)));
}
imgRect = image4.copyRect(Rect(7,6,10,10));
CPPUNIT_ASSERT_EQUAL( 3, static_cast<int>(imgRect->width()));
CPPUNIT_ASSERT_EQUAL( 4, static_cast<int>(imgRect->height()));
CPPUNIT_ASSERT_EQUAL( 68/1, static_cast<int>(imgRect->getPixelQuick(0,0,0)));
CPPUNIT_ASSERT_EQUAL( 68/2, static_cast<int>(imgRect->getPixelQuick(0,0,1)));
CPPUNIT_ASSERT_EQUAL( 68/3, static_cast<int>(imgRect->getPixelQuick(0,0,2)));
CPPUNIT_ASSERT_EQUAL( 69/1, static_cast<int>(imgRect->getPixelQuick(1,0,0)));
CPPUNIT_ASSERT_EQUAL( 69/2, static_cast<int>(imgRect->getPixelQuick(1,0,1)));
CPPUNIT_ASSERT_EQUAL( 69/3, static_cast<int>(imgRect->getPixelQuick(1,0,2)));
CPPUNIT_ASSERT_EQUAL( 70/1, static_cast<int>(imgRect->getPixelQuick(2,0,0)));
CPPUNIT_ASSERT_EQUAL( 70/2, static_cast<int>(imgRect->getPixelQuick(2,0,1)));
CPPUNIT_ASSERT_EQUAL( 70/3, static_cast<int>(imgRect->getPixelQuick(2,0,2)));
CPPUNIT_ASSERT_EQUAL( 98/1, static_cast<int>(imgRect->getPixelQuick(0,3,0)));
CPPUNIT_ASSERT_EQUAL( 98/2, static_cast<int>(imgRect->getPixelQuick(0,3,1)));
CPPUNIT_ASSERT_EQUAL( 98/3, static_cast<int>(imgRect->getPixelQuick(0,3,2)));
CPPUNIT_ASSERT_EQUAL( 99/1, static_cast<int>(imgRect->getPixelQuick(1,3,0)));
CPPUNIT_ASSERT_EQUAL( 99/2, static_cast<int>(imgRect->getPixelQuick(1,3,1)));
CPPUNIT_ASSERT_EQUAL( 99/3, static_cast<int>(imgRect->getPixelQuick(1,3,2)));
CPPUNIT_ASSERT_EQUAL(100/1, static_cast<int>(imgRect->getPixelQuick(2,3,0)));
CPPUNIT_ASSERT_EQUAL(100/2, static_cast<int>(imgRect->getPixelQuick(2,3,1)));
CPPUNIT_ASSERT_EQUAL(100/3, static_cast<int>(imgRect->getPixelQuick(2,3,2)));
}
void VisionSubsystem::ProcessImage() {
printf("Vision: Starting Vision Subsystem\n");
///////////////
// Seting Up //
///////////////
targetVisable[TOP_TARGET] = false;
targetVisable[MIDDLE_TARGET] = false;
targetVisable[BOTTOM_TARGET] = false;
targetDistances[TOP_TARGET] = 0.0;
targetDistances[MIDDLE_TARGET] = 0.0;
targetDistances[BOTTOM_TARGET] = 0.0;
targetPositionX[TOP_TARGET] = 0.0;
targetPositionY[TOP_TARGET] = 0.0;
targetPositionX[MIDDLE_TARGET] = 0.0;
targetPositionY[MIDDLE_TARGET] = 0.0;
targetPositionX[BOTTOM_TARGET] = 0.0;
targetPositionY[BOTTOM_TARGET] = 0.0;
/*
* This creates a object with the needed values for the processing
* the image later on, for a certain color.
*/
printf("Vision: Setting the clor threshold values\n");
Threshold threshold(THRESHOLD_HUE_MIN,
THRESHOLD_HUE_MAX,
THRESHOLD_SATURATION_MIN,
THRESHOLD_SATURATION_MAX,
THRESHOLD_VALUE_MIN,
THRESHOLD_VALUE_MAX);
ParticleFilterCriteria2 criteria[] = {
{IMAQ_MT_AREA, AREA_MINIMUM, 65535, false, false}
};
/*
* This is the function that sets up the axis camera to get images.
* To use the camera on the second port on the cRIO, uncommet the second line below
* with "192.168.0.90" in it.
*/
printf("Vision: Setting camera IP to 10.30.81.12/n");
AxisCamera &camera = AxisCamera::GetInstance("10.30.81.12");
//AxisCamera &camera = AxisCamera::GetInstance("192.168.0.90"); //
// This creates a Color image, then on the second line it fills it with the image from the camera.
printf("Vision: Creating ColorImage object image\n");
ColorImage *image;
printf("Vision: Getting the Image from the camera \n");
image = camera.GetImage();
//////////////////////////////
// Image processing section //
//////////////////////////////
//Process the image with the threshold values
printf("Vision: Filtering the image with threshold values into object thresholdImage\n");
BinaryImage *thesholdImage = image->ThresholdHSV(threshold);
//This will fill shape that is complete and fill in the inside of siad shape.
printf("Vision: Filling in the convex shapes into the object of convexHullImage\n");
BinaryImage *convexHullImage = thesholdImage->ConvexHull(false);
//This will get rid of random particles in the image that are notconcentrated enougth.
printf("Vision: Filtering image for the unwanted random particles");
BinaryImage *filteredImage = convexHullImage->ParticleFilter(criteria, 1);
//This creates a report that will be used later to idenify targets
printf("Vision: Creating the report of the filtered Image\n");
vector<ParticleAnalysisReport> *reports = filteredImage->GetOrderedParticleAnalysisReports();
//This creates a data stucture that is used to score objects.
scores = new Scores[reports->size()];
for (unsigned i = 0; i < reports->size(); i++) {
ParticleAnalysisReport *report = &(reports->at(i));
scores[i].rectangularity = scoreRectangularity(report);
scores[i].aspectRatioOuter = scoreAspectRatio(filteredImage, report, true);
scores[i].aspectRatioInner = scoreAspectRatio(filteredImage, report, false);
scores[i].xEdge = scoreXEdge(thesholdImage, report);
scores[i].yEdge = scoreYEdge(thesholdImage, report);
if(scoreCompare(scores[i], false)) {
printf("Vision: particle: %d is High Goal centerX %f centerY: %f \n", i , report->center_mass_x_normalized, report->center_mass_y_normalized);
printf("Vision: Distance: %f \n", computeDistance(thesholdImage, report, false));
targetPositionX[TOP_TARGET] = report->center_mass_x;
targetPositionY[TOP_TARGET] = report->center_mass_y;
targetDistances[TOP_TARGET] = computeDistance(thesholdImage, report, false);
targetVisable[TOP_TARGET] = true;
targetPositionX[TOP_TARGET] = targetPosition(TOP_TARGET, true);
targetPositionY[TOP_TARGET] = targetPosition(TOP_TARGET, false);
} else if (scoreCompare(scores[i], true)){
printf("Vision: particle: %d is Middle Goal centerX %f centerY: %f \n", i , report->center_mass_x_normalized, report->center_mass_y_normalized);
printf("Vision: Distance: %f \n", computeDistance(thesholdImage, report, true));
targetPositionX[MIDDLE_TARGET] = report->center_mass_x;
targetPositionY[MIDDLE_TARGET] = report->center_mass_y;
targetDistances[MIDDLE_TARGET] = computeDistance(thesholdImage, report, true);
targetVisable[MIDDLE_TARGET] = true;
targetPositionX[MIDDLE_TARGET] = targetPosition(MIDDLE_TARGET, true);
targetPositionY[MIDDLE_TARGET] = targetPosition(MIDDLE_TARGET, false);
} else {
printf("Vision: particle %d is not a goal centerX: %f centery: %f \n" , i, report->center_mass_x_normalized, report->center_mass_y_normalized);
}
printf("Vision: rect: %f ARinner: %f \n", scores[i].rectangularity, scores[i].aspectRatioInner);
printf("Vision: ARouter: %f xEdge: %f yEdge: %f \n", scores[i].aspectRatioOuter, scores[i].xEdge, scores[i].yEdge);
}
//printf("\n");
printf("Vision: Deleting the object filtered image\n");
delete filteredImage;
printf("Vision: Deleting the objectconvexHullImage\n");
delete convexHullImage;
printf("Vision: Deleting the object thresholdimage\n");
delete thesholdImage;
printf("Vision: Deleting the object image");
delete image;
delete scores;
delete reports;
printf("Vision: Done\n");
}
int VisionControl::ProcessImage()
{
if(m_camera->IsFreshImage())
{
ColorImage *image = NULL;
m_camera->GetImage(image);
Threshold threshold(60,100,90,255,20,255);
ParticleFilterCriteria2 criteria[] = {IMAQ_MT_AREA,AREA_MINIMUM,65535,false,false};
BinaryImage *thresholdImage = image->ThresholdHSV(threshold);
BinaryImage *convexHullImage = thresholdImage->ConvexHull(false);
BinaryImage *filteredImage = convexHullImage->ParticleFilter(criteria, 1);
vector<ParticleAnalysisReport> *reports = filteredImage->GetOrderedParticleAnalysisReports();
for (unsigned int i = 0; i < reports->size(); i++)
{
ParticleAnalysisReport *report = &(reports->at(i));
// first, determine if this is a particle we are looking at.
if(report->boundingRect.left > 320/2 || report->boundingRect.left + report->boundingRect.width < 320/2)
{
// particle is not lined up with center of vision
// note: may not want to do this for autonomous!
continue;
}
double aspectRatio = AspectRatio(filteredImage, report);
double difference3ptGoal = fabs(1-(aspectRatio / ((54.f+4+4)/(12.f+4+4))));
double difference2ptGoal = fabs(1-(aspectRatio / ((54.f+4+4)/(21.f+4+4))));
if(difference2ptGoal < 0.25 && difference2ptGoal < difference3ptGoal)
{
m_elevation = 0;
m_distance = ComputeDistance(thresholdImage, report, true);
m_relativeAzimuth = 0;
}
else if(difference3ptGoal < 0.25 && difference3ptGoal < difference2ptGoal)
{
m_elevation = 0;
m_distance = ComputeDistance(thresholdImage, report, false);
m_relativeAzimuth = 0;
}
else
{
// didn't sufficiently match a target!
}
}
/*
Scores *scores = new Scores[reports->size()];
//Iterate through each particle, scoring it and determining whether it is a target or not
for (unsigned i = 0; i < reports->size(); i++)
{
ParticleAnalysisReport *report = &(reports->at(i));
scores[i].rectangularity = ScoreRectangularity(report);
scores[i].aspectRatioOuter = ScoreAspectRatio(filteredImage, report, true);
scores[i].aspectRatioInner = ScoreAspectRatio(filteredImage, report, false);
scores[i].xEdge = ScoreXEdge(thresholdImage, report);
scores[i].yEdge = ScoreYEdge(thresholdImage, report);
if(ScoreCompare(scores[i], false))
{
printf("particle: %d is a High Goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
printf("Distance: %f \n", ComputeDistance(thresholdImage, report, false));
}
else if (ScoreCompare(scores[i], true))
{
printf("particle: %d is a Middle Goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
printf("Distance: %f \n", ComputeDistance(thresholdImage, report, true));
}
else
{
printf("particle: %d is not a goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
}
printf("rect: %f ARinner: %f \n", scores[i].rectangularity, scores[i].aspectRatioInner);
printf("ARouter: %f xEdge: %f yEdge: %f \n", scores[i].aspectRatioOuter, scores[i].xEdge, scores[i].yEdge);
}
*/
delete image;
delete thresholdImage;
delete convexHullImage;
delete filteredImage;
delete reports;
//delete scores;
return 1;
}
return 0;
}
inline void processTaskFunc(UINT32 hotGoalPtr...)
{
bool hotGoal = (bool *) hotGoalPtr;
Scores *scores;
TargetReport target;
int verticalTargets[MAX_PARTICLES];
int horizontalTargets[MAX_PARTICLES];
int verticalTargetCount, horizontalTargetCount;
Threshold threshold(0, 255, 0, 255, 220, 255); //HSV threshold criteria, ranges are in that order ie. Hue is 60-100
ParticleFilterCriteria2 criteria[] = {
{IMAQ_MT_AREA, AREA_MINIMUM, 65535, false, false}
}; //Particle filter criteria, used to filter out small particles
//AxisCamera &camera = AxisCamera::GetInstance(); //To use the Axis camera uncomment this line
/**
* Do the image capture with the camera and apply the algorithm described above. This
* sample will either get images from the camera or from an image file stored in the top
* level directory in the flash memory on the cRIO. The file name in this case is "testImage.jpg"
*/
ColorImage *image;
image = new RGBImage("/testImage.jpg"); // get the sample image from the cRIO flash
//image = camera.GetImage(); //To get the images from the camera comment the line above and uncomment this one
BinaryImage *thresholdImage = image->ThresholdHSV(threshold); // get just the green target pixels
//thresholdImage->Write("/threshold.bmp");
BinaryImage *filteredImage = thresholdImage->ParticleFilter(criteria, 1); //Remove small particles
//filteredImage->Write("Filtered.bmp");
vector<ParticleAnalysisReport> *reports = filteredImage->GetOrderedParticleAnalysisReports(); //get a particle analysis report for each particle
verticalTargetCount = horizontalTargetCount = 0;
//Iterate through each particle, scoring it and determining whether it is a target or not
if(reports->size() > 0)
{
scores = new Scores[reports->size()];
for (unsigned int i = 0; i < MAX_PARTICLES && i < reports->size(); i++) {
ParticleAnalysisReport *report = &(reports->at(i));
//Score each particle on rectangularity and aspect ratio
scores[i].rectangularity = scoreRectangularity(report);
scores[i].aspectRatioVertical = scoreAspectRatio(filteredImage, report, true);
scores[i].aspectRatioHorizontal = scoreAspectRatio(filteredImage, report, false);
//Check if the particle is a horizontal target, if not, check if it's a vertical target
if(scoreCompare(scores[i], false))
{
printf("particle: %d is a Horizontal Target centerX: %d centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
horizontalTargets[horizontalTargetCount++] = i; //Add particle to target array and increment count
} else if (scoreCompare(scores[i], true)) {
printf("particle: %d is a Vertical Target centerX: %d centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
verticalTargets[verticalTargetCount++] = i; //Add particle to target array and increment count
} else {
printf("particle: %d is not a Target centerX: %d centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
}
printf("Scores rect: %f ARvert: %f \n", scores[i].rectangularity, scores[i].aspectRatioVertical);
printf("ARhoriz: %f \n", scores[i].aspectRatioHorizontal);
}
//Zero out scores and set verticalIndex to first target in case there are no horizontal targets
target.totalScore = target.leftScore = target.rightScore = target.tapeWidthScore = target.verticalScore = 0;
target.verticalIndex = verticalTargets[0];
for (int i = 0; i < verticalTargetCount; i++)
{
ParticleAnalysisReport *verticalReport = &(reports->at(verticalTargets[i]));
for (int j = 0; j < horizontalTargetCount; j++)
{
ParticleAnalysisReport *horizontalReport = &(reports->at(horizontalTargets[j]));
double horizWidth, horizHeight, vertWidth, leftScore, rightScore, tapeWidthScore, verticalScore, total;
//Measure equivalent rectangle sides for use in score calculation
imaqMeasureParticle(filteredImage->GetImaqImage(), horizontalReport->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &horizWidth);
imaqMeasureParticle(filteredImage->GetImaqImage(), verticalReport->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &vertWidth);
imaqMeasureParticle(filteredImage->GetImaqImage(), horizontalReport->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &horizHeight);
//Determine if the horizontal target is in the expected location to the left of the vertical target
leftScore = ratioToScore(1.2*(verticalReport->boundingRect.left - horizontalReport->center_mass_x)/horizWidth);
//Determine if the horizontal target is in the expected location to the right of the vertical target
rightScore = ratioToScore(1.2*(horizontalReport->center_mass_x - verticalReport->boundingRect.left - verticalReport->boundingRect.width)/horizWidth);
//Determine if the width of the tape on the two targets appears to be the same
tapeWidthScore = ratioToScore(vertWidth/horizHeight);
//Determine if the vertical location of the horizontal target appears to be correct
verticalScore = ratioToScore(1-(verticalReport->boundingRect.top - horizontalReport->center_mass_y)/(4*horizHeight));
total = leftScore > rightScore ? leftScore:rightScore;
total += tapeWidthScore + verticalScore;
//If the target is the best detected so far store the information about it
if(total > target.totalScore)
{
target.horizontalIndex = horizontalTargets[j];
target.verticalIndex = verticalTargets[i];
target.totalScore = total;
target.leftScore = leftScore;
target.rightScore = rightScore;
target.tapeWidthScore = tapeWidthScore;
target.verticalScore = verticalScore;
}
}
//Determine if the best target is a Hot target
target.Hot = hotOrNot(target);
}
if(verticalTargetCount > 0)
{
//Information about the target is contained in the "target" structure
//To get measurement information such as sizes or locations use the
//horizontal or vertical index to get the particle report as shown below
ParticleAnalysisReport *distanceReport = &(reports->at(target.verticalIndex));
double distance = computeDistance(filteredImage, distanceReport);
if(target.Hot)
{
printf("Hot target located \n");
printf("Distance: %f \n", distance);
hotGoal = true;
} else {
printf("No hot target present \n");
printf("Distance: %f \n", distance);
hotGoal = false;
}
}
}
// be sure to delete images after using them
delete filteredImage;
delete thresholdImage;
delete image;
//delete allocated reports and Scores objects also
delete scores;
delete reports;
}
int Vision::particleAnalysis()
{
printf("Vision::particleAnalysis\n");
// Get an instance of the Axis Camera
AxisCamera &camera = AxisCamera::GetInstance(CAMERA_IP);
TracePrint(TRACE_VISION, "check fresh\n");
// check if there is a new image
if (camera.IsFreshImage())
{
TracePrint(TRACE_VISION, "get image\n");
// Get the Image
ColorImage *colorImage = camera.GetImage();
TracePrint(TRACE_VISION, "colorImage is %s\n", colorImage ? "not null" : "null");
BinaryImage *binImage = colorImage->ThresholdRGB(COLOR_THRESHOLD);
TracePrint(TRACE_VISION, "binImage is %s\n", binImage ? "not null" : "null");
if(!colorImage || !binImage)
return -1;
TracePrint(TRACE_VISION, "Getting Particle Analysis Report.");
if (particles)
{
delete particles;
}
particles = binImage->GetOrderedParticleAnalysisReports();
if(!particles)
{
TracePrint(TRACE_VISION, "NULL PARTICLES");
return -1;
}
if(!particles->empty())
{
TracePrint(TRACE_VISION, "Stepping through particle report to remove particles with area too small (total %d particles).\n", particles->size());
int maxHeight = 0, maxIndex = -1;
// Step through the particles and elimate any that are too small
for (int i = 0; i<(int)particles->size(); i++)
{
TracePrint(TRACE_VISION, "Particle %d:\n", i);
TracePrint(TRACE_VISION, "area %.4lf\n", particles->at(i).particleArea);
if(particles->at(i).particleArea<MIN_PARTICLE_AREA)
{
TracePrint(TRACE_VISION, "Particle too small, erasing... ");
// Erase the current particle from view
particles->erase(particles->begin()+i);
// Because erasing an element actually adjusts all elements
// after the current one, we need to bump <tt>i</tt> down one
i--;
TracePrint(TRACE_VISION, "... erased.\n");
}
else
{
TracePrint(TRACE_VISION, "Checking height...");
if (particles->at(i).center_mass_y>maxHeight) {
maxIndex = i;
maxHeight = particles->at(i).center_mass_y;
}
TracePrint(TRACE_VISION, "... checked\n");
}
}
if (maxIndex!=-1)
targetParticle = particles->at(maxIndex);
}
else
{
targetParticle.center_mass_x_normalized = 0;
}
if (colorImage)
{
printDebug("Deleting colorImages.");
delete colorImage;
}
if (binImage)
{
printDebug("Deleting binImages.");
delete binImage;
}
printDebug("Done Deleting Images.");
}
printDebug("Done processing image.");
return particles->size();
}
