A Study in Vision

Monday, 30 November 2015

Multiple definition problem

I often see multiple definition problem when including the same header file in several places. For instance, if I have a common.h, and put all the function body plus declaration in it, then there is a problem.

It turns out that I have to put function declaration in the common.h, and put the body in common.cpp, which addresses this problem.

Sunday, 22 November 2015

一步步学习SFM

有本很好的学习OpenCV的教材，mastering opencv with practical computer vision projects, 在第四章讲解了SFM。SFM解决的是从运动相机拍摄的图片中还原运动轨迹和场景几何结构的问题。

从图像中计算相机运动

先从已知条件开始，我们知道两张图片，由相机在同一场景中运动得到。书中用的图片在这里。要恢复相机运动，我们要用到fundamental matrix F和essential matrix E。F可以从findFundamentalMat函数中得到。为了得到E，我们需要先校准相机，得到相机矩阵K。然后E=K.t*F*K.E是个3x3的矩阵，描述了同一点x在两个图像中的关系,x'Ex=0.

用特征描述算子匹配特征点

为了得到同一点在两张图像中的位置x和x'，我们需要找到匹配的点。我们可以用SURF来寻找特征点和描述算子，然后用brute force来寻找匹配。不过这些匹配中很多是错误的，所以需要过滤掉这些错误的匹配。opencv的brute force方法已经有了cross check filtering，只有当一个特征点对无论从图像A到图像B还是图像B到图像A都是最佳匹配的时候，它才会被保留。除此之外，在计算fundamental matrix的时候，我们只保留inlier。

用光流匹配特征点

光流可以用calcOpticalFlowPyrLK函数得到。从图像A的特征点计算光流到图像B的到的新特征点，并不一定会和图像B中检测到的特征点完全匹配。我们需要用KNN来把它们匹配在一起。另一个在SFM中常用的减小误差的方法是ratio test，它处理图像A中某特征点匹配到图像B中两个点的情况，这时我们不知道两个点哪个才是真正的匹配。ratio test会去掉图像B中两个相距很近的特征点，因为这时两个点都有可能是匹配点，我们完全无法得知该选择哪一个。判断方式是两个匹配点距离图像A中特征点距离的比值不能太大。

处理完距离太近的点，我们还要处理重复的点。这里使用了STL里面的set函数，可以给元素排序。关于set的find的说明在这里。注意原来书中用的是指针vector<DMatch>* matches，在自己写的函数中需要替换成std::vector<cv::DMatch>。我理解的处理重复的点是这样的。因为现在是把跟踪的点和图像B中实际的点做匹配，同一个跟踪点可能被匹配多次，这时候就通过if (found_in_cur_points.find(_m.trainIdx) == found_in_cur_points.end())来判断是不是之前出现过这个跟踪点。如果是新的跟踪点，我们还要使用原来图像A中的特征点的id，而不是图像A中跟踪成功的特征点的id。

使用光流的好处是比较快，能产生更多特征点，得到dense reconstruction。光流通常只用很基本的特征，比如特征点周围的patch，SURF则会考虑更多高级的特征。

恢复相机运动轨迹

相机运动可以用矩阵P表示，包含旋转R和平移t。一个很重要的关系是x=PX，x是图像中的2D点，X是空间中的3D点。这个公式描述了图像点和空间点之间的关系。我们对E进行SVD，然后乘以矩阵W。SVD把E分解为了旋转和平移两个部分。我们现在只得到了一个运动矩阵，它是相对于第一个相机而言的。我们假定第一个相机没有旋转和平移。所以通过x=PX恢复的3D点的坐标原点就是第一个相机。

很多时候F矩阵是错误的，从而运动矩阵也是错误的。为了判断运动的正确性，我们可以检查旋转矩阵，因为它的determinant必须是1或者-1.

重建场景

从2D特征点匹配和运动矩阵，我们已经得到了x=PX, x'=PX'。x和x'是图像A和图像B中的匹配点，X是空间中的3D点。用所有的特征点，我们可以得到一组公式来求解X。假定X=(x,y,z,1)t，可得AX=B。从而可以得到2D匹配点对所对应的3D点，这个过程叫做三角定位。这里注意2D点也是在homogeneous coordinates下，所以在x,y后面加个1.同时这些坐标需要normalized，就是说在使用之前要先乘以相机矩阵K。

