深蓝学院-视觉SLAM课程-第5讲作业

已于 2023-11-24 15:59:24 修改
阅读量3.3k 收藏 16
点赞数
分类专栏： SLAM C++编程 文章标签： c++ slam
于 2022-03-10 12:04:51 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_37746927/article/details/123397880
版权
SLAM 同时被 2 个专栏收录
21 篇文章 18 订阅
订阅专栏
C++编程
14 篇文章 2 订阅
订阅专栏

课程Github地址：https://github.com/wrk666/VSLAM-Course/tree/master

1. 内容

作业已完成，这里分享一下部分代码。

2. T2

1. ORB提取

void computeAngle(const cv::Mat &image, vector<cv::KeyPoint> &keypoints) {
    int half_patch_size = 8;
//  int half_boundry = 16;
    int bad_points =0;   //角点中不能计算角度的点
    for (auto &kp : keypoints) {
        // START YOUR CODE HERE (~7 lines)
        int u=kp.pt.x, v = kp.pt.y;
        if(u>=half_patch_size && v>=half_patch_size && u+half_patch_size<=image.cols && v+half_patch_size<=image.rows)
        {
            float m01=0, m10=0;
            for(int i=u-half_patch_size; i < u + half_patch_size; ++i)  //x方向遍历16个点(右)
                for(int j=v-half_patch_size; j < v + half_patch_size; ++j)  //y方向遍历16个点(下)
                {
                    m10 +=i * image.at<uchar>(j, i);
                    m01 +=j * image.at<uchar>(j, i);
                }
            //计算角度（弧度制）并转换为角度
            kp.angle = (float)std::atan(m01/m10) * 180/pi ;  //或者std::atan2(m01, m10)*180/pi;
        }
        // END YOUR CODE HERE
    }
    return;
}

在这里插入图片描述

2. ORB描述

void computeORBDesc(const cv::Mat &image, vector<cv::KeyPoint> &keypoints, vector<DescType> &desc)
{
    int half_patch_size = 8, bad_points = 0;
    for (auto &kp : keypoints)
    {
        int u = kp.pt.x, v = kp.pt.y;
        DescType d(256, false);   //256位描述子
        //STEP1：检查是否越界
        if(u>=half_patch_size && v>=half_patch_size && u+half_patch_size<=image.cols && v+half_patch_size<=image.rows)
        {
            //STEP2：若不越界，则根据pattern来采集点，结合特征点的方向角theta来旋转点p，q->p',q'，计算p',q'的坐标，并比较大小，结果作为该位描述子的结果(0 or 1)
            for (int i = 0; i < 256; i++)
            {
                // START YOUR CODE HERE (~7 lines)
                //寻找取点pattern的下标
                cv::Point2f p(ORB_pattern[i * 4], ORB_pattern[i * 4 + 1]);
                cv::Point2f q(ORB_pattern[i * 4 + 2], ORB_pattern[i * 4 + 3]);

                //使用sin,cos, 角度转换为弧度 *pi/180
                double theta = kp.angle * pi / 180;
                double cos_theta = cos(theta) , sin_theta = sin(theta);

                int u_p_ = (int)(cos_theta * p.x - sin_theta * p.y) + u;
                int v_p_ = (int)(sin_theta * p.x + cos_theta * p.y) + v;
                int u_q_ = (int)(cos_theta * q.x - sin_theta * q.y) + u;
                int v_q_ = (int)(sin_theta * q.x + cos_theta * q.y) + v;
                //判断根据关键点得到的经过旋转的p、q是否出界,若出界，则该描述子清空作废
                if(u_p_<0 || v_p_<0 || u_p_ >image.cols || v_p_ > image.rows || u_q_<0 || v_q_<0 || u_q_ >image.cols || v_q_ > image.rows)
                {
                    d = {};
                    break;  //跳出描述子循环
                }
                d[i] = image.at<uchar>(v_p_, u_p_) > image.at<uchar>(v_q_, u_q_) ? false : true;  //前者大取false，后者大取true，vector随机访问器，不够快，但是掌握算法是关键
            }
        }
        //越界则不使用
        else
        {
            ++bad_points;
            d.clear();
        };
        desc.push_back(d);
        // END YOUR CODE HERE
    }
    cout << "bad/total: " << bad_points << "/" << desc.size() << endl;
    return;
}

3. 暴力匹配

