C ++ Eigen Library

时间:2017-05-30 02:17:39

标签: c++ eigen ros ellipse

我在使用#include编译此程序时遇到问题。我看到,如果我注释掉这行,它就会编译。

MatrixXd A =(1.0 /(double)d)*(p * U * p.transpose() - (p * u)*(p * u).transpose())。inverse();

我无法更改标题,因为我需要在ROS中运行此代码,我必须使用内置的Eigen库。我正在使用此链接中描述的代码
How to fit a bounding ellipse around a set of 2D points
任何帮助都非常适合。

   pound include iostream   
   pound include Eigen/Dense  
   using namespace std;  
   using Eigen::MatrixXd;  
   int main ( )  
   {  

    //The tolerance for error in fitting the ellipse
    double tolerance = 0.2;
    int n = 12; // number of points
    int d = 2; // dimension
    MatrixXd p(d,n); //Fill matrix with random points

    p(0,0) = -2.644722; 
    p(0,1) = -2.644961;
    p(0,2) = -2.647504;
    p(0,3) = -2.652942;
    p(0,4) = -2.652745;
    p(0,5) = -2.649508;
    p(0,6) = -2.651345;
    p(0,7) = -2.654530;
    p(0,8) = -2.651370;
    p(0,9) = -2.653966;
    p(0,10) = -2.661322;
    p(0,11) = -2.648208;

    p(1,0) = 4.764553; 
    p(1,1) = 4.718605;
    p(1,2) = 4.676985;
    p(1,3) = 4.640509;
    p(1,4) = 4.595640;
    p(1,5) = 4.546657;
    p(1,6) = 4.506177;
    p(1,7) = 4.468277;
    p(1,8) = 4.421263;
    p(1,9) = 4.383508;
    p(1,10) = 4.353276;
    p(1,11) = 4.293307;


    cout << p << endl; 

    MatrixXd q = p;
    q.conservativeResize(p.rows() + 1, p.cols());

    for(size_t i = 0; i < q.cols(); i++)
    {
        q(q.rows() - 1, i) = 1;
    }

    int count = 1;
    double err = 1;

    const double init_u = 1.0 / (double) n;
    MatrixXd u = MatrixXd::Constant(n, 1, init_u);


    while(err > tolerance)
    {
        MatrixXd Q_tr = q.transpose();
        cout << "1 " << endl;
        MatrixXd X = q * u.asDiagonal() * Q_tr;
        cout << "1a " << endl;
        MatrixXd M = (Q_tr * X.inverse() * q).diagonal();
        cout << "1b " << endl;



        int j_x, j_y;
        double maximum = M.maxCoeff(&j_x, &j_y);
        double step_size = (maximum - d - 1) / ((d + 1) * (maximum + 1));

        MatrixXd new_u = (1 - step_size) * u;
        new_u(j_x, 0) += step_size;

        cout << "2 " << endl;

        //Find err
        MatrixXd u_diff = new_u - u;
        for(size_t i = 0; i < u_diff.rows(); i++)
        {
         for(size_t j = 0; j < u_diff.cols(); j++)
                u_diff(i, j) *= u_diff(i, j); // Square each element of the            matrix
        }
        err = sqrt(u_diff.sum());
        count++;
        u = new_u;
    }

    cout << "3 " << endl;
    MatrixXd U = u.asDiagonal();
    MatrixXd A = (1.0 / (double) d) * (p * U * p.transpose() - (p * u) * (p * u).transpose()).inverse();
    MatrixXd c = p * u;

   cout << A << endl;
   cout << c << endl;

    return 0; 
}

1 个答案:

答案 0 :(得分:1)

如果我用

替换明显的pound include伪造
#include <iostream>
#include <Eigen/Dense>

它编译得很好。它也运行,打印一些数字并返回0。