RANSAC 加Guass-newton拟合曲线

#include <iostream>
#include <fstream>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;



vector<int> getRandomSample(int n, int num)
{
    vector<int> sample;
    vector<int> total_set;
    for(int i = 0; i < n; ++i)
        total_set.push_back(i);
    for(int i = 0; i < num; ++i)
    {
        int j = rand()%total_set.size();
        sample.push_back( total_set[j] );
        total_set.erase( total_set.begin()+j );
    }
    return sample;
}

//k次函数
double funcX(double* a, double x, int k)
{
    double xx = 1.0;
    double y = 0;
    for(int i = 0; i < k; i++)
    {
        y = y + a[i]*xx;
        xx *= x;
    }
    return y;
}

vector<int> get_inliners(vector<cv::Point2d>& points, double* a, int k)
{
    double inlier_threshold = 5;
    vector<int> inliers;

    for(int i = 0; i < points.size(); i++)
    {
        cv::Point2d p = points[i];
        double y_predict = funcX(a, p.x, k);
        if( fabs(p.y - y_predict) < inlier_threshold)
            inliers.push_back( i );
    }
    cout << "inliers:" << inliers.size() << "\n";
    return inliers;
}

bool estimateLine(vector<cv::Point2d>& points, vector<int>&active, double* a, int k)
{
    const double learning_rate = 0.1;
    const int N = active.size();