Skip to content

Commit

Permalink
Big ELCH cleanup.
Browse files Browse the repository at this point in the history 
elch.h (API changes):
- Use Boost bundled properties for LoopGraph, every vertex stores a
  pointer to the corresponding point cloud in graph[vertex].cloud.
- loop_graph_ is now stored as a shared_ptr.
- loop_start_ and loop_end_ are saved as ints for now (will change in
  the future).
- loop_transform_ is not stored as a pointer (may change in the future).

elch.hpp:
- Remove call to PCLBase::initCompute() as ELCH doesn't use a single
  input cloud or indices.
- Change loopOptimizerAlgorithm to use start and end of the loop as
  stored in the object.
- Adapt to API changes in elch.h.

git-svn-id: svn+ssh://svn.pointclouds.org/pcl/trunk@3124 a9d63959-f2ad-4865-b262-bf0e56cfafb6
  • Loading branch information
@jspricke
jspricke committed Nov 10, 2011
1 parent 8ab2f72 commit 934bcaf
Show file tree
Hide file tree
Showing 2 changed files with 56 additions and 39 deletions.
48 changes: 31 additions & 17 deletions registration/include/pcl/registration/elch.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/shared_ptr.hpp>

#include <Eigen/Geometry>

Expand Down Expand Up @@ -70,19 +71,27 @@ namespace pcl
typedef typename PointCloud::Ptr PointCloudPtr;
typedef typename PointCloud::ConstPtr PointCloudConstPtr;

struct Vertex
{
PointCloudPtr cloud;
std::string name;
};

/** \brief graph structure to hold the SLAM graph */
typedef boost::adjacency_list<
boost::vecS, boost::vecS, boost::undirectedS,
PointCloudPtr,
boost::listS, boost::vecS, boost::undirectedS,
Vertex,
boost::no_property>
LoopGraph;

typedef boost::shared_ptr< LoopGraph > LoopGraphPtr;

typedef typename pcl::Registration<PointT, PointT> Registration;
typedef typename Registration::Ptr RegistrationPtr;
typedef typename Registration::ConstPtr RegistrationConstPtr;

/** \brief Empty constructor. */
ELCH () : loop_graph_ (), loop_start_ (), loop_end_ (), reg_(new pcl::IterativeClosestPoint<PointT, PointT>)
ELCH () : loop_graph_ (new LoopGraph), loop_start_ (0), loop_end_ (0), reg_(new pcl::IterativeClosestPoint<PointT, PointT>), compute_loop_(true)
{};

