第一章:SLAM 读取照片
imageBasics.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;
}
// 遍历图像, 请注意以下遍历方式亦可使用于随机像素访问
// 使用 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;
}
CMakeLists.txt
cmake_minimum_required( VERSION 2.8 )
project( imageBasics )
# 添加c++ 11标准支持
set( CMAKE_CXX_FLAGS "-std=c++11" )
# 寻找OpenCV库
find_package( OpenCV 3 REQUIRED )
# 添加头文件
include_directories( ${OpenCV_INCLUDE_DIRS} )
add_executable( imageBasics imageBasics.cpp )
# 链接OpenCV库
target_link_libraries( imageBasics ${OpenCV_LIBS} )
创建build文件进行编译
mkdir build
cd build
cmake ..
make
cd ..
build形成文件imageBasics
程序命令显示
./build/imageBasics ubuntu.png
第二章:SLAM 3D点云程序
color:文件为彩色照片
depth:为深度照片
pose.txt:为xyz和四元素
-0.228993 0.00645704 0.0287837 -0.0004327 -0.113131 -0.0326832 0.993042
-0.50237 -0.0661803 0.322012 -0.00152174 -0.32441 -0.0783827 0.942662
-0.970912 -0.185889 0.872353 -0.00662576 -0.278681 -0.0736078 0.957536
-1.41952 -0.279885 1.43657 -0.00926933 -0.222761 -0.0567118 0.973178
-1.55819 -0.301094 1.6215 -0.02707 -0.250946 -0.0412848 0.966741
joinMap.cpp
#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 )
{
vector<cv::Mat> colorImgs, depthImgs; // 彩色图和深度图
vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> 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};//pose.txt有7个数
for ( auto& d:data )
fin>>d;
Eigen::Quaterniond q( data[6], data[3], data[4], data[5] ); //后4个为4元素,3个虚数一个实数,三维向量来表示旋转的坐标轴和一个旋转角度。
Eigen::Isometry3d T(q);
T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));//前3位为坐标轴x,y,z,平移
poses.push_back( T );//欧式变换
}
// 计算点云并拼接
// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;
double depthScale = 1000.0;
cout<<"正在将图像转换为点云..."<<endl;
// 定义点云使用的格式:这里用的是XYZRGB
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
// 新建一个点云
PointCloud::Ptr pointCloud( new PointCloud );
for ( int i=0; i<5; i++ )//每个进行处理
{
cout<<"转换图像中: "<<i+1<<endl;
cv::Mat color = colorImgs[i];
cv::Mat depth = depthImgs[i];
Eigen::Isometry3d 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;//知道uv计算xy
point[1] = (v-cy)*point[2]/fy;
Eigen::Vector3d pointWorld = T*point;//乘pointworld世界坐标系的坐标
//处理点云库
PointT p ;
p.x = pointWorld[0];
p.y = pointWorld[1];
p.z = pointWorld[2];
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 ];
pointCloud->points.push_back( p );
}
}
pointCloud->is_dense = false;
//存储到map.pcd中
cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
return 0;
}
CMakeLists.txt
cmake_minimum_required( VERSION 2.8 )
project( joinMap )
set( CMAKE_BUILD_TYPE Release )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
# opencv
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )
# eigen
include_directories( "/usr/include/eigen3/" )
# pcl
find_package( PCL REQUIRED COMPONENT common io )
include_directories( ${PCL_INCLUDE_DIRS} )
add_definitions( ${PCL_DEFINITIONS} )
add_executable( joinMap joinMap.cpp )
target_link_libraries( joinMap ${OpenCV_LIBS} ${PCL_LIBRARIES} )
创建build文件进行编译
mkdir build
cd build
cmake ..
make
cd ..
采集点形成文件map.pcd
build/joinMap
上面代码进行点存储到map.pcd中
绘制3D点云图
pcl_viewer map.pcd