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
90 int polygon = -1, std::string name =
"");
93 bool computeDesiredFeatures(
const vpHomogeneousMatrix &cMo,
unsigned int width,
unsigned int height,
94 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
95 vpColVector &desired_features,
unsigned int stepX,
unsigned int stepY
96 #
if DEBUG_DISPLAY_DEPTH_NORMAL
103 bool computeDesiredFeatures(
const vpHomogeneousMatrix &cMo,
unsigned int width,
unsigned int height,
104 const std::vector<vpColVector> &point_cloud,
vpColVector &desired_features,
105 unsigned int stepX,
unsigned int stepY
106 #
if DEBUG_DISPLAY_DEPTH_NORMAL
115 void computeVisibility();
116 void computeVisibilityDisplay();
118 void computeNormalVisibility(
double nx,
double ny,
double nz,
const vpColVector ¢roid_point,
121 void computeNormalVisibility(
float nx,
float ny,
float nz,
const pcl::PointXYZ ¢roid_point,
122 pcl::PointXYZ &face_normal);
124 void computeNormalVisibility(
double nx,
double ny,
double nz,
const vpHomogeneousMatrix &cMo,
128 const vpColor &col,
unsigned int thickness = 1,
bool displayFullModel =
false);
130 const vpColor &col,
unsigned int thickness = 1,
bool displayFullModel =
false);
133 double scale = 0.05,
unsigned int thickness = 1);
135 double scale = 0.05,
unsigned int thickness = 1);
138 double scale = 0.05);
140 std::vector<std::vector<double> > getModelForDisplay(
unsigned int width,
unsigned int height,
142 bool displayFullModel =
false);
144 inline bool isTracked()
const {
return m_isTrackedDepthNormalFace; }
160 m_pclPlaneEstimationRansacThreshold = threshold;
163 void setScanLineVisibilityTest(
bool v);
165 inline void setTracked(
bool tracked) { m_isTrackedDepthNormalFace = tracked; }
182 PolygonLine() : m_p1(NULL), m_p2(NULL), m_poly(), m_imPt1(), m_imPt2() {}
184 PolygonLine(
const PolygonLine &polyLine)
185 : m_p1(NULL), m_p2(NULL), m_poly(polyLine.m_poly), m_imPt1(polyLine.m_imPt1), m_imPt2(polyLine.m_imPt2)
191 PolygonLine &operator=(PolygonLine other)
198 void swap(PolygonLine &first, PolygonLine &second)
201 swap(first.m_p1, second.m_p1);
202 swap(first.m_p2, second.m_p2);
203 swap(first.m_poly, second.m_poly);
204 swap(first.m_imPt1, second.m_imPt1);
205 swap(first.m_imPt2, second.m_imPt2);
209 template <
class T>
class Mat33
216 inline T operator[](
const size_t i)
const {
return data[i]; }
218 inline T &operator[](
const size_t i) {
return data[i]; }
220 Mat33 inverse()
const
223 T det = data[0] * (data[4] * data[8] - data[7] * data[5]) - data[1] * (data[3] * data[8] - data[5] * data[6]) +
224 data[2] * (data[3] * data[7] - data[4] * data[6]);
228 minv[0] = (data[4] * data[8] - data[7] * data[5]) * invdet;
229 minv[1] = (data[2] * data[7] - data[1] * data[8]) * invdet;
230 minv[2] = (data[1] * data[5] - data[2] * data[4]) * invdet;
231 minv[3] = (data[5] * data[6] - data[3] * data[8]) * invdet;
232 minv[4] = (data[0] * data[8] - data[2] * data[6]) * invdet;
233 minv[5] = (data[3] * data[2] - data[0] * data[5]) * invdet;
234 minv[6] = (data[3] * data[7] - data[6] * data[4]) * invdet;
235 minv[7] = (data[6] * data[1] - data[0] * data[7]) * invdet;
236 minv[8] = (data[0] * data[4] - data[3] * data[1]) * invdet;
272 bool computeDesiredFeaturesPCL(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud_face,
276 void computeDesiredFeaturesRobustFeatures(
const std::vector<double> &point_cloud_face_custom,
280 void computeDesiredFeaturesSVD(
const std::vector<double> &point_cloud_face,
const vpHomogeneousMatrix &cMo,
286 bool computePolygonCentroid(
const std::vector<vpPoint> &points,
vpPoint ¢roid);
289 std::vector<vpImagePoint> &roiPts
290 #
if DEBUG_DISPLAY_DEPTH_NORMAL
292 std::vector<std::vector<vpImagePoint> > &roiPts_vec
296 void estimateFeatures(
const std::vector<double> &point_cloud_face,
const vpHomogeneousMatrix &cMo,
297 vpColVector &x_estimated, std::vector<double> &weights);
299 void estimatePlaneEquationSVD(
const std::vector<double> &point_cloud_face,
const vpHomogeneousMatrix &cMo,
305 #ifdef VISP_HAVE_NLOHMANN_JSON
306 #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.