I want to make a dynamic matrix and assign a values specifically to its index so I have to state the size of the matrix before calling the indices, please see the following code
void Visual_Servoing::Pose_callback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
Rotations.resize(index + 3,3);
Translations.resize(3, iterator + 1);
std::cout << " #### I'm in Pose_Callback #### " << std::endl;
this->hole_detection(frame);
this->generate_line();
Translation << msg->pose.position.x ,msg->pose.position.y ,msg->pose.position.z;
Translations.col(iterator) = Translation;
std::cout << "iterator =" << iterator << std::endl;
QuatX = msg->pose.orientation.x;
QuatY = msg->pose.orientation.y;
QuatZ = msg->pose.orientation.z;
QuatW = msg->pose.orientation.w;
Rotation << (1 - (2*pow(QuatY,2)) - (2*pow(QuatZ,2))), (2*QuatX*QuatY + 2*QuatW*QuatZ) , (2*QuatX*QuatZ - 2*QuatW*QuatY),
(2*QuatX*QuatY - 2*QuatW*QuatZ) , (1 - (2*pow(QuatX,2)) - (2*pow(QuatZ,2))) , (2*QuatY*QuatZ + 2*QuatW*QuatX),
(2*QuatX*QuatZ + 2*QuatW*QuatY) , (2*QuatY*QuatZ - 2*QuatW*QuatX) , (1 - (2*pow(QuatX,2)) - (2*pow(QuatY,2)));
Rotations.block(index, 0, 3, 3) = Rotation;
std::cout << Translation << std::endl;
std::cout << Translations << std::endl;
std::cout << Rotation << std::endl;
std::cout << Rotations << std::endl;
if (iterator>10)
{
Eigen::MatrixXf VectorsCam(3, theta1.size()); // Matrix contains the vectors in camera frame
VecsinInertial.resize(3, theta1.size());
CirclePosinFrame();
for (int i=0; i<theta1.size(); i++)
{
VecsinInertial.col(i) = Translations.col(i) + Rotations.block(index, 0, 3, 3) * VectorsCam.col(i); // Each Column represents a vector in inertial frame
}
VectorCam = Eigen::MatrixXf::Zero(3, 1);
VectorsCam = Eigen::MatrixXf::Zero(3, theta1.size());
theta1.clear();
theta2.clear();
iterator = 0;
}
iterator = iterator + 1;
index = index + 3;
}
How can I update Translations and Rotations matrices sizes without loosing the already existing values in the matrices ? I already assigned Translations and Rotations in the header file as:
Eigen::MatrixXf Translations; // Matrix of Camera Translations Vectors
Eigen::MatrixXf Rotations;
Related
I've been trying to make a simple collision system for my 3D game, I'm creating a bounding sphere like this:
struct ACollSphr
{
glm::vec3* pos;
float radius;
};
And in the while loop that renders my game, in main, I give them a position like this:
for (unsigned int i = 0; i < meshModelMatrices2.size(); i++)
{
Ackerfe::ACollSphr tempSphr;
glm::vec3 *temporary = new glm::vec3(meshRenderer2.getBoundingSpherePos(*meshRenderer2.getMesh()) * glm::vec3(vec[i][12], vec[i][13], vec[i][14]));
tempSphr.pos = temporary;
radius = meshRenderer2.getBoundingSphereRadius(*meshRenderer2.getMesh(), *tempSphr.pos);
tempSphr.radius = radius;
meshSphr.push_back(tempSphr);
//std::cout << pos.x << " " << pos.y << " " << pos.z << std::endl;
//std::cout << vec[i][12] << " " << vec[i][13] << " " << vec[i][14] << std::endl;
}
where meshSphr is a vector of spheres and meshRenderer2 is the renderer I use for the meshes I'm loading in, basically I get the mesh, pass it to getBoundingSpherePos and get the position of that mesh, then I multiply it by a glm::vec3 made up of the position values inside the model matrix of each mesh and I get the radius after that and put my newly created sphere inside the meshSphr vector (which I clear after using it for the collision checks so it can get repopulated again in the next iteration)
my collision check looks like this:
for (unsigned int i = 0; i < meshSphr.size(); i++)
{
if (Ackerfe::sphrSphrColl(camera3D.getSphr(), &meshSphr[i]))
{
camera3D.changePosition(camera3D.getPosition()+glm::vec3(-5.0f));
}
}
and my sphrSphrColl function looks like this:
bool sphrSphrColl(ACollSphr *first, ACollSphr *second)
{
if (fabs((first->pos->x - second->pos->x) * (first->pos->x - second->pos->x) +
(first->pos->y - second->pos->y) * (first->pos->y - second->pos->y) +
(first->pos->z - second->pos->z) * (first->pos->z - second->pos->z) < (first->radius + second->radius) * (first->radius + second->radius)))
{
//std::cout <<"DISTANCE: "<<std::endl<<glm::length(*first->pos - *second->pos) << std::endl << std::endl << std::endl << std::endl;
return true;
}
return false;
}
I'm checking the position of the bounding spheres with a cout in my main while loop and the positions are registered correctly but when I pass them to the sphrSphrColl function it only seems to put a bounding sphere in the origin and that's it.
My question: Any idea why it's doing that? While I do have a mesh in the origin why does it only put the bounding sphere there? it's almost like the pointers aren't getting updated when I pass them in the function
I have received that function in my code, written by someone else. I don't understand the theory behind it, but it seems to be working.
Does somebody could lead me in the right direction ?
We are calculating the slope of a road from a pointcloud with ransac.
rotation is the world to local matrix, so plane_normal_rot is the normal vector of the plane in the world.
But after that I don't understand what is going on..
ransac.getModelCoefficients(model_coefficients);
std::cout << "#############################" << std::endl;
std::cout << "PLANE MODEL: " << model_coefficients[0] << " "<< model_coefficients[1] << " "<< model_coefficients[2] << " " << model_coefficients[3];
std::cout << "#############################" << std::endl;
double a = model_coefficients[0];
double b = model_coefficients[1];
double c = model_coefficients[2];
tf::Vector3 plane_normal(a,b,c);
tf::Vector3 plane_normal_rot(0,0,0);
//tf::Matrix3x3 rotation_tr = rotation.transpose();
tf::Matrix3x3 rotation_tr = rotation;
plane_normal_rot.setX( (plane_normal.getX() * rotation_tr[0][0])
+ (plane_normal.getY() * rotation_tr[0][1])
+ (plane_normal.getZ() * rotation_tr[0][2]));
plane_normal_rot.setY( (plane_normal.getX() * rotation_tr[1][0])
+ (plane_normal.getY() * rotation_tr[1][1])
+ (plane_normal.getZ() * rotation_tr[1][2]));
plane_normal_rot.setZ( (plane_normal.getX() * rotation_tr[2][0])
+ (plane_normal.getY() * rotation_tr[2][1])
+ (plane_normal.getZ() * rotation_tr[2][2]));
//Check sign
if(plane_normal_rot.getZ() < 0)
{
plane_normal_rot *= (-1);
}
pitch = asin(plane_normal_rot.getX());
If I havn't been clear or you feel like you're missing info please tell me.
I'm struggling with a problem I thought should be easy to solve:
I'm given two points x1, x2 and a width value. How can I calculate two other points parallel to x1 and x2 so that it forms a rectangle?
I tried answers from here 1 and here 2. Though both solutions are off.
As background: This is about projecting an image into real world coordinates. Therefore I need to find the parallel line to the line I'm provided with, so that the points of both lines create a rectangle. I do not want to apply a rotation on my own.
Here is a drawing that shows what I want to achieve:
In the example you see x1, x2 and the width I'm provided with. And I'm looking for x3 and x4 so that the points form a rectangle.
I'm looking for a C++ implementation if possible.
1 https://gamedev.stackexchange.com/questions/86755/how-to-calculate-corner-positions-marks-of-a-rotated-tilted-rectangle
2 Calculating vertices of a rotated rectangle
Here is the code I've implemented. As you can see I'm using top right and top left coordinates that I'm provided with. But I'd rather find a line parallel to the provided points instead:
double distance = 77.5;//[self normalizedDistanceWithCRS:crs p1:topLeft p2:topRight];
// calculate the rotated coordinates for bottom right and bottom left with provided height
double angle = atan2(sinuTL.y - sinuTR.y, sinuTL.x - sinuTR.x); // * 180 / M_PI
double x = distance;
double y = height;
double xBRTrans = x*cos(angle) - y*sin(angle);
xBRTrans = sinuTL.x - xBRTrans;
double yBRTrans = x*sin(angle) + y*cos(angle);
yBRTrans = sinuTL.y - yBRTrans;
x = 0;
y = height;
double xBLTrans = x*cos(angle) - y*sin(angle);
xBLTrans += sinuTL.x;
double yBLTrans = x*sin(angle) + y*cos(angle);
yBLTrans = sinuTL.y - yBLTrans;
** Update **
I've adapted the code from the solution provided below, The result is still not what I expect. The two points on the left are given, the two points on the right are calculated. You can see that there is an offset (the points should be at the corner of the building. Also ignore the blue point in the middle - it's meaningless to this question):
The code:
double height = 57;
// get coords from provided input
double x1x=629434.24373957072, x1y=5476196.7595944777, x2x=629443.08914538298, x2y=5476120.1852802411;
// x2x3 = Vector from point x2 to point x3, assume x value as 1
double x2x3x = 1;
// calculate y-value, using the fact that dot-product of orthogonal vectors is 0
double x2x3y = x2x*x2x3x / (-1 * x2y);
// calculate length of vector x2x3
double length_e_vec_x2_x3 = sqrt(pow(x2x3x,2) + pow(x2x3y,2));
// stretch vector to provided witdh
x2x3x = x2x3x*height / length_e_vec_x2_x3;
x2x3y = x2x3y*height / length_e_vec_x2_x3;
// since x2x3 and x1x4 are equal, simple addition remains
double x3x, x3y, x4x, x4y;
x3x = x2x + x2x3x;
x3y = x2y + x2x3y;
x4x = x1x + x2x3x;
x4y = x1y + x2x3y;
UPDATE
Actually, all answers and also my own code work as expected. I was unfortunately blindfolded and didn't notice the issue was due to an inappropriate geo projection used for this area. So the coordinates come from WGS84 long/lat, and before the calculation is done get converted into a sinusoidal projection and later back into WGS84. The sinusoidal projection preserves the area (equal area projection) - but distorts shapes within an area. And you cannot just add some meters, and later convert back. I should have realized this earlier and was looking at the wrong place.
I'll choose the most elaborate answer as "winner". Though after testing I can say that all provided solutions actually work.
General recommonendation:
If I were you I would build classes for the vectors, and build functions for the required operations, however this example should do what you wish.
Vectors, absolute and relative coordinates:
Important note: you are working with coordinates, and this is a really simplified approach to it. If the person providing you a solution is setting a given Point to 0/0, aka the Origin, you can't just Change this. I changed the code below to adjust to the changes you did to the Input provided.
double width = 35;
// get coords from provided input
double x1x=0, x1y=0, x2x=x, x2y=y;
// x2x3 = Vector from point x2 to point x3, assume x value as 1
double x2x3x = 1;
// calculate y-value, using the fact that dot-product of orthogonal vectors is 0
double x2x3y = x2x*x2x3x / (-1 * x2y);
// calculate length of vector x2x3
double length_e_vec_x2_x3 = sqrt(pow(x2x3x,2) + pow(x2x3y,2));
// stretch vector to provided witdh
x2x3x = x2x3x*width / length_e_vec_x2_x3;
x2x3y = x2x3y*width / length_e_vec_x2_x3;
// since x2x3 and x1x4 are equal, simple addition remains
double x3x, x3y, x4x, x4y;
x3x = x2x + x2x3x;
x3y = x2y + x2x3y;
x4x = x1x + x2x3x;
x4y = x1y + x2x3y;
// check results
cout << "X1: " << x1x << "/" << x1y << endl;
cout << "X2: " << x2x << "/" << x2y << endl;
cout << "X3: " << x3x << "/" << x3y << endl;
cout << "X4: " << x4x << "/" << x4y << endl;
Output:
X1: 629434/5.4762e+06
X2: 629443/5.47612e+06
X3: 629500/5.47613e+06
X4: 629491/5.4762e+06
Verification
As mentioned in the comments of this code, the dot-product of two vectors will return 0 if those vectors are orthogonal to each other. By using this, one can verify the provided results.
Add this little amount of code to verify the results:
// verify results
cout << "Dotproduct should be 0: " << (x2x*x2x3x)+(x2y*x2x3y) << endl;
Output of verification
Dotproduct should be 0: 5.68434e-14
Which prints 0, so the code is doing what it should do.
Improvements
However since you use rather big Numbers, using a float instead of a double might help. Also converting x1 into the origin of your little system might improve it.
Finally a more suitable datastructure would be appreciated.
// using x1 as origin:
double x1x0 = 0, x1y0 = 0, x2x0 = x2x - x1x, x2y0 = x2y - x1y;
double x2x3x0 = 1;
// calculate y-value, using the fact that dot-product of orthogonal vectors is 0
double x2x3y0 = x2x0*x2x3x0 / (-1 * x2y0);
// calculate length of vector x2x3
double length_e_vec_x2_x30 = sqrt(pow(x2x3x0, 2) + pow(x2x3y0, 2));
// stretch vector to provided witdh
x2x3x0 = x2x3x0*width / length_e_vec_x2_x30;
x2x3y0 = x2x3y0*width / length_e_vec_x2_x30;
// since x2x3 and x1x4 are equal, simple addition remains
double x3x0, x3y0, x4x0, x4y0;
x3x0 = x2x0 + x2x3x0;
x3y0 = x2y0 + x2x3y0;
x4x0 = x1x0 + x2x3x0;
x4y0 = x1y0 + x2x3y0;
// check results
cout << "X1: " << x1x0 << "/" << x1y0 << endl;
cout << "X2: " << x2x0 << "/" << x2y0 << endl;
cout << "X3: " << x3x0 << "/" << x3y0 << endl;
cout << "X4: " << x4x0 << "/" << x4y0 << endl;
// verify results
cout << "Dotproduct should be 0: " << (x2x0*x2x3x0) + (x2y0*x2x3y0) << endl;
// compare results (adding offset before comparing):
cout << "X3 to X30: " << x3x0+x1x-x3x << "/" << x3y0+x1y-x3y << endl;
cout << "X4 to X40: " << x4x0 +x1x-x4x << "/" << x4y0 +x1y-x4y << endl;
Results:
X1: 0/0
X2: 8.84541/-76.5743
X3: 65.4689/-70.0335
X4: 56.6235/6.5408
Dotproduct should be 0: 5.68434e-14
X3 to X30: 0/0
X4 to X40: 0/0
Now the output using floats:
X1: 629434/5.4762e+06
X2: 629443/5.47612e+06
X3: 629500/5.47613e+06
X4: 629491/5.4762e+06
Dotproduct should be 0: 0
X1: 0/0
X2: 8.8125/-77
X3: 65.4428/-70.5188
X4: 56.6303/6.48123
Dotproduct should be 0: 0
X3 to X30: 0/0
X4 to X40: 0/0
Building the whole thing less messy:
using namespace std;
class vector2D
{
protected:
bool equal(vector2D& param) { return this->X == param.X && this->Y == param.Y; }
vector2D absAlVal() { return vector2D(abs(X), abs(Y)); }
public:
float X, Y;
vector2D(float x, float y) : X(x), Y(y) {};
vector2D() : X(0), Y(0) {};
vector2D operator+ (vector2D& param) { return vector2D(this->X+param.X,this->Y+param.Y); }
vector2D operator- (vector2D& param) { return vector2D(this->X - param.X, this->Y - param.Y); }
bool operator!=(vector2D& param) { return this->equal(param); }
vector2D getUnitVector()
{
return vector2D(this->X / this->getLength(), this->Y / this->getLength());
}
bool parallel(vector2D& param) { return (this->getUnitVector()).equal(param.getUnitVector()); }
bool colinear(vector2D& param) { return (this->getUnitVector().absAlVal()).equal(param.getUnitVector().absAlVal()); }
float dotproduct(vector2D vec)
{
return this->X * vec.X + this->Y * vec.Y;
}
vector2D dotproduct(float scalar)
{
return vector2D(this->X * scalar, this->Y * scalar);
}
float getLength(void)
{
return sqrt(pow(this->X, 2) + pow(this->Y, 2));
}
};
void main()
{
// get coords from provided input
float x1x = 629434.24373957072, x1y = 5476196.7595944777, x2x = 629443.08914538298, x2y = 5476120.1852802411;
float width = 35;
// Build vectors
vector2D X1 = vector2D(x1x, x1y), X2 = vector2D(x2x, x2y), X3, X4, X2X3, X1X2=X2-X1;
// assum x-direction for X2X3 is positive, chosing 1
X2X3.X = 1;
// calculate y-direction using dot-product
X2X3.Y = X1X2.X*X2X3.X / (-1 * X1X2.Y);
//check if assumtion is correct:
cout << "Evaluate wether vector has been build accordingly or not:" << endl;
cout << "Dotproduct of X1X2 * X2X3 should be 0 -> Result:" << X1X2.dotproduct(X2X3) << endl;
// stretch X2X3 to width
X2X3=X2X3.getUnitVector().dotproduct(width);
// Create X3 and X4 by simple addition:
X3 = X2 + X2X3;
X4 = X1 + X2X3;
// print Points:
cout << "Summary of Points X / Y coordinates:" << endl;
cout << "X1: " << X1.X << "/" << X1.Y << endl;
cout << "X2: " << X2.X << "/" << X2.Y << endl;
cout << "X3: " << X3.X << "/" << X3.Y << endl;
cout << "X4: " << X4.X << "/" << X4.Y << endl;
// compare sides
cout << "\n" << "Lenght of sides:" << endl;
cout << "X1X2: " << (X2 - X1).getLength() << " -> should be same length as X3X4" << endl;
cout << "X2X3: " << (X3 - X2).getLength() << " -> should be same length as X4X1 and with, which is:" << width << endl;
cout << "X3X4: " << (X4 - X3).getLength() << " -> should be same length as X1X2" << endl;
cout << "X4X1: " << (X1 - X4).getLength() << " -> should be same length as X2X3, which is:" << width << endl;
}
Given a vector (x, y), the direction (y, -x) is rotated by 90 degrees clockwise with respect to it. This is exactly the rotation we need to perform to obtain the direction of the side x1 -> x4 from x1 -> x2.
// input point struct
struct point { double x, y; };
// pass in output points by reference
void calculate_other_points(
const point& x1, const point& x2, // input points x1 x2
double w, // input width
point& x3, point& x4) // output points x3 x4
{
// span vector x1 -> x2
double dx = x2.x - x1.x,
dy = x2.y - x1.y;
// height
double h = hypot(dx, dy);
// perpendicular edge x1 -> x4 or x2 -> x3
double px = dy * (w / h),
py = -dx * (w / h);
// add onto x1 / x2 to obtain x3 / x4
x4.x = x1.x + px; x4.y = x1.y + py;
x3.x = x2.x + px; x3.y = x2.y + py;
}
Note that my code is similar in principle to that of the previous answer, but is somewhat more optimized, and (hopefully) fixes the direction issue.
Below is the first part of a function meant to analyze a Tic-Tac-Toe board.
The aboutToWin() function returns true if a player is "about to win," ie has two in a row. The board is represented such that if player 1 has made a move in that square, the value in the 3x3 matrix will be 1. Player 2 will be -1. If no one has made a move, it will be 0.
The part that I put in this question is the first part, which checks the negative diagonal (ie positions 1, 5, and 9 on the board).
bool aboutToWin(int squares[3][3], int& position, bool p1)
{
// The value you are looking for is 2 for p1 and -2 for p2
int check = 2;
if (!p1)
{
check = -2;
}
// Check negative diagonal
int sum = 0;
// Go through negative diagonal
for (int i = 0; i < 3; i++)
{
sum += squares[i][i];
// Saves the position of the last checked 0 square
// If check passes, this will be the winning square
// If not, it will get handled and overwritten
if (squares[i][i] == 0)
{
// Calculates position from i
position = 1 + (4 * i);
std::cout << "\nPosition: " << position << "\n";
}
}
// If this diagonal added to check, stop the function and return now
if (sum == check)
return true;
...
}
This is the code that I'm running from the main() function to test this functionality:
int p;
std::cout << p;
int a3[3][3] = {{1, 0, 1},
{0, 0, 0},
{0, 0, 1}};
std::cout << "\nShould be 1, 5: " << aboutToWin(a3, p, true) << ", " << p;
The output is the following:
0
Position: 5
Should be true, 5: 1, 0
Why is this? I can see that the value gets changed during the function, but it doesn't transfer out of the function.
The problem with using:
std::cout << "\nShould be 1, 5: " << aboutToWin(a3, p, true) << ", " << p;
is that the order of the evaluation of the arguments is not defined unless you use C++17.
It looks like p is evaluated first in your setup before the call to aboutToWin.
Separate the calls.
auto ret = aboutToWin(a3, p, true);
std::cout << "\nShould be 1, 5: " << ret << ", " << p;
I'm trying to code a simple structure from motion scenario, using only 2 images taken from the same camera.
I use SIFT to find matching points between the images (total of 72 matches), out of which 62 are correct.
I use OpenCV to calculate the fundamental matrix, then the essential. When I try to verify the essential matrix by doing p2^T * E * p1 I get very high values instead of close to zero.
Am I doing something wrong?
Here's the code: (pts1, pts2 are std::vector<Point2f>. dmat is Mat_<double>)
int n = pts1.size();
std::cout << "Total point matches: " << n << std::endl;
std::vector<unsigned char> status(n);
std::cout << "K=" << K << std::endl;
F = findFundamentalMat(pts1, pts2,FM_RANSAC,3,0.99,status);
std::cout << "F=" << F << std::endl;
std::cout << "Total inliers: " << std::accumulate(status.begin(),status.end(),0) << std::endl;
E = K.t() * F * K;
std::cout << "E=" << E << std::endl;
for (int i = 0; i < n;++i)
{
dmat p1(3,1), p2(3,1);
p1 << pts1[i].x, pts1[i].y, 1;
p2 << pts2[i].x, pts2[i].y, 1;
dmat mv = p2.t() * E * p1;
double v = mv(0, 0);
std::cout << v << std::endl;
}
and here is the output from this code:
Total point matches: 72
K=[390.0703661671206, 0, 319.5;
0, 390.0703661671206, 239.5;
0, 0, 1]
F=[-2.723736291531157e-007, 7.660367616625481e-005, -0.01766345189507435;
-4.219955880897177e-005, 9.025976628215733e-006, -0.04376995849516735;
0.009562535474535394, 0.03723116011143099, 1]
Total inliers: 62
E=[-0.04144297973569942, 11.65562396370436, 0.2325229628055823;
-6.420869252333299, 1.373346486079092, -21.48936503378938;
-0.2462444924550576, 24.91291898830852, -0.03174504032716108]
188648
-38467.5
-34880.7
289671
257263
87504.7
462472
-30138.1
-30569.3
174520
-32342.8
-32342.8
-37543.4
241378
-36875.4
-36899
-38796.4
-38225.2
-38120.9
394285
-440986
396805
455397
543629
14281.6
630398
-29714.6
191699
-37854.1
-39295.8
-3395.93
-3088.56
629769
-28132.9
178537
878596
-58957.9
-31034.5
-30677.3
-29854.5
165689
-13575.9
-13294.3
-6607.96
-3446.41
622355
-31803
-35149
-38455.4
2068.12
82164.6
-35731.2
-36252.7
-36746.9
-35325.3
414185
-35216.3
-126107
-5551.84
100196
2.29755e+006
177785
-31991.8
-31991.8
100340
108897
108897
84660.4
-7828.65
225817
225817
295423
The equation v2^T * E * v1 is true for the essential matrix only when v2 and v1 are in normalized coordinates, i.e. v1 = K^(-1)*p1, with p1 the observed point in pixels. Same goes for v2 and p2.
If you have it, you can refer to definition 9.16 page 257 of Hartley and Zisserman's book. But note that this makes sense, given the relation E = K.t() * F * K.