36 #include <visp3/core/vpCPUFeatures.h> 37 #include <visp3/mbt/vpMbtFaceDepthNormal.h> 38 #include <visp3/mbt/vpMbtTukeyEstimator.h> 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(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),
65 m_pclPlaneEstimationRansacMaxIter(200), m_pclPlaneEstimationRansacThreshold(0.001), m_polygonLines()
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;
151 const unsigned int height,
152 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
153 vpColVector &desired_features,
const unsigned int stepX,
154 const unsigned int stepY
155 #
if DEBUG_DISPLAY_DEPTH_NORMAL
158 std::vector<std::vector<vpImagePoint> > &roiPts_vec
165 if (width == 0 || height == 0)
168 std::vector<vpImagePoint> roiPts;
172 #
if DEBUG_DISPLAY_DEPTH_NORMAL
178 if (roiPts.size() <= 2) {
180 std::cerr <<
"Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
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()));
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;
216 double prev_x, prev_y, prev_z;
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() &&
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);
246 prev_z = (*point_cloud)(j, i).z;
249 point_cloud_face_custom.push_back(prev_x);
250 point_cloud_face_custom.push_back(x);
252 point_cloud_face_custom.push_back(prev_y);
253 point_cloud_face_custom.push_back(y);
255 point_cloud_face_custom.push_back(prev_z);
256 point_cloud_face_custom.push_back((*point_cloud)(j, i).z);
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);
267 #if DEBUG_DISPLAY_DEPTH_NORMAL 268 debugImage[i][j] = 255;
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);
282 if (point_cloud_face->empty() && point_cloud_face_custom.empty() && point_cloud_face_vec.empty()) {
297 desired_normal, centroid_point);
311 const unsigned int height,
312 const std::vector<vpColVector> &point_cloud,
313 vpColVector &desired_features,
const unsigned int stepX,
314 const unsigned int stepY
315 #
if DEBUG_DISPLAY_DEPTH_NORMAL
318 std::vector<std::vector<vpImagePoint> > &roiPts_vec
325 if (width == 0 || height == 0)
328 std::vector<vpImagePoint> roiPts;
332 #
if DEBUG_DISPLAY_DEPTH_NORMAL
338 if (roiPts.size() <= 2) {
340 std::cerr <<
"Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
348 unsigned int top = (
unsigned int)std::max(0.0, bb.
getTop());
349 unsigned int bottom = (
unsigned int)std::min((
double)height, std::max(0.0, bb.
getBottom()));
350 unsigned int left = (
unsigned int)std::max(0.0, bb.
getLeft());
351 unsigned int right = (
unsigned int)std::min((
double)width, std::max(0.0, bb.
getRight()));
359 std::vector<double> point_cloud_face, point_cloud_face_custom;
371 double prev_x, prev_y, prev_z;
374 double x = 0.0, y = 0.0;
375 for (
unsigned int i = top; i < bottom; i += stepY) {
376 for (
unsigned int j = left; j < right; j += stepX) {
378 (
m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
379 j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
383 point_cloud_face.push_back(point_cloud[i * width + j][0]);
384 point_cloud_face.push_back(point_cloud[i * width + j][1]);
385 point_cloud_face.push_back(point_cloud[i * width + j][2]);
397 prev_z = point_cloud[i * width + j][2];
400 point_cloud_face_custom.push_back(prev_x);
401 point_cloud_face_custom.push_back(x);
403 point_cloud_face_custom.push_back(prev_y);
404 point_cloud_face_custom.push_back(y);
406 point_cloud_face_custom.push_back(prev_z);
407 point_cloud_face_custom.push_back(point_cloud[i * width + j][2]);
411 point_cloud_face_custom.push_back(x);
412 point_cloud_face_custom.push_back(y);
413 point_cloud_face_custom.push_back(point_cloud[i * width + j][2]);
417 #if DEBUG_DISPLAY_DEPTH_NORMAL 418 debugImage[i][j] = 255;
425 if (checkSSE2 && push) {
426 point_cloud_face_custom.push_back(prev_x);
427 point_cloud_face_custom.push_back(prev_y);
428 point_cloud_face_custom.push_back(prev_z);
432 if (point_cloud_face.empty() && point_cloud_face_custom.empty()) {
441 pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face_pcl(
new pcl::PointCloud<pcl::PointXYZ>);
442 point_cloud_face_pcl->reserve(point_cloud_face.size() / 3);
444 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
445 point_cloud_face_pcl->push_back(
446 pcl::PointXYZ(point_cloud_face[3 * i], point_cloud_face[3 * i + 1], point_cloud_face[3 * i + 2]));
456 desired_normal, centroid_point);
475 pcl::ModelCoefficients::Ptr coefficients(
new pcl::ModelCoefficients);
476 pcl::PointIndices::Ptr inliers(
new pcl::PointIndices);
478 pcl::SACSegmentation<pcl::PointXYZ> seg;
480 seg.setOptimizeCoefficients(
true);
482 seg.setModelType(pcl::SACMODEL_PLANE);
487 seg.setInputCloud(point_cloud_face);
488 seg.segment(*inliers, *coefficients);
490 pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face_extracted(
new pcl::PointCloud<pcl::PointXYZ>);
492 pcl::ExtractIndices<pcl::PointXYZ> extract;
495 extract.setInputCloud(point_cloud_face);
496 extract.setIndices(inliers);
497 extract.setNegative(
false);
498 extract.filter(*point_cloud_face_extracted);
500 pcl::PointXYZ centroid_point_pcl;
501 if (pcl::computeCentroid(*point_cloud_face_extracted, centroid_point_pcl)) {
502 pcl::PointXYZ face_normal;
504 centroid_point_pcl, face_normal);
506 desired_features.
resize(3,
false);
507 desired_features[0] = -coefficients->values[0] / coefficients->values[3];
508 desired_features[1] = -coefficients->values[1] / coefficients->values[3];
509 desired_features[2] = -coefficients->values[2] / coefficients->values[3];
511 desired_normal[0] = face_normal.x;
512 desired_normal[1] = face_normal.y;
513 desired_normal[2] = face_normal.z;
515 centroid_point[0] = centroid_point_pcl.x;
516 centroid_point[1] = centroid_point_pcl.y;
517 centroid_point[2] = centroid_point_pcl.z;
519 std::cerr <<
"Cannot compute centroid!" << std::endl;
522 }
catch (
const pcl::PCLException &e) {
523 std::cerr <<
"Catch a PCL exception: " << e.what() << std::endl;
532 const std::vector<double> &point_cloud_face,
538 std::vector<double> weights;
543 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
544 centroid_point[0] += weights[i] * point_cloud_face[3 * i];
545 centroid_point[1] += weights[i] * point_cloud_face[3 * i + 1];
546 centroid_point[2] += weights[i] * point_cloud_face[3 * i + 2];
551 centroid_point[0] /= den;
552 centroid_point[1] /= den;
553 centroid_point[2] /= den;
566 desired_features.
resize(3,
false);
567 desired_features[0] = -plane_equation_SVD[0] / plane_equation_SVD[3];
568 desired_features[1] = -plane_equation_SVD[1] / plane_equation_SVD[3];
569 desired_features[2] = -plane_equation_SVD[2] / plane_equation_SVD[3];
581 centroid_cam[0] = centroid_point[0];
582 centroid_cam[1] = centroid_point[1];
583 centroid_cam[2] = centroid_point[2];
591 face_normal_cam[0] = desired_normal[0];
592 face_normal_cam[1] = desired_normal[1];
593 face_normal_cam[2] = desired_normal[2];
594 face_normal_cam[3] = 1;
602 if (points_.empty()) {
606 if (points_.size() < 2) {
607 centroid = points_[0];
611 std::vector<vpPoint> points = points_;
612 points.push_back(points_.front());
614 double A1 = 0.0, A2 = 0.0, c_x1 = 0.0, c_x2 = 0.0, c_y = 0.0, c_z = 0.0;
616 for (
size_t i = 0; i < points.size() - 1; i++) {
618 c_x1 += (points[i].get_X() + points[i + 1].get_X()) *
619 (points[i].get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y());
620 c_y += (points[i].get_Y() + points[i + 1].get_Y()) *
621 (points[i].get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y());
622 A1 += points[i].
get_X() * points[i + 1].get_Y() - points[i + 1].get_X() * points[i].get_Y();
625 c_x2 += (points[i].get_X() + points[i + 1].get_X()) *
626 (points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z());
627 c_z += (points[i].get_Z() + points[i + 1].get_Z()) *
628 (points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z());
629 A2 += points[i].get_X() * points[i + 1].get_Z() - points[i + 1].get_X() * points[i].get_Z();
638 centroid.
set_X(c_x1);
640 centroid.
set_X(c_x2);
650 const unsigned int height, std::vector<vpImagePoint> &roiPts
651 #
if DEBUG_DISPLAY_DEPTH_NORMAL
653 std::vector<std::vector<vpImagePoint> > &roiPts_vec
662 it->m_p1->changeFrame(cMo);
663 it->m_p2->changeFrame(cMo);
667 it->m_poly.changeFrame(cMo);
668 it->m_poly.computePolygonClipped(
m_cam);
670 if (it->m_poly.polyClipped.size() == 2 &&
678 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
682 for (
unsigned int i = 0; i < linesLst.size(); i++) {
683 linesLst[i].first.project();
684 linesLst[i].second.project();
692 roiPts.push_back(ip1);
693 roiPts.push_back(ip2);
695 #if DEBUG_DISPLAY_DEPTH_NORMAL 696 std::vector<vpImagePoint> roiPts_;
697 roiPts_.push_back(ip1);
698 roiPts_.push_back(ip2);
699 roiPts_vec.push_back(roiPts_);
708 #if DEBUG_DISPLAY_DEPTH_NORMAL 709 roiPts_vec.push_back(roiPts);
723 bool isvisible =
false;
727 int index = *itindex;
760 std::vector<vpImagePoint> roiPts;
763 std::vector<vpPoint> polyPts;
771 e4[0] = -centroid.
get_X();
772 e4[1] = -centroid.
get_Y();
773 e4[2] = -centroid.
get_Z();
776 double centroid_x = 0.0;
777 double centroid_y = 0.0;
778 double centroid_z = 0.0;
780 for (
size_t i = 0; i < polyPts.size(); i++) {
781 centroid_x += polyPts[i].get_X();
782 centroid_y += polyPts[i].get_Y();
783 centroid_z += polyPts[i].get_Z();
786 centroid_x /= polyPts.
size();
787 centroid_y /= polyPts.size();
788 centroid_z /= polyPts.size();
795 centroid.
set_X(centroid_x);
796 centroid.
set_Y(centroid_y);
797 centroid.
set_Z(centroid_z);
800 correct_normal.
resize(3,
false);
802 if (angle < M_PI_2) {
803 correct_normal = faceNormal;
805 correct_normal[0] = -faceNormal[0];
806 correct_normal[1] = -faceNormal[1];
807 correct_normal[2] = -faceNormal[2];
813 const pcl::PointXYZ ¢roid_point, pcl::PointXYZ &face_normal)
822 e4[0] = -centroid_point.x;
823 e4[1] = -centroid_point.y;
824 e4[2] = -centroid_point.z;
828 if (angle < M_PI_2) {
829 face_normal = pcl::PointXYZ(faceNormal[0], faceNormal[1], faceNormal[2]);
831 face_normal = pcl::PointXYZ(-faceNormal[0], -faceNormal[1], -faceNormal[2]);
839 face_normal.
resize(3,
false);
849 if (angle >= M_PI_2) {
850 face_normal[0] = -face_normal[0];
851 face_normal[1] = -face_normal[1];
852 face_normal[2] = -face_normal[2];
858 L.
resize(3, 6,
false,
false);
871 features.
resize(3,
false);
872 features[0] = -ux / D;
873 features[1] = -uy / D;
874 features[2] = -uz / D;
877 L[0][0] = ux * ux / D2;
878 L[0][1] = ux * uy / D2;
879 L[0][2] = ux * uz / D2;
885 L[1][0] = ux * uy / D2;
886 L[1][1] = uy * uy / D2;
887 L[1][2] = uy * uz / D2;
893 L[2][0] = ux * uz / D2;
894 L[2][1] = uy * uz / D2;
895 L[2][2] = uz * uz / D2;
903 const bool displayFullModel)
911 line->
display(I, cMo, cam, col, thickness, displayFullModel);
918 const bool displayFullModel)
926 line->
display(I, cMo, cam, col, thickness, displayFullModel);
933 const unsigned int thickness)
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);
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);
989 const unsigned int thickness)
1002 pt_normal.project();
1005 pt_extremity.
set_X(pt_centroid.
get_X() + pt_normal.get_X() * scale);
1006 pt_extremity.
set_Y(pt_centroid.
get_Y() + pt_normal.get_Y() * scale);
1007 pt_extremity.
set_Z(pt_centroid.
get_Z() + pt_normal.get_Z() * scale);
1032 pt_extremity.
set_X(pt_centroid.
get_X() + correct_normal[0] * scale);
1033 pt_extremity.
set_Y(pt_centroid.
get_Y() + correct_normal[1] * scale);
1034 pt_extremity.
set_Z(pt_centroid.
get_Z() + correct_normal[2] * scale);
1046 vpMbtTukeyEstimator<double> tukey_robust;
1047 std::vector<double> residues(point_cloud_face.size() / 3);
1049 w.resize(point_cloud_face.size() / 3, 1.0);
1051 unsigned int max_iter = 30, iter = 0;
1052 double error = 0.0, prev_error = -1.0;
1053 double A = 0.0, B = 0.0, C = 0.0;
1055 Mat33<double> ATA_3x3;
1064 while (std::fabs(error - prev_error) > 1e-6 && (iter < max_iter)) {
1081 if (point_cloud_face.size() / 3 >= 2) {
1082 const double *ptr_point_cloud = &point_cloud_face[0];
1083 const __m128d vA = _mm_set1_pd(A);
1084 const __m128d vB = _mm_set1_pd(B);
1085 const __m128d vC = _mm_set1_pd(C);
1086 const __m128d vones = _mm_set1_pd(1.0);
1088 double *ptr_residues = &residues[0];
1090 for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_residues += 2) {
1091 const __m128d vxi = _mm_loadu_pd(ptr_point_cloud);
1092 const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2);
1093 const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4);
1094 const __m128d vinvZi = _mm_div_pd(vones, vZi);
1097 _mm_add_pd(_mm_add_pd(_mm_mul_pd(vA, vxi), _mm_mul_pd(vB, vyi)), _mm_sub_pd(vC, vinvZi));
1098 _mm_storeu_pd(ptr_residues, tmp);
1102 for (; cpt < point_cloud_face.size(); cpt += 3) {
1103 double xi = point_cloud_face[cpt];
1104 double yi = point_cloud_face[cpt + 1];
1105 double Zi = point_cloud_face[cpt + 2];
1107 residues[cpt / 3] = (A * xi + B * yi + C - 1 / Zi);
1111 tukey_robust.MEstimator(residues, w, 1e-2);
1113 __m128d vsum_wi2_xi2 = _mm_setzero_pd();
1114 __m128d vsum_wi2_yi2 = _mm_setzero_pd();
1115 __m128d vsum_wi2 = _mm_setzero_pd();
1116 __m128d vsum_wi2_xi_yi = _mm_setzero_pd();
1117 __m128d vsum_wi2_xi = _mm_setzero_pd();
1118 __m128d vsum_wi2_yi = _mm_setzero_pd();
1120 __m128d vsum_wi2_xi_Zi = _mm_setzero_pd();
1121 __m128d vsum_wi2_yi_Zi = _mm_setzero_pd();
1122 __m128d vsum_wi2_Zi = _mm_setzero_pd();
1126 if (point_cloud_face.size() / 3 >= 2) {
1127 const double *ptr_point_cloud = &point_cloud_face[0];
1128 double *ptr_w = &w[0];
1130 const __m128d vones = _mm_set1_pd(1.0);
1132 for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_w += 2) {
1133 const __m128d vwi2 = _mm_mul_pd(_mm_loadu_pd(ptr_w), _mm_loadu_pd(ptr_w));
1135 const __m128d vxi = _mm_loadu_pd(ptr_point_cloud);
1136 const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2);
1137 const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4);
1138 const __m128d vinvZi = _mm_div_pd(vones, vZi);
1140 vsum_wi2_xi2 = _mm_add_pd(vsum_wi2_xi2, _mm_mul_pd(vwi2, _mm_mul_pd(vxi, vxi)));
1141 vsum_wi2_yi2 = _mm_add_pd(vsum_wi2_yi2, _mm_mul_pd(vwi2, _mm_mul_pd(vyi, vyi)));
1142 vsum_wi2 = _mm_add_pd(vsum_wi2, vwi2);
1143 vsum_wi2_xi_yi = _mm_add_pd(vsum_wi2_xi_yi, _mm_mul_pd(vwi2, _mm_mul_pd(vxi, vyi)));
1144 vsum_wi2_xi = _mm_add_pd(vsum_wi2_xi, _mm_mul_pd(vwi2, vxi));
1145 vsum_wi2_yi = _mm_add_pd(vsum_wi2_yi, _mm_mul_pd(vwi2, vyi));
1147 const __m128d vwi2_invZi = _mm_mul_pd(vwi2, vinvZi);
1148 vsum_wi2_xi_Zi = _mm_add_pd(vsum_wi2_xi_Zi, _mm_mul_pd(vxi, vwi2_invZi));
1149 vsum_wi2_yi_Zi = _mm_add_pd(vsum_wi2_yi_Zi, _mm_mul_pd(vyi, vwi2_invZi));
1150 vsum_wi2_Zi = _mm_add_pd(vsum_wi2_Zi, vwi2_invZi);
1155 _mm_storeu_pd(vtmp, vsum_wi2_xi2);
1156 double sum_wi2_xi2 = vtmp[0] + vtmp[1];
1158 _mm_storeu_pd(vtmp, vsum_wi2_yi2);
1159 double sum_wi2_yi2 = vtmp[0] + vtmp[1];
1161 _mm_storeu_pd(vtmp, vsum_wi2);
1162 double sum_wi2 = vtmp[0] + vtmp[1];
1164 _mm_storeu_pd(vtmp, vsum_wi2_xi_yi);
1165 double sum_wi2_xi_yi = vtmp[0] + vtmp[1];
1167 _mm_storeu_pd(vtmp, vsum_wi2_xi);
1168 double sum_wi2_xi = vtmp[0] + vtmp[1];
1170 _mm_storeu_pd(vtmp, vsum_wi2_yi);
1171 double sum_wi2_yi = vtmp[0] + vtmp[1];
1173 _mm_storeu_pd(vtmp, vsum_wi2_xi_Zi);
1174 double sum_wi2_xi_Zi = vtmp[0] + vtmp[1];
1176 _mm_storeu_pd(vtmp, vsum_wi2_yi_Zi);
1177 double sum_wi2_yi_Zi = vtmp[0] + vtmp[1];
1179 _mm_storeu_pd(vtmp, vsum_wi2_Zi);
1180 double sum_wi2_Zi = vtmp[0] + vtmp[1];
1182 for (; cpt < point_cloud_face.size(); cpt += 3) {
1183 double wi2 = w[cpt / 3] * w[cpt / 3];
1185 double xi = point_cloud_face[cpt];
1186 double yi = point_cloud_face[cpt + 1];
1187 double Zi = point_cloud_face[cpt + 2];
1188 double invZi = 1.0 / Zi;
1190 sum_wi2_xi2 += wi2 * xi * xi;
1191 sum_wi2_yi2 += wi2 * yi * yi;
1193 sum_wi2_xi_yi += wi2 * xi * yi;
1194 sum_wi2_xi += wi2 * xi;
1195 sum_wi2_yi += wi2 * yi;
1197 sum_wi2_xi_Zi += wi2 * xi * invZi;
1198 sum_wi2_yi_Zi += wi2 * yi * invZi;
1199 sum_wi2_Zi += wi2 * invZi;
1202 ATA_3x3[0] = sum_wi2_xi2;
1203 ATA_3x3[1] = sum_wi2_xi_yi;
1204 ATA_3x3[2] = sum_wi2_xi;
1205 ATA_3x3[3] = sum_wi2_xi_yi;
1206 ATA_3x3[4] = sum_wi2_yi2;
1207 ATA_3x3[5] = sum_wi2_yi;
1208 ATA_3x3[6] = sum_wi2_xi;
1209 ATA_3x3[7] = sum_wi2_yi;
1210 ATA_3x3[8] = sum_wi2;
1212 Mat33<double> minv = ATA_3x3.inverse();
1214 A = minv[0] * sum_wi2_xi_Zi + minv[1] * sum_wi2_yi_Zi + minv[2] * sum_wi2_Zi;
1215 B = minv[3] * sum_wi2_xi_Zi + minv[4] * sum_wi2_yi_Zi + minv[5] * sum_wi2_Zi;
1216 C = minv[6] * sum_wi2_xi_Zi + minv[7] * sum_wi2_yi_Zi + minv[8] * sum_wi2_Zi;
1224 __m128d verror = _mm_set1_pd(0.0);
1225 if (point_cloud_face.size() / 3 >= 2) {
1226 const double *ptr_point_cloud = &point_cloud_face[0];
1227 const __m128d vA = _mm_set1_pd(A);
1228 const __m128d vB = _mm_set1_pd(B);
1229 const __m128d vC = _mm_set1_pd(C);
1230 const __m128d vones = _mm_set1_pd(1.0);
1232 double *ptr_residues = &residues[0];
1234 for (; cpt <= point_cloud_face.size() - 6; cpt += 6, ptr_point_cloud += 6, ptr_residues += 2) {
1235 const __m128d vxi = _mm_loadu_pd(ptr_point_cloud);
1236 const __m128d vyi = _mm_loadu_pd(ptr_point_cloud + 2);
1237 const __m128d vZi = _mm_loadu_pd(ptr_point_cloud + 4);
1238 const __m128d vinvZi = _mm_div_pd(vones, vZi);
1240 const __m128d tmp = _mm_add_pd(_mm_add_pd(_mm_mul_pd(vA, vxi), _mm_mul_pd(vB, vyi)), _mm_sub_pd(vC, vinvZi));
1241 verror = _mm_add_pd(verror, _mm_mul_pd(tmp, tmp));
1243 _mm_storeu_pd(ptr_residues, tmp);
1247 _mm_storeu_pd(vtmp, verror);
1248 error = vtmp[0] + vtmp[1];
1250 for (
size_t idx = cpt; idx < point_cloud_face.size(); idx += 3) {
1251 double xi = point_cloud_face[idx];
1252 double yi = point_cloud_face[idx + 1];
1253 double Zi = point_cloud_face[idx + 2];
1255 error +=
vpMath::sqr(A * xi + B * yi + C - 1 / Zi);
1256 residues[idx / 3] = (A * xi + B * yi + C - 1 / Zi);
1259 error /= point_cloud_face.size() / 3;
1265 while (std::fabs(error - prev_error) > 1e-6 && (iter < max_iter)) {
1281 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1282 double xi = point_cloud_face[3 * i];
1283 double yi = point_cloud_face[3 * i + 1];
1284 double Zi = point_cloud_face[3 * i + 2];
1286 residues[i] = (A * xi + B * yi + C - 1 / Zi);
1290 tukey_robust.MEstimator(residues, w, 1e-2);
1293 double sum_wi2_xi2 = 0.0, sum_wi2_yi2 = 0.0, sum_wi2 = 0.0;
1294 double sum_wi2_xi_yi = 0.0, sum_wi2_xi = 0.0, sum_wi2_yi = 0.0;
1296 double sum_wi2_xi_Zi = 0.0, sum_wi2_yi_Zi = 0.0, sum_wi2_Zi = 0.0;
1298 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1299 double wi2 = w[i] * w[i];
1301 double xi = point_cloud_face[3 * i];
1302 double yi = point_cloud_face[3 * i + 1];
1303 double Zi = point_cloud_face[3 * i + 2];
1304 double invZi = 1 / Zi;
1306 sum_wi2_xi2 += wi2 * xi * xi;
1307 sum_wi2_yi2 += wi2 * yi * yi;
1309 sum_wi2_xi_yi += wi2 * xi * yi;
1310 sum_wi2_xi += wi2 * xi;
1311 sum_wi2_yi += wi2 * yi;
1313 sum_wi2_xi_Zi += wi2 * xi * invZi;
1314 sum_wi2_yi_Zi += wi2 * yi * invZi;
1315 sum_wi2_Zi += wi2 * invZi;
1318 ATA_3x3[0] = sum_wi2_xi2;
1319 ATA_3x3[1] = sum_wi2_xi_yi;
1320 ATA_3x3[2] = sum_wi2_xi;
1321 ATA_3x3[3] = sum_wi2_xi_yi;
1322 ATA_3x3[4] = sum_wi2_yi2;
1323 ATA_3x3[5] = sum_wi2_yi;
1324 ATA_3x3[6] = sum_wi2_xi;
1325 ATA_3x3[7] = sum_wi2_yi;
1326 ATA_3x3[8] = sum_wi2;
1328 Mat33<double> minv = ATA_3x3.inverse();
1330 A = minv[0] * sum_wi2_xi_Zi + minv[1] * sum_wi2_yi_Zi + minv[2] * sum_wi2_Zi;
1331 B = minv[3] * sum_wi2_xi_Zi + minv[4] * sum_wi2_yi_Zi + minv[5] * sum_wi2_Zi;
1332 C = minv[6] * sum_wi2_xi_Zi + minv[7] * sum_wi2_yi_Zi + minv[8] * sum_wi2_Zi;
1338 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1339 double xi = point_cloud_face[3 * i];
1340 double yi = point_cloud_face[3 * i + 1];
1341 double Zi = point_cloud_face[3 * i + 2];
1343 error +=
vpMath::sqr(A * xi + B * yi + C - 1 / Zi);
1344 residues[i] = (A * xi + B * yi + C - 1 / Zi);
1347 error /= point_cloud_face.size() / 3;
1353 x_estimated.
resize(3,
false);
1363 const unsigned int max_iter = 10;
1364 double prev_error = 1e3;
1365 double error = 1e3 - 1;
1367 std::vector<double> weights(point_cloud_face.size() / 3, 1.0);
1368 std::vector<double> residues(point_cloud_face.size() / 3);
1369 vpMatrix M((
unsigned int)(point_cloud_face.size() / 3), 3);
1370 vpMbtTukeyEstimator<double> tukey;
1373 for (
unsigned int iter = 0; iter < max_iter && std::fabs(error - prev_error) > 1e-6; iter++) {
1375 tukey.MEstimator(residues, weights, 1e-4);
1387 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1388 residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] +
1389 C * point_cloud_face[3 * i + 2] + D) /
1390 sqrt(A * A + B * B + C * C);
1393 tukey.MEstimator(residues, weights, 1e-4);
1394 plane_equation_estimated.
resize(4,
false);
1398 double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0;
1399 double total_w = 0.0;
1401 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1402 centroid_x += weights[i] * point_cloud_face[3 * i];
1403 centroid_y += weights[i] * point_cloud_face[3 * i + 1];
1404 centroid_z += weights[i] * point_cloud_face[3 * i + 2];
1405 total_w += weights[i];
1408 centroid_x /= total_w;
1409 centroid_y /= total_w;
1410 centroid_z /= total_w;
1413 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1414 M[(
unsigned int)i][0] = weights[i] * (point_cloud_face[3 * i] - centroid_x);
1415 M[(
unsigned int)i][1] = weights[i] * (point_cloud_face[3 * i + 1] - centroid_y);
1416 M[(
unsigned int)i][2] = weights[i] * (point_cloud_face[3 * i + 2] - centroid_z);
1425 double smallestSv = W[0];
1426 unsigned int indexSmallestSv = 0;
1427 for (
unsigned int i = 1; i < W.
size(); i++) {
1428 if (W[i] < smallestSv) {
1430 indexSmallestSv = i;
1434 normal = V.
getCol(indexSmallestSv);
1437 double A = normal[0], B = normal[1], C = normal[2];
1438 double D = -(A * centroid_x + B * centroid_y + C * centroid_z);
1441 plane_equation_estimated[0] = A;
1442 plane_equation_estimated[1] = B;
1443 plane_equation_estimated[2] = C;
1444 plane_equation_estimated[3] = D;
1449 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1450 residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] +
1451 C * point_cloud_face[3 * i + 2] + D) /
1452 sqrt(A * A + B * B + C * C);
1453 error += residues[i] * residues[i];
1455 error /= sqrt(error / total_w);
1459 tukey.MEstimator(residues, weights, 1e-4);
1462 centroid.
resize(3,
false);
1463 double total_w = 0.0;
1465 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1466 centroid[0] += weights[i] * point_cloud_face[3 * i];
1467 centroid[1] += weights[i] * point_cloud_face[3 * i + 1];
1468 centroid[2] += weights[i] * point_cloud_face[3 * i + 2];
1469 total_w += weights[i];
1472 centroid[0] /= total_w;
1473 centroid[1] /= total_w;
1474 centroid[2] /= total_w;
1477 double A = normal[0], B = normal[1], C = normal[2];
1478 double D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
1481 plane_equation_estimated[0] = A;
1482 plane_equation_estimated[1] = B;
1483 plane_equation_estimated[2] = C;
1484 plane_equation_estimated[3] = D;
1502 if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
1503 dz <= std::numeric_limits<double>::epsilon())
1515 (*it)->setCameraParameters(camera);
1525 (*it)->useScanLine = v;
vpFeatureEstimationType m_featureEstimationMethod
Method to estimate the desired features.
void svd(vpColVector &w, vpMatrix &V)
Used to indicate that a value is not in the allowed range.
Implementation of a matrix and operations on matrices.
void getRoiClipped(const vpCameraParameters &cam, std::vector< vpImagePoint > &roi)
Implements a 3D polygon with render functionnalities like clipping.
void computeVisibilityDisplay()
void setVisible(bool _isvisible)
unsigned int getWidth() const
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
Implementation of an homogeneous matrix and operations on such kind of matrices.
bool m_isTrackedDepthNormalFace
std::list< int > Lindex_polygon
Index of the faces which contain the line.
void setFarClippingDistance(const double &dist)
int m_pclPlaneEstimationMethod
PCL plane estimation method.
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
unsigned int m_clippingFlag
Flags specifying which clipping to used.
Class to define colors available for display functionnalities.
int m_pclPlaneEstimationRansacMaxIter
PCL pane estimation max number of iterations.
vpPoint * p1
The first extremity.
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)
double get_oY() const
Get the point Y coordinate in the object frame.
error that can be emited by ViSP classes.
vpMbScanLine & getMbScanLineRenderer()
Manage the line of a polygon used in the model-based tracker.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
unsigned int size() const
Return the number of elements of the 2D array.
double get_y() const
Get the point y coordinate in the image plane.
double m_distNearClip
Distance for near clipping.
void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces< vpMbtPolygon > *const faces, int polygon=-1, std::string name="")
void set_X(const double X)
Set the point X coordinate in the camera frame.
vpMbtPolygon & getPolygon()
bool m_useScanLine
Scan line visibility.
vpCameraParameters m_cam
Camera intrinsic parameters.
static bool inMask(const vpImage< bool > *mask, const unsigned int i, const unsigned int j)
Class that defines what is a point.
void computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height, std::vector< vpImagePoint > &roiPts)
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
void setScanLineVisibilityTest(const bool v)
bool computePolygonCentroid(const std::vector< vpPoint > &points, vpPoint ¢roid)
void setCameraParameters(const vpCameraParameters &camera)
bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud, vpColVector &desired_features, const unsigned int stepX, const unsigned int stepY, const vpImage< bool > *mask=NULL)
bool m_faceActivated
True if the face should be considered by the tracker.
vpMbtPolygon * m_polygon
Polygon defining the face.
Defines a generic 2D polygon.
vpRect getBoundingBox() const
vpColVector & normalize()
void set_Z(const double Z)
Set the point Z coordinate in the camera frame.
void computeInteractionMatrix(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &features)
vpPoint * p2
The second extremity.
void changeFrame(const vpHomogeneousMatrix &cMo)
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)
static double sqr(double x)
bool computeDesiredFeaturesPCL(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &point_cloud_face, vpColVector &desired_features, vpColVector &desired_normal, vpColVector ¢roid_point)
VISP_EXPORT bool checkSSE2()
void getPolygonClipped(std::vector< std::pair< vpPoint, unsigned int > > &poly)
vpMbHiddenFaces< vpMbtPolygon > * m_hiddenFace
Pointer to the list of faces.
Generic class defining intrinsic camera parameters.
vpColVector getCol(const unsigned int j) const
double get_oZ() const
Get the point Z coordinate in the object frame.
void setIndex(const unsigned int i)
void set_Y(const double Y)
Set the point Y coordinate in the camera frame.
double get_x() const
Get the point x coordinate in the image plane.
vpPoint m_faceDesiredNormal
Face (normalized) normal (computed from the sensor)
virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), const vpImage< unsigned char > &I=vpImage< unsigned char >())
double m_distFarClip
Distance for near clipping.
double get_Y() const
Get the point Y coordinate in the camera frame.
std::vector< vpMbtDistanceLine * > m_listOfFaceLines
void displayFeature(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const double scale=0.05, const unsigned int thickness=1)
void setClipping(const unsigned int &flags)
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
std::vector< PolygonLine > m_polygonLines
double get_Z() const
Get the point Z coordinate in the camera frame.
vpFaceCentroidType m_faceCentroidMethod
Method to compute the face centroid for the current features.
void setName(const std::string &line_name)
void setCameraParameters(const vpCameraParameters &camera)
double get_oX() const
Get the point X coordinate in the object frame.
void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector< std::pair< vpPoint, vpPoint > > &lines, const bool &displayResults=false)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
void estimatePlaneEquationSVD(const std::vector< double > &point_cloud_face, const vpHomogeneousMatrix &cMo, vpColVector &plane_equation_estimated, vpColVector ¢roid)
void computeDesiredFeaturesSVD(const std::vector< double > &point_cloud_face, const vpHomogeneousMatrix &cMo, vpColVector &desired_features, vpColVector &desired_normal, vpColVector ¢roid_point)
void computeDesiredNormalAndCentroid(const vpHomogeneousMatrix &cMo, const vpColVector &desired_normal, const vpColVector ¢roid_point)
Implementation of column vector and the associated operations.
static double dotProd(const vpColVector &a, const vpColVector &b)
void setRight(double pos)
vpHomogeneousMatrix inverse() const
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
bool isVisible(const unsigned int i)
void addPolygon(const int &index)
void setNearClippingDistance(const double &dist)
unsigned int getHeight() const
vpPoint m_faceDesiredCentroid
Desired centroid (computed from the sensor)
Defines a rectangle in the plane.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Compute the geometric centroid.
virtual ~vpMbtFaceDepthNormal()
vpPlane m_planeObject
Plane equation described in the object frame.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
void estimateFeatures(const std::vector< double > &point_cloud_face, const vpHomogeneousMatrix &cMo, vpColVector &x_estimated, std::vector< double > &weights)
double get_X() const
Get the point X coordinate in the camera frame.
void setBottom(double pos)
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
bool useScanLine
Use scanline rendering.
void buildFrom(vpPoint &_p1, vpPoint &_p2)
static const vpColor blue
void computeNormalVisibility(const double nx, const double ny, const double nz, const vpColVector ¢roid_point, vpColVector &face_normal)
double m_pclPlaneEstimationRansacThreshold
PCL plane estimation RANSAC threshold.
void resize(const unsigned int i, const bool flagNullify=true)
void computeFov(const unsigned int &w, const unsigned int &h)