视觉SLAM学习打卡【8-2】-视觉里程计·直接法代码详解

于 2024-04-06 08:57:32 发布
阅读量947 收藏 22
点赞数 24
分类专栏： # SLAM 文章标签： c++ 计算机视觉 opencv
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_52757671/article/details/137395052
版权

视觉SLAM学习打卡【8-2】-视觉里程计·直接法代码详解

1. 光流法 optical_flow.cpp

// 引入OpenCV库，它包含了计算机视觉任务所需的各种函数和数据结构  
#include <opencv2/opencv.hpp>  
  
// 引入C++标准库中的string，用于处理字符串，如文件路径  
#include <string>  
  
// 引入C++标准库中的chrono，通常用于计时和性能评估  
#include <chrono>  
  
// 引入Eigen库的核心和密集矩阵模块，Eigen是一个高级C++库，用于进行线性代数、矩阵和向量操作  
#include <Eigen/Core>  
#include <Eigen/Dense>  
  
// 使用C++标准库的命名空间，以便更方便地访问其函数和对象  
using namespace std;  
  
// 使用OpenCV的命名空间，以便更方便地访问其函数和对象  
using namespace cv;  
  
// 定义两个字符串变量，分别存储两个图像文件的路径  
string file_1 = "./LK1.png";  // 第一个图像文件路径  
string file_2 = "./LK2.png";  // 第二个图像文件路径  
  
// 定义一个名为OpticalFlowTracker的类，用于光流跟踪和相关接口  
class OpticalFlowTracker {  
public:  
    // 构造函数，初始化类的成员变量，并接收一系列参数，包括两张图像、关键点集合、成功跟踪的标志等  
    OpticalFlowTracker(  
        const Mat &img1_,  // 第一张图像  
        const Mat &img2_,  // 第二张图像  
        const vector<KeyPoint> &kp1_,  // 在第一张图像中检测到的关键点  
        vector<KeyPoint> &kp2_,  // 在第二张图像中对应的关键点（输出参数）  
        vector<bool> &success_,  // 跟踪成功的标志（输出参数）  
        bool inverse_ = true,  // 是否使用逆向光流法，默认为true  
        bool has_initial_ = false  // 是否已有初始猜测，默认为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;  // 是否已有初始猜测  
};  
  
/**  
 * 单层光流法函数声明，用于计算两张图像之间的光流  
 * @param [in] img1 第一张图像  
 * @param [in] img2 第二张图像  
 * @param [in] kp1 在第一张图像中检测到的关键点  
 * @param [in|out] kp2 在第二张图像中对应的关键点，如果为空，则使用kp1中的初始猜测  
 * @param [out] success 跟踪成功的标志  
 * @param [in] inverse 是否使用逆向光流法，默认为false  
 * @param [in] has_initial_guess 是否已有初始猜测，默认为false  
 */  
void OpticalFlowSingleLevel(  
    const Mat &img1,  // 第一张图像  
    const Mat &img2,  // 第二张图像  
    const vector<KeyPoint> &kp1,  // 在第一张图像中检测到的关键点  
    vector<KeyPoint> &kp2,  // 在第二张图像中对应的关键点（可能是输出参数）  
    vector<bool> &success,  // 跟踪成功的标志（输出参数）  
    bool inverse = false,  // 是否使用逆向光流法，默认为false  
    bool has_initial_guess = false  // 是否已有初始猜测，默认为false  
);  
  
/**  
 * 多层光流法函数声明，用于计算两张图像之间的光流，使用图像金字塔来提高精度和稳定性   
 * @param [in] img1 第一张图像金字塔  
 * @param [in] img2 第二张图像金字塔  
 * @param [in] kp1 在第一张图像中检测到的关键点  
 * @param [out] kp2 在第二张图像中对应的关键点  
 * @param [out] success 跟踪成功的标志  
 * @param [in] inverse 是否启用逆向光流法来提高精度和稳定性，特别是在快速运动时  
 */  
// 函数声明：多层光流法，用于在两张图像之间计算关键点的光流  
void OpticalFlowMultiLevel(  
    const Mat &img1,  // 第一张图像  
    const Mat &img2,  // 第二张图像  
    const vector<KeyPoint> &kp1,  // 第一张图像中的关键点  
    vector<KeyPoint> &kp2,  // 第二张图像中对应的关键点（输出参数）  
    vector<bool> &success,  // 跟踪成功的标志（输出参数）  
    bool inverse = false  // 是否使用逆向光流法，默认为false  
);  
  
/**  
 * 从参考图像中获取灰度值（双线性插值）  
 * @param img 输入图像  
 * @param x 浮点型x坐标  
 * @param y 浮点型y坐标  
 * @return 插值后的像素值  
 */  
  
// 定义一个内联函数，用于通过双线性插值从图像中获取灰度值  
inline float GetPixelValue(const cv::Mat &img, float x, float y) {  
    // 边界检查，确保坐标在图像范围内  
    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);  // 确保x坐标不超出图像边界  
    int y_a1 = std::min(img.rows - 1, int(y) + 1);  // 确保y坐标不超出图像边界  
      
