一、opencv的简单使用
该程序实现的功能是输出图片的基本信息，同时将图片左上角的100x100区域用白色的100x100窗口覆盖掉
源码如下：
//
//
#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]="/home/nnz/data/slam_pratice/pratice_cv/ubuntu.png";
cv::Mat image=cv::imread(argv[1]);
//判断图片有没有读取成功
if(image.data== nullptr)
{
cerr<<"文件不存在！"<<endl;
return 0;
}
//文件读取成功，对该图片进行简单的操作
//首先输出图片的基本信息，宽，高，通道数等等
cout<<"图片的宽为："<<image.cols<<"\t图片高为："<<image.rows<<
"图片的通道数为："<<image.channels()<<endl;
//判断image的类型
//U代表unsigned C是通道，C后面的数字代表通道数
if(image.type()!=CV_8UC1 && image.type()!=CV_8UC3)
{
cout<<"请输入一张灰度图或者彩色图！"<<endl;
}
chrono::steady_clock::time_point t1=chrono::steady_clock::now();
//遍历图像的像素
for(size_t y=0;y<image.rows;y++)
{
//用cv::Ma::ptr获取行指针
//定义行指针 row_ptr
unsigned char *row_ptr=image.ptr<unsigned char>(y);
for(size_t x=0;x<image.cols;x++)
{
//定义访问 （x ,y）像素的指针 data_ptr
unsigned char *data_ptr=&row_ptr[x*image.channels()];
for(int c=0;c<image.channels();c++)
{
unsigned char data=data_ptr[c];//data是第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 image_clone=image 这样赋值的话，若改变了image_clone，则image也会改变
//所以应该用image_clone=image.clone()
cv::Mat image_clone=image.clone();//拷贝图片
image_clone(cv::Rect(0,0,100,100)).setTo(255);//255代表RGB255 截取的区域是白色
cv::imshow("image",image);
cv::imshow("image_clone",image_clone);
cv::waitKey(0);//按任意键清屏？结束
//int cvWaitKey( int delay=0 )
//返回值为int型，函数的参数为int型，当delay小于等于0的时候，如果没有键盘触发，则一直等待，此时的返回值为-1，否则返回值为键盘按下的码字；
// 当delay大于0时，如果没有键盘的的触发，则等待delay的时间，此时的返回值是-1，否则返回值为键盘按下的码字。
cv::destroyAllWindows();//销毁所有窗口
//上面的Rect()函数中的参数是要切割掉的窗口的大小、位置，后面的setTo()中的255是
return 0;
}
运行结果如下：
图片的宽为：1200 图片高为：674图片的通道数为：3
遍历图片用时：5.7e-08
可以看出拷贝的图片左上角被覆盖了100*100的区域，覆盖的颜色为白色
二、图像去畸变
该程序是将畸变的图形进行去畸变的操作，并显示去畸变和未去畸变的图像（手写去畸变）
源码如下：
//
// Created by wenbo on 2020/10/27.
//
#include<iostream>
#include<opencv2/opencv.hpp>
//首先确定5个畸变参数 k1,k2,k3,p1,p2
//k1,k2,k3纠正径向畸变
//p1,p2纠正切向畸变
//r=sqrt(x*x+y*y)
//x_d=x(1+k1r^2+k2r^4+k3r^6)+2p1xy+p2(r^2+2x^2) //归一化平面下的畸变点x_d
//y_d=y(1+k1r^2+k2r^4+k3r^6)+2p2xy+p1(r^2+2y^2) //归一化平面下的畸变点y_d
//最后再将畸变的归一化下的x_d 和 y_d 变为像素坐标即可
//u_d=fx*x_d+cx
//v_d=fy*y_d+cy
#include<string>
using namespace std;
string file="/home/nnz/data/slam_pratice/pratice_cv/distorted.png";
int main(int argc,char** argv)
{
//定义畸变参数
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(file,0);//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++)
{
//咱们应该先得到归一化下的坐标x,y 见P99 公式5.4
double x=(u-cx)/fx, y=(v-cy)/fy;
double r=sqrt(x*x+y*y);
double x_d=x*(1+k1*r*r+k2*r*r*r*r)+2*p1*x*y+p2*(r*r+2*x*x);
double y_d=y*(1+k1*r*r+k2*r*r*r*r)+2*p2*x*y+p1*(r*r+2*y*y);
double u_d=fx*x_d+cx;
double v_d=fy*y_d+cy;
//赋值
if(u_d>=0 && v_d>=0 && u_d<cols && v_d<rows)
{
image_undistort.at<uchar>(v,u)=image.at<uchar>((int)v_d,(int)u_d);
}
else
{
image_undistort.at<uchar>(v,u)=0;
}
}
}
//画出没有去畸变和去畸变的图片
cv::imshow("undistorted_image",image_undistort);
cv::imshow("distort_image",image);
cv::waitKey(0);
return 0;
}
运行结果如下：
三、stereo双目点云成像(重点是获取视差，得到深度信息)
利用pangolin绘制点云
源码如下：
//
//
#include<iostream>
#include<opencv2/opencv.hpp>
#include <vector>
#include<string>
#include<Eigen/Core>
#include<pangolin/pangolin.h>
#include<unistd.h>
using namespace std;
using namespace Eigen;
//path of file
string left_file="/home/nnz/data/slam_pratice/pratice_3dvision/left.png";
string right_file="/home/nnz/data/slam_pratice/pratice_3dvision/right.png";
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;
//base line
double b = 0.573;
//读图像
cv::Mat image_left=cv::imread(left_file,0);
cv::Mat image_right=cv::imread(right_file,0);
//static Ptr<StereoSGBM> create(int minDisparity = 0, int numDisparities = 16, int blockSize = 3, int P1 = 0, int P2 = 0, int disp12MaxDiff = 0, int preFilterCap = 0, int uniquenessRatio = 0, int speckleWindowSize = 0, int //speckleRange = 0,int mode = StereoSGBM::MODE_SGBM);
//semi-global matching（SGM）是一种用于计算双目视觉中视差（disparity）的半全局匹配算法，
// 在OpenCV中的实现为semi-global block matching（SGBM）
//minDisparity 最小的可能的视差值
//numDisparity 是最大视差减去最小视差
//disp12MaxDiff是左右图视差检查所允许的最大的不同值
//P1, P2：控制视差变化平滑性的参数。P1、P2的值越大，视差越平滑。P1是相邻像素点视差增/减 1 时的惩罚系数；P2是相邻像素点视差变化值大于1时的惩罚系数。P2必须大于P1。
//preFilterCap：预处理滤波器的截断值，预处理的输出值仅保留[-preFilterCap, preFilterCap]范围内的值，
//speckleWindowSize：检查视差连通区域变化度的窗口大小, 值为 0 时取消 speckle 检查，int 型
//speckleRange：视差变化阈值，当窗口内视差变化大于阈值时，该窗口内的视差清零，int 型
//uniquenessRatio：视差唯一性百分比， 视差窗口范围内最低代价是次低代价的(1 + uniquenessRatio/100)倍时，最低代价对应的视差值才是该像素点的视差，否则该像素点的视差为 0
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(image_left, image_right, disparity_sgbm);
//4-byte floating point (float)
//CV_32F是 float -像素是在0-1.0之间的任意值，这对于一些数据集的计算很有用，但是它必须通过将每个像素乘以255来转换成8位来保存或显示
disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);// disparity is optical parallax (shi cha)
// 生成点云
vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;
// 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
for (int v = 0; v < image_left.rows; v++)
for (int u = 0; u < image_left.cols; u++)
{
if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;
Vector4d point(0, 0, 0, image_left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色
// 根据双目模型计算 point 的位置
double x = (u - cx) / fx;//归一化坐标系下的x p99 5.5
double y = (v - cy) / fy;//归一化坐标系下的y
double depth = fx * b / (disparity.at<float>(v, u));// P104 5.15 get depth
point[0] = x * depth;//
point[1] = y * depth;
point[2] = depth;//
pointcloud.push_back(point);// get pointcloud
}
cv::imshow("disparity",disparity/96.0);
cv::waitKey(0);
showPointCloud(pointcloud);
return 0;
}
void showPointCloud(const vector<Vector4d,Eigen::aligned_allocator<Vector4d>> &pointcloud)
{
//pangolin的使用可以参考我之前的文章
//https://blog.csdn.net/joun772/article/details/109246680
判断点云存不存在
if(pointcloud.empty())
{
cerr<<"point cloud is empty"<<endl;
return;
}
//设置窗口
pangolin::CreateWindowAndBind("Point Cloud Viewer",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);
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);//slepp 5ms
}
return;
}
运行结果如下：
四、利用RGBD获得点云并显示点云图像
该程序主要将彩色图以及深度图的信息提取出来，得到具有深度信息的3d点，再通过已有的相机坐标文件（pose.txt），将相机坐标转换为世界坐标，利用Pangolin画出点云
源码如下：
//
//
#include<iostream>
#include<string>
#include <fstream>
#include<Eigen/Core>
#include <vector>
#include<boost/format.hpp>
#include<sophus/se3.hpp>
#include <pangolin/pangolin.h>
#include <opencv2/opencv.hpp>
#include<unistd.h>
using namespace std;
using namespace Eigen;
typedef vector<Sophus::SE3d,Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
typedef Eigen::Matrix<double,6,1> Vector6d;
void showPointCloud(const vector<Vector6d,Eigen::aligned_allocator<Vector6d>> &pointcloud);
int main()
{
//判断是不是在有pose.txt的路径下运行
ifstream fin("./pose.txt");
if(!fin)
{
cout<<"提示，请在有pose.txt的目录下运行程序！"<<endl;
return 1;//return 0 代表程序正常退出，return 1代表程序异常退出！
}
TrajectoryType poses;//定义装位姿的容器
vector<cv::Mat> colorim,depthim;
//读入深度图片和彩色图片以及位姿
//该程序用到了模板格式 format
for(int i=0;i<5;i++)
{
boost::format fmt("./%s/%d.%s");//图片文件路径格式
colorim.push_back(cv::imread((fmt%"color"%(i+1)%"png").str()));
//-1 读取原始图像
depthim.push_back(cv::imread((fmt%"depth"%(i+1)%"pgm").str(),-1));
//定义数组data[7]暂时接受 pose
double data[7]={
0};
for(auto &d:data) fin>>d;//利用d 把pose.txt中的内容遍历传给data
//调用Sophus库构建pose矩阵
Eigen::Quaterniond q(data[6],data[3],data[4],data[5]);//得到pose的旋转（四元数）
Vector3d t(data[0],data[1],data[2]);//得到平移向量
Sophus::SE3d pose(q,t);//得到pose
poses.push_back(pose);
}
// 计算点云并拼接
// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;
double depthScale = 1000.0;
//定义一个点云
vector<Vector6d,Eigen::aligned_allocator<Vector6d>> pointcloud;
//reserve为容器预先分配内存空间，并未初始化空间元素，因此不能用[]操作符访问元素，因为只是内存一块，
// 只能先用push_back，insert等函数插入函数后后再访问。
//其中size()函数是查看容器中有多少的元素，而不是内存；
pointcloud.reserve(1000000);
for(int i=0;i<5;i++)
{
cout<<"图像转换中："<<i+1<<endl;
cv::Mat color=colorim[i];//接收彩色图
cv::Mat depth=depthim[i];//接收深度图
Sophus::SE3d T=poses[i];//接收相机的位姿
//遍历每个图片的像素，并将深度图的深度信息提取出来，将像素点变为归一化后，再乘上深度信息得到相机坐标下的点，之后再左乘T,变为世界坐标
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];//得到对应像素点的深度信息，这里要注意过得深度信息的用法，利用ptr指针
if(d==0)
continue;
Eigen::Vector3d point;
point[2]=double(d)/depthScale;
point[1]=(v-cy)*point[2]/fy;
point[0]=(u-cx)*point[2]/fx;
Eigen::Vector3d w_point=T*point;//得到世界坐标下的3d点
Vector6d p;//前三个是坐标，后三个是颜色
p.head<3>()=w_point;
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
pointcloud.push_back(p);
}
}
}
cout<<"点云总共有："<<pointcloud.size()<<endl;
showPointCloud(pointcloud);
return 0;
}
void showPointCloud(const vector<Vector6d,Eigen::aligned_allocator<Vector6d>> &pointcloud)
{
//pangolin的使用可以参考我之前的文章
//https://blog.csdn.net/joun772/article/details/109246680
判断点云存不存在
if(pointcloud.empty())
{
cerr<<"point cloud is empty"<<endl;
return;
}
//设置窗口
pangolin::CreateWindowAndBind("Point Cloud Viewer",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,0, 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);
glPointSize(2);//点的尺寸
glBegin(GL_POINTS);//
//
for(auto &p: pointcloud)
{
glColor3f(p[3]/255.0,p[4]/255.0,p[5]/255.0);
glVertex3d(p[0],p[1],p[2]);
}
glEnd();
pangolin::FinishFrame();
usleep(5000);//slepp 5ms
}
return;
}
运行结果如下：
slam十四讲ch5利用RGBD获得点云图像