00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_TYPES_H_
00037 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_TYPES_H_
00038
00039 #include <limits>
00040 #include <Eigen/Core>
00041
00042 namespace pcl
00043 {
00044 namespace registration
00045 {
00047 struct Correspondence
00048 {
00049 union {
00050 float data[4];
00051 struct {
00053 int indexQuery;
00055 int indexMatch;
00057 float distance;
00058 };
00059 }; EIGEN_ALIGN16;
00060 EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00062 inline Correspondence()
00063 : indexQuery (0)
00064 , indexMatch (-1)
00065 , distance (std::numeric_limits<float>::max ())
00066 {
00067 data[3] = 1.0f;
00068 }
00070 inline Correspondence(int _indexQuery, int _indexMatch, float _distance)
00071 : indexQuery (indexQuery)
00072 , indexMatch (indexMatch)
00073 , distance (distance)
00074 {
00075 data[3] = 1.0f;
00076 }
00077 };
00078
00080 inline std::ostream& operator << (std::ostream& os, const Correspondence& c)
00081 {
00082 os << c.indexQuery << " " << c.indexMatch << " " << c.distance;
00083 return (os);
00084 }
00085 }
00086 }
00087
00094 inline void
00095 pcl::registration::getCorDistMeanStd(const Correspondences &correspondences, double &mean, double &stddev)
00096 {
00097 if ( correspondences.size() == 0 )
00098 return;
00099
00100 double sum = 0, sq_sum = 0;
00101
00102 for (size_t i = 0; i < correspondences.size(); ++i)
00103 {
00104 sum += correspondences[i].distance;
00105 sq_sum += correspondences[i].distance * correspondences[i].distance;
00106 }
00107 mean = sum / correspondences.size();
00108 double variance = (double)(sq_sum - sum * sum / correspondences.size()) / (correspondences.size() - 1);
00109 stddev = sqrt(variance);
00110 }
00111
00117 inline void
00118 pcl::registration::getQueryIndices(const Correspondences& correspondences, std::vector<int>& indices)
00119 {
00120 indices.resize(correspondences.size());
00121 for ( unsigned int i = 0; i < correspondences.size(); ++i)
00122 indices[i] = correspondences[i].indexQuery;
00123 }
00124
00130 inline void
00131 pcl::registration::getMatchIndices(const Correspondences& correspondences, std::vector<int>& indices)
00132 {
00133 indices.resize(correspondences.size());
00134 for ( unsigned int i = 0; i < correspondences.size(); ++i)
00135 indices[i] = correspondences[i].indexMatch;
00136 }
00137
00138 #endif