SLAM高翔十四讲（八）第八讲 视觉里程计（直接法）

已于 2023-07-22 10:11:57 修改
阅读量304 收藏 2
点赞数
分类专栏： 高翔十四讲 SLAM 文章标签： c++ linux
于 2023-07-19 16:04:14 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/shendaiyan/article/details/131805786
版权

文章目录

一、直接法的引出

2.1 特征点法的缺点

  1. 关键点的提取有描述子的计算耗时；
  2. 只使用特征点，丢弃了大部分可能有用的信息；
  3. 相机会运动到特征缺失的地方，这里找不到足够的特征点；
  4. 只能用于构建稀疏特征点（稀疏地图）。

2.2 光流法

  1. 保留关键点，把匹配描述子替换成光流跟踪，估计相机运动时仍使用对极几何、PnP或ICP算法，需要角点。
  2. 光流是一种描述像素随时间在图像间运动的方法。
  3. 根据使用像素的多少，光流法可以分成：
    1）稀疏光流（计算部分像素运动）：以Lucas-Kanade光流为代表，即L-K光流
    2）稠密光流（计算所有像素运动）：以Horn-Schunck光流为代表，即H-S光流
  4. 灰度不变假设：同一空间点的像素灰度值在不同图像是不变的。
  5. L-K光流常用来跟踪角点的运动，L-K光流的具体算法过程：P208-210

2.3 直接法

  1. 在光流中，我们首先确定特征点的位置，然后用光流跟踪确定相机运动，但是很难保证全局最优，因此引入直接法。
  2. 直接法：无需计算关键点和描述子，根据图像的像素灰度信息同时估计相机运动和点的投影，随机选点，无需角点。
  3. 假设P是一个位置已知的空间点，根据P的来源，直接法分成：
    1）稀疏直接法：来自稀疏关键点；
    2）半稠密直接法：来自部分像素，只考虑带有梯度的像素点，舍弃像素梯度不明显的地方；
    3）稠密直接法：来自所有像素。
  4. 直接法的思路是，根据空间点P在当前相机的像素下，通过位姿估计值寻找另一个相机的像素位置，因为存在光度误差，我们需要进行优化。
  5. 求解直接法最后等价于求解一个优化问题，计算出雅克比矩阵，然后通过使用g2o或者Ceres这些优化库，也可以手写高斯牛顿法或者列文伯格-马夸尔特法，来计算增量，迭代求解。
  6. 直接法的具体算法过程：P219-220，包括计算雅克比矩阵、增量等。

二、光流法实践

2.1 理论

我们在第一张图片提取角点，然后用光流跟踪他们在第二张图片的位置。
采用3种方法：
1）OpenCV自带的LK光流；
2）基于高斯牛顿法的光流：因为光流可以看做一个优化问题，通过最小化灰度误差估计最优的像素偏移；
3）多层光流：使用图像金字塔。

2.2 CMakeList.txt

cmake_minimum_required(VERSION 2.8)
project(ch8)

set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_FLAGS "-std=c++14 -msse4")

find_package(OpenCV REQUIRED)
find_package(Sophus REQUIRED)
find_package(Pangolin REQUIRED)

include_directories(
        ${OpenCV_INCLUDE_DIRS}
        ${G2O_INCLUDE_DIRS}
        ${Sophus_INCLUDE_DIRS}
        "/usr/include/eigen3/"
        ${Pangolin_INCLUDE_DIRS}
)

add_executable(optical_flow optical_flow.cpp)
target_link_libraries(optical_flow ${OpenCV_LIBS} ${FMT_LIBRARIES} fmt)

add_executable(direct_method direct_method.cpp)
target_link_libraries(direct_method ${OpenCV_LIBS} ${Pangolin_LIBRARIES} ${FMT_LIBRARIES} fmt)

2.3 代码展示

//
// Created by Xiang on 2017/12/19.
//

#include <opencv2/opencv.hpp>
#include <string>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <opencv2/imgproc/types_c.h>

using namespace std;
using namespace cv;

string file_1 = "/home/robot/桌面/slambook2-master/ch8/LK1.png";  // first image
string file_2 = "/home/robot/桌面/slambook2-master/ch8/LK2.png";  // second image

/// 光流跟踪器和接口Optical flow tracker and interface
class OpticalFlowTracker {
public:
    OpticalFlowTracker(
        const Mat &img1_,
        const Mat &img2_,
        const vector<KeyPoint> &kp1_,
        vector<KeyPoint> &kp2_,
        vector<bool> &success_,
        bool inverse_ = true, bool has_initial_ = false) :
        img1(img1_), img2(img2_), kp1(kp1_), kp2(kp2_), success(success_), inverse(inverse_),
        has_initial(has_initial_) {}

    void calculateOpticalFlow(const Range &range);

private:
    const Mat &img1;
    const Mat &img2;
    const vector<KeyPoint> &kp1;
    vector<KeyPoint> &kp2;
    vector<bool> &success;
    bool inverse = true;
    bool has_initial = false;
};

