Kalman filter 3D implementation - opencv

I want to implement the kalman filter for a moving object in r3 (X,Y,Z-coordinate) in OpenCV.
I tried to understand the OpenCV documentation but this is really not helpful and very rare.
The syntax for the initialization is:
KalmanFilter::KalmanFilter ( int dynamParams, int measureParams, int
controlParams = 0, int type = CV_32F )
In my case, is dynamParams = 9 and measureParams=3?
And what is the transitionMatrix in my case?


In that case the Transition Matrix A looks like:
A = [1, 0, 0, v, 0, 0, a, 0, 0;
0, 1, 0, 0, v, 0, 0, a, 0;
0, 0, 1, 0, 0, v, 0, 0, a;
0, 0, 0, 1, 0, 0, v, 0, 0;
0, 0, 0, 0, 1, 0, 0, v, 0;
0, 0, 0, 0, 0, 1, 0, 0, v;
0, 0, 0, 0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 0, 0, 0, 1, 0;
0, 0, 0, 0, 0, 0, 0, 0, 1]
With
v = dt
a = 0.5*dt^2
See http://campar.in.tum.de/Chair/KalmanFilter


I found out, that for the 3D-Case often the position, velocity and acceleration is used. That means, that for the OpenCV implementation dynamParams=9 and measureParams=3 is right.

Related

How do I make Dartfmt "friendly" for Matrices?

For a Matrix that supposed to look like this:
const ColorFilter sepia = ColorFilter.matrix(<double>[
0.393, 0.769, 0.189, 0, 0,
0.349, 0.686, 0.168, 0, 0,
0.272, 0.534, 0.131, 0, 0,
0, 0, 0, 1, 0,
]);
But dartfmt changed it to become like this:
const ColorFilter sepia = ColorFilter.matrix(<double>[
0.393,
0.769,
0.189,
0,
0,
0.349,
0.686,
0.168,
0,
0,
0.272,
0.534,
0.131,
0,
0,
0,
0,
0,
1,
0,
]);
This is hard to read. Thus, how can I keep the original format so that the Matrix can be seen more "friendly". Or at least how can I make Dartfmt not to reformat any List?

This scenario is described in the FAQ of dart_style which dartfmt is based on:
https://github.com/dart-lang/dart_style/wiki/FAQ#why-does-the-formatter-mess-up-my-collection-literals
In short, you just need to add a comment somewhere in you matrix definition like:
const ColorFilter sepia = ColorFilter.matrix(<double>[
0.393, 0.769, 0.189, 0, 0, //
0.349, 0.686, 0.168, 0, 0,
0.272, 0.534, 0.131, 0, 0,
0, 0, 0, 1, 0,
]);
Then dartfmt will not try to format the newlines in the matrix. It will however, still fixes non-needed spaces so it will make your example into:
const ColorFilter sepia = ColorFilter.matrix(<double>[
0.393, 0.769, 0.189, 0, 0, //
0.349, 0.686, 0.168, 0, 0,
0.272, 0.534, 0.131, 0, 0,
0, 0, 0, 1, 0,
]);
Which can be fixed by changing the 0 to 0.000:
const ColorFilter sepia = ColorFilter.matrix(<double>[
0.393, 0.769, 0.189, 0, 0, //
0.349, 0.686, 0.168, 0, 0,
0.272, 0.534, 0.131, 0, 0,
0.000, 0.000, 0.000, 1, 0,
]);

Whereas #julemand101's answer should be the accepted one, as it directly changes dartfmt's behavior, this spread operator workaround could be worth to mention :
final matrix = [
...[1, 2, 3],
...[4, 5, 6],
...[7, 8, 9],
];

Understanding spacer/z3 unsat proof

We use spacer as a backend CHC solver and want to get proof when the result is unsat.
I think that the proof is sufficient to construct a counterexample(a graph reachable to a bad state) but spacer's output is difficult to understand for me. There are some unfamiliar words like asserted or hyper-res.
I couldn't find any document to understand that output. If there is any, please tell me.
I'm really happy if I can construct a counterexample similar to Eldarica's one[0].
For example,
0: FALSE -> 1
1: main_h_30(0, 0, 0, 0, 0, 1, 0, 0, -1, 0, 0, 1) -> 2
2: main_h_28(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1) -> 3
3: main_h_24(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1) -> 4
4: main_h_22(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1) -> 5
5: main_h_21(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1) -> 6
6: main_h_18(0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1) -> 7
7: main_h_16(0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1) -> 8
8: main_h_13(0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1) -> 9
9: main_h_11(0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1) -> 10
10: main_h_9(0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1) -> 11
11: main_h_8(0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1) -> 12
12: main_h_7(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1) -> 13
13: main_h_6(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1) -> 14
14: main_h_5(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1) -> 15
15: main_h_4(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1) -> 16
16: main_h_3(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1) -> 17
17: main_h_2(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1) -> 18
18: main_h_1(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1)
[0] http://logicrunch.it.uu.se:4096/~wv/eldarica/?ex=Prolog%2Flistcounter.unsat