因为只有一个相机，那么问题来了，恢复出来的几何结构单位到底是mm还是m呢？这个我们无从知晓，所以恢复的结构是up to scale的。两个相机之间的运动向量的单位可以是mm也可以是m，它只是一个unit scale。我们只能说两个相机之间的距离是一个单位长度，而不知道真实的物理距离。当有多个运动的时候，每两个相机运动的单位长度都是不同的，它们之间并不存在一个统一的单位。

为了得到更准确的重建，我们可以从得到的3D点反投影到图像中得到2D点，然后比较这些新的点和原来图像中的特征点的距离。如果距离很大，那么我们的三角定位得到的3D点很可能是错误的，我们就需要去掉这个点。这些距离的平均值可以用来作为一个全局的评判，如果平均值过大，那么P矩阵是错误的，可能因为错误的E矩阵或者错误的特征点匹配导致。

从多幅图像中重建场景

因为不同的图相对给出的重建有着不同的尺度，多幅图像重建并不是重复两幅图像重建过程那么简单。我们可以用PNP来根据已知空间点来求得新的相机位置。另一种方法是三角定位更多点然后观察它们是不是符合已经重建的场景，这时我们可以用ICP来求得相机位置。这里我们使用opencv中的solvePnP函数来实现第一种方法。

我们首先要找到一个baseline来确定场景的尺度。这个可以通过第一幅和第二幅图像得到相机运动矩阵，然后再三角定位获得空间点的位置。对每个空间的3D点，我们还需要记录它原来的2D点的位置，然后可以在新的图像中匹配得到对应点。这样我们就能获取新的相机位置。注意这里我们用solvePnPRansac而不是solvePnP，因为前者能更好的应对outliers.获得新相机位置之后，我们可以继续三角定位新的3D点。

我们需要新建一个结构来记录3D点和对应的2D点
struct CloudPoint {
cv::Point3d pt;
std::vector<int> index_of_2d_origin;
};
index_of_2d_origin 记录了3D点在图像中所有特征点的index.

重建优化

SFM中一个很重要的过程就是优化重建的场景，又叫做BA。我们把所有已知的数据合并成一个整体。这些数据包括空间点的位置和相机的位置。优化的判断依据是3D点反投影到图像得到2D点之后与原来真实的2D点之间的残差。

我们用SSBA来实现优化过程，具体由CommonInternalsMetricBundleOptimizer函数用来实现优化。这个函数需要知道相机参数，3D点云，每个3D点对应的2D点，和每个相机的位置。

用PCL实现3D点云可视化

首先把3D点转变为PCL的数据结构，也可以把RGB信息包括进去，然后用SOR来过滤outlier。

总结

这个SFM只是最基本的算法，有很多可以改进的地方。比如使用更加精确的三角定位方法，例如N view三角定位。对于SFM，还有很多其他的资源，如libMV，Bundler, VisualSfM。如果需要实时的进行场景重建，我们就要用到SLAM。

code的结构

在这里有上面的code。
在main函数中，第一步是RecoverDepthFromImages.
OnlyMatchFeatures我们首先用光流或者特征点匹配来匹配相邻图像的特征点。
PruneMatchesBasedOnF算出F矩阵之后去掉不符合的特征点
GetBaseLineTriangulation在两个图像中找到初始点云。用homography找到inlier最小的两个图像进行初始化，用的函数是FindCameraMatrices。先找到F矩阵，在计算出E矩阵，从E中回复R和T，判断四个解答中哪一个是对的。如果F矩阵的inlier足够多，那么认为它是合适的。接下来是TriangulatePointsBetweenViews，先得到两张图像中的对应点，再三角定位匹配点。然后我们去掉那些反投影残差很高的点。我们还要判断哪些点已经在之前的点云中出现过，避免重复添加相同的点。

得到初始的匹配之后，我们以此运动为基准处理以后的图像。用Find2D3DCorrespondences找到新的图像中特征点在已有点云中的对应特征点, 然后用FindPoseEstimation找到新的相机的运动轨迹。根据已知2D-3D点计算运动用的是solvePnPRansac。

得到几个相机的运动轨迹和点云后，我们可以用ssba来优化。关于ssba的解释在这里。要build这个library，新建一个3rdparty文件夹，然后在里面放入ssba的source code。根据这里提供的方法，再新建一个build文件夹，就可以进行build。在cmake，make之后，build里面会生成两个.a文件，就是static library。要在cmake里面link这个library，一个简单粗暴的方式就是直接把.a的full path，包括.a文件名，写到cmake的target_link_libraries.另外还要在include_directories中加入3rdparty/SSBA-3.0/，这样才能找到那些header file。

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不是向前的运动，我们就忽略它。