备份

 Result res;
    constexpr unsigned int Dimension = 3;
    using PointSetType = itk::PointSet< double, Dimension >;
    PointSetType::Pointer fixedPointSet  = PointSetType::New();
    PointSetType::Pointer movingPointSet = PointSetType::New();
    
    using PointsContainer = PointSetType::PointsContainer;
    PointsContainer::Pointer fixedPointContainer  = PointsContainer::New();
    PointsContainer::Pointer movingPointContainer = PointsContainer::New();
    
    for(int pointId = 0; pointId<fixed_points.size(); pointId++)
    {
        fixedPointContainer->InsertElement( pointId, fixed_points[pointId] );
    }
    
    for(int pointId = 0; pointId<moving_points.size(); pointId++)//at least six
    {
        movingPointContainer->InsertElement( pointId, moving_points[pointId] );
    }
    
    movingPointSet->SetPoints( movingPointContainer );
    std::cout <<"Number of moving Points = "<< movingPointSet->GetNumberOfPoints() << std::endl;
    
    fixedPointSet->SetPoints( fixedPointContainer );
    std::cout << "Number of fixed Points = "<< fixedPointSet->GetNumberOfPoints() << std::endl;
    
//    using MetricType = itk::EuclideanDistancePointMetric< PointSetType, PointSetType >;//ICP最近邻算法
//    using MetricType = itk::PointSetToPointSetMetricv4< PointSetType, PointSetType,float >;//计算相似性//object
//    using MetricType = itk::LabeledPointSetToPointSetMetricv4< PointSetType, PointSetType >;//Computes the distance metric and gradient between two labeled point sets.
//    using MetricType = itk::ExpectationBasedPointSetToPointSetMetricv4< PointSetType, PointSetType >;  //计算基于期望的度量,k_d tree
    using MetricType = itk::EuclideanDistancePointSetToPointSetMetricv4< PointSetType, PointSetType >; //计算欧式距离
//    using MetricType = itk::PointSetToPointSetMetric< PointSetType, PointSetType >;
//    using MetricType = itk::JensenHavrdaCharvatTsallisPointSetToPointSetMetricv4< PointSetType, PointSetType >;
//
    MetricType::Pointer metric = MetricType::New();
 
    //using TransformType = itk::Euler3DTransform< double >;//点集1必须大于等于6
    using TransformType = itk::VersorRigid3DTransform< double >;  //点集1必须大于等于6//对等点集
    // using TransformType = itk::QuaternionRigidTransform< double >;//7个参数
    //using TransformType = itk::KernelTransform< double ,3>;
    TransformType::Pointer transform = TransformType::New();
    
    // Optimizer Type
    using OptimizerType = itk::LevenbergMarquardtOptimizer;
    OptimizerType::Pointer optimizer = OptimizerType::New();
    optimizer->SetUseCostFunctionGradient(false);
    
    // Registration Method
    using RegistrationType = itk::PointSetToPointSetRegistrationMethod<PointSetType, PointSetType >;
    RegistrationType::Pointer   registration  = RegistrationType::New();
    cout<<"NumberOfParameters : "<<transform->GetNumberOfParameters()<<endl;
    OptimizerType::ScalesType scales( transform->GetNumberOfParameters() );
    
    constexpr double translationScale = 1000.0; // dynamic range of translations
    constexpr double rotationScale = 1.0;       // dynamic range of rotations
    
    scales[0] = 1.0 / rotationScale;
    scales[1] = 1.0 / rotationScale;
    scales[2] = 1.0 / rotationScale;
    scales[3] = 1.0 / rotationScale;
    scales[4] = 1.0 / translationScale;
    scales[5] = 1.0 / translationScale;
    //scales[6] = 1.0 / translationScale;
    
    unsigned long   numberOfIterations =  100;
    double          gradientTolerance  =  1e-5;   // convergence criterion
    double          valueTolerance     =  1e-5;   // convergence criterion
    double          epsilonFunction    =  1e-6;   // convergence criterion
    
    
    optimizer->SetScales( scales );
    optimizer->SetNumberOfIterations( numberOfIterations );
    optimizer->SetValueTolerance( valueTolerance );
    optimizer->SetGradientTolerance( gradientTolerance );
    optimizer->SetEpsilonFunction( epsilonFunction );
    
    transform->SetIdentity();
    
    registration->SetInitialTransformParameters( transform->GetParameters() );
    //add
    metric->SetTransform(transform);
    
    
    registration->SetMetric(metric);
    registration->SetOptimizer(     optimizer     );
    registration->SetTransform(     transform     );
    registration->SetFixedPointSet( fixedPointSet );
    registration->SetMovingPointSet(   movingPointSet   );
    
    // Connect an observer
    CommandIterationUpdate::Pointer observer = CommandIterationUpdate::New();
    optimizer->AddObserver( itk::IterationEvent(), observer );
    
    try
    {
        registration->Update();
    }
    catch( itk::ExceptionObject & e )
    {
        std::cerr << e << std::endl;
    }
    
    typedef itk::Matrix<double, 3, 3> MatrixType;
    MatrixType R = transform->GetMatrix();
    std::cout << "Solution = " << transform->GetParameters() << std::endl;
    // std::cout << " Matrix = " << transform->ComputeWMatrix()<<
    // std::cout << "New points = " <<  transform->TransformPoint();
    // std::cout << "T = " << transform->GetTranslation() << std::endl;
    // std::cout<< "Center = " <<transform->GetCenter()<<std::endl;
    std::cout << "Stopping condition: " << optimizer->GetStopConditionDescription() << std::endl;
    //test
    itk::Vector<double> TT  =  transform->GetTranslation() ;
    itk::Matrix<double, 1, 3> P,Q;
    itk::Matrix<double, 1, 3> T;
    // float Error = 0 ;
    T(0,0) = TT[0];
    T(0,1) = TT[1];
    T(0,2) = TT[2];
    
    int Ndimensions = moving_points.size();
    
    for(int i = 0; i < Ndimensions; i++)
    {
        PointRegistration::Point point = transform->TransformPoint(moving_points[i]);
        P(0,0) = moving_points[i][0];
        P(0,1) = moving_points[i][1];
        P(0,2) = moving_points[i][2];
        
        Q = P * R  + T;
        // Error+= sqrt((Q(0,0)-P(0,0))*(Q(0,0)-P(0,0))+(Q(0,1)-P(0,1))*(Q(0,1)-P(0,1))+(Q(0,2)-P(0,2))*(Q(0,2)-P(0,2)));
        
        PointRegistration::Point QQ ;
        QQ[0] = Q(0,0);
        QQ[1] = Q(0,1);
        QQ[2] = Q(0,2);
        QPoints.push_back(point);
    }
    
    // cout<<" error: "<< Error/Ndimensions <<endl;
    res.error = kNoError;
    res.upper_accuracy = 0.1;
    res.lower_accuracy = 0.1;
    
    return res;