OpenCV的Sample分析:real_time_tracking(3)
我认为分析程序的主要原因不是弄清楚这个程序是怎样实现的,尽管这也很重要。最为重要的是,这段代码能不能给你解决一类问题的思路。比如上一讲的类RobustMatcher,学习它之后,如果我自己写一个PreciseMatcher,是不是可以从RobustMatcher中学到什么呢?
分析程序,从具体的解决方案开始,到一般性一类问题的解决结束
今天来看kalman滤波的初始化(说只前必须要说kalman到底估计什么,在这个例程中,kalman滤波器估计盒子的运动轨迹以及自身的转动,一共有18个变量,盒子的位置,速度,加速度,盒子的Euler角,角速度,角加速度)
void initKalmanFilter( KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt); void updateKalmanFilter( KalmanFilter &KF, Mat &measurements, Mat &translation_estimated, Mat &rotation_estimated ); void fillMeasurements( Mat &measurements, const Mat &translation_measured, const Mat &rotation_measured);
先来看第一个函数initKalmanFiter(),首先需要了解类KalmanFilter的构成,
class CV_EXPORTS_W KalmanFilter { public: /** @brief The constructors. @note In C API when CvKalman\* kalmanFilter structure is not needed anymore, it should be released with cvReleaseKalman(&kalmanFilter) */ CV_WRAP KalmanFilter(); /** @overload @param dynamParams Dimensionality of the state. @param measureParams Dimensionality of the measurement. @param controlParams Dimensionality of the control vector. @param type Type of the created matrices that should be CV_32F or CV_64F. */ CV_WRAP KalmanFilter( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F ); /** @brief Re-initializes Kalman filter. The previous content is destroyed. @param dynamParams Dimensionality of the state. @param measureParams Dimensionality of the measurement. @param controlParams Dimensionality of the control vector. @param type Type of the created matrices that should be CV_32F or CV_64F. */ void init( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F ); /** @brief Computes a predicted state. @param control The optional input control */ CV_WRAP const Mat& predict( const Mat& control = Mat() ); /** @brief Updates the predicted state from the measurement. @param measurement The measured system parameters */ CV_WRAP const Mat& correct( const Mat& measurement ); CV_PROP_RW Mat statePre; //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k) CV_PROP_RW Mat statePost; //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) CV_PROP_RW Mat transitionMatrix; //!< state transition matrix (A) CV_PROP_RW Mat controlMatrix; //!< control matrix (B) (not used if there is no control) CV_PROP_RW Mat measurementMatrix; //!< measurement matrix (H) CV_PROP_RW Mat processNoiseCov; //!< process noise covariance matrix (Q) CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R) CV_PROP_RW Mat errorCovPre; //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/ CV_PROP_RW Mat gain; //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R) CV_PROP_RW Mat errorCovPost; //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k) // temporary matrices Mat temp1; Mat temp2; Mat temp3; Mat temp4; Mat temp5; };
比较长,来看一下它的成员函数,以及成员变量。kalman滤波器的初始化,模型预测函数以及模型矫正函数。以及物体运动模型中的状态变量,观测值,噪声,以及相应的观测矩阵,系统矩阵,控制矩阵,噪声的方差矩阵等。以及
@param dynamParams Dimensionality of the state.
@param measureParams Dimensionality of the measurement.
@param controlParams Dimensionality of the control vector.
对kalman这个类熟悉以后,再来看initKalmanFiter
void initKalmanFilter(KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt) { KF.init(nStates, nMeasurements, nInputs, CV_64F); // init Kalman Filter setIdentity(KF.processNoiseCov, Scalar::all(1e-5)); // set process noise setIdentity(KF.measurementNoiseCov, Scalar::all(1e-2)); // set measurement noise setIdentity(KF.errorCovPost, Scalar::all(1)); // error covariance /** DYNAMIC MODEL **/ // [1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0 0] // [0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0] // [0 0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0] // [0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0 0] // [0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0] // [0 0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0] // [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0] // [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0] // [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0] // [0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0 0] // [0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0] // [0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2] // [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0] // [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0] // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt] // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0] // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0] // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1] // position KF.transitionMatrix.at<double>(0,3) = dt; KF.transitionMatrix.at<double>(1,4) = dt; KF.transitionMatrix.at<double>(2,5) = dt; KF.transitionMatrix.at<double>(3,6) = dt; KF.transitionMatrix.at<double>(4,7) = dt; KF.transitionMatrix.at<double>(5,8) = dt; KF.transitionMatrix.at<double>(0,6) = 0.5*pow(dt,2); KF.transitionMatrix.at<double>(1,7) = 0.5*pow(dt,2); KF.transitionMatrix.at<double>(2,8) = 0.5*pow(dt,2); // orientation KF.transitionMatrix.at<double>(9,12) = dt; KF.transitionMatrix.at<double>(10,13) = dt; KF.transitionMatrix.at<double>(11,14) = dt; KF.transitionMatrix.at<double>(12,15) = dt; KF.transitionMatrix.at<double>(13,16) = dt; KF.transitionMatrix.at<double>(14,17) = dt; KF.transitionMatrix.at<double>(9,15) = 0.5*pow(dt,2); KF.transitionMatrix.at<double>(10,16) = 0.5*pow(dt,2); KF.transitionMatrix.at<double>(11,17) = 0.5*pow(dt,2); /** MEASUREMENT MODEL **/ // [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0] // [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0] // [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0] // [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0] // [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0] // [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0] KF.measurementMatrix.at<double>(0,0) = 1; // x KF.measurementMatrix.at<double>(1,1) = 1; // y KF.measurementMatrix.at<double>(2,2) = 1; // z KF.measurementMatrix.at<double>(3,9) = 1; // roll KF.measurementMatrix.at<double>(4,10) = 1; // pitch KF.measurementMatrix.at<double>(5,11) = 1; // yaw }构建了系统的控制矩阵以及观测矩阵。 在建立系统控制矩阵的时候,有一点很奇怪,即少了单位阵,不知道为什么?
再来看updateKalmanFilter()函数,
void updateKalmanFilter( KalmanFilter &KF, Mat &measurement, Mat &translation_estimated, Mat &rotation_estimated ) { // First predict, to update the internal statePre variable Mat prediction = KF.predict(); // The "correct" phase that is going to use the predicted value and our measurement Mat estimated = KF.correct(measurement); // Estimated translation translation_estimated.at<double>(0) = estimated.at<double>(0); translation_estimated.at<double>(1) = estimated.at<double>(1); translation_estimated.at<double>(2) = estimated.at<double>(2); // Estimated euler angles Mat eulers_estimated(3, 1, CV_64F); eulers_estimated.at<double>(0) = estimated.at<double>(9); eulers_estimated.at<double>(1) = estimated.at<double>(10); eulers_estimated.at<double>(2) = estimated.at<double>(11); // Convert estimated quaternion to rotation matrix rotation_estimated = euler2rot(eulers_estimated); }
这个很好理解,最后看fillMeasurements,指获取观测数据,
void fillMeasurements( Mat &measurements, const Mat &translation_measured, const Mat &rotation_measured) { // Convert rotation matrix to euler angles Mat measured_eulers(3, 1, CV_64F); measured_eulers = rot2euler(rotation_measured); // Set measurement to predict measurements.at<double>(0) = translation_measured.at<double>(0); // x measurements.at<double>(1) = translation_measured.at<double>(1); // y measurements.at<double>(2) = translation_measured.at<double>(2); // z measurements.at<double>(3) = measured_eulers.at<double>(0); // roll measurements.at<double>(4) = measured_eulers.at<double>(1); // pitch measurements.at<double>(5) = measured_eulers.at<double>(2); // yaw }到此为止,kalman的常用函数以及使用方式介绍到这里