12 #include <mrpt/3rdparty/do_opencv_includes.h>
30 #include <unordered_map>
66 res = res * 31 + hash<mrpt::img::TCamera>()(k.
calib);
68 for (
unsigned int i = 0; i < 6; i++)
69 res = res * 31 + hash<double>()(k.
sensorPose[i]);
75 static std::unordered_map<LUT_info, CObservation3DRangeScan::unproject_LUT_t>
80 CObservation3DRangeScan::get_unproj_lut()
const
85 linfo.
calib = this->cameraParams;
99 ASSERT_EQUAL_(rangeImage.cols(),
static_cast<int>(cameraParams.ncols));
100 ASSERT_EQUAL_(rangeImage.rows(),
static_cast<int>(cameraParams.nrows));
103 unsigned int H = cameraParams.nrows, W = cameraParams.ncols;
104 const size_t WH = W * H;
105 if (ret.
Kxs.size() == WH)
return ret;
113 lut.Kxs_rot.resize(WH);
114 lut.Kys_rot.resize(WH);
115 lut.Kzs_rot.resize(WH);
118 cv::Mat pts(1, WH, CV_32FC2), undistort_pts(1, WH, CV_32FC2);
120 const auto& intrMat = cameraParams.intrinsicParams;
121 const auto& dist = cameraParams.dist;
122 cv::Mat cv_distortion(
123 1, dist.size(), CV_64F,
const_cast<double*
>(&dist[0]));
124 cv::Mat cv_intrinsics(3, 3, CV_64F);
125 for (
int i = 0; i < 3; i++)
126 for (
int j = 0; j < 3; j++)
127 cv_intrinsics.at<
double>(i, j) = intrMat(i, j);
129 for (
unsigned int r = 0; r < H; r++)
130 for (
unsigned int c = 0; c < W; c++)
132 auto& p = pts.at<cv::Vec2f>(r * W + c);
137 cv::undistortPoints(pts, undistort_pts, cv_intrinsics, cv_distortion);
142 ASSERT_EQUAL_(undistort_pts.size().area(),
static_cast<int>(WH));
143 undistort_pts.reshape(WH);
145 float* kxs = &lut.Kxs[0];
146 float* kys = &lut.Kys[0];
147 float* kzs = &lut.Kzs[0];
148 float* kxs_rot = &lut.Kxs_rot[0];
149 float* kys_rot = &lut.Kys_rot[0];
150 float* kzs_rot = &lut.Kzs_rot[0];
152 for (
size_t idx = 0; idx < WH; idx++)
154 const auto& p = undistort_pts.at<cv::Vec2f>(idx);
155 const float c = p[0], r = p[1];
161 if (!this->range_is_depth) v *= 1.0f / v.norm();
164 auto v_rot = sensorPose.rotateVector(v);
170 *kxs_rot++ = v_rot.x;
171 *kys_rot++ = v_rot.y;
172 *kzs_rot++ = v_rot.z;
182 void CObservation3DRangeScan::EXTERNALS_AS_TEXT(
bool value)
186 bool CObservation3DRangeScan::EXTERNALS_AS_TEXT()
192 #define COBS3DRANGE_USE_MEMPOOL
198 #ifdef COBS3DRANGE_USE_MEMPOOL
215 std::vector<float> pts_x, pts_y,
pts_z;
231 return H == req.
H && W == req.
W;
290 CObservation3DRangeScan::~CObservation3DRangeScan()
292 #ifdef COBS3DRANGE_USE_MEMPOOL
298 uint8_t CObservation3DRangeScan::serializeGetVersion()
const {
return 10; }
299 void CObservation3DRangeScan::serializeTo(
303 out << maxRange << sensorPose;
308 points3D_x.size() == points3D_y.size() &&
309 points3D_x.size() == points3D_z.size() &&
310 points3D_idxs_x.size() == points3D_x.size() &&
311 points3D_idxs_y.size() == points3D_x.size());
312 uint32_t N = points3D_x.size();
316 out.WriteBufferFixEndianness(&points3D_x[0], N);
317 out.WriteBufferFixEndianness(&points3D_y[0], N);
318 out.WriteBufferFixEndianness(&points3D_z[0], N);
319 out.WriteBufferFixEndianness(&points3D_idxs_x[0], N);
320 out.WriteBufferFixEndianness(&points3D_idxs_y[0], N);
324 out << hasRangeImage;
328 out.WriteAs<uint32_t>(rangeImage.rows());
329 out.WriteAs<uint32_t>(rangeImage.cols());
330 if (rangeImage.size() != 0)
331 out.WriteBufferFixEndianness<uint16_t>(
332 rangeImage.data(), rangeImage.size());
334 out << hasIntensityImage;
335 if (hasIntensityImage)
out << intensityImage;
336 out << hasConfidenceImage;
337 if (hasConfidenceImage)
out << confidenceImage;
339 out << cameraParamsIntensity;
340 out << relativePoseIntensityWRTDepth;
347 out << m_points3D_external_stored << m_points3D_external_file;
348 out << m_rangeImage_external_stored << m_rangeImage_external_file;
351 out << range_is_depth;
354 out.WriteAs<int8_t>(intensityImageChannel);
357 out << hasPixelLabels();
358 if (hasPixelLabels())
360 pixelLabels->writeToStream(
out);
364 out.WriteAs<uint8_t>(rangeImageOtherLayers.size());
365 for (
const auto& kv : rangeImageOtherLayers)
368 const auto& ri = kv.second;
369 out.WriteAs<uint32_t>(ri.cols());
370 out.WriteAs<uint32_t>(ri.rows());
372 out.WriteBufferFixEndianness<uint16_t>(ri.data(), ri.size());
376 void CObservation3DRangeScan::serializeFrom(
395 in >> maxRange >> sensorPose;
405 resizePoints3DVectors(N);
415 vector<char> validRange(N);
417 &validRange[0],
sizeof(validRange[0]) * N);
428 this->resizePoints3DVectors(0);
446 const uint32_t rows = ri.
rows(), cols = ri.cols();
450 rangeImage_setSize(rows, cols);
452 for (uint32_t r = 0; r < rows; r++)
453 for (uint32_t c = 0; c < cols; c++)
454 rangeImage(r, c) =
static_cast<uint16_t
>(
459 const uint32_t rows = in.
ReadAs<uint32_t>();
460 const uint32_t cols = in.
ReadAs<uint32_t>();
463 rangeImage_setSize(rows, cols);
466 if (rangeImage.size() != 0)
468 rangeImage.data(), rangeImage.size());
472 in >> hasIntensityImage;
473 if (hasIntensityImage) in >> intensityImage;
475 in >> hasConfidenceImage;
476 if (hasConfidenceImage) in >> confidenceImage;
484 in >> cameraParamsIntensity >>
485 relativePoseIntensityWRTDepth;
489 cameraParamsIntensity = cameraParams;
490 relativePoseIntensityWRTDepth =
CPose3D();
502 in >> m_points3D_external_stored >> m_points3D_external_file;
503 in >> m_rangeImage_external_stored >>
504 m_rangeImage_external_file;
508 m_points3D_external_stored =
false;
509 m_rangeImage_external_stored =
false;
514 in >> range_is_depth;
518 range_is_depth =
true;
529 intensityImageChannel = CH_VISIBLE;
538 in >> do_have_labels;
542 TPixelLabelInfoBase::readAndBuildFromStream(in));
545 rangeImageOtherLayers.clear();
548 const auto numLayers = in.
ReadAs<uint8_t>();
549 for (
size_t i = 0; i < numLayers; i++)
553 auto& ri = rangeImageOtherLayers[name];
555 const uint32_t rows = in.
ReadAs<uint32_t>();
556 const uint32_t cols = in.
ReadAs<uint32_t>();
557 ri.resize(rows, cols);
560 ri.data(), ri.size());
565 if (hasRangeImage && !rangeImage_isExternallyStored() &&
566 (
static_cast<int>(cameraParams.ncols) != rangeImage.cols() ||
567 static_cast<int>(cameraParams.nrows) != rangeImage.rows()))
569 std::cerr <<
"[CObservation3DRangeScan] Warning: autofixing "
570 "incorrect camera resolution in TCamera:"
571 << cameraParams.ncols <<
"x" << cameraParams.nrows
572 <<
" => " << rangeImage.cols() <<
"x"
573 << rangeImage.rows() <<
"\n";
574 cameraParams.ncols = rangeImage.cols();
575 cameraParams.nrows = rangeImage.rows();
586 CObservation::swap(o);
623 void CObservation3DRangeScan::load()
const
625 if (hasPoints3D && m_points3D_external_stored)
627 const string fil = points3D_getExternalStorageFileAbsolutePath();
634 const auto N = M.
cols();
636 auto& xs =
const_cast<std::vector<float>&
>(points3D_x);
637 auto& ys =
const_cast<std::vector<float>&
>(points3D_y);
638 auto& zs =
const_cast<std::vector<float>&
>(points3D_z);
650 f >>
const_cast<std::vector<float>&
>(points3D_x) >>
651 const_cast<std::vector<float>&
>(points3D_y) >>
652 const_cast<std::vector<float>&
>(points3D_z);
656 if (hasRangeImage && m_rangeImage_external_stored)
658 for (
size_t idx = 0; idx < 1 + rangeImageOtherLayers.size(); idx++)
660 std::string layerName;
666 auto it = rangeImageOtherLayers.
begin();
667 std::advance(it, idx - 1);
668 layerName = it->first;
672 rangeImage_getExternalStorageFileAbsolutePath(layerName);
684 const uint32_t rows = f.ReadAs<uint32_t>();
685 const uint32_t cols = f.ReadAs<uint32_t>();
686 me.rangeImage_setSize(rows, cols);
688 f.ReadBufferFixEndianness<uint16_t>(ri->
data(), ri->
size());
695 void CObservation3DRangeScan::unload()
697 if (hasPoints3D && m_points3D_external_stored)
704 if (hasRangeImage && m_rangeImage_external_stored) rangeImage.setSize(0, 0);
706 intensityImage.unload();
707 confidenceImage.unload();
710 std::string CObservation3DRangeScan::rangeImage_getExternalStorageFile(
711 const std::string& rangeImageLayer)
const
713 std::string filName = m_rangeImage_external_file;
714 if (!rangeImageLayer.empty())
718 filName, std::string(
"layer_") + rangeImageLayer + curExt);
723 void CObservation3DRangeScan::rangeImage_getExternalStorageFileAbsolutePath(
724 std::string& out_path,
const std::string& rangeImageLayer)
const
726 std::string filName = rangeImage_getExternalStorageFile(rangeImageLayer);
729 if (filName[0] ==
'/' || (filName[1] ==
':' && filName[2] ==
'\\'))
735 out_path = CImage::getImagesPathBase();
736 size_t N = CImage::getImagesPathBase().size() - 1;
737 if (CImage::getImagesPathBase()[N] !=
'/' &&
738 CImage::getImagesPathBase()[N] !=
'\\')
743 void CObservation3DRangeScan::points3D_getExternalStorageFileAbsolutePath(
744 std::string& out_path)
const
746 ASSERT_(m_points3D_external_file.size() > 2);
747 if (m_points3D_external_file[0] ==
'/' ||
748 (m_points3D_external_file[1] ==
':' &&
749 m_points3D_external_file[2] ==
'\\'))
751 out_path = m_points3D_external_file;
755 out_path = CImage::getImagesPathBase();
756 size_t N = CImage::getImagesPathBase().size() - 1;
757 if (CImage::getImagesPathBase()[N] !=
'/' &&
758 CImage::getImagesPathBase()[N] !=
'\\')
760 out_path += m_points3D_external_file;
764 void CObservation3DRangeScan::points3D_convertToExternalStorage(
765 const std::string& fileName_,
const std::string& use_this_base_dir)
767 ASSERT_(!points3D_isExternallyStored());
769 points3D_x.size() == points3D_y.size() &&
770 points3D_x.size() == points3D_z.size());
773 m_points3D_external_file =
776 m_points3D_external_file =
781 const string savedDir = CImage::getImagesPathBase();
782 CImage::setImagesPathBase(use_this_base_dir);
783 const string real_absolute_path =
784 points3D_getExternalStorageFileAbsolutePath();
785 CImage::setImagesPathBase(savedDir);
789 const size_t nPts = points3D_x.size();
802 f << points3D_x << points3D_y << points3D_z;
805 m_points3D_external_stored =
true;
814 void CObservation3DRangeScan::rangeImage_convertToExternalStorage(
815 const std::string& fileName_,
const std::string& use_this_base_dir)
817 ASSERT_(!rangeImage_isExternallyStored());
819 m_rangeImage_external_file =
822 m_rangeImage_external_file =
827 const string savedDir = CImage::getImagesPathBase();
828 CImage::setImagesPathBase(use_this_base_dir);
830 for (
size_t idx = 0; idx < 1 + rangeImageOtherLayers.size(); idx++)
832 std::string layerName;
838 auto it = rangeImageOtherLayers.
begin();
839 std::advance(it, idx - 1);
840 layerName = it->first;
843 const string real_absolute_path =
844 rangeImage_getExternalStorageFileAbsolutePath(layerName);
855 f.WriteAs<uint32_t>(ri->
rows());
856 f.WriteAs<uint32_t>(ri->
cols());
858 f.WriteBufferFixEndianness<uint16_t>(ri->
data(), ri->
size());
862 m_rangeImage_external_stored =
true;
863 rangeImage.setSize(0, 0);
865 CImage::setImagesPathBase(savedDir);
869 #define CALIB_DECIMAT 15
878 : obs(obs_), z_offset(z_offset_)
892 for (
size_t i = 0; i < 4; i++) x[4 + i] = camPar.
dist[i];
901 for (
size_t i = 0; i < 4; i++) camPar.
dist[i] = x[4 + i];
920 const size_t idx = nC * r + c;
931 const double x = P.
x / P.
z;
932 const double y = P.
y / P.
z;
936 const double r4 =
square(r2);
942 2 *
params.dist[2] * x * y +
948 2 *
params.dist[3] * x * y +
967 double CObservation3DRangeScan::recoverCameraCalibrationParameters(
969 const double camera_offset)
980 TMyLevMar::TResultInfo info;
998 increments_x.
fill(1e-4);
1008 1e-3, 1e-9, 1e-9,
false);
1010 const double avr_px_err =
1013 out_camParams.
ncols = nC;
1014 out_camParams.
nrows = nR;
1023 void CObservation3DRangeScan::getZoneAsObs(
1025 const unsigned int& r2,
const unsigned int& c1,
const unsigned int& c2)
1027 unsigned int cols = cameraParams.ncols;
1028 unsigned int rows = cameraParams.nrows;
1030 ASSERT_((r1 < r2) && (c1 < c2));
1031 ASSERT_((r2 < rows) && (c2 < cols));
1042 if (hasIntensityImage)
1043 intensityImage.extract_patch(
1048 if (hasConfidenceImage)
1049 confidenceImage.extract_patch(
1067 for (
unsigned int i = r1; i < r2; i++)
1068 for (
unsigned int j = c1; j < c2; j++)
1070 obs.
points3D_x.push_back(points3D_x.at(cols * i + j));
1071 obs.
points3D_y.push_back(points3D_y.at(cols * i + j));
1072 obs.
points3D_z.push_back(points3D_z.at(cols * i + j));
1085 void CObservation3DRangeScan::resizePoints3DVectors(
const size_t WH)
1087 #ifdef COBS3DRANGE_USE_MEMPOOL
1099 if (WH <= points3D_x.size())
1101 points3D_x.resize(WH);
1102 points3D_y.resize(WH);
1103 points3D_z.resize(WH);
1104 points3D_idxs_x.resize(WH);
1105 points3D_idxs_y.resize(WH);
1121 points3D_x.swap(mem_block->
pts_x);
1122 points3D_y.swap(mem_block->
pts_y);
1123 points3D_z.swap(mem_block->
pts_z);
1124 points3D_idxs_x.swap(mem_block->
idxs_x);
1125 points3D_idxs_y.swap(mem_block->
idxs_y);
1133 points3D_x.resize(WH);
1134 points3D_y.resize(WH);
1135 points3D_z.resize(WH);
1136 points3D_idxs_x.resize(WH);
1137 points3D_idxs_y.resize(WH);
1140 size_t CObservation3DRangeScan::getScanSize()
const
1143 return points3D_x.size();
1148 void CObservation3DRangeScan::rangeImage_setSize(
const int H,
const int W)
1150 bool ri_done = rangeImage.cols() == W && rangeImage.rows() == H;
1152 #ifdef COBS3DRANGE_USE_MEMPOOL
1155 if (pool && !ri_done)
1174 if (!ri_done) rangeImage.setSize(H, W);
1177 for (
auto& layer : rangeImageOtherLayers) layer.second.setSize(H, W);
1182 bool CObservation3DRangeScan::doDepthAndIntensityCamerasCoincide()
const
1184 static const double EPSILON = 1e-7;
1187 return (relativePoseIntensityWRTDepth.m_coords.array().abs() < EPSILON)
1190 relativePoseIntensityWRTDepth.getRotationMatrix())
1196 void CObservation3DRangeScan::convertTo2DScan(
1203 if (!this->hasRangeImage)
1209 const size_t nCols = this->rangeImage.cols();
1210 const size_t nRows = this->rangeImage.rows();
1230 const double cx = this->cameraParams.cx();
1231 const double cy = this->cameraParams.cy();
1232 const double fx = this->cameraParams.fx();
1233 const double fy = this->cameraParams.fy();
1236 const double real_FOV_left = atan2(cx, fx);
1237 const double real_FOV_right = atan2(nCols - 1 - cx, fx);
1240 const float FOV_equiv =
1241 mrpt::d2f(2 * std::max(real_FOV_left, real_FOV_right));
1252 out_scan2d.
maxRange = this->maxRange;
1269 std::vector<float> vert_ang_tan(nRows);
1270 for (
size_t r = 0; r < nRows; r++) vert_ang_tan[r] =
d2f((cy - r) / fy);
1280 double ang = -FOV_equiv * 0.5;
1281 const double A_ang = FOV_equiv / (nLaserRays - 1);
1290 for (
size_t i = 0; i < nLaserRays; i++, ang += A_ang)
1293 const double tan_ang = tan(ang);
1295 const size_t c = std::min(
1296 static_cast<size_t>(std::max(0.0, cx + fx * tan_ang)),
1299 bool any_valid =
false;
1300 float closest_range = out_scan2d.
maxRange;
1302 for (
size_t r = 0; r < nRows; r++)
1304 const float D = rangeImage.coeff(r, c) * rangeUnits;
1308 const float this_point_tan = vert_ang_tan[r] * D;
1309 if (this_point_tan > tan_min && this_point_tan < tan_max)
1322 closest_range * std::sqrt(1.0 + tan_ang * tan_ang)));
1339 const std::vector<mrpt::math::TPoint3Df>& pts = pc->getArrayPoints();
1340 const size_t N = pts.size();
1342 const double A_ang = FOV_equiv / (nLaserRays - 1);
1343 const double ang0 = -FOV_equiv * 0.5;
1345 for (
size_t i = 0; i < N; i++)
1347 if (pts[i].z < sp.
z_min || pts[i].z > sp.
z_max)
continue;
1349 const double phi_wrt_origin = atan2(pts[i].y, pts[i].x);
1351 int i_range =
mrpt::round((phi_wrt_origin - ang0) / A_ang);
1352 if (i_range < 0 || i_range >=
int(nLaserRays))
continue;
1354 const float r_wrt_origin = ::hypotf(pts[i].x, pts[i].y);
1362 void CObservation3DRangeScan::getDescriptionAsText(std::ostream& o)
const
1364 CObservation::getDescriptionAsText(o);
1369 o <<
"Homogeneous matrix for the sensor's 3D pose, relative to robot "
1371 o << sensorPose.getHomogeneousMatrixVal<
CMatrixDouble44>() << sensorPose
1374 o <<
"maxRange = " << maxRange <<
" [meters]"
1376 o <<
"rangeUnits = " << rangeUnits <<
" [meters]"
1379 o <<
"Has 3D point cloud? ";
1382 o <<
"YES: " << points3D_x.
size() <<
" points";
1383 if (points3D_isExternallyStored())
1384 o <<
". External file: " << points3D_getExternalStorageFile()
1394 o <<
"Range is depth: " << (range_is_depth ?
"YES" :
"NO") <<
"\n";
1395 o <<
"Has raw range data? " << (hasRangeImage ?
"YES" :
"NO");
1398 if (rangeImage_isExternallyStored())
1399 o <<
". External file: " << rangeImage_getExternalStorageFile(
"");
1401 o <<
" (embedded).";
1405 for (
auto& layer : rangeImageOtherLayers)
1407 o <<
"Additional rangeImage layer: '" << layer.first <<
"'";
1408 if (rangeImage_isExternallyStored())
1409 o <<
". External file: " << rangeImage_getExternalStorageFile(
"");
1411 o <<
" (embedded).";
1416 <<
"Has intensity data? " << (hasIntensityImage ?
"YES" :
"NO");
1417 if (hasIntensityImage)
1419 if (intensityImage.isExternallyStored())
1420 o <<
". External file: " << intensityImage.getExternalStorageFile()
1423 o <<
" (embedded).\n";
1425 o <<
"Source channel: "
1428 value2name(intensityImageChannel)
1433 <<
"Has confidence data? " << (hasConfidenceImage ?
"YES" :
"NO");
1434 if (hasConfidenceImage)
1436 if (confidenceImage.isExternallyStored())
1437 o <<
". External file: " << confidenceImage.getExternalStorageFile()
1445 <<
"Has pixel labels? " << (hasPixelLabels() ?
"YES" :
"NO");
1446 if (hasPixelLabels())
1448 o <<
" Human readable labels:"
1450 for (
auto it = pixelLabels->pixelLabelNames.begin();
1451 it != pixelLabels->pixelLabelNames.end(); ++it)
1452 o <<
" label[" << it->first <<
"]: '" << it->second <<
"'"
1458 o <<
"Depth camera calibration parameters:"
1462 cameraParams.saveToConfigFile(
"DEPTH_CAM_PARAMS", cfg);
1466 <<
"Intensity camera calibration parameters:"
1470 cameraParamsIntensity.saveToConfigFile(
"INTENSITY_CAM_PARAMS", cfg);
1475 <<
"Pose of the intensity cam. wrt the depth cam:\n"
1476 << relativePoseIntensityWRTDepth <<
"\n"
1477 << relativePoseIntensityWRTDepth
1482 T3DPointsTo2DScanParams::T3DPointsTo2DScanParams()
1485 z_min(-std::numeric_limits<double>::max()),
1486 z_max(std::numeric_limits<double>::max())
1498 const cv::Mat distortion(
1500 const cv::Mat intrinsics(
1506 const cv::Mat newIntrinsics = cv::getOptimalNewCameraMatrix(
1507 intrinsics, distortion, imgSize, alpha);
1512 const cv::Mat R_eye = cv::Mat::eye(3, 3, CV_32FC1);
1516 cv::initUndistortRectifyMap(
1517 intrinsics, distortion, R_eye, newIntrinsics, imgSize, CV_32FC1, m1,
1528 std::advance(it, idx - 1);
1531 cv::Mat rangeImg(ri->
rows(), ri->
cols(), CV_16UC1, ri->
data());
1534 cv::remap(rangeImg, outRangeImg, m1, m2, cv::INTER_NEAREST);
1536 outRangeImg.copyTo(rangeImg);
1540 for (
int r = 0; r < 3; r++)
1541 for (
int c = 0; c < 3; c++)
1543 newIntrinsics.at<
double>(r, c);
1563 float rangeUnits,
const std::optional<mrpt::img::TColormap> color)
1570 const float range_inv =
rangeUnits / (val_max - val_min);
1576 const int cols = ri.
cols(), rows = ri.
rows();
1584 for (
int r = 0; r < rows; r++)
1586 for (
int c = 0; c < cols; c++)
1589 const float val_01 = (ri.
coeff(r, c) - val_min) * range_inv;
1592 img.
setPixel(c, r,
static_cast<uint8_t
>(val_01 * 255));
1602 static_cast<uint8_t
>(
R * 255),
1603 static_cast<uint8_t
>(
G * 255),
1604 static_cast<uint8_t
>(B * 255)));
1616 const std::optional<mrpt::img::TColormap> color,
1617 const std::optional<float> normMinRange,
1618 const std::optional<float> normMaxRange,
1619 const std::optional<std::string> additionalLayerName)
const
1623 (!additionalLayerName || additionalLayerName->empty())
1627 const float val_min = normMinRange.value_or(.0f);
1628 const float val_max = normMaxRange.value_or(this->
maxRange);