NDT 运行自己数据出问题

你猜

大家好 遇到一个问题，我在运行NDT的时候出现一个问题，vector越界，运行官网自带的数据没有问题，我对代码稍作了修改，输入彩色点云，然后复制给XYZ型的，复制过程中没有发生错误，但是当运行到ndt.align (*output_cloud, init_guess)时出现了vector越界。请大家指点。
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

int
        main (int argc, char** argv)
{
        // Loading first scan of room.
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>);
        //pcl::io::loadPCDFile<pcl::PointXYZRGB>(argv[1],*target_cloud1);
        pcl::io::loadPCDFile<pcl::PointXYZRGB>("..\\t521.pcd",*target_cloud1);


        if((*target_cloud1).size()==0)
        {
                PCL_ERROR ("Couldn't read target file  \n");
                return (-1);
        }

        std::cout << "Loaded " << target_cloud1->size () << " data points from target" << std::endl;
        // Loading second scan of room from new perspective.
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>);
        //pcl::io::loadPCDFile<pcl::PointXYZRGB> (argv[2], *input_cloud1);
        pcl::io::loadPCDFile<pcl::PointXYZRGB> ("..\\t21.pcd", *input_cloud1);
        if((*input_cloud1).size()==0)
        {
                PCL_ERROR ("Couldn't read input file  \n");
                return (-1);

        }
        std::cout << "Loaded " << input_cloud1->size () << " data points from input" << std::endl;

        //covert xyzrgb=xyz
        pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
        //初始化大小 很重要 不然容易溢出程序报错
        (*target_cloud).points.resize((*target_cloud1).size());
        for(size_t i=0;i<(*target_cloud1).size();i++)
        {
                (*target_cloud).points[i].x=(*target_cloud1).points[i].x;
                (*target_cloud).points[i].y=(*target_cloud1).points[i].y;
                (*target_cloud).points[i].z=(*target_cloud1).points[i].z;
        }
        std::cout << "Loaded " << target_cloud->size () << " data points from target" << std::endl;


        pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
        (*input_cloud).points.resize((*input_cloud1).size());
        for(size_t i=0;i<(*input_cloud1).size();i++)
        {
                (*input_cloud).points[i].x=(*input_cloud1).points[i].x;
                (*input_cloud).points[i].y=(*input_cloud1).points[i].y;
                (*input_cloud).points[i].z=(*input_cloud1).points[i].z;
        }
        std::cout << "Loaded " << input_cloud->size () << " data points from input" << std::endl;

       
         // Filtering input scan to roughly 10% of original size to increase speed of registration.
         pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
         pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
         approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
         approximate_voxel_filter.setInputCloud (input_cloud);
         approximate_voxel_filter.filter (*filtered_cloud);
         std::cout << "Filtered cloud contains " << filtered_cloud->size ()
                   << " data points from input" << std::endl;

         // Initializing Normal Distributions Transform (NDT).
         pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

         // Setting scale dependent NDT parameters
         // Setting minimum transformation difference for termination condition.
         ndt.setTransformationEpsilon (0.1);
         // Setting maximum step size for More-Thuente line search.
         ndt.setStepSize (0.3);
         //Setting Resolution of NDT grid structure (VoxelGridCovariance).
         ndt.setResolution (3.0);

         // Setting max number of registration iterations.
         ndt.setMaximumIterations (35);

         // Setting point cloud to be aligned.
         ndt.setInputSource (filtered_cloud);
         // Setting point cloud to be aligned to.
         ndt.setInputTarget (target_cloud);

         // Set initial alignment estimate found using robot odometry.
         Eigen::AngleAxisf init_rotation (1, Eigen::Vector3f::UnitZ ());
         Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
         Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

         // Calculating required rigid transform to align the input cloud to the target cloud.
         pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
         (*output_cloud).points.resize((*input_cloud).size());
         ndt.align (*output_cloud, init_guess);

         std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
                   << " score: " << ndt.getFitnessScore () << std::endl;

         // Transforming unfiltered, input cloud using found transform.
         pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());

         // Saving transformed input cloud.
         pcl::io::savePCDFileASCII ("input_transformed.pcd", *output_cloud);

         // Initializing point cloud visualizer
         boost::shared_ptr<pcl::visualization::PCLVisualizer>
         viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
         viewer_final->setBackgroundColor (0, 0, 0);

         // Coloring and visualizing target cloud (red).
         pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
         target_color (target_cloud, 255, 0, 0);
         viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
         viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                         1, "target cloud");

         // Coloring and visualizing transformed input cloud (green).
         pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
         output_color (output_cloud, 0, 255, 0);
         viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
         viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                         1, "output cloud");

         // Starting visualizer
         viewer_final->addCoordinateSystem (1.0);
         viewer_final->initCameraParameters ();

         // Wait until visualizer window is closed.
         while (!viewer_final->wasStopped ())
         {
           viewer_final->spinOnce (100);
           boost::this_thread::sleep (boost::posix_time::microseconds (100000));
         }

        return (0);
}

chuanzhang1990

这个问题也没有解决？

chrisfxz

然后呢，你是怎么解决的

MRQ

chrisfxz 发表于 2014-11-4 20:21
然后呢，你是怎么解决的

解决了，是因为1.7.1版本的Flann的一个文件，nn_index.h里面的函数有bug，升级一下Flann到较高的版本就行了，但是还是需要重新编译pcl点云库才行，点云库用1.7.2版本，我的刚刚弄好