Commit a138b97a authored by li412076173's avatar li412076173

move KalmanFilter from delegates to coreopencvextensions

parent 89053048
......@@ -29,6 +29,8 @@
#include "TickTockTimer.h"
#include "ScreenCameraInfo.h"
#include "KalmanFilter.h"
#define debugcout(x) std::cout<<#x<<x<<std::endl
#endif
#include "KalmanFilter.h"
KalmanFilter::KalmanFilter(int number, int dtype) {
int depth = CV_MAT_DEPTH(dtype);
filter = new cv::KalmanFilter(number * 2, number, 0, depth);
stateNumber = number;
// first, constructing the transition matrix, consisting of 4 blocks.
cv::Mat topLeft = cv::Mat::eye(number, number, depth);
cv::Mat topRight, bottomRight;
topLeft.copyTo(topRight);
topLeft.copyTo(bottomRight);
cv::Mat bottomLeft = cv::Mat::zeros(number, number, depth);
cv::Mat top, bottom, transitionMatrix;
cv::hconcat(topLeft, topRight, top);
cv::hconcat(bottomLeft, bottomRight, bottom);
cv::vconcat(top, bottom, transitionMatrix);
transitionMatrix.copyTo(this->transitionMatrix);
filter->transitionMatrix = this->transitionMatrix;
cv::setIdentity(filter->measurementMatrix);
cv::setIdentity(filter->errorCovPre, cv::Scalar::all(0.1f));
setProcessNoiseCov(1e-4f);
setMeasurementNoiseCov(0.01f);
}
KalmanFilter::KalmanFilter(int number) {
KalmanFilter(number, CV_64F);
}
KalmanFilter::KalmanFilter(int number, cv::Point2f) {
KalmanFilter(number * 2, CV_64F);
}
KalmanFilter::KalmanFilter(int number, cv::Point3f) {
KalmanFilter(number * 3, CV_64F);
}
KalmanFilter::KalmanFilter(int number, int dtype, cv::Point2f) {
KalmanFilter(number * 2, dtype);
}
KalmanFilter::KalmanFilter(int number, int dtype, cv::Point3f) {
KalmanFilter(number * 3, dtype);
}
KalmanFilter::~KalmanFilter()
{
}
void KalmanFilter::setProcessNoiseCov(double value) {
this->processNoiseCov = value;
cv::setIdentity(filter->processNoiseCov, cv::Scalar::all(this->processNoiseCov));
}
void KalmanFilter::setMeasurementNoiseCov(double value) {
this->measurementNoiseCov = value;
cv::setIdentity(filter->measurementNoiseCov, cv::Scalar::all(this->measurementNoiseCov));
}
cv::Mat KalmanFilter::smooth(const cv::Mat & measurementMat) {
cv::Mat estimated;
if (count++ == 0) {
measurementMat.copyTo(filter->statePre);
// measurementMat.copyTo(filter->statePost);
}
filter->predict();
filter->correct(measurementMat).rowRange(0, stateNumber).copyTo(estimated);
return estimated;
}
std::vector<cv::Point2f> KalmanFilter::smooth(const std::vector<cv::Point2f> & measurementPoints) {
int insz[1] = { (int)measurementPoints.size() * 2 };
int outsz[1] = { (int)measurementPoints.size() };
cv::Mat pointsMat = cv::Mat(measurementPoints).reshape(1, 1, insz);
cv::Mat smoothedPointsMat = this->smooth(pointsMat);
std::vector<cv::Point2f> returnVector;
smoothedPointsMat.reshape(2, 1, outsz).copyTo(returnVector);
return returnVector;
}
std::vector<cv::Point3f> KalmanFilter::smooth(const std::vector<cv::Point3f> & measurementPoints) {
int insz[1] = { (int)measurementPoints.size() * 3 };
int outsz[1] = { (int)measurementPoints.size() };
cv::Mat pointsMat = cv::Mat(measurementPoints).reshape(1, 1, insz);
cv::Mat smoothedPointsMat = this->smooth(pointsMat);
std::vector<cv::Point3f> returnVector;
smoothedPointsMat.reshape(3, 1, outsz).copyTo(returnVector);
return returnVector;
}
\ No newline at end of file
#pragma once
#include <vector>
#include <CoreOpenCVExtensions/CoreOpenCVExtensions.h>
class KalmanFilter
{
public:
KalmanFilter() {};
KalmanFilter(int number, int dtype);
KalmanFilter(int number);
KalmanFilter(int number, cv::Point2f);
KalmanFilter(int number, cv::Point3f);
KalmanFilter(int number, int dtype, cv::Point2f);
KalmanFilter(int number, int dtype, cv::Point3f);
~KalmanFilter();
double processNoiseCov;
double measurementNoiseCov;
cv::Mat transitionMatrix;
void setProcessNoiseCov(double value);
void setMeasurementNoiseCov(double value);
cv::Mat smooth(const cv::Mat & measurementMat);
std::vector<cv::Point2f> smooth(const std::vector<cv::Point2f> & measurementPoints);
std::vector<cv::Point3f> smooth(const std::vector<cv::Point3f> & measurementPoints);
private:
cv::KalmanFilter * filter;
int count;
int stateNumber;
};
......@@ -35,8 +35,8 @@ namespace cve {
cv::Mat2d resultPoint;
cv::transform(cv::Mat2d(originalPoint - rotatedRect.center), resultPoint, localRotationM);
int x = resultPoint(0)[0] < 0.0 ? 0 : resultPoint(0)[0];
int y = resultPoint(0)[1] < 0.0 ? 0 : resultPoint(0)[1];
double x = resultPoint(0)[0] < 0.0 ? 0 : resultPoint(0)[0];
double y = resultPoint(0)[1] < 0.0 ? 0 : resultPoint(0)[1];
x = x > rotatedRect.size.width ? rotatedRect.size.width : x;
y = y > rotatedRect.size.height ? rotatedRect.size.height : y;
......
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