Kalman Filter with OpenCV (C++), Assertion failed (a_size.width == len) in gemm

C++
1

'm trying to create a simple Kalman Filter tracker using the openCV library. I based on tutorials for similiar goals in: http://opencvexamples.blogspot.com/2014/01/kalman-filter-implementation-tracking.html Object Tracking: Simple Implementation of Kalman Filter in Python

However, I’m getting an issue with matrix sizes. I’m confident that I’m setting them right and I’ve been around this for hours without being able to fix it. Any suggestions?

    cv::KalmanFilter KF(STATE_SIZE, MEAS_SIZE, CONTR_SIZE, CV_32F);

    // Initial State Matrix
    // [p_x] 
    // [p_y]
    // [p_z]
    // [v_x]
    // [v_y]
    // [v_z] 
    KF.statePre = cv::Mat::zeros(cv::Size(1, STATE_SIZE), CV_32FC1);
    KF.statePre.at<float>(0) = odometry.x;
    KF.statePre.at<float>(1) = odometry.y;
    KF.statePre.at<float>(2) = odometry.z;

    cv::setIdentity(KF.measurementMatrix);
    cv::setIdentity(KF.processNoiseCov, cv::Scalar::all(1e-4));
    cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(10));
    cv::setIdentity(KF.errorCovPost, cv::Scalar::all(.1));

    // State Transition Matrix A
    // [ 1  0   0   dT  0   0 ]
    // [ 0  1   0   0   dT  0 ]
    // [ 0  0   1   0   0   dT]
    // [ 0  0   0   1   0   0 ]
    // [ 0  0   0   0   1   0 ]
    // [ 0  0   0   0   0   1 ] 
    KF.transitionMatrix = cv::Mat::zeros(cv::Size(STATE_SIZE, STATE_SIZE), CV_32FC1);
    KF.transitionMatrix.at<float>(0) = 1.0;
    KF.transitionMatrix.at<float>(3) = dT;
    KF.transitionMatrix.at<float>(7) = 1.0;
    KF.transitionMatrix.at<float>(10) = dT;
    KF.transitionMatrix.at<float>(14) = 1.0;
    KF.transitionMatrix.at<float>(17) = dT;
    KF.transitionMatrix.at<float>(21) = 1.0;
    KF.transitionMatrix.at<float>(28) = 1.0;
    KF.transitionMatrix.at<float>(35) = 1.0;

    // Control Input Matrix B
    // [ dT^2/2   0        0   ]
    // [    0   dT^2/2     0   ]
    // [    0     0     dT^2/2 ]
    // [   dT     0        0   ]
    // [    0     dT       0   ]
    // [    0     0        dT  ]
    KF.controlMatrix = cv::Mat::zeros(cv::Size(STATE_SIZE, MEAS_SIZE), CV_32FC1);
    KF.controlMatrix.at<float>(0) = pow(dT,2)/2;
    KF.controlMatrix.at<float>(4) = pow(dT,2)/2;
    KF.controlMatrix.at<float>(8) = pow(dT,2)/2;
    KF.controlMatrix.at<float>(9) = dT;
    KF.controlMatrix.at<float>(13) = dT;
    KF.controlMatrix.at<float>(17) = dT;

    // Measurement Mapping Transformation Matrix H
    // [ 1 0 0 0 0 0]
    // [ 0 1 0 0 0 0]
    // [ 0 0 1 0 0 0]
    KF.measurementMatrix = cv::Mat::zeros(cv::Size(MEAS_SIZE, STATE_SIZE), CV_32FC1);
    KF.measurementMatrix.at<float>(0) = 1.0;
    KF.measurementMatrix.at<float>(7) = 1.0;
    KF.measurementMatrix.at<float>(14) = 1.0;

    // Process Noise Covariance Matrix Q
    // [ dT^4/4    0       0   dT^3/2    0      0    ]
    // [   0    dT^4/4     0      0    dT^3/2   0    ]
    // [   0       0    dT^4/4    0      0     dT^3/2]
    // [ dT^3/2    0       0    dT^2     0      0    ]
    // [   0    dT^3/2     0      0     dT^2    0    ]
    // [   0       0    dT^3/2    0      0     dT^2  ]
    KF.processNoiseCov = cv::Mat::zeros(cv::Size(STATE_SIZE, STATE_SIZE), CV_32FC1);
    KF.processNoiseCov.at<float>(0) = SIGMA_A2*pow(dT,4)/4;
    KF.processNoiseCov.at<float>(3) = SIGMA_A2*pow(dT,3)/2;
    KF.processNoiseCov.at<float>(7) = SIGMA_A2*pow(dT,4)/4;
    KF.processNoiseCov.at<float>(10) = SIGMA_A2*pow(dT,3)/2;
    KF.processNoiseCov.at<float>(14) = SIGMA_A2*pow(dT,4)/4;
    KF.processNoiseCov.at<float>(17) = SIGMA_A2*pow(dT,3)/2;
    KF.processNoiseCov.at<float>(18) = SIGMA_A2*pow(dT,3)/2;
    KF.processNoiseCov.at<float>(21) = SIGMA_A2*pow(dT,2);
    KF.processNoiseCov.at<float>(25) = SIGMA_A2*pow(dT,3)/2;
    KF.processNoiseCov.at<float>(28) = SIGMA_A2*pow(dT,2);
    KF.processNoiseCov.at<float>(32) = SIGMA_A2*pow(dT,3)/2;
    KF.processNoiseCov.at<float>(35) = SIGMA_A2*pow(dT,2);

    // Measurement Noise Covariance Matrix R
    // [ SIGMA_X   0        0   ]
    // [    0    SIGMA_Y    0   ]
    // [    0      0     SIGMA_Z] 
    KF.measurementNoiseCov = cv::Mat::zeros(cv::Size(MEAS_SIZE, MEAS_SIZE), CV_32FC1);
    KF.measurementNoiseCov.at<float>(0) = SIGMA_X;
    KF.measurementNoiseCov.at<float>(4) = SIGMA_Y;
    KF.measurementNoiseCov.at<float>(8) = SIGMA_Z;
    cv::setIdentity(KF.measurementNoiseCov, SIGMA_A2);

    // Initial Covariance Matrix P
    // >>> to initialize as Identity Matrix
    // KF.errorCovPost = cv::Mat::zeros(cv::Size(STATE_SIZE, STATE_SIZE), CV_32FC1);
    cv::setIdentity(KF.errorCovPost, 1.0);

    cv::Mat prediction = KF.predict();

    cv::Mat boat_meas = cv::Mat::zeros(cv::Size(MEAS_SIZE, 1), CV_32FC1);
    boat_meas.at<float>(0) = odometry.x;
    boat_meas.at<float>(1) = odometry.y;
    boat_meas.at<float>(2) = odometry.z;
    
    cv::Mat estimation = KF.correct(boat_meas);

