相機與圖像
主要目標:
- 理解針孔相機的模型、內參與徑向畸變係數。
- 理解一個空間點是如何投影到相機成像平面的。
- 掌握Opencv的圖像存儲方式
- 學會基本的攝像頭標定方法
前面幾個博客講了“相機如何表達自身位置的問題”,這個博客的重點是“機器人如何觀測外部世界”。
這部分我在第二個博客中已經寫了相機模型的筆記。
5.2 圖像
5.3 實踐:計算機中的圖像
安裝opencv有很多教程。。。。以往沒少重裝,這裏就略過了。
#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;
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 處的像素
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);
cv::imshow("image", image);
cv::imshow("image_clone", image_clone);
cv::waitKey(0);
// 對於圖像還有很多基本的操作,如剪切,旋轉,縮放等,限於篇幅就不一一介紹了,請參看OpenCV官方文檔查詢每個函數的調用方法.
cv::destroyAllWindows();
return 0;
}
#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;
}
#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 = "./left.png";
string right_file = "./right.png";
// 在pangolin中畫圖,已寫好,無需調整
void showPointCloud(
const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);
int main(int argc, char **argv) {
// 內參
double fx = 459.8806, fy = 460.0172, cx = 323.1796, cy = 265.2708;
// 基線
double b = 0.10;
// 讀取圖像
cv::Mat left = cv::imread(left_file, 0);
cv::Mat right = cv::imread(right_file, 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(left, right, disparity_sgbm);
disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);
// 生成點雲
vector<Vector4d, Eigen::aligned_allocator<Vector4d>> 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 的位置
double x = (u - cx) / fx;
double y = (v - cy) / fy;
double depth = fx * b / (disparity.at<float>(v, u));
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<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;
}
#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<Vector6d>> &pointcloud);
int main(int argc, char **argv) {
vector<cv::Mat> colorImgs, depthImgs; // 彩色圖和深度圖
TrajectoryType poses; // 相機位姿
ifstream fin("./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;
pointcloud.reserve(1000000);
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];
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;
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) {
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;
}
課後題: