A Study in Vision: October 2015

Wednesday 21 October 2015

solvePnP

I have used solvePnP a lot recently for camera pose estimation. Here is a fascinating post about some possible issues of the function.

Monday 19 October 2015

Loopy BP, MRF

今天读到一篇介绍BP和MRF的blog, 在stereo vision中，如果只考虑单独的像素做匹配，而忽略像素周围的pattern，就容易导致不连贯的disparity map。实际上我们希望disparity map中相邻像素有相同的值。这个时候就可以用到MRF。MRF是无向图，但是跟Bayesian不同的是它可以有cycle和loop。

蓝色的点表示观测到的变量，就是像素值。粉色的点表示隐藏变量，就是disparity。通常隐藏变量叫做label。node之间的link表示dependency，比如中间粉色的点只跟周围四个点和上面的蓝色的点有关。这个某点只跟周围点有关的假设就是Markov假设。这个假设使我们能够高效的求解隐藏变量。

如果用MRF来表达stereo vision，它的energy function就是

$energy(Y,X) = \sum\limits_{i} DataCost\left(y_i, x_i\right) \; + \sum\limits_{j = \mbox{neighbours of i}} SmoothnessCost\left(x_i,x_j\right)$

Y表示观测变量，X表示隐藏变量。i是pixel的index，j是xi相邻的node。给定一个图像Y和一些label X，这个能量方程求得了每个link的cost的和。我们的目标是找到一个label X，比如disparity map，使得这个能量方程最小化。接下来我们分开来看data cost和smoothness cost。

Datacost主要指把label xi赋值给data yi造成的cost。对于正确的匹配，datacost很低。对错误的匹配datacost就很高。常用的衡量datacost的有差值绝对值的和，SSD等。

Smoothness cost确保相邻像素有相同的label。我们需要一个函数来惩罚相邻像素有不同label的情况。常用的函数有如下几种

$f\left(n\right) = \begin{cases} 0 & \mbox{if } n = 0 \\ \lambda & \mbox{otherwise} \end{cases}$ Also known as the Potts model.

$f\left(n\right) = \lambda\times\mbox{min}\left(\left|n\right|, K\right)$ Truncated linear model.

$f\left(n\right) = \lambda\times\mbox{min}\left(n^2, K\right)$ Truncated quadratic model.

如何选取合适的函数和参数看起来就像黑魔法，大多数paper并没有提到应该怎样做。

Loopy Belief Propagation

因为图像中有很多像素，disparity value也有很多可能，所以很难找到MRF的精确解。LBP提供了一种方法来寻找近似解，类似的方法还有graph cut, ICM.不过LBP不保证convergence。

LBP是中用来传输信息的方法，当一个node收到了所有信息的时候，它就发给相邻node一个信息。下图展示了从x1传送到x2的过程。

x1首先需要从A,B,C,D接收到信息，然后才会给x2传输信息。x2不会返回给x1信息。准确来说信息的定义是 $msg_{i \rightarrow j}\left(l\right)$ ，表示从node i发送label l的信息给node j。换言之就是node i对node j属于label l的belief。这些信息只在隐藏变量之间传递。一个完整的信息包含所有可能的label。比如node i会给node j发送如下信息

hey node j，我认为你是label 0，概率是s0

hey node j，我认为你是label 1，概率是s1

。。。

Node i记载了所有关于node j的可能性。概率的计算取决于MRF。

LBP的第一步是初始化信息。因为node要等到所有相邻node都发送信息，这就变成了一个鸡生蛋蛋生鸡的问题，因为所有node都会等待其他node发送信息，实际上谁也没有发送任何东西。为了解决这个问题，我们把所有信息都初始化成一个常数，通常是0或1.

LBP主体算法是iterative的。如同其他iterative的算法，我们可以在一定循环次数后结束，或者到energy的变化小于一个阈值。在每个iteration，信息在MRF中传递。信息传递的次序是随机的。一旦这个过程结束，我们就可以根据每个node的belief计算这个node的label。

接下来我们一个个来看信息更新，初始化，和belief的步骤，和三个不同算法sum product，max product, min sum。

用于信息更新的sum product
$msg_{i \rightarrow j}\left( l \right) = \sum\limits_{l' \in \mbox{all labels}} \left[ \begin{array}{c} exp\left(-DataCost\left(y_i,l'\right)\right) exp\left(-SmoothnessCost\left(l,l'\right)\right) \times \\ \prod\limits_{k=\left( \begin{array}{c} \mbox{neighbours of i} \\ \mbox{except j} \end{array} \right)} msg_{k\rightarrow i}\left(l'\right) \end{array} \right]$
等式左边表示从node i发到node j，关于label l的信息。右边的y表示图像像素。这里我们遍历所有的label，在disparity map中共有15种。因为有加和，内积的计算，所以叫sum product。这个算法用于概率的计算，所以要用exp函数把data cost, smoothness cost，转换到在0到1之间的概率，这个概率越大越好。在中括号里面的是data cost, smoothness cost对于label l的所有信息的joint probability. 中括号外面的加和是对概率在变量l上的marginalization.