// brute-force matching
void bfMatch(const vector<DescType> &desc1, const vector<DescType> &desc2,
             vector<cv::DMatch> &matches) {
  int d_max = 50;

  // START YOUR CODE HERE (~12 lines)
  // find matches between desc1 and desc2.  核心方法是按位异或^，但是这里是按照vector<bool>的每一位来判断
    for(size_t i1=0; i1<desc1.size(); ++i1)
    {
        if(desc1[i1].empty()) continue;
        cv::DMatch m{i1, 0, 256};  //DMatch(int _queryIdx, int _trainIdx, float _distance)：_queryIdx：为第i1个点进行匹配; _trainIdx:目标点匹配点为第0个;_distance:最小距离初始化为256
        for(size_t i2=0;i2<desc2.size();++i2)
        {
            if(desc2[i2].empty()) continue;
            int hanming_distance = 0;

            for(unsigned int j=0; j!=256; ++j)  //暴力匹配desc1和desc2中所有的关键点
            {
                hanming_distance += desc1[i1][j] ^ desc2[i2][j];   //不相等时距离增加
            }

            if(hanming_distance<d_max && hanming_distance<m.distance)  //如果distance符合阈值且小于现在的distance，更新最小distance和最佳匹配点
            {
                m.queryIdx = i1;        //被匹配点
                m.trainIdx = i2;        //更新最佳匹配点
                m.distance = hanming_distance;  //更新最小distance
            }
        }
        //匹配成功的
        if(m.distance<d_max)
            matches.push_back(m);
    }
  // END YOUR CODE HERE

//  for (auto &m : matches) {
//    cout << m.queryIdx << ", " << m.trainIdx << ", " << m.distance << endl;
//  }
  return;
}

4. 多线程ORB

结果如图
在这里插入图片描述

这部分主要按照群里助教给的文档安装了gcc，g++和tbb库（如果有需要的话，可以评论区跟我说），之前没安装过gcc，所以直接装上9.4版本，无需版本管理。tbb库安装时主要碰到软链接的问题，SLAM的ORB程序中需要libtbb.so.2的软链接，但是安装时只创建了libtbb.so的软链接，所以多创建一个即可。

重点是多线程程序的编写，在遍历所有特征点时，使用多线程来加速。补充一些关于多线程的知识\cite{ref2}：

for_each是按照迭代器来遍历元素，从C++17开始添加了执行策略ExecutionPolicy，有3种策略：

  1. std::execution::seq 使算法在单个线程中以确定性顺序执行，即不并行且不并发;
  2. std::execution::par 使算法在多个线程中执行，并且线程各自具有自己的顺序任务，即并行但不并发;
  3. std::execution::par_unseq 使算法在多个线程中执行，并且线程可以具有并发的多个任务，即并行和并发。

first和last分别是起始和终止的迭代器，最后一个是函数f（程序里使用了lambda表达式），接受[first,last)内的迭代器。函数执行策略从C++17开始可以选择，在这里我们使用par_unseq(并行和并发)，
需要#include <execution>

下面的加锁部分很关键，当时卡了有一会儿：

计算Angle时不用关心数据冲突问题，因为都是使用指针进行访问，顺序无所谓，但是在计算描述子时，我们需要把计算出来的描述子push_back到vector中，而vector是顺序容器，并行且并发不能保证push_back的顺序，在多线程环境下，对共享的资源的插入会出现不可预知的错误。所以需要加锁保护，在C++11中新增了<mutex>，我们#include <mutex>并加锁（参考这里）,这点至关重要，否则计算描述子只能在单线程seq中计算，最终各部分核心代码如Listing4所示，运行结果如图2.3所示。（不明白的是为什么开始计算多线程会比较慢，紧接着再调用就较快了，是否是因为有缓存？）

void computeAngleMT(const cv::Mat &image, vector<cv::KeyPoint> &keypoints)
{
    int half_patch_size = 8;
    std::mutex m;
    //or each设计的初衷就是要遍历each one，所以只能遍历完，
    std::for_each(std::execution::par_unseq, keypoints.begin(), keypoints.end(),
                  [&half_patch_size, &image, &m](auto &kp)
                  {
                      // START YOUR CODE HERE
                      int u=kp.pt.x, v = kp.pt.y;
                      if(u>=half_patch_size && v>=half_patch_size && u+half_patch_size<=image.cols && v+half_patch_size<=image.rows)
                      {
                          float m01=0, m10=0;
                          for(int i=u-half_patch_size; i < u + half_patch_size; ++i)  //x方向遍历16个点(右)
                              for(int j=v-half_patch_size; j < v + half_patch_size; ++j)  //y方向遍历16个点(下)
                              {
                                  m10 +=i * image.at<uchar>(j, i);
                                  m01 +=j * image.at<uchar>(j, i);
                              }
                          std::lock_guard<std::mutex> guard(m);//代替m.lock; m.unlock();
                          //计算角度（弧度制）并转换为角度
                          kp.angle = (float)std::atan(m01/m10) * 180/pi ;  //或者std::atan2(m01, m10)*180/pi;
                      }
                      // END YOUR CODE HERE
                  });
    return;
}


