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, NONE = 0, ROLL_PITCH = 1, ROLL_PITCH_Z = 2 } |
enum | MotionConstraint { NONE = 0, ROLL_PITCH = 1, ROLL_PITCH_Z = 2, 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. | |
virtual int | getModelSize () const |
Return the size of the internal model. Overriden for classes that use a model. | |
AffineTransform | getMotionEstimation (RGBDFrame &frame, const AffineTransform &prediction=AffineTransform::Identity()) |
Main function for estimating motion. | |
AffineTransform | getMotionEstimation (RGBDFrame &frame, const AffineTransform &prediction=AffineTransform::Identity()) |
Main function for estimating motion. | |
MotionEstimation () | |
Constructor from ROS nodehandles. | |
MotionEstimation () | |
Constructor from ROS nodehandles. | |
void | setBaseToCameraTf (const AffineTransform &b2c) |
Set the transformation between the base and camera frames. | |
void | setBaseToCameraTf (const AffineTransform &b2c) |
Set the transformation between the base and camera frames. | |
void | setMotionConstraint (int motion_constraint) |
void | setMotionConstraint (int motion_constraint) |
virtual | ~MotionEstimation () |
Default destructor. | |
virtual | ~MotionEstimation () |
Default destructor. | |
Protected Member Functions | |
void | constrainMotion (AffineTransform &motion) |
Constrains the motion in accordance to the class motion constraints. | |
void | constrainMotion (AffineTransform &motion) |
Constrains the motion in accordance to the class motion constraints. | |
virtual bool | getMotionEstimationImpl (RGBDFrame &frame, const AffineTransform &prediction, AffineTransform &motion)=0 |
Implementation of the motion estimation algorithm. | |
virtual bool | getMotionEstimationImpl (RGBDFrame &frame, const AffineTransform &prediction, AffineTransform &motion)=0 |
Implementation of the motion estimation algorithm. | |
Protected Attributes | |
AffineTransform | b2c_ |
Base (moving) frame to Camera-optical frame. | |
int | motion_constraint_ |
The motion constraint type. |
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 39 of file include/rgbdtools/registration/motion_estimation.h.
Definition at line 43 of file include/rgbdtools/registration/motion_estimation.h.
Definition at line 43 of file rgbdtools_git/include/rgbdtools/registration/motion_estimation.h.
Constructor from ROS nodehandles.
nh | the public nodehandle |
nh_private | the private nodehandle |
Definition at line 28 of file motion_estimation.cpp.
rgbdtools::MotionEstimation::~MotionEstimation | ( | ) | [virtual] |
Default destructor.
Definition at line 33 of file motion_estimation.cpp.
Constructor from ROS nodehandles.
nh | the public nodehandle |
nh_private | the private nodehandle |
virtual rgbdtools::MotionEstimation::~MotionEstimation | ( | ) | [virtual] |
Default destructor.
void rgbdtools::MotionEstimation::constrainMotion | ( | AffineTransform & | 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 67 of file motion_estimation.cpp.
void rgbdtools::MotionEstimation::constrainMotion | ( | AffineTransform & | 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. |
virtual int rgbdtools::MotionEstimation::getModelSize | ( | ) | const [inline, virtual] |
Return the size of the internal model. Overriden for classes that use a model.
Reimplemented in rgbdtools::MotionEstimationICPProbModel, and rgbdtools::MotionEstimationICPProbModel.
Definition at line 82 of file rgbdtools_git/include/rgbdtools/registration/motion_estimation.h.
virtual int rgbdtools::MotionEstimation::getModelSize | ( | ) | const [inline, virtual] |
Return the size of the internal model. Overriden for classes that use a model.
Reimplemented in rgbdtools::MotionEstimationICPProbModel, and rgbdtools::MotionEstimationICPProbModel.
Definition at line 82 of file include/rgbdtools/registration/motion_estimation.h.
AffineTransform rgbdtools::MotionEstimation::getMotionEstimation | ( | RGBDFrame & | frame, |
const AffineTransform & | prediction = AffineTransform::Identity() |
||
) |
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 38 of file motion_estimation.cpp.
AffineTransform rgbdtools::MotionEstimation::getMotionEstimation | ( | RGBDFrame & | frame, |
const AffineTransform & | prediction = AffineTransform::Identity() |
||
) |
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 |
virtual bool rgbdtools::MotionEstimation::getMotionEstimationImpl | ( | RGBDFrame & | frame, |
const AffineTransform & | prediction, | ||
AffineTransform & | 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 rgbdtools::MotionEstimationPairwiseRANSAC, rgbdtools::MotionEstimationPairwiseRANSAC, rgbdtools::MotionEstimationICPProbModel, and rgbdtools::MotionEstimationICPProbModel.
virtual bool rgbdtools::MotionEstimation::getMotionEstimationImpl | ( | RGBDFrame & | frame, |
const AffineTransform & | prediction, | ||
AffineTransform & | 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 rgbdtools::MotionEstimationPairwiseRANSAC, rgbdtools::MotionEstimationPairwiseRANSAC, rgbdtools::MotionEstimationICPProbModel, and rgbdtools::MotionEstimationICPProbModel.
void rgbdtools::MotionEstimation::setBaseToCameraTf | ( | const AffineTransform & | 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. |
void rgbdtools::MotionEstimation::setBaseToCameraTf | ( | const AffineTransform & | 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 85 of file motion_estimation.cpp.
void rgbdtools::MotionEstimation::setMotionConstraint | ( | int | motion_constraint | ) |
Definition at line 90 of file motion_estimation.cpp.
void rgbdtools::MotionEstimation::setMotionConstraint | ( | int | motion_constraint | ) |
AffineTransform rgbdtools::MotionEstimation::b2c_ [protected] |
Base (moving) frame to Camera-optical frame.
Definition at line 86 of file include/rgbdtools/registration/motion_estimation.h.
int rgbdtools::MotionEstimation::motion_constraint_ [protected] |
The motion constraint type.
Definition at line 88 of file include/rgbdtools/registration/motion_estimation.h.