    // 进行双线性插值计算  
    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) {  
    // 读取两张图像，注意它们是灰度图像（CV_8UC1），不是彩色图像（CV_8UC3）  
    Mat img1 = imread(file_1, 0);  // 读取第一张图像为灰度图  
    Mat img2 = imread(file_2, 0);  // 读取第二张图像为灰度图  
  
    // 检测关键点，这里使用GFTT（Good Features To Track）方法  
    vector<KeyPoint> kp1;  // 存储第一张图像中的关键点  
    Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20);  // 最多检测500个关键点，其他参数为算法设置  
    detector->detect(img1, kp1);  // 在第一张图像中检测关键点  
  
    // 接下来在第二张图像中跟踪这些关键点  
    // 首先使用单层LK光流法进行测试  
    vector<KeyPoint> kp2_single;  // 存储单层LK光流法跟踪到的关键点  
    vector<bool> success_single;  // 存储单层LK光流法跟踪成功的标志  
    OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single);  // 执行单层LK光流法  
  
    // 然后测试多层LK光流法  
    vector<KeyPoint> kp2_multi;  // 存储多层LK光流法跟踪到的关键点  
    vector<bool> success_multi;  // 存储多层LK光流法跟踪成功的标志  
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();  // 记录开始时间  
    OpticalFlowMultiLevel(img1, img2, kp1, kp2_multi, success_multi, true);  // 执行多层LK光流法，启用逆向光流  
    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;  // 输出执行时间  
  
    // 使用OpenCV的calcOpticalFlowPyrLK函数进行验证  
    vector<Point2f> pt1, pt2;  // 存储输入和输出的关键点位置  
    for (auto &kp: kp1) pt1.push_back(kp.pt);  // 将关键点位置转换为OpenCV的点格式  
    vector<uchar> status;  // 存储每个关键点的跟踪状态  
    vector<float> error;  // 存储每个关键点的跟踪误差  
    t1 = chrono::steady_clock::now();  // 记录开始时间  
    cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);  // 执行OpenCV的光流法函数  
    t2 = chrono::steady_clock::now();  // 记录结束时间  
    time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);  // 计算执行时间  
    cout << "optical flow by opencv: " << time_used.count() << endl;  // 输出执行时间  
}
// 转换img2图像从灰度到BGR颜色空间，以便在彩色图像上绘制  
Mat img2_single;  
cv::cvtColor(img2, img2_single, CV_GRAY2BGR);  
  
// 遍历关键点kp2_single，这些关键点可能是在单尺度上检测到的特征点  
for (int i = 0; i < kp2_single.size(); i++) {  
    // 如果当前关键点匹配成功（由success_single数组标记）  
    if (success_single[i]) {  
        // 在匹配的关键点位置上画一个绿色的圆圈，表示匹配成功的位置  
        cv::circle(img2_single, kp2_single[i].pt, 2, cv::Scalar(0, 250, 0), 2);  
        // 画一条从kp1[i]到kp2_single[i]的线，表示两个图像之间的关键点匹配  
        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));  
    }  
}  
  
// 这里的操作与上面类似，但是使用的是OpenCV函数找到的匹配点  
Mat img2_CV;  
cv::cvtColor(img2, img2_CV, CV_GRAY2BGR);  
for (int i = 0; i < pt2.size(); i++) {  
    if (status[i]) { // status数组表示每个点的匹配状态  
        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);       // 显示OpenCV跟踪结果  
  
// 等待用户按键，然后继续执行后续代码（如果有的话）或退出程序  
cv::waitKey(0);  
  
return 0; // 程序正常退出，返回0
// OpticalFlowSingleLevel函数用于计算两个图像之间的光流  
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()); // 调整kp2的大小以匹配kp1的大小  
    success.resize(kp1.size()); // 调整success的大小以匹配kp1的大小  
  
    // 创建一个OpticalFlowTracker对象来处理光流计算  
    OpticalFlowTracker tracker(img1, img2, kp1, kp2, success, inverse, has_initial);  
  
    // 使用并行计算来加速光流计算过程  
    parallel_for_(Range(0, kp1.size()),  
                  std::bind(&OpticalFlowTracker::calculateOpticalFlow, &tracker, placeholders::_1));  
}  
  