void computeORBDescMT(const cv::Mat &image, vector<cv::KeyPoint> &keypoints, vector<DescType> &desc)
{
    // START YOUR CODE HERE (~20 lines)
    std::mutex m;
    std::for_each(std::execution::par_unseq, keypoints.begin(), keypoints.end(),
                  [&image, &desc, &m]  (auto& kp)   //lambda表达式,这个function函数接受迭代器
                  {
                      int u = kp.pt.x, v = kp.pt.y;  //迭代器要使用->来访问成员（将解引用和成员访问.结合在一起）
                      DescType d(256, false);   //256位描述子
                      //STEP1：检查是否越界
                      if(u>=8 && v>=8 && u+8<=image.cols && v+8<=image.rows)
                      {
                          //STEP2：若不越界，则根据pattern来采集点，结合特征点的方向角theta来旋转点p，q->p',q'，计算p',q'的坐标，并比较大小，结果作为该位描述子的结果(0 or 1)
                          for (int i = 0; i < 256; i++)
                          {
                              // START YOUR CODE HERE (~7 lines)
                              //寻找取点pattern的下标
                              cv::Point2f p(ORB_pattern[i * 4], ORB_pattern[i * 4 + 1]);
                              cv::Point2f q(ORB_pattern[i * 4 + 2], ORB_pattern[i * 4 + 3]);

                              //使用sin,cos, 角度转换为弧度 *pi/180
                              double theta = kp.angle * pi / 180;
                              double cos_theta = cos(theta) , sin_theta = sin(theta);

                              int u_p_ = (int)(cos_theta * p.x - sin_theta * p.y) + u;
                              int v_p_ = (int)(sin_theta * p.x + cos_theta * p.y) + v;
                              int u_q_ = (int)(cos_theta * q.x - sin_theta * q.y) + u;
                              int v_q_ = (int)(sin_theta * q.x + cos_theta * q.y) + v;
                              //判断根据关键点得到的经过旋转的p、q是否出界,若出界，则该描述子清空作废
                              if(u_p_<0 || v_p_<0 || u_p_ >image.cols || v_p_ > image.rows || u_q_<0 || v_q_<0 || u_q_ >image.cols || v_q_ > image.rows)
                              {
                                  d.clear();
//                                  d = {};
                                  break;  //跳出描述子循环
                              }
                              d[i] = image.at<uchar>(v_p_, u_p_) > image.at<uchar>(v_q_, u_q_) ? false : true;  //前者大取false，后者大取true，vector随机访问器，不够快，但是掌握算法是关键
                          }
                      }
                      //越界则不使用
                      else
                      {
                          d.clear();
//                          d = {};
                      }
                      std::lock_guard<std::mutex> guard(m);//代替m.lock; m.unlock();
                      desc.push_back(d);
                  });

    int bad_points = 0;
    for(auto d:desc)
    {
        if(d.empty())
            ++bad_points;
    }
    cout << "Desc bad/total: " << bad_points << "/" << desc.size() << endl;
    return;
    // END YOUR CODE HERE
}

CMakeLists.txt(别忘了link tbb库)

cmake_minimum_required(VERSION 3.21)
project(ORB_Extract)

set(CMAKE_CXX_STANDARD 17)

set(CMAKE_BUILD_TYPE "Release")
#set(CMAKE_BUILD_TYPE "Debug")
MESSAGE(STATUS "CMAKE_BUILD_TYPE IS ${CMAKE_BUILD_TYPE}")

find_package(OpenCV 3 REQUIRED)

#添加头文件
include_directories(
        ${OpenCV_INCLUDE_DIRS}
        ${G2O_INCLUDE_DIRS}
        ${Sophus_INCLUDE_DIRS}
        "/usr/local/include/eigen3/"
)

# 手写ORB特征
add_executable(computeORB computeORB.cpp)
#链接OpenCV库和tbb库
target_link_libraries(computeORB ${OpenCV_LIBS} tbb)

3. 从E中恢复R，t

