【slam十四讲第二版】【课本例题代码向】【第七讲~视觉里程计Ⅱ】【使用LK光流（cv）】【高斯牛顿法实现单层光流和多层光流】【实现单层直接法和多层直接法】
2024-05-09 03:37:15
【slam十四讲第二版】【课本例题代码向】【第七讲~视觉里程计Ⅱ】【使用LK光流（cv）】【高斯牛顿法实现单层光流和多层光流】【实现单层直接法和多层直接法】
- 0 前言
- 1 使用LK光流（cv）
- 1.1 前言
- 1.2 useLK.cpp
- 1.3 CMakeLists.txt
- 1.4 输出数据为
- 1.5 感悟
- 2 高斯牛顿法实现单层光流和多层光流
- 2.1 前言
- 2.2 optical_flow.cpp
- 2.3 CMakeLists.txt
- 2.4 输出结果为
- 2.5 感悟
- 3 直接法
- 3.1 前言
- 3.2 direct_method.cpp
- 3.3 CMakeLists.txt
- 3.4 输出结果
- 3.5 感悟
【slam十四讲第二版】【课本例题代码向】【第三~四讲刚体运动、李群和李代数】【eigen3.3.4和pangolin安装，Sophus及fim的安装使用】【绘制轨迹】【计算轨迹误差】
【slam十四讲第二版】【课本例题代码向】【第五讲~相机与图像】【OpenCV、图像去畸变、双目和RGB-D、遍历图像像素14种】
【slam十四讲第二版】【课本例题代码向】【第六讲~非线性优化】【安装对应版本ceres2.0.0和g2o教程】【手写高斯牛顿、ceres2.0.0、g2o拟合曲线及报错解决方案】
【slam十四讲第二版】【课本例题代码向】【第七讲~视觉里程计Ⅰ】【1OpenCV的ORB特征】【2手写ORB特征】【3对极约束求解相机运动】【4三角测量】【5求解PnP】【3D-3D:ICP】
0 前言
- 这一章节东西不多，希望快点学完
- 该章节所使用的数据集自取：链接: https://pan.baidu.com/s/1daebAx4DNHOtKrX8Up4lew 提取码: dcf5
1 使用LK光流（cv）
1.1 前言
- 这个部分仅仅涉及了如何配置cv中的LK函数的参数问题
- 其中所需要的数据集，以及我的工程包自取：链接：https://pan.baidu.com/s/1mlBdAiSiy2-jc23fYRyImg
提取码：20dv
1.2 useLK.cpp
#include <iostream>
using namespace std;#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>#include <fstream>
#include <list>
#include <vector>
#include <chrono>int main(int argc, char** argv)
{string path_to_dataset;if (argc != 2){path_to_dataset = "../data";}else{path_to_dataset = argv[1];}string associate_file = path_to_dataset + "/associate.txt";ifstream fin(associate_file);if (!fin){cerr<<"I cann't find associate.txt!"<<endl;return 1;}string rgb_file, depth_file, time_rgb, time_depth;list< cv::Point2d > keypoints;// 因为要删除跟踪失败的点，使用listcv::Mat color, depth, last_color;for (int index = 0; index < 100; index++) {fin >> time_rgb >> rgb_file >> time_depth >> depth_file;color = cv::imread(path_to_dataset + "/" + rgb_file);//默认是IMREAD_COLOR = 1，彩色图depth = cv::imread(path_to_dataset + "/" + depth_file, -1);//-1 =IMREAD_UNCHANGED，深度图if (index == 0) {// 对第一帧提取FAST特征点vector<cv::KeyPoint> kps;cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();detector->detect(color, kps);for (auto kp: kps)keypoints.push_back(kp.pt);last_color = color;continue;}if (color.data == nullptr || depth.data == nullptr) continue;// 对其他帧用LK跟踪特征点vector<cv::Point2f> next_keypoints;vector<cv::Point2f> prev_keypoints;for (auto kp: keypoints)prev_keypoints.push_back(kp);vector<unsigned char> status;vector<float> error;chrono::steady_clock::time_point t1 = chrono::steady_clock::now();cv::calcOpticalFlowPyrLK( last_color, color, prev_keypoints, next_keypoints, status, error );chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );cout<<"LK Flow use time："<<time_used.count()<<" seconds."<<endl;// 把跟丢的点删掉int i = 0;for (auto iter = keypoints.begin(); iter != keypoints.end(); i++) {if (status[i] == 0) {iter = keypoints.erase(iter);continue;}*iter = next_keypoints[i];iter++;}cout << "tracked keypoints: " << keypoints.size() << endl;if (keypoints.size() == 0) {cout << "all keypoints are lost." << endl;break;}// 画出 keypointscv::Mat img_show = color.clone();for (auto kp: keypoints)cv::circle(img_show, kp, 10, cv::Scalar(0, 240, 0), 1);cv::imshow("corners", img_show);cv::waitKey(0);last_color = color;}return 0;
}1.3 CMakeLists.txt
cmake_minimum_required(VERSION 2.8)project(useLK)set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_STANDARD 14)find_package(OpenCV 3 REQUIRED)include_directories(${OpenCV_INCLUDE_DIRECTORIES})add_executable(useLK src/useLK.cpp)target_link_libraries(useLK ${OpenCV_LIBRARIES})
1.4 输出数据为
/home/bupo/shenlan/zuoye/cap8/useLK/cmake-build-release/useLK /home/bupo/shenlan/zuoye/cap8/useLK/data
LK Flow use time：0.0401681 seconds.
tracked keypoints: 1749
LK Flow use time：0.0229127 seconds.
tracked keypoints: 1742
LK Flow use time：0.0223572 seconds.
tracked keypoints: 1703
LK Flow use time：0.0220847 seconds.
tracked keypoints: 1676
LK Flow use time：0.0232433 seconds.
tracked keypoints: 1664
LK Flow use time：0.0211483 seconds.
tracked keypoints: 1656
LK Flow use time：0.0216098 seconds.
tracked keypoints: 1641
LK Flow use time：0.0220525 seconds.
tracked keypoints: 1634进程已结束,退出代码0
- 图片单张看并直观，最好亲身实践对比更为直观
1.5 感悟
- 暂无
2 高斯牛顿法实现单层光流和多层光流
2.1 前言
- 工程所必须的图片，以及我的实现工程自取：链接：https://pan.baidu.com/s/1CCNdyz1ozrX47z4s49CE5A
提取码：gpjc
2.2 optical_flow.cpp
//
// Created by czy on 2022/4/10.
//#include <opencv2/opencv.hpp>
#include <string>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>using namespace std;
using namespace cv;string file_1 = "../src/LK1.png"; // first image
string file_2 = "../src/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 checkif (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) {// images, note they are CV_8UC1, not CV_8UC3Mat img1 = imread(file_1, 0);//如果设置，总是将图像转换为单通道灰度图像。Mat img2 = imread(file_2, 0);// key points, using GFTT here.vector<KeyPoint> kp1;/** 之前角点检测的算法中,一个是cornerHarris计算角点，但* 是这种角点检测算法容易出现聚簇现象以及角点信息有丢失和位置偏移现象，* 所以后面又提出一种名为goodFeatureToTrack的角点检测算法，* opencv的feature2D接口集成了这种算法,名称为 GFTTDetector。* */Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypointsdetector->detect(img1, kp1);// now lets track these key points in the second image//现在让我们在第二张图中跟踪这些关键点// first use single level LK in the validation picture//首先在验证图中使用单级LKvector<KeyPoint> kp2_single;vector<bool> success_single;OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single);// then test multi-level LK然后测试多级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 validationvector<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 functionsMat 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);//Range表示一个范围，即并行计算哪些需要追踪的点,kp1中存放的就是随机选的追踪点//bind是一个绑定函数，它相当于调用OpticalFlowTracker.calculateOpticalFlow()，//std::placeholders::_1是占位符，表示传入的参数是OpticalFlowTracker.calculateOpticalFlow()的第一个参数，// 此处Range(0, kp1.size())为传入的参数。parallel_for_(Range(0, kp1.size()),std::bind(&OpticalFlowTracker::calculateOpticalFlow, &tracker, placeholders::_1));
}void OpticalFlowTracker::calculateOpticalFlow(const Range &range) {// parametersint 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 estimatedif (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 iterationsEigen::Matrix2d H = Eigen::Matrix2d::Zero(); // hessianEigen::Vector2d b = Eigen::Vector2d::Zero(); // biasEigen::Vector2d J; // jacobianfor (int iter = 0; iter < iterations; iter++){/** 这里是采用反向（Inverse）光流法。* 在反向的光流法中，可以使用第一个图像的像素梯度代替第二个像素的梯度* 在反向光流法中，第一个图像的向素梯度是不变的，所以可以保留第一次迭代的结果，在后续的迭代中使用* 当雅可比不变时，H矩阵不变，每次迭代只需计算残差，可以节省一部分计算量*/if (inverse == false) {//H = Eigen::Matrix2d::Zero();b = Eigen::Vector2d::Zero();} else {// only reset bb = Eigen::Vector2d::Zero();}cost = 0;// compute cost and jacobianfor (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);; // Jacobianif (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 errorJ = -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 HH += J * J.transpose();}}// compute updateEigen::Vector2d update = H.ldlt().solve(b);if (std::isnan(update[0])) {// sometimes occurred when we have a black or white patch and H is irreversiblecout << "update is nan" << endl;succ = false;break;}if (iter > 0 && cost > lastCost) {break;}// update dx, dydx += update[0];dy += update[1];lastCost = cost;succ = true;if (update.norm() < 1e-2) {// convergebreak;}}success[i] = succ;// set kp2kp2[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) {// parametersint 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 pyramidsfor (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从粗到细LK金字塔跟踪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 finesuccess.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.3 CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(optical_flow)set(CMAKE_BUILD_TYPE "Release")
#add_definitions("-DENABLE_SSE")
#set(CMAKE_CXX_FLAGS "-std=c++11 ${SSE_FLAGS} -g -O3 -march=native")
set(CMAKE_CXX_STANDARD 14)
find_package(OpenCV 3 REQUIRED)include_directories(${OpenCV_INCLUDE_DIRS}"/usr/include/eigen3/"
)add_executable(optical_flow src/optical_flow.cpp)
target_link_libraries(optical_flow ${OpenCV_LIBS})2.4 输出结果为
/home/bupo/shenlan/zuoye/cap8/optical_flow/cmake-build-release/optical_flow
build pyramid time: 0.000120757
track pyr 3 cost time: 0.00112688
track pyr 2 cost time: 0.000585512
track pyr 1 cost time: 0.000655574
track pyr 0 cost time: 0.00117114
optical flow by gauss-newton: 0.0200876
optical flow by opencv: 0.00293408进程已结束,退出代码0
2.5 感悟
- 计算时间上没法作比较，但是这样看，多层光流法的耗时和OpenCV大致相当
- 从结果上看，多层光流与OpenCV的效果相当，单层光流要明显弱于多层光流
- 光流对图像的连续性和光照稳定性要求更高一些
- 如果角点提的位置不好，光流也容易跟丢或给出错误的结果，需要后续算法拥有一定的异常值去除机制。
- 光流法可以加速基于特征点的视觉里程计算法，避免计算和匹配描述子的过程，但要求相机运动较平滑（或采样频率较高）
3 直接法
3.1 前言
- 此工程所必需的图片，以及我的工程实现自取：链接：https://pan.baidu.com/s/1kHG3Ukcw3MxgXVCN3GkzIA
提取码：v1vj
3.2 direct_method.cpp
#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>
#include <pangolin/pangolin.h>
#include <mutex>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 = "../src/left.png";
string disparity_file = "../src/disparity.png";
boost::format fmt_others("../src/%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;/*** 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 checkif (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]);
}/// class for accumulator jacobians in parallel并行累加器雅可比矩阵的类
class JacobianAccumulator: public cv::ParallelLoopBody {private:const cv::Mat &img1;const cv::Mat &img2;const VecVector2d &px_ref;const vector<double> depth_ref;Sophus::SE3d &T21;mutable VecVector2d projection; // projected pointsmutable std::mutex hessian_mutex;mutable Matrix6d H = Matrix6d::Zero();mutable Vector6d b = Vector6d::Zero();mutable double cost = 0;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 matrixMatrix6d hessian() const { return H; }/// get biasVector6d bias() const { return b; }/// get total costdouble cost_func() const { return cost; }/// get projected pointsVecVector2d projected_points() const { return projection; }/// reset h, b, cost to zerovoid reset() {H = Matrix6d::Zero();b = Vector6d::Zero();cost = 0;}virtual void operator()(const cv::Range& range) const {// parametersconst 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 invalidcontinue;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 jacobianfor (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 jacobianVector6d 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 costunique_lock<mutex> lck(hessian_mutex);H += hessian;b += bias;cost += cost_tmp / cnt_good;}}};int main(int argc, char **argv) {cv::Mat left_img = cv::imread(left_file, 0);cv::Mat disparity_img = cv::imread(disparity_file, 0);if (left_img.empty() || disparity_img.empty()){std::cout << "!!! Failed imread(): image not found" << std::endl;return 1;}// let's randomly pick pixels in the first image and generate some 3d points in the first image's frame//让我们在第一个图像中随机选取像素，并在第一个图像的帧中生成一些3d点cv::RNG rng;int nPoints = 2000;int boarder = 20;VecVector2d pixels_ref;vector<double> depth_ref;cout << "left_img.cols" << left_img.cols << endl;//cout << "left_img: " << left_img << endl;// generate pixels in ref and load depth data在ref和加载深度数据中生成像素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 boarderint disparity = disparity_img.at<uchar>(y, x);double depth = fx * baseline / disparity; // you know this is disparity to depth 这是深度差 p104depth_ref.push_back(depth);pixels_ref.push_back(Eigen::Vector2d(x, y));}// estimates 01~05.png's pose using this information 利用这些信息估计01~05.png的姿势Sophus::SE3d T_cur_ref_multi;for (int i = 1; i < 6; i++) { // 1~10cv::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_multi);}/*//单层直接法Sophus::SE3d T_cur_ref_single;for (int i = 1; i < 6; i++) { // 1~10cv::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_single);//DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref_multi);}*/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()), jaco_accu);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 irreversiblecout << "update is nan" << endl;break;}if (iter > 0 && cost > lastCost) {cout << "cost increased: " << cost << ", " << lastCost << endl;break;}if (update.norm() < 1e-3) {// convergebreak;}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 herecv::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 DirectPoseEstimationMultiLayer(const cv::Mat &img1,const cv::Mat &img2,const VecVector2d &px_ref,const vector<double> depth_ref,Sophus::SE3d &T21) {// parametersint pyramids = 4;double pyramid_scale = 0.5;double scales[] = {1.0, 0.5, 0.25, 0.125};// create pyramidsvector<cv::Mat> pyr1, pyr2; // image pyramidsfor (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 valuesfor (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, fy, cx, cy在不同的金字塔层次上进行缩放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.3 CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(direct_method)set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_STANDARD 14 )find_package(OpenCV 3 REQUIRED)
find_package(Sophus REQUIRED)
find_package(Pangolin REQUIRED)include_directories(${OpenCV_INCLUDE_DIRS}${Sophus_INCLUDE_DIRS}"/usr/include/eigen3/"${Pangolin_INCLUDE_DIRS}
)add_executable(direct_method src/direct_method.cpp)
target_link_libraries(direct_method${OpenCV_LIBS}${Pangolin_LIBRARIES}${Sophus_LIBRARIES} fmt)3.4 输出结果
iteration: 0, cost: 396047
iteration: 1, cost: 170343
iteration: 2, cost: 67689.3
iteration: 3, cost: 41756.8
iteration: 4, cost: 41334.7
cost increased: 41340.7, 41334.7
T21 = 0.999992 0.00236441 0.00334249 0.00504878
-0.00237328 0.999994 0.00265186 0.00118042
-0.00333619 -0.00265977 0.999991 -0.7339650 0 0 1
direct method for single layer: 0.014914
iteration: 0, cost: 51023
iteration: 1, cost: 49681
cost increased: 49726.2, 49681
T21 = 0.999989 0.00303736 0.00345483 0.00149286-0.0030451 0.999993 0.00223785 0.00662606
-0.00344801 -0.00224834 0.999992 -0.7290610 0 0 1
direct method for single layer: 0.00205969
iteration: 0, cost: 70562.6
iteration: 1, cost: 65330.6
T21 = 0.999991 0.0025155 0.00346069 -0.00257406
-0.00252359 0.999994 0.00233607 0.00238353-0.0034548 -0.00234479 0.999991 -0.7346460 0 0 1
direct method for single layer: 0.00266652
iteration: 0, cost: 94610.3
T21 = 0.999991 0.00248065 0.00343352 -0.00373127-0.0024882 0.999994 0.00219463 0.00304309
-0.00342806 -0.00220315 0.999992 -0.7323340 0 0 1
direct method for single layer: 0.00196489
iteration: 0, cost: 343266
iteration: 1, cost: 227570
iteration: 2, cost: 143200
iteration: 3, cost: 92257.3
iteration: 4, cost: 65107.9
iteration: 5, cost: 57510.2
cost increased: 57801.7, 57510.2
T21 = 0.999972 0.000930072 0.00740586 0.0130114
-0.000964031 0.999989 0.00458318 0.00227666-0.00740151 -0.0045902 0.999962 -1.459840 0 0 1
direct method for single layer: 0.00456659
iteration: 0, cost: 84146.3
iteration: 1, cost: 82026.1
cost increased: 82319.1, 82026.1
T21 = 0.999971 0.00111773 0.00759593 0.00445965
-0.00114863 0.999991 0.00406483 0.00340717
-0.00759132 -0.00407343 0.999963 -1.471390 0 0 1
direct method for single layer: 0.00226185
iteration: 0, cost: 131483
iteration: 1, cost: 128277
cost increased: 130601, 128277
T21 = 0.99997 0.000717595 0.00767486 -0.00126053
-0.000747681 0.999992 0.00391796 0.00298949-0.00767198 -0.00392358 0.999963 -1.482120 0 0 1
direct method for single layer: 0.0051404
iteration: 0, cost: 116201
iteration: 1, cost: 107584
T21 = 0.999971 0.000699842 0.0075908 -0.00251934
-0.000728114 0.999993 0.0037224 0.00402137-0.00758814 -0.00372782 0.999964 -1.48140 0 0 1
direct method for single layer: 0.00574368
iteration: 0, cost: 343049
iteration: 1, cost: 278665
iteration: 2, cost: 228254
cost increased: 259728, 228254
T21 = 0.999936 0.00162163 0.0111886 -0.0311607
-0.00171416 0.999964 0.00826583 -0.060339-0.0111748 -0.00828448 0.999903 -2.029190 0 0 1
direct method for single layer: 0.00641602
iteration: 0, cost: 365842
iteration: 1, cost: 161026
iteration: 2, cost: 144814
cost increased: 178462, 144814
T21 = 0.999932 0.00143823 0.0115848 -0.0100368-0.0015029 0.999983 0.00557577 -0.00555529-0.0115765 -0.0055928 0.999917 -2.163620 0 0 1
direct method for single layer: 0.00550733
iteration: 0, cost: 178501
cost increased: 208142, 178501
T21 = 0.999935 0.00136293 0.0113536 0.00549402
-0.00142462 0.999984 0.00542701 -0.00318685-0.011346 -0.00544282 0.999921 -2.200970 0 0 1
direct method for single layer: 0.00316008
iteration: 0, cost: 199364
iteration: 1, cost: 197451
iteration: 2, cost: 186346
cost increased: 187693, 186346
T21 = 0.999935 0.00135574 0.0113347 0.00437121
-0.00141595 0.999985 0.00530537 -0.00269083-0.0113274 -0.00532107 0.999922 -2.236560 0 0 1
direct method for single layer: 0.00400765
iteration: 0, cost: 374461
iteration: 1, cost: 338582
iteration: 2, cost: 272162
iteration: 3, cost: 239430
iteration: 4, cost: 199236
iteration: 5, cost: 172497
iteration: 6, cost: 149510
iteration: 7, cost: 138870
iteration: 8, cost: 134446
iteration: 9, cost: 133814
T21 = 0.999872 -0.000298538 0.0160126 0.02535760.000182741 0.999974 0.00723266 -0.00504601-0.0160143 -0.0072288 0.999846 -2.966920 0 0 1
direct method for single layer: 0.00723083
iteration: 0, cost: 160214
iteration: 1, cost: 153883
iteration: 2, cost: 153388
cost increased: 154647, 153388
T21 = 0.999865 -0.000321718 0.0163994 0.01314720.000212646 0.999978 0.00665231 -0.00453241-0.0164012 -0.00664793 0.999843 -3.007640 0 0 1
direct method for single layer: 0.00315668
iteration: 0, cost: 228034
iteration: 1, cost: 219766
iteration: 2, cost: 217807
iteration: 3, cost: 216831
T21 = 0.999865 -0.000174516 0.0164416 0.002773367.12694e-05 0.99998 0.00627998 -0.00533273-0.0164424 -0.00627796 0.999845 -3.018190 0 0 1
direct method for single layer: 0.00339888
iteration: 0, cost: 321505
iteration: 1, cost: 305955
iteration: 2, cost: 301146
iteration: 3, cost: 300392
cost increased: 300465, 300392
T21 = 0.999864 0.000259749 0.0164957 -0.0113293
-0.000356412 0.999983 0.00585725 0.000432521-0.0164939 -0.00586233 0.999847 -3.02690 0 0 1
direct method for single layer: 0.00484743
iteration: 0, cost: 417833
iteration: 1, cost: 380020
iteration: 2, cost: 307367
iteration: 3, cost: 240955
iteration: 4, cost: 199866
iteration: 5, cost: 178705
iteration: 6, cost: 170523
iteration: 7, cost: 167044
iteration: 8, cost: 167006
iteration: 9, cost: 165164
T21 = 0.999797 0.000553496 0.0201461 0.0322426
-0.000698722 0.999974 0.00720231 0.0119608-0.0201416 -0.00721492 0.999771 -3.76320 0 0 1
direct method for single layer: 0.00491216
iteration: 0, cost: 247795
iteration: 1, cost: 235625
iteration: 2, cost: 231769
iteration: 3, cost: 230604
cost increased: 231102, 230604
T21 = 0.999785 0.000734281 0.0207129 0.00793156
-0.000875909 0.999976 0.00682944 0.00753941-0.0207073 -0.00684612 0.999762 -3.835120 0 0 1
direct method for single layer: 0.0034232
iteration: 0, cost: 340213
iteration: 1, cost: 332062
iteration: 2, cost: 329315
cost increased: 329633, 329315
T21 = 0.999777 0.00109462 0.0210725 -0.00805546
-0.00122663 0.99998 0.00625257 0.011897-0.0210652 -0.00627702 0.999758 -3.854990 0 0 1
direct method for single layer: 0.00298498
iteration: 0, cost: 429783
iteration: 1, cost: 420138
iteration: 2, cost: 416552
cost increased: 418560, 416552
T21 = 0.999785 0.00137068 0.0206667 -0.00333605
-0.00149613 0.999981 0.00605567 0.00868662-0.020658 -0.00608529 0.999768 -3.860680 0 0 1
direct method for single layer: 0.00266961进程已结束,退出代码0- 我们会用一些实例图片测试直接法的结果，计算相机的位姿
- 为了展示直接法对特征点的不敏感性，随机地在第一张图片中选取一些点，不使用任何角点或特征点提取算法，来看看结果
3.5 感悟
- 书上p230详细介绍了直接法的优缺点
- 上述的结果图中：每个小圆点是此时的点，而每个点都有一个自己的”尾巴“，这个尾巴的末尾就是该点在之前时刻的位置，所以可以看出，摄像头是向前前进的，从而有一种像素扑面而来的可视化
【slam十四讲第二版】【课本例题代码向】【第七讲~视觉里程计Ⅱ】【使用LK光流（cv）】【高斯牛顿法实现单层光流和多层光流】【实现单层直接法和多层直接法】相关推荐
- 视觉SLAM十四讲学习记录 第二讲
书接上回: 第一讲 第二讲 初识SLAM 2.1 引子:小萝卜的例子 首先作者借"小萝卜"这类机器人引出了几个概念: 自主运动能力是很多高级功能的前提,需要定位与感知(建图)来 ...
- 视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点法和特征提取和匹配实践
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习 ...
- 高博-《视觉SLAM十四讲》
0 讲座 (1)SLAM定义 对比雷达传感器和视觉传感器的优缺点(主要介绍视觉SLAM) 单目:不知道尺度信息 双目:知道尺度信息,但测量范围根据预定的基线相关 RGBD:知道深度信息,但是深度信息对 ...
- 视觉SLAM14讲——视觉里程计1（特征点法）
前面说过视觉SLAM系统分为前端和后端两个内容,前端也叫做视觉里程计.视觉里程计的主要作用是根据相邻的两张图像的信息粗略的估计出相机运动,给后端一个较好的初始值.视觉里程计的两大算法为:特征点法和直接 ...
- 【slam十四讲第二版】【课本例题代码向】【第九讲~后端Ⅰ】【安装Meshlab】【BAL数据集格式】【ceres求解BA】【g2o求解BA】
[slam十四讲第二版][课本例题代码向][第九讲~后端Ⅰ][安装Meshlab][BAL数据集格式][ceres求解BA][g2o求解BA] 0 前言 1 安装Meshlab: 三维几何网格处理 2 ...
- slam十四讲第二版 pdf_先搞定SLAM，再谈如何抓住下一代互联网产业爆发点！
在移动互联网大潮之后,自动驾驶.无人机.服务机器人等人工智能硬件会成为下一个产业爆发点,其中关键的技术之一就是动态定位和环境建模的SLAM技术! 在计算机视觉(Computer Vision)创立之初 ...
- 视觉SLAM十四讲从理论到实践第二版源码调试笔记（理论基础1-6章）
2019-2020-2学期机器人工程专业需要开设SLAM技术课程,使用教材为视觉SLAM十四讲从理论到实践第二版. 为方便学生学习课程知识,将Arduino.ROS1.ROS2和SLAM集成到课程定制 ...
- 高翔视觉SLAM十四讲（第二版）各种软件、库安装的以及报错解决方法
目录 前言 系统版本 下载高翔视觉SLAM十四讲第二版的源代码 一.安装 Vim 二.安装 g++ 三.安装 KDevelop 以及汉化 1.安装 2.汉化 四.安装 Eigen 库 五.安装 Pan ...
- slam十四讲第二版 pdf_聊聊这两年学习slam啃过的书
作者:Amber 来源:微信公众号|3D视觉工坊(系投稿) 「3D视觉工坊」技术交流群已经成立,目前大约有8000人,方向主要涉及3D视觉.CV&深度学习.SLAM.三维重建.点云后处理.自动 ...
最新文章
- AngularJS和DataModel
- [置顶] Android面试题目之四: 归并排序
- 正确的理解iOS MVC
- Python编程语言学习：在根目录的py文件内调用某叶目录文件内的包/库或者函数(常用在GUI编程)之详细攻略
- 注册域名的时候一定要注意的事项
- 置顶java[常用]-[语法]-[基础操作]
- 【问题2】为什么TIME_WAIT状态需要经过2MSL(最大报文段生存时间)才能返回到CLOSE状态？
- ejb生命周期_EJB 3.x：生命周期和并发模型（第2部分）
- linux新建samba账户,ubuntu上创建账户和samba用户
- Python基础第一天
- iostat命令简单使用
- Spring Boot实战笔记（一）-- Spring简介
- 解决fiexd和transform一起用导致的失效问题
- 基于树莓派的遥控开锁装置
- php使用adodb下载,ADODB的使用
- 帝国CMS浅浅滴谈一下——博客园老牛大讲堂
- P3853 [TJOI2007]路标设置
- 电容器选型指南-电子元器件选型指导系列
- python第五章总结
- 刷题刷题（个人记录）