一个完整的信息是一个矢量
$msg_{i \rightarrow j}=\left[ \begin{array}{c} msg_{i \rightarrow j}\left(0\right)\\ msg_{i \rightarrow j}\left(1\right)\\ msg_{i \rightarrow j}\left(2\right)\\{..} \end{array} \right]$
所以对于每个label都要遍历所有可能，复杂度就是O（L^2）.

连续对概率做乘积的时候，很快就会接近0.为了避免这个情况，我们要把信息向量normalize
$msg_{i \rightarrow j} = \frac{msg_{i \rightarrow j}}{\sum\limits_{l}msg_{i \rightarrow j}\left(l\right)}$
进行初始化的时候，所有信息的概率都设为1.每个node的belief是所有信息的乘积。
$Belief \left(x_i = l\right) = exp\left(-DataCost\left(y_i, l\right)\right) \times \prod\limits_{k=\mbox{neighbours of i}} msg_{k \rightarrow i}\left(l\right)$
这是node i对于label l的belief。为了找到最合适的label，需要遍历所有label然后找到最高的belief。

用于信息更新的max product
sum product可以找到每个node的最佳label。但是总体来说并不一定是最优解。举例来说，假设有两个变量x，y

P(x,y)	x=0	x=1
y=0	0.5	0.4	P(y=0) = 0.9
y=1	0.1	0.3	P(y=1) = 0.4
	P(x=0) = 0.6	P(x=1) = 0.7

表格外边的是变量的marginal。如果用单独的marginal计算，我们会选择x=1, y = 0,得到P(x=1,y=0) = 0.4。但是最佳的解是p(x=0,y=0) = 0.5。我们最关心的是Joint probability。此类问题经常会在maximum a posteriori (MAP)求解中出现，因为这时我们想找到全局的最优解。max product在sum product的基础上做了一点点改变
$msg_{i \rightarrow j}\left( l \right) = \max\limits_{l' \in \mbox{all labels}} \left[ \begin{array}{c} exp\left(-DataCost\left(y_i,l'\right)\right) exp\left(-SmoothnessCost\left(l,l'\right)\right) \times \\ \prod\limits_{k=\left(\begin{array}{c} \mbox{neighours of i} \\ \mbox{except j} \end{array} \right)} msg_{k\rightarrow i}\left(l'\right) \end{array} \right]$
现在不再求和，而是计算marginal probability的最大值。

用来更新信息的min sum
和max sum相似，min sum也是计算每个node的max marginal，不过是在log space中。
$msg_{i \rightarrow j}\left( l \right) = \min\limits_{l' \in \mbox{all labels}} \left[ \begin{array}{c} DataCost\left(y_i,l'\right) + SmoothnessCost\left(l,l'\right) + \\ \sum\limits_{k=\mbox{neighours of i except j}} msg_{k\rightarrow i}\left(l'\right) \end{array} \right]$
这是个求解最小值的问题。在初始化的时候所有的数值都是0. 这时的belief是

$Belief \left(x_i = l\right) = DataCost\left(y_i, l\right) + \sum\limits_{k=neighbours of x_i} msg_{k \rightarrow i}\left(l\right)$

不过因为我们其实在找最小值，称它为cost更合适。

在这些方法中，min sum是最方便实现的，它没有exp函数，只有加和。如果用sum product的话，就要在exp里面加上scaling来避免underflow。eg. exp(-DataCost(…)*scaling) * exp(-SmoothnessCost(..)*scaling), scaling是 0 到1之间的数.

Sunday 11 October 2015

ROS catkin init workspace error

I encounter an error while initializing a catkin workspace. We are supposed to mkdir src in the catkin_ws folder, and then catkin_init_workspace in the src folder. Then we step out in the catkin_ws folder again and catkin_make.

Now the error occurs, setup.sh: rm: Argument list too long, this is probably because I have sourced the setup.bash. Use another terminal to catkin_make solves this problem.

CPU和GPU

这篇blog讲解了GPU的入门知识。首先为何要用GPU，因为CPU的clock time已经几乎不能再增加，否则就要消耗更多power，需要更强大的降温处理。此外，一个高端CPU只有8个核，而一个普通GPU有96个核！CPU注重的是latency，就是完成一个任务需要的时间。GPU更注重throughput，就是单位时间内完成的工作量。