// OpticalFlowTracker类的成员函数，用于计算指定范围内的光流  
void OpticalFlowTracker::calculateOpticalFlow(const Range &range) {  
    int half_patch_size = 4; // 补丁的一半大小，用于定义关键点周围的区域  
    int iterations = 10; // Gauss-Newton迭代的次数  
  
    // 遍历指定范围内的所有关键点  
    for (size_t i = range.start; i < range.end; i++) {  
        auto kp = kp1[i]; // 获取当前关键点  
        double dx = 0, dy = 0; // 初始化光流的x和y分量  
  
        // 如果有初始估计，则从kp2中获取dx和dy的初始值  
        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; // 标记当前关键点是否成功匹配  
  
        // 使用Gauss-Newton方法进行迭代优化  
        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++) {  
            // 根据是否是逆向模式来重置H和b  
            if (!inverse) {  
                H = Eigen::Matrix2d::Zero();  
                b = Eigen::Vector2d::Zero();  
            } else {  
                b = Eigen::Vector2d::Zero();  
            }  
  
            cost = 0; // 重置代价  
  
            // 在关键点周围的区域内计算代价和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) {  
                        // ...（省略了梯度J的计算过程）  
                    } else if (iter == 0) {  
                        // ...（省略了梯度J的计算过程）  
                    }  
  
                    // 更新b, H和代价  
                    b += -error * J;  
                    cost += error * error;  
                    if (!inverse || iter == 0) {  
                        H += J * J.transpose();  
                    }  
                }  
            }  
  
            // 使用LDLT分解来求解更新量  
            Eigen::Vector2d update = H.ldlt().solve(b);  
  
            // 检查更新量是否有效  
            if (std::isnan(update[0])) {  
                // 如果更新量是NaN，则表示失败  
                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) {  
                break;  
            }  
        }  
  
        // 设置成功匹配的标记和第二个图像中的关键点位置  
        success[i] = succ;  
        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 // 是否反向跟踪（从第二帧跟踪到第一帧）  
) {  
    // 设定金字塔的层数和每层之间的缩放比例  
    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; // 定义两个图像金字塔  
    // 构建图像金字塔  
    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--) {  
        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);  
}

注：上述路径需改为绝对路径.

（1）理论推导：双线性插值

首先，来看单线性插值（双线性插值可看作3次双线性插值）
在这里插入图片描述
由斜率相等可得 y − y 0 x − x 0 = y 1 − y 0 x 1 − x 0 \frac{y-y_{0}}{x-x_{0}}=\frac{y_{1}-y_{0}}{x_{1}-x_{0}} x−x0​y−y0​​=x1​−x0​y1​−y0​​化简得 y = x 1 − x x 1 − x 0 y 0 + x − x 0 x 1 − x 0 y 1 y=\frac{x_{1}-x}{x_{1}-x_{0}}y_{0}+\frac{x_{-}x_{0}}{x_{1}-x_{0}}y_{1} y=x1​−x0​x1​−x​y0​+x1​−x0​x−​x0​​y1​把y看作函数值P P = x 1 − x x 1 − x 0 P 0 + x − x 0 x 1 − x 0 P 1 . P=\frac{x_{1}-x}{x_{1}-x_{0}}P_{0}+\frac{x-x_{0}}{x_{1}-x_{0}}P_{1}. P=x1​−x0​x1​−x​P0​+x1​−x0​x−x0​​P1​.
在这里插入图片描述
以左梯形面作x方向上的线性插值，即在这里插入图片描述
P 1 = x 2 − x x 2 − x 1 P 11 + x − x 1 x 2 − x 1 P 21 P_{1}=\frac{x_{2}-x}{x_{2}-x_{1}} P_{11} +\frac{x-x_{1}}{x_{2}-x_{1}} P_{21} P1​=x2​−x1​x2​−x​P11​+x2​−x1​x−x1​​P21​
以右梯形面作x方向上的线性插值，即
在这里插入图片描述
P 2 = x 2 − x x 2 − x 1 P 12 + x − x 1 x 2 − x 1 P 22 P_{2}=\frac{x_{2}-x}{x_{2}-x_{1}}P_{12}+\frac{x-x_{1}}{x_{2}-x_{1}}P_{22} P2​=x2​−x1​x2​−x​P12​+x2​−x1​x−x1​​P22​以中间梯形面作y方向上的线性插值，即在这里插入图片描述
P = y 2 − y y 2 − y 1 P 1 + y − y 1 y 2 − y 1 p 2 P=\frac{y_{2}-y}{y_{2}-y_{1}} P_{1}+\frac{y-y_{1}}{y_{2}-y_{1}} p_{2} P=y2​−y1​y2​−y​P1​+y2​−y1​y−y1​​p2​将P1、P2代入上式得 P = P 11 ( x 2 − x 1 ) ( y 2 − y 1 ) ( x 2 − x 1 ) ( y 2 − y ) + P 21 ( x 2 − x 1 ) ( y 2 − y 1 ) ( x − x 1 ) ( y 2 − y ) + P 12 ( x 2 − x 1 ) ( y 2 − y 1 ) ( x 2 − x 1 ) ( y − y 1 ) + P 22 ( x 2 − x 1 ) ( y 2 − y 1 ) ( x − x 1 ) ( y − y 1 ) \begin{gathered} P=\frac{P_{11}}{(x_{2}-x_{1})(y_{2}-y_{1})}(x_{2}-x_{1})(y_{2}-y)+\frac{P_{21}}{(x_{2}-x_{1})(y_{2}-y_{1})}(x-x_{1})(y_{2}-y)+ \\ \frac{P_{12}}{(x_{2}-x_{1})(y_{2}-y_{1})}(x_{2}-x_{1})(y-y_{1})+\frac{P_{22}}{(x_{2}-x_{1})(y_{2}-y_{1})}(x-x_{1})(y-y_{1}) \\\end{gathered} P=(x2​−x1​)(y2​−y1​)P11​​(x2​−x1​)(y2​−y)+(x2​−x1​)(y2​−y1​)P21​​(x−x1​)(y2​−y)+(x2​−x1​)(y2​−y1​)P12​​(x2​−x1​)(y−y1​)+(x2​−x1​)(y2​−y1​)P22​​(x−x1​)(y−y1​)​由于相邻四个点x,y只差1，即在这里插入图片描述
所以，上式化简为 P = P 11 ( x 2 − x ) ( y 2 − y ) + P 21 ( x − x 1 ) ( y 2 − y ) + P 12 ( x 2 − x ) ( y − y 1 ) + P 22 ( x − x 1 ) ( y − y 1 ) \begin{aligned}P&=P_{11}(x_2-x)(y_2-y)+P_{21}(x-x_1)(y_2-y)\\&+P_{12}(x_2-x)(y-y_1)+P_{22}(x-x_1)(y-y_1)\end{aligned} P​=P11​(x2​−x)(y2​−y)+P21​(x−x1​)(y2​−y)+P12​(x2​−x)(y−y1​)+P22​(x−x1​)(y−y1​)​【直通车】——附：参考自视频-图像处理·双线性插值

（2）GFTTDetector函数介绍

Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20);

这行代码是在使用OpenCV库创建一个GFTT（Good Features to Track）检测器对象。GFTT是一个计算机视觉中用于特征点检测的方法，常用于目标跟踪、图像拼接等应用中。

Ptr detector：这里定义了一个指向GFTTDetector类的智能指针detector。Ptr是OpenCV中的一个模板类，用于自动管理对象的生命周期，类似于C++11中的std::shared_ptr。当Ptr对象超出其作用域或被重置时，它会自动删除其所指向的对象。

GFTTDetector::create(500, 0.01, 20)：这是调用GFTTDetector类的静态成员函数create来创建一个GFTTDetector实例。这个函数接受三个参数：

  • 500：这是要检测的最大特征点数量。在这个例子中，检测器将尝试找到最多500个特征点。
  • 0.01：这是特征点检测的质量等级，值范围通常在0到1之间（或表示为百分比）。这个值越高，检测到的特征点就越少，但每个特征点的“质量”或“独特性”通常也越高。这里的0.01是一个相对较低的值，意味着检测器会尝试找到相对较多的特征点，尽管它们可能不是最独特或最显著的。
  • 20：这是两个特征点之间的最小距离。这有助于确保检测到的特征点在图像中分布均匀，而不是都集中在图像的某个特定区域。

总的来说，这行代码创建了一个GFTTDetector对象，该对象被配置为在图像中查找最多500个特征点，这些特征点的质量至少为0.01，并且特征点之间的最小距离为20个像素单位。这个检测器之后可以用于在图像中查找符合这些条件的特征点。

（3）梯度计算

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

这段代码是在计算图像中某个关键点（kp）附近像素的强度梯度。具体地说，它是在一个二维图像（img1）中，对于给定的关键点位置（kp.pt.x, kp.pt.y），计算该点在水平和垂直方向上的强度梯度。这里的x和y可能是相对于关键点位置的某个偏移量，用于在关键点周围的某个具体位置计算梯度。

