4. Camera Model & Camera Distortion Practice
15 Jun 2020 | Visual SLAM
1. Install Opencv
sudo apt-get install build-essential libgtk2.0-dev libvtk5-dev libjpeg-dev libtiff4-dev libjasper-dev libopenexr-dev libtbb-dev
2. building it in Cmake
project(imageBasics)
set(CMAKE_CXX_STANDARD 11)
find_package( OpenCV REQUIRED)
include_directories( ${OpenCV_INCLUDE_DIRS})
add_executable(imageBasics imageBasics.cpp)
# 链接OpenCV库
target_link_libraries(imageBasics ${OpenCV_LIBS})
add_executable(undistortImage undistortImage.cpp)
# 链接OpenCV库
target_link_libraries(undistortImage ${OpenCV_LIBS})
mkdir build
cd build
cmake ..
make
3. Imagebasic.cpp
#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;
}
// 遍历(traversal)图像, 请注意以下遍历方式亦可使用于随机像素访问
// 使用 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);
out << "遍历图像用时：" << 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;
}
build/image_basics ubuntu.png
4. undistortImage.cpp
#include <opencv2/opencv.hpp>
#include <string>
using namespace std;
string image_file = "/home/chan/Desktop/slambook2/ch5/imageBasics/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); // 图像是灰度图，CV_8UC1
int rows = image.rows, cols = image.cols;
cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1); // 去畸(ji)变以后的图
// 计算去畸变后图像的内容
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;
// 赋值(fuzhi)(value assignment) (最近邻插值)
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;
}
5. Stereo Vision.cpp(Point Cloud)
#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) {
// 内参(camera 내부 파라미터)
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// 基线(Baseline)
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(0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32); // 神奇的参数
cv::Mat disparity_sgbm, disparity; // 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;
}
- CMAKE
find_package(Pangolin REQUIRED)
add_executable(stereoVision stereoVision.cpp)
target_link_libraries(stereoVision ${OpenCV_LIBS} ${Pangolin_LIBRARIES})
mkdir build
cd build
cmake ..
make
./stereoVision
6. Pointcloud Using Depth Camera
- 궁국적인 목적은 camera pose를 이용하여 카메라에서 적출된 observation point의 pose xyz를 구하는 것
#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/format.hpp> // for formating strings
#include <sophus/se3.h>
#include <pangolin/pangolin.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
using namespace std;
typedef vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> TrajectoryType; // pointcloud pose
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"); //frame 별로 camerap pose가 있음
if (!fin) {
cerr << "请在有pose.txt的目录下运行此程序" << endl;
return 1;
}
for (int i = 0; i < 5; i++) { // 5개 프레임
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,0,0,0,0,0,0}; // double data[7] = {0}; same as above
for (auto &d:data)
{
fin >> d; // pose 데이터 넣기 각 프레임의
}
Sophus::SE3 pose(Eigen::Quaterniond(data[6], data[3], data[4], data[5]), Eigen::Vector3d(data[0], data[1], data[2])) // Speical eclidea 을 써서 리군 변환 리대수 변환으로 쉽게 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; //포인트 클라우드
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::SE3 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; // Depth로 통하여 현재 표현되는 값들을 모두 pointcloud로 변환하여 표현(octomap처럼, 아직까지는 특정점에대한 포인트 클라우드가 아니다.)
point[2] = double(d) / depthScale;
point[0] = (u - cx) * point[2] / fx;
point[1] = (v - cy) * point[2] / fy;
Eigen::Vector3d pointWorld = T * point; // point를 월드에 reprent
Vector6d p;
p.head<3>() = pointWorld; // giving pose
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;
}
- CMAKE
cmake_minimum_required(VERSION 2.8)
project(jointmap)
set(CMAKE_CXX_STANDARD 11)
# opencv
find_package( OpenCV REQUIRED)
include_directories( ${OpenCV_INCLUDE_DIRS})
# Sophus
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
# Pangolin
find_package(Pangolin REQUIRED)
# eigen
include_directories("/usr/include/eigen3/")
# pcl
find_package( PCL REQUIRED COMPONENT common io)
include_directories(${PCL_INCLUDE_DIRS})
add_definitions(${PCL_DEFINITIONS})
list(REMOVE_ITEM PCL_LIBRARIES “vtkproj4”)
#execute
add_executable(joinMap joinMap.cpp)
target_link_libraries(joinMap ${OpenCV_LIBS} ${Pangolin_LIBRARIES} ${PCL_LIBRARIES} )
./jointmap
7. Pointcloud Using Depth Camera 2 with pointcloud library
#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <boost/format.hpp> // for formating strings
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
int main( int argc, char** argv )
{
/*彩色图和灰度图各5张，所以用容器来存储*/
vector<cv::Mat> colorImgs, depthImgs; // 彩色图和深度图
/*vector有两个参数，后面的参数一般是默认的，这里用适合Eigen库的对齐方式来初始化容器，总共有5张图片 所以对应着5个位姿矩阵*/
vector<Eigen::Isometry3d,Eigen::aligned_allocator<Eigen::Isometry3d>> poses; // 相机位姿
ifstream fin("../pose.txt"); // pose route
if (fin.bad())//如果没有打开 那么提示错误！
{
cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
return 1;
}
/*循环读取图像*/
for ( int i=0; i<5; i++ )
{
/*用boost中的format格式类，来循环读取图片，否则单张读取图片就会有问题*/
/* 当在命令行中执行的时候这里必须要为../ 在当前ide中执行的时候要修改为*/
boost::format fmt( "../%s/%d.%s" ); //图像文件格式 "../%s/%d.%s" ../ 表示可执行文件在build中,图像在上一个目录,所以用../
/* 这里的%对应. color对应%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读取原始图像
/*基于范围的for循环，表示从data数组的第一项开始 循环遍历 auto表示自动根据后面的元素 获得符合要求的类型*/
double data[7] = {0};
for ( auto& d:data )//auto自动类型转换
{
fin>>d;//文件流类型的变量fin将pose.txt中的数据给了d数组
}
Eigen::Quaterniond q( data[6], data[3], data[4], data[5] ); //四元数 data[6]是实数 但是coeffis输出的是先虚数后实数
Eigen::Isometry3d T(q); //变换矩阵初始化旋转部分，
T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));//变换矩阵初始化平移向量部分
poses.push_back( T ); //存储变换矩阵到位姿数组中
}
// 计算点云并拼接
// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;
double depthScale = 1000.0;//
// 定义点云使用的格式：这里用的是XYZRGB
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
// 新建一个点云//PointCoud::Ptr是一个智能指针类 通过构造函数初始化指针指向的申请的空间
/*Ptr是一个智能指针，返回一个PointCloud<PointT> 其中PointT是pcl::PointXYZRGB类型。它重载了-> 返回了指向PointCloud<PointT>的指针
*Ptr是下面类型 boost::shared_ptr<PointCloud<PointT> > */
/*pointCloud 是一个智能指针类型的对象 具体可以参考http://blog.csdn.net/worldwindjp/article/details/18843087*/
PointCloud::Ptr pointCloud( new PointCloud );
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud( new pcl::PointCloud<pcl::PointXYZRGB> );
/*将5张图片 像素坐标转换到相机坐标 之后转换到世界坐标存储到点云格式的变量中 for循环之后用pcl的相关函数将点云转换到pcl能够显示的格式*/
for ( int i=0; i<5; i++ )//转换5张图像
{
cout<<"转换图像中: "<<i+1<<endl;
cv::Mat color = colorImgs[i];
cv::Mat depth = depthImgs[i];
Eigen::Isometry3d T = poses[i];
/* 插入部分
* //color.at<cv::Vec3b>(471,537)[0] = 12;//修改图像上的对应像素位置的值
//color.ptr<cv::Vec3b>(471)[537][0] = 12;//与上面的效果一样
//测试像素的输出效果,这里无法通过cout<<color.at<cv::Vec3b>(471,537)[0] 这种方式来输出第一个通道的值,因为每个通道的像素占了8位而unsigned char
// 表示ascii码 所以输出的时候不是正确的数字，可以通过下面的方式强制转化为int类型（或者用自带的类型转换方式进行显示转换），就可以看到内部的值了
//需要注意的一点是 cout页无法输出char类型的变量的地址，也是需要强制转换成void *类型的指针才能正常输出char类型变量的地址信息。
if(colorImgs[i].channels() == 3) {
std::cout << "测试1结果 " << color.ptr<cv::Vec3b>(471)[537] << "正确的结果: "
<< (char) color.at<cv::Vec3b>(471, 537)[0] << std::endl;
std::cout << depth.ptr<unsigned short>(471)[537] << std::endl;
std::cout << colorImgs[i].at<cv::Vec3b>(471, 537) << std::endl;
}
插入部分结束
*/
/*对图像像素进行坐标转换，将图像的坐标通过内参矩阵K转换为相机坐标系下的坐标，之后通过外参矩阵T 转化为世界坐标系下的坐标*/
for ( int v=0; v<color.rows; v++ )
{
/*通过用Mat中的ptr模板函数 返回一个unsigned short类型的指针。v表示行 根据内部计算返回data头指针 + 偏移量来计算v行的头指针
* 图像为单通道的 depth.ptr<unsigned short> ( v ) 来获取行指针*/
unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值16位存储 一般color图像像素中每一个通道是8位
/* 单通道遍历图像的方式总结:
* 注意深度图像的像素占16位 与普通图片每个通道的像素为8位不同
* 1、同样是用上面的双层for循环，遍历图像 用at方式
* for ( int v=0; v<color.rows; v++ )
* for ( int u=0; u<color.cols; u++ )
* unsigned int d = depth.at<unsigned short >(v,u);
*
*
* 2、使用迭代器进行图像的遍历
* 不是基于for循环了
* cv::MatIterator_<unsigned short > begin,end;
* for( begin =depth.begin<unsigned short >(), end = depth.end<unsigned short >(); begin != end; ){}
*
* 3、使用指针的方式 如本实验的结果
* * /
//迭代器的参数是通道数，因为深度图是单通道的，每个像素的值是unsigned short，所以参数是unsigned short
//begin代表像素的开始地方
if ( d==0 ) continue; // 为0表示没有测量到 然后继续进行for循环那么跳过这个像素继续执行 在后面形成点云时需要设置is_dense为假
Eigen::Vector3d point;
// pixel coordinate (u,v,d) ->camera coordinate p_c(x,y,z)
point[2] = double(d)/depthScale; //对实际尺度的一个缩放 z
point[0] = (u-cx)* point[2]/fx; //根据书上5.5式子---86页 x
point[1] = (v-cy)* point[2]/fy; //y
Eigen::Vector3d pointWorld = T*point; //将相机坐标系转换为世界坐标系
PointT p ;
p.x = pointWorld[0]; //将世界坐标系下的坐标用pcl专门的点云格式存储起来
p.y = pointWorld[1];
p.z = pointWorld[2];
/* color.step 虽然是一个类，但是它内部有一个转换操作符 operator size_t() const;
* 此时的color.size编译器就会把它当做size_t类型的变量,这个值的大小是1920 这个是随着图像的读入MAT类中会有自动转换然后存储的buf[]中 * /
p.b = color.data[ v* color.step+u*color.channels() ];
p.g = color.data[ v*color.step+u*color.channels()+ 1 ];
p.r = color.data[ v*color.step+u*color.channels()+ 2 ];
/* -> 是智能指针重载的 然后返回的类型就是输入的类型 可以看上面Ptr的解释说明 * /
pointCloud->points.push_back( p );//存储格式好的点
}
}
std::cout<<"点云的列和行为 ： "<<pointCloud->width<<" "<<pointCloud->height<<std::endl;
//这里有可能深度图中某些像素没有深度信息，那么就是包含无效的像素，所以先置为假，但是如果设置成true的话 也没有看出来有什么不一样的地方
pointCloud->is_dense = false;
std::cout<<"点云的列和行为 ： "<<pointCloud->width<<" "<<pointCloud->height<<std::endl;
cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
pcl::io::savePCDFileBinary("map.pcd", *pointCloud );//获取pointCloud指向的对象 这个就当做获取普通指针指向的对象来理解，这个对象是在定义的时候new出来的一段内存空间。
return 0;
}
/* 备注： 3通道的图像的遍历方式总结
* 对于单通道来说 每个像素占8位 3通道则是每个矩阵元素是一个Vec3b 即一个三维的向量 向量内部元素为8位数的unsigned char类型
* 1、使用at遍历图像
* for(v)row
* for(u)col
* image.at<Vec3b>（v,u）[0] 表示第一个通道的像素的值
* image.at<Vec3b>(v,u)[1]
* image.at<Vec3b>(v,u)[2]
* 2、使用迭代器方式 (实际上就是一个指针指向了 cv::Mat矩阵元素)
* cv::MatIterator_<Vec3b>begin,end;
* for( begin = image.begin<Vec3b>(), end = image.end<Vec3b>() ; begin != end; )
* (*begin)[0] = ...
* (*begin)[1] = ...
* (*begin)[2] = ...
*
* 3、用指针的方式操作
* for(v)
* for(u)
* image.ptr<Vec3b>(v)[u][0] 表示第一个通道
* image.ptr<Vec3b>(v)[u][0] 表示第二通道
* .
* .
* .
* */
./jointmap
pcl_viewer map.pcd
8. Monocular Camera Calibration
https://blog.csdn.net//heyijia0327/article/details/43538695
9. RGB-D Camera Calibration
https://blog.csdn.net//Siyuada/article/details/78981555
Reference
SLAM KR
1. Install Opencv
sudo apt-get install build-essential libgtk2.0-dev libvtk5-dev libjpeg-dev libtiff4-dev libjasper-dev libopenexr-dev libtbb-dev
2. building it in Cmake
project(imageBasics)
set(CMAKE_CXX_STANDARD 11)
find_package( OpenCV REQUIRED)
include_directories( ${OpenCV_INCLUDE_DIRS})
add_executable(imageBasics imageBasics.cpp)
# 链接OpenCV库
target_link_libraries(imageBasics ${OpenCV_LIBS})
add_executable(undistortImage undistortImage.cpp)
# 链接OpenCV库
target_link_libraries(undistortImage ${OpenCV_LIBS})
mkdir build
cd build
cmake ..
make
3. Imagebasic.cpp
#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;
}
// 遍历(traversal)图像, 请注意以下遍历方式亦可使用于随机像素访问
// 使用 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);
out << "遍历图像用时：" << 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;
}
build/image_basics ubuntu.png
4. undistortImage.cpp
#include <opencv2/opencv.hpp>
#include <string>
using namespace std;
string image_file = "/home/chan/Desktop/slambook2/ch5/imageBasics/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); // 图像是灰度图，CV_8UC1
int rows = image.rows, cols = image.cols;
cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1); // 去畸(ji)变以后的图
// 计算去畸变后图像的内容
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;
// 赋值(fuzhi)(value assignment) (最近邻插值)
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;
}
5. Stereo Vision.cpp(Point Cloud)
#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) {
// 内参(camera 내부 파라미터)
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// 基线(Baseline)
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(0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32); // 神奇的参数
cv::Mat disparity_sgbm, disparity; // 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;
}
- CMAKE
find_package(Pangolin REQUIRED)
add_executable(stereoVision stereoVision.cpp)
target_link_libraries(stereoVision ${OpenCV_LIBS} ${Pangolin_LIBRARIES})
mkdir build
cd build
cmake ..
make
./stereoVision
6. Pointcloud Using Depth Camera
- 궁국적인 목적은 camera pose를 이용하여 카메라에서 적출된 observation point의 pose xyz를 구하는 것
#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/format.hpp> // for formating strings
#include <sophus/se3.h>
#include <pangolin/pangolin.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
using namespace std;
typedef vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> TrajectoryType; // pointcloud pose
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"); //frame 별로 camerap pose가 있음
if (!fin) {
cerr << "请在有pose.txt的目录下运行此程序" << endl;
return 1;
}
for (int i = 0; i < 5; i++) { // 5개 프레임
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,0,0,0,0,0,0}; // double data[7] = {0}; same as above
for (auto &d:data)
{
fin >> d; // pose 데이터 넣기 각 프레임의
}
Sophus::SE3 pose(Eigen::Quaterniond(data[6], data[3], data[4], data[5]), Eigen::Vector3d(data[0], data[1], data[2])) // Speical eclidea 을 써서 리군 변환 리대수 변환으로 쉽게 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; //포인트 클라우드
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::SE3 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; // Depth로 통하여 현재 표현되는 값들을 모두 pointcloud로 변환하여 표현(octomap처럼, 아직까지는 특정점에대한 포인트 클라우드가 아니다.)
point[2] = double(d) / depthScale;
point[0] = (u - cx) * point[2] / fx;
point[1] = (v - cy) * point[2] / fy;
Eigen::Vector3d pointWorld = T * point; // point를 월드에 reprent
Vector6d p;
p.head<3>() = pointWorld; // giving pose
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;
}
- CMAKE
cmake_minimum_required(VERSION 2.8)
project(jointmap)
set(CMAKE_CXX_STANDARD 11)
# opencv
find_package( OpenCV REQUIRED)
include_directories( ${OpenCV_INCLUDE_DIRS})
# Sophus
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
# Pangolin
find_package(Pangolin REQUIRED)
# eigen
include_directories("/usr/include/eigen3/")
# pcl
find_package( PCL REQUIRED COMPONENT common io)
include_directories(${PCL_INCLUDE_DIRS})
add_definitions(${PCL_DEFINITIONS})
list(REMOVE_ITEM PCL_LIBRARIES “vtkproj4”)
#execute
add_executable(joinMap joinMap.cpp)
target_link_libraries(joinMap ${OpenCV_LIBS} ${Pangolin_LIBRARIES} ${PCL_LIBRARIES} )
./jointmap
7. Pointcloud Using Depth Camera 2 with pointcloud library
#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <boost/format.hpp> // for formating strings
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
int main( int argc, char** argv )
{
/*彩色图和灰度图各5张，所以用容器来存储*/
vector<cv::Mat> colorImgs, depthImgs; // 彩色图和深度图
/*vector有两个参数，后面的参数一般是默认的，这里用适合Eigen库的对齐方式来初始化容器，总共有5张图片 所以对应着5个位姿矩阵*/
vector<Eigen::Isometry3d,Eigen::aligned_allocator<Eigen::Isometry3d>> poses; // 相机位姿
ifstream fin("../pose.txt"); // pose route
if (fin.bad())//如果没有打开 那么提示错误！
{
cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
return 1;
}
/*循环读取图像*/
for ( int i=0; i<5; i++ )
{
/*用boost中的format格式类，来循环读取图片，否则单张读取图片就会有问题*/
/* 当在命令行中执行的时候这里必须要为../ 在当前ide中执行的时候要修改为*/
boost::format fmt( "../%s/%d.%s" ); //图像文件格式 "../%s/%d.%s" ../ 表示可执行文件在build中,图像在上一个目录,所以用../
/* 这里的%对应. color对应%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读取原始图像
/*基于范围的for循环，表示从data数组的第一项开始 循环遍历 auto表示自动根据后面的元素 获得符合要求的类型*/
double data[7] = {0};
for ( auto& d:data )//auto自动类型转换
{
fin>>d;//文件流类型的变量fin将pose.txt中的数据给了d数组
}
Eigen::Quaterniond q( data[6], data[3], data[4], data[5] ); //四元数 data[6]是实数 但是coeffis输出的是先虚数后实数
Eigen::Isometry3d T(q); //变换矩阵初始化旋转部分，
T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));//变换矩阵初始化平移向量部分
poses.push_back( T ); //存储变换矩阵到位姿数组中
}
// 计算点云并拼接
// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;
double depthScale = 1000.0;//
// 定义点云使用的格式：这里用的是XYZRGB
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
// 新建一个点云//PointCoud::Ptr是一个智能指针类 通过构造函数初始化指针指向的申请的空间
/*Ptr是一个智能指针，返回一个PointCloud<PointT> 其中PointT是pcl::PointXYZRGB类型。它重载了-> 返回了指向PointCloud<PointT>的指针
*Ptr是下面类型 boost::shared_ptr<PointCloud<PointT> > */
/*pointCloud 是一个智能指针类型的对象 具体可以参考http://blog.csdn.net/worldwindjp/article/details/18843087*/
PointCloud::Ptr pointCloud( new PointCloud );
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud( new pcl::PointCloud<pcl::PointXYZRGB> );
/*将5张图片 像素坐标转换到相机坐标 之后转换到世界坐标存储到点云格式的变量中 for循环之后用pcl的相关函数将点云转换到pcl能够显示的格式*/
for ( int i=0; i<5; i++ )//转换5张图像
{
cout<<"转换图像中: "<<i+1<<endl;
cv::Mat color = colorImgs[i];
cv::Mat depth = depthImgs[i];
Eigen::Isometry3d T = poses[i];
/* 插入部分
* //color.at<cv::Vec3b>(471,537)[0] = 12;//修改图像上的对应像素位置的值
//color.ptr<cv::Vec3b>(471)[537][0] = 12;//与上面的效果一样
//测试像素的输出效果,这里无法通过cout<<color.at<cv::Vec3b>(471,537)[0] 这种方式来输出第一个通道的值,因为每个通道的像素占了8位而unsigned char
// 表示ascii码 所以输出的时候不是正确的数字，可以通过下面的方式强制转化为int类型（或者用自带的类型转换方式进行显示转换），就可以看到内部的值了
//需要注意的一点是 cout页无法输出char类型的变量的地址，也是需要强制转换成void *类型的指针才能正常输出char类型变量的地址信息。
if(colorImgs[i].channels() == 3) {
std::cout << "测试1结果 " << color.ptr<cv::Vec3b>(471)[537] << "正确的结果: "
<< (char) color.at<cv::Vec3b>(471, 537)[0] << std::endl;
std::cout << depth.ptr<unsigned short>(471)[537] << std::endl;
std::cout << colorImgs[i].at<cv::Vec3b>(471, 537) << std::endl;
}
插入部分结束
*/
/*对图像像素进行坐标转换，将图像的坐标通过内参矩阵K转换为相机坐标系下的坐标，之后通过外参矩阵T 转化为世界坐标系下的坐标*/
for ( int v=0; v<color.rows; v++ )
{
/*通过用Mat中的ptr模板函数 返回一个unsigned short类型的指针。v表示行 根据内部计算返回data头指针 + 偏移量来计算v行的头指针
* 图像为单通道的 depth.ptr<unsigned short> ( v ) 来获取行指针*/
unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值16位存储 一般color图像像素中每一个通道是8位
/* 单通道遍历图像的方式总结:
* 注意深度图像的像素占16位 与普通图片每个通道的像素为8位不同
* 1、同样是用上面的双层for循环，遍历图像 用at方式
* for ( int v=0; v<color.rows; v++ )
* for ( int u=0; u<color.cols; u++ )
* unsigned int d = depth.at<unsigned short >(v,u);
*
*
* 2、使用迭代器进行图像的遍历
* 不是基于for循环了
* cv::MatIterator_<unsigned short > begin,end;
* for( begin =depth.begin<unsigned short >(), end = depth.end<unsigned short >(); begin != end; ){}
*
* 3、使用指针的方式 如本实验的结果
* * /
//迭代器的参数是通道数，因为深度图是单通道的，每个像素的值是unsigned short，所以参数是unsigned short
//begin代表像素的开始地方
if ( d==0 ) continue; // 为0表示没有测量到 然后继续进行for循环那么跳过这个像素继续执行 在后面形成点云时需要设置is_dense为假
Eigen::Vector3d point;
// pixel coordinate (u,v,d) ->camera coordinate p_c(x,y,z)
point[2] = double(d)/depthScale; //对实际尺度的一个缩放 z
point[0] = (u-cx)* point[2]/fx; //根据书上5.5式子---86页 x
point[1] = (v-cy)* point[2]/fy; //y
Eigen::Vector3d pointWorld = T*point; //将相机坐标系转换为世界坐标系
PointT p ;
p.x = pointWorld[0]; //将世界坐标系下的坐标用pcl专门的点云格式存储起来
p.y = pointWorld[1];
p.z = pointWorld[2];
/* color.step 虽然是一个类，但是它内部有一个转换操作符 operator size_t() const;
* 此时的color.size编译器就会把它当做size_t类型的变量,这个值的大小是1920 这个是随着图像的读入MAT类中会有自动转换然后存储的buf[]中 * /
p.b = color.data[ v* color.step+u*color.channels() ];
p.g = color.data[ v*color.step+u*color.channels()+ 1 ];
p.r = color.data[ v*color.step+u*color.channels()+ 2 ];
/* -> 是智能指针重载的 然后返回的类型就是输入的类型 可以看上面Ptr的解释说明 * /
pointCloud->points.push_back( p );//存储格式好的点
}
}
std::cout<<"点云的列和行为 ： "<<pointCloud->width<<" "<<pointCloud->height<<std::endl;
//这里有可能深度图中某些像素没有深度信息，那么就是包含无效的像素，所以先置为假，但是如果设置成true的话 也没有看出来有什么不一样的地方
pointCloud->is_dense = false;
std::cout<<"点云的列和行为 ： "<<pointCloud->width<<" "<<pointCloud->height<<std::endl;
cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
pcl::io::savePCDFileBinary("map.pcd", *pointCloud );//获取pointCloud指向的对象 这个就当做获取普通指针指向的对象来理解，这个对象是在定义的时候new出来的一段内存空间。
return 0;
}
/* 备注： 3通道的图像的遍历方式总结
* 对于单通道来说 每个像素占8位 3通道则是每个矩阵元素是一个Vec3b 即一个三维的向量 向量内部元素为8位数的unsigned char类型
* 1、使用at遍历图像
* for(v)row
* for(u)col
* image.at<Vec3b>（v,u）[0] 表示第一个通道的像素的值
* image.at<Vec3b>(v,u)[1]
* image.at<Vec3b>(v,u)[2]
* 2、使用迭代器方式 (实际上就是一个指针指向了 cv::Mat矩阵元素)
* cv::MatIterator_<Vec3b>begin,end;
* for( begin = image.begin<Vec3b>(), end = image.end<Vec3b>() ; begin != end; )
* (*begin)[0] = ...
* (*begin)[1] = ...
* (*begin)[2] = ...
*
* 3、用指针的方式操作
* for(v)
* for(u)
* image.ptr<Vec3b>(v)[u][0] 表示第一个通道
* image.ptr<Vec3b>(v)[u][0] 表示第二通道
* .
* .
* .
* */
./jointmap
pcl_viewer map.pcd
8. Monocular Camera Calibration
https://blog.csdn.net//heyijia0327/article/details/43538695
9. RGB-D Camera Calibration
https://blog.csdn.net//Siyuada/article/details/78981555
Reference
SLAM KR
Comments