/**单层光流
 * single level optical flow
 * @param [in] img1 the first image
 * @param [in] img2 the second image
 * @param [in] kp1 keypoints in img1
 * @param [in|out] kp2 keypoints in img2, if empty, use initial guess in kp1
 * @param [out] success true if a keypoint is tracked successfully
 * @param [in] inverse use inverse formulation?
 */
void OpticalFlowSingleLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint> &kp1,
    vector<KeyPoint> &kp2,
    vector<bool> &success,
    bool inverse = false,
    bool has_initial_guess = false
);

/**多层光流，金字塔的比例默认设置为2
 * multi level optical flow, scale of pyramid is set to 2 by default
 * the image pyramid will be create inside the function
 * @param [in] img1 the first pyramid
 * @param [in] img2 the second pyramid
 * @param [in] kp1 keypoints in img1
 * @param [out] kp2 keypoints in img2
 * @param [out] success true if a keypoint is tracked successfully
 * @param [in] inverse set true to enable inverse formulation
 */
void OpticalFlowMultiLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint> &kp1,
    vector<KeyPoint> &kp2,
    vector<bool> &success,
    bool inverse = false
);

/**从参考图像中获取灰度值（双线性插值）
 * get a gray scale value from reference image (bi-linear interpolated)
 * @param img
 * @param x
 * @param y
 * @return the interpolated value of this pixel
 */

inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    // boundary check
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols - 1) x = img.cols - 2;
    if (y >= img.rows - 1) y = img.rows - 2;
    
    float xx = x - floor(x);
    float yy = y - floor(y);
    int x_a1 = std::min(img.cols - 1, int(x) + 1);
    int y_a1 = std::min(img.rows - 1, int(y) + 1);
    
    return (1 - xx) * (1 - yy) * img.at<uchar>(y, x)
    + xx * (1 - yy) * img.at<uchar>(y, x_a1)
    + (1 - xx) * yy * img.at<uchar>(y_a1, x)
    + xx * yy * img.at<uchar>(y_a1, x_a1);
}

