void read_images(DistortedLines<T>& distLines, int& w, int& h, int argc, char ** argv, int beginIdx, int endIdx) {
int w_tmp = 0, h_tmp = 0;
ntuple_ll point_set,p;
image_double image;
int length_thresh;
double down_factor, x, y;
int total_nb_lines, total_threshed_nb_lines, threshed_nb_lines;
ntuple_list convolved_pts;
/* check parameters */
if( argc < 4 ) error("use: %s length_threshold sampling_factor <input1.pgm> [input2.pgm...] <polyout_filename>", argv[0]);
assert(beginIdx >= 3 && beginIdx < argc-1 && endIdx >= 3 && endIdx < argc-1);
length_thresh = (int)(atoi(argv[1]));
down_factor = (double)(atoi(argv[2]));
printf("There are %d input images. The minimal length of lines is set to %d\n", endIdx+1-beginIdx, length_thresh);
/* initialize memory */
point_set = new_ntuple_ll(1);
total_nb_lines = 0;
total_threshed_nb_lines = 0;
/* process each of the input images */
for(int i = beginIdx; i <= endIdx; i++) {
threshed_nb_lines = 0;
/* open image, compute edge points, close it */
image = read_pgm_image_double(argv[i]);
p = straight_edge_points(image,sigma,th_low,th_hi,min_length);
w = image->xsize; h = image->ysize;
if (i == beginIdx) {w_tmp = w; h_tmp = h;}
else { assert(w == w_tmp && h == h_tmp); }
free_image_double(image);
/* copy the points set in the new image to the global set of points */
for(int j=0; j<(int)p->size; j++) {
if((int)p->list[j]->size > length_thresh) {
add_ntuple_list(point_set, p->list[j]);
p->list[j] = 0; }
else
threshed_nb_lines += 1; }
printf("For image %d, there are totally %d lines detected and %d of them are eliminated.\n", i-2, p->size, threshed_nb_lines);
total_nb_lines += p->size;
total_threshed_nb_lines += threshed_nb_lines;
free_ntuple_ll(p);
}
distLines.pushMemGroup((int)point_set->size);
int countL = 0;
for(unsigned int i=0; i<point_set->size; i++) {
/* Gaussian convolution and sub-sampling */
convolved_pts = gaussian_convol_on_curve(unit_sigma, Nsigma, resampling, eliminate_border, up_factor, down_factor, point_set->list[i]);
countL++;
/* Save points to DistortionLines structure */
for(unsigned int j = 0; j < convolved_pts->size; j++) {
x = convolved_pts->values[j*convolved_pts->dim];
y = convolved_pts->values[j*convolved_pts->dim+1];
distLines.pushPoint(countL-1,x,y); }
}
printf("Totally there are %d lines detected and %d of them are eliminated.\n", total_nb_lines, total_threshed_nb_lines);
/* free memory */
free_ntuple_ll(point_set);
free_ntuple_list(convolved_pts);
}
T centerLMA(image_double sub_img, bool clr, T& centerX, T& centerY)
{
image_double img_avg = average_image(sub_img);
int w = sub_img->xsize;
int h = sub_img->ysize;
T cx = w/2, cy = h/2, radi = 0.4*w;
vector<T> P(11);
initial_tache(sub_img, P, radi, clr, cx, cy);
vector<T> trgData = trgtDataCalc<T>(img_avg, P[3], P[4], radi*2);
LMTacheC<T> ellipseLMA(img_avg, P[3], P[4], radi*2, clr, w, h);
T rmse = ellipseLMA.minimize(P, trgData, 0.001);
free_image_double(img_avg);
//T lambda1 = P[0]; T lambda2 = P[1]; T theta = P[2];
centerX = P[3];
centerY = P[4];
return rmse;
}
int main( int argc, char** argv){
/*colors*/
CvScalar green = CV_RGB(0,255,0);
CvScalar white = CV_RGB(255,255,255);
CvScalar black = CV_RGB(0,0,0);
CvScalar red = CV_RGB(255,0,0);
CvScalar blue = CV_RGB(0,0,255);
cvNamedWindow( "opencv on acid",CV_WINDOW_AUTOSIZE);
cvNamedWindow( "lsd",CV_WINDOW_AUTOSIZE);
CvCapture* capture;
if ( argc == 1 ) {
capture = cvCreateCameraCapture (0);
} else {
capture = cvCreateFileCapture (argv[1]);
}
assert( capture != NULL );
IplImage* frame;
image_double image;
int width, height, i, j;
while (1) {
frame = cvQueryFrame( capture );
if( !frame ) break;
/* get image properties */
width = frame->width;
height = frame->height;
/* create new image for the grayscale version */
IplImage* frameBW = cvCreateImage( cvSize( width, height ), IPL_DEPTH_8U, 1 );
/*convert to grayscale*/
cvCvtColor( frame , frameBW, CV_RGB2GRAY);
/*cast into lsd image struct*/
image = new_image_double(width, height);
uchar* data = (uchar*)frameBW->imageData;
for (i=0;i<height;i++){
for(j=0;j<width;j++){
image->data[ j + i * width ] = data[j + i*width];
};
};
/*run lsd*/
ntuple_list ntl;
ntl = lsd(image);
free_image_double(image);
/*filter lsd segments*/
int degrees_thr = 20; // degrees of threshold
int distance_thr = 49; // 7 pixels
ntuple_list ntlFilt = new_ntuple_list(5);
filterSegments( ntl, ntlFilt , distance_thr);
/********************/
//printf("A ver: %3.2f\n",ntl->values[1]);
/*draw segments on frame and frameLsd*/
IplImage* frameLsd = cvCreateImage( cvSize( width, height ), IPL_DEPTH_8U, 3 );
cvSet(frameLsd, black, 0);
for (j=0; j<ntl->size ; j++){
//define segment end-points
CvPoint pt1 = cvPoint(ntl->values[ 0 + j * ntl->dim ],ntl->values[ 1 + j * ntl->dim ]);
CvPoint pt2 = cvPoint(ntl->values[ 2 + j * ntl->dim ],ntl->values[ 3 + j * ntl->dim ]);
// draw line on frame
cvLine(frame,pt1,pt2,white,1.5,8,0);
// draw line on frameLsd
cvLine(frameLsd,pt1,pt2,white,1.5,8,0);
}
/*draw segments on actual frameLsdFilt*/
IplImage* frameLsdFilt = cvCreateImage( cvSize( width, height ), IPL_DEPTH_8U, 3 );
cvSet(frameLsdFilt, black, 0);
j = 0;
for (j=0; j<ntlFilt->size ; j++){
//define segment end-points
CvPoint pt1 = cvPoint(ntlFilt->values[ 0 + j * ntlFilt->dim ],ntlFilt->values[ 1 + j * ntlFilt->dim ]);
CvPoint pt2 = cvPoint(ntlFilt->values[ 2 + j * ntlFilt->dim ],ntlFilt->values[ 3 + j * ntlFilt->dim ]);
// draw line on frameLsd
cvLine(frameLsdFilt,pt1,pt2,white,1.5,8,0);
}
cvShowImage("opencv on acid",frame);
cvShowImage("lsd",frameLsd);
cvShowImage("lsd filtrado",frameLsdFilt);
char c = cvWaitKey(25);
if( c == 27 ) break;
}
cvReleaseCapture( &capture );
cvDestroyWindow( "opencv on acid");
cvDestroyWindow( "lsd");
cvDestroyWindow( "lsd filtrado");
}
/**
* @brief Coarse detection of the pattern by using segments detected from LSD
* @param in - input image float
* @return Array of floats containing the OPQR 2D point positions of the
* pattern
*/
static float *detect_pattern_coarse(ImageFloat in)
{
image_double in_lsd;
ntuple_list seg;
float min_val, max_val;
double *seg_length, *seg_midpoint_x, *seg_midpoint_y;
char is_in_distance;
double xO, xP, xQ, xR, yO, yQ, yP, yR, l;
int *has_seg, *has_all_seg, point;
/* To keep track of the error from the optimal segment position
* just to choose the closest segment to the optimal location
*/
double seg_error[NUM_SEG];
double xi1, xi2, yi1, yi2, xj1, xj2, yj1, yj2, xjN1, xjN2, yjN1, yjN2;
double xoMp, xpMq, xqMr, xrMo, yoMp, ypMq, yqMr, yrMo;
double xC, yC;
/* Structure of the Pattern Seg0,Seg1,...,Seg10,Seg_Orient0,Seg_Orient1 */
const double x1S[] = { 3, 6, 7, 7, 7, 6, 3, 0, -2, -2, -2, 1, 8 };
const double y1S[] = { 0, 0, -1, -4, -7, -9, -9, -9, -7, -4, -1, 1, 0 };
const double x2S[] = { 3, 6, 8, 8, 8, 6, 3, 0, -1, -1, -1, 1, 7 };
const double y2S[] = { 1, 1, -1, -4, -7, -8, -8, -8, -7, -4, -1, 0, 0 };
int i = 0, j = 0, k = 0, c = 0;
double errorc;
double actual_scale;
char ready;
float *opqr = (float *) malloc(8 * sizeof(float));
/* Execute LSD */
/* Convert between images types and renormalize the image to [0,255]*/
min_val = BIG_NUMBER;
max_val = 0;
for (i=0; i< in->ncol *in->nrow;i++)
{
if(in->val[i] < min_val) min_val = in->val[i];
if(in->val[i] > max_val) max_val = in->val[i];
}
in_lsd = new_image_double((unsigned int) in->ncol,
(unsigned int) in->nrow);
for (i = 0; i < in->ncol * in->nrow; i++)
in_lsd->data[i] = (double) 255/(max_val-min_val)*(in->val[i]-min_val);
/* We do a LOOP from INITIAL_SCALE_LSD to MAX_SCALE_LSD */
actual_scale = INITIAL_SCALE_LSD;
ready = 0;
while (actual_scale < MAX_SCALE_LSD && !ready)
{
printf(" -->LSD scale =%f\n",actual_scale);
seg = lsd_scale(in_lsd, actual_scale);
/* allocate the array or exit with an error */
if ((seg_length = (double *) malloc(seg->size * sizeof(double)))
== NULL
|| (seg_midpoint_x = (double *) malloc(seg->size * sizeof(double)))
== NULL
|| (seg_midpoint_y = (double *) malloc(seg->size * sizeof(double)))
== NULL)
{
error("PSF_ESTIM - Unable to allocate double array space");
exit(EXIT_FAILURE);
}
/*
The i component, of the n-tuple number j, of an n-tuple list 'ntl'
is accessed with:
*/
for (i = 0; i < (int) seg->size; i++)
{
/* segment length */
seg_length[i] = dist_l2(seg->values[i * seg->dim],
seg->values[i * seg->dim + 1],
seg->values[i * seg->dim + 2],
seg->values[i * seg->dim + 3]);
/* segment midpoint */
seg_midpoint_x[i] =
0.5 * (seg->values[i * seg->dim]
+ seg->values[i * seg->dim + 2]);
seg_midpoint_y[i] =
0.5 * (seg->values[i * seg->dim + 1]
+ seg->values[i * seg->dim + 3]);
}
/* Accessing to segment j=0...12 associated to segment i
* has_seg[NUM_SEG*i+j], initialization default to 0
*/
if ((has_seg =
(int *) malloc(NUM_SEG * seg->size * sizeof(int))) == NULL
|| (has_all_seg =
(int *) malloc(seg->size * sizeof(int))) == NULL)
{
error("PSF_ESTIM - Unable to allocate double array space");
exit(EXIT_FAILURE);
}
/* has_seg[], Initialization default to -1 */
for (i = 0; i < NUM_SEG * (int) seg->size; i++)
{
has_seg[i] = -1;
}
/* has_all_seg[], Initialization default to 0 */
/* First pass */
for (i = 0; i < (int) seg->size; i++)
{
xi1 = seg->values[i * seg->dim];
xi2 = seg->values[i * seg->dim + 2];
yi1 = seg->values[i * seg->dim + 1];
yi2 = seg->values[i * seg->dim + 3];
/* Reinitialize the error track */
for (j = 0; j < NUM_SEG; j++)
{
seg_error[j] = BIG_NUMBER;
}
for (j = 0; j < (int) seg->size; j++)
{
xj1 = seg->values[j * seg->dim];
xj2 = seg->values[j * seg->dim + 2];
yj1 = seg->values[j * seg->dim + 1];
yj2 = seg->values[j * seg->dim + 3];
/* Convert the (x,y) coordinates to a new Coordinate System
* (xN, yN) having:
* (xi1,yi1) at (0,0)
* (xi2,yi2) at (0,1)
* The vectors x1s,y1s,x2s,y2s are given within this
* new (xN,yN) coordinate system
*/
l = seg_length[i];
xjN1 =
1 / (l * l) * ((yi2 - yi1) * (xj1 - xi1)
- (xi2 - xi1) * (yj1 - yi1));
yjN1 =
1 / (l * l) * ((xi2 - xi1) * (xj1 - xi1)
+ (yi2 - yi1) * (yj1 - yi1));
xjN2 =
1 / (l * l) * ((yi2 - yi1) * (xj2 - xi1)
- (xi2 - xi1) * (yj2 - yi1));
yjN2 =
1 / (l * l) * ((xi2 - xi1) * (xj2 - xi1)
+ (yi2 - yi1) * (yj2 - yi1));
for (c = 0; c < NUM_SEG; c++)
{
is_in_distance =
(fabs(xjN1 - x1S[c]) < TOL * (2 + fabs(x1S[c])))
&& (fabs(yjN1 - y1S[c]) < TOL * (2 + fabs(y1S[c])))
&& (fabs(xjN2 - x2S[c]) < TOL * (2 + fabs(x2S[c])))
&& (fabs(yjN2 - y2S[c]) < TOL * (2 + fabs(y2S[c])));
if (is_in_distance)
{
/* Need to check that there isn't a previous segment
* closer to the optimal location and already marked
* as good (errorc). I just keep the segment with
* minimum total error l1.
*/
errorc = fabs(xjN1 - x1S[c]) + fabs(yjN1 - y1S[c])
+ fabs(xjN2 - x2S[c]) + fabs(yjN2 - y2S[c]);
if (errorc < seg_error[c])
{
has_seg[i * NUM_SEG + c] = j;
seg_error[c] = errorc;
}
}
}
}
/*has_all_seg[i] will be one if all segments are present */
has_all_seg[i] = 1;
for (j=0;j< NUM_SEG;j++)
{
has_all_seg[i] =
has_all_seg[i] && (has_seg[i * NUM_SEG + j] >= 0);
}
if (has_all_seg[i])
{
point = i;
k++;
}
}
ready = (k==1);
actual_scale *= 1.15;
}
if (k > 1)
{
printf("More than one pattern was detected.");
printf("\nCrop the image surounding the desired pattern and re-run.");
exit(EXIT_SEVERAL_PATTERNS_DETECTED);
}
else if(k<1)
{
printf("No pattern was detected. Use another image");
exit(EXIT_NO_PATTERN_DETECTED);
}
/*Calculate C - center, u = unit_length, theta = angle */
/* 1/3*(DET + 0 + 1) = oMp */
xoMp = 0.33333 * (seg_midpoint_x[point]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 0]]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 1]]);
yoMp = 0.33333 * (seg_midpoint_y[point]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 0]]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 1]]);
/* 1/3*(2 + 3 + 4) = pMq */
xpMq = 0.33333 * (seg_midpoint_x[has_seg[point * NUM_SEG + 2]]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 3]]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 4]]);
ypMq = 0.33333 * (seg_midpoint_y[has_seg[point * NUM_SEG + 2]]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 3]]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 4]]);
/* 1/3*(5 + 6 + 7) = qMr */
xqMr = 0.33333 * (seg_midpoint_x[has_seg[point * NUM_SEG + 5]]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 6]]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 7]]);
yqMr = 0.33333 * (seg_midpoint_y[has_seg[point * NUM_SEG + 5]]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 6]]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 7]]);
/* 1/3*(8 + 9 + 10) = rMo */
xrMo = 0.33333 * (seg_midpoint_x[has_seg[point * NUM_SEG + 8]]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 9]]
+ seg_midpoint_x[has_seg[point * NUM_SEG + 10]]);
yrMo = 0.33333 * (seg_midpoint_y[has_seg[point * NUM_SEG + 8]]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 9]]
+ seg_midpoint_y[has_seg[point * NUM_SEG + 10]]);
/*Center */
xC = 0.25 * (xoMp + xpMq + xqMr + xrMo);
yC = 0.25 * (yoMp + ypMq + yqMr + yrMo);
/*O = C + CoMr + CoMp */
xO = xC + (xrMo - xC) + (xoMp - xC);
yO = yC + (yrMo - yC) + (yoMp - yC);
/*P = C + CpMq + CoMp */
xP = xC + (xpMq - xC) + (xoMp - xC);
yP = yC + (ypMq - yC) + (yoMp - yC);
/*Q = C + CqMr + CpMq */
xQ = xC + (xqMr - xC) + (xpMq - xC);
yQ = yC + (yqMr - yC) + (ypMq - yC);
/*R = C + CrMo + CqMr */
xR = xC + (xrMo - xC) + (xqMr - xC);
yR = yC + (yrMo - yC) + (yqMr - yC);
/*Array of OPQR coordinates*/
opqr[0] = (float) xO;
opqr[1] = (float) yO;
opqr[2] = (float) xP;
opqr[3] = (float) yP;
opqr[4] = (float) xQ;
opqr[5] = (float) yQ;
opqr[6] = (float) xR;
opqr[7] = (float) yR;
/* free memory */
free_image_double(in_lsd);
free_ntuple_list(seg);
free(seg_length);
free(seg_midpoint_y);
free(seg_midpoint_x);
free(has_seg);
free(has_all_seg);
return opqr;
}
//返回直线个数，计算k,b
int lineDetect(double* k, double* b)
{
int i, j;
UINT32T col;
UINT32T row;
image_double image_LSD = new_image_double(C, R);
ntuple_list detected_lines;
int dim;
UINT8T pGray[SIZE];
double angletemp;
double length;
double temp_k, temp_b;
int counter = 0;
#ifdef WIN32
CvPoint start_pt;
CvPoint end_pt;
#endif
// LSD算法检测直线
for (row = 1; row<(R - 1); row++)
{
for (col = 1; col<(C - 1); col++)
{
image_LSD->data[row*C + col] = image_Gray[row*C + col];//im_gray是灰度图像，没有颜色通道
}
}
detected_lines = lsd(image_LSD);//detected_lines中存储提取直线的首位坐标及宽度，具体意义见说明文档
free_image_double(image_LSD);
// LSD结果显示
#ifdef WIN32
memset(pGray, 0, SIZE);
cvSetData(image_1ch, pGray, C);
#endif
dim = detected_lines->dim;
//printf("Number of lines detected = %d", detected_lines->size);
//double angle[770]; //detected_lines->size = 770
for (j = 0; j < detected_lines->size; j++)
{
//cvLine(res_im,start_pt,end_pt,CV_RGB(j%255,(5*j)%255,(9*j)%255),1,CV_AA);
//��
temp_k = (detected_lines->values[j*dim + 1] - detected_lines->values[j*dim + 3]) / (detected_lines->values[j*dim + 0] - detected_lines->values[j*dim + 2]);
angletemp = (int)(atan(temp_k) * 180 / 3.1416);
//检查倾角是否满足条件
if (angletemp>20 || angletemp < -20)
continue;
//判断长度是否满足条件
length = sqrt((detected_lines->values[j*dim + 2] - detected_lines->values[j*dim + 0]) * (detected_lines->values[j*dim + 2] - detected_lines->values[j*dim + 0]) + (detected_lines->values[j*dim + 3] - detected_lines->values[j*dim + 1])*(detected_lines->values[j*dim + 3] - detected_lines->values[j*dim + 1]));
if (length < (double)C / 8)
continue;
//printf("j = %d, length = %f\n", j, length);
//对满足两个条件的线段，求直线方程
k[counter] = temp_k;
temp_b = detected_lines->values[j*dim + 1] - temp_k * detected_lines->values[j*dim + 0];
b[counter] = temp_b;
counter++;
#ifdef WIN32
start_pt = cvPoint((int)detected_lines->values[j*dim + 0], (int)detected_lines->values[j*dim + 1]);
end_pt = cvPoint((int)detected_lines->values[j*dim + 2], (int)detected_lines->values[j*dim + 3]);
cvLine(image_1ch, start_pt, end_pt, CV_RGB(0, 0, 255), 1, CV_AA, 0);
#endif
}
#ifdef WIN32
cvNamedWindow("LSD", 0);
cvShowImage("LSD", image_1ch);
cvWaitKey(0);
#endif
return counter;
}
