door_handle_detector: nearest.h Source File

00001 /*
00002  * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  * $Id$
00028  *
00029  */
00030 
00033 #ifndef _CLOUD_GEOMETRY_NEAREST_H_
00034 #define _CLOUD_GEOMETRY_NEAREST_H_
00035 
00036 // ROS includes
00037 #include <sensor_msgs/PointCloud.h>
00038 #include <geometry_msgs/PointStamped.h>
00039 #include <geometry_msgs/Point32.h>
00040 #include <geometry_msgs/Polygon.h>
00041 
00042 #include <Eigen/Core>
00043 #include <Eigen/QR>
00044 
00045 namespace cloud_geometry
00046 {
00047 
00048   namespace nearest
00049   {
00050 
00052 
00056     inline void
00057       computeCentroid (const sensor_msgs::PointCloud &points, geometry_msgs::Point32 &centroid)
00058     {
00059       centroid.x = centroid.y = centroid.z = 0;
00060       // For each point in the cloud
00061       for (unsigned int i = 0; i < points.points.size (); i++)
00062       {
00063         centroid.x += points.points.at (i).x;
00064         centroid.y += points.points.at (i).y;
00065         centroid.z += points.points.at (i).z;
00066       }
00067 
00068       centroid.x /= points.points.size ();
00069       centroid.y /= points.points.size ();
00070       centroid.z /= points.points.size ();
00071     }
00072 
00073 
00075 
00079     inline void
00080       computeCentroid (const geometry_msgs::Polygon &poly, geometry_msgs::Point32 &centroid)
00081     {
00082       centroid.x = centroid.y = centroid.z = 0;
00083       // For each point in the cloud
00084       for (unsigned int i = 0; i < poly.points.size (); i++)
00085       {
00086         centroid.x += poly.points.at (i).x;
00087         centroid.y += poly.points.at (i).y;
00088         centroid.z += poly.points.at (i).z;
00089       }
00090 
00091       centroid.x /= poly.points.size ();
00092       centroid.y /= poly.points.size ();
00093       centroid.z /= poly.points.size ();
00094     }
00095 
00097 
00102     inline void
00103       computeCentroid (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, geometry_msgs::Point32 &centroid)
00104     {
00105       centroid.x = centroid.y = centroid.z = 0;
00106       // For each point in the cloud
00107       for (unsigned int i = 0; i < indices.size (); i++)
00108       {
00109         centroid.x += points.points.at (indices.at (i)).x;
00110         centroid.y += points.points.at (indices.at (i)).y;
00111         centroid.z += points.points.at (indices.at (i)).z;
00112       }
00113 
00114       centroid.x /= indices.size ();
00115       centroid.y /= indices.size ();
00116       centroid.z /= indices.size ();
00117     }
00118 
00120 
00125     inline void
00126       computeCentroid (const sensor_msgs::PointCloudConstPtr &points, const std::vector<int> &indices, geometry_msgs::Point32 &centroid)
00127     {
00128       centroid.x = centroid.y = centroid.z = 0;
00129       // For each point in the cloud
00130       for (unsigned int i = 0; i < indices.size (); i++)
00131       {
00132         centroid.x += points->points.at (indices.at (i)).x;
00133         centroid.y += points->points.at (indices.at (i)).y;
00134         centroid.z += points->points.at (indices.at (i)).z;
00135       }
00136 
00137       centroid.x /= indices.size ();
00138       centroid.y /= indices.size ();
00139       centroid.z /= indices.size ();
00140     }
00141 
00143 
00148     inline void
00149       computeCentroid (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, std::vector<double> &centroid)
00150     {
00151       centroid.resize (3);
00152       centroid[0] = centroid[1] = centroid[2] = 0;
00153       // For each point in the cloud
00154       for (unsigned int i = 0; i < indices.size (); i++)
00155       {
00156         centroid[0] += points.points.at (indices.at (i)).x;
00157         centroid[1] += points.points.at (indices.at (i)).y;
00158         centroid[2] += points.points.at (indices.at (i)).z;
00159       }
00160 
00161       centroid[0] /= indices.size ();
00162       centroid[1] /= indices.size ();
00163       centroid[2] /= indices.size ();
00164     }
00165 
00167 
00173     inline void
00174       computeCovarianceMatrix (const sensor_msgs::PointCloud &points, Eigen::Matrix3d &covariance_matrix, geometry_msgs::Point32 &centroid)
00175     {
00176       computeCentroid (points, centroid);
00177 
00178       // Initialize to 0
00179       covariance_matrix = Eigen::Matrix3d::Zero ();
00180 
00181       // Sum of outer products
00182       // covariance_matrix (k, i)  += points_c (j, k) * points_c (j, i);
00183       for (unsigned int j = 0; j < points.points.size (); j++)
00184       {
00185         covariance_matrix (0, 0) += (points.points[j].x - centroid.x) * (points.points[j].x - centroid.x);
00186         covariance_matrix (0, 1) += (points.points[j].x - centroid.x) * (points.points[j].y - centroid.y);
00187         covariance_matrix (0, 2) += (points.points[j].x - centroid.x) * (points.points[j].z - centroid.z);
00188 
00189         covariance_matrix (1, 0) += (points.points[j].y - centroid.y) * (points.points[j].x - centroid.x);
00190         covariance_matrix (1, 1) += (points.points[j].y - centroid.y) * (points.points[j].y - centroid.y);
00191         covariance_matrix (1, 2) += (points.points[j].y - centroid.y) * (points.points[j].z - centroid.z);
00192 
00193         covariance_matrix (2, 0) += (points.points[j].z - centroid.z) * (points.points[j].x - centroid.x);
00194         covariance_matrix (2, 1) += (points.points[j].z - centroid.z) * (points.points[j].y - centroid.y);
00195         covariance_matrix (2, 2) += (points.points[j].z - centroid.z) * (points.points[j].z - centroid.z);
00196       }
00197     }
00198 
00200 
00204     inline void
00205       computeCovarianceMatrix (const sensor_msgs::PointCloud &points, Eigen::Matrix3d &covariance_matrix)
00206     {
00207       geometry_msgs::Point32 centroid;
00208       computeCovarianceMatrix (points, covariance_matrix, centroid);
00209     }
00210 
00212 
00220     inline void
00221       computeCovarianceMatrix (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, Eigen::Matrix3d &covariance_matrix, geometry_msgs::Point32 &centroid)
00222     {
00223       computeCentroid (points, indices, centroid);
00224 
00225       // Initialize to 0
00226       covariance_matrix = Eigen::Matrix3d::Zero ();
00227 
00228       for (unsigned int j = 0; j < indices.size (); j++)
00229       {
00230         covariance_matrix (0, 0) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].x - centroid.x);
00231         covariance_matrix (0, 1) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].y - centroid.y);
00232         covariance_matrix (0, 2) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].z - centroid.z);
00233 
00234         covariance_matrix (1, 0) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].x - centroid.x);
00235         covariance_matrix (1, 1) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].y - centroid.y);
00236         covariance_matrix (1, 2) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].z - centroid.z);
00237 
00238         covariance_matrix (2, 0) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].x - centroid.x);
00239         covariance_matrix (2, 1) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].y - centroid.y);
00240         covariance_matrix (2, 2) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].z - centroid.z);
00241       }
00242     }
00243 
00245 
00253     inline void
00254       computeCovarianceMatrix (const sensor_msgs::PointCloudConstPtr &points, const std::vector<int> &indices, Eigen::Matrix3d &covariance_matrix, geometry_msgs::Point32 &centroid)
00255     {
00256       computeCentroid (points, indices, centroid);
00257 
00258       // Initialize to 0
00259       covariance_matrix = Eigen::Matrix3d::Zero ();
00260 
00261       for (unsigned int j = 0; j < indices.size (); j++)
00262       {
00263         covariance_matrix (0, 0) += (points->points[indices.at (j)].x - centroid.x) * (points->points[indices.at (j)].x - centroid.x);
00264         covariance_matrix (0, 1) += (points->points[indices.at (j)].x - centroid.x) * (points->points[indices.at (j)].y - centroid.y);
00265         covariance_matrix (0, 2) += (points->points[indices.at (j)].x - centroid.x) * (points->points[indices.at (j)].z - centroid.z);
00266 
00267         covariance_matrix (1, 0) += (points->points[indices.at (j)].y - centroid.y) * (points->points[indices.at (j)].x - centroid.x);
00268         covariance_matrix (1, 1) += (points->points[indices.at (j)].y - centroid.y) * (points->points[indices.at (j)].y - centroid.y);
00269         covariance_matrix (1, 2) += (points->points[indices.at (j)].y - centroid.y) * (points->points[indices.at (j)].z - centroid.z);
00270 
00271         covariance_matrix (2, 0) += (points->points[indices.at (j)].z - centroid.z) * (points->points[indices.at (j)].x - centroid.x);
00272         covariance_matrix (2, 1) += (points->points[indices.at (j)].z - centroid.z) * (points->points[indices.at (j)].y - centroid.y);
00273         covariance_matrix (2, 2) += (points->points[indices.at (j)].z - centroid.z) * (points->points[indices.at (j)].z - centroid.z);
00274       }
00275     }
00276 
00278 
00284     inline void
00285       computeCovarianceMatrix (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, Eigen::Matrix3d &covariance_matrix)
00286     {
00287       geometry_msgs::Point32 centroid;
00288       computeCovarianceMatrix (points, indices, covariance_matrix, centroid);
00289     }
00290 
00292 
00304     inline double
00305       getAngleWithViewpoint (float px, float py, float pz, float qx, float qy, float qz,
00306                              float vp_x = 0, float vp_y = 0, float vp_z = 0)
00307     {  
00308       double dir_a[3], dir_b[3];
00309       dir_a[0] = vp_x - px; dir_a[1] = vp_y - py; dir_a[2] = vp_z - pz;
00310       dir_b[0] = qx   - px; dir_b[1] = qy   - py; dir_b[2] = qz   - pz;
00311 
00312       // sqrt (sqr (x) + sqr (y) + sqr (z))
00313       double norm_a = sqrt (dir_a[0]*dir_a[0] + dir_a[1]*dir_a[1] + dir_a[2]*dir_a[2]);
00314       // Check for bogus 0,0,0 points
00315       if (norm_a == 0) return (0);
00316       double norm_b = sqrt (dir_b[0]*dir_b[0] + dir_b[1]*dir_b[1] + dir_b[2]*dir_b[2]);
00317       if (norm_b == 0) return (0);
00318       // dot_product (x, y)
00319       double dot_pr = dir_a[0]*dir_b[0] + dir_a[1]*dir_b[1] + dir_a[2]*dir_b[2];
00320       if (dot_pr != dot_pr)     // Check for NaNs  
00321         return (0);
00322 
00323       return ( acos (dot_pr / (norm_a * norm_b) ) * 180.0 / M_PI);
00324     }
00325 
00326     void computeCentroid (const sensor_msgs::PointCloud &points, sensor_msgs::PointCloud &centroid);
00327     void computeCentroid (const sensor_msgs::PointCloud &points, std::vector<int> &indices, sensor_msgs::PointCloud &centroid);
00328 
00329     void computePatchEigen (const sensor_msgs::PointCloud &points, Eigen::Matrix3d &eigen_vectors, Eigen::Vector3d &eigen_values);
00330     void computePatchEigen (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, Eigen::Matrix3d &eigen_vectors, Eigen::Vector3d &eigen_values);
00331     void computePatchEigenNormalized (const sensor_msgs::PointCloud &points, Eigen::Matrix3d &eigen_vectors, Eigen::Vector3d &eigen_values, geometry_msgs::Point32& centroid);
00332     void computePatchEigenNormalized (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, Eigen::Matrix3d &eigen_vectors, Eigen::Vector3d &eigen_values, geometry_msgs::Point32& centroid);
00333 
00334     void computePointNormal (const sensor_msgs::PointCloud &points, Eigen::Vector4d &plane_parameters, double &curvature);
00335     void computePointNormal (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, Eigen::Vector4d &plane_parameters, double &curvature);
00336     void computePointNormal (const sensor_msgs::PointCloudConstPtr &points, const std::vector<int> &indices, Eigen::Vector4d &plane_parameters, double &curvature);
00337 
00338     void computeMomentInvariants (const sensor_msgs::PointCloud &points, double &j1, double &j2, double &j3);
00339     void computeMomentInvariants (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, double &j1, double &j2, double &j3);
00340 
00341     bool isBoundaryPoint (const sensor_msgs::PointCloud &points, int q_idx, const std::vector<int> &neighbors, const Eigen::Vector3d& u, const Eigen::Vector3d& v, double angle_threshold = M_PI / 2.0);
00342 
00343     void computePointCloudNormals (sensor_msgs::PointCloud &points, const sensor_msgs::PointCloud &surface, int k, const geometry_msgs::PointStamped &viewpoint);
00344     void computePointCloudNormals (sensor_msgs::PointCloud &points, const sensor_msgs::PointCloud &surface, double radius, const geometry_msgs::PointStamped &viewpoint);
00345     void computePointCloudNormals (sensor_msgs::PointCloud &points, int k, const geometry_msgs::PointStamped &viewpoint);
00346     void computePointCloudNormals (sensor_msgs::PointCloud &points, double radius, const geometry_msgs::PointStamped &viewpoint);
00347 
00348     void computeOrganizedPointCloudNormals (sensor_msgs::PointCloud &points, const sensor_msgs::PointCloud &surface, int k, int downsample_factor, int width, int height, double max_z, 
00349                                             const geometry_msgs::Point32 &viewpoint);
00350     void computeOrganizedPointCloudNormals (sensor_msgs::PointCloud &points, const sensor_msgs::PointCloudConstPtr &surface, int k, int downsample_factor, int width, int height, double max_z, 
00351                                             const geometry_msgs::Point32 &viewpoint);
00352     void computeOrganizedPointCloudNormalsWithFiltering (sensor_msgs::PointCloud &points, const sensor_msgs::PointCloud &surface, int k, int downsample_factor, int width, int height, 
00353                                                          double max_z, double min_angle, double max_angle, const geometry_msgs::Point32 &viewpoint);
00354 
00355     void extractEuclideanClusters (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, double tolerance, std::vector<std::vector<int> > &clusters,
00356                                    int nx_idx, int ny_idx, int nz_idx, double eps_angle, unsigned int min_pts_per_cluster = 1);
00357 
00358     void extractEuclideanClusters (const sensor_msgs::PointCloud &points, double tolerance, std::vector<std::vector<int> > &clusters,
00359                                    int nx_idx, int ny_idx, int nz_idx, double eps_angle, unsigned int min_pts_per_cluster = 1);
00360                                    
00361     void filterJumpEdges (const sensor_msgs::PointCloud &points, sensor_msgs::PointCloud &points_filtered, int k, int width, int height, 
00362                           double min_angle, double max_angle, const geometry_msgs::Point32 &viewpoint);
00363                                                                     
00364   }
00365 }
00366 
00367 #endif