armadillo库小案例

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <armadillo>
using namespace std;
using namespace arma;

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)
{
    mat X1(1, 4), F1(4, 4), P(4, 4), Q(4, 4), H(4, 4), R(4, 4);
    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;
    for (int j = 0; j < 16; j++)
    {