From 2cb1185b30e6c7371d21dd608a315062306b6dc5 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 16 Dec 2023 22:13:44 +0100 Subject: [PATCH 1/3] Simplify simplemap API --- .../carmen2simplemap_main.cpp | 7 +- apps/map-partition/map-partition.cpp | 18 +- .../observations2map_main.cpp | 2 +- apps/robot-map-gui/CDocument.cpp | 33 +-- apps/robot-map-gui/CDocument.h | 25 +- apps/robot-map-gui/gui/CMainWindow.cpp | 10 +- apps/robot-map-gui/gui/CMainWindow.h | 2 +- .../gui/observationTree/CPairNode.cpp | 2 +- .../gui/observationTree/CPairNode.h | 2 +- doc/source/doxygen-docs/changelog.md | 6 +- libs/apps/src/CGridMapAlignerApp.cpp | 29 ++- libs/apps/src/MonteCarloLocalization_App.cpp | 2 +- libs/obs/include/mrpt/maps/CMetricMap.h | 9 +- libs/obs/include/mrpt/maps/CSimpleMap.h | 236 +++++++----------- libs/obs/src/CMetricMap.cpp | 5 +- libs/obs/src/CSimpleMap.cpp | 115 ++------- .../src/slam/CIncrementalMapPartitioner.cpp | 37 +-- .../CIncrementalMapPartitioner_unittest.cpp | 5 +- libs/slam/src/slam/CMetricMapBuilderICP.cpp | 10 +- .../CMonteCarloLocalization2D_unittest.cpp | 2 +- libs/slam/src/slam/CMultiMetricMapPDF.cpp | 8 +- libs/slam/src/slam/CRangeBearingKFSLAM.cpp | 11 +- libs/slam/src/slam/CRangeBearingKFSLAM2D.cpp | 11 +- 23 files changed, 216 insertions(+), 371 deletions(-) diff --git a/apps/carmen2simplemap/carmen2simplemap_main.cpp b/apps/carmen2simplemap/carmen2simplemap_main.cpp index ad0d28e49f..f2f175af92 100644 --- a/apps/carmen2simplemap/carmen2simplemap_main.cpp +++ b/apps/carmen2simplemap/carmen2simplemap_main.cpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include #include #include #include @@ -153,9 +153,8 @@ int main(int argc, char** argv) } // Insert (observations, pose) pair: - CPosePDFGaussian::Ptr pos = - std::make_shared(); - pos->mean = gt_pose; + auto pos = CPose3DPDFGaussian::Create(); + pos->mean = mrpt::poses::CPose3D(gt_pose); theSimpleMap.insert(pos, SF); } diff --git a/apps/map-partition/map-partition.cpp b/apps/map-partition/map-partition.cpp index 1a123061db..989f5cb1ea 100644 --- a/apps/map-partition/map-partition.cpp +++ b/apps/map-partition/map-partition.cpp @@ -100,14 +100,11 @@ void Test() const size_t n = in_map.size(); for (size_t i = 0; i < n; i++) { - CSensoryFrame::Ptr sf; - CPose3DPDF::Ptr posePDF; - - in_map.get(i, posePDF, sf); + const auto& [posePDF, sf, twist] = in_map.get(i); imp.addMapFrame(*sf, *posePDF); - printf("[%u/%u]...", (unsigned int)i, (unsigned int)n); + printf("[%zu/%zu]...", i, n); // if ((i%1)==0) if (i == n - 1) @@ -154,11 +151,7 @@ void Test() out_map.clear(); for (unsigned int j : parts[i]) { - CSensoryFrame::Ptr sf; - CPose3DPDF::Ptr posePDF; - - in_map.get(j, posePDF, sf); - + const auto& [posePDF, sf, twist] = in_map.get(j); out_map.insert(posePDF, sf); } @@ -262,11 +255,8 @@ void Test() CPose2D meanPose; for (size_t j = 0; j < parts[i].size(); j++) { - CSensoryFrame::Ptr sf; - CPose3DPDF::Ptr posePDF; - // Get the pose: - in_map.get(parts[i][j], posePDF, sf); + const auto& [posePDF, sf, twist] = in_map.get(parts[i][j]); meanPose = CPose2D(posePDF->getMeanVal()); diff --git a/apps/observations2map/observations2map_main.cpp b/apps/observations2map/observations2map_main.cpp index bbdc98c09d..ce06df6b69 100644 --- a/apps/observations2map/observations2map_main.cpp +++ b/apps/observations2map/observations2map_main.cpp @@ -92,7 +92,7 @@ int main(int argc, char** argv) // Build metric maps: cout << "Building metric maps..." << std::endl; - metricMap.loadFromProbabilisticPosesAndObservations(simplemap); + metricMap.loadFromSimpleMap(simplemap); cout << "done." << endl; diff --git a/apps/robot-map-gui/CDocument.cpp b/apps/robot-map-gui/CDocument.cpp index 2e5f278f1f..7c7f5cb34a 100644 --- a/apps/robot-map-gui/CDocument.cpp +++ b/apps/robot-map-gui/CDocument.cpp @@ -158,7 +158,7 @@ std::vector CDocument::remove(const std::vector& indexes) void CDocument::move( const std::vector& indexes, - const CSimpleMap::TPosePDFSensFramePairList& posesObsPairs) + const CSimpleMap::KeyframeList& posesObsPairs) { for (size_t i = 0; i < indexes.size(); ++i) move(indexes[i], posesObsPairs[i], true); @@ -168,18 +168,18 @@ void CDocument::move( } void CDocument::move( - size_t index, const CSimpleMap::Pair& posesObsPair, + size_t index, const CSimpleMap::Keyframe& posesObsPair, bool disableUpdateMetricMap) { - m_simplemap.set(index, posesObsPair); + auto& kf = m_simplemap.get(index); + kf = posesObsPair; m_changedFile = true; if (!disableUpdateMetricMap) updateMetricMap(); } void CDocument::insert( - const std::vector& idx, - CSimpleMap::TPosePDFSensFramePairList& posesObsPairs) + const std::vector& idx, CSimpleMap::KeyframeList& posesObsPairs) { for (size_t i = 0; i < idx.size(); ++i) m_simplemap.insert(posesObsPairs[i]); @@ -187,10 +187,9 @@ void CDocument::insert( updateMetricMap(); } -CSimpleMap::TPosePDFSensFramePairList CDocument::get( - const std::vector& idxs) +CSimpleMap::KeyframeList CDocument::get(const std::vector& idxs) { - CSimpleMap::TPosePDFSensFramePairList posesObsPairs; + CSimpleMap::KeyframeList posesObsPairs; for (auto& idx : idxs) { auto pair = get(idx); @@ -199,21 +198,11 @@ CSimpleMap::TPosePDFSensFramePairList CDocument::get( return posesObsPairs; } -CSimpleMap::ConstPair CDocument::get(size_t idx) const +CSimpleMap::KeyframeList CDocument::getReverse(const std::vector& idx) { - return m_simplemap.getAsPair(idx); -} -mrpt::maps::CSimpleMap::Pair CDocument::get(size_t idx) -{ - return m_simplemap.getAsPair(idx); -} - -CSimpleMap::TPosePDFSensFramePairList CDocument::getReverse( - const std::vector& idx) -{ - CSimpleMap::TPosePDFSensFramePairList posesObsPairs; + CSimpleMap::KeyframeList posesObsPairs; for (int i = idx.size() - 1; i >= 0; --i) - posesObsPairs.emplace_back(m_simplemap.getAsPair(idx[i])); + posesObsPairs.emplace_back(m_simplemap.get(idx[i])); return posesObsPairs; } @@ -236,7 +225,7 @@ void CDocument::addMapToRenderizableMaps( void CDocument::updateMetricMap() { - m_metricmap.loadFromProbabilisticPosesAndObservations(m_simplemap); + m_metricmap.loadFromSimpleMap(m_simplemap); m_typeConfigs.clear(); m_typeConfigs.emplace(TypeOfConfig::PointsMap, std::vector()); diff --git a/apps/robot-map-gui/CDocument.h b/apps/robot-map-gui/CDocument.h index 4a57f7ce84..e59ea3e616 100644 --- a/apps/robot-map-gui/CDocument.h +++ b/apps/robot-map-gui/CDocument.h @@ -64,22 +64,29 @@ class CDocument void move( const std::vector& indexes, - const mrpt::maps::CSimpleMap::TPosePDFSensFramePairList& posesObsPairs); + const mrpt::maps::CSimpleMap::KeyframeList& posesObsPairs); void move( - size_t index, const mrpt::maps::CSimpleMap::Pair& posesObsPair, + size_t index, const mrpt::maps::CSimpleMap::Keyframe& posesObsPair, bool disableUpdateMetricMap = false); void insert( const std::vector& idx, - mrpt::maps::CSimpleMap::TPosePDFSensFramePairList& posesObsPairs); - - mrpt::maps::CSimpleMap::TPosePDFSensFramePairList get( + mrpt::maps::CSimpleMap::KeyframeList& posesObsPairs); + + mrpt::maps::CSimpleMap::KeyframeList get( const std::vector& idx); - mrpt::maps::CSimpleMap::ConstPair get(size_t idx) const; - mrpt::maps::CSimpleMap::Pair get(size_t idx); - - mrpt::maps::CSimpleMap::TPosePDFSensFramePairList getReverse( + const mrpt::maps::CSimpleMap::Keyframe & get(size_t idx) const + { + return m_simplemap.get(idx); + } + + mrpt::maps::CSimpleMap::Keyframe& get(size_t idx) + { + return m_simplemap.get(idx); + } + + mrpt::maps::CSimpleMap::KeyframeList getReverse( const std::vector& idx); private: diff --git a/apps/robot-map-gui/gui/CMainWindow.cpp b/apps/robot-map-gui/gui/CMainWindow.cpp index 47c4cac6cf..de87cb70e1 100644 --- a/apps/robot-map-gui/gui/CMainWindow.cpp +++ b/apps/robot-map-gui/gui/CMainWindow.cpp @@ -339,8 +339,7 @@ void CMainWindow::selectedChanged(const std::vector& idx) } void CMainWindow::addRobotPosesFromMap( - std::vector idx, - mrpt::maps::CSimpleMap::TPosePDFSensFramePairList posesObsPairs) + std::vector idx, mrpt::maps::CSimpleMap::KeyframeList posesObsPairs) { if (!m_document || idx.empty()) return; @@ -363,8 +362,7 @@ void CMainWindow::moveRobotPosesOnMap( { if (!m_document || idx.empty()) return; - mrpt::maps::CSimpleMap::TPosePDFSensFramePairList posesObsPairs = - m_document->get(idx); + mrpt::maps::CSimpleMap::KeyframeList posesObsPairs = m_document->get(idx); for (auto& poseSf : posesObsPairs) { mrpt::poses::CPose3DPDF::Ptr& posePDF = poseSf.pose; @@ -398,7 +396,7 @@ void CMainWindow::deleteRobotPoses(const std::vector& idx) { if (!m_document || idx.empty()) return; - mrpt::maps::CSimpleMap::TPosePDFSensFramePairList posesObsPairs = + mrpt::maps::CSimpleMap::KeyframeList posesObsPairs = m_document->getReverse(idx); auto reverseInd = m_document->remove(idx); @@ -469,7 +467,7 @@ void CMainWindow::updateDirection( { if (!m_document) return; - mrpt::maps::CSimpleMap::Pair posesObsPair = m_document->get(index); + auto posesObsPair = m_document->get(index); auto posePDF = posesObsPair.pose; auto pose = posePDF->getMeanVal(); diff --git a/apps/robot-map-gui/gui/CMainWindow.h b/apps/robot-map-gui/gui/CMainWindow.h index 1cefd68661..18073888d2 100644 --- a/apps/robot-map-gui/gui/CMainWindow.h +++ b/apps/robot-map-gui/gui/CMainWindow.h @@ -36,7 +36,7 @@ class CMainWindow : public QMainWindow void addRobotPosesFromMap( std::vector idx, - mrpt::maps::CSimpleMap::TPosePDFSensFramePairList posesObsPairs); + mrpt::maps::CSimpleMap::KeyframeList posesObsPairs); void deleteRobotPosesFromMap(const std::vector& idx); void moveRobotPosesOnMap( const std::vector& idx, const QPointF& dist); diff --git a/apps/robot-map-gui/gui/observationTree/CPairNode.cpp b/apps/robot-map-gui/gui/observationTree/CPairNode.cpp index 4d9c712293..46a976b205 100644 --- a/apps/robot-map-gui/gui/observationTree/CPairNode.cpp +++ b/apps/robot-map-gui/gui/observationTree/CPairNode.cpp @@ -16,7 +16,7 @@ using namespace mrpt; using namespace mrpt::maps; CPairNode::CPairNode( - CNode* parent, const CSimpleMap::Pair& poseSensFramePair, + CNode* parent, const CSimpleMap::Keyframe& poseSensFramePair, size_t indexInSimpleMap) : CNode(parent), m_indexInSimpleMap(indexInSimpleMap) { diff --git a/apps/robot-map-gui/gui/observationTree/CPairNode.h b/apps/robot-map-gui/gui/observationTree/CPairNode.h index a4a64194f8..99c8790987 100644 --- a/apps/robot-map-gui/gui/observationTree/CPairNode.h +++ b/apps/robot-map-gui/gui/observationTree/CPairNode.h @@ -20,7 +20,7 @@ class CPairNode : public CNode { public: CPairNode( - CNode* parent, const mrpt::maps::CSimpleMap::Pair& poseSensFramePair, + CNode* parent, const mrpt::maps::CSimpleMap::Keyframe& poseSensFramePair, size_t indexInSimpleMap); ~CPairNode() override; diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index e1008c5e4f..e2e20465a8 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,7 +1,11 @@ \page changelog Change Log # Version 2.11.5: UNRELEASED -(none yet) +- Changes in libraries: + - \ref mrpt_obs_grp + - mrpt::maps::CSimpleMap changes: + - Added an optional twist field. + - Simplified API for preferred usage with structured binding tuples. # Version 2.11.4: Released Dec 15th, 2023 - Changes in apps: diff --git a/libs/apps/src/CGridMapAlignerApp.cpp b/libs/apps/src/CGridMapAlignerApp.cpp index 5f74c30cba..36e5071827 100644 --- a/libs/apps/src/CGridMapAlignerApp.cpp +++ b/libs/apps/src/CGridMapAlignerApp.cpp @@ -27,7 +27,7 @@ #include #include #include -#include +#include #include #include #include @@ -348,7 +348,7 @@ void CGridMapAlignerApp::run() } // Generate the_map1 now: - the_map1.loadFromProbabilisticPosesAndObservations(map1); + the_map1.loadFromSimpleMap(map1); size_t N1 = max( 40, @@ -389,10 +389,7 @@ void CGridMapAlignerApp::run() for (unsigned int q = 0; q < map2noisy.size(); q++) { - CPose3DPDF::Ptr PDF; - CSensoryFrame::Ptr SF; - - map2noisy.get(q, PDF, SF); + const auto [PDF, SF, twist] = map2noisy.get(q); // If it's detect_test, translate the map2 by a fixed, known // quantity: @@ -421,8 +418,8 @@ void CGridMapAlignerApp::run() if (NOISE_IN_POSE) { - CPosePDFGaussian::Ptr newPDF = - std::make_shared(); + auto& kf = map2noisy.get(q); + auto newPDF = std::make_shared(); newPDF->copyFrom(*PDF); // Change the pose: @@ -430,17 +427,19 @@ void CGridMapAlignerApp::run() getRandomGenerator().drawGaussian1D(0, STD_NOISE_XY)); newPDF->mean.y_incr( getRandomGenerator().drawGaussian1D(0, STD_NOISE_XY)); - newPDF->mean.phi_incr(getRandomGenerator().drawGaussian1D( - 0, DEG2RAD(STD_NOISE_PHI))); - newPDF->mean.normalizePhi(); - // Change into the map: - map2noisy.set(q, newPDF, CSensoryFrame::Ptr()); + double yaw, pitch, roll; + newPDF->mean.getYawPitchRoll(yaw, pitch, roll); + yaw += getRandomGenerator().drawGaussian1D( + 0, DEG2RAD(STD_NOISE_PHI)); + newPDF->mean.setYawPitchRoll(yaw, pitch, roll); - } // end of NOISE_IN_POSE + // Change into the map: + kf.pose = newPDF; + } } - the_map2.loadFromProbabilisticPosesAndObservations(map2noisy); + the_map2.loadFromSimpleMap(map2noisy); size_t N2 = max( 40, diff --git a/libs/apps/src/MonteCarloLocalization_App.cpp b/libs/apps/src/MonteCarloLocalization_App.cpp index cd397edff0..4243a3f8be 100644 --- a/libs/apps/src/MonteCarloLocalization_App.cpp +++ b/libs/apps/src/MonteCarloLocalization_App.cpp @@ -299,7 +299,7 @@ void MonteCarloLocalization_Base::do_pf_localization() // Build metric map: // ------------------------------ MRPT_LOG_INFO("Building metric map(s) from '.simplemap'..."); - metricMap->loadFromProbabilisticPosesAndObservations(simpleMap); + metricMap->loadFromSimpleMap(simpleMap); MRPT_LOG_INFO("Done."); } else if (!mapExt.compare("gridmap")) diff --git a/libs/obs/include/mrpt/maps/CMetricMap.h b/libs/obs/include/mrpt/maps/CMetricMap.h index 2341f4fb09..fba102d9c9 100644 --- a/libs/obs/include/mrpt/maps/CMetricMap.h +++ b/libs/obs/include/mrpt/maps/CMetricMap.h @@ -132,14 +132,7 @@ class CMetricMap : public mrpt::serialization::CSerializable, * \exception std::exception Some internal steps in invoked methods can * raise exceptions on invalid parameters, etc... */ - void loadFromProbabilisticPosesAndObservations( - const mrpt::maps::CSimpleMap& Map); - - ///! \overload - inline void loadFromSimpleMap(const mrpt::maps::CSimpleMap& Map) - { - loadFromProbabilisticPosesAndObservations(Map); - } + void loadFromSimpleMap(const mrpt::maps::CSimpleMap& Map); /** Insert the observation information into this map. This method must be * implemented in derived classes. diff --git a/libs/obs/include/mrpt/maps/CSimpleMap.h b/libs/obs/include/mrpt/maps/CSimpleMap.h index 2aa924ab11..483a76095e 100644 --- a/libs/obs/include/mrpt/maps/CSimpleMap.h +++ b/libs/obs/include/mrpt/maps/CSimpleMap.h @@ -8,6 +8,7 @@ +------------------------------------------------------------------------+ */ #pragma once +#include #include #include #include @@ -18,25 +19,38 @@ namespace mrpt::maps { -/** A view-based representation of a metric map. +/** A view-based map: a set of poses and what the robot saw from those poses. * - * This comprises a list of `` pairs, that is, - * the **poses** (keyframes) from which a set of **observations** where - * gathered: - * - Poses, in the global `map` frame of reference, are stored as probabilistic - * PDFs over SE(3) as instances of mrpt::poses::CPose3DPDF - * - Observations are stored as mrpt::obs::CSensoryFrame. + * A simplemap comprises a sequence of tuples, each containing: + * - The **keyframe SE(3) pose** of the robot, including (optionally) its + * uncertainty, as instances of mrpt::poses::CPose3DPDF + * - The **raw observations** from that keyframe, in a mrpt::obs::CSensoryFrame + * - Optionally, the **twist** (linear and angular velocity) of the robot in the + * local frame of reference, at that moment. It can be used to undistort data + * from a rotatory lidar, for example. * - * Note that in order to generate an actual metric map (occupancy grid, point - * cloud, octomap, etc.) from a "simple map", you must instantiate the desired - * metric map class and invoke its virtual method - * mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(). + * To generate an actual metric map (occupancy grid, point cloud, octomap, etc.) + * from a "simple map", the user must instantiate the desired metric map + * class(es) and invoke its virtual method + * mrpt::maps::CMetricMap::loadFromSimpleMap(). * - * \note Objects of this class are serialized into GZ-compressed - * files with the extension `.simplemap`. + * Users can also use the new top-level [library + * mp2p_icp_filters](https://github.com/MOLAorg/mp2p_icp/) and its CLI + * application + * [sm2mm](https://github.com/MOLAorg/mp2p_icp/tree/master/apps/sm2mm) + * (simple-map to metric-map) + * to generate metric maps including pre-processing of raw data in a flexible + * way. + * + * To programatically change an existing simplemap, use the non-const get() + * method and modify the returned reference. + * + * \note Objects of this class are serialized into GZ-compressed files with + * the extension `.simplemap`. * See [Robotics file formats](robotics_file_formats.html). * - * \sa mrpt::obs::CSensoryFrame, mrpt::poses::CPose3DPDF, mrpt::maps::CMetricMap + * \sa mrpt::obs::CSensoryFrame, mrpt::poses::CPose3DPDF, + * mrpt::maps::CMetricMap, https://github.com/MOLAorg/mp2p_icp/ * * \ingroup mrpt_obs_grp */ @@ -48,29 +62,28 @@ class CSimpleMap : public mrpt::serialization::CSerializable ~CSimpleMap() = default; /** Copy constructor, makes a deep copy of all data. */ - CSimpleMap(const CSimpleMap& o); + explicit CSimpleMap(const CSimpleMap& o); /** Copy, making a deep copy of all data. */ CSimpleMap& operator=(const CSimpleMap& o); - struct Pair + struct Keyframe { - Pair() = default; - ~Pair() = default; + Keyframe() = default; + ~Keyframe() = default; + + Keyframe( + const mrpt::poses::CPose3DPDF::Ptr& kfPose, + const mrpt::obs::CSensoryFrame::Ptr& kfSf, + const std::optional& kflocalTwist = + std::nullopt) + : pose(kfPose), sf(kfSf), localTwist(kflocalTwist) + { + } mrpt::poses::CPose3DPDF::Ptr pose; - mrpt::obs::CSensoryFrame::Ptr sf; - }; - - struct ConstPair - { - ConstPair() = default; - ~ConstPair() = default; - - ConstPair(const Pair& p) : pose(p.pose), sf(p.sf) {} - - mrpt::poses::CPose3DPDF::ConstPtr pose; - mrpt::obs::CSensoryFrame::ConstPtr sf; + mrpt::obs::CSensoryFrame::Ptr sf; //!< raw observations + std::optional localTwist; }; /** \name Map access and modification @@ -89,124 +102,54 @@ class CSimpleMap : public mrpt::serialization::CSerializable * \return false on any error. */ bool loadFromFile(const std::string& filName); - /** Returns the count of (pose,sensoryFrame) pairs */ - size_t size() const { return m_posesObsPairs.size(); } + /** Returns the number of keyframes in the map */ + size_t size() const { return m_keyframes.size(); } /** Returns size()!=0 */ - bool empty() const { return m_posesObsPairs.empty(); } - - /** Access to the 0-based index i'th pair. - * \exception std::exception On index out of bounds. - */ - void get( - size_t index, mrpt::poses::CPose3DPDF::ConstPtr& out_posePDF, - mrpt::obs::CSensoryFrame::ConstPtr& out_SF) const - { - ASSERTMSG_(index < m_posesObsPairs.size(), "Index out of bounds"); - out_posePDF = m_posesObsPairs[index].pose; - out_SF = m_posesObsPairs[index].sf; - } - /// \overload - std::tuple< - mrpt::poses::CPose3DPDF::ConstPtr, mrpt::obs::CSensoryFrame::ConstPtr> - get(size_t index) const - { - ASSERTMSG_(index < m_posesObsPairs.size(), "Index out of bounds"); - return {m_posesObsPairs[index].pose, m_posesObsPairs[index].sf}; - } - - ConstPair getAsPair(size_t index) const - { - ASSERTMSG_(index < m_posesObsPairs.size(), "Index out of bounds"); - return m_posesObsPairs.at(index); - } - Pair& getAsPair(size_t index) - { - ASSERTMSG_(index < m_posesObsPairs.size(), "Index out of bounds"); - return m_posesObsPairs.at(index); - } + bool empty() const { return m_keyframes.empty(); } - /// \overload - void get( - size_t index, mrpt::poses::CPose3DPDF::Ptr& out_posePDF, - mrpt::obs::CSensoryFrame::Ptr& out_SF) + /// const accessor + const Keyframe& get(size_t index) const { - ASSERTMSG_(index < m_posesObsPairs.size(), "Index out of bounds"); - out_posePDF = m_posesObsPairs[index].pose; - out_SF = m_posesObsPairs[index].sf; + ASSERT_LT_(index, m_keyframes.size()); + return m_keyframes[index]; } - /// \overload - std::tuple get( - size_t index) + /// non-const accessor, returning a reference suitable for modification + Keyframe& get(size_t index) { - ASSERTMSG_(index < m_posesObsPairs.size(), "Index out of bounds"); - return {m_posesObsPairs[index].pose, m_posesObsPairs[index].sf}; + ASSERT_LT_(index, m_keyframes.size()); + return m_keyframes[index]; } - /** Changes the 0-based index i'th pair. - * If one of either `in_posePDF` or `in_SF` are empty `shared_ptr`s, the - * corresponding field in the map is not modified. - * - * \exception std::exception On index out of bounds. - * \sa insert, get, remove - */ - void set( - size_t index, const mrpt::poses::CPose3DPDF::Ptr& in_posePDF, - const mrpt::obs::CSensoryFrame::Ptr& in_SF); - - /// \overload - void set(size_t index, const Pair& poseSF) - { - set(index, poseSF.pose, poseSF.sf); - } - - /// \overload For SE(2) pose PDF, internally converted to SE(3). - void set( - size_t index, const mrpt::poses::CPosePDF::Ptr& in_posePDF, - const mrpt::obs::CSensoryFrame::Ptr& in_SF); - - /** Deletes the 0-based index i'th pair. + /** Deletes the 0-based index i'th keyframe. * \exception std::exception On index out of bounds. * \sa insert, get, set */ void remove(size_t index); - /** Adds a new keyframe (SE(3) pose) to the view-based map, making a deep - * copy of the pose PDF (observations within the SF are always copied as - * `shared_ptr`s). - */ - void insert( - const mrpt::poses::CPose3DPDF& in_posePDF, - const mrpt::obs::CSensoryFrame& in_SF); - /** Adds a new keyframe (SE(3) pose) to the view-based map. * Both shared pointers are copied (shallow object copies). */ - void insert( - const mrpt::poses::CPose3DPDF::Ptr& in_posePDF, - const mrpt::obs::CSensoryFrame::Ptr& in_SF); - - /// \overload - void insert(const Pair& poseSF) { insert(poseSF.pose, poseSF.sf); } - - /** Adds a new keyframe (SE(2) pose) to the view-based map, making a deep - * copy of the pose PDF (observations within the SF are always copied as - * `shared_ptr`s). - */ - void insert( - const mrpt::poses::CPosePDF& in_posePDF, - const mrpt::obs::CSensoryFrame& in_SF); + void insert(const Keyframe& kf); - /** Adds a new keyframe (SE(2) pose) to the view-based map. + /** Adds a new keyframe (SE(3) pose) to the view-based map. * Both shared pointers are copied (shallow object copies). */ void insert( - const mrpt::poses::CPosePDF::Ptr& in_posePDF, - const mrpt::obs::CSensoryFrame::Ptr& in_SF); + const mrpt::poses::CPose3DPDF::Ptr& in_posePDF, + const mrpt::obs::CSensoryFrame::Ptr& in_SF, + const std::optional& twist = std::nullopt) + { + Keyframe kf; + kf.pose = in_posePDF; + kf.sf = in_SF; + kf.localTwist = twist; + insert(kf); + } - /** Remove all stored pairs. \sa remove */ - void clear() { m_posesObsPairs.clear(); } + /** Remove all stored keyframes. \sa remove */ + void clear() { m_keyframes.clear(); } /** Change the coordinate origin of all stored poses, that is, translates * and rotates the map such that the old SE(3) origin (identity @@ -218,31 +161,30 @@ class CSimpleMap : public mrpt::serialization::CSerializable /** \name Iterators API * @{ */ - using TPosePDFSensFramePairList = std::deque; - - using const_iterator = TPosePDFSensFramePairList::const_iterator; - using iterator = TPosePDFSensFramePairList::iterator; - using reverse_iterator = TPosePDFSensFramePairList::reverse_iterator; - using const_reverse_iterator = - TPosePDFSensFramePairList::const_reverse_iterator; - - const_iterator begin() const { return m_posesObsPairs.begin(); } - const_iterator end() const { return m_posesObsPairs.end(); } - const_iterator cbegin() const { return m_posesObsPairs.cbegin(); } - const_iterator cend() const { return m_posesObsPairs.cend(); } - iterator begin() { return m_posesObsPairs.begin(); } - iterator end() { return m_posesObsPairs.end(); } - const_reverse_iterator rbegin() const { return m_posesObsPairs.rbegin(); } - const_reverse_iterator rend() const { return m_posesObsPairs.rend(); } - const_reverse_iterator crbegin() const { return m_posesObsPairs.crbegin(); } - const_reverse_iterator crend() const { return m_posesObsPairs.crend(); } - reverse_iterator rbegin() { return m_posesObsPairs.rbegin(); } - reverse_iterator rend() { return m_posesObsPairs.rend(); } + using KeyframeList = std::deque; + + using const_iterator = KeyframeList::const_iterator; + using iterator = KeyframeList::iterator; + using reverse_iterator = KeyframeList::reverse_iterator; + using const_reverse_iterator = KeyframeList::const_reverse_iterator; + + const_iterator begin() const { return m_keyframes.begin(); } + const_iterator end() const { return m_keyframes.end(); } + const_iterator cbegin() const { return m_keyframes.cbegin(); } + const_iterator cend() const { return m_keyframes.cend(); } + iterator begin() { return m_keyframes.begin(); } + iterator end() { return m_keyframes.end(); } + const_reverse_iterator rbegin() const { return m_keyframes.rbegin(); } + const_reverse_iterator rend() const { return m_keyframes.rend(); } + const_reverse_iterator crbegin() const { return m_keyframes.crbegin(); } + const_reverse_iterator crend() const { return m_keyframes.crend(); } + reverse_iterator rbegin() { return m_keyframes.rbegin(); } + reverse_iterator rend() { return m_keyframes.rend(); } /** @} */ private: /** The stored data */ - TPosePDFSensFramePairList m_posesObsPairs; + KeyframeList m_keyframes; }; // End of class def. diff --git a/libs/obs/src/CMetricMap.cpp b/libs/obs/src/CMetricMap.cpp index 39a6a22244..a3a1680fda 100644 --- a/libs/obs/src/CMetricMap.cpp +++ b/libs/obs/src/CMetricMap.cpp @@ -33,14 +33,13 @@ void CMetricMap::clear() publishEvent(mrptEventMetricMapClear(this)); } -void CMetricMap::loadFromProbabilisticPosesAndObservations( - const mrpt::maps::CSimpleMap& sfSeq) +void CMetricMap::loadFromSimpleMap(const mrpt::maps::CSimpleMap& sfSeq) { // Erase previous contents: this->clear(); // Insert new content: - for (const auto& [pose, sf] : sfSeq) + for (const auto& [pose, sf, twist] : sfSeq) { ASSERTMSG_(pose, "Input map has an empty `CPose3DPDF` ptr"); ASSERTMSG_(sf, "Input map has an empty `CSensoryFrame` ptr"); diff --git a/libs/obs/src/CSimpleMap.cpp b/libs/obs/src/CSimpleMap.cpp index 57357358f4..e67c74f9c6 100644 --- a/libs/obs/src/CSimpleMap.cpp +++ b/libs/obs/src/CSimpleMap.cpp @@ -14,6 +14,7 @@ #include #include #include +#include using namespace mrpt::obs; using namespace mrpt::maps; @@ -30,10 +31,9 @@ const auto fn_pair_make_unique = [](auto& ptr) { ptr.sf.reset(dynamic_cast(ptr.sf->clone())); }; -CSimpleMap::CSimpleMap(const CSimpleMap& o) : m_posesObsPairs(o.m_posesObsPairs) +CSimpleMap::CSimpleMap(const CSimpleMap& o) : m_keyframes(o.m_keyframes) { - for_each( - m_posesObsPairs.begin(), m_posesObsPairs.end(), fn_pair_make_unique); + for_each(m_keyframes.begin(), m_keyframes.end(), fn_pair_make_unique); } CSimpleMap& CSimpleMap::operator=(const CSimpleMap& o) @@ -41,9 +41,8 @@ CSimpleMap& CSimpleMap::operator=(const CSimpleMap& o) MRPT_START if (this == &o) return *this; // It may be used sometimes - m_posesObsPairs = o.m_posesObsPairs; - for_each( - m_posesObsPairs.begin(), m_posesObsPairs.end(), fn_pair_make_unique); + m_keyframes = o.m_keyframes; + for_each(m_keyframes.begin(), m_keyframes.end(), fn_pair_make_unique); return *this; MRPT_END @@ -52,97 +51,23 @@ CSimpleMap& CSimpleMap::operator=(const CSimpleMap& o) void CSimpleMap::remove(size_t index) { MRPT_START - ASSERTMSG_(index < m_posesObsPairs.size(), "Index out of bounds"); - m_posesObsPairs.erase(m_posesObsPairs.begin() + index); + ASSERT_LT_(index, m_keyframes.size()); + m_keyframes.erase(m_keyframes.begin() + index); MRPT_END } -void CSimpleMap::set( - size_t index, const CPose3DPDF::Ptr& in_posePDF, - const CSensoryFrame::Ptr& in_SF) -{ - MRPT_START - ASSERTMSG_(index < m_posesObsPairs.size(), "Index out of bounds"); - if (in_posePDF) m_posesObsPairs[index].pose = in_posePDF; - if (in_SF) m_posesObsPairs[index].sf = in_SF; - MRPT_END -} - -void CSimpleMap::set( - size_t index, const CPosePDF::Ptr& in_posePDF, - const CSensoryFrame::Ptr& in_SF) -{ - MRPT_START - ASSERTMSG_(index < m_posesObsPairs.size(), "Index out of bounds"); - if (in_posePDF) - m_posesObsPairs[index].pose = - CPose3DPDF::Ptr(CPose3DPDF::createFrom2D(*in_posePDF)); - if (in_SF) m_posesObsPairs[index].sf = in_SF; - - MRPT_END -} - -void CSimpleMap::insert( - const CPose3DPDF::Ptr& in_posePDF, const CSensoryFrame::Ptr& in_SF) -{ - MRPT_START - - Pair pair; - - pair.sf = in_SF; - pair.pose = in_posePDF; - - m_posesObsPairs.push_back(pair); - - MRPT_END -} - -void CSimpleMap::insert( - const CPose3DPDF& in_posePDF, const CSensoryFrame& in_SF) -{ - MRPT_START - - Pair pair; - - pair.sf = CSensoryFrame::Create(in_SF); - pair.pose = CPose3DPDF::Ptr(dynamic_cast(in_posePDF.clone())); - ASSERT_(pair.pose); - - m_posesObsPairs.push_back(pair); - - MRPT_END -} - -void CSimpleMap::insert(const CPosePDF& in_posePDF, const CSensoryFrame& in_SF) -{ - MRPT_START - - Pair pair; +void CSimpleMap::insert(const Keyframe& kf) { m_keyframes.emplace_back(kf); } - pair.sf = CSensoryFrame::Create(in_SF); - pair.pose = CPose3DPDF::Ptr(dynamic_cast(in_posePDF.clone())); - ASSERT_(pair.pose); - - m_posesObsPairs.push_back(pair); - - MRPT_END -} - -void CSimpleMap::insert( - const CPosePDF::Ptr& in_posePDF, const CSensoryFrame::Ptr& in_SF) -{ - insert(CPose3DPDF::Ptr(CPose3DPDF::createFrom2D(*in_posePDF)), in_SF); -} - -uint8_t CSimpleMap::serializeGetVersion() const { return 1; } +uint8_t CSimpleMap::serializeGetVersion() const { return 2; } void CSimpleMap::serializeTo(mrpt::serialization::CArchive& out) const { - out.WriteAs(m_posesObsPairs.size()); - for (const auto& p : m_posesObsPairs) + out.WriteAs(m_keyframes.size()); + for (const auto& p : m_keyframes) { ASSERT_(p.pose); ASSERT_(p.sf); out << *p.pose << *p.sf; + out << p.localTwist; } } @@ -151,14 +76,18 @@ void CSimpleMap::serializeFrom( { switch (version) { + case 2: case 1: { uint32_t i, n; clear(); in >> n; - m_posesObsPairs.resize(n); + m_keyframes.resize(n); for (i = 0; i < n; i++) - in >> m_posesObsPairs[i].pose >> m_posesObsPairs[i].sf; + { + in >> m_keyframes[i].pose >> m_keyframes[i].sf; + if (version >= 2) in >> m_keyframes[i].localTwist; + } } break; case 0: @@ -167,12 +96,12 @@ void CSimpleMap::serializeFrom( uint32_t i, n; clear(); in >> n; - m_posesObsPairs.resize(n); + m_keyframes.resize(n); for (i = 0; i < n; i++) { CPosePDF::Ptr aux2Dpose; - in >> aux2Dpose >> m_posesObsPairs[i].sf; - m_posesObsPairs[i].pose = + in >> aux2Dpose >> m_keyframes[i].sf; + m_keyframes[i].pose = CPose3DPDF::Ptr(CPose3DPDF::createFrom2D(*aux2Dpose)); } } @@ -183,7 +112,7 @@ void CSimpleMap::serializeFrom( void CSimpleMap::changeCoordinatesOrigin(const CPose3D& newOrigin) { - for (auto& m_posesObsPair : m_posesObsPairs) + for (auto& m_posesObsPair : m_keyframes) { ASSERT_(m_posesObsPair.pose); m_posesObsPair.pose->changeCoordinatesReference(newOrigin); diff --git a/libs/slam/src/slam/CIncrementalMapPartitioner.cpp b/libs/slam/src/slam/CIncrementalMapPartitioner.cpp index b464340e56..a6b9fccd7b 100644 --- a/libs/slam/src/slam/CIncrementalMapPartitioner.cpp +++ b/libs/slam/src/slam/CIncrementalMapPartitioner.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -136,7 +137,9 @@ uint32_t CIncrementalMapPartitioner::addMapFrame( frame.insertObservationsInto(*newMetricMap); // Add tuple (pose,SF) to "simplemap": - m_individualFrames.insert(robotPose, frame); + auto pose = mrpt::poses::CPose3DPDFGaussian::Create(); + pose->copyFrom(robotPose); + m_individualFrames.insert(pose, mrpt::obs::CSensoryFrame::Create(frame)); // Expand the adjacency matrix (pads with 0) m_A.setSize(n, n); @@ -170,8 +173,10 @@ uint32_t CIncrementalMapPartitioner::addMapFrame( map_keyframe_t map_i; map_i.kf_id = i; map_i.metric_map = m_individualMaps[i]; - CPose3DPDF::Ptr posePDF_i; - m_individualFrames.get(i, posePDF_i, map_i.raw_observations); + + const auto& kf = m_individualFrames.get(i); + map_i.raw_observations = kf.sf; + const CPose3DPDF::Ptr posePDF_i = kf.pose; auto pose_i = posePDF_i->getMeanVal(); for (uint32_t j = 0; j < new_id; j++) @@ -187,9 +192,10 @@ uint32_t CIncrementalMapPartitioner::addMapFrame( { // KF "j": map_keyframe_t map_j; - CPose3DPDF::Ptr posePDF_j; map_j.kf_id = j; - m_individualFrames.get(j, posePDF_j, map_j.raw_observations); + const auto& kf_j = m_individualFrames.get(j); + map_j.raw_observations = kf_j.sf; + const CPose3DPDF::Ptr posePDF_j = kf_j.pose; auto pose_j = posePDF_j->getMeanVal(); map_j.metric_map = m_individualMaps[j]; @@ -318,16 +324,12 @@ void CIncrementalMapPartitioner::removeSetOfNodes( m_individualFrames.remove(*it); // Change coordinates reference of frames: - CSensoryFrame::Ptr SF; - CPose3DPDF::Ptr posePDF; - if (changeCoordsRef) { ASSERT_(m_individualFrames.size() > 0); - m_individualFrames.get(0, posePDF, SF); + const auto& kf = m_individualFrames.get(0); - CPose3D p; - posePDF->getMean(p); + const CPose3D p = kf.pose->getMeanVal(); m_individualFrames.changeCoordinatesOrigin(p); } @@ -346,12 +348,9 @@ void CIncrementalMapPartitioner::changeCoordinatesOriginPoseIndex( { MRPT_START - CPose3DPDF::Ptr pdf; - CSensoryFrame::Ptr sf; - m_individualFrames.get(newOriginPose, pdf, sf); + const auto& kf = m_individualFrames.get(newOriginPose); - CPose3D p; - pdf->getMean(p); + const CPose3D p = kf.pose->getMeanVal(); changeCoordinatesOrigin(-p); MRPT_END @@ -372,8 +371,9 @@ void CIncrementalMapPartitioner::getAs3DScene( for (size_t i = 0; i < m_individualFrames.size(); i++) { - const auto [i_pdf, i_sf] = m_individualFrames.get(i); + const auto [i_pdf, i_sf, twist] = m_individualFrames.get(i); (void)i_sf; // unused + (void)twist; CPose3D i_mean; i_pdf->getMean(i_mean); @@ -405,8 +405,9 @@ void CIncrementalMapPartitioner::getAs3DScene( // Arcs: for (size_t j = i + 1; j < m_individualFrames.size(); j++) { - const auto [j_pdf, j_sf] = m_individualFrames.get(j); + const auto [j_pdf, j_sf, twist] = m_individualFrames.get(j); (void)j_sf; // unused + (void)twist; CPose3D j_mean; j_pdf->getMean(j_mean); diff --git a/libs/slam/src/slam/CIncrementalMapPartitioner_unittest.cpp b/libs/slam/src/slam/CIncrementalMapPartitioner_unittest.cpp index 32ec699320..f5e62d4aba 100644 --- a/libs/slam/src/slam/CIncrementalMapPartitioner_unittest.cpp +++ b/libs/slam/src/slam/CIncrementalMapPartitioner_unittest.cpp @@ -30,11 +30,8 @@ TEST(CIncrementalMapPartitioner, test_dataset) mrpt::maps::CSimpleMap in_map, out_map; in_map.loadFromFile(map_file); - for (const auto& pair : in_map) - { - const auto& [posePDF, sf] = pair; + for (const auto& [posePDF, sf, twist] : in_map) imp.addMapFrame(*sf, *posePDF); - } std::vector> parts; imp.updatePartitions(parts); diff --git a/libs/slam/src/slam/CMetricMapBuilderICP.cpp b/libs/slam/src/slam/CMetricMapBuilderICP.cpp index a949736abe..ae50c60835 100644 --- a/libs/slam/src/slam/CMetricMapBuilderICP.cpp +++ b/libs/slam/src/slam/CMetricMapBuilderICP.cpp @@ -551,17 +551,13 @@ void CMetricMapBuilderICP::initialize( for (size_t i = 0; i < SF_Poses_seq.size(); i++) { - CPose3DPDF::Ptr posePDF; - CSensoryFrame::Ptr SF; - // Get the SF and its pose: - SF_Poses_seq.get(i, posePDF, SF); + const auto& kf = SF_Poses_seq.get(i); - CPose3D estimatedPose3D; - posePDF->getMean(estimatedPose3D); + const CPose3D estimatedPose3D = kf.pose->getMeanVal(); // Insert observations into the map: - SF->insertObservationsInto(metricMap, estimatedPose3D); + kf.sf->insertObservationsInto(metricMap, estimatedPose3D); } MRPT_END diff --git a/libs/slam/src/slam/CMonteCarloLocalization2D_unittest.cpp b/libs/slam/src/slam/CMonteCarloLocalization2D_unittest.cpp index 58db50055d..d3a75a72fd 100644 --- a/libs/slam/src/slam/CMonteCarloLocalization2D_unittest.cpp +++ b/libs/slam/src/slam/CMonteCarloLocalization2D_unittest.cpp @@ -124,7 +124,7 @@ void run_test_pf_localization(CPose2D& meanPose, CMatrixDouble33& cov) mrpt::serialization::archiveFrom(f) >> simpleMap; ASSERT_(simpleMap.size() > 0); // Build metric map: - metricMap->loadFromProbabilisticPosesAndObservations(simpleMap); + metricMap->loadFromSimpleMap(simpleMap); } else { diff --git a/libs/slam/src/slam/CMultiMetricMapPDF.cpp b/libs/slam/src/slam/CMultiMetricMapPDF.cpp index 27ccc49a98..b0b7f45d87 100644 --- a/libs/slam/src/slam/CMultiMetricMapPDF.cpp +++ b/libs/slam/src/slam/CMultiMetricMapPDF.cpp @@ -116,7 +116,7 @@ void CMultiMetricMapPDF::clear( p.d->robotPath.resize(nOldKeyframes); for (size_t i = 0; i < nOldKeyframes; i++) { - const auto [keyframe_pose, sfkeyframe_sf] = prevMap.get(i); + const auto [keyframe_pose, sfkeyframe_sf, twist] = prevMap.get(i); // as pose, use: if the PDF is also a PF with the same number of // samples, use those particles; @@ -542,19 +542,17 @@ void CMultiMetricMapPDF::updateSensoryFrameSequence() { MRPT_START CPose3DPDFParticles posePartsPDF; - CPose3DPDF::Ptr previousPosePDF; - CSensoryFrame::Ptr dummy; for (size_t i = 0; i < SFs.size(); i++) { // Get last estimation: - SFs.get(i, previousPosePDF, dummy); + auto& kf = SFs.get(i); // Compute the new one: getEstimatedPosePDFAtTime(SF2robotPath[i], posePartsPDF); // Copy into SFs: - previousPosePDF->copyFrom(posePartsPDF); + kf.pose->copyFrom(posePartsPDF); } MRPT_END diff --git a/libs/slam/src/slam/CRangeBearingKFSLAM.cpp b/libs/slam/src/slam/CRangeBearingKFSLAM.cpp index 0fae56c9f9..a35e45a6d7 100644 --- a/libs/slam/src/slam/CRangeBearingKFSLAM.cpp +++ b/libs/slam/src/slam/CRangeBearingKFSLAM.cpp @@ -1040,8 +1040,8 @@ void CRangeBearingKFSLAM::getAs3DObject( CLandmark::TLandmarkID i_th_ID = m_IDs.inverse(i); // Look for the lm_ID in the SF: - const CSensoryFrame::ConstPtr& SF_i = - SFs->getAsPair(m_lastPartitionSet[p][w]).sf; + const auto& [pose_i, SF_i, twist_t] = + SFs->get(m_lastPartitionSet[p][w]); CObservationBearingRange::Ptr obs = SF_i->getObservationByClass(); @@ -1137,8 +1137,8 @@ void CRangeBearingKFSLAM::getLastPartitionLandmarks( CLandmark::TLandmarkID i_th_ID = m_IDs.inverse(i); // Look for the lm_ID in the SF: - const CSensoryFrame::ConstPtr& SF_i = - SFs->getAsPair(m_lastPartitionSet[p][w]).sf; + const auto& [pose_i, SF_i, twist_t] = + SFs->get(m_lastPartitionSet[p][w]); CObservationBearingRange::Ptr obs = SF_i->getObservationByClass(); @@ -1289,7 +1289,8 @@ void CRangeBearingKFSLAM::saveMapAndPath2DRepresentationAsMATLABFile( os::fprintf(f, "\nROB_PATH=["); for (size_t i = 0; i < m_SFs.size(); i++) { - CPose3D p = m_SFs.getAsPair(i).pose->getMeanVal(); + const auto& kf = m_SFs.get(i); + const CPose3D p = kf.pose->getMeanVal(); os::fprintf(f, "%.04f %.04f", p.x(), p.y()); if (i < (m_SFs.size() - 1)) os::fprintf(f, ";"); diff --git a/libs/slam/src/slam/CRangeBearingKFSLAM2D.cpp b/libs/slam/src/slam/CRangeBearingKFSLAM2D.cpp index 3f961f75d2..e06049b3ec 100644 --- a/libs/slam/src/slam/CRangeBearingKFSLAM2D.cpp +++ b/libs/slam/src/slam/CRangeBearingKFSLAM2D.cpp @@ -17,8 +17,8 @@ #include #include #include +#include #include -#include #include #include #include @@ -153,8 +153,10 @@ void CRangeBearingKFSLAM2D::processActionObservation( // ============================================================= if (options.create_simplemap) { - CPosePDFGaussian::Ptr auxPosePDF = std::make_shared(); - getCurrentRobotPose(*auxPosePDF); + auto auxPosePDF = std::make_shared(); + CPosePDFGaussian p; + getCurrentRobotPose(p); + auxPosePDF->copyFrom(p); m_SFs.insert(auxPosePDF, SF); } @@ -1039,7 +1041,8 @@ void CRangeBearingKFSLAM2D::saveMapAndPath2DRepresentationAsMATLABFile( os::fprintf(f, "\nROB_PATH=["); for (i = 0; i < m_SFs.size(); i++) { - const CPose3D p = m_SFs.getAsPair(i).pose->getMeanVal(); + const auto& kf = m_SFs.get(i); + const CPose3D p = kf.pose->getMeanVal(); CPoint3D pnt3D(p); // 6D -> 3D only os::fprintf(f, "%.04f %.04f", pnt3D.x(), pnt3D.y()); From d3d44f9ce0486895527d4671b17473fe71261e4e Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 17 Dec 2023 00:18:02 +0100 Subject: [PATCH 2/3] Update python wrapper --- python/all_mrpt_maps3.cpp | 1 + python/src/mrpt/core/format.cpp | 6 +- python/src/mrpt/maps/CBeacon.cpp | 1 + python/src/mrpt/maps/CColouredOctoMap.cpp | 2 +- .../mrpt/maps/CGasConcentrationGridMap2D.cpp | 2 +- .../src/mrpt/maps/CHeightGridMap2D_Base.cpp | 2 +- python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp | 2 +- python/src/mrpt/maps/CLandmarksMap.cpp | 2 +- python/src/mrpt/maps/CMetricMap.cpp | 3 +- python/src/mrpt/maps/CMetricMap_1.cpp | 6 +- python/src/mrpt/maps/CMultiMetricMap.cpp | 126 +-------------- python/src/mrpt/maps/CMultiMetricMapPDF.cpp | 1 + python/src/mrpt/maps/COccupancyGridMap2D.cpp | 2 +- python/src/mrpt/maps/COccupancyGridMap3D.cpp | 2 +- python/src/mrpt/maps/COctoMap.cpp | 2 +- python/src/mrpt/maps/COctoMapBase.cpp | 3 +- python/src/mrpt/maps/COctoMapBase_1.cpp | 3 +- .../mrpt/maps/CPointCloudFilterByDistance.cpp | 2 +- python/src/mrpt/maps/CPointsMapXYZI.cpp | 2 +- python/src/mrpt/maps/CPointsMap_1.cpp | 2 +- .../src/mrpt/maps/CReflectivityGridMap2D.cpp | 2 +- python/src/mrpt/maps/CSimpleMap.cpp | 148 ++++++++++++++++++ python/src/mrpt/maps/CVoxelMap.cpp | 2 +- python/src/mrpt/maps/CVoxelMapBase.cpp | 6 +- .../mrpt/maps/CVoxelMapOccupancyBase_1.cpp | 3 +- .../mrpt/maps/CVoxelMapOccupancyBase_2.cpp | 3 +- .../src/mrpt/maps/CWirelessPowerGridMap2D.cpp | 2 +- python/src/mrpt/math/TTwist3D.cpp | 1 + .../mrpt/slam/CIncrementalMapPartitioner.cpp | 2 +- python/src/mrpt/slam/CMetricMapBuilder.cpp | 1 + python/src/mrpt/slam/CMetricMapBuilderICP.cpp | 1 + python/src/pymrpt.cpp | 6 +- python/src/pymrpt.sources | 3 +- python/src/unknown/unknown_6.cpp | 12 +- python/src/unknown/unknown_7.cpp | 16 +- python/src/unknown/unknown_8.cpp | 12 +- python/stubs-out/mrpt/pymrpt/mrpt/config.pyi | 2 + python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi | 115 +++++++------- python/stubs-out/mrpt/pymrpt/mrpt/math.pyi | 47 +++++- .../mrpt/pymrpt/mrpt/obs/__init__.pyi | 48 +++++- .../mrpt/pymrpt/mrpt/opengl/__init__.pyi | 12 +- .../mrpt/pymrpt/mrpt/poses/__init__.pyi | 16 ++ 42 files changed, 380 insertions(+), 252 deletions(-) create mode 100644 python/src/mrpt/maps/CSimpleMap.cpp diff --git a/python/all_mrpt_maps3.cpp b/python/all_mrpt_maps3.cpp index c6073548dd..f4d3c42c54 100644 --- a/python/all_mrpt_maps3.cpp +++ b/python/all_mrpt_maps3.cpp @@ -4,6 +4,7 @@ #include "src/mrpt/maps/CRandomFieldGridMap2D.cpp" #include "src/mrpt/maps/CRandomFieldGridMap3D.cpp" #include "src/mrpt/maps/CReflectivityGridMap2D.cpp" +#include "src/mrpt/maps/CSimpleMap.cpp" #include "src/mrpt/maps/CSimplePointsMap.cpp" #include "src/mrpt/maps/CWeightedPointsMap.cpp" #include "src/mrpt/maps/CWirelessPowerGridMap2D.cpp" diff --git a/python/src/mrpt/core/format.cpp b/python/src/mrpt/core/format.cpp index 144154e9a9..a725d5adc0 100644 --- a/python/src/mrpt/core/format.cpp +++ b/python/src/mrpt/core/format.cpp @@ -21,12 +21,12 @@ void bind_mrpt_core_format(std::function< pybind11::module &(std::string const & // mrpt::to_string(double) file:mrpt/core/format.h line:31 M("mrpt").def("to_string", (std::string (*)(double)) &mrpt::to_string, "C++: mrpt::to_string(double) --> std::string", pybind11::arg("v")); - // mrpt::to_string(unsigned long) file:mrpt/core/format.h line:31 - M("mrpt").def("to_string", (std::string (*)(unsigned long)) &mrpt::to_string, "C++: mrpt::to_string(unsigned long) --> std::string", pybind11::arg("v")); - // mrpt::to_string(int) file:mrpt/core/format.h line:31 M("mrpt").def("to_string", (std::string (*)(int)) &mrpt::to_string, "C++: mrpt::to_string(int) --> std::string", pybind11::arg("v")); + // mrpt::to_string(unsigned long) file:mrpt/core/format.h line:31 + M("mrpt").def("to_string", (std::string (*)(unsigned long)) &mrpt::to_string, "C++: mrpt::to_string(unsigned long) --> std::string", pybind11::arg("v")); + // mrpt::to_string(unsigned int) file:mrpt/core/format.h line:31 M("mrpt").def("to_string", (std::string (*)(unsigned int)) &mrpt::to_string, "C++: mrpt::to_string(unsigned int) --> std::string", pybind11::arg("v")); diff --git a/python/src/mrpt/maps/CBeacon.cpp b/python/src/mrpt/maps/CBeacon.cpp index 18745ef11b..9feedf656c 100644 --- a/python/src/mrpt/maps/CBeacon.cpp +++ b/python/src/mrpt/maps/CBeacon.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/mrpt/maps/CColouredOctoMap.cpp b/python/src/mrpt/maps/CColouredOctoMap.cpp index 267f0864df..d84321a45e 100644 --- a/python/src/mrpt/maps/CColouredOctoMap.cpp +++ b/python/src/mrpt/maps/CColouredOctoMap.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -79,7 +80,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/mrpt/maps/CGasConcentrationGridMap2D.cpp b/python/src/mrpt/maps/CGasConcentrationGridMap2D.cpp index 8c1dfcf1d9..7c31633815 100644 --- a/python/src/mrpt/maps/CGasConcentrationGridMap2D.cpp +++ b/python/src/mrpt/maps/CGasConcentrationGridMap2D.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -73,7 +74,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/mrpt/maps/CHeightGridMap2D_Base.cpp b/python/src/mrpt/maps/CHeightGridMap2D_Base.cpp index 6b186ccc6f..d92c8455e0 100644 --- a/python/src/mrpt/maps/CHeightGridMap2D_Base.cpp +++ b/python/src/mrpt/maps/CHeightGridMap2D_Base.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -77,7 +78,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp b/python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp index 9ad25e11dc..b9669f8869 100644 --- a/python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp +++ b/python/src/mrpt/maps/CHeightGridMap2D_MRF.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -74,7 +75,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/mrpt/maps/CLandmarksMap.cpp b/python/src/mrpt/maps/CLandmarksMap.cpp index 5a5dc8fcb9..160d1a3a0f 100644 --- a/python/src/mrpt/maps/CLandmarksMap.cpp +++ b/python/src/mrpt/maps/CLandmarksMap.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -79,7 +80,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/mrpt/maps/CMetricMap.cpp b/python/src/mrpt/maps/CMetricMap.cpp index 8501ebb7df..8c77e53148 100644 --- a/python/src/mrpt/maps/CMetricMap.cpp +++ b/python/src/mrpt/maps/CMetricMap.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -32,7 +33,6 @@ #include #include #include -#include #include #include #include @@ -40,7 +40,6 @@ #include #include #include -#include #include #include diff --git a/python/src/mrpt/maps/CMetricMap_1.cpp b/python/src/mrpt/maps/CMetricMap_1.cpp index e978d10307..026291b4de 100644 --- a/python/src/mrpt/maps/CMetricMap_1.cpp +++ b/python/src/mrpt/maps/CMetricMap_1.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -28,7 +29,6 @@ #include #include #include -#include #include #include #include @@ -37,7 +37,6 @@ #include #include // __str__ #include -#include #include #include @@ -63,8 +62,7 @@ void bind_mrpt_maps_CMetricMap_1(std::function< pybind11::module &(std::string c cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::boundingBox, "Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the\n default value of mrpt::math::TBoundingBoxf() if not implemented in the\n derived class or the map is empty.\n\nC++: mrpt::maps::CMetricMap::boundingBox() const --> struct mrpt::math::TBoundingBox_"); - cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); - cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("obs"), pybind11::arg("robotPose")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CSensoryFrame & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("sf")); diff --git a/python/src/mrpt/maps/CMultiMetricMap.cpp b/python/src/mrpt/maps/CMultiMetricMap.cpp index b97a5246d0..783b5ec1e6 100644 --- a/python/src/mrpt/maps/CMultiMetricMap.cpp +++ b/python/src/mrpt/maps/CMultiMetricMap.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -75,7 +76,6 @@ #include #include #include -#include #include #include #include @@ -361,77 +361,6 @@ struct PyCallBack_mrpt_maps_CMultiMetricMap : public mrpt::maps::CMultiMetricMap } }; -// mrpt::maps::CSimpleMap file:mrpt/maps/CSimpleMap.h line:43 -struct PyCallBack_mrpt_maps_CSimpleMap : public mrpt::maps::CSimpleMap { - using mrpt::maps::CSimpleMap::CSimpleMap; - - const struct mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "GetRuntimeClass"); - if (overload) { - auto o = overload.operator()(); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - return CSimpleMap::GetRuntimeClass(); - } - class mrpt::rtti::CObject * clone() const override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "clone"); - if (overload) { - auto o = overload.operator()(); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - return CSimpleMap::clone(); - } - uint8_t serializeGetVersion() const override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "serializeGetVersion"); - if (overload) { - auto o = overload.operator()(); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - return CSimpleMap::serializeGetVersion(); - } - void serializeTo(class mrpt::serialization::CArchive & a0) const override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "serializeTo"); - if (overload) { - auto o = overload.operator()(a0); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - return CSimpleMap::serializeTo(a0); - } - void serializeFrom(class mrpt::serialization::CArchive & a0, uint8_t a1) override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "serializeFrom"); - if (overload) { - auto o = overload.operator()(a0, a1); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - return CSimpleMap::serializeFrom(a0, a1); - } -}; - void bind_mrpt_maps_CMultiMetricMap(std::function< pybind11::module &(std::string const &namespace_) > &M) { { // mrpt::maps::CMultiMetricMap file:mrpt/maps/CMultiMetricMap.h line:120 @@ -458,57 +387,4 @@ void bind_mrpt_maps_CMultiMetricMap(std::function< pybind11::module &(std::strin cl.def("getAsSimplePointsMap", (const class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMultiMetricMap::*)() const) &mrpt::maps::CMultiMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMultiMetricMap::getAsSimplePointsMap() const --> const class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); cl.def("asString", (std::string (mrpt::maps::CMultiMetricMap::*)() const) &mrpt::maps::CMultiMetricMap::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::CMultiMetricMap::asString() const --> std::string"); } - { // mrpt::maps::CSimpleMap file:mrpt/maps/CSimpleMap.h line:43 - pybind11::class_, PyCallBack_mrpt_maps_CSimpleMap, mrpt::serialization::CSerializable> cl(M("mrpt::maps"), "CSimpleMap", "A view-based representation of a metric map.\n\n This comprises a list of `` pairs, that is,\n the **poses** (keyframes) from which a set of **observations** where\n gathered:\n - Poses, in the global `map` frame of reference, are stored as probabilistic\n PDFs over SE(3) as instances of mrpt::poses::CPose3DPDF\n - Observations are stored as mrpt::obs::CSensoryFrame.\n\n Note that in order to generate an actual metric map (occupancy grid, point\n cloud, octomap, etc.) from a \"simple map\", you must instantiate the desired\n metric map class and invoke its virtual method\n mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations().\n\n \n Objects of this class are serialized into GZ-compressed\n files with the extension `.simplemap`.\n See [Robotics file formats](robotics_file_formats.html).\n\n \n mrpt::obs::CSensoryFrame, mrpt::poses::CPose3DPDF, mrpt::maps::CMetricMap\n\n \n\n "); - cl.def( pybind11::init( [](){ return new mrpt::maps::CSimpleMap(); }, [](){ return new PyCallBack_mrpt_maps_CSimpleMap(); } ) ); - cl.def( pybind11::init( [](PyCallBack_mrpt_maps_CSimpleMap const &o){ return new PyCallBack_mrpt_maps_CSimpleMap(o); } ) ); - cl.def( pybind11::init( [](mrpt::maps::CSimpleMap const &o){ return new mrpt::maps::CSimpleMap(o); } ) ); - cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CSimpleMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CSimpleMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); - cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::maps::CSimpleMap::*)() const) &mrpt::maps::CSimpleMap::GetRuntimeClass, "C++: mrpt::maps::CSimpleMap::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); - cl.def("clone", (class mrpt::rtti::CObject * (mrpt::maps::CSimpleMap::*)() const) &mrpt::maps::CSimpleMap::clone, "C++: mrpt::maps::CSimpleMap::clone() const --> class mrpt::rtti::CObject *", pybind11::return_value_policy::automatic); - cl.def_static("CreateObject", (class std::shared_ptr (*)()) &mrpt::maps::CSimpleMap::CreateObject, "C++: mrpt::maps::CSimpleMap::CreateObject() --> class std::shared_ptr"); - cl.def("assign", (class mrpt::maps::CSimpleMap & (mrpt::maps::CSimpleMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CSimpleMap::operator=, "Copy, making a deep copy of all data. \n\nC++: mrpt::maps::CSimpleMap::operator=(const class mrpt::maps::CSimpleMap &) --> class mrpt::maps::CSimpleMap &", pybind11::return_value_policy::automatic, pybind11::arg("o")); - cl.def("saveToFile", (bool (mrpt::maps::CSimpleMap::*)(const std::string &) const) &mrpt::maps::CSimpleMap::saveToFile, "Save this object to a .simplemap binary file (compressed with gzip)\n See [Robotics file formats](robotics_file_formats.html).\n \n\n loadFromFile()\n \n\n false on any error. \n\nC++: mrpt::maps::CSimpleMap::saveToFile(const std::string &) const --> bool", pybind11::arg("filName")); - cl.def("loadFromFile", (bool (mrpt::maps::CSimpleMap::*)(const std::string &)) &mrpt::maps::CSimpleMap::loadFromFile, "Load the contents of this object from a .simplemap binary file (possibly\n compressed with gzip)\n See [Robotics file formats](robotics_file_formats.html).\n \n\n saveToFile()\n \n\n false on any error. \n\nC++: mrpt::maps::CSimpleMap::loadFromFile(const std::string &) --> bool", pybind11::arg("filName")); - cl.def("size", (size_t (mrpt::maps::CSimpleMap::*)() const) &mrpt::maps::CSimpleMap::size, "Returns the count of (pose,sensoryFrame) pairs \n\nC++: mrpt::maps::CSimpleMap::size() const --> size_t"); - cl.def("empty", (bool (mrpt::maps::CSimpleMap::*)() const) &mrpt::maps::CSimpleMap::empty, "Returns size()!=0 \n\nC++: mrpt::maps::CSimpleMap::empty() const --> bool"); - cl.def("get", (void (mrpt::maps::CSimpleMap::*)(size_t, class std::shared_ptr &, class std::shared_ptr &) const) &mrpt::maps::CSimpleMap::get, "Access to the 0-based index i'th pair.\n \n\n std::exception On index out of bounds.\n\nC++: mrpt::maps::CSimpleMap::get(size_t, class std::shared_ptr &, class std::shared_ptr &) const --> void", pybind11::arg("index"), pybind11::arg("out_posePDF"), pybind11::arg("out_SF")); - cl.def("getAsPair", (struct mrpt::maps::CSimpleMap::Pair & (mrpt::maps::CSimpleMap::*)(size_t)) &mrpt::maps::CSimpleMap::getAsPair, "C++: mrpt::maps::CSimpleMap::getAsPair(size_t) --> struct mrpt::maps::CSimpleMap::Pair &", pybind11::return_value_policy::automatic, pybind11::arg("index")); - cl.def("get", (void (mrpt::maps::CSimpleMap::*)(size_t, class std::shared_ptr &, class std::shared_ptr &)) &mrpt::maps::CSimpleMap::get, "C++: mrpt::maps::CSimpleMap::get(size_t, class std::shared_ptr &, class std::shared_ptr &) --> void", pybind11::arg("index"), pybind11::arg("out_posePDF"), pybind11::arg("out_SF")); - cl.def("get", (class std::tuple, class std::shared_ptr > (mrpt::maps::CSimpleMap::*)(size_t)) &mrpt::maps::CSimpleMap::get, "C++: mrpt::maps::CSimpleMap::get(size_t) --> class std::tuple, class std::shared_ptr >", pybind11::arg("index")); - cl.def("set", (void (mrpt::maps::CSimpleMap::*)(size_t, const class std::shared_ptr &, const class std::shared_ptr &)) &mrpt::maps::CSimpleMap::set, "Changes the 0-based index i'th pair.\n If one of either `in_posePDF` or `in_SF` are empty `shared_ptr`s, the\n corresponding field in the map is not modified.\n\n \n std::exception On index out of bounds.\n \n\n insert, get, remove\n\nC++: mrpt::maps::CSimpleMap::set(size_t, const class std::shared_ptr &, const class std::shared_ptr &) --> void", pybind11::arg("index"), pybind11::arg("in_posePDF"), pybind11::arg("in_SF")); - cl.def("set", (void (mrpt::maps::CSimpleMap::*)(size_t, const struct mrpt::maps::CSimpleMap::Pair &)) &mrpt::maps::CSimpleMap::set, "C++: mrpt::maps::CSimpleMap::set(size_t, const struct mrpt::maps::CSimpleMap::Pair &) --> void", pybind11::arg("index"), pybind11::arg("poseSF")); - cl.def("set", (void (mrpt::maps::CSimpleMap::*)(size_t, const class std::shared_ptr &, const class std::shared_ptr &)) &mrpt::maps::CSimpleMap::set, "C++: mrpt::maps::CSimpleMap::set(size_t, const class std::shared_ptr &, const class std::shared_ptr &) --> void", pybind11::arg("index"), pybind11::arg("in_posePDF"), pybind11::arg("in_SF")); - cl.def("remove", (void (mrpt::maps::CSimpleMap::*)(size_t)) &mrpt::maps::CSimpleMap::remove, "Deletes the 0-based index i'th pair.\n \n\n std::exception On index out of bounds.\n \n\n insert, get, set\n\nC++: mrpt::maps::CSimpleMap::remove(size_t) --> void", pybind11::arg("index")); - cl.def("insert", (void (mrpt::maps::CSimpleMap::*)(const class mrpt::poses::CPose3DPDF &, const class mrpt::obs::CSensoryFrame &)) &mrpt::maps::CSimpleMap::insert, "Adds a new keyframe (SE(3) pose) to the view-based map, making a deep\n copy of the pose PDF (observations within the SF are always copied as\n `shared_ptr`s).\n\nC++: mrpt::maps::CSimpleMap::insert(const class mrpt::poses::CPose3DPDF &, const class mrpt::obs::CSensoryFrame &) --> void", pybind11::arg("in_posePDF"), pybind11::arg("in_SF")); - cl.def("insert", (void (mrpt::maps::CSimpleMap::*)(const class std::shared_ptr &, const class std::shared_ptr &)) &mrpt::maps::CSimpleMap::insert, "Adds a new keyframe (SE(3) pose) to the view-based map.\n Both shared pointers are copied (shallow object copies).\n\nC++: mrpt::maps::CSimpleMap::insert(const class std::shared_ptr &, const class std::shared_ptr &) --> void", pybind11::arg("in_posePDF"), pybind11::arg("in_SF")); - cl.def("insert", (void (mrpt::maps::CSimpleMap::*)(const struct mrpt::maps::CSimpleMap::Pair &)) &mrpt::maps::CSimpleMap::insert, "C++: mrpt::maps::CSimpleMap::insert(const struct mrpt::maps::CSimpleMap::Pair &) --> void", pybind11::arg("poseSF")); - cl.def("insert", (void (mrpt::maps::CSimpleMap::*)(const class mrpt::poses::CPosePDF &, const class mrpt::obs::CSensoryFrame &)) &mrpt::maps::CSimpleMap::insert, "Adds a new keyframe (SE(2) pose) to the view-based map, making a deep\n copy of the pose PDF (observations within the SF are always copied as\n `shared_ptr`s).\n\nC++: mrpt::maps::CSimpleMap::insert(const class mrpt::poses::CPosePDF &, const class mrpt::obs::CSensoryFrame &) --> void", pybind11::arg("in_posePDF"), pybind11::arg("in_SF")); - cl.def("insert", (void (mrpt::maps::CSimpleMap::*)(const class std::shared_ptr &, const class std::shared_ptr &)) &mrpt::maps::CSimpleMap::insert, "Adds a new keyframe (SE(2) pose) to the view-based map.\n Both shared pointers are copied (shallow object copies).\n\nC++: mrpt::maps::CSimpleMap::insert(const class std::shared_ptr &, const class std::shared_ptr &) --> void", pybind11::arg("in_posePDF"), pybind11::arg("in_SF")); - cl.def("clear", (void (mrpt::maps::CSimpleMap::*)()) &mrpt::maps::CSimpleMap::clear, "Remove all stored pairs. \n remove \n\nC++: mrpt::maps::CSimpleMap::clear() --> void"); - cl.def("changeCoordinatesOrigin", (void (mrpt::maps::CSimpleMap::*)(const class mrpt::poses::CPose3D &)) &mrpt::maps::CSimpleMap::changeCoordinatesOrigin, "Change the coordinate origin of all stored poses, that is, translates\n and rotates the map such that the old SE(3) origin (identity\n transformation) becomes the new provided one.\n\nC++: mrpt::maps::CSimpleMap::changeCoordinatesOrigin(const class mrpt::poses::CPose3D &) --> void", pybind11::arg("newOrigin")); - - { // mrpt::maps::CSimpleMap::Pair file:mrpt/maps/CSimpleMap.h line:56 - auto & enclosing_class = cl; - pybind11::class_> cl(enclosing_class, "Pair", ""); - cl.def( pybind11::init( [](){ return new mrpt::maps::CSimpleMap::Pair(); } ) ); - cl.def( pybind11::init( [](mrpt::maps::CSimpleMap::Pair const &o){ return new mrpt::maps::CSimpleMap::Pair(o); } ) ); - cl.def_readwrite("pose", &mrpt::maps::CSimpleMap::Pair::pose); - cl.def_readwrite("sf", &mrpt::maps::CSimpleMap::Pair::sf); - cl.def("assign", (struct mrpt::maps::CSimpleMap::Pair & (mrpt::maps::CSimpleMap::Pair::*)(const struct mrpt::maps::CSimpleMap::Pair &)) &mrpt::maps::CSimpleMap::Pair::operator=, "C++: mrpt::maps::CSimpleMap::Pair::operator=(const struct mrpt::maps::CSimpleMap::Pair &) --> struct mrpt::maps::CSimpleMap::Pair &", pybind11::return_value_policy::automatic, pybind11::arg("")); - } - - { // mrpt::maps::CSimpleMap::ConstPair file:mrpt/maps/CSimpleMap.h line:65 - auto & enclosing_class = cl; - pybind11::class_> cl(enclosing_class, "ConstPair", ""); - cl.def( pybind11::init( [](){ return new mrpt::maps::CSimpleMap::ConstPair(); } ) ); - cl.def( pybind11::init(), pybind11::arg("p") ); - - cl.def( pybind11::init( [](mrpt::maps::CSimpleMap::ConstPair const &o){ return new mrpt::maps::CSimpleMap::ConstPair(o); } ) ); - cl.def_readwrite("pose", &mrpt::maps::CSimpleMap::ConstPair::pose); - cl.def_readwrite("sf", &mrpt::maps::CSimpleMap::ConstPair::sf); - cl.def("assign", (struct mrpt::maps::CSimpleMap::ConstPair & (mrpt::maps::CSimpleMap::ConstPair::*)(const struct mrpt::maps::CSimpleMap::ConstPair &)) &mrpt::maps::CSimpleMap::ConstPair::operator=, "C++: mrpt::maps::CSimpleMap::ConstPair::operator=(const struct mrpt::maps::CSimpleMap::ConstPair &) --> struct mrpt::maps::CSimpleMap::ConstPair &", pybind11::return_value_policy::automatic, pybind11::arg("")); - } - - } } diff --git a/python/src/mrpt/maps/CMultiMetricMapPDF.cpp b/python/src/mrpt/maps/CMultiMetricMapPDF.cpp index e6b7d92b80..0318ba2527 100644 --- a/python/src/mrpt/maps/CMultiMetricMapPDF.cpp +++ b/python/src/mrpt/maps/CMultiMetricMapPDF.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/mrpt/maps/COccupancyGridMap2D.cpp b/python/src/mrpt/maps/COccupancyGridMap2D.cpp index 9ba6f5aec2..6e5ee4fce2 100644 --- a/python/src/mrpt/maps/COccupancyGridMap2D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap2D.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -78,7 +79,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/mrpt/maps/COccupancyGridMap3D.cpp b/python/src/mrpt/maps/COccupancyGridMap3D.cpp index 72772ebc28..3a791eb55b 100644 --- a/python/src/mrpt/maps/COccupancyGridMap3D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap3D.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include @@ -71,7 +72,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/mrpt/maps/COctoMap.cpp b/python/src/mrpt/maps/COctoMap.cpp index 0d0a72f709..b48883b808 100644 --- a/python/src/mrpt/maps/COctoMap.cpp +++ b/python/src/mrpt/maps/COctoMap.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -78,7 +79,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/mrpt/maps/COctoMapBase.cpp b/python/src/mrpt/maps/COctoMapBase.cpp index c08e39eb80..6118923961 100644 --- a/python/src/mrpt/maps/COctoMapBase.cpp +++ b/python/src/mrpt/maps/COctoMapBase.cpp @@ -166,8 +166,7 @@ void bind_mrpt_maps_COctoMapBase(std::function< pybind11::module &(std::string c cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::boundingBox, "Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the\n default value of mrpt::math::TBoundingBoxf() if not implemented in the\n derived class or the map is empty.\n\nC++: mrpt::maps::CMetricMap::boundingBox() const --> struct mrpt::math::TBoundingBox_"); - cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); - cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("obs"), pybind11::arg("robotPose")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CSensoryFrame & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("sf")); diff --git a/python/src/mrpt/maps/COctoMapBase_1.cpp b/python/src/mrpt/maps/COctoMapBase_1.cpp index 265b29995f..faa8b45bfb 100644 --- a/python/src/mrpt/maps/COctoMapBase_1.cpp +++ b/python/src/mrpt/maps/COctoMapBase_1.cpp @@ -166,8 +166,7 @@ void bind_mrpt_maps_COctoMapBase_1(std::function< pybind11::module &(std::string cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::boundingBox, "Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the\n default value of mrpt::math::TBoundingBoxf() if not implemented in the\n derived class or the map is empty.\n\nC++: mrpt::maps::CMetricMap::boundingBox() const --> struct mrpt::math::TBoundingBox_"); - cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); - cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("obs"), pybind11::arg("robotPose")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CSensoryFrame & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("sf")); diff --git a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp index fac1e6cf8f..04ec6cc83b 100644 --- a/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp +++ b/python/src/mrpt/maps/CPointCloudFilterByDistance.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -77,7 +78,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/mrpt/maps/CPointsMapXYZI.cpp b/python/src/mrpt/maps/CPointsMapXYZI.cpp index 0730248630..ca257d79ca 100644 --- a/python/src/mrpt/maps/CPointsMapXYZI.cpp +++ b/python/src/mrpt/maps/CPointsMapXYZI.cpp @@ -32,7 +32,7 @@ void bind_mrpt_maps_CPointsMapXYZI(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::opengl::PointCloudAdapter file:mrpt/maps/CPointsMapXYZI.h line:304 + { // mrpt::opengl::PointCloudAdapter file:mrpt/maps/CPointsMapXYZI.h line:309 pybind11::class_, std::shared_ptr>> cl(M("mrpt::opengl"), "PointCloudAdapter_mrpt_maps_CPointsMapXYZI_t", "Specialization\n mrpt::opengl::PointCloudAdapter\n \n"); cl.def( pybind11::init(), pybind11::arg("obj") ); diff --git a/python/src/mrpt/maps/CPointsMap_1.cpp b/python/src/mrpt/maps/CPointsMap_1.cpp index 3fcae64cb3..bbc956bd98 100644 --- a/python/src/mrpt/maps/CPointsMap_1.cpp +++ b/python/src/mrpt/maps/CPointsMap_1.cpp @@ -39,7 +39,7 @@ void bind_mrpt_maps_CPointsMap_1(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::opengl::PointCloudAdapter file:mrpt/maps/CPointsMap.h line:1254 + { // mrpt::opengl::PointCloudAdapter file:mrpt/maps/CPointsMap.h line:1291 pybind11::class_, std::shared_ptr>> cl(M("mrpt::opengl"), "PointCloudAdapter_mrpt_maps_CPointsMap_t", "Specialization mrpt::opengl::PointCloudAdapter\n \n"); cl.def( pybind11::init(), pybind11::arg("obj") ); diff --git a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp index 09244fb041..a86a2c0bd3 100644 --- a/python/src/mrpt/maps/CReflectivityGridMap2D.cpp +++ b/python/src/mrpt/maps/CReflectivityGridMap2D.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -80,7 +81,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/mrpt/maps/CSimpleMap.cpp b/python/src/mrpt/maps/CSimpleMap.cpp new file mode 100644 index 0000000000..ece5108608 --- /dev/null +++ b/python/src/mrpt/maps/CSimpleMap.cpp @@ -0,0 +1,148 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // __str__ +#include +#include + +#include +#include +#include +#include + + +#ifndef BINDER_PYBIND11_TYPE_CASTER + #define BINDER_PYBIND11_TYPE_CASTER + PYBIND11_DECLARE_HOLDER_TYPE(T, std::shared_ptr) + PYBIND11_DECLARE_HOLDER_TYPE(T, T*) + PYBIND11_MAKE_OPAQUE(std::shared_ptr) +#endif + +// mrpt::maps::CSimpleMap file:mrpt/maps/CSimpleMap.h line:57 +struct PyCallBack_mrpt_maps_CSimpleMap : public mrpt::maps::CSimpleMap { + using mrpt::maps::CSimpleMap::CSimpleMap; + + const struct mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "GetRuntimeClass"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CSimpleMap::GetRuntimeClass(); + } + class mrpt::rtti::CObject * clone() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "clone"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CSimpleMap::clone(); + } + uint8_t serializeGetVersion() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "serializeGetVersion"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CSimpleMap::serializeGetVersion(); + } + void serializeTo(class mrpt::serialization::CArchive & a0) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "serializeTo"); + if (overload) { + auto o = overload.operator()(a0); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CSimpleMap::serializeTo(a0); + } + void serializeFrom(class mrpt::serialization::CArchive & a0, uint8_t a1) override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "serializeFrom"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CSimpleMap::serializeFrom(a0, a1); + } +}; + +void bind_mrpt_maps_CSimpleMap(std::function< pybind11::module &(std::string const &namespace_) > &M) +{ + { // mrpt::maps::CSimpleMap file:mrpt/maps/CSimpleMap.h line:57 + pybind11::class_, PyCallBack_mrpt_maps_CSimpleMap, mrpt::serialization::CSerializable> cl(M("mrpt::maps"), "CSimpleMap", "A view-based map: a set of poses and what the robot saw from those poses.\n\n A simplemap comprises a sequence of tuples, each containing:\n - The **keyframe SE(3) pose** of the robot, including (optionally) its\n uncertainty, as instances of mrpt::poses::CPose3DPDF\n - The **raw observations** from that keyframe, in a mrpt::obs::CSensoryFrame\n - Optionally, the **twist** (linear and angular velocity) of the robot in the\n local frame of reference, at that moment. It can be used to undistort data\n from a rotatory lidar, for example.\n\n To generate an actual metric map (occupancy grid, point cloud, octomap, etc.)\n from a \"simple map\", the user must instantiate the desired metric map\n class(es) and invoke its virtual method\n mrpt::maps::CMetricMap::loadFromSimpleMap().\n\n Users can also use the new top-level [library\n mp2p_icp_filters](https://github.com/MOLAorg/mp2p_icp/) and its CLI\n application\n [sm2mm](https://github.com/MOLAorg/mp2p_icp/tree/master/apps/sm2mm)\n (simple-map to metric-map)\n to generate metric maps including pre-processing of raw data in a flexible\n way.\n\n To programatically change an existing simplemap, use the non-const get()\n method and modify the returned reference.\n\n \n Objects of this class are serialized into GZ-compressed files with\n the extension `.simplemap`.\n See [Robotics file formats](robotics_file_formats.html).\n\n \n mrpt::obs::CSensoryFrame, mrpt::poses::CPose3DPDF,\n mrpt::maps::CMetricMap, https://github.com/MOLAorg/mp2p_icp/\n\n \n\n "); + cl.def( pybind11::init( [](){ return new mrpt::maps::CSimpleMap(); }, [](){ return new PyCallBack_mrpt_maps_CSimpleMap(); } ) ); + cl.def( pybind11::init( [](PyCallBack_mrpt_maps_CSimpleMap const &o){ return new PyCallBack_mrpt_maps_CSimpleMap(o); } ) ); + cl.def( pybind11::init( [](mrpt::maps::CSimpleMap const &o){ return new mrpt::maps::CSimpleMap(o); } ) ); + cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CSimpleMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CSimpleMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); + cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::maps::CSimpleMap::*)() const) &mrpt::maps::CSimpleMap::GetRuntimeClass, "C++: mrpt::maps::CSimpleMap::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); + cl.def("clone", (class mrpt::rtti::CObject * (mrpt::maps::CSimpleMap::*)() const) &mrpt::maps::CSimpleMap::clone, "C++: mrpt::maps::CSimpleMap::clone() const --> class mrpt::rtti::CObject *", pybind11::return_value_policy::automatic); + cl.def_static("CreateObject", (class std::shared_ptr (*)()) &mrpt::maps::CSimpleMap::CreateObject, "C++: mrpt::maps::CSimpleMap::CreateObject() --> class std::shared_ptr"); + cl.def("assign", (class mrpt::maps::CSimpleMap & (mrpt::maps::CSimpleMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CSimpleMap::operator=, "Copy, making a deep copy of all data. \n\nC++: mrpt::maps::CSimpleMap::operator=(const class mrpt::maps::CSimpleMap &) --> class mrpt::maps::CSimpleMap &", pybind11::return_value_policy::automatic, pybind11::arg("o")); + cl.def("saveToFile", (bool (mrpt::maps::CSimpleMap::*)(const std::string &) const) &mrpt::maps::CSimpleMap::saveToFile, "Save this object to a .simplemap binary file (compressed with gzip)\n See [Robotics file formats](robotics_file_formats.html).\n \n\n loadFromFile()\n \n\n false on any error. \n\nC++: mrpt::maps::CSimpleMap::saveToFile(const std::string &) const --> bool", pybind11::arg("filName")); + cl.def("loadFromFile", (bool (mrpt::maps::CSimpleMap::*)(const std::string &)) &mrpt::maps::CSimpleMap::loadFromFile, "Load the contents of this object from a .simplemap binary file (possibly\n compressed with gzip)\n See [Robotics file formats](robotics_file_formats.html).\n \n\n saveToFile()\n \n\n false on any error. \n\nC++: mrpt::maps::CSimpleMap::loadFromFile(const std::string &) --> bool", pybind11::arg("filName")); + cl.def("size", (size_t (mrpt::maps::CSimpleMap::*)() const) &mrpt::maps::CSimpleMap::size, "Returns the number of keyframes in the map \n\nC++: mrpt::maps::CSimpleMap::size() const --> size_t"); + cl.def("empty", (bool (mrpt::maps::CSimpleMap::*)() const) &mrpt::maps::CSimpleMap::empty, "Returns size()!=0 \n\nC++: mrpt::maps::CSimpleMap::empty() const --> bool"); + cl.def("get", (struct mrpt::maps::CSimpleMap::Keyframe & (mrpt::maps::CSimpleMap::*)(size_t)) &mrpt::maps::CSimpleMap::get, "non-const accessor, returning a reference suitable for modification\n\nC++: mrpt::maps::CSimpleMap::get(size_t) --> struct mrpt::maps::CSimpleMap::Keyframe &", pybind11::return_value_policy::automatic, pybind11::arg("index")); + cl.def("remove", (void (mrpt::maps::CSimpleMap::*)(size_t)) &mrpt::maps::CSimpleMap::remove, "Deletes the 0-based index i'th keyframe.\n \n\n std::exception On index out of bounds.\n \n\n insert, get, set\n\nC++: mrpt::maps::CSimpleMap::remove(size_t) --> void", pybind11::arg("index")); + cl.def("insert", (void (mrpt::maps::CSimpleMap::*)(const struct mrpt::maps::CSimpleMap::Keyframe &)) &mrpt::maps::CSimpleMap::insert, "Adds a new keyframe (SE(3) pose) to the view-based map.\n Both shared pointers are copied (shallow object copies).\n\nC++: mrpt::maps::CSimpleMap::insert(const struct mrpt::maps::CSimpleMap::Keyframe &) --> void", pybind11::arg("kf")); + cl.def("clear", (void (mrpt::maps::CSimpleMap::*)()) &mrpt::maps::CSimpleMap::clear, "Remove all stored keyframes. \n remove \n\nC++: mrpt::maps::CSimpleMap::clear() --> void"); + cl.def("changeCoordinatesOrigin", (void (mrpt::maps::CSimpleMap::*)(const class mrpt::poses::CPose3D &)) &mrpt::maps::CSimpleMap::changeCoordinatesOrigin, "Change the coordinate origin of all stored poses, that is, translates\n and rotates the map such that the old SE(3) origin (identity\n transformation) becomes the new provided one.\n\nC++: mrpt::maps::CSimpleMap::changeCoordinatesOrigin(const class mrpt::poses::CPose3D &) --> void", pybind11::arg("newOrigin")); + + { // mrpt::maps::CSimpleMap::Keyframe file:mrpt/maps/CSimpleMap.h line:70 + auto & enclosing_class = cl; + pybind11::class_> cl(enclosing_class, "Keyframe", ""); + cl.def( pybind11::init( [](){ return new mrpt::maps::CSimpleMap::Keyframe(); } ) ); + cl.def( pybind11::init( [](mrpt::maps::CSimpleMap::Keyframe const &o){ return new mrpt::maps::CSimpleMap::Keyframe(o); } ) ); + cl.def_readwrite("pose", &mrpt::maps::CSimpleMap::Keyframe::pose); + cl.def_readwrite("sf", &mrpt::maps::CSimpleMap::Keyframe::sf); + cl.def_readwrite("localTwist", &mrpt::maps::CSimpleMap::Keyframe::localTwist); + cl.def("assign", (struct mrpt::maps::CSimpleMap::Keyframe & (mrpt::maps::CSimpleMap::Keyframe::*)(const struct mrpt::maps::CSimpleMap::Keyframe &)) &mrpt::maps::CSimpleMap::Keyframe::operator=, "C++: mrpt::maps::CSimpleMap::Keyframe::operator=(const struct mrpt::maps::CSimpleMap::Keyframe &) --> struct mrpt::maps::CSimpleMap::Keyframe &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } + + } +} diff --git a/python/src/mrpt/maps/CVoxelMap.cpp b/python/src/mrpt/maps/CVoxelMap.cpp index 6955a6caf6..e8bc80b5d8 100644 --- a/python/src/mrpt/maps/CVoxelMap.cpp +++ b/python/src/mrpt/maps/CVoxelMap.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -71,7 +72,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/mrpt/maps/CVoxelMapBase.cpp b/python/src/mrpt/maps/CVoxelMapBase.cpp index 64a11658b0..8d31660086 100644 --- a/python/src/mrpt/maps/CVoxelMapBase.cpp +++ b/python/src/mrpt/maps/CVoxelMapBase.cpp @@ -52,8 +52,7 @@ void bind_mrpt_maps_CVoxelMapBase(std::function< pybind11::module &(std::string cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::boundingBox, "Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the\n default value of mrpt::math::TBoundingBoxf() if not implemented in the\n derived class or the map is empty.\n\nC++: mrpt::maps::CMetricMap::boundingBox() const --> struct mrpt::math::TBoundingBox_"); - cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); - cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("obs"), pybind11::arg("robotPose")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CSensoryFrame & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("sf")); @@ -84,8 +83,7 @@ void bind_mrpt_maps_CVoxelMapBase(std::function< pybind11::module &(std::string cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::boundingBox, "Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the\n default value of mrpt::math::TBoundingBoxf() if not implemented in the\n derived class or the map is empty.\n\nC++: mrpt::maps::CMetricMap::boundingBox() const --> struct mrpt::math::TBoundingBox_"); - cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); - cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("obs"), pybind11::arg("robotPose")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CSensoryFrame & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("sf")); diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp index 7fbf0e079e..6cd258ca20 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp @@ -85,8 +85,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(s cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::boundingBox, "Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the\n default value of mrpt::math::TBoundingBoxf() if not implemented in the\n derived class or the map is empty.\n\nC++: mrpt::maps::CMetricMap::boundingBox() const --> struct mrpt::math::TBoundingBox_"); - cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); - cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("obs"), pybind11::arg("robotPose")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CSensoryFrame & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("sf")); diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp index d24835a7dd..f76785b4a5 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp @@ -85,8 +85,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(s cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); cl.def("boundingBox", (struct mrpt::math::TBoundingBox_ (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::boundingBox, "Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the\n default value of mrpt::math::TBoundingBoxf() if not implemented in the\n derived class or the map is empty.\n\nC++: mrpt::maps::CMetricMap::boundingBox() const --> struct mrpt::math::TBoundingBox_"); - cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); - cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("obs"), pybind11::arg("robotPose")); cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CSensoryFrame & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("sf")); diff --git a/python/src/mrpt/maps/CWirelessPowerGridMap2D.cpp b/python/src/mrpt/maps/CWirelessPowerGridMap2D.cpp index 0f9d9bbd61..498de30614 100644 --- a/python/src/mrpt/maps/CWirelessPowerGridMap2D.cpp +++ b/python/src/mrpt/maps/CWirelessPowerGridMap2D.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -73,7 +74,6 @@ #include #include #include -#include #include #include #include diff --git a/python/src/mrpt/math/TTwist3D.cpp b/python/src/mrpt/math/TTwist3D.cpp index 174b12b147..714012d67b 100644 --- a/python/src/mrpt/math/TTwist3D.cpp +++ b/python/src/mrpt/math/TTwist3D.cpp @@ -49,5 +49,6 @@ void bind_mrpt_math_TTwist3D(std::function< pybind11::module &(std::string const cl.def("rotated", (struct mrpt::math::TTwist3D (mrpt::math::TTwist3D::*)(const struct mrpt::math::TPose3D &) const) &mrpt::math::TTwist3D::rotated, "Like rotate(), but returning a copy of the rotated twist.\n \n\n New in MRPT 2.3.2 \n\nC++: mrpt::math::TTwist3D::rotated(const struct mrpt::math::TPose3D &) const --> struct mrpt::math::TTwist3D", pybind11::arg("rot")); cl.def("fromString", (void (mrpt::math::TTwist3D::*)(const std::string &)) &mrpt::math::TTwist3D::fromString, "Set the current object value from a string generated by 'asString' (eg:\n \"[vx vy vz wx wy wz]\" )\n \n\n asString\n \n\n std::exception On invalid format\n\nC++: mrpt::math::TTwist3D::fromString(const std::string &) --> void", pybind11::arg("s")); cl.def_static("FromString", (struct mrpt::math::TTwist3D (*)(const std::string &)) &mrpt::math::TTwist3D::FromString, "C++: mrpt::math::TTwist3D::FromString(const std::string &) --> struct mrpt::math::TTwist3D", pybind11::arg("s")); + cl.def("assign", (struct mrpt::math::TTwist3D & (mrpt::math::TTwist3D::*)(const struct mrpt::math::TTwist3D &)) &mrpt::math::TTwist3D::operator=, "C++: mrpt::math::TTwist3D::operator=(const struct mrpt::math::TTwist3D &) --> struct mrpt::math::TTwist3D &", pybind11::return_value_policy::automatic, pybind11::arg("")); } } diff --git a/python/src/mrpt/slam/CIncrementalMapPartitioner.cpp b/python/src/mrpt/slam/CIncrementalMapPartitioner.cpp index b9e3f23bf1..768ded4af8 100644 --- a/python/src/mrpt/slam/CIncrementalMapPartitioner.cpp +++ b/python/src/mrpt/slam/CIncrementalMapPartitioner.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -36,7 +37,6 @@ #include #include // __str__ #include -#include #include #include #include diff --git a/python/src/mrpt/slam/CMetricMapBuilder.cpp b/python/src/mrpt/slam/CMetricMapBuilder.cpp index 0e0a1e67dc..62f0e656f1 100644 --- a/python/src/mrpt/slam/CMetricMapBuilder.cpp +++ b/python/src/mrpt/slam/CMetricMapBuilder.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/mrpt/slam/CMetricMapBuilderICP.cpp b/python/src/mrpt/slam/CMetricMapBuilderICP.cpp index 5e8ce1ba60..3e5c11fe0f 100644 --- a/python/src/mrpt/slam/CMetricMapBuilderICP.cpp +++ b/python/src/mrpt/slam/CMetricMapBuilderICP.cpp @@ -31,6 +31,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/pymrpt.cpp b/python/src/pymrpt.cpp index 9bc222edb2..b0dd90f7db 100644 --- a/python/src/pymrpt.cpp +++ b/python/src/pymrpt.cpp @@ -106,6 +106,8 @@ void bind_mrpt_apps_MonteCarloLocalization_App(std::function< pybind11::module & void bind_mrpt_img_TColor(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_img_CCanvas(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_maps_CMultiMetricMap(std::function< pybind11::module &(std::string const &namespace_) > &M); +void bind_mrpt_math_TTwist3D(std::function< pybind11::module &(std::string const &namespace_) > &M); +void bind_mrpt_maps_CSimpleMap(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_bayes_CProbabilityParticle(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_bayes_CParticleFilterData(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_bayes_CParticleFilterData_1(std::function< pybind11::module &(std::string const &namespace_) > &M); @@ -288,7 +290,6 @@ void bind_mrpt_opengl_CSetOfTriangles(std::function< pybind11::module &(std::str void bind_mrpt_opengl_CPlanarLaserScan(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_math_CAtan2LookUpTable(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_math_ops_containers(std::function< pybind11::module &(std::string const &namespace_) > &M); -void bind_mrpt_math_TTwist3D(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_math_data_utils(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_math_fresnel(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_nav_holonomic_ClearanceDiagram(std::function< pybind11::module &(std::string const &namespace_) > &M); @@ -536,6 +537,8 @@ PYBIND11_MODULE(pymrpt, root_module) { bind_mrpt_img_TColor(M); bind_mrpt_img_CCanvas(M); bind_mrpt_maps_CMultiMetricMap(M); + bind_mrpt_math_TTwist3D(M); + bind_mrpt_maps_CSimpleMap(M); bind_mrpt_bayes_CProbabilityParticle(M); bind_mrpt_bayes_CParticleFilterData(M); bind_mrpt_bayes_CParticleFilterData_1(M); @@ -718,7 +721,6 @@ PYBIND11_MODULE(pymrpt, root_module) { bind_mrpt_opengl_CPlanarLaserScan(M); bind_mrpt_math_CAtan2LookUpTable(M); bind_mrpt_math_ops_containers(M); - bind_mrpt_math_TTwist3D(M); bind_mrpt_math_data_utils(M); bind_mrpt_math_fresnel(M); bind_mrpt_nav_holonomic_ClearanceDiagram(M); diff --git a/python/src/pymrpt.sources b/python/src/pymrpt.sources index 5015a1f833..d182f5c286 100644 --- a/python/src/pymrpt.sources +++ b/python/src/pymrpt.sources @@ -96,6 +96,8 @@ mrpt/apps/MonteCarloLocalization_App.cpp mrpt/img/TColor.cpp mrpt/img/CCanvas.cpp mrpt/maps/CMultiMetricMap.cpp +mrpt/math/TTwist3D.cpp +mrpt/maps/CSimpleMap.cpp mrpt/bayes/CProbabilityParticle.cpp mrpt/bayes/CParticleFilterData.cpp mrpt/bayes/CParticleFilterData_1.cpp @@ -278,7 +280,6 @@ mrpt/opengl/CSetOfTriangles.cpp mrpt/opengl/CPlanarLaserScan.cpp mrpt/math/CAtan2LookUpTable.cpp mrpt/math/ops_containers.cpp -mrpt/math/TTwist3D.cpp mrpt/math/data_utils.cpp mrpt/math/fresnel.cpp mrpt/nav/holonomic/ClearanceDiagram.cpp diff --git a/python/src/unknown/unknown_6.cpp b/python/src/unknown/unknown_6.cpp index 4a99579891..05fb332486 100644 --- a/python/src/unknown/unknown_6.cpp +++ b/python/src/unknown/unknown_6.cpp @@ -115,7 +115,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_GENERIC_SHORT_FRAME : public mrp } }; -// mrpt::obs::gnss::Message_NV_OEM6_BESTPOS file: line:82 +// mrpt::obs::gnss::Message_NV_OEM6_BESTPOS file: line:86 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_BESTPOS : public mrpt::obs::gnss::Message_NV_OEM6_BESTPOS { using mrpt::obs::gnss::Message_NV_OEM6_BESTPOS::Message_NV_OEM6_BESTPOS; @@ -160,7 +160,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_BESTPOS : public mrpt::obs::gnss } }; -// mrpt::obs::gnss::Message_NV_OEM6_INSPVAS file: line:88 +// mrpt::obs::gnss::Message_NV_OEM6_INSPVAS file: line:92 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSPVAS : public mrpt::obs::gnss::Message_NV_OEM6_INSPVAS { using mrpt::obs::gnss::Message_NV_OEM6_INSPVAS::Message_NV_OEM6_INSPVAS; @@ -205,7 +205,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSPVAS : public mrpt::obs::gnss } }; -// mrpt::obs::gnss::Message_NV_OEM6_INSCOVS file: line:94 +// mrpt::obs::gnss::Message_NV_OEM6_INSCOVS file: line:98 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSCOVS : public mrpt::obs::gnss::Message_NV_OEM6_INSCOVS { using mrpt::obs::gnss::Message_NV_OEM6_INSCOVS::Message_NV_OEM6_INSCOVS; @@ -317,7 +317,7 @@ void bind_unknown_unknown_6(std::function< pybind11::module &(std::string const cl.def("fixEndianness", (void (mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME::*)()) &mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME::fixEndianness, "C++: mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME::fixEndianness() --> void"); cl.def("assign", (struct mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME & (mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME::*)(const struct mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME &)) &mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME::operator=, "C++: mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME::operator=(const struct mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME &) --> struct mrpt::obs::gnss::Message_NV_OEM6_GENERIC_SHORT_FRAME &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::obs::gnss::Message_NV_OEM6_BESTPOS file: line:82 + { // mrpt::obs::gnss::Message_NV_OEM6_BESTPOS file: line:86 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_BESTPOS, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_BESTPOS", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_BESTPOS(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_BESTPOS(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_BESTPOS const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_BESTPOS(o); } ) ); @@ -357,7 +357,7 @@ void bind_unknown_unknown_6(std::function< pybind11::module &(std::string const } } - { // mrpt::obs::gnss::Message_NV_OEM6_INSPVAS file: line:88 + { // mrpt::obs::gnss::Message_NV_OEM6_INSPVAS file: line:92 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSPVAS, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_INSPVAS", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_INSPVAS(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSPVAS(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSPVAS const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSPVAS(o); } ) ); @@ -389,7 +389,7 @@ void bind_unknown_unknown_6(std::function< pybind11::module &(std::string const } } - { // mrpt::obs::gnss::Message_NV_OEM6_INSCOVS file: line:94 + { // mrpt::obs::gnss::Message_NV_OEM6_INSCOVS file: line:98 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSCOVS, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_INSCOVS", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_INSCOVS(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSCOVS(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSCOVS const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_INSCOVS(o); } ) ); diff --git a/python/src/unknown/unknown_7.cpp b/python/src/unknown/unknown_7.cpp index 6955b09b24..8cbcc882ef 100644 --- a/python/src/unknown/unknown_7.cpp +++ b/python/src/unknown/unknown_7.cpp @@ -25,7 +25,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::obs::gnss::Message_NV_OEM6_RXSTATUS file: line:100 +// mrpt::obs::gnss::Message_NV_OEM6_RXSTATUS file: line:104 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RXSTATUS : public mrpt::obs::gnss::Message_NV_OEM6_RXSTATUS { using mrpt::obs::gnss::Message_NV_OEM6_RXSTATUS::Message_NV_OEM6_RXSTATUS; @@ -70,7 +70,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RXSTATUS : public mrpt::obs::gns } }; -// mrpt::obs::gnss::Message_NV_OEM6_RAWEPHEM file: line:106 +// mrpt::obs::gnss::Message_NV_OEM6_RAWEPHEM file: line:110 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWEPHEM : public mrpt::obs::gnss::Message_NV_OEM6_RAWEPHEM { using mrpt::obs::gnss::Message_NV_OEM6_RAWEPHEM::Message_NV_OEM6_RAWEPHEM; @@ -160,7 +160,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_VERSION : public mrpt::obs::gnss } }; -// mrpt::obs::gnss::Message_NV_OEM6_RAWIMUS file: line:112 +// mrpt::obs::gnss::Message_NV_OEM6_RAWIMUS file: line:116 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWIMUS : public mrpt::obs::gnss::Message_NV_OEM6_RAWIMUS { using mrpt::obs::gnss::Message_NV_OEM6_RAWIMUS::Message_NV_OEM6_RAWIMUS; @@ -205,7 +205,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWIMUS : public mrpt::obs::gnss } }; -// mrpt::obs::gnss::Message_NV_OEM6_MARKPOS file: line:118 +// mrpt::obs::gnss::Message_NV_OEM6_MARKPOS file: line:122 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKPOS : public mrpt::obs::gnss::Message_NV_OEM6_MARKPOS { using mrpt::obs::gnss::Message_NV_OEM6_MARKPOS::Message_NV_OEM6_MARKPOS; @@ -252,7 +252,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKPOS : public mrpt::obs::gnss void bind_unknown_unknown_7(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::obs::gnss::Message_NV_OEM6_RXSTATUS file: line:100 + { // mrpt::obs::gnss::Message_NV_OEM6_RXSTATUS file: line:104 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RXSTATUS, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_RXSTATUS", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_RXSTATUS(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RXSTATUS(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RXSTATUS const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RXSTATUS(o); } ) ); @@ -290,7 +290,7 @@ void bind_unknown_unknown_7(std::function< pybind11::module &(std::string const } } - { // mrpt::obs::gnss::Message_NV_OEM6_RAWEPHEM file: line:106 + { // mrpt::obs::gnss::Message_NV_OEM6_RAWEPHEM file: line:110 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWEPHEM, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_RAWEPHEM", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_RAWEPHEM(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWEPHEM(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWEPHEM const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWEPHEM(o); } ) ); @@ -333,7 +333,7 @@ void bind_unknown_unknown_7(std::function< pybind11::module &(std::string const } } - { // mrpt::obs::gnss::Message_NV_OEM6_RAWIMUS file: line:112 + { // mrpt::obs::gnss::Message_NV_OEM6_RAWIMUS file: line:116 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWIMUS, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_RAWIMUS", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_RAWIMUS(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWIMUS(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWIMUS const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_RAWIMUS(o); } ) ); @@ -362,7 +362,7 @@ void bind_unknown_unknown_7(std::function< pybind11::module &(std::string const } } - { // mrpt::obs::gnss::Message_NV_OEM6_MARKPOS file: line:118 + { // mrpt::obs::gnss::Message_NV_OEM6_MARKPOS file: line:122 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKPOS, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_MARKPOS", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_MARKPOS(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKPOS(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKPOS const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKPOS(o); } ) ); diff --git a/python/src/unknown/unknown_8.cpp b/python/src/unknown/unknown_8.cpp index ac4aaccbb1..a475ccb6c0 100644 --- a/python/src/unknown/unknown_8.cpp +++ b/python/src/unknown/unknown_8.cpp @@ -25,7 +25,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::obs::gnss::Message_NV_OEM6_MARKTIME file: line:124 +// mrpt::obs::gnss::Message_NV_OEM6_MARKTIME file: line:128 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKTIME : public mrpt::obs::gnss::Message_NV_OEM6_MARKTIME { using mrpt::obs::gnss::Message_NV_OEM6_MARKTIME::Message_NV_OEM6_MARKTIME; @@ -70,7 +70,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKTIME : public mrpt::obs::gns } }; -// mrpt::obs::gnss::Message_NV_OEM6_MARK2TIME file: line:130 +// mrpt::obs::gnss::Message_NV_OEM6_MARK2TIME file: line:5 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARK2TIME : public mrpt::obs::gnss::Message_NV_OEM6_MARK2TIME { using mrpt::obs::gnss::Message_NV_OEM6_MARK2TIME::Message_NV_OEM6_MARK2TIME; @@ -115,7 +115,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARK2TIME : public mrpt::obs::gn } }; -// mrpt::obs::gnss::Message_NV_OEM6_IONUTC file: line:6 +// mrpt::obs::gnss::Message_NV_OEM6_IONUTC file: line:11 struct PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_IONUTC : public mrpt::obs::gnss::Message_NV_OEM6_IONUTC { using mrpt::obs::gnss::Message_NV_OEM6_IONUTC::Message_NV_OEM6_IONUTC; @@ -252,7 +252,7 @@ struct PyCallBack_mrpt_obs_gnss_Message_TOPCON_SATS : public mrpt::obs::gnss::Me void bind_unknown_unknown_8(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::obs::gnss::Message_NV_OEM6_MARKTIME file: line:124 + { // mrpt::obs::gnss::Message_NV_OEM6_MARKTIME file: line:128 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKTIME, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_MARKTIME", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_MARKTIME(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKTIME(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKTIME const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARKTIME(o); } ) ); @@ -278,7 +278,7 @@ void bind_unknown_unknown_8(std::function< pybind11::module &(std::string const } } - { // mrpt::obs::gnss::Message_NV_OEM6_MARK2TIME file: line:130 + { // mrpt::obs::gnss::Message_NV_OEM6_MARK2TIME file: line:5 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARK2TIME, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_MARK2TIME", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_MARK2TIME(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARK2TIME(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARK2TIME const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_MARK2TIME(o); } ) ); @@ -304,7 +304,7 @@ void bind_unknown_unknown_8(std::function< pybind11::module &(std::string const } } - { // mrpt::obs::gnss::Message_NV_OEM6_IONUTC file: line:6 + { // mrpt::obs::gnss::Message_NV_OEM6_IONUTC file: line:11 pybind11::class_, PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_IONUTC, mrpt::obs::gnss::gnss_message> cl(M("mrpt::obs::gnss"), "Message_NV_OEM6_IONUTC", ""); cl.def( pybind11::init( [](){ return new mrpt::obs::gnss::Message_NV_OEM6_IONUTC(); }, [](){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_IONUTC(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_IONUTC const &o){ return new PyCallBack_mrpt_obs_gnss_Message_NV_OEM6_IONUTC(o); } ) ); diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/config.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/config.pyi index 35155a8807..04359036f2 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/config.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/config.pyi @@ -59,6 +59,7 @@ class CConfigFileBase: def getAllSections(classstd) -> void: ... def getContentAsYAML(self) -> str: ... def keyExists(self, section: str, key: str) -> bool: ... + def keys(self, section: str) -> List[str]: ... @overload def read_bool(self, section: str, name: str, defaultValue: bool) -> bool: ... @overload @@ -95,6 +96,7 @@ class CConfigFileBase: def sectionExists(self, section_name: str) -> bool: ... @overload def sectionExists(conststd) -> bool: ... + def sections(self) -> List[str]: ... @overload def setContentFromYAML(self, yaml_block: str) -> None: ... @overload diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi index ae626cfbc6..05e8fe7254 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi @@ -1223,10 +1223,6 @@ class CMetricMap(mrpt.pymrpt.mrpt.serialization.CSerializable, mrpt.pymrpt.mrpt. @overload def isEmpty() -> bool: ... @overload - def loadFromProbabilisticPosesAndObservations(self, Map) -> None: ... - @overload - def loadFromProbabilisticPosesAndObservations(constclassmrpt) -> void: ... - @overload def loadFromSimpleMap(self, Map) -> None: ... @overload def loadFromSimpleMap(constclassmrpt) -> void: ... @@ -2372,10 +2368,6 @@ class COctoMapBase_octomap_ColorOcTree_octomap_ColorOcTreeNode_t(CMetricMap): @overload def isEmpty() -> bool: ... @overload - def loadFromProbabilisticPosesAndObservations(self, Map: CSimpleMap) -> None: ... - @overload - def loadFromProbabilisticPosesAndObservations(constclassmrpt) -> void: ... - @overload def loadFromSimpleMap(self, Map: CSimpleMap) -> None: ... @overload def loadFromSimpleMap(constclassmrpt) -> void: ... @@ -2632,10 +2624,6 @@ class COctoMapBase_octomap_OcTree_octomap_OcTreeNode_t(CMetricMap): @overload def isEmpty() -> bool: ... @overload - def loadFromProbabilisticPosesAndObservations(self, Map: CSimpleMap) -> None: ... - @overload - def loadFromProbabilisticPosesAndObservations(constclassmrpt) -> void: ... - @overload def loadFromSimpleMap(self, Map: CSimpleMap) -> None: ... @overload def loadFromSimpleMap(constclassmrpt) -> void: ... @@ -2935,11 +2923,19 @@ class CPointsMap(CMetricMap, mrpt.pymrpt.mrpt.opengl.PLY_Importer, mrpt.pymrpt.m def mark_as_modified(self) -> None: ... @overload def mark_as_modified() -> void: ... + @overload def nn_has_indices_or_ids(self) -> bool: ... @overload + def nn_has_indices_or_ids() -> bool: ... + @overload def nn_index_count(self) -> int: ... @overload def nn_index_count() -> size_t: ... + def nn_prepare_for_2d_queries(self) -> None: ... + @overload + def nn_prepare_for_3d_queries(self) -> None: ... + @overload + def nn_prepare_for_3d_queries() -> void: ... @overload def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... @overload @@ -3489,25 +3485,15 @@ class CReflectivityGridMap2D(CMetricMap, mrpt.pymrpt.mrpt.containers.CDynamicGri def saveMetricMapRepresentationToFile(conststd) -> void: ... class CSimpleMap(mrpt.pymrpt.mrpt.serialization.CSerializable): - class ConstPair: + class Keyframe: + localTwist: Optional[mrpt.pymrpt.mrpt.math.TTwist3D] pose: mrpt.pymrpt.mrpt.poses.CPose3DPDF sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame @overload def __init__(self) -> None: ... @overload - def __init__(self, p: CSimpleMap.Pair) -> None: ... - @overload - def __init__(self, arg0: CSimpleMap.ConstPair) -> None: ... - def assign(self) -> CSimpleMap.ConstPair: ... - - class Pair: - pose: mrpt.pymrpt.mrpt.poses.CPose3DPDF - sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame - @overload - def __init__(self) -> None: ... - @overload - def __init__(self, arg0: CSimpleMap.Pair) -> None: ... - def assign(self) -> CSimpleMap.Pair: ... + def __init__(self, arg0: CSimpleMap.Keyframe) -> None: ... + def assign(self) -> CSimpleMap.Keyframe: ... @overload def __init__(self) -> None: ... @overload @@ -3531,26 +3517,12 @@ class CSimpleMap(mrpt.pymrpt.mrpt.serialization.CSerializable): def empty(self) -> bool: ... @overload def empty() -> bool: ... + def get(self, *args, **kwargs) -> Any: ... @overload - def get(self, index: int, out_posePDF: mrpt.pymrpt.mrpt.poses.CPose3DPDF, out_SF: mrpt.pymrpt.mrpt.obs.CSensoryFrame) -> None: ... - @overload - def get(self, index: int, out_posePDF: mrpt.pymrpt.mrpt.poses.CPose3DPDF, out_SF: mrpt.pymrpt.mrpt.obs.CSensoryFrame) -> None: ... - @overload - def get(self, index: int) -> Tuple[mrpt.pymrpt.mrpt.poses.CPose3DPDF,mrpt.pymrpt.mrpt.obs.CSensoryFrame]: ... - def getAsPair(self, *args, **kwargs) -> Any: ... - @overload - def insert(self, in_posePDF: mrpt.pymrpt.mrpt.poses.CPose3DPDF, in_SF: mrpt.pymrpt.mrpt.obs.CSensoryFrame) -> None: ... - @overload - def insert(self, in_posePDF: mrpt.pymrpt.mrpt.poses.CPose3DPDF, in_SF: mrpt.pymrpt.mrpt.obs.CSensoryFrame) -> None: ... - @overload - def insert(self, poseSF) -> None: ... + def insert(self, kf) -> None: ... @overload def insert(conststructmrpt) -> void: ... @overload - def insert(self, in_posePDF: mrpt.pymrpt.mrpt.poses.CPosePDF, in_SF: mrpt.pymrpt.mrpt.obs.CSensoryFrame) -> None: ... - @overload - def insert(self, in_posePDF: mrpt.pymrpt.mrpt.poses.CPosePDF, in_SF: mrpt.pymrpt.mrpt.obs.CSensoryFrame) -> None: ... - @overload def loadFromFile(self, filName: str) -> bool: ... @overload def loadFromFile(conststd) -> bool: ... @@ -3562,7 +3534,6 @@ class CSimpleMap(mrpt.pymrpt.mrpt.serialization.CSerializable): def saveToFile(self, filName: str) -> bool: ... @overload def saveToFile(conststd) -> bool: ... - def set(self, *args, **kwargs) -> Any: ... @overload def size(self) -> int: ... @overload @@ -3702,10 +3673,6 @@ class CVoxelMapBase_mrpt_maps_VoxelNodeOccRGB_t(CMetricMap): @overload def isEmpty() -> bool: ... @overload - def loadFromProbabilisticPosesAndObservations(self, Map: CSimpleMap) -> None: ... - @overload - def loadFromProbabilisticPosesAndObservations(constclassmrpt) -> void: ... - @overload def loadFromSimpleMap(self, Map: CSimpleMap) -> None: ... @overload def loadFromSimpleMap(constclassmrpt) -> void: ... @@ -3773,10 +3740,6 @@ class CVoxelMapBase_mrpt_maps_VoxelNodeOccupancy_t(CMetricMap): @overload def isEmpty() -> bool: ... @overload - def loadFromProbabilisticPosesAndObservations(self, Map: CSimpleMap) -> None: ... - @overload - def loadFromProbabilisticPosesAndObservations(constclassmrpt) -> void: ... - @overload def loadFromSimpleMap(self, Map: CSimpleMap) -> None: ... @overload def loadFromSimpleMap(constclassmrpt) -> void: ... @@ -3867,10 +3830,6 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t(CVoxelMapBa def l2p(self, *args, **kwargs) -> Any: ... def l2p_255(self, *args, **kwargs) -> Any: ... @overload - def loadFromProbabilisticPosesAndObservations(self, Map: CSimpleMap) -> None: ... - @overload - def loadFromProbabilisticPosesAndObservations(constclassmrpt) -> void: ... - @overload def loadFromSimpleMap(self, Map: CSimpleMap) -> None: ... @overload def loadFromSimpleMap(constclassmrpt) -> void: ... @@ -3891,6 +3850,22 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t(CVoxelMapBa @overload def nn_index_count() -> size_t: ... @overload + def nn_prepare_for_2d_queries(self) -> None: ... + @overload + def nn_prepare_for_2d_queries() -> void: ... + @overload + def nn_prepare_for_2d_queries(self) -> None: ... + @overload + def nn_prepare_for_2d_queries() -> void: ... + @overload + def nn_prepare_for_3d_queries(self) -> None: ... + @overload + def nn_prepare_for_3d_queries() -> void: ... + @overload + def nn_prepare_for_3d_queries(self) -> None: ... + @overload + def nn_prepare_for_3d_queries() -> void: ... + @overload def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... @overload def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... @@ -3989,10 +3964,6 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t(CVoxelMa def l2p(self, *args, **kwargs) -> Any: ... def l2p_255(self, *args, **kwargs) -> Any: ... @overload - def loadFromProbabilisticPosesAndObservations(self, Map: CSimpleMap) -> None: ... - @overload - def loadFromProbabilisticPosesAndObservations(constclassmrpt) -> void: ... - @overload def loadFromSimpleMap(self, Map: CSimpleMap) -> None: ... @overload def loadFromSimpleMap(constclassmrpt) -> void: ... @@ -4013,6 +3984,22 @@ class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t(CVoxelMa @overload def nn_index_count() -> size_t: ... @overload + def nn_prepare_for_2d_queries(self) -> None: ... + @overload + def nn_prepare_for_2d_queries() -> void: ... + @overload + def nn_prepare_for_2d_queries(self) -> None: ... + @overload + def nn_prepare_for_2d_queries() -> void: ... + @overload + def nn_prepare_for_3d_queries(self) -> None: ... + @overload + def nn_prepare_for_3d_queries() -> void: ... + @overload + def nn_prepare_for_3d_queries(self) -> None: ... + @overload + def nn_prepare_for_3d_queries() -> void: ... + @overload def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... @overload def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrID: int) -> bool: ... @@ -4113,7 +4100,7 @@ class CWeightedPointsMap(CPointsMap): @overload def setPointWeight(self, index: int, w: int) -> None: ... @overload - def setPointWeight(size_t, unsignedlong) -> void: ... + def setPointWeight(size_t, uint64_t) -> void: ... @overload def setSize(self, newLength: int) -> None: ... @overload @@ -4186,6 +4173,14 @@ class NearestNeighborsCapable: @overload def nn_index_count() -> size_t: ... @overload + def nn_prepare_for_2d_queries(self) -> None: ... + @overload + def nn_prepare_for_2d_queries() -> void: ... + @overload + def nn_prepare_for_3d_queries(self) -> None: ... + @overload + def nn_prepare_for_3d_queries() -> void: ... + @overload def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrIDOrID: int) -> bool: ... @overload def nn_single_search(self, query, result, out_dist_sqr: float, resultIndexOrIDOrID: int) -> bool: ... diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/math.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/math.pyi index 84a8af62ce..a6f93ee69b 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/math.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/math.pyi @@ -257,6 +257,49 @@ class CMatrixDynamic_float_t: def __getitem__(self, arg0: tuple) -> float: ... def __setitem__(self, arg0: tuple, arg1: float) -> None: ... +class CMatrixDynamic_mrpt_math_TPoint3D_float_t: + @overload + def __init__(self, arg0: CMatrixDynamic_mrpt_math_TPoint3D_float_t) -> None: ... + @overload + def __init__(self) -> None: ... + @overload + def __init__(self, row: int) -> None: ... + @overload + def __init__(self, row: int, col: int) -> None: ... + @overload + def __init__(self, m: CMatrixDynamic_mrpt_math_TPoint3D_float_t, cropRowCount: int, cropColCount: int) -> None: ... + @overload + def assign(self, m: CMatrixDynamic_mrpt_math_TPoint3D_float_t) -> CMatrixDynamic_mrpt_math_TPoint3D_float_t: ... + @overload + def assign(self, m: CMatrixDynamic_mrpt_math_TPoint3D_float_t) -> CMatrixDynamic_mrpt_math_TPoint3D_float_t: ... + @overload + def cols(self) -> int: ... + @overload + def cols() -> int: ... + def conservativeResize(self, row: int, col: int) -> None: ... + def data(self, *args, **kwargs) -> Any: ... + def derived(self) -> CMatrixDynamic_mrpt_math_TPoint3D_float_t: ... + @overload + def resize(self, row: int, col: int) -> None: ... + @overload + def resize(self, vectorLen: int) -> None: ... + @overload + def resize(size_t) -> void: ... + @overload + def rows(self) -> int: ... + @overload + def rows() -> int: ... + @overload + def setSize(self, row: int, col: int) -> None: ... + @overload + def setSize(self, row: int, col: int, zeroNewElements: bool) -> None: ... + @overload + def swap(self, o: CMatrixDynamic_mrpt_math_TPoint3D_float_t) -> None: ... + @overload + def swap(classmrpt) -> void: ... + def __call__(self, *args, **kwargs) -> Any: ... + def __getitem__(self, index) -> Any: ... + class CMatrixDynamic_unsigned_char_t: @overload def __init__(self, arg0: CMatrixDynamic_unsigned_char_t) -> None: ... @@ -1023,9 +1066,6 @@ class CMatrixFixed_float_3UL_1UL_t: def __call__(self, row: int, col: int) -> float: ... @overload def __call__(self, i: int) -> float: ... - @overload - def __getitem__(self, i: int) -> float: ... - @overload def __getitem__(self, arg0: tuple) -> float: ... def __setitem__(self, arg0: tuple, arg1: float) -> None: ... @@ -2844,6 +2884,7 @@ class TTwist3D: def asString(std) -> void: ... @overload def asString(self) -> str: ... + def assign(self) -> TTwist3D: ... @overload def fromString(self, s: str) -> None: ... @overload diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi index b7931b987d..0529ea041f 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi @@ -1327,6 +1327,25 @@ class CObservationRobotPose(CObservation): def setSensorPose(constclassmrpt) -> void: ... class CObservationRotatingScan(CObservation): + class ExternalStorageFormat: + __doc__: ClassVar[str] = ... # read-only + __members__: ClassVar[dict] = ... # read-only + MRPT_Serialization: ClassVar[CObservationRotatingScan.ExternalStorageFormat] = ... + None: ClassVar[CObservationRotatingScan.ExternalStorageFormat] = ... + PlainTextFile: ClassVar[CObservationRotatingScan.ExternalStorageFormat] = ... + __entries: ClassVar[dict] = ... + def __init__(self, value: int) -> None: ... + def __eq__(self, other: object) -> bool: ... + def __getstate__(self) -> int: ... + def __hash__(self) -> int: ... + def __index__(self) -> int: ... + def __int__(self) -> int: ... + def __ne__(self, other: object) -> bool: ... + def __setstate__(self, state: int) -> None: ... + @property + def name(self) -> str: ... + @property + def value(self) -> int: ... azimuthSpan: float columnCount: int has_satellite_timestamp: bool @@ -1334,6 +1353,7 @@ class CObservationRotatingScan(CObservation): lidarModel: str maxRange: float minRange: float + organizedPoints: mrpt.pymrpt.mrpt.math.CMatrixDynamic_mrpt_math_TPoint3D_float_t originalReceivedTimestamp: mrpt.pymrpt.std.chrono.time_point_mrpt_Clock_std_chrono_duration_long_std_ratio_1_10000000_t rangeImage: mrpt.pymrpt.mrpt.math.CMatrixDynamic_unsigned_short_t rangeOtherLayers: Dict[str,mrpt.pymrpt.mrpt.math.CMatrixDynamic_unsigned_short_t] @@ -1358,23 +1378,39 @@ class CObservationRotatingScan(CObservation): @overload def fromGeneric(constclassmrpt) -> bool: ... @overload - def fromPointCloud(self, o: CObservationPointCloud) -> None: ... - @overload - def fromPointCloud(constclassmrpt) -> void: ... - @overload def fromScan2D(self, o: CObservation2DRangeScan) -> None: ... @overload def fromScan2D(constclassmrpt) -> void: ... def fromVelodyne(self, o: CObservationVelodyneScan) -> None: ... + def getExternalStorageFile(self) -> str: ... def getOriginalReceivedTimeStamp(self) -> mrpt.pymrpt.std.chrono.time_point_mrpt_Clock_std_chrono_duration_long_std_ratio_1_10000000_t: ... @overload def getSensorPose(self, out_sensorPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> None: ... @overload def getSensorPose(classmrpt) -> void: ... + def isExternallyStored(self) -> bool: ... + def load(self) -> None: ... + @overload + def loadFromTextFile(self, filename: str) -> bool: ... + @overload + def loadFromTextFile(conststd) -> bool: ... + @overload + def overrideExternalStorageFormatFlag(self, fmt: CObservationRotatingScan.ExternalStorageFormat) -> None: ... + @overload + def overrideExternalStorageFormatFlag(constenummrpt) -> void: ... + def saveToTextFile(self, *args, **kwargs) -> Any: ... + @overload + def setAsExternalStorage(self, fileName: str, fmt: CObservationRotatingScan.ExternalStorageFormat) -> None: ... + @overload + def setAsExternalStorage(conststd, constenummrpt) -> void: ... @overload def setSensorPose(self, newSensorPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> None: ... @overload def setSensorPose(constclassmrpt) -> void: ... + @overload + def unload(self) -> None: ... + @overload + def unload() -> void: ... class CObservationSkeleton(CObservation): class TSkeletonJoint: @@ -2156,6 +2192,10 @@ def obsPointCloud_to_viz(obs: CObservationPointCloud, p: VisualizationParameters @overload def obsPointCloud_to_viz(constclassstd, conststructmrpt, classmrpt) -> void: ... @overload +def obsRotatingScan_to_viz(obs: CObservationRotatingScan, p: VisualizationParameters, out: mrpt.pymrpt.mrpt.opengl.CSetOfObjects) -> None: ... +@overload +def obsRotatingScan_to_viz(constclassstd, conststructmrpt, classmrpt) -> void: ... +@overload def obsVelodyne_to_viz(obs: CObservationVelodyneScan, p: VisualizationParameters, out: mrpt.pymrpt.mrpt.opengl.CSetOfObjects) -> None: ... @overload def obsVelodyne_to_viz(constclassstd, conststructmrpt, classmrpt) -> void: ... diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi index 5bc9a4c2dc..834566e71f 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi @@ -3932,6 +3932,10 @@ class Viewport(mrpt.pymrpt.mrpt.serialization.CSerializable, mrpt.pymrpt.mrpt.sy def getRenderMatrices(self) -> TRenderMatrices: ... def getViewportClipDistances(self, clip_min: float, clip_max: float) -> None: ... def getViewportPosition(self, *args, **kwargs) -> Any: ... + @overload + def getViewportVisibility(self) -> bool: ... + @overload + def getViewportVisibility() -> bool: ... def insert(self, *args, **kwargs) -> Any: ... @overload def isImageViewMode(self) -> bool: ... @@ -4001,7 +4005,9 @@ class Viewport(mrpt.pymrpt.mrpt.serialization.CSerializable, mrpt.pymrpt.mrpt.sy @overload def setImageView(self, img: mrpt.pymrpt.mrpt.img.CImage) -> None: ... @overload - def setImageView(constclassmrpt) -> void: ... + def setImageView(self, img: mrpt.pymrpt.mrpt.img.CImage, transparentBackground: bool) -> None: ... + @overload + def setImageView(constclassmrpt, bool) -> void: ... def setLightShadowClipDistances(self, clip_min: float, clip_max: float) -> None: ... @overload def setNormalMode(self) -> None: ... @@ -4014,6 +4020,10 @@ class Viewport(mrpt.pymrpt.mrpt.serialization.CSerializable, mrpt.pymrpt.mrpt.sy def setViewportClipDistances(self, clip_min: float, clip_max: float) -> None: ... def setViewportPosition(self, *args, **kwargs) -> Any: ... @overload + def setViewportVisibility(self, visible: bool) -> None: ... + @overload + def setViewportVisibility(bool) -> void: ... + @overload def size(self) -> int: ... @overload def size() -> size_t: ... diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/poses/__init__.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/poses/__init__.pyi index 001eab5d16..938120a808 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/poses/__init__.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/poses/__init__.pyi @@ -1723,12 +1723,20 @@ class CPoseInterpolatorBase_2_t: def loadFromTextFile(self, s: str) -> bool: ... @overload def loadFromTextFile(conststd) -> bool: ... + @overload + def loadFromTextFile_TUM(self, s: str) -> bool: ... + @overload + def loadFromTextFile_TUM(conststd) -> bool: ... def saveInterpolatedToTextFile(self, s: str, period: mrpt.pymrpt.std.chrono.duration_long_std_ratio_1_10000000_t) -> bool: ... @overload def saveToTextFile(self, s: str) -> bool: ... @overload def saveToTextFile(conststd) -> bool: ... @overload + def saveToTextFile_TUM(self, s: str) -> bool: ... + @overload + def saveToTextFile_TUM(conststd) -> bool: ... + @overload def setInterpolationMethod(self, method: TInterpolatorMethod) -> None: ... @overload def setInterpolationMethod(enummrpt) -> void: ... @@ -1781,12 +1789,20 @@ class CPoseInterpolatorBase_3_t: def loadFromTextFile(self, s: str) -> bool: ... @overload def loadFromTextFile(conststd) -> bool: ... + @overload + def loadFromTextFile_TUM(self, s: str) -> bool: ... + @overload + def loadFromTextFile_TUM(conststd) -> bool: ... def saveInterpolatedToTextFile(self, s: str, period: mrpt.pymrpt.std.chrono.duration_long_std_ratio_1_10000000_t) -> bool: ... @overload def saveToTextFile(self, s: str) -> bool: ... @overload def saveToTextFile(conststd) -> bool: ... @overload + def saveToTextFile_TUM(self, s: str) -> bool: ... + @overload + def saveToTextFile_TUM(conststd) -> bool: ... + @overload def setInterpolationMethod(self, method: TInterpolatorMethod) -> None: ... @overload def setInterpolationMethod(enummrpt) -> void: ... From 200f1411ecd783f22dc98e4db55acb94f5df0b75 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 17 Dec 2023 02:02:14 +0100 Subject: [PATCH 3/3] fix format --- apps/robot-map-gui/CDocument.h | 15 +++++++-------- .../robot-map-gui/gui/observationTree/CPairNode.h | 3 ++- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/apps/robot-map-gui/CDocument.h b/apps/robot-map-gui/CDocument.h index e59ea3e616..76ae71f269 100644 --- a/apps/robot-map-gui/CDocument.h +++ b/apps/robot-map-gui/CDocument.h @@ -72,20 +72,19 @@ class CDocument void insert( const std::vector& idx, mrpt::maps::CSimpleMap::KeyframeList& posesObsPairs); - - mrpt::maps::CSimpleMap::KeyframeList get( - const std::vector& idx); - const mrpt::maps::CSimpleMap::Keyframe & get(size_t idx) const + mrpt::maps::CSimpleMap::KeyframeList get(const std::vector& idx); + + const mrpt::maps::CSimpleMap::Keyframe& get(size_t idx) const { - return m_simplemap.get(idx); + return m_simplemap.get(idx); } - + mrpt::maps::CSimpleMap::Keyframe& get(size_t idx) { - return m_simplemap.get(idx); + return m_simplemap.get(idx); } - + mrpt::maps::CSimpleMap::KeyframeList getReverse( const std::vector& idx); diff --git a/apps/robot-map-gui/gui/observationTree/CPairNode.h b/apps/robot-map-gui/gui/observationTree/CPairNode.h index 99c8790987..67d89de0cd 100644 --- a/apps/robot-map-gui/gui/observationTree/CPairNode.h +++ b/apps/robot-map-gui/gui/observationTree/CPairNode.h @@ -20,7 +20,8 @@ class CPairNode : public CNode { public: CPairNode( - CNode* parent, const mrpt::maps::CSimpleMap::Keyframe& poseSensFramePair, + CNode* parent, + const mrpt::maps::CSimpleMap::Keyframe& poseSensFramePair, size_t indexInSimpleMap); ~CPairNode() override;