36 #include <visp3/core/vpCPUFeatures.h>
37 #include <visp3/mbt/vpMbtFaceDepthNormal.h>
38 #include <visp3/mbt/vpMbtTukeyEstimator.h>
40 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
41 #include <pcl/common/centroid.h>
42 #include <pcl/filters/extract_indices.h>
43 #include <pcl/segmentation/sac_segmentation.h>
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
51 #define USE_SSE_CODE 1
52 #if VISP_HAVE_SSE2 && USE_SSE_CODE
59 : m_cam(), m_clippingFlag(
vpPolygon3D::NO_CLIPPING), m_distFarClip(100), m_distNearClip(0.001), m_hiddenFace(nullptr),
60 m_planeObject(), m_polygon(nullptr), 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),
65 m_pclPlaneEstimationRansacMaxIter(200), m_pclPlaneEstimationRansacThreshold(0.001), m_polygonLines()
90 vpUniRand &rand_gen,
int polygon, std::string name)
93 PolygonLine polygon_line;
96 polygon_line.m_poly.setNbPoint(2);
97 polygon_line.m_poly.addPoint(0, P1);
98 polygon_line.m_poly.addPoint(1, P2);
104 polygon_line.m_p1 = &polygon_line.m_poly.p[0];
105 polygon_line.m_p2 = &polygon_line.m_poly.p[1];
110 bool already_here =
false;
149 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
152 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
153 vpColVector &desired_features,
unsigned int stepX,
unsigned int stepY
154 #
if DEBUG_DISPLAY_DEPTH_NORMAL
157 std::vector<std::vector<vpImagePoint> > &roiPts_vec
164 if (width == 0 || height == 0)
167 std::vector<vpImagePoint> roiPts;
171 #
if DEBUG_DISPLAY_DEPTH_NORMAL
177 if (roiPts.size() <= 2) {
179 std::cerr <<
"Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
185 vpRect bb = polygon_2d.getBoundingBox();
187 unsigned int top = (
unsigned int)std::max<double>(0.0, bb.
getTop());
188 unsigned int bottom = (
unsigned int)std::min<double>((
double)height, std::max<double>(0.0, bb.
getBottom()));
189 unsigned int left = (
unsigned int)std::max<double>(0.0, bb.
getLeft());
190 unsigned int right = (
unsigned int)std::min<double>((
double)width, std::max<double>(0.0, bb.
getRight()));
198 pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face(
new pcl::PointCloud<pcl::PointXYZ>);
199 std::vector<double> point_cloud_face_vec, point_cloud_face_custom;
217 double prev_x, prev_y, prev_z;
220 double x = 0.0, y = 0.0;
221 for (
unsigned int i = top; i < bottom; i += stepY) {
222 for (
unsigned int j = left; j < right; j += stepX) {
223 if (
vpMeTracker::inRoiMask(mask, i, j) && pcl::isFinite((*point_cloud)(j, i)) && (*point_cloud)(j, i).z > 0 &&
224 (
m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
225 j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
230 point_cloud_face->push_back((*point_cloud)(j, i));
234 point_cloud_face_vec.push_back((*point_cloud)(j, i).x);
235 point_cloud_face_vec.push_back((*point_cloud)(j, i).y);
236 point_cloud_face_vec.push_back((*point_cloud)(j, i).z);
248 prev_z = (*point_cloud)(j, i).z;
252 point_cloud_face_custom.push_back(prev_x);
253 point_cloud_face_custom.push_back(x);
255 point_cloud_face_custom.push_back(prev_y);
256 point_cloud_face_custom.push_back(y);
258 point_cloud_face_custom.push_back(prev_z);
259 point_cloud_face_custom.push_back((*point_cloud)(j, i).z);
264 point_cloud_face_custom.push_back(x);
265 point_cloud_face_custom.push_back(y);
266 point_cloud_face_custom.push_back((*point_cloud)(j, i).z);
271 #if DEBUG_DISPLAY_DEPTH_NORMAL
272 debugImage[i][j] = 255;
279 if (checkSSE2 && push) {
280 point_cloud_face_custom.push_back(prev_x);
281 point_cloud_face_custom.push_back(prev_y);
282 point_cloud_face_custom.push_back(prev_z);
286 if (point_cloud_face->empty() && point_cloud_face_custom.empty() && point_cloud_face_vec.empty()) {
294 if (!computeDesiredFeaturesPCL(point_cloud_face, desired_features, desired_normal, centroid_point)) {
303 desired_normal, centroid_point);
318 unsigned int height,
const std::vector<vpColVector> &point_cloud,
319 vpColVector &desired_features,
unsigned int stepX,
unsigned int stepY
320 #
if DEBUG_DISPLAY_DEPTH_NORMAL
323 std::vector<std::vector<vpImagePoint> > &roiPts_vec
330 if (width == 0 || height == 0)
333 std::vector<vpImagePoint> roiPts;
337 #
if DEBUG_DISPLAY_DEPTH_NORMAL
343 if (roiPts.size() <= 2) {
345 std::cerr <<
"Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
353 unsigned int top = (
unsigned int)std::max<double>(0.0, bb.
getTop());
354 unsigned int bottom = (
unsigned int)std::min<double>((
double)height, std::max<double>(0.0, bb.
getBottom()));
355 unsigned int left = (
unsigned int)std::max<double>(0.0, bb.
getLeft());
356 unsigned int right = (
unsigned int)std::min<double>((
double)width, std::max<double>(0.0, bb.
getRight()));
364 std::vector<double> point_cloud_face, point_cloud_face_custom;
376 double prev_x, prev_y, prev_z;
379 double x = 0.0, y = 0.0;
380 for (
unsigned int i = top; i < bottom; i += stepY) {
381 for (
unsigned int j = left; j < right; j += stepX) {
383 (
m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
384 j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
388 point_cloud_face.push_back(point_cloud[i * width + j][0]);
389 point_cloud_face.push_back(point_cloud[i * width + j][1]);
390 point_cloud_face.push_back(point_cloud[i * width + j][2]);
402 prev_z = point_cloud[i * width + j][2];
406 point_cloud_face_custom.push_back(prev_x);
407 point_cloud_face_custom.push_back(x);
409 point_cloud_face_custom.push_back(prev_y);
410 point_cloud_face_custom.push_back(y);
412 point_cloud_face_custom.push_back(prev_z);
413 point_cloud_face_custom.push_back(point_cloud[i * width + j][2]);
418 point_cloud_face_custom.push_back(x);
419 point_cloud_face_custom.push_back(y);
420 point_cloud_face_custom.push_back(point_cloud[i * width + j][2]);
424 #if DEBUG_DISPLAY_DEPTH_NORMAL
425 debugImage[i][j] = 255;
432 if (checkSSE2 && push) {
433 point_cloud_face_custom.push_back(prev_x);
434 point_cloud_face_custom.push_back(prev_y);
435 point_cloud_face_custom.push_back(prev_z);
439 if (point_cloud_face.empty() && point_cloud_face_custom.empty()) {
446 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
448 pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face_pcl(
new pcl::PointCloud<pcl::PointXYZ>);
449 point_cloud_face_pcl->reserve(point_cloud_face.size() / 3);
451 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
452 point_cloud_face_pcl->push_back(
453 pcl::PointXYZ(point_cloud_face[3 * i], point_cloud_face[3 * i + 1], point_cloud_face[3 * i + 2]));
456 computeDesiredFeaturesPCL(point_cloud_face_pcl, desired_features, desired_normal, centroid_point);
465 desired_normal, centroid_point);
479 unsigned int height,
const vpMatrix &point_cloud,
480 vpColVector &desired_features,
unsigned int stepX,
unsigned int stepY
481 #
if DEBUG_DISPLAY_DEPTH_NORMAL
484 std::vector<std::vector<vpImagePoint> > &roiPts_vec
491 if (width == 0 || height == 0)
494 std::vector<vpImagePoint> roiPts;
498 #
if DEBUG_DISPLAY_DEPTH_NORMAL
504 if (roiPts.size() <= 2) {
506 std::cerr <<
"Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
514 unsigned int top = (
unsigned int)std::max<double>(0.0, bb.
getTop());
515 unsigned int bottom = (
unsigned int)std::min<double>((
double)height, std::max<double>(0.0, bb.
getBottom()));
516 unsigned int left = (
unsigned int)std::max<double>(0.0, bb.
getLeft());
517 unsigned int right = (
unsigned int)std::min<double>((
double)width, std::max<double>(0.0, bb.
getRight()));
525 std::vector<double> point_cloud_face, point_cloud_face_custom;
537 double prev_x, prev_y, prev_z;
540 double x = 0.0, y = 0.0;
541 for (
unsigned int i = top; i < bottom; i += stepY) {
542 for (
unsigned int j = left; j < right; j += stepX) {
544 (
m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
545 j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
549 point_cloud_face.push_back(point_cloud[i * width + j][0]);
550 point_cloud_face.push_back(point_cloud[i * width + j][1]);
551 point_cloud_face.push_back(point_cloud[i * width + j][2]);
563 prev_z = point_cloud[i * width + j][2];
567 point_cloud_face_custom.push_back(prev_x);
568 point_cloud_face_custom.push_back(x);
570 point_cloud_face_custom.push_back(prev_y);
571 point_cloud_face_custom.push_back(y);
573 point_cloud_face_custom.push_back(prev_z);
574 point_cloud_face_custom.push_back(point_cloud[i * width + j][2]);
579 point_cloud_face_custom.push_back(x);
580 point_cloud_face_custom.push_back(y);
581 point_cloud_face_custom.push_back(point_cloud[i * width + j][2]);
585 #if DEBUG_DISPLAY_DEPTH_NORMAL
586 debugImage[i][j] = 255;
593 if (checkSSE2 && push) {
594 point_cloud_face_custom.push_back(prev_x);
595 point_cloud_face_custom.push_back(prev_y);
596 point_cloud_face_custom.push_back(prev_z);
600 if (point_cloud_face.empty() && point_cloud_face_custom.empty()) {
607 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
609 pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face_pcl(
new pcl::PointCloud<pcl::PointXYZ>);
610 point_cloud_face_pcl->reserve(point_cloud_face.size() / 3);
612 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
613 point_cloud_face_pcl->push_back(
614 pcl::PointXYZ(point_cloud_face[3 * i], point_cloud_face[3 * i + 1], point_cloud_face[3 * i + 2]));
617 computeDesiredFeaturesPCL(point_cloud_face_pcl, desired_features, desired_normal, centroid_point);
626 desired_normal, centroid_point);
638 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
639 bool vpMbtFaceDepthNormal::computeDesiredFeaturesPCL(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud_face,
645 pcl::ModelCoefficients::Ptr coefficients(
new pcl::ModelCoefficients);
646 pcl::PointIndices::Ptr inliers(
new pcl::PointIndices);
648 pcl::SACSegmentation<pcl::PointXYZ> seg;
650 seg.setOptimizeCoefficients(
true);
652 seg.setModelType(pcl::SACMODEL_PLANE);
657 seg.setInputCloud(point_cloud_face);
658 seg.segment(*inliers, *coefficients);
660 pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face_extracted(
new pcl::PointCloud<pcl::PointXYZ>);
662 pcl::ExtractIndices<pcl::PointXYZ> extract;
665 extract.setInputCloud(point_cloud_face);
666 extract.setIndices(inliers);
667 extract.setNegative(
false);
668 extract.filter(*point_cloud_face_extracted);
670 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
671 pcl::PointXYZ centroid_point_pcl;
672 if (pcl::computeCentroid(*point_cloud_face_extracted, centroid_point_pcl)) {
673 pcl::PointXYZ face_normal;
675 centroid_point_pcl, face_normal);
677 desired_features.
resize(3,
false);
678 desired_features[0] = -coefficients->values[0] / coefficients->values[3];
679 desired_features[1] = -coefficients->values[1] / coefficients->values[3];
680 desired_features[2] = -coefficients->values[2] / coefficients->values[3];
682 desired_normal[0] = face_normal.x;
683 desired_normal[1] = face_normal.y;
684 desired_normal[2] = face_normal.z;
686 centroid_point[0] = centroid_point_pcl.x;
687 centroid_point[1] = centroid_point_pcl.y;
688 centroid_point[2] = centroid_point_pcl.z;
691 std::cerr <<
"Cannot compute centroid!" << std::endl;
695 std::cerr <<
"Cannot compute centroid using PCL " << PCL_VERSION_PRETTY <<
"!" << std::endl;
699 catch (
const pcl::PCLException &e) {
700 std::cerr <<
"Catch a PCL exception: " << e.what() << std::endl;
709 const std::vector<double> &point_cloud_face,
715 std::vector<double> weights;
720 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
721 centroid_point[0] += weights[i] * point_cloud_face[3 * i];
722 centroid_point[1] += weights[i] * point_cloud_face[3 * i + 1];
723 centroid_point[2] += weights[i] * point_cloud_face[3 * i + 2];
728 centroid_point[0] /= den;
729 centroid_point[1] /= den;
730 centroid_point[2] /= den;
743 desired_features.
resize(3,
false);
744 desired_features[0] = -plane_equation_SVD[0] / plane_equation_SVD[3];
745 desired_features[1] = -plane_equation_SVD[1] / plane_equation_SVD[3];
746 desired_features[2] = -plane_equation_SVD[2] / plane_equation_SVD[3];
758 centroid_cam[0] = centroid_point[0];
759 centroid_cam[1] = centroid_point[1];
760 centroid_cam[2] = centroid_point[2];
768 face_normal_cam[0] = desired_normal[0];
769 face_normal_cam[1] = desired_normal[1];
770 face_normal_cam[2] = desired_normal[2];
771 face_normal_cam[3] = 1;
779 if (points_.empty()) {
783 if (points_.size() < 2) {
784 centroid = points_[0];
788 std::vector<vpPoint> points = points_;
789 points.push_back(points_.front());
791 double A1 = 0.0, A2 = 0.0, c_x1 = 0.0, c_x2 = 0.0, c_y = 0.0, c_z = 0.0;
793 for (
size_t i = 0; i < points.size() - 1; i++) {
795 c_x1 += (points[i].get_X() + points[i + 1].get_X()) *
796 (points[i].get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y());
797 c_y += (points[i].get_Y() + points[i + 1].get_Y()) *
798 (points[i].get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y());
799 A1 += points[i].
get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y();
802 c_x2 += (points[i].get_X() + points[i + 1].get_X()) *
803 (points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z());
804 c_z += (points[i].get_Z() + points[i + 1].get_Z()) *
805 (points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z());
806 A2 += points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z();
815 centroid.
set_X(c_x1);
818 centroid.
set_X(c_x2);
828 std::vector<vpImagePoint> &roiPts
829 #
if DEBUG_DISPLAY_DEPTH_NORMAL
831 std::vector<std::vector<vpImagePoint> > &roiPts_vec
840 it->m_p1->changeFrame(cMo);
841 it->m_p2->changeFrame(cMo);
845 it->m_poly.changeFrame(cMo);
846 it->m_poly.computePolygonClipped(
m_cam);
848 if (it->m_poly.polyClipped.size() == 2 &&
856 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
860 for (
unsigned int i = 0; i < linesLst.size(); i++) {
861 linesLst[i].first.project();
862 linesLst[i].second.project();
870 roiPts.push_back(ip1);
871 roiPts.push_back(ip2);
873 #if DEBUG_DISPLAY_DEPTH_NORMAL
874 std::vector<vpImagePoint> roiPts_;
875 roiPts_.push_back(ip1);
876 roiPts_.push_back(ip2);
877 roiPts_vec.push_back(roiPts_);
887 #if DEBUG_DISPLAY_DEPTH_NORMAL
888 roiPts_vec.push_back(roiPts);
902 bool isvisible =
false;
906 int index = *itindex;
941 std::vector<vpImagePoint> roiPts;
944 std::vector<vpPoint> polyPts;
952 e4[0] = -centroid.
get_X();
953 e4[1] = -centroid.
get_Y();
954 e4[2] = -centroid.
get_Z();
958 double centroid_x = 0.0;
959 double centroid_y = 0.0;
960 double centroid_z = 0.0;
962 for (
size_t i = 0; i < polyPts.size(); i++) {
963 centroid_x += polyPts[i].get_X();
964 centroid_y += polyPts[i].get_Y();
965 centroid_z += polyPts[i].get_Z();
968 centroid_x /= polyPts.
size();
969 centroid_y /= polyPts.size();
970 centroid_z /= polyPts.size();
977 centroid.
set_X(centroid_x);
978 centroid.
set_Y(centroid_y);
979 centroid.
set_Z(centroid_z);
982 correct_normal.
resize(3,
false);
984 if (angle < M_PI_2) {
985 correct_normal = faceNormal;
988 correct_normal[0] = -faceNormal[0];
989 correct_normal[1] = -faceNormal[1];
990 correct_normal[2] = -faceNormal[2];
994 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
996 pcl::PointXYZ &face_normal)
1002 faceNormal.normalize();
1005 e4[0] = -centroid_point.x;
1006 e4[1] = -centroid_point.y;
1007 e4[2] = -centroid_point.z;
1011 if (angle < M_PI_2) {
1012 face_normal = pcl::PointXYZ(faceNormal[0], faceNormal[1], faceNormal[2]);
1015 face_normal = pcl::PointXYZ(-faceNormal[0], -faceNormal[1], -faceNormal[2]);
1023 face_normal.
resize(3,
false);
1024 face_normal[0] = nx;
1025 face_normal[1] = ny;
1026 face_normal[2] = nz;
1033 if (angle >= M_PI_2) {
1034 face_normal[0] = -face_normal[0];
1035 face_normal[1] = -face_normal[1];
1036 face_normal[2] = -face_normal[2];
1042 L.resize(3, 6,
false,
false);
1055 features.
resize(3,
false);
1056 features[0] = -ux / D;
1057 features[1] = -uy / D;
1058 features[2] = -uz / D;
1061 L[0][0] = ux * ux / D2;
1062 L[0][1] = ux * uy / D2;
1063 L[0][2] = ux * uz / D2;
1069 L[1][0] = ux * uy / D2;
1070 L[1][1] = uy * uy / D2;
1071 L[1][2] = uy * uz / D2;
1077 L[2][0] = ux * uz / D2;
1078 L[2][1] = uy * uz / D2;
1079 L[2][2] = uz * uz / D2;
1087 bool displayFullModel)
1089 std::vector<std::vector<double> > models =
1092 for (
size_t i = 0; i < models.size(); i++) {
1101 bool displayFullModel)
1103 std::vector<std::vector<double> > models =
1106 for (
size_t i = 0; i < models.size(); i++) {
1157 pt_extremity.
set_X(pt_centroid.
get_X() + correct_normal[0] * scale);
1158 pt_extremity.
set_Y(pt_centroid.
get_Y() + correct_normal[1] * scale);
1159 pt_extremity.
set_Z(pt_centroid.
get_Z() + correct_normal[2] * scale);
1212 pt_extremity.
set_X(pt_centroid.
get_X() + correct_normal[0] * scale);
1213 pt_extremity.
set_Y(pt_centroid.
get_Y() + correct_normal[1] * scale);
1214 pt_extremity.
set_Z(pt_centroid.
get_Z() + correct_normal[2] * scale);
1226 vpMbtTukeyEstimator<double> tukey_robust;
1227 std::vector<double> residues(point_cloud_face.size() / 3);
1229 w.resize(point_cloud_face.size() / 3, 1.0);
1231 unsigned int max_iter = 30, iter = 0;
1232 double error = 0.0, prev_error = -1.0;
1233 double A = 0.0, B = 0.0, C = 0.0;
1235 Mat33<double> ATA_3x3;
1244 while (std::fabs(error - prev_error) > 1e-6 && (iter < max_iter)) {
1261 if (point_cloud_face.size() / 3 >= 2) {
1262 const double *ptr_point_cloud = &point_cloud_face[0];
1263 const __m128d vA = _mm_set1_pd(A);
1264 const __m128d vB = _mm_set1_pd(B);
1265 const __m128d vC = _mm_set1_pd(C);
1266 const __m128d vones = _mm_set1_pd(1.0);
1268 double *ptr_residues = &residues[0];
1270 for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_residues += 2) {
1271 const __m128d vxi = _mm_loadu_pd(ptr_point_cloud);
1272 const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2);
1273 const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4);
1274 const __m128d vinvZi = _mm_div_pd(vones, vZi);
1277 _mm_add_pd(_mm_add_pd(_mm_mul_pd(vA, vxi), _mm_mul_pd(vB, vyi)), _mm_sub_pd(vC, vinvZi));
1278 _mm_storeu_pd(ptr_residues, tmp);
1282 for (; cpt < point_cloud_face.size(); cpt += 3) {
1283 double xi = point_cloud_face[cpt];
1284 double yi = point_cloud_face[cpt + 1];
1285 double Zi = point_cloud_face[cpt + 2];
1287 residues[cpt / 3] = (A * xi + B * yi + C - 1 / Zi);
1291 tukey_robust.MEstimator(residues, w, 1e-2);
1293 __m128d vsum_wi2_xi2 = _mm_setzero_pd();
1294 __m128d vsum_wi2_yi2 = _mm_setzero_pd();
1295 __m128d vsum_wi2 = _mm_setzero_pd();
1296 __m128d vsum_wi2_xi_yi = _mm_setzero_pd();
1297 __m128d vsum_wi2_xi = _mm_setzero_pd();
1298 __m128d vsum_wi2_yi = _mm_setzero_pd();
1300 __m128d vsum_wi2_xi_Zi = _mm_setzero_pd();
1301 __m128d vsum_wi2_yi_Zi = _mm_setzero_pd();
1302 __m128d vsum_wi2_Zi = _mm_setzero_pd();
1306 if (point_cloud_face.size() / 3 >= 2) {
1307 const double *ptr_point_cloud = &point_cloud_face[0];
1308 double *ptr_w = &w[0];
1310 const __m128d vones = _mm_set1_pd(1.0);
1312 for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_w += 2) {
1313 const __m128d vwi2 = _mm_mul_pd(_mm_loadu_pd(ptr_w), _mm_loadu_pd(ptr_w));
1315 const __m128d vxi = _mm_loadu_pd(ptr_point_cloud);
1316 const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2);
1317 const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4);
1318 const __m128d vinvZi = _mm_div_pd(vones, vZi);
1320 vsum_wi2_xi2 = _mm_add_pd(vsum_wi2_xi2, _mm_mul_pd(vwi2, _mm_mul_pd(vxi, vxi)));
1321 vsum_wi2_yi2 = _mm_add_pd(vsum_wi2_yi2, _mm_mul_pd(vwi2, _mm_mul_pd(vyi, vyi)));
1322 vsum_wi2 = _mm_add_pd(vsum_wi2, vwi2);
1323 vsum_wi2_xi_yi = _mm_add_pd(vsum_wi2_xi_yi, _mm_mul_pd(vwi2, _mm_mul_pd(vxi, vyi)));
1324 vsum_wi2_xi = _mm_add_pd(vsum_wi2_xi, _mm_mul_pd(vwi2, vxi));
1325 vsum_wi2_yi = _mm_add_pd(vsum_wi2_yi, _mm_mul_pd(vwi2, vyi));
1327 const __m128d vwi2_invZi = _mm_mul_pd(vwi2, vinvZi);
1328 vsum_wi2_xi_Zi = _mm_add_pd(vsum_wi2_xi_Zi, _mm_mul_pd(vxi, vwi2_invZi));
1329 vsum_wi2_yi_Zi = _mm_add_pd(vsum_wi2_yi_Zi, _mm_mul_pd(vyi, vwi2_invZi));
1330 vsum_wi2_Zi = _mm_add_pd(vsum_wi2_Zi, vwi2_invZi);
1335 _mm_storeu_pd(vtmp, vsum_wi2_xi2);
1336 double sum_wi2_xi2 = vtmp[0] + vtmp[1];
1338 _mm_storeu_pd(vtmp, vsum_wi2_yi2);
1339 double sum_wi2_yi2 = vtmp[0] + vtmp[1];
1341 _mm_storeu_pd(vtmp, vsum_wi2);
1342 double sum_wi2 = vtmp[0] + vtmp[1];
1344 _mm_storeu_pd(vtmp, vsum_wi2_xi_yi);
1345 double sum_wi2_xi_yi = vtmp[0] + vtmp[1];
1347 _mm_storeu_pd(vtmp, vsum_wi2_xi);
1348 double sum_wi2_xi = vtmp[0] + vtmp[1];
1350 _mm_storeu_pd(vtmp, vsum_wi2_yi);
1351 double sum_wi2_yi = vtmp[0] + vtmp[1];
1353 _mm_storeu_pd(vtmp, vsum_wi2_xi_Zi);
1354 double sum_wi2_xi_Zi = vtmp[0] + vtmp[1];
1356 _mm_storeu_pd(vtmp, vsum_wi2_yi_Zi);
1357 double sum_wi2_yi_Zi = vtmp[0] + vtmp[1];
1359 _mm_storeu_pd(vtmp, vsum_wi2_Zi);
1360 double sum_wi2_Zi = vtmp[0] + vtmp[1];
1362 for (; cpt < point_cloud_face.size(); cpt += 3) {
1363 double wi2 = w[cpt / 3] * w[cpt / 3];
1365 double xi = point_cloud_face[cpt];
1366 double yi = point_cloud_face[cpt + 1];
1367 double Zi = point_cloud_face[cpt + 2];
1368 double invZi = 1.0 / Zi;
1370 sum_wi2_xi2 += wi2 * xi * xi;
1371 sum_wi2_yi2 += wi2 * yi * yi;
1373 sum_wi2_xi_yi += wi2 * xi * yi;
1374 sum_wi2_xi += wi2 * xi;
1375 sum_wi2_yi += wi2 * yi;
1377 sum_wi2_xi_Zi += wi2 * xi * invZi;
1378 sum_wi2_yi_Zi += wi2 * yi * invZi;
1379 sum_wi2_Zi += wi2 * invZi;
1382 ATA_3x3[0] = sum_wi2_xi2;
1383 ATA_3x3[1] = sum_wi2_xi_yi;
1384 ATA_3x3[2] = sum_wi2_xi;
1385 ATA_3x3[3] = sum_wi2_xi_yi;
1386 ATA_3x3[4] = sum_wi2_yi2;
1387 ATA_3x3[5] = sum_wi2_yi;
1388 ATA_3x3[6] = sum_wi2_xi;
1389 ATA_3x3[7] = sum_wi2_yi;
1390 ATA_3x3[8] = sum_wi2;
1392 Mat33<double> minv = ATA_3x3.inverse();
1394 A = minv[0] * sum_wi2_xi_Zi + minv[1] * sum_wi2_yi_Zi + minv[2] * sum_wi2_Zi;
1395 B = minv[3] * sum_wi2_xi_Zi + minv[4] * sum_wi2_yi_Zi + minv[5] * sum_wi2_Zi;
1396 C = minv[6] * sum_wi2_xi_Zi + minv[7] * sum_wi2_yi_Zi + minv[8] * sum_wi2_Zi;
1404 __m128d verror = _mm_set1_pd(0.0);
1405 if (point_cloud_face.size() / 3 >= 2) {
1406 const double *ptr_point_cloud = &point_cloud_face[0];
1407 const __m128d vA = _mm_set1_pd(A);
1408 const __m128d vB = _mm_set1_pd(B);
1409 const __m128d vC = _mm_set1_pd(C);
1410 const __m128d vones = _mm_set1_pd(1.0);
1412 double *ptr_residues = &residues[0];
1414 for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_residues += 2) {
1415 const __m128d vxi = _mm_loadu_pd(ptr_point_cloud);
1416 const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2);
1417 const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4);
1418 const __m128d vinvZi = _mm_div_pd(vones, vZi);
1420 const __m128d tmp = _mm_add_pd(_mm_add_pd(_mm_mul_pd(vA, vxi), _mm_mul_pd(vB, vyi)), _mm_sub_pd(vC, vinvZi));
1421 verror = _mm_add_pd(verror, _mm_mul_pd(tmp, tmp));
1423 _mm_storeu_pd(ptr_residues, tmp);
1427 _mm_storeu_pd(vtmp, verror);
1428 error = vtmp[0] + vtmp[1];
1430 for (
size_t idx = cpt; idx < point_cloud_face.size(); idx += 3) {
1431 double xi = point_cloud_face[idx];
1432 double yi = point_cloud_face[idx + 1];
1433 double Zi = point_cloud_face[idx + 2];
1435 error +=
vpMath::sqr(A * xi + B * yi + C - 1 / Zi);
1436 residues[idx / 3] = (A * xi + B * yi + C - 1 / Zi);
1439 error /= point_cloud_face.size() / 3;
1446 while (std::fabs(error - prev_error) > 1e-6 && (iter < max_iter)) {
1462 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1463 double xi = point_cloud_face[3 * i];
1464 double yi = point_cloud_face[3 * i + 1];
1465 double Zi = point_cloud_face[3 * i + 2];
1467 residues[i] = (A * xi + B * yi + C - 1 / Zi);
1471 tukey_robust.MEstimator(residues, w, 1e-2);
1474 double sum_wi2_xi2 = 0.0, sum_wi2_yi2 = 0.0, sum_wi2 = 0.0;
1475 double sum_wi2_xi_yi = 0.0, sum_wi2_xi = 0.0, sum_wi2_yi = 0.0;
1477 double sum_wi2_xi_Zi = 0.0, sum_wi2_yi_Zi = 0.0, sum_wi2_Zi = 0.0;
1479 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1480 double wi2 = w[i] * w[i];
1482 double xi = point_cloud_face[3 * i];
1483 double yi = point_cloud_face[3 * i + 1];
1484 double Zi = point_cloud_face[3 * i + 2];
1485 double invZi = 1 / Zi;
1487 sum_wi2_xi2 += wi2 * xi * xi;
1488 sum_wi2_yi2 += wi2 * yi * yi;
1490 sum_wi2_xi_yi += wi2 * xi * yi;
1491 sum_wi2_xi += wi2 * xi;
1492 sum_wi2_yi += wi2 * yi;
1494 sum_wi2_xi_Zi += wi2 * xi * invZi;
1495 sum_wi2_yi_Zi += wi2 * yi * invZi;
1496 sum_wi2_Zi += wi2 * invZi;
1499 ATA_3x3[0] = sum_wi2_xi2;
1500 ATA_3x3[1] = sum_wi2_xi_yi;
1501 ATA_3x3[2] = sum_wi2_xi;
1502 ATA_3x3[3] = sum_wi2_xi_yi;
1503 ATA_3x3[4] = sum_wi2_yi2;
1504 ATA_3x3[5] = sum_wi2_yi;
1505 ATA_3x3[6] = sum_wi2_xi;
1506 ATA_3x3[7] = sum_wi2_yi;
1507 ATA_3x3[8] = sum_wi2;
1509 Mat33<double> minv = ATA_3x3.inverse();
1511 A = minv[0] * sum_wi2_xi_Zi + minv[1] * sum_wi2_yi_Zi + minv[2] * sum_wi2_Zi;
1512 B = minv[3] * sum_wi2_xi_Zi + minv[4] * sum_wi2_yi_Zi + minv[5] * sum_wi2_Zi;
1513 C = minv[6] * sum_wi2_xi_Zi + minv[7] * sum_wi2_yi_Zi + minv[8] * sum_wi2_Zi;
1519 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1520 double xi = point_cloud_face[3 * i];
1521 double yi = point_cloud_face[3 * i + 1];
1522 double Zi = point_cloud_face[3 * i + 2];
1524 error +=
vpMath::sqr(A * xi + B * yi + C - 1 / Zi);
1525 residues[i] = (A * xi + B * yi + C - 1 / Zi);
1528 error /= point_cloud_face.size() / 3;
1534 x_estimated.
resize(3,
false);
1544 unsigned int max_iter = 10;
1545 double prev_error = 1e3;
1546 double error = 1e3 - 1;
1548 std::vector<double> weights(point_cloud_face.size() / 3, 1.0);
1549 std::vector<double> residues(point_cloud_face.size() / 3);
1550 vpMatrix M((
unsigned int)(point_cloud_face.size() / 3), 3);
1551 vpMbtTukeyEstimator<double> tukey;
1554 for (
unsigned int iter = 0; iter < max_iter && std::fabs(error - prev_error) > 1e-6; iter++) {
1556 tukey.MEstimator(residues, weights, 1e-4);
1569 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1570 residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] +
1571 C * point_cloud_face[3 * i + 2] + D) /
1572 sqrt(A * A + B * B + C * C);
1575 tukey.MEstimator(residues, weights, 1e-4);
1576 plane_equation_estimated.
resize(4,
false);
1580 double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0;
1581 double total_w = 0.0;
1583 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1584 centroid_x += weights[i] * point_cloud_face[3 * i];
1585 centroid_y += weights[i] * point_cloud_face[3 * i + 1];
1586 centroid_z += weights[i] * point_cloud_face[3 * i + 2];
1587 total_w += weights[i];
1590 centroid_x /= total_w;
1591 centroid_y /= total_w;
1592 centroid_z /= total_w;
1595 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1596 M[(
unsigned int)i][0] = weights[i] * (point_cloud_face[3 * i] - centroid_x);
1597 M[(
unsigned int)i][1] = weights[i] * (point_cloud_face[3 * i + 1] - centroid_y);
1598 M[(
unsigned int)i][2] = weights[i] * (point_cloud_face[3 * i + 2] - centroid_z);
1607 double smallestSv = W[0];
1608 unsigned int indexSmallestSv = 0;
1609 for (
unsigned int i = 1; i < W.
size(); i++) {
1610 if (W[i] < smallestSv) {
1612 indexSmallestSv = i;
1616 normal = V.
getCol(indexSmallestSv);
1619 double A = normal[0], B = normal[1], C = normal[2];
1620 double D = -(A * centroid_x + B * centroid_y + C * centroid_z);
1623 plane_equation_estimated[0] = A;
1624 plane_equation_estimated[1] = B;
1625 plane_equation_estimated[2] = C;
1626 plane_equation_estimated[3] = D;
1631 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1632 residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] +
1633 C * point_cloud_face[3 * i + 2] + D) /
1634 sqrt(A * A + B * B + C * C);
1635 error += weights[i] * residues[i];
1641 tukey.MEstimator(residues, weights, 1e-4);
1644 centroid.
resize(3,
false);
1645 double total_w = 0.0;
1647 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1648 centroid[0] += weights[i] * point_cloud_face[3 * i];
1649 centroid[1] += weights[i] * point_cloud_face[3 * i + 1];
1650 centroid[2] += weights[i] * point_cloud_face[3 * i + 2];
1651 total_w += weights[i];
1654 centroid[0] /= total_w;
1655 centroid[1] /= total_w;
1656 centroid[2] /= total_w;
1659 double A = normal[0], B = normal[1], C = normal[2];
1660 double D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
1663 plane_equation_estimated[0] = A;
1664 plane_equation_estimated[1] = B;
1665 plane_equation_estimated[2] = C;
1666 plane_equation_estimated[3] = D;
1674 std::vector<std::vector<double> >
1677 std::vector<std::vector<double> > features;
1702 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
1703 std::vector<double> params = { 2,
1704 im_centroid.
get_i(),
1705 im_centroid.
get_j(),
1706 im_extremity.
get_i(),
1707 im_extremity.
get_j() };
1709 std::vector<double> params;
1710 params.push_back(2);
1711 params.push_back(im_centroid.
get_i());
1712 params.push_back(im_centroid.
get_j());
1713 params.push_back(im_extremity.
get_i());
1714 params.push_back(im_extremity.
get_j());
1716 features.push_back(params);
1734 pt_extremity.
set_X(pt_centroid.
get_X() + correct_normal[0] * scale);
1735 pt_extremity.
set_Y(pt_centroid.
get_Y() + correct_normal[1] * scale);
1736 pt_extremity.
set_Z(pt_centroid.
get_Z() + correct_normal[2] * scale);
1742 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
1743 std::vector<double> params = { 3,
1744 im_centroid.
get_i(),
1745 im_centroid.
get_j(),
1746 im_extremity.
get_i(),
1747 im_extremity.
get_j() };
1749 std::vector<double> params;
1750 params.push_back(3);
1751 params.push_back(im_centroid.
get_i());
1752 params.push_back(im_centroid.
get_j());
1753 params.push_back(im_extremity.
get_i());
1754 params.push_back(im_extremity.
get_j());
1756 features.push_back(params);
1777 bool displayFullModel)
1779 std::vector<std::vector<double> > models;
1787 std::vector<std::vector<double> > lineModels =
1789 models.insert(models.end(), lineModels.begin(), lineModels.end());
1811 if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
1812 dz <= std::numeric_limits<double>::epsilon())
1824 (*it)->setCameraParameters(camera);
1834 (*it)->useScanLine = v;
unsigned int size() const
Return the number of elements of the 2D array.
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.
static const vpColor blue
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.
@ badValue
Used to indicate that a value is not in the allowed range.
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 ...
unsigned int getWidth() const
unsigned int getHeight() const
static double sqr(double x)
Implementation of a matrix and operations on matrices.
void svd(vpColVector &w, vpMatrix &V)
vpColVector getCol(unsigned int j) const
vpMbScanLine & getMbScanLineRenderer()
bool isVisible(unsigned int i)
void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector< std::pair< vpPoint, vpPoint > > &lines, const bool &displayResults=false)
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)
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)
vpMbtPolygon & getPolygon()
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 ¢roid_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 std::vector< vpColVector > &point_cloud, vpColVector &desired_features, unsigned int stepX, unsigned int stepY, const vpImage< bool > *mask=nullptr)
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 ¢roid)
void computeROI(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height, std::vector< vpImagePoint > &roiPts)
void setScanLineVisibilityTest(bool v)
void estimatePlaneEquationSVD(const std::vector< double > &point_cloud_face, const vpHomogeneousMatrix &cMo, vpColVector &plane_equation_estimated, vpColVector ¢roid)
std::vector< vpMbtDistanceLine * > m_listOfFaceLines
void computeDesiredNormalAndCentroid(const vpHomogeneousMatrix &cMo, const vpColVector &desired_normal, const vpColVector ¢roid_point)
bool m_isTrackedDepthNormalFace
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.
@ ROBUST_FEATURE_ESTIMATION
@ ROBUST_SVD_PLANE_ESTIMATION
vpPlane m_planeObject
Plane equation described in the object frame.
void computeVisibilityDisplay()
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 ¢roid_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)
virtual ~vpMbtFaceDepthNormal()
std::vector< PolygonLine > m_polygonLines
void computeNormalVisibility(double nx, double ny, double nz, const vpColVector ¢roid_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)
static bool inRoiMask(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)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_oX() const
Get the point oX coordinate in the object frame.
double get_y() const
Get the point y coordinate in the image plane.
double get_Y() const
Get the point cY coordinate in the camera frame.
double get_oZ() const
Get the point oZ coordinate in the object frame.
void set_X(double cX)
Set the point cX coordinate in the camera frame.
double get_x() const
Get the point x coordinate in the image plane.
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
double get_Z() const
Get the point cZ coordinate in the camera frame.
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
double get_oY() const
Get the point oY coordinate in the object frame.
double get_X() const
Get the point cX coordinate in the camera frame.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const vp_override
void setWorldCoordinates(double oX, double oY, double oZ)
Implements a 3D polygon with render functionalities like clipping.
void setFarClippingDistance(const double &dist)
void setNearClippingDistance(const double &dist)
void setClipping(const unsigned int &flags)
void getRoiClipped(const vpCameraParameters &cam, std::vector< vpImagePoint > &roi)
void getPolygonClipped(std::vector< std::pair< vpPoint, unsigned int > > &poly)
Defines a generic 2D polygon.
vpRect getBoundingBox() const
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Defines a rectangle in the plane.
void setRight(double pos)
void setBottom(double pos)
Class for generating random numbers with uniform probability density.
VISP_EXPORT bool checkSSE2()