Getting the pitch of a rotation in c++ - c++

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.

Related

Specific right-angled Triangles are not being recognized as right-angled in Cpp

I have to take the coordinates of the vertices of a triangle from the user and tell if it is a right-angled triangle or not. I'm using Pythagoras Theorem to Find out i.e. h * h = b * b + p * p
But surprisingly this doesn't work for some specific right-angled triangles.
Here is one such Triangle:
Vertex A: (x, y) = (1, 3)
Vertex B: (x, y) = (1, 1)
Vertex C: (x, y) = (5, 1)
It calculates perfectly, which I figured out by printing the calculation, but still doesn't work.
Then I tried by using sqrt() function from the cmath library this way:
h = sqrt(b * b + p * p)
Logically it is the same, but it worked.
I want to understand, why the earlier method is not working?
Here is a simplified version of My Code:
#include <iostream>
#include <cmath>
using namespace std;
class Vertex {
double x, y;
public:
void take_input(char obj) {
cout << endl << " Taking Coordinates of Vertex " << obj << ": " << endl;
cout << " Enter the x component: ";
cin >> x;
cout << " Enter the y component: ";
cin >> y;
}
double distance(Vertex p) {
double dist = sqrt((x-p.x)*(x-p.x) + (y-p.y)*(y-p.y));
return dist;
}
};
class Triangle {
Vertex a, b, c;
public:
void take_inp(string obj) {
cout << endl << "Taking Vertices of the Triangle " << obj << ": " << endl;
cout << " Verteces should be in a counter clockwise order (as per convention)." << endl;
a.take_input('A');
b.take_input('B');
c.take_input('C');
}
void is_rt_ang() {
double h = a.distance(c)*a.distance(c);
double bp = a.distance(b)*a.distance(b) + b.distance(c)*b.distance(c);
/*
// Strangely this attempt works which is logically the same:
double h = a.distance(c);
double bp = sqrt(a.distance(b)*a.distance(b) + b.distance(c)*b.distance(c));
*/
if (h == bp) {
cout << "Angle is 90" << endl;
cout << h << " = " << bp << endl;
cout << "It is Right-Angled" << endl;
}
else {
cout << "Angle is not 90!" << endl;
cout << h << " != " << bp << endl;
cout << "It is Not a Right-Angled" << endl;
}
}
};
int main()
{
Triangle tri1, tri2;
tri1.take_inp("tri1");
tri1.is_rt_ang();
return 0;
}
The line
double dist = sqrt((x-p.x)*(x-p.x) + (y-p.y)*(y-p.y));
in the Vertex::distance method gives you an approximation of a square root which is rarely going to coincide with an exact answer. This is because most real numbers can't be represented in floating point arithmetic.
But in given code sample you can make do without sqrt. Replace Vertex::distance method with a method
double distance_square(Vertex p) {
double dist_square = (x-p.x)*(x-p.x) + (y-p.y)*(y-p.y);
return dist_square;
}
and call it like this in Triangle::is_rt_ang:
double h = a.distance_square(c);
double bp = a.distance_square(b) + b.distance_square(c);
This solution is still flawed because floating-point multiplication is also a subject to rounding errors. But if it is guaranteed that you are going to work only with integer coordinates, you can replace all doubles in your code with ints and for them there is no problem with multiplication (besides possibly going out of bounds for large numbers).
EDIT: Also a comment on printing
It calculates perfectly, which I figured out by printing the
calculation, but still doesn't work.
When you print doubles you need to set precision manually in order to avoid rounding. If in your code I replace a line
cout << h << " != " << bp << endl;
with
cout << std::setprecision(std::numeric_limits<double>::digits10) << std::fixed << h << " != " << bp << endl;
then for example triangle from the question I get the output
Angle is not 90!
20.000000000000004 != 20.000000000000000
It is Not a Right-Angled
For this to compile you will need to add #include <limits> and #include <iomanip>.
In your is_rt_ang function you're assuming that your hypotenuse is always going to be the edge AC, but it doesn't seem like you're doing anything to verify this.
double h = a.distance(c)*a.distance(c);
double bp = a.distance(b)*a.distance(b) + b.distance(c)*b.distance(c);
You could try getting the squares of all your distances first, (AC)^2, (AB)^2, and (BC)^2, then finding the candidate for hypotenuse by taking the max value out of the three, then do something like:
bool isRightTriangle = max == (min1 + min2)
You may also be running into some kind of round-off error with floating point numbers. It is common to use a an epsilon value when comparing floating point numbers because of the inherent round-off errors with them. If you don't need floating point values maybe use an integer, or if you do need floating point values try using an epsilon value in your equalities like:
abs(h - bp) <= epsilon
You should be able to find more information about floating point values, round-off errors, and machine epsilons on the web.
Here is a link to a SO Q/A that talks about floating point math that may be a good resource for you: Is floating point math broken?

Pointer Exception while getting RGB values from (video) frame Intel Realsense

I'm trying to get the different RGB values from a frame with the Realsense SDK. This is for a 3D depth camera with RGB. According to https://github.com/IntelRealSense/librealsense/issues/3364 I need to use
int i = 100, j = 100; // fetch pixel 100,100
rs2::frame rgb = ...
auto ptr = (uint8_t*)rgb.get_data();
auto stride = rgb.as<rs2::video_frame>().stride();
cout << "R=" << ptr[3*(i * stride + j)];
cout << ", G=" << ptr[3*(i * stride + j) + 1];
cout << ", B=" << ptr[3*(i * stride + j) + 2];
In my code I'm getting a pointer exception if I want to get the values for pixel (x,y)=1000,1000. With (x,y)=100,100 it works every time... Error: Exception thrown: read access violation. ptr was 0x11103131EB9192A.
I set the enable_stream to cfg.enable_stream(RS2_STREAM_COLOR, WIDTH_COLOR_FRAME, HEIGTH_COLOR_FRAME, RS2_FORMAT_RGB8, 15); where in the .h file are:
#define WIDTH_COLOR_FRAME 1920
#define HEIGTH_COLOR_FRAME 1080
This is my code. Maybe it has something to do with the RS2_FORMAT_RGB8?
frameset frames = pl.wait_for_frames();
frame color = frames.get_color_frame();
uint8_t* ptr = (uint8_t*)color.get_data();
int stride = color.as<video_frame>().get_stride_in_bytes();
int i = 1000, j = 1000; // fetch pixel 100,100
cout << "R=" << int(ptr[3 * (i * stride + j)]);
cout << ", G=" << int(ptr[3 * (i * stride + j) + 1]);
cout << ", B=" << int(ptr[3 * (i * stride + j) + 2]);
cout << endl;
Thanks in advance!
stride is in bytes (length of row in bytes), multiplication with 3 is not required.
cout << " R= " << int(ptr[i * stride + (3*j) ]);
cout << ", G= " << int(ptr[i * stride + (3*j) + 1]);
cout << ", B= " << int(ptr[i * stride + (3*j) + 2]);
I had the same problem and even with the last answers I still got segfaults.
I found out that when you do
uint8_t *ptr = color.get_data()
the realsense sdk won't increase/track some internal reference and the pointer went invalid after some time, causing the segfaults.
my Fix is copy the content to a local buffer.
malloc new buffer with RGB size.
right after get_data() copy data to the new buffer.
that fixed all my issues.
all the best.

Bounding sphere stuck in place

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

Given one side of a rectangle calculate the others

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.

Verifying essential matrix

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.