36 #ifndef _vpMbtFaceDepthNormal_h_
37 #define _vpMbtFaceDepthNormal_h_
41 #include <visp3/core/vpConfig.h>
42 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS)
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
71 ROBUST_FEATURE_ESTIMATION = 0,
72 ROBUST_SVD_PLANE_ESTIMATION = 1,
73 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS)
74 PCL_PLANE_ESTIMATION = 2
99 int polygon = -1, std::string name =
"");
101 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS)
102 bool computeDesiredFeatures(
const vpHomogeneousMatrix &cMo,
unsigned int width,
unsigned int height,
103 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
104 vpColVector &desired_features,
unsigned int stepX,
unsigned int stepY
105 #
if DEBUG_DISPLAY_DEPTH_NORMAL
112 bool computeDesiredFeatures(
const vpHomogeneousMatrix &cMo,
unsigned int width,
unsigned int height,
113 const std::vector<vpColVector> &point_cloud,
vpColVector &desired_features,
114 unsigned int stepX,
unsigned int stepY
115 #
if DEBUG_DISPLAY_DEPTH_NORMAL
121 bool computeDesiredFeatures(
const vpHomogeneousMatrix &cMo,
unsigned int width,
unsigned int height,
123 unsigned int stepX,
unsigned int stepY
124 #
if DEBUG_DISPLAY_DEPTH_NORMAL
133 void computeVisibility();
134 void computeVisibilityDisplay();
136 void computeNormalVisibility(
double nx,
double ny,
double nz,
const vpColVector ¢roid_point,
138 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS)
139 void computeNormalVisibility(
float nx,
float ny,
float nz,
const pcl::PointXYZ ¢roid_point,
140 pcl::PointXYZ &face_normal);
142 void computeNormalVisibility(
double nx,
double ny,
double nz,
const vpHomogeneousMatrix &cMo,
146 const vpColor &col,
unsigned int thickness = 1,
bool displayFullModel =
false);
148 const vpColor &col,
unsigned int thickness = 1,
bool displayFullModel =
false);
151 double scale = 0.05,
unsigned int thickness = 1);
153 double scale = 0.05,
unsigned int thickness = 1);
156 double scale = 0.05);
158 std::vector<std::vector<double> > getModelForDisplay(
unsigned int width,
unsigned int height,
160 bool displayFullModel =
false);
162 inline bool isTracked()
const {
return m_isTrackedDepthNormalFace; }
178 m_pclPlaneEstimationRansacThreshold = threshold;
181 void setScanLineVisibilityTest(
bool v);
183 inline void setTracked(
bool tracked) { m_isTrackedDepthNormalFace = tracked; }
200 PolygonLine() : m_p1(nullptr), m_p2(nullptr), m_poly(), m_imPt1(), m_imPt2() { }
202 PolygonLine(
const PolygonLine &polyLine)
203 : m_p1(nullptr), m_p2(nullptr), m_poly(polyLine.m_poly), m_imPt1(polyLine.m_imPt1), m_imPt2(polyLine.m_imPt2)
209 PolygonLine &operator=(PolygonLine other)
216 void swap(PolygonLine &first, PolygonLine &second)
219 swap(first.m_p1, second.m_p1);
220 swap(first.m_p2, second.m_p2);
221 swap(first.m_poly, second.m_poly);
222 swap(first.m_imPt1, second.m_imPt1);
223 swap(first.m_imPt2, second.m_imPt2);
227 template <
class T>
class Mat33
232 Mat33() : data(9) { }
234 inline T operator[](
const size_t i)
const {
return data[i]; }
236 inline T &operator[](
const size_t i) {
return data[i]; }
238 Mat33 inverse()
const
241 T det = data[0] * (data[4] * data[8] - data[7] * data[5]) - data[1] * (data[3] * data[8] - data[5] * data[6]) +
242 data[2] * (data[3] * data[7] - data[4] * data[6]);
246 minv[0] = (data[4] * data[8] - data[7] * data[5]) * invdet;
247 minv[1] = (data[2] * data[7] - data[1] * data[8]) * invdet;
248 minv[2] = (data[1] * data[5] - data[2] * data[4]) * invdet;
249 minv[3] = (data[5] * data[6] - data[3] * data[8]) * invdet;
250 minv[4] = (data[0] * data[8] - data[2] * data[6]) * invdet;
251 minv[5] = (data[3] * data[2] - data[0] * data[5]) * invdet;
252 minv[6] = (data[3] * data[7] - data[6] * data[4]) * invdet;
253 minv[7] = (data[6] * data[1] - data[0] * data[7]) * invdet;
254 minv[8] = (data[0] * data[4] - data[3] * data[1]) * invdet;
289 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS)
290 bool computeDesiredFeaturesPCL(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud_face,
294 void computeDesiredFeaturesRobustFeatures(
const std::vector<double> &point_cloud_face_custom,
298 void computeDesiredFeaturesSVD(
const std::vector<double> &point_cloud_face,
const vpHomogeneousMatrix &cMo,
304 bool computePolygonCentroid(
const std::vector<vpPoint> &points,
vpPoint ¢roid);
307 std::vector<vpImagePoint> &roiPts
308 #
if DEBUG_DISPLAY_DEPTH_NORMAL
310 std::vector<std::vector<vpImagePoint> > &roiPts_vec
314 void estimateFeatures(
const std::vector<double> &point_cloud_face,
const vpHomogeneousMatrix &cMo,
315 vpColVector &x_estimated, std::vector<double> &weights);
317 void estimatePlaneEquationSVD(
const std::vector<double> &point_cloud_face,
const vpHomogeneousMatrix &cMo,
324 #ifdef VISP_HAVE_NLOHMANN_JSON
325 #include<nlohmann/json.hpp>
326 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS)
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionalities.
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Implementation of a matrix and operations on matrices.
double m_pclPlaneEstimationRansacThreshold
PCL plane estimation RANSAC threshold.
int m_pclPlaneEstimationMethod
PCL plane estimation method.
double m_distNearClip
Distance for near clipping.
double m_distFarClip
Distance for near clipping.
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
std::vector< vpMbtDistanceLine * > m_listOfFaceLines
void setPclPlaneEstimationMethod(int method)
bool m_isTrackedDepthNormalFace
vpFeatureEstimationType m_featureEstimationMethod
Method to estimate the desired features.
void setPclPlaneEstimationRansacThreshold(double threshold)
vpMbtPolygon * m_polygon
Polygon defining the face.
vpPoint m_faceDesiredCentroid
Desired centroid (computed from the sensor)
vpCameraParameters m_cam
Camera intrinsic parameters.
@ ROBUST_FEATURE_ESTIMATION
Robust scheme to estimate the normal of the plane.
@ ROBUST_SVD_PLANE_ESTIMATION
Use SVD and robust scheme to estimate the normal of the plane.
@ PCL_PLANE_ESTIMATION
Use PCL to estimate the normal of the plane.
vpPlane m_planeObject
Plane equation described in the object frame.
bool m_faceActivated
True if the face should be considered by the tracker.
@ GEOMETRIC_CENTROID
Compute the geometric centroid.
vpFaceCentroidType m_faceCentroidMethod
Method to compute the face centroid for the current features.
int m_pclPlaneEstimationRansacMaxIter
PCL pane estimation max number of iterations.
unsigned int m_clippingFlag
Flags specifying which clipping to used.
void setTracked(bool tracked)
vpPoint m_faceDesiredNormal
Face (normalized) normal (computed from the sensor)
void setFeatureEstimationMethod(const vpFeatureEstimationType &method)
std::vector< PolygonLine > m_polygonLines
void setFaceCentroidMethod(const vpFaceCentroidType &method)
void setPclPlaneEstimationRansacMaxIter(int maxIter)
bool m_useScanLine
Scan line visibility.
Implementation of a polygon of the model used by the model-based tracker.
bool isvisible
flag to specify whether the face is visible or not
This class defines the container for a plane geometrical structure.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
vpPoint * p
corners in the object frame
Class for generating random numbers with uniform probability density.