motion_estimation.cpp
Go to the documentation of this file.
00001 
00024 #include "ccny_rgbd/registration/motion_estimation.h"
00025 
00026 namespace ccny_rgbd {
00027 
00028 MotionEstimation::MotionEstimation(
00029   const ros::NodeHandle& nh, 
00030   const ros::NodeHandle& nh_private):
00031   nh_(nh), 
00032   nh_private_(nh_private)
00033 {
00034   // params
00035   if (!nh_private_.getParam ("reg/motion_constraint", motion_constraint_ ))
00036     motion_constraint_  = 0;
00037 }
00038 
00039 MotionEstimation::~MotionEstimation()
00040 {
00041 
00042 }
00043 
00044 tf::Transform MotionEstimation::getMotionEstimation(RGBDFrame& frame)
00045 {
00047   
00048   // motion prediction 
00050   tf::Transform prediction;
00051   prediction.setIdentity();
00052 
00053   tf::Transform motion;
00054   bool result;
00055 
00056   if (frame.n_valid_keypoints == 0)
00057   {
00058     ROS_WARN("No features detected.");
00059     result = false;
00060   }
00061   else
00062   {
00063     result = getMotionEstimationImpl(frame, prediction, motion);
00064   }
00065 
00066   if (!result)
00067   {
00068     ROS_WARN("Could not estimate motion from RGBD data, using Identity transform.");
00069     motion.setIdentity();
00070   }
00071 
00072   return motion;
00073 }
00074 
00075 void MotionEstimation::constrainMotion(tf::Transform& motion)
00076 {
00077   // **** degree-of-freedom constraints
00078 
00079   if (motion_constraint_ == ROLL_PITCH)
00080   {
00081     tf::Quaternion q;
00082     q.setRPY(0.0, 0.0, tf::getYaw(motion.getRotation()));
00083 
00084     motion.setRotation(q); 
00085   }
00086   else if (motion_constraint_ == ROLL_PITCH_Z)
00087   {
00088     tf::Quaternion q;
00089     q.setRPY(0.0, 0.0, tf::getYaw(motion.getRotation()));
00090     
00091     tf::Vector3 p(motion.getOrigin().getX(), motion.getOrigin().getY(), 0.0);
00092     
00093     motion.setOrigin(p);
00094     motion.setRotation(q); 
00095   }
00096 }
00097 
00098 void MotionEstimation::setBaseToCameraTf(const tf::Transform& b2c)
00099 {
00100   b2c_ = b2c;
00101 }
00102 
00103 } // namespace ccny_rgbd
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends


ccny_rgbd
Author(s): Ivan Dryanovski
autogenerated on Fri Apr 12 2013 20:38:48