CUDA是NAVIDA构建的framework，可以在CPU或者GPU上面运行。在GPU上运行的程序又叫kernel，虽然看着很像serial program，但实际上是在多个GPU核上面运行的。通常一个CUDA程序包含一下几步：给GPU分配内存，将CPU上的数据传输到GPU上，在GPU上运行程序，将结果从GPU传回CPU。

Saturday 10 October 2015

视觉里程计

最近读到了一个很好的blog，讲的简单易懂。里程计就是车里面常见的记录里程的仪表，通过车轮转过的圈数统计汽车走过的距离。视觉里程计主要指机器人的6DOF的轨迹。

我们可以用单目或者双目相机来实现视觉里程计（VO）。双目相机的好处是可以得到准确的轨迹，比如x方向动了1米，而单目相机只能得到一个不能确定尺度的轨迹，比如x方向动了1个单位。不过如果机器人很小，双目相机的baseline就会很小，对于远处的物体，双目相机实际上相当于一个单目相机了。因为现在的无人机正在变得越来越小，所以Scaramuzza组主要在研究单目相机。

双目VO

对于双目相机，我们的输入是在t和t+1时刻的图像，输出是这两个时刻间机器人旋转和平移的矩阵。具体来说，我们先计算disparity map，然后用FAST算法检测特征点，用KLT进行匹配。为了使特征均匀分布，我们在每个grid中都选取20个特征点。

在计算disparity map之前，我们要先进行一些预处理。首先要做undistortion，用来处理相机lens的distortion。然后是rectification，使得epipolar line变得平行，这样disparity map就只需要在一个方向做block matching。

在disparity map里我们可以得到这些特征点的3D位置，这样就有了两个点云。接下来，我们需要得到这些点云在真实3D世界中的坐标。在匹配中选取哪些互相配对的匹配点，然后在这些inliers中计算运动矩阵。这里的inlier是通过maximum clique problem找到的，就是寻找一个graph中的subset，在这个subset里面所有node都是互相连通的。

我们可以用Levenberg Marquardt方法来求最小值，这里的目标函数就是特征点在t和t+1时刻图像的匹配度，还有它们的3D坐标在运动轨迹中的匹配度。

一个重要的hack是当一个很大的车占据图像的大部分的时候，scene rigidity的假设不再成立。这时我们只保留那些主要运动方向是向前的运动矩阵。这个hack会极大的提高算法在KITTI上的performance。

单目VO

和双目一样，单目VO的input也是两个时刻的图像，输出是R和T。只不过这里的T不考虑尺度。首先我们还是undistort这些图像，然后用FAST检测特征点。接下来用Nister的5点算法和RANSAC来计算essential matrix。在essential matrix中得到R和t。通过其他sensor得到尺度信息来更正R和T。

在OpenCV中，我们可以用FAST函数来检测特征点。然后用KeyPoint::convert来把KeyPoint类型转换成Point2f类型，用来做特征跟踪。KLT的函数是featureTracking。在KLT中，会有一些点被丢掉。当总特征点数很少时，我们需要重新检测特征点。通过匹配的特征点，可以得到essential matrix。因为essential matrix只有5个自由度，我们可以用5点算法来计算它。

如果特征匹配是完美的，我们只需要5个匹配就可以了。但实际上肯定有误匹配。所以需要RANSAC来筛选inlier。在每个iteration，我们随机选取5个匹配，然后计算essential matrix。接着查看其它点是否满足这个essential matrix，直到得到了足够多的支持点。在OpenCV中这一切都可以通过findEssentialMat实现。得到essential matrix之后，就可以计算pose。这个函数是recoverPose。相机的轨迹就是R_pose = RR_pose，T_pose = T_pose + TR_pose。

和双目VO一样，这里也假设几乎所有点都是rigid。但是我们可能会遇到一辆巴士驶过的情况。算法可能会错误的认为运动是朝向侧面的。所以如果T不是向前的运动，我们就忽略它。

Friday 9 October 2015

Publishing images to http in ROS

If we want to view the image only, we could do it in a web browser. In ROS, there is a package named web_video_server as mentioned here, that achieves this. After the image is published, we have to rosrun web_video_server, and following the wiki, we only need to copy this in the browser: http://localhost:8080/stream_viewer?topic={ROS_TOPIC}.

For instance, if the topic is called /camera/image_color, then the address should be http://localhost:8080/stream_viewer?topic=camera/image_color, and we can see the image in the browser.