视觉SLAM十四讲CH5代码解析及课后习题详解

最新推荐文章于 2024-07-23 11:04:57 发布

长沙有肥鱼

最新推荐文章于 2024-07-23 11:04:57 发布

阅读量6.6k

点赞数 22

分类专栏：视觉SLAM十四讲文章标签： c++

本文链接：https://blog.csdn.net/weixin_53660567/article/details/120942158

版权

视觉SLAM十四讲专栏收录该内容

26 篇文章 195 订阅

订阅专栏

imageBasics.cpp

#include <iostream>
#include <chrono> //std::chrono 来给算法计时所需头文件

using namespace std;

#include <opencv2/core/core.hpp>//opencv核心模块
#include <opencv2/highgui/highgui.hpp>//opencv gui模块

int main(int argc, char **argv) {
  // 读取argv[1]指定的图像
  cv::Mat image;
  image = cv::imread(argv[1]); //cv::imread函数读取指定路径下的图像

  // 判断图像文件是否正确读取
  if (image.data == nullptr) { //数据不存在,可能是文件不存在
    cerr << "文件" << argv[1] << "不存在." << endl;
    return 0;
  }

  // 文件顺利读取, 首先输出一些基本信息
  cout << "图像宽为" << image.cols << ",高为" << image.rows << ",通道数为" << image.channels() << endl;
  cv::imshow("image", image);      // 用cv::imshow显示图像
  cv::waitKey(0);                  // 暂停程序,等待一个按键输入

  // 判断image的类型
  if (image.type() != CV_8UC1 && image.type() != CV_8UC3) {
    // 图像类型不符合要求
    cout << "请输入一张彩色图或灰度图." << endl;
    return 0;
  }

  // 遍历图像, 请注意以下遍历方式亦可使用于随机像素访问
  // 使用 std::chrono 来给算法计时
  //使用指针遍历图像image中的像素
  //steady_clock是单调的时钟，相当于教练中的秒表，只会增长，适合用于记录程序耗时。
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  //size_t全称是size_type，它表示sizeof()函数的返回值，是无符号整型unsigned int变量。
  //使用size_t的目的是提供一种可移植的方法来声明与系统中可寻址的内存区域一致的长度。
  for (size_t y = 0; y < image.rows; y++) {
    // 用cv::Mat::ptr获得图像的行指针
    unsigned char *row_ptr = image.ptr<unsigned char>(y);  // row_ptr是第y行的头指针
    for (size_t x = 0; x < image.cols; x++) {
      // 访问位于 x,y 处的像素
      unsigned char *data_ptr = &row_ptr[x * image.channels()]; // data_ptr 指向待访问的像素数据
      // 输出该像素的每个通道,如果是灰度图就只有一个通道
      for (int c = 0; c != image.channels(); c++) {
        unsigned char data = data_ptr[c]; // data为I(x,y)第c个通道的值
      }
    }
  }
 
  
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast < chrono::duration < double >> (t2 - t1);
  cout << "遍历图像用时：" << time_used.count() << " 秒。" << endl;

  // 关于 cv::Mat 的拷贝
  // 直接赋值并不会拷贝数据
  cv::Mat image_another = image;
  // 修改 image_another 会导致 image 发生变化
  image_another(cv::Rect(0, 0, 100, 100)).setTo(0); // 将左上角100*100的块置为黑色
  cv::imshow("image", image);
  cv::waitKey(0);//停止执行，等待一个按键输入

  // 使用clone函数来拷贝数据
  cv::Mat image_clone = image.clone();
  image_clone(cv::Rect(0, 0, 100, 100)).setTo(255);//将左上角100*100的块置为白色
  cv::imshow("image", image);
  cv::imshow("image_clone", image_clone);
  cv::waitKey(0);//停止执行，等待一个按键输入

  // 对于图像还有很多基本的操作,如剪切,旋转,缩放等,限于篇幅就不一一介绍了,请参看OpenCV官方文档查询每个函数的调用方法.
  cv::destroyAllWindows();
  return 0;
}

CMakeLists.txt :

project(imageBasics)
cmake_minimum_required(VERSION 3.10)
add_executable(imageBasics imageBasics.cpp)
# 链接OpenCV库
find_package( OpenCV REQUIRED )
#添加头文件
include_directories( ${OpenCV-INCLUDE_DIRS})
target_link_libraries(imageBasics ${OpenCV_LIBS})
 
add_executable(undistortImage undistortImage.cpp)
# 链接OpenCV库
target_link_libraries(undistortImage ${OpenCV_LIBS})
set(CMAKE_CXX_FLAGS "-std=c++11")

执行结果：

./imageBasics ../ubuntu.png

图像宽为1200,高为674,通道数为3
遍历图像用时：0.0136354 秒。

undistortImage.cpp

#include <opencv2/opencv.hpp>
#include <string>

using namespace std;

