From d2c238aed58eee1631db4f96416094f794102d5a Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Wed, 7 Feb 2024 09:41:49 +0100 Subject: [PATCH 1/6] patch version bump --- appveyor.yml | 2 +- doc/source/doxygen-docs/changelog.md | 3 +++ package.xml | 2 +- version_prefix.txt | 2 +- 4 files changed, 6 insertions(+), 3 deletions(-) diff --git a/appveyor.yml b/appveyor.yml index 618a58a5e4..bb4d632eca 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -1,5 +1,5 @@ # version format -version: 2.11.8-{branch}-build{build} +version: 2.11.9-{branch}-build{build} os: Visual Studio 2019 diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index aaaeeee808..3447f2fcf9 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,5 +1,8 @@ \page changelog Change Log +# Version 2.11.9: UNRELEASED +(none yet) + # Version 2.11.8: Released Feb 7th, 2024 - Changes in apps: - RawLogViewer: Show pointcloud ring, intensity, and time min/max ranges. diff --git a/package.xml b/package.xml index 3df498f725..560a79ebc2 100644 --- a/package.xml +++ b/package.xml @@ -7,7 +7,7 @@ mrpt2 - 2.11.8 + 2.11.9 Mobile Robot Programming Toolkit (MRPT) version 2.x Jose-Luis Blanco-Claraco diff --git a/version_prefix.txt b/version_prefix.txt index 8e77878ebf..442f347259 100644 --- a/version_prefix.txt +++ b/version_prefix.txt @@ -1,4 +1,4 @@ -2.11.8 +2.11.9 # IMPORTANT: This file is parsed by CMake, don't add any comment to # the first line. # This file is used in both Windows and Linux scripts to automatically From 479280606e4b943e705acaaf7efeea315a359ddd Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Wed, 7 Feb 2024 09:52:35 +0100 Subject: [PATCH 2/6] fix ros1 build deps --- doc/source/doxygen-docs/changelog.md | 3 ++- package.xml | 3 +++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 3447f2fcf9..99c6f53692 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,7 +1,8 @@ \page changelog Change Log # Version 2.11.9: UNRELEASED -(none yet) +- BUG FIXES: + - Fix missing build dep on tf2_geometry_msgs for ROS 1 only. # Version 2.11.8: Released Feb 7th, 2024 - Changes in apps: diff --git a/package.xml b/package.xml index 560a79ebc2..36f540d862 100644 --- a/package.xml +++ b/package.xml @@ -72,6 +72,9 @@ roscpp rclcpp + + tf2_geometry_msgs + doxygen From dece3ef09044592f4a413ad773f7db1bfd927ddc Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Wed, 7 Feb 2024 12:57:04 +0100 Subject: [PATCH 3/6] Show errors on simplemap load failure --- libs/obs/src/CSimpleMap.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libs/obs/src/CSimpleMap.cpp b/libs/obs/src/CSimpleMap.cpp index 4c36e52b5e..b376bdaa14 100644 --- a/libs/obs/src/CSimpleMap.cpp +++ b/libs/obs/src/CSimpleMap.cpp @@ -16,6 +16,8 @@ #include #include +#include + using namespace mrpt::obs; using namespace mrpt::maps; using namespace mrpt::poses; @@ -131,8 +133,9 @@ bool CSimpleMap::loadFromFile(const std::string& filName) archiveFrom(fi) >> *this; return true; } - catch (...) + catch (const std::exception& e) { + std::cerr << "[CSimpleMap::loadFromFile]" << e.what() << std::endl; return false; } } From 9402d15fb6dfdca76cf652af0e5de5e064dacc72 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Wed, 7 Feb 2024 23:37:19 +0100 Subject: [PATCH 4/6] Fixes to CImage and stereo calibration API and GUI --- .../kinect_calibrate_guiMain.cpp | 47 ++++++++++++------- doc/source/doxygen-docs/changelog.md | 6 +++ libs/img/include/mrpt/img/CImage.h | 6 +-- libs/img/src/CImage.cpp | 2 +- .../vision/chessboard_stereo_camera_calib.h | 8 ++-- .../src/chessboard_stereo_camera_calib.cpp | 7 ++- 6 files changed, 47 insertions(+), 29 deletions(-) diff --git a/apps/kinect-stereo-calib/kinect_calibrate_guiMain.cpp b/apps/kinect-stereo-calib/kinect_calibrate_guiMain.cpp index a6a2c0df40..ea9d0d7017 100644 --- a/apps/kinect-stereo-calib/kinect_calibrate_guiMain.cpp +++ b/apps/kinect-stereo-calib/kinect_calibrate_guiMain.cpp @@ -780,7 +780,7 @@ kinect_calibrate_guiDialog::kinect_calibrate_guiDialog( cbOptK3 = new wxCheckBox( Panel12, ID_CHECKBOX6, _("k3 (r^6 dist.)"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX6")); - cbOptK3->SetValue(false); + cbOptK3->SetValue(true); FlexGridSizer35->Add( cbOptK3, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 1); FlexGridSizer35->Add( @@ -789,13 +789,13 @@ kinect_calibrate_guiDialog::kinect_calibrate_guiDialog( cbOptT1 = new wxCheckBox( Panel12, ID_CHECKBOX7, _("t1 (tang. dist.)"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX7")); - cbOptT1->SetValue(false); + cbOptT1->SetValue(true); FlexGridSizer35->Add( cbOptT1, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 1); cbOptT2 = new wxCheckBox( Panel12, ID_CHECKBOX8, _("t2 (tang. dist.)"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX8")); - cbOptT2->SetValue(false); + cbOptT2->SetValue(true); FlexGridSizer35->Add( cbOptT2, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 1); Panel12->SetSizer(FlexGridSizer35); @@ -826,7 +826,7 @@ kinect_calibrate_guiDialog::kinect_calibrate_guiDialog( Panel13, ID_CHECKBOX3, _("Pseudo-Huber robust kernel"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX3")); - cbCalibUseRobust->SetValue(false); + cbCalibUseRobust->SetValue(true); FlexGridSizer34->Add( cbCalibUseRobust, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5); @@ -1752,16 +1752,20 @@ void kinect_calibrate_guiDialog::ProcessNewSelectedImageListBox() { try { + static std::optional noImg; + if (!noImg) + { + noImg.emplace(320, 240, CH_RGB); + noImg->filledRectangle(0, 0, 319, 239, TColor(200, 200, 200)); + noImg->textOut(100, 110, "(No image selected)", TColor::white()); + } + const int sel = lbImagePairs->GetSelection(); if (sel == wxNOT_FOUND || sel >= (int)m_calib_images.size()) { - mrpt::img::CImage img(320, 240, CH_RGB); - img.filledRectangle(0, 0, 319, 239, TColor(200, 200, 200)); - img.textOut(100, 110, "(No image selected)", TColor::white()); - - this->m_view_left->AssignImage(img); - this->m_view_right->AssignImage(img); + this->m_view_left->AssignImage(*noImg); + this->m_view_right->AssignImage(*noImg); } else { @@ -1769,11 +1773,13 @@ void kinect_calibrate_guiDialog::ProcessNewSelectedImageListBox() CImage il, ir; + const auto& selPair = m_calib_images[sel]; + // Common part to all (but one) modes: if (image_mode != 1) { - il = m_calib_images[sel].left.img_original; - ir = m_calib_images[sel].right.img_original; + il = selPair.left.img_original; + ir = selPair.right.img_original; if (cbCalibNormalize->IsChecked()) { @@ -1792,8 +1798,15 @@ void kinect_calibrate_guiDialog::ProcessNewSelectedImageListBox() // ======= Detected chessboard ======= case 1: { - il = m_calib_images[sel].left.img_checkboard; - ir = m_calib_images[sel].right.img_checkboard; + if (!selPair.left.img_checkboard.isEmpty()) + il = selPair.left.img_checkboard; + else + il = *noImg; + + if (!selPair.right.img_checkboard.isEmpty()) + ir = selPair.right.img_checkboard; + else + ir = *noImg; } break; @@ -1804,11 +1817,11 @@ void kinect_calibrate_guiDialog::ProcessNewSelectedImageListBox() ir = ir.colorImage(); il.drawChessboardCorners( - m_calib_images[sel].left.projectedPoints_distorted, + selPair.left.projectedPoints_distorted, m_calib_params.check_size_x, m_calib_params.check_size_y); ir.drawChessboardCorners( - m_calib_images[sel].right.projectedPoints_distorted, + selPair.right.projectedPoints_distorted, m_calib_params.check_size_x, m_calib_params.check_size_y); } @@ -2133,7 +2146,7 @@ void kinect_calibrate_guiDialog::OnbtnSaveCalibClick(wxCommandEvent& event) c["right_camera_pose"] = m_calib_result.cam_params.rightCameraPose.asString(); - auto& r = c["calibration_results"] = mrpt::containers::yaml::Map(); + auto r = c["calibration_results"] = mrpt::containers::yaml::Map(); r["iters"] = m_calib_result.final_iters; r["good_image_pairs"] = m_calib_result.final_number_good_image_pairs; r["final_rmse"] = m_calib_result.final_rmse; diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 99c6f53692..ad283416f7 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,8 +1,14 @@ \page changelog Change Log # Version 2.11.9: UNRELEASED +- Changes in libraries: + - \ref mrpt_vision_grp: + - mrpt::vision::TStereoCalibParams::use_robust_kernel now defaults to `true`. + - \ref mrpt_img_grp: + - Fix const-correctness of arguments in mrpt::img::CImage::drawChessboardCorners() - BUG FIXES: - Fix missing build dep on tf2_geometry_msgs for ROS 1 only. + - Application kinect-stereo-calib: fix exceptions when selecting images without a good chessboard detection on the left list box. # Version 2.11.8: Released Feb 7th, 2024 - Changes in apps: diff --git a/libs/img/include/mrpt/img/CImage.h b/libs/img/include/mrpt/img/CImage.h index ffec7ac6ef..3b662d7934 100644 --- a/libs/img/include/mrpt/img/CImage.h +++ b/libs/img/include/mrpt/img/CImage.h @@ -445,9 +445,9 @@ class CImage : public mrpt::serialization::CSerializable, public CCanvas * \sa mrpt::vision::findChessboardCorners */ bool drawChessboardCorners( - std::vector& cornerCoords, unsigned int check_size_x, - unsigned int check_size_y, unsigned int lines_width = 1, - unsigned int circles_radius = 4); + const std::vector& cornerCoords, + unsigned int check_size_x, unsigned int check_size_y, + unsigned int lines_width = 1, unsigned int circles_radius = 4); /** Joins two images side-by-side horizontally. Both images must have the * same number of rows and be of the same type (i.e. depth and color mode) diff --git a/libs/img/src/CImage.cpp b/libs/img/src/CImage.cpp index a1c743b8ad..c996fc7d4b 100644 --- a/libs/img/src/CImage.cpp +++ b/libs/img/src/CImage.cpp @@ -1847,7 +1847,7 @@ void CImage::rotateImage( } bool CImage::drawChessboardCorners( - std::vector& cornerCoords, unsigned int check_size_x, + const std::vector& cornerCoords, unsigned int check_size_x, unsigned int check_size_y, unsigned int lines_width, unsigned int r) { #if MRPT_HAS_OPENCV diff --git a/libs/vision/include/mrpt/vision/chessboard_stereo_camera_calib.h b/libs/vision/include/mrpt/vision/chessboard_stereo_camera_calib.h index 37776ab8b0..8df8763eaa 100644 --- a/libs/vision/include/mrpt/vision/chessboard_stereo_camera_calib.h +++ b/libs/vision/include/mrpt/vision/chessboard_stereo_camera_calib.h @@ -80,11 +80,11 @@ struct TStereoCalibParams bool optimize_k1{true}, optimize_k2{true}, optimize_k3{false}, optimize_t1{false}, optimize_t2{false}; - /** Employ a Pseudo-Huber robustifier kernel (Default: false) */ - bool use_robust_kernel{false}; + /** Employ a Pseudo-Huber robustifier kernel (Default: true) */ + bool use_robust_kernel = true; /** The parameter of the robust kernel, in pixels (only if - * use_robust_kernel=true) (Default=10) */ - double robust_kernel_param{10}; + * use_robust_kernel=true) (Default=5) */ + double robust_kernel_param = 5; /** If set to !=NULL, this function will be called within each Lev-Marq. * iteration (don't do heavy stuff here since performance will degrade) */ diff --git a/libs/vision/src/chessboard_stereo_camera_calib.cpp b/libs/vision/src/chessboard_stereo_camera_calib.cpp index 46fa564352..7debc14d5d 100644 --- a/libs/vision/src/chessboard_stereo_camera_calib.cpp +++ b/libs/vision/src/chessboard_stereo_camera_calib.cpp @@ -149,7 +149,7 @@ bool mrpt::vision::checkerBoardStereoCalibration( if (!dat.img_original.isExternallyStored()) { // Checkboad as color image: - dat.img_original.colorImage(dat.img_checkboard); + dat.img_checkboard = dat.img_original.colorImage(); dat.img_checkboard.drawChessboardCorners( dat.detected_corners, check_size.x, check_size.y); } @@ -199,8 +199,8 @@ bool mrpt::vision::checkerBoardStereoCalibration( if (has_to_redraw_corners) { // Checkboad as color image: - images[i].right.img_original.colorImage( - images[i].right.img_checkboard); + images[i].right.img_checkboard = + images[i].right.img_original.colorImage(); images[i].right.img_checkboard.drawChessboardCorners( images[i].right.detected_corners, check_size.x, check_size.y); @@ -276,7 +276,6 @@ bool mrpt::vision::checkerBoardStereoCalibration( // Run stereo calibration in two stages: // (0) Estimate all parameters except distortion // (1) Estimate all parameters, using as starting point the guess from - // (0) // =============================================================================== size_t nUnknownsCamParams = 0; size_t iter = 0; From a9841c7af07bfc37f83960017ef0fabbe5979390 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Wed, 7 Feb 2024 23:58:52 +0100 Subject: [PATCH 5/6] more enums added to TEnumType --- doc/source/doxygen-docs/changelog.md | 2 ++ libs/obs/include/mrpt/obs/CActionRobotMovement2D.h | 5 +++++ libs/obs/include/mrpt/obs/CActionRobotMovement3D.h | 6 ++++++ 3 files changed, 13 insertions(+) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index ad283416f7..8e8781d1cd 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -6,6 +6,8 @@ - mrpt::vision::TStereoCalibParams::use_robust_kernel now defaults to `true`. - \ref mrpt_img_grp: - Fix const-correctness of arguments in mrpt::img::CImage::drawChessboardCorners() + - \ref mrpt_obs_grp: + - Added support for mrpt::typemeta::TEnumType to mrpt::obs::CActionRobotMovement3D::TDrawSampleMotionModel and mrpt::obs::CActionRobotMovement2D::TDrawSampleMotionModel. - BUG FIXES: - Fix missing build dep on tf2_geometry_msgs for ROS 1 only. - Application kinect-stereo-calib: fix exceptions when selecting images without a good chessboard detection on the left list box. diff --git a/libs/obs/include/mrpt/obs/CActionRobotMovement2D.h b/libs/obs/include/mrpt/obs/CActionRobotMovement2D.h index 06d7b368f2..2bd0d59440 100644 --- a/libs/obs/include/mrpt/obs/CActionRobotMovement2D.h +++ b/libs/obs/include/mrpt/obs/CActionRobotMovement2D.h @@ -247,3 +247,8 @@ MRPT_ENUM_TYPE_BEGIN(mrpt::obs::CActionRobotMovement2D::TEstimationMethod) MRPT_FILL_ENUM_MEMBER(mrpt::obs::CActionRobotMovement2D, emOdometry); MRPT_FILL_ENUM_MEMBER(mrpt::obs::CActionRobotMovement2D, emScan2DMatching); MRPT_ENUM_TYPE_END() + +MRPT_ENUM_TYPE_BEGIN(mrpt::obs::CActionRobotMovement2D::TDrawSampleMotionModel) +MRPT_FILL_ENUM_MEMBER(mrpt::obs::CActionRobotMovement2D, mmGaussian); +MRPT_FILL_ENUM_MEMBER(mrpt::obs::CActionRobotMovement2D, mmThrun); +MRPT_ENUM_TYPE_END() diff --git a/libs/obs/include/mrpt/obs/CActionRobotMovement3D.h b/libs/obs/include/mrpt/obs/CActionRobotMovement3D.h index c595cde9ae..f0ae028a45 100644 --- a/libs/obs/include/mrpt/obs/CActionRobotMovement3D.h +++ b/libs/obs/include/mrpt/obs/CActionRobotMovement3D.h @@ -10,6 +10,7 @@ #include #include +#include namespace mrpt::obs { @@ -125,3 +126,8 @@ class CActionRobotMovement3D : public CAction }; // End of class def. } // namespace mrpt::obs + +MRPT_ENUM_TYPE_BEGIN(mrpt::obs::CActionRobotMovement3D::TDrawSampleMotionModel) +MRPT_FILL_ENUM_MEMBER(mrpt::obs::CActionRobotMovement3D, mmGaussian); +MRPT_FILL_ENUM_MEMBER(mrpt::obs::CActionRobotMovement3D, mm6DOF); +MRPT_ENUM_TYPE_END() From 5a6c70838d194522b858913f68361251aedb7531 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 11 Feb 2024 19:00:20 +0100 Subject: [PATCH 6/6] release date --- doc/source/doxygen-docs/changelog.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 8e8781d1cd..4b75bdbab4 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,6 +1,6 @@ \page changelog Change Log -# Version 2.11.9: UNRELEASED +# Version 2.11.9: Released Feb 11th, 2024 - Changes in libraries: - \ref mrpt_vision_grp: - mrpt::vision::TStereoCalibParams::use_robust_kernel now defaults to `true`.