NeuroSLAM 代码解析

1 整体框架

NeuroSLAM 代码是基于MATLAB语言的，整体包括八个部分，如下所示：

• 01 联合位姿细胞网络，包括三维栅格细胞网络和多层头朝向细胞网络，主要包括两个网络的初始化以及更新过程；
• 02 多层经验图，主要包括经验图的初始化与更新；
• 03 视觉里程计，主要是视觉里程计模块的初始化以及线速度、角速度以及高度速度的计算与更新；
• 04 视觉模板，主要是视觉模板的创建与更新；
• 05 工具库，包括导入数据、处理视觉数据、图像排序、可视化历史数据以及保存历史数据等；
• 06 主函数，包括NeuroSLAM整体的流程，从图像的输入到经验图的构建；
• 07 测试，包括三维建图和里程计测试两个部分，其中每个部分使用三个数据集进行测试；
• 08 论文中附图的代码，包括经验图的history，视觉模板的history，栅格细胞和头朝向细胞的活性分析等等。

各模块整体流程图如下所示：

2 代码解析

2.1 conjunctive_pose_cells_network

联合位姿细胞网络包括三维栅格细胞网络和多层头朝向细胞网络，如下所示。

2.1.1 3d_grid_cell_network

该部分主要包括三维栅格细胞的初始化（gc_initial.m）、更新（gc_iteration.m）以及获取更新后的栅格细胞的位置（get_gc_xyz.m）。流程如下图所示：

• gc_initial.m主要是用于创建栅格细胞的兴奋权重矩阵${\epsilon }_{x,y,z}^{gc}$、抑制权重矩阵${\psi }_{xyz}^{gc}$以及栅格细胞的初始位置$(gcX,gcY,gcZ)$ ，兴奋矩阵和抑制权重矩阵的创建如下所示：

xDimCentre = floor(xDim / 2) + 1;
yDimCentre = floor(yDim / 2) + 1;
zDimCentre = floor(zDim / 2) + 1;
weight = zeros(xDim, yDim, zDim);
for z = 1 : zDim  
    for x = 1 : xDim
        for y = 1 : yDim
           weight(x,y,z) = 1/(xVar*sqrt(2*pi))*exp((-(x - xDimCentre) ^ 2) / (2 * xVar ^ 2)) ...
               * 1/(yVar*sqrt(2*pi))*exp((-(y - yDimCentre) ^ 2) / (2 * yVar ^ 2)) ...
               * 1/(zVar*sqrt(2*pi))*exp((-(z - zDimCentre) ^ 2) / (2 * zVar ^ 2)); 
        end
    end
end

total = sum(sum(sum(weight)));
weight = weight./total;     

与之前公式相对应，即(x-xDimCentre)对应公式中的$u$（公式中的$u$给定是否不正确？）

• gc_iteration.m文件包含的栅格细胞的更新过程包括：
  • Add view template energy;
  • Local excitation;
  • Local inhibition;
  • Global inhibition;
  • Normalization;
  • Path Integration（重点）
    该过程是通过两个部分实现的，分别在xy平面和z维度进行更新。参考以下公式进行：
    
    公式中的$U_{lmn}^{gc}$相当于栅格细胞的活性表示$P_{xyz}^{gc}$，代码实现如下所示：

% pi in x-y plane
gcInZPlane90 = rot90(GRIDCELLS(:,:,indZ), floor(curYawThetaInRadian *2/pi));   %逆时针旋转floor(curYawThetaInRadian *2/pi)度
dir90 = curYawThetaInRadian - floor(curYawThetaInRadian *2/pi)* pi/2;    % 角度的小数
gcInZPlaneNew = zeros(GC_X_DIM + 2, GC_Y_DIM + 2);            
gcInZPlaneNew(2:end-1,2:end-1) = gcInZPlane90;
weight_sw = transV^2 * cos(dir90) * sin(dir90);                         %表示delta_xf*delta_yf=v*cos(dir90)*v*sin(dir90)
weight_se = transV * sin(dir90) - transV^2 * cos(dir90) * sin(dir90);   %表示delta_yf*(1-delta_xf)
weight_nw = transV * cos(dir90) - transV^2 * cos(dir90) * sin(dir90);   %表示delta_xf*(1-delta_yf)
weight_ne = 1.0 - weight_sw - weight_se - weight_nw;                    %表示(1-delta_xf)*(1-delta_yf)
gcInZPlaneNew = gcInZPlaneNew.*weight_ne + circshift(gcInZPlaneNew, [0 1]).*weight_nw + circshift(gcInZPlaneNew, [1 0]).*weight_se + circshift(gcInZPlaneNew, [1 1]).*weight_sw;
gcInZPlane90 = gcInZPlaneNew(2:end-1,2:end-1);
gcInZPlane90(2:end,1) = gcInZPlane90(2:end,1) + gcInZPlaneNew(3:end-1,end);
gcInZPlane90(1,2:end) = gcInZPlane90(1,2:end) + gcInZPlaneNew(end,3:end-1);
gcInZPlane90(1,1) = gcInZPlane90(1,1) + gcInZPlaneNew(end:end);
GRIDCELLS(:,:,indZ) = rot90(gcInZPlane90, 4 - floor(curYawThetaInRadian * 2/pi));

