Merge branch 'develop'

AUTHORS

@@ -169,5 +169,8 @@ the GitHub contributors page (https://github.com/MRPT/mrpt/graphs/contributors).
 * Edward Rosten
   - Part of his libCVD (Free BSD) has been used in libs/opengl/src/gltext.cpp
 
+* Miguel Castillón, University of Girona (2022). https://miguelcastillon.github.io/ - https://github.com/miguelcastillon
+  - Added covariance mapping to se3 for GTSAM
+
 Several bug reports have been provided from MRPT users world-wide. We kindly thank all of
 them for this valuable feedback, and they are usually mentioned in changelogs.

apps/RawLogViewer/main_show_selected_object.cpp

@@ -711,6 +711,8 @@ void xRawLogViewerFrame::SelectObjectInTreeView(
 	}
 
 	myRedirector.reset();  // ensures cout is redirected to text box
+	wxTheApp->Yield(true);	// Let the app. process messages
+
 	memo->ShowPosition(0);
 }
 catch (CExceptionExternalImageNotFound& e)

apps/ptg-configurator/ptgConfiguratorMain.cpp

@@ -28,6 +28,8 @@
 #include <mrpt/system/os.h>
 #include <mrpt/system/string_utils.h>
 
+#include <fstream>
+
 #include "../wx-common/mrpt_logo.xpm"
 #include "imgs/main_icon.xpm"
 
@@ -131,6 +133,7 @@ const long ptgConfiguratorframe::idMenuAbout = wxNewId();
 const long ptgConfiguratorframe::ID_STATUSBAR1 = wxNewId();
 //*)
 const long ID_TEXTCTRL_SEL_TRAJ = wxNewId();
+const long idMenuExportPath = wxNewId();
 
 BEGIN_EVENT_TABLE(ptgConfiguratorframe, wxFrame)
 //(*EventTable(ptgConfiguratorframe)
@@ -149,6 +152,7 @@ ptgConfiguratorframe::ptgConfiguratorframe(wxWindow* parent, wxWindowID id)
 	wxFlexGridSizer* FlexGridSizer10;
 	wxFlexGridSizer* FlexGridSizer3;
 	wxMenuItem* MenuItem1;
+	wxMenuItem* mnuItemExportPath;
 	wxFlexGridSizer* FlexGridSizer9;
 	wxFlexGridSizer* FlexGridSizer2;
 	wxMenu* Menu1;
@@ -169,7 +173,7 @@ ptgConfiguratorframe::ptgConfiguratorframe(wxWindow* parent, wxWindowID id)
 		parent, wxID_ANY, _("PTG configurator - Part of the MRPT project"),
 		wxDefaultPosition, wxDefaultSize, wxDEFAULT_FRAME_STYLE,
 		_T("wxID_ANY"));
