Ubuntu 16.04 下 VINS-Kidnap(Docker) 的安装和使用

[TOC]

Overview

Install Nvidia Drive and CUDA

Install Docker and Nvidia-Docker

Run MYNT-EYE-S-SDK

roslaunch mynt_eye_ros_wrapper mynteye.launch

VINS-Kidnap Docker

  • https://hub.docker.com/r/mpkuse/kusevisionkit

  • run docker image
    docker run --runtime=nvidia -it \
            --add-host `hostname`:172.17.0.1 \
            --env ROS_MASTER_URI=http://`hostname`:11311/ \
            --env ROS_IP=172.17.0.2 \
            --env CUDA_VISIBLE_DEVICES=0 \
            --hostname happy_go \
            --name happy_go  \
            mpkuse/kusevisionkit:vins-kidnap bash
    
  • modify yaml config file related to mynteye camera in
    /app_deployed/catkin_ws/src/cerebro/config/vinsfusion/mynteye
    
  • run mynteye_vinsfusion
    roslaunch cerebro mynteye_vinsfusion.launch
    
Read More

Maplab 学习笔记

[TOC]

Overview

Maplab Framework

Typical workflow

Online ROVIOLI frontend

Offline maplab console

  • a convenient console user interface and a map manager to access the maps,
  • a plug-in architecture to easily extend it with new commands and algorithms,
  • visual-inertial least-squares optimization that can be extended with additional sensors,
  • robust pose-graph relaxation using switchable constraints,
  • BRISK/FREAK-based loopclosure,
  • map summarization for lifelong mapping,
  • dense reconstruction and an interface to Voxblox.
Read More

传感器融合:基于EKF的Lidar与Radar数据融合

[TOC]

Overview

System State Vector

\[x = [p_x, p_y, v_x, v_y]^T \in \mathbb{R}^{4 \times 1}\]

State Transition & Measurement Function

State transition function:

\[x^{\prime}=f(x)+\nu\]

Measurement function:

\[z=h\left(x^{\prime}\right)+\omega\]

其中,$f(x)$ 和 $h(x)$ 非线性,通过一阶泰勒展开可被线性化为

\[f(x) \approx f(\mu)+\underbrace{\frac{\partial f(\mu)}{\partial x}}_{F_{j}}(x-\mu)\] \[h(x) \approx h(\mu)+\underbrace{\frac{\partial h(\mu)}{\partial x}}_{H_{j}}(x-\mu)\]

Kalman Filter Algorithm

State Prediction:

\[\begin{aligned} x^{\prime}_k &= F x_{k-1} + \nu_k, \quad u = 0 \\ P^{\prime}_k &= F P_{k-1} F^{T} + Q_k \end{aligned}\]

Measurement Update:

\[y_k = z_k - H x^{\prime}_k\] \[S = H P^{\prime}_k H^{T}+R\] \[K = P^{\prime}_k H^{T} S^{-1}\] \[\begin{aligned} x_k &= x^{\prime}_k + K y_k \\ P_k &= (I-K H) P^{\prime}_k \end{aligned}\]

EKF Fusion Process

Initialization

  • system state vector dimension: $n = 4$
  • timestep: $t_0$
  • system state vector: $x_0 \in \mathbb{R}^{4 \times 1}$
  • process covariance matrix: $P_0$
  • system state transition matrix: $F_0 = I_{4 \times 4}$
  • process noise covariance matrix: $Q_0 = 0_{4 \times 4}$

Prediction

当前时间戳与上一测量数据时间戳的偏移(timeoffset)

\[\Delta t = t_k - t_{k-1}\]

状态转移方程

\[x^{\prime} = f(x) + \nu\]

2D常加速度运动模型

