This is something I've been trying to work out for a while - I'm trying to go from a 2D x,y,w homogeneous transform matrix (eg. Android's graphics.matrix) and turn it into a 3D x,y,z,w transform matrix (eg. Android's opengl.matrix)
Convert an android.graphics.Matrix to a GL mat4? seems to be a similar question but the answer only applies to affine transformations while I need to handle perspective transformations in x and y. This also seems like something more general than a Android specific implementation.
I've tried making a system of equations to solve for what the 3D matrix values need to be such that for five points, the results for X and Y equal their value after the 2D transformation is applied and Z always equals one, but if the Z value is always left the same (what I want to happen) then the matrix is not invertible.
import numpy as np
# arbitrary points to sample
# need 5 points to have 5 * 3d = 15 equations to solve for 15 matrix coefficents
x0 = -1
y0 = 1
z0 = 1
u0 = -1
v0 = 1
w0 = 1
x1 = 1
y1 = 1
z1 = 1
u1 = 1
v1 = 1
w1 = 1
x2 = 1
y2 = -1
z2 = 1
u2 = 1
v2 = -1
w2 = 1
x3 = -1
y3 = -1
z3 = 1
u3 = -1
v3 = -1
w3 = 1
x4 = 0
y4 = 0
z4 = 1
u4 = 0
v4 = 0
w4 = 1
# The following numbers seem to allow the calculation to finish but are not what i want
# x4 = 21
# y4 = 15
# z4 = 36
# u4 = 12
# v4 = 21
# w4 = 31
# matrix made by extending https://stackoverflow.com/a/57280136 into 3d
A = np.matrix([
[x0, y0, z0, 1, 0, 0, 0, 0, 0, 0, 0, 0, -x0*u0, -y0*u0, -z0*u0],
[x1, y1, z1, 1, 0, 0, 0, 0, 0, 0, 0, 0, -x1*u1, -y1*u1, -z1*u1],
[x2, y2, z2, 1, 0, 0, 0, 0, 0, 0, 0, 0, -x2*u2, -y2*u2, -z2*u2],
[x3, y3, z3, 1, 0, 0, 0, 0, 0, 0, 0, 0, -x3*u3, -y3*u3, -z3*u3],
[x4, y4, z4, 1, 0, 0, 0, 0, 0, 0, 0, 0, -x4*u4, -y4*u4, -z4*u4],
[0, 0, 0, 0, x0, y0, z0, 1, 0, 0, 0, 0, -x0*v0, -y0*v0, -z0*v0],
[0, 0, 0, 0, x1, y1, z1, 1, 0, 0, 0, 0, -x1*v1, -y1*v1, -z1*v1],
[0, 0, 0, 0, x2, y2, z2, 1, 0, 0, 0, 0, -x2*v2, -y2*v2, -z2*v2],
[0, 0, 0, 0, x3, y3, z3, 1, 0, 0, 0, 0, -x3*v3, -y3*v3, -z3*v3],
[0, 0, 0, 0, x4, y4, z4, 1, 0, 0, 0, 0, -x4*v4, -y4*v4, -z4*v4],
[0, 0, 0, 0, 0, 0, 0, 0, x0, y0, z0, 1, -x0*w0, -y0*w0, -z0*w0],
[0, 0, 0, 0, 0, 0, 0, 0, x1, y1, z1, 1, -x1*w1, -y1*w1, -z1*w1],
[0, 0, 0, 0, 0, 0, 0, 0, x2, y2, z2, 1, -x2*w2, -y2*w2, -z2*w2],
[0, 0, 0, 0, 0, 0, 0, 0, x3, y3, z3, 1, -x3*w3, -y3*w3, -z3*w3],
[0, 0, 0, 0, 0, 0, 0, 0, x4, y4, z4, 1, -x4*w4, -y4*w4, -z4*w4]
])
print(A)
print(np.linalg.det(A)) # zero
b = np.array([u0, u1, u2, u3, u4, v0, v1, v2, v3, v4, w0, w1, w2, w3, w4])
c = np.linalg.solve(A, b) # crashes here
mat3d = np.matrix([
[c[0], c[1], c[2], c[3]],
[c[4], c[5], c[6], c[7]],
[c[8], c[9], c[10], c[11]],
[c[12], c[13], c[14], 1]
])
print(mat3d)
Is there a way to reliably extend a 2D homogeneous coordinates matrix to a 3D one?
Thanks!
If I understand your question, have a 3x3 matrix M1 and you want a 4x4 matrix M2, such that:
Wherever M1[x1,y1,w1] = [x2,y2,w2]...
You want M2[x1,y1,0,w1] = [x3,y3,z3,w3]...
Such that x2/w2 = x3/w3, y2/w2 = y3/w3, z3/w3 = 1
The easiest way is just to extend the matrix so that x3 = x2, y3 = y2, z3 = w2, w3 = w2. To do that you just add a z column that is the same as the w column, and a z row that is all zeros
Related
I wrote a kalman Filter implementation using the Eigen Library in C++ and also using the implementation at this link to test my filter: My prediction step looks like this:
void KalmanFilter::Predict()
{
// state Estimate = state transition matrix * previous state
// No control input present.
x = A * x;
// State Covariance Matrix = (State Transition Matrix * Previous State
Covariance matrix * (State Transition Matrix)^T ) + Process Noise
P = A * P * A.transpose() + Q;
}
while my update step is:
void KalmanFilter::Update(VectorXd z)
{
//Kalman Gain = (State Covariance Matrix * Measurement matrix.transpose) * (H*P*H^T + Measurement Noise)^-1
K = (P * H.transpose()) * (H * P * H.transpose()+ R).inverse();
//Estimated Stated = Estimated state + Kalman Gain (Measurement Innovation)
x = x + K*(z - H * x);
//State Covariance matrix = (Identity Matrix of the size of x.size * x.size) - K* H * P;
long x_size = x.size();
MatrixXd I = MatrixXd::Identity(x_size, x_size);
P = (I - K * H) * P ;
}
My initial values are:
pos_x = 0.0;
pos_y = 0.0;
pos_z = 1.0;
vel_x = 10.0;
vel_y = 0.0;
vel_z = 0.0;
acc_x = 0.0;
acc_y = 0.0;
acc_z = -9.81;
and I'm generating "fake data" by doing the following in a loop:
double c = 0.1; // Drag resistance coefficient
double damping = 0.9 ; // Damping
double sigma_position = 0.1 ; // position_noise
// Create simulated position data
for (int i = 0; i < N; i ++)
{
acc_x = -c * pow(vel_x, 2); // calculate acceleration ( Drag Resistance)
vel_x += acc_x * dt; // Integrate acceleration to give you velocity in the x axis.
pos_x += vel_x * dt; // Integrate velocity to return the position in the x axis
acc_z = -9.806 + c * pow(vel_z, 2); // Gravitation + Drag
vel_z += acc_z * dt; // z axis velocity
pos_z += vel_z * dt; // position in z axis
// generate y position here later.
if(pos_z < 0.01)
{
vel_z = -vel_z * damping;
pos_z += vel_z * dt;
}
if (vel_x < 0.1)
{
acc_x = 0.0;
acc_z = 0.0;
}
// add some noise
pos_x = pos_x + sigma_position * process_noise(generator);
pos_y = pos_y + sigma_position * process_noise(generator);
pos_z = pos_z + sigma_position * process_noise(generator);
I then run my prediction and update step by:
// Prediction Step
kalmanFilter.Predict();
// Correction Step
kalmanFilter.Update(z);
where z is a 3 x 1 vector containing pos_x, pos_y and pos_z
My State Transition Matrix A looks like this:
A << 1, 0, 0, dt, 0, 0, dt_squared, 0 , 0,
0, 1, 0, 0, dt, 0, 0, dt_squared, 0,
0, 0, 1, 0, 0, dt, 0, 0, dt_squared,
0, 0, 0, 1, 0, 0, dt, 0, 0,
0, 0, 0, 0, 1, 0, 0 , dt, 0,
0, 0, 0, 0, 0, 1, 0, 0, dt,
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;
where dt_squared is (dt * dt) /2;
P is
P<< 100, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100;
and
R << 1, 0, 0,
0, 1, 0,
0, 0, 1;
and
Q = G * G.transpose()* a * a;
where G is a 9 x 1 Matrix of
G << dt_squared, dt_squared, dt_squared, dt, dt, dt, 1, 1, 1;
a = 0.1 //( acceleration process noise)
My issue is my estimated position for y and z are off and diverge from the "real" positions. If you look at the following graphs,
This is what pos_x looks like:
This is what pos_y looks like:
And finally Z:
This is my first foray with Kalman filters and I'm not sure what I'm doing wrong here. My final goal would be to use this to estimate the position of a drone. Additionally, I have the following questions:
In a real life situation for a drone for example, how do you about choosing your Process Noise if you can't directly observe the process? Do you simply just pick arbitrary values?
My apologies for the long post. Any help is appreciated.
I am not sure if it's a code related issue, an algorithm implementation issue, or an expectation issue.
You do realize that a filter like this will NOT reproduce truth data, or even anything close to it, if the fake data has too much maneuvering in it.
Also, your graphs are not present.
I know my response doesn't follow community standards but I cannot comment or I'd do that.
Until you provide the plots and check the curvature of the path against the update rate I would not attempt to go into detail. Also filters need to be "tuned" to a specific system. You may need to play with noise parameters to tune it better. For maneuvering tracks one may need to go to higher order filters, Singer, or Jerk filters... The filter needs to model the system well enough. Based on your update matrix it looks like you have a parabolic (second order) estimate. You may also want to ask about this in other forums that are not s/w or code specific.
Every System has variances. Let's say the Filter has a variance of +-1% and the real value has +-5%; If you predict a value you have to make a choice for the update to use the predicted or the meassures value. Depending on which one you believe more.
Otherwise your filter does develop always based on it's own values...
I am new and have an issue in C++. I am creating 3D coordinates, (x, y and z for each corner of a face, then 6 faces) and receive many errors. Here is my code:
#include <vector>
int main()
{
std::vector<int> xyzCoords = {
{ 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, 0, 0 }
};
int x1 = 0;
int y1 = 1;
int z1 = 2;
int x2 = 3;
int y2 = 4;
int z2 = 5;
xyzCoords = {
{ x1, y1, z1, x2, y1, z1, x2, y1, z2, x1, y1, z2 },
{ x1, y2, z1, x2, y2, z1, x2, y2, z2, x1, y2, z2 },
{ x1, y2, z1, x1, y1, z1, x1, y1, z2, x1, y2, z2 },
{ x2, y2, z1, x2, y1, z1, x2, y1, z2, x2, y2, z2 },
{ x1, y2, z2, x1, y1, z2, x2, y1, z2, x2, y2, z2 },
{ x1, y2, z1, x1, y1, z1, x2, y1, z1, x2, y2, z1 }
};
return 0;
}
This the code where the problem occurs. You can see I'm defining xyzCoords as a vector. I'm not sure if this is the correct way to do it. I also don't want to be defining xyz123 separately. What is the best way to achieve this? Shall I use a list, array or vector? Please write the code for how to do this. Thanks!
Errors:
E0289: no instance of constructor "std::vector<_Ty, _Alloc>::vector [with _Ty=int, _Alloc=std::allocator]" matches the argument list
E0349: no operator "=" matches these operands
C2440: 'initializing': cannot convert from 'initializer list' to 'std::vector>'
C2679: binary '=': no operator found which takes a right-hand operand of type 'initializer list' (or there is no acceptable conversion)
Your initialization is unmatch with the type you had declared. You declare a 1D array whereas your initialization is 2D array.
std::vector<int> xyzCoords = {
{ 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, 0, 0 }
};
It should be :
std::vector<std::vector<int>> xyzCoords = {
{ 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, 0, 0 }
};
You mentioned about xyz coordinate, so why don't you make a class specially used for coordinate, for instance :
struct Point {
int x, y, z;
};
Anyway, I don't recommend you to use naive approach for this problem because it will be a matrix-operation-intensive computation. You should use BLAS/LAPACK library or it's wrapper like Eigen or Armadillo. They are surely far faster.
Perhaps you should restructure your code to something like this, for visibility and better understanding:
#include <vector>
int main()
{
struct Corner
{
int x;
int y;
int z;
Corner(int x, int y, int z) :
x(x), y(y), z(z) {}
};
struct Face
{
Corner c1;
Corner c2;
Corner c3;
Corner c4;
Face(Corner c1, Corner c2, Corner c3, Corner c4) :
c1(c1), c2(c2), c3(c3), c4(c4) {}
};
Corner c1(0, 1, 2);
Corner c2(3, 4, 5);
Face f1(c1, c2, c1, c2);
std::vector<Face> faces = { f1, f1, f1, f1, f1, f1 };
return 0;
}
This creates array of 6 faces with coordinates of the corners (x1,y1,z1) (x2,y2,z2) (x1,y1,z1) (x2,y2,z2)
#include <vector>
struct Vertex {
float x_; // can change these to int
float y_;
float z_;
Vertex() : x_(0), y_(0), z_(0) {}
Vertex( float x, float y, float z ) : x_(x), y_(y), z_(z) {}
explicit Vertex( float val ) : x_(val), y_(val), z_(val) {}
};
struct Face {
Vertex v0_;
Vertex v1_;
Vertex v2_;
Vertex v3_;
Face() :
v0_(Vertex()),
v1_(Vertex()),
v2_(Vertex()),
v3_(Vertex()) {
}
Face( Vertex v0, Vertex v1, Vertex v2, Vertex v3 ) :
v0_(v0),
v1_(v1),
v2_(v2),
v3_(v3) {
}
};
int main() {
std::vector<Face> faces;
// Not exact values the OP is looking for,
// just a quick way to populate the vector of faces
// for demonstration purposes of the usefulness of structs & classes.
for ( unsigned n = 1; n <= 6; n++ ) {
faces.push_back( Face( Vertex(n), Vertex(n+1), Vertex(n+2), Vertex(n+3) ) );
}
return 0;
}
I try to understand a code of mirror, it contains this matrix transformation:
matrix[0][0] = -matrix[0][0];
matrix[1][0] = -matrix[1][0];
matrix[2][0] = -matrix[2][0];
matrix[3][0] = -matrix[3][0];
The first think I have this is:
matrix.tra() matrix.scale(-1,1,1); matrix.tra()
But GL doesn't have commands for transpose of matrix and it's not possible to execute these commands. I want to understand how to use it.
What kind of matrix transformation can be in this code?
Below I will write more code to provide more details.
md3_vox_calcmat_common(tspr, dvoxa0, f, matrix);
if (grhalfxdown10x < 0) {
matrix[0][0] = -matrix[0][0];
matrix[1][0] = -matrix[1][0];
matrix[2][0] = -matrix[2][0];
matrix[3][0] = -matrix[3][0];
}
matrix[0][3] = matrix[1][3] = matrix[2][3] = 0.f;
matrix[3][3] = 1.f;
gl.bglMatrixMode(GL_MODELVIEW);
// Let OpenGL (and perhaps hardware) handle the matrix rotation
gl.bglLoadMatrixf(matrix);
//calcmat_common is:
mat.idt();
mat.rotate(0.0f, 0.0f, -1.0f, roll);
mat.rotate(-1.0f, 0.0f, 0.0f, pitch);
mat.rotate(0.0f, -1.0f, 0.0f, yaw);
mat.scale(-1 / 16f, 1.0f, 1 / 16f);
mat.translate(a0.y, a0.z, a0.x);
mat.rotate(0.0f, -1.0f, 0.0f, spriteang);
Let's do some algebra:
A = I, J, K, L
M, N, O, P
Q, R, S, T
U, V, W, X
C = -I,-J,-K,-L
M, N, O, P
Q, R, S, T
U, V, W, X
Let
B = -1, 0, 0, 0
0, 1, 0, 0
0, 0, 1, 0
0, 0, 0, 1
Then
B x A = C
Further:
C'= -I,-J,-K,-L
M, N, O, P
Q, R, S, T
0, 0, 0, 1
Let
B'= -1, 0, 0, 0
0, 1, 0, 0
0, 0, 1, 0
0, 0, 0, 0
D = 0, 0, 0, 0
0, 0, 0, 0
0, 0, 0, 0
0, 0, 0, 1
Then
B'x A + D = C'
When i try:
__m256 a = _mm256_set_ps(1, 1, 1, 1, 1, 1, 1, 1);
__m256 b = _mm256_set_ps(0, 0, 0, 0, 0, 0, 0, 0);
__m256 c = _mm256_cmp_ps(a, b, _CMP_LT_OQ);
Which is a < b I get the output:
[0, 0, 0, 0, 0, 0, 0, 0]
But when trying:
__m256 a = _mm256_set_ps(1, 1, 1, 1, 1, 1, 1, 1);
__m256 b = _mm256_set_ps(0, 0, 0, 0, 0, 0, 0, 0);
__m256 c = _mm256_cmp_ps(b, a, _CMP_LT_OQ);
or
__m256 a = _mm256_set_ps(1, 1, 1, 1, 1, 1, 1, 1);
__m256 b = _mm256_set_ps(0, 0, 0, 0, 0, 0, 0, 0);
__m256 c = _mm256_cmp_ps(a, b, _CMP_GT_OQ);
I get
[NaN, NaN, NaN, NaN, NaN, NaN, NaN, NaN]
is this expected behaviour? The documentation at https://software.intel.com/en-us/node/524077 just says that it returns the result without specifying.
Yes, the returned value is a bitmask: it is set to all zeroes for false, or all ones for true. 32 bits of ones happen to be encoding of NaN when interpreted as a 32-bit float.
Bitmasks are useful because you can use them to mask out some results, e.g. (A & M) | (B & ~M) will select the value of A when the mask M was true (all ones) and the value of B when the mask was false (all zeroes).
So I'm trying to place an image into a scene using opencv. I can rotate it about its centre (z axis) using GetRotationMatrix2d and warpaffine. I'm wondering to I need to use a perspective transform? Or can I just create a 3d rotation matrix and plug it into warpaffine? And is there any getRotationMatrix equivalent for 3d?
Thanks!
Edit: I found this post but it does not work for me.
My code is below And I have gotten it to rotate, although I'm not sure what focal length I should be using? everytime I rotate about a different angle I need to play around with the focal length to get it right. Say if I rotate my image 45' it looks like the pic below but that's clearly warped off in some way...
void rotateImage(const Mat &input, Mat &output, double alpha, double beta, double gamma, double dx, double dy, double dz, double f)
{
alpha = (alpha - 90.)*CV_PI/180.;
beta = (beta - 90.)*CV_PI/180.;
gamma = (gamma - 90.)*CV_PI/180.;
// get width and height for ease of use in matrices
double w = (double)input.cols;
double h = (double)input.rows;
// Projection 2D -> 3D matrix
Mat A1 = (Mat_<double>(4,3) <<
1, 0, -w/2,
0, 1, -h/2,
0, 0, 0,
0, 0, 1);
// Rotation matrices around the X, Y, and Z axis
Mat RX = (Mat_<double>(4, 4) <<
1, 0, 0, 0,
0, cos(alpha), -sin(alpha), 0,
0, sin(alpha), cos(alpha), 0,
0, 0, 0, 1);
Mat RY = (Mat_<double>(4, 4) <<
cos(beta), 0, -sin(beta), 0,
0, 1, 0, 0,
sin(beta), 0, cos(beta), 0,
0, 0, 0, 1);
Mat RZ = (Mat_<double>(4, 4) <<
cos(gamma), -sin(gamma), 0, 0,
sin(gamma), cos(gamma), 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
// Composed rotation matrix with (RX, RY, RZ)
Mat R = RX * RY * RZ;
// Translation matrix
Mat T = (Mat_<double>(4, 4) <<
1, 0, 0, dx,
0, 1, 0, dy,
0, 0, 1, dz,
0, 0, 0, 1);
// 3D -> 2D matrix
Mat A2 = (Mat_<double>(3,4) <<
f, 0, w/2, 0,
0, f, h/2, 0,
0, 0, 1, 0);
// Final transformation matrix
Mat trans = A2 * (T * (R * A1));
// Apply matrix transformation
warpPerspective(input, output, trans, input.size(), INTER_LANCZOS4);
}
Does translation work? Try the following code instead.
Mat T = (Mat_<double>(4, 4) <<
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
dx, dy, dz, 1);