inline OSVR_ReturnCode applyRenderBufferConstructor(const int numBuffers,
                                                    F &&bufferConstructor,
                                                    G &&bufferCleanup) {
    /// If we bail any time before the end, we'll automatically clean up the
    /// render buffers with this lambda.
    auto cleanupBuffers = osvr::util::finally([&] {
        DebugLog("[OSVR Rendering Plugin] Cleaning up render buffers.");
        for (auto &rb : s_renderBuffers) {
            bufferCleanup(rb);
        }
        s_renderBuffers.clear();
        DebugLog("[OSVR Rendering Plugin] Render buffer cleanup complete.");
    });

    /// Construct all the buffers as isntructed
    for (int i = 0; i < numBuffers; ++i) {
        auto ret = bufferConstructor(i);
        if (ret != OSVR_RETURN_SUCCESS) {
            DebugLog("[OSVR Rendering Plugin] Failed in a buffer constructor!");
            return OSVR_RETURN_FAILURE;
        }
    }

    /// Register our constructed buffers so that we can use them for
    /// presentation.
    if (!s_render->RegisterRenderBuffers(s_renderBuffers)) {
        DebugLog("RegisterRenderBuffers() returned false, cannot continue");
        return OSVR_RETURN_FAILURE;
    }
    /// Only if we succeed, do we cancel the cleanup and carry on.
    cleanupBuffers.cancel();
    return OSVR_RETURN_SUCCESS;
}
Exemplo n.º 2
0
int CCircleDetect::adjustDimensions(int wi,int he)
{
	width = wi;
	height = he;
	len = width*height;
	free(buffer);
	free(queue);
	buffer = (int*)malloc(len*sizeof(int));
	queue = (int*)malloc(len*sizeof(int));
	SSegment dummy;
	bufferCleanup(dummy);
}
Exemplo n.º 3
0
//Variable initialization
CCircleDetect::CCircleDetect(int wi, int he, float diamRatio) {
	lastTrackOK = false;
	debug = false;
	draw = false;
	drawAll = false;
	maxFailed = 0;
	minSize = 10;
	thresholdStep = 256;
	maxThreshold = 3 * 256;
	centerDistanceToleranceRatio = 1.1; //TODO
	centerDistanceToleranceAbs = 5;
	circularTolerance = 0.3;
	ratioTolerance = 0.1;
	threshold = maxThreshold / 2;
	numFailed = maxFailed;
	track = true;
	//circularityTolerance = 0.02;
	//circularityTolerance = 0.1;
	circularityTolerance = 0.05;

	//initialization - fixed params
	width = wi;
	height = he;
	len = width * height;
	siz = len * 3;
	ownBuffer = false;
	if (buffer == NULL) {
		ownBuffer = true;
		buffer = (int*) malloc(len * sizeof(int));
		queue = (int*) malloc(len * sizeof(int));
		SSegment dummy;
		bufferCleanup(dummy);
	}
	//diameterRatio = 5.0/14.0; //inner vs. outer circle diameter
	diameterRatio = diamRatio;
	float areaRatioInner_Outer = diameterRatio * diameterRatio;
	outerAreaRatio = M_PI * (1.0 - areaRatioInner_Outer) / 4;
	innerAreaRatio = M_PI / 4.0;
	areasRatio = (1.0 - areaRatioInner_Outer) / areaRatioInner_Outer;
	tima = timb = timc = timd = sizer = sizerAll = 0;
}
Exemplo n.º 4
0
SSegment CCircleDetect::findSegment(CRawImage* image, SSegment init)
{

	SSegment result;
	result.x = 0;
	result.y = 0;
	result.round = false;
	result.valid = false;
	numSegments = 0;
	if (image->width != width || image->height != height){
		 adjustDimensions(image->width,image->height);
		 init.valid = false;
	}
	int pos = 0;
	int ii = 0;
	int start = 0;
	bool cont = true;
	int step = image->bpp;
	//bufferCleanup(init);
	if (init.valid && track){
		ii = ((int)init.y)*image->width+init.x;
		start = ii;
	}
	while (cont) 
	{
		if (buffer[ii] == 0){
			ptr = &image->data[ii*step];
			if ((ptr[0]+ptr[1]+ptr[2]) < threshold) buffer[ii] = -2;
		}
		if (buffer[ii] == -2 && numSegments<MAX_SEGMENTS){
			//new segment found
			queueEnd = 0;
			queueStart = 0;
			//if the segment looks like a ring, we check its inside area
			if (examineSegment(image,&segmentArray[numSegments],ii,outerAreaRatio)){
				pos = segmentArray[numSegments-1].y*image->width+segmentArray[numSegments-1].x;
				if (buffer[pos] == 0){
					ptr = &image->data[pos*step];
					buffer[pos]=(ptr[0]+ptr[1]+ptr[2] > threshold)-2;
				}
				if (buffer[pos] == -1 && numSegments<MAX_SEGMENTS){
					if (examineSegment(image,&segmentArray[numSegments],pos,innerAreaRatio)){
						//the inside area is a circle. now what is the area ratio of the black and white ? also, are the circles concentric ?

						if (debug) printf("Area ratio %i/%i is %.3f %.3f %.3f\n",numSegments-2,numSegments-1,(float)segmentArray[numSegments-2].size/segmentArray[numSegments-1].size,areasRatio,segmentArray[numSegments-2].size/areasRatio/segmentArray[numSegments-1].size);
						if (
								((float)segmentArray[numSegments-2].size/areasRatio/(float)segmentArray[numSegments-1].size - ratioTolerance < 1.0 && (float)segmentArray[numSegments-2].size/areasRatio/(float)segmentArray[numSegments-1].size + ratioTolerance > 1.0) && 
								(abs(segmentArray[numSegments-1].x-segmentArray[numSegments-2].x) <= centerDistanceToleranceAbs+centerDistanceToleranceRatio*((float)(segmentArray[numSegments-2].maxx-segmentArray[numSegments-2].minx))) &&
								(abs(segmentArray[numSegments-1].y-segmentArray[numSegments-2].y) <= centerDistanceToleranceAbs+centerDistanceToleranceRatio*((float)(segmentArray[numSegments-2].maxy-segmentArray[numSegments-2].miny)))

						   ){
							float fm0,fm1,fm2,f0,f1;
							long int six,siy,tx,ty,cm0,cm1,cm2;
							float cm0f,cm1f,cm2f;
							six=siy=cm0=cm1=cm2=0;
						//	segmentArray[numSegments-1].x = segmentArray[numSegments-2].x;
						//	segmentArray[numSegments-1].y = segmentArray[numSegments-2].y;
							for (int p = queueOldStart;p<queueEnd;p++){
								pos = queue[p];
								tx = pos%image->width;
								ty = pos/image->width;
								six += tx;
								siy += ty;
								cm0+=tx*tx; 
								cm1+=tx*ty;
								cm2+=ty*ty; 
							}
							segmentArray[numSegments-2].x = (float)six/(queueEnd-queueOldStart);
							segmentArray[numSegments-2].y = (float)siy/(queueEnd-queueOldStart);

							/*float sxi = (float)six/(queueEnd-queueOldStart);
							float syi = (float)siy/(queueEnd-queueOldStart);
							cm0f = (cm0 - sxi*sxi*(queueEnd-queueOldStart));
							cm1f = (cm1 - sxi*syi*(queueEnd-queueOldStart));
							cm2f = (cm2 - syi*syi*(queueEnd-queueOldStart));
							fm0 = cm0f/(queueEnd-queueOldStart);
							fm1 = cm1f/(queueEnd-queueOldStart);
							fm2 = cm2f/(queueEnd-queueOldStart);
							f0 = ((fm0+fm2)+sqrt((fm0+fm2)*(fm0+fm2)-4*(fm0*fm2-fm1*fm1)))/2;
							f1 = ((fm0+fm2)-sqrt((fm0+fm2)*(fm0+fm2)-4*(fm0*fm2-fm1*fm1)))/2;
							float m0i = sqrt(f0);
							float m1i = sqrt(f1);*/
							
							for (int p = 0;p<queueOldStart;p++){
								pos = queue[p];
								tx = pos%image->width;
								ty = pos/image->width;
								six += tx;
								siy += ty;
								cm0+=tx*tx; 
								cm1+=tx*ty;
								cm2+=ty*ty; 
							}

							float sx = (float)six/queueEnd;
							float sy = (float)siy/queueEnd;
							cm0f = (cm0 - sx*sx*queueEnd);
							cm1f = (cm1 - sx*sy*queueEnd);
							cm2f = (cm2 - sy*sy*queueEnd);
							fm0 = cm0f/queueEnd;
							fm1 = cm1f/queueEnd;
							fm2 = cm2f/queueEnd;
							float det = (fm0+fm2)*(fm0+fm2)-4*(fm0*fm2-fm1*fm1);
							if (det > 0) det = sqrt(det); else det = 0;

							f0 = ((fm0+fm2)+det)/2;
							f1 = ((fm0+fm2)-det)/2;
							segmentArray[numSegments-1].m0 = sqrt(f0);
							segmentArray[numSegments-1].m1 = sqrt(f1);
							if (fm1 != 0){                                                            //aligned ?
							segmentArray[numSegments-1].v0 = -fm1/sqrt(fm1*fm1+(fm0-f0)*(fm0-f0));
							segmentArray[numSegments-1].v1 = (fm0-f0)/sqrt(fm1*fm1+(fm0-f0)*(fm0-f0));
							}else{
								result.v0 = result.v1 = 0;
								if (fm0 > fm2) result.v0 = 1.0; else result.v1 = 1.0;   // aligned, hm, is is aligned with x or y ?
							}
							segmentArray[numSegments-1].bwRatio = (float)segmentArray[numSegments-2].size/segmentArray[numSegments-1].size;

							if (track) ii = start -1;
							sizer+=segmentArray[numSegments-2].size + segmentArray[numSegments-1].size; //for debugging
							sizerAll+=len; 								    //for debugging
							float circularity = M_PI*4*(segmentArray[numSegments-1].m0)*(segmentArray[numSegments-1].m1)/queueEnd;
							if (debug) fprintf(stdout,"Circularity: %f %i \n",circularity,queueEnd);
							if (circularity-1.0 < circularityTolerance && circularity-1.0 > -circularityTolerance){
								segmentArray[numSegments-2].valid = segmentArray[numSegments-1].valid = true;
								threshold = (segmentArray[numSegments-2].mean+segmentArray[numSegments-1].mean)/2;
								if (debug) fprintf(stdout,"Circularity: %i %03f %03f %03f \n",queueEnd,M_PI*4*(segmentArray[numSegments-1].m0)*(segmentArray[numSegments-1].m1)/queueEnd,M_PI*4*(segmentArray[numSegments-1].m0)*(segmentArray[numSegments-1].m1),segmentArray[numSegments-1].x/1000.0);

								//chromatic aberation correction
								float r = diameterRatio*diameterRatio;
								float m0o = sqrt(f0);
								float m1o = sqrt(f1);
								float ratio = (float)segmentArray[numSegments-1].size/(segmentArray[numSegments-2].size + segmentArray[numSegments-1].size);
								float m0i = sqrt(ratio)*m0o;
								float m1i = sqrt(ratio)*m1o;
								float a = (1-r);
								float b = -(m0i+m1i)-(m0o+m1o)*r;
								float c = (m0i*m1i)-(m0o*m1o)*r;
							 	float t = (-b-sqrt(b*b-4*a*c))/(2*a);
								//plc second version
								//float t0 = (-b-sqrt(b*b-4*a*c))/(2*a);	
								//float t1 = (-b+sqrt(b*b-4*a*c))/(2*a);
								//if (m1i - t0 > 0 && m1i - t1 >0) printf("AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA\n"); 
								//float t0 = (m0i-diameterRatio*m0o)/(1+diameterRatio);
								//float t1 = (m1i-diameterRatio*m1o)/(1+diameterRatio);
								//m0i-=t;m1i-=t;m0o+=t;m1o+=t;
								//fprintf(stdout,"UUU: %f R: %f %f R: %f %f\n",t,m1i/m1o*0.14,m0i/m0o*0.14,(m0o*m1o-m0i*m1i)/(m0i*m1i),(0.14*0.14-0.05*0.05)/(0.05*0.05));
								segmentArray[numSegments-1].m0 = m0o;
								segmentArray[numSegments-1].m1 = m1o;
								segmentArray[numSegments-1].maxx = segmentArray[numSegments-2].maxx;
								segmentArray[numSegments-1].maxy = segmentArray[numSegments-2].maxy;
								segmentArray[numSegments-1].minx = segmentArray[numSegments-2].minx;
								segmentArray[numSegments-1].miny = segmentArray[numSegments-2].miny;
								segmentArray[numSegments-1].x = sx;
								segmentArray[numSegments-1].y = sy;
								segmentArray[numSegments-1].size = segmentArray[numSegments-2].size+segmentArray[numSegments-1].size;
								segmentArray[numSegments-1].horizontal = sx-segmentArray[numSegments-2].x;
								segmentArray[numSegments-1].angle = atan2(sy-segmentArray[numSegments-2].y,sx-segmentArray[numSegments-2].x);
								segmentArray[numSegments-2].x = sx;
								segmentArray[numSegments-2].y = sy;
							}
						}
					}
				}
			}
		}
		ii++;
		if (ii >= len) ii = 0;
		cont = (ii != start);
	}
	for (int i = 0;i< numSegments;i++){
		if (segmentArray[i].size > minSize && (segmentArray[i].valid || debug)){
			if (debug)fprintf(stdout,"Segment %i Type: %i Pos: %.2f %.2f Area: %i Vx: %i Vy: %i Mean: %i Thr: %i Eigen: %03f %03f %03f Roundness: %03f\n",i,segmentArray[i].type,segmentArray[i].x,segmentArray[i].y,segmentArray[i].size,segmentArray[i].maxx-segmentArray[i].minx,segmentArray[i].maxy-segmentArray[i].miny,segmentArray[i].mean,threshold,segmentArray[i].m0,segmentArray[i].m1,M_PI*4*segmentArray[i].m0*segmentArray[i].m1,segmentArray[i].roundness);
			if (segmentArray[i].valid) result = segmentArray[i];
		}
	}
	if (numSegments == 2 && segmentArray[0].valid && segmentArray[1].valid) lastTrackOK = true; else lastTrackOK = false;
	//Drawing results 
	if (result.valid){
		lastThreshold = threshold;
		drawAll = false;
		numFailed = 0;	
	}else if (numFailed < maxFailed){
		if (numFailed++%2 == 0) changeThreshold(); else threshold = lastThreshold;
		if (debug) drawAll = true;
	}else{
		numFailed++;
		if (changeThreshold()==false) numFailed = 0;
		if (debug) drawAll = true;
	}
	int j = 0;
	for (int p = queueOldStart;p<queueEnd;p++){
			pos = queue[p];	
			image->data[step*pos] = 0;
	}
	for (int p = 0;p< queueOldStart;p++){
			pos = queue[p];	
			image->data[step*pos] = 255;
	}
	bufferCleanup(result);
	return result;
}
Exemplo n.º 5
0
SSegment CCircleDetect::findSegment(CRawImage* image, SSegment init) {//printf("findingSegment\n");
	SSegment result;
	result.x = 0;
	result.y = 0;
	result.round = false;
	result.valid = false;
	numSegments = 0;

	timer.reset();
	timer.start();
	tima += timer.getTime();
	int pos = 0;
	int ii = 0;
	int start = 0;
	bool cont = true;

	//bufferCleanup(init);
	if (init.valid && track) {
		ii = ((int) init.y) * image->getwidth() + init.x;
		start = ii;
	}
	while (cont) {
		if (buffer[ii] == 0) {
			ptr = &image->data[ii * 3];
			//buffer[ii]=((ptr[0]+ptr[1]+ptr[2]) > threshold)-2;
			if ((ptr[0] + ptr[1] + ptr[2]) < threshold)
				buffer[ii] = -2;
		}
		if (buffer[ii] == -2 && numSegments < MAX_SEGMENTS) {
			//new segment found
			queueEnd = 0;
			queueStart = 0;
			//if the segment looks like a ring, we check its inside area
			if (examineSegment(image, &segmentArray[numSegments], ii,
					outerAreaRatio)) {
				pos = segmentArray[numSegments - 1].y * image->getwidth()
						+ segmentArray[numSegments - 1].x;
				if (buffer[pos] == 0) {
					ptr = &image->data[pos * 3];
					buffer[pos] = ((ptr[0] + ptr[1] + ptr[2]) > threshold) - 2;
				}
				if (buffer[pos] == -1 && numSegments < MAX_SEGMENTS) {
					if (examineSegment(image, &segmentArray[numSegments], pos,
							innerAreaRatio)) {
						//the inside area is a circle. now what is the area ratio of the black and white ? also, are the circles concentric ?

						if (debug)
							printf("Area ratio %i/%i is %.3f %.3f %.3f\n",
									numSegments - 2, numSegments - 1,
									(float) segmentArray[numSegments - 2].size
											/ segmentArray[numSegments - 1].size,
									areasRatio,
									segmentArray[numSegments - 2].size
											/ areasRatio
											/ segmentArray[numSegments - 1].size);
						if (((float) segmentArray[numSegments - 2].size
								/ areasRatio
								/ (float) segmentArray[numSegments - 1].size
								- ratioTolerance < 1.0
								&& (float) segmentArray[numSegments - 2].size
										/ areasRatio
										/ (float) segmentArray[numSegments - 1].size
										+ ratioTolerance > 1.0)
								&& (abs(
										segmentArray[numSegments - 1].x
												- segmentArray[numSegments - 2].x)
										<= centerDistanceToleranceAbs
												+ centerDistanceToleranceRatio
														* ((float) (segmentArray[numSegments
																- 2].maxx
																- segmentArray[numSegments
																		- 2].minx)))
								&& (abs(
										segmentArray[numSegments - 1].y
												- segmentArray[numSegments - 2].y)
										<= centerDistanceToleranceAbs
												+ centerDistanceToleranceRatio
														* ((float) (segmentArray[numSegments
																- 2].maxy
																- segmentArray[numSegments
																		- 2].miny)))

								) {
							float tx, ty, cm0, cm1, cm2, sx, sy, fm0, fm1, fm2,
									f0, f1;
							sx = sy = cm0 = cm1 = cm2 = 0;
							//	segmentArray[numSegments-1].x = segmentArray[numSegments-2].x;
							//	segmentArray[numSegments-1].y = segmentArray[numSegments-2].y;

							for (int p = queueOldStart; p < queueEnd; p++) {
								pos = queue[p];
								sx += pos % image->getwidth();
								sy += pos / image->getwidth();
							}
							segmentArray[numSegments - 2].x = sx
									/ (queueEnd - queueOldStart);
							segmentArray[numSegments - 2].y = sy
									/ (queueEnd - queueOldStart);
							for (int p = 0; p < queueOldStart; p++) {
								pos = queue[p];
								sx += pos % image->getwidth();
								sy += pos / image->getwidth();
							}
							sx = sx / queueEnd;
							sy = sy / queueEnd;
							for (int p = 0; p < queueEnd; p++) {
								pos = queue[p];
								tx = pos % image->getwidth() - sx;
								ty = pos / image->getwidth() - sy;
								cm0 += tx * tx;
								cm1 += tx * ty;
								cm2 += ty * ty;
							}
							fm0 = cm0 / queueEnd;
							fm1 = cm1 / queueEnd;
							fm2 = cm2 / queueEnd;
							f0 =
									((fm0 + fm2)
											+ sqrt(
													(fm0 + fm2) * (fm0 + fm2)
															- 4
																	* (fm0 * fm2
																			- fm1
																					* fm1)))
											/ 2;
							f1 =
									((fm0 + fm2)
											- sqrt(
													(fm0 + fm2) * (fm0 + fm2)
															- 4
																	* (fm0 * fm2
																			- fm1
																					* fm1)))
											/ 2;
							segmentArray[numSegments - 1].m0 = sqrt(f0);
							segmentArray[numSegments - 1].m1 = sqrt(f1);
							segmentArray[numSegments - 1].v0 = -fm1
									/ sqrt(fm1 * fm1 + (fm0 - f0) * (fm0 - f0));
							segmentArray[numSegments - 1].v1 = (fm0 - f0)
									/ sqrt(fm1 * fm1 + (fm0 - f0) * (fm0 - f0));
							segmentArray[numSegments - 1].bwRatio =
									(float) segmentArray[numSegments - 2].size
											/ segmentArray[numSegments - 1].size;

							if (track)
								ii = start - 1;
							sizer += segmentArray[numSegments - 2].size
									+ segmentArray[numSegments - 1].size; //for debugging
							sizerAll += len; 					//for debugging
							float circularity = M_PI * 4
									* (segmentArray[numSegments - 1].m0)
									* (segmentArray[numSegments - 1].m1)
									/ queueEnd;
							if (debug)
								fprintf(stdout, "Circularity: %f  \n",
										circularity);
							if (circularity - 1.0 < circularityTolerance
									&& circularity - 1.0
											> -circularityTolerance) {
								segmentArray[numSegments - 2].valid =
										segmentArray[numSegments - 1].valid =
												true;
								threshold = (segmentArray[numSegments - 2].mean
										+ segmentArray[numSegments - 1].mean)
										/ 2;
								if (debug)
									fprintf(stdout,
											"Circularity: %i %03f %03f %03f \n",
											queueEnd,
											M_PI * 4
													* (segmentArray[numSegments
															- 1].m0)
													* (segmentArray[numSegments
															- 1].m1) / queueEnd,
											M_PI * 4
													* (segmentArray[numSegments
															- 1].m0)
													* (segmentArray[numSegments
															- 1].m1),
											segmentArray[numSegments - 1].x
													/ 1000.0);

								//pixel leakage correction
								float r = diameterRatio * diameterRatio;
								float m0o = sqrt(f0);
								float m1o = sqrt(f1);
								float ratio =
										(float) segmentArray[numSegments - 1].size
												/ (segmentArray[numSegments - 2].size
														+ segmentArray[numSegments
																- 1].size);
								float m0i = sqrt(ratio) * m0o;
								float m1i = sqrt(ratio) * m1o;
								float a = (1 - r);
								float b = -(m0i + m1i) - (m0o + m1o) * r;
								float c = (m0i * m1i) - (m0o * m1o) * r;
								float t = (-b - sqrt(b * b - 4 * a * c))
										/ (2 * a);
								m0i -= t;
								m1i -= t;
								m0o += t;
								m1o += t;
								//fprintf(stdout,"UUU: %f %f %f %f %f\n",t,ratio,(m1i-t)/(m1o+t)*0.14,(m0i-t)/(m0o+t)*0.14,(m0o*m1o-m0i*m1i)/(m0i*m1i));
								segmentArray[numSegments - 1].m0 = sqrt(f0) + t;
								segmentArray[numSegments - 1].m1 = sqrt(f1) + t;
								segmentArray[numSegments - 1].maxx =
										segmentArray[numSegments - 2].maxx;
								segmentArray[numSegments - 1].maxy =
										segmentArray[numSegments - 2].maxy;
								segmentArray[numSegments - 1].minx =
										segmentArray[numSegments - 2].minx;
								segmentArray[numSegments - 1].miny =
										segmentArray[numSegments - 2].miny;
								segmentArray[numSegments - 1].x = sx;
								segmentArray[numSegments - 1].y = sy;
								segmentArray[numSegments - 1].size =
										segmentArray[numSegments - 2].size
												+ segmentArray[numSegments - 1].size;
								segmentArray[numSegments - 1].horizontal = sx
										- segmentArray[numSegments - 2].x;
								segmentArray[numSegments - 1].angle = atan2(
										sy - segmentArray[numSegments - 2].y,
										sx - segmentArray[numSegments - 2].x);
								segmentArray[numSegments - 2].x = sx;
								segmentArray[numSegments - 2].y = sy;
							}
						}
					}
				}
			}
		}
		ii++;
		if (ii >= len)
			ii = 0;
		cont = (ii != start);
	}
	for (int i = 0; i < numSegments; i++) {
		if (segmentArray[i].size > minSize
				&& (segmentArray[i].valid || debug)) {
			if (debug)
				fprintf(stdout,
						"Segment %i Type: %i Pos: %.2f %.2f Area: %i Vx: %i Vy: %i Mean: %i Thr: %i Eigen: %03f %03f %03f Roundness: %03f\n",
						i, segmentArray[i].type, segmentArray[i].x,
						segmentArray[i].y, segmentArray[i].size,
						segmentArray[i].maxx - segmentArray[i].minx,
						segmentArray[i].maxy - segmentArray[i].miny,
						segmentArray[i].mean, threshold, segmentArray[i].m0,
						segmentArray[i].m1,
						M_PI * 4 * segmentArray[i].m0 * segmentArray[i].m1,
						segmentArray[i].roundness);
			if (segmentArray[i].valid)
				result = segmentArray[i];
		}
	}
	if (numSegments == 2 && segmentArray[0].valid && segmentArray[1].valid)
		lastTrackOK = true;
	else
		lastTrackOK = false;
	//Drawing results 
	if (result.valid) {
		lastThreshold = threshold;
		drawAll = false;
		numFailed = 0;
	} else if (numFailed < maxFailed) {
		if (numFailed++ % 2 == 0)
			changeThreshold();
		else
			threshold = lastThreshold;
		if (debug)
			drawAll = true;
	} else {
		numFailed++;
		if (changeThreshold() == false)
			numFailed = 0;
		if (debug)
			drawAll = true;
	}
	int j = 0;
	for (int p = queueOldStart; p < queueEnd; p++) {
		pos = queue[p];
		image->data[3 * pos + 0] = 0;
		image->data[3 * pos + 1] = 0;
		image->data[3 * pos + 2] = 0;
	}

	if (draw) {
		for (int i = 0; i < len; i++) {
			j = buffer[i];
			if (j > 0) {
				if (drawAll || segmentArray[j - 1].valid) {
					image->data[i * 3 + j % 3] = 0;
					image->data[i * 3 + (j + 1) % 3] = 255;
					image->data[i * 3 + (j + 2) % 3] = 255;
				}
			}
		}
	}
	bufferCleanup(result);
	return result;
}