template <typename PointT> bool
pcl::registration::ELCH<PointT>::initCompute ()
{
/*if (!PCLBase<PointT>::initCompute ())
{
PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed.\n");
return (false);
}*/ //TODO
if (loop_end_ == 0)
{
PCL_ERROR ("[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
deinitCompute ();
return (false);
}
//compute transformation if it's not given
if (compute_loop_)
{
PointCloudPtr meta_start (new PointCloud);
PointCloudPtr meta_end (new PointCloud);
*meta_start = *(*loop_graph_)[loop_start_].cloud;
*meta_end = *(*loop_graph_)[loop_end_].cloud;
typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++)
*meta_start += *(*loop_graph_)[*si].cloud;
for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++)
*meta_end += *(*loop_graph_)[*si].cloud;
//TODO use real pose instead of centroid
//Eigen::Vector4f pose_start;
//pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start);
//Eigen::Vector4f pose_end;
//pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end);
PointCloudPtr tmp (new PointCloud);
//Eigen::Vector4f diff = pose_start - pose_end;
//Eigen::Translation3f translation (diff.head (3));
//Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity ();
//pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans);
reg_->setInputTarget (meta_start);
reg_->setInputCloud (meta_end);
reg_->align (*tmp);
loop_transform_ = reg_->getFinalTransformation ();
//TODO hack
//loop_transform_ *= trans.matrix ();
}
return (true);
}
static void meta_flush(void)
{
if (!xymonmetaqueued) {
dbgprintf("Flush, but xymonmeta is empty\n");
return;
}
sendmessage(STRBUF(metamsg), NULL, XYMON_TIMEOUT, NULL);
meta_start(); /* Get ready for the next */
}
