Commit bfb7fae0 authored by Zhiping Jiang (蒋志平)'s avatar Zhiping Jiang (蒋志平) 💬

move KalmanDataTracker to OpenCVExtensions. cmake include OpenCVExtensions.

parent 65bd7faf
//
// Created by root on 10/30/17.
//
#ifndef MEGAARRAY_KALMANDATATRACKER_H
#define MEGAARRAY_KALMANDATATRACKER_H
#include <opencv2/opencv.hpp>
#include "OpencvMatxExtensions.h"
template<typename _Tp = double, int NumOfMeasurementVar = 1, int order = 1> class KalmanDataTracker{
public:
KalmanDataTracker() {
auto orderPlus = order + 1;
kf = new cv::KalmanFilter(NumOfMeasurementVar * orderPlus, NumOfMeasurementVar, 0, cv::DataDepth<_Tp>::value);
cv::Mat_<_Tp> transitionMat = cv::Mat_<_Tp>::eye(NumOfMeasurementVar * orderPlus, NumOfMeasurementVar * orderPlus);
cv::Mat_<_Tp> measurementMat = cv::Mat_<_Tp>::zeros(NumOfMeasurementVar, NumOfMeasurementVar * orderPlus);
for(auto i = 0 ; i < NumOfMeasurementVar; i ++ ) {
for (auto j = 0 ; j <order ; j ++) {
transitionMat(i * NumOfMeasurementVar + j, i * NumOfMeasurementVar + j + 1) = 1;
}
measurementMat(i, i * order) = 1;
}
transitionMat.copyTo(kf->transitionMatrix);
measurementMat.copyTo(kf->measurementMatrix);
cv::setIdentity(kf->errorCovPre, cv::Scalar_<_Tp>::all(0.1f));
}
void setProcessNoiseCov(_Tp processNoise) {
cv::setIdentity(kf->processNoiseCov, cv::Scalar_<_Tp>::all(processNoise));
}
void setMeasuremetNoiseCov(_Tp measurementNoise) {
cv::setIdentity(kf->measurementNoiseCov, cv::Scalar_<_Tp>::all(measurementNoise));
}
cv::Mat_<_Tp> smooth(const cv::Mat_<_Tp> & measurement) {
if (count++ == 0) {
cv::Mat_<_Tp> stateMat(NumOfMeasurementVar * (order + 1), 1);
for (auto i = 0 ; i < NumOfMeasurementVar; i++ ) {
stateMat(i * (order + 1)) = measurement(i);
}
stateMat.copyTo(kf->statePre);
}
kf->predict();
auto statePost = kf->correct(measurement);
return statePost.clone();
}
cv::Vec<_Tp, NumOfMeasurementVar * (order + 1)> smooth(const cv::Matx<_Tp, NumOfMeasurementVar, 1> & measurement) {
auto smoothMat = this->smooth(cv::Mat_<_Tp>(measurement));
auto result = cve::vec::fromMatx(cve::matx::fromMat<_Tp, NumOfMeasurementVar * (order + 1), 1>(smoothMat));
return result;
};
std::vector<_Tp> smooth(const std::vector<_Tp> & measurement) {
auto smoothMat = this->smooth(cv::Mat_<_Tp>(cv::Mat_<_Tp>(measurement).reshape(1)));
std::vector<_Tp> resultVector(NumOfMeasurementVar * (order + 1));
for(auto i = 0 ; i < NumOfMeasurementVar * (order + 1); i++)
resultVector[i] = smoothMat[i];
}
private:
cv::KalmanFilter * kf;
uint64_t count = 0;
};
#endif //MEGAARRAY_KALMANDATATRACKER_H
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment