int main() {
initscr();
cbreak();
noecho();
keypad(stdscr, TRUE); // make keys work
curs_set(0); // hide cursor
timeout(100);
int xmax;
int ymax;
getmaxyx(stdscr, ymax, xmax);
enum Direction dir = RIGHT;
Board* board = create_board(create_snake(), NULL, xmax, ymax);
int i;
for (i = 0; i < 6; i++) {
add_new_food(board);
}
while(true) {
erase();
display_points(board->snake, ACS_BLOCK);
display_points(board->foods, ACS_DIAMOND);
dir = get_next_move(dir);
enum Status status = move_snake(board, dir);
if (status == FAILURE) break;
}
endwin();
return 0;
}
static int rast_redraw(void)
{
Erase_view(VIEW_MAP1);
drawcell(VIEW_MAP1, 0); /* 0 means don't initialize VIEW_MAP2 */
display_points(1);
return 0;
}
static int zoom1(int x, int y)
{ /* called by Input_pointer */
int top, bottom, left, right;
int n, row, col;
int nrows, ncols;
struct Cell_head cellhd;
int mag;
double magnification();
double north, south, east, west;
if (In_view(pick_view = VIEW_MAP1, x, y)) {
main_view = VIEW_MAP1;
zoom_view = VIEW_MAP1_ZOOM;
target_flag = 0;
}
else if (In_view(pick_view = VIEW_MAP2, x, y)) {
if (!pick_view->cell.configured)
return 0; /* ignore the mouse event */
main_view = VIEW_MAP2;
zoom_view = VIEW_MAP2_ZOOM;
target_flag = 1;
}
else if (In_view(pick_view = VIEW_MAP1_ZOOM, x, y)) {
if (!pick_view->cell.configured)
return 0; /* ignore the mouse event */
main_view = VIEW_MAP1;
zoom_view = VIEW_MAP1_ZOOM;
target_flag = 0;
}
else if (In_view(pick_view = VIEW_MAP2_ZOOM, x, y)) {
if (!pick_view->cell.configured)
return 0; /* ignore the mouse event */
main_view = VIEW_MAP2;
zoom_view = VIEW_MAP2_ZOOM;
target_flag = 1;
}
else
return 0; /* ignore the mouse event */
if (!pick_view->cell.configured)
return 0; /* just to be sure */
/*
* make sure point is within edges of image as well
*/
if (x <= pick_view->cell.left)
return 0;
if (x >= pick_view->cell.right)
return 0;
if (y <= pick_view->cell.top)
return 0;
if (y >= pick_view->cell.bottom)
return 0;
/*
* ok, erase menu messages
*/
Menu_msg("");
/* determine magnification of zoom */
if (zoom_view->cell.configured) {
if (zoom_view == pick_view)
mag = floor(magnification(zoom_view) + 1.0) + .1;
else
mag = ceil(magnification(zoom_view)) + .1;
}
else {
mag = floor(magnification(main_view) + 1.0) + .1;
}
if (!ask_magnification(&mag))
return 1;
/*
* Determine the the zoom window (ie, cellhd)
*/
G_copy(&cellhd, &main_view->cell.head, sizeof(cellhd));
cellhd.ns_res = main_view->cell.ns_res / mag;
cellhd.ew_res = main_view->cell.ew_res / mag;
cellhd.cols = (cellhd.east - cellhd.west) / cellhd.ew_res;
cellhd.rows = (cellhd.north - cellhd.south) / cellhd.ns_res;
/* convert x,y to col,row */
col = view_to_col(pick_view, x);
east = col_to_easting(&pick_view->cell.head, col, 0.5);
col = easting_to_col(&cellhd, east);
row = view_to_row(pick_view, y);
north = row_to_northing(&pick_view->cell.head, row, 0.5);
row = northing_to_row(&cellhd, north);
ncols = zoom_view->ncols;
nrows = zoom_view->nrows;
n = cellhd.cols - col;
if (n > col)
n = col;
if (n + n + 1 >= ncols) {
n = ncols / 2;
if (n + n + 1 >= ncols)
n--;
}
left = col - n;
right = col + n;
n = cellhd.rows - row;
if (n > row)
n = row;
if (n + n + 1 >= nrows) {
n = nrows / 2;
if (n + n + 1 >= nrows)
n--;
}
top = row - n;
bottom = row + n;
north = row_to_northing(&cellhd, top, 0.0);
west = col_to_easting(&cellhd, left, 0.0);
south = row_to_northing(&cellhd, bottom, 1.0);
east = col_to_easting(&cellhd, right, 1.0);
cellhd.north = north;
cellhd.south = south;
cellhd.east = east;
cellhd.west = west;
cellhd.rows = (cellhd.north - cellhd.south) / cellhd.ns_res;
cellhd.cols = (cellhd.east - cellhd.west) / cellhd.ew_res;
/*
* Outline the zoom window on the main map
* Turn previous one to grey.
*/
if (zoom_view->cell.configured) {
R_standard_color(GREY);
Outline_cellhd(main_view, &zoom_view->cell.head);
}
R_standard_color(RED);
Outline_cellhd(main_view, &cellhd);
/*
* zoom
*/
if (target_flag)
select_target_env();
G_copy(&zoom_view->cell.head, &cellhd, sizeof(cellhd));
Configure_view(zoom_view, pick_view->cell.name, pick_view->cell.mapset,
pick_view->cell.ns_res, pick_view->cell.ew_res);
drawcell(zoom_view);
select_current_env();
display_points(1);
return 1; /* pop back */
}
static int drawvect(int zoomit, /* -1 = refresh, 0 = new image, 1 = zoom, 2 = warp */
View * zoom_view, double E[], double N[], int trans_order)
{ /* order of transformation if warping vectors */
int stat = 0;
int i;
char name[GNAME_MAX], mapset[GMAPSET_MAX];
struct Cell_head cellhd;
struct line_pnts *Points;
char msg[100], win_name[100];
int t, b, l, r;
int blank = 0;
View *active_view;
int left, top, nrows, ncols;
static int vectclr[VFILES];
/* if refresh screen or overlay & no displayed vector maps return */
if ((zoomit == DO_REFRESH || zoomit == DO_WARP) && !numfiles) {
if (zoomit == DO_REFRESH)
display_points(1);
return 0;
}
/* numfiles stays at 0 until the end of the first vector map init */
if (numfiles >= VFILES) {
G_warning
("Can't display another map; reached maximum number of files");
return 0;
}
select_target_env();
if (zoomit == DO_REFRESH || zoomit == DO_NEW) { /* New Map File or Refresh Screen */
if (zoomit == DO_NEW) { /* zoomit==0, Draw New Map File */
if (!choose_vectfile(name, mapset))
return 0;
strcpy(vect_file[numfiles], name);
strcpy(vect_mapset[numfiles], mapset);
get_vector_color(); /* ask line_color to draw map */
if (!numfiles) { /* first map: SET VECTOR WINDOW BY WIND */
G_get_window(&cellhd);
G_copy(&VIEW_MAP2->cell.head, &cellhd, sizeof(cellhd));
}
else /* not the first map */
G_copy(&cellhd, &VIEW_MAP2->cell.head,
sizeof(VIEW_MAP2->cell.head));
numfiles++;
}
else { /* zoomit=-1 Refresh Screen */
G_copy(&cellhd, &VIEW_MAP2->cell.head,
sizeof(VIEW_MAP2->cell.head));
if (!cellmap_present)
Erase_view(VIEW_MAP2_ZOOM);
VIEW_MAP2_ZOOM->cell.configured = 0;
blank = BLACK;
}
strcpy(win_name, "vect_map");
if (!view2on) {
t = VIEW_MAP2->top;
b = VIEW_MAP2->bottom;
l = VIEW_MAP2->left;
r = VIEW_MAP2->right;
D_new_window(win_name, t, b, l, r);
if (!cellmap_present)
blank = BLACK;
else
blank = 0; /* don't erase viewport */
view2on = 1;
}
active_view = VIEW_MAP2;
}
else { /* zoomit>0 Zoom or Warp */
G_copy(&cellhd, &zoom_view->cell.head, sizeof(zoom_view->cell.head));
if (!(zoom_view == VIEW_MAP1)) { /* target side */
VIEW_MAP2_ZOOM->cell.configured = 0;
strcpy(win_name, "zoom_map");
if (!view2zoomon) {
t = VIEW_MAP2_ZOOM->top;
b = VIEW_MAP2_ZOOM->bottom;
l = VIEW_MAP2_ZOOM->left;
r = VIEW_MAP2_ZOOM->right;
D_new_window(win_name, t, b, l, r);
view2zoomon = 1;
}
active_view = VIEW_MAP2_ZOOM;
blank = BLACK;
}
else {
strcpy(win_name, "warp_map"); /* defined in drawcell routine */
active_view = VIEW_MAP1;
blank = 0; /* don't erase viewport */
}
}
nrows = active_view->nrows;
ncols = active_view->ncols;
left = active_view->left;
top = active_view->top;
D_set_cur_wind(win_name);
R_standard_color(YELLOW);
Outline_box(top, top + nrows - 1, left, left + ncols - 1);
Points = Vect_new_line_struct();
if (zoomit != DO_WARP) {
Curses_clear_window(INFO_WINDOW);
Curses_write_window(INFO_WINDOW, 1, 13, "COORDINATES");
Curses_write_window(INFO_WINDOW, 3, 2, "MAIN WINDOW");
sprintf(msg, "N = %10.2f E = %10.2f", VIEW_MAP2->cell.head.north,
VIEW_MAP2->cell.head.east);
Curses_write_window(INFO_WINDOW, 5, 4, msg);
sprintf(msg, "S = %10.2f W = %10.2f", VIEW_MAP2->cell.head.south,
VIEW_MAP2->cell.head.west);
Curses_write_window(INFO_WINDOW, 6, 4, msg);
Curses_write_window(INFO_WINDOW, 9, 2, "ZOOM WINDOW");
sprintf(msg, "N = %10.2f E = %10.2f",
VIEW_MAP2_ZOOM->cell.head.north,
VIEW_MAP2_ZOOM->cell.head.east);
Curses_write_window(INFO_WINDOW, 11, 4, msg);
sprintf(msg, "S = %10.2f W = %10.2f",
VIEW_MAP2_ZOOM->cell.head.south,
VIEW_MAP2_ZOOM->cell.head.west);
Curses_write_window(INFO_WINDOW, 12, 4, msg);
}
if (zoomit) { /* ie ! DO_NEW */
dsp_setup(blank, &cellhd);
for (i = 0; i < numfiles; i++) {
sprintf(msg, "Displaying %s", vect_file[i]);
Menu_msg(msg);
R_standard_color(vectclr[i]);
if (zoomit != DO_WARP)
stat = plot(vect_file[i], vect_mapset[i], Points);
else
stat = plot_warp(vect_file[i], vect_mapset[i],
Points, E, N, trans_order);
}
}
else { /* ie DO_NEW */
if (numfiles == 1) { /* let first file set window */
G_copy(&VIEW_MAP2->cell.head, &cellhd, sizeof(cellhd));
cellhd.rows = VIEW_MAP2->nrows;
cellhd.cols = VIEW_MAP2->ncols;
cellhd.ns_res = (cellhd.north - cellhd.south) / cellhd.rows;
cellhd.ew_res = (cellhd.east - cellhd.west) / cellhd.cols;
if (cellhd.ns_res > cellhd.ew_res)
cellhd.ew_res = cellhd.ns_res;
else
cellhd.ns_res = cellhd.ew_res;
VIEW_MAP2->cell.ns_res = cellhd.ns_res;
VIEW_MAP2->cell.ew_res = cellhd.ew_res;
G_copy(&VIEW_MAP2->cell.head, &cellhd, sizeof(cellhd));
G_adjust_window_to_box(&cellhd, &VIEW_MAP2->cell.head,
VIEW_MAP2->nrows, VIEW_MAP2->ncols);
if (!cellmap_present) {
Configure_view(VIEW_MAP2, vect_file[numfiles - 1],
vect_mapset[numfiles - 1], cellhd.ns_res,
cellhd.ew_res);
}
Curses_write_window(INFO_WINDOW, 15, 2,
"WHERE CURSOR-> Mid Button");
}
dsp_setup(blank, &cellhd);
R_standard_color(YELLOW);
Outline_box(top, top + nrows - 1, left, left + ncols - 1);
sprintf(msg, "Displaying %s", vect_file[numfiles - 1]);
Menu_msg(msg);
R_standard_color(line_color);
vectclr[numfiles - 1] = line_color;
get_clr_name(vect_color[numfiles - 1], line_color);
stat =
plot(vect_file[numfiles - 1], vect_mapset[numfiles - 1], Points);
}
display_points(1);
R_standard_color(WHITE);
Outline_box(top, top + nrows - 1, left, left + ncols - 1);
Menu_msg("");
Vect_destroy_line_struct(Points);
/* VIEW_MAP2->cell.configured = 1; XXX */
select_current_env();
if (from_screen < 0) {
from_flag = 1;
from_screen = 0;
if (from_keyboard < 0) {
from_keyboard = 0;
from_screen = 1;
}
}
if (numfiles) {
Curses_clear_window(MENU_WINDOW);
Curses_write_window(MENU_WINDOW, 1, 5, "COLOR MAP FILE");
for (i = 0; i < numfiles; i++) {
sprintf(msg, "%7s %s", vect_color[i], vect_file[i]);
Curses_write_window(MENU_WINDOW, i + 3, 3, msg);
}
}
return 0;
}
int analyze(void)
{
static int use = 1;
static Objects objects[] = {
MENU("DONE", done, &use),
MENU("PRINT", to_printer, &use),
MENU("FILE", to_file, &use),
MENU("OVERLAY", do_warp, &use),
MENU(delete_msg, delete_mark, &use),
INFO("Transform->", &use),
MENU(order_msg, get_order, &use),
INFO(pick_msg, &use),
OTHER(pick, &use),
{0}
};
int color;
int tsize;
int cury;
int len;
int line;
int top, bottom, left, right, width, middle, nums;
/* to give user a response of some sort */
Menu_msg("Preparing analysis ...");
/*
* build a popup window at center of the screen.
* 35% the height and wide enough to hold the report
*
*/
/* height of 1 line, based on NLINES taking up 35% vertical space */
height = (.35 * (SCREEN_BOTTOM - SCREEN_TOP)) / NLINES + 1;
/* size of text, 80% of line height */
tsize = .8 * height;
size = tsize - 2; /* fudge for computing pixels width of text */
/* indent for the text */
edge = .1 * height + 1;
/* determine the length, in chars, of printed line */
FMT0(buf, 0);
nums = strlen(buf) * size;
FMT1(buf, 0.0, 0.0, 0.0);
len = strlen(buf);
middle = len * size;
FMT2(buf, 0.0, 0.0, 0.0, 0.0);
len += strlen(buf);
/* width is for max chars plus sidecar for more/less */
width = len * size + nums + 2 * height;
if ((SCREEN_RIGHT - SCREEN_LEFT) < width)
width = SCREEN_RIGHT - SCREEN_LEFT;
/* define the window */
bottom = VIEW_MENU->top - 1;
top = bottom - height * NLINES;
left = SCREEN_LEFT;
right = left + width;
middle += left + nums;
nums += left;
/* save what is under this area, so it can be restored */
R_panel_save(tempfile1, top, bottom + 1, left, right + 1);
/* fill it with white */
R_standard_color(BACKGROUND);
R_box_abs(left, top, right, bottom);
right -= 2 * height; /* reduce it to exclude sidecar */
/* print messages in message area */
R_text_size(tsize, tsize);
/* setup the more/less boxes in the sidecar */
R_standard_color(BLACK);
less.top = top;
less.bottom = top + 2 * height;
less.left = right;
less.right = right + 2 * height;
Outline_box(less.top, less.bottom, less.left, less.right);
more.top = bottom - 2 * height;
more.bottom = bottom;
more.left = right;
more.right = right + 2 * height;
Outline_box(more.top, more.bottom, more.left, more.right);
/*
* top two lines are for column labels
* last two line is for overall rms error.
*/
nlines = NLINES - 3;
first_point = 0;
/* allocate predicted values */
xres = (double *)G_calloc(group.points.count, sizeof(double));
yres = (double *)G_calloc(group.points.count, sizeof(double));
gnd = (double *)G_calloc(group.points.count, sizeof(double));
/* compute transformation for the first time */
compute_transformation();
/* put head on the report */
cury = top;
dotext(LHEAD1, cury, cury + height, left, middle, 0, BLACK);
dotext(RHEAD1, cury, cury + height, middle, right - 1, 0, BLACK);
cury += height;
dotext(LHEAD2, cury, cury + height, left, middle, 0, BLACK);
dotext(RHEAD2, cury, cury + height, middle, right - 1, 0, BLACK);
cury += height;
R_move_abs(left, cury - 1);
R_cont_abs(right, cury - 1);
/* isolate the sidecar */
R_move_abs(right, top);
R_cont_abs(right, bottom);
/* define report box */
report.top = cury;
report.left = left;
report.right = right;
/* lets do it */
pager = 1;
while (1) {
R_text_size(tsize, tsize);
line = 0;
curp = first_point;
cury = top + 2 * height;
while (1) {
if (line >= nlines || curp >= group.points.count)
break;
line++;
if (!delete_mode)
color = BLACK;
else
color = BLUE;
if (group.equation_stat > 0 && group.points.status[curp] > 0) {
/* color = BLACK; */
FMT1(buf, xres[curp], yres[curp], gnd[curp]);
if (curp == xmax || curp == ymax || curp == gmax)
color = RED;
dotext(buf, cury, cury + height, nums, middle, 0, color);
}
else if (group.points.status[curp] > 0)
dotext("?", cury, cury + height, nums, middle, 1, color);
else
dotext("not used", cury, cury + height, nums, middle, 1,
color);
if (pager) {
FMT0(buf, curp + 1);
dotext(buf, cury, cury + height, left, nums, 0, color);
FMT2(buf,
group.points.e1[curp],
group.points.n1[curp],
group.points.e2[curp], group.points.n2[curp]);
dotext(buf, cury, cury + height, middle, right - 1, 0, color);
}
cury += height;
curp++;
}
report.bottom = cury;
downarrow(&more, curp < group.points.count ? color : BACKGROUND);
uparrow(&less, first_point > 0 ? color : BACKGROUND);
R_standard_color(BACKGROUND);
R_box_abs(left, cury, right - 1, bottom);
if (group.equation_stat < 0) {
if (group.equation_stat == -1) {
color = RED;
strcpy(buf, "Poorly placed control points");
}
else {
if (group.equation_stat == -2)
G_fatal_error("NOT ENOUGH MEMORY");
else
G_fatal_error("PARAMETER ERROR");
}
}
else if (group.equation_stat == 0) {
color = RED;
strcpy(buf, "No active control points");
}
else {
color = BLACK;
sprintf(buf, "Overall rms error: %.2f", rms);
}
dotext(buf, bottom - height, bottom, left, right - 1, 0, color);
R_standard_color(BLACK);
R_move_abs(left, bottom - height);
R_cont_abs(right - 1, bottom - height);
pager = 0;
which = -1;
if (Input_pointer(objects) < 0)
break;
display_points(1);
}
/* all done. restore what was under the window */
right += 2 * height; /* move it back over the sidecar */
R_standard_color(BACKGROUND);
R_box_abs(left, top, right, bottom);
R_panel_restore(tempfile1);
R_panel_delete(tempfile1);
R_flush();
G_free(xres);
G_free(yres);
G_free(gnd);
I_put_control_points(group.name, &group.points);
display_points(1);
return 0; /* return but don't QUIT */
}
bool Pcl_grabing::isBlock()
{
update_kinect_points();
int npts = transformed_pclKinect_clr_ptr_->points.size();
Eigen::Vector3f pt;
Eigen::Vector3f dist;
vector<int> index;
index.clear();
double distance = 1;
BlockColor<<0,0,0;
for (int i = 0; i < npts; i++)
{
pt = transformed_pclKinect_clr_ptr_->points[i].getVector3fMap();
dist = pt - TableCentroid;
dist[2]=0;
distance = dist.norm();
if(distance < TableRadius)
if(pt[2]>(TableHeight+0.003) && pt[2]<BlockMaxHeight)
{
index.push_back(i);
BlockColor(0)+=transformed_pclKinect_clr_ptr_->points[i].r;
BlockColor(1)+=transformed_pclKinect_clr_ptr_->points[i].g;
BlockColor(2)+=transformed_pclKinect_clr_ptr_->points[i].b;
}
}
int n_block_points = index.size();
if(n_block_points<10)
{
ROS_INFO("There is no block on the stool.");
return 0;
}
ROS_INFO("There is a block with %d points", n_block_points);
BlockColor/=n_block_points;
ROS_INFO_STREAM("The block color:"<<BlockColor.transpose());
display_ptr_->header = transformed_pclKinect_clr_ptr_->header;
display_ptr_->is_dense = transformed_pclKinect_clr_ptr_->is_dense;
display_ptr_->width = n_block_points; //transformed_pclKinect_clr_ptr_->width;
display_ptr_->height = transformed_pclKinect_clr_ptr_->height;
display_ptr_->points.resize(n_block_points);
for (int i = 0; i < n_block_points; i++)
{
display_ptr_->points[i].getVector3fMap() = transformed_pclKinect_clr_ptr_->points[index[i]].getVector3fMap();
}
display_points(*display_ptr_);
Eigen::Vector3f BlockCentroid;
BlockCentroid =pcl_wsn.compute_centroid(display_ptr_);
ROS_INFO_STREAM("The centroid of the block:"<<BlockCentroid.transpose());
vector<int> block_index;
block_index.clear();
for (int i = 0; i < n_block_points; i++)
{
pt=display_ptr_->points[i].getVector3fMap();
dist = pt - BlockCentroid;
dist[2]=0;
distance = dist.norm();
if(distance < BlockTopRadius)
{
block_index.push_back(i);
}
}
int n_block_top = block_index.size();
ROS_INFO("There are %d points around the block's top center",n_block_top);
pcl::PointCloud<pcl::PointXYZ>::Ptr block_ptr_(new PointCloud<pcl::PointXYZ>);
block_ptr_->header=display_ptr_->header;
block_ptr_->is_dense=display_ptr_->is_dense;
block_ptr_->width=n_block_top;
block_ptr_->height=display_ptr_->height;
block_ptr_->points.resize(n_block_top);
for (int i = 0; i < n_block_top; i++)
{
block_ptr_->points[i].getVector3fMap()=display_ptr_->points[block_index[i]].getVector3fMap();
}
BlockTopCentroid = pcl_wsn.compute_centroid(block_ptr_);
ROS_INFO_STREAM("The centroid of the block's top:"<<BlockTopCentroid.transpose());
//display_points(*block_ptr_);
block_index.clear();
for(int i = 0; i < n_block_points; i++)
{
pt=display_ptr_->points[i].getVector3fMap();
if(abs(pt[2]-BlockTopCentroid[2])<0.002)
{
block_index.push_back(i);
}
}
n_block_top = block_index.size();
block_ptr_->header=display_ptr_->header;
block_ptr_->is_dense=display_ptr_->is_dense;
block_ptr_->width=n_block_top;
block_ptr_->height=display_ptr_->height;
block_ptr_->points.resize(n_block_top);
for (int i = 0; i < n_block_top; i++)
{
block_ptr_->points[i].getVector3fMap()=display_ptr_->points[block_index[i]].getVector3fMap();
}
double block_dist;
pcl_wsn.fit_points_to_plane(block_ptr_,Block_Normal,block_dist);
Block_Major = pcl_wsn.get_major_axis();
ROS_INFO_STREAM("The major vector of the block's top:"<<Block_Major.transpose());
//display_points(*block_ptr_);
return true;
}
bool Pcl_grabing::findTableTop() {
update_kinect_points();
int npts = transformed_pclKinect_clr_ptr_->points.size();
vector<int> index;
Eigen::Vector3f pt;
vector<double> color_err_RGB;
double color_err;
color_err = 255;
color_err_RGB.resize(3);
index.clear();
ROS_INFO("begin to find the table top");
for (int i = 0; i < npts; i++)
{
pt = transformed_pclKinect_clr_ptr_->points[i].getVector3fMap();
color_err_RGB[0] = abs(roughColor_R - transformed_pclKinect_clr_ptr_->points[i].r);
color_err_RGB[1] = abs(roughColor_G - transformed_pclKinect_clr_ptr_->points[i].g);
color_err_RGB[2] = abs(roughColor_B - transformed_pclKinect_clr_ptr_->points[i].b);
color_err = color_err_RGB[0] + color_err_RGB[1] + color_err_RGB[2];
if (abs(pt[2] - roughHeight) < HeightRange)
{
if (color_err < ColorRange)
{
if(pt[0]>Table_X_Min && pt[0]<Table_X_Max)
{
if(pt[0]>Table_Y_Min && pt[0]<Table_Y_Max)
{
index.push_back(i);
}
}
}
}
}
if (index.size() < 20)
{
ROS_INFO("failed to find table top.");
return 0;
}
int n_display = index.size();
ROS_INFO("found out %d points on table.", n_display);
display_ptr_->header = transformed_pclKinect_clr_ptr_->header;
display_ptr_->is_dense = transformed_pclKinect_clr_ptr_->is_dense;
display_ptr_->width = n_display;
display_ptr_->height = transformed_pclKinect_clr_ptr_->height;
display_ptr_->points.resize(n_display);
for (int i = 0; i < n_display; i++) {
display_ptr_->points[i].getVector3fMap() = transformed_pclKinect_clr_ptr_->points[index[i]].getVector3fMap();
}
ROS_INFO("display_point conversed.");
display_points(*display_ptr_);
TableCentroid =pcl_wsn.compute_centroid(display_ptr_);
TableHeight = TableCentroid(2);
ROS_INFO_STREAM("Table Centroid:"<<TableCentroid.transpose());
ROS_INFO_STREAM("Table Height"<<TableHeight);
return true;
}
int
run_program(int argc, char **argv)
{
CommandLineArgument<std::string> points_pathname;
Configuration cfg;
cfg.width = 640;
cfg.height = 480;
cfg.window_title = "";
cfg.circle_radius = 2;
cfg.circle_thickness = 1;
cfg.circle_linetype = 8;
cfg.circle_shift = 0;
cfg.circle_colour = cv::Scalar(0, 0, 255);
for (int i = 1; i < argc; i++) {
std::string argument(argv[i]);
if (argument == "--help") {
print_usage();
return 0;
} else if (argument == "--width") {
cfg.width = get_argument<int>(&i, argc, argv);
} else if (argument == "--height") {
cfg.height = get_argument<int>(&i, argc, argv);
} else if (argument == "--window-title") {
cfg.window_title = get_argument(&i, argc, argv);
} else if (argument == "--circle-radius") {
cfg.circle_radius = get_argument<int>(&i, argc, argv);
} else if (!assign_argument(argument, points_pathname)) {
throw make_runtime_error("Unable to process argument: %s\n", argument.c_str());
}
}
if (!have_argument_p(points_pathname)) {
print_usage();
return -1;
}
if (cfg.window_title == "")
cfg.window_title = points_pathname->c_str();
std::vector<cv::Point3_<double> > shape3D;
try {
shape3D = load_points3(points_pathname->c_str());
} catch (std::exception &e) {
throw make_runtime_error("Encountered error when reading file '%s': %s", points_pathname->c_str(), e.what());
}
cv::Point3_<double> mean = calculate_mean(shape3D);
for (size_t i = 0; i < shape3D.size(); i++) {
shape3D[i] -= mean;
}
cv::Mat_<double> projection = cv::Mat_<double>::eye(3,4);
if (cfg.height < cfg.width) {
projection(0,3) = double(cfg.width - cfg.height) / 2.0;
} else {
projection(1,3) = double(cfg.height - cfg.width) / 2.0;
}
cv::Mat_<double> starting_A = calculate_similarity_transform(shape3D, cfg.height, cfg.width);
cv::Mat_<double> A(starting_A.clone());
bool quit = false;
while (!quit) {
int key = cv::waitKey(1);
switch (key) {
case 27:
quit = true;
break;
case 'z':
A = A * scaling_matrix(1.1);
break;
case 'x':
A = A * scaling_matrix(0.9);
break;
case 'a':
A = A * rotation_about_y_axis(-0.1);
break;
case 'd':
A = A * rotation_about_y_axis(0.1);
break;
case 'w':
A = A * rotation_about_x_axis(-0.1);
break;
case 's':
A = A * rotation_about_x_axis(0.1);
break;
case 'q':
A = A * rotation_about_z_axis(0.1);
break;
case 'e':
A = A * rotation_about_z_axis(-0.1);
break;
case 'r':
A = starting_A.clone();
break;
}
display_points(cfg, projection * A, shape3D);
}
return 0;
}
static int zoom2(int x, int y)
{
int top, bottom, left, right;
int row, col;
struct Cell_head cellhd;
x2 = x;
y2 = y;
/*
* user has completed the zoom window.
* must be in same view as first corner
*/
if (x1 == x2 || y1 == y2)
return 0; /* ignore event */
if (!In_view(pick_view, x2, y2))
return 0;
/*
* ok, erase menu messages
*/
Menu_msg("");
/*
* assign window coordinates to top,bottom,left,right
*/
if (x1 < x2) {
left = x1;
right = x2;
}
else {
left = x2;
right = x1;
}
if (y1 < y2) {
top = y1;
bottom = y2;
}
else {
top = y2;
bottom = y1;
}
/*
* Determine the the zoom window (ie, cellhd)
* must copy the current view cellhd first, to preserve header info
* (such as projection, zone, and other items.)
* compute zoom window northings,eastings, rows, cols, and resolution
*/
G_copy(&cellhd, &pick_view->cell.head, sizeof(cellhd));
/* convert top to northing at top edge of cell
* left to easting at left edge
*/
col = view_to_col(pick_view, left);
row = view_to_row(pick_view, top);
cellhd.north = row_to_northing(&pick_view->cell.head, row, 0.0);
cellhd.west = col_to_easting(&pick_view->cell.head, col, 0.0);
/* convert bottom to northing at bottom edge of cell
* right to easting at right edge
*/
col = view_to_col(pick_view, right);
row = view_to_row(pick_view, bottom);
cellhd.south = row_to_northing(&pick_view->cell.head, row, 1.0);
cellhd.east = col_to_easting(&pick_view->cell.head, col, 1.0);
cellhd.rows = bottom - top + 1;
cellhd.cols = right - left + 1;
cellhd.ns_res = (cellhd.north - cellhd.south) / cellhd.rows;
cellhd.ew_res = (cellhd.east - cellhd.west) / cellhd.cols;
/*
* Outline the zoom window on the main map
* Turn previous one to grey.
*/
if (zoom_view->cell.configured) {
R_standard_color(GREY);
Outline_cellhd(main_view, &zoom_view->cell.head);
}
R_standard_color(RED);
Outline_cellhd(main_view, &cellhd);
/*
* zoom
*/
if (target_flag)
select_target_env();
G_adjust_window_to_box(&cellhd, &zoom_view->cell.head, zoom_view->nrows,
zoom_view->ncols);
Configure_view(zoom_view, pick_view->cell.name, pick_view->cell.mapset,
pick_view->cell.ns_res, pick_view->cell.ew_res);
drawcell(zoom_view);
select_current_env();
display_points(1);
return 1; /* pop back */
}
int main(int argc, char *argv[])
{
char name[GNAME_MAX], mapset[GMAPSET_MAX], xmapset[GMAPSET_MAX];
struct Cell_head cellhd;
struct GModule *module;
struct Option *grp;
/* must run in a term window */
G_putenv("GRASS_UI_TERM", "1");
G_gisinit(argv[0]);
module = G_define_module();
G_add_keyword(_("imagery"));
G_add_keyword(_("geometry"));
module->description =
_("Mark ground control points on image to be rectified.");
grp = G_define_option();
grp->key = "group";
grp->type = TYPE_STRING;
grp->required = YES;
grp->gisprompt = "old,group,group";
grp->description = _("Name of imagery group to be registered");
if (G_parser(argc, argv))
exit(EXIT_FAILURE);
Rast_suppress_masking(); /* need to do this for target location */
interrupt_char = G_intr_char();
tempfile1 = G_tempfile();
tempfile2 = G_tempfile();
cell_list = G_tempfile();
vect_list = G_tempfile();
group_list = G_tempfile();
digit_points = G_tempfile();
digit_results = G_tempfile();
if (R_open_driver() != 0)
G_fatal_error(_("No graphics device selected"));
/* parse group name */
/* only enforce local-mapset-only due to I_get_group_ref() not liking "@mapset" */
if (G_name_is_fully_qualified(grp->answer, group.name, xmapset)) {
if (0 != strcmp(G_mapset(), xmapset))
G_fatal_error(_("[%s] Only local groups may be used"),
grp->answer);
}
else {
strncpy(group.name, grp->answer, GNAME_MAX - 1);
group.name[GNAME_MAX - 1] = '\0'; /* strncpy() doesn't null terminate on overflow */
}
if (!I_get_group_ref(group.name, &group.ref))
G_fatal_error(_("Group [%s] contains no maps, run i.group"),
group.name);
if (group.ref.nfiles <= 0)
G_fatal_error(_("Group [%s] contains no maps, run i.group"),
group.name);
/* write group files to group list file */
prepare_group_list();
/* get target info and environment */
get_target();
find_target_files();
/* read group control points, if any */
G_suppress_warnings(1);
if (!I_get_control_points(group.name, &group.points))
group.points.count = 0;
G_suppress_warnings(0);
/* determine transformation equation */
Compute_equation();
signal(SIGINT, SIG_IGN);
/* signal (SIGQUIT, SIG_IGN); */
Init_graphics();
display_title(VIEW_MAP1);
select_target_env();
display_title(VIEW_MAP2);
select_current_env();
Begin_curses();
G_set_error_routine(error);
/*
#ifdef SIGTSTP
signal (SIGTSTP, SIG_IGN);
#endif
*/
/* ask user for group file to be displayed */
do {
if (!choose_groupfile(name, mapset))
quit(0);
/* display this file in "map1" */
}
while (!G_find_raster2(name, mapset));
Rast_get_cellhd(name, mapset, &cellhd);
G_adjust_window_to_box(&cellhd, &VIEW_MAP1->cell.head, VIEW_MAP1->nrows,
VIEW_MAP1->ncols);
Configure_view(VIEW_MAP1, name, mapset, cellhd.ns_res, cellhd.ew_res);
drawcell(VIEW_MAP1);
display_points(1);
Curses_clear_window(PROMPT_WINDOW);
/* determine initial input method. */
setup_digitizer();
if (use_digitizer) {
from_digitizer = 1;
from_keyboard = 0;
from_flag = 1;
}
/* go do the work */
driver();
quit(0);
}
int edit_line_update(void *closure, int sxn, int syn, int button)
{
struct edit_line *el = closure;
double x = D_d_to_u_col(sxn);
double y = D_d_to_u_row(syn);
G_debug(3, "button = %d x = %d = %f y = %d = %f", button, sxn, x, syn, y);
if (button == 3) /* Tool broken by GUI */
return 1;
switch (el->phase) {
case 1:
if (button != 1)
return 0;
/* Find nearest point or line */
el->line =
Vect_find_line(&Map, x, y, 0, GV_LINE | GV_BOUNDARY, el->thresh,
0, 0);
G_debug(2, "line found = %d", el->line);
/* Display new selected line if any */
if (el->line > 0) {
display_line(el->line, SYMB_HIGHLIGHT, 1);
edit_line_phase2(el, x, y);
}
break;
case 2:
if (button == 1) { /* New point */
snap(&x, &y);
Vect_append_point(el->Points, x, y, 0);
if (el->line_type == GV_LINE)
symb_set_driver_color(SYMB_LINE);
else
symb_set_driver_color(SYMB_BOUNDARY_0);
display_points(el->Points, 1);
set_location(sxn, syn);
i_prompt_buttons(_("New Point"), _("Undo Last Point"), _("Close line"));
}
else if (button == 2) { /* Undo last point */
if (el->Points->n_points > 1) {
symb_set_driver_color(SYMB_BACKGROUND);
display_points(el->Points, 1);
el->Points->n_points--;
if (el->line_type == GV_LINE)
symb_set_driver_color(SYMB_LINE);
else
symb_set_driver_color(SYMB_BOUNDARY_0);
display_points(el->Points, 1);
set_location(D_u_to_d_col
(el->Points->x[el->Points->n_points - 1]),
D_u_to_d_row(el->Points->
y[el->Points->n_points - 1])
);
if (el->Points->n_points == 1)
i_prompt_buttons(_("New Point"), "", _("Delete line and exit"));
}
}
break;
}
return 0;
}
int new_line_update(void *closure, int sxn, int syn, int button)
{
struct new_line *nl = closure;
double x = D_d_to_u_col(sxn);
double y = D_d_to_u_row(syn);
double dist;
G_debug(3, "button = %d x = %d = %f y = %d = %f", button, sxn, x, syn, y);
if (nl->first && button == 3) { /* Quit tool ( points & lines ), first is always for points */
Tool_next = TOOL_NOTHING;
return 1;
}
if (button > 3) /* Do nothing on mouse scroll */
return 0;
if (nl->type & GV_POINTS && (button == 1 || button == 2)) {
snap(&x, &y);
Vect_append_point(nl->Points, x, y, 0);
write_line(&Map, nl->type, nl->Points);
updated_lines_and_nodes_erase_refresh_display();
return 1;
}
else { /* GV_LINES */
/* Button may be 1,2,3 */
if (button == 1) { /* New point */
if (snap(&x, &y) == 0) {
/* If not snapping to other features, try to snap to lines start.
* Allows to create area of single boundary. */
if (nl->Points->n_points > 2) {
dist = Vect_points_distance(nl->Points->x[0], nl->Points->y[0], 0, x, y, 0, WITHOUT_Z);
if (dist < get_thresh()) {
x = nl->Points->x[0];
y = nl->Points->y[0];
}
}
}
if (Vect_append_point(nl->Points, x, y, 0) == -1) {
G_warning(_("Out of memory! Point not added."));
return 0;
}
if (nl->type == GV_LINE)
symb_set_driver_color(SYMB_LINE);
else
symb_set_driver_color(SYMB_BOUNDARY_0);
display_points(nl->Points, 1);
set_location(D_u_to_d_col(x), D_u_to_d_row(y));
nl->first = 0;
set_mode(MOUSE_LINE);
}
else if (button == 2) { /* Undo last point */
if (nl->Points->n_points >= 1) {
symb_set_driver_color(SYMB_BACKGROUND);
display_points(nl->Points, 1);
nl->Points->n_points--;
if (nl->type == GV_LINE)
symb_set_driver_color(SYMB_LINE);
else
symb_set_driver_color(SYMB_BOUNDARY_0);
display_points(nl->Points, 1);
set_location(D_u_to_d_col
(nl->Points->x[nl->Points->n_points - 1]),
D_u_to_d_row(nl->Points->
y[nl->Points->n_points - 1])
);
}
if (nl->Points->n_points == 0) {
i_prompt_buttons(_("New point"), "", _("Quit tool"));
nl->first = 1;
set_mode(MOUSE_POINT);
}
}
else if (button == 3) { /* write the line and quit */
if (nl->Points->n_points > 1) {
/* Before the line is written, we must check if connected to existing nodes, if yes,
* such nodes must be add to update list before! the line is written (areas/isles */
int node1 =
Vect_find_node(&Map, nl->Points->x[0], nl->Points->y[0],
nl->Points->z[0], 0, Vect_is_3d(&Map));
int i = nl->Points->n_points - 1;
int node2 =
Vect_find_node(&Map, nl->Points->x[i], nl->Points->y[i],
nl->Points->z[i], 0, Vect_is_3d(&Map));
G_debug(2, " old node1 = %d old node2 = %d", node1, node2);
write_line(&Map, nl->type, nl->Points);
updated_lines_and_nodes_erase_refresh_display();
}
else
G_warning(_("Less than 2 points for line -> nothing written"));
return 1;
}
G_debug(2, "n_points = %d", nl->Points->n_points);
}
i_prompt_buttons(_("New point"), _("Undo last point"), _("Close line"));
return 0;
}
