tf::Transform MotionEstimation::getMotionEstimation(RGBDFrame& frame)
{
///@todo this should return a covariance
// motion prediction
/// @todo motion prediction disabled for now
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;
}
AffineTransform MotionEstimation::getMotionEstimation(RGBDFrame& frame)
{
///@todo this should return a covariance
use_Descriptors_ = true;
// motion prediction
/// @todo motion prediction disabled for now
AffineTransform prediction;
prediction.setIdentity();
AffineTransform motion;
bool result;
if (frame.n_valid_keypoints == 0)
{
std::cerr << "No features detected." << std::endl;
result = false;
}
else
{
use_Descriptors_ = false;
if(use_Descriptors_)
result = getMotionEstimationImplDescriptor(frame, prediction, motion);
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;
}
