0. 実践前の豆知識のご紹介

オドメーターの歴史的な起源は何ですか?

オドメーターは、通常、車両またはロボットの車輪の回転を検出することによって、車両またはロボットの移動距離を測定するために使用されるデバイスです。オドメーターの歴史は、車両の走行距離を測定するために機械装置が使用されていた 1600 年代初頭に遡ります。これらのユニットは通常、ホイールの回転に応じて走行距離を記録する機械式カウンターを使用します。18 世紀後半、発明家のトーマスゴールドスミスは、機械式カウンターを使用して馬車や自転車の移動距離を記録する、オドマイトとして知られる装置を開発しました。この装置は、現代の走行距離計の初期の形式と考えられています。

時間が経つにつれて、走行距離計は電子およびコンピュータ化されたデバイスに進化しました。最近の車両やロボットは、多くの場合、レーザーまたは赤外線センサーを使用して車輪の回転を測定し、データをコンピューターまたは制御システムに送信します。一般に、走行距離計の歴史は機械式から電子化、コンピューター化の過程を経てきました。

1. 実運用前の準備

ターミナルでch8フォルダに入り、以下のコマンドを実行してコンパイルします。

mkdir build
cd build
cmake ..
//注意，j8还是其他主要看自己的电脑情况
make -j8

ビルドファイル内で実行します。
注:作成する前に、ファイル内の画像の取得パスを次のように変更してください。そうしないと、後の操作で問題が発生し、再度変更してから作成する必要があります。

2. 練習プロセス

2.1 LK オプティカルフロー

コード：

//
// 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/fighter/slam/slambook2/ch8/LK1.png";  // first image
string file_2 = "/home/fighter/slam/slambook2/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
);

