Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMbtFaceDepthNormal.cpp
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#include <visp3/core/vpCPUFeatures.h>
37#include <visp3/mbt/vpMbtFaceDepthNormal.h>
38#include <visp3/mbt/vpMbtTukeyEstimator.h>
39
40#ifdef VISP_HAVE_PCL
41#include <pcl/common/centroid.h>
42#include <pcl/filters/extract_indices.h>
43#include <pcl/segmentation/sac_segmentation.h>
44#endif
45
46#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
47#include <emmintrin.h>
48#define VISP_HAVE_SSE2 1
49#endif
50
51#define USE_SSE_CODE 1
52#if VISP_HAVE_SSE2 && USE_SSE_CODE
53#define USE_SSE 1
54#else
55#define USE_SSE 0
56#endif
57
59 : m_cam(), m_clippingFlag(vpPolygon3D::NO_CLIPPING), m_distFarClip(100), m_distNearClip(0.001), m_hiddenFace(NULL),
60 m_planeObject(), m_polygon(NULL), m_useScanLine(false), m_faceActivated(false),
61 m_faceCentroidMethod(GEOMETRIC_CENTROID), m_faceDesiredCentroid(), m_faceDesiredNormal(),
62 m_featureEstimationMethod(ROBUST_FEATURE_ESTIMATION), m_isTrackedDepthNormalFace(true), m_isVisible(false),
63 m_listOfFaceLines(), m_planeCamera(),
64 m_pclPlaneEstimationMethod(2), // SAC_MSAC, see pcl/sample_consensus/method_types.h
65 m_pclPlaneEstimationRansacMaxIter(200), m_pclPlaneEstimationRansacThreshold(0.001), m_polygonLines()
66{
67}
68
70{
71 for (size_t i = 0; i < m_listOfFaceLines.size(); i++) {
72 delete m_listOfFaceLines[i];
73 }
74}
75
91 vpUniRand &rand_gen, int polygon, std::string name)
92{
93 // Build a PolygonLine to be able to easily display the lines model
94 PolygonLine polygon_line;
95
96 // Add polygon
97 polygon_line.m_poly.setNbPoint(2);
98 polygon_line.m_poly.addPoint(0, P1);
99 polygon_line.m_poly.addPoint(1, P2);
100
101 polygon_line.m_poly.setClipping(m_clippingFlag);
102 polygon_line.m_poly.setNearClippingDistance(m_distNearClip);
103 polygon_line.m_poly.setFarClippingDistance(m_distFarClip);
104
105 polygon_line.m_p1 = &polygon_line.m_poly.p[0];
106 polygon_line.m_p2 = &polygon_line.m_poly.p[1];
107
108 m_polygonLines.push_back(polygon_line);
109
110 // suppress line already in the model
111 bool already_here = false;
113
114 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
115 ++it) {
116 l = *it;
117 if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) || (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
118 already_here = true;
119 l->addPolygon(polygon);
120 l->hiddenface = faces;
122 }
123 }
124
125 if (!already_here) {
126 l = new vpMbtDistanceLine;
127
129 l->buildFrom(P1, P2, rand_gen);
130 l->addPolygon(polygon);
131 l->hiddenface = faces;
133
134 l->setIndex((unsigned int)m_listOfFaceLines.size());
135 l->setName(name);
136
139
142
145
146 m_listOfFaceLines.push_back(l);
147 }
148}
149
150#ifdef VISP_HAVE_PCL
152 unsigned int height,
153 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
154 vpColVector &desired_features, unsigned int stepX, unsigned int stepY
155#if DEBUG_DISPLAY_DEPTH_NORMAL
156 ,
157 vpImage<unsigned char> &debugImage,
158 std::vector<std::vector<vpImagePoint> > &roiPts_vec
159#endif
160 ,
161 const vpImage<bool> *mask)
162{
163 m_faceActivated = false;
164
165 if (width == 0 || height == 0)
166 return false;
167
168 std::vector<vpImagePoint> roiPts;
169 vpColVector desired_normal(3);
170
171 computeROI(cMo, width, height, roiPts
172#if DEBUG_DISPLAY_DEPTH_NORMAL
173 ,
174 roiPts_vec
175#endif
176 );
177
178 if (roiPts.size() <= 2) {
179#ifndef NDEBUG
180 std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
181#endif
182 return false;
183 }
184
185 vpPolygon polygon_2d(roiPts);
186 vpRect bb = polygon_2d.getBoundingBox();
187
188 unsigned int top = (unsigned int)std::max(0.0, bb.getTop());
189 unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom()));
190 unsigned int left = (unsigned int)std::max(0.0, bb.getLeft());
191 unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight()));
192
193 bb.setTop(top);
194 bb.setBottom(bottom);
195 bb.setLeft(left);
196 bb.setRight(right);
197
198 // Keep only 3D points inside the projected polygon face
199 pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face(new pcl::PointCloud<pcl::PointXYZ>);
200 std::vector<double> point_cloud_face_vec, point_cloud_face_custom;
201
203 point_cloud_face_custom.reserve((size_t)(3 * bb.getWidth() * bb.getHeight()));
204 point_cloud_face_vec.reserve((size_t)(3 * bb.getWidth() * bb.getHeight()));
206 point_cloud_face_vec.reserve((size_t)(3 * bb.getWidth() * bb.getHeight()));
208 point_cloud_face->reserve((size_t)(bb.getWidth() * bb.getHeight()));
209 }
210
211 bool checkSSE2 = vpCPUFeatures::checkSSE2();
212#if !USE_SSE
213 checkSSE2 = false;
214#else
215 bool push = false;
216 double prev_x, prev_y, prev_z;
217#endif
218
219 double x = 0.0, y = 0.0;
220 for (unsigned int i = top; i < bottom; i += stepY) {
221 for (unsigned int j = left; j < right; j += stepX) {
222 if (vpMeTracker::inMask(mask, i, j) && pcl::isFinite((*point_cloud)(j, i)) && (*point_cloud)(j, i).z > 0 &&
223 (m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
224 j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
225 m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex())
226 : polygon_2d.isInside(vpImagePoint(i, j)))) {
227
229 point_cloud_face->push_back((*point_cloud)(j, i));
232 point_cloud_face_vec.push_back((*point_cloud)(j, i).x);
233 point_cloud_face_vec.push_back((*point_cloud)(j, i).y);
234 point_cloud_face_vec.push_back((*point_cloud)(j, i).z);
235
237 // Add point for custom method for plane equation estimation
239
240 if (checkSSE2) {
241#if USE_SSE
242 if (!push) {
243 push = true;
244 prev_x = x;
245 prev_y = y;
246 prev_z = (*point_cloud)(j, i).z;
247 } else {
248 push = false;
249 point_cloud_face_custom.push_back(prev_x);
250 point_cloud_face_custom.push_back(x);
251
252 point_cloud_face_custom.push_back(prev_y);
253 point_cloud_face_custom.push_back(y);
254
255 point_cloud_face_custom.push_back(prev_z);
256 point_cloud_face_custom.push_back((*point_cloud)(j, i).z);
257 }
258#endif
259 } else {
260 point_cloud_face_custom.push_back(x);
261 point_cloud_face_custom.push_back(y);
262 point_cloud_face_custom.push_back((*point_cloud)(j, i).z);
263 }
264 }
265 }
266
267#if DEBUG_DISPLAY_DEPTH_NORMAL
268 debugImage[i][j] = 255;
269#endif
270 }
271 }
272 }
273
274#if USE_SSE
275 if (checkSSE2 && push) {
276 point_cloud_face_custom.push_back(prev_x);
277 point_cloud_face_custom.push_back(prev_y);
278 point_cloud_face_custom.push_back(prev_z);
279 }
280#endif
281
282 if (point_cloud_face->empty() && point_cloud_face_custom.empty() && point_cloud_face_vec.empty()) {
283 return false;
284 }
285
286 // Face centroid computed by the different methods
287 vpColVector centroid_point(3);
288
290 if (!computeDesiredFeaturesPCL(point_cloud_face, desired_features, desired_normal, centroid_point)) {
291 return false;
292 }
294 computeDesiredFeaturesSVD(point_cloud_face_vec, cMo, desired_features, desired_normal, centroid_point);
296 computeDesiredFeaturesRobustFeatures(point_cloud_face_custom, point_cloud_face_vec, cMo, desired_features,
297 desired_normal, centroid_point);
298 } else {
299 throw vpException(vpException::badValue, "Unknown feature estimation method!");
300 }
301
302 computeDesiredNormalAndCentroid(cMo, desired_normal, centroid_point);
303
304 m_faceActivated = true;
305
306 return true;
307}
308#endif
309
311 unsigned int height, const std::vector<vpColVector> &point_cloud,
312 vpColVector &desired_features, unsigned int stepX, unsigned int stepY
313#if DEBUG_DISPLAY_DEPTH_NORMAL
314 ,
315 vpImage<unsigned char> &debugImage,
316 std::vector<std::vector<vpImagePoint> > &roiPts_vec
317#endif
318 ,
319 const vpImage<bool> *mask)
320{
321 m_faceActivated = false;
322
323 if (width == 0 || height == 0)
324 return false;
325
326 std::vector<vpImagePoint> roiPts;
327 vpColVector desired_normal(3);
328
329 computeROI(cMo, width, height, roiPts
330#if DEBUG_DISPLAY_DEPTH_NORMAL
331 ,
332 roiPts_vec
333#endif
334 );
335
336 if (roiPts.size() <= 2) {
337#ifndef NDEBUG
338 std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
339#endif
340 return false;
341 }
342
343 vpPolygon polygon_2d(roiPts);
344 vpRect bb = polygon_2d.getBoundingBox();
345
346 unsigned int top = (unsigned int)std::max(0.0, bb.getTop());
347 unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom()));
348 unsigned int left = (unsigned int)std::max(0.0, bb.getLeft());
349 unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight()));
350
351 bb.setTop(top);
352 bb.setBottom(bottom);
353 bb.setLeft(left);
354 bb.setRight(right);
355
356 // Keep only 3D points inside the projected polygon face
357 std::vector<double> point_cloud_face, point_cloud_face_custom;
358
359 point_cloud_face.reserve((size_t)(3 * bb.getWidth() * bb.getHeight()));
361 point_cloud_face_custom.reserve((size_t)(3 * bb.getWidth() * bb.getHeight()));
362 }
363
364 bool checkSSE2 = vpCPUFeatures::checkSSE2();
365#if !USE_SSE
366 checkSSE2 = false;
367#else
368 bool push = false;
369 double prev_x, prev_y, prev_z;
370#endif
371
372 double x = 0.0, y = 0.0;
373 for (unsigned int i = top; i < bottom; i += stepY) {
374 for (unsigned int j = left; j < right; j += stepX) {
375 if (vpMeTracker::inMask(mask, i, j) && point_cloud[i * width + j][2] > 0 &&
376 (m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
377 j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
378 m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs()[i][j] == m_polygon->getIndex())
379 : polygon_2d.isInside(vpImagePoint(i, j)))) {
380 // Add point
381 point_cloud_face.push_back(point_cloud[i * width + j][0]);
382 point_cloud_face.push_back(point_cloud[i * width + j][1]);
383 point_cloud_face.push_back(point_cloud[i * width + j][2]);
384
386 // Add point for custom method for plane equation estimation
388
389 if (checkSSE2) {
390#if USE_SSE
391 if (!push) {
392 push = true;
393 prev_x = x;
394 prev_y = y;
395 prev_z = point_cloud[i * width + j][2];
396 } else {
397 push = false;
398 point_cloud_face_custom.push_back(prev_x);
399 point_cloud_face_custom.push_back(x);
400
401 point_cloud_face_custom.push_back(prev_y);
402 point_cloud_face_custom.push_back(y);
403
404 point_cloud_face_custom.push_back(prev_z);
405 point_cloud_face_custom.push_back(point_cloud[i * width + j][2]);
406 }
407#endif
408 } else {
409 point_cloud_face_custom.push_back(x);
410 point_cloud_face_custom.push_back(y);
411 point_cloud_face_custom.push_back(point_cloud[i * width + j][2]);
412 }
413 }
414
415#if DEBUG_DISPLAY_DEPTH_NORMAL
416 debugImage[i][j] = 255;
417#endif
418 }
419 }
420 }
421
422#if USE_SSE
423 if (checkSSE2 && push) {
424 point_cloud_face_custom.push_back(prev_x);
425 point_cloud_face_custom.push_back(prev_y);
426 point_cloud_face_custom.push_back(prev_z);
427 }
428#endif
429
430 if (point_cloud_face.empty() && point_cloud_face_custom.empty()) {
431 return false;
432 }
433
434 // Face centroid computed by the different methods
435 vpColVector centroid_point(3);
436
437#ifdef VISP_HAVE_PCL
439 pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face_pcl(new pcl::PointCloud<pcl::PointXYZ>);
440 point_cloud_face_pcl->reserve(point_cloud_face.size() / 3);
441
442 for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
443 point_cloud_face_pcl->push_back(
444 pcl::PointXYZ(point_cloud_face[3 * i], point_cloud_face[3 * i + 1], point_cloud_face[3 * i + 2]));
445 }
446
447 computeDesiredFeaturesPCL(point_cloud_face_pcl, desired_features, desired_normal, centroid_point);
448 } else
449#endif
451 computeDesiredFeaturesSVD(point_cloud_face, cMo, desired_features, desired_normal, centroid_point);
453 computeDesiredFeaturesRobustFeatures(point_cloud_face_custom, point_cloud_face, cMo, desired_features,
454 desired_normal, centroid_point);
455 } else {
456 throw vpException(vpException::badValue, "Unknown feature estimation method!");
457 }
458
459 computeDesiredNormalAndCentroid(cMo, desired_normal, centroid_point);
460
461 m_faceActivated = true;
462
463 return true;
464}
465
466#ifdef VISP_HAVE_PCL
467bool vpMbtFaceDepthNormal::computeDesiredFeaturesPCL(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud_face,
468 vpColVector &desired_features, vpColVector &desired_normal,
469 vpColVector &centroid_point)
470{
471 try {
472 // Compute plane equation for this subset of point cloud
473 pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
474 pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
475 // Create the segmentation object
476 pcl::SACSegmentation<pcl::PointXYZ> seg;
477 // Optional
478 seg.setOptimizeCoefficients(true);
479 // Mandatory
480 seg.setModelType(pcl::SACMODEL_PLANE);
481 seg.setMethodType(m_pclPlaneEstimationMethod);
482 seg.setDistanceThreshold(m_pclPlaneEstimationRansacThreshold);
483 seg.setMaxIterations(m_pclPlaneEstimationRansacMaxIter);
484
485 seg.setInputCloud(point_cloud_face);
486 seg.segment(*inliers, *coefficients);
487
488 pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face_extracted(new pcl::PointCloud<pcl::PointXYZ>);
489 // Create the filtering object
490 pcl::ExtractIndices<pcl::PointXYZ> extract;
491
492 // Extract the inliers
493 extract.setInputCloud(point_cloud_face);
494 extract.setIndices(inliers);
495 extract.setNegative(false);
496 extract.filter(*point_cloud_face_extracted);
497
498#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
499 pcl::PointXYZ centroid_point_pcl;
500 if (pcl::computeCentroid(*point_cloud_face_extracted, centroid_point_pcl)) {
501 pcl::PointXYZ face_normal;
502 computeNormalVisibility(coefficients->values[0], coefficients->values[1], coefficients->values[2],
503 centroid_point_pcl, face_normal);
504
505 desired_features.resize(3, false);
506 desired_features[0] = -coefficients->values[0] / coefficients->values[3];
507 desired_features[1] = -coefficients->values[1] / coefficients->values[3];
508 desired_features[2] = -coefficients->values[2] / coefficients->values[3];
509
510 desired_normal[0] = face_normal.x;
511 desired_normal[1] = face_normal.y;
512 desired_normal[2] = face_normal.z;
513
514 centroid_point[0] = centroid_point_pcl.x;
515 centroid_point[1] = centroid_point_pcl.y;
516 centroid_point[2] = centroid_point_pcl.z;
517 } else {
518 std::cerr << "Cannot compute centroid!" << std::endl;
519 return false;
520 }
521#else
522 std::cerr << "Cannot compute centroid using PCL " << PCL_VERSION_PRETTY << "!" << std::endl;
523 return false;
524#endif
525 } catch (const pcl::PCLException &e) {
526 std::cerr << "Catch a PCL exception: " << e.what() << std::endl;
527 throw;
528 }
529
530 return true;
531}
532#endif
533
534void vpMbtFaceDepthNormal::computeDesiredFeaturesRobustFeatures(const std::vector<double> &point_cloud_face_custom,
535 const std::vector<double> &point_cloud_face,
536 const vpHomogeneousMatrix &cMo,
537 vpColVector &desired_features,
538 vpColVector &desired_normal,
539 vpColVector &centroid_point)
540{
541 std::vector<double> weights;
542 double den = 0.0;
543 estimateFeatures(point_cloud_face_custom, cMo, desired_features, weights);
544
545 // Compute face centroid
546 for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
547 centroid_point[0] += weights[i] * point_cloud_face[3 * i];
548 centroid_point[1] += weights[i] * point_cloud_face[3 * i + 1];
549 centroid_point[2] += weights[i] * point_cloud_face[3 * i + 2];
550
551 den += weights[i];
552 }
553
554 centroid_point[0] /= den;
555 centroid_point[1] /= den;
556 centroid_point[2] /= den;
557
558 computeNormalVisibility(-desired_features[0], -desired_features[1], -desired_features[2], centroid_point,
559 desired_normal);
560}
561
562void vpMbtFaceDepthNormal::computeDesiredFeaturesSVD(const std::vector<double> &point_cloud_face,
563 const vpHomogeneousMatrix &cMo, vpColVector &desired_features,
564 vpColVector &desired_normal, vpColVector &centroid_point)
565{
566 vpColVector plane_equation_SVD;
567 estimatePlaneEquationSVD(point_cloud_face, cMo, plane_equation_SVD, centroid_point);
568
569 desired_features.resize(3, false);
570 desired_features[0] = -plane_equation_SVD[0] / plane_equation_SVD[3];
571 desired_features[1] = -plane_equation_SVD[1] / plane_equation_SVD[3];
572 desired_features[2] = -plane_equation_SVD[2] / plane_equation_SVD[3];
573
574 computeNormalVisibility(-desired_features[0], -desired_features[1], -desired_features[2], centroid_point,
575 desired_normal);
576}
577
579 const vpColVector &desired_normal,
580 const vpColVector &centroid_point)
581{
582 // Compute desired centroid in the object frame
583 vpColVector centroid_cam(4);
584 centroid_cam[0] = centroid_point[0];
585 centroid_cam[1] = centroid_point[1];
586 centroid_cam[2] = centroid_point[2];
587 centroid_cam[3] = 1;
588
589 vpColVector centroid_obj = cMo.inverse() * centroid_cam;
590 m_faceDesiredCentroid.setWorldCoordinates(centroid_obj[0], centroid_obj[1], centroid_obj[2]);
591
592 // Compute desired face normal in the object frame
593 vpColVector face_normal_cam(4);
594 face_normal_cam[0] = desired_normal[0];
595 face_normal_cam[1] = desired_normal[1];
596 face_normal_cam[2] = desired_normal[2];
597 face_normal_cam[3] = 1;
598
599 vpColVector face_normal_obj = cMo.inverse() * face_normal_cam;
600 m_faceDesiredNormal.setWorldCoordinates(face_normal_obj[0], face_normal_obj[1], face_normal_obj[2]);
601}
602
603bool vpMbtFaceDepthNormal::computePolygonCentroid(const std::vector<vpPoint> &points_, vpPoint &centroid)
604{
605 if (points_.empty()) {
606 return false;
607 }
608
609 if (points_.size() < 2) {
610 centroid = points_[0];
611 return true;
612 }
613
614 std::vector<vpPoint> points = points_;
615 points.push_back(points_.front());
616
617 double A1 = 0.0, A2 = 0.0, c_x1 = 0.0, c_x2 = 0.0, c_y = 0.0, c_z = 0.0;
618
619 for (size_t i = 0; i < points.size() - 1; i++) {
620 // projection onto xy plane
621 c_x1 += (points[i].get_X() + points[i + 1].get_X()) *
622 (points[i].get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y());
623 c_y += (points[i].get_Y() + points[i + 1].get_Y()) *
624 (points[i].get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y());
625 A1 += points[i].get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y();
626
627 // projection onto xz plane
628 c_x2 += (points[i].get_X() + points[i + 1].get_X()) *
629 (points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z());
630 c_z += (points[i].get_Z() + points[i + 1].get_Z()) *
631 (points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z());
632 A2 += points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z();
633 }
634
635 c_x1 /= 3.0 * A1;
636 c_y /= 3.0 * A1;
637 c_x2 /= 3.0 * A2;
638 c_z /= 3.0 * A2;
639
640 if (A1 > A2) {
641 centroid.set_X(c_x1);
642 } else {
643 centroid.set_X(c_x2);
644 }
645
646 centroid.set_Y(c_y);
647 centroid.set_Z(c_z);
648
649 return true;
650}
651
652void vpMbtFaceDepthNormal::computeROI(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height,
653 std::vector<vpImagePoint> &roiPts
654#if DEBUG_DISPLAY_DEPTH_NORMAL
655 ,
656 std::vector<std::vector<vpImagePoint> > &roiPts_vec
657#endif
658)
659{
660 if (m_useScanLine || m_clippingFlag > 2)
661 m_cam.computeFov(width, height);
662
663 if (m_useScanLine) {
664 for (std::vector<PolygonLine>::iterator it = m_polygonLines.begin(); it != m_polygonLines.end(); ++it) {
665 it->m_p1->changeFrame(cMo);
666 it->m_p2->changeFrame(cMo);
667
668 vpImagePoint ip1, ip2;
669
670 it->m_poly.changeFrame(cMo);
671 it->m_poly.computePolygonClipped(m_cam);
672
673 if (it->m_poly.polyClipped.size() == 2 &&
674 ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
675 ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
676 ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
677 ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
678 ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
679 ((it->m_poly.polyClipped[1].second & it->m_poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
680
681 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
682 m_hiddenFace->computeScanLineQuery(it->m_poly.polyClipped[0].first, it->m_poly.polyClipped[1].first, linesLst,
683 true);
684
685 for (unsigned int i = 0; i < linesLst.size(); i++) {
686 linesLst[i].first.project();
687 linesLst[i].second.project();
688
689 vpMeterPixelConversion::convertPoint(m_cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
690 vpMeterPixelConversion::convertPoint(m_cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
691
692 it->m_imPt1 = ip1;
693 it->m_imPt2 = ip2;
694
695 roiPts.push_back(ip1);
696 roiPts.push_back(ip2);
697
698#if DEBUG_DISPLAY_DEPTH_NORMAL
699 std::vector<vpImagePoint> roiPts_;
700 roiPts_.push_back(ip1);
701 roiPts_.push_back(ip2);
702 roiPts_vec.push_back(roiPts_);
703#endif
704 }
705 }
706 }
707 } else {
708 // Get polygon clipped
709 m_polygon->getRoiClipped(m_cam, roiPts, cMo);
710
711#if DEBUG_DISPLAY_DEPTH_NORMAL
712 roiPts_vec.push_back(roiPts);
713#endif
714 }
715}
716
718
720{
721 // Compute lines visibility, only for display
722 vpMbtDistanceLine *line;
723 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
724 ++it) {
725 line = *it;
726 bool isvisible = false;
727
728 for (std::list<int>::const_iterator itindex = line->Lindex_polygon.begin(); itindex != line->Lindex_polygon.end();
729 ++itindex) {
730 int index = *itindex;
731 if (index == -1) {
732 isvisible = true;
733 } else {
734 if (line->hiddenface->isVisible((unsigned int)index)) {
735 isvisible = true;
736 }
737 }
738 }
739
740 // Si la ligne n'appartient a aucune face elle est tout le temps visible
741 if (line->Lindex_polygon.empty())
742 isvisible = true; // Not sure that this can occur
743
744 if (isvisible) {
745 line->setVisible(true);
746 } else {
747 line->setVisible(false);
748 }
749 }
750}
751
752void vpMbtFaceDepthNormal::computeNormalVisibility(double nx, double ny, double nz, const vpHomogeneousMatrix &cMo,
753 const vpCameraParameters &camera, vpColVector &correct_normal,
754 vpPoint &centroid)
755{
756 vpColVector faceNormal(3);
757 faceNormal[0] = nx;
758 faceNormal[1] = ny;
759 faceNormal[2] = nz;
760 faceNormal.normalize();
761
762 // Get polygon clipped
763 std::vector<vpImagePoint> roiPts;
764 m_polygon->getRoiClipped(camera, roiPts, cMo);
765
766 std::vector<vpPoint> polyPts;
768
769 vpColVector e4(3);
771 computePolygonCentroid(polyPts, centroid);
772 centroid.project();
773
774 e4[0] = -centroid.get_X();
775 e4[1] = -centroid.get_Y();
776 e4[2] = -centroid.get_Z();
777 e4.normalize();
778 } else {
779 double centroid_x = 0.0;
780 double centroid_y = 0.0;
781 double centroid_z = 0.0;
782
783 for (size_t i = 0; i < polyPts.size(); i++) {
784 centroid_x += polyPts[i].get_X();
785 centroid_y += polyPts[i].get_Y();
786 centroid_z += polyPts[i].get_Z();
787 }
788
789 centroid_x /= polyPts.size();
790 centroid_y /= polyPts.size();
791 centroid_z /= polyPts.size();
792
793 e4[0] = -centroid_x;
794 e4[1] = -centroid_y;
795 e4[2] = -centroid_z;
796 e4.normalize();
797
798 centroid.set_X(centroid_x);
799 centroid.set_Y(centroid_y);
800 centroid.set_Z(centroid_z);
801 }
802
803 correct_normal.resize(3, false);
804 double angle = acos(vpColVector::dotProd(e4, faceNormal));
805 if (angle < M_PI_2) {
806 correct_normal = faceNormal;
807 } else {
808 correct_normal[0] = -faceNormal[0];
809 correct_normal[1] = -faceNormal[1];
810 correct_normal[2] = -faceNormal[2];
811 }
812}
813
814#ifdef VISP_HAVE_PCL
815void vpMbtFaceDepthNormal::computeNormalVisibility(float nx, float ny, float nz, const pcl::PointXYZ &centroid_point,
816 pcl::PointXYZ &face_normal)
817{
818 vpColVector faceNormal(3);
819 faceNormal[0] = nx;
820 faceNormal[1] = ny;
821 faceNormal[2] = nz;
822 faceNormal.normalize();
823
824 vpColVector e4(3);
825 e4[0] = -centroid_point.x;
826 e4[1] = -centroid_point.y;
827 e4[2] = -centroid_point.z;
828 e4.normalize();
829
830 double angle = acos(vpColVector::dotProd(e4, faceNormal));
831 if (angle < M_PI_2) {
832 face_normal = pcl::PointXYZ(faceNormal[0], faceNormal[1], faceNormal[2]);
833 } else {
834 face_normal = pcl::PointXYZ(-faceNormal[0], -faceNormal[1], -faceNormal[2]);
835 }
836}
837#endif
838
839void vpMbtFaceDepthNormal::computeNormalVisibility(double nx, double ny, double nz, const vpColVector &centroid_point,
840 vpColVector &face_normal)
841{
842 face_normal.resize(3, false);
843 face_normal[0] = nx;
844 face_normal[1] = ny;
845 face_normal[2] = nz;
846 face_normal.normalize();
847
848 vpColVector e4 = -centroid_point;
849 e4.normalize();
850
851 double angle = acos(vpColVector::dotProd(e4, face_normal));
852 if (angle >= M_PI_2) {
853 face_normal[0] = -face_normal[0];
854 face_normal[1] = -face_normal[1];
855 face_normal[2] = -face_normal[2];
856 }
857}
858
860{
861 L.resize(3, 6, false, false);
862
863 // Transform the plane equation for the current pose
866
867 double ux = m_planeCamera.getA();
868 double uy = m_planeCamera.getB();
869 double uz = m_planeCamera.getC();
870 double D = m_planeCamera.getD();
871 double D2 = D * D;
872
873 // Features
874 features.resize(3, false);
875 features[0] = -ux / D;
876 features[1] = -uy / D;
877 features[2] = -uz / D;
878
879 // L_A
880 L[0][0] = ux * ux / D2;
881 L[0][1] = ux * uy / D2;
882 L[0][2] = ux * uz / D2;
883 L[0][3] = 0.0;
884 L[0][4] = uz / D;
885 L[0][5] = -uy / D;
886
887 // L_B
888 L[1][0] = ux * uy / D2;
889 L[1][1] = uy * uy / D2;
890 L[1][2] = uy * uz / D2;
891 L[1][3] = -uz / D;
892 L[1][4] = 0.0;
893 L[1][5] = ux / D;
894
895 // L_C
896 L[2][0] = ux * uz / D2;
897 L[2][1] = uy * uz / D2;
898 L[2][2] = uz * uz / D2;
899 L[2][3] = uy / D;
900 L[2][4] = -ux / D;
901 L[2][5] = 0.0;
902}
903
905 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
906 bool displayFullModel)
907{
908 std::vector<std::vector<double> > models =
909 getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
910
911 for (size_t i = 0; i < models.size(); i++) {
912 vpImagePoint ip1(models[i][1], models[i][2]);
913 vpImagePoint ip2(models[i][3], models[i][4]);
914 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
915 }
916}
917
919 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
920 bool displayFullModel)
921{
922 std::vector<std::vector<double> > models =
923 getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
924
925 for (size_t i = 0; i < models.size(); i++) {
926 vpImagePoint ip1(models[i][1], models[i][2]);
927 vpImagePoint ip2(models[i][3], models[i][4]);
928 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
929 }
930}
931
933 const vpCameraParameters &cam, double scale, unsigned int thickness)
934{
936 // Desired feature
937 vpPoint pt_centroid = m_faceDesiredCentroid;
938 pt_centroid.changeFrame(cMo);
939 pt_centroid.project();
940
941 vpImagePoint im_centroid;
942 vpMeterPixelConversion::convertPoint(cam, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid);
943
944 vpPoint pt_normal = m_faceDesiredNormal;
945 pt_normal.changeFrame(cMo);
946 pt_normal.project();
947
948 vpPoint pt_extremity;
949 pt_extremity.set_X(pt_centroid.get_X() + pt_normal.get_X() * scale);
950 pt_extremity.set_Y(pt_centroid.get_Y() + pt_normal.get_Y() * scale);
951 pt_extremity.set_Z(pt_centroid.get_Z() + pt_normal.get_Z() * scale);
952 pt_extremity.project();
953
954 vpImagePoint im_extremity;
955 vpMeterPixelConversion::convertPoint(cam, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity);
956
957 vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::blue, 4, 2, thickness);
958
959 // Current feature
960 // Transform the plane equation for the current pose
963
964 double ux = m_planeCamera.getA();
965 double uy = m_planeCamera.getB();
966 double uz = m_planeCamera.getC();
967
968 vpColVector correct_normal;
969 vpCameraParameters cam_copy = cam;
970 cam_copy.computeFov(I.getWidth(), I.getHeight());
971 computeNormalVisibility(ux, uy, uz, cMo, cam_copy, correct_normal, pt_centroid);
972
973 pt_centroid.project();
974 vpMeterPixelConversion::convertPoint(cam_copy, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid);
975
976 pt_extremity.set_X(pt_centroid.get_X() + correct_normal[0] * scale);
977 pt_extremity.set_Y(pt_centroid.get_Y() + correct_normal[1] * scale);
978 pt_extremity.set_Z(pt_centroid.get_Z() + correct_normal[2] * scale);
979 pt_extremity.project();
980
981 vpMeterPixelConversion::convertPoint(cam_copy, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity);
982
983 vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::red, 4, 2, thickness);
984 }
985}
986
988 const vpCameraParameters &cam, double scale, unsigned int thickness)
989{
991 // Desired feature
992 vpPoint pt_centroid = m_faceDesiredCentroid;
993 pt_centroid.changeFrame(cMo);
994 pt_centroid.project();
995
996 vpImagePoint im_centroid;
997 vpMeterPixelConversion::convertPoint(cam, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid);
998
999 vpPoint pt_normal = m_faceDesiredNormal;
1000 pt_normal.changeFrame(cMo);
1001 pt_normal.project();
1002
1003 vpPoint pt_extremity;
1004 pt_extremity.set_X(pt_centroid.get_X() + pt_normal.get_X() * scale);
1005 pt_extremity.set_Y(pt_centroid.get_Y() + pt_normal.get_Y() * scale);
1006 pt_extremity.set_Z(pt_centroid.get_Z() + pt_normal.get_Z() * scale);
1007 pt_extremity.project();
1008
1009 vpImagePoint im_extremity;
1010 vpMeterPixelConversion::convertPoint(cam, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity);
1011
1012 vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::blue, 4, 2, thickness);
1013
1014 // Current feature
1015 // Transform the plane equation for the current pose
1018
1019 double ux = m_planeCamera.getA();
1020 double uy = m_planeCamera.getB();
1021 double uz = m_planeCamera.getC();
1022
1023 vpColVector correct_normal;
1024 vpCameraParameters cam_copy = cam;
1025 cam_copy.computeFov(I.getWidth(), I.getHeight());
1026 computeNormalVisibility(ux, uy, uz, cMo, cam_copy, correct_normal, pt_centroid);
1027
1028 pt_centroid.project();
1029 vpMeterPixelConversion::convertPoint(cam_copy, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid);
1030
1031 pt_extremity.set_X(pt_centroid.get_X() + correct_normal[0] * scale);
1032 pt_extremity.set_Y(pt_centroid.get_Y() + correct_normal[1] * scale);
1033 pt_extremity.set_Z(pt_centroid.get_Z() + correct_normal[2] * scale);
1034 pt_extremity.project();
1035
1036 vpMeterPixelConversion::convertPoint(cam_copy, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity);
1037
1038 vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::red, 4, 2, thickness);
1039 }
1040}
1041
1042void vpMbtFaceDepthNormal::estimateFeatures(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
1043 vpColVector &x_estimated, std::vector<double> &w)
1044{
1045 vpMbtTukeyEstimator<double> tukey_robust;
1046 std::vector<double> residues(point_cloud_face.size() / 3);
1047
1048 w.resize(point_cloud_face.size() / 3, 1.0);
1049
1050 unsigned int max_iter = 30, iter = 0;
1051 double error = 0.0, prev_error = -1.0;
1052 double A = 0.0, B = 0.0, C = 0.0;
1053
1054 Mat33<double> ATA_3x3;
1055
1056 bool checkSSE2 = vpCPUFeatures::checkSSE2();
1057#if !USE_SSE
1058 checkSSE2 = false;
1059#endif
1060
1061 if (checkSSE2) {
1062#if USE_SSE
1063 while (std::fabs(error - prev_error) > 1e-6 && (iter < max_iter)) {
1064 if (iter == 0) {
1065 // Transform the plane equation for the current pose
1068
1069 double ux = m_planeCamera.getA();
1070 double uy = m_planeCamera.getB();
1071 double uz = m_planeCamera.getC();
1072 double D = m_planeCamera.getD();
1073
1074 // Features
1075 A = -ux / D;
1076 B = -uy / D;
1077 C = -uz / D;
1078
1079 size_t cpt = 0;
1080 if (point_cloud_face.size() / 3 >= 2) {
1081 const double *ptr_point_cloud = &point_cloud_face[0];
1082 const __m128d vA = _mm_set1_pd(A);
1083 const __m128d vB = _mm_set1_pd(B);
1084 const __m128d vC = _mm_set1_pd(C);
1085 const __m128d vones = _mm_set1_pd(1.0);
1086
1087 double *ptr_residues = &residues[0];
1088
1089 for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_residues += 2) {
1090 const __m128d vxi = _mm_loadu_pd(ptr_point_cloud);
1091 const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2);
1092 const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4);
1093 const __m128d vinvZi = _mm_div_pd(vones, vZi);
1094
1095 const __m128d tmp =
1096 _mm_add_pd(_mm_add_pd(_mm_mul_pd(vA, vxi), _mm_mul_pd(vB, vyi)), _mm_sub_pd(vC, vinvZi));
1097 _mm_storeu_pd(ptr_residues, tmp);
1098 }
1099 }
1100
1101 for (; cpt < point_cloud_face.size(); cpt += 3) {
1102 double xi = point_cloud_face[cpt];
1103 double yi = point_cloud_face[cpt + 1];
1104 double Zi = point_cloud_face[cpt + 2];
1105
1106 residues[cpt / 3] = (A * xi + B * yi + C - 1 / Zi);
1107 }
1108 }
1109
1110 tukey_robust.MEstimator(residues, w, 1e-2);
1111
1112 __m128d vsum_wi2_xi2 = _mm_setzero_pd();
1113 __m128d vsum_wi2_yi2 = _mm_setzero_pd();
1114 __m128d vsum_wi2 = _mm_setzero_pd();
1115 __m128d vsum_wi2_xi_yi = _mm_setzero_pd();
1116 __m128d vsum_wi2_xi = _mm_setzero_pd();
1117 __m128d vsum_wi2_yi = _mm_setzero_pd();
1118
1119 __m128d vsum_wi2_xi_Zi = _mm_setzero_pd();
1120 __m128d vsum_wi2_yi_Zi = _mm_setzero_pd();
1121 __m128d vsum_wi2_Zi = _mm_setzero_pd();
1122
1123 // Estimate A, B, C
1124 size_t cpt = 0;
1125 if (point_cloud_face.size() / 3 >= 2) {
1126 const double *ptr_point_cloud = &point_cloud_face[0];
1127 double *ptr_w = &w[0];
1128
1129 const __m128d vones = _mm_set1_pd(1.0);
1130
1131 for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_w += 2) {
1132 const __m128d vwi2 = _mm_mul_pd(_mm_loadu_pd(ptr_w), _mm_loadu_pd(ptr_w));
1133
1134 const __m128d vxi = _mm_loadu_pd(ptr_point_cloud);
1135 const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2);
1136 const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4);
1137 const __m128d vinvZi = _mm_div_pd(vones, vZi);
1138
1139 vsum_wi2_xi2 = _mm_add_pd(vsum_wi2_xi2, _mm_mul_pd(vwi2, _mm_mul_pd(vxi, vxi)));
1140 vsum_wi2_yi2 = _mm_add_pd(vsum_wi2_yi2, _mm_mul_pd(vwi2, _mm_mul_pd(vyi, vyi)));
1141 vsum_wi2 = _mm_add_pd(vsum_wi2, vwi2);
1142 vsum_wi2_xi_yi = _mm_add_pd(vsum_wi2_xi_yi, _mm_mul_pd(vwi2, _mm_mul_pd(vxi, vyi)));
1143 vsum_wi2_xi = _mm_add_pd(vsum_wi2_xi, _mm_mul_pd(vwi2, vxi));
1144 vsum_wi2_yi = _mm_add_pd(vsum_wi2_yi, _mm_mul_pd(vwi2, vyi));
1145
1146 const __m128d vwi2_invZi = _mm_mul_pd(vwi2, vinvZi);
1147 vsum_wi2_xi_Zi = _mm_add_pd(vsum_wi2_xi_Zi, _mm_mul_pd(vxi, vwi2_invZi));
1148 vsum_wi2_yi_Zi = _mm_add_pd(vsum_wi2_yi_Zi, _mm_mul_pd(vyi, vwi2_invZi));
1149 vsum_wi2_Zi = _mm_add_pd(vsum_wi2_Zi, vwi2_invZi);
1150 }
1151 }
1152
1153 double vtmp[2];
1154 _mm_storeu_pd(vtmp, vsum_wi2_xi2);
1155 double sum_wi2_xi2 = vtmp[0] + vtmp[1];
1156
1157 _mm_storeu_pd(vtmp, vsum_wi2_yi2);
1158 double sum_wi2_yi2 = vtmp[0] + vtmp[1];
1159
1160 _mm_storeu_pd(vtmp, vsum_wi2);
1161 double sum_wi2 = vtmp[0] + vtmp[1];
1162
1163 _mm_storeu_pd(vtmp, vsum_wi2_xi_yi);
1164 double sum_wi2_xi_yi = vtmp[0] + vtmp[1];
1165
1166 _mm_storeu_pd(vtmp, vsum_wi2_xi);
1167 double sum_wi2_xi = vtmp[0] + vtmp[1];
1168
1169 _mm_storeu_pd(vtmp, vsum_wi2_yi);
1170 double sum_wi2_yi = vtmp[0] + vtmp[1];
1171
1172 _mm_storeu_pd(vtmp, vsum_wi2_xi_Zi);
1173 double sum_wi2_xi_Zi = vtmp[0] + vtmp[1];
1174
1175 _mm_storeu_pd(vtmp, vsum_wi2_yi_Zi);
1176 double sum_wi2_yi_Zi = vtmp[0] + vtmp[1];
1177
1178 _mm_storeu_pd(vtmp, vsum_wi2_Zi);
1179 double sum_wi2_Zi = vtmp[0] + vtmp[1];
1180
1181 for (; cpt < point_cloud_face.size(); cpt += 3) {
1182 double wi2 = w[cpt / 3] * w[cpt / 3];
1183
1184 double xi = point_cloud_face[cpt];
1185 double yi = point_cloud_face[cpt + 1];
1186 double Zi = point_cloud_face[cpt + 2];
1187 double invZi = 1.0 / Zi;
1188
1189 sum_wi2_xi2 += wi2 * xi * xi;
1190 sum_wi2_yi2 += wi2 * yi * yi;
1191 sum_wi2 += wi2;
1192 sum_wi2_xi_yi += wi2 * xi * yi;
1193 sum_wi2_xi += wi2 * xi;
1194 sum_wi2_yi += wi2 * yi;
1195
1196 sum_wi2_xi_Zi += wi2 * xi * invZi;
1197 sum_wi2_yi_Zi += wi2 * yi * invZi;
1198 sum_wi2_Zi += wi2 * invZi;
1199 }
1200
1201 ATA_3x3[0] = sum_wi2_xi2;
1202 ATA_3x3[1] = sum_wi2_xi_yi;
1203 ATA_3x3[2] = sum_wi2_xi;
1204 ATA_3x3[3] = sum_wi2_xi_yi;
1205 ATA_3x3[4] = sum_wi2_yi2;
1206 ATA_3x3[5] = sum_wi2_yi;
1207 ATA_3x3[6] = sum_wi2_xi;
1208 ATA_3x3[7] = sum_wi2_yi;
1209 ATA_3x3[8] = sum_wi2;
1210
1211 Mat33<double> minv = ATA_3x3.inverse();
1212
1213 A = minv[0] * sum_wi2_xi_Zi + minv[1] * sum_wi2_yi_Zi + minv[2] * sum_wi2_Zi;
1214 B = minv[3] * sum_wi2_xi_Zi + minv[4] * sum_wi2_yi_Zi + minv[5] * sum_wi2_Zi;
1215 C = minv[6] * sum_wi2_xi_Zi + minv[7] * sum_wi2_yi_Zi + minv[8] * sum_wi2_Zi;
1216
1217 cpt = 0;
1218
1219 // Compute error
1220 prev_error = error;
1221 error = 0.0;
1222
1223 __m128d verror = _mm_set1_pd(0.0);
1224 if (point_cloud_face.size() / 3 >= 2) {
1225 const double *ptr_point_cloud = &point_cloud_face[0];
1226 const __m128d vA = _mm_set1_pd(A);
1227 const __m128d vB = _mm_set1_pd(B);
1228 const __m128d vC = _mm_set1_pd(C);
1229 const __m128d vones = _mm_set1_pd(1.0);
1230
1231 double *ptr_residues = &residues[0];
1232
1233 for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_residues += 2) {
1234 const __m128d vxi = _mm_loadu_pd(ptr_point_cloud);
1235 const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2);
1236 const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4);
1237 const __m128d vinvZi = _mm_div_pd(vones, vZi);
1238
1239 const __m128d tmp = _mm_add_pd(_mm_add_pd(_mm_mul_pd(vA, vxi), _mm_mul_pd(vB, vyi)), _mm_sub_pd(vC, vinvZi));
1240 verror = _mm_add_pd(verror, _mm_mul_pd(tmp, tmp));
1241
1242 _mm_storeu_pd(ptr_residues, tmp);
1243 }
1244 }
1245
1246 _mm_storeu_pd(vtmp, verror);
1247 error = vtmp[0] + vtmp[1];
1248
1249 for (size_t idx = cpt; idx < point_cloud_face.size(); idx += 3) {
1250 double xi = point_cloud_face[idx];
1251 double yi = point_cloud_face[idx + 1];
1252 double Zi = point_cloud_face[idx + 2];
1253
1254 error += vpMath::sqr(A * xi + B * yi + C - 1 / Zi);
1255 residues[idx / 3] = (A * xi + B * yi + C - 1 / Zi);
1256 }
1257
1258 error /= point_cloud_face.size() / 3;
1259
1260 iter++;
1261 } // while ( std::fabs(error - prev_error) > 1e-6 && (iter < max_iter) )
1262#endif
1263 } else {
1264 while (std::fabs(error - prev_error) > 1e-6 && (iter < max_iter)) {
1265 if (iter == 0) {
1266 // Transform the plane equation for the current pose
1269
1270 double ux = m_planeCamera.getA();
1271 double uy = m_planeCamera.getB();
1272 double uz = m_planeCamera.getC();
1273 double D = m_planeCamera.getD();
1274
1275 // Features
1276 A = -ux / D;
1277 B = -uy / D;
1278 C = -uz / D;
1279
1280 for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1281 double xi = point_cloud_face[3 * i];
1282 double yi = point_cloud_face[3 * i + 1];
1283 double Zi = point_cloud_face[3 * i + 2];
1284
1285 residues[i] = (A * xi + B * yi + C - 1 / Zi);
1286 }
1287 }
1288
1289 tukey_robust.MEstimator(residues, w, 1e-2);
1290
1291 // Estimate A, B, C
1292 double sum_wi2_xi2 = 0.0, sum_wi2_yi2 = 0.0, sum_wi2 = 0.0;
1293 double sum_wi2_xi_yi = 0.0, sum_wi2_xi = 0.0, sum_wi2_yi = 0.0;
1294
1295 double sum_wi2_xi_Zi = 0.0, sum_wi2_yi_Zi = 0.0, sum_wi2_Zi = 0.0;
1296
1297 for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1298 double wi2 = w[i] * w[i];
1299
1300 double xi = point_cloud_face[3 * i];
1301 double yi = point_cloud_face[3 * i + 1];
1302 double Zi = point_cloud_face[3 * i + 2];
1303 double invZi = 1 / Zi;
1304
1305 sum_wi2_xi2 += wi2 * xi * xi;
1306 sum_wi2_yi2 += wi2 * yi * yi;
1307 sum_wi2 += wi2;
1308 sum_wi2_xi_yi += wi2 * xi * yi;
1309 sum_wi2_xi += wi2 * xi;
1310 sum_wi2_yi += wi2 * yi;
1311
1312 sum_wi2_xi_Zi += wi2 * xi * invZi;
1313 sum_wi2_yi_Zi += wi2 * yi * invZi;
1314 sum_wi2_Zi += wi2 * invZi;
1315 }
1316
1317 ATA_3x3[0] = sum_wi2_xi2;
1318 ATA_3x3[1] = sum_wi2_xi_yi;
1319 ATA_3x3[2] = sum_wi2_xi;
1320 ATA_3x3[3] = sum_wi2_xi_yi;
1321 ATA_3x3[4] = sum_wi2_yi2;
1322 ATA_3x3[5] = sum_wi2_yi;
1323 ATA_3x3[6] = sum_wi2_xi;
1324 ATA_3x3[7] = sum_wi2_yi;
1325 ATA_3x3[8] = sum_wi2;
1326
1327 Mat33<double> minv = ATA_3x3.inverse();
1328
1329 A = minv[0] * sum_wi2_xi_Zi + minv[1] * sum_wi2_yi_Zi + minv[2] * sum_wi2_Zi;
1330 B = minv[3] * sum_wi2_xi_Zi + minv[4] * sum_wi2_yi_Zi + minv[5] * sum_wi2_Zi;
1331 C = minv[6] * sum_wi2_xi_Zi + minv[7] * sum_wi2_yi_Zi + minv[8] * sum_wi2_Zi;
1332
1333 prev_error = error;
1334 error = 0.0;
1335
1336 // Compute error
1337 for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1338 double xi = point_cloud_face[3 * i];
1339 double yi = point_cloud_face[3 * i + 1];
1340 double Zi = point_cloud_face[3 * i + 2];
1341
1342 error += vpMath::sqr(A * xi + B * yi + C - 1 / Zi);
1343 residues[i] = (A * xi + B * yi + C - 1 / Zi);
1344 }
1345
1346 error /= point_cloud_face.size() / 3;
1347
1348 iter++;
1349 } // while ( std::fabs(error - prev_error) > 1e-6 && (iter < max_iter) )
1350 }
1351
1352 x_estimated.resize(3, false);
1353 x_estimated[0] = A;
1354 x_estimated[1] = B;
1355 x_estimated[2] = C;
1356}
1357
1358void vpMbtFaceDepthNormal::estimatePlaneEquationSVD(const std::vector<double> &point_cloud_face,
1359 const vpHomogeneousMatrix &cMo,
1360 vpColVector &plane_equation_estimated, vpColVector &centroid)
1361{
1362 unsigned int max_iter = 10;
1363 double prev_error = 1e3;
1364 double error = 1e3 - 1;
1365
1366 std::vector<double> weights(point_cloud_face.size() / 3, 1.0);
1367 std::vector<double> residues(point_cloud_face.size() / 3);
1368 vpMatrix M((unsigned int)(point_cloud_face.size() / 3), 3);
1369 vpMbtTukeyEstimator<double> tukey;
1370 vpColVector normal;
1371
1372 for (unsigned int iter = 0; iter < max_iter && std::fabs(error - prev_error) > 1e-6; iter++) {
1373 if (iter != 0) {
1374 tukey.MEstimator(residues, weights, 1e-4);
1375 } else {
1376 // Transform the plane equation for the current pose
1379
1380 double A = m_planeCamera.getA();
1381 double B = m_planeCamera.getB();
1382 double C = m_planeCamera.getC();
1383 double D = m_planeCamera.getD();
1384
1385 // Compute distance point to estimated plane
1386 for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1387 residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] +
1388 C * point_cloud_face[3 * i + 2] + D) /
1389 sqrt(A * A + B * B + C * C);
1390 }
1391
1392 tukey.MEstimator(residues, weights, 1e-4);
1393 plane_equation_estimated.resize(4, false);
1394 }
1395
1396 // Compute centroid
1397 double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0;
1398 double total_w = 0.0;
1399
1400 for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1401 centroid_x += weights[i] * point_cloud_face[3 * i];
1402 centroid_y += weights[i] * point_cloud_face[3 * i + 1];
1403 centroid_z += weights[i] * point_cloud_face[3 * i + 2];
1404 total_w += weights[i];
1405 }
1406
1407 centroid_x /= total_w;
1408 centroid_y /= total_w;
1409 centroid_z /= total_w;
1410
1411 // Minimization
1412 for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1413 M[(unsigned int)i][0] = weights[i] * (point_cloud_face[3 * i] - centroid_x);
1414 M[(unsigned int)i][1] = weights[i] * (point_cloud_face[3 * i + 1] - centroid_y);
1415 M[(unsigned int)i][2] = weights[i] * (point_cloud_face[3 * i + 2] - centroid_z);
1416 }
1417
1418 vpMatrix J = M.t() * M;
1419
1420 vpColVector W;
1421 vpMatrix V;
1422 J.svd(W, V);
1423
1424 double smallestSv = W[0];
1425 unsigned int indexSmallestSv = 0;
1426 for (unsigned int i = 1; i < W.size(); i++) {
1427 if (W[i] < smallestSv) {
1428 smallestSv = W[i];
1429 indexSmallestSv = i;
1430 }
1431 }
1432
1433 normal = V.getCol(indexSmallestSv);
1434
1435 // Compute plane equation
1436 double A = normal[0], B = normal[1], C = normal[2];
1437 double D = -(A * centroid_x + B * centroid_y + C * centroid_z);
1438
1439 // Update plane equation
1440 plane_equation_estimated[0] = A;
1441 plane_equation_estimated[1] = B;
1442 plane_equation_estimated[2] = C;
1443 plane_equation_estimated[3] = D;
1444
1445 // Compute error points to estimated plane
1446 prev_error = error;
1447 error = 0.0;
1448 for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1449 residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] +
1450 C * point_cloud_face[3 * i + 2] + D) /
1451 sqrt(A * A + B * B + C * C);
1452 error += weights[i] * residues[i];
1453 }
1454 error /= total_w;
1455 }
1456
1457 // Update final weights
1458 tukey.MEstimator(residues, weights, 1e-4);
1459
1460 // Update final centroid
1461 centroid.resize(3, false);
1462 double total_w = 0.0;
1463
1464 for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1465 centroid[0] += weights[i] * point_cloud_face[3 * i];
1466 centroid[1] += weights[i] * point_cloud_face[3 * i + 1];
1467 centroid[2] += weights[i] * point_cloud_face[3 * i + 2];
1468 total_w += weights[i];
1469 }
1470
1471 centroid[0] /= total_w;
1472 centroid[1] /= total_w;
1473 centroid[2] /= total_w;
1474
1475 // Compute final plane equation
1476 double A = normal[0], B = normal[1], C = normal[2];
1477 double D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
1478
1479 // Update final plane equation
1480 plane_equation_estimated[0] = A;
1481 plane_equation_estimated[1] = B;
1482 plane_equation_estimated[2] = C;
1483 plane_equation_estimated[3] = D;
1484}
1485
1491std::vector<std::vector<double> >
1493{
1494 std::vector<std::vector<double> > features;
1495
1497 // Desired feature
1498 vpPoint pt_centroid = m_faceDesiredCentroid;
1499 pt_centroid.changeFrame(cMo);
1500 pt_centroid.project();
1501
1502 vpImagePoint im_centroid;
1503 vpMeterPixelConversion::convertPoint(cam, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid);
1504
1505 vpPoint pt_normal = m_faceDesiredNormal;
1506 pt_normal.changeFrame(cMo);
1507 pt_normal.project();
1508
1509 vpPoint pt_extremity;
1510 pt_extremity.set_X(pt_centroid.get_X() + pt_normal.get_X() * scale);
1511 pt_extremity.set_Y(pt_centroid.get_Y() + pt_normal.get_Y() * scale);
1512 pt_extremity.set_Z(pt_centroid.get_Z() + pt_normal.get_Z() * scale);
1513 pt_extremity.project();
1514
1515 vpImagePoint im_extremity;
1516 vpMeterPixelConversion::convertPoint(cam, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity);
1517
1518 {
1519#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
1520 std::vector<double> params = {2, // desired normal
1521 im_centroid.get_i(), im_centroid.get_j(), im_extremity.get_i(),
1522 im_extremity.get_j()};
1523#else
1524 std::vector<double> params;
1525 params.push_back(2); // desired normal
1526 params.push_back(im_centroid.get_i());
1527 params.push_back(im_centroid.get_j());
1528 params.push_back(im_extremity.get_i());
1529 params.push_back(im_extremity.get_j());
1530#endif
1531 features.push_back(params);
1532 }
1533
1534 // Current feature
1535 // Transform the plane equation for the current pose
1538
1539 double ux = m_planeCamera.getA();
1540 double uy = m_planeCamera.getB();
1541 double uz = m_planeCamera.getC();
1542
1543 vpColVector correct_normal;
1544 computeNormalVisibility(ux, uy, uz, cMo, cam, correct_normal, pt_centroid);
1545
1546 pt_centroid.project();
1547 vpMeterPixelConversion::convertPoint(cam, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid);
1548
1549 pt_extremity.set_X(pt_centroid.get_X() + correct_normal[0] * scale);
1550 pt_extremity.set_Y(pt_centroid.get_Y() + correct_normal[1] * scale);
1551 pt_extremity.set_Z(pt_centroid.get_Z() + correct_normal[2] * scale);
1552 pt_extremity.project();
1553
1554 vpMeterPixelConversion::convertPoint(cam, pt_extremity.get_x(), pt_extremity.get_y(), im_extremity);
1555
1556 {
1557#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
1558 std::vector<double> params = {3, // normal at current pose
1559 im_centroid.get_i(), im_centroid.get_j(), im_extremity.get_i(),
1560 im_extremity.get_j()};
1561#else
1562 std::vector<double> params;
1563 params.push_back(3); // normal at current pose
1564 params.push_back(im_centroid.get_i());
1565 params.push_back(im_centroid.get_j());
1566 params.push_back(im_extremity.get_i());
1567 params.push_back(im_extremity.get_j());
1568#endif
1569 features.push_back(params);
1570 }
1571 }
1572
1573 return features;
1574}
1575
1587std::vector<std::vector<double> > vpMbtFaceDepthNormal::getModelForDisplay(unsigned int width, unsigned int height,
1588 const vpHomogeneousMatrix &cMo,
1589 const vpCameraParameters &cam,
1590 bool displayFullModel)
1591{
1592 std::vector<std::vector<double> > models;
1593
1594 if ((m_polygon->isVisible() && m_isTrackedDepthNormalFace) || displayFullModel) {
1596
1597 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
1598 ++it) {
1599 vpMbtDistanceLine *line = *it;
1600 std::vector<std::vector<double> > lineModels =
1601 line->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1602 models.insert(models.end(), lineModels.begin(), lineModels.end());
1603 }
1604 }
1605
1606 return models;
1607}
1608
1618bool vpMbtFaceDepthNormal::samePoint(const vpPoint &P1, const vpPoint &P2) const
1619{
1620 double dx = fabs(P1.get_oX() - P2.get_oX());
1621 double dy = fabs(P1.get_oY() - P2.get_oY());
1622 double dz = fabs(P1.get_oZ() - P2.get_oZ());
1623
1624 if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
1625 dz <= std::numeric_limits<double>::epsilon())
1626 return true;
1627 else
1628 return false;
1629}
1630
1632{
1633 m_cam = camera;
1634
1635 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
1636 ++it) {
1637 (*it)->setCameraParameters(camera);
1638 }
1639}
1640
1642{
1643 m_useScanLine = v;
1644
1645 for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
1646 ++it) {
1647 (*it)->useScanLine = v;
1648 }
1649}
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
Implementation of column vector and the associated operations.
static double dotProd(const vpColVector &a, const vpColVector &b)
vpColVector & normalize()
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
static const vpColor red
Definition vpColor.h:211
static const vpColor blue
Definition vpColor.h:217
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayArrow(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
double get_j() const
double get_i() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
static double sqr(double x)
Definition vpMath.h:124
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void svd(vpColVector &w, vpMatrix &V)
vpMatrix t() const
Definition vpMatrix.cpp:461
vpColVector getCol(unsigned int j) const
Implementation of the polygons management for the model-based trackers.
bool isVisible(unsigned int i)
void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector< std::pair< vpPoint, vpPoint > > &lines, const bool &displayResults=false)
vpMbScanLine & getMbScanLineRenderer()
Manage the line of a polygon used in the model-based tracker.
void setIndex(unsigned int i)
vpPoint * p2
The second extremity.
std::list< int > Lindex_polygon
Index of the faces which contain the line.
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
vpMbtPolygon & getPolygon()
bool useScanLine
Use scanline rendering.
vpPoint * p1
The first extremity.
void setCameraParameters(const vpCameraParameters &camera)
void setName(const std::string &line_name)
void setVisible(bool _isvisible)
void addPolygon(const int &index)
double m_pclPlaneEstimationRansacThreshold
PCL plane estimation RANSAC threshold.
int m_pclPlaneEstimationMethod
PCL plane estimation method.
double m_distNearClip
Distance for near clipping.
void computeInteractionMatrix(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &features)
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, vpUniRand &rand_gen, int polygon=-1, std::string name="")
void setCameraParameters(const vpCameraParameters &camera)
void computeDesiredFeaturesSVD(const std::vector< double > &point_cloud_face, const vpHomogeneousMatrix &cMo, vpColVector &desired_features, vpColVector &desired_normal, vpColVector &centroid_point)
double m_distFarClip
Distance for near clipping.
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, vpColVector &desired_features, unsigned int stepX, unsigned int stepY, const vpImage< bool > *mask=NULL)
void displayFeature(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double scale=0.05, unsigned int thickness=1)
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
std::vector< std::vector< double > > getFeaturesForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double scale=0.05)
bool computePolygonCentroid(const std::vector< vpPoint > &points, vpPoint &centroid)
void computeROI(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height, std::vector< vpImagePoint > &roiPts)
void estimatePlaneEquationSVD(const std::vector< double > &point_cloud_face, const vpHomogeneousMatrix &cMo, vpColVector &plane_equation_estimated, vpColVector &centroid)
std::vector< vpMbtDistanceLine * > m_listOfFaceLines
void computeDesiredNormalAndCentroid(const vpHomogeneousMatrix &cMo, const vpColVector &desired_normal, const vpColVector &centroid_point)
vpFeatureEstimationType m_featureEstimationMethod
Method to estimate the desired features.
vpMbtPolygon * m_polygon
Polygon defining the face.
vpPoint m_faceDesiredCentroid
Desired centroid (computed from the sensor)
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
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 computeDesiredFeaturesRobustFeatures(const std::vector< double > &point_cloud_face_custom, const std::vector< double > &point_cloud_face, const vpHomogeneousMatrix &cMo, vpColVector &desired_features, vpColVector &desired_normal, vpColVector &centroid_point)
vpPoint m_faceDesiredNormal
Face (normalized) normal (computed from the sensor)
void estimateFeatures(const std::vector< double > &point_cloud_face, const vpHomogeneousMatrix &cMo, vpColVector &x_estimated, std::vector< double > &weights)
std::vector< PolygonLine > m_polygonLines
bool computeDesiredFeaturesPCL(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud_face, vpColVector &desired_features, vpColVector &desired_normal, vpColVector &centroid_point)
void computeNormalVisibility(double nx, double ny, double nz, const vpColVector &centroid_point, vpColVector &face_normal)
bool m_useScanLine
Scan line visibility.
virtual bool isVisible(const vpHomogeneousMatrix &cMo, double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), unsigned int width=0, unsigned int height=0)
int getIndex() const
static bool inMask(const vpImage< bool > *mask, unsigned int i, unsigned int j)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition vpPlane.cpp:361
double getD() const
Definition vpPlane.h:106
double getA() const
Definition vpPlane.h:100
double getC() const
Definition vpPlane.h:104
double getB() const
Definition vpPlane.h:102
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition vpPoint.cpp:458
double get_y() const
Get the point y coordinate in the image plane.
Definition vpPoint.cpp:469
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition vpPoint.cpp:451
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition vpPoint.cpp:462
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition vpPoint.cpp:490
double get_x() const
Get the point x coordinate in the image plane.
Definition vpPoint.cpp:467
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition vpPoint.cpp:492
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition vpPoint.cpp:453
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition vpPoint.cpp:494
double get_oY() const
Get the point oY coordinate in the object frame.
Definition vpPoint.cpp:460
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition vpPoint.cpp:236
double get_X() const
Get the point cX coordinate in the camera frame.
Definition vpPoint.cpp:449
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
Implements a 3D polygon with render functionalities like clipping.
Definition vpPolygon3D.h:55
void setFarClippingDistance(const double &dist)
void setNearClippingDistance(const double &dist)
vpPoint * p
corners in the object frame
Definition vpPolygon3D.h:76
virtual void setNbPoint(unsigned int nb)
void setClipping(const unsigned int &flags)
void getRoiClipped(const vpCameraParameters &cam, std::vector< vpImagePoint > &roi)
void addPoint(unsigned int n, const vpPoint &P)
void getPolygonClipped(std::vector< std::pair< vpPoint, unsigned int > > &poly)
Defines a generic 2D polygon.
Definition vpPolygon.h:97
vpRect getBoundingBox() const
Definition vpPolygon.h:171
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Defines a rectangle in the plane.
Definition vpRect.h:76
double getWidth() const
Definition vpRect.h:224
void setTop(double pos)
Definition vpRect.h:354
double getLeft() const
Definition vpRect.h:170
void setLeft(double pos)
Definition vpRect.h:318
void setRight(double pos)
Definition vpRect.h:345
double getRight() const
Definition vpRect.h:176
double getBottom() const
Definition vpRect.h:94
double getHeight() const
Definition vpRect.h:163
void setBottom(double pos)
Definition vpRect.h:285
double getTop() const
Definition vpRect.h:189
Class for generating random numbers with uniform probability density.
Definition vpUniRand.h:122
VISP_EXPORT bool checkSSE2()