Trouble with code for camera

Right now I am working some of the code form the 2010Vision Sample code. Here is what i have.
if (trigger = stick->GetTrigger())
{
//if (trigger != lastTrigger) // if there’s a fresh and we’re at the previous target heading then
// get a camera image and process it
if (camera.IsFreshImage())
{
// get the camera image

				HSLImage *image = camera.GetImage();
				//HSLImage *image;
				//image = new HSLImage("/10ft2.jpg");		// get the sample image from the cRIO flash
				BinaryImage *thresholdImage = image->ThresholdHSL(threshold);	// get just the red target pixels
				BinaryImage *bigObjectsImage = thresholdImage->RemoveSmallObjects(false, 2);  // remove small objects (noise)
				BinaryImage *convexHullImage = bigObjectsImage->ConvexHull(false);  // fill in partial and full rectangles
				BinaryImage *filteredImage = convexHullImage->ParticleFilter(criteria, 2);  // find the rectangles
				vector<ParticleAnalysisReport> *reports = filteredImage->GetOrderedParticleAnalysisReports();  // get the results
				int b = 0;
				for (unsigned i = 0; i < reports->size(); i++) 
				{
						ParticleAnalysisReport *r = &(reports->at(i));
						DS->DriverStationLCD::Printf(DriverStationLCD::kUser_Line1,1,"Particle: %d  center_mass: %d 

",r->center_mass_x);
//DS->DriverStationLCD::UpdateLCD();
//printf("particle: %d center_mass_x: %d
", i, r->center_mass_x);
b = r->center_mass_x;

			}

When I press the trigger to get an image I lose communication with the robot. After a minute or two it robots and I can enable and drive. I tested the orginal code with added printf statements to to get an understanding of how it works and I did not have this problem. Is something wrong with my code?

Vision processing can take a LONG time if you do not have the filtering parameters set correctly (i.e. generating way too many false positive targets). If vision processing takes too much time, it will prevent your main robot loop from “feeding” the watchdog (i.e. setting your motors) and hence causing the SafetyHelper to cut out the motors.

Before you enable the entire code doing vision processing as well as tele-operating/driving the robot, you probably want to disable safety and fully debug the vision processing code before you re-enable safety for driving.

BTW, to make sure it is really a performance issue, add some performance code like below and note how much time it takes to process an image frame. Also, I noticed your code did not delete all the intermediate images. That could cause the cRIO to run out of memory so it would terminate your robot task. Also, try to print out the number of particles found to make sure you don’t have an unreasonable amount of false positive targets.


#define GetMsecTime()           (GetFPGATime()/1000)
if (trigger = stick->GetTrigger())
{
    UINT32 startTime = GetMsecTime();
    if (camera.IsFreshImage())
    {
        HSLImage *image = camera.GetImage();
        BinaryImage *thresholdImage = image->ThresholdHSL(threshold);    // get just the red target pixels
        BinaryImage *bigObjectsImage = thresholdImage->RemoveSmallObjects(false, 2);  // remove small objects (noise)
        BinaryImage *convexHullImage = bigObjectsImage->ConvexHull(false);  // fill in partial and full rectangles
        BinaryImage *filteredImage = convexHullImage->ParticleFilter(criteria, 2);  // find the rectangles
        vector<ParticleAnalysisReport> *reports = filteredImage->GetOrderedParticleAnalysisReports();  // get the results
        printf("Elapsed time = %d
", GetMsecTime() - startTime);
        printf("Num particles found = %d
", reports->size());
        int b = 0;
        for (unsigned i = 0; i < reports->size(); i++) 
        {
            ParticleAnalysisReport *r = &(reports->at(i));
            DS->DriverStationLCD:Printf(DriverStationLCD::kUser_Line1,1,"Particle: %d  center_mass: %d 
",r->center_mass_x);
            //DS->DriverStationLCD::UpdateLCD();
            //printf("particle: %d  center_mass_x: %d
", i, r->center_mass_x);
            b = r->center_mass_x;
        }
        delete reports;
        delete filteredImage;
        delete convexHullImage;
        delete bigObjectsImage;
        delete thresholdImage;
        delete image;
    }
}

Thanks!!!.