Pose.cpp

Go to the documentation of this file.

#include "Pose.hpp"
#include "JsonStream.hpp"
#include "utility/GeometryHelper.hpp"

namespace ISM {
  std::ostream& operator<<(std::ostream &strm, const ISM::Pose &p) {
    return strm<<"pose: ["<<p.point<<";"<<p.quat<<"]";
  }

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

  bool operator==(const PosePtr& pose1, const PosePtr& pose2)
  {
      return *pose1 == *pose2;
  }

  bool operator==(const Pose& pose1, const Pose& pose2)
  {
      return pose1.point == pose2.point && pose1.quat == pose2.quat;
  }

  void Pose::serialize(std::ostream& strm) const {
    strm<<"{"<<std::endl<<"\"point\": "<<json(this->point)<<", "<<std::endl
    <<"\"quaternion\": "<<json(this->quat)<<", "<<std::endl
        <<"\"xAxis\": "<<json(GeometryHelper::vectorToPoint(GeometryHelper::getAxisFromQuat(this->quat)))
        <<std::endl<<"}";
  }

  void Pose::convertPoseIntoFrame(const boost::shared_ptr<Pose>& pFrame, boost::shared_ptr<Pose>& pResult)
  {
    // Initialize the result pointer.
    pResult.reset(new Pose());

    // Calculate the relative position by subtracting the position of the child from the parent position.
    // Rotate the resulting relative position into the parent frame.
    pResult->point.reset(new ISM::Point(pFrame->quat->getEigen().toRotationMatrix().inverse() * (point->getEigen() - pFrame->point->getEigen())));

    // The relative orientation is defined as the difference between the orientation of parent and child.
    pResult->quat.reset(new ISM::Quaternion(quat->getEigen() * pFrame->quat->getEigen().inverse()));
  }
}