水平梯度计算：

  • GetPixelValue(img1, kp.pt.x + x + 1, kp.pt.y + y) 获取关键点右侧紧邻像素的值。
  • GetPixelValue(img1, kp.pt.x + x - 1, kp.pt.y + y) 获取关键点左侧紧邻像素的值。
  • 两者之差的一半表示该点在水平方向上的梯度（或称为x方向上的导数近似值）。

垂直梯度计算：

  • GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y + 1) 获取关键点下侧紧邻像素的值。
  • GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y - 1) 获取关键点上侧紧邻像素的值。
  • 两者之差的一半表示该点在垂直方向上的梯度（或称为y方向上的导数近似值）。

最后，这两个梯度值被组合成一个Eigen::Vector2d类型的向量J，其中J[0]是水平梯度，J[1]是垂直梯度。

这种梯度计算在计算机视觉中非常常见，特别是在特征检测、光流估计、图像配准等任务中。梯度提供了关于图像局部强度和方向变化的重要信息。在这个例子中，负号可能是为了将梯度方向与强度增加的方向对应起来（这取决于具体的梯度定义和后续应用）。不过，在很多情况下，梯度的具体方向并不是关键，而是其大小和方向的变化更为重要。

2. 直接法 direct_method.cpp

// 引入OpenCV库，一个开源的计算机视觉和机器学习库  
#include <opencv2/opencv.hpp>  
// 引入Sophus库，一个用于处理3D变换（如旋转和平移）的C++库  
#include <sophus/se3.hpp>  
// 引入Boost库中的format模块，用于字符串格式化  
#include <boost/format.hpp>  
//一个用于视觉SLAM的轻量级GUI库
#include <pangolin/pangolin.h>  // 注意：这里可能是拼写错误，应该是Pangolin而不是Pangolin  
  
// 使用C++标准库的命名空间  
using namespace std;  
  
// 定义一个使用Eigen对齐分配器的vector，用于存储2D向量  
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;  
  
// 相机内参 
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;  
// 双目相机的基线长度  
double baseline = 0.573;  
// 图像文件的路径  
string left_file = "./left.png";  
string disparity_file = "./disparity.png";  
// 用于格式化文件名的boost::format对象  
boost::format fmt_others("./%06d.png"); // 其他文件  
  
// 定义一些方便的别名  
typedef Eigen::Matrix<double, 6, 6> Matrix6d;  // 6x6的双精度矩阵  
typedef Eigen::Matrix<double, 2, 6> Matrix26d; // 2x6的双精度矩阵  
typedef Eigen::Matrix<double, 6, 1> Vector6d;  // 6x1的双精度向量  
  
/// 用于并行计算中累积雅可比矩阵的类  
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_) {  
        // 初始化投影点的vector，大小和px_ref相同，初始值为(0, 0)  
        projection = VecVector2d(px_ref.size(), Eigen::Vector2d(0, 0));  
    }  
  
    // 在给定的范围内累积雅可比矩阵  
    void accumulate_jacobian(const cv::Range &range);  
  
    // 获取海森矩阵  
    Matrix6d hessian() const { return H; }  
  
    // 获取偏差向量  
    Vector6d bias() const { return b; }  
  
    // 获取总代价  
    double cost_func() const { return cost; }  
  
    // 获取投影点  
    VecVector2d projected_points() const { return projection; }  
  
    // 重置H, b, cost为0  
    void reset() {  
        H = Matrix6d::Zero();  // 将H初始化为6x6的零矩阵  
        b = Vector6d::Zero();  // 将b初始化为6x1的零向量  
        cost = 0;  // 将代价重置为0  
    }  
  
private:  
    // 成员变量：两张图像、参考像素坐标、参考深度值、相机间的变换矩阵、投影点等  
    const cv::Mat &img1;  
    const cv::Mat &img2;  
    const VecVector2d &px_ref;  
    const vector<double> depth_ref;  
    Sophus::SE3d &T21;  
    VecVector2d projection;  // 投影点  
  
    // 用于保护海森矩阵H的互斥锁（在多线程环境中防止数据竞争）  
    std::mutex hessian_mutex;    
    Matrix6d H = Matrix6d::Zero();  // 初始化为6x6的零矩阵，用于存储海森矩阵  
    Vector6d b = Vector6d::Zero();  // 初始化为6x1的零向量，用于存储偏差向量  
    double cost = 0;  // 代价函数的值，初始化为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  
); // 此函数定义了一个单层直接法位姿估计的接口
  