OpenCV Error: Assertion failed (a_size.width == len) in gemm, file /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/core/src/matmul.cpp, line 1537
terminate called after throwing an instance of 'cv::Exception'
  what():  /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/core/src/matmul.cpp:1537: error: (-215) a_size.width == len in function gemm

Aborted
2

please try again with a recent version. if this is a bug, in v3.2.0, it might be fixed. then any time spent on this would be wasted.

3

however, one of them still has the wrong shape

debug it, get a backtrace (bt in gdb) , then you know, which of (sadly a lot)
the matrix multiplications to check.

and maybe you can improve the code example above, to make it reproducable for others

4

I hate that I have to use ROS for this project and I’m very uncomfortable with it. I have no clue on how to use backtrace with it. I also don’t think I can update openCV. I know that the gazebo simulator I’m used needs ros melocic to run, on noetic it doesn’t behave as expected. I didn’t create this model, I just have to get the position from it and work on object tracking.

However, I tried printing the matrix sizes (with size()) and content.

Kalman Filter Init → dt = 0.322711 , measurements: [12.9959, -11.8709, -0.0850451]
predict 1 > [0, 0, 0] → this is the return of KF.predict()

KF.statePre: [1 x 6]
[0;
 0;
 0;
 0;
 0;
 0]

KF.statePost: [1 x 6]
[0;
 0;
 0;
 0;
 0;
 0]

KF.transitionMatrix: [6 x 6]
[1, 0, 0, 0.32271138, 0, 0;
 0, 1, 0, 0, 0.32271138, 0;
 0, 0, 1, 0, 0, 0.32271138;
 0, 0, 0, 1, 0, 0;
 0, 0, 0, 0, 1, 0;
 0, 0, 0, 0, 0, 1]

KF.controlMatrix: [3 x 6]
[0.052071314, 0, 0;
 0, 0.052071314, 0;
 0, 0, 0.052071314;
 0.32271138, 0, 0;
 0, 0.32271138, 0;
 0, 0, 0.32271138]

KF.measurementMatrix: [6 x 3]
[1, 0, 0, 0, 0, 0;
 0, 1, 0, 0, 0, 0;
 0, 0, 1, 0, 0, 0]

KF.processNoiseCov: [6 x 6]
[0.00027114217, 0, 0, 0.0016804005, 0, 0;
 0, 0.00027114217, 0, 0, 0.0016804005, 0;
 0, 0, 0.00027114217, 0, 0, 0.0016804005;
 0.0016804005, 0, 0, 0.010414262, 0, 0;
 0, 0.0016804005, 0, 0, 0.010414262, 0;
 0, 0, 0.0016804005, 0, 0, 0.010414262]

KF.measurementNoiseCov: [3 x 3]
[0.1, 0, 0;
 0, 0.1, 0;
 0, 0, 0.1]

KF.errorCovPost: [6 x 6]
[1.1044137, 0, 0, 0.32439178, 0, 0;
 0, 1.1044137, 0, 0, 0.32439178, 0;
 0, 0, 1.1044137, 0, 0, 0.32439178;
 0.32439178, 0, 0, 1.0104142, 0, 0;
 0, 0.32439178, 0, 0, 1.0104142, 0;
 0, 0, 0.32439178, 0, 0, 1.0104142]

KF.errorCovPre: [6 x 6]
[1.1044137, 0, 0, 0.32439178, 0, 0;
 0, 1.1044137, 0, 0, 0.32439178, 0;
 0, 0, 1.1044137, 0, 0, 0.32439178;
 0.32439178, 0, 0, 1.0104142, 0, 0;
 0, 0.32439178, 0, 0, 1.0104142, 0;
 0, 0, 0.32439178, 0, 0, 1.0104142]

KF.gain: [3 x 6]
[0, 0, 0;
 0, 0, 0;
 0, 0, 0;
 0, 0, 0;
 0, 0, 0;
 0, 0, 0]

OpenCV Error: Assertion failed (C.type() == type && (((flags&GEMM_3_T) == 0 && C.rows == d_size.height && C.cols == d_size.width) || ((flags&GEMM_3_T) != 0 && C.rows == d_size.width && C.cols == d_size.height))) in gemm, file /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/core/src/matmul.cpp, line 1560
terminate called after throwing an instance of 'cv::Exception'
  what():  /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/core/src/matmul.cpp:1560: error: (-215) C.type() == type && (((flags&GEMM_3_T) == 0 && C.rows == d_size.height && C.cols == d_size.width) || ((flags&GEMM_3_T) != 0 && C.rows == d_size.width && C.cols == d_size.height)) in function gemm