Transform Odometry::process(const SensorData & data, OdometryInfo * info)
{
if(_pose.isNull())
{
_pose.setIdentity(); // initialized
}
UASSERT(!data.image().empty());
if(dynamic_cast<OdometryMono*>(this) == 0)
{
UASSERT(!data.depthOrRightImage().empty());
}
if(data.fx() <= 0 || data.fyOrBaseline() <= 0)
{
UERROR("Rectified images required! Calibrate your camera. (fx=%f, fy/baseline=%f, cx=%f, cy=%f)",
data.fx(), data.fyOrBaseline(), data.cx(), data.cy());
return Transform();
}
UTimer time;
Transform t = this->computeTransform(data, info);
if(info)
{
info->time = time.elapsed();
info->lost = t.isNull();
}
if(!t.isNull())
{
_resetCurrentCount = _resetCountdown;
if(_force2D)
{
float x,y,z, roll,pitch,yaw;
t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
t = Transform(x,y,0, 0,0,yaw);
}
return _pose *= t; // updated
}
else if(_resetCurrentCount > 0)
{
UWARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", _resetCurrentCount);
--_resetCurrentCount;
if(_resetCurrentCount == 0)
{
UWARN("Odometry automatically reset to latest pose!");
this->reset(_pose);
}
}
return Transform();
}
// return not null transform if odometry is correctly computed
Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * info)
{
UTimer timer;
Transform output;
bool hasConverged = false;
double variance = 0;
unsigned int minPoints = 100;
if(!data.depth().empty())
{
if(data.depth().type() == CV_8UC1)
{
UERROR("ICP 3D cannot be done on stereo images!");
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
data.depth(),
data.fx(),
data.fy(),
data.cx(),
data.cy(),
_decimation,
this->getMaxDepth(),
_voxelSize,
_samples,
data.localTransform());
if(_pointToPlane)
{
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(newCloud);
if(newCloudXYZ->size() != newCloud->size())
{
UWARN("removed nan normals...");
}
if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints)
{
int correspondences = 0;
Transform transform = util3d::icpPointToPlane(newCloud,
_previousCloudNormal,
_maxCorrespondenceDistance,
_maxIterations,
&hasConverged,
&variance,
&correspondences);
// verify if there are enough correspondences
float correspondencesRatio = float(correspondences)/float(_previousCloudNormal->size()>newCloud->size()?_previousCloudNormal->size():newCloud->size());
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= _correspondenceRatio)
{
output = transform;
_previousCloudNormal = newCloud;
}
else
{
UWARN("Transform not valid (hasConverged=%s variance = %f)",
hasConverged?"true":"false", variance);
}
}
else if(newCloud->size() > minPoints)
{
output.setIdentity();
_previousCloudNormal = newCloud;
}
}
else
{
//point to point
if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints)
{
int correspondences = 0;
Transform transform = util3d::icp(newCloudXYZ,
_previousCloud,
_maxCorrespondenceDistance,
_maxIterations,
&hasConverged,
&variance,
&correspondences);
// verify if there are enough correspondences
float correspondencesRatio = float(correspondences)/float(_previousCloud->size()>newCloudXYZ->size()?_previousCloud->size():newCloudXYZ->size());
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= _correspondenceRatio)
{
output = transform;
_previousCloud = newCloudXYZ;
}
else
{
UWARN("Transform not valid (hasConverged=%s variance = %f)",
hasConverged?"true":"false", variance);
}
}
else if(newCloudXYZ->size() > minPoints)
{
output.setIdentity();
_previousCloud = newCloudXYZ;
}
}
}
else
{
UERROR("Depth is empty?!?");
}
if(info)
{
info->variance = variance;
}
UINFO("Odom update time = %fs hasConverged=%s variance=%f cloud=%d",
timer.elapsed(),
hasConverged?"true":"false",
variance,
(int)(_pointToPlane?_previousCloudNormal->size():_previousCloud->size()));
return output;
}
