Point Cloud Library (PCL)
1.3.1
|
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_