TrackTwoColors execution time

Technical C/C++
1

It seems (at least in our tests) that the TrackTwoColors function from the example two color tracking code is bogging down the cRIO to an almost unusable point. Has anyone else had this issue?

Symptoms are as follows:

  1. When function is called in sequence with motor control code, execution of the main loop is slowed to a few Hz, lagging operator control.
  2. Code runs relatively quick when there are no HSL color matches in view. As soon as a pink or green target appears, regardless of orientation, the process slows down immensely. This makes sense, but the lag seems very big for the amount of code that is running.

To fix #1, we split the object matching code out into a separate thread, which fixed the motor update lag. Unfortunately, our servos now overshoot the target by quite a bit before the “target found” flag is raised.

We tried to characterize the execution time of the code block, and came up with the attached graph. I don’t have the source on hand, but it looked something like this:


/*This runs in a seperate thread, so I have no idea how the other threads effect execution time.*/
Timer timer;
float start,finish,delta;
timer.Start();
while(1){
   start = timer.Get();
   TrackTwoColors(params here);
   finish = timer.Get();
   delta = finish - start;
   cout << start << "," << delta << endl ;
}

A graph generated from the stdout is attached. The low points happen when the camera is panning with no target, and the peaks occur when the target is upside down. The high holds occur when the target is oriented the correct way.

Is there any kind of hidden debug stuff going on? I have turned down all the do-debug flags in target.h, but I don’t see a difference. I don’t want to re-write a signal processor, and want to make sure I’m not alone before I start any such work.

Link to graph: http://i43.tinypic.com/119w6k9.png

2

We’ve been tracking the target up to 20-25 feet away with image size of 160x120. Your graph says that you are using 800x600. You might want to try reducing your image size. That is 1/25 the area and should go about 25 times faster.

3

We are grabbing 160x120 pixels from the cam on the robot. 800x600 is the size of the image I saved from the spreadsheet.

4

There are a few things that you could try. One is to reduce the number of particle hits that FindTwoColors will check. The default is 3. You might try to reduce this to 2 and see if it helps. That could reduce the processing by 40%. There is an overloaded method of FindTwoColors that takes an additional parameter. That is the number of hits.

Another option is to tweak the GREEN color description in td2. We have found that the values in the example are very generous and will accept almost anything resembling green.

Also in the function FindTwoColors the behavior for the option ABOVE is different from that for BELOW. The BELOW case needs the addition call to SizesRelative because there is so much green. This isn’t a speed related issue but the camera will lock on to a Pink over Green if there is almost any green in the image. This might not be a problem if the green filter is tightened up a bit.

5

The Find Color will slow down when the primary color is in view. Here is a rough overview of the processing, including those things that are variable in their computation time.

First step is decoding the JPG – pretty fixed time.

Next, any decimation or subsetting of the image to limit where you process – fixed.

Next, color threshold with the primary color (lets say pink). – highly variable since we take advantage of the fast threshold on L and S before computing H. This means that dark pixels are fast because we only have to compute one thing before they are excluded. Nonsaturated pixels like white are also quite fast, two quick calculations and they are excluded. Bright and saturated pixels undergo the more complicated conversion to hue which involves trig. All bright and saturated pixels that aren’t pink will be excluded.

Next is a call to get the particle properties such as size, location, etc – pretty variable. There is no way in advance to know how many particles or where in the image your BIG target is.

Next is a sort of the particles by size – pretty fixed.

Next is looking near the big pink particles for green ones. I’ll look at the C code later today to make sure it is doing this in a good way, but since the image pieces it extracts are tiny, this should be fast, but also highly variable.

Anyway, I hope this helps explain why the algorithm behaves the way it does. It is far from realtime, and all of the optimizations we’ve put in have only made it more variable in time.

Please let me know of any insight you gain.

Greg McKaskle

6

I am looking at this code also. It is unusable as is, toooo slow as you analyzed. It dynamically allocates and deletes memory for the hit nodes. This is not a good practice in any code where the goal is anything approaching real-time performance. With the color thresholds pretty liberal, this could cause a lot of delay.

It also seems to fetch two pictures, one each time it looks for one of the 2 colors. I think it need only process a single image. In fact, this would be preferable, correct?

Comments? Suggestions?

7

I fooled with this a little more tonight. I changed the image size to 160x120 and reduced the frame rate to 5Hz. Using the spy utility I can see that camera-related activities are consuming nearly 50% of the CPU bandwidth.

It looks like there are some memory problems with this code under certain conditions. The demo runs with the FRC_Robot and FRC_Camera tasks at the same priority. In our code, we run the camera demo stuff in its own task. If you lower the priority of the FRC_Camera task (raising the number in VxWorks) relative to the demo code (in whatever task context) you will get occasional memPartAlloc errors that eventually kill the robot. So there are some possible mutual exclusion issues with the FRC_Camera task and the demo code.

It looks like my comments about the waste of getting 2 images might be wrong. They change the image while looking for each color. But it may still be more efficient to copy (and there is a method for this) the camera image than run GetImage again.

8

We’ve been trying to break up the image processing into three pieces, so that each piece would hopefully take up no more that 15ms. That way as long as all the other code that we run for each periodic loop executes in less than 5ms we will keep the system deterministic. That way we could process at 15fps which is what our tracking group says the need to keep the kalman filter converged for a worst case 20fps closing scenario. (In the latest WPILIB, the periodic period is always the DS packet rate or 20ms - we missed that in the release notes and it took a while to figure out what happened)

Anyway, what we did was in the first stage create three images. One to get the image from the camera and the other two as the output binary images from frcColorThreshold(). We want to hang onto the threshold output images so we can move the processing around to tweak the timing. What we have found so far is that frcColorThreshold() only seems to work if the source and destination image pointers are the same? Of course, I can’t find the definition of the Image or Image_struct structure all I can find is:

01918 typedef struct Image_struct Image;

So it is pretty hard to figure out why frcColorThreshold() is failing - returning a 0.

One thing is that I think frcColorThreshold() is supposed to return a “binary” image but there are no image types for binary, U8 is the smallest. So right now the code creates all three images with the reasoning being that the threshold routine works with cameraImage as the input and output image.

    cameraImage = frcCreateImage(IMAQ_IMAGE_HSL);
    firstColorThreshImage = frcCreateImage(IMAQ_IMAGE_HSL);
    secondColorThreshImage = frcCreateImage(IMAQ_IMAGE_HSL);

Anyway, the reason we are trying to do this is because we want to make sure that the pink and green thresholding use the same frame. I suppose that we could copy the image and then overwrite the original and the copy in the threshold function but I was wondering if anyone had any insight as to why

success = frcColorThreshold(colorThreshImage, cameraImage, mode, plane1Range, plane2Range, plane3Range);

always fails and

success = frcColorThreshold(cameraImage, cameraImage, mode, plane1Range, plane2Range, plane3Range);

Is successful.

9

I found a question about the structure of Image on the NI forum page - the answer was:

prince -

The easiest way to get a representation of the image you can use with external toolkits is to call imaqImageToArray() to get the pixel data. Be sure to call imaqDispose() on the result after you’re done with it to avoid memory leaks. You can call imaqGetImageInfo() to get information about the image. More information about these function calls are available in the Function Reference Help.

Greg Stoll
Vision R&D
National Instruments

imaqGetImageInfo() isn’t in the CVI help file or the FRC Vision API Specification. Does anyone know to figure out what is in Image structure?

10

I think frcDispose calls imaqDispose and frcCreateImage calls imaqCreateImage.

11

I agree that the imaq functions are called but I still can’t find any information on the format of the image structure and for instance where the storage for the actual image is allocated. By looking at the value of the Image pointer it is clear that the delta between the pointers to different images is not large enough to accomodate the image so it looks like there are subsequent structure members that hold the pointer to the actual image storage area.

Also it is very confusing as to whether you have to call imaqDispose() for each image created or if a single call destroys all images that are currently created. The documentation says that frcDispose disposes all images but the documentation for imaqDispose() talks about just disposing the image associated with the object. Furthermore, the documentation states that frcCreateImage() returns 1 or 0 but that is clearly not the case from debugging the program.

I’m still looking for the definition of the Image structure.

Actually the WIPLIB trackapi.cpp file shows how multiple images can be Disposed of in a single call:

What is interesting to me is how in FindColor the camera image is copied to histImage but histImage is never used?

/* save a copy of the image to another image for color thresholding later */
Image* histImage = frcCreateImage(IMAQ_IMAGE_HSL);
if (!histImage)  { frcDispose(cameraImage); return success; }
success = frcCopyImage(histImage,cameraImage);
if ( !success )	{ 
	errorCode = GetLastVisionError(); 
	frcDispose(funcName,cameraImage,histImage,NULL); 
	return success; 
}
12

The image structure is complicated, and I don’t think it is publicly described. It has the ability to hold onto pixel data of various formats + analysis results + ROI + geometric metadata if it is a template. So yes, it has internal allocations and isn’t a simple block of memory. For each call to imaqCreate, you should call imaqDispose to match. I’m less sure of the usage of frcCreate and Dispose, but I think that you need to call an frcDispose for each, or use the comma delimited form to dispose of each.

