Problem background
Let's say I have a bunch of points on the 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:
// 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) );
}
Unupgraded version:
bool sortEuclideanToOriginalPoint(const Eigen::Vector3d &ev3dPtsOnLine1, const Eigen::Vector3d &ev3dPtsOnLine2, const Eigen::Vector3d &ev3dOriginalPoint)
{
return( calculateEuclideanDistance( ev3dPtsOnLine1, ev3dOriginalPoint) < calculateEuclideanDistance( ev3dPtsOnLine2, ev3dOriginalPoint) );
}
std::vector<Eigen::Vector3d> vEv3dPtsOnLine;
double dPtToOriginalPoint; // calculate the square root of distance between ePtsOnLine and ev3dOriginalPoint
for (int nIndex = 0; nIndex < vEv3dPtsOnLine.size(); nIndex++)
{
dPtToEnd = std::sqrt( calculateEuclideanDistance( vEv3dPtsOnLine.at(nIndex), ev3dOriginalPoint ) );
vE4dPtsOnLine.push_back( Eigen::Vector4d(vEPtsOnLine.at(nIndex)(0), vEPtsOnLine.at(nIndex)(1), vEPtsOnLine.at(nIndex)(2), ev3dOriginalPoint) );
}
// In this way to get errors
std::sort (vEv4dPtsOnLine.begin(), vEv4dPtsOnLine.end(), sortEuclideanToOriginalPoint( , , ) );
I look for the usage of std::sort
[1], and the comparison function seems forbidden to take the third variable.
So I update my code, using Eigen::Vector4d
instead of Eigen::Vector3d
, So the euclidean distance can be taken in the variable itself and no need for a third one.
// Load the square Euclidean distances of ePtsOnLine and eLineEnd to Eigen::Vector4d
// So std::sort can be used directly
std::vector<Eigen::Vector4d> vEv4dPtsOnLine;
double dPtToOriginalPoint; // calculate the square root of distance between ePtsOnLine and ev3dOriginalPoint
for (int nIndex = 0; nIndex < vEPtsOnLine.size(); nIndex++)
{
// calculate the distance with closet poleEnd. Here is poleEnd1
dPtToEnd = std::sqrt( calculateEuclideanDistance( vEv4dPtsOnLine.at(nIndex), ev3dOriginalPoint ) );
vE4dPtsOnLine.push_back( Eigen::Vector4d(vEPtsOnLine.at(nIndex)(0), vEPtsOnLine.at(nIndex)(1), vEPtsOnLine.at(nIndex)(2), ev3dOriginalPoint) );
}
std::sort (vEv4dPtsOnLine.begin(), vEv4dPtsOnLine.end(), sortEuclideanToOriginalPoint );