diff --git a/apps/RawLogViewer/ParametersView3DPoints.h b/apps/RawLogViewer/ParametersView3DPoints.h index 111deab40a..3fcbaae5c8 100644 --- a/apps/RawLogViewer/ParametersView3DPoints.h +++ b/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; diff --git a/apps/RawLogViewer/ViewOptions3DPoints.cpp b/apps/RawLogViewer/ViewOptions3DPoints.cpp index b87653d103..9e1046d000 100644 --- a/apps/RawLogViewer/ViewOptions3DPoints.cpp +++ b/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,6 +184,7 @@ 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")); @@ -181,6 +192,7 @@ ViewOptions3DPoints::ViewOptions3DPoints(wxWindow* parent, wxWindowID id) 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); diff --git a/apps/RawLogViewer/ViewOptions3DPoints.h b/apps/RawLogViewer/ViewOptions3DPoints.h index 70d797648c..37c095cd83 100644 --- a/apps/RawLogViewer/ViewOptions3DPoints.h +++ b/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; //*) diff --git a/apps/RawLogViewer/main_show_selected_object.cpp b/apps/RawLogViewer/main_show_selected_object.cpp index fbf8a70d4a..573dada4c1 100644 --- a/apps/RawLogViewer/main_show_selected_object.cpp +++ b/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) diff --git a/appveyor.yml b/appveyor.yml index 12535434e5..43645eb855 100644 --- a/appveyor.yml +++ b/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 diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 8a59665e24..f457457f48 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/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 diff --git a/libs/maps/include/mrpt/maps/CColouredPointsMap.h b/libs/maps/include/mrpt/maps/CColouredPointsMap.h index 88f697ef3a..3585500b9c 100644 --- a/libs/maps/include/mrpt/maps/CColouredPointsMap.h +++ b/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 diff --git a/libs/maps/include/mrpt/maps/CPointsMap.h b/libs/maps/include/mrpt/maps/CPointsMap.h index c13df90486..71d50704ed 100644 --- a/libs/maps/include/mrpt/maps/CPointsMap.h +++ b/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 diff --git a/libs/maps/include/mrpt/maps/CPointsMapXYZI.h b/libs/maps/include/mrpt/maps/CPointsMapXYZI.h index 307b99a748..6f3c2e3537 100644 --- a/libs/maps/include/mrpt/maps/CPointsMapXYZI.h +++ b/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 diff --git a/libs/maps/include/mrpt/maps/CSimplePointsMap.h b/libs/maps/include/mrpt/maps/CSimplePointsMap.h index 7b568f97d6..0d4cc32e06 100644 --- a/libs/maps/include/mrpt/maps/CSimplePointsMap.h +++ b/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. } diff --git a/libs/maps/include/mrpt/maps/CWeightedPointsMap.h b/libs/maps/include/mrpt/maps/CWeightedPointsMap.h index 767d9ff033..c3f51c382a 100644 --- a/libs/maps/include/mrpt/maps/CWeightedPointsMap.h +++ b/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 diff --git a/libs/maps/src/maps/CColouredPointsMap.cpp b/libs/maps/src/maps/CColouredPointsMap.cpp index 5371418591..cd4647f769 100644 --- a/libs/maps/src/maps/CColouredPointsMap.cpp +++ b/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++; } } } diff --git a/libs/maps/src/maps/CPointsMap.cpp b/libs/maps/src/maps/CPointsMap.cpp index 8c0b7a1868..220febad0b 100644 --- a/libs/maps/src/maps/CPointsMap.cpp +++ b/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& 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(); } diff --git a/libs/maps/src/maps/CPointsMapXYZI.cpp b/libs/maps/src/maps/CPointsMapXYZI.cpp index a03489245f..49b328b549 100644 --- a/libs/maps/src/maps/CPointsMapXYZI.cpp +++ b/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++; + } } } diff --git a/libs/maps/src/maps/CPointsMap_unittest.cpp b/libs/maps/src/maps/CPointsMap_unittest.cpp index 8fcdc7f7d8..7a5bdaa6d2 100644 --- a/libs/maps/src/maps/CPointsMap_unittest.cpp +++ b/libs/maps/src/maps/CPointsMap_unittest.cpp @@ -76,6 +76,45 @@ void do_test_insertPoints() EXPECT_EQ(z2, z3); } } + + // test 3: Insert a map into another + { + MAP pts1; + load_demo_9pts_map(pts1); + + EXPECT_EQ(pts1.size(), demo9_N); + + MAP pts; + + // Insert with += syntax; + pts += pts1; + + // Insert via method: + pts.insertAnotherMap(&pts1, {}, false /* filter out */); + + for (size_t i = 0; i < 2 * demo9_N; i++) + { + float x, y, z; + pts.getPoint(i, x, y, z); + EXPECT_EQ(x, demo9_xs[i % demo9_N]); + EXPECT_EQ(y, demo9_ys[i % demo9_N]); + EXPECT_EQ(z, demo9_zs[i % demo9_N]); + } + EXPECT_EQ(pts.size(), 2 * demo9_N); + } + + // test 4: Insert a map into another with (0,0,0) filter: + { + MAP pts1; + load_demo_9pts_map(pts1); + + EXPECT_EQ(pts1.size(), demo9_N); + + MAP pts; + pts.insertAnotherMap(&pts1, {}, true /* filter out */); + + EXPECT_EQ(pts.size(), demo9_N - 1); + } } template diff --git a/libs/maps/src/maps/CWeightedPointsMap.cpp b/libs/maps/src/maps/CWeightedPointsMap.cpp index af498d7b95..bdc3a3df95 100644 --- a/libs/maps/src/maps/CWeightedPointsMap.cpp +++ b/libs/maps/src/maps/CWeightedPointsMap.cpp @@ -111,7 +111,8 @@ void CWeightedPointsMap::impl_copyFrom(const CPointsMap& obj) addFrom_classSpecific ---------------------------------------------------------------*/ void CWeightedPointsMap::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(); @@ -121,8 +122,17 @@ void CWeightedPointsMap::addFrom_classSpecific( if (anotheMap_w) { - for (size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++) + for (size_t i = 0, j = nPreviousPoints; i < nOther; i++) + { + if (filterOutPointsAtZero && + anotheMap_w->getPointsBufferRef_x()[i] == 0 && + anotheMap_w->getPointsBufferRef_y()[i] == 0 && + anotheMap_w->getPointsBufferRef_z()[i] == 0) + continue; + pointWeight[j] = anotheMap_w->pointWeight[i]; + j++; + } } } diff --git a/libs/obs/include/mrpt/obs/CObservation3DRangeScan_project3D_impl.h b/libs/obs/include/mrpt/obs/CObservation3DRangeScan_project3D_impl.h index db7f7e8928..e395df082f 100644 --- a/libs/obs/include/mrpt/obs/CObservation3DRangeScan_project3D_impl.h +++ b/libs/obs/include/mrpt/obs/CObservation3DRangeScan_project3D_impl.h @@ -12,6 +12,7 @@ #include // round() #include #include +#include #include #include #include @@ -232,8 +233,8 @@ void unprojectInto( const auto img_stride = iimg.getRowStride(); for (size_t i = 0; i < nPts; i++) { - int img_idx_x, - img_idx_y; // projected pixel coordinates, in the + // projected pixel coordinates, in the + int img_idx_x, img_idx_y; // RGB image plane bool pointWithinImage = false; if (isDirectCorresp) @@ -263,6 +264,7 @@ void unprojectInto( } } + bool ptValid = true; if (pointWithinImage) { if (hasColorIntensityImg) @@ -282,9 +284,20 @@ void unprojectInto( else { pCol.R = pCol.G = pCol.B = 255; + if (pp.onlyPointsWithIntensityColor) ptValid = false; } - // Set color: - pca.setPointRGBu8(i, pCol.R, pCol.G, pCol.B); + + if (ptValid) + { + // Set color: + pca.setPointRGBu8(i, pCol.R, pCol.G, pCol.B); + } + else + { + // Ignore this point: + pca.setInvalidPoint(i); + } + } // end for each point } // end if src_obs has intensity image } diff --git a/libs/obs/include/mrpt/obs/T3DPointsProjectionParams.h b/libs/obs/include/mrpt/obs/T3DPointsProjectionParams.h index 348673dc36..c6a994f856 100644 --- a/libs/obs/include/mrpt/obs/T3DPointsProjectionParams.h +++ b/libs/obs/include/mrpt/obs/T3DPointsProjectionParams.h @@ -46,6 +46,12 @@ struct T3DPointsProjectionParams * here the name of the layer you want to unproject, from those available in * rangeImageOtherLayers. */ std::string layer; + + /** (Default=false) If set to `true`, depth image points without an + * associated color image will not + * \note (New in MRPT 2.4.7) + */ + bool onlyPointsWithIntensityColor = false; }; } // namespace mrpt::obs diff --git a/package.xml b/package.xml index b9ecdd0da9..bd824d3cb9 100644 --- a/package.xml +++ b/package.xml @@ -6,7 +6,7 @@ --> mrpt2 - 2.4.6 + 2.4.7 Mobile Robot Programming Toolkit (MRPT) version 2.x Jose-Luis Blanco-Claraco @@ -30,6 +30,7 @@ geometry_msgs nav_msgs sensor_msgs + suitesparse assimp-dev @@ -43,7 +44,7 @@ libfreenect-dev libopenni2-dev octomap - suitesparse + tinyxml2 wx-common wxwidgets zlib diff --git a/samples/gui_depth_camera_distortion/test.cpp b/samples/gui_depth_camera_distortion/test.cpp index faa535ec32..3cb3ae88f9 100644 --- a/samples/gui_depth_camera_distortion/test.cpp +++ b/samples/gui_depth_camera_distortion/test.cpp @@ -62,6 +62,8 @@ struct AppData std::vector sliders /* cx,cy,... dist[7] */, slidersExtrinsics /*x,y,... pitch, roll*/; + nanogui::ComboBox* cbDistType; + void obs_params_to_gui(); void gui_params_to_obs(); }; @@ -107,6 +109,8 @@ void AppData::obs_params_to_gui() edBoxes[2]->setValue(mrpt::format("%.04f", p.fx())); edBoxes[3]->setValue(mrpt::format("%.04f", p.fy())); + cbDistType->setSelectedIndex(static_cast(p.distortion)); + for (int i = 0; i < 8; i++) edBoxes[4 + i]->setValue(mrpt::format("%.04e", p.dist[i])); @@ -117,7 +121,7 @@ void AppData::obs_params_to_gui() std::min(std::min(p.cx(), p.cy()), std::min(p.fx(), p.fy())); for (int i = 0; i < 4; i++) { - sliders[i]->setRange({0.5 * minPixelRanges, 2.0 * maxPixelRanges}); + sliders[i]->setRange({0.75 * minPixelRanges, 1.5 * maxPixelRanges}); } for (int i = 4; i < 12; i++) { @@ -166,6 +170,9 @@ void AppData::gui_params_to_obs() for (int i = 0; i < 8; i++) p.dist.at(i) = std::stod(edBoxes[4 + i]->value()); + p.distortion = mrpt::typemeta::str2enum( + cbDistType->items().at((cbDistType->selectedIndex()))); + // Extrinsics: app.obs->sensorPose = mrpt::poses::CPose3D::FromXYZYawPitchRoll( std::stod(edBoxesExtrinsics[0]->value()), // x @@ -263,7 +270,8 @@ static void AppDepthCamDemo() catch (const std::exception& e) { std::cerr << e.what() << std::endl; - auto dlg = new nanogui::MessageDialog( + // auto dlg = + new nanogui::MessageDialog( app.win->screen(), nanogui::MessageDialog::Type::Warning, "Exception", e.what()); } @@ -287,6 +295,16 @@ static void AppDepthCamDemo() nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill, 5, 0)); + pn->add("Distortion model"); + app.cbDistType = pn->add( + std::vector({"none", "plumb_bob", "kannala_brandt"})); + pn->add(" "); + + app.cbDistType->setCallback([](int) { + app.gui_params_to_obs(); + recalcAll(); + }); + std::vector lb = { "cx=", "cy=", "fx=", "fy=", "dist[0] (k1)=", "dist[1] (k2)=", "dist[2] (t1)=", "dist[3] (t2)=", diff --git a/version_prefix.txt b/version_prefix.txt index c8f80e3aa9..02fa01fb46 100644 --- a/version_prefix.txt +++ b/version_prefix.txt @@ -1,4 +1,4 @@ -2.4.6 +2.4.7 # 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