SISL 插值、近似 example 运行测试

void
VisualizeSISLCurve(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cv, double r, double g, double b, bool show_cps)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    cloud->clear();
    //输入点数
    const int number = cloud_cv->size();
    //三次四阶
    const int order = 4;
    std::vector<double> coef;
    
    for (int j = 0; j < cloud_cv->size(); j++)
    {
        auto&pnt=cloud_cv->at(j);
        coef.push_back(pnt.x);
        coef.push_back(pnt.y);
        coef.push_back(pnt.z);


    }
   
    //节点向量的大小似乎不能初始化为number+order-3
    int no_repeate_nknot = number-(order-1)+1;
    std::vector<double> knots(no_repeate_nknot + 2 * (order - 1), 0);
    for (int j = order - 1,k=0; j < order - 1 + no_repeate_nknot; ++j,++k)
    {
        knots[j] = k;
    }
    for (int j = order - 1 + no_repeate_nknot; j < knots.size(); ++j)
    {
        knots[j] = knots[order - 1 + no_repeate_nknot-1];
    }
    //创建开口曲线
    SISLCurve* curve = newCurve(number, // number of control points
        order,  // order of spline curve (degree + 1)
        knots.data(),  // pointer to knot vector (parametrization)
        coef.data(),   // pointer to coefficient vector (control points)
        1,      // kind = polynomial B-spline curve
        3,      // dimension
        0);     // no copying of information, 'borrow' arrays
    if (!curve) {
        throw std::runtime_error("Error occured while generating curve.");
    }
    freeCurve(curve);
   
    double astpar = 0.0;
    int iopen = -1;
    int idim = 3;
    int ik = 4;
    curve = NULL;
    int jstat = 0;
    //将数据作为控制点创建封闭曲线
    s1630(coef.data(), number, astpar, iopen, idim, ik, &curve, &jstat);
    freeCurve(curve);
    curve = NULL;
    //插值封闭曲线
    {
        
        std::vector<int> nptyp(number,1);
        int icnsta = 0;
        int icnend = 0;
        int iopen = -1;
      
        double astpar = 0.0;
        double cendpar = 0.0;
       
        double* gpar = NULL;
        int jnbpar = 0;
        int jstat = 0;
        s1356(coef.data(), number, idim, nptyp.data(), icnsta, icnend, iopen, ik, astpar, &cendpar, &curve, &gpar, &jnbpar, &jstat);
           
        
    }
    freeCurve(curve);
    curve = NULL;
    //生成近似曲线,拟合?噪声点的影响如何消除,SISL 中有没有类似pcl中样条曲线拟合的接口?
    {
        const double tolerance = 1.0e-5; // pointwise tolerance
        const double afctol = 0.1; // number between 0 and 1 describing how tolerance
        // is to be distributed in two different steps of the
        // algorithm.
        const int max_iterations = 6;
        std::vector<double> tolerance_vec(number, tolerance); // same tolerance for all samples
        std::vector<double> emxerr(number);
        int jstat = 0;
        int iopen = -1;// Produce closed, periodic curve if possible 没有强制闭合
        int ik = 4;
        s1961(coef.data(), // array of points to be approximated
            number,              // number of sample points
            3,                        // dimension of Euclidean space
            2,                        // flag indicating uniform parametrization
            NULL,                     // we do not use this argument
            &tolerance_vec[0],        // the pointwise tolerance
            0,                        // number of fixed derivatives at left
            0,                        // number of fixed derivatives at right
            iopen,                        // flag indicating that we want an open curve
            afctol,                   // distribution of tolerance
            max_iterations,           // maximum number of iterations in the routine
            ik,                        // polynomial order of approximation
            &curve,            // pointer to the generated curve
            &emxerr[0],               // reporting the max deviation in each point
            &jstat);                  // status variable

        if (jstat < 0) {
            throw std::runtime_error("Error occured inside call to SISL routine s1961.");
        }
        else if (jstat > 0) {
            cerr << "WARNING: warning occured inside call to SISL routine s1961. \n"
                << endl;
        }
    }




//输出曲线500离散点
#define CURVE_EVALUATIONS 500
    
    double discr_curve[3 * CURVE_EVALUATIONS];
    int left = 0;
    for (int j = 0; j < CURVE_EVALUATIONS; j++)
    {
        double t =
            curve->et[curve->ik - 1] +
            (curve->et[curve->in] - curve->et[curve->ik - 1]) *
            j / (CURVE_EVALUATIONS - 1.0);
        {
            int stat;

            s1227(curve, 0, t, &left, discr_curve + 3 * j, &stat);
            if (stat != 0)
                printf("s1227 returned status %d.\n", stat);
        }
    }
        
    for (int j = 0; j < CURVE_EVALUATIONS; j++)
    {
        pcl::PointXYZRGB p;

        p.x = static_cast<float> (discr_curve[3 * j+0]);
        p.y = static_cast<float> (discr_curve[3 * j + 1]);
        p.z = static_cast<float> (discr_curve[3 * j + 2]);
        p.r = 255;
        p.g = 0;
        p.b = 0;
        cloud->push_back(p);
       
    }
    
    freeCurve(curve);



    for (std::size_t i = 0; i < cloud->size() - 1; i++)
    {
        pcl::PointXYZRGB& p1 = cloud->at(i);
        pcl::PointXYZRGB& p2 = cloud->at(i + 1);
        std::ostringstream os;
        os << "line_" << r << "_" << g << "_" << b << "_" << i;
        viewer.addLine<pcl::PointXYZRGB>(p1, p2, r, g, b, os.str());

    }
    /*if (show_cps)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cps(new pcl::PointCloud<pcl::PointXYZ>);
        for (int i = 0; i < cloud_cv->size(); i++)
        {
            pcl::PointXYZ& p1 = cloud_cv->at(i);
            ON_3dPoint cp(p1.x, p1.y, p1.z);


            pcl::PointXYZ p;
            p.x = float(cp.x);
            p.y = float(cp.y);
            p.z = float(cp.z);
            cps->push_back(p);
        }
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler(cps, 255 * r, 255 * g, 255 * b);
        viewer.addPointCloud<pcl::PointXYZ>(cps, handler, "cloud_cps");
    }*/
}
s1356 example02 的结果