string image_file = "/home/liqiang/slambook2/ch5/imageBasics/distorted.png";   // 请确保路径正确
//string image_file = "../distorted.png"; //或者使用下面这种方式

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

  // 本程序实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。
 
  double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05; // 畸变参数
  double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;  // 相机内参

  cv::Mat image = cv::imread(image_file, 0);   // 图像是灰度图，CV_8UC1 0表示返回一张灰度图
  int rows = image.rows, cols = image.cols;
  cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 去畸变以后的图

  // 计算去畸变后图像的内容
  for (int v = 0; v < rows; v++) {
    for (int u = 0; u < cols; u++) {
      // 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)
      //依据公式，计算像素点image_undistort(v,u)去畸变后的坐标image(v_distorted,u_distorted)
      double x = (u - cx) / fx, y = (v - cy) / fy;//书上p99式5.5
      double r = sqrt(x * x + y * y);//对x平方和y平方开根
      double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);//书上p102式5.12
      double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;//书上p102式5.12
      double u_distorted = fx * x_distorted + cx;//书上p102式5.13
      double v_distorted = fy * y_distorted + cy;//书上p102式5.13

      // 赋值 (最近邻插值) 
      if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
        image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);//将image(v_distorted,u_distorted)处的颜色信息赋值到image_undistort(v,u)处
      } else {
        image_undistort.at<uchar>(v, u) = 0;
      }
    }
  }

  // 画图去畸变后图像
  cv::imshow("distorted", image);
  cv::imshow("undistorted", image_undistort);
  cv::waitKey(); //程序终止，等待一个按键输入
  return 0;
}

执行结果：

stereoVision.cpp

#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <Eigen/Core>//Eigen核心模块
#include <pangolin/pangolin.h>
#include <unistd.h>

using namespace std;
using namespace Eigen;

// 文件路径
string left_file = "../left.png";
string right_file = "../right.png";

//string left_file = "./left.png";
//string right_file = "./right.png";//该方法需要把两张图像放在build文件夹下面


// 在pangolin中画图，已写好，无需调整
void showPointCloud(
    const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);

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

    // 内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 基线
    double b = 0.573;

    // 读取图像
    cv::Mat left = cv::imread(left_file, 0);//0表示返回一张灰度图
    cv::Mat right = cv::imread(right_file, 0);//0表示返回一张灰度图
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
        0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);    //关于sgbm算法的经典参数配置
    cv::Mat disparity_sgbm, disparity;
    sgbm->compute(left, right, disparity_sgbm);
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f); //注意disparity才是最后的视差图

    // 生成点云
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;//前三维为X,Y,Z，表示位置信息，后一维表示颜色信息，在此处为灰度

    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) 
        {
            //设置一个Check，排除视差不在(10.0, 96.0)范围内的像素点
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;
            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色 创建一个Vector4d类型的变量，前三维用来存储位置信息，后一维为归一化之后的灰度

            // 根据双目模型计算 point 的位置  根据双目模型恢复像素点的三维位置
            double x = (u - cx) / fx;
            double y = (v - cy) / fy;
            double depth = fx * b / (disparity.at<float>(v, u));
             //将X,Y,Z赋值给point的前三维
            point[0] = x * depth;
            point[1] = y * depth;
            point[2] = depth;
            pointcloud.push_back(point);
        }

    cv::imshow("disparity", disparity / 96.0);//disparity/96表示归一化之后的视差图像
    cv::waitKey(0);//停止执行，等待一个按键输入
    // 画出点云
    showPointCloud(pointcloud);
    return 0;
}

void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }
    //创建一个pangolin窗口
    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);//分别表示窗口名Point Cloud Viewer、窗口宽度=1024和窗口高度=768
    glEnable(GL_DEPTH_TEST);//根据物体远近，实现遮挡效果
    glEnable(GL_BLEND); //使用颜色混合模型，让物体显示半透明效果
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);//GL_SRC_ALPHA表示使用源颜色的alpha值作为权重因子，GL_ONE_MINUS_SRC_ALPHA表示使用(1-源颜色的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)
    );
   //ProjectionMatrix()中各参数依次为图像宽度=1024、图像高度=768、fx=500、fy=500、cx=512、cy=389、最近距离=0.1和最远距离=1000
   //ModelViewLookAt()中各参数为相机位置，被观察点位置和相机哪个轴朝上
   //比如，ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)表示相机在(0, -0.1, -1.8)位置处观看视点(0, 0, 0)，并设置相机XYZ轴正方向为（0，-1，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));
   //SetBounds()内的前4个参数分别表示交互视图的大小，均为相对值，范围在0.0至1.0之间
   //第1个参数表示bottom，即为视图最下面在整个窗口中的位置
   //第2个参数为top，即为视图最上面在整个窗口中的位置
   //第3个参数为left，即视图最左边在整个窗口中的位置
   //第4个参数为right，即为视图最右边在整个窗口中的位置
   //第5个参数为aspect，表示横纵比


    while (pangolin::ShouldQuit() == false) //如果pangolin窗口没有关闭，则执行
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);//清空颜色和深度缓存，使得前后帧不会互相干扰

        d_cam.Activate(s_cam);//激活显示，并设置相机状态
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);//设置背景颜色为白色

        glPointSize(2);
        glBegin(GL_POINTS); //绘制点云
        for (auto &p: pointcloud) {
            glColor3f(p[3], p[3], p[3]); //设置颜色信息
            glVertex3d(p[0], p[1], p[2]);//设置位置信息
        }
        glEnd();
        pangolin::FinishFrame();//按照上面的设置执行渲染
        usleep(5000);   // sleep 5 ms 停止执行5毫秒
    }
    return;
}

