Ubuntu16.04下ARM远程调试环境搭建

  1. 安装 交叉编译工具 g++-arm-linux-gnueabi

    sudo apt install g++-arm-linux-gnueabi
    
  2. 下载gdb源码:http://ftp.gnu.org/gnu/gdb/,我下载的版本为 gdb-7.11.1.tar.gz,解压

  3. 安装 交叉编译工具 arm-linux-gdb

    cd gdb-7.11.1
    ./configure --target=arm-linux --prefix=/usr/local/arm-gdb
    make
    sudo make install
    

    导出环境变量:export PATH=/usr/local/arm-gdb/bin:$PATH

  4. 编译 gdbserver

    cd gdb-7.11.1/gdb/gdbserver
    ./configure --target=arm-linux --host=arm-linux
    make CC=arm-linux-gnueabi-gcc
    

    make 过程中出现 sys/reg.h: No such file or directory 错误时,可注释掉相应源文件中该头文件的包含,再 make 即可

    将生成的 gdbserver 拷贝至 ARM开发板

  5. 远程调试

    • arm端: gdbserver <pc-ip>:<port> <program>
    • pc 端: arm-linux-gdb,然后 target remote <arm-ip>:<port>
Read More

Raspberry Pi

Read More

Ubuntu 16.04 下 RGBDSLAM-v2 的安装和使用

简介

RGBD-SLAM-v2 is a state-of-the-art SLAM system for RGB-D cameras, e.g., the Microsoft Kinect. You can use it to create highly accurate 3D point clouds or OctoMaps.

RGBDSLAMv2 is based on the ROS project, OpenCV, PCL, OctoMap, SiftGPU and more.

rgbdslam_v2_cg

!!! Important

本人已建立了 RGBDSLAM-v2 的改版 cggos/rgbdslam_v2_cg,可直接跳转到该项目下,阅读 README.md 进行项目的安装,本文以下部分不必阅读。

!!! Important

运行环境

  • 平台
    • Ubuntu 16.04 + ROS kinetic
  • 数据源
    • RGB-D dataset: rgbd_dataset_freiburg1_xyz.bag
    • RGB-D camera: Realsense Camera ZR300

安装依赖

  • sudo apt-get install libglew1.5-dev libdevil-dev libsuitesparse-dev
  • rgbdslam作者的g2o
    git clone https://github.com/felixendres/g2o.git
    
  • pcl-1.8.0
    wget https://github.com/PointCloudLibrary/pcl/archive/pcl-1.8.0.tar.gz
    

    pcl-1.8.0 的 CMakeLists.txt 中加入 C++11 支持:

    SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
    

    依赖库g2o和pcl不兼容,可能导致 “required process[rgbdslam-2] has died”……Iinitiating down 问题,程序挂掉。

编译 rgbdslam_v2

  • 下载 rgbdslam_v2
    wget -q http://github.com/felixendres/rgbdslam_v2/archive/kinetic.zip
    
  • rosdep
    rosdep update
    rosdep install rgbdslam
    
  • 修改 CMakeLists.txt
    • 第6行加入: add_compile_options(-std=c++11)
    • set(USE_SIFT_GPU 1 CACHE BOOL "build with support for siftgpu") 改为 set(USE_SIFT_GPU 0 CACHE BOOL "build with support for siftgpu")
    • find_package(PCL 1.7 REQUIRED COMPONENTS common io) 改为 find_package(PCL 1.8 REQUIRED COMPONENTS common io)
  • 编译
    catkin_make
    

运行

RGB-D 数据集

RGB-D 数据集 使用 rgbd_dataset_freiburg1_xyz.bag

  • 修改 rgbdslam_v2 的 launch文件 rgbdslam.launch 中的 图像topic
    <param name="config/topic_image_mono"              value="/camera/rgb/image_color"/>
    <param name="config/topic_image_depth"             value="/camera/depth/image"/>
    
  • 运行
    roscore & rosbag play rgbd_dataset_freiburg1_xyz.bag
    roslaunch rgbdslam rgbdslam.launch
    
  • 结果
    rgbdslam_v2_rgbd_dataset_freiburg1_xyz.png

RGB-D 相机

RGB-D 相机 使用 Realsense Camera ZR300

  • 修改 rgbdslam_v2 的 launch文件 rgbdslam.launch 中的 图像topic
    <param name="config/topic_image_mono"              value="/camera/rgb/image_color"/>
    <param name="config/topic_image_depth"             value="/camera/depth_registered/sw_registered/image_rect_raw"/>
    
  • 运行
    roslaunch realsense_camera zr300_nodelet_rgbd.launch
    roslaunch rgbdslam rgbdslam.launch
    
  • 结果
    rgbdslam_v2_realsense_zr300.png

