motion_estimation.cpp

Go to the documentation of this file.

#include "rgbdtools/registration/motion_estimation.h"

namespace rgbdtools {

MotionEstimation::MotionEstimation()
{

}

MotionEstimation::~MotionEstimation()
{

}

AffineTransform MotionEstimation::getMotionEstimation(
  RGBDFrame& frame,
  const AffineTransform& prediction)
{
  
  AffineTransform motion;
  bool result;

  if (frame.n_valid_keypoints == 0)
  {
    std::cerr << "No features detected." << std::endl;
    result = false;
  }
  else
  {
    result = getMotionEstimationImpl(frame, prediction, motion);
  }

  if (!result)
  {
    std::cerr << "Could not estimate motion from RGBD data, using Identity transform." 
              << std::endl;
    motion.setIdentity();
  }

  return motion;
}

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

  if (motion_constraint_ == ROLL_PITCH)
  {
    float x, y, z, roll, pitch, yaw;
    eigenAffineToXYZRPY(motion, x, y, z, roll, pitch, yaw);
    XYZRPYToEigenAffine(x, y, z, 0, 0, yaw, motion);
  }
  else if (motion_constraint_ == ROLL_PITCH_Z)
  {
    float x, y, z, roll, pitch, yaw;
    eigenAffineToXYZRPY(motion, x, y, z, roll, pitch, yaw);
    XYZRPYToEigenAffine(x, y, 0, 0, 0, yaw, motion);
  }
}

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

void MotionEstimation::setMotionConstraint(int motion_constraint)
{
  motion_constraint_ = motion_constraint;
}

} // namespace rgbdtools