error：

/usr/bin/ld: CMakeFiles/stereoVision.dir/stereoVision.o: in function `main':
stereoVision.cpp:(.text+0xae): undefined reference to `cv::imread(cv::String const&, int)'
/usr/bin/ld: stereoVision.cpp:(.text+0xf1): undefined reference to `cv::imread(cv::String const&, int)'
/usr/bin/ld: stereoVision.cpp:(.text+0x136): undefined reference to `cv::StereoSGBM::create(int, int, int, int, int, int, int, int, int, int, int)'
/usr/bin/ld: stereoVision.cpp:(.text+0x149): undefined reference to `cv::Mat::Mat()'
/usr/bin/ld: stereoVision.cpp:(.text+0x158): undefined reference to `cv::Mat::Mat()'
/usr/bin/ld: stereoVision.cpp:(.text+0x251): undefined reference to `cv::Mat::convertTo(cv::_OutputArray const&, int, double, double) const'
/usr/bin/ld: stereoVision.cpp:(.text+0x4fa): undefined reference to `cv::operator/(cv::Mat const&, double)'
/usr/bin/ld: stereoVision.cpp:(.text+0x513): undefined reference to `cv::_InputArray::_InputArray(cv::MatExpr const&)'
/usr/bin/ld: stereoVision.cpp:(.text+0x542): undefined reference to `cv::imshow(cv::String const&, cv::_InputArray const&)'
/usr/bin/ld: stereoVision.cpp:(.text+0x579): undefined reference to `cv::waitKey(int)'
/usr/bin/ld: stereoVision.cpp:(.text+0x5ab): undefined reference to `cv::Mat::~Mat()'
/usr/bin/ld: stereoVision.cpp:(.text+0x5ba): undefined reference to `cv::Mat::~Mat()'
/usr/bin/ld: stereoVision.cpp:(.text+0x5d8): undefined reference to `cv::Mat::~Mat()'
/usr/bin/ld: stereoVision.cpp:(.text+0x5e7): undefined reference to `cv::Mat::~Mat()'
/usr/bin/ld: stereoVision.cpp:(.text+0x701): undefined reference to `cv::Mat::~Mat()'
/usr/bin/ld: CMakeFiles/stereoVision.dir/stereoVision.o:stereoVision.cpp:(.text+0x710): more undefined references to `cv::Mat::~Mat()' follow
/usr/bin/ld: CMakeFiles/stereoVision.dir/stereoVision.o: in function `cv::String::String(char const*)':
stereoVision.cpp:(.text._ZN2cv6StringC2EPKc[_ZN2cv6StringC5EPKc]+0x58): undefined reference to `cv::String::allocate(unsigned long)'
/usr/bin/ld: CMakeFiles/stereoVision.dir/stereoVision.o: in function `cv::String::~String()':
stereoVision.cpp:(.text._ZN2cv6StringD2Ev[_ZN2cv6StringD5Ev]+0x18): undefined reference to `cv::String::deallocate()'
/usr/bin/ld: CMakeFiles/stereoVision.dir/stereoVision.o: in function `cv::String::String(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)':
stereoVision.cpp:(.text._ZN2cv6StringC2ERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE[_ZN2cv6StringC5ERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE]+0x61): undefined reference to `cv::String::allocate(unsigned long)'
/usr/bin/ld: CMakeFiles/stereoVision.dir/stereoVision.o: in function `cv::MatExpr::~MatExpr()':
stereoVision.cpp:(.text._ZN2cv7MatExprD2Ev[_ZN2cv7MatExprD5Ev]+0x1e): undefined reference to `cv::Mat::~Mat()'
/usr/bin/ld: stereoVision.cpp:(.text._ZN2cv7MatExprD2Ev[_ZN2cv7MatExprD5Ev]+0x2e): undefined reference to `cv::Mat::~Mat()'
/usr/bin/ld: stereoVision.cpp:(.text._ZN2cv7MatExprD2Ev[_ZN2cv7MatExprD5Ev]+0x3e): undefined reference to `cv::Mat::~Mat()'
collect2: error: ld returned 1 exit status
make[2]: *** [CMakeFiles/stereoVision.dir/build.make:118: stereoVision] Error 1
make[1]: *** [CMakeFiles/Makefile2:76: CMakeFiles/stereoVision.dir/all] Error 2
make: *** [Makefile:84: all] Error 2

CMakeLists.txt 不完整！

CMakeLists.txt :

find_package(Pangolin REQUIRED)

add_executable(stereoVision stereoVision.cpp)


project(stereo)
cmake_minimum_required(VERSION 3.10)
# 链接OpenCV库
find_package( OpenCV REQUIRED )
#添加头文件
include_directories( ${OpenCV-INCLUDE_DIRS})
target_link_libraries(stereoVision ${OpenCV_LIBS})
 
# 链接OpenCV库
target_link_libraries(stereoVision ${OpenCV_LIBS} ${Pangolin_LIBRARIES})
set(CMAKE_CXX_FLAGS "-std=c++11")

terminate called after throwing an instance of 'cv::Exception'
  what():  OpenCV(3.4.15) /home/liqiang/Videos/opencv/opencv-3.4.15/modules/imgproc/src/median_blur.dispatch.cpp:283: error: (-215:Assertion failed) !_src0.empty() in function 'medianBlur'

Aborted (core dumped)

图片路径没有弄对！！cpp文件里有两种解决方法。

joinMap.cpp

#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <boost/format.hpp>  // for formating strings
#include <sophus/se3.hpp>
#include <pangolin/pangolin.h>

using namespace std;
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
typedef Eigen::Matrix<double, 6, 1> Vector6d;//6*1矩阵

// 在pangolin中画图，已写好，无需调整
//在pangolin中绘制点云图
void showPointCloud(
    const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud);

int main(int argc, char **argv) {
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图 定义彩色图向量类容器和深度图向量类容器
    TrajectoryType poses;         // 相机位姿

    ifstream fin("/home/liqiang/slambook2/ch5/rgbd/pose.txt");//  ifstream fin("../pose.txt"); //创建文件输入流fin
    if (!fin) {
        cerr << "请在有pose.txt的目录下运行此程序" << endl;
        return 1;
    }

    for (int i = 0; i < 5; i++) //有五张图
     {
        //把彩色图放到colorImages中，把深度图放到depthImages中！
        boost::format fmt("../%s/%d.%s"); //图像文件格式
        colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));
        depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "pgm").str(), -1)); // 使用-1读取原始图像
        
        //把位姿放到poses中！
        double data[7] = {0};//用数组存储单个位姿SE3d
        for (auto &d:data)//基于范围的for循环，auto表示自动类型推导
            fin >> d;//fin表示文件输入流
        //用李群存储的单个位姿
        Sophus::SE3d pose(Eigen::Quaterniond(data[6], data[3], data[4], data[5]),
                          Eigen::Vector3d(data[0], data[1], data[2]));
        poses.push_back(pose);
    }

    // 计算点云并拼接
    // 相机内参 
    double cx = 325.5;//x方向上的原点平移量
    double cy = 253.5;//y方向上的原点平移量
    double fx = 518.0;//焦距
    double fy = 519.0;//焦距
    double depthScale = 1000.0; //现实世界中1米在深度图中存储为一个depthScale值
    vector<Vector6d, Eigen::aligned_allocator<Vector6d>> pointcloud; //创建一个vector<Vector6d>变量
    pointcloud.reserve(1000000);//reserve()函数用来给vector预分配存储区大小，但不对该段内存进行初始化

    for (int i = 0; i < 5; i++) {
        cout << "转换图像中: " << i + 1 << endl;
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Sophus::SE3d T = poses[i];//用SE3d表示的从当前相机坐标系到世界坐标系的变换
        //遍历每个像素点
        for (int v = 0; v < color.rows; v++)
            for (int u = 0; u < color.cols; u++) {
                unsigned int d = depth.ptr<unsigned short>(v)[u]; // 深度值
                if (d == 0) continue; // 为0表示没有测量到
                Eigen::Vector3d point;
                point[2] = double(d) / depthScale;//真实世界中的深度值
                point[0] = (u - cx) * point[2] / fx;
                point[1] = (v - cy) * point[2] / fy;
                Eigen::Vector3d pointWorld = T * point;

                Vector6d p;//前三维表示点云的位置，后三维表示点云的颜色
                p.head<3>() = pointWorld;//head<n>()函数是对于Eigen库中的向量类型而言的，表示提取前n个元素
                //opencv中图像的data数组表示把其颜色信息按行优先的方式展成的一维数组！
                //color.step等价于color.cols
                //color.channels()表示图像的通道数
                p[5] = color.data[v * color.step + u * color.channels()];   // blue 蓝色分量
                p[4] = color.data[v * color.step + u * color.channels() + 1]; // green 绿色分量
                p[3] = color.data[v * color.step + u * color.channels() + 2]; // red 红色分量
            }
    }

    cout << "点云共有" << pointcloud.size() << "个点." << endl;
    showPointCloud(pointcloud);
    return 0;
}

void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud) {

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }
    //生成一个pangolin窗口
    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);//分别表示窗口名Point Cloud Viewer、窗口宽度=1024和窗口高度=768
    glEnable(GL_DEPTH_TEST);//根据物体远近，实现遮挡效果
    glEnable(GL_BLEND);//使用颜色混合模型，让物体显示半透明效果
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);//GL_SRC_ALPHA表示使用源颜色的alpha值作为权重因子，GL_ONE_MINUS_SRC_ALPHA表示使用(1-源颜色的alpha值)作为权重因子
   
   //ProjectionMatrix()中各参数依次为图像宽度=1024、图像高度=768、fx=500、fy=500、cx=512、cy=389、最近距离=0.1和最远距离=1000
   //ModelViewLookAt()中各参数为相机位置，被观察点位置和相机哪个轴朝上
   //比如，ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)表示相机在(0, -0.1, -1.8)位置处观看视点(0, 0, 0)，并设置相机XYZ轴正方向为（0，-1，0），即右上前

   //创建交互视图，显示上一帧图像内容

    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));
   //SetBounds()内的前4个参数分别表示交互视图的大小，均为相对值，范围在0.0至1.0之间
   //第1个参数表示bottom，即为视图最下面在整个窗口中的位置
   //第2个参数为top，即为视图最上面在整个窗口中的位置
   //第3个参数为left，即视图最左边在整个窗口中的位置
   //第4个参数为right，即为视图最右边在整个窗口中的位置
   //第5个参数为aspect，表示横纵比
   
    while (pangolin::ShouldQuit() == false) //如果pangolin窗口没有关闭，则执行
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);//清空颜色和深度缓存，使得前后帧不会互相干扰

        d_cam.Activate(s_cam);//激活显示，并设置相机状态
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);//设置背景颜色为白色

        glPointSize(2);
        glBegin(GL_POINTS); //绘制点云
        for (auto &p: pointcloud) {
            glColor3f(p[3], p[3], p[3]); //设置颜色信息
            glVertex3d(p[0], p[1], p[2]);//设置位置信息
        }
        glEnd();
        pangolin::FinishFrame();//按照上面的设置执行渲染
        usleep(5000);   // sleep 5 ms 停止执行5毫秒
    }
    return;
}

CMakeLists.txt :

project(rgbd)
 
cmake_minimum_required(VERSION 3.10)
 
find_package(Sophus REQUIRED)
 
add_executable(joinMap joinMap.cpp)
add_executable(joinMap1 joinMap1.cpp)
 
include_directories(${Sophus_INCLUDE_DIRS})
 
#链接OpenCV库
 
find_package(OpenCV REQUIRED)
 
#添加头文件
 
include_directories( ${OpenCV-INCLUDE_DIRS})
 
target_link_libraries(joinMap ${OpenCV_LIBS} ${Pangolin_LIBRARIES})


 
 
find_package(Pangolin REQUIRED)
 
 
target_link_libraries(joinMap ${OpenCV_LIBS} ${Pangolin_LIBRARIES})

 
 
# Eigen
 
include_directories("/usr/local/include/eigen3")
 
target_link_libraries(joinMap ${Sophus_LIBRARIES} fmt)

 
 
#链接c++11
 
set(CMAKE_CXX_FLAGS "-std=c++14")

执行结果：

转换图像中: 1
转换图像中: 2
转换图像中: 3
转换图像中: 4
转换图像中: 5
点云共有1081843个点.

习题解答

1. *寻找一个相机（你手机或笔记本的摄像头即可），标定它的内参。你可能会用到标定板，或者自己打印一张标定用的棋盘格。

参考我之前写过的这篇博客：

ubuntu18.04笔记本摄像头标定详细步骤_nudt一枚研究生-CSDN博客

2. 叙述相机内参的物理意义。如果一个相机的分辨率变成两倍而其他地方不变，它的内参如何变化？

参考链接：相机内参的物理意义_@清欢_-CSDN博客

3. 搜索特殊的相机（鱼眼或全景）相机的标定方法。它们与普通的针孔模型有何不同？

本人没有亲自尝试过（鱼眼或全景）相机的标定，有需要这方面的大家可以去自己深入研究，我有同学无人机方向的，就需要对鱼眼相机进行标定。

转载于：相机标定4之单目鱼眼相机标定--(Chessboard) - 知乎

大家可以自己去实践，我手头上也没有鱼眼相机。有机会的话找同学借一个鱼眼相机做一下标定。

#include "opencv2/calib3d.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"

#include <vector>
#include <string>
#include <algorithm>
#include <iostream>
#include <iterator>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
using namespace std;
using namespace cv;
enum Pattern
{
    CHESSBOARD,
    CIRCLES_GRID,
    ASYMMETRIC_CIRCLES_GRID
};
String keys = "{help      |           |print input information}"
              "{w         |9          |boardSize.weight}"
              "{h         |6          |boardSize.height}"
              "{s         |0.05       |squareSize       }";
static void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f> &corners, Pattern patternType = CHESSBOARD)
{
    cout << endl;
    corners.resize(0);

    switch (patternType)
    {
    case CHESSBOARD:
    case CIRCLES_GRID:
        for (int i = 0; i < boardSize.height; i++)
        {
            for (int j = 0; j < boardSize.width; j++)
            {
                corners.push_back(Point3f(float(j * squareSize),
                                          float(i * squareSize), 0));
                cout << Point3f(float(j * squareSize),
                                float(i * squareSize), 0)
                     << "\t";
                //  cout<<Point3f(0.05,0.06,0)<<endl;
            }
            cout << endl;
        }
        break;

    case ASYMMETRIC_CIRCLES_GRID:
        for (int i = 0; i < boardSize.height; i++)
            for (int j = 0; j < boardSize.width; j++)
                corners.push_back(Point3f(float((2 * j + i % 2) * squareSize),
                                          float(i * squareSize), 0));
        break;

    default:
        CV_Error(Error::StsBadArg, "Unknown pattern type\n");
    }
}

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

    CommandLineParser parser(argc, argv, keys);
    Size boardSize(9, 6);
    float squareSize = 1;
    if (parser.has("w"))
    {

        boardSize.width = parser.get<int>("w");
        boardSize.height = parser.get<int>("h");
    }
    if (parser.has("s"))
        squareSize = parser.get<float>("s");

    Size imageSize;
    vector<Mat> images;
    vector<string> images_list;
    vector<Point2f> corner;
    vector<vector<Point2f>> imagePoints;
    vector<Point3f> objectCorner;
    vector<vector<Point3f>> objectPoints;

    int j = 0;
    Mat temp;
    bool found = false;

    glob("./data/fisheye/*.jpg", images_list, false);
    for (int i = 0; i < images_list.size(); i++)
    {
        temp = imread(images_list[i], 1);

        Mat gray;
        cvtColor(temp, gray, COLOR_BGR2GRAY);
        // imshow("src_temp", gray);
        // waitKey(-1);
        found = findChessboardCornersSB(gray, boardSize, corner, CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_ACCURACY);
        // found = findChessboardCorners(gray, boardSize, corner,
        //                               CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);
        // found = findChessboardCorners(gray,boardSize,corner,0);
        // cout << corner.size() << endl;
        if (found)
        {
            if (j == 0)
                imageSize = temp.size();
            cornerSubPix(gray, corner, Size(11, 11), Size(-1, -1),
                         TermCriteria(TermCriteria::COUNT + TermCriteria::EPS,
                                      30, 0.01));
            drawChessboardCorners(temp, Size(9, 6), corner, found);
            images.push_back(imread(images_list[i], 1));
            imagePoints.push_back(corner);
            // String filename = images_list[i].substr(0, images_list[i].length() - 4) + ".jpg";
            // cout << filename << endl;
            // imwrite(filename, images[j]);
            // j++;
            namedWindow("src_temp", WINDOW_KEEPRATIO | WINDOW_KEEPRATIO);
            imshow("src_temp", temp);
            // waitKey(1000);
            j++;
        }
        else
            continue;
    }

    // cout << j << endl;
    // cout << images.size() << endl
    //      << imagePoints.size() << endl;
    Mat K1, K2;
    Vec4d D1;
    Mat D2;
    vector<Vec3d> R1, R2, t1, t2;

    calcChessboardCorners(boardSize, squareSize, objectCorner, CHESSBOARD);
    objectPoints.resize(images.size(), objectCorner);

    // cout << K << endl;
    int flags = 0;
    flags |= fisheye::CALIB_CHECK_COND;
    // flags |= fisheye::CALIB_USE_INTRINSIC_GUESS;
    flags |= fisheye::CALIB_RECOMPUTE_EXTRINSIC;

    calibrateCamera(objectPoints, imagePoints, imageSize, K2, D2, R2, t2);
    cout << "K2=" << K2 << endl;
    cout << "D2=" << D2 << endl;

    // K1 = initCameraMatrix2D(objectPoints, imagePoints, imageSize);
    // K1 = K2.clone();
    // cout<<K1<<endl;

    fisheye::calibrate(objectPoints, imagePoints, imageSize, K1, D1, R1, t1, flags);

    cout << "K1=" << K1 << endl;
    cout << "D1" << D1 << endl;
    namedWindow("undistortImage1", WINDOW_KEEPRATIO | WINDOW_NORMAL);
    // namedWindow("undistortImage2", WINDOW_KEEPRATIO | WINDOW_NORMAL);

    namedWindow("distortImage", WINDOW_KEEPRATIO | WINDOW_NORMAL);
    namedWindow("undistortImage__", WINDOW_KEEPRATIO | WINDOW_NORMAL);
    // namedWindow("image_concat", WINDOW_KEEPRATIO | WINDOW_NORMAL);
    for (int i = 0; i < images.size(); i++)
    {
        Mat undistortedImage1;
        // Mat undistortedImage2;
        Mat distortedImage = images[i];
        // Mat image_concat;
        fisheye::undistortImage(distortedImage, undistortedImage1, K1, D1, K1, imageSize);
        // undistort(distortedImage, undistortedImage2, K2, D2, K2);
        // vector<Mat> all = {undistortedImage1, distortedImage, undistortedImage2};
        // hconcat(all, image_concat);
        // fisheye::undistortPoints()
        imshow("distortImage", distortedImage);
        imshow("undistortImage1", undistortedImage1);
        // imshow("undistortImage2", undistortedImage2);
        // imshow("distortImage", image_concat);
        // waitKey(-1);
    }

    Mat P;
    Mat mapx, mapy;

    fisheye::estimateNewCameraMatrixForUndistortRectify(K1, D1, imageSize, Matx33d::eye(), P, 0.0, imageSize, 0.8);
    cout << "P:" << endl
         << P << endl;

    fisheye::initUndistortRectifyMap(K1, D1, Mat(), P, imageSize, CV_16SC2, mapx, mapy);
    for (int i = 0; i < images.size(); i++)
    {
        Mat undistortImage2;
        Mat distortImage = images[i];
        remap(distortImage, undistortImage2, mapx, mapy, INTER_LINEAR);
        imshow("distortImage", distortImage);
        imshow("undistortImage__", undistortImage2);
        // waitKey(-1);
    }
    cout << "project erorr" << endl;
    double total_error = 0;
    for (int i = 0; i < imagePoints.size(); i++)
    {
        vector<Point2f> imagePointstemp;
        // fisheye::projectPoints(objectCorner, imagePointstemp, R1[i], t1[i], K1, D1);

        fisheye::projectPoints(objectCorner, imagePointstemp, Affine3d(R1[i], t1[i]), K1, D1);
        // cout<<"p_rt"<<p_rt<<endl;

        double error = norm(Mat(imagePointstemp), Mat(imagePoints[i]), NORM_L2);
        error = error / (boardSize.height * boardSize.width);
        cout << "error " << i << "=" << error << endl;
        total_error += error;
    }
    cout << "preject error=" << total_error / imagePoints.size() << endl;
    vector<Point2f> undistortedPoint__;
    for (int i = 0; i < imagePoints.size(); i++)
    {
        fisheye::undistortPoints(imagePoints[i], undistortedPoint__, K1, D1, noArray(), P);
        Mat image__;
        remap(images[i], image__, mapx, mapy, INTER_LINEAR);
        drawChessboardCorners(image__, boardSize, undistortedPoint__, found);
        drawChessboardCorners(images[i], boardSize, imagePoints[i], found);
        imshow("ddd", image__);
        imshow("ccc", images[i]);
        waitKey(-1);
    }

    return EXIT_SUCCESS;
    // found =findChessboardCornersSB(image,boardSize,corners,CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE );
}

4. 调研全局快门相机（global shutter）和卷帘快门相机（rolling shutter）的异同。它们在SLAM 中有何优缺点？

参考下面这些文章：

全局快门与卷帘式快门 - 知乎

果冻效应rolling shutter - 知乎

5. RGB-D 相机是如何标定的？以Kinect 为例，需要标定哪些参数？（参照https://github.com/code-iai/iai_kinect2.）

我手头上有一个Intel realsense d435i的相机，intel的相机在出厂前已经标定好了，我们并不需要标定，因为标定的时候很容易标定错误，然后就得返厂，所以大家一定要考虑好，但是如果你为了学习的话，是可以自己搞的，大不了到时候让技术人员弄，为了求知，也是可以的，希望大家考虑好。

标定可以参照之前我写的博客：

Ubuntu18.04安装realsense d435i SDK和ROS Wrapper以及相机标定全过程_nudt一枚研究生-CSDN博客

6. 除了示例程序演示的遍历图像的方式，你还能举出哪些遍历图像的方法？

转载于：视觉slam十四讲（第二版）习题-第5章 - 知乎

#include <iostream>
#include <chrono> //std::chrono 来给算法计时所需头文件

using namespace std;

#include <opencv2/core/core.hpp>//opencv核心模块
#include <opencv2/highgui/highgui.hpp>//opencv gui模块

int main(int argc, char **argv) {
  // 读取argv[1]指定的图像
  cv::Mat image;
  image = cv::imread(argv[1]); //cv::imread函数读取指定路径下的图像

  // 判断图像文件是否正确读取
  if (image.data == nullptr) { //数据不存在,可能是文件不存在
    cerr << "文件" << argv[1] << "不存在." << endl;
    return 0;
  }

  // 文件顺利读取, 首先输出一些基本信息
  cout << "图像宽为" << image.cols << ",高为" << image.rows << ",通道数为" << image.channels() << endl;
  cv::imshow("image", image);      // 用cv::imshow显示图像
  cv::waitKey(0);                  // 暂停程序,等待一个按键输入

  // 判断image的类型
  if (image.type() != CV_8UC1 && image.type() != CV_8UC3) {
    // 图像类型不符合要求
    cout << "请输入一张彩色图或灰度图." << endl;
    return 0;
  }

  // 遍历图像, 请注意以下遍历方式亦可使用于随机像素访问
  // 使用 std::chrono 来给算法计时
  //使用指针遍历图像image中的像素
  //steady_clock是单调的时钟，相当于教练中的秒表，只会增长，适合用于记录程序耗时。
// 遍历图像, 请注意以下遍历方式亦可使用于随机像素访问
  // 使用 std::chrono 来给算法计时
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (size_t y = 0; y < image.rows; y++) {
    // 用cv::Mat::ptr获得图像的行指针
    unsigned char *row_ptr = image.ptr<unsigned char>(y);  // row_ptr是第y行的头指针
    for (size_t x = 0; x < image.cols; x++) {
      // 访问位于 x,y 处的像素
      unsigned char *data_ptr = &row_ptr[x * image.channels()]; // data_ptr 指向待访问的像素数据
      // 输出该像素的每个通道,如果是灰度图就只有一个通道
      for (int c = 0; c != image.channels(); c++) {
        unsigned char data = data_ptr[c]; // data为I(x,y)第c个通道的值
      }
    }
  }
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast < chrono::duration < double >> (t2 - t1);
  cout << "image.ptr行指针 遍历图像用时：" << time_used.count() << " 秒。" << endl;



  chrono::steady_clock::time_point t3 = chrono::steady_clock::now();
  for (int y=0; y<image.rows; y++)
  {
    for (int x=0; x<image.cols; x++)
    {
      for (int c=0; c<image.channels(); c++)
      {
        unsigned char data=image.at<cv::Vec3b>(y , x)[c];
      }
    }
  }
  chrono::steady_clock::time_point t4 = chrono::steady_clock::now();
  chrono::duration<double> time_used2 = chrono::duration_cast < chrono::duration < double >> (t4 - t3);
  cout << "at访问 遍历图像用时：" << time_used2.count() << " 秒。" << endl;

  chrono::steady_clock::time_point t5 = chrono::steady_clock::now();
    cv::MatConstIterator_<cv::Vec3b> it_in=image.begin<cv::Vec3b>();
    cv::MatConstIterator_<cv::Vec3b> itend_in=image.end<cv::Vec3b>();
  while(it_in!=itend_in)
  {
    for (int c=0; c<image.channels(); c++)
    {
      unsigned char data=(*it_in)[c];
    }
    it_in++;
  }
  chrono::steady_clock::time_point t6 = chrono::steady_clock::now();
  chrono::duration<double> time_used3 = chrono::duration_cast < chrono::duration < double >> (t6 - t5);
  cout << "iterator访问 遍历图像用时：" << time_used3.count() << " 秒。" << endl;


  // 关于 cv::Mat 的拷贝
  // 直接赋值并不会拷贝数据
  cv::Mat image_another = image;
  // 修改 image_another 会导致 image 发生变化
  image_another(cv::Rect(0, 0, 100, 100)).setTo(0); // 将左上角100*100的块置为黑色
  cv::imshow("image", image);
  cv::waitKey(0);//停止执行，等待一个按键输入

  // 使用clone函数来拷贝数据
  cv::Mat image_clone = image.clone();
  image_clone(cv::Rect(0, 0, 100, 100)).setTo(255);//将左上角100*100的块置为白色
  cv::imshow("image", image);
  cv::imshow("image_clone", image_clone);
  cv::waitKey(0);//停止执行，等待一个按键输入

  // 对于图像还有很多基本的操作,如剪切,旋转,缩放等,限于篇幅就不一一介绍了,请参看OpenCV官方文档查询每个函数的调用方法.
  cv::destroyAllWindows();
  return 0;
}

操作和上面的imageBasics.cpp是一样的

图像宽为1200,高为674,通道数为3
image.ptr行指针 遍历图像用时：0.0158651 秒。
at访问 遍历图像用时：0.0120214 秒。
iterator访问 遍历图像用时：0.0192036 秒。

长沙有肥鱼

关注

22
点赞
踩
153

收藏

觉得还不错? 一键收藏
打赏
16
评论
视觉SLAM十四讲CH5代码解析及课后习题详解

/usr/bin/ld: CMakeFiles/stereoVision.dir/stereoVision.o: in function `main':stereoVision.cpp:(.text+0xae): undefined reference to `cv::imread(cv::String const&, int)'/usr/bin/ld: stereoVision.cpp:(.text+0xf1): undefined reference to `cv::imread(...
复制链接

扫一扫