static void
fix_movedellipsepoint(int x, int y)
{
switch (cur_e->type) {
case T_ELLIPSE_BY_RAD:
elastic_ebr();
break;
case T_CIRCLE_BY_RAD:
elastic_cbr();
break;
case T_ELLIPSE_BY_DIA:
elastic_ebd();
break;
case T_CIRCLE_BY_DIA:
elastic_cbd();
break;
}
canvas_ref_proc = canvas_locmove_proc = null_proc;
adjust_box_pos(x, y, from_x, from_y, &cur_x, &cur_y);
new_e = copy_ellipse(cur_e);
relocate_ellipsepoint(new_e, cur_x, cur_y, movedpoint_num);
change_ellipse(cur_e, new_e);
/* redraw anything under the old ellipse */
redisplay_ellipse(cur_e);
/* and the new one */
redisplay_ellipse(new_e);
/* turn back on all relevant markers */
update_markers(new_objmask);
wrapup_movepoint();
}
static void
fix_movedcompoundpoint(int x, int y)
{
float scalex, scaley;
canvas_ref_proc = canvas_locmove_proc = null_proc;
elastic_box(fix_x, fix_y, cur_x, cur_y);
/* erase last lengths if appres.showlengths is true */
erase_lengths();
adjust_box_pos(x, y, from_x, from_y, &cur_x, &cur_y);
/* make a copy of the original and save as unchanged object */
old_c = copy_compound(cur_c);
clean_up();
old_c->next = cur_c;
set_latestcompound(old_c);
set_action_object(F_EDIT, O_COMPOUND);
scalex = ((float) (cur_x - fix_x)) / (from_x - fix_x);
scaley = ((float) (cur_y - fix_y)) / (from_y - fix_y);
/* scale the compound */
scale_compound(cur_c, scalex, scaley, fix_x, fix_y);
/* redraw anything under the old compound */
redisplay_compound(old_c);
/* and the new compound */
redisplay_compound(cur_c);
/* turn back on all relevant markers */
update_markers(new_objmask);
set_lastposition(from_x, from_y);
set_newposition(cur_x, cur_y);
set_modifiedflag();
wrapup_movepoint();
}
static void
place_spline_x(int x, int y)
{
canvas_leftbut_proc = null_proc;
canvas_middlebut_proc = null_proc;
canvas_rightbut_proc = null_proc;
canvas_locmove_proc = null_proc;
canvas_ref_proc = null_proc;
adjust_pos(x, y, fix_x, fix_y, &x, &y);
translate_spline(new_s, x - fix_x, y - fix_y);
if (return_proc == copy_selected) {
add_spline(new_s);
} else {
list_add_spline(&objects.splines, new_s);
clean_up();
set_lastposition(fix_x, fix_y);
set_newposition(x, y);
set_action_object(F_MOVE, O_SPLINE);
set_latestspline(new_s);
set_modifiedflag();
}
redisplay_spline(new_s);
/* turn back on all relevant markers */
update_markers(new_objmask);
(*return_proc) ();
draw_mousefun_canvas();
}
static void
place_line_x(int x, int y)
{
int dx, dy;
canvas_leftbut_proc = null_proc;
canvas_middlebut_proc = null_proc;
canvas_rightbut_proc = null_proc;
canvas_locmove_proc = null_proc;
canvas_ref_proc = null_proc;
adjust_pos(x, y, fix_x, fix_y, &x, &y);
dx = x - fix_x;
dy = y - fix_y;
translate_line(new_l, dx, dy);
clean_up();
set_latestline(new_l);
if (return_proc == copy_selected) {
add_line(new_l);
adjust_links(cur_linkmode, cur_links, dx, dy, 0, 0, 1.0, 1.0, True);
free_linkinfo(&cur_links);
} else {
list_add_line(&objects.lines, new_l);
adjust_links(cur_linkmode, cur_links, dx, dy, 0, 0, 1.0, 1.0, False);
set_lastposition(fix_x, fix_y);
set_newposition(x, y);
set_lastlinkinfo(cur_linkmode, cur_links);
cur_links = NULL;
set_action_object(F_MOVE, O_POLYLINE);
}
set_modifiedflag();
redisplay_line(new_l);
/* turn back on all relevant markers */
update_markers(new_objmask);
(*return_proc) ();
draw_mousefun_canvas();
}
static void
fix_box(int x, int y)
{
canvas_ref_proc = canvas_locmove_proc = null_proc;
elastic_box(fix_x, fix_y, cur_x, cur_y);
/* erase last lengths if appres.showlengths is true */
erase_box_lengths();
adjust_box_pos(x, y, from_x, from_y, &x, &y);
new_l = copy_line(cur_l);
if (new_l->type == T_PICTURE) {
if (signof(fix_x - from_x) != signof(fix_x - x))
new_l->pic->flipped = 1 - new_l->pic->flipped;
if (signof(fix_y - from_y) != signof(fix_y - y))
new_l->pic->flipped = 1 - new_l->pic->flipped;
}
assign_newboxpoint(new_l, fix_x, fix_y, x, y);
change_line(cur_l, new_l);
/* redraw anything under the old line */
redisplay_line(cur_l);
/* and the new line */
redisplay_line(new_l);
/* turn back on all relevant markers */
update_markers(new_objmask);
wrapup_movepoint();
}
static void
fix_movedlinepoint(int x, int y)
{
(*canvas_locmove_proc) (x, y);
canvas_ref_proc = canvas_locmove_proc = null_proc;
elastic_linelink();
/* erase last lengths if appres.showlengths is true */
erase_lengths();
if (cur_latexcursor != crosshair_cursor)
set_cursor(crosshair_cursor);
/* make a copy of the original and save as unchanged object */
old_l = copy_line(cur_l);
clean_up();
set_latestline(old_l);
set_action_object(F_EDIT, O_POLYLINE);
old_l->next = cur_l;
/* now change the original to become the new object */
relocate_linepoint(cur_l, cur_x, cur_y, moved_point, left_point);
/* redraw anything under the old line */
redisplay_line(old_l);
/* and the new line */
redisplay_line(cur_l);
/* turn back on all relevant markers */
update_markers(new_objmask);
wrapup_movepoint();
}
static void
cancel_movedellipsepoint(void)
{
canvas_ref_proc = canvas_locmove_proc = null_proc;
/* erase elastic version */
switch (cur_e->type) {
case T_ELLIPSE_BY_RAD:
elastic_ebr();
break;
case T_CIRCLE_BY_RAD:
elastic_cbr();
break;
case T_ELLIPSE_BY_DIA:
elastic_ebd();
break;
case T_CIRCLE_BY_DIA:
elastic_cbd();
break;
}
/* redraw original ellipse */
redisplay_ellipse(cur_e);
/* turn back on all relevant markers */
update_markers(new_objmask);
wrapup_movepoint();
}
static void
init_move(F_line *p, int type, int x, int y, int px, int py)
{
/* turn off all markers */
update_markers(0);
switch (type) {
case O_COMPOUND:
set_cursor(wait_cursor);
cur_c = (F_compound *) p;
list_delete_compound(&objects.compounds, cur_c);
redisplay_compound(cur_c);
set_cursor(null_cursor);
init_compounddragging(cur_c, px, py);
break;
case O_POLYLINE:
set_cursor(wait_cursor);
cur_l = (F_line *) p;
list_delete_line(&objects.lines, cur_l);
redisplay_line(cur_l);
set_cursor(null_cursor);
init_linedragging(cur_l, px, py);
break;
case O_TXT:
set_cursor(wait_cursor);
cur_t = (F_text *) p;
list_delete_text(&objects.texts, cur_t);
redisplay_text(cur_t);
set_cursor(null_cursor);
init_textdragging(cur_t, x, y);
break;
case O_ELLIPSE:
set_cursor(wait_cursor);
cur_e = (F_ellipse *) p;
list_delete_ellipse(&objects.ellipses, cur_e);
redisplay_ellipse(cur_e);
set_cursor(null_cursor);
init_ellipsedragging(cur_e, px, py);
break;
case O_ARC:
set_cursor(wait_cursor);
cur_a = (F_arc *) p;
list_delete_arc(&objects.arcs, cur_a);
redisplay_arc(cur_a);
set_cursor(null_cursor);
init_arcdragging(cur_a, px, py);
break;
case O_SPLINE:
set_cursor(wait_cursor);
cur_s = (F_spline *) p;
list_delete_spline(&objects.splines, cur_s);
redisplay_spline(cur_s);
set_cursor(null_cursor);
init_splinedragging(cur_s, px, py);
break;
default:
return;
}
}
static void
init_splinepointmoving(void)
{
set_action_on();
/* turn off all markers */
update_markers(0);
from_x = cur_x = moved_point->x;
from_y = cur_y = moved_point->y;
set_cursor(crosshair_cursor);
if (open_spline(cur_s)) {
if (left_point == NULL || right_point == NULL) {
if (left_point != NULL) {
fix_x = left_point->x;
fix_y = left_point->y;
} else if (right_point != NULL) {
fix_x = right_point->x;
fix_y = right_point->y;
}
if (latexline_mode || latexarrow_mode) {
canvas_locmove_proc = latex_line;
canvas_ref_proc = elastic_line;
cur_latexcursor = crosshair_cursor;
} else if (mountain_mode || manhattan_mode) {
canvas_locmove_proc = constrainedangle_line;
canvas_ref_proc = elastic_line;
} else {
/* freehand line */
canvas_locmove_proc = reshaping_line;
canvas_ref_proc = elastic_linelink;
}
} else {
/* linelink, always freehand */
force_noanglegeom();
canvas_locmove_proc = reshaping_line;
canvas_ref_proc = elastic_linelink;
}
} else {
/* must be closed spline */
force_noanglegeom();
canvas_locmove_proc = reshaping_line;
canvas_ref_proc = elastic_linelink;
if (left_point == NULL) {
for (left_point = right_point;
left_point->next != NULL;
left_point = left_point->next);
}
if (right_point == NULL) {
right_point = cur_s->points; /* take the first */
}
}
/* show current length(s) */
(canvas_locmove_proc)(cur_x, cur_y);
elastic_linelink();
canvas_leftbut_proc = fix_movedsplinepoint;
canvas_rightbut_proc = cancel_movedsplinepoint;
}
static void
cancel_compound(void)
{
canvas_ref_proc = canvas_locmove_proc = null_proc;
elastic_box(fix_x, fix_y, cur_x, cur_y);
/* erase last lengths if appres.showlengths is true */
erase_lengths();
/* turn back on all relevant markers */
update_markers(new_objmask);
wrapup_movepoint();
}
static void
cancel_movedsplinepoint(void)
{
canvas_ref_proc = canvas_locmove_proc = null_proc;
elastic_linelink();
/* erase last lengths if appres.showlengths is true */
erase_lengths();
/* turn back on all relevant markers */
update_markers(new_objmask);
wrapup_movepoint();
}
static void
array_place_ellipse(int x, int y)
{
int i, j, delta_x, delta_y, start_x, start_y;
int nx, ny;
F_ellipse *save_ellipse;
elastic_moveellipse();
/* erase last lengths if appres.showlengths is true */
erase_lengths();
tail(&objects, &object_tails);
save_ellipse = new_e;
if ((!cur_numxcopies) && (!cur_numycopies)) {
place_ellipse(x, y);
}
else {
delta_x = cur_x - fix_x;
delta_y = cur_y - fix_y;
start_x = cur_x - delta_x;
start_y = cur_y - delta_y;
if ((cur_numxcopies < 2) && (cur_numycopies < 2)) { /* special cases */
if (cur_numxcopies > 0) {
place_ellipse_x(start_x+delta_x, start_y);
new_e = copy_ellipse(cur_e);
}
if (cur_numycopies > 0) {
place_ellipse_x(start_x, start_y+delta_y);
new_e = copy_ellipse(cur_e);
}
} else {
nx = cur_numxcopies;
if (nx == 0)
nx++;
ny = cur_numycopies;
if (ny == 0)
ny++;
for (i = 0, x = start_x; i < nx; i++, x+=delta_x) {
for (j = 0, y = start_y; j < ny; j++, y+=delta_y) {
if (i || j ) {
place_ellipse_x(x, y);
new_e = copy_ellipse(cur_e);
}
}
}
}
}
/* put all new ellipses in the saved objects structure for undo */
saved_objects.ellipses = save_ellipse;
set_action_object(F_ADD, O_ALL_OBJECT);
/* turn back on all relevant markers */
update_markers(new_objmask);
}
static void
cancel_movedlinepoint(void)
{
canvas_ref_proc = canvas_locmove_proc = null_proc;
/* erase the elastic line */
elastic_linelink();
/* erase last lengths if appres.showlengths is true */
erase_lengths();
/* redraw original line */
redisplay_line(cur_l);
/* turn back on all relevant markers */
update_markers(new_objmask);
wrapup_movepoint();
}
static void
init_arcpointmoving(void)
{
set_action_on();
/* turn off all markers */
update_markers(0);
cur_x = cur_a->point[movedpoint_num].x;
cur_y = cur_a->point[movedpoint_num].y;
set_cursor(crosshair_cursor);
canvas_locmove_proc = reshaping_arc;
canvas_ref_proc = elastic_arclink;
canvas_leftbut_proc = fix_movedarcpoint;
canvas_rightbut_proc = cancel_movedarcpoint;
elastic_arclink();
/* show current length(s) */
(canvas_locmove_proc)(cur_x, cur_y);
}
static void
init_compoundpointmoving(void)
{
double dx, dy, l;
F_line *line, *dum;
set_action_on();
if (cur_x == cur_c->nwcorner.x)
fix_x = cur_c->secorner.x;
else
fix_x = cur_c->nwcorner.x;
if (cur_y == cur_c->nwcorner.y)
fix_y = cur_c->secorner.y;
else
fix_y = cur_c->nwcorner.y;
from_x = cur_x;
from_y = cur_y;
/* turn off all markers */
update_markers(0);
set_cursor(crosshair_cursor);
elastic_box(fix_x, fix_y, cur_x, cur_y);
/* is this a dimension line? */
if (dimline_components(cur_c, &line, &dum, &dum, &dum) && line) {
/* yes, constrain the resizing in the direction of the line */
if (line->points->x == line->points->next->x)
/* vertical line */
constrained = BOX_VSTRETCH;
else
/* horizontal line */
constrained = BOX_HSTRETCH;
}
if (constrained) {
dx = cur_x - fix_x;
dy = cur_y - fix_y;
l = sqrt(dx * dx + dy * dy);
cosa = fabs(dx / l);
sina = fabs(dy / l);
}
canvas_locmove_proc = constrained_resizing_scale_box;
canvas_ref_proc = elastic_fixedbox;
canvas_leftbut_proc = fix_movedcompoundpoint;
canvas_rightbut_proc = cancel_compound;
/* show current length(s) */
(canvas_locmove_proc)(cur_x, cur_y);
}
void undo_deletepoint(void)
{
last_action = F_NULL; /* to avoid doing a clean-up during adding */
if (last_object == O_POLYLINE) {
linepoint_adding(saved_objects.lines, last_prev_point,
last_selected_point);
/* turn back on all relevant markers */
update_markers(new_objmask);
} else { /* last_object is a spline */
splinepoint_adding(saved_objects.splines, last_prev_point,
last_selected_point, last_next_point,
last_selected_sfactor->s);
}
last_next_point = NULL;
}
static void
cancel_ellipse(void)
{
canvas_ref_proc = canvas_locmove_proc = null_proc;
elastic_moveellipse();
/* erase last lengths if appres.showlengths is true */
erase_lengths();
if (return_proc == copy_selected) {
free_ellipse(&new_e);
} else {
list_add_ellipse(&objects.ellipses, new_e);
redisplay_ellipse(new_e);
}
/* turn back on all relevant markers */
update_markers(new_objmask);
(*return_proc) ();
draw_mousefun_canvas();
}
static void
cancel_drag_compound(void)
{
canvas_ref_proc = canvas_locmove_proc = null_proc;
elastic_movebox();
/* erase last lengths if appres.showlengths is true */
erase_lengths();
free_linkinfo(&cur_links);
if (return_proc == copy_selected) {
free_compound(&new_c);
} else {
list_add_compound(&objects.compounds, new_c);
redisplay_compound(new_c);
}
/* turn back on all relevant markers */
update_markers(new_objmask);
(*return_proc) ();
draw_mousefun_canvas();
}
static void
fix_movedarcpoint(int x, int y)
{
canvas_ref_proc = canvas_locmove_proc = null_proc;
elastic_arclink();
/* erase last lengths if appres.showlengths is true */
erase_lengths();
adjust_pos(x, y, cur_a->point[movedpoint_num].x,
cur_a->point[movedpoint_num].y, &x, &y);
new_a = copy_arc(cur_a);
relocate_arcpoint(new_a, x, y, movedpoint_num);
change_arc(cur_a, new_a);
/* redraw anything under the old arc */
redisplay_arc(cur_a);
/* and the new one */
redisplay_arc(new_a);
/* turn back on all relevant markers */
update_markers(new_objmask);
wrapup_movepoint();
}
void
Tracker::spinOnce()
{
if (started){
auto begin_time=std::chrono::high_resolution_clock::now();
track();
update_markers();
publish_markers();
broadcast_tf();
auto end_time=std::chrono::high_resolution_clock::now();
auto elapsed_time=std::chrono::duration_cast<std::chrono::milliseconds>(end_time - begin_time).count();
ROS_INFO_THROTTLE(10,"[Tracker::%s]\tStep time: %ld ms.",__func__,elapsed_time);
}
else if (lost_it){
find_object_in_scene();
}
else{
if(!storage->readObjNames(est_names))
ROS_WARN_THROTTLE(30,"[Tracker::%s]\tLooks like no Pose Estimation has been performed, perform one in order to start using the object tracker.",__func__);
}
}
static void
fix_movedsplinepoint(int x, int y)
{
(*canvas_locmove_proc) (x, y);
canvas_ref_proc = canvas_locmove_proc = null_proc;
elastic_linelink();
/* erase last lengths if appres.showlengths is true */
erase_lengths();
old_s = copy_spline(cur_s);
clean_up();
set_latestspline(old_s);
set_action_object(F_EDIT, O_SPLINE);
old_s->next = cur_s;
relocate_splinepoint(cur_s, cur_x, cur_y, moved_point);
/* redraw anything under the old spline */
redisplay_spline(old_s);
/* and the new one */
redisplay_spline(cur_s);
/* turn back on all relevant markers */
update_markers(new_objmask);
wrapup_movepoint();
}
void
chop_selected(void)
{
set_mousefun("Select axe object", "Select log object", "Clear axe list",
LOC_OBJ, LOC_OBJ, LOC_OBJ);
draw_mousefun_canvas();
canvas_kbd_proc = null_proc;
canvas_locmove_proc = null_proc;
canvas_ref_proc = null_proc;
init_searchproc_left(select_axe_object);
init_searchproc_middle(select_log_object);
/* init_searchproc_right(init_chop_right); */ /* fixme don't need this now */
canvas_leftbut_proc = object_search_left; /* point search for axe */
canvas_middlebut_proc = object_search_middle; /* object search for log */
canvas_rightbut_proc = clear_axe_objects;
set_cursor(pick9_cursor);
/* set the markers to show we only allow POLYLINES, ARCS and ELLIPSES */
/* (the markers are originally set this way from the mode panel, but
we need to set them again after the previous chop */ /* fixme -- neede for chop ? */
update_markers(M_POLYLINE | M_ARC | M_ELLIPSE);
reset_action_on();
axe_objects_next = 0;
}
static Boolean
init_ellipsepointmoving(void)
{
double dx, dy, l;
if (constrained &&
(cur_e->type == T_CIRCLE_BY_DIA || cur_e->type == T_CIRCLE_BY_RAD)) {
put_msg("Constrained move not supported for CIRCLES");
return False; /* abort - constrained move for circle not allowed */
}
if (movedpoint_num == 0) {
if (cur_e->type == T_ELLIPSE_BY_RAD ||
cur_e->type == T_CIRCLE_BY_RAD) {
put_msg("Cannot move CENTER point");
return False; /* abort - center point is selected */
}
cur_x = cur_e->start.x;
cur_y = cur_e->start.y;
fix_x = cur_e->end.x;
fix_y = cur_e->end.y;
} else {
cur_x = cur_e->end.x;
cur_y = cur_e->end.y;
fix_x = cur_e->start.x;
fix_y = cur_e->start.y;
}
if (constrained) {
dx = cur_x - fix_x;
dy = cur_y - fix_y;
l = sqrt(dx * dx + dy * dy);
cosa = fabs(dx / l);
sina = fabs(dy / l);
}
cur_angle = cur_e->angle;
set_action_on();
/* turn off all markers */
update_markers(0);
switch (cur_e->type) {
case T_ELLIPSE_BY_RAD:
canvas_locmove_proc = constrained_resizing_ebr;
canvas_ref_proc = elastic_ebr;
break;
case T_CIRCLE_BY_RAD:
canvas_locmove_proc = resizing_cbr;
canvas_ref_proc = elastic_cbr;
break;
case T_ELLIPSE_BY_DIA:
canvas_locmove_proc = constrained_resizing_ebd;
canvas_ref_proc = elastic_ebd;
break;
case T_CIRCLE_BY_DIA:
canvas_locmove_proc = resizing_cbd;
canvas_ref_proc = elastic_cbd;
break;
}
/* show current radius(ii) */
(canvas_locmove_proc)(cur_x, cur_y);
from_x = cur_x;
from_y = cur_y;
set_cursor(crosshair_cursor);
canvas_leftbut_proc = fix_movedellipsepoint;
canvas_rightbut_proc = cancel_movedellipsepoint;
return True;
}
static void
init_linepointmoving(void)
{
F_point *p;
set_action_on();
/* turn off all markers */
update_markers(0);
from_x = cur_x = moved_point->x;
from_y = cur_y = moved_point->y;
set_cursor(crosshair_cursor);
switch (cur_l->type) {
case T_POLYGON:
if (left_point == NULL)
for (left_point = right_point, p = left_point->next;
p->next != NULL;
left_point = p, p = p->next);
force_noanglegeom();
canvas_locmove_proc = reshaping_line;
canvas_ref_proc = elastic_linelink;
break;
case T_BOX:
case T_ARCBOX:
case T_PICTURE:
if (right_point->next == NULL) { /* point 4 */
fix_x = cur_l->points->next->x;
fix_y = cur_l->points->next->y;
} else {
fix_x = right_point->next->x;
fix_y = right_point->next->y;
}
if (constrained) {
double dx, dy, l;
dx = cur_x - fix_x;
dy = cur_y - fix_y;
l = sqrt(dx * dx + dy * dy);
cosa = fabs(dx / l);
sina = fabs(dy / l);
}
force_noanglegeom();
if (cur_l->thickness != 1)
elastic_box(fix_x, fix_y, cur_x, cur_y);
canvas_locmove_proc = constrained_resizing_box;
canvas_ref_proc = elastic_fixedbox;
canvas_leftbut_proc = fix_box;
canvas_rightbut_proc = cancel_movept_box;
/* show current length(s) */
(canvas_locmove_proc)(cur_x, cur_y);
return;
case T_POLYLINE:
if (left_point == NULL || right_point == NULL) {
if (left_point != NULL) {
fix_x = left_point->x;
fix_y = left_point->y;
} else if (right_point != NULL) {
fix_x = right_point->x;
fix_y = right_point->y;
}
if (latexline_mode || latexarrow_mode) {
canvas_locmove_proc = latex_line;
canvas_ref_proc = elastic_line;
cur_latexcursor = crosshair_cursor;
} else if (mountain_mode || manhattan_mode) {
canvas_locmove_proc = constrainedangle_line;
canvas_ref_proc = elastic_line;
} else {
/* unconstrained or freehand line */
canvas_locmove_proc = reshaping_line;
canvas_ref_proc = elastic_linelink;
}
} else {
/* linelink, always freehand */
force_noanglegeom();
canvas_locmove_proc = reshaping_line;
canvas_ref_proc = elastic_linelink;
}
break;
}
canvas_leftbut_proc = fix_movedlinepoint;
canvas_rightbut_proc = cancel_movedlinepoint;
/* show current length(s) */
(canvas_locmove_proc)(cur_x, cur_y);
}