SLAM3-根据相机模型计算视差形成点云

参考文献：https://blog.csdn.net/zkk9527/category_8809309.html 一、相机模型和畸变 畸变是因为相机上的透镜导致成像产生了误差，畸变有两种：径向畸变和切向畸变。 径向畸变是由于透镜形状导致的；分为桶形（四周膨胀）和枕形（四周收缩） 切向畸变是因为透镜和成像平面不能严格平行； 去除畸变：我们一般根据公式来进行纠正；1、找一个三维空间点，把他的（x,y,z）除以z投射到归一化坐标系上；2、利用公式计算畸变之后的点在归一化坐标系的坐标；3、把归一化坐标转换到像素坐标系

相机模型下面有四个坐标系：世界坐标系，相机坐标系（光心坐标系）；归一化相机坐标系；像素坐标系（图片成像平面系）。 世界坐标系转为相机坐标系：把该点左乘T就可以转换到相机坐标系 相机坐标系下的坐标转化到像素坐标系：因为图片尺寸受限，所以要在尺度上除以Z进行归一化（（3000,4000,5000）的坐标点无法成像），认为像素坐标系原点平移了cx,cy，在u轴上缩放了α倍，v轴上缩放了β倍，生成了fx=αf, fy=βf;转换公式为：K是相机内参矩阵，一般已经标定完毕 归一化坐标系：相机坐标系下的(X,Y,Z)三个值除以第三维，把Z归为1的一个坐标系；归一化坐标系转换到像素坐标系：X/Z就是归一化坐标系的坐标

二、双目相机计算深度

根据公式z=fb/d;d=uL-uR来计算深度z b:基线，两个光心的距离；f:焦距；uL，uR同一点在两个像素坐标系下的的x轴坐标

三、使用OpenCV进行图片纠正

#include #include

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("/home/hotfinda/example_slambook/slambook2-master/ch5/imageBasics/ubuntu.png"); //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 来给算法计时 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(y); // 获取第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 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); cv::imshow(“image”, image); cv::imshow(“image_clone”, image_clone); cv::waitKey(0);

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

// 计算去畸变后图像的内容 for (int v = 0; v < rows; v++) { for (int u = 0; u < cols; u++) { // 步骤1、计算点(u,v)对应到诡异坐标系中的坐标 double x = (u - cx) / fx, y = (v - cy) / fy; double r = sqrt(x * x + y * y); //步骤2、归一坐标进行系数整改 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; //步骤3、返回到像素坐标获得纠正值 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(v, u) = image.at((int) v_distorted, (int) u_distorted); } else { image_undistort.at(v, u) = 0; } } }

四、根据左右视图，生成点云

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

using namespace std; using namespace Eigen;

// 文件路径 string left_file = “/home/hotfinda/example_slambook/slambook2-master/ch5/stereo/left.png”; string right_file = “/home/hotfinda/example_slambook/slambook2-master/ch5/stereo/right.png”;

// 在pangolin中画图，已写好，无需调整 void showPointCloud( const vector<Vector4d, Eigen::aligned_allocator> &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); cv::Mat right = cv::imread(right_file, 0); cv::Ptr< cv::StereoSGBM> sgbm = cv::StereoSGBM::create(生成一个智能指针，指向内容为cv::StereoSGBM：StereoSGBM用于生成两幅图像间的差异黑白图，差异越大白色越大 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); // 生成点云vector4d因为点只用黑白色 vector<Vector4d, Eigen::aligned_allocator> pointcloud; // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2 for (int v = 0; v < left.rows; v++) for (int u = 0; u < left.cols; u++) { 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,第四维为颜色，先输入点云点的颜色，在计算点云点的坐标 // 根据双目模型计算 point 的位置 //计算点云点坐标，先生成归一化平面上的点坐标，再乘以深度Z，变为相机坐标系下的点 归一化平面点坐标 double x = (u - cx) / fx; double y = (v - cy) / fy; //计算距离左边摄像头的深度Z double depth = fx * b / (disparity.at(v, u)); //获得XYZ坐标 point[0] = x * depth; point[1] = y * depth; point[2] = depth; pointcloud.push_back(point); } cv::imshow(“disparity”, disparity / 96.0); cv::waitKey(0); // 画出点云 showPointCloud(pointcloud); return 0; }

void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator> &pointcloud) { // 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); // sleep 5 ms } return; }

五、根据左视图以及深度图生成拼接点云

#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;

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

int main(int argc, char **argv) { vector< cv::Mat> colorImgs, depthImgs; // 彩色图和深度图 TrajectoryType poses; // 相机位姿T // ifstream fin("/home/hotfinda/example_slambook/slambook2-master/ch5/rgbd/pose.txt"); if (!fin) { cerr << “请在有pose.txt的目录下运行此程序” << endl; return 1; } // for (int i = 0; i < 5; i++) { 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读取原始图像 // double data[7] = {0}; for (auto &d:data) fin >> d; 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; double cy = 253.5; double fx = 518.0; double fy = 519.0; double depthScale = 1000.0; vector<Vector6d, Eigen::aligned_allocator< Vector6d>> pointcloud建立6d点云，颜色用RGB表示; pointcloud.reserve(1000000);预留空间 // for (int i = 0; i < 5; i++) {使用for循环把每个图片的点都变为三维点云点 cout << "转换图像中: " << i + 1 << endl; cv::Mat color = colorImgs[i]; cv::Mat depth = depthImgs[i]; Sophus::SE3d T = poses[i]; 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]; // 先获取深度值Z，方便后面使用公式获得相机坐标系和世界坐标系下的坐标 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;指定点云前三个元素 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> &pointcloud) { // 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) { glColor3d(p[3] / 255.0, p[4] / 255.0, p[5] / 255.0); glVertex3d(p[0], p[1], p[2]); } glEnd(); pangolin::FinishFrame(); usleep(5000); // sleep 5 ms } return; }