OpenCV, how can we normalize a Mat min to max and max to min?

I want to normalize a Mat to the min value goes to 255 and max goes to 0 (normalize the Mat between 0~255).
For example, if we have an array like [0.02, 0.002, 0.0002] after normalization I want to get a result like this: [3, 26, 255], but now when I am using NORM_MINMAX I got [255, 26, 3].
But I did not find any function to do the inverted operation of the NORM_MINMAX.
Code used:
cv::Mat mat(10, 10, CV_64F);
mat.setTo(0);
mat.row(0) = 0.02;
mat.row(1) = 0.002;
mat.row(2) = 0.0002;
cv::normalize(mat, mat, 255, 0, cv::NORM_MINMAX);
mat.convertTo(mat, CV_8UC1);
std::cout << mat << std::endl;
Result is:
[255, 255, 255, 255, 255, 255, 255, 255, 255, 255;
26, 26, 26, 26, 26, 26, 26, 26, 26, 26;
3, 3, 3, 3, 3, 3, 3, 3, 3, 3;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
But I want the inverse of the above result.
Update: When I subtract 255 from the mat like:
cv::subtract(255, mat, mat, mat); // the last mat acts as mask
std::cout << mat << std::endl;
Result is:
[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
229, 229, 229, 229, 229, 229, 229, 229, 229, 229;
252, 252, 252, 252, 252, 252, 252, 252, 252, 252;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

I finally found the way how to calculate, below are the steps:
By using inverse proportions formula, we can easily calculate the inverse of the NORM_MINMAX
x = a*b/c
Where the a= min value of the mat element, b=255 (max value), and c= the element which we want to calculate it.
cv::Mat mat(10, 10, CV_64F);
mat.setTo(0);
mat.row(0) = 0.02;
mat.row(1) = 0.002;
mat.row(2) = 0.0002;
std::cout << mat<< std::endl;
// craete a mask
cv::Mat mask(mat.size(), CV_8U);
mask.setTo(0);
mask.row(0) = 255;
mask.row(1) = 255;
mask.row(2) = 255;
// find the min value
double min;
cv::minMaxLoc(mat, &min, nullptr, nullptr, nullptr, mask);
std::cout << "min=" << min << std::endl;
// unfortunately opencv divide operation does not support mask, so we need some extra steps to perform.
cv::Mat result, maskNeg;
cv::divide(min*255, mat, result); // this is the magic line
cv::bitwise_not(mask, maskNeg);
mat.copyTo(result, maskNeg);
std::cout << result << std::endl;
// convert to 8bit
result .convertTo(result , CV_8UC1);
std::cout << "the final result:" << std::endl;
std::cout << temp << std::endl;
And the outputs:
original mat
[0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02;
0.002, 0.002, 0.002, 0.002, 0.002, 0.002, 0.002, 0.002, 0.002, 0.002;
0.0002, 0.0002, 0.0002, 0.0002, 0.0002, 0.0002, 0.0002, 0.0002, 0.0002, 0.0002;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
min=0.0002
the calculated min-max
[2.55, 2.55, 2.55, 2.55, 2.55, 2.55, 2.55, 2.55, 2.55, 2.55;
25.5, 25.5, 25.5, 25.5, 25.5, 25.5, 25.5, 25.5, 25.5, 25.5;
255, 255, 255, 255, 255, 255, 255, 255, 255, 255;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
the final result:
[ 3, 3, 3, 3, 3, 3, 3, 3, 3, 3;
26, 26, 26, 26, 26, 26, 26, 26, 26, 26;
255, 255, 255, 255, 255, 255, 255, 255, 255, 255;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
Yes, It is what I want.

How to understand the math behind the CTM(current transformation matrix)?

When I written some test code about modifying CTM, I found it can't be explain with The Math Behind the Matrices in Quartz 2D Programming Guide. The test code is as follow:
// {a, b, c, d, tx, ty}
NSLog(#"UIKit CTM:%#\n", NSStringFromCGAffineTransform(CGContextGetCTM(ctx)));
CGContextSaveGState(ctx);
CGContextTranslateCTM(ctx, 0, CGRectGetHeight(rect));// 1
NSLog(#"Quartz part 1 CTM:%#\n", NSStringFromCGAffineTransform(CGContextGetCTM(ctx)));
CGContextScaleCTM(ctx, 1, -1);// 2
NSLog(#"Quartz CTM:%#\n", NSStringFromCGAffineTransform(CGContextGetCTM(ctx)));
CGContextTranslateCTM(ctx, 0, CGRectGetHeight(rect)); // 3
NSLog(#"UIKit part 1 CTM:%#\n", NSStringFromCGAffineTransform(CGContextGetCTM(ctx)));
CGContextScaleCTM(ctx, 1, -1);// 4
NSLog(#"UIKit part 2 CTM:%#\n", NSStringFromCGAffineTransform(CGContextGetCTM(ctx)));
CGContextRestoreGState(ctx);
The output:
2017-09-29 09:51:27.166 QuartzDemo[53287:31120880] UIKit CTM:[2, 0, 0, -2, 0, 1136]
2017-09-29 09:51:27.167 QuartzDemo[53287:31120880] Quartz part 1 CTM:[2, 0, 0, -2, 0, 0]
2017-09-29 09:51:27.167 QuartzDemo[53287:31120880] Quartz CTM:[2, 0, -0, 2, 0, 0]
2017-09-29 09:51:27.167 QuartzDemo[53287:31120880] UIKit part 1 CTM:[2, 0, -0, 2, 0, 1136]
2017-09-29 09:51:27.167 QuartzDemo[53287:31120880] UIKit part 2 CTM:[2, 0, 0, -2, 0, 1136]
First, let's focus on UIKit CTM transforms to Quartz CTM, we use array express matrix, on line 1:
[2, 0, 0, 0, -2, 0, 0, 1136, 1] x [1, 0, 0, 0, 1, 0, tx1, ty1, 1] = [2, 0, 0, 0, -2, 0, 0, 0, 1]
then
[1, 0, 0, 0, 1, 0, tx1, ty1, 1] = [1, 0, 0, 0, 1, 0, -1136, 1]
so in this case CGContextTranslateCTM(ctx, 0, CGRectGetHeight(rect)); equal to [1, 0, 0, 0, 1, 0, -1136, 1], Question 1: Where is minus sign come from?
On line 2:
[2, 0, 0, 0, -2, 0, 0, 0, 1] x [sx1, 0, 0, 0, sy1, 0, 0, 0, 1] = [2, 0, 0, -0, 2, 0, 0, 0, 1]
then
[sx1, 0, 0, 0, sy1, 0, 0, 0, 1] = [1, 0, 0, 0, -1, 0, 0, 0, 1]
so CGContextScaleCTM(ctx, 1, -1); equal to [1, 0, 0, 0, -1, 0, 0, 0, 1], this result is the same as theory value.
On line 3:
[2, 0, 0, -0, 2, 0, 0, 0, 1] x [1, 0, 0, 0, 1, 0, tx2, ty2, 1] = [2, 0, 0, -0, 2, 0, 0, 1136, 1]
then
[1, 0, 0, 0, 1, 0, tx2, ty2, 1] = [2, 0, 0, 0, 2, 0, 0, 1136, 1]
this result also the same as theory value.
On line 4:
[2, 0, 0, 0, 2, 0, 0, 1136, 1] x [sx2, 0, 0, 0, sy2, 0, 0, 0, 1] = [2*sx2, 0, 0, 0, 2*sy2, 0, 0, 1136*sy2, 1]
then
[2*sx2, 0, 0, 0, 2*sy2, 0, 0, 1136*sy2, 1] = [2, 0, 0, 0, -2, 0, 0, 1136, 1]
2*sx2 = 2 => sx2 = 1; but
2*sy2 = -2 (1)
1136*sy2 = 1136 (2)
In equation (1) sy2 = -1, but in equation (2) sy2 = 1, So Question 2: Why this happened? How to explain this case?

OpenCV arithmetic operations on matrices

Can someone explain the following, totally unintuitive, results:
Mat_<Vec3f> mat(Size(3,3),0);
Mat_<Vec3f> mat_add = (mat + 9);
Mat_<Vec3f> mat_add_div = (mat + 9) / 3;
magically, I don't have any other explanation for it:
mat_add = [9,0,0,9,0,0,9,0,0];
mat_add_div = [3,3,3,3,3,3,3,3,3];
EDIT:
My take: this is a legacy bug that can't be fixed anymore because it is retroactive and will impact lot of projects. Also it would be pretty nasty to debug (except project has already pretty thorough unit testing)

Mat_<Vec3f> mat_add = (mat + 9);
is equivalent to
Mat_<Vec3f> temp(Size(3,3),Vec3f(9,0,0));
Mat_<Vec3f> mat_add = mat+temp;
So, you will get
mat_add =
[9, 0, 0, 9, 0, 0, 9, 0, 0;
9, 0, 0, 9, 0, 0, 9, 0, 0;
9, 0, 0, 9, 0, 0, 9, 0, 0]
However, I have no clue for why you will get values like that for mat_add_div. In fact, if you replace it by:
Mat_<Vec3f> mat_add_div = mat_add / 3;
You will end up with
mat_add_div =
[3, 0, 0, 3, 0, 0, 3, 0, 0;
3, 0, 0, 3, 0, 0, 3, 0, 0;
3, 0, 0, 3, 0, 0, 3, 0, 0]
This result is however reasonable based on the same theory as said in above.

Resources