motion_estimation.cpp

Go to the documentation of this file.

#include "ccny_rgbd/registration/motion_estimation.h"

namespace ccny_rgbd {

MotionEstimation::MotionEstimation(
  const ros::NodeHandle& nh, 
  const ros::NodeHandle& nh_private):
  nh_(nh), 
  nh_private_(nh_private)
{
  // params
  if (!nh_private_.getParam ("reg/motion_constraint", motion_constraint_ ))
    motion_constraint_  = 0;
}

MotionEstimation::~MotionEstimation()
{

}

tf::Transform MotionEstimation::getMotionEstimation(RGBDFrame& frame)
{
  
  // motion prediction 
  tf::Transform prediction;
  prediction.setIdentity();

  tf::Transform motion;
  bool result;

  if (frame.n_valid_keypoints == 0)
  {
    ROS_WARN("No features detected.");
    result = false;
  }
  else
  {
    result = getMotionEstimationImpl(frame, prediction, motion);
  }

  if (!result)
  {
    ROS_WARN("Could not estimate motion from RGBD data, using Identity transform.");
    motion.setIdentity();
  }

  return motion;
}

void MotionEstimation::constrainMotion(tf::Transform& motion)
{
  // **** degree-of-freedom constraints

  if (motion_constraint_ == ROLL_PITCH)
  {
    tf::Quaternion q;
    q.setRPY(0.0, 0.0, tf::getYaw(motion.getRotation()));

    motion.setRotation(q); 
  }
  else if (motion_constraint_ == ROLL_PITCH_Z)
  {
    tf::Quaternion q;
    q.setRPY(0.0, 0.0, tf::getYaw(motion.getRotation()));
    
    tf::Vector3 p(motion.getOrigin().getX(), motion.getOrigin().getY(), 0.0);
    
    motion.setOrigin(p);
    motion.setRotation(q); 
  }
}

void MotionEstimation::setBaseToCameraTf(const tf::Transform& b2c)
{
  b2c_ = b2c;
}

} // namespace ccny_rgbd