Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMbtFaceDepthNormal.h
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Manage depth normal features for a particular face.
33 *
34*****************************************************************************/
35
36#ifndef _vpMbtFaceDepthNormal_h_
37#define _vpMbtFaceDepthNormal_h_
38
39#include <iostream>
40
41#include <visp3/core/vpConfig.h>
42#ifdef VISP_HAVE_PCL
43#include <pcl/point_cloud.h>
44#include <pcl/point_types.h>
45#endif
46
47#include <visp3/core/vpPlane.h>
48#include <visp3/mbt/vpMbTracker.h>
49#include <visp3/mbt/vpMbtDistanceLine.h>
50
51#define DEBUG_DISPLAY_DEPTH_NORMAL 0
52
53class VISP_EXPORT vpMbtFaceDepthNormal
54{
55public:
58 MEAN_CENTROID
59 };
60
62 ROBUST_FEATURE_ESTIMATION = 0,
63 ROBUST_SVD_PLANE_ESTIMATION = 1,
64#ifdef VISP_HAVE_PCL
65 PCL_PLANE_ESTIMATION = 2
66#endif
67 };
68
72 unsigned int m_clippingFlag;
85
87 virtual ~vpMbtFaceDepthNormal();
88
89 void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces<vpMbtPolygon> *const faces, vpUniRand &rand_gen,
90 int polygon = -1, std::string name = "");
91
92#ifdef VISP_HAVE_PCL
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
97 ,
98 vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
99#endif
100 ,
101 const vpImage<bool> *mask = NULL);
102#endif
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
107 ,
108 vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
109#endif
110 ,
111 const vpImage<bool> *mask = NULL);
112
113 void computeInteractionMatrix(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &features);
114
115 void computeVisibility();
116 void computeVisibilityDisplay();
117
118 void computeNormalVisibility(double nx, double ny, double nz, const vpColVector &centroid_point,
119 vpColVector &face_normal);
120#ifdef VISP_HAVE_PCL
121 void computeNormalVisibility(float nx, float ny, float nz, const pcl::PointXYZ &centroid_point,
122 pcl::PointXYZ &face_normal);
123#endif
124 void computeNormalVisibility(double nx, double ny, double nz, const vpHomogeneousMatrix &cMo,
125 const vpCameraParameters &camera, vpColVector &correct_normal, vpPoint &centroid);
126
127 void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
128 const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
129 void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
130 const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);
131
132 void displayFeature(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
133 double scale = 0.05, unsigned int thickness = 1);
134 void displayFeature(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
135 double scale = 0.05, unsigned int thickness = 1);
136
137 std::vector<std::vector<double> > getFeaturesForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
138 double scale = 0.05);
139
140 std::vector<std::vector<double> > getModelForDisplay(unsigned int width, unsigned int height,
141 const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
142 bool displayFullModel = false);
143
144 inline bool isTracked() const { return m_isTrackedDepthNormalFace; }
145
146 inline bool isVisible() const { return m_polygon->isvisible; }
147
148 void setCameraParameters(const vpCameraParameters &camera);
149
150 inline void setFaceCentroidMethod(const vpFaceCentroidType &method) { m_faceCentroidMethod = method; }
151
152 inline void setFeatureEstimationMethod(const vpFeatureEstimationType &method) { m_featureEstimationMethod = method; }
153
154 inline void setPclPlaneEstimationMethod(int method) { m_pclPlaneEstimationMethod = method; }
155
156 inline void setPclPlaneEstimationRansacMaxIter(int maxIter) { m_pclPlaneEstimationRansacMaxIter = maxIter; }
157
158 inline void setPclPlaneEstimationRansacThreshold(double threshold)
159 {
160 m_pclPlaneEstimationRansacThreshold = threshold;
161 }
162
163 void setScanLineVisibilityTest(bool v);
164
165 inline void setTracked(bool tracked) { m_isTrackedDepthNormalFace = tracked; }
166
167private:
168 class PolygonLine
169 {
170 public:
172 vpPoint *m_p1;
174 vpPoint *m_p2;
176 vpMbtPolygon m_poly;
178 vpImagePoint m_imPt1;
180 vpImagePoint m_imPt2;
181
182 PolygonLine() : m_p1(NULL), m_p2(NULL), m_poly(), m_imPt1(), m_imPt2() {}
183
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)
186 {
187 m_p1 = &m_poly.p[0];
188 m_p2 = &m_poly.p[1];
189 }
190
191 PolygonLine &operator=(PolygonLine other)
192 {
193 swap(*this, other);
194
195 return *this;
196 }
197
198 void swap(PolygonLine &first, PolygonLine &second)
199 {
200 using std::swap;
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);
206 }
207 };
208
209 template <class T> class Mat33
210 {
211 public:
212 std::vector<T> data;
213
214 Mat33() : data(9) {}
215
216 inline T operator[](const size_t i) const { return data[i]; }
217
218 inline T &operator[](const size_t i) { return data[i]; }
219
220 Mat33 inverse() const
221 {
222 // determinant
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]);
225 T invdet = 1 / det;
226
227 Mat33<T> minv;
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;
237
238 return minv;
239 }
240 };
241
242protected:
258 std::vector<vpMbtDistanceLine *> m_listOfFaceLines;
269 std::vector<PolygonLine> m_polygonLines;
270
271#ifdef VISP_HAVE_PCL
272 bool computeDesiredFeaturesPCL(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud_face,
273 vpColVector &desired_features, vpColVector &desired_normal,
274 vpColVector &centroid_point);
275#endif
276 void computeDesiredFeaturesRobustFeatures(const std::vector<double> &point_cloud_face_custom,
277 const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
278 vpColVector &desired_features, vpColVector &desired_normal,
279 vpColVector &centroid_point);
280 void computeDesiredFeaturesSVD(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
281 vpColVector &desired_features, vpColVector &desired_normal,
282 vpColVector &centroid_point);
283 void computeDesiredNormalAndCentroid(const vpHomogeneousMatrix &cMo, const vpColVector &desired_normal,
284 const vpColVector &centroid_point);
285
286 bool computePolygonCentroid(const std::vector<vpPoint> &points, vpPoint &centroid);
287
288 void computeROI(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
289 std::vector<vpImagePoint> &roiPts
290#if DEBUG_DISPLAY_DEPTH_NORMAL
291 ,
292 std::vector<std::vector<vpImagePoint> > &roiPts_vec
293#endif
294 );
295
296 void estimateFeatures(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
297 vpColVector &x_estimated, std::vector<double> &weights);
298
299 void estimatePlaneEquationSVD(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
300 vpColVector &plane_equation_estimated, vpColVector &centroid);
301
302 bool samePoint(const vpPoint &P1, const vpPoint &P2) const;
303};
304
305#ifdef VISP_HAVE_NLOHMANN_JSON
306#include<nlohmann/json.hpp>
307#ifdef VISP_HAVE_PCL
308NLOHMANN_JSON_SERIALIZE_ENUM( vpMbtFaceDepthNormal::vpFeatureEstimationType, {
312});
313#else
314NLOHMANN_JSON_SERIALIZE_ENUM(vpMbtFaceDepthNormal::vpFeatureEstimationType, {
317});
318#endif
319#endif
320
321#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
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 ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Implementation of the polygons management for the model-based trackers.
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)
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.
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.
Definition vpPlane.h:54
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
vpPoint * p
corners in the object frame
Definition vpPolygon3D.h:76
Class for generating random numbers with uniform probability density.
Definition vpUniRand.h:122