用Fast Sweeping Method解路径规划问题

1 Fast Sweeping Method

1.1 Armadillo C++ 线性代数库的实现

　　Armadillo库在处理张量时比较方便易用，因为它的函数调用故意被设计成与MATLAB一样，API参考官方网站。
　　想声明一个三维张量，用下面的语句即可，其中cube=Cube，即它里面每个元素都是double类型。可以把张量u想象成5个矩阵叠起来，而每个矩阵都是3行4列。还可以把张量u直观地看成在三维坐标系里，x轴上的整数代表行，y轴上的整数代表列，z轴上的整数代表层。xy坐标就是正常的矩阵索引。这样的定义与我们的直觉相符，不用绕弯就知道怎么用。
　　cube u=zeros(3,4,5);
　　想改变大小，用set_size即可：
　　u.set_size(6,7,8);
　　把所有值都设成一样的，用fill即可：
　　u.fill(0);
　　取元素就用小括号。例如，取第1层矩阵中第3行第4列的那个元素，很简单，就是u(2,3,0);，即u(行索引、列索引、层索引)。注意，索引都是从0开始的。
　　有的时候我们想一次取很多元素，怎么办呢？Armadillo都能办到。
　　取某层矩阵，用slice即可。例如，取第1层矩阵就是u.slice(0)。
　　取所有层的某行或某列怎么做呢？用tube即可，输入可以是起止行号和列号，如u.tube(first_row, first_col, last_row, last_col)。
　　例如，取所有矩阵的第一行就是u.tube(0,0,0,3)。
　　如果不取整行，而只想某行的若干列，可以和span函数搭配使用。
　　取出张量中所有元素的最大值，用max即可，写法是：
　　double max_value = u.max()

1.2 Eigen C++ 线性代数库的实现

　　Eigen库在处理张量时跟Armadillo有一些区别，API参考官方网站。
　　首先，它声明张量的方法是Eigen::Tensor u(5,3,4);。注意，Eigen的层在最前面，而Armadillo在最后面，这是区别之一。所以，Eigen的a(5,3,4)就等价于Armadillo的a(3,4,5)。
　　取元素也是用小括号。例如，取第1层矩阵中第3行第4列的那个元素，那就是u(0,2,3);，即u(层索引、行索引、列索引)。
　　想改变一个张量的大小，用resize，例如：
　　u.resize(6,7,8)。
　　把所有值都设成一样的，用setConstant即可，例如：
　　u.setConstant(0);
　　取出张量中所有元素的最大值，用maximum方法，用法如下。注意，取出来的东西不是一个double类型，而是Eigen::Tensor类型，这个类型不能与普通的double类型的变量运算，也不能用auto替代。想从中取出double类型就要用取元素符号()。这又是与Armadillo不一样的地方。
　　Eigen::Tensor max_tensor = u.maximum();
　　double max_value = max_tensor(0);
　　取张量的某部分，可以用chip，它能取某行某列某层，用法如下图所示。第一个位置填偏移量，第二个位置填想要的维数：chip(偏移,维数)。但是暂时没有像Armadillo中的span函数，无法更精细地取某些连续元素。