参考

Read More

StereoScan(libVISO2) 学习笔记

Overview

LIBVISO2 (Library for Visual Odometry 2) is a very fast cross-platfrom (Linux, Windows) C++ library with MATLAB wrappers for computing the 6 DOF motion of a moving mono/stereo camera.

  • The stereo version is based on minimizing the reprojection error of sparse feature matches and is rather general (no motion model or setup restrictions except that the input images must be rectified and calibration parameters are known).

  • The monocular version is still very experimental and uses the 8-point algorithm for fundamental matrix estimation. It further assumes that the camera is moving at a known and fixed height over ground (for estimating the scale). Due to the 8 correspondences needed for the 8-point algorithm, many more RANSAC samples need to be drawn, which makes the monocular algorithm slower than the stereo algorithm, for which 3 correspondences are sufficent to estimate parameters.

Paper & Code

  • papers: StereoScan: Dense 3D Reconstruction in Real-time
  • code: cggos/viso2_cg

Scen Flow

Feature Matching

Feature Detection

Blob/Corner Detector:

  • filter the input images with 5×5 blob and corner masks
  • employ non-maximum- and non-minimum-suppression on the filtered images, resulting in feature candidates

Feature Descriptor:

The descriptor concatenates Sobel filter responses using the layout given in below:

simply compare 11 × 11 block windows of horizontal and vertical Sobel filter responses to each other by using the sum of absolute differences (SAD) error metric; to speed-up matching, we quantize the Sobel responses to 8 bits and sum the differences over a sparse set of 16 locations instead of summing over the whole block window; since the SAD of 16 bytes can be computed efficiently using a single SSE instruction we only need two calls (for horizontal + vertical Sobel responses) in order to evaluate this error metric

  • 16 locations within 11 × 11 block window
  • 32 bytes per descriptor
  • Efficient Sum-of-Absolute-Differences (SAD) via SIMD

Circle Match

Starting from all feature candidates in the current left image, we find the best match in the previous left image within a M × M search window, next in the previous right image, the current right image and last in the current left image again.

A ’circle match’ gets accepted, if the last feature coincides with the first feature.

  • Detect interest points using non-maximum-suppression
  • Match 4 images in a ’space-time’ circle
  • make use of the epipolar constraint using an error tolerance of 1 pixel when matching between the left and right images
  • Accept if last feature coincides with first feature

Fast Feature Matching

  • 1st: Match a sparse set of interest points within each class
  • Build statistics over likely displacements within each bin
  • Use this statistics for speeding up 2nd matching stage
  • Rejection outliers (Delaunay triangulation)

Egomotion Estimation

Features Bucketing

keeps only max_features per bucket, where the domain is split into buckets of size (bucket_width,bucket_height).

  • reduce the number of features (in practice we retain between 200 and 500 features)
  • spread them uniformly over the image domain

3D Points Calcuation

  • disparity
\[d = max(u_l - u_r, 0.001)\]
  • Stereo Baseline
\[B = stereomodel.baseline\]
  • 3d points
\[\begin{aligned} \begin{cases} Z = \frac{f \cdot B}{d} \\ X = (u-c_u) \cdot \frac{B}{d} \\ Y = (v-c_v) \cdot \frac{B}{d} \end{cases} \end{aligned}\]

Projection Model

Assuming squared pixels and zero skew, the reprojection into the current image is given by

\[\begin{bmatrix} u_c \\ v_c \\ 1 \end{bmatrix} = \begin{bmatrix} f & 0 & c_u \\ 0 & f & c_v \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} (\mathbf{R}(r) \quad \mathbf{t}) \left( \begin{array}{cccc} X_p \\ Y_p \\ Z_p \end{array} \right) - \left( \begin{array}{cccc} s \\ 0 \\ 0 \end{array} \right) \end{bmatrix}\]
  • homogeneous image coordinates $(u_c \quad v_c \quad 1 )^T$
  • focal length $f$
  • principal point $(c_u, c_v )$
  • rotation matrix $\mathbf{R}(r) = \mathbf{R}_x (r_x ) \mathbf{R}_y(r_y) \mathbf{R}_z(r_z)$
  • translation vector $\mathbf{t} = (t_x \quad t_y \quad t_z )$
  • 3d point coordinates $\mathbf{P} = (X_p \quad Y_p \quad Z_p)$
  • shift $s = 0$ (left image), $s = baseline$ (right image)