\[\left\{\begin{array}{l} {p_{x}^{\prime}=p_{x}+v_{x} \Delta t+\frac{a_{x} \Delta t^{2}}{2}} \\ {p_{y}^{\prime}=p_{y}+v_{y} \Delta t+\frac{a_{y} \Delta t^{2}}{2}} \\ {v_{x}^{\prime}=v_{x}+a_{x} \Delta t} \\ {v_{y}^{\prime}=v_{y}+a_{y} \Delta t} \end{array}\right.\]

写成矩阵形式

\[\left(\begin{array}{c} {p_{x}^{\prime}} \\ {p_{y}^{\prime}} \\ {v_{x}^{\prime}} \\ {v_{y}^{\prime}} \end{array}\right) = \left(\begin{array}{cccc} {1} & {0} & {\Delta t} & {0} \\ {0} & {1} & {0} & {\Delta t} \\ {0} & {0} & {1} & {0} \\ {0} & {0} & {0} & {1} \end{array}\right) \left(\begin{array}{l} {p_{x}} \\ {p_{y}} \\ {v_{x}} \\ {v_{y}} \end{array}\right) + \left(\begin{array}{c} {\frac{a_{x} \Delta t^{2}}{2}} \\ {\frac{a_{y} \Delta t^{2}}{2}} \\ {a_{x} \Delta t} \\ {a_{y} \Delta t} \end{array}\right)\]

抽象简写为

\[x^{\prime}=Fx + v\]

其中

\[v \sim N(0, Q)\]

State Transition Matrix

\[F = \left(\begin{array}{cccc} {1} & {0} & {\Delta t} & {0} \\ {0} & {1} & {0} & {\Delta t} \\ {0} & {0} & {1} & {0} \\ {0} & {0} & {0} & {1} \end{array}\right)\]

Process Noise Covariance Matrix

由上式

\[\nu= \left(\begin{array}{c} {\nu_{p x}} \\ {\nu_{p y}} \\ {\nu_{v x}} \\ {\nu_{v y}} \end{array}\right) = \left(\begin{array}{c} {\frac{a_{x} \Delta t^{2}}{2}} \\ {\frac{a_{y} \Delta t^{2}}{2}} \\ {a_{x} \Delta t} \\ {a_{y} \Delta t} \end{array}\right) = \underbrace{ \left(\begin{array}{cc} {\frac{\Delta t^{2}}{2}} & {0} \\ {0} & {\frac{\Delta t^{2}}{2}} \\ {\Delta t} & {0} \\ {0} & {\Delta t}\end{array}\right)}_{G} \underbrace{\left(\begin{array}{l}{a_{x}} \\ {a_{y}}\end{array}\right)}_{a} = Ga\]

根据 $v \sim N(0, Q)$

\[Q=E\left[\nu \nu^{T}\right]=E\left[G a a^{T} G^{T}\right]\]

因为 $G$ 不包含随机变量,将其移出

\[Q = G E\left[a a^{T}\right] G^{T} = G \left(\begin{array}{cc} {\sigma_{a x}^{2}} & {\sigma_{a x y}} \\ {\sigma_{a x y}} & {\sigma_{a y}^{2}} \end{array}\right) G^{T} = G Q_{\nu} G^{T}\]

$a_x$ 和 $a_y$ 假设不相关,则

\[Q_{\nu} = \left(\begin{array}{cc} {\sigma_{a x}^{2}} & {\sigma_{a x y}} \\ {\sigma_{a x y}} & {\sigma_{a y}^{2}} \end{array}\right) = \left(\begin{array}{cc} {\sigma_{a x}^{2}} & {0} \\ {0} & {\sigma_{a y}^{2}} \end{array}\right)\]

最终

\[Q = G Q_{\nu} G^{T} = \left(\begin{array}{cccc} {\frac{\Delta t^{4}}{4} \sigma_{a x}^{2}} & {0} & {\frac{\Delta t^{3}}{2} \sigma_{a x}^{2}} & {0} \\ {0} & {\frac{\Delta t^{4}}{4} \sigma_{a y}^{2}} & {0} & {\frac{\Delta t^{3}}{2} \sigma_{a y}^{2}} \\ {\frac{\Delta t^{3}}{2} \sigma_{a x}^{2}} & {0} & {\Delta t^{2} \sigma_{a x}^{2}} & {0} \\ {0} & {\frac{\Delta t^{3}}{2} \sigma_{a y}^{2}} & {0} & {\Delta t^{2} \sigma_{a y}^{2}} \end{array}\right)\]

Measurement Update

测量方程

\[z = h(x^{\prime}) + \omega\]

Lidar Measurements

Lidar测量方程

\[z = \left(\begin{array}{c}{p_{x}} \\ {p_{y}}\end{array}\right) = \left(\begin{array}{cccc} {1} & {0} & {0} & {0} \\ {0} & {1} & {0} & {0} \end{array}\right) \left(\begin{array}{c} {p_{x}^{\prime}} \\ {p_{y}^{\prime}} \\ {v_{x}^{\prime}} \\ {v_{y}^{\prime}} \end{array}\right)\]

简写为

\[z = H x^{\prime} + \omega \quad s.t. \quad \omega \sim N(0,R)\]

Measurement Jacobian Matrix

\[H = \left(\begin{array}{cccc} {1} & {0} & {0} & {0} \\ {0} & {1} & {0} & {0} \end{array}\right)\]

Lidar Measurement Noise Covariance Matrix

\[R = E\left[\omega \omega^{T}\right] = \left(\begin{array}{cc} {\sigma_{p x}^{2}} & {0} \\ {0} & {\sigma_{p y}^{2}} \end{array}\right)\]

Radar Measurements

Radar测量方程

\[z = \left(\begin{array}{l} {\rho} \\ {\varphi} \\ {\dot{\rho}} \end{array}\right) = h(x^{\prime}) = \left(\begin{array}{c} {\sqrt{p_{x}^{\prime 2}+p_{y}^{\prime 2}}} \\ {\arctan \left(p_{y}^{\prime} / p_{x}^{\prime}\right)} \\ {\frac{p_{x}^{\prime} v_{x}^{\prime}+p_{y} v_{y}^{\prime}}{\sqrt{p_{x}^{\prime 2}+p_{y}^{\prime 2}}}} \end{array}\right)\]
  • range $\rho$: the radial distance from the origin to our pedestrian
  • bearing $\varphi$: the angle between the ray and x direction
  • range rate $\dot{\rho}$: known as Doppler or radial velocity is the velocity along this ray

通过一阶泰勒展开将 $h(x^{\prime})$ 在 $\mu = 0$ 处线性化

\[h(x^{\prime}) = H x^{\prime}\]

简写为

\[z = H x^{\prime} + \omega \quad s.t. \quad \omega \sim N(0,R)\]

其中,Measurement Jacobian Matrix

\[H = \left[\begin{array}{llll} {\frac{\partial \rho}{\partial p_{x}}} & {\frac{\partial \rho}{\partial p_{y}}} & {\frac{\partial \rho}{\partial v_{x}}} & {\frac{\partial \rho}{\partial v_{y}}} \\ {\frac{\partial \varphi}{\partial p_{x}}} & {\frac{\partial \varphi}{\partial p_{y}}} & {\frac{\partial \varphi}{\partial v_{x}}} & {\frac{\partial \varphi}{\partial v_{y}}} \\ {\frac{\partial \dot{\rho}}{\partial p_{x}}} & {\frac{\partial \rho}{\partial p_{y}}} & {\frac{\partial \dot{\rho}}{\partial v_{x}}} & {\frac{\partial \rho}{\partial v_{y}}} \end{array}\right] = \left[\begin{array}{cccc} {\frac{p_{x}}{\sqrt{p_{x}^{2}+p_{y}^{2}}}} & {\frac{p_{y}}{\sqrt{p_{x}^{2}+p_{y}^{2}}}} & {0} & {0} \\ {-\frac{p_{y}}{p_{x}^{2}+p_{y}^{2}}} & {\frac{p_{x}}{p_{x}^{2}+p_{y}^{2}}} & {0} & {0} \\ {\frac{p_{y}\left(v_{x} p_{y}-v_{y} p_{x}\right)}{\left(p_{x}^{2}+p_{y}^{2}\right)^{3 / 2}}} & {\frac{p_{x}\left(v_{y} p_{x}-v_{x} p_{y}\right)}{\left(p_{x}^{2}+p_{y}^{2}\right)^{3 / 2}}} & {\frac{p_{x}}{\sqrt{p_{x}^{2}+p_{y}^{2}}}} & {\frac{p_{y}}{\sqrt{p_{x}^{2}+p_{y}^{2}}}} \end{array}\right]\]

Radar Measurement Noise Covariance Matrix

\[R = \left(\begin{array}{ccc} {\sigma_{\rho}^{2}} & {0} & {0} \\ {0} & {\sigma_{\varphi}^{2}} & {0} \\ {0} & {0} & {\sigma_{\dot{\rho}}^{2}} \end{array}\right)\]

R 表示了测量值的不确定度,一般由传感器的厂家提供

Reference

  • Self-Driving Car ND - Sensor Fusion - Extended Kalman Filters
Read More

非线性最小二乘问题解决方法

[TOC]

最小二乘问题

最小二乘问题(Least Squares Problem),即

\[\min_{\mathbf{x}} \mathbf{F}(\mathbf{x}) = \frac{1}{2} {\|\mathbf{f}(\mathbf{x})\|}^2_2\]

其中,$\mathbf{F}(\mathbf{x})$ 为 损失函数,$\mathbf{f}(\mathbf{x})$ 为 残差函数,定义为

\[\mathbf{F}(\mathbf{x}) = \frac{1}{2} \sum_{i=1}^{m}\left(f_{i}(\mathbf{x})\right)^{2} = \frac{1}{2} \mathbf{f}^{\top}(\mathbf{x}) \mathbf{f}(\mathbf{x}) = \frac{1}{2} {\|\mathbf{f}(\mathbf{x})\|}^2_2\]
  • 若 残差函数 $\mathbf{f}(\mathbf{x})$ 为 线性函数,则这个问题即为 线性最小二乘问题
  • 若 残差函数 $\mathbf{f}(\mathbf{x})$ 为 非线性函数,则这个问题即为 非线性最小二乘问题

线性最小二乘问题

\[\min_{x} \|Ax-b\|\]
  • 正规方程组
\[A^TA x = A^T b\]
  • QR分解
\[\min_{x} \|Ax-b\| = \min_{x} \|QRx-b\| = \min_{x} \|Rx - Q^T b\|\]
  • 乔列斯基分解

非线性最小二乘问题

对于不方便求解的最小二乘问题,我们用 迭代 的方式,从一个初始值出发,不断地更新当前的优化变量,使目标函数下降。这让求解 导函数为零 的问题变成了一个不断 寻找下降增量 $\Delta \mathbf{x}$ 的问题。

  1. 给定某个初始值 $\mathbf{x}_0$
  2. 对于第k次迭代,寻找一个增量 $\Delta \mathbf{x}_k$,使得 ${|f(\mathbf{x}_k + \Delta \mathbf{x}_k)|}_2^2$ 达到极小值
  3. 若 $\Delta \mathbf{x}_k$ 足够小,则停止
  4. 否则,令 $\mathbf{x}_{k+1} = \mathbf{x}_k + \Delta \mathbf{x}_k$,返回第2步

实际上,非线性优化的所有迭代求解方案,都需要用户提供一个 良好的初始值

损失函数 线性化

对 $\mathbf{F}(\mathbf{x})$ 进行线性化,对其进行 二阶泰勒展开,得

\[\mathbf{F}(\mathbf{x} + \Delta \mathbf{x}) = \mathbf{F}(\mathbf{x}) + \Delta \mathbf{x}^{\top} \mathbf{F}^{\prime}(\mathbf{x}) + \frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{F}^{\prime\prime}(\mathbf{x}) \Delta \mathbf{x} + O \left(\|\Delta \mathbf{x}\|^{3}\right)\]

\[\begin{aligned} \mathbf{g} &= \mathbf{F}^{\prime}(\mathbf{x}) = \mathbf{f}^{\prime}(\mathbf{x})^\top \mathbf{f}(\mathbf{x}) \\ \mathbf{H} &= \mathbf{F}^{\prime\prime}(\mathbf{x}) \end{aligned}\]

忽略高阶余项,损失函数变成了二次函数,可以轻易得到如下性质:

  • 如果在点 $\mathbf{x}_s$ 处有 $\mathbf{g}_s$ 为 0(对应 $\mathbf{H}_s$),则称这个点为 驻点(stationary point)
    • 如果 $\mathbf{H}_s$ 是正定矩阵,则 $\mathbf{x}_s$ 为 局部最小值点(local minimizer)
    • 如果 $\mathbf{H}_s$ 是负定矩阵,则 $\mathbf{x}_s$ 为 局部最大值点(local maximizer)
    • 如果 $\mathbf{H}_s$ 是不定矩阵,则 $\mathbf{x}_s$ 为 鞍点(saddle point)

对于泰勒展开,

  • 保留 一阶项,求解方法为 一阶梯度法
  • 保留 二阶项,求解方法为 二阶梯度法

一阶梯度法(最速下降法)

保留 泰勒展开 一阶项

\[\mathbf{F}(\mathbf{x} + \Delta \mathbf{x}) = \mathbf{F}(\mathbf{x}) + \Delta \mathbf{x}^{\top} \mathbf{g}\]

梯度的负方向为最速下降方向,增量方程 为

\[\Delta \mathbf{x}_{\mathrm{sd}} = - \mathbf{F}^{\prime}(\mathbf{x}) = - \mathbf{g}\]
  • 适用于迭代的开始阶段
  • 缺点:最优值附近震荡,收敛慢

二阶梯度法(牛顿法)

保留 泰勒展开 二阶项

\[\mathbf{F}(\mathbf{x} + \Delta \mathbf{x}) = \mathbf{F}(\mathbf{x}) + \Delta \mathbf{x}^{\top} \mathbf{g} + \frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{H} \Delta \mathbf{x}\]

求 $\mathbf{F}(\mathbf{x} + \Delta \mathbf{x})$ 关于 $\Delta \mathbf{x}$ 的导数并令其为零,即

\[\mathbf{F}^{\prime}(\mathbf{x} + \Delta \mathbf{x}) = 0 \quad \Longrightarrow \quad \mathbf{g} + \mathbf{H} \Delta \mathbf{x} = 0 \quad \Longrightarrow \quad \mathbf{H} \Delta \mathbf{x} = - \mathbf{g}\]

则,增量方程 为

\[\Delta \mathbf{x}_{\mathrm{n}} = -\mathbf{H}^{-1} \mathbf{g}\]
  • 适用于最优值附近
  • 缺点:二阶导矩阵 $\mathbf{H}$ 计算复杂

残差函数 线性化

对 $\mathbf{f}(\mathbf{x})$ 进行线性化,对其进行 一阶泰勒展开,得

\[\mathbf{f}(\mathbf{x}+\Delta \mathbf{x}) \approx \ell(\Delta \mathbf{x}) \equiv \mathbf{f}(\mathbf{x}) + \mathbf{J}(\mathbf{x}) \Delta \mathbf{x} \quad \text{with} \quad \mathbf{J}(\mathbf{x}) = \mathbf{f}^{\prime}(\mathbf{x})\]

代入 损失函数

\[\begin{aligned} \mathbf{F}(\mathbf{x}+\Delta \mathbf{x}) \approx \mathbf{L}(\Delta \mathbf{x}) & \equiv \frac{1}{2} \ell(\Delta \mathbf{x})^{\top} \ell(\Delta \mathbf{x}) \\ &=\frac{1}{2} \mathbf{f}^{\top} \mathbf{f} + \Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{f}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{J} \Delta \mathbf{x} \\ &=\mathbf{F}(\mathbf{x}) + \Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{f}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{J} \Delta \mathbf{x} \end{aligned}\]

并且

\[\begin{aligned} \mathbf{g} &= \mathbf{F}^{\prime}(\mathbf{x}) = \mathbf{J}^\top \mathbf{f} \\ \mathbf{H} &= \mathbf{F}^{\prime\prime}(\mathbf{x}) \approx \mathbf{J}^{\top} \mathbf{J} \end{aligned}\]

高斯-牛顿法(G-N)

求 $\mathbf{L}(\Delta \mathbf{x})$ 关于 $\Delta \mathbf{x}$ 的导数并令其为零,即

\[\mathbf{L}^{\prime}(\Delta \mathbf{x}) = 0 \quad \Longrightarrow \quad \mathbf{J}^{\top} \mathbf{f} + \mathbf{J}^{\top} \mathbf{J} \Delta \mathbf{x} = 0\]

则,增量方程 为

\[\left(\mathbf{J}^{\top} \mathbf{J}\right) \Delta \mathbf{x}_{\mathrm{gn}}=-\mathbf{J}^{\top} \mathbf{f}\]

简写为

\[\mathbf{H} \Delta \mathbf{x}_{\mathrm{gn}} = -\mathbf{g} \quad \text{with} \quad \mathbf{H} \approx \mathbf{J}^{\top} \mathbf{J}\]
  • $\mathbf{J}^{\top} \mathbf{J}$ 可能为奇异矩阵或病态的情况,此时增量的稳定性较差,导致算法不收敛

列文伯格-马夸尔特法(L-M)

\[\left(\mathbf{J}^{\top} \mathbf{J}+\mu \mathbf{I}\right) \Delta \mathbf{x}_{\operatorname{lm}}=-\mathbf{J}^{\top} \mathbf{f} \quad \text { with } \mu \geq 0\]

鲁棒核函数(M估计)

  • Huber
  • Cauchy
Read More

RTAB-Map 学习笔记

version: 0.11.13-kinetic

RTAB-Map (Real-Time Appearance-Based Mapping) is a RGB-D, Stereo and Lidar Graph-Based SLAM approach based on an incremental appearance-based loop closure detector.


[TOC]

概述

RTAB-Map (Real-Time Appearance-Based Mapping) is a RGB-D Graph-Based SLAM approach based on an incremental appearance-based loop closure detector.

The loop closure detector uses a bag-of-words approach to determinate how likely a new image comes from a previous location or a new location. When a loop closure hypothesis is accepted, a new constraint is added to the map’s graph, then a graph optimizer minimizes the errors in the map.

A memory management approach is used to limit the number of locations used for loop closure detection and graph optimization, so that real-time constraints on large-scale environnements are always respected.

RTAB-Map can be used alone with a handheld Kinect or stereo camera for 6DoF RGB-D mapping, or on a robot equipped with a laser rangefinder for 3DoF mapping.

RTAB-Map框架图

Sensors

  • Stereo Camera: Bumblebee2, ZED camera, etc.
  • RGB-D Camera: Kinect, RealSense, etc.

Front-end

Back-end

Add Key-Frame to Graph

Loop Closure Detection

Graph Optimization

Map(2D/3D) Generation

Demo Robot Mapping

  • Command
roslaunch rtabmap_ros demo_robot_mapping.launch rtabmapviz:=false rviz:=true
  • Node Graph

demo_robot_mapping_graph

Nodes

/rtabmap/rtabmap

  • 流程图
digraph g {
    rankdir = LR;

    subgraph cluster0{
        label="Parameters";
        parameters_;
    }

    subgraph cluster1{
        label="MapsManager";
        initMaps;
    }

    subgraph cluster2{
        label="CoreWrapper Nodelet";
        onInit;
        Callback;
    }

    subgraph cluster3{
        label="RTABMap Corelib";
        initRTABMap;
        process;
    }

    subgraph cluster4{
        label="Memory";
        newMemory;
        initMemory;
        loadDataFromDb;
    }

    subgraph cluster5{
        label="DBDriver";
        createDB;
    }
    subgraph cluster6{
        node [shape=box, size="20,20"];
        {
              Feature2D_create;
              newVWDictionary;
              Registration_create;
              newRegistrationIcp;
              newOccupancyGrid;
        }
    }

    CoreNode -> onInit [label="nodelet::load()"];

    onInit -> parameters_;
    onInit -> initMaps;
    onInit -> initRTABMap;
    onInit -> Callback;

    Callback -> process;

    initRTABMap -> newMemory;
    initRTABMap -> initMemory;

    newMemory -> Feature2D_create [lhead=cluster6];

    initMemory -> createDB;
    initMemory -> loadDataFromDb;
}

/points_xyzrgb

  • 流程图
digraph g{
    rankdir = LR;
    rviz -> points_xyzrgb [label="launch nodelet"];
}

Code Analization

CameraModel

\[FOV_{Horizontal} = \frac{W_{img}/2}{f_x}\]

Problem

  • Transform::interpolate
Read More

^