没什么好说的，不过手写一遍印象更深刻。
在这里插入图片描述

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

    // 给定Essential矩阵
    Matrix3d E;
    E << -0.0203618550523477, -0.4007110038118445, -0.03324074249824097,
            0.3939270778216369, -0.03506401846698079, 0.5857110303721015,
            -0.006788487241438284, -0.5815434272915686, -0.01438258684486258;
    // 待计算的R,t
    Matrix3d R;
    Vector3d t;
    // SVD and fix sigular values
    // START YOUR CODE HERE
    // SVD on Sigma
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);  //Eigen的svd函数，计算满秩的U和V
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    Eigen::Vector3d  sv = svd.singularValues();
    cout << "U=" << U << endl;
    cout << "V=" << V << endl;
    cout << "sv=" << sv << endl;

    Eigen::Matrix3d Sigma = Eigen::Matrix3d::Zero();
    Sigma(0,0) = sv(0);
    Sigma(1,1) = sv(1);

    cout << "Sigma:\n" << Sigma << endl;
    // END YOUR CODE HERE

    // set t1, t2, R1, R2
    // START YOUR CODE HERE

    //use AngleAxis
    Eigen::AngleAxisd rotation_vector_neg ( -M_PI/2, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 -90 度
    Eigen::AngleAxisd rotation_vector_pos ( M_PI/2, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 90 度
    Eigen::Matrix3d  RzNegHalfPi = rotation_vector_neg.toRotationMatrix();
    Eigen::Matrix3d  RzPosHalfPi = rotation_vector_pos.toRotationMatrix();


    Matrix3d t_wedge1 = U * RzPosHalfPi * Sigma * U.transpose();
    Matrix3d t_wedge2 = U * RzNegHalfPi * Sigma * U.transpose();

    Matrix3d R1 = U * RzPosHalfPi.transpose() * V.transpose();
    Matrix3d R2 = U * RzNegHalfPi.transpose() * V.transpose();
    // END YOUR CODE HERE

    cout << "R1 = \n" << R1 << endl;
    cout << "R2 = \n" << R2 << endl;
    cout << "t1 = \n" << Sophus::SO3d::vee(t_wedge1) << endl;  //求李代数？？
    cout << "t2 = \n" << Sophus::SO3d::vee(t_wedge2) << endl;

    // check t^R=E up to scale
    Matrix3d tR = t_wedge1 * R1;
    cout << "t^R = " << tR << endl;

    return 0;
}

上面只是为了理解原理，其实可以更简洁，由于绕Z轴顺逆时针旋转的旋转矩阵是固定的，所以可以直接定义出来，
定义的方法见下图：

授人以渔：
在这里插入图片描述
顺便再授人以鱼：
在这里插入图片描述

以下是VINS-MONO中对应的代码，加了些注释：

void InitialEXRotation::decomposeE(cv::Mat E,
                                 cv::Mat_<double> &R1, cv::Mat_<double> &R2,
                                 cv::Mat_<double> &t1, cv::Mat_<double> &t2)
{
    cv::SVD svd(E, cv::SVD::MODIFY_A);
    //绕z顺时针90°
    cv::Matx33d W(0, -1, 0,
                  1, 0, 0,
                  0, 0, 1);
    //绕z逆时针90°
    cv::Matx33d Wt(0, 1, 0,
                   -1, 0, 0,
                   0, 0, 1);
    R1 = svd.u * cv::Mat(W) * svd.vt;
    R2 = svd.u * cv::Mat(Wt) * svd.vt;
    t1 = svd.u.col(2);//3*3取最后一个是待求量（为什么t2可以直接取负号？）
    t2 = -svd.u.col(2);
}

4. 用G-N实现Bundle Adjustment中的位姿估计

int main(int argc, char **argv)
{
    VecVector2d p2d;
    VecVector3d p3d;
    Matrix3d K;
    double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
    K << fx, 0, cx, 0, fy, cy, 0, 0, 1;

    // load points in to p3d and p2d
    // START YOUR CODE HERE
    double data2d[2] = {0}, data3d[3] = {0};
    ifstream fin2d(p2d_file), fin3d(p3d_file);
    for(int i=0;i<76;++i)
    {
        fin2d>>data2d[0];
        fin2d>>data2d[1];
        p2d.push_back(Eigen::Vector2d(data2d[0], data2d[1]));
        fin3d>>data3d[0];
        fin3d>>data3d[1];
        fin3d>>data3d[2];
        p3d.push_back(Eigen::Vector3d(data3d[0], data3d[1], data3d[2]));
    }

    // END YOUR CODE HERE
    assert(p3d.size() == p2d.size());

    int iterations = 100;
    double cost = 0, lastCost = 0;
    int nPoints = p3d.size();
    cout << "points: " << nPoints << endl;

    Sophus::SE3d T_esti; // estimated pose,李群，不是李代数，李代数是se3，是Vector3d

    for (int iter = 0; iter < iterations; iter++) {

        Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();
        Vector6d b = Vector6d::Zero();

        cost = 0;
        // compute cost  计算误差,是 观测-预测
        for (int i = 0; i < nPoints; i++)
        {
            // compute cost for p3d[I] and p2d[I]
            // START YOUR CODE HERE
        Eigen::Vector3d pc = T_esti * p3d[i];  //3D点转换到相机坐标系下(取了前3维)
        double inv_z = 1.0 / pc[2];
        double inv_z2 = inv_z * inv_z;
        Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);  //重投影，预测
        Eigen::Vector2d e = p2d[i] - proj;
        cost += e.transpose() * e;
	    // END YOUR CODE HERE

	    // compute jacobian
            Matrix<double, 2, 6> J;
            // START YOUR CODE HERE
        J<<fx * inv_z,
           0,
           -fx * pc[0] * inv_z2,
           -fx * pc[0] * pc[1] * inv_z2,
           fx + fx * pc[0] * pc[0] * inv_z2,
           -fx * pc[1] * inv_z,
           0,
           fy * inv_z,
           -fy * pc[1] * inv_z2,
           -fy - fy * pc[1] * pc[1] * inv_z2,
           fy * pc[0] * pc[1] * inv_z2,
           fy * pc[0] * inv_z;
        J = -J;
	    // END YOUR CODE HERE
            //高斯牛顿的系数矩阵和非齐次项
            H += J.transpose() * J;
            b += -J.transpose() * e;
        }

	// solve dx
        Vector6d dx;  //解出来的△x是李代数

        // START YOUR CODE HERE
        dx = H.ldlt().solve(b);  //解方程
        // END YOUR CODE HERE

        if (isnan(dx[0]))
        {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost >= lastCost) {
            // cost increase, update is not good
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // update your estimation
        // START YOUR CODE HERE
        T_esti = Sophus::SE3d::exp(dx) * T_esti;

        // END YOUR CODE HERE

        lastCost = cost;

        cout << "iteration " << iter << " cost=" << cout.precision(12) << cost << endl;
    }

    cout << "estimated pose: \n" << T_esti.matrix() << endl;
    return 0;
}

在这里插入图片描述

5. 使用ICP实现轨迹对齐

使用第三章的轨迹绘制和读取代码进行修改，添加了手写的ICP部分的程序，最终轨迹输出和 T g e T_{ge} Tge​的估计结果如图所示，核心部分代码如下所示。

思考：本身想计算出两条轨迹的RMSE，但是思考之后发现此处将平移部分看做位置P，那么两个轨迹也就相当于两帧图像，之间只有一次位姿变换T，而计算RMSE是计算整条轨迹上所有的T的误差，那么求出 T g e T_{ge} Tge​后对 T e T_e Te​进行变换，就能计算RMSE了。

在这里插入图片描述

在这里插入图片描述

#include <iostream>
#include <fstream>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>
#include <opencv2/core/core.hpp>

using namespace Sophus;
using namespace std;
using namespace cv;

string compare_file = "./compare.txt";

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
typedef vector<TrajectoryType> LongTrajectoryType;
typedef Eigen::Matrix<double,6,1> Vector6d;

void DrawTrajectory(const vector<Point3d> &gt, const vector<Point3d> &esti, const string& title);
vector<TrajectoryType> ReadTrajectory(const string &path);
vector<Point3d> GetPoint(TrajectoryType TT);
void pose_estimation_3d3d(const vector<Point3d> &pts1, const vector<Point3d> &pts2, Mat &R, Mat &t);
vector<Point3d> TrajectoryTransform(Mat T, Mat t, vector<Point3d> esti );

int main(int argc, char **argv) {
    LongTrajectoryType CompareData = ReadTrajectory(compare_file);
    assert(!CompareData.empty());
    cout<<"size: "<<CompareData.size()<<endl;

    vector<Point3d> EstiPt = GetPoint(CompareData[0]);
    vector<Point3d> GtPt = GetPoint(CompareData[1]);

    Mat R, t;  //待求位姿
    pose_estimation_3d3d( GtPt, EstiPt, R, t);
    cout << "ICP via SVD results: \n" << endl;
    cout << "R = \n" << R << endl;
    cout << "t = \n" << t << endl;
    cout << "R_inv = \n" << R.t() << endl;
    cout << "t_inv = \n" << -R.t() * t << endl;

    DrawTrajectory(GtPt, EstiPt, "Before Calibrate");
    vector<Point3d> EstiCali = TrajectoryTransform(R, t, EstiPt);
    DrawTrajectory(GtPt, EstiCali, "Atfer Calibrate");
    return 0;
}

LongTrajectoryType ReadTrajectory(const string &path)
{
    ifstream fin(path);
    TrajectoryType trajectory1, trajectory2;
    if (!fin) {
        cerr << "trajectory " << path << " not found." << endl;
        return {};
    }

    while (!fin.eof()) {
        double time1, tx1, ty1, tz1, qx1, qy1, qz1, qw1;
        fin >> time1 >> tx1 >> ty1 >> tz1 >> qx1 >> qy1 >> qz1 >> qw1;
        double time2, tx2, ty2, tz2, qx2, qy2, qz2, qw2;
        fin >> time2 >> tx2 >> ty2 >> tz2 >> qx2 >> qy2 >> qz2 >> qw2;
        Sophus::SE3d p1(Eigen::Quaterniond(qw1, qx1, qy1, qz1), Eigen::Vector3d(tx1, ty1, tz1));
        trajectory1.push_back(p1);
        Sophus::SE3d p2(Eigen::Quaterniond(qw2, qx2, qy2, qz2), Eigen::Vector3d(tx2, ty2, tz2));
        trajectory2.push_back(p2);
    }
    LongTrajectoryType ret{trajectory1, trajectory2};
    return ret;
}


void DrawTrajectory(const vector<Point3d> &gt, const vector<Point3d> &esti, const string& title)
{
    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind(title, 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));


    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);
        for (size_t i = 0; i < gt.size() - 1; i++) {
            glColor3f(0.0f, 0.0f, 1.0f);  // blue for ground truth
            glBegin(GL_LINES);
            auto p1 = gt[i], p2 = gt[i + 1];
            glVertex3d(p1.x, p1.y, p1.z);
            glVertex3d(p2.x, p2.y, p2.z);
            glEnd();
        }

        for (size_t i = 0; i < esti.size() - 1; i++) {
            glColor3f(1.0f, 0.0f, 0.0f);  // red for estimated
            glBegin(GL_LINES);
            auto p1 = esti[i], p2 = esti[i + 1];
            glVertex3d(p1.x, p1.y, p1.z);
            glVertex3d(p2.x, p2.y, p2.z);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }

}