Minimize Reprojection Errors

Compute the camera motion by minimizing the sum of reprojection errors iteratly by using Gauss-Newton optimization and RANSAC.

  • reprojection errors
\[\begin{aligned} \min_{\beta} {\| r(\beta) \|}^2 =& \sum_{i=1}^N {\| p_i^{(l)} - p_i'^{(l)} \|}^2 + {\| p_i^{(r)} - p_i'^{(r)} \|}^2\\ =& \sum_{i=1}^N {\| p_i^{(l)} - \pi^{(l)}(P_i; R,t) \|}^2 + {\| p_i^{(r)} - \pi^{(r)}(P_i; R,t) \|}^2 \\ =& \sum_{i=1}^3 (u_i^l-u^l)^2 + (v_i^l-v^l)^2 + (u_i^r-u^r)^2 + (v_i^r-v^r)^2 \end{aligned}\]
  • errors vector
\[\begin{aligned} r(\beta) =& (r_0, r_1, r_2, \cdots, r_{N}) \\ =& ( r_{u_{0l}}, r_{v_{0l}}, r_{u_{0r}}, r_{v_{0r}}, r_{u_{1l}}, r_{v_{1l}}, r_{u_{1r}}, r_{v_{1r}}, r_{u_{2l}}, r_{v_{2l}}, r_{u_{2r}}, r_{v_{2r}} ) \in \mathbb{R}^{4 \times N} \quad \text{s.t.} \quad N = 3 \end{aligned}\]
  • Optimization parameters vector
\[\beta = (r_x, r_y, r_z, t_x, t_y, t_z) \in \mathbb{R}^{6 \times 1}\]
  • Jocobians Matrix
\[J_{(4 \times N) \times 6} = \frac{\partial{r(\beta)}}{\partial{\beta}} \quad \text{s.t.} \quad N = 3\]
  • Parameter iteration
\[\beta_i = \beta_{i-1} - (J^TJ)^{-1} \cdot J^T r(\beta_{i-1})\]

Jacobian Matrix Compute

