后期地图处理:建墙

  • 建墙只需处理分隔两个房间或走廊的,没必要处理外墙。墙必须真实存在。
  • 锚点,或叫固定点,是在旋转或拉伸时,位置保持不变的点。在旋转时,它是旋转中心。

 

建图时,因为算法缺陷,以及不会走遍房屋,一些本该是墙的地方没生成出来。为避免在导航阶段,机器人穿过本该是墙的位置,可手动在墙的位置建墙。建墙时,只需考虑分隔两个房间、走廊的,不必管外墙。要注意,墙必须真实存在。如果在不是墙的地方强制建墙,会干扰机器人重定位,导航会出错。

 

一、建墙

图1 5堵墙

图1显示的地图包括3个房间、一条走廊,手动建了5堵墙。2墙用于分隔房间,3墙用于分隔房间和走廊,它们都真实存在。实际使用时,分隔房间的两堵墙必须建,至于分隔走廊的3墙,或许可以不用。房屋外墙则不必管了。

在代码中,建墙主要通过一个叫tmarker_placer的类。

 

二、tmarker_placer

图2 编辑墙

2.1 trsp_rosmapmarker、tmap_marker

设想中,地图上将来会添加多种标记,墙是当中一种。用trsp_rosmapmarker统一封装标记,标记也是以着这结构存储到地图文件。

<apps-src>/apps/external/3rdparty/rose_filesystem.hpp
------
enum {rspmapmarkertype_virtual, rspmapmarkertype_wall, rspmapmarkertype_count};
 
#define RSP_MIN_WALL_WIDTH	1.5
#define RSP_MAX_WALL_WIDTH	12.0
struct trsp_rosmapmarker {
	char uuid[36 + 1]; // 38a08106-e2c9-48ed-a19f-874f5690a7bd.
	int type;
	double x;
	double y;
	double width;
	double height;
	double altitude; // 3D: width(->length), height(->width), altitude(->height)
	double theta;
};
  • uuid。该标记在一众标记中唯一标识。
  • type。对墙这种标记,type值是rspmapmarkertype_wall。目前也只支持这种标记。不同类型标记,对后面5字段会有不同语义。
  • x、y。对墙,锚点在world系下坐标。单位米。注意不是墙所在矩形的左上角坐标。什么是锚点,下文有解释。
  • width。对墙,表示墙的长度。注意,变量名width表示长度,height反而表示宽度,这么定义是为和后绪表示矩形的变量名一致,像SDL_Rect。
  • height。对墙,值固定0。表示宽度由后面app自行决定。目前用了一个3像素宽度的矩形,反映到reslution=0.05(cm)的world地图,2、3像素等价墙宽度是10cm、15cm厘米。墙不是全横或全竖,旋转角度不是0、90、180、270时,有些宽度会小于3像素,但应该能有2像素。
  • altitude。对墙,没用字段。值固定0。这字段用于表示某种三维标记中的高度。
  • theta。对墙,表示旋转角度。一堵墙,必须是直的,只是可以有一个旋转角度,旋转中心是锚点。

tmap_marker是trsp_rosmapmarker的再封装,成员变量rsp的类型便是后者。

 

2.2 tfloat_panel

tmarker_placer派生于tfloat_panel。tfloat_panel命名中“float”表示这功能是由浮动控件实现,具体类型是track控件。“panel”表示出来的行为类似面板,上面可以按钮、图像等内容。

tfloat_panel设计初衷是实现这样功能面板:窗口中有一个标记物,要编辑了,弹出这个面板,然后可以编辑标记物,操作包括移动、旋转、拉伸等。

移动、旋转、拉伸行为由tfloat_panel这个基类实现,其它操作,像保存,则是通知派生类,“保存”按钮被按下了,派生类自行处理。

 

2.3 旋转

锚点,或叫固定点,是在旋转或拉伸时,位置保持不变的点。在旋转时,它是旋转中心。图2中红线上端的蓝色小方块是锚点。

旋转总是基于标准矩形(std_rect)开始,标准矩形指的是旋转角度(theta)是0的那个矩形。图2中的红色矩形框是标准矩形,由于有旋转了,右则一部分没显示出来。从计算量来说,从0转到90,和从45转到90,可说是一样的。标准矩形旋转后的结果称为显示矩形(rect),图2中就是半透明绿色矩形。当旋转角度是0时,标准矩形和显示矩形重合。

如何计算此次要旋转多少度?——很快会想到种方法,用此次和之前某次鼠标左键(或手指)落在的x、y坐标差,然后用反正切算出一个角度。

    int deltax = x - downing_pt_.x;
    int deltay = y - downing_pt_.y;
    double theta = atan2(deltay, deltax);
    double ros_theta = -1 * theta;

downing_pt_是之前某次落在的坐标,像左键按下(left_button_down)时坐标。x、y是拖着鼠标移动(mouse_motion)、到达的坐标。在数学坐标系中,结果为正表示从X轴逆时针旋转的角度,结果为负表示从X轴顺时针旋转的角度,但atan2返回值和这个相反,结果要乘上“-1”。

这里有个疑问,如何取downing_pt_,有人会想到取左键按下时坐标,但用这会产生个问题。让看下deltax=0时,atan2返回值。

要进入计算theta,x、y必须得变动,因而会先滤掉deltax、deltay都是0情况。当delta=0时,atan2会有两种结果:+90度或-90度。这会造成这么现象。

前面不管是处在什么角度,只要这次deltax等于0,那图2中红线立刻变成竖直。

这在旋转初始时其容易出现。按下鼠标左键时,导航箭头是0度,然后鼠标笔直上、下移,,箭头立刻变竖直了。总的来说,如果取左键按下时坐标为downing_pt_,会导致旋转角度有突变。

解决办法是要让deltax不易出现0。针对这里场景,downing_pt_不是取左键按下时坐标,而是取锚点。锚点和旋转按钮间有一定距离,初始时,天然避免了deltax是0。

 

2.4 拉伸

对墙,只须伸缩长度,不用管高度。

拉伸时,锚点位置保持不变。计算拉伸后长度也在标准矩形,在标准矩矩形按要求计算出来后,然后继续之前已有的角度theta进行旋转。

 

2.5 放大倍数发生变化(gui_ratio_changed)

查看房屋地图时,用户可能会在多种放大倍数间切来切去。float_panel需要处理这个变化。

  • 锚点对应world地图中的位置不能变。
  • 墙的实际长度不能变,显示长度随着倍数变化按比例缩放。

处理倍数变化用三个步骤。mainmap指的是界面显示时用于当背景板、由一个个四方栅格组成的大地图,不是表示房屋的world地图。

  1. 获得锚点在旧倍数下的大地图坐标old_mainmap_xy。不能用标准矩形的左上角坐标,当倍数发生变时,锚点和左上角之间偏移是会变化的。需要确保的是锚点不变。
  2. 计算锚点在新倍数下的大地图坐标new_mainmap_xy。计算方法是把old_mainmap_xy按两次放大倍数比例缩放。
  3. 由锚点大地图坐标new_mainmap_xy算出屏幕坐标mouxe_xy,这mouxe_xy便可计出新倍数下标准矩形左上角坐标。

知道标准矩形后,旋转theta角度计算出显示矩形,从而在新倍数下显示float_panel。

 

全部评论: 0

    写评论: