Point Cloud Library (PCL)  1.3.1
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 3042 2011-11-01 04:44:47Z svn $
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, int f, int l, double *weights)
00058 {
00059   std::list<int> crossings, branches;
00060   crossings.push_back (f);
00061   crossings.push_back (l);
00062   weights[f] = 0;
00063   weights[l] = 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 (*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 (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 (*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   }
00165 
00166   if (!loop_start_)
00167   {
00168     PCL_ERROR ("[pcl::registration::ELCH::initCompute] no start of loop defined is empty!\n");
00169     deinitCompute ();
00170     return (false);
00171   }
00172 
00173   if (!loop_end_)
00174   {
00175     PCL_ERROR ("[pcl::registration::ELCH::initCompute] no end of loop defined is empty!\n");
00176     deinitCompute ();
00177     return (false);
00178   }
00179 
00180   //compute transformation if it's not given
00181   if (!loop_transform_)
00182   {
00183     //TODO use real pose instead of centroid
00184     Eigen::Vector4f pose_start;
00185     pcl::compute3DCentroid (*loop_start_, pose_start);
00186 
00187     Eigen::Vector4f pose_end;
00188     pcl::compute3DCentroid (*loop_end_, pose_end);
00189 
00190     PointCloudPtr tmp (new PointCloud);
00191     pcl::transformPointCloud (*loop_end_, *tmp, pose_end - pose_start);
00192 
00193     //reg_->setMaximumIterations (50);
00194     //setMaxCorrespondenceDistance (1.5);
00195     //setRANSACOutlierRejectionThreshold (1.5);
00196     //reg_->setRANSACOutlierRejectionThreshold (DBL_MAX);
00197 
00198     reg_->setInputTarget (loop_start_);
00199 
00200     reg_->setInputCloud (tmp);
00201 
00202     reg_->align (*tmp);
00203 
00204     loop_transform_ = reg_->getFinalTransformation ();
00205     //TODO hack
00206     //t += ce2;
00207 
00208   }
00209 
00210   return (true);
00211 }
00212 
00214 template <typename PointT> void
00215 pcl::registration::ELCH<PointT>::compute ()
00216 {
00217   if (!initCompute ())
00218   {
00219     return;
00220   }
00221 
00222   LOAGraph grb[4];
00223 
00224   typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end;
00225   for (tie (edge_it, edge_it_end) = edges (loop_graph_); edge_it != edge_it_end; edge_it++)
00226   {
00227     for (int j = 0; j < 4; j++)
00228       add_edge (source (*edge_it, loop_graph_), target (*edge_it, loop_graph_), 1, grb[j]);  //TODO add variance
00229   }
00230 
00231   double *weights[4];
00232   for (int i = 0; i < 4; i++)
00233   {
00234     weights[i] = new double[num_vertices (loop_graph_)];
00235     loopOptimizerAlgorithm (grb[i], weights[i]);
00236   }
00237 
00238   //TODO use pose
00239   Eigen::Vector4f cend;
00240   pcl::compute3DCentroid (*loop_end_, cend);
00241   Eigen::Translation3f tend (cend[0], cend[1], cend[2]);
00242   Eigen::Affine3f aend (tend);
00243   Eigen::Affine3f aendI = aend.inverse ();
00244 
00245   //TODO iterate ovr loop_graph_
00246   typename boost::graph_traits<LoopGraph>::vertex_iterator vertex_it, vertex_it_end;
00247   for (tie (vertex_it, vertex_it_end) = vertices (loop_graph_); vertex_it != vertex_it_end; vertex_it++)
00248   {
00249     Eigen::Vector3f t2;
00250     //t2[0] = loop_transform_->translation ()[0] * weights[0][i]; //TODO
00251     //t2[1] = loop_transform_->translation ()[1] * weights[1][i];
00252     //t2[2] = loop_transform_->translation ()[2] * weights[2][i];
00253 
00254     Eigen::Quaternionf q2;
00255     //q2 = Eigen::Quaternionf::Identity ().slerp (weights[3][i], q); //TODO
00256 
00257     //TODO use rotation from branch start
00258     Eigen::Translation3f t3 (t2);
00259     Eigen::Affine3f a (t3 * q2);
00260     a = aend * a * aendI;
00261 
00262     //std::cout << "transform cloud " << i << " to:" << std::endl << a.matrix () << std::endl;
00263     pcl::transformPointCloud (*vertex_it, *vertex_it, a);
00264   }
00265 
00266   deinitCompute ();
00267 }
00268 
00269 #endif // PCL_REGISTRATION_IMPL_ELCH_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines