36 #ifndef _vpMbtFaceDepthNormal_h_
37 #define _vpMbtFaceDepthNormal_h_
41 #include <visp3/core/vpConfig.h>
43 #include <pcl/point_cloud.h>
44 #include <pcl/point_types.h>
47 #include <visp3/core/vpPlane.h>
48 #include <visp3/mbt/vpMbTracker.h>
49 #include <visp3/mbt/vpMbtDistanceLine.h>
51 #define DEBUG_DISPLAY_DEPTH_NORMAL 0
62 ROBUST_FEATURE_ESTIMATION = 0,
63 ROBUST_SVD_PLANE_ESTIMATION = 1,
65 PCL_PLANE_ESTIMATION = 2
72 unsigned int m_clippingFlag;
76 double m_distNearClip;
90 std::string name =
"");
93 bool computeDesiredFeatures(
const vpHomogeneousMatrix &cMo,
const unsigned int width,
const unsigned int height,
94 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
95 vpColVector &desired_features,
const unsigned int stepX,
const unsigned int stepY
96 #
if DEBUG_DISPLAY_DEPTH_NORMAL
103 bool computeDesiredFeatures(
const vpHomogeneousMatrix &cMo,
const unsigned int width,
const unsigned int height,
104 const std::vector<vpColVector> &point_cloud,
vpColVector &desired_features,
105 const unsigned int stepX,
const unsigned int stepY
106 #
if DEBUG_DISPLAY_DEPTH_NORMAL
115 void computeVisibility();
116 void computeVisibilityDisplay();
118 void computeNormalVisibility(
const double nx,
const double ny,
const double nz,
const vpColVector ¢roid_point,
121 void computeNormalVisibility(
const float nx,
const float ny,
const float nz,
const pcl::PointXYZ ¢roid_point,
122 pcl::PointXYZ &face_normal);
124 void computeNormalVisibility(
const double nx,
const double ny,
const double nz,
const vpHomogeneousMatrix &cMo,
128 const vpColor &col,
const unsigned int thickness = 1,
const bool displayFullModel =
false);
130 const vpColor &col,
const unsigned int thickness = 1,
const bool displayFullModel =
false);
133 const double scale = 0.05,
const unsigned int thickness = 1);
135 const double scale = 0.05,
const unsigned int thickness = 1);
137 inline bool isTracked()
const {
return m_isTrackedDepthNormalFace; }
139 inline bool isVisible()
const {
return m_polygon->
isvisible; }
143 inline void setFaceCentroidMethod(
const vpFaceCentroidType &method) { m_faceCentroidMethod = method; }
145 inline void setFeatureEstimationMethod(
const vpFeatureEstimationType &method) { m_featureEstimationMethod = method; }
147 inline void setPclPlaneEstimationMethod(
const int method) { m_pclPlaneEstimationMethod = method; }
149 inline void setPclPlaneEstimationRansacMaxIter(
const int maxIter) { m_pclPlaneEstimationRansacMaxIter = maxIter; }
151 inline void setPclPlaneEstimationRansacThreshold(
const double threshold)
153 m_pclPlaneEstimationRansacThreshold = threshold;
156 void setScanLineVisibilityTest(
const bool v);
158 inline void setTracked(
const bool tracked) { m_isTrackedDepthNormalFace = tracked; }
175 PolygonLine() : m_p1(NULL), m_p2(NULL), m_poly(), m_imPt1(), m_imPt2() {}
177 PolygonLine(
const PolygonLine &polyLine)
178 : m_p1(NULL), m_p2(NULL), m_poly(polyLine.m_poly), m_imPt1(polyLine.m_imPt1), m_imPt2(polyLine.m_imPt2)
184 PolygonLine &operator=(PolygonLine other)
191 void swap(PolygonLine &first, PolygonLine &second)
194 swap(first.m_p1, second.m_p1);
195 swap(first.m_p2, second.m_p2);
196 swap(first.m_poly, second.m_poly);
197 swap(first.m_imPt1, second.m_imPt1);
198 swap(first.m_imPt2, second.m_imPt2);
202 template <
class T>
class Mat33
209 inline T operator[](
const size_t i)
const {
return data[i]; }
211 inline T &operator[](
const size_t i) {
return data[i]; }
213 Mat33 inverse()
const
216 T det = data[0] * (data[4] * data[8] - data[7] * data[5]) - data[1] * (data[3] * data[8] - data[5] * data[6]) +
217 data[2] * (data[3] * data[7] - data[4] * data[6]);
221 minv[0] = (data[4] * data[8] - data[7] * data[5]) * invdet;
222 minv[1] = (data[2] * data[7] - data[1] * data[8]) * invdet;
223 minv[2] = (data[1] * data[5] - data[2] * data[4]) * invdet;
224 minv[3] = (data[5] * data[6] - data[3] * data[8]) * invdet;
225 minv[4] = (data[0] * data[8] - data[2] * data[6]) * invdet;
226 minv[5] = (data[3] * data[2] - data[0] * data[5]) * invdet;
227 minv[6] = (data[3] * data[7] - data[6] * data[4]) * invdet;
228 minv[7] = (data[6] * data[1] - data[0] * data[7]) * invdet;
229 minv[8] = (data[0] * data[4] - data[3] * data[1]) * invdet;
237 bool m_faceActivated;
239 vpFaceCentroidType m_faceCentroidMethod;
245 vpFeatureEstimationType m_featureEstimationMethod;
247 bool m_isTrackedDepthNormalFace;
251 std::vector<vpMbtDistanceLine *> m_listOfFaceLines;
256 int m_pclPlaneEstimationMethod;
258 int m_pclPlaneEstimationRansacMaxIter;
260 double m_pclPlaneEstimationRansacThreshold;
262 std::vector<PolygonLine> m_polygonLines;
265 bool computeDesiredFeaturesPCL(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud_face,
269 void computeDesiredFeaturesRobustFeatures(
const std::vector<double> &point_cloud_face_custom,
273 void computeDesiredFeaturesSVD(
const std::vector<double> &point_cloud_face,
const vpHomogeneousMatrix &cMo,
279 bool computePolygonCentroid(
const std::vector<vpPoint> &points,
vpPoint ¢roid);
281 void computeROI(
const vpHomogeneousMatrix &cMo,
const unsigned int width,
const unsigned int height,
282 std::vector<vpImagePoint> &roiPts
283 #
if DEBUG_DISPLAY_DEPTH_NORMAL
285 std::vector<std::vector<vpImagePoint> > &roiPts_vec
289 void estimateFeatures(
const std::vector<double> &point_cloud_face,
const vpHomogeneousMatrix &cMo,
290 vpColVector &x_estimated, std::vector<double> &weights);
292 void estimatePlaneEquationSVD(
const std::vector<double> &point_cloud_face,
const vpHomogeneousMatrix &cMo,