how t use this function "pcl::geometry::squaredDistance" correctly

Question

How can I get the distance between 2 points in PCL? I know there is a function pcl::geometry::squaredDistance in PCL, but when I call this function I get this error

/usr/include/pcl-1.7/pcl/common/geometry.h: In instantiation of ‘float pcl::geometry::squaredDistance(const PointT&, const PointT&) [with PointT = pcl::PointXYZ]’:
error: no match for ‘operator-’ (operand types are ‘const pcl::PointXYZ’ and ‘const pcl::PointXYZ’)
Eigen::Vector3f diff = p1 -p2;
                          ^

Here is the code showing how I use the function

    pcl::PointXYZ p1(3, 4, 5);

    pcl::PointXYZ p2(0, 0, 0);

    double d = pcl::geometry::squaredDistance(p1, p2);

    std::cout << d << std::endl;

Any help would be appreciated.

Answer 1

This is a bug in PCL 1.7 (that was fixed in linked commit).

It is a simple function, so you don't have to update to a newer version of PCL:

return the euclidean distance between 2 points

    template <typename PointT> inline float 
    distance (const PointT& p1, const PointT& p2)
    {
      Eigen::Vector3f diff = p1.getVector3fMap () - p2.getVector3fMap ();
      return (diff.norm ());
    }

return the squared euclidean distance between 2 points (which is faster to calculate than the true euclidean distance)

    template<typename PointT> inline float 
    squaredDistance (const PointT& p1, const PointT& p2)
    {
      Eigen::Vector3f diff = p1.getVector3fMap () - p2.getVector3fMap ();
      return (diff.squaredNorm ());
    }