这里写自定义目录标题
做点云拼接时遇到的问题,一直没能解决
slamBase.hpp
# pragma once //保证头文件只被编译一次
#include <iostream>
#pragma pack(push, 16)
// opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// pcl
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/se3.h>
#pragma pack(pop)
using namespace std;
using namespace cv;
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
// camera instrinsic parameters
struct CAMERA_INTRINSIC_PARAMETERS
{
double fx, fy, cx, cy, scale;
};
struct FRAME
{
cv::Mat rgb, depth;
};
PointCloud::Ptr image2PointCloud(Mat rgb, Mat depth, CAMERA_INTRINSIC_PARAMETERS camera);
PointCloud::Ptr pointCloudFusion( PointCloud::Ptr &original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS camera );
Eigen::aligned_allocator<Sophus::SE3>> T, CAMERA_INTRINSIC_PARAMETERS camera );
void readCameraTrajectory(string camTransFile, vector<Eigen::Isometry3d> &poses);
pointCloudFusion.cpp
#include "slamBase.hpp"
int main( int argc, char** argv )
{
CAMERA_INTRINSIC_PARAMETERS cameraParams{517.0,516.0,318.6,255.3,5000.0};
int frameNum = 3;
vector<Eigen::Isometry3d> poses;
// vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;
PointCloud::Ptr fusedCloud(new PointCloud());//新建一个点云
string path = "../data/";
string cameraPosePath = path + "cameraTrajectory.txt";
readCameraTrajectory(cameraPosePath, poses);
for (int idx = 0; idx < frameNum; idx++)
{
string rgbPath = path + "rgb/rgb" + to_string(idx) + ".png";
string depthPath = path + "depth/depth" + to_string(idx) + ".png";
FRAME frm;
frm.rgb = cv::imread(rgbPath);
if (frm.rgb.empty()) {
cerr << "Fail to load rgb image!" << endl;
}
frm.depth = cv::imread(depthPath, -1);
if (frm.depth.empty()) {
cerr << "Fail to load depth image!" << endl;
}
if (idx == 0)
{
fusedCloud = image2PointCloud(frm.rgb, frm.depth, cameraParams);
}
else
{
fusedCloud = pointCloudFusion( fusedCloud, frm, poses[idx], cameraParams );
}
}
pcl::io::savePCDFile( "./fusedCloud1.pcd", *fusedCloud );
return 0;
}
slamBase.cpp
#include "slamBase.hpp"
#include <string>
PointCloud::Ptr image2PointCloud(Mat rgb, Mat depth, CAMERA_INTRINSIC_PARAMETERS camera)
{
PointCloud::Ptr cloud ( new PointCloud );
for (int m = 0; m < depth.rows; m++)
for (int n = 0; n < depth.cols; n++)
{
// 获取深度图中(m,n)处的值
ushort d = depth.ptr<ushort>(m)[n];
// d 可能没有值,若如此,跳过此点
if (d == 0)
continue;
// d 存在值,则向点云增加一个点
PointT p;
// 计算这个点的空间坐标
p.z = double(d) / camera.scale;
p.x = (n - camera.cx) * p.z / camera.fx;
p.y = (m - camera.cy) * p.z / camera.fy;
// 从rgb图像中获取它的颜色
p.b = rgb.ptr<uchar>(m)[n * 3];
p.g = rgb.ptr<uchar>(m)[n * 3 + 1];
p.r = rgb.ptr<uchar>(m)[n * 3 + 2];
// 把p加入到点云中
cloud->points.push_back(p);
}
// 设置并保存点云
cloud->height = 1;
cloud->width = cloud->points.size();
cloud->is_dense = false;
return cloud;
}
PointCloud::Ptr pointCloudFusion( PointCloud::Ptr &original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS camera )
//PointCloud::Ptr pointCloudFusion( PointCloud::Ptr &original, FRAME& newFrame, vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> T, CAMERA_INTRINSIC_PARAMETERS camera )
{
// ---------- 开始你的代码 ------------- -//
// 简单的点云叠加融合
/*
//自己写的可以运行但是结果不对,缺少
original->is_dense=false;
PointCloud::Ptr output (new PointCloud());
pcl::transformPointCloud( *original, *output, T.matrix() );
*output += *original;
return output;*/
//答案
PointCloud::Ptr newCloud(new PointCloud()), transCloud(new PointCloud());
newCloud = image2PointCloud(newFrame.rgb, newFrame.depth, camera);
pcl::transformPointCloud(*newCloud, *transCloud, T.matrix());
*original += *transCloud;
return original;
// ---------- 结束你的代码 ------------- -//
}
void readCameraTrajectory(string camTransFile, vector<Eigen::Isometry3d> &poses)
//void readCameraTrajectory(string camTransFile,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses)
{
ifstream fcamTrans(camTransFile);
if(!fcamTrans.is_open())
{
cerr << "trajectory is empty!" << endl;
return;
}
// ---------- 开始你的代码 ------------- -//
// 参考作业8 绘制轨迹
string line;
double t, tx, ty, tz, qx, qy, qz, qw;
while( getline(fcamTrans,line) )
{
stringstream lineStream(line);
lineStream>>t>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
string a ="-1.#INF";
if (t == atof(a.c_str()))
{
cout << "WARN: INF ERROR" << endl;
continue;
}
Eigen::Quaterniond q = Eigen::Quaterniond(qw,qx,qy,qz).normalized();
Eigen::Isometry3d T(q); //变换矩阵初始化旋转部分,
T.pretranslate( Eigen::Vector3d(tx,ty,tz));//变换矩阵初始化平移向量部分
poses.push_back( T ); //存储变换矩阵到位姿数组
}
// ---------- 结束你的代码 ------------- -//
}
出现报错
pointCloudFusion: /usr/include/eigen3/Eigen/src/Core/DenseStorage.h:128:
Eigen::internal::plain_array<T, Size, MatrixOrArrayOptions, 32>::plain_array()
[with T = double; int Size = 16; int MatrixOrArrayOptions = 0]: Assertion `(reinterpret_cast<size_t>(eigen_unaligned_array_assert_workaround_gcc47(array)) & (31))
== 0 && "this assertion is explained here: " "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" " **** READ THIS WEB PAGE !!! ****"' failed.
Process finished with exit code 134 (interrupted by signal 6: SIGABRT)
不知道如何解决,求大神指点。