Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 00041 #ifndef PCL_REGISTRATION_IMPL_ELCH_H_ 00042 #define PCL_REGISTRATION_IMPL_ELCH_H_ 00043 00044 #include <list> 00045 #include <algorithm> 00046 00047 #include <pcl/common/transforms.h> 00048 #include <pcl/registration/eigen.h> 00049 #include <pcl/registration/boost.h> 00050 #include <pcl/registration/registration.h> 00051 00052 ////////////////////////////////////////////////////////////////////////////////////////////// 00053 template <typename PointT> void 00054 pcl::registration::ELCH<PointT>::loopOptimizerAlgorithm (LOAGraph &g, double *weights) 00055 { 00056 std::list<int> crossings, branches; 00057 crossings.push_back (static_cast<int> (loop_start_)); 00058 crossings.push_back (static_cast<int> (loop_end_)); 00059 weights[loop_start_] = 0; 00060 weights[loop_end_] = 1; 00061 00062 int *p = new int[num_vertices (g)]; 00063 int *p_min = new int[num_vertices (g)]; 00064 double *d = new double[num_vertices (g)]; 00065 double *d_min = new double[num_vertices (g)]; 00066 double dist; 00067 bool do_swap = false; 00068 std::list<int>::iterator crossings_it, end_it, start_min, end_min; 00069 00070 // process all junctions 00071 while (!crossings.empty ()) 00072 { 00073 dist = -1; 00074 // find shortest crossing for all vertices on the loop 00075 for (crossings_it = crossings.begin (); crossings_it != crossings.end (); ) 00076 { 00077 dijkstra_shortest_paths (g, *crossings_it, 00078 predecessor_map(boost::make_iterator_property_map(p, get(boost::vertex_index, g))). 00079 distance_map(boost::make_iterator_property_map(d, get(boost::vertex_index, g)))); 00080 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 00156 ////////////////////////////////////////////////////////////////////////////////////////////// 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_->setInputSource (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 00216 ////////////////////////////////////////////////////////////////////////////////////////////// 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_