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++)
    {
      F1[j] = 0;
    }
    for (int k = 0; k < 4; k++)
    {
      F1[k * 5] = 1;
    }
    F1[2] = F1[7] = 0.25;
    mat F = F1.t();
    
    for (int i = 0; i < 16; i++)
    {
      P[i] = 0;
    }
    P[0] = P[5] = 0.3;
    P[10] = P[15] = 0.1;

    for (int i = 0; i < 16; i++)
    {
      Q[i] = 0;
    }
    for (int i = 0; i < 4; i++)
    {
      Q[i * 5] = 0.3;
    }
    
    for (int i = 0; i < 16; i++)
    {
      H[i] = 0;
    }
    H[0] = H[5] = 1;

    for (int i = 0; i < 16; i++)
    {
      R[i] = 0;
    }
    R[0] = R[5] = 2;
    R[10] = R[15] = 0.1;
 
    mat I = eye(4, 4);
    
    X1[0] = f_in.x;
    X1[1] = f_in.y;
    X1[2] = X1[3] = 0;
    mat X = X1.t();
    if(fabs(cx - lx) > 5 || fabs(cy  - ly) > 5 || (cx == 0 && cy == 0))
		{
			printf("abs > 5 or x = y = 0\n");
			return -1;
		}

    if (fabs(cx - lx) < 5 && fabs(cy  - ly) < 5)
    {
      mat LX = X;
      mat X_ = F * LX;
      mat P_ = F * P * F.t() + Q;
      mat K = P_ * H.t() * inv(H * P_ * H.t() + R);
      X1[0] = cx;
      X1[1] = cy;
      X1[2] = X1[3] = 0;
      mat Z = X1.t();
      X = X_ + K * (Z - H * X_);
      P = (I - K * H) * P_;
      out->xout = X[0];
      out->yout = X[1];
    }
    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;
}


0
427