Point.cpp

Go to the documentation of this file.

#include "Point.hpp"
#include "JsonStream.hpp"
#include "recognizer/VotingSpace.hpp"

#include "math.h"

#include <gtest/internal/gtest-internal.h>

namespace ISM {
    std::ostream& operator<<(std::ostream &strm, const ISM::Point &p) {
      return strm<<"point: ["<<p.eigen.x()<<";"<<p.eigen.y()<<";"<<p.eigen.z()<<"]";
    }

    std::ostream& operator<<(std::ostream &strm, const ISM::PointPtr &p) {
        return strm<<(*p);
    }

    void Point::serialize(std::ostream& strm) const {
      strm<<"{\"x\": "<<this->eigen.x()<<", \"y\": "<<this->eigen.y()<<", \"z\": "<<this->eigen.z()<<"}";
    }

    bool operator==(const PointPtr& p1, const PointPtr& p2)
        {
        return *p1 == *p2;
        }

    bool operator==(const Point& p1, const Point& p2)
        {
        /*
         * this would be the correct way to do it but since pose from vote has inaccuracy
         * use threshold
        testing::internal::FloatingPoint<double> x1(p1.eigen.x()), x2(eigen.x());
        testing::internal::FloatingPoint<double> y1(p1.eigen.y()), y2(eigen.y());
        testing::internal::FloatingPoint<double> z1(p1.eigen.z()), z2(eigen.z());
        return x1.AlmostEquals(x2) && y1.AlmostEquals(y2) && z1.AlmostEquals(z2);
        */

        //threshold determined statistically
        const double threshold = 1e-15;
        double diffX = fabs(p1.eigen.x() - p2.eigen.x());
        double diffY = fabs(p1.eigen.y() - p2.eigen.y());
        double diffZ = fabs(p1.eigen.z() - p2.eigen.z());
        return diffX < threshold && diffY < threshold && diffZ < threshold;
        }

    Eigen::Vector3d Point::getEigen() {
        return this->eigen;
    }
}