% pi in z axis
weight = mod(abs(heightV) / GC_Z_TH_SIZE, 1);
if weight == 0
    weight = 1.0;
end
GRIDCELLS = circshift(GRIDCELLS, [0 0 sign(heightV)* floor(mod(abs(heightV) / GC_Z_TH_SIZE, GC_Z_DIM))]) * (1.0 - weight) ...
    + circshift(GRIDCELLS, [0 0 sign(heightV) * ceil(mod(abs(heightV) / GC_Z_TH_SIZE, GC_Z_DIM))]) * (weight);

• get_gc_xyz.m文件用于获取激活的栅格细胞的位置。实现过程包括：
  • 找到激活细胞的最大值；
  • take the max activated cell ± AVG_CELL in 3d space；
  • 获得栅格细胞激活的三维位置[gcX, gcY, gcZ]

2.1.2 yaw_height_hdc_network

该部分主要包括头朝向细胞的初始化（yaw_height_hdc_initial.m）、更新（yaw_height_hdc_iteration.m）以及获取更新后的头朝向细胞的位置（get_current_yaw_height_value.m），与栅格细胞更新类似。

2.2 multilayered_experience_map

该部分主要包括多层经验图的初始化（exp_initial.m）和更新（exp_map_iteration.m）。

• 定义经验图的一个经验为$E_i$，可表示为：
  E i = { V i l v , P i g c , P i h d c , P i e x p } E_i = \{V^{lv}_i,P^{gc}_i,P^{hdc}_i,P^{exp}_i \} Ei​={Vilv​,Pigc​,Pihdc​,Piexp​}
  创建一个新的经验：

% Initial the experience
EXP_HISTORY = 1;
NUM_EXPS = 1;
CUR_EXP_ID = 1;
EXPERIENCES(CUR_EXP_ID).x_gc = gcX;
EXPERIENCES(CUR_EXP_ID).y_gc = gcY;
EXPERIENCES(CUR_EXP_ID).z_gc = gcZ;
EXPERIENCES(CUR_EXP_ID).yaw_hdc = curYawTheta;
EXPERIENCES(CUR_EXP_ID).height_hdc = curHeightValue;
EXPERIENCES(CUR_EXP_ID).vt_id = 1;
EXPERIENCES(CUR_EXP_ID).x_exp = 0;
EXPERIENCES(CUR_EXP_ID).y_exp = 0;
EXPERIENCES(CUR_EXP_ID).z_exp = 0;
EXPERIENCES(CUR_EXP_ID).yaw_exp_rad = 0;
EXPERIENCES(CUR_EXP_ID).numlinks = 0;
EXPERIENCES(CUR_EXP_ID).links = [];

• 经验experience的更新：
  • 当视觉模板是新的或者三维栅格细胞和头朝向细胞发生变化足够创建一个新的经验，即当以下公式大于设置的阈值：
    
  • 如果视觉模板发生变化（但不是新的模板），寻找相匹配的经验。找到与当前视觉模板相关的经验，且该经验到栅格细胞活动中心和头朝向细胞活动中心的距离小于阈值。
    • 如果没有经验与当前视觉模板、三维栅格细胞以及头朝向细胞相匹配，则创建一个新的经验；
    • 如果多个经验都低于阈值，则不进行匹配（reduce hash collisions），且不创建新的经验；
    • 如果找到一个经验与之相匹配，判断之前是否有经验与该经验已经存在link，如果不存在link，我们会创建一个当前经验和用于当前视觉模板经验经验的link，link信息包括：

EXPERIENCES(CUR_EXP_ID).numlinks = EXPERIENCES(CUR_EXP_ID).numlinks + 1;
EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).exp_id = matched_exp_id;
EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).d_xy = sqrt(ACCUM_DELTA_X^2 + ACCUM_DELTA_Y^2);
EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).d_z = ACCUM_DELTA_Z; EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).heading_yaw_exp_rad = get_signed_delta_radian(EXPERIENCES(CUR_EXP_ID).yaw_exp_rad, atan2(ACCUM_DELTA_Y, ACCUM_DELTA_X)); 
EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).facing_yaw_exp_rad = get_signed_delta_radian(EXPERIENCES(CUR_EXP_ID).yaw_exp_rad, ACCUM_DELTA_YAW);

• 经验experience的修正（即发生回环时，对experience进行优化）
  
  在代码中，设置校正率$\psi$为0.5，则在校正过程中更依赖于$e_0$的link信息：

