使用Eigen出现了问题

这里写自定义目录标题


做点云拼接时遇到的问题,一直没能解决

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)

不知道如何解决,求大神指点。

评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值