/** \brief Add a new point cloud to the internal graph.
Expand All @@ -92,10 +101,11 @@ namespace pcl
addPointCloud (PointCloudConstPtr cloud)
{
add_vertex (loop_graph_, cloud);
//TODO add edge as well
}

/** \brief Getter for the internal graph. */
inline LoopGraph
inline LoopGraphPtr
getLoopGraph ()
{
return (loop_graph_);
Expand All @@ -105,13 +115,13 @@ namespace pcl
* \param[in] loop_graph the new graph
*/
inline void
setLoopGraph (LoopGraph loop_graph)
setLoopGraph (LoopGraphPtr loop_graph)
{
loop_graph_ = loop_graph;
}

/** \brief Getter for the first scan of a loop. */
inline PointCloudConstPtr
inline int
getLoopStart ()
{
return (loop_start_);
Expand All @@ -121,13 +131,13 @@ namespace pcl
* \param[in] loop_start the scan that starts the loop
*/
inline void
setLoopStart (PointCloudConstPtr loop_start)
setLoopStart (int loop_start)
{
loop_start_ = loop_start;
}

/** \brief Getter for the last scan of a loop. */
inline PointCloudConstPtr
inline int
getLoopEnd ()
{
return (loop_end_);
Expand All @@ -137,7 +147,7 @@ namespace pcl
* \param[in] loop_end the scan that ends the loop
*/
inline void
setLoopend (PointCloudConstPtr loop_end)
setLoopEnd (int loop_end)
{
loop_end_ = loop_end;
}
Expand All @@ -159,7 +169,7 @@ namespace pcl
}

/** \brief Getter for the transformation between the first and the last scan. */
inline Eigen::Matrix4f &
inline Eigen::Matrix4f
getLoopTransform ()
{
return (loop_transform_);
Expand All @@ -169,9 +179,10 @@ namespace pcl
* \param[in] loop_transform the transformation between the first and the last scan
*/
inline void
setLoopTransform (Eigen::Matrix4f &loop_transform)
setLoopTransform (Eigen::Matrix4f loop_transform)
{
loop_transform_ = loop_transform;
compute_loop_ = false;
}

/** \brief Computes now poses for all point clouds by closing the loop
Expand All @@ -191,7 +202,7 @@ namespace pcl
private:
/** \brief graph structure for the internal optimization graph */
typedef boost::adjacency_list<
boost::vecS, boost::vecS, boost::undirectedS,
boost::listS, boost::vecS, boost::undirectedS,
boost::no_property,
boost::property< boost::edge_weight_t, double > >
LOAGraph;
Expand All @@ -204,22 +215,25 @@ namespace pcl
* @param[out] weights array for the weights
*/
void
loopOptimizerAlgorithm (LOAGraph &g, int f, int l, double *weights);
loopOptimizerAlgorithm (LOAGraph &g, double *weights);

/** \brief The internal loop graph. */
LoopGraph *loop_graph_;
LoopGraphPtr loop_graph_;

/** \brief The first scan of the loop. */
PointCloudConstPtr loop_start_;
//PointCloudConstPtr loop_start_; //TODO
int loop_start_;

/** \brief The last scan of the loop. */
PointCloudConstPtr loop_end_;
//PointCloudConstPtr loop_end_; //TODO
int loop_end_;

/** \brief The registration object used to close the loop. */
RegistrationPtr reg_;

/** \brief The transformation between that start and end of the loop. */
Eigen::Matrix4f *loop_transform_;
Eigen::Matrix4f loop_transform_;
bool compute_loop_;

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Expand Down
47 changes: 25 additions & 22 deletions registration/include/pcl/registration/impl/elch.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,13 +54,13 @@

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::registration::ELCH<PointT>::loopOptimizerAlgorithm (LOAGraph &g, int f, int l, double *weights)
pcl::registration::ELCH<PointT>::loopOptimizerAlgorithm (LOAGraph &g, double *weights)
{
std::list<int> crossings, branches;
crossings.push_back (f);
crossings.push_back (l);
weights[f] = 0;
weights[l] = 1;
crossings.push_back (loop_start_);
crossings.push_back (loop_end_);
weights[loop_start_] = 0;
weights[loop_end_] = 1;

int *p = new int[num_vertices (g)];
int *p_min = new int[num_vertices (g)];
Expand Down Expand Up @@ -157,11 +157,11 @@ pcl::registration::ELCH<PointT>::loopOptimizerAlgorithm (LOAGraph &g, int f, int
template <typename PointT> bool
pcl::registration::ELCH<PointT>::initCompute ()
{
if (!PCLBase<PointT>::initCompute ())
/*if (!PCLBase<PointT>::initCompute ())
{
PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed.\n");
return (false);
}
}*/ //TODO

if (!loop_start_)
{
Expand All @@ -178,35 +178,35 @@ pcl::registration::ELCH<PointT>::initCompute ()
}

//compute transformation if it's not given
if (!loop_transform_)
if (compute_loop_)
{
//TODO use real pose instead of centroid
Eigen::Vector4f pose_start;
pcl::compute3DCentroid (*loop_start_, pose_start);
pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start);

Eigen::Vector4f pose_end;
pcl::compute3DCentroid (*loop_end_, pose_end);
pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end);

PointCloudPtr tmp (new PointCloud);
Eigen::Vector4f diff = pose_end - pose_start;
Eigen::Translation3f translation (diff.head (3));
Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity ();
pcl::transformPointCloud (*loop_end_, *tmp, trans);
pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans);

//reg_->setMaximumIterations (50);
//setMaxCorrespondenceDistance (1.5);
//setRANSACOutlierRejectionThreshold (1.5);
//reg_->setRANSACOutlierRejectionThreshold (DBL_MAX);

reg_->setInputTarget (loop_start_);
reg_->setInputTarget ((*loop_graph_)[loop_start_].cloud);

reg_->setInputCloud (tmp);

reg_->align (*tmp);

*loop_transform_ = reg_->getFinalTransformation ();
loop_transform_ = reg_->getFinalTransformation ();
//TODO hack
*loop_transform_ *= trans.matrix ();
loop_transform_ *= trans.matrix ();

}

Expand All @@ -225,45 +225,48 @@ pcl::registration::ELCH<PointT>::compute ()
LOAGraph grb[4];

typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end;
for (tie (edge_it, edge_it_end) = edges (loop_graph_); edge_it != edge_it_end; edge_it++)
for (tie (edge_it, edge_it_end) = edges (*loop_graph_); edge_it != edge_it_end; edge_it++)
{
for (int j = 0; j < 4; j++)
add_edge (source (*edge_it, loop_graph_), target (*edge_it, loop_graph_), 1, grb[j]); //TODO add variance
add_edge (source (*edge_it, *loop_graph_), target (*edge_it, *loop_graph_), 1, grb[j]); //TODO add variance
}

double *weights[4];
for (int i = 0; i < 4; i++)
{
weights[i] = new double[num_vertices (loop_graph_)];
weights[i] = new double[num_vertices (*loop_graph_)];
loopOptimizerAlgorithm (grb[i], weights[i]);
}

//TODO use pose
Eigen::Vector4f cend;
pcl::compute3DCentroid (*loop_end_, cend);
pcl::compute3DCentroid (*((*loop_graph_)[loop_end_].cloud), cend);
Eigen::Translation3f tend (cend.head (3));
Eigen::Affine3f aend (tend);
Eigen::Affine3f aendI = aend.inverse ();

//TODO iterate ovr loop_graph_
typename boost::graph_traits<LoopGraph>::vertex_iterator vertex_it, vertex_it_end;
for (tie (vertex_it, vertex_it_end) = vertices (loop_graph_); vertex_it != vertex_it_end; vertex_it++)
//typename boost::graph_traits<LoopGraph>::vertex_iterator vertex_it, vertex_it_end;
//for (tie (vertex_it, vertex_it_end) = vertices (*loop_graph_); vertex_it != vertex_it_end; vertex_it++)
for (size_t i = 0; i < num_vertices (*loop_graph_); i++)
{
Eigen::Vector3f t2;
t2[0] = loop_transform_ (3, 0) * weights[0][i]; //TODO
t2[1] = loop_transform_ (3, 1) * weights[1][i];
t2[2] = loop_transform_ (3, 2) * weights[2][i];

Eigen::Affine3f bl(loop_transform_); //TODO
Eigen::Quaternionf q (bl.rotation());
Eigen::Quaternionf q2;
//q2 = Eigen::Quaternionf::Identity ().slerp (weights[3][i], q); //TODO
q2 = Eigen::Quaternionf::Identity ().slerp (weights[3][i], q); //TODO

//TODO use rotation from branch start
Eigen::Translation3f t3 (t2);
Eigen::Affine3f a (t3 * q2);
a = aend * a * aendI;

//std::cout << "transform cloud " << i << " to:" << std::endl << a.matrix () << std::endl;
pcl::transformPointCloud (*vertex_it, *vertex_it, a);
pcl::transformPointCloud (*(*loop_graph_)[i].cloud, *(*loop_graph_)[i].cloud, a);
}

deinitCompute ();
Expand Down

0 comments on commit 934bcaf

Please sign in to comment.