void find_corners_from_lines(image_u32_t* im, loc_t* lines, int num_lines,
int* num_corners, loc_t* corners)
{
// find intersection of each line with the other lines,
// if intersection is unique, add to the corners
for(int i = 0; i < num_lines; i++) {
for(int j = 0; j < num_lines; j++) {
if(i != j) {
loc_t intersect = get_line_intersection(lines[i*2], lines[i*2+1],
lines[j*2], lines[j*2+1]);
// printf("(%d, %d) (%d, %d) X (%d, %d) (%d, %d) = (%d, %d)\n",
// lines[i*2].x, lines[i*2].y, lines[i*2+1].x, lines[i*2+1].y,
// lines[j*2].x, lines[j*2].y, lines[j*2+1].x, lines[j*2+1].y,
// intersect.x, intersect.y);
// intersect inside image
if(!in_range(im, intersect.x, intersect.y)) continue;
// not already a corner
if(loc_already_in(corners, intersect, *num_corners)) continue;
// printf(" good\n");
corners[*num_corners] = intersect;
(*num_corners)++;
}
}
}
printf("num_corners: %d\n", *num_corners);
}
int lzr_frame_mask(lzr_frame* frame, lzr_frame* mask)
{
//bail early if there aren't any mask points
if(!mask->n_points)
return LZR_SUCCESS;
//setup a buffer to build the finished product
lzr_frame output;
output.n_points = 0;
//eeewww
//TODO: find a faster way to do this
for(size_t i = 1; i < frame->n_points; i++)
{
for(size_t m = 1; i < mask->n_points; i++)
{
lzr_point intersection;
if(get_line_intersection(frame->points[i - 1],
frame->points[i],
mask->points[i - 1],
mask->points[i],
&intersection))
{
//TODO
}
}
}
return LZR_SUCCESS;
}
double localization_node::getClosestWallDistanceFL(double x, double y, double theta)
{
double min = 999;
double new_min = 0;
double contactx,dx;
double contacty,dy;
double theta1 = theta;
double x1 = x + (x_fw_offset_*cos(-theta) + y_fw_offset_*sin(-theta));
double y1 = y + (-x_fw_offset_*sin(-theta)+ y_fw_offset_*cos(-theta));
double x2 = x + ((x_fw_offset_+2.8)*cos(-theta)+(y_fw_offset_)*sin(-theta));
double y2 = y + (-(x_fw_offset_+2.8)*sin(-theta)+(y_fw_offset_)*cos(-theta));
double front_dist;
int collision;
for(int i=0;i<=14;i++)
{
collision = get_line_intersection(x1,y1,x2,y2,map_segments[i].x1, map_segments[i].y1, map_segments[i].x2, map_segments[i].y2, &front_dist, contactx, contacty);
if(front_dist>0.8)
{
front_dist = 0.8;
}
if(collision)
{
if(front_dist>0)
{
if(front_dist<min)
{
min=front_dist;
dx=contactx;
dy=contacty;
}
}
}
if(min>RANGE_FOR_LONG)
{
min = RANGE_FOR_LONG;
}
}
//draw_cool_laser(x1,y1,x2,y2);
return min + WALL_THICKNESS;
}
double localization_node::getClosestWallDistanceLB(const double x,const double y,const double theta)
{
double min = 999;
double new_min = 0;
double contactx,dx;
double contacty,dy;
double theta1 = theta + M_PI/2; //TODO
double x1 = x + (-x_offset_*cos(-theta) + y_offset_*sin(-theta));
double y1 = y + (x_offset_*sin(-theta)+ y_offset_*cos(-theta));
double x2 = x + (-x_offset_*cos(-theta)+(y_offset_+0.8)*sin(-theta));
double y2 = y + (x_offset_*sin(-theta)+(y_offset_+0.8)*cos(-theta));
double left_dist;
int collision;
for(int i=0;i<=14;i++)
{
collision = get_line_intersection(x1,y1,x2,y2,map_segments[i].x1, map_segments[i].y1, map_segments[i].x2, map_segments[i].y2, &left_dist, contactx, contacty);
if(left_dist>0.8)
{
left_dist = 0.8;
}
if(collision)
{
if(left_dist>0)
{
if(left_dist<min)
{
min=left_dist;
dx=contactx;
dy=contacty;
}
}
}
}
if(min>RANGE_FOR_SHORT)
{
min = RANGE_FOR_SHORT;
}
//draw_cool_laser(x1,y1,dx,dy);
return min+ WALL_THICKNESS;
}
double localization_node::getClosestWallDistanceRF(double x, double y, double theta)
{
double min = 999;
double new_min = 0;
double contactx,dx;
double contacty,dy;
double theta1 = theta - M_PI/2;
double x1 = x + (x_offset_*cos(-theta) + -y_offset_*sin(-theta));
double y1 = y + (-x_offset_*sin(-theta)+ -y_offset_*cos(-theta));
double x2 = x + (x_offset_*cos(-theta)+(-y_offset_-0.8)*sin(-theta));
double y2 = y + (-x_offset_*sin(-theta)+(-y_offset_-0.8)*cos(-theta));
double right_dist;
int collision;
for(int i=0;i<=14;i++) //Iterate over the walls
{
collision = get_line_intersection(x1,y1,x2,y2,map_segments[i].x1, map_segments[i].y1, map_segments[i].x2, map_segments[i].y2, &right_dist, contactx, contacty);
if(right_dist>0.8)
{
right_dist = 0.8;
}
if(collision)
{
if(right_dist>0)
{
if(right_dist<min)
{
min=right_dist;
dx=contactx;
dy=contacty;
}
}
}
}
if(min>RANGE_FOR_SHORT)
{
min = RANGE_FOR_SHORT;
}
//draw_cool_laser(x1,y1,dx,dy);
return min+ WALL_THICKNESS;
}
bool Graph::isPathObstructed(int x0, int y0, int x1, int y1 ) {
vector<MapWall> walls = map->getWalls();
vector<MapWall>::iterator iter;
for( iter = walls.begin(); iter != walls.end(); iter++ ) {
double * ix = new double();
double * iy = new double();
if ( get_line_intersection( static_cast<double>(x0), static_cast<double>(y0), static_cast<double>(x1),
static_cast<double>(y1), static_cast<double>(iter->getX0()),
static_cast<double>(iter->getY0()),static_cast<double>(iter->getX1()),
static_cast<double>(iter->getY1()), ix, iy ) )
return true;
}
return false;
}
void extend_lines_to_edge_of_image(image_u32_t* im, loc_t p1, loc_t p2, loc_t* out)
{
loc_t boundary[8] = { {1,1}, {1, 10}, // left-side
{1,1}, {10, 1}, // bottom-side
{(im->width-1), 1}, {(im->width-1), 10}, // right-side
{1, (im->height-1)}, {10, (im->height-1)}}; // top-side
int num_point = 0;
// search over all 4 sides of box for intersection that is within boundary
for(int i = 0; i < 4; i++) {
loc_t intersect = get_line_intersection(p1, p2, boundary[i*2], boundary[i*2+1]);
if(in_range(im, intersect.x, intersect.y)) {
out[num_point++] = intersect;
}
}
}
bool Observation::isWallBlocking(MapMarker marker, Position position) const{
double mx = marker.getX();
double my = marker.getY();
double px = position.getX();
double py = position.getY();
double ix, iy;
vector<MapWall> walls = map->getWalls();
vector<MapWall>::iterator iter;
for( iter = walls.begin(); iter !=walls.end(); iter++ ){
if ( get_line_intersection(iter->getX0(),iter->getY0(),iter->getX1(),iter->getY1(),mx,my,px,py,&ix,&iy) ){
return true;
}
}
return false;
}
double localization_node::getClosestWallDistanceLB(const double x,const double y,const double theta)
{
double min = 999;
double new_min = 0;
double contactx;
double contacty;
double theta1 = theta + M_PI/2; //TODO
//double x1 = lbtransform_.getOrigin().x();
//double y1 = lbtransform_.getOrigin().y();
double x1 = x + (-x_offset_*cos(-theta) + y_offset_*sin(-theta));
double y1 = y + (x_offset_*sin(-theta)+ y_offset_*cos(-theta));
double x2 = x + (-x_offset_*cos(-theta)+(y_offset_+0.8)*sin(-theta));
double y2 = y + (x_offset_*sin(-theta)+(y_offset_+0.8)*cos(-theta));
// double x2 = x1 + 2*cos(theta1);
// double y2 = y1 + 2*sin(theta1);
leftirx_ = x2;
leftiry_ = y2;
left_ir_marker.pose.position.x = leftirx_;
left_ir_marker.pose.position.y = leftiry_;
double left_dist;
int collision;
for(int i=0;i<=14;i++)
{
collision = get_line_intersection(x1,y1,x2,y2,map_segments[i].x1, map_segments[i].y1, map_segments[i].x2, map_segments[i].y2, &left_dist, contactx, contacty);
if(left_dist>0.8)
{
left_dist = 0.8;
}
if(collision)
{
if(left_dist<min)
{
if(left_dist>0.001)
{
min=left_dist;
ipt_x_l_ = contactx;
ipt_y_l_ = contacty;
}
}
}
if(min>0.8||min<=0.1)
{
min = 0.8;
}
}
return min;
}
double localization_node::getClosestWallDistanceFL(double x, double y, double theta)
{
double min = 999;
double new_min = 0;
double contactx;
double contacty;
double theta1 = theta;
double x1 = x + (x_fw_offset_*cos(-theta) + y_fw_offset_*sin(-theta));
double y1 = y + (-x_fw_offset_*sin(-theta)+ y_fw_offset_*cos(-theta));
double x2 = x + ((x_fw_offset_+0.8)*cos(-theta)+(y_fw_offset_)*sin(-theta));
double y2 = y + ((-x_fw_offset_+0.8)*sin(-theta)+(y_fw_offset_)*cos(-theta));
//double x1 = fwltransform_.getOrigin().x();
//double y1 = fwltransform_.getOrigin().y();
//double x2 = x1 + 2.0*cos(theta1);
//double y2 = y1 + 2.0*sin(theta1);
double front_dist;
int collision;
for(int i=0;i<=14;i++)
{
collision = get_line_intersection(x1,y1,x2,y2,map_segments[i].x1, map_segments[i].y1, map_segments[i].x2, map_segments[i].x2, &front_dist, contactx, contacty);
if(front_dist>0.8)
{
front_dist = 0.8;
}
if(collision)
{
if(front_dist<min)
{
if(front_dist>0.001)
{
min=front_dist;
}
}
}
if(front_dist==0)
{
//return -1.0f;
}
}
if(min>0.8||min<=0.1)
{
min = 0.8;
}
return min;
}
double localization_node::getClosestWallDistanceRB(double x, double y, double theta)
{
/*Function does not have global variables to store intersection point*/
double min = 999;
double new_min = 0;
double contactx;
double contacty;
double dx;
double dy;
double theta1 = theta - M_PI/2;
//double x1 = rbtransform_.getOrigin().x();
//double y1 = rbtransform_.getOrigin().y();
//std::cout<<"rx: "<<x1<<" ry: "<<y1<<std::endl;
double x1 = x + (-x_offset_*cos(-theta) + -y_offset_*sin(-theta));
double y1 = y + (x_offset_*sin(-theta)+ -y_offset_*cos(-theta));
double x2 = x + (-x_offset_*cos(-theta)+(-y_offset_-0.8)*sin(-theta));
double y2 = y + (x_offset_*sin(-theta)+(-y_offset_-0.8)*cos(-theta));
//double x2 = x1 + 2*cos(theta1);
//double y2 = y1 + 2*sin(theta1);
rightirx_ = x2;
rightiry_ = y2;
right_ir_marker.pose.position.x = rightirx_;
right_ir_marker.pose.position.y = rightiry_;
double right_dist;
int collision;
for(int i=0;i<=14;i++) //Iterate over the walls
{
collision = get_line_intersection(x1,y1,x2,y2,map_segments[i].x1, map_segments[i].y1, map_segments[i].x2, map_segments[i].y2, &right_dist, contactx, contacty);
if(right_dist>0.8)
{
right_dist = 0.8;
}
if(collision)
{
//std::cout<<"Distance: "<<right_dist<<std::endl;
if(right_dist>0.001)
{
if(right_dist<min)
{
min=right_dist;
ipt_x_r_ = contactx;
ipt_y_r_ = contacty;
}
}
}
if(right_dist==0)
{
//return -1.0f;
}
}
if(min>0.8||min<=0.1)
{
min = 0.8;
}
return min;
}
// returns the 35 points associated to the test chart in [x1,y1,x2,y2]
// format if there are more than 35 points will return NULL
matd_t* build_homography(image_u32_t* im, vx_buffer_t* buf, metrics_t met)
{
frame_t frame = {{0,0}, {im->width-1, im->height-1},
{0,0}, {1,1}};
int good_size = 0;
zarray_t* blobs = zarray_create(sizeof(node_t));
hsv_find_balls_blob_detector(im, frame, met, blobs, buf);
// remove unqualified blobs
if(met.qualify) {
for(int i = 0; i < zarray_size(blobs); i++) {
node_t n;
zarray_get(blobs, i, &n);
if(!blob_qualifies(im, &n, met, buf))
zarray_remove_index(blobs, i, 0);
}
}
if(zarray_size(blobs) == NUM_TARGETS ||zarray_size(blobs) == NUM_CHART_BLOBS) good_size = 1;
zarray_sort(blobs, compare);
int pix_array[zarray_size(blobs)*2];
// iterate through
int idx = 0;
double size = 2.0;
for(int i = 0; i < zarray_size(blobs); i++) {
node_t n;
zarray_get(blobs, i, &n);
loc_t center = { .x = n.ave_loc.x/n.num_children,
.y = n.ave_loc.y/n.num_children};
loc_t parent = { .x = n.id % im->stride,
.y = n.id / im->stride};
if(buf != NULL) {
add_circle_to_buffer(buf, size, center, vx_maroon);
// add_circle_to_buffer(buf, size, parent, vx_olive);
// add_sides_to_buffer(im, buf, 1.0, &n, vx_orange, met);
loc_t* lp = fit_lines(im, &n, buf, met, NULL);
if(lp != NULL) {
// printf("(%d, %d) (%d, %d) (%d, %d) (%d, %d) \n",
// lp[0].x, lp[0].y, lp[1].x, lp[1].y, lp[2].x, lp[2].y, lp[3].x, lp[3].y);
loc_t intersect = get_line_intersection(lp[0], lp[1], lp[2], lp[3]);
if(in_range(im, intersect.x, intersect.y)) {
loc_t ext_lines[2];
extend_lines_to_edge_of_image(im, intersect, center, ext_lines);
add_line_to_buffer(im, buf, 2.0, ext_lines[0], ext_lines[1], vx_blue);
}
for(int i = 0; i < 4; i++) {
pix_array[i*2] = lp[i].x;
pix_array[i*2+1] = lp[i].y;
add_circle_to_buffer(buf, 3.0, lp[i], vx_orange);
}
}
free(n.sides);
// loc_t corners[4] = {{n.box.right, n.box.top},
// {n.box.right, n.box.bottom},
// {n.box.left, n.box.bottom},
// {n.box.left, n.box.top}};
// print extremes of box
// if(1) {
// add_circle_to_buffer(buf, size, corners[0], vx_green);
// add_circle_to_buffer(buf, size, corners[1], vx_yellow);
// add_circle_to_buffer(buf, size, corners[2], vx_red);
// add_circle_to_buffer(buf, size, corners[3], vx_blue);
// for(int j = 0; j < 4; j++) {
// // add_circle_to_buffer(buf, size, corners[j], vx_maroon);
// }
// }
}
}
matd_t* H;
H = dist_homography(pix_array, NUM_TARGETS);
// if(0) {//zarray_size(blobs) == NUM_CHART_BLOBS){
// H = dist_homography(pix_array, NUM_CHART_BLOBS);
// }
// else if(zarray_size(blobs) == NUM_TARGETS){
// H = dist_homography(pix_array, NUM_TARGETS);
// if(met.add_lines) connect_lines(blobs, buf);
// }
// else {
// if(met.dothis)
// printf("num figures: %d\n", zarray_size(blobs));
// return(NULL);
// }
// make projected points
// project_measurements_through_homography(H, buf, blobs, zarray_size(blobs));
zarray_destroy(blobs);
return(H);
}
/*
{ R00, R01, R02, TX,
R10, R11, R12, TY,
R20, R21, R22, TZ,
0, 0, 0, 1 });
*/
double get_rotation(const char* axis, matd_t* H)
{
double cosine, sine, theta;
if(strncmp(axis,"x", 1)) {
cosine = MATD_EL(H, 1, 1);
sine = MATD_EL(H, 2, 1);
}
else if(strncmp(axis,"y", 1)) {
cosine = MATD_EL(H, 0, 0);
sine = MATD_EL(H, 0, 2);
}
else if(strncmp(axis,"z", 1)) {
cosine = MATD_EL(H, 0, 0);
sine = MATD_EL(H, 1, 0);
}
else assert(0);
theta = atan2(sine, cosine);
return(theta);
}
// if buf is NULL, will not fill with points of the homography
void take_measurements(image_u32_t* im, vx_buffer_t* buf, metrics_t met)
{
// form homography
matd_t* H = build_homography(im, buf, met);
if(H == NULL) return;
// get model view from homography
matd_t* Model = homography_to_pose(H, 654, 655, 334, 224);
// printf("\n");
// matd_print(H, matrix_format);
// printf("\n\n");
// printf("model:\n");
// matd_print(Model, "%15f");
// printf("\n\n");
// matd_print(matd_op("M^-1",Model), matrix_format);
// printf("\n");
// extrapolate metrics from model view
double TX = MATD_EL(Model, 0, 3);
double TY = MATD_EL(Model, 1, 3);
double TZ = MATD_EL(Model, 2, 3);
// double rot_x = get_rotation("x", H);
// double rot_y = get_rotation("y", H);
// double rot_z = get_rotation("z", H);
double cosine = MATD_EL(Model, 0, 0);
double rot_z = acos(cosine) * 180/1.5 - 180;
cosine = MATD_EL(Model, 2, 2);
double rot_x = asin(cosine) * 90/1.3 + 90;
cosine = MATD_EL(Model, 1, 1);
double rot_y = asin(cosine);
char str[200];
sprintf(str, "<<#00ffff,serif-30>> DIST:%lf Offset:(%lf, %lf)\n rot: (%lf, %lf, %lf)\n",
TZ, TX, TY, rot_x, rot_y, rot_z);
vx_object_t *text = vxo_text_create(VXO_TEXT_ANCHOR_BOTTOM_LEFT, str);
vx_buffer_add_back(buf, vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT, text));
// printf("dist: %lf cos:%lf angle: %lf\n", TZ, cosine, theta);
}
loc_t* fit_lines(image_u32_t* im, node_t* n, vx_buffer_t* buf, metrics_t met, loc_t* out)
{
// usleep(2000000);
srand(time(NULL));
// isolate valid entries
zarray_t* loc_arr = zarray_create(sizeof(loc_t*));
for(int i = 0; i < im->height; i++) {
if(n->sides[i].leftmost.x == im->width) continue; // not apart of blob
loc_t* loc = malloc(sizeof(loc_t));
loc->x = n->sides[i].leftmost.x;
loc->y = n->sides[i].leftmost.y;
loc->valid = 0;
zarray_add(loc_arr, &loc);
}
for(int i = 0; i < im->height; i++) {
if(n->sides[i].rightmost.x == -1) continue;
loc_t* loc = malloc(sizeof(loc_t));
loc->x = n->sides[i].rightmost.x;
loc->y = n->sides[i].rightmost.y;
loc->valid = 0;
zarray_add(loc_arr, &loc);
}
// printf("\n\nall\n");
// for(int i = 0; i < zarray_size(loc_arr); i++)
// {
// loc_t* p1;
// zarray_get(loc_arr, i, &p1);
// printf("(%d, %d)\n", p1->x, p1->y);
// }
// printf("\n\n");
int iterations = 0;
int best_score = 0;
int lines_found = 0;
loc_t line_match[8];
int max_iterations = 500;
while(lines_found < 2 && zarray_size(loc_arr) > met.num_outliers) // still a lot of points left
{
if(iterations > max_iterations) break;
// reset image and array
// vx_object_t *vim = vxo_image_from_u32(im, 0, 0);
// vx_buffer_add_back(buf, vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT, vim));
add_arr_of_locs_to_buffer(loc_arr, buf, 1.0, vx_black, met);
int num_outliers = met.num_outliers/met.consensus_accuracy;
while(best_score < ((zarray_size(loc_arr) - num_outliers)/(4 - lines_found)))
{
reset_locs(loc_arr);
// pick random sample (2 points)
loc_t* p1;
loc_t* p2;
zarray_get(loc_arr, (rand()%zarray_size(loc_arr)), &p1);
p2 = p1;
while(p1 == p2)
zarray_get(loc_arr, (rand()%zarray_size(loc_arr)), &p2); // don't get same point
// find consensus score of line from other points
// if inside consensus range add 1
int tmp_score = 0;
for(int j = 0; j < zarray_size(loc_arr); j++) {
loc_t* tmp;
zarray_get(loc_arr, j, &tmp);
if(fabs(dist_from_point_to_line(p1, p2, tmp)) < (double)met.consensus_accuracy)
{
tmp->valid = 1;
// printf("dist: (%d, %d, %d) %lf\n", tmp->x, tmp->y, tmp->valid,
// fabs(dist_from_point_to_line(p1, p2, tmp)));
tmp_score++;
}
}
// keep best line so far
if(tmp_score > best_score) {
if(lines_found != 0) { // if 2nd line intersects, throw it out and find another
loc_t intersect = get_line_intersection(line_match[0], line_match[1], *p1, *p2);
if(in_range(im, intersect.x, intersect.y)) continue;
}
best_score = tmp_score;
// printf(" score:%d, %d, %d %lf\n", best_score, ((zarray_size(loc_arr)-1)/(4-lines_found)),
// zarray_size(loc_arr), 10/met.std_dev_from_square);
line_match[lines_found*2] = *p1;
line_match[lines_found*2+1] = *p2;
}
iterations++;
if(iterations > max_iterations) break;
}
// loc_t ext_lines[2];
// extend_lines_to_edge_of_image(im, line_match[lines_found*2], line_match[lines_found*2+1], ext_lines);
// add_line_to_buffer(im, buf, 2.0, ext_lines[0], ext_lines[1], vx_yellow);
// delete all points associated with the found line
zarray_t* endpoints_arr = zarray_create(sizeof(loc_t*));
for(int i = 0; i < zarray_size(loc_arr); i++) {
loc_t* tmp;
zarray_get(loc_arr, i, &tmp);
// printf("removed: (%d, %d, %d) \n", tmp->x, tmp->y, tmp->valid);
if(tmp->valid)
{
// add_circle_to_buffer(buf, 1.0, *tmp, vx_red);
zarray_add(endpoints_arr, &tmp);
zarray_remove_index(loc_arr, i, 0);
i--;
}
}
// find endpoints of line
loc_t ext_lines[2];
extend_lines_to_edge_of_image(im, line_match[lines_found*2], line_match[lines_found*2+1], ext_lines);
line_t endpoints = find_line_endpoints(endpoints_arr, &ext_lines[0], &ext_lines[1]);
add_circle_to_buffer(buf, 2.0, endpoints.start, vx_red);
add_circle_to_buffer(buf, 2.0, endpoints.end, vx_red);
line_match[lines_found*2] = endpoints.start;
line_match[lines_found*2+1] = endpoints.end;
lines_found++;
best_score = 0;
// vx_buffer_swap(buf);
// usleep(500000);
}
loc_t* ret = calloc(lines_found*2, sizeof(loc_t));
for(int i = 0; i < lines_found; i++) {
ret[i*2] = line_match[i*2];
ret[i*2+1] = line_match[i*2+1];
loc_t ext_lines[2];
extend_lines_to_edge_of_image(im, line_match[i*2], line_match[i*2+1], ext_lines);
add_line_to_buffer(im, buf, 2.0, ext_lines[0], ext_lines[1], vx_blue);
}
zarray_vmap(loc_arr, free);
zarray_destroy(loc_arr);
return(ret);
// int corners_found = 0;
// loc_t corners[4];
// find_corners_from_lines(im,line_match, 4, &corners_found, corners);
// for(int i = 0; i < corners_found; i++) {
// printf("(%d, %d)\n", corners[i].x, corners[i].y);
// add_circle_to_buffer(buf, 3.0, corners[i], vx_blue);
// }
// printf("(%d, %d) (%d, %d) %lf\n", line_match[0].x, line_match[0].y,
// line_match[1].x, line_match[1].y,
// (double)best_score/N);
}