// 双线性插值函数  
inline float GetPixelValue(const cv::Mat &img, float x, float y) {  
    // 边界检查  
    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);  
  
    // 使用随机数生成器在左图像中随机选择像素，并生成对应的三维点  
    cv::RNG rng;  
    int nPoints = 2000; // 选择的点数  
    int boarder = 20; // 边界宽度，避免选择图像边缘的像素  
    VecVector2d pixels_ref; // 参考像素坐标  
    vector<double> depth_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); // 获取视差值  
        double depth = fx * baseline / disparity; // 计算深度值  
        depth_ref.push_back(depth); // 存储深度值  
        pixels_ref.push_back(Eigen::Vector2d(x, y)); // 存储像素坐标  
    }  
  
    // 估计01~05.png图像的位姿  
    Sophus::SE3d T_cur_ref; // 当前图像相对于参考图像的位姿  
  
    // 遍历01~05.png图像，进行位姿估计  
    for (int i = 1; i < 6; i++) {  
        cv::Mat img = cv::imread((fmt_others % i).str(), 0); // 读取图像  
        // 可以尝试取消注释以下行以使用单层直接法位姿估计  
        // 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;  // 设定迭代次数为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();  
  
        // 使用LDLT分解法求解更新量  
        Vector6d update = H.ldlt().solve(b);  
  
        // 更新位姿变换  
        T21 = Sophus::SE3d::exp(update) * T21;  
  
        // 计算当前位姿下的代价  
        cost = jaco_accu.cost_func();  
  
        // 检查更新量是否为NaN，如果是则中断迭代  
        if (std::isnan(update[0])) {  
            cout << "update is nan" << endl;  
            break;  
        }  
  
        // 如果代价增加，中断迭代  
        if (iter > 0 && cost > lastCost) {  
            cout << "cost increased: " << cost << ", " << lastCost << endl;  
            break;  
        }  
  
        // 如果更新量的范数小于阈值，则认为已经收敛，中断迭代  
        if (update.norm() < 1e-3) {  
            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;  
  
    // 在图像上显示投影点  
    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) {  
    // 定义并初始化一些参数  
    const int half_patch_size = 1;  // 区块半径  
    int cnt_good = 0;  // 记录有效点的数量  
    Matrix6d hessian = Matrix6d::Zero();  // 初始化海森矩阵为0  
    Vector6d bias = Vector6d::Zero();  // 初始化偏置向量为0  
    double cost_tmp = 0;  // 初始化临时代价为0  
  
    // 遍历给定的范围  
    for (size_t i = range.start; i < range.end; i++) {  
  
        // 通过参考图像中的点计算在当前图像中的投影  
        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;  // 使用变换矩阵T21进行变换  
        if (point_cur[2] < 0)   // 如果深度无效，则跳过此点  
            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++;  // 有效点数量加1  
  
        // 在补丁范围内计算误差和雅可比矩阵  
        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，即像素坐标对相机位姿的偏导数  
                // ... (此处省略了具体的偏导数计算过程，代码已直接给出结果)  
  
                // 计算图像梯度J_img_pixel，使用中心差分法  
                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))  
                );  
  
                // 计算总的雅可比矩阵J  
                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) {  
        unique_lock<mutex> lck(hessian_mutex);  // 使用互斥锁保证线程安全  
        H += hessian;  // 累加海森矩阵  
        b += bias;  // 累加偏置向量  
        cost += cost_tmp / cnt_good;  // 计算平均成本  
    }  
}
// 函数声明：直接姿态估计多层方法  
// 输入：两个图像(img1, img2)，参考像素坐标(px_ref)，参考深度值(depth_ref)  
// 输出：从第一个图像坐标系到第二个图像坐标系的变换矩阵T21  
void DirectPoseEstimationMultiLayer(  
    const cv::Mat &img1,         // 第一个输入图像  
    const cv::Mat &img2,         // 第二个输入图像  
    const VecVector2d &px_ref,   // 参考像素坐标  
    const vector<double> depth_ref, // 参考像素对应的深度值  
    Sophus::SE3d &T21) {        // 输出变换矩阵  
  
    // 定义参数  
    int pyramids = 4;            // 图像金字塔的层数  
    double pyramid_scale = 0.5;  // 金字塔每一层的缩放比例  
    double scales[] = {1.0, 0.5, 0.25, 0.125}; // 金字塔每一层的实际缩放系数  
  
    // 创建图像金字塔  
    vector<cv::Mat> pyr1, pyr2;  // 图像金字塔  
    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;  // 备份原始的相机内参  
    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. 报错解决方案

（1）路径问题

修改2个cpp文件路径如下

  • optical_flow.cpp
string file_1 = "/home/user_name/slambook2/ch8/LK1.png";  // first image
string file_2 = "/home/user_name/slambook2/ch8/LK2.png";  // second image
  • direct_method.cpp
string left_file = "/home/user_name/slambook2/ch8/left.png";
string disparity_file = "/home/user_name/slambook2/ch8/disparity.png";
boost::format fmt_others("/home/user_name/slambook2/ch8/%06d.png");    // other files

（2）opencv4安装

问题1：error: invalid initialization of reference of type ‘const cv::ParallelLoopB

原因1：之前章节安装的是opencv3，而本章运用了一些opencv4中的函数，且CMakeLists.txt中也有find_package(OpenCV 4 REQUIRED)，故直接安装opencv4

解决1：参考：opencv4安装步骤

问题2：error: “CV_GRAY2BGR was not declared in this scope cv::cvtColor(img2，img2_single，CV_GRAY2BGR)”

解决2：将optical_flow.cpp与direct_method.cpp两个文件中的CV_GRAY2BGR 改为 cv::COLOR_GRAY2BGR（注意区分大小写）。Ctrl+f 查找并修改（2个文件共需修改4处）

光流 | 基于光流法实现视觉里程计Visual Odometry（源代码）
尘世冰封的专栏
05-09 542
===================================================== github：https://github.com/MichaelBeechan CSDN：https://blog.csdn.net/u011344545 ===================================================== mian.cpp #include <iostream> #include <opencv2/core/core.hp
VO：简单的视觉里程计代码注释（代码可运行）
try_again_later的博客
10-11 5753
走完SLAM十四讲前端之后，代码都已经注释完，但还是感觉有点迷茫，所以专门参考冯兵的博客，实现简单的视觉里程计。 收获是又重新认识到了C++基础的薄弱，决定之后的晚上要刷牛客题。不过就SLAM前端而言这部分基本可以理解代码了，这篇对VO代码进行注释。 基本过程： 1、获取图像 2、对图像进行处理 3、通过FAST算法对图像进行特征检测，通过KLT光流法跟踪图像的特征，如果跟踪的特征有所丢失，特...
SLAMBook（ch7）视觉里程计特征匹配代码笔记
m0_74142169的博客
06-29 197
另外，这里img_1和img_2存储了两张图像的颜色信息，其维度为2维，大小为480*640（这里存储数据的矩阵维度并非480*640，而是480*(640*3)，因为要存储每个像素点的BGR信息，因此每个像素的信息对应1行和3列）。DRAW_RICH_KEYPOINTS：绘制特征点的时候绘制的是一个个带有方向的圆,这种方法同时显示图像的坐标,size，和方向,是最能显示特征的一种绘制方式。DEFAULT：只绘制特征点的坐标点,显示在图像上就是一个个小圆点,每个小圆点的圆心坐标都是特征点的坐标。
单目视觉里程记代码
qq_33898609的博客
04-11 1661
视觉slam /* The MIT License Copyright (c) 2015 Avi Singh Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Softwar...
视觉里程计 -- 单层多层 直接法（附代码解析）
蓝羽飞鸟的博客
04-06 750
直接法是根据像素的亮度信息(Intensity）来估计相机的运动。 还记得估计相机的运动有以下方法： 1.提取特征点和描述子，比如SIFT，FAST特征点，不同视角的图片特征点匹配，然后用对极几何，PnP或ICP法估计相机运动。 2. 提取特征点，不提取描述子，然后用光流法跟踪特征点的运动，再估计相机运动。 而直接法可以在特征点的基础上计算，也可以连特征点都不提取了，直接随机采点。 也不用计算光流，不需要知道点和点之间的对应关系，而是通过最小化光度误差来得到，是一个优化问题。 直接法相比于特征点，克服了以下
视觉SLAM第八讲视觉里程计2--- LK光流---代码详细讲解
weixin_51547017的博客
03-31 1449
光流是一种描述像素随时间在图像之间运动的方法： 光流法有两个假设：（1）灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的；（2）假设某个窗口内的像素具有相同的运动 一、本讲的代码使用了三种方法来追踪图像上的特征点 （1）第一种：使用OpenCV中的LK光流； （2）第二种：用高斯牛顿实现光流：单层光流； （3）第三种：用高斯牛顿实现光流：多层光流。 其中高斯牛顿法，即最小化灰度误差估计最优的像素偏移。在具体函数实现中（即calculateOpticalFlow)，求解这样一个问题： 二、代
SLAM 06.视觉里程计-3-直接法
jiankangyq的博客
08-12 2223
直接法跳过了提取特征点的步骤。它构建一个优化问题，直接根据像素信息（通常是亮度），来估计相机的运动。这种方法省去了提特征的时间，然而代价则是，利用了所有信息之后，使得优化问题规模远远大于使用特征点的规模。因此，基于直接法的VO，多数需要GPU加速，才能做到实时化。...
RTAB-MAP开源视觉-激光-里程计SLAM代码
04-22
RTAB-MAP压缩包里有以下个开源代码： 1、RTAB-Map as an open-source lidar and visual simultaneous localization and mapping library for large-scale and long-term online operation-2018.pdf 2、RTABMAP_...
视觉里程计+基于深度学习的图像匹配算法研究+视觉SLAM
06-26
综上所述，这个研究主题涉及了计算机视觉的核心技术，包括深度学习驱动的图像匹配算法、视觉里程计的实现以及视觉SLAM系统的构建。通过Python实现，我们可以深入理解这些概念，并有可能进行实际的实验和改进，这对于...
视觉SLAM学习打卡【8-1】-视觉里程计·直接法
小幸运召唤师的博客
04-06 1869
本节直接法与上节特征点法，为视觉里程计估计位姿的两大主流方法。而在引出直接法前，先介绍光流法（二者均对灰度值 I 做文章）。
机器人视觉-移动机器人-VS-SLAM-ORB-SLAM2-深度学习目标检测-yolov3-行为检测-MVision.zip
08-29
机器人视觉_移动机器人_VS-SLAM_ORB-SLAM2_深度学习目标检测_yolov3_行为检测_MVision.zip机器人视觉_移动机器人_VS-SLAM_ORB-SLAM2_深度学习目标检测_yolov3_行为检测_MVision.zip 机器人视觉_移动机器人_VS-SLAM_...
视觉里程计VO
weixin_30784141的博客
06-07 221
视觉里程计的主要问题是如何根据图像来估计相机运动，VO的实现方法，按照是否需要提取特征，分为特征点法的前端以及不提取特征的直接法前端。基于特征点法的前端，长久以来被认为是视觉里程计的主流方法，它运行稳定，对光照、动态物体不敏感，是目前较为成熟的解决方案。 计算机视觉邻域的研究者们，设计了许多比角点更加稳定的局部图像特征，比如SIFT,SURF,ORB等。特征点由关键点（Key-point)和描述...
视觉里程计
04-15
视觉里程计（visual odometry）
SLAM视觉里程计高端代码.zip
05-13
基于C++语言，建立工程文件，编写SLAM视觉里程计高端代码
SLAM项目：从0开始，利用C++复现一个基于优化算法的视觉里程计，详细解释原理和代码实现
weixin_43890835的博客
06-26 2228
这篇文章的主要记录我对于基于优化方法的视觉里程计的理解，并利用C++代码实现了一个输入两张图片和图片对应的深度，输出一个位姿变换的视觉里程计算法。 视觉里程计是视觉SLAM中最重要的部分之一，其位于整个视觉SLAM的前端，核心任务是为整个SLAM过程提供一个不错的位姿估计，实现视觉里程计的方法有很多，本文主要探讨基于优化方法实现的视觉里程计的原理和代码实现。
视觉SLAM实践入门——（20）视觉里程计之直接法
floatinglong的博客
06-07 1052
啊
SLAM-具有SLAM功能+3D网格生成功能的视觉惯性里程计-优质项目实战.zip
最新发布
10-20
SLAM_具有SLAM功能+3D网格生成功能的视觉惯性里程计_优质项目实战
视觉里程计设计与实现
chengde6896383的专栏
10-31 1864
https://www.jianshu.com/p/484e4c2b020a 一、前言 视觉里程计与传统的里程计不同，不使用码盘等设备，只利用摄像头拍摄的连续图像帧就可以计算里程，非常方便，因而用途广泛。本文介绍SLAM中的视觉里程计的设计与实现。 有了前面的基础，我们就可以着手做一个简单的视觉里程计（Visual Odometry，VO）。VO作为SLAM系统的前端，负责接收图像序列，通过...
计算机视觉与深度学习 | 视觉里程计综述（框架+算法）
尘世冰封的专栏
05-10 3193
博主github：https://github.com/MichaelBeechan 博主CSDN：https://blog.csdn.net/u011344545 OF-VO：Robust and Efficient Stereo Visual Odometry Using Points and Feature Optical Flow 代码：https://github.com/Michael...
SLAM代码（ORB-SLAM阅读）
何必浓墨重彩
12-07 3750
主要步骤 1. 为什么是这样的步骤，为什么只要这几个步骤，为什么是这样的顺序？ 2. 这样做的目的是什么？步骤1.1ORB的跟踪器使用一个运动模型，在每次完成位姿计算之后会跟新这个运动模型，根据匀速度模型进行对上一帧的MapPoints进行跟踪，根据上一帧特征点对应的3D点投影的位置缩小特征点匹配范围。这里意思是说我们假设相机是匀速运动的，然后根据上一帧的位姿，我么预测到这一帧对应的位姿，然后使
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值