int main(int argc, char **argv) {

    // 1. 读取图像
    Mat img1 = imread(file_1, 0);
    Mat img2 = imread(file_2, 0);

    // 2. 提取第一幅图的关键点，在此使用GFTT算法
    vector<KeyPoint> kp1;
    Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypoints
    detector->detect(img1, kp1);

    // 3. 跟踪第二张图像中的这些关键点
    // 1）首先在验证图片中使用单级LK
    vector<KeyPoint> kp2_single;
    vector<bool> success_single;
    OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single);

    // 2）然后测试多级LK
    vector<KeyPoint> kp2_multi;
    vector<bool> success_multi;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    OpticalFlowMultiLevel(img1, img2, kp1, kp2_multi, success_multi, true);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optical flow by gauss-newton: " << time_used.count() << endl;

    // 3）使用opencv的光流进行验证
    vector<Point2f> pt1, pt2;
    for (auto &kp: kp1) pt1.push_back(kp.pt);
    vector<uchar> status;
    vector<float> error;
    t1 = chrono::steady_clock::now();
    cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);
    t2 = chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optical flow by opencv: " << time_used.count() << endl;

    // 4. 绘制这些函数的差异
    Mat img2_single;
    cv::cvtColor(img2, img2_single, CV_GRAY2BGR);
    for (int i = 0; i < kp2_single.size(); i++) {
        if (success_single[i]) {
            cv::circle(img2_single, kp2_single[i].pt, 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_single, kp1[i].pt, kp2_single[i].pt, cv::Scalar(0, 250, 0));
        }
    }

    Mat img2_multi;
    cv::cvtColor(img2, img2_multi, CV_GRAY2BGR);
    for (int i = 0; i < kp2_multi.size(); i++) {
        if (success_multi[i]) {
            cv::circle(img2_multi, kp2_multi[i].pt, 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_multi, kp1[i].pt, kp2_multi[i].pt, cv::Scalar(0, 250, 0));
        }
    }

    Mat img2_CV;
    cv::cvtColor(img2, img2_CV, CV_GRAY2BGR);
    for (int i = 0; i < pt2.size(); i++) {
        if (status[i]) {
            cv::circle(img2_CV, pt2[i], 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_CV, pt1[i], pt2[i], cv::Scalar(0, 250, 0));
        }
    }

    cv::imshow("tracked single level", img2_single);
    cv::imshow("tracked multi level", img2_multi);
    cv::imshow("tracked by opencv", img2_CV);
    cv::waitKey(0);

    return 0;
}
// 单层光流
void OpticalFlowSingleLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint> &kp1,
    vector<KeyPoint> &kp2,
    vector<bool> &success,
    bool inverse, bool has_initial) {
    kp2.resize(kp1.size());//将第二个图像的特征点数量设置和第一个一样
    success.resize(kp1.size());
    OpticalFlowTracker tracker(img1, img2, kp1, kp2, success, inverse, has_initial);
    parallel_for_(Range(0, kp1.size()),
                  std::bind(&OpticalFlowTracker::calculateOpticalFlow, &tracker, placeholders::_1));//计算光流
}
// 计算光流
void OpticalFlowTracker::calculateOpticalFlow(const Range &range) {
    // parameters
    int half_patch_size = 4;
    int iterations = 10;
    for (size_t i = range.start; i < range.end; i++) {
        auto kp = kp1[i];
        // 1. 设置dx dy
        double dx = 0, dy = 0; 
        if (has_initial) {
            dx = kp2[i].pt.x - kp.pt.x;
            dy = kp2[i].pt.y - kp.pt.y;
        }

        double cost = 0, lastCost = 0;
        bool succ = true; // indicate if this point succeeded

        // 2. 高斯牛顿法
        Eigen::Matrix2d H = Eigen::Matrix2d::Zero();    // hessian
        Eigen::Vector2d b = Eigen::Vector2d::Zero();    // bias
        Eigen::Vector2d J;  // jacobian
        for (int iter = 0; iter < iterations; iter++) { //迭代10次
            if (inverse == false) {
                H = Eigen::Matrix2d::Zero();
                b = Eigen::Vector2d::Zero();
            } else {
                // only reset b
                b = Eigen::Vector2d::Zero();
            }

            cost = 0;

            // 计算 cost 和 jacobian
            for (int x = -half_patch_size; x < half_patch_size; x++)
                for (int y = -half_patch_size; y < half_patch_size; y++) {
                    double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y) -
                                   GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;  // Jacobian
                    if (inverse == false) {
                        J = -1.0 * Eigen::Vector2d(
                            0.5 * (GetPixelValue(img2, kp.pt.x + dx + x + 1, kp.pt.y + dy + y) -
                                   GetPixelValue(img2, kp.pt.x + dx + x - 1, kp.pt.y + dy + y)),
                            0.5 * (GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y + 1) -
                                   GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
                        );
                    } else if (iter == 0) {
                        // 在逆模式中，J在所有迭代中保持不变
                        // 注意，当dx，dy更新时，这个J不会改变，所以我们可以存储它，只计算错误
                        // in inverse mode, J keeps same for all iterations
                        // NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
                        J = -1.0 * Eigen::Vector2d(
                            0.5 * (GetPixelValue(img1, kp.pt.x + x + 1, kp.pt.y + y) -
                                   GetPixelValue(img1, kp.pt.x + x - 1, kp.pt.y + y)),
                            0.5 * (GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y + 1) -
                                   GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y - 1))
                        );
                    }
                    // 计算 H, b and set cost;
                    b += -error * J;
                    cost += error * error;
                    if (inverse == false || iter == 0) {
                        // also update H
                        H += J * J.transpose();
                    }
                }

            // 得到 H * 更新量 = b 的 更新量
            Eigen::Vector2d update = H.ldlt().solve(b);

            if (std::isnan(update[0])) {
                // sometimes occurred when we have a black or white patch and H is irreversible
                cout << "update is nan" << endl;
                succ = false;
                break;
            }

            if (iter > 0 && cost > lastCost) {
                break;
            }

            // 更新 dx, dy
            dx += update[0];
            dy += update[1];
            lastCost = cost;
            succ = true;

            if (update.norm() < 1e-2) {
                // converge
                break;
            }
        }

        success[i] = succ; // 如果这个点成功就设置成功，反之

        // 设置 kp2
        kp2[i].pt = kp.pt + Point2f(dx, dy);
    }
}
// 多层光流
void OpticalFlowMultiLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint> &kp1,
    vector<KeyPoint> &kp2,
    vector<bool> &success,
    bool inverse) {

    // parameters
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    // 构建金字塔
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    vector<Mat> pyr1, pyr2; // image pyramids
    for (int i = 0; i < pyramids; i++) {
        if (i == 0) { // 第一个放入最底层
            pyr1.push_back(img1);
            pyr2.push_back(img2);
        } else {   // 其余要进行缩放
            Mat img1_pyr, img2_pyr;
            cv::resize(pyr1[i - 1], img1_pyr,
                       cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
            cv::resize(pyr2[i - 1], img2_pyr,
                       cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
            pyr1.push_back(img1_pyr);
            pyr2.push_back(img2_pyr);
        }
    }
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "build pyramid time: " << time_used.count() << endl;

    // 特征点缩放
    vector<KeyPoint> kp1_pyr, kp2_pyr;
    for (auto &kp:kp1) {
        auto kp_top = kp;
        kp_top.pt *= scales[pyramids - 1];
        kp1_pyr.push_back(kp_top);
        kp2_pyr.push_back(kp_top);
    }

    // 对每层调用单层直接法
    for (int level = pyramids - 1; level >= 0; level--) {
        // from coarse to fine
        success.clear();
        t1 = chrono::steady_clock::now();
        OpticalFlowSingleLevel(pyr1[level], pyr2[level], kp1_pyr, kp2_pyr, success, inverse, true);
        t2 = chrono::steady_clock::now();
        auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
        cout << "track pyr " << level << " cost time: " << time_used.count() << endl;

        if (level > 0) {
            for (auto &kp: kp1_pyr)
                kp.pt /= pyramid_scale;
            for (auto &kp: kp2_pyr)
                kp.pt /= pyramid_scale;
        }
    }

    for (auto &kp: kp2_pyr)
        kp2.push_back(kp);
}

2.4 结果展示

build pyramid time: 0.001775
track pyr 3 cost time: 0.000317644
track pyr 2 cost time: 0.00025403
track pyr 1 cost time: 0.000238879
track pyr 0 cost time: 0.000227848
optical flow by gauss-newton: 0.00309121
optical flow by opencv: 0.00202506

在这里插入图片描述

三、直接法实践

3.1 理论

求解直接法最后等价于求解一个优化问题，计算出雅克比矩阵，然后通过使用g2o或者Ceres这些优化库，也可以手写高斯牛顿法或者列文伯格-马夸尔特法，来计算增量，迭代求解。
这里我们使用手写高斯牛顿法，演示稀疏的直接法，分成两种方式：
1）单层直接法
2）多层直接法：图像金字塔，每层调用单层直接法

3.2 CMakeList.txt

3.3 代码展示

#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>
#include <pangolin/pangolin.h>
#include <opencv2/imgproc/types_c.h>

using namespace std;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// 相机内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// baseline
double baseline = 0.573;
// paths
string left_file = "/home/robot/桌面/slambook2-master/ch8/left.png";
string disparity_file = "/home/robot/桌面/slambook2-master/ch8/disparity.png";
boost::format fmt_others("/home/robot/桌面/slambook2-master/ch8/%06d.png");    // other files

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

/// class for accumulator jacobians in parallel
class JacobianAccumulator {
public:
    JacobianAccumulator(
        const cv::Mat &img1_,
        const cv::Mat &img2_,
        const VecVector2d &px_ref_,
        const vector<double> depth_ref_,
        Sophus::SE3d &T21_) :
        img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_) {
        projection = VecVector2d(px_ref.size(), Eigen::Vector2d(0, 0));
    }

    /// accumulate jacobians in a range
    void accumulate_jacobian(const cv::Range &range);

    /// get hessian matrix
    Matrix6d hessian() const { return H; }

    /// get bias
    Vector6d bias() const { return b; }

    /// get total cost
    double cost_func() const { return cost; }

    /// get projected points
    VecVector2d projected_points() const { return projection; }

    /// reset h, b, cost to zero
    void reset() {
        H = Matrix6d::Zero();
        b = Vector6d::Zero();
        cost = 0;
    }

private:
    const cv::Mat &img1;
    const cv::Mat &img2;
    const VecVector2d &px_ref;
    const vector<double> depth_ref;
    Sophus::SE3d &T21;
    VecVector2d projection; // projected points

    std::mutex hessian_mutex;
    Matrix6d H = Matrix6d::Zero();
    Vector6d b = Vector6d::Zero();
    double cost = 0;
};

/**
 * 使用多层直接法进行位姿态估计
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationMultiLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21
);

/**
 * 使用单层直接法进行位姿态估计
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21
);

// 双线性插值bilinear interpolation
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    // 边界检查boundary check
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols) x = img.cols - 1;
    if (y >= img.rows) y = img.rows - 1;
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
        (1 - xx) * (1 - yy) * data[0] +
        xx * (1 - yy) * data[1] +
        (1 - xx) * yy * data[img.step] +
        xx * yy * data[img.step + 1]
    );
}

int main(int argc, char **argv) {
    // 1.图像读取
    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);//rgb-d相机采集的数据

    // 2. 让我们随机选取第一张图像中的像素，并在第一张图像的帧中生成一些3d点
    cv::RNG rng;
    int nPoints = 2000;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // 3. 在ref中生成像素并加载深度数据
    for (int i = 0; i < nPoints; i++) {
        int x = rng.uniform(boarder, left_img.cols - boarder);  // 不要拾取靠近边界的像素
        int y = rng.uniform(boarder, left_img.rows - boarder);  // 不要拾取靠近边界的像素
        int disparity = disparity_img.at<uchar>(y, x); //从rgb-d相机采集的图像中提取某个像素点距离
        // 根据这个式子计算深度，本人不太懂原理
        double depth = fx * baseline / disparity; // 深度=焦距*基线/距离 
        depth_ref.push_back(depth);               // 深度
        pixels_ref.push_back(Eigen::Vector2d(x, y));// 像素
    }

    // 4. 估计01-05.png的位姿
    Sophus::SE3d T_cur_ref;

    for (int i = 1; i < 6; i++) {  // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        // 1）单层直接法
        DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
        // 2）多层直接法
        // DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
    }
    return 0;
}

void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21) {

    const int iterations = 10;
    double cost = 0, lastCost = 0;
    auto t1 = chrono::steady_clock::now();
    JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);

    // 1. 计算出H b 更新量，并更新到估计值上
    for (int iter = 0; iter < iterations; iter++) {
        // 1） 构建Hdx=b
        jaco_accu.reset();
        cv::parallel_for_(cv::Range(0, px_ref.size()),
                          std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1)); 
        // 上式中的accumulate_jacobian用来计算雅可比
        Matrix6d H = jaco_accu.hessian();
        Vector6d b = jaco_accu.bias();

        // 2） 解决更新并进行估计
        Vector6d update = H.ldlt().solve(b);;  //求出dx
        T21 = Sophus::SE3d::exp(update) * T21; // 左乘更新
        cost = jaco_accu.cost_func();

        if (std::isnan(update[0])) {
            // 当有黑色或白色斑块并且H是不可逆的时候，有时发生
            cout << "update is nan" << endl;
            break;
        }
        if (iter > 0 && cost > lastCost) {
            cout << "cost increased: " << cost << ", " << lastCost << endl;
            break;
        }
        if (update.norm() < 1e-3) {
            // converge
            break;
        }

        lastCost = cost;
        cout << "iteration: " << iter << ", cost: " << cost << endl;
    }

    // 2. 输出位姿和时间
    cout << "T21 = \n" << T21.matrix() << endl;
    auto t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "单层直接法所用时间: " << time_used.count() << endl;

    // 3. 在此处绘制投影像素
    cv::Mat img2_show;
    cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
    VecVector2d projection = jaco_accu.projected_points();
    for (size_t i = 0; i < px_ref.size(); ++i) {
        auto p_ref = px_ref[i];
        auto p_cur = projection[i];
        if (p_cur[0] > 0 && p_cur[1] > 0) {
            cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
                     cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("current", img2_show);
    cv::waitKey();
}

void JacobianAccumulator::accumulate_jacobian(const cv::Range &range) {
    // 这里的误差和雅可比计算公式按照P220
    // parameters
    const int half_patch_size = 1;
    int cnt_good = 0;
    Matrix6d hessian = Matrix6d::Zero();
    Vector6d bias = Vector6d::Zero();
    double cost_tmp = 0;

    for (size_t i = range.start; i < range.end; i++) {

        // 1. 计算第二幅图像中的投影P219的p1和p2
        // point_ref表示第一张图片像素坐标P1
        Eigen::Vector3d point_ref =
            depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
        // point_cur表示第二张图片的投影的下像素坐标P2
        Eigen::Vector3d point_cur = T21 * point_ref;
        if (point_cur[2] < 0)   // 深度<0无效
            continue;

        // 2. 求出雅可比矩阵一面的一些参数u v
        float u = fx * point_cur[0] / point_cur[2] + cx, v = fy * point_cur[1] / point_cur[2] + cy;
        if (u < half_patch_size || u > img2.cols - half_patch_size || v < half_patch_size ||
            v > img2.rows - half_patch_size)
            continue;

        // 求出雅可比矩阵一面的一些参数 X Y Z Z^2 1/Z 1/Z^2
        projection[i] = Eigen::Vector2d(u, v);
        double X = point_cur[0], Y = point_cur[1], Z = point_cur[2],
            Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;
        cnt_good++;

        // 3. 计算误差和雅可比
        for (int x = -half_patch_size; x <= half_patch_size; x++)
            for (int y = -half_patch_size; y <= half_patch_size; y++) {
                
                // 1）e=I1(p1)-I2(P2) P219 公式8.11
                double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) -
                               GetPixelValue(img2, u + x, v + y);
                // 2）求出J
                Matrix26d J_pixel_xi;
                Eigen::Vector2d J_img_pixel;

                J_pixel_xi(0, 0) = fx * Z_inv;
                J_pixel_xi(0, 1) = 0;
                J_pixel_xi(0, 2) = -fx * X * Z2_inv;
                J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;
                J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;
                J_pixel_xi(0, 5) = -fx * Y * Z_inv;

                J_pixel_xi(1, 0) = 0;
                J_pixel_xi(1, 1) = fy * Z_inv;
                J_pixel_xi(1, 2) = -fy * Y * Z2_inv;
                J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;
                J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;
                J_pixel_xi(1, 5) = fy * X * Z_inv;

                J_img_pixel = Eigen::Vector2d(
                    0.5 * (GetPixelValue(img2, u + 1 + x, v + y) - GetPixelValue(img2, u - 1 + x, v + y)),
                    0.5 * (GetPixelValue(img2, u + x, v + 1 + y) - GetPixelValue(img2, u + x, v - 1 + y))
                );

                // total jacobian
                Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();

                // 3）利用高斯牛顿求出H * dx = b中的H b
                hessian += J * J.transpose();
                bias += -error * J;
                cost_tmp += error * error;
            }
    }

    if (cnt_good) {
        // set hessian, bias and cost
        unique_lock<mutex> lck(hessian_mutex);
        H += hessian;
        b += bias;
        cost += cost_tmp / cnt_good;
    }
}

void DirectPoseEstimationMultiLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21) {

    // 1. 定义参数
    int pyramids = 4; // 金字塔层数
    double pyramid_scale = 0.5; //每层图片缩放倍率
    double scales[] = {1.0, 0.5, 0.25, 0.125}; //每层内参缩放倍率

    // 2. 构建金字塔
    vector<cv::Mat> pyr1, pyr2; // image pyramids
    for (int i = 0; i < pyramids; i++) {
        if (i == 0) { //第一张直接放到金字塔底部
            pyr1.push_back(img1);
            pyr2.push_back(img2);
        } else {  //其余每层在上层的基础上缩放
            cv::Mat img1_pyr, img2_pyr;
            cv::resize(pyr1[i - 1], img1_pyr,
                       cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
            cv::resize(pyr2[i - 1], img2_pyr,
                       cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
            pyr1.push_back(img1_pyr);
            pyr2.push_back(img2_pyr);
        }
    }

    // 3.  开始执行
    double fxG = fx, fyG = fy, cxG = cx, cyG = cy;  // 备份初始内参
    for (int level = pyramids - 1; level >= 0; level--) {
        // 每层的关键点也要缩放
        VecVector2d px_ref_pyr; 
        for (auto &px: px_ref) {
            px_ref_pyr.push_back(scales[level] * px);
        }
        // 不同金字塔级别中的内参的缩放
        fx = fxG * scales[level];
        fy = fyG * scales[level];
        cx = cxG * scales[level];
        cy = cyG * scales[level];
        // 每层用单层直接法计算
        DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
    }

}

3.4 结果展示

  1. 单层直接法：分别对五张图片计算出外参T，图片几乎类似这里只显示一张。
iteration: 0, cost: 4.0882e+06
iteration: 1, cost: 3.89309e+06
iteration: 2, cost: 3.80651e+06
iteration: 3, cost: 3.73133e+06
iteration: 4, cost: 3.66689e+06
iteration: 5, cost: 3.62699e+06
iteration: 6, cost: 3.60182e+06
cost increased: 3.60262e+06, 3.60182e+06
T21 = 
    0.999999 -0.000516409  0.000938655   -0.0107857
 0.000513252     0.999994   0.00336124   -0.0199524
-0.000940385  -0.00336075     0.999994    -0.115379
           0            0            0            1
单层直接法所用时间: 0.00370231
iteration: 0, cost: 4.51727e+06
iteration: 1, cost: 4.44097e+06
iteration: 2, cost: 4.40338e+06
iteration: 3, cost: 4.38437e+06
iteration: 4, cost: 4.36851e+06
iteration: 5, cost: 4.3375e+06
iteration: 6, cost: 4.31656e+06
iteration: 7, cost: 4.28746e+06
iteration: 8, cost: 4.28125e+06
cost increased: 4.29874e+06, 4.28125e+06
T21 = 
           1  -0.00023899 -0.000423502    0.0318101
 0.000240825     0.999991    0.0043384   -0.0140133
 0.000422461   -0.0043385      0.99999   -0.0616333
           0            0            0            1
单层直接法所用时间: 0.00604826
iteration: 0, cost: 6.27375e+06
iteration: 1, cost: 6.21322e+06
iteration: 2, cost: 6.09078e+06
iteration: 3, cost: 5.94502e+06
iteration: 4, cost: 5.8199e+06
iteration: 5, cost: 5.7109e+06
iteration: 6, cost: 5.66204e+06
iteration: 7, cost: 5.63571e+06
iteration: 8, cost: 5.56997e+06
iteration: 9, cost: 5.12689e+06
T21 = 
   0.999966 -0.00781871 -0.00278991   0.0766677
 0.00783866    0.999943  0.00721305  -0.0372605
 0.00273336 -0.00723467     0.99997   -0.128187
          0           0           0           1
单层直接法所用时间: 0.00568748
iteration: 0, cost: 6.13602e+06
iteration: 1, cost: 6.11936e+06
iteration: 2, cost: 6.09768e+06
iteration: 3, cost: 6.00533e+06
iteration: 4, cost: 6.00515e+06
iteration: 5, cost: 5.99342e+06
iteration: 6, cost: 5.9524e+06
iteration: 7, cost: 5.90819e+06
iteration: 8, cost: 5.89092e+06
iteration: 9, cost: 5.87545e+06
T21 = 
    0.999941   -0.0108718  0.000193771    0.0288788
   0.0108699     0.999909   0.00799969   -0.0580119
-0.000280724  -0.00799711     0.999968    -0.256918
           0            0            0            1
单层直接法所用时间: 0.00389448
iteration: 0, cost: 7.42899e+06
iteration: 1, cost: 7.412e+06
cost increased: 7.41608e+06, 7.412e+06
T21 = 
    0.999924   -0.0123424 -0.000713318    0.0351923
   0.0123486     0.999878   0.00951657   -0.0727886
 0.000595773  -0.00952466     0.999954    -0.231601
           0            0            0            1
单层直接法所用时间: 0.00221729

在这里插入图片描述
2. 多层直接法：由于篇幅原因，这里只展示对第一张图片的金字塔多层优化，这里是4层金字塔。

iteration: 0, cost: 1.59493e+06
iteration: 1, cost: 652954
iteration: 2, cost: 263926
iteration: 3, cost: 166670
cost increased: 172304, 166670
T21 = 
   0.999991  0.00226062  0.00346118 -0.00358483
-0.00226947    0.999994  0.00255482  0.00317976
-0.00345538 -0.00256266    0.999991   -0.720607
          0           0           0           1
单层直接法所用时间: 0.00162608
iteration: 0, cost: 178898
cost increased: 180789, 178898
T21 = 
   0.999989  0.00303756  0.00346181  0.00140036
-0.00304538    0.999993  0.00225747  0.00627627
-0.00345493 -0.00226798    0.999991    -0.72901
          0           0           0           1
单层直接法所用时间: 0.00266667
iteration: 0, cost: 248601
iteration: 1, cost: 229994
T21 = 
   0.999991  0.00251351  0.00346626 -0.00271352
-0.00252162    0.999994  0.00233523  0.00243136
-0.00346037 -0.00234395    0.999991    -0.73472
          0           0           0           1
单层直接法所用时间: 0.00587702
iteration: 0, cost: 348714
T21 = 
   0.999991  0.00248082  0.00343393 -0.00374045
-0.00248837    0.999994  0.00219446   0.0030455
-0.00342847 -0.00220298    0.999992   -0.732343
          0           0           0           1
单层直接法所用时间: 0.00270028
iteration: 0, cost: 1.31076e+06
iteration: 1, cost: 918151
iteration: 2, cost: 604174
iteration: 3, cost: 384708
iteration: 4, cost: 269610
iteration: 5, cost: 235605
cost increased: 239882, 235605
T21 = 
    0.99997   0.0010374  0.00769571  0.00572471
-0.00107293    0.999989  0.00461427 0.000557331
-0.00769083 -0.00462238     0.99996    -1.46166
          0           0           0           1
单层直接法所用时间: 0.00987296

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

一万年可长否
关注
  • 0
    点赞
  • 踩
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
【视觉SLAM十四讲】视觉里程计—直接法
weixin_44413191的博客
07-30 1224
本文为视觉 SLAM 学习总结，讲解直接法的相关知识。内容包括直接法的引出、光流、实践：光流、直接法、实践：RGB-D 的直接法
SLAM 06.视觉里程计-3-直接法
jiankangyq的博客
08-12 2160
直接法跳过了提取特征点的步骤。它构建一个优化问题，直接根据像素信息（通常是亮度），来估计相机的运动。这种方法省去了提特征的时间，然而代价则是，利用了所有信息之后，使得优化问题规模远远大于使用特征点的规模。因此，基于直接法的VO，多数需要GPU加速，才能做到实时化。...
[学习笔记-SLAM篇]视觉SLAM十四讲ch8
warningm_dm的博客
07-30 838
第8讲和第7讲内容都是前端的原理，这一讲主要集中于直接法。 已近暂时放弃实践了，先把理论梳理完吧。 反正就，学习的时候想摸鱼，摸鱼的时候闲不住，太难了。 注： 1）学习视频：【高翔】视觉SLAM十四讲。 视觉SLAM十四讲第8讲8.1 理论部分8.2 实践部分 第8讲 8.1 理论部分 7-8讲均是视觉里程计的内容，主要区别在于前端原理不同，分别对应特征点法和直接法。 此外计算相机的运动，依据点的关系不同，采用不同方法。 2D-2D：对极几何；3D-3D：ICP；2D-3D：PnP。 1）特征点法 前
SLAM十四讲-ch8-直接法(包含手写单层、多层光流跟踪和直接法代码的注释)
joun772的博客
11-06 2441
一、利用cv的光流追踪函数进行光流跟踪 void cv::calcOpticalFlowPyrLK ( InputArray prevImg, InputArray nextImg, InputArray prevPts, InputOutputArray nextPts, OutputArray status, OutputArray err, Size winSize = Siz
SLAM14讲ch8代码实践部分
m0_51461473的博客
11-24 1781
一、LK光流法 1.1 optical_flow.cpp文件 #include <opencv2/opencv.hpp> #include <string> #include <chrono> #include <Eigen/Core> #include <Eigen/Dense> #include "opencv2/highgui/highgui_c.h" using namespace std; using namespac
SLAM系列——视觉里程计（直接法）
xiaohu的博客
06-25 1642
高翔视觉slam十四讲有完整库的代码文件
09-20
高翔视觉slam十四讲有完整库的代码文件 ： 因为高翔视觉slam十四讲文件里面的，3rdparty文件夹下下载下来是空的，所以这里有专门的已经下载好了的文件夹供大家使用，不用再自己下载了，避免因版本不同而编译错误。...
【视觉SLAM十四讲】直接法视觉里程计.pdf
08-27
视觉SLAM十四讲是视觉SLAM领域的经典教程，涵盖了视觉SLAM的基本概念、直接法视觉里程计、LK光流、光流法、金字塔光流法等重要知识点。本文将详细介绍视觉SLAM十四讲的重要概念和技术，并对LK光流、光流法和金字塔...
《视觉SLAM十四讲（高翔）》高清中文版PDF 1
08-03
《视觉SLAM十四讲》是一本深入探讨视觉Simultaneous Localization And Mapping（SLAM）的书籍，由高翔和张涛合著。该书旨在从理论到实践全面讲解SLAM技术，适合对机器人导航、自动驾驶或计算机视觉感兴趣的读者。书...
14.视觉SLAM十四讲(高翔第二版)1
08-04
《视觉SLAM十四讲》是高翔撰写的一本关于同时定位与地图构建（Simultaneous Localization and Mapping，简称SLAM）的专业书籍，旨在为读者提供SLAM领域的基础知识和核心算法。该书涵盖了从数学理论到计算机视觉算法...
slam十四讲，ch8编译报错
qq_41333880的博客
04-25 1108
1运行高翔博士的slam十四讲ch8光流法，报错如下 2 make错误提示，没有匹配的cv函数，猜测可能是opencv版本的问题，因为我现在是opencv249 3 因为之前装过opencv310，因此在cmakelists里设置opencv的寻找路径如下： 加入set(CMAKE_PREFIX_PATH "/usr/local/opencv3.1.0/share/OpenCV") 4...
视觉里程计5（SLAM十四讲ch8）-LK光流
赤豆几维
09-01 1527
特征点法——&gt;直接法  光流（Optical Flow） 实践 采用TUM的RGB-D数据集 深度图和彩色图时间对齐 python associate.py rgb.txt depth.txt &gt; associate.txt  ./build/useLK ../data #include &lt;iostream&gt; #i...
视觉SLAM十四讲-高翔 第8讲 视觉里程计2
qin_liang的博客
09-15 950
视觉里程计2
视觉里程计6（SLAM十四讲ch8)-直接法
赤豆几维
09-01 3396
转载于https://blog.csdn.net/qq_23225073/article/details/78579458 直接法（Direct Method） 简介推导 投影方程如下 在直接法中，是求解一个优化问题，但这个优化最小化的不是重投影误差，而是测量误差（Photometric Error），也就是 P的两个像的亮度误差   优化该误差的目标函数  直接法分类...
《视觉SLAM14讲》ch8学习记录
只是什么都不会的博客
10-31 143
由于特征点法的提取和描述子十分耗时，可以使用直接法和光流法来解决这个问题。
视觉SLAM学习打卡【8-1】-视觉里程计·直接法
最新发布
小幸运召唤师的博客
04-06 1783
本节直接法与上节特征点法，为视觉里程计估计位姿的两大主流方法。而在引出直接法前，先介绍光流法（二者均对灰度值 I 做文章）。
视觉SLAM十四讲CH8代码解析及课后习题详解
生活如此美好，致敬每一个热爱“热爱”的人！
11-11 8485
1. 除了LK 光流之外，还有哪些光流方法？它们各有什么特点？ KLT 光流 - mthoutai - 博客园
视觉SLAM⑧----视觉里程计Ⅱ(光流法与直接法)
qq_41694024的博客
05-20 2784
1.理解光流法跟踪特征点的原理 2.理解直接法是如何估计相机位姿的 3.实现多层直接法的计算
视觉里程计问题的公式推导
枫叶南国的博客
04-21 9044
视觉里程计问题的公式推导 　　假设在时间k，通过相机移动，追踪环境图像。如果是单目相机，k时间上拍摄的一组图像表示为I0:n={I0，…，In}。如果是立体相机，每个时刻有左右两幅图像，记为Il, 0:n={Il,0，…，Il,n}和Ir, 0:n={Ir,0，…，Ir,n}。图1表示了这个具体问题。 　　相邻的相机位置的相对位姿Tk,K-1（或相机系统位置），通过视觉特征进行
高翔视觉slam十四讲
08-25
高翔视觉SLAM（Simultaneous Localization and Mapping）十四讲是一本关于视觉SLAM算法和实践的教材，其中包含了完整的代码实现。在这本教材中，作者提供了一个已经下载好的文件夹，里面包含了完整的代码文件，可以避免因版本不同而导致的编译错误。 如果你想查看具体的代码实现，你可以在github上找到该教材的代码仓库，其中包含了主要的代码文件。 此外，你还可以在Bilibili网站上找到高翔视觉SLAM十四讲的视频教程，通过观看视频来更好地理解和学习SLAM算法和实践。<span class="em">1</span><span class="em">2</span><span class="em">3</span> #### 引用[.reference_title] - *1* [高翔视觉slam十四讲有完整库的代码文件](https://download.csdn.net/download/weixin_51938716/86566040)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v93^chatsearchT3_2"}}] [.reference_item style="max-width: 50%"] - *2* *3* [SLAM十四讲 高翔](https://blog.csdn.net/shike951128/article/details/124921532)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v93^chatsearchT3_2"}}] [.reference_item style="max-width: 50%"] [ .reference_list ]
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值