\[\begin{aligned} J_{2 \times 6} =& \frac{\partial(r_{u_{0l}}, r_{v_{0l}})}{\partial \beta} \\ =& J_1 \cdot J_2 \cdot J_3 \\ =& \frac{ \partial{ (p^{(l)} - p'^{(l)}) } }{ \partial{ p'^{(l)} } } \cdot \frac{ \partial{ p'^{(l)} } }{ \partial{P'^{(l)}} } \cdot \frac{ \partial{ P'^{(l)} } }{ \partial \beta} \\ =& I \cdot \begin{bmatrix} \frac{f}{Z'} & 0 & -\frac{f}{Z'^2} X' \\ 0 & \frac{f}{Z'} & -\frac{f}{Z'^2} Y' \end{bmatrix} \cdot \begin{bmatrix} \frac{\partial \mathbf R}{\partial r_x} X_p & \frac{\partial \mathbf R}{\partial r_y} X_p & \frac{\partial \mathbf R}{\partial r_z} X_p & 1 & 0 & 0 \\ \frac{\partial \mathbf R}{\partial r_x} Y_p & \frac{\partial \mathbf R}{\partial r_y} Y_p & \frac{\partial \mathbf R}{\partial r_z} Y_p & 0 & 1 & 0 \\ \frac{\partial \mathbf R}{\partial r_x} Z_p & \frac{\partial \mathbf R}{\partial r_y} Z_p & \frac{\partial \mathbf R}{\partial r_z} Z_p & 0 & 0 & 1 \end{bmatrix} \end{aligned}\]

Kalman Filter

refine the obtained velocity estimates by means of a Kalman filter(constant acceleration model)

Stereo Matching

Stereo matching stage takes the output of the feature matching stage and builds a disparity map for each frame. In this paper, the disparity map is built with a free library called ELAS. ELAS is a novel approach to binocular stereo for fast matching of high resolution imagery.

3D Reconstruction

The simplest method to point-based 3d reconstructions map all valid pixels to 3d and projects them into a common coordinate system using the egomotion matrix.

Read More

双目立体视觉三维重建

[TOC]

Overview

双目立体视觉的整体流程包括:

  • 图像采集
  • 双目标定
  • 双目矫正
  • 立体匹配
  • 三维重建

1. 图像采集

双目相机采集 左右目图像

2. 双目标定

通过 双目标定工具 对双目相机进行标定,得到如下结果参数:

内参 外参
相机矩阵 $K_1, K_2$ 旋转矩阵 $R$
畸变系数 $D_1, D_2$ 平移向量 $t$

《Learning OpenCV》中对于 Translation 和 Rotation 的图示是这样的:

示例代码:

cv::Matx33d K1, K2, R;
cv::Vec3d T;
cv::Vec4d D1, D2;

int flag = 0;
flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
flag |= cv::fisheye::CALIB_CHECK_COND;
flag |= cv::fisheye::CALIB_FIX_SKEW;

cv::fisheye::stereoCalibrate(
        obj_points_, img_points_l_, img_points_r_,
        K1, D1, K2, D2, img_size_, R, T,
        flag, cv::TermCriteria(3, 12, 0));

3. 双目矫正

双目矫正 主要包括两方面:畸变矫正立体矫正

利用 OpenCV的函数,主要分为

  • stereoRectify
  • initUndistortRectifyMap
  • remap

stereoRectify

根据双目标定的结果 $K_1, K_2, D_1, D_2, R, t$,利用 OpenCV函数 stereoRectify,计算得到如下参数

  • 左目 矫正矩阵(旋转矩阵) $R_1$ (3x3)
  • 右目 矫正矩阵(旋转矩阵) $R_2$ (3x3)
  • 左目 投影矩阵 $P_1$ (3x4)
  • 右目 投影矩阵 $P_2$ (3x4)
  • disparity-to-depth 映射矩阵 $Q$ (4x4)

其中,

左右目投影矩阵(horizontal stereo, ${c_x}_1’={c_x}_2’$ if CV_CALIB_ZERO_DISPARITY is set)

\[P_1 = \begin{bmatrix} f' & 0 & {c_x}_1' & 0 \\ 0 & f' & c_y' & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix}\] \[P_2 = \begin{bmatrix} f' & 0 & {c_x}_2' & t_x' \cdot f' \\ 0 & f' & c_y' & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix}\]

where

\[t_x' = -B\]

disparity-to-depth 映射矩阵

\[Q = \begin{bmatrix} 1 & 0 & 0 & -{c_x}_1' \\ 0 & 1 & 0 & -c_y' \\ 0 & 0 & 0 & f' \\ 0 & 0 & -\frac{1}{t_x'} & \frac{ {c_x}_1'-{c_x}_2'}{t_x'} \end{bmatrix}\]

通过 $P_2$ 可计算出 基线 长度:

\[\begin{aligned} \text{baseline} = B = - t_x' = - \frac{ {P_2}^{(03)} }{f'} \end{aligned}\]

示例代码:

cv::Mat R1, R2, P1, P2, Q;
cv::fisheye::stereoRectify(
        K1, D1, K2, D2, img_size_, R, T,
        R1, R2, P1, P2, Q,
        CV_CALIB_ZERO_DISPARITY, img_size_, 0.0, 1.1);

CameraInfo DKRP

参考:sensor_msgs/CameraInfo Message

  • D: distortion parameters.
    • For “plumb_bob”, the 5 parameters are: (k1, k2, t1, t2, k3)
  • K: Intrinsic camera matrix for the raw (distorted) images.
    • Projects 3D points in the camera coordinate frame to 2D pixel coordinates using the focal lengths (fx, fy) and principal point (cx, cy). \(\mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}\)
  • R: Rectification matrix (stereo cameras only).
    • A rotation matrix aligning the camera coordinate system to the ideal stereo image plane so that epipolar lines in both stereo images are parallel.
    • For monocular cameras $\mathbf{R} = \mathbf{I}$
  • P: Projection/camera matrix.
    • For monocular cameras \(\mathbf{P} = \begin{bmatrix} f_x & 0 & c_x & 0 \\ 0 & f_y & c_y & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix}\)
    • For a stereo pair, the fourth column [Tx Ty 0]’ is related to the position of the optical center of the second camera in the first camera’s frame. We assume Tz = 0 so both cameras are in the same stereo image plane.
      • The first camera \(\mathbf{P} = \begin{bmatrix} f_x' & 0 & c_x' & 0 \\ 0 & f_y' & c_y' & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix}\)
      • The second camera \(\mathbf{P} = \begin{bmatrix} f_x' & 0 & c_x' & -f_x' \cdot B \\ 0 & f_y' & c_y' & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix}\)
    • Given a 3D point $[X Y Z]’$, the projection $(x, y)$ of the point onto the rectified image is given by:
    \[\begin{bmatrix} u \\ v \\ w \end{bmatrix} = \mathbf{P} \cdot \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} , \quad \begin{cases} x = \frac{u}{w} \\ y = \frac{v}{w} \end{cases}\]

