SLAM基础——卡尔曼滤波（二）

class kalmanTrack
{
public:
    kalmanTrack(int status_size,int measure_size,int control_size = 2,
            float processNoiseCovariance=1e-4, float measurementNoiseCovariance=1e-1, float errorCovariancePost=0.1):
    kalmanFilter_(cv::KalmanFilter(status_size,measure_size,control_size)),
    status_(cv::Mat(status_size,1,CV_32F)),
    measurement_(cv::Mat(measure_size,1,CV_32F)),
    noise_(cv::Mat(status_size,1,CV_32F))
    {
        cv::setIdentity(kalmanFilter_.measurementMatrix);
        cv::setIdentity(kalmanFilter_.processNoiseCov,cv::Scalar(processNoiseCovariance));
        cv::setIdentity(kalmanFilter_.measurementNoiseCov,cv::Scalar(measurementNoiseCovariance));
        cv::setIdentity(kalmanFilter_.errorCovPost,cv::Scalar(errorCovariancePost));
        cout << kalmanFilter_.measurementMatrix<<endl;
    }

    void setA(cv::Mat& A){ kalmanFilter_.transitionMatrix = A;}
    void setB(cv::Mat& B){kalmanFilter_.controlMatrix = B;}
    void setC(cv::Mat& C){kalmanFilter_.measurementMatrix = C;}
    inline void update(cv::Mat& measurements){
        kalmanFilter_.predict();
        kalmanFilter_.correct(measurements);
    }
public:
    cv::KalmanFilter kalmanFilter_;
    cv::Mat status_;
    cv::Mat noise_;
    cv::Mat measurement_;
};


struct mouseInfo
{
    mouseInfo(int x,int y):x_(x),y_(y){}
    int x_;
    int y_;
};

//void (*MouseCallback)(int event, int x, int y, int flags, void* userdata);
void mouseCallback(int event,int x,int y,int flags,void* info)
{
    auto* info_ = reinterpret_cast<mouseInfo*>(info);
    info_->x_ = x;
    info_->y_ = y;
}

 实现逻辑中，定义了两个卡尔曼滤波，一个和python + opencv: kalman 跟踪中一样，另一个在此基础上增加了加速度变量，并降低控制噪声的不确定度（协方差小点），对比两个结果，从而从直观上加深理解。

kalmanTrack* withG()
{
    auto test = new kalmanTrack(6,2,2,1e-5);
    cv::Mat_<float> A(6,6,CV_32F);
    cv::setIdentity(A,cv::Scalar(1));
    A(0,2) = 1;
    A(1,3) = 1;
    A(0,4) = 0.5;
    A(1,5) = 0.5;
    A(2,4) = 1;
    A(3,5) = 1;
    cout << A << endl;
    test->setA(A);
    return test;
}

kalmanTrack* withoutG()
{
    auto test = new kalmanTrack(4,2);
    cv::Mat_<float> A(4,4,CV_32F);
    cv::setIdentity(A,cv::Scalar(1));
    A(0,2) = 1;
    A(1,3) = 1;
    cout << A << endl;
    test->setA(A);
    return test;
}


int main(int argc,char** argv)
{
    auto test = *withoutG();


    string window_name = "mouse_track";


    cv::Mat img(640,480,CV_32F);
    cvtColor(img, img, CV_GRAY2RGB);
    cv::namedWindow(window_name);
    mouseInfo mouse_info(0,0);
    cv::setMouseCallback(window_name,mouseCallback, reinterpret_cast<void*>(&mouse_info));

    cout << sizeof(float) << endl;
    std::vector<cv::Point2i> measured_points;
    std::vector<cv::Point2i> kalman_points;
    cv::imshow(window_name,img);




    while(true)
    {
        cv::imshow(window_name,img);
        auto keypress = cv::waitKey(20);
        if(keypress == 27) break;
        if(mouse_info.x_==0 && mouse_info.y_==0) continue;
        measured_points.emplace_back(mouse_info.x_,mouse_info.y_);
//Update the Kalman filter with the mouse point
        cv::Mat_<float> measurementMat(2,1);
        measurementMat(0,0) = mouse_info.x_;
        measurementMat(1,0) = mouse_info.y_;
        test.update(measurementMat);
//Get the current Kalman estimate and add it to the trajectory
        kalman_points.emplace_back(test.kalmanFilter_.statePost.at<float>(0),
                                            test.kalmanFilter_.statePost.at<float>(1));
        cout << kalman_points.back() << endl;
        cv::polylines(img,kalman_points, false,cv::Scalar(187,255,255));
        cv::polylines(img,measured_points, false,cv::Scalar(0,0,205));
    }

}

下面是对比结果（前者不带加速度，后者带），截图上的对比似乎看不出来什么，实际运行过程中感觉是带加速度的跟踪要更灵敏一些（虽然不知道是不是主观因素）。