Base class for visual odometry motion estimation methods. More...
#include <motion_estimation.h>
Public Types | |
enum | MotionConstraint { NONE = 0, ROLL_PITCH = 1, ROLL_PITCH_Z = 2 } |
Public Member Functions | |
virtual int | getModelSize () const |
Return the size of the internal model. Overriden for classes that use a model. | |
tf::Transform | getMotionEstimation (RGBDFrame &frame) |
Main function for estimating motion. | |
MotionEstimation (const ros::NodeHandle &nh, const ros::NodeHandle &nh_private) | |
Constructor from ROS nodehandles. | |
void | setBaseToCameraTf (const tf::Transform &b2c) |
Set the transformation between the base and camera frames. | |
virtual | ~MotionEstimation () |
Default destructor. | |
Protected Member Functions | |
void | constrainMotion (tf::Transform &motion) |
Constrains the motion in accordance to the class motion constraints. | |
virtual bool | getMotionEstimationImpl (RGBDFrame &frame, const tf::Transform &prediction, tf::Transform &motion)=0 |
Implementation of the motion estimation algorithm. | |
Protected Attributes | |
tf::Transform | b2c_ |
Base (moving) frame to Camera-optical frame. | |
int | motion_constraint_ |
The motion constraint type. | |
ros::NodeHandle | nh_ |
The public nodehandle. | |
ros::NodeHandle | nh_private_ |
The private nodehandle. |
Base class for visual odometry motion estimation methods.
The motion is estimated in increments of the change of pose of the base frame. The increments are expressed wrt fixed frame.
Definition at line 40 of file motion_estimation.h.
Definition at line 44 of file motion_estimation.h.
ccny_rgbd::MotionEstimation::MotionEstimation | ( | const ros::NodeHandle & | nh, |
const ros::NodeHandle & | nh_private | ||
) |
Constructor from ROS nodehandles.
nh | the public nodehandle |
nh_private | the private nodehandle |
Definition at line 28 of file motion_estimation.cpp.
ccny_rgbd::MotionEstimation::~MotionEstimation | ( | ) | [virtual] |
Default destructor.
Definition at line 39 of file motion_estimation.cpp.
void ccny_rgbd::MotionEstimation::constrainMotion | ( | tf::Transform & | motion | ) | [protected] |
Constrains the motion in accordance to the class motion constraints.
This method can be called by the inheriting classes if desired.
motion | the incremental motion which is constrained. |
Definition at line 75 of file motion_estimation.cpp.
virtual int ccny_rgbd::MotionEstimation::getModelSize | ( | ) | const [inline, virtual] |
Return the size of the internal model. Overriden for classes that use a model.
Reimplemented in ccny_rgbd::MotionEstimationICPProbModel, and ccny_rgbd::MotionEstimationICP.
Definition at line 79 of file motion_estimation.h.
Main function for estimating motion.
The motion is equal to the change of pose of the base frame, and is expressed wrt the fixed frame
Pose_new = motion * Pose_old;
frame | The RGBD Frame for which the motion is estimated |
Definition at line 44 of file motion_estimation.cpp.
virtual bool ccny_rgbd::MotionEstimation::getMotionEstimationImpl | ( | RGBDFrame & | frame, |
const tf::Transform & | prediction, | ||
tf::Transform & | motion | ||
) | [protected, pure virtual] |
Implementation of the motion estimation algorithm.
frame | the current RGBD frame |
prediction | the motion prediction (currently ignored) |
motion | the output motion |
true | the motion estimation was successful |
false | the motion estimation failed |
Implemented in ccny_rgbd::MotionEstimationICPProbModel, and ccny_rgbd::MotionEstimationICP.
void ccny_rgbd::MotionEstimation::setBaseToCameraTf | ( | const tf::Transform & | b2c | ) |
Set the transformation between the base and camera frames.
b2c | The transform from the base frame to the camera frame, expressed wrt the base frame. |
Definition at line 98 of file motion_estimation.cpp.
tf::Transform ccny_rgbd::MotionEstimation::b2c_ [protected] |
Base (moving) frame to Camera-optical frame.
Definition at line 86 of file motion_estimation.h.
int ccny_rgbd::MotionEstimation::motion_constraint_ [protected] |
The motion constraint type.
Definition at line 88 of file motion_estimation.h.
ros::NodeHandle ccny_rgbd::MotionEstimation::nh_ [protected] |
The public nodehandle.
Definition at line 83 of file motion_estimation.h.
The private nodehandle.
Definition at line 84 of file motion_estimation.h.