The mask is really a monochrome image, and can in fact hold several mask results, each in a different plane. The LV example shows this with the green and pink in one mask. If you want to do your own pixel manipulations, use the image to array call. Otherwise, you might try making your images static to avoid allocating and destroying them each time.

I can’t comment on why frcThreshold doesn’t like putting the results into a different image. Can you step inside it? My guess is the image type is incompatible. The error code should indicate why. If you are wanting to pipeline the image processing you may feel the need to skip past the frc layer, use it for inspiration, and go straight to using the imaq layer. It will be a little more difficult, but you will have a bit more control as well.

Greg McKaskle

13

Greg,

Thanks for the response and help. We think we have a deterministic 15fps solution for 160x60 pixel images. We don’t need much vertical FOV because our camera is mounted toward the top of our robot on the shooter turret so we can throw away part of the image.

To get around the problem with the thresholding not taking different input and output files we read a 160x120 image from the camera, crop it to 160x60 and then copy it. We use the first image for color1 and the second for color 2 that way we know that we are not using different frames for color 1 and color 2. We break the processing up as follows:

If a new frame is ready:
Threshold for color 1 (about 10ms)
Set state to threshold for color 2
Else return and test for new frame ready the next periodic cycle (20ms later)

If state == threshold for color 2
Threshold for color 2 using the image copy (about 10 ms)
Set state for particle processing
Endif

If state == particle processing
Do all the processing for both colors that is in the dual color example code (about 14ms)
Return results to tracking code
Endif

Because our frame rate is 15fps 66.6666 ms period we get a result every 3,3,4 frames.

To us, the advantage of this approach is that we can insure that everything we will ever do in either teleop or auto mode will be complete in less than 20ms so the system operation is predictable. When I was a young engineer one of my first supervisors drilled into me that “Synchronousness is next to Godliness”. If you take the approach that you are going to run the image processing in a separate thread at the same or higher/lower priority then it becomes much more difficult to prove that other processes like the traction control loops for instance will always execute as intended.

Anyway, that is our solution and so far it looks like it will work.

Greg

14

I agree that it will work. The pretty obvious tradeoff you are making is that by pipelining the work, you are delaying whey you get meaningful data from an image. This just adds to the lag and as long as it it bounded, your approach will work fine, and you can take it into account when aiming, steering, or whatever the camera is controlling.

Greg McKaskle

15

Greg is correct that the Image is a proprietary structure and we don’t have insight about its format. It’s declared in nivision.h.

frcDispose() can be used to dispose of one or many Images, they have to be listed as input parameters.

The header for frcCreateImage states that it returns the Image structure (or NULL if error). The FRC Vision API spec also says this. What documentation are you referring to that says it returns 0 or 1?

The histImage is used in FindColor to create the colorReport.

Beth

16

Another aspect that will effect the CPU loading is how often you are calling the TrackTwoColors() routine. The number of images per second that are retrieved from the camera is set at the StartCameraTask() call. Once you have a new image and process it once, if you keep calling TrackTwoColors() you will not only chew up CPU time, but you will keep getting the same answer back (you’re checking the same image). If the camera task is a lower priority than the main task (I don’t know) then the constant vision processing in the main loop may be backlogging the camera task. Regardless, it’s a whole lot of useless CPU hogging.

We made a ‘newImageAvailable()’ function, which uses the GetImage() function (which returns a timestamp) to determine when a new image is available. So we only have to do our version of the TrackTwoColors() call once per new image. With a 160x120 image size, we easily do 15 frames per second. We could maybe do more but haven’t tried.

Steve C.

17

Is there any way that we could see a sample of your newImageAvailable() function Kruuzr? I’m looking at the GetImage() function but I see it returning an int (1 for success and -1 for failure). Not the double timestamp.

18

Sorry I didn’t get back to you earlier. (Last build night already started).

I don’t have the code in front of me, but I believe the two parameters for GetImage() are Image* and double*. It may get the image into your Image* param but it also puts the timestamp into your double. The code looks something like this:

Image* image; // created using frcCreateImage() somewhere else

bool newImageAvailable()
{
double timestamp;
static double lastTimestamp = 0.0;

GetImage( image, &timestamp );
if ( timestamp > lastTimestamp )
{
lastTimestamp = timestamp;
return true;
}

return false;
}

I may have missed something but I think that’s it.

Steve C.

19

Thank you for the code. I don’t know why I didn’t notice that GetImage() used the double pointer in that way. I guess that’s what happens when you become sleep deprived at the end of a build season…