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
64 ROBUST_FEATURE_ESTIMATION = 0,
65 ROBUST_SVD_PLANE_ESTIMATION = 1,
67 PCL_PLANE_ESTIMATION = 2
92 int polygon = -1, std::string name =
"");
95 bool computeDesiredFeatures(
const vpHomogeneousMatrix &cMo,
unsigned int width,
unsigned int height,
96 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
97 vpColVector &desired_features,
unsigned int stepX,
unsigned int stepY
98 #
if DEBUG_DISPLAY_DEPTH_NORMAL
105 bool computeDesiredFeatures(
const vpHomogeneousMatrix &cMo,
unsigned int width,
unsigned int height,
106 const std::vector<vpColVector> &point_cloud,
vpColVector &desired_features,
107 unsigned int stepX,
unsigned int stepY
108 #
if DEBUG_DISPLAY_DEPTH_NORMAL
114 bool computeDesiredFeatures(
const vpHomogeneousMatrix &cMo,
unsigned int width,
unsigned int height,
116 unsigned int stepX,
unsigned int stepY
117 #
if DEBUG_DISPLAY_DEPTH_NORMAL
126 void computeVisibility();
127 void computeVisibilityDisplay();
129 void computeNormalVisibility(
double nx,
double ny,
double nz,
const vpColVector ¢roid_point,
132 void computeNormalVisibility(
float nx,
float ny,
float nz,
const pcl::PointXYZ ¢roid_point,
133 pcl::PointXYZ &face_normal);
135 void computeNormalVisibility(
double nx,
double ny,
double nz,
const vpHomogeneousMatrix &cMo,
139 const vpColor &col,
unsigned int thickness = 1,
bool displayFullModel =
false);
141 const vpColor &col,
unsigned int thickness = 1,
bool displayFullModel =
false);
144 double scale = 0.05,
unsigned int thickness = 1);
146 double scale = 0.05,
unsigned int thickness = 1);
149 double scale = 0.05);
151 std::vector<std::vector<double> > getModelForDisplay(
unsigned int width,
unsigned int height,
153 bool displayFullModel =
false);
155 inline bool isTracked()
const {
return m_isTrackedDepthNormalFace; }
171 m_pclPlaneEstimationRansacThreshold = threshold;
174 void setScanLineVisibilityTest(
bool v);
176 inline void setTracked(
bool tracked) { m_isTrackedDepthNormalFace = tracked; }
193 PolygonLine() : m_p1(nullptr), m_p2(nullptr), m_poly(), m_imPt1(), m_imPt2() { }
195 PolygonLine(
const PolygonLine &polyLine)
196 : m_p1(nullptr), m_p2(nullptr), m_poly(polyLine.m_poly), m_imPt1(polyLine.m_imPt1), m_imPt2(polyLine.m_imPt2)
202 PolygonLine &operator=(PolygonLine other)
209 void swap(PolygonLine &first, PolygonLine &second)
212 swap(first.m_p1, second.m_p1);
213 swap(first.m_p2, second.m_p2);
214 swap(first.m_poly, second.m_poly);
215 swap(first.m_imPt1, second.m_imPt1);
216 swap(first.m_imPt2, second.m_imPt2);
220 template <
class T>
class Mat33
225 Mat33() : data(9) { }
227 inline T operator[](
const size_t i)
const {
return data[i]; }
229 inline T &operator[](
const size_t i) {
return data[i]; }
231 Mat33 inverse()
const
234 T det = data[0] * (data[4] * data[8] - data[7] * data[5]) - data[1] * (data[3] * data[8] - data[5] * data[6]) +
235 data[2] * (data[3] * data[7] - data[4] * data[6]);
239 minv[0] = (data[4] * data[8] - data[7] * data[5]) * invdet;
240 minv[1] = (data[2] * data[7] - data[1] * data[8]) * invdet;
241 minv[2] = (data[1] * data[5] - data[2] * data[4]) * invdet;
242 minv[3] = (data[5] * data[6] - data[3] * data[8]) * invdet;
243 minv[4] = (data[0] * data[8] - data[2] * data[6]) * invdet;
244 minv[5] = (data[3] * data[2] - data[0] * data[5]) * invdet;
245 minv[6] = (data[3] * data[7] - data[6] * data[4]) * invdet;
246 minv[7] = (data[6] * data[1] - data[0] * data[7]) * invdet;
247 minv[8] = (data[0] * data[4] - data[3] * data[1]) * invdet;
283 bool computeDesiredFeaturesPCL(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud_face,
287 void computeDesiredFeaturesRobustFeatures(
const std::vector<double> &point_cloud_face_custom,
291 void computeDesiredFeaturesSVD(
const std::vector<double> &point_cloud_face,
const vpHomogeneousMatrix &cMo,
297 bool computePolygonCentroid(
const std::vector<vpPoint> &points,
vpPoint ¢roid);
300 std::vector<vpImagePoint> &roiPts
301 #
if DEBUG_DISPLAY_DEPTH_NORMAL
303 std::vector<std::vector<vpImagePoint> > &roiPts_vec
307 void estimateFeatures(
const std::vector<double> &point_cloud_face,
const vpHomogeneousMatrix &cMo,
308 vpColVector &x_estimated, std::vector<double> &weights);
310 void estimatePlaneEquationSVD(
const std::vector<double> &point_cloud_face,
const vpHomogeneousMatrix &cMo,
316 #ifdef VISP_HAVE_NLOHMANN_JSON
317 #include<nlohmann/json.hpp>
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_SVD_PLANE_ESTIMATION
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.