Go to the documentation of this file.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 #ifndef QUADRIC_H
00033 #define QUADRIC_H
00034
00035 #include <Eigen/Dense>
00036 #include <pcl/point_cloud.h>
00037 #include <pcl/point_types.h>
00038 #include <pcl/visualization/pcl_visualizer.h>
00039 #include <vector>
00040
00041
00043 extern "C" void dggev_(const char* JOBVL, const char* JOBVR, const int* N, const double* A, const int* LDA,
00044 const double* B, const int* LDB, double* ALPHAR, double* ALPHAI, double* BETA, double* VL,
00045 const int* LDVL, double* VR, const int* LDVR, double* WORK, const int* LWORK, int* INFO);
00046
00047
00048 typedef pcl::PointCloud<pcl::PointXYZRGBA> PointCloud;
00049
00050
00060 class Quadric
00061 {
00062 public:
00063
00067 Quadric()
00068 {
00069 }
00070
00078 Quadric(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >& T_cams,
00079 const PointCloud::Ptr& input, const Eigen::Vector3d& sample, bool is_deterministic);
00080
00086 void findTaubinNormalAxis(const std::vector<int> &indices, const Eigen::VectorXi& cam_source);
00087
00092 void fitQuadric(const std::vector<int>& indices);
00093
00098 void setInputCloud(const PointCloud::Ptr& input)
00099 {
00100 this->input_ = input;
00101 }
00102
00106 void print();
00107
00113 void plotAxes(void* viewer_void, int id) const;
00114
00119 const Eigen::Vector3d& getSample() const
00120 {
00121 return sample_;
00122 }
00123
00128 const Eigen::Vector3d& getBinormal() const
00129 {
00130 return binormal_;
00131 }
00132
00137 const Eigen::Vector3d& getCurvatureAxis() const
00138 {
00139 return curvature_axis_;
00140 }
00141
00146 const Eigen::Vector3d& getNormal() const
00147 {
00148 return normal_;
00149 }
00150
00151
00152 private:
00153
00158 void findAverageNormalAxis(const Eigen::MatrixXd& normals);
00159
00163 void unpackQuadric();
00164
00175 bool solveGeneralizedEigenProblem(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, Eigen::MatrixXd& v,
00176 Eigen::MatrixXd& lambda);
00177
00178 bool is_deterministic_;
00179 Eigen::Matrix3Xd cam_origins_;
00180 Eigen::Vector3d sample_;
00181 int majority_cam_source_;
00182 PointCloud::Ptr input_;
00183 Eigen::Vector3d curvature_axis_, normal_, binormal_;
00184 Eigen::Matrix<double, 10, 1> parameters_;
00185 Eigen::Vector3d centroid_;
00186 Eigen::Matrix3d covariance_matrix_;
00187 double normals_ratio_;
00188
00189 static const int TAUBIN_MATRICES_SIZE = 10;
00190 };
00191
00192 #endif // PCL_FEATURES_CURVATURE_ESTIMATION_TAUBIN_H_