代码如下:
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>
#include <pcl/io/pcd_io.h>
using namespace std;
using namespace Eigen;
// 文件路径,如果不对,请调整
string left_file = "/home/lgl/project/第四讲习题/作业题/双目视差的使用/left.png";
string right_file = "/home/lgl/project/第四讲习题/作业题/双目视差的使用/right.png";
string disparity_file = "/home/lgl/project/第四讲习题/作业题/双目视差的使用/disparity.png";
// 在panglin中画图,已写好,无需调整
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);
cv::Mat right = cv::imread(right_file, 0);
cv::Mat disparity = cv::imread(disparity_file, 0); // disparty 为CV_8U,单位为像素
if(left.data)
cout<<"loaded left.png"<<endl;
cv::imshow("left",left);
cv::imshow("right",right);
cv::imshow("disparity",disparity);
cv::waitKey(0);
cv::destroyAllWindows();
// 生成点云
vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;
// TODO 根据双目模型计算点云
// 如果你的机器慢,请把后面的v++和u++改成v+=2, u+=2
for (int v = 0; v < left.rows; v++)
for (int u = 0; u < left.cols; u++) {
Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色
// start your code here (~6 lines)
// 根据双目模型计算 point 的位置
unsigned int d=disparity.ptr<unsigned short>(v)[u];
if(d==0)
{
cout<<"d==0"<<endl;
continue;
}
point[2]=(fx*b*1000)/d;
point[1]=(v-cy)*point[2]/fy;
point[0]=(u-cx)*point[2]/fx;
cout<<"point = [ "<<point[0]<<" "<<point[1]<<" "<<point[2]<<" "<<point[3]<<" ]"<<endl;
pointcloud.push_back(point);
// end your code here
}
cout<<"点云共有 : "<<pointcloud.size()<<"个点"<<endl;
// 画出点云
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::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 ;
}