-	// SetClientSize(wxSize(893, 576));
+
 	{
 		wxIcon FrameIcon;
 		FrameIcon.CopyFromBitmap(wxArtProvider::GetBitmap(
@@ -621,10 +625,17 @@ ptgConfiguratorframe::ptgConfiguratorframe(wxWindow* parent, wxWindowID id)
 	SetSizer(FlexGridSizer1);
 	MenuBar1 = new wxMenuBar();
 	Menu1 = new wxMenu();
+
+	mnuItemExportPath = new wxMenuItem(
+		Menu1, idMenuExportPath, _("Export path to .m"),
+		_("Export selected path"), wxITEM_NORMAL);
+	Menu1->Append(mnuItemExportPath);
+
 	MenuItem1 = new wxMenuItem(
 		Menu1, idMenuQuit, _("Quit\tAlt-F4"), _("Quit the application"),
 		wxITEM_NORMAL);
 	Menu1->Append(MenuItem1);
+
 	MenuBar1->Append(Menu1, _("&File"));
 	Menu2 = new wxMenu();
 	MenuItem2 = new wxMenuItem(
@@ -667,6 +678,7 @@ ptgConfiguratorframe::ptgConfiguratorframe(wxWindow* parent, wxWindowID id)
 	Bind(wxEVT_CHECKBOX, &pcf::OnrbShowTPSelectSelect, this, ID_CHECKBOX7);
 	Bind(wxEVT_MENU, &pcf::OnQuit, this, idMenuQuit);
 	Bind(wxEVT_MENU, &pcf::OnAbout, this, idMenuAbout);
+	Bind(wxEVT_MENU, &pcf::OnExportSelectedPath, this, idMenuExportPath);
 	//*)
 
 	m_plot->Bind(wxEVT_MOTION, &pcf::Onplot3DMouseMove, this);
@@ -1455,3 +1467,104 @@ void ptgConfiguratorframe::OnrbShowTPSelectSelect(wxCommandEvent& event)
 	m_plotTPSpace->Refresh();
 	WX_END_TRY;
 }
+
+static std::string vectorToMatlab(
+	const std::string& varName, const std::vector<double>& v)
+{
+	std::string s;
+	s = varName;
+	s += "=[";
+
+	for (size_t i = 0; i < v.size(); i++)
+	{
+		s += mrpt::format("%.04g", v[i]);
+		if (i + 1 != v.size()) s += ", ";
+		if ((i % 20) == 19) s += " ...\n  ";
+	}
+
+	s += "];\n";
+
+	return s;
+}
+
+void ptgConfiguratorframe::OnExportSelectedPath(wxCommandEvent&)
+{
+	WX_START_TRY
+
+	if (!ptg) return;
+
+	wxFileDialog openFileDialog(
+		this, _("Save selected path to .m"), wxT(""), wxT(""),
+		wxT("m files (*.m)|*.m"), wxFD_SAVE | wxFD_OVERWRITE_PROMPT);
+
+	if (openFileDialog.ShowModal() == wxID_CANCEL) return;
+
+	const std::string sFil = openFileDialog.GetPath().ToStdString();
+
+	std::ofstream f(sFil.c_str());
+	if (!f.is_open())
+		THROW_EXCEPTION_FMT(
+			"Cannot open file for writting: '%s'", sFil.c_str());
+
+	f << "%\n";
+	f << "% File generated automatically by ptg-configurator, MRPT project.\n";
+	f << "%\n";
+	f << "% For PTG path 0-based index " << edIndexHighlightPath->GetValue()
+	  << " out of " << ptg->getPathCount() << " alpha="
+	  << mrpt::RAD2DEG(ptg->index2alpha(edIndexHighlightPath->GetValue()))
+	  << " [deg]\n";
+	f << "% PTG details:\n%{\n";
+
+	const std::string sKeyPrefix =
+		mrpt::format("PTG%d_", (int)edPTGIndex->GetValue());
+	const std::string sSection = "PTG_PARAMS";
+	mrpt::config::CConfigFileMemory cfg;
+	mrpt::config::CConfigFilePrefixer cfp;
+	cfp.bind(cfg);
+	cfp.setPrefixes("", sKeyPrefix);
+	const int WN = 25, WV = 30;
+	cfp.write(
+		sSection, "Type", ptg->GetRuntimeClass()->className, WN, WV,
+		"PTG C++ class name");
+	ptg->saveToConfigFile(cfp, sSection);
+
+	f << cfg.getContent();
+	f << "%}\n";
+
+	const auto& ts = m_graph_path_x->GetDataX();
+	const auto& xs = m_graph_path_x->GetDataY();
+	const auto& ys = m_graph_path_y->GetDataY();
+	const auto& phis = m_graph_path_phi->GetDataY();
+
+	const auto& vxs = m_graph_path_vx->GetDataY();
+	const auto& vys = m_graph_path_vy->GetDataY();
+	const auto& ws = m_graph_path_omega->GetDataY();
+
+	f << vectorToMatlab("t", ts);
+	f << vectorToMatlab("x", xs);
+	f << vectorToMatlab("y", ys);
+	f << vectorToMatlab("phi", phis);
+	f << vectorToMatlab("vx", vxs);
+	f << vectorToMatlab("vy", vys);
+	f << vectorToMatlab("w", ws);
+
+	f <<
+		R"XX(
+figure();
+plot(x,y,'.');
+axis equal; grid minor;
+title('XY path');
+
+figure();
+subplot(3,1,1); plot(t,x,'.'); xlabel('t [s]');title('x(t) [m]'); grid minor;
+subplot(3,1,2); plot(t,y,'.'); xlabel('t [s]');title('y(t) [m]'); grid minor;
+subplot(3,1,3); plot(t,phi,'.'); xlabel('t [s]');title('phi(t) [deg]'); grid minor;
+
+figure();
+subplot(3,1,1); plot(t,vx,'.'); xlabel('t [s]');title('vx(t) [m/s]'); grid minor;
+subplot(3,1,2); plot(t,vy,'.'); xlabel('t [s]');title('vy(t) [m/s]'); grid minor;
+subplot(3,1,3); plot(t,w,'.'); xlabel('t [s]');title('w(t) [deg/s]'); grid minor;
+)XX";
+
+	WX_END_TRY
+}

apps/ptg-configurator/ptgConfiguratorMain.h

@@ -61,6 +61,7 @@ class ptgConfiguratorframe : public wxFrame
 	//(*Handlers(ptgConfiguratorframe)
 	void OnAbout(wxCommandEvent& event);
 	void OnQuit(wxCommandEvent& event);
+	void OnExportSelectedPath(wxCommandEvent& event);
 	void OnbtnReloadParamsClick(wxCommandEvent& event);
 	void OncbPTGClassSelect(wxCommandEvent& event);
 	void OnedPTGIndexChange(wxSpinEvent& event);

apps/rosbag2rawlog/rosbag2rawlog_main.cpp

@@ -360,7 +360,7 @@ Obs toTf(tf2::BufferCore& tfBuffer, const rosbag::MessageInstance& rosmsg)
 			{
 				tfBuffer.setTransform(tf, "bagfile", isStatic);
 			}
-			catch (tf2::TransformException& ex)
+			catch (const tf2::TransformException& ex)
 			{
 				std::cerr << ex.what() << std::endl;
 			}

appveyor.yml

@@ -1,5 +1,5 @@
 # version format
-version: 2.4.9-{branch}-build{build}
+version: 2.4.10-{branch}-build{build}
 
 os: Visual Studio 2019
 

cmakemodules/script_ros.cmake

@@ -15,8 +15,9 @@ if (NOT DISABLE_ROS)
 	endif()
 
 	# ROS libs:
+	find_package(rclcpp QUIET)
 	find_package(ament_cmake QUIET)
-	if(ament_cmake_FOUND)
+	if(ament_cmake_FOUND AND rclcpp_FOUND)
 		set(MRPT_ROS_VERSION 2)
 	else()
 		find_package(roscpp QUIET)

doc/source/doxygen-docs/changelog.md

@@ -1,5 +1,22 @@
 \page changelog Change Log
 
+# Version 2.4.10: Relased June 24th, 2022
+- Changes in applications:
+  - ptg-configurator:
+    - New menu action to export selected path to matlab/octave script.
+  - RawLogViewer:
+    - Visual improvements and display of timestamps in local time too.
+- Changes in libraries:
+  - \ref mrpt_poses_grp
+    - Adds covariance mapping to SE(3) for GTSAM (Closes [#1229](https://github.com/MRPT/mrpt/issues/1229))
+  - \ref mrpt_ros1bridge_grp
+    - Import mrptToROSLoggerCallback() from the now obsolete mrpt_bridge package into mrpt::ros1bridge.
+- Build system
+  - Fix ROS version detection; select ROS2 if packages for both versions are found.
+- BUG FIXES:
+  - Fix mrpt-comms rare timeout in busy build farms.
+  - mrpt::ros1bridge and mrpt::ros2bridge were not correctly exporting the `fromROS()` function for LaserScan messages.
+
 # Version 2.4.9: Released June 7th, 2022
 - Changes in libraries
   - \ref mrpt_math_grp

libs/gui/include/mrpt/3rdparty/mathplot/mathplot.h

@@ -77,13 +77,13 @@
 // this definition uses windows dll to export function.
 // WXDLLIMPEXP_MATHPLOT definition definition changed to WXDLLIMPEXP_MATHPLOT
 // mathplot_EXPORTS will be defined by cmake
-//#ifdef mathplot_EXPORTS
+// #ifdef mathplot_EXPORTS
 // #define WXDLLIMPEXP_MATHPLOT WXEXPORT
 // #define WXDLLIMPEXP_DATA_MATHPLOT(type) WXEXPORT type
-//#else // not making DLL
+// #else // not making DLL
 // #define WXDLLIMPEXP_MATHPLOT
 // #define WXDLLIMPEXP_DATA_MATHPLOT(type) type
-//#endif
+// #endif
 
 // Hack for MRPT: Link as part of mrpt-gui itself.
 #define WXDLLIMPEXP_MATHPLOT
@@ -1551,6 +1551,9 @@ class WXDLLIMPEXP_MATHPLOT mpFXYVector : public mpFXY
 	 */
 	size_t GetDataLength() const { return m_xs.size(); }
 
+	const std::vector<double>& GetDataX() const { return m_xs; }
+	const std::vector<double>& GetDataY() const { return m_ys; }
+
 	/** Append a new data point (x,y)
 	 * @sa SetData
 	 */

libs/math/include/mrpt/math/CQuaternion.h

@@ -535,6 +535,52 @@ class CQuaternion : public CVectorFixed<T, 4>
 		return q;
 	}
 
+	/** Computes the 3x4 rotation Jacobian that maps quaternion to SE(3)
+	 * It is obtained by partial differentiation of the following
+	 * equation wrt the quaternion components (quat=$[q_r, \bm{q}_v]^T):
+	 * $\boldsymbol{\omega} =
+	 *    \frac{2\arccos{q_r}}{|\mathbf{q}_v|}\mathbf{q}_v$
+	 */
+	mrpt::math::CMatrixFixed<double, 3, 4> jacobian_rodrigues_from_quat() const
+	{
+		ASSERT_NEAR_(this->normSqr(), 1.0, 1e-5);
+
+		double qr = this->r();
+		double qx = this->x();
+		double qy = this->y();
+		double qz = this->z();
+
+		mrpt::math::CVectorFixed<double, 3> q_imaginary;
+		q_imaginary(0) = qx;
+		q_imaginary(1) = qy;
+		q_imaginary(2) = qz;
+		double q_imaginary_norm = q_imaginary.norm();
+		double q_imaginary_norm_cubic = std::pow(q_imaginary_norm, 3);
+
+		ASSERT_(1 - qr * qr >= 0.0);
+		ASSERT_(q_imaginary_norm > 0.0);
+
+		mrpt::math::CMatrixFixed<double, 3, 4> out_domega_dq;
+		double denom_inv = 2.0 / (q_imaginary_norm * std::sqrt(1 - qr * qr));
+		out_domega_dq(0, 0) = -qx * denom_inv;
+		out_domega_dq(1, 0) = qy * denom_inv;
+		out_domega_dq(2, 0) = -qz * denom_inv;
+
+		denom_inv = 2.0 * acos(qr) / q_imaginary_norm_cubic;
+		out_domega_dq(0, 1) = (qy * qy + qz * qz) * denom_inv;
+		out_domega_dq(0, 2) = (-qx * qy) * denom_inv;
+		out_domega_dq(0, 3) = (-qx * qz) * denom_inv;
+		out_domega_dq(1, 2) = (qx * qx + qz * qz) * denom_inv;
+		out_domega_dq(1, 3) = (-qy * qz) * denom_inv;
+		out_domega_dq(2, 3) = (qy * qy + qy * qy) * denom_inv;
+
+		out_domega_dq(1, 1) = out_domega_dq(0, 2);
+		out_domega_dq(2, 1) = out_domega_dq(0, 3);
+		out_domega_dq(2, 2) = out_domega_dq(1, 3);
+
+		return out_domega_dq;
+	}
+
 };	// end class
 
 /** A quaternion of data type "double" */

libs/math/include/mrpt/math/gtsam_wrappers.h

@@ -9,11 +9,9 @@
 #pragma once
 
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Pose3.h>
 #include <gtsam/navigation/NavState.h>	// Velocity3
 #include <mrpt/math/CMatrixFixed.h>
 #include <mrpt/math/TPoint3D.h>
-#include <mrpt/math/TPose3D.h>
 #include <mrpt/math/TTwist3D.h>
 
 namespace mrpt::gtsam_wrappers
@@ -60,7 +58,7 @@ static mrpt::math::CMatrixDouble66 to_mrpt_se3_cov6(
 	return C;
 }
 
-static gtsam::Matrix6 to_gtsam_se3_cov6(
+static gtsam::Matrix6 to_gtsam_se3_cov6_reordering(
 	const mrpt::math::CMatrixDouble66& se3_cov)
 {
 	gtsam::Matrix6 C;
@@ -74,6 +72,17 @@ static gtsam::Matrix6 to_gtsam_se3_cov6(
 	return C;
 }
 
+static gtsam::Matrix6 to_gtsam_se3_cov6_isotropic(
+	const mrpt::math::CMatrixDouble66& se3_cov)
+{
+	gtsam::Matrix6 C = to_gtsam_se3_cov6_reordering(se3_cov);
+	for (int i = 0; i < 6; i++)
+		for (int j = 0; j < 6; j++)
+			if (i != j) ASSERT_EQUAL_(C(i, j), 0);
+
+	return C;
+}
+
 /** @} */
 
 }  // namespace mrpt::gtsam_wrappers

libs/obs/src/CAction.cpp

@@ -24,12 +24,13 @@ void CAction::getDescriptionAsText(std::ostream& o) const
 {
 	using namespace mrpt::system;  // for the TTimeStamp << op
 
-	o << "Timestamp (UTC): " << mrpt::system::dateTimeToString(timestamp)
-	  << std::endl;
-	o << "  (as time_t): " << std::fixed << std::setprecision(5)
-	  << mrpt::system::timestampTotime_t(timestamp) << std::endl;
-	o << "  (as TTimestamp): " << timestamp << "\n";
-	o << "\n";
+	o << mrpt::format(
+		"Timestamp (UTC): %s\n"
+		"        (local): %s\n"
+		"    (as time_t): %.09f\n",
+		mrpt::system::dateTimeToString(timestamp).c_str(),
+		mrpt::system::dateTimeLocalToString(timestamp).c_str(),
+		mrpt::Clock::toDouble(timestamp));
 
 	o << "ClassName: " << this->GetRuntimeClass()->className << "\n"
 	  << "\n";

libs/obs/src/CObservation.cpp

@@ -46,8 +46,10 @@ void CObservation::getDescriptionAsText(std::ostream& o) const
 
 	o << mrpt::format(
 		"Timestamp (UTC): %s\n"
-		"  (as time_t): %.09f\n",
+		"        (local): %s\n"
+		"    (as time_t): %.09f\n",
 		mrpt::system::dateTimeToString(timestamp).c_str(),
+		mrpt::system::dateTimeLocalToString(timestamp).c_str(),
 		mrpt::Clock::toDouble(timestamp));
 
 	o << "  (as TTimestamp): " << timestamp