elch.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: elch.hpp 6152 2012-07-04 22:58:53Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_REGISTRATION_IMPL_ELCH_H_
00041 #define PCL_REGISTRATION_IMPL_ELCH_H_
00042 
00043 #include <list>
00044 #include <algorithm>
00045 
00046 #include <boost/graph/adjacency_list.hpp>
00047 #include <boost/graph/graph_traits.hpp>
00048 #include <boost/graph/dijkstra_shortest_paths.hpp>
00049 
00050 #include <Eigen/Geometry>
00051 
00052 #include <pcl/common/transforms.h>
00053 #include <pcl/registration/registration.h>
00054 
00056 template <typename PointT> void
00057 pcl::registration::ELCH<PointT>::loopOptimizerAlgorithm (LOAGraph &g, double *weights)
00058 {
00059   std::list<int> crossings, branches;
00060   crossings.push_back (static_cast<int> (loop_start_));
00061   crossings.push_back (static_cast<int> (loop_end_));
00062   weights[loop_start_] = 0;
00063   weights[loop_end_] = 1;
00064 
00065   int *p = new int[num_vertices (g)];
00066   int *p_min = new int[num_vertices (g)];
00067   double *d = new double[num_vertices (g)];
00068   double *d_min = new double[num_vertices (g)];
00069   double dist;
00070   bool do_swap = false;
00071   std::list<int>::iterator crossings_it, end_it, start_min, end_min;
00072 
00073   // process all junctions
00074   while (!crossings.empty ())
00075   {
00076     dist = -1;
00077     // find shortest crossing for all vertices on the loop
00078     for (crossings_it = crossings.begin (); crossings_it != crossings.end (); )
00079     {
00080       dijkstra_shortest_paths (g, *crossings_it, boost::predecessor_map (p).distance_map (d));
00081       end_it = crossings_it;
00082       end_it++;
00083       // find shortest crossing for one vertex
00084       for (; end_it != crossings.end (); end_it++)
00085       {
00086         if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist))
00087         {
00088           dist = d[*end_it];
00089           start_min = crossings_it;
00090           end_min = end_it;
00091           do_swap = true;
00092         }
00093       }
00094       if (do_swap)
00095       {
00096         std::swap (p, p_min);
00097         std::swap (d, d_min);
00098         do_swap = false;
00099       }
00100       // vertex starts a branch
00101       if (dist < 0)
00102       {
00103         branches.push_back (static_cast<int> (*crossings_it));
00104         crossings_it = crossings.erase (crossings_it);
00105       }
00106       else
00107         crossings_it++;
00108     }
00109 
00110     if (dist > -1)
00111     {
00112       remove_edge (*end_min, p_min[*end_min], g);
00113       for (int i = p_min[*end_min]; i != *start_min; i = p_min[i])
00114       {
00115         //even right with weights[*start_min] > weights[*end_min]! (math works)
00116         weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) * d_min[i] / d_min[*end_min];
00117         remove_edge (i, p_min[i], g);
00118         if (degree (i, g) > 0)
00119         {
00120           crossings.push_back (i);
00121         }
00122       }
00123 
00124       if (degree (*start_min, g) == 0)
00125         crossings.erase (start_min);
00126 
00127       if (degree (*end_min, g) == 0)
00128         crossings.erase (end_min);
00129     }
00130   }
00131 
00132   delete[] p;
00133   delete[] p_min;
00134   delete[] d;
00135   delete[] d_min;
00136 
00137   boost::graph_traits<LOAGraph>::adjacency_iterator adjacent_it, adjacent_it_end;
00138   int s;
00139 
00140   // error propagation
00141   while (!branches.empty ())
00142   {
00143     s = branches.front ();
00144     branches.pop_front ();
00145 
00146     for (boost::tuples::tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it)
00147     {
00148       weights[*adjacent_it] = weights[s];
00149       if (degree (*adjacent_it, g) > 1)
00150         branches.push_back (static_cast<int> (*adjacent_it));
00151     }
00152     clear_vertex (s, g);
00153   }
00154 }
00155 
00157 template <typename PointT> bool
00158 pcl::registration::ELCH<PointT>::initCompute ()
00159 {
00160   /*if (!PCLBase<PointT>::initCompute ())
00161   {
00162     PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed.\n");
00163     return (false);
00164   }*/ //TODO
00165 
00166   if (loop_end_ == 0)
00167   {
00168     PCL_ERROR ("[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
00169     deinitCompute ();
00170     return (false);
00171   }
00172 
00173   //compute transformation if it's not given
00174   if (compute_loop_)
00175   {
00176     PointCloudPtr meta_start (new PointCloud);
00177     PointCloudPtr meta_end (new PointCloud);
00178     *meta_start = *(*loop_graph_)[loop_start_].cloud;
00179     *meta_end = *(*loop_graph_)[loop_end_].cloud;
00180 
00181     typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
00182     for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++)
00183       *meta_start += *(*loop_graph_)[*si].cloud;
00184 
00185     for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++)
00186       *meta_end += *(*loop_graph_)[*si].cloud;
00187 
00188     //TODO use real pose instead of centroid
00189     //Eigen::Vector4f pose_start;
00190     //pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start);
00191 
00192     //Eigen::Vector4f pose_end;
00193     //pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end);
00194 
00195     PointCloudPtr tmp (new PointCloud);
00196     //Eigen::Vector4f diff = pose_start - pose_end;
00197     //Eigen::Translation3f translation (diff.head (3));
00198     //Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity ();
00199     //pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans);
00200 
00201     reg_->setInputTarget (meta_start);
00202 
00203     reg_->setInputCloud (meta_end);
00204 
00205     reg_->align (*tmp);
00206 
00207     loop_transform_ = reg_->getFinalTransformation ();
00208     //TODO hack
00209     //loop_transform_ *= trans.matrix ();
00210 
00211   }
00212 
00213   return (true);
00214 }
00215 
00217 template <typename PointT> void
00218 pcl::registration::ELCH<PointT>::compute ()
00219 {
00220   if (!initCompute ())
00221   {
00222     return;
00223   }
00224 
00225   LOAGraph grb[4];
00226 
00227   typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end;
00228   for (boost::tuples::tie (edge_it, edge_it_end) = edges (*loop_graph_); edge_it != edge_it_end; edge_it++)
00229   {
00230     for (int j = 0; j < 4; j++)
00231       add_edge (source (*edge_it, *loop_graph_), target (*edge_it, *loop_graph_), 1, grb[j]);  //TODO add variance
00232   }
00233 
00234   double *weights[4];
00235   for (int i = 0; i < 4; i++)
00236   {
00237     weights[i] = new double[num_vertices (*loop_graph_)];
00238     loopOptimizerAlgorithm (grb[i], weights[i]);
00239   }
00240 
00241   //TODO use pose
00242   //Eigen::Vector4f cend;
00243   //pcl::compute3DCentroid (*((*loop_graph_)[loop_end_].cloud), cend);
00244   //Eigen::Translation3f tend (cend.head (3));
00245   //Eigen::Affine3f aend (tend);
00246   //Eigen::Affine3f aendI = aend.inverse ();
00247 
00248   //TODO iterate ovr loop_graph_
00249   //typename boost::graph_traits<LoopGraph>::vertex_iterator vertex_it, vertex_it_end;
00250   //for (boost::tuples::tie (vertex_it, vertex_it_end) = vertices (*loop_graph_); vertex_it != vertex_it_end; vertex_it++)
00251   for (size_t i = 0; i < num_vertices (*loop_graph_); i++)
00252   {
00253     Eigen::Vector3f t2;
00254     t2[0] = loop_transform_ (0, 3) * static_cast<float> (weights[0][i]);
00255     t2[1] = loop_transform_ (1, 3) * static_cast<float> (weights[1][i]);
00256     t2[2] = loop_transform_ (2, 3) * static_cast<float> (weights[2][i]);
00257 
00258     Eigen::Affine3f bl (loop_transform_);
00259     Eigen::Quaternionf q (bl.rotation ());
00260     Eigen::Quaternionf q2;
00261     q2 = Eigen::Quaternionf::Identity ().slerp (static_cast<float> (weights[3][i]), q);
00262 
00263     //TODO use rotation from branch start
00264     Eigen::Translation3f t3 (t2);
00265     Eigen::Affine3f a (t3 * q2);
00266     //a = aend * a * aendI;
00267 
00268     pcl::transformPointCloud (*(*loop_graph_)[i].cloud, *(*loop_graph_)[i].cloud, a);
00269   }
00270 
00271   add_edge (loop_start_, loop_end_, *loop_graph_);
00272 
00273   deinitCompute ();
00274 }
00275 
00276 #endif // PCL_REGISTRATION_IMPL_ELCH_H_


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:14:51