ICP仅仅迭代一次是为什么呢

heye0601

写了一个交互观察ICP每次迭代结果的代码，可以每次只迭代一次就出结果似乎结果还不错，真心不知道问题出在哪
************************************************************************************************
// PCReg_GAN.cpp : Defines the entry point for the console application.

#include "stdafx.h"
#include <stdio.h>  
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <iostream>
#include <vector>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/registration/icp.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/parse.h>  //pcl控制台解析

typedef pcl::PointXYZRGBNormal PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
bool next_iteration = false;//设置键盘交互函数

PointCloudT :: Ptr cloud_ori  (new PointCloudT);
PointCloudT :: Ptr cloud_trgt (new PointCloudT);
PointCloudT :: Ptr cloud_rslt (new PointCloudT);
pcl::IterativeClosestPoint<PointT, PointT> icp;

void keyboardEvent(const pcl::visualization::KeyboardEvent &event,void *nothing)
{
    if(event.getKeySym() == "space" && event.keyDown())
        next_iteration = true;
}

void PairWise()
{
    icp.setMaxCorrespondenceDistance(0.1);
    icp.setTransformationEpsilon(1e-10);
    icp.setEuclideanFitnessEpsilon(0.01);
    icp.setMaximumIterations (1);

    icp.setInputCloud(cloud_ori);
    icp.setInputTarget (cloud_trgt);
    icp.align(*cloud_rslt);
}
int ViewICP()
{   
    int iterations = 0;
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer("ICP Demo"));  //定义窗口共享指针
    int v1 ; //定义两个窗口v1，v2，窗口v1用来显示初始位置，v2用以显示配准过程
    int v2 ;
    viewer->createViewPort(0.0,0.0,0.5,1.0,v1);  //四个窗口参数分别对应x_min,y_min,x_max.y_max.
    viewer->createViewPort(0.5,0.0,1.0,1.0,v2);

    pcl::visualization::PointCloudColorHandlerCustom<PointT> sources_cloud_color(cloud_ori,255,255,255); //设置源点云的颜色为白色
    viewer->addPointCloud<PointT>(cloud_ori, sources_cloud_color,"sources_cloud_v1",v1);
    viewer->addPointCloud<PointT>(cloud_ori, sources_cloud_color,"sources_cloud_v2",v2);

    pcl::visualization::PointCloudColorHandlerCustom<PointT> target_cloud_color (cloud_trgt,0,250,0);  //目标点云为绿色
    viewer->addPointCloud<PointT>(cloud_trgt,target_cloud_color,"target_cloud_v2",v2); //将点云添加到v2窗口
    viewer->addPointCloud<PointT>(cloud_trgt,target_cloud_color,"target_cloud_v1",v1); //将点云添加到v2窗口

    pcl::visualization::PointCloudColorHandlerCustom<PointT> rslt_cloud_color (cloud_rslt,255,0,0);  //目标点云为红色
    viewer->addPointCloud<PointT>(cloud_rslt,rslt_cloud_color,"rslt_cloud_v2",v2); //将点云添加到v2窗口
    viewer->addText("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, 0.5,0.5,0.5,"icp_info_1", v1);

    viewer->setBackgroundColor(0.0,0.05,0.05,v1); //设置两个窗口的背景色
    viewer->setBackgroundColor(0.05,0.05,0.05,v2);

    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"sources_cloud_v1");  //设置显示点的大小
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"target_cloud_v1");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"aligend_cloud_v2");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"target_cloud_v2");

    viewer->addText("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16,0.5,0.5,0.5, "icp_info_2", v2);

    viewer->registerKeyboardCallback(&keyboardEvent,(void*)NULL);  //设置键盘回调函数

    while(!viewer->wasStopped())
    {
        viewer->spinOnce();  //运行视图
        if (next_iteration)
        {
            icp.align (*cloud_rslt);
            viewer->addPointCloud<PointT>(cloud_rslt,rslt_cloud_color,"rslt_cloud_v2",v2); //将点云添加到v2窗口
            if (icp.hasConverged ())
            {
                cout <<endl<<"has conveged:"<<icp.hasConverged()<<endl<<"score:"<<icp.getFitnessScore()<<endl;
                cout<<"matrix:\n"<<icp.getFinalTransformation()<<endl;
                cout<<"iteration = "<<++iterations<<endl;

                if (iterations == 100)  //设置最大迭代次数
                        return 0;
                viewer->updatePointCloud(cloud_rslt,rslt_cloud_color ,"aligend_cloud_v2");
             }
             else
             {
                PCL_ERROR ("\nICP has not converged.\n");
                return (-1);
             }
         }
        next_iteration = false;  //本次迭代结束，等待触发
    }
    return 1;
}

/**********************************点云显示*******************************/
void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
{
    viewer.setBackgroundColor (1.0, 1.0, 1.0);
    viewer.removeCoordinateSystem();
}   
void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
{
    static unsigned count = 0;
    std::stringstream ss;
    ss << "Once per viewer loop: " << count++;
    viewer.removeShape ("text", 0);
    viewer.addText (ss.str(), 200, 300, "text", 0);
   
    //FIXME: possible race condition here:
}

int _tmain(int argc, _TCHAR* argv[])
{
    printf("//*************************************************\n");
    printf("\              \n\nHello Point Cloud!\n\n");
    printf("***************************************************\n");
   
    pcl::PLYReader reader;

    if(reader.read("deer.ply", *cloud_ori) < 0)//pcl::io::loadPL//YFile<pcl::PointXYZ> ("4_Mimosa.ply", *cloud) == -1)
    {
        PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
        return (-1);
    }

    printf("File Loaded!--Num of Points of cloud_ori: %d\n",cloud_ori->points.size());
   
    if(reader.read("deer1.ply", *cloud_trgt) < 0)//pcl::io::loadPLYFile<pcl::PointXYZ> ("4_Mimosa.ply", *cloud) == -1)
    {
        PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
        return (-1);
    }
    printf("File Loaded!--Num of Points of cloud_trgt: %d\n",cloud_trgt->points.size());
   
    PairWise();
    ViewICP();
}
**********************************************************************************************
白色和绿色是初始点云，红色是配准后点云。