- 首页 > it技术 > >
视觉slam十四讲ch5 joinMap.cpp 代码注释（笔记版）
转自：https://www.cnblogs.com/newneul/p/8407369.html
#include
#include
using namespace std;
#include
#include
#include
#include // for formating strings
#include
#include
#include 【视觉slam十四讲ch5 joinMap.cpp 代码注释（笔记版）】int main( int argc, char** argv )
{
/*彩色图和灰度图各5张，所以用容器来存储*/
vector colorImgs, depthImgs;
// 彩色图和深度图/*vector有两个参数，后面的参数一般是默认的，这里用适合Eigen库的对齐方式来初始化容器，总共有5张图片 所以对应着5个位姿矩阵*/
vector> poses;
// 相机位姿ifstream fin("./pose.txt");
if (fin.bad())//如果没有打开 那么提示错误！
{
cerr<<"请在有pose.txt的目录下运行此程序"<>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;
//cout<<"正在将图像转换为点云..."<返回了指向PointCloud的指针
*Ptr是下面类型 boost::shared_ptr > */
/*pointCloud 是一个智能指针类型的对象 具体可以参考http://blog.csdn.net/worldwindjp/article/details/18843087*/
PointCloud::Ptr pointCloud( new PointCloud );
//pcl::PointCloud::Ptr pointCloud( new pcl::PointCloud );
/*将5张图片 像素坐标转换到相机坐标 之后转换到世界坐标存储到点云格式的变量中 for循环之后用pcl的相关函数将点云转换到pcl能够显示的格式*/
for ( int i=0;
i<5;
i++ )//转换5张图像
{
cout<<"转换图像中: "<(471,537)[0] = 12;
//修改图像上的对应像素位置的值
//color.ptr(471)[537][0] = 12;
//与上面的效果一样//测试像素的输出效果,这里无法通过cout<(471,537)[0] 这种方式来输出第一个通道的值,因为每个通道的像素占了8位而unsigned char
// 表示ascii码 所以输出的时候不是正确的数字，可以通过下面的方式强制转化为int类型（或者用自带的类型转换方式进行显示转换），就可以看到内部的值了
//需要注意的一点是 cout页无法输出char类型的变量的地址，也是需要强制转换成void *类型的指针才能正常输出char类型变量的地址信息。
if(colorImgs[i].channels() == 3) {
std::cout << "测试1结果 " << color.ptr(471)[537] << "正确的结果:"
<< (char) color.at(471, 537)[0] << std::endl;
std::cout << depth.ptr(471)[537] << std::endl;
std::cout << colorImgs[i].at(471, 537) << std::endl;
}
插入部分结束
*//*对图像像素进行坐标转换，将图像的坐标通过内参矩阵K转换为相机坐标系下的坐标，之后通过外参矩阵T 转化为世界坐标系下的坐标*/
for ( int v=0;
v ( v ) 来获取行指针*/
unsigned int d = depth.ptr ( v )[u];
// 深度值16位存储 一般color图像像素中每一个通道是8位/*单通道遍历图像的方式总结:
*注意深度图像的像素占16位 与普通图片每个通道的像素为8位不同
* 1、同样是用上面的双层for循环，遍历图像 用at方式
* for ( int v=0;
v(v,u);
*
*
* 2、使用迭代器进行图像的遍历
* 不是基于for循环了
* cv::MatIterator_ begin,end;
* for( begin =depth.begin(), end = depth.end();
begin != end;
){}
*
* 3、使用指针的方式 如本实验的结果
* *///迭代器的参数是通道数，因为深度图是单通道的，每个像素的值是unsigned short，所以参数是unsigned short
//begin代表像素的开始地方
if ( d==0 ) continue;
// 为0表示没有测量到 然后继续进行for循环那么跳过这个像素继续执行 在后面形成点云时需要设置is_dense为假
Eigen::Vector3d point;
point[2] = double(d)/depthScale;
//对实际尺度的一个缩放
point[0] = (u-cx)*point[2]/fx;
//根据书上5.5式子---86页
point[1] = (v-cy)*point[2]/fy;
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<<"点云的列和行为 ： "（v,u）[0] 表示第一个通道的像素的值
*image.at(v,u)[1]
*image.at(v,u)[2]
* 2、使用迭代器方式 (实际上就是一个指针指向了 cv::Mat矩阵元素)
* cv::MatIterator_begin,end;
* for( begin = image.begin(), end = image.end() ;
begin != end;
)
*(*begin)[0] = ...
*(*begin)[1] = ...
*(*begin)[2] = ...
*
* 3、用指针的方式操作
* for(v)
*for(u)
*image.ptr(v)[u][0] 表示第一个通道
*image.ptr(v)[u][0] 表示第二通道
*.
*.
*.
* */
推荐阅读
