Pose.cpp

Go to the documentation of this file.

#include "Pose.h"

namespace ResourcesForPsm {
  
  Pose::Pose()
  {
    mPosition.reset(new Eigen::Vector3d(0, 0, 0));
    mOrientation.reset(new Eigen::Quaternion<double>(1, 0, 0, 0));
  }
  
  Pose::Pose(const asr_msgs::AsrObject pObject)
  {

    if(!pObject.sampledPoses.size()){
      std::cerr << "Got a AsrObject without poses." << std::endl;
      std::exit(1);    
    }

    // Copy the position.
    mPosition.reset(new Eigen::Vector3d(pObject.sampledPoses.front().pose.position.x,
                                        pObject.sampledPoses.front().pose.position.y,
                                        pObject.sampledPoses.front().pose.position.z));
    
    // Copy the orientation.
    mOrientation.reset(new Eigen::Quaternion<double>(pObject.sampledPoses.front().pose.orientation.w,
                                                     pObject.sampledPoses.front().pose.orientation.x,
                                                     pObject.sampledPoses.front().pose.orientation.y,
                                                     pObject.sampledPoses.front().pose.orientation.z));

  }
  
  Pose::Pose(const geometry_msgs::Pose pPose)
  {
    // Copy the position.
    mPosition.reset(new Eigen::Vector3d(pPose.position.x,
                                        pPose.position.y,
                                        pPose.position.z));
    
    // Copy the orientation.
    mOrientation.reset(new Eigen::Quaternion<double>(pPose.orientation.w,
                                                     pPose.orientation.x,
                                                     pPose.orientation.y,
                                                     pPose.orientation.z));
  }
  
  Pose::Pose(const boost::shared_ptr<SceneModel::Object> pObject)
  { 
    mPosition = pObject->mPosition;
    mOrientation = pObject->mOrientation;
  }
  
  Pose::~Pose()
  {
  }
  
  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->mPosition.reset(new Eigen::Vector3d(pFrame->mOrientation->toRotationMatrix().inverse() * (*pFrame->mPosition - *mPosition)));
    pResult->mPosition.reset(new Eigen::Vector3d(pFrame->mOrientation->toRotationMatrix().inverse() * (*mPosition - *pFrame->mPosition)));
    
    // The relative orientation is defined as the difference between the orientation of parent and child.
    pResult->mOrientation.reset(new Eigen::Quaternion<double>(*mOrientation * pFrame->mOrientation->inverse()));
  }
  
  Eigen::Vector3d Pose::getPosition()
  {
    return *mPosition;
  }
  
  Eigen::Quaternion<double> Pose::getOrientation()
  {
    return *mOrientation;
  }
  
}