% experience 0 has a link to experience 1
e0 = exp_id;
e1 = EXPERIENCES(exp_id).links(link_id).exp_id;
% work out where e0 thinks e1 (x,y) should be based on the stored link information
lx = EXPERIENCES(e0).x_exp + EXPERIENCES(e0).links(link_id).d_xy * cos(EXPERIENCES(e0).yaw_exp_rad + EXPERIENCES(e0).links(link_id).heading_yaw_exp_rad);
ly = EXPERIENCES(e0).y_exp + EXPERIENCES(e0).links(link_id).d_xy * sin(EXPERIENCES(e0).yaw_exp_rad + EXPERIENCES(e0).links(link_id).heading_yaw_exp_rad);
lz = EXPERIENCES(e0).z_exp + EXPERIENCES(e0).links(link_id).d_z;  % 
% correct e0 and e1 (x,y) by equal but opposite amounts. a 0.5 correction parameter means that e0 and e1 will be fully corrected based on e0's link information
EXPERIENCES(e0).x_exp = EXPERIENCES(e0).x_exp + (EXPERIENCES(e1).x_exp - lx) * EXP_CORRECTION;
EXPERIENCES(e0).y_exp = EXPERIENCES(e0).y_exp + (EXPERIENCES(e1).y_exp - ly) * EXP_CORRECTION;
EXPERIENCES(e0).z_exp = EXPERIENCES(e0).z_exp + (EXPERIENCES(e1).z_exp - lz) * EXP_CORRECTION;
EXPERIENCES(e1).x_exp = EXPERIENCES(e1).x_exp - (EXPERIENCES(e1).x_exp - lx) * EXP_CORRECTION;
EXPERIENCES(e1).y_exp = EXPERIENCES(e1).y_exp - (EXPERIENCES(e1).y_exp - ly) * EXP_CORRECTION;
EXPERIENCES(e1).z_exp = EXPERIENCES(e1).z_exp - (EXPERIENCES(e1).z_exp - lz) * EXP_CORRECTION;

2.3 visual_odometry

该部分主要包括视觉里程计的初始化（visual_odo_initial.m）以及线速度、角速度以及高度速度的计算与更新（visual_odometry.m、visual_odometry_down.m以及visual_odometry_up.m）。

visual_odometry.m、visual_odometry_down.m以及visual_odometry_up.m文件实现功能相同，只是visual_odometry_down.m和visual_odometry_up.m文件添加了对高度偏移量的判断，下面以visual_odometry.m为例解析代码，如下图所示：

compare_segments.m实现了以下公式的功能，subImg1和subImg2分别表示当前帧图像和前一帧图像的感兴趣区域的一维强度分布(profile)；shift_len表示列维度的平移量，相当于公式中的$s^h$。

该公式是最小平均强度差的平移量的计算，关于角速度、线速度以及高度速度的比例计算公式就不列举了。

% 角速度的计算
yawRotV = ODO_YAW_ROT_V_SCALE * minOffsetYawRot * horiDegPerPixel;
% 平移速度的计算
transV = minDiffIntensityRot * ODO_TRANS_V_SCALE;
% 高度速度的计算
heightV = ODO_HEIGHT_V_SCALE * minDiffIntensityHeight;

2.4 visual_template

visual_template主要包括视觉模板的创建与更新；visual_template.m文件用于视觉模板的创建与更新，下图表示visual_template的实现流程：

其中，gc_x，gc_y，gc_z，yaw以及height用于存储在VT中，并不参与模板匹配的计算。

compare_segments.m文件的输入seg1和seg2表示当前归一化的图像强度和模板图像强度（二维数组），归一化操作参考以下公式；cwly和cwlx分别表示图像的高度和宽度；sleny和slenx分别表示在y轴和在x轴的偏移量，类似于视觉里程计中的$s^h$； vtPanoramic表示是否进行了patch normalization操作。文件的输出offsetY，offsetX，sdif分别表示强度差值最小时y轴偏移量，x轴偏移量以及强度差最小值。

计算出当前图像的强度差与模板图像的最小值之后，判断是否创建模板：

[diff, diff_id] = min(MIN_DIFF_CURR_IMG_VTS);
DIFFS_ALL_IMGS_VTS = [DIFFS_ALL_IMGS_VTS; diff];

% then create a new template
if (diff > VT_MATCH_THRESHOLD)
    NUM_VT = NUM_VT + 1;
    VT(NUM_VT).id = NUM_VT;
    VT(NUM_VT).template = normVtImg;
    VT(NUM_VT).decay = VT_ACTIVE_DECAY;
    VT(NUM_VT).gc_x = x;
    VT(NUM_VT).gc_y = y;
    VT(NUM_VT).gc_z = z;
    VT(NUM_VT).hdc_yaw = yaw;
    VT(NUM_VT).hdc_height = height;
    VT(NUM_VT).first = 1;           % don't want to inject energy as the vt is been created
    VT(NUM_VT).numExp = 0;
    VT(NUM_VT).exps = [];
    vt_id = NUM_VT;
    VT_HISTORY_FIRST = [VT_HISTORY_FIRST; vt_id];
else
    vt_id = diff_id;
    VT(vt_id).decay = VT(vt_id).decay + VT_ACTIVE_DECAY;
    VT_HISTORY_OLD = [VT_HISTORY_OLD; vt_id];
end

2.5 其他

05、06、07和08是工具库、主函数以及数据集的配置文件，论文的核心是以上几个模块。