I am trying to follow an algebraic equation, and convert it to c++.
I am stuck on:
Calculate the radius as r = ||dp||
where dp is a vector, and:
dp = (dx,dy)
According to my google searching, the vertical bars in r = ||dp|| mean I need to normalize the vector.
I have:
std::vector<double> dpVector;
dpVector.push_back(dx);
dpVector.push_back(dy);
How should I be normalizing this so that it returns a double as 'r'?
||dp|| is the euclidean norm of the vector dp. Take a look at this link for a more complete explanation:
https://en.wikipedia.org/wiki/Euclidean_distance
The euclidean norm is computed as follow: ||dp|| = sqrt(dp.dp), where . represents the dot product.
In C++, this would equate to ||dp|| = std::sqrt(dx*dx + dy*dy). If dp had more dimensions, you would be better off using a linear algebra library for the dot product.
A normalized vector is one that has a length of 1, that is not what you want if you are looking for a length. Calculating the length is the first step for normalizing a vector, but I don't think you need the final step!
To calculate the length you need Pythagoras's Theorem. I'm not going to go into a full description but basically you take the square root of the square of both sides.
In other words multiply dx and dy by themselves, add them together, then square root the result.
r = std::sqrt(dx*dx + dy*dy);
If you really did want to normalize the vector then all you do as the final step is to divide dx and dy both by r. This gives a resulting unit vector of length 1.
You're probably looking for the euclidean norm which is the geometric length of the vector and a scalar value.
double r = std::sqrt(dx*dx + dy*dy);
In contrast to that, normalization of a vector represents the same direction with it length (its euclidean norm ;)) being set to 1. This is again a vector.
Fixed-dimensional vector objects (especially with low dimensionality) lend themselves to be represented as a class type.
A simple example:
namespace wse
{
struct v2d { double x, y; };
inline double dot(v2d const &a, v2d const &b)
{
return a.x*b.x + a.y*b.y;
}
inline double len(v2d const &v) { return std::sqrt(dot(v,v)); }
}
// ...
wse::v2d dp{2.4, 3.4};
// ... Euclidean norm:
auto r = len(dp);
// Normalized vector
wse::v2d normalized_dp{dp.x/r, dp.y/r};
Related
I have some C++ code that is getting a bunch of X,Y values and doing
a linear fit
Eigen::Matrix<float, Eigen::Dynamic, 2> DX;
Eigen::Matrix<float, Eigen::Dynamic, 1> DY;
For loop over the data values (edited a bit because my data source
is a bit more complicated than simple arrays):
{
DX(i,0) = x[i];
DX(i,1) = 1;
DY(i,0) = y[i];
}
then
Eigen::Vector2f Dsolution = DX.colPivHouseholderQr().solve(DY);
// linear solution is in Dsolution[0] and Dsolution[1]
I need the correlation coefficient from that calculation.
How do I obtain it?
Most Eigen stuff is about two floors above my head, so you may need to spell it out in an elementary way.
The fundamental issue is that I'm running this routine on multiple data sets
and I need some indication of the quality of data as regards to internal noise and variance.
Thanks!
I'm assuming you are looking to compute the R² coefficient of your least-square fitting.
Linear least squares
First, let's recap what you're doing. In your Dsolution vector are two coefficients (lets call them a and b, which are your estimated parameters for an affine model between your xs and your y s). This means that for each x[i] your model's estimate for the corresponding y[i] is estimated_y[i] = a * x[i] + b.
a and b are computed by minimizing the sum of the squares of the difference between the observed y[i] and their estimated value a*x[i] + b, also called the residuals. It turns out that you can simply do that by solving a linear problem, which is why you use Eigen's solve() to find them.
Computing R²
Now we want to compute R², which is an indicator of how "good" your fit is.
If we follow the definition from Wikipedia linked above, to compute R² you need to :
Compute the average of the observed values y_avg
Compute the total sum of squares i.e. the sum of the square differences between the observed values and their average (this is like the variance but you don't divide by the number of samples)
Compute the total sum of squared residuals by summing the square of differences between the predicted and observed value of each y
Then R² is 1 - (sum_residuals_squares / sum_squares)
Eigen code
Let's see how we can do this with Eigen :
float r_squared(const MatrixX2f& DX, const VectorXf& DY, const Vector2f& model)
{
// Compute average
const float y_avg = DY.mean();
// Compute total sum of squares
const int N = DX.rows();
const float sum_squares = (DY - (y_avg * VectorXf::Ones(N))).squaredNorm();
// Compute predicted values
const VectorXf estimated_DY = DX * model;
// Compute sum of residual squared
const float sum_residuals_square = (DY - estimated_DY).squaredNorm();
return 1 - (sum_residuals_square / sum_squares);
}
The trick used in both sum of squares's expression is to use the squared norm function, because the squared norm of a vector is the sum of squares of its components. We do it twice because we have two sum of squares to compute.
In the first case, we created a vector of size N full of ones that we multiply by y_avg, to get a vector whose elements are all y_avg. Then each element of DY minus that vector will be y[i] - y_avg, and we compute the square norm to get the total sum of squares.
In the second case, we first compute the predicted y's by using your linear model, and then compute the difference with the observed values, using the squared norm to compute the sum of squared differences.
I'm trying to implement a function that takes two geometry vectors in 3D space and returns a rotation matrix that rotates the first vector to the second vector. My function currently uses Rodrigues' rotation formula to create a matrix, but my implementation of this formula gives the wrong answer for some inputs. I checked the math by hand for one test that gave an incorrect result, and my work gave the same result.
Here is the code for my function:
Matrix3d rotation_matrix(Vector3d vector0, Vector3d vector1)
{
vector0.normalize();
vector1.normalize();
// vector orthogonal to both inputs
Vector3d u = vector0.cross(vector1);
if (!u.norm())
{
if (vector0 == vector1)
return Matrix3d::Identity();
// return rotation matrix that represents 180 degree rotation
Matrix3d m1;
m1 << -1, 0, 0,
0,-1, 0,
0, 0, 1;
return m1;
}
/* For the angle between both inputs:
* 1) The sine is the magnitude of their cross product.
* 2) The cosine equals their dot product.
*/
// sine must be calculated using original cross product
double sine = u.norm();
double cosine = vector0.dot(vector1);
u.normalize();
double ux = u[0];
double uy = u[1];
double uz = u[2];
Matrix3d cross_product_matrix;
cross_product_matrix << 0, -uz, uy,
uz, 0,-ux,
-uy, ux, 0;
Matrix3d part1 = Matrix3d::Identity();
Matrix3d part2 = cross_product_matrix * sine;
Matrix3d part3 = cross_product_matrix*cross_product_matrix * (1 - cosine);
return part1 + part2 + part3;
}
I use the Eigen C++ library for linear algebra (available here):
http://eigen.tuxfamily.org/index.php?title=Main_Page
Any help is appreciated. Thanks.
A one liner version consists in using Eigen's Quaternion:
return Matrix3d(Quaterniond::FromTwoVectors(v0,v1));
If you want to rotate from one vector to another just use built in "Eigen::Quaternion::setFromTwoVectors"
http://eigen.tuxfamily.org/dox/classEigen_1_1QuaternionBase.html#ac35460294d855096e9b687cadf821452
It makes exactly what you need and implementation much faster. Then you can call
"Eigen::Quaternion::toRotationMatrix" , to convert to matrix. Both operations are comparably fast and probably faster than direct Rodrigues formula.
I want to fit a plane to a 3D point cloud. I use a RANSAC approach, where I sample several points from the point cloud, calculate the plane, and store the plane with the smallest error. The error is the distance between the points and the plane. I want to do this in C++, using Eigen.
So far, I sample points from the point cloud and center the data. Now, I need to fit the plane to the samples points. I know I need to solve Mx = 0, but how do I do this? So far I have M (my samples), I want to know x (the plane) and this fit needs to be as close to 0 as possible.
I have no idea where to continue from here. All I have are my sampled points and I need more data.
From you question I assume that you are familiar with the Ransac algorithm, so I will spare you of lengthy talks.
In a first step, you sample three random points. You can use the Random class for that but picking them not truly random usually gives better results. To those points, you can simply fit a plane using Hyperplane::Through.
In the second step, you repetitively cross out some points with large Hyperplane::absDistance and perform a least-squares fit on the remaining ones. It may look like this:
Vector3f mu = mean(points);
Matrix3f covar = covariance(points, mu);
Vector3 normal = smallest_eigenvector(covar);
JacobiSVD<Matrix3f> svd(covariance, ComputeFullU);
Vector3f normal = svd.matrixU().col(2);
Hyperplane<float, 3> result(normal, mu);
Unfortunately, the functions mean and covariance are not built-in, but they are rather straightforward to code.
Recall that the equation for a plane passing through origin is Ax + By + Cz = 0, where (x, y, z) can be any point on the plane and (A, B, C) is the normal vector perpendicular to this plane.
The equation for a general plane (that may or may not pass through origin) is Ax + By + Cz + D = 0, where the additional coefficient D represents how far the plane is away from the origin, along the direction of the normal vector of the plane. [Note that in this equation (A, B, C) forms a unit normal vector.]
Now, we can apply a trick here and fit the plane using only provided point coordinates. Divide both sides by D and rearrange this term to the right-hand side. This leads to A/D x + B/D y + C/D z = -1. [Note that in this equation (A/D, B/D, C/D) forms a normal vector with length 1/D.]
We can set up a system of linear equations accordingly, and then solve it by an Eigen solver as follows.
// Example for 5 points
Eigen::Matrix<double, 5, 3> matA; // row: 5 points; column: xyz coordinates
Eigen::Matrix<double, 5, 1> matB = -1 * Eigen::Matrix<double, 5, 1>::Ones();
// Find the plane normal
Eigen::Vector3d normal = matA.colPivHouseholderQr().solve(matB);
// Check if the fitting is healthy
double D = 1 / normal.norm();
normal.normalize(); // normal is a unit vector from now on
bool planeValid = true;
for (int i = 0; i < 5; ++i) { // compare Ax + By + Cz + D with 0.2 (ideally Ax + By + Cz + D = 0)
if ( fabs( normal(0)*matA(i, 0) + normal(1)*matA(i, 1) + normal(2)*matA(i, 2) + D) > 0.2) {
planeValid = false; // 0.2 is an experimental threshold; can be tuned
break;
}
}
This method is equivalent to the typical SVD-based method, but much faster. It is suitable for use when points are known to be roughly in a plane shape. However, the SVD-based method is more numerically stable (when the plane is far far away from origin) and robust to outliers.
I am implementing a marching cubes algorithm generally based on the implementation of Paul Bourke with some major adjustments:
precalculation of the scalarfield (floating point values)
avoiding duplicated vertices in the final list using a std::map
vertex storing to visualize the final mesh in Ogre3D
Basically I changed nearly 80% of his code. My resulting mesh has some ugly terrasses and I am not sure how to avoid them. I assumed that using floating points for the scalar field would do the job. Is this a common effect? How can you avoid it?
calculating the vertex positions on the edges. (cell.val[p1] contains the scalar value for the given vertex):
//if there is an intersection on this edge
if (cell.iEdgeFlags & (1 << iEdge))
{
const int* edge = a2iEdgeConnection[iEdge];
int p1 = edge[0];
int p2 = edge[1];
//find the approx intersection point by linear interpolation between the two edges and the density value
float length = cell.val[p1] / (cell.val[p2] + cell.val[p1]);
asEdgeVertex[iEdge] = cell.p[p1] + length * (cell.p[p2] - cell.p[p1]);
}
You can find the complete sourcecode here: https://github.com/DieOptimistin/MarchingCubes
I use Ogre3D as library for this example.
As Andy Newmann said, the devil was in the linear interpolation. Correct is:
float offset;
float delta = cell.val[p2] - cell.val[p1];
if (delta == 0) offset = 0.5;
else offset = (mTargetValue - cell.val[p1]) / delta;
asEdgeVertex[iEdge] = cell.p[p1] + offset* (cell.p[p2] - cell.p[p1]);
Im trying to calculate the angle between two edges in a graph, in order to do that I transfer both edges to origin and then used dot product to calculate the angle. my problem is that for some edges like e1 and e2 the output of angle(e1,e2) is -1.#INDOO.
what is this output? is it an error?
Here is my code:
double angle(Edge e1, Edge e2){
Edge t1 = e1, t2 = e2;
Point tail1 = t1.getTail(), head1 = t1.getHead();
Point u(head1.getX() - tail1.getX(), head1.getY() - tail1.getY());
Point tail2 = t2.getTail(), head2 = t2.getHead();
Point v(head2.getX() - tail2.getX(), head2.getY() - tail2.getY());
double dotProduct = u.getX()*v.getX() + u.getY()*v.getY();
double cosAlpha = dotProduct / (e1.getLength()*e2.getLength());
return acos(cosAlpha);
}
Edge is a class that holds two Points, and Point is a class that holds two double numbers as x and y.
Im using angle(e1,e2) to calculate the orthogonal projection length of a vector like b on to a vector like a :
double orthogonalProjectionLength(Edge b, Edge a){
return (b.getLength()*sin(angle(b, a) * (PI / 180)));
}
and this function also sometimes gives me -1.#INDOO. you can see the implementation of Point and Edge here.
My input is a set S of n Points in 2D space. Iv constructed all edges between p and q (p,q are in S) and then tried to calculate the angle like this:
for (int i = 0; i < E.size(); i++)
for (int j = 0; j < E.size(); j++){
if (i == j)
cerr << fixed << angle(E[i], E[j]) << endl; //E : set of all edges
}
If the problem comes from cos() and sin() functions, how can I fix it? is here other libraries that calculate sin and cos in more efficient way?
look at this example.
the inputs in this example are two distinct points(like p and q), and there are two Edges between them (pq and qp). shouldnt the angle(pq , qp) always be 180 ? and angle(pq,pq) and angle(qp,qp) should be 0. my programm shows two different kinds of behavior, sometimes angle(qp,qp) == angle(pq,pq) ==0 and angle(pq , qp) == angle(pq , qp) == 180.0, and sometimes the answer is -1.#INDOO for all four edges.
Here is a code example.
run it for several times and you will see the error.
You want the projection and you go via all this trig? You just need to dot b with the unit vector in the direction of a. So the final answer is
(Xa.Xb + Ya.Yb) / square_root(Xa^2 + Ya^2)
Did you check that cosAlpha doesn't reach 1.000000000000000000000001? That would explain the results, and provide another reason not to go all around the houses like this.
It seems like dividing by zero. Make sure that your vectors always have 0< length.
Answer moved from mine comment
check if your dot product is in <-1,+1> range ...
due to float rounding it can be for example 1.000002045 which will cause acos to fail.
so add two ifs and clamp to this range.
or use faster way: acos(0.99999*dot)
but that lowers the precision for all angles
and also if 0.9999 constant is too big then the error is still present
A recommended way to compute angles is by means of the atan2 function, taking two arguments. It returns the angle on four quadrants.
You can use it in two ways:
compute the angles of u and v separately and subtract: atan2(Vy, Vx) - atan2(Uy, Ux).
compute the cross- and dot-products: atan2(Ux.Vy - Uy.Vx, Ux.Uy + Vx.Vy).
The only case of failure is (0, 0).