#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++)
{
F
armadillo库小案例
最新推荐文章于 2025-07-09 10:06:32 发布