I have a transformation matrix of type Eigen::Matrix4d. And I would like to get its inverse. I write a function my self to compute it by the following formular.
.
And here is my code:
Eigen::Matrix4d inverseTransformation(Eigen::Matrix4d T)
{
Eigen::MatrixX3d R;
Eigen::Vector3d t;
R = T.block<3, 3>(0, 0);
t = T.block<3, 1>(0, 3);
Eigen::Matrix4d result;
result.setIdentity();
result.block<3, 3>(0, 0) = R.transpose();
result.block<3, 1>(0, 3) = -R.transpose() * t;
return result;
}
std::cout<<"Input transformation matrix is:\n" << T << std::endl;
std::cout << "inverse of T , my implementation:\n" << inverseTransformation(T) << std::endl << std::endl;
std::cout << "inverse of T , Eigen implementation::\n" << T.inverse() << std::endl << std::endl;
std::cout << "T * T^(-1), my implementation\n " << T * inverseTransformation(T) << std::endl << std::endl;
std::cout << "T * T^(-1), eigen's implementation\n " << T * T.inverse() << std::endl << std::endl;
Ideally, T * T^(1) should be I. However, there is some error in my result (the red part in the following picture.)
By contrast, the result from T * T.inverse() is much more accurate.
Could someone please tell me why? Thanks a lot in advance!
Update:
Inverse of a rotation matrix is its transpose. The result will be more accurate if I replace R.tranpose() with R.inverse().
Thanks to the comments. Since I would like to close this question, I will answer it my self.
Theoretically, R.inverse() equals to R.tranpose(). But this is not correct in floating point system. Once I use R.inverse(), the result is more accurate.
Related
I am trying to do something as simple as:
std::cout << e << std::endl;
where e is of type Eigen::Affine3d. However, I am getting unhelpful error messages like:
cannot bind 'std::ostream {aka std::basic_ostream<char>}'
lvalue to 'std::basic_ostream<char>&&'
The reason for which is helpfully explained here, but where the answer does not apply.
The official documentation is curt, implying only that Affine3d and Affine3f objects are matrices. Eigen matrices and vectors can be printed by std::cout without issue though. So what is the problem?
Annoyingly, the << operator is not defined for Affine objects. You have to call the matrix() function to get the printable representation:
std::cout << e.matrix() << std::endl;
If you're not a fan of homogenous matrices:
Eigen::Matrix3d m = e.rotation();
Eigen::Vector3d v = e.translation();
std::cout << "Rotation: " << std::endl << m << std::endl;
std::cout << "Translation: " << std::endl << v << std::endl;
Hopefully someone can save a few minutes of annoyance.
PS:Another lonely SO question mentioned this solution in passing.
To be honest, I would prefer to overload the stream operator. This makes the repeated use more convinient. This you can do like this
std::ostream& operator<<(std::ostream& stream, const Eigen::Affine3d& affine)
{
stream << "Rotation: " << std::endl << affine.rotation() << std::endl;
stream << "Translation: " << std::endl << affine.translation() << std::endl;
return stream;
}
int main()
{
Eigen::Affine3d l;
std::cout << l << std::endl;
return 0;
}
Be aware that l is uninitialized
I'm having a really weird bug when trying to intersect two triangles inside a 3D space while using the CGAL::Exact_predicates_inexact_constructions_kernel kernel. Essentially, I have two triangles that should not intersect. The function CGAL::do_intersect returns always false when testing them, but the function CGAL::intersection builds an intersection, depending on the order of the vertices of the triangles.
The bug disappears when I use the CGAL::Exact_predicates_exact_constructions_kernel kernel, but I can't afford to use it in the real case scenario.
Below is a minimal code with the bug. Triangles B and C are equal (up to a permutation of the vertices), and should return the same intersection with Triangle A.
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Intersections.h>
#include <iostream>
#include <vector>
typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::Point_3 Point_3;
typedef Kernel::Triangle_3 Triangle_3;
int main(int argc, char *argv[])
{
std::vector<Point_3> APoints(3);
std::vector<Point_3> BPoints(3);
APoints[0] = Point_3(2, 2, 0.9423616295572568);
APoints[1] = Point_3(0.9685134704003172, 2, 0.9678422992674797);
APoints[2] = Point_3(2, 1.124710354419025, 1.068692504586136);
BPoints[0] = Point_3(2.5, 2.5, 1.442361629557257);
BPoints[1] = Point_3(1.588259113885977, 2.5, 0.5);
BPoints[2] = Point_3(2.5, 1.624710354419025, 1.568692504586136);
Triangle_3 TriangleA(APoints[0],APoints[1],APoints[2]);
Triangle_3 TriangleB(BPoints[0],BPoints[1],BPoints[2]);
Triangle_3 TriangleC(BPoints[2],BPoints[1],BPoints[0]);
std::cout.precision(16);
std::cout << " - Tried to intersect: " << std::endl;
std::cout << " - Triangle (A) " << " : "
<< "(" << TriangleA.vertex(0) << ") "
<< "(" << TriangleA.vertex(1) << ") "
<< "(" << TriangleA.vertex(2) << ") " << std::endl;
std::cout << " - Triangle (B) " << " : "
<< "(" << TriangleB.vertex(0) << ") "
<< "(" << TriangleB.vertex(1) << ") "
<< "(" << TriangleB.vertex(2) << ") " << std::endl;
std::cout << " - Triangle (C) " << " : "
<< "(" << TriangleC.vertex(0) << ") "
<< "(" << TriangleC.vertex(1) << ") "
<< "(" << TriangleC.vertex(2) << ") " << std::endl;
if( TriangleB.vertex(0)==TriangleC.vertex(2) &&
TriangleB.vertex(1)==TriangleC.vertex(1) &&
TriangleB.vertex(2)==TriangleC.vertex(0))
{
std::cout << " - Triangles (B) and (C) have the same vertices " << std::endl;
}
bool bIntersectAB = CGAL::do_intersect(TriangleA,TriangleB);
bool bIntersectAC = CGAL::do_intersect(TriangleA,TriangleC);
bool bIntersectInexactAB = CGAL::intersection(TriangleA,TriangleB);
bool bIntersectInexactAC = CGAL::intersection(TriangleA,TriangleC);
if(bIntersectAB)
{
std::cout << " --> A and B are intersecting (exact) ..." << std::endl;
}
if(bIntersectAC)
{
std::cout << " --> A and C are intersecting (exact) ..." << std::endl;
}
if(bIntersectInexactAB)
{
std::cout << " --> A and B are intersecting (inexact) ..." << std::endl;
}
if(bIntersectInexactAC)
{
std::cout << " --> A and C are intersecting (inexact) ..." << std::endl;
}
return 0;
}
Here's the output ...
- Tried to intersect:
- Triangle (A) : (2 2 0.9423616295572568) (0.9685134704003172 2 0.9678422992674797) (2 1.124710354419025 1.068692504586136)
- Triangle (B) : (2.5 2.5 1.442361629557257) (1.588259113885977 2.5 0.5) (2.5 1.624710354419025 1.568692504586136)
- Triangle (C) : (2.5 1.624710354419025 1.568692504586136) (1.588259113885977 2.5 0.5) (2.5 2.5 1.442361629557257)
- Triangles (B) and (C) have the same vertices
--> A and C are intersecting (inexact) ...
... and a figure with the two triangles (A: vertices 1, 2, 3 ; B: vertices 11,12,13) and the "intersection" (segment 21 - 22), found using a similar version of this program.
What could be wrong? I'm using CGAL 4.6.1 on OS X 10.10.5 (Yosemite). Thanks in advance!
I've also sent this question to CGAL's mailing list, and the developers answered that this behaviour is not a bug, although
it is unfortunate. intersection is a generic function, implemented the same way for all CGAL kernels, and it uses one step that is not always handled correctly by inexact kernels - hence the intersection error. According to this thread at CGAL's GitHub page,
In order to keep using a kernel with inexact constructions, I usually advice to first call the do_intersect predicate and then call the intersection function using EPECK on primitives converted on the fly using CGAL::Cartesian_converter. You'll have to convert the output using another CGAL::Cartesian_converter. The call to do_intersect is not mandatory, it usually depends on your setting.
I am getting incorrect conversions from polar to cartesian coordinates and vice versa. My code produces weird points like (1,-0). Im using this calculator to check my conversions. Also one of the conversions is completely wrong when I convert back to cartesian coordinates.
Point b: (0,1) => (1,1.5708) => (0,0)
#include <math.h>
#include <iostream>
/* Title: Polar - Cartesian Coordinate Conversion
* References: HackerRank > All Domains > Mathematics > Geometry > Polar Angles
* Cartesian to Polar: (radius = sqrt(x^2 + y^2), theta = atan(y/x))
* Polar to Cartesian: (x = radius*cos(theta), y = radius*sin(theta))
*/
//General 2D coordinate pair
struct point{
point(float a_val, float b_val) : a(a_val), b(b_val){;};
point(void){;};
float a, b;
};
//Converts 2D Cartesian coordinates to 2D Polar coordinates
point to_polar(/*const*/ point& p){//*** Conversion of origin result in (r, -nan) ***
point ans(sqrt(pow(p.a,2) + pow(p.b,2)), atan(p.b/p.a));
return ans;
}
//Converts 2D Polar coordinates to 2D Cartesian coordinates
point to_cartesian(/*const*/ point& p){
point ans(p.a * cos(p.b), p.a * sin(p.b));
return ans;
}
//Outputs 2D coordinate pair
std::ostream& operator<<(std::ostream& stream, const point& p){
stream << "(" << p.a << "," << p.b << ")";
return stream;
}
int main(){
//Test Points - Cartesian
point a(0, 0);
point b(0, 1);
point c(1, 0);
point d(0,-1);
point e(-1,0);
//Print Cartesian/Rectangular points
std::cout << "Cartesian Coordinates:" << std::endl;
std::cout << a << std::endl;
std::cout << b << std::endl;
std::cout << c << std::endl;
std::cout << d << std::endl;
std::cout << e << std::endl;
//Print Cartesian to Polar
std::cout << "Polar Coordinates:" << std::endl;
std::cout << to_polar(a) << std::endl;//Failure (0,-nan)
std::cout << to_polar(b) << std::endl;//Success
std::cout << to_polar(c) << std::endl;//Success
std::cout << to_polar(d) << std::endl;//Success
std::cout << to_polar(e) << std::endl;//Failure (1,-0)
//Print Polar to Cartesian
std::cout << "Cartesian Coordinates:" << std::endl;
std::cout << to_cartesian(a) << std::endl;//Success
std::cout << to_cartesian(b) << std::endl;//Failure (0,0)
std::cout << to_cartesian(c) << std::endl;//Success
std::cout << to_cartesian(d) << std::endl;//Failure (0,-0)
std::cout << to_cartesian(e) << std::endl;//Failure (-1,-0)
return 0;
}
You are converting to cartesian the points which are in cartesian already. What you want is:
std::cout << "Cartesian Coordinates:" << std::endl;
std::cout << to_cartesian(to_polar(a)) << std::endl;
std::cout << to_cartesian(to_polar(b)) << std::endl;
//...
Edit: using atan2 solves the NaN problem, (0, 0) is converted to (0, 0) which is fine.
As a first step, you need to switch to atan2 instead of atan in your conversion to polar coordinates. atan gives wrong results for half the plane.
for example im getting an area of 6 for a triangle, i need my output to show 006.000
i know setprecision will do the trick for limiting the decimal places to three but how do i place zeros in front of my solutions?
this is what i have
CTriangle Sides(3,4,5);
cout << std::fixed << std::setprecision(3);
cout << "\n\n\t\tThe perimeter of this tringle is = " << Sides.Perimeter();
cout << "\n\n\t\tThe area of the triangle is = " << Sides.Area();
cout << "\n\n\t\tThe angle one is = " << Sides.Angleone();
cout << "\n\n\t\tThe angle two is = " << Sides.Angletwo();
cout << "\n\n\t\tThe angle three is = " << Sides.Anglethree();
cout << "\n\n\t\tThe altitude one of the triangle = " << Sides.Altitudeone();
cout << "\n\n\t\tThe altitude two of the triangle = " << Sides.Altitudetwo();
cout << "\n\n\t\tThe altitude three of the triangle = " << Sides.Altitudethree();
with the output being
The perimeter of this triangle is = 12.000
The area of the triangle is = 6.000
The angle one is = 90.000
The angle two is = 36.870
The angle three is = 53.130
The altitude one of the triangle = 2.400
The altitude two of the triangle = 6.667
The altitude three of the triangle = 3.750
but i need all the answers to be in this form XXX.XXX regardless of what my solutions are.(because the values will change)
any help is appreciated, thanks!
Use can use padding and filling manipulators:
std::setfill('0'); // is persistent
//...
cout << std::setw(7) << value; // required for each output
Using std::internal, std::fixed, std::setfill, std::setw and std::setprecision from iomanip and related headers, you can do:
std::cout << std::fixed << std::setfill('0') << std::internal << std::setprecision(3);
std::cout << std::setw(7);
std::cout << 12.34f << "\n";
and get the desired output. See it live on Coliru!
See "format": String and I/O Formatting (Modern C++)
The printf function can do that for you have a look at the docs:
http://www.cplusplus.com/reference/cstdio/printf/
Your case is somewhat unique because:
(these are not complete code snippets, apologies)
precision only applies to floating point formatting:
$ printf("%03.3f\n", 6)
> 6.000
And left padding only applies to integer formatting:
$ printf("%03.3d\n", 6)
> 006
Good luck hopefully you can take it from here
So I'm having what seems to me to be a very bizarre problem. I've got a crude system for applying forces to objects on 2D planes, and one of the simplest calculations seems to be causing one of my variables to overflow. I have the following line:
int ySign = m_Momentum.y / abs(m_Momentum.y);
Where Momentum has two data members, x y (m_Momentum is an SFML sf::Vector2 of floats). Now, normally the formula should always return either 1 or -1, depending on the sign of Momentum.y (unless I'm grossly mistaken).
However, it occasionally returns insanely high numbers such as -2147483648. In that particular case, the value of m_Momentum.y was 0.712165 (both values were obtained by sending to std::cout); I tried again, m_Momentum.y was -0.578988 and ySign was still -2147483648. There is a corresponding xSign that also flips out sometimes, often with the same final value. I can't confirm 100% that this is always the result, but at the moment that seems to be the case.
I'm sort of stumped as to why this is happening, and when it does, it basically invalidates my program (it instantly sends objects millions of pixels in the wrong direction). It seems logically impossible that the line above is returning such strange results.
Below is the function I am working on. Probably the wrong way to do it, but I didn't expect it to go so horribly wrong. The printout it produces reveals that all numbers look normal until the signs are printed out; one of them is invariably massive, and afterwards you see numbers like -2.727e+008 (which, as far as I'm aware, is scientific notation - i.e. -2.727 * 10 ^ 8).
///MODIFY MOMENTUM
//Reset, if necessary
if (Reset == true)
{
m_Momentum.x = 0;
m_Momentum.y = 0;
}
sf::Vector2<float> OldMoment = m_Momentum;
//Apply the force to the new momentum.
m_Momentum.x += Force.x;
m_Momentum.y += Force.y;
sf::Vector2<float> NewMoment = m_Momentum;
//Calculate total momentum.
float sqMomentum = m_Momentum.x * m_Momentum.x + m_Momentum.y * m_Momentum.y;
float tMomentum = sqrt(sqMomentum);
//Preserve signs for later use.
int xSign = m_Momentum.x / abs(m_Momentum.x);
int ySign = m_Momentum.y / abs(m_Momentum.y);
//Determine more or less the ratio of importance between x and y components
float xProp;
float yProp;
if (abs(tMomentum) > m_MaxVelocity)
{
//Get square of maximum velocity
int sqMax = m_MaxVelocity * m_MaxVelocity;
//Get proportion of contribution of each direction to velocity
xProp = (m_Momentum.x * m_Momentum.x) / sqMomentum;
yProp = (m_Momentum.y * m_Momentum.y) / sqMomentum;
//Reset such that the total does not exceed maximum velocity.
m_Momentum.x = sqrt(sqMax * xProp) * xSign;
m_Momentum.y = sqrt(sqMax * yProp) * ySign;
}
///SANITY CHECK
//Preserve old tMomentum
float tOld = tMomentum;
//Calculate current tMomentum
sqMomentum = m_Momentum.x * m_Momentum.x + m_Momentum.y * m_Momentum.y;
tMomentum = sqrt(sqMomentum);
//If it's still too high, print a report.
if (tMomentum > m_MaxVelocity)
{
std::cout << "\n\nSANITY CHECK FAILED\n";
std::cout << "-\n";
std::cout << "Old Components: " << OldMoment.x << ", " << OldMoment.y << "\n";
std::cout << "Force Components: " << Force.x << ", " << Force.y << "\n";
std::cout << "-\n";
std::cout << "New Components: " << NewMoment.x << ", " << NewMoment.y << "\n";
std::cout << "Which lead to...\n";
std::cout << "tMomentum: " << tOld << "\n";
std::cout << "-\n";
std::cout << "Found these proportions: " << xProp << ", " << yProp << "\n";
std::cout << "Using these signs: " << xSign << ", " << ySign << "\n";
std::cout << "New Components: " << m_Momentum.x << ", " << m_Momentum.y << "\n";
std::cout << "-\n";
std::cout << "Current Pos: " << m_RealPosition.x << ", " << m_RealPosition.y << "\n";
std::cout << "New Pos: " << m_RealPosition.x + m_Momentum.x << ", " << m_RealPosition.y + m_Momentum.y << "\n";
std::cout << "\n\n";
}
///APPLY FORCE
//To the object's position.
m_RealPosition.x += m_Momentum.x;
m_RealPosition.y += m_Momentum.y;
//To the sprite's position.
m_Sprite.Move(m_Momentum.x, m_Momentum.y);
Can somebody explain what's going on here?
EDIT: RedX helpfully directed me to the following post: Is there a standard sign function (signum, sgn) in C/C++? Which led me to write the following lines of code:
//Preserve signs for later use.
//int xSign = m_Momentum.x / abs(m_Momentum.x);
//int ySign = m_Momentum.y / abs(m_Momentum.y);
int xSign = (m_Momentum.x > 0) - (m_Momentum.x < 0);
int ySign = (m_Momentum.y > 0) - (m_Momentum.y < 0);
Thanks to the above, I no longer have the strange problem. For an explanation/alternative solution, see Didier's post below.
You should use fabs() instead of abs() to get the absolute value of a floating point number. If you use the integer absolute function, then the result is an integer ...
For instance, -0.5 / abs(-0.5) is treated as -0.5 / 0 which results in negative infinity (as a floating point value) that is converted to the minimum value of an int 0x80000000 = -2147483648
Taking absolute values and dividing sounds like an awful waste of cycles to me. What's wrong with
x > 0 ? 1 : -1
which you could always put in a function
template <class T>
inline int sgn(const T &x) { return x > 0 ? : 1; }