initUndistortRectifyMap

左右目 分别利用 OpenCV函数 initUndistortRectifyMap 计算 the undistortion and rectification transformation map,得到

  • 左目map: $map^l_1, map^l_2$
  • 右目map: $map^r_1, map^r_2$

示例代码:

cv::fisheye::initUndistortRectifyMap(K1, D1, R1, P1, img_size, CV_16SC2, rect_map_[0][0], rect_map_[0][1]);
cv::fisheye::initUndistortRectifyMap(K2, D2, R2, P2, img_size, CV_16SC2, rect_map_[1][0], rect_map_[1][1]);

Remap

左右目 分别利用 OpenCV函数 remap 并根据 左右目map 对左右目图像进行 去畸变 和 立体矫正,得到 左右目矫正图像

示例代码:

cv::remap(img_l, img_rect_l, rect_map_[0][0], rect_map_[0][1], cv::INTER_LINEAR);
cv::remap(img_r, img_rect_r, rect_map_[1][0], rect_map_[1][1], cv::INTER_LINEAR);

4. 立体匹配

根据双目矫正图像,通过 BM或SGM等立体匹配算法 对其进行立体匹配,计算 视差图

视差计算

通过 OpenCV函数 stereoBM (block matching algorithm),生成 视差图(Disparity Map) (CV_16S or CV_32F)

disparity map from stereoBM of OpenCV : It has the same size as the input images. When disptype == CV_16S, the map is a 16-bit signed single-channel image, containing disparity values scaled by 16. To get the true disparity values from such fixed-point representation, you will need to divide each disp element by 16. If disptype == CV_32F, the disparity map will already contain the real disparity values on output.

So if you’ve chosen disptype = CV_16S during computation, you can access a pixel at pixel-position (X,Y) by: short pixVal = disparity.at<short>(Y,X);, while the disparity value is float disparity = pixVal / 16.0f;; if you’ve chosen disptype = CV_32F during computation, you can access the disparity directly: float disparity = disparity.at<float>(Y,X);

5. 三维重建

(1)算法1:根据视差图,利用 $f’$ 和 $B$ 通过几何关系计算 深度值,并利用相机内参计算 三维坐标

根据上图相似三角形关系,得

\[\frac{Z}{B} = \frac{Z-f}{B-d_w} \quad \Longrightarrow \quad Z = \frac{Bf}{d_w}\]

其中,$f$ 和 $d_w$ 分别为 成像平面的焦距和视差,单位均为 物理单位,将其转换为 像素单位,上式写为

\[Z = \frac{B f'}{d_p}\]

其中,

\[d_p = (O_r - u_r) + (u_l - O_l) = (u_l - u_r) + (O_r - O_l)\]

最终,深度计算公式如下,通过遍历图像可生成 深度图

\[Z = \text{depth} = \frac{B \cdot f'}{d_p} \quad \text{with} \quad d_p = \text{disp}(u,v) + ({c_x}_2' - {c_x}_1')\]

根据 小孔成像模型,已知 $Z$ 和 相机内参 可计算出 三维点坐标,从而可生成 三维点云

\[\begin{aligned} \begin{cases} Z = \text{depth} = \frac{f' \cdot B}{d_p} \\ X = \frac{u-{c_x}_1'}{f'} \cdot Z \\ Y = \frac{v-{c_y}'}{f'} \cdot Z \end{cases} \end{aligned} \quad \text{or} \quad \begin{aligned} \begin{cases} \text{bd} = \frac{B}{d_p}\\ Z = \text{depth} = f' \cdot \text{bd} \\ X = (u-{c_x}_1') \cdot \text{bd} \\ Y = (v-{c_y}') \cdot \text{bd} \end{cases} \end{aligned}\]

其中,$\text{disp}(u,v)$ 代表 视差图 坐标值

(2)算法2:根据视差图,利用 $Q$ 矩阵 计算 三维点坐标(reprojectImageTo3D

\[\begin{bmatrix} X' \\ Y' \\ Z' \\ W \end{bmatrix} = Q \cdot \begin{bmatrix} u \\ v \\ \text{disp}(u,v) \\ 1 \end{bmatrix}\]

最终,三维点坐标为

\[\begin{bmatrix} X \\ Y \\ Z \end{bmatrix} = \begin{bmatrix} \frac{X'}{W} \\[2ex] \frac{Y'}{W} \\[2ex] \frac{Z'}{W} \end{bmatrix}\]

深度图 图像类型

  • 单位meter –> 32FC1
  • 单位millimeter –> 16UC1

总结

Read More

^