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
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 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
316 std::vector<std::vector<vpImagePoint> > &roiPts_vec
323 if (width == 0 || height == 0)
326 std::vector<vpImagePoint> roiPts;
330#
if DEBUG_DISPLAY_DEPTH_NORMAL
336 if (roiPts.size() <= 2) {
338 std::cerr <<
"Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
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()));
357 std::vector<double> point_cloud_face, point_cloud_face_custom;
369 double prev_x, prev_y, prev_z;
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) {
376 (
m_useScanLine ? (i < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
377 j < m_hiddenFace->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
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]);
395 prev_z = point_cloud[i * width + j][2];
398 point_cloud_face_custom.push_back(prev_x);
399 point_cloud_face_custom.push_back(x);
401 point_cloud_face_custom.push_back(prev_y);
402 point_cloud_face_custom.push_back(y);
404 point_cloud_face_custom.push_back(prev_z);
405 point_cloud_face_custom.push_back(point_cloud[i * width + j][2]);
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]);
415#if DEBUG_DISPLAY_DEPTH_NORMAL
416 debugImage[i][j] = 255;
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);
430 if (point_cloud_face.empty() && point_cloud_face_custom.empty()) {
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);
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]));
454 desired_normal, centroid_point);
1045 vpMbtTukeyEstimator<double> tukey_robust;
1046 std::vector<double> residues(point_cloud_face.size() / 3);
1048 w.resize(point_cloud_face.size() / 3, 1.0);
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;
1054 Mat33<double> ATA_3x3;
1063 while (std::fabs(error - prev_error) > 1e-6 && (iter < max_iter)) {
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);
1087 double *ptr_residues = &residues[0];
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);
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);
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];
1106 residues[cpt / 3] = (A * xi + B * yi + C - 1 / Zi);
1110 tukey_robust.MEstimator(residues, w, 1e-2);
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();
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();
1125 if (point_cloud_face.size() / 3 >= 2) {
1126 const double *ptr_point_cloud = &point_cloud_face[0];
1127 double *ptr_w = &w[0];
1129 const __m128d vones = _mm_set1_pd(1.0);
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));
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);
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));
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);
1154 _mm_storeu_pd(vtmp, vsum_wi2_xi2);
1155 double sum_wi2_xi2 = vtmp[0] + vtmp[1];
1157 _mm_storeu_pd(vtmp, vsum_wi2_yi2);
1158 double sum_wi2_yi2 = vtmp[0] + vtmp[1];
1160 _mm_storeu_pd(vtmp, vsum_wi2);
1161 double sum_wi2 = vtmp[0] + vtmp[1];
1163 _mm_storeu_pd(vtmp, vsum_wi2_xi_yi);
1164 double sum_wi2_xi_yi = vtmp[0] + vtmp[1];
1166 _mm_storeu_pd(vtmp, vsum_wi2_xi);
1167 double sum_wi2_xi = vtmp[0] + vtmp[1];
1169 _mm_storeu_pd(vtmp, vsum_wi2_yi);
1170 double sum_wi2_yi = vtmp[0] + vtmp[1];
1172 _mm_storeu_pd(vtmp, vsum_wi2_xi_Zi);
1173 double sum_wi2_xi_Zi = vtmp[0] + vtmp[1];
1175 _mm_storeu_pd(vtmp, vsum_wi2_yi_Zi);
1176 double sum_wi2_yi_Zi = vtmp[0] + vtmp[1];
1178 _mm_storeu_pd(vtmp, vsum_wi2_Zi);
1179 double sum_wi2_Zi = vtmp[0] + vtmp[1];
1181 for (; cpt < point_cloud_face.size(); cpt += 3) {
1182 double wi2 = w[cpt / 3] * w[cpt / 3];
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;
1189 sum_wi2_xi2 += wi2 * xi * xi;
1190 sum_wi2_yi2 += wi2 * yi * yi;
1192 sum_wi2_xi_yi += wi2 * xi * yi;
1193 sum_wi2_xi += wi2 * xi;
1194 sum_wi2_yi += wi2 * yi;
1196 sum_wi2_xi_Zi += wi2 * xi * invZi;
1197 sum_wi2_yi_Zi += wi2 * yi * invZi;
1198 sum_wi2_Zi += wi2 * invZi;
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;
1211 Mat33<double> minv = ATA_3x3.inverse();
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;
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);
1231 double *ptr_residues = &residues[0];
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);
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));
1242 _mm_storeu_pd(ptr_residues, tmp);
1246 _mm_storeu_pd(vtmp, verror);
1247 error = vtmp[0] + vtmp[1];
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];
1254 error +=
vpMath::sqr(A * xi + B * yi + C - 1 / Zi);
1255 residues[idx / 3] = (A * xi + B * yi + C - 1 / Zi);
1258 error /= point_cloud_face.size() / 3;
1264 while (std::fabs(error - prev_error) > 1e-6 && (iter < max_iter)) {
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];
1285 residues[i] = (A * xi + B * yi + C - 1 / Zi);
1289 tukey_robust.MEstimator(residues, w, 1e-2);
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;
1295 double sum_wi2_xi_Zi = 0.0, sum_wi2_yi_Zi = 0.0, sum_wi2_Zi = 0.0;
1297 for (
size_t i = 0; i < point_cloud_face.size() / 3; i++) {
1298 double wi2 = w[i] * w[i];
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;
1305 sum_wi2_xi2 += wi2 * xi * xi;
1306 sum_wi2_yi2 += wi2 * yi * yi;
1308 sum_wi2_xi_yi += wi2 * xi * yi;
1309 sum_wi2_xi += wi2 * xi;
1310 sum_wi2_yi += wi2 * yi;
1312 sum_wi2_xi_Zi += wi2 * xi * invZi;
1313 sum_wi2_yi_Zi += wi2 * yi * invZi;
1314 sum_wi2_Zi += wi2 * invZi;
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;
1327 Mat33<double> minv = ATA_3x3.inverse();
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;
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];
1342 error +=
vpMath::sqr(A * xi + B * yi + C - 1 / Zi);
1343 residues[i] = (A * xi + B * yi + C - 1 / Zi);
1346 error /= point_cloud_face.size() / 3;
1352 x_estimated.
resize(3,
false);
1362 unsigned int max_iter = 10;
1363 double prev_error = 1e3;
1364 double error = 1e3 - 1;
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;
1372 for (
unsigned int iter = 0; iter < max_iter && std::fabs(error - prev_error) > 1e-6; iter++) {
1374 tukey.MEstimator(residues, weights, 1e-4);
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);
1392 tukey.MEstimator(residues, weights, 1e-4);
1393 plane_equation_estimated.
resize(4,
false);
1397 double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0;
1398 double total_w = 0.0;
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];
1407 centroid_x /= total_w;
1408 centroid_y /= total_w;
1409 centroid_z /= total_w;
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);
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) {
1429 indexSmallestSv = i;
1433 normal = V.
getCol(indexSmallestSv);
1436 double A = normal[0], B = normal[1], C = normal[2];
1437 double D = -(A * centroid_x + B * centroid_y + C * centroid_z);
1440 plane_equation_estimated[0] = A;
1441 plane_equation_estimated[1] = B;
1442 plane_equation_estimated[2] = C;
1443 plane_equation_estimated[3] = D;
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];
1458 tukey.MEstimator(residues, weights, 1e-4);
1461 centroid.
resize(3,
false);
1462 double total_w = 0.0;
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];
1471 centroid[0] /= total_w;
1472 centroid[1] /= total_w;
1473 centroid[2] /= total_w;
1476 double A = normal[0], B = normal[1], C = normal[2];
1477 double D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
1480 plane_equation_estimated[0] = A;
1481 plane_equation_estimated[1] = B;
1482 plane_equation_estimated[2] = C;
1483 plane_equation_estimated[3] = D;