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.
Related
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?
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;
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.
In matlab/octave pairwise distances between matrices as required for e.g. k-means are calculated by one function call (see cvKmeans.m), to distFunc(Codebook, X) with as arguments two matrices of dimensions KxD.
In Eigen this can be done for a matrix and one vector by using broadcasting, as explained on eigen.tuxfamily.org:
(m.colwise() - v).colwise().squaredNorm().minCoeff(&index);
However, in this case v is not just a vector, but a matrix. What's the equivalent oneliner in Eigen to calculate such pairwise (Euclidean) distances across all entries between two matrices?
I think the appropriate solution is to abstract this functionality into a function. That function may well be templated; and it may well use a loop - the loop will be really short, after all. Many matrix operations are implemented using loops - that's not a problem.
For example, given your example of...
MatrixXd p0(2, 4);
p0 <<
1, 23, 6, 9,
3, 11, 7, 2;
MatrixXd p1(2, 2);
p1 <<
2, 20,
3, 10;
then we can construct a matrix D such that D(i,j) = |p0(i) - p1(j)|2
MatrixXd D(p0.cols(), p0.rows());
for (int i = 0; i < p1.cols(); i++)
D.col(i) = (p0.colwise() - p1.col(i)).colwise().squaredNorm().transpose();
I think this is fine - we can use some broadcasting to avoid 2 levels of nesting: we iterate over p1's points, but not over p0's points, nor over their dimensions.
However, you can make a oneliner if you observe that |p0(i) - p1(j)|2 = |p0(i)|2 + |p1(j)|2 - 2 p0(i)T p1(j). In particular, the last component is just matrix multiplication, so D = -2 p0T p1 + ...
The blank left to be filled is composed of a component that only depends on the row; and a component that only depends on the column: these can be expressed using rowwise and columnwise operations.
The final "oneliner" is then:
D = ( (p0.transpose() * p1 * -2
).colwise() + p0.colwise().squaredNorm().transpose()
).rowwise() + p1.colwise().squaredNorm();
You could also replace the rowwise/colwise trickery with an (outer) product with a 1 vector.
Both methods result in the following (squared) distances:
1 410
505 10
32 205
50 185
You'd have to benchmark which is fastest, but I wouldn't be surprised to see the loop win, and I expect that's more readable too.
Eigen is more of a headache than I thought on first sight.
There is no reshape() functionality for example (and conservativeResize is something else).
It also seems (I'd like to be corrected) to be the case that Map does not just offer a view on the data, but assignments to temporary variables seem to be required.
The minCoeff function after the colwise operator cannot return a minimum element and an index to that element.
It is unclear to me if replicate is actually allocating duplicates of the data. The reason behind broadcasting is that this is not required.
matrix_t data(2,4);
matrix_t means(2,2);
// data points
data << 1, 23, 6, 9,
3, 11, 7, 2;
// means
means << 2, 20,
3, 10;
std::cout << "Data: " << std::endl;
std::cout << data.replicate(2,1) << std::endl;
column_vector_t temp1(4);
temp1 = Eigen::Map<column_vector_t>(means.data(),4);
std::cout << "Means: " << std::endl;
std::cout << temp1.replicate(1,4) << std::endl;
matrix_t temp2(4,4);
temp2 = (data.replicate(2,1) - temp1.replicate(1,4));
std::cout << "Differences: " << std::endl;
std::cout << temp2 << std::endl;
matrix_t temp3(2,8);
temp3 = Eigen::Map<matrix_t>(temp2.data(),2,8);
std::cout << "Remap to 2xF: " << std::endl;
std::cout << temp3 << std::endl;
matrix_t temp4(1,8);
temp4 = temp3.colwise().squaredNorm();
std::cout << "Squared norm: " << std::endl;
std::cout << temp4 << std::endl;//.minCoeff(&index);
matrix_t temp5(2,4);
temp5 = Eigen::Map<matrix_t>(temp4.data(),2,4);
std::cout << "Squared norm result, the distances: " << std::endl;
std::cout << temp5.transpose() << std::endl;
//matrix_t::Index x, y;
std::cout << "Cannot get the indices: " << std::endl;
std::cout << temp5.transpose().colwise().minCoeff() << std::endl; // .minCoeff(&x,&y);
This is not a nice oneliner and seems overkill just to compare every column in data with every column in means and return a matrix with their differences. However, the versatility of Eigen does not seem to be such that this can be written down much shorter.
Could you like to tell how to split clauses of unsat cores?
And here is question 2 regarding after found out unsat cores, I will try to seek again.
Would you like to tell how to do this?
Thank you very much.
How to split the clauses as below
`and` (`or` (`<=_int` 1002 x1) (`<=_int` 1000 x1)) (`and` (`or` (`<=_int` 0 (`+_int` x2 (`*_int` -1003 x1))) (`<=_int` 0 (`+_int` x2 (`*_int` -1230 x1)))) (`and` (`or` (`<=_int` 0 (`+_int` x3 (`*_int` -1999 x2)))
Regarding to the question 2,
cout<<s.check(3,assumptions)<<endl;
expr_vector core = s.unsat_core();
................
expr assumptions2[2] = {p1,p3};
cout<<"check next"<<s.check(2,assumptions2)<<endl;
expr_vector core1 = s.unsat_core();
for(unsigned int k=0;k<core1.size();++k){
cout<<"New core size "<<k<<endl;
cout<<"New unsat core "<<core1[k]<<endl;
}
calling the unsat core function again, it cannot give the unsat cores again.
Thank you very much.
I'm not sure if I understood your question. It seems you have an assertion of the form (and c1 (and c2 c3)), and you want to track c1, c2 and c3 individually.
In Z3, we use answer literals to track assertions. An answer literal is essentially a fresh Boolean that is used to track an assertion. That is, whether the assertion was used (by Z3) to show unsatisfiability of the whole set of assertions or not. For example, if we want to track assertion F, we create a fresh Boolean variable p and assert p implies F. Then, we provide p as an argument for the check method.
If F is a big conjunction and we want to track its elements individually, we should extract its elements and create an answer literal for each one of them. Here is the complete example that does the trick. You can test it by including it in the example.cpp file that is included in the Z3 distribution. Note that you have to include #include<vector>.
/**
\brief Unsat core example 2
*/
void unsat_core_example2() {
std::cout << "unsat core example 2\n";
context c;
// The answer literal mechanism, described in the previous example,
// tracks assertions. An assertion can be a complicated
// formula containing containing the conjunction of many subformulas.
expr p1 = c.bool_const("p1");
expr x = c.int_const("x");
expr y = c.int_const("y");
solver s(c);
expr F = x > 10 && y > x && y < 5 && y > 0;
s.add(implies(p1, F));
expr assumptions[1] = { p1 };
std::cout << s.check(1, assumptions) << "\n";
expr_vector core = s.unsat_core();
std::cout << core << "\n";
std::cout << "size: " << core.size() << "\n";
for (unsigned i = 0; i < core.size(); i++) {
std::cout << core[i] << "\n";
}
// The core is not very informative, since p1 is tracking the formula F
// that is a conjunction of subformulas.
// Now, we use the following piece of code to break this conjunction
// into individual subformulas. First, we flat the conjunctions by
// using the method simplify.
std::vector<expr> qs; // auxiliary vector used to store new answer literals.
assert(F.is_app()); // I'm assuming F is an application.
if (F.decl().decl_kind() == Z3_OP_AND) {
// F is a conjunction
std::cout << "F num. args (before simplify): " << F.num_args() << "\n";
F = F.simplify();
std::cout << "F num. args (after simplify): " << F.num_args() << "\n";
for (unsigned i = 0; i < F.num_args(); i++) {
std::cout << "Creating answer literal q" << i << " for " << F.arg(i) << "\n";
std::stringstream qname; qname << "q" << i;
expr qi = c.bool_const(qname.str().c_str()); // create a new answer literal
s.add(implies(qi, F.arg(i)));
qs.push_back(qi);
}
}
// The solver s already contains p1 => F
// To disable F, we add (not p1) as an additional assumption
qs.push_back(!p1);
std::cout << s.check(qs.size(), &qs[0]) << "\n";
expr_vector core2 = s.unsat_core();
std::cout << core2 << "\n";
std::cout << "size: " << core2.size() << "\n";
for (unsigned i = 0; i < core2.size(); i++) {
std::cout << core2[i] << "\n";
}
}