Merge branch 'develop'

apps/RawLogViewer/ParametersView3DPoints.h

@@ -39,6 +39,7 @@ struct ParametersView3DPoints
 	double pointSize = 4.0;
 	bool drawSensorPose = true;
 	double sensorPoseScale = 0.3;
+	bool onlyPointsWithColor = false;
 
 	bool showSurfaceIn2Dscans = true;
 	bool showPointsIn2Dscans = true;

apps/RawLogViewer/ViewOptions3DPoints.cpp

@@ -30,11 +30,12 @@ const long ViewOptions3DPoints::ID_CHECKBOX2 = wxNewId();
 const long ViewOptions3DPoints::ID_STATICTEXT4 = wxNewId();
 const long ViewOptions3DPoints::ID_SPINCTRL1 = wxNewId();
 const long ViewOptions3DPoints::ID_CHECKBOX3 = wxNewId();
+const long ViewOptions3DPoints::ID_CHECKBOX5 = wxNewId();
 const long ViewOptions3DPoints::ID_STATICTEXT5 = wxNewId();
 const long ViewOptions3DPoints::ID_TEXTCTRL4 = wxNewId();
-const long ViewOptions3DPoints::ID_CHECKBOX5 = wxNewId();
-const long ViewOptions3DPoints::ID_COLOURPICKERCTRL1 = wxNewId();
 const long ViewOptions3DPoints::ID_CHECKBOX6 = wxNewId();
+const long ViewOptions3DPoints::ID_COLOURPICKERCTRL1 = wxNewId();
+const long ViewOptions3DPoints::ID_CHECKBOX7 = wxNewId();
 const long ViewOptions3DPoints::ID_COLOURPICKERCTRL2 = wxNewId();
 //*)
 
@@ -134,6 +135,15 @@ ViewOptions3DPoints::ViewOptions3DPoints(wxWindow* parent, wxWindowID id)
 		cbColorFromRGB, 1,
 		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
 	wxString __wxRadioBoxChoices_1[4] = {_("x"), _("y"), _("z"), _("none")};
