Hey,
I try to build a kalman filter and get runtime errors at the step Mat prediction = KF.predict(control); in regards to the matrix types. I checked all the types of the input matrices and they are of type 5 == CV_32FC1, which should be the correct one. But I assume I make some CV related mistake that I am unaware of.
Relevant code:
#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <tf/transform_datatypes.h>
#include "localizer_mm/localizer_mm_lib.h"
using namespace cv;
using namespace localizer_mm;
cv::KalmanFilter KF;
cv::Mat measurement(3,1,CV_32FC1);
cv::Mat state(3,1,CV_32FC1); // (x, y, psi)
cv::Mat pred(3,1,CV_32FC1);
cv::Mat corr(3,1,CV_32FC1);
cv::Mat control(3,1,CV_32FC1);
bool init = true;
Pose adma_pose, adma_pose_last, odom_pose, mama_pose,result;
void initKalman(float x, float y, float psi)
{
// Initialize Kalman Filter
KF.init(3, 3, 0);
// initialize measurement, prior and posterior state
measurement = Mat::zeros(3,1,CV_32FC1);
measurement.at<float>(0,0) = x;
measurement.at<float>(1,0) = y;
measurement.at<float>(2,0) = psi;
KF.statePre.setTo(0);
KF.statePre.at<float>(0,0) = x;
KF.statePre.at<float>(1,0) = y;
KF.statePre.at<float>(2,0) = psi;
KF.statePost.setTo(0);
KF.statePost.at<float>(0,0) = x;
KF.statePost.at<float>(1,0) = y;
KF.statePost.at<float>(2,0) = psi;
// initialize A, H, Q, R, P
setIdentity(KF.transitionMatrix); // A
setIdentity(KF.measurementMatrix); // H
setIdentity(KF.processNoiseCov, Scalar::all(.005)); // Q
setIdentity(KF.measurementNoiseCov, Scalar::all(.1)); // R
setIdentity(KF.errorCovPost, Scalar::all(.1)); // P posterior
}
Mat kalmanPredict(float x, float y, float psi)
{
std::cout << "measurement = \n";
std::cout << measurement << "\n";
std::cout << "statePre = \n";
std::cout << KF.statePre << "\n";
std::cout << "statePost = \n";
std::cout << KF.statePost << "\n";
std::cout << "transitionMatrix = \n";
std::cout << KF.transitionMatrix << "\n";
std::cout << "measurementMatrix = \n";
std::cout << KF.measurementMatrix << "\n";
std::cout << "processNoiseCov = \n";
std::cout << KF.processNoiseCov << "\n";
std::cout << "measurementNoiseCov = \n";
std::cout << KF.measurementNoiseCov << "\n";
std::cout << "errorCovPost = \n";
std::cout << KF.errorCovPost << "\n";
control = Mat::zeros(3,1,CV_32FC1);
control.at<float>(0,0) = x;
control.at<float>(1,0) = y;
control.at<float>(2,0) = psi;
std::cout << "control = \n";
std::cout << control << "\n";
Mat prediction = KF.predict(control); //ERROR
std::cout << "finished prediction step\n";
return prediction;
}
Mat kalmanCorrect(float x, float y, float psi)
{
measurement.at<float>(0,0) = x;
measurement.at<float>(1,0) = y;
measurement.at<float>(2,0) = psi;
Mat correction = KF.correct(measurement);
return correction;
}
void localizerCallback(const localization_msgs::Pose2DWithCovarianceStampedConstPtr &matcher_pose)
{
ROS_INFO("entering localizerCallback callback");
// initialize Kalman Filter
if (init == true) initKalman((float)adma_pose.x,(float)adma_pose.y,(float)adma_pose.psi);
init = false;
// convert messages, [x y psi] are double values
odom_pose.x = adma_pose.x - adma_pose_last.x;
odom_pose.y = adma_pose.y - adma_pose_last.y;
odom_pose.psi = adma_pose.psi - adma_pose_last.psi;
msg2Pose(matcher_pose, mama_pose);
// apply prediction and correction step
pred = kalmanPredict((float)odom_pose.x,(float)odom_pose.y,(float)odom_pose.psi);
corr = kalmanCorrect((float)mama_pose.x,(float)mama_pose.y,(float)mama_pose.psi);
// write result
result.x = (double) corr.at<float>(0,0);
result.y = (double) corr.at<float>(1,0);
result.psi = (double) corr.at<float>(2,0);
std::cout << "final psi = " << 180*result.psi/3.14159265 << std::endl;
std::cout << "final [x_trans, y_trans] = " << "[" << result.x << ", " << result.y << "]" <<std::endl;
}
Console error:
[ INFO] [1479370325.028416744]: entering localizerCallback callback
measurement =
[511551.72; 5406469.5; -2.7221901]
statePre =
[511551.72; 5406469.5; -2.7221901]
statePost =
[511551.72; 5406469.5; -2.7221901]
transitionMatrix =
[1, 0, 0;
0, 1, 0;
0, 0, 1]
measurementMatrix =
[1, 0, 0;
0, 1, 0;
0, 0, 1]
processNoiseCov =
[0.0049999999, 0, 0;
0, 0.0049999999, 0;
0, 0, 0.0049999999]
measurementNoiseCov =
[0.1, 0, 0;
0, 0.1, 0;
0, 0, 0.1]
errorCovPost =
[0.1, 0, 0;
0, 0.1, 0;
0, 0, 0.1]
control =
[511551.72; 5406469.5; -2.7221901]
OpenCV Error: Assertion failed (type == B.type() && (type == CV_32FC1 || type == CV_64FC1 || type == CV_32FC2 || type == CV_64FC2)) in gemm, file /build/buildd/opencv-2.4.8+dfsg1/modules/core/src/matmul.cpp, line 711
terminate called after throwing an instance of 'cv::Exception'
what(): /build/buildd/opencv-2.4.8+dfsg1/modules/core/src/matmul.cpp:711: error: (-215) type == B.type() && (type == CV_32FC1 || type == CV_64FC1 || type == CV_32FC2 || type == CV_64FC2) in function gemm
Aborted (core dumped)