void pose_estimation_3d3d(const vector<Point3d> &pts1,
                          const vector<Point3d> &pts2,
                          Mat &R, Mat &t) {
    Point3d p1, p2;     // center of mass 质心,这里p1表示第1幅图，p2表示第2幅图，和书上的R是反着的，所以要计算R21=这里的R12^(-1)=R12^(T)，最后也输出了
    int N = pts1.size();
    for (int i = 0; i < N; i++) {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = Point3d(Vec3d(p1) / N);
    p2 = Point3d(Vec3d(p2) / N);
    vector<Point3d> q1(N), q2(N); // remove the center  去质心
    for (int i = 0; i < N; i++) {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for (int i = 0; i < N; i++)
    {
        W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();  //这里是2->1  R12,求R21要转置
    }
    cout << "W= \n" << W << endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);  //Eigen的svd函数，计算满秩的U和V
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    cout << "U= \n" << U << endl;
    cout << "V= \n" << V << endl;

    Eigen::Matrix3d R_ = U * (V.transpose());  //这里能保证满足det(R)=1且正交吗？
    cout<<"我的输出： det(R_): "<<R_.determinant()<<"\nR_: \n"<<R_<<endl;  //Eigen的Mat
    if (R_.determinant() < 0)  //若行列式为负，取-R
    {
        R_ = -R_;
    }
    Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);   //最优的t=p-Rp'

    // convert to cv::Mat
    R = (Mat_<double>(3, 3) <<
            R_(0, 0), R_(0, 1), R_(0, 2),
            R_(1, 0), R_(1, 1), R_(1, 2),
            R_(2, 0), R_(2, 1), R_(2, 2)
    );
    t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

vector<Point3d> GetPoint(TrajectoryType TT)
{
    vector<Point3d> pts;
    for(auto each:TT)
        //不用做相机模型的处理,也不/5000
        pts.push_back(Point3d(each.translation()[0], each.translation()[1], each.translation()[2]));
    return pts;
}

//转换
vector<Point3d> TrajectoryTransform(Mat T, Mat t, vector<Point3d> esti )
{
    vector<Point3d> calibrated={};
    Mat Mat__31;
    Sophus::SE3d SE3D;
    for(auto each:esti)
    {
        Mat__31 = (Mat_<double>(3, 1)<<each.x, each.y, each.z);
        Mat__31 = T * Mat__31 + t;
        calibrated.push_back( Point3d(Mat__31));
    }
    return calibrated;
}

接着赶进度！

读书健身敲代码
关注
  • 0
    点赞
  • 踩
  • 16
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
专栏目录
深蓝学院视觉SLAM第五次习题
weixin_44218240的博客
05-08 1930
手里的代码跑不出来，遇到的bug完全不知道是哪里出了错，心塞的只能先看看视频做做习题了 1.ORB 特征点
深蓝学院SLAM课程第二讲作业
不愿透漏姓名的王建森的博客
07-25 3844
第一题：熟悉Eigen矩阵运算 #include <iostream> #include <ctime> #include <Eigen/Core> #include <Eigen/Dense> #define MATRIX_SIZE 10 using namespace std; int main() { std::cout <...
深蓝-视觉slam-第五节习题
weixin_46417419的博客
04-18 346
ORB特征点 实现代码： #include <opencv2/opencv.hpp> #include <execution> //并行处理for_each中的std::execution #include <string> #include <mutex> #include <math.h> #include <chrono> using namespace std; using namespace cv; string.
深蓝学院-视觉SLAM十四讲-第五章作业
记录和分享
11-12 3858
第五节课作业 2. ORB特征点 ORB(Oriented FAST and BRIEF) 特征是SLAM 中一种很常用的特征，由于其二进制特性，使得它可以非常快速地提取与计算。下面，你将按照本题的指导，自行书写ORB 的提取、描述子的计算以及匹配的代码。代码框架参照computeORB.cpp 文件，图像见1.png 文件和2.png。 2.1 ORB提取 本题Fast特征点提取部分使用的OpenCV自带的FAST提取算法，只用按照公式进行角度计算就可以了。根据课本上或者视频给的PPT上的计算公式进行计算
深蓝学院-视觉SLAM理论与实践-第十二期-第5章作业
weixin_40918019的博客
02-27 556
源代码gitee：https://gitee.com/sldwlk_admin/vslamlesson 欢迎收藏 ^ ^ 一起交流 ORB特征点 问题1：ORB提取 程序思路 判断是否在边界，如果在就跳过 计算图片在x、y方向的重心，这里要注意坐标原点时在局部图片的中心位置 用std::atan2计算重心夹角 核心代码 // compute the angle void computeAngle(const cv::Mat &image, vector<cv::KeyPoint> &
视觉slam十四讲 学习笔记-5
echo_gou的博客
10-17 478
信息：位置、覆盖的大小、朝向等通过在一个矩形中将亮度的值看做质量，然后去计算他的质心，然后通过图像中心和质心的连线就可以得出角点的方向，这样就得出了图像之间的旋转和匹配关系。ORB 特征亦由关键点和描述子两部分组成：BRIEF描述子： 随机取多对坐标点（pi,qi），eg：如果p处的值大于q我们就记作1，否则记为0，这样多个点组成的一个1维矩阵就为BRIEF描述（通常为128对，即128*1的矩阵），这个描述是具有较好的鲁棒性的。下图就表示5种取pq点的方式（随机或者按照某种固定方式）。
视觉SLAM-slam十四讲源代码
最新发布
04-20
slam十四讲源代码——SLAM (simultaneous localization and mapping),也称为CML (Concurrent Mapping and Localization), 即时定位与地图构建，或并发建图与定位。问题可以描述为：将一个机器人放入未知环境中的未知...
VI_ORB_SLAM2:基于ORB-SLAM2的MonocularStereo视觉惯性ORB-SLAM
05-14
VI_ORB_SLAM2：基于ORB-SLAM2的单目/立体视觉惯性ORB-SLAM 该存储库包括Visual-Inertial ORB-SLAM的Monocular版本和Stereo版本。 这两个是的和的。 有关详细信息，您可以参考Examples/Monocular/mono_euroc_VI.cc和...
RTAB-MAP开源视觉-激光-里程计SLAM代码
04-22
RTAB-MAP压缩包里有以下个开源代码： ...RTAB-MAP，2009年开始设计，2013年首次开源，2016年完全基于图优化，2017年扩展了很多新应用，是RGB-D SLAM中比较经典的方案。目前被发展成跨平台独立的C++库和一个ROS功能包。
视觉SLAM十四讲第3次作业-第4题-推导 SE(3)的指数映射（详解版）
06-19
视觉SLAM十四讲，深蓝学院， SE(3)的指数映射
SLAM-基于ORB-SLAM的鱼眼相机实时鲁棒单目视觉SLAM系统-无需校正或裁剪输入图像-优质项目实战.zip
02-22
本项目实战是基于ORB-SLAM的鱼眼相机实时鲁棒单目视觉SLAM系统，无需进行额外的图像校正或裁剪，从而简化了系统的复杂性，提高了运行效率。 ORB-SLAM，全称是Oriented FAST and Rotated BRIEF，是一种高效且强大的...
ORB-SLAM2 ---- computeOrbDescriptor函数
qq_41694024的博客
08-12 643
ORB-SLAM2 ---- computeOrbDescriptor函数解析
ORB-SLAM2多线程用法总结
heurobocon的博客
02-02 1995
文章目录前言一、线程创建二、mutex互斥锁三、unique_lock总结 前言 ORB-SLAM2以高效的多线程结构闻名。其运用到的C++多线程方法相对来说还是比较基础的，今天总结一下其源码中的多线程编程方法。 一、线程创建 thread类是C++标准库中的一个类，可以创建独立运行的线程。 执行线程是一个指令序列，它可以在多线程环境中与其他这样的序列同时执行，同时多个线程之间可以共享相同的地址空间。 在ORB-SLAM2中创建线程的代码为： //初始化局部建图线程并运行 //Init.
SLAM十四讲第五次作业-深蓝学院
weixin_41074793的博客
12-20 3502
void computeAngle(const cv::Mat &amp;amp;amp;image, vector&amp;amp;lt;cv::KeyPoint&amp;amp;gt; &amp;amp;amp;keypoints) { int half_patch_size = 8; vector&amp;amp;lt;int&amp;amp;gt; umax(half_patch_size + 2); int v, vmax
视觉slam学习笔记以及课后习题《第五讲特征点法视觉里程计》
m0_61238051的博客
10-29 887
这篇博客主要记录了我在深蓝学院视觉slam课程中的课后习题，因为是为了统计知识点来方便自己以后查阅，所以有部分知识可能不太严谨，如果给大家造成了困扰请见谅，大家发现了问题也可以私信或者评论给我及时改正，欢迎大家一起探讨。其他章的笔记可以在我的首页文章中查看。 整个作业的代码和文档都可以参考我的GitHub存储库GitHub - 1481588643/vslam 如果想要了解视觉slam其他章节的内容，也可以查阅以下链接视觉slam学习笔记以及课后习题《第三讲李群李代数》_m0_61238051的博客..
深蓝学院视觉slam十四讲第5章作业
Jevin-L的博客
06-27 1925
2.1 // compute the angle void computeAngle(const cv::Mat &image, vector<cv::KeyPoint> &keypoints) { int half_patch_size = 8; for (auto &kp : keypoints) { float m01 = 0, m10 = 0; //从(u − 8, v − 8) 取到 (u + 7, v + ..
深蓝学院-视觉SLAM课程-第5讲笔记
qq_37746927的博客
03-02 2082
深蓝学院-视觉SLAM课程-第5讲笔记
关于slam14讲ch7手写ORB特征里的遍历方式循环次数的改变问题。
Jsjsshiz的博客
09-13 252
对slam14讲ch7手写orb那段代码内的遍历数大小进行相关修改
《视觉slam十四讲》学习笔记——ch7实践部分 比较opencv库下的ORB特征的提取和手写ORB的区别
weixin_70026476的博客
10-20 1808
在学习了《视觉slam十四讲》第七章视觉里程计的相关知识后，运行了里面的基于opencv库函数的ORB特征点提取和匹配的代码。在使用代码自带的模板图片时，出来的效果很好，特征点匹配成功率很高，一方面原因是两张模板照片拍摄角度相差不大，于是我自己拍摄了两张照片，一张是正面照片，一张是侧面照片，再次使用该代码进行特征点匹配时发现误匹配的概率有点高，然后使用手写ORB算法后发现匹配效果很好。
基于ORB-SLAM3库搭建SLAM系统
07-09
基于ORB-SLAM3库搭建SLAM系统

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值