+
+	cbOnlyPointsWithColor = new wxCheckBox(
+		this, ID_CHECKBOX7, _("Only points with RGB"), wxDefaultPosition,
+		wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX7"));
+	cbOnlyPointsWithColor->SetValue(false);
+	FlexGridSizer3->Add(
+		cbOnlyPointsWithColor, 1,
+		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
+
 	rbColorByAxis = new wxRadioBox(
 		this, ID_RADIOBOX1, _("Otherwise, color from: "), wxDefaultPosition,
 		wxDefaultSize, 4, __wxRadioBoxChoices_1, 1, 0, wxDefaultValidator,
@@ -174,13 +184,15 @@ ViewOptions3DPoints::ViewOptions3DPoints(wxWindow* parent, wxWindowID id)
 	StaticBoxSizer3 =
 		new wxStaticBoxSizer(wxHORIZONTAL, this, _("Sensor pose"));
 	FlexGridSizer6 = new wxFlexGridSizer(0, 1, 0, 0);
+
 	cbShowSensorPose = new wxCheckBox(
 		this, ID_CHECKBOX3, _("Show sensor pose"), wxDefaultPosition,
 		wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX3"));
 	cbShowSensorPose->SetValue(false);
 	FlexGridSizer6->Add(
 		cbShowSensorPose, 1,
 		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
+
 	StaticText5 = new wxStaticText(
 		this, ID_STATICTEXT5, _("XYZ corner scale [m]:"), wxDefaultPosition,
 		wxDefaultSize, 0, _T("ID_STATICTEXT5"));
@@ -276,6 +288,7 @@ void ParametersView3DPoints::to_UI(ViewOptions3DPoints& ui) const
 	ui.edPointSize->SetValue(pointSize);
 
 	ui.cbShowSensorPose->SetValue(drawSensorPose);
+	ui.cbOnlyPointsWithColor->SetValue(onlyPointsWithColor);
 	ui.edSensorPoseScale->SetValue(wxString::Format("%.03f", sensorPoseScale));
 
 	ui.cbShowAxes->SetValue(showAxis);
@@ -307,6 +320,7 @@ void ParametersView3DPoints::from_UI(const ViewOptions3DPoints& ui)
 	pointSize = ui.edPointSize->GetValue();
 
 	drawSensorPose = ui.cbShowSensorPose->IsChecked();
+	onlyPointsWithColor = ui.cbOnlyPointsWithColor->IsChecked();
 	ui.edSensorPoseScale->GetValue().ToCDouble(&sensorPoseScale);
 
 	showAxis = ui.cbShowAxes->GetValue();
@@ -339,6 +353,7 @@ void ParametersView3DPoints::save_to_ini_file() const
 	MRPT_SAVE_CONFIG_VAR(showAxis, c, s);
 	MRPT_SAVE_CONFIG_VAR(showSurfaceIn2Dscans, c, s);
 	MRPT_SAVE_CONFIG_VAR(showPointsIn2Dscans, c, s);
+	MRPT_SAVE_CONFIG_VAR(onlyPointsWithColor, c, s);
 
 	MRPT_SAVE_CONFIG_VAR(surface2DscansColor.R, c, s);
 	MRPT_SAVE_CONFIG_VAR(surface2DscansColor.G, c, s);
@@ -369,6 +384,7 @@ void ParametersView3DPoints::load_from_ini_file()
 	MRPT_LOAD_CONFIG_VAR_CS(showAxis, bool);
 	MRPT_LOAD_CONFIG_VAR_CS(showSurfaceIn2Dscans, bool);
 	MRPT_LOAD_CONFIG_VAR_CS(showPointsIn2Dscans, bool);
+	MRPT_LOAD_CONFIG_VAR_CS(onlyPointsWithColor, bool);
 
 	MRPT_LOAD_CONFIG_VAR_CS(surface2DscansColor.R, int);
 	MRPT_LOAD_CONFIG_VAR_CS(surface2DscansColor.G, int);

apps/RawLogViewer/ViewOptions3DPoints.h

@@ -42,6 +42,7 @@ class ViewOptions3DPoints : public wxPanel
 	wxStaticText* StaticText3;
 	wxTextCtrl* edSensorPoseScale;
 	wxCheckBox* cbShowSensorPose;
+	wxCheckBox* cbOnlyPointsWithColor;
 	wxButton* btnApply;
 	wxColourPickerCtrl* colorSurface;
 	wxStaticText* StaticText4;
@@ -80,6 +81,7 @@ class ViewOptions3DPoints : public wxPanel
 	static const long ID_CHECKBOX5;
 	static const long ID_COLOURPICKERCTRL1;
 	static const long ID_CHECKBOX6;
+	static const long ID_CHECKBOX7;
 	static const long ID_COLOURPICKERCTRL2;
 	//*)
 

apps/RawLogViewer/main_show_selected_object.cpp

@@ -140,6 +140,7 @@ void obs3Dscan_to_viz(
 	mrpt::maps::CColouredPointsMap::Ptr pointMapCol;
 	mrpt::obs::T3DPointsProjectionParams pp;
 	pp.takeIntoAccountSensorPoseOnRobot = true;
+	pp.onlyPointsWithIntensityColor = p.onlyPointsWithColor;
 
 	// Color from intensity image?
 	if (p.colorFromRGBimage && obs->hasRangeImage && obs->hasIntensityImage)

appveyor.yml

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

doc/source/doxygen-docs/changelog.md

@@ -1,5 +1,18 @@
 \page changelog Change Log
 
+# Version 2.4.7: Released May 26th, 2022
+- Examples:
+  - gui_depth_camera_distortion: Added option to change distortion model.
+- Build system
+  - Fixed ROS-level public dependency on suitesparse.
+  - Enable tinyxml2 for ROS builds.
+- Changes in libraries:
+  - \ref mrpt_maps_grp
+    - Method mrpt::maps::CPointsMap::addFrom() removed, it overlapped with mrpt::maps::CPointsMap::insertAnotherMap()
+    - New optional parameter in mrpt::maps::CPointsMap::insertAnotherMap()
+  - \ref mrpt_obs_grp
+    - New option mrpt::obs::T3DPointsProjectionParams::onlyPointsWithIntensityColor
+
 # Version 2.4.6: Released May 24th, 2022
 - Build system
   - Fixed ROS-level public dependencies via package.xml

libs/maps/include/mrpt/maps/CColouredPointsMap.h

@@ -110,7 +110,8 @@ class CColouredPointsMap : public CPointsMap
    protected:
 	void impl_copyFrom(const CPointsMap& obj) override;
 	void addFrom_classSpecific(
-		const CPointsMap& anotherMap, const size_t nPreviousPoints) override;
+		const CPointsMap& anotherMap, const size_t nPreviousPoints,
+		const bool filterOutPointsAtZero) override;
 
 	// Friend methods:
 	template <class Derived>

libs/maps/include/mrpt/maps/CPointsMap.h

@@ -200,7 +200,8 @@ class CPointsMap : public CMetricMap,
 	/** Auxiliary method called from within \a addFrom() automatically, to
 	 * finish the copying of class-specific data  */
 	virtual void addFrom_classSpecific(
-		const CPointsMap& anotherMap, const size_t nPreviousPoints) = 0;
+		const CPointsMap& anotherMap, const size_t nPreviousPoints,
+		const bool filterOutPointsAtZero) = 0;
 
    public:
 	/** @} */
@@ -335,34 +336,26 @@ class CPointsMap : public CMetricMap,
 	};
 	TRenderOptions renderOptions;
 
-	/** Adds all the points from \a anotherMap to this map, without fusing.
-	 *  This operation can be also invoked via the "+=" operator, for example:
-	 *  \code
-	 *   mrpt::maps::CSimplePointsMap m1, m2;
-	 *   ...
-	 *   m1.addFrom( m2 );  // Add all points of m2 to m1
-	 *   m1 += m2;          // Exactly the same than above
-	 *  \endcode
-	 * \note The method in CPointsMap is generic but derived classes may
-	 * redefine this virtual method to another one more optimized.
-	 */
-	virtual void addFrom(const CPointsMap& anotherMap);
-
-	/** This operator is synonymous with \a addFrom. \sa addFrom */
-	inline void operator+=(const CPointsMap& anotherMap)
-	{
-		this->addFrom(anotherMap);
-	}
-
 	/** Insert the contents of another map into this one with some geometric
 	 * transformation, without fusing close points.
 	 * \param otherMap The other map whose points are to be inserted into this
 	 * one.
 	 * \param otherPose The pose of the other map in the coordinates of THIS map
+	 * \param filterOutPointsAtZero If true, points at (0,0,0) (in the frame of
+	 * reference of `otherMap`) will be assumed to be invalid and will not be
+	 * copied.
+	 *
 	 * \sa fuseWith, addFrom
 	 */
 	void insertAnotherMap(
-		const CPointsMap* otherMap, const mrpt::poses::CPose3D& otherPose);
+		const CPointsMap* otherMap, const mrpt::poses::CPose3D& otherPose,
+		const bool filterOutPointsAtZero = false);
+
+	/** Inserts another map into this one. \sa insertAnotherMap() */
+	void operator+=(const CPointsMap& anotherMap)
+	{
+		insertAnotherMap(&anotherMap, mrpt::poses::CPose3D::Identity());
+	}
 
 	// --------------------------------------------------
 	/** @name File input/output methods

libs/maps/include/mrpt/maps/CPointsMapXYZI.h

@@ -109,7 +109,8 @@ class CPointsMapXYZI : public CPointsMap
 	void impl_copyFrom(const CPointsMap& obj) override;
 	// See base class
 	void addFrom_classSpecific(
-		const CPointsMap& anotherMap, const size_t nPreviousPoints) override;
+		const CPointsMap& anotherMap, const size_t nPreviousPoints,
+		const bool filterOutPointsAtZero) override;
 
 	// Friend methods:
 	template <class Derived>

libs/maps/include/mrpt/maps/CSimplePointsMap.h

@@ -102,7 +102,8 @@ class CSimplePointsMap : public CPointsMap
 	void impl_copyFrom(const CPointsMap& obj) override;
 	void addFrom_classSpecific(
 		[[maybe_unused]] const CPointsMap& anotherMap,
-		[[maybe_unused]] const size_t nPreviousPoints) override
+		[[maybe_unused]] const size_t nPreviousPoints,
+		[[maybe_unused]] const bool filterOutPointsAtZero) override
 	{
 		// No extra data.
 	}

libs/maps/include/mrpt/maps/CWeightedPointsMap.h

@@ -106,7 +106,8 @@ class CWeightedPointsMap : public CPointsMap
    protected:
 	void impl_copyFrom(const CPointsMap& obj) override;
 	void addFrom_classSpecific(
-		const CPointsMap& anotherMap, const size_t nPreviousPoints) override;
+		const CPointsMap& anotherMap, const size_t nPreviousPoints,
+		const bool filterOutPointsAtZero) override;
 
 	// Friend methods:
 	template <class Derived>

libs/maps/src/maps/CColouredPointsMap.cpp

@@ -627,7 +627,8 @@ void CColouredPointsMap::PLY_export_get_vertex(
 addFrom_classSpecific
 ---------------------------------------------------------------*/
 void CColouredPointsMap::addFrom_classSpecific(
-	const CPointsMap& anotherMap, const size_t nPreviousPoints)
+	const CPointsMap& anotherMap, const size_t nPreviousPoints,
+	const bool filterOutPointsAtZero)
 {
 	const size_t nOther = anotherMap.size();
 
@@ -637,11 +638,16 @@ void CColouredPointsMap::addFrom_classSpecific(
 
 	if (anotheMap_col)
 	{
-		for (size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
+		for (size_t i = 0, j = nPreviousPoints; i < nOther; i++)
 		{
+			if (filterOutPointsAtZero && anotheMap_col->m_x[i] == 0 &&
+				anotheMap_col->m_y[i] == 0 && anotheMap_col->m_z[i] == 0)
+				continue;  // skip
+
 			m_color_R[j] = anotheMap_col->m_color_R[i];
 			m_color_G[j] = anotheMap_col->m_color_G[i];
 			m_color_B[j] = anotheMap_col->m_color_B[i];
+			j++;
 		}
 	}
 }

libs/maps/src/maps/CPointsMap.cpp

@@ -1573,30 +1573,6 @@ void CPointsMap::PLY_export_get_vertex(
 	pt.z = m_z[idx];
 }
 
-// Generic implementation (a more optimized one should exist in derived
-// classes):
-void CPointsMap::addFrom(const CPointsMap& anotherMap)
-{
-	const size_t nThis = this->size();
-	const size_t nOther = anotherMap.size();
-
-	const size_t nTot = nThis + nOther;
-
-	this->resize(nTot);
-
-	for (size_t i = 0, j = nThis; i < nOther; i++, j++)
-	{
-		m_x[j] = anotherMap.m_x[i];
-		m_y[j] = anotherMap.m_y[i];
-		m_z[j] = anotherMap.m_z[i];
-	}
-
-	// Also copy other data fields (color, ...)
-	addFrom_classSpecific(anotherMap, nThis);
-
-	mark_as_modified();
-}
-
 /*---------------------------------------------------------------
 						applyDeletionMask
  ---------------------------------------------------------------*/
@@ -1627,25 +1603,28 @@ void CPointsMap::applyDeletionMask(const std::vector<bool>& mask)
 					insertAnotherMap
  ---------------------------------------------------------------*/
 void CPointsMap::insertAnotherMap(
-	const CPointsMap* otherMap, const CPose3D& otherPose)
+	const CPointsMap* otherMap, const CPose3D& otherPose,
+	const bool filterOutPointsAtZero)
 {
 	const size_t N_this = size();
 	const size_t N_other = otherMap->size();
 
 	// Set the new size:
-	this->resize(N_this + N_other);
+	this->reserve(N_this + N_other);
 
 	// Optimization: detect the case of no transformation needed and avoid the
 	// matrix multiplications:
 	const bool identity_tf = (otherPose == CPose3D::Identity());
 
 	mrpt::math::TPoint3Df pt;
-	size_t src, dst;
-	for (src = 0, dst = N_this; src < N_other; src++, dst++)
+	for (size_t src = 0; src < N_other; src++)
 	{
 		// Load the next point:
 		otherMap->getPointFast(src, pt.x, pt.y, pt.z);
 
+		if (filterOutPointsAtZero && pt.x == 0 && pt.y == 0 && pt.z == 0)
+			continue;  // Skip
+
 		// Translation:
 		mrpt::math::TPoint3D g;
 
@@ -1657,11 +1636,11 @@ void CPointsMap::insertAnotherMap(
 		}
 
 		// Add to this map:
-		this->setPointFast(dst, g.x, g.y, g.z);
+		this->insertPointFast(g.x, g.y, g.z);
 	}
 
 	// Also copy other data fields (color, ...)
-	addFrom_classSpecific(*otherMap, N_this);
+	addFrom_classSpecific(*otherMap, N_this, filterOutPointsAtZero);
 
 	mark_as_modified();
 }

libs/maps/src/maps/CPointsMapXYZI.cpp

@@ -281,7 +281,8 @@ bool CPointsMapXYZI::loadXYZI_from_text_file(const std::string& file)
 addFrom_classSpecific
 ---------------------------------------------------------------*/
 void CPointsMapXYZI::addFrom_classSpecific(
-	const CPointsMap& anotherMap, const size_t nPreviousPoints)
+	const CPointsMap& anotherMap, const size_t nPreviousPoints,
+	const bool filterOutPointsAtZero)
 {
 	const size_t nOther = anotherMap.size();
 
@@ -291,8 +292,17 @@ void CPointsMapXYZI::addFrom_classSpecific(
 
 	if (anotheMap_col)
 	{
-		for (size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
+		for (size_t i = 0, j = nPreviousPoints; i < nOther; i++)
+		{
+			if (filterOutPointsAtZero &&
+				anotheMap_col->getPointsBufferRef_x()[i] == 0 &&
+				anotheMap_col->getPointsBufferRef_y()[i] == 0 &&
+				anotheMap_col->getPointsBufferRef_z()[i] == 0)
+				continue;
+
 			m_intensity[j] = anotheMap_col->m_intensity[i];
+			j++;
+		}
 	}
 }