SLAM: 基于运动和视觉里程方法的实时三维构建：Kalman Filter Predict/Refined代码
二、根据EKF预测状态，更新特征信息！
//2. EKF prediction (state and measurement prediction)
o_EKfPreor.ekf_prediction(filter, features_info );
使用类 class CEkfPredict：
#pragma once
//#include <math.h>
//#include <limits>
//#include <cmath>
//
//#include <iostream>
//#include <string>
//#include <vector>
//#include <strstream>
//
//#include <cv.h>
//#include <highgui.h>
//
//#include <Eigen/Core>
//#include <Eigen/Geometry>
//
//#include <opencv2/opencv.hpp>
//
//#include "opencv2/imgproc/imgproc.hpp"
//
//#include "opencv2/video/video.hpp"
//#include "opencv2/video/tracking.hpp"
//
//#include <opencv2/nonfree/nonfree.hpp>
//#include <opencv2/nonfree/features2d.hpp>
//
//#include "DataStruct.h"
//#include "OperReFunc.h"
#include "MatConv.h"
#include "TestPrint.h"
using namespace std;
using namespace cv;
class CEkfPredict
{
public:
CEkfPredict(void);
~CEkfPredict(void);
public:
int ekf_prediction(
StruEkf & filter, std::vector<StruFeaturesInfo > &features_info );
int predict_state_and_covariance(
Eigen::MatrixXf & X_k,Eigen::MatrixXf & P_k,
std::string type,
double SD_A_component_filter,
double SD_alpha_component_filter,
Eigen::MatrixXf &X_km1_k, Eigen::MatrixXf &P_km1_k );
public:
int dfv_by_dxv(
Eigen::MatrixXf &Xv,Eigen::MatrixXf &u,Eigen::MatrixXf &dt,
std::string type,
Eigen::MatrixXf &dfv_by_dxvRES);
Eigen::MatrixXf dfv_by_dxv(
Eigen::MatrixXf &Xv,Eigen::MatrixXf &u,Eigen::MatrixXf &dt,
std::string type);
//获取相机的参数：FOV
int fv(
Eigen::MatrixXf &X_k_k,
double delta_t,std::string type,double std_a,double std_alpha,
Eigen::MatrixXf &X_k_km1);
int func_Q(
Eigen::MatrixXf &Xv,Eigen::MatrixXf &u,Eigen::MatrixXf &Pn,
double delta_t, std::string type,
Eigen::MatrixXf &Q,Eigen::MatrixXf &G );
Eigen::MatrixXf func_Q(
Eigen::MatrixXf &Xv,Eigen::MatrixXf &u,Eigen::MatrixXf &Pn,
double delta_t, std::string type,
Eigen::MatrixXf &G );
public:
double qprod(
Eigen::MatrixXf &q,Eigen::MatrixXf &p,Eigen::MatrixXf &pq);
Eigen::MatrixXf qprod(Eigen::MatrixXf &q,Eigen::MatrixXf &p);
int cross(
Eigen::MatrixXf &a,Eigen::MatrixXf &b,
int dim,
Eigen::MatrixXf &c);
public:
int v2q(
Eigen::MatrixXf &omegaOld, Eigen::MatrixXf &qwt);
Eigen::MatrixXf v2q(Eigen::MatrixXf &v);
int quaternion(
Eigen::MatrixXf &v,double theta,Eigen::MatrixXf &q);
int dq3_by_dq1(
Eigen::MatrixXf &qOld,Eigen::MatrixXf &M);
Eigen::MatrixXf dq3_by_dq1(Eigen::MatrixXf &qOld);
int dq3_by_dq2(
Eigen::MatrixXf &q1_in,Eigen::MatrixXf &dq3_by_dq2RES);
Eigen::MatrixXf dq3_by_dq2(Eigen::MatrixXf &q1_in);
public:
int tr2rpy(Eigen::MatrixXf &m,Eigen::MatrixXf &rpy);
Eigen::MatrixXf tr2rpy(Eigen::MatrixXf &m);
Eigen::MatrixXf q2tr(Eigen::MatrixXf &q);
int dq_by_deuler(Eigen::MatrixXf &euler_angles,Eigen::MatrixXf &G);
Eigen::MatrixXf dq_by_deuler(Eigen::MatrixXf &euler_angles);
int
dqomegadt_by_domega(
Eigen::MatrixXf &omegaOld,double delta_t,Eigen::MatrixXf &M);
Eigen::MatrixXf
dqomegadt_by_domega(
Eigen::MatrixXf &omegaOld,double delta_t);
public:
inline double dq0_by_domegaA(double omegaA,double omega,double delta_t);
inline double dqA_by_domegaA(double omegaA,double omega,double delta_t);
inline double dqA_by_domegaB(double omegaA,double omegaB,double omega, double delta_t);
public:
//多维重排
int ipermute(
Eigen::MatrixXf &b,Eigen::MatrixXf &order,Eigen::MatrixXf &a);
private:
//全局变量
//初始化Eigen矩阵使用
//const
double eps ;
//生成随机球体使用
//const
double gSphereRadiusP ;
//特征更新使用标识符
//const
std::string gSVOC ;
//需要检测的图像文件夹
//const
std::string gSequencePath ;
//起止元素
//const
int gInitIm ;
//const
int gLastIm ;
private:
//返回特定元素值所在的位置，行优先，逐列遍历
int find(Eigen::MatrixXf &M, Eigen::MatrixXf &index ,double Value);
//返回数组中大于value的下标，返回索引
int find(Eigen::VectorXf &M, Eigen::VectorXf &index , double Value);
private:
CTestPrint testor;
};
调用函数流程：
//使用 路标地图，使用卡曼滤波的方法，进行逐帧状态分析和 参数更新。
int CEkfPredict::ekf_prediction(StruEkf & f, std::vector<StruFeaturesInfo > &features_info )
{
predict_state_and_covariance(
f.x_k_k, f.p_k_k, f.type,
f.std_a, f.std_alpha,
f.x_k_km1, f.p_k_km1 );
return 1;
}
//使用 路标地图，使用卡曼滤波的方法，进行逐帧状态分析和 参数更行。
int CEkfPredict::predict_state_and_covariance(
Eigen::MatrixXf & X_k,Eigen::MatrixXf & P_k,
std::string type,
double SD_A_component_filter,
double SD_alpha_component_filter,
Eigen::MatrixXf &X_km1_k, Eigen::MatrixXf &P_km1_k )
{
double delta_t = 1;
Eigen::VectorXf X_Kv =Eigen::VectorXf::Zero(13);
X_Kv = X_k.block(0,0,13,1);
Eigen::MatrixXf X_KvM(X_Kv);
testor.print_EigenMat(X_k,"D:/CodeBase/IisuVR/DebugInfo/DebugMatX_k.txt","X_K;");
testor.print_EigenMat(X_KvM,"D:/CodeBase/IisuVR/DebugInfo/DebugMatX_Kv.txt","X_Kv;");
//X_Kv = X_Kv.segment(0,13);
Eigen::MatrixXf X_Km = Eigen::MatrixXf::Zero(13,1);
X_Km = X_KvM;
// camera motion prediction
Eigen::MatrixXf Xv_km1_k(13,1) ;//=
fv( X_Km, delta_t, type, SD_A_component_filter, SD_alpha_component_filter ,Xv_km1_k );
// features prediction
//13*1 + 96*1 =109*1
//X_km1_k = [ Xv_km1_k; X_k( 14:end,: ) ];
Eigen::MatrixXf X_km1_k2(Xv_km1_k.rows()+X_k.rows()-13, 1 );//Eigen::VectorXf X_km1_k2(Xv_km1_k.rows()+X_k.rows()-13, 1 );
X_km1_k2<<
Xv_km1_k,
X_k.block(13,0,X_k.rows()-13,X_k.cols() );//在狄一珍根本检测不到特征，直接跳到此行，算法有问题！
X_km1_k =X_km1_k2;
//// state transition equation derivatives
//13*13 double！
//Eigen::MatrixXf F =sparse( dfv_by_dxv( X_k(1:13,:),zeros(6,1),delta_t, type ) );
Eigen::MatrixXf F1;
F1 = X_k.block(0,0,13,X_k.cols() );
Eigen::MatrixXf delta_tM(1,1);
delta_tM << delta_t;
Eigen::MatrixXf T=Eigen::MatrixXf::Zero(6,1);
Eigen::MatrixXf F;
F = dfv_by_dxv( F1 ,T,delta_tM, type );//(1:13,:),zeros(6,1),delta_t, type ) );
////state noise
double linear_acceleration_noise_covariance =
(SD_A_component_filter*delta_t)*SD_A_component_filter*delta_t;
double angular_acceleration_noise_covariance =
(SD_alpha_component_filter*delta_t)*SD_alpha_component_filter*delta_t;
//6*6的对角阵
//Pn = sparse (diag( [linear_acceleration_noise_covariance linear_acceleration_noise_covariance linear_acceleration_noise_covariance...
// angular_acceleration_noise_covariance angular_acceleration_noise_covariance angular_acceleration_noise_covariance] ) );
Eigen::MatrixXf Pn=Eigen::MatrixXf::Identity(6,6);
//Pn = sparse (diag( [
Pn(0,0) = linear_acceleration_noise_covariance;
Pn(1,1) = linear_acceleration_noise_covariance;
Pn(2,2) = linear_acceleration_noise_covariance;//...
Pn(3,3) = angular_acceleration_noise_covariance;
Pn(4,4) = angular_acceleration_noise_covariance;
Pn(5,5) = angular_acceleration_noise_covariance;
//R.diagonal() ;
Eigen::MatrixXf Q(13,13);
Eigen::MatrixXf G(13,13);
////func_Q( X_k(1:13,:), zeros(6,1), Pn, delta_t, type,Q);
//func_Q( X_Km, Eigen::MatrixXf::Zero(6,1), Pn, delta_t, type,Q);
Eigen::MatrixXf E(6,1);E = Eigen::MatrixXf::Zero(6,1);
Q = func_Q( X_Km,E , Pn, delta_t, type, G);
int size_P_k = P_k.rows();//size(P_k,1);
//P_km1_k = [ F*P_k(1:13,1:13)*F' + Q F*P_k(1:13,14:size_P_k);
// P_k(14:size_P_k,1:13)*F' P_k(14:size_P_k,14:size_P_k)];
Eigen::MatrixXf M11,M12,M21,M22;
M11 = (F*P_k.block(0,0,13,13)*F ).transpose() + Q ;
M12 = F*P_k.block(0,13,13,size_P_k-14);
M21 = (P_k.block(13,0,size_P_k-14,13)*F );//.transpose() ;
M22 = P_k.block(13,13,size_P_k-14,size_P_k-14);
P_km1_k.resize(M11.rows()+M22.rows(),M11.cols()+M22.cols() );
P_km1_k <<
M11 , M12 ,//(1:13,14:size_P_k),
M21 , M22 ;//(14:size_P_k,14:size_P_k);
return 1;
}
其他函数：
有待调试优化！