目录
代码来源:gaoxiang12/slambook2: edition 2 of the slambook (github.com)
一、图像
图像可以由一个二维数组来表示。
其中灰度图每一个元素的下标表示一个像素的坐标,元素的值代表0~255(unsigned short)的灰度读数。值得注意的是,我们讨论一个位于(x,y)的像素时,表示为image[y][x],因为像素平面上x相右,y向下。
在彩色图中,一个像素占据3个字节,对应三个通道(RGB)。因此,image[y][x]表示像素的第一个字节在(x,y)。而image[y][x],image[y+1][x],image[y+2][x]的值表示三个通道的值。
二、相机
A、单目相机
1.相机模型
2. 成像过程
先介绍一下归一化平面:如图的+1处,即相机坐标的X、Y都除以Z:(X/Z,Y/Z,1)
3.opencv代码实战
1. 指向图像的变量(不知道可不可以理解为指针)
cv::Mat image=cv::imread("path of image",0);//后面这个0可有可无
2. 显示图像:cv::imshow("anything",image)
3. 图像基本信息
宽:image.cols 高:image.rows 通道数:image.channels()
CV_8UC1表示单通道图片,即灰度图;CV_8UC3表示三通道,即彩色图
4. 用image.ptr<unsigned char>(y)获得指向y行的行指针
unsigned char *row_ptr = image.ptr<unsigned char>(y)
5. 置0:
image(cv::Rect(0, 0, 100, 100)).setTo(0);
6. 复制(改变复制项不会对被复制项造成影响)
cv::Mat image_clone=image.clone();
直接复制相当于两个指针指向同一处
7. argv[1]指向在DOS命令行中执行程序名后的第一个字符串
所以程序里输入./executable ./image.jpg
#include <iostream>
#include <chrono> //计时
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
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;
//CV_8UC1表示单通道图片,即灰度图,CV_8UC3表示三通道,即彩色图
return 0;
}
// 遍历图像, 请注意以下遍历方式亦可使用于随机像素访问
// 使用 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 处的像素
//x * image.channels()表示单通道一个一个过,三通道一下跳3个
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);
//waitKey刷新图像的时间。waitKey(0)无限等待,waitKey(30)表示等待30ms
// 使用clone函数来拷贝数据
cv::Mat image_clone = image.clone();
image_clone(cv::Rect(0, 0, 100, 100)).setTo(255);
cv::imshow("image", image);
cv::imshow("image_clone", image_clone);
cv::waitKey(0);
cv::destroyAllWindows();
return 0;
}
4.图像去畸化
已知一张畸变之后的图像,内参,畸变参数,求去畸变的图像
思路:实现坐标的一一映射,再对灰度赋值
1. 设未畸变的图像的坐标为(u,v),畸变的图像坐标为(ud,vd)
2. 通过(u,v)->(x,y)
-畸变公式->(xd,yd)->(ud,vd)得到(u,v)和(ud,vd)的关系
3. 让(u,v)从(0,0)开始走,遍历到(rows,cols),确定对应的(ud,vd)的坐标
4. 把(ud,vd)对应的灰度值赋给(u,v)即得到,去畸变得图像
实现
灰度值为image.at<uchar>(v,u),直接赋值
image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);
#include <opencv2/opencv.hpp>
#include <string>
using namespace std;
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
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)
//建立畸变图像与非畸变图像的一一映射
double x = (u - cx) / fx, y = (v - cy) / fy;//非畸变图像坐标归一化
double r = sqrt(x * x + y * y);
double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);
double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;
double u_distorted = fx * x_distorted + cx;
double v_distorted = fy * y_distorted + cy;
// 赋值 (最近邻插值)
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);
}
else {
image_undistort.at<uchar>(v, u) = 0;
}
}
}
// 画图去畸变后图像
cv::imshow("distorted", image);
cv::imshow("undistorted", image_undistort);
cv::waitKey();
return 0;
}
B、双目相机
|- (u,v)和单目相机相同
|-
其中b为基线
代码实践
1. 得出d:(其实代码看不太懂)
用sgbm算法得出,disparity
cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32); // 神奇的参数
cv::Mat disparity_sgbm, disparity;
sgbm->compute(left, right, disparity_sgbm);
disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);
2. 得出深度depth=fb/disparity.at<float>(v,u);
这里disparity是一个视差图
3. 将数据存入一个向量Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0);生成数据点
4. 将数据点存入动态数组 vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;生成点云。
这里代码的注释来自:(1条消息) 第5讲 相机与图像_YMWM_的博客-CSDN博客
#include <iostream>
#include <ctime>
#include <vector>
#include <unistd.h>
using namespace std;
#include <opencv2/opencv.hpp>
using namespace cv;
#include <Eigen/Core> //Eigen核心模块
using namespace Eigen;
#include <pangolin/pangolin.h>
//相机内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157, b = 0.573;
//文件路径
string left_file = "../left.png";
string right_file = "../right.png";
//在pangolin中画图,已写好,无需调整
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>>& pointcloud);
clock_t start, last;
int main()
{
start = clock();
Mat left = imread(left_file, 0); //0表示返回一张灰度图
Mat right = imread(right_file, 0); //0表示返回一张灰度图
//调用opencv中的sgbm算法来求解深度
Ptr<StereoSGBM> sgbm = StereoSGBM::create(
0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32
); //来自网络上的关于sgbm算法的经典参数配置
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,表示位置信息,后一维表示颜色信息,在此处为灰度
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) <= 10.0 || disparity.at<float>(v, u) >= 96.0)
continue;
Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); //创建一个Vector4d类型的变量,前三维用来存储位置信息,后一维为归一化之后的灰度
//根据双目模型恢复像素点的三维位置
double Z = fx * b / (disparity.at<float>(v, u));
double X = (u - cx) / fx * Z;
double Y = (v - cy) / fy * Z;
//将X,Y,Z赋值给point的前三维
point[0] = X; point[1] = Y; point[2] = Z;
pointcloud.push_back(point);
}
imshow("视差图", disparity / 96.0); //disparity/96表示归一化之后的视差图像!
waitKey(0); //停止执行,等待一个按键输入
//用pangolin画出点云
showPointCloud(pointcloud);
last = clock();
cout << "执行程序总共花费的时间为:" << (double)(last - start) * 1000 / CLOCKS_PER_SEC << "毫秒!" << endl;
return 0;
}
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>>& pointcloud)
{
if(pointcloud.empty())
{
cerr << "点云为空,返回!" << endl;
return;
}
//创建一个pangolin窗口
pangolin::CreateWindowAndBind("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()中参数依次表示相机观察视图的宽度和高度,fx, fy, cx, cy,最小深度和最大深度
//ModelViewLookAt()中参数依次为相机光心位置,被观察点位置,相机坐标系各坐标轴的正方向(000表示右下前)
//创建交互视图,显示上一帧图像内容
pangolin::Handler3D handler(s_cam); //交互视图句柄
pangolin::View &d_cam = pangolin::CreateDisplay()
.SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
.SetHandler(&handler);
//SetBounds()中各参数表示交互视图范围、交互视图横纵比
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); //设置线宽为2
//绘制点云
glBegin(GL_POINTS);
for(auto &p : pointcloud) //使用基于范围的for进行遍历
{
glColor3f(p[3], p[3], p[3]); //设置颜色信息
glVertex3d(p[0], p[1], p[2]); //设置位置信息
}
glEnd();
pangolin::FinishFrame(); //按照上面的设置执行渲染!
usleep(5000); //停止执行5毫秒
}
return;
}
C、RGB-D相机
这里代码的注释仍然来自:(1条消息) 第5讲 相机与图像_YMWM_的博客-CSDN博客
思路:
#include <iostream>
#include <fstream>
#include <boost/format.hpp>
#include <ctime>
using namespace std;
#include <opencv2/opencv.hpp>
using namespace cv;
#include <sophus/se3.hpp>
using namespace Sophus;
#include <pangolin/pangolin.h>
typedef vector<SE3d, Eigen::aligned_allocator<SE3d>> TrajectoryType;
typedef Eigen::Matrix<double, 6, 1> Vector6d;
//在pangolin中绘制点云图
void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>>& pointcloud);
//相机内参
double cx = 325.5, cy = 253.5, fx = 518.0, fy = 519.0;
double depthScale = 1000.0; //现实世界中1米在深度图中存储为一个depthScale值
clock_t start, last;
int main()
{
start = clock();
vector<Mat> colorImages, depthImages; //定义彩色图向量类容器和深度图向量类容器
TrajectoryType poses; //定义相机位姿
ifstream fin("../data/pose.txt"); //创建文件输入流fin
if(!fin)
{
cerr << "找不到文件位姿文件!" << endl;
return 1;
}
for(int i = 0; i < 5; i++) //有五张图
{
//把彩色图放到colorImages中,把深度图放到depthImages中!
boost::format fmt("../data/%s/%d.%s"); //定义图像文件格式
colorImages.push_back( imread( (fmt % "color" % (i + 1) % "png").str() ) );
depthImages.push_back( imread( (fmt % "depth" % (i + 1) % "pgm").str() ) );
//把位姿放到poses中!
double tmp[7] = {0}; //用数组存储单个位姿SE3d
for(auto &val : tmp) //基于范围的for循环,auto表示自动类型推导
fin >> val; //fin表示文件输入流
//用李群存储的单个位姿
SE3d pose(Eigen::Quaterniond(tmp[6], tmp[3], tmp[4], tmp[5]),
Eigen::Vector3d(tmp[0], tmp[1], tmp[2]));
poses.push_back(pose);
}
fin.close(); //关闭文件输入流fin
//计算点云并拼接
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;
Mat color = colorImages[i];
Mat depth = depthImages[i];
SE3d T = poses[i]; //用SE3d表示的从当前相机坐标系到世界坐标系的变换
//遍历每个像素点
for(int v = 0; v < color.rows; v++)
for(int u = 0; u < color.cols; u++)
{
//depth.ptr<unsigned short>(v)表示行指针
unsigned int d = depth.ptr<unsigned short>(v)[u]; //深度值
if(d == 0) continue; //d为0表示没有观测到!
//计算点云在该帧相机坐标系下的坐标值
double X, Y, Z;
Z = d / depthScale; //真实世界中的深度值
X = (u - cx) * Z / fx;
Y = (v - cy) * Z / fy;
Vector3d point(X, Y, Z);
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()]; //蓝色分量
p[4] = color.data[v * color.step + u * color.channels() + 1]; //绿色分量
p[3] = color.data[v * color.step + u * color.channels() + 2]; //红色分量
pointCloud.push_back(p);
}
}
cout << "总共有" << pointCloud.size() << "个点云!" << endl;
showPointCloud(pointCloud);
last = clock();
cout << "程序总共花费的时间为:" << (double)(last - start) * 1000 / CLOCKS_PER_SEC << "毫秒!" << endl;
return 0;
}
void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>>& pointcloud)
{
if(pointcloud.empty())
{
cerr << "点云是空的!" << endl;
return;
}
//生成一个pangolin窗口
pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
glEnable(GL_DEPTH_TEST); //使能遮挡渲染
glEnable(GL_BLEND); //使用颜色混合模式
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
//使用(源图像的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()中参数依次为pangolin窗口的宽度、pangolin窗口的高度、fx、fy、cx、cy、最小深度值和最大深度值
//ModelViewLookAt()中各参数依次为相机位置、被观察点位置和相机各轴正方向(000表示右下前)
//设置相机窗口
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); //设置相机视图背景颜色
//绘制点云
glBegin(GL_POINTS); //绘制点开始指令
for(auto &p : pointcloud)
{
glPointSize(2); //设置点的大小
glColor3d(p[3] / 255.0, p[4] / 255.0, p[5] / 255.0);
glVertex3d(p[0], p[1], p[2]);
}
glEnd(); //绘制点结束指令
pangolin::FinishFrame(); //按照上述设置执行渲染,并显示在pangolin窗口中
usleep(5000); //程序停止执行5毫秒!
}
return;
}