semantic_point_annotator: semantic_point_annotator: sac_model

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: sac_model_line.h 20633 2009-08-04 07:19:09Z tfoote $
00028  *
00029  */
00030 
00033 #ifndef _SAMPLE_CONSENSUS_SACMODELLINE_H_
00034 #define _SAMPLE_CONSENSUS_SACMODELLINE_H_
00035 
00036 #include <sac_model.h>
00037 #include <model_types.h>
00038 
00040 #define MAX_ITERATIONS_UNIQUE 1000
00041 
00042 namespace sample_consensus
00043 {
00046   class SACModelLine : public SACModel
00047   {
00048     public:
00050 
00051       SACModelLine () { }
00052 
00054 
00055       virtual ~SACModelLine () { }
00056 
00057       virtual void getSamples (int &iterations, std::vector<int> &samples);
00058 
00060 
00064       bool testModelCoefficients (const std::vector<double> &model_coefficients) { return true; }
00065 
00066       virtual bool computeModelCoefficients (const std::vector<int> &samples);
00067 
00068       virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);
00069       virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances);
00070       virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers);
00071 
00072       virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, sensor_msgs::PointCloud &projected_points);
00073 
00074       virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
00075       virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold);
00076 
00078 
00079       virtual int getModelType () { return (SACMODEL_LINE); }
00080 
00082 
00086       inline geometry_msgs::Point32
00087         cross (const geometry_msgs::Point32 &p1, const geometry_msgs::Point32 &p2)
00088       {
00089         geometry_msgs::Point32 r;
00090         r.x = p1.y * p2.z - p1.z * p2.y;
00091         r.y = p1.z * p2.x - p1.x * p2.z;
00092         r.z = p1.x * p2.y - p1.y * p2.x;
00093         return (r);
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 
00128       inline void
00129         computeCovarianceMatrix (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, Eigen::Matrix3d &covariance_matrix, geometry_msgs::Point32 &centroid)
00130       {
00131         computeCentroid (points, indices, centroid);
00132 
00133         // Initialize to 0
00134         covariance_matrix = Eigen::Matrix3d::Zero ();
00135 
00136         for (unsigned int j = 0; j < indices.size (); j++)
00137         {
00138           covariance_matrix (0, 0) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].x - centroid.x);
00139           covariance_matrix (0, 1) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].y - centroid.y);
00140           covariance_matrix (0, 2) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].z - centroid.z);
00141 
00142           covariance_matrix (1, 0) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].x - centroid.x);
00143           covariance_matrix (1, 1) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].y - centroid.y);
00144           covariance_matrix (1, 2) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].z - centroid.z);
00145 
00146           covariance_matrix (2, 0) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].x - centroid.x);
00147           covariance_matrix (2, 1) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].y - centroid.y);
00148           covariance_matrix (2, 2) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].z - centroid.z);
00149         }
00150       }
00151     };
00152 }
00153 
00154 #endif