12 #include <mrpt/3rdparty/do_opencv_includes.h>
28 #include <Eigen/Dense>
50 const CImage& img,
const CImage& patch_img,
size_t& x_max,
size_t& y_max,
51 double& max_val,
int x_search_ini,
int y_search_ini,
int x_search_size,
58 (x_search_ini < 0 || y_search_ini < 0 || x_search_size < 0 ||
77 const int patch_w = patch_im.
getWidth();
82 x_search_size = im_w - patch_w;
83 y_search_size = im_h - patch_h;
87 if ((x_search_ini + x_search_size + patch_w) > im_w)
88 x_search_size -= (x_search_ini + x_search_size + patch_w) - im_w;
90 if ((y_search_ini + y_search_size + patch_h) > im_h)
91 y_search_size -= (y_search_ini + y_search_size + patch_h) - im_h;
93 ASSERT_((x_search_ini + x_search_size + patch_w) <= im_w);
94 ASSERT_((y_search_ini + y_search_size + patch_h) <= im_h);
95 CImage img_region_to_search;
104 img_region_to_search,
107 patch_w + x_search_size,
108 patch_h + y_search_size);
111 cv::Mat result(cvSize(x_search_size + 1, y_search_size + 1), CV_32FC1);
120 cv::Point min_point, max_point;
123 result, &mini, &max_val, &min_point, &max_point, cv::noArray());
124 x_max = max_point.x + x_search_ini + (
mrpt::round(patch_w - 1) >> 1);
125 y_max = max_point.y + y_search_ini + (
mrpt::round(patch_h - 1) >> 1);
128 THROW_EXCEPTION(
"The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
142 res.
x = xy.
x -
A(0, 2);
143 res.
y = xy.
y -
A(1, 2);
147 const double u = res.
norm();
158 const double focalLengthX,
const double focalLengthY,
const double centerX,
159 const double centerY)
163 A(0, 0) = focalLengthX;
164 A(1, 1) = focalLengthY;
177 unsigned int camIndex,
unsigned int resX,
unsigned int resY)
179 float fx, fy, cx, cy;
202 "Unknown camera index!! for 'camIndex'=%u", camIndex);
207 resX * fx, resY * fy, resX * cx, resY * cy);
220 TMatchingPairList::const_iterator it;
222 for (it = feat_list.begin(); it != feat_list.end(); it++)
224 err.
x = it->other_x - (it->this_x * mat(0, 0) + it->this_y * mat(0, 1) +
225 it->this_z * mat(0, 2) + Rt.
x());
226 err.
y = it->other_y - (it->this_x * mat(1, 0) + it->this_y * mat(1, 1) +
227 it->this_z * mat(1, 2) + Rt.
y());
228 err.
z = it->other_z - (it->this_x * mat(2, 0) + it->this_y * mat(2, 1) +
229 it->this_z * mat(2, 2) + Rt.z());
234 return (acum / feat_list.size());
251 if (itLand1->ID == itLand2->ID)
256 pair.
this_x = itLand1->pose_mean.x;
257 pair.
this_y = itLand1->pose_mean.y;
258 pair.
this_z = itLand1->pose_mean.z;
260 pair.
other_x = itLand2->pose_mean.x;
261 pair.
other_y = itLand2->pose_mean.y;
262 pair.
other_z = itLand2->pose_mean.z;
264 outList.push_back(pair);
272 const CImage& image,
unsigned int x,
unsigned int y)
275 float orientation = 0;
276 if ((
int(x) - 1 >= 0) && (
int(y) - 1 >= 0) && (x + 1 < image.
getWidth()) &&
278 orientation = (float)atan2(
279 (
double)*image(x, y + 1) - (double)*image(x, y - 1),
280 (double)*image(x + 1, y) - (double)*image(x - 1, y));
305 for (
int k1 = 0; k1 < (int)nim.
cols(); ++k1)
306 for (
int k2 = 0; k2 < (int)nim.
rows(); ++k2)
307 nim(k2, k1) = (im(k2, k1) - m) / s;
325 size_t sz1 = list1.
size(), sz2 = list2.
size();
327 ASSERT_((sz1 > 0) && (sz2 > 0));
344 double minSAD1, minSAD2;
346 vector<int> idxLeftList, idxRightList;
349 vector<double> distCorrs(sz1);
351 int minLeftIdx = 0, minRightIdx;
355 for (lFeat = 0, itList1 = list1.
begin(); itList1 != list1.
end();
373 for (rFeat = 0, itList2 = list2.
begin(); itList2 != list2.
end();
381 d = itList1->keypoint.pt.y - itList2->keypoint.pt.y;
389 itList2->keypoint.pt.x, itList2->keypoint.pt.y);
392 p(0, 0) = itList1->keypoint.pt.x;
393 p(1, 0) = itList1->keypoint.pt.y;
398 epiLine.
coefs[0] = l(0, 0);
399 epiLine.
coefs[1] = l(1, 0);
400 epiLine.
coefs[2] = l(2, 0);
412 ? itList1->keypoint.pt.x - itList2->keypoint.pt.x > 0
423 itList1->descriptors.hasDescriptorSIFT() &&
424 itList2->descriptors.hasDescriptorSIFT());
427 distDesc = itList1->descriptorSIFTDistanceTo(*itList2);
430 if (distDesc < minDist1)
437 else if (distDesc < minDist2)
450 itList1->patchSize > 0 && itList2->patchSize > 0);
452 *itList1->patch, *itList2->patch, u, v, res);
462 else if (res > maxCC2)
472 itList1->descriptors.hasDescriptorSURF() &&
473 itList2->descriptors.hasDescriptorSURF());
476 distDesc = itList1->descriptorSURFDistanceTo(*itList2);
479 if (distDesc < minDist1)
486 else if (distDesc < minDist2)
496 itList1->descriptors.hasDescriptorORB() &&
497 itList2->descriptors.hasDescriptorORB());
498 distDesc = itList1->descriptorORBDistanceTo(*itList2);
501 if (distDesc < minDist1)
508 else if (distDesc < minDist2)
518 itList1->patchSize > 0 &&
519 itList2->patchSize == itList1->patchSize);
522 "MRPT has been compiled without OpenCV");
531 for (
unsigned int ii = 0; ii < h; ++ii)
532 for (
unsigned int jj = 0; jj < w; ++jj)
535 aux1.
at<uint8_t>(jj, ii)) -
537 aux2.
at<uint8_t>(jj, ii)));
538 res = res / (255.0 * w * h);
547 else if (res < minSAD2)
556 bool cond1 =
false, cond2 =
false;
564 cond2 = (minDist1 / minDist2) <
571 cond2 = (maxCC2 / maxCC1) <
581 (minDist1 / minDist2) <
587 cond2 = (minSAD1 / minSAD2) < options.
SAD_RATIO;
602 int auxIdx = idxRightList[minRightIdx];
605 if (distCorrs[auxIdx] > minVal)
608 distCorrs[minLeftIdx] = minVal;
609 idxLeftList[minLeftIdx] = minRightIdx;
610 idxRightList[minRightIdx] = minLeftIdx;
612 distCorrs[auxIdx] = 1.0;
618 idxRightList[minRightIdx] = minLeftIdx;
619 idxLeftList[minLeftIdx] = minRightIdx;
620 distCorrs[minLeftIdx] = minVal;
631 for (
int vCnt = 0; vCnt < (int)idxLeftList.size(); ++vCnt)
635 std::pair<CFeature, CFeature> thisMatch;
638 double dp1 = -1.0, dp2 = -1.0;
643 list1[vCnt], list2[idxLeftList[vCnt]], p3D,
params);
644 dp1 = sqrt(p3D.
x * p3D.
x + p3D.
y * p3D.
y + p3D.
z * p3D.
z);
647 p3D.
y * p3D.
y + p3D.
z * p3D.
z);
657 thisMatch.first = list1[vCnt];
658 thisMatch.second = list2[idxLeftList[vCnt]];
663 idLeft = thisMatch.first.keypoint.ID;
664 idRight = thisMatch.second.keypoint.ID;
668 keep_max(idLeft, thisMatch.first.keypoint.ID);
671 keep_max(idRight, thisMatch.second.keypoint.ID);
678 thisMatch.first.initialDepth = dp1;
679 thisMatch.first.p3D = p3D;
681 thisMatch.second.initialDepth = dp2;
682 thisMatch.second.p3D =
687 matches.push_back(thisMatch);
691 return matches.size();
709 int hwsize = (int)(0.5 * wSize);
710 int mx = mask1.
cols(), my = mask1.
rows();
713 CMatchedFeatureList::const_iterator it;
714 for (it = mList.begin(); it != mList.end(); ++it)
716 for (
int ii = -hwsize; ii < hwsize; ++ii)
717 for (
int jj = -hwsize; jj < hwsize; ++jj)
719 idx = (int)(it->first.keypoint.pt.x) + ii;
720 idy = (int)(it->first.keypoint.pt.y) + jj;
721 if (idx >= 0 && idy >= 0 && idx < mx && idy < my)
722 mask1(idy, idx) =
false;
725 for (
int ii = -hwsize; ii < hwsize; ++ii)
726 for (
int jj = -hwsize; jj < hwsize; ++jj)
728 idx = (int)(it->second.keypoint.pt.x) + ii;
729 idy = (int)(it->second.keypoint.pt.y) + jj;
730 if (idx >= 0 && idy >= 0 && idx < mx && idy < my)
731 mask2(idy, idx) =
false;
743 for (
unsigned int ii = 0; ii < h; ++ii)
744 for (
unsigned int jj = 0; jj < w; ++jj)
746 static_cast<double>(p1.
at<uint8_t>(jj, ii)) -
747 static_cast<double>(p2.
at<uint8_t>(jj, ii)));
749 return res / (255.0 * w * h);
752 "MRPT compiled without OpenCV, can't compute SAD of images!");
761 for (
const auto& it : theList)
763 it.keypoint.pt.x - 5, it.keypoint.pt.y - 5, it.keypoint.pt.x + 5,
764 it.keypoint.pt.y + 5,
TColor(255, 0, 0));
775 out_points.reserve(matches.size());
776 for (
const auto& match : matches)
778 const auto& pt1 = match.first.keypoint.pt;
779 const auto& pt2 = match.second.keypoint.pt;
781 const double disp = pt1.x - pt2.x;
782 if (disp < 1)
continue;
785 out_points.emplace_back(
798 vP3D.reserve(leftList.
size());
800 for (it1 = leftList.
begin(), it2 = rightList.
begin(); it1 != leftList.
end();
817 const double f0 = 600;
821 const double x = nfx1 * f0;
822 const double y = nfy1 * f0;
823 const double xd = nfx2 * f0;
824 const double yd = nfy2 * f0;
826 const double f2 = f0 * f0;
827 const double p9 = f2 *
params.F(2, 2);
833 double Jh = (std::numeric_limits<double>::max)();
845 const double p1 = (xh * xhd + xhd * xt + xh * xtd) *
params.F(0, 0);
846 const double p2 = (xh * yhd + yhd * xt + xh * ytd) *
params.F(0, 1);
847 const double p3 = (f0 * (xh + xt)) *
params.F(0, 2);
848 const double p4 = (yh * xhd + xhd * yt + yh * xtd) *
params.F(1, 0);
849 const double p5 = (yh * yhd + yhd * yt + yh * ytd) *
params.F(1, 1);
850 const double p6 = (f0 * (yh + yt)) *
params.F(1, 2);
851 const double p7 = (f0 * (xhd + xtd)) *
params.F(2, 0);
852 const double p8 = (f0 * (yhd + ytd)) *
params.F(2, 1);
854 const double udotxi = p1 + p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9;
857 (xh * xh + xhd * xhd) *
params.F(0, 0) *
params.F(0, 0);
859 (xh * xh + yhd * yhd) *
params.F(0, 1) *
params.F(0, 1);
861 (yh * yh + xhd * xhd) *
params.F(1, 0) *
params.F(1, 0);
863 (yh * yh + yhd * yhd) *
params.F(1, 1) *
params.F(1, 1);
864 const double Q12 = xhd * yhd *
params.F(0, 0) *
params.F(0, 1);
865 const double Q13 = f0 * xhd *
params.F(0, 0) *
params.F(0, 2);
866 const double Q14 = xh * yh *
params.F(0, 0) *
params.F(1, 0);
867 const double Q17 = f0 * xh *
params.F(0, 0) *
params.F(2, 0);
868 const double Q23 = f0 * yhd *
params.F(0, 1) *
params.F(0, 2);
869 const double Q25 = xh * yh *
params.F(0, 1) *
params.F(1, 1);
870 const double Q28 = f0 * xh *
params.F(0, 1) *
params.F(2, 1);
871 const double Q45 = xhd * yhd *
params.F(1, 0) *
params.F(1, 1);
872 const double Q46 = f0 * xhd *
params.F(1, 0) *
params.F(1, 2);
873 const double Q47 = f0 * yh *
params.F(1, 0) *
params.F(2, 0);
874 const double Q56 = f0 * yhd *
params.F(1, 1) *
params.F(1, 2);
875 const double Q58 = f0 * yh *
params.F(1, 1) *
params.F(2, 1);
877 const double udotV0xiu = Q00 + Q11 + Q22 + Q44 + Q55 +
878 2.0 * (Q12 + Q13 + Q14 + Q17 + Q23 + Q25 +
879 Q28 + Q45 + Q46 + Q47 + Q56 + Q58);
881 ASSERT_(fabs(udotV0xiu) > 1e-5);
883 const double C = udotxi / udotV0xiu;
894 const double Jt = xt * xt + yt * yt + xtd * xtd + ytd * ytd;
896 if ((std::abs)(Jt - Jh) <= 1e-5)
915 double disp = nfx1 - nfx2;
916 double aux =
params.baseline / disp;
917 p3D.
x = (nfx1 -
params.K(0, 2)) * aux;
918 p3D.
y = (nfy1 -
params.K(1, 2)) * aux;
937 for (
auto itList = mfList.begin(); itList != mfList.end();)
939 const auto& pt1 = itList->first.keypoint.pt;
940 const auto& pt2 = itList->second.keypoint.pt;
942 const float disp = pt1.x - pt2.x;
947 itList = mfList.erase(itList);
951 float x3D = (pt1.x - param.
K(0, 2)) * ((param.
baseline)) / disp;
952 float y3D = (pt1.y - param.
K(1, 2)) * ((param.
baseline)) / disp;
953 float z3D = (param.
K(0, 0)) * ((param.
baseline)) / disp;
956 if ((z3D < param.
minZ) || (z3D > param.
maxZ))
958 itList = mfList.erase(itList);
968 norm3D *= -1 / norm3D.
norm();
972 lm.
ID = itList->first.keypoint.ID;
986 float foc2 =
square(param.
K(0, 0));
987 float c0 = param.
K(0, 2);
988 float r0 = param.
K(1, 2);
990 float disp2 =
square(disp);
993 stdPixel2 * base2 / disp2 +
996 (pt1.y - r0) /
square(disp2);
998 (pt1.x - c0) /
square(disp2);
1000 stdPixel2 * base2 / disp2 +
1003 (pt1.y - r0) /
square(disp2);
1011 unsigned int Na = 3;
1016 float w0 = k / (Na + k);
1017 float w1 = 1 / (2 * (Na + k));
1033 B.resize(2 * Na + 1);
1056 for (i = 1; i <= 2 * Na; i++)
1063 myPoint[0] = meanA[0] + vAux[0];
1064 myPoint[1] = meanA[1] + vAux[1];
1065 myPoint[2] = meanA[2] + vAux[2];
1070 myPoint[0] = meanA[0] - vAux[0];
1071 myPoint[1] = meanA[1] - vAux[1];
1072 myPoint[2] = meanA[2] - vAux[2];
1076 x3D = (myPoint[0] - param.
K(0, 2)) *
1078 y3D = (myPoint[1] - param.
K(1, 2)) *
1080 z3D = (param.
K(0, 0)) * ((param.
baseline)) / myPoint[2];
1083 meanB.
x = meanB.
x + w1 * x3D;
1084 meanB.
y = meanB.
y + w1 * y3D;
1085 meanB.
z = meanB.
z + w1 * z3D;
1094 for (i = 0; i <= 2 * Na; i++)
1102 v(0, 0) = B[i].x - meanB.
x;
1103 v(1, 0) = B[i].y - meanB.
y;
1104 v(2, 0) = B[i].z - meanB.
z;
1123 unsigned int Na = 3;
1130 float lambda =
square(a) * (Na + k) - Na;
1132 float w0_m = lambda / (Na + lambda);
1133 float w0_c = w0_m + (1 -
square(a) + b);
1134 float w1 = 1 / (2 * (Na + lambda));
1140 Pa(0, 0) = Pa(1, 1) =
1151 B.resize(2 * Na + 1);
1167 meanB.
x = w0_m * x3D;
1168 meanB.
y = w0_m * y3D;
1169 meanB.
z = w0_m * z3D;
1174 for (i = 1; i <= 2 * Na; i++)
1181 myPoint = meanA + vAux;
1188 vAux = L.
row((i - Na) - 1);
1189 myPoint = meanA - vAux;
1196 x3D = (myPoint[0] - param.
K(0, 2)) *
1198 y3D = (myPoint[1] - param.
K(1, 2)) *
1200 z3D = (param.
K(0, 0)) * ((param.
baseline)) / myPoint[2];
1203 meanB.
x = meanB.
x + w1 * x3D;
1204 meanB.
y = meanB.
y + w1 * y3D;
1205 meanB.
z = meanB.
z + w1 * z3D;
1214 for (i = 0; i <= 2 * Na; i++)
1222 v(0, 0) = B[i].x - meanB.
x;
1223 v(1, 0) = B[i].y - meanB.
y;
1224 v(2, 0) = B[i].z - meanB.
z;
1267 for (itListL = leftList.
begin(), itListR = rightList.
begin();
1268 itListL != leftList.
end();)
1270 const auto& ptL = itListL->keypoint.pt;
1271 const auto& ptR = itListR->keypoint.pt;
1273 float disp = ptL.x - ptR.x;
1276 itListL = leftList.
erase(itListL);
1277 itListR = rightList.
erase(itListR);
1282 float x3D = (ptL.x - param.
K(0, 2)) * param.
baseline / disp;
1283 float y3D = (ptL.y - param.
K(1, 2)) * param.
baseline / disp;
1284 float z3D = (param.
K(0, 0)) * param.
baseline / disp;
1287 if ((z3D < param.
minZ) || (z3D > param.
maxZ))
1289 itListL = leftList.
erase(itListL);
1290 itListR = rightList.
erase(itListR);
1300 norm3D *= -1. / norm3D.
norm();
1304 lm.
ID = itListL->keypoint.ID;
1319 float foc2 =
square(param.
K(0, 0));
1320 float c0 = param.
K(0, 2);
1321 float r0 = param.
K(1, 2);
1323 float disp2 =
square(ptL.x - ptR.x);
1326 stdPixel2 * base2 / disp2 +
1328 lm.
pose_cov_12 = stdDisp2 * base2 * (ptL.x - c0) *
1329 (ptL.y - r0) /
square(disp2);
1331 (ptL.x - c0) /
square(disp2);
1333 stdPixel2 * base2 / disp2 +
1336 (ptL.y - r0) /
square(disp2);
1344 unsigned int Na = 3;
1349 float w0 = k / (Na + k);
1350 float w1 = 1 / (2 * (Na + k));
1366 B.resize(2 * Na + 1);
1389 for (i = 1; i <= 2 * Na; i++)
1395 myPoint[0] = meanA[0] + vAux[0];
1396 myPoint[1] = meanA[1] + vAux[1];
1397 myPoint[2] = meanA[2] + vAux[2];
1401 vAux = L.
col((i - Na) - 1);
1402 myPoint[0] = meanA[0] - vAux[0];
1403 myPoint[1] = meanA[1] - vAux[1];
1404 myPoint[2] = meanA[2] - vAux[2];
1408 x3D = (myPoint[0] - param.
K(0, 2)) *
1410 y3D = (myPoint[1] - param.
K(1, 2)) *
1412 z3D = (param.
K(0, 0)) * ((param.
baseline)) / myPoint[2];
1415 meanB.
x = meanB.
x + w1 * x3D;
1416 meanB.
y = meanB.
y + w1 * y3D;
1417 meanB.
z = meanB.
z + w1 * z3D;
1426 for (i = 0; i <= 2 * Na; i++)
1434 v(0, 0) = B[i].x - meanB.
x;
1435 v(1, 0) = B[i].y - meanB.
y;
1436 v(2, 0) = B[i].z - meanB.
z;
1455 unsigned int Na = 3;
1462 float lambda =
square(a) * (Na + k) - Na;
1464 float w0_m = lambda / (Na + lambda);
1465 float w0_c = w0_m + (1 -
square(a) + b);
1466 float w1 = 1 / (2 * (Na + lambda));
1472 Pa(0, 0) = Pa(1, 1) =
1483 B.resize(2 * Na + 1);
1499 meanB.
x = w0_m * x3D;
1500 meanB.
y = w0_m * y3D;
1501 meanB.
z = w0_m * z3D;
1506 for (i = 1; i <= 2 * Na; i++)
1511 vAux = L.
row(i - 1);
1512 myPoint = meanA + vAux;
1516 vAux = L.
col((i - Na) - 1);
1517 myPoint = meanA - vAux;
1521 x3D = (myPoint[0] - param.
K(0, 2)) *
1523 y3D = (myPoint[1] - param.
K(1, 2)) *
1525 z3D = (param.
K(0, 0)) * ((param.
baseline)) / myPoint[2];
1528 meanB.
x = meanB.
x + w1 * x3D;
1529 meanB.
y = meanB.
y + w1 * y3D;
1530 meanB.
z = meanB.
z + w1 * z3D;
1539 for (i = 0; i <= 2 * Na; i++)
1547 v(0, 0) = B[i].x - meanB.
x;
1548 v(1, 0) = B[i].y - meanB.
y;
1549 v(2, 0) = B[i].z - meanB.
z;
1581 const CPose3D& sensorPose,
const vector<double>& sg,
1585 double f = intrinsicParams(0, 0);
1586 double x0 = intrinsicParams(0, 2);
1587 double y0 = intrinsicParams(1, 2);
1588 double b = baseline;
1589 double sg_c2 =
square(sg[0]);
1590 double sg_r2 =
square(sg[1]);
1591 double sg_d2 =
square(sg[2]);
1593 for (
const auto& inMatche : inMatches)
1595 double x = inMatche.first.keypoint.pt.x;
1596 double y = inMatche.first.keypoint.pt.y;
1598 double d = x - inMatche.second.keypoint.pt.x;
1600 double k =
square(b / d);
1605 double X = (x - x0) * b / d;
1606 double Y = (y - y0) * b / d;
1607 double Z = f * b / d;
1618 m.
yaw = atan2(Y, X);
1632 aux(0, 0) = k * (sg_c2 + sg_d2 *
square(x - x0) / d2);
1633 aux(0, 1) = aux(1, 0) = k * (sg_d2 * (x - x0) * (y - y0) / d2);
1634 aux(0, 2) = aux(2, 0) = k * (sg_d2 * (x - x0) * f / d2);
1636 aux(1, 1) = k * (sg_r2 + sg_d2 *
square(y - y0) / d2);
1637 aux(1, 2) = aux(2, 1) = k * (sg_d2 * (y - y0) * f / d2);
1639 aux(2, 2) = k * (sg_d2 *
square(f) / d2);
1645 JG(0, 0) = X / m.
range;
1646 JG(0, 1) = Y / m.
range;
1647 JG(0, 2) = Z / m.
range;
1680 unsigned int id = 0;
1700 double sg_c2 =
square(sg[0]);
1701 double sg_r2 =
square(sg[1]);
1702 double sg_d2 =
square(sg[2]);
1704 for (
auto itMatchList = matchList.begin(); itMatchList != matchList.end();
1705 itMatchList++,
id++)
1707 double x = itMatchList->first.keypoint.pt.x;
1708 double y = itMatchList->first.keypoint.pt.y;
1710 double d = x - itMatchList->second.keypoint.pt.x;
1712 double k =
square(b / d);
1717 double X = (x - x0) * b / d;
1718 double Y = (y - y0) * b / d;
1719 double Z = f * b / d;
1732 m.
yaw = atan2(X, Z);
1733 m.
pitch = atan2(Y, Z);
1745 aux(0, 0) = k * (sg_c2 + sg_d2 *
square(x - x0) / d2);
1746 aux(0, 1) = aux(1, 0) = k * (sg_d2 * (x - x0) * (y - y0) / d2);
1747 aux(0, 2) = aux(2, 0) = k * (sg_d2 * (x - x0) * f / d2);
1749 aux(1, 1) = k * (sg_r2 + sg_d2 *
square(y - y0) / d2);
1750 aux(1, 2) = aux(2, 1) = k * (sg_d2 * (y - y0) * f / d2);
1752 aux(2, 2) = k * (sg_d2 *
square(f) / d2);
1758 JG(0, 0) = X / m.
range;
1759 JG(0, 1) = Y / m.
range;
1760 JG(0, 2) = Z / m.
range;
1803 square(itCloud->pose_mean.x) +
square(itCloud->pose_mean.y) +
1804 square(itCloud->pose_mean.z));
1808 m.
yaw = atan2(itCloud->pose_mean.y, itCloud->pose_mean.x);
1809 m.
pitch = -sin(itCloud->pose_mean.z / m.
range);
1821 const poses::CPose3D& rightCameraPose,
void* outMap1x,
void* outMap1y,
1822 void* outMap2x,
void* outMap2y)
1828 cv::Mat *mapx1, *mapy1, *mapx2, *mapy2;
1829 mapx1 =
static_cast<cv::Mat*
>(outMap1x);
1830 mapy1 =
static_cast<cv::Mat*
>(outMap1y);
1831 mapx2 =
static_cast<cv::Mat*
>(outMap2x);
1832 mapy2 =
static_cast<cv::Mat*
>(outMap2y);
1834 const int resX = cam1.
ncols;
1835 const int resY = cam1.
nrows;
1841 rcTrans[0] = hMatrix(0, 3);
1842 rcTrans[1] = hMatrix(1, 3);
1843 rcTrans[2] = hMatrix(2, 3);
1846 for (
unsigned int i = 0; i < 3; ++i)
1847 for (
unsigned int j = 0; j < 3; ++j) m1[i][j] = hMatrix(i, j);
1849 double ipl[3][3], ipr[3][3], dpl[5], dpr[5];
1850 for (
unsigned int i = 0; i < 3; ++i)
1851 for (
unsigned int j = 0; j < 3; ++j)
1857 for (
unsigned int i = 0; i < 5; ++i)
1859 dpl[i] = cam1.
dist[i];
1860 dpr[i] = cam2.
dist[i];
1863 cv::Mat
R(3, 3, CV_64F, &m1);
1864 cv::Mat T(3, 1, CV_64F, &rcTrans);
1866 cv::Mat K1(3, 3, CV_64F, ipl);
1867 cv::Mat K2(3, 3, CV_64F, ipr);
1868 cv::Mat D1(1, 5, CV_64F, dpl);
1869 cv::Mat D2(1, 5, CV_64F, dpr);
1871 double _R1[3][3], _R2[3][3], _P1[3][4], _P2[3][4], _Q[4][4];
1872 cv::Mat R1(3, 3, CV_64F, _R1);
1873 cv::Mat R2(3, 3, CV_64F, _R2);
1874 cv::Mat P1(3, 4, CV_64F, _P1);
1875 cv::Mat P2(3, 4, CV_64F, _P2);
1876 cv::Mat Q(4, 4, CV_64F, _Q);
1878 cv::Size nSize(resX, resY);
1884 K1, D1, K2, D2, nSize,
R, T, R1, R2, P1, P2, Q,
1885 cv::CALIB_ZERO_DISPARITY, alpha);
1889 cv::initUndistortRectifyMap(
1890 K1, D1, R1, P1, cv::Size(resX, resY), CV_32FC1, *mapx1, *mapy1);
1891 cv::initUndistortRectifyMap(
1892 K2, D2, R2, P2, cv::Size(resX, resY), CV_32FC1, *mapx2, *mapy2);
1915 unc =
iniFile.read_int(section.c_str(),
"uncPropagation", uncPropagation);
1919 uncPropagation = Prop_Linear;
1922 uncPropagation = Prop_UT;
1925 uncPropagation = Prop_SUT;
1932 for (
unsigned int ii = 0; ii < 3; ++ii)
1933 for (
unsigned int jj = 0; jj < 3; ++jj) K(ii, jj) = k_vec[ii * 3 + jj];
1938 for (
unsigned int ii = 0; ii < 3; ++ii)
1939 for (
unsigned int jj = 0; jj < 3; ++jj) F(ii, jj) = f_vec[ii * 3 + jj];
1941 baseline =
iniFile.read_float(section.c_str(),
"baseline", baseline);
1942 stdPixel =
iniFile.read_float(section.c_str(),
"stdPixel", stdPixel);
1943 stdDisp =
iniFile.read_float(section.c_str(),
"stdDisp", stdDisp);
1944 maxZ =
iniFile.read_float(section.c_str(),
"maxZ", maxZ);
1945 minZ =
iniFile.read_float(section.c_str(),
"minZ", minZ);
1946 maxY =
iniFile.read_float(section.c_str(),
"maxY", maxY);
1947 factor_k =
iniFile.read_float(section.c_str(),
"factor_k", factor_k);
1948 factor_a =
iniFile.read_float(section.c_str(),
"factor_a", factor_a);
1949 factor_b =
iniFile.read_float(section.c_str(),
"factor_b", factor_b);
1957 out <<
"\n----------- [vision::TStereoSystemParams] ------------ \n";
1958 out <<
"Method for 3D Uncert. \t= ";
1959 switch (uncPropagation)
1962 out <<
"Linear propagation\n";
1965 out <<
"Unscented Transform\n";
1968 out <<
"Scaled Unscented Transform\n";
1972 out <<
mrpt::format(
"K\t\t\t= [%f\t%f\t%f]\n", K(0, 0), K(0, 1), K(0, 2));
1973 out <<
mrpt::format(
" \t\t\t [%f\t%f\t%f]\n", K(1, 0), K(1, 1), K(1, 2));
1974 out <<
mrpt::format(
" \t\t\t [%f\t%f\t%f]\n", K(2, 0), K(2, 1), K(2, 2));
1976 out <<
mrpt::format(
"F\t\t\t= [%f\t%f\t%f]\n", F(0, 0), F(0, 1), F(0, 2));
1977 out <<
mrpt::format(
" \t\t\t [%f\t%f\t%f]\n", F(1, 0), F(1, 1), F(1, 2));
1978 out <<
mrpt::format(
" \t\t\t [%f\t%f\t%f]\n", F(2, 0), F(2, 1), F(2, 2));
1990 out <<
"-------------------------------------------------------- \n";
2007 iniFile.read_int(section.c_str(),
"matching_method", matching_method);
2011 matching_method = mmCorrelation;
2014 matching_method = mmDescriptorSIFT;
2017 matching_method = mmDescriptorSURF;
2020 matching_method = mmSAD;
2023 matching_method = mmDescriptorORB;
2027 useEpipolarRestriction =
iniFile.read_bool(
2028 section.c_str(),
"useEpipolarRestriction", useEpipolarRestriction);
2029 hasFundamentalMatrix =
iniFile.read_bool(
2030 section.c_str(),
"hasFundamentalMatrix", hasFundamentalMatrix);
2031 parallelOpticalAxis =
iniFile.read_bool(
2032 section.c_str(),
"parallelOpticalAxis", parallelOpticalAxis);
2034 iniFile.read_bool(section.c_str(),
"useXRestriction", useXRestriction);
2035 addMatches =
iniFile.read_bool(section.c_str(),
"addMatches", addMatches);
2036 useDisparityLimits =
iniFile.read_bool(
2037 section.c_str(),
"useDisparityLimits", useDisparityLimits);
2039 min_disp =
iniFile.read_float(section.c_str(),
"min_disp", min_disp);
2040 max_disp =
iniFile.read_float(section.c_str(),
"max_disp", max_disp);
2043 iniFile.read_float(section.c_str(),
"epipolar_TH", epipolar_TH);
2044 maxEDD_TH =
iniFile.read_float(section.c_str(),
"maxEDD_TH", maxEDD_TH);
2045 EDD_RATIO =
iniFile.read_float(section.c_str(),
"minDIF_TH", EDD_RATIO);
2046 minCC_TH =
iniFile.read_float(section.c_str(),
"minCC_TH", minCC_TH);
2047 minDCC_TH =
iniFile.read_float(section.c_str(),
"minDCC_TH", minDCC_TH);
2048 rCC_TH =
iniFile.read_float(section.c_str(),
"rCC_TH", rCC_TH);
2049 maxEDSD_TH =
iniFile.read_float(section.c_str(),
"maxEDSD_TH", maxEDSD_TH);
2050 EDSD_RATIO =
iniFile.read_float(section.c_str(),
"EDSD_RATIO", EDSD_RATIO);
2051 maxSAD_TH =
iniFile.read_float(section.c_str(),
"maxSAD_TH", maxSAD_TH);
2052 SAD_RATIO =
iniFile.read_float(section.c_str(),
"SAD_RATIO", SAD_RATIO);
2054 iniFile.read_float(section.c_str(),
"maxORB_dist", maxORB_dist);
2057 iniFile.read_bool(section.c_str(),
"estimateDepth", estimateDepth);
2058 maxDepthThreshold =
iniFile.read_float(
2059 section.c_str(),
"maxDepthThreshold", maxDepthThreshold);
2072 out <<
"\n----------- [vision::TMatchingOptions] ------------ \n";
2073 out <<
"Matching method: ";
2074 switch (matching_method)
2077 out <<
"Cross Correlation\n";
2079 "· Min. CC. Threshold: %f\n", minCC_TH);
2081 "· Min. Dif. CC Threshold: %f\n", minDCC_TH);
2084 case mmDescriptorSIFT:
2085 out <<
"SIFT descriptor\n";
2087 "· Max. EDD Threshold: %f\n", maxEDD_TH);
2089 "· EDD Ratio: %f\n", EDD_RATIO);
2091 case mmDescriptorSURF:
2092 out <<
"SURF descriptor\n";
2094 "· EDD Ratio: %f\n", maxEDSD_TH);
2096 "· Min. CC Threshold: %f\n", EDSD_RATIO);
2101 "· Max. Dif. SAD Threshold: %f\n", maxSAD_TH);
2103 "· Ratio SAD Threshold: %f\n", SAD_RATIO);
2105 case mmDescriptorORB:
2108 "· Max. distance between desc: %f\n", maxORB_dist);
2112 "Epipolar Thres: %.2f px\n", epipolar_TH);
2113 out <<
"Using epipolar restriction?: "
2114 << (useEpipolarRestriction ?
"Yes\n" :
"No\n");
2115 out <<
"Has Fundamental Matrix?: "
2116 << (hasFundamentalMatrix ?
"Yes\n" :
"No\n");
2117 out <<
"Are camera axis parallel?: "
2118 << (parallelOpticalAxis ?
"Yes\n" :
"No\n");
2119 out <<
"Use X-coord restriction?: "
2120 << (useXRestriction ?
"Yes\n" :
"No\n");
2121 out <<
"Use disparity limits?: "
2122 << (useDisparityLimits ?
"Yes\n" :
"No\n");
2123 if (useDisparityLimits)
2125 "· Min/max disp limits: %.2f/%.2f px\n", min_disp,
2127 out <<
"Estimate depth?: "
2128 << (estimateDepth ?
"Yes\n" :
"No\n");
2132 "· Maximum depth allowed: %f m\n", maxDepthThreshold);
2134 out <<
"Add matches to list?: ";
2135 out << (addMatches ?
"Yes\n" :
"No\n");
2136 out <<
"-------------------------------------------------------- \n";