Eigen库小案例

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <iostream>

#include "Eigen/Eigen"

using namespace std;
using namespace Eigen;

typedef struct _coor_fin
{
    double x;
    double y;
}coor_fin;

typedef struct _coor_lin
{
    double x;
    double y;
}coor_lin;

typedef struct _coor_cin
{
    double x;
    double y;   
}coor_cin;

typedef struct _coor_out
{
    double xout;
    double yout;
}coor_out;

/*
 *函数功能：坐标平滑矫正
 *函数入参：coor_fin:基准坐标
 *          l_in:上一次坐标
 *          c_in：本次坐标
 *函数出参  coor_out 矫正后的坐标
 *函数返回值：成功返回 0， 失败返回 -1
*/
int correct_position(coor_fin f_in, coor_lin l_in, coor_cin c_in, coor_out *out)
{
      MatrixXf X(1, 4), X1(1, 4), F(4, 4), F1(4, 4), P(4, 4), Q(4, 4), H(4, 4), R(4, 4), I(4, 4);
        F << 1,0,0.25,0,0,1,0,0.25,0,0,1,0,0,0,0,1;
        P << 0.3,0,0,0,0,0.3,0,0,0,0,0.1,0,0,0,0,0.1;
        Q << 0.3,0,0,0,0,0.3,0,0,0,0,0.3,0,0,0,0,0.3;
        H << 1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0;
        R << 2,0,0,0,0,2,0,0,0,0,0.1,0,0,0,0,0.1;
        I << 1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1; 
      double fx = f_in.x;
      double fy = f_in.y;
    double lx = l_in.x;
    double ly = l_in.y;
    double cx = c_in.x;
    double cy = c_in.y;

    X1 << f_in.x, f_in.y, 0, 0; 
    X = X1.transpose();

    if(fabs(cx - lx) > 5 || fabs(cy  - ly) > 5 || (cx == 0 && cy == 0))
        {
            printf("fabs > 5 or x = y = 0\n");
            return -1;
        }

    if (fabs(cx - lx) < 5 && fabs(cy  - ly) < 5)
    {
        MatrixXf LX = X;
      MatrixXf X_ = F * LX;
      MatrixXf P_ = F * P * F.transpose() + Q;
      MatrixXf MN = H * P_ * H.transpose() + R;
      MatrixXf K = P_ * H.transpose() * MN.inverse(); 
      X1 << cx, cy, 0, 0; 
      MatrixXf Z = X1.transpose();
      X = X_ + K * (Z - H * X_);
      P = (I - K * H) * P_;                    
      out->xout = X(0,0);
      out->yout = X(1,0);
    }
    return 0;
}

int main()
{
     coor_fin f_in;
     f_in.x = 43.432743;
     f_in.y = 0.229601;
     coor_lin l_in;
     l_in.x = 43.374115;
     l_in.y = 0.002367;
     coor_cin c_in;
     c_in.x = 43.469775;
     c_in.y = 0.240061;
     coor_out out;
     int ret = correct_position(f_in, l_in, c_in, &out);
     if(ret == -1)
     {
            printf("correct_position error\n");
            return -1;  
     }
     printf("out_x = %f\n", out.xout);
     printf("out_y = %f\n", out.yout);
   return 0;
}