Problem background
Let's say I have a bunch of points on a 3D line. Each of them is an Eigen::Vector3d
variable with three values of x, y, and z of its coordinate. The algorithm is to sort them by the distance to the original point in order from the closest to farthest.
Coding
At first, the code snippet is:
#include <vector>
#include <math.h>
#include <algorithm>
// Eigen
#include <Eigen/Core>
// 'Cause we just want to sort them, it doesn't matter about whether should take the square root of them
double calculateEuclideanDistance(const Eigen::Vector3d &ev3dPt1, const Eigen::Vector3d &ev3dPt2)
{
return ( (ev3dPt1 - ev3dPt2).dot(ev3dPt1 - ev3dPt2) );
}
Not upgraded version:
bool sortEuclideanToOriginalPoint(const Eigen::Vector3d &ev3dPtsOnLine1, const Eigen::Vector3d &ev3dPtsOnLine2, const Eigen::Vector3d &ev3dOriginalPoint)
{
return( calculateEuclideanDistance( ev3dPtsOnLine1, ev3dOriginalPoint) < calculateEuclideanDistance( ev3dPtsOnLine2, ev3dOriginalPoint) );
}
std::vevtor<Eigen::Vector3d> vEv3dPtsOnLine; // contains points on line
// In this way getting errors
std::sort (vEv3dPtsOnLine.begin(), vEv3dPtsOnLine.end(), sortEuclideanToOriginalPoint( , , ev3dOriginalPoint) );
I look for the usage of std::sort
[1], and the comparison function seems forbidden to take the third variable.
Updated code: using Eigen::Vector4d
instead of Eigen::Vector3d
, the euclidean distance can be taken in the variable itself and no need for a third one.
// Comparison function with third variable
// the distance compared is stored in the fouth value, index [3]
// it can be retrieved easily
bool sortEuclideanToOriginalPoint(const Eigen::Vector4d &ev4dPtsOnLine1, const Eigen::Vector4d &ev4dPtsOnLine2)
{
return( ev4dPtsOnLine1[3] < ev4dPtsOnLine2[3] );
}
std::vevtor<Eigen::Vector3d> vEv3dPtsOnLine; // contains points on line
// Load the square Euclidean distances of vEv3dPtsOnLine and ev3dOriginalPoint to the fouth value of Eigen::Vector4d, vEv3dPtsOnLine[3]
// So std::sort can be used directly
std::vector<Eigen::Vector4d> vEv3dPtsOnLine;
double dDisToOriginalPoint; // calculate the square root of distance between vEv3dPtsOnLine and ev3dOriginalPoint
for (int nIndex = 0; nIndex < vEPtsOnLine.size(); nIndex++)
{
dDisToOriginalPoint = std::sqrt( calculateEuclideanDistance( vEv3dPtsOnLine.at(nIndex), dPtToOriginalPoint ) );
vE4dPtsOnLine.push_back( Eigen::Vector4d(vEv3dPtsOnLine.at(nIndex)(0), vEv3dPtsOnLine.at(nIndex)(1), vEv3dPtsOnLine.at(nIndex)(2), dDisToOriginalPoint) );
}
std::sort (vEv4dPtsOnLine.begin(), vEv4dPtsOnLine.end(), sortEuclideanToOriginalPoint );
A little trick is needed by the brain, not by the algorithm.