/**
 * 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) {
    
    

    // images, note they are CV_8UC1, not CV_8UC3
    Mat img1 = imread(file_1, 0);
    Mat img2 = imread(file_2, 0);

    // key points, using GFTT here.
    vector<KeyPoint> kp1;
    Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypoints
    detector->detect(img1, kp1);

    // now lets track these key points in the second image
    // first use single level LK in the validation picture
    vector<KeyPoint> kp2_single;
    vector<bool> success_single;
    OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single);

    // then test multi-level 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;

    // use opencv's flow for validation
    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;

    // plot the differences of those functions
    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];
        double dx = 0, dy = 0; // dx,dy need to be estimated
        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

        // Gauss-Newton iterations
        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++) {
    
    
            if (inverse == false) {
    
    
                H = Eigen::Matrix2d::Zero();
                b = Eigen::Vector2d::Zero();
            } else {
    
    
                // only reset b
                b = Eigen::Vector2d::Zero();
            }

            cost = 0;

            // compute cost and 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) {
    
    
                        // 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))
                        );
                    }
                    // compute H, b and set cost;
                    b += -error * J;
                    cost += error * error;
                    if (inverse == false || iter == 0) {
    
    
                        // also update H
                        H += J * J.transpose();
                    }
                }

            // compute update
            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;
            }

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

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

        success[i] = succ;

        // set 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};

    // create pyramids
    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;

    // coarse-to-fine LK tracking in pyramids
    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);
}

ビルドでステートメントを実行します。

 ./optical_flow

実行結果:
実行後、opencv、単一レイヤー、および複数レイヤーを使用した追跡と
opencv

ターミナル出力が同時に表示されます。

build pyramid time: 0.0072683
track pyr 3 cost time: 0.0004321
track pyr 2 cost time: 0.0002794
track pyr 1 cost time: 0.0002624
track pyr 0 cost time: 0.0003014
optical flow by gauss-newton: 0.0087955
optical flow by opencv: 0.0054821

2.2 直接法

コード：

#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;

// Camera intrinsics
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// baseline
double baseline = 0.573;
// paths
string left_file = "/home/fighter/slam/slambook2/ch8/left.png";
string disparity_file = "/home/fighter/slam/slambook2/ch8/disparity.png";
boost::format fmt_others("/home/fighter/slam/slambook2/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;
};

/**
 * pose estimation using direct method
 * @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
);

/**
 * pose estimation using direct method
 * @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) {
    
    

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 2000;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
    
    
        int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    Sophus::SE3d T_cur_ref;

    for (int i = 1; i < 6; i++) {
    
      // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        // try single layer by uncomment this line
        // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
        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);

    for (int iter = 0; iter < iterations; iter++) {
    
    
        jaco_accu.reset();
        cv::parallel_for_(cv::Range(0, px_ref.size()),
                          std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1));
        Matrix6d H = jaco_accu.hessian();
        Vector6d b = jaco_accu.bias();

        // solve update and put it into estimation
        Vector6d update = H.ldlt().solve(b);;
        T21 = Sophus::SE3d::exp(update) * T21;
        cost = jaco_accu.cost_func();

        if (std::isnan(update[0])) {
    
    
            // sometimes occurred when we have a black or white patch and H is irreversible
            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;
    }

    cout << "T21 = \n" << T21.matrix() << endl;
    auto t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "direct method for single layer: " << time_used.count() << endl;

    // plot the projected pixels here
    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) {
    
    

    // 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++) {
    
    

        // compute the projection in the second image
        Eigen::Vector3d point_ref =
            depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
        Eigen::Vector3d point_cur = T21 * point_ref;
        if (point_cur[2] < 0)   // depth invalid
            continue;

        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;

        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++;

        // and compute error and 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, px_ref[i][0] + x, px_ref[i][1] + y) -
                               GetPixelValue(img2, u + x, v + y);
                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();

                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) {
    
    

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

    // create pyramids
    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);
        }
    }

    double fxG = fx, fyG = fy, cxG = cx, cyG = cy;  // backup the old values
    for (int level = pyramids - 1; level >= 0; level--) {
    
    
        VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
        for (auto &px: px_ref) {
    
    
            px_ref_pyr.push_back(scales[level] * px);
        }

        // scale fx, fy, cx, cy in different pyramid levels
        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);
    }

}

ビルドでステートメントを実行します: ./direct_method

実行結果:
実行結果グラフで追跡フローを確認できます。その変化を確認するにはウィンドウを閉じ続ける必要があります。

最後に
端末は対応する情報を出力します。

iteration: 0, cost: 1.59797e+06
iteration: 1, cost: 651716
iteration: 2, cost: 243255
iteration: 3, cost: 176884
cost increased: 183909, 176884
T21 =
   0.999991  0.00245009   0.0033858  0.00303273
-0.00245906    0.999993  0.00264927 0.000424829
-0.00337929 -0.00265757    0.999991   -0.730917
          0           0           0           1
direct method for single layer: 0.0016574
iteration: 0, cost: 186361
T21 =
   0.999989  0.00302157  0.00347121 0.000762356
-0.00302936    0.999993  0.00224074  0.00666315
-0.00346442 -0.00225123    0.999991   -0.728227
          0           0           0           1
direct method for single layer: 0.002358
iteration: 0, cost: 247529
iteration: 1, cost: 229117
T21 =
   0.999991  0.00251345  0.00346578 -0.00270253
-0.00252155    0.999994  0.00233534  0.00243076
-0.00345989 -0.00234406    0.999991   -0.734719
          0           0           0           1
direct method for single layer: 0.00523089
iteration: 0, cost: 348441
T21 =
   0.999991  0.00248082  0.00343389 -0.00373965
-0.00248836    0.999994  0.00219448  0.00304522
-0.00342843 -0.00220301    0.999992   -0.732343
          0           0           0           1
direct method for single layer: 0.0012425
iteration: 0, cost: 1.315e+06
iteration: 1, cost: 906037
iteration: 2, cost: 603626
iteration: 3, cost: 399435
iteration: 4, cost: 280889
iteration: 5, cost: 237691
cost increased: 238395, 237691
T21 =
    0.999971  0.000902974   0.00759567   0.00772499
-0.000938067     0.999989   0.00461783   0.00179863
 -0.00759142  -0.00462482      0.99996     -1.46052
           0            0            0            1
direct method for single layer: 0.0045787
iteration: 0, cost: 355480
iteration: 1, cost: 348267
cost increased: 348423, 348267
T21 =
   0.999972  0.00120085  0.00742895   0.0085892
-0.00123022    0.999991  0.00395007  0.00531883
-0.00742414  -0.0039591    0.999965    -1.46883
          0           0           0           1
direct method for single layer: 0.0009226

iteration: 0, cost: 443225
iteration: 1, cost: 435054
cost increased: 437537, 435054
T21 =
    0.999971  0.000737127   0.00764046 -0.000242531
-0.000767091     0.999992   0.00391957   0.00279348
 -0.00763751  -0.00392532     0.999963      -1.4818
           0            0            0            1
direct method for single layer: 0.0009165
iteration: 0, cost: 501709
iteration: 1, cost: 463084
cost increased: 463953, 463084
T21 =
    0.999971  0.000695392   0.00758989  -0.00249798
-0.000723685     0.999993   0.00372567   0.00395279
 -0.00758725  -0.00373106     0.999964     -1.48132
           0            0            0            1
direct method for single layer: 0.0008786
iteration: 0, cost: 1.37107e+06
iteration: 1, cost: 1.10683e+06
iteration: 2, cost: 921990
iteration: 3, cost: 794740
iteration: 4, cost: 601342
iteration: 5, cost: 559319
iteration: 6, cost: 394434
iteration: 7, cost: 363978
cost increased: 374118, 363978
T21 =
   0.999945  0.00160897   0.0103684   0.0493737
-0.00166631    0.999983  0.00552457   0.0132374
 -0.0103594 -0.00554155    0.999931    -2.18064
          0           0           0           1
direct method for single layer: 0.0020563
iteration: 0, cost: 461649
iteration: 1, cost: 443603
iteration: 2, cost: 436513
iteration: 3, cost: 432080
iteration: 4, cost: 423494
cost increased: 431930, 423494
T21 =
   0.999938  0.00146627    0.011054   0.0282033
-0.00152599    0.999984   0.0053958  0.00256267
 -0.0110459 -0.00541233    0.999924    -2.21468
          0           0           0           1
direct method for single layer: 0.0015141
iteration: 0, cost: 646880
iteration: 1, cost: 614318
iteration: 2, cost: 613113
cost increased: 620133, 613113
T21 =
   0.999935  0.00152579   0.0112714   0.0183767
-0.00158773    0.999984  0.00548783 -0.00540064
 -0.0112629 -0.00550537    0.999921    -2.23461
          0           0           0           1
direct method for single layer: 0.0011636
iteration: 0, cost: 924370
iteration: 1, cost: 828022
iteration: 2, cost: 821445
iteration: 3, cost: 803411
cost increased: 811368, 803411
T21 =
    0.999934   0.00125001    0.0114068   0.00255272
 -0.00131019     0.999985   0.00527034 -0.000605904
  -0.0114001  -0.00528494     0.999921     -2.24055
           0            0            0            1
direct method for single layer: 0.0015292
iteration: 0, cost: 1.43709e+06
iteration: 1, cost: 1.31501e+06
iteration: 2, cost: 1.06723e+06
iteration: 3, cost: 938977
iteration: 4, cost: 788005
iteration: 5, cost: 680776
iteration: 6, cost: 605861
iteration: 7, cost: 548408
iteration: 8, cost: 516721
iteration: 9, cost: 513621
T21 =
    0.999872 -0.000312873    0.0159856    0.0259369
 0.000197362     0.999974   0.00722705  -0.00480823
  -0.0159874  -0.00722297     0.999846     -2.96617
           0            0            0            1
direct method for single layer: 0.0048151
iteration: 0, cost: 640692
iteration: 1, cost: 616653
iteration: 2, cost: 610486
cost increased: 615297, 610486
T21 =
    0.999864 -0.000319108    0.0164719   0.00993795
 0.000208632     0.999977   0.00670821  -0.00627072
  -0.0164737  -0.00670386     0.999842       -3.005
           0            0            0            1
direct method for single layer: 0.0009756
iteration: 0, cost: 848724
iteration: 1, cost: 823518
iteration: 2, cost: 780844
cost increased: 802765, 780844
T21 =
    0.999865 -0.000227727    0.0164536    0.0022434
 0.000124997      0.99998    0.0062444  -0.00399514
  -0.0164547  -0.00624149     0.999845     -3.01734
           0            0            0            1
direct method for single layer: 0.0010828
iteration: 0, cost: 1.26838e+06
iteration: 1, cost: 1.16447e+06
cost increased: 1.19957e+06, 1.16447e+06
T21 =
    0.999865   0.00017071    0.0164584  -0.00906366
-0.000267333     0.999983   0.00586871  0.000576184
  -0.0164571  -0.00587231     0.999847     -3.02444
           0            0            0            1
direct method for single layer: 0.0008361
iteration: 0, cost: 1.64476e+06
iteration: 1, cost: 1.49383e+06
iteration: 2, cost: 1.23318e+06
iteration: 3, cost: 950472
iteration: 4, cost: 794112
iteration: 5, cost: 686345
iteration: 6, cost: 671817
iteration: 7, cost: 659908
iteration: 8, cost: 652671
iteration: 9, cost: 605440
T21 =
    0.999803   0.00057056    0.0198394    0.0427397
-0.000712283     0.999974   0.00713717    0.0136135
  -0.0198348  -0.00714989     0.999778     -3.76444
           0            0            0            1
direct method for single layer: 0.0088727
iteration: 0, cost: 983836
iteration: 1, cost: 948750
iteration: 2, cost: 945444
iteration: 3, cost: 895561
cost increased: 898341, 895561
T21 =
     0.99978  0.000643056    0.0209471  0.000477452
-0.000787155     0.999976   0.00687165   0.00707341
  -0.0209422  -0.00688663     0.999757     -3.83472
           0            0            0            1
direct method for single layer: 0.0012023
iteration: 0, cost: 1.27161e+06
iteration: 1, cost: 1.22543e+06
iteration: 2, cost: 1.04807e+06
cost increased: 1.2001e+06, 1.04807e+06
T21 =
   0.999777  0.00108579   0.0210816 -0.00872002
-0.00121752     0.99998  0.00623637   0.0124058
 -0.0210744 -0.00626065    0.999758    -3.85459
          0           0           0           1
direct method for single layer: 0.0010238
iteration: 0, cost: 1.67716e+06
iteration: 1, cost: 1.64927e+06
iteration: 2, cost: 1.63771e+06
cost increased: 1.64371e+06, 1.63771e+06
T21 =
   0.999786  0.00136909   0.0206569 -0.00336234
-0.00149442    0.999981   0.0060529  0.00874311
 -0.0206482 -0.00608247    0.999768    -3.86001
          0           0           0           1
direct method for single layer: 0.001018

3. 発生した問題と解決策

3.1 コンパイル中に発生した問題

make 中の opencv に関連する問題:

解決策: 前の方法に従って解決します。クエリリンク: https://blog.csdn.net/qq_44164791/article/details/131210608?spm=1001.2014.3001.5502

Visual SLAM 講義 14 — ch8 実践 (Visual Odometry 2)

Visual SLAM講座14 ～ch8の実践操作とピット回避～

0. 実践前の豆知識のご紹介

1. 実運用前の準備

2. 練習プロセス

2.1 LK オプティカルフロー

2.2 直接法

3. 発生した問題と解決策

3.1 コンパイル中に発生した問題

おすすめ

Visual SLAM 講義 14 — ch8 実践 (Visual Odometry 2)

Visual SLAM講座14 ～ch8の実践操作とピット回避～

0. 実践前の豆知識のご紹介

1. 実運用前の準備

2. 練習プロセス

2.1 LK オプティカル フロー

2.2 直接法

3. 発生した問題と解決策

3.1 コンパイル中に発生した問題

おすすめ

2.1 LK オプティカルフロー