diff --git a/g2o/apps/g2o_viewer/abstract_properties_widget.cpp b/g2o/apps/g2o_viewer/abstract_properties_widget.cpp index e4b8782df..c4acdb4c7 100644 --- a/g2o/apps/g2o_viewer/abstract_properties_widget.cpp +++ b/g2o/apps/g2o_viewer/abstract_properties_widget.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "g2o/stuff/property.h" @@ -87,12 +88,14 @@ void AbstractPropertiesWidget::updateDisplayedProperties() { void AbstractPropertiesWidget::applyProperties() { assert(tableWidget->rowCount() == (int)propNames_.size()); - g2o::PropertyMap* properties = propertyMap(); + const g2o::PropertyMap* properties = propertyMap(); if (!properties) return; for (int r = 0; r < tableWidget->rowCount(); ++r) { const std::string& propName = propNames_[r]; + // HACK cast away the const std::shared_ptr baseProp = - properties->getProperty(propName); + std::const_pointer_cast( + properties->getProperty(propName)); if (!baseProp) continue; if (dynamic_cast*>(baseProp.get())) { diff --git a/g2o/apps/g2o_viewer/abstract_properties_widget.h b/g2o/apps/g2o_viewer/abstract_properties_widget.h index f992dbb89..f9509cc53 100644 --- a/g2o/apps/g2o_viewer/abstract_properties_widget.h +++ b/g2o/apps/g2o_viewer/abstract_properties_widget.h @@ -20,7 +20,6 @@ #define G2O_ABSTRACT_PROPERTIES_WINDOW_H #include -#include #include #include @@ -42,7 +41,7 @@ class G2O_VIEWER_API AbstractPropertiesWidget explicit AbstractPropertiesWidget(QWidget* parent = nullptr); ~AbstractPropertiesWidget() override = default; - virtual g2o::PropertyMap* propertyMap() = 0; + virtual const g2o::PropertyMap* propertyMap() = 0; void on_btnApply_clicked(); void on_btnOK_clicked(); diff --git a/g2o/apps/g2o_viewer/g2o_qglviewer.cpp b/g2o/apps/g2o_viewer/g2o_qglviewer.cpp index 4dd6baa5f..02c41e3cc 100644 --- a/g2o/apps/g2o_viewer/g2o_qglviewer.cpp +++ b/g2o/apps/g2o_viewer/g2o_qglviewer.cpp @@ -51,6 +51,16 @@ namespace g2o { namespace { +void applyAction(HyperGraph& graph, HyperGraphElementAction& action, + HyperGraphElementAction::Parameters& parameters) { + for (auto& it : graph.vertices()) { + (action)(*it.second, parameters); + } + for (const auto& it : graph.edges()) { + (action)(*it, parameters); + } +} + /** * \brief helper for setting up a camera for qglviewer */ @@ -80,7 +90,6 @@ class StandardCamera : public qglviewer::Camera { G2oQGLViewer::G2oQGLViewer(QWidget* parent) : QGLViewer(parent), drawActions_(nullptr) { setAxisIsDrawn(false); - drawActionParameters_ = std::make_shared(); } G2oQGLViewer::~G2oQGLViewer() { glDeleteLists(drawList_, 1); } diff --git a/g2o/apps/g2o_viewer/g2o_qglviewer.h b/g2o/apps/g2o_viewer/g2o_qglviewer.h index 4b887c913..a86d42a75 100644 --- a/g2o/apps/g2o_viewer/g2o_qglviewer.h +++ b/g2o/apps/g2o_viewer/g2o_qglviewer.h @@ -49,7 +49,7 @@ class G2O_VIEWER_API G2oQGLViewer : public QGLViewer { [[nodiscard]] bool updateDisplay() const { return updateDisplay_; } void setUpdateDisplay(bool updateDisplay); - [[nodiscard]] std::shared_ptr parameters() const { + [[nodiscard]] const DrawAction::Parameters& parameters() const { return drawActionParameters_; } @@ -59,7 +59,7 @@ class G2O_VIEWER_API G2oQGLViewer : public QGLViewer { HyperGraphElementAction::HyperGraphElementActionPtr drawActions_; GLuint drawList_ = 0; bool updateDisplay_ = true; - std::shared_ptr drawActionParameters_; + DrawAction::Parameters drawActionParameters_; }; } // namespace g2o diff --git a/g2o/apps/g2o_viewer/gui_hyper_graph_action.cpp b/g2o/apps/g2o_viewer/gui_hyper_graph_action.cpp index bd841ddd3..997b418cb 100644 --- a/g2o/apps/g2o_viewer/gui_hyper_graph_action.cpp +++ b/g2o/apps/g2o_viewer/gui_hyper_graph_action.cpp @@ -25,16 +25,15 @@ namespace g2o { -bool GuiHyperGraphAction::operator()( - const HyperGraph& graph, - const std::shared_ptr& parameters) { +bool GuiHyperGraphAction::operator()(const HyperGraph& graph, + HyperGraphAction::Parameters& parameters) { (void)graph; if (viewer) { viewer->setUpdateDisplay(true); viewer->update(); if (dumpScreenshots) { - auto p = std::dynamic_pointer_cast(parameters); + const auto* p = dynamic_cast(¶meters); if (p) { viewer->setSnapshotFormat(QString("PNG")); viewer->setSnapshotQuality(-1); diff --git a/g2o/apps/g2o_viewer/gui_hyper_graph_action.h b/g2o/apps/g2o_viewer/gui_hyper_graph_action.h index 59a2907af..cdef2c885 100644 --- a/g2o/apps/g2o_viewer/gui_hyper_graph_action.h +++ b/g2o/apps/g2o_viewer/gui_hyper_graph_action.h @@ -36,8 +36,7 @@ class G2O_VIEWER_API GuiHyperGraphAction : public HyperGraphAction { * iteration */ bool operator()(const HyperGraph& graph, - const std::shared_ptr& - parameters = nullptr) override; + HyperGraphAction::Parameters& parameters) override; G2oQGLViewer* viewer = nullptr; ///< the viewer which visualizes the graph bool dumpScreenshots = false; diff --git a/g2o/apps/g2o_viewer/properties_widget.cpp b/g2o/apps/g2o_viewer/properties_widget.cpp index ffbe7a0af..bb4164e0f 100644 --- a/g2o/apps/g2o_viewer/properties_widget.cpp +++ b/g2o/apps/g2o_viewer/properties_widget.cpp @@ -32,8 +32,7 @@ void PropertiesWidget::setSolver( updateDisplayedProperties(); } -g2o::PropertyMap* PropertiesWidget::propertyMap() { +const g2o::PropertyMap* PropertiesWidget::propertyMap() { if (!solver_) return nullptr; - // HACK cast away the constness of the property map - return const_cast(&solver_->properties()); + return &solver_->properties(); } diff --git a/g2o/apps/g2o_viewer/properties_widget.h b/g2o/apps/g2o_viewer/properties_widget.h index 9fde511a0..74a1c95e6 100644 --- a/g2o/apps/g2o_viewer/properties_widget.h +++ b/g2o/apps/g2o_viewer/properties_widget.h @@ -33,7 +33,7 @@ class G2O_VIEWER_API PropertiesWidget : public AbstractPropertiesWidget { explicit PropertiesWidget(QWidget* parent = nullptr); ~PropertiesWidget() override = default; - g2o::PropertyMap* propertyMap() override; + const g2o::PropertyMap* propertyMap() override; void setSolver(std::shared_ptr solver); diff --git a/g2o/apps/g2o_viewer/viewer_properties_widget.cpp b/g2o/apps/g2o_viewer/viewer_properties_widget.cpp index 0cd4813e2..335316b27 100644 --- a/g2o/apps/g2o_viewer/viewer_properties_widget.cpp +++ b/g2o/apps/g2o_viewer/viewer_properties_widget.cpp @@ -18,7 +18,7 @@ #include "viewer_properties_widget.h" -#include "g2o/stuff/property.h" +#include "g2o/core/hyper_graph_action.h" #include "g2o_qglviewer.h" #ifdef __GNUC__ @@ -80,7 +80,9 @@ std::string ViewerPropertiesWidget::humanReadablePropName( return demangleName(propertyName); } -g2o::PropertyMap* ViewerPropertiesWidget::propertyMap() { +const g2o::PropertyMap* ViewerPropertiesWidget::propertyMap() { if (!viewer_) return nullptr; - return viewer_->parameters().get(); + const auto* params = + dynamic_cast(&viewer_->parameters()); + return params; } diff --git a/g2o/apps/g2o_viewer/viewer_properties_widget.h b/g2o/apps/g2o_viewer/viewer_properties_widget.h index 98a2046f4..a52d212d6 100644 --- a/g2o/apps/g2o_viewer/viewer_properties_widget.h +++ b/g2o/apps/g2o_viewer/viewer_properties_widget.h @@ -35,7 +35,7 @@ class G2O_VIEWER_API ViewerPropertiesWidget : public AbstractPropertiesWidget { ~ViewerPropertiesWidget() override = default; void setViewer(g2o::G2oQGLViewer* viewer); - g2o::PropertyMap* propertyMap() override; + const g2o::PropertyMap* propertyMap() override; protected: g2o::G2oQGLViewer* viewer_ = nullptr; diff --git a/g2o/core/hyper_graph_action.cpp b/g2o/core/hyper_graph_action.cpp index bc6c2ea4c..2a6d0df27 100644 --- a/g2o/core/hyper_graph_action.cpp +++ b/g2o/core/hyper_graph_action.cpp @@ -39,42 +39,24 @@ namespace g2o { std::unique_ptr HyperGraphActionLibrary::actionLibInstance_; -HyperGraphAction::Parameters::~Parameters() = default; - HyperGraphAction::ParametersIteration::ParametersIteration(int iter) : HyperGraphAction::Parameters(), iteration(iter) {} -HyperGraphAction::~HyperGraphAction() = default; - -bool HyperGraphAction::operator()(const HyperGraph&, - const std::shared_ptr&) { - return false; -} - -HyperGraphElementAction::Parameters::~Parameters() = default; - -HyperGraphElementAction::HyperGraphElementAction(std::string typeName) - : typeName_(std::move(typeName)) {} +HyperGraphElementAction::HyperGraphElementAction(std::string typeName, + std::string name) + : typeName_(std::move(typeName)), name_(std::move(name)) {} void HyperGraphElementAction::setTypeName(std::string typeName) { typeName_ = std::move(typeName); } -bool HyperGraphElementAction::operator()(HyperGraph::HyperGraphElement&, - const std::shared_ptr&) { - return false; -} - -HyperGraphElementAction::~HyperGraphElementAction() = default; - HyperGraphElementActionCollection::HyperGraphElementActionCollection( const std::string& name) { name_ = name; } bool HyperGraphElementActionCollection::operator()( - HyperGraph::HyperGraphElement& element, - const std::shared_ptr& parameters) { + HyperGraph::HyperGraphElement& element, Parameters& parameters) { auto it = actionMap_.find(typeid(element).name()); if (it == actionMap_.end()) return false; HyperGraphElementAction* action = it->second.get(); @@ -83,9 +65,8 @@ bool HyperGraphElementActionCollection::operator()( bool HyperGraphElementActionCollection::registerAction( const HyperGraphElementAction::HyperGraphElementActionPtr& action) { -#ifdef G2O_DEBUG_ACTIONLIB - G2O_DEBUG("{} {}", action->name(), action->typeName()); -#endif + G2O_TRACE("Register action name: {} type: {}", action->name(), + action->typeName()); if (action->name() != name()) { G2O_ERROR( "invalid attempt to register an action in a collection with a " @@ -143,9 +124,7 @@ bool HyperGraphActionLibrary::registerAction( } return collection->registerAction(action); } -#ifdef G2O_DEBUG_ACTIONLIB - G2O_DEBUG("creating collection for {}", action->name()); -#endif + G2O_TRACE("creating collection for {}", action->name()); collection = std::make_shared(action->name()); actionMap_.insert(std::make_pair(action->name(), collection)); @@ -185,23 +164,20 @@ DrawAction::Parameters::Parameters() = default; DrawAction::DrawAction(const std::string& typeName_) : HyperGraphElementAction(typeName_) { name_ = "draw"; - // previousParams_.reset(reinterpret_cast(0x42)); - refreshPropertyPtrs(nullptr); - cacheDrawActions_ = nullptr; } -bool DrawAction::refreshPropertyPtrs( - const std::shared_ptr& params) { - if (previousParams_ == params) return false; - auto p = std::dynamic_pointer_cast(params); +DrawAction::Parameters* DrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params) { + if (previousParams_ == ¶ms) return nullptr; + auto* p = dynamic_cast(¶ms); if (!p) { previousParams_ = nullptr; show_ = nullptr; } else { - previousParams_ = p; + previousParams_ = &p; show_ = p->makeProperty(typeName_ + "::SHOW", true); } - return true; + return p; } void DrawAction::initializeDrawActionsCache() { @@ -211,39 +187,20 @@ void DrawAction::initializeDrawActionsCache() { } } -void DrawAction::drawCache( - const CacheContainer& caches, - const std::shared_ptr& params) { +void DrawAction::drawCache(const CacheContainer& caches, + HyperGraphElementAction::Parameters& params) { for (const auto& cache : caches) { Cache* c = cache.second.get(); (*cacheDrawActions_)(*c, params); } } -void DrawAction::drawUserData( - const HyperGraph::DataContainer::DataVector& data, - const std::shared_ptr& params) { +void DrawAction::drawUserData(const HyperGraph::DataContainer::DataVector& data, + HyperGraphElementAction::Parameters& params) { if (!cacheDrawActions_) return; for (const auto& d : data) { (*cacheDrawActions_)(*d, params); } } -void applyAction( - HyperGraph& graph, HyperGraphElementAction& action, - const std::shared_ptr& parameters, - const std::string& typeName) { - for (auto& it : graph.vertices()) { - auto& aux = *it.second; - if (typeName.empty() || typeid(aux).name() == typeName) { - (action)(*it.second, parameters); - } - } - for (const auto& it : graph.edges()) { - auto& aux = *it; - if (typeName.empty() || typeid(aux).name() == typeName) - (action)(aux, parameters); - } -} - } // namespace g2o diff --git a/g2o/core/hyper_graph_action.h b/g2o/core/hyper_graph_action.h index 11d7ae5d6..8d70c4ca0 100644 --- a/g2o/core/hyper_graph_action.h +++ b/g2o/core/hyper_graph_action.h @@ -35,9 +35,6 @@ #include "g2o_core_api.h" #include "hyper_graph.h" -// define to get verbose output -// #define G2O_DEBUG_ACTIONLIB - namespace g2o { class CacheContainer; @@ -49,7 +46,7 @@ class G2O_CORE_API HyperGraphAction { public: class G2O_CORE_API Parameters { public: - virtual ~Parameters(); + virtual ~Parameters() = default; }; class G2O_CORE_API ParametersIteration : public Parameters { @@ -58,14 +55,12 @@ class G2O_CORE_API HyperGraphAction { int iteration; }; - virtual ~HyperGraphAction(); + virtual ~HyperGraphAction() = default; /** * re-implement to carry out an action given the graph */ - virtual bool operator()( - const HyperGraph& graph, - const std::shared_ptr& parameters = nullptr); + virtual bool operator()(const HyperGraph& graph, Parameters& parameters) = 0; }; /** @@ -74,18 +69,19 @@ class G2O_CORE_API HyperGraphAction { class G2O_CORE_API HyperGraphElementAction { public: struct G2O_CORE_API Parameters { - virtual ~Parameters(); + virtual ~Parameters() = default; }; using HyperGraphElementActionPtr = std::shared_ptr; //! an action should be instantiated with the typeid.name of the graph element //! on which it operates - explicit HyperGraphElementAction(std::string typeName = ""); + explicit HyperGraphElementAction(std::string typeName = "", + std::string name = ""); //! redefine this to do the action stuff. If successful, returns true. virtual bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& parameters); + Parameters& parameters) = 0; - virtual ~HyperGraphElementAction(); + virtual ~HyperGraphElementAction() = default; //! returns the typeid name of the action [[nodiscard]] const std::string& typeName() const { return typeName_; } @@ -119,7 +115,7 @@ class G2O_CORE_API HyperGraphElementActionCollection * actionMap that was active on element */ bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& parameters) override; + Parameters& parameters) override; ActionMap& actionMap() { return actionMap_; } //! inserts an action in the pool. The action should have the same name of the //! container. returns false on failure (the container has a different name @@ -170,15 +166,6 @@ class G2O_CORE_API HyperGraphActionLibrary { static std::unique_ptr actionLibInstance_; }; -/** - * apply an action to all the elements of the graph. - */ -void G2O_CORE_API applyAction( - HyperGraph& graph, HyperGraphElementAction& action, - const std::shared_ptr& parameters = - nullptr, - const std::string& typeName = ""); - /** * \brief draw actions */ @@ -192,28 +179,23 @@ class G2O_CORE_API DrawAction : public HyperGraphElementAction { explicit DrawAction(const std::string& typeName_); protected: - virtual bool refreshPropertyPtrs( - const std::shared_ptr& params); + virtual DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params); void initializeDrawActionsCache(); - void drawCache( - const CacheContainer& caches, - const std::shared_ptr& params); - void drawUserData( - const HyperGraph::DataContainer::DataVector& data, - const std::shared_ptr& params); - std::shared_ptr previousParams_; - std::shared_ptr show_; - HyperGraphElementAction::HyperGraphElementActionPtr cacheDrawActions_; + void drawCache(const CacheContainer& caches, + HyperGraphElementAction::Parameters& params); + void drawUserData(const HyperGraph::DataContainer::DataVector& data, + HyperGraphElementAction::Parameters& params); + void* previousParams_ = nullptr; + std::shared_ptr show_ = nullptr; + HyperGraphElementAction::HyperGraphElementActionPtr cacheDrawActions_ = + nullptr; }; template class RegisterActionProxy { public: RegisterActionProxy() { -#ifdef G2O_DEBUG_ACTIONLIB - std::cout << __FUNCTION__ << ": Registering action of type " - << typeid(T).name() << std::endl; -#endif action_.reset(new T()); HyperGraphActionLibrary::instance()->registerAction(action_); } diff --git a/g2o/core/optimizable_graph.cpp b/g2o/core/optimizable_graph.cpp index ffd867b8c..013513d74 100644 --- a/g2o/core/optimizable_graph.cpp +++ b/g2o/core/optimizable_graph.cpp @@ -148,10 +148,6 @@ void OptimizableGraph::Vertex::updateCache() { } } -OptimizableGraph::Edge::Edge() : robustKernel_(nullptr) {} - -OptimizableGraph::Edge::~Edge() = default; - bool OptimizableGraph::Edge::setParameterId(int argNum, int paramId) { if (static_cast(parameters_.size()) <= argNum) return false; if (argNum < 0) return false; @@ -165,15 +161,10 @@ bool OptimizableGraph::Edge::resolveParameters(const OptimizableGraph& graph) { for (decltype(parameters_)::size_type i = 0; i < parameters_.size(); i++) { const int index = parameterIds_[i]; parameters_[i] = graph.parameter(index); -#ifndef NDEBUG - auto& aux = *parameters_[i]; - if (typeid(aux).name() != parameterTypes_[i]) { - G2O_CRITICAL("parameter type mismatch - encountered {}; should be {}", - typeid(aux).name(), parameterTypes_[i]); - } -#endif if (!parameters_[i]) { - G2O_CRITICAL("_parameters[i] == nullptr"); + G2O_CRITICAL( + "Parameter {} with expected Id {} not contained in the graph", i, + index); return false; } } @@ -189,10 +180,7 @@ bool OptimizableGraph::Edge::resolveCaches() { return true; } bool OptimizableGraph::Edge::setMeasurementFromState() { return false; } -OptimizableGraph::OptimizableGraph() { - nextEdgeId_ = 0; - graphActions_.resize(kAtNumElements); -} +OptimizableGraph::OptimizableGraph() : graphActions_(kAtNumElements) {} OptimizableGraph::~OptimizableGraph() { clear(); @@ -651,11 +639,10 @@ std::set OptimizableGraph::dimensions() const { } void OptimizableGraph::performActions(int iter, HyperGraphActionSet& actions) { - if (!actions.empty()) { - auto params = std::make_shared(iter); - for (const auto& action : actions) { - (*action)(*this, params); - } + if (actions.empty()) return; + HyperGraphAction::ParametersIteration params(iter); + for (const auto& action : actions) { + (*action)(*this, params); } } diff --git a/g2o/core/optimizable_graph.h b/g2o/core/optimizable_graph.h index 5fc24312b..8bf6d9fac 100644 --- a/g2o/core/optimizable_graph.h +++ b/g2o/core/optimizable_graph.h @@ -35,7 +35,6 @@ #include #include #include -#include #include #include @@ -424,9 +423,6 @@ class G2O_CORE_API OptimizableGraph : public HyperGraph { friend class OptimizableGraph; public: - Edge(); - ~Edge() override; - // indicates if all vertices are fixed [[nodiscard]] virtual bool allVerticesFixed() const = 0; @@ -558,20 +554,18 @@ class G2O_CORE_API OptimizableGraph : public HyperGraph { protected: int dimension_ = -1; int level_ = 0; - std::shared_ptr robustKernel_; + std::shared_ptr robustKernel_ = nullptr; int64_t internalId_ = -1; void resizeParameters(size_t newSize) { parameters_.resize(newSize, nullptr); parameterIds_.resize(newSize, -1); - parameterTypes_.resize(newSize, typeid(void*).name()); } template bool installParameter(size_t argNo, int paramId = -1) { if (argNo >= parameters_.size()) return false; parameterIds_[argNo] = paramId; - parameterTypes_[argNo] = typeid(ParameterType).name(); return true; } @@ -583,7 +577,6 @@ class G2O_CORE_API OptimizableGraph : public HyperGraph { bool resolveParameters(const OptimizableGraph& graph); virtual bool resolveCaches(); - std::vector parameterTypes_; ParameterVector parameters_; std::vector parameterIds_; }; @@ -792,7 +785,7 @@ class G2O_CORE_API OptimizableGraph : public HyperGraph { protected: std::unordered_map renamedTypesLookup_; - int64_t nextEdgeId_; + int64_t nextEdgeId_ = 0; std::vector graphActions_; ParameterContainer parameters_; diff --git a/g2o/core/sparse_optimizer.cpp b/g2o/core/sparse_optimizer.cpp index 2e668b4af..9ef05e59c 100644 --- a/g2o/core/sparse_optimizer.cpp +++ b/g2o/core/sparse_optimizer.cpp @@ -79,7 +79,8 @@ void SparseOptimizer::computeActiveErrors() { // call the callbacks in case there is something registered HyperGraphActionSet& actions = graphActions_[kAtComputeactiverror]; if (!actions.empty()) { - for (const auto& action : actions) (*action)(*this); + HyperGraphAction::Parameters empty_params; + for (const auto& action : actions) (*action)(*this, empty_params); } #ifdef G2O_OPENMP diff --git a/g2o/core/sparse_optimizer_terminate_action.cpp b/g2o/core/sparse_optimizer_terminate_action.cpp index d596b2a97..a0df0bd13 100644 --- a/g2o/core/sparse_optimizer_terminate_action.cpp +++ b/g2o/core/sparse_optimizer_terminate_action.cpp @@ -44,17 +44,16 @@ void SparseOptimizerTerminateAction::setGainThreshold(double gainThreshold) { gainThreshold_ = gainThreshold; } -bool SparseOptimizerTerminateAction::operator()( - const HyperGraph& graph, const std::shared_ptr& parameters) { +bool SparseOptimizerTerminateAction::operator()(const HyperGraph& graph, + Parameters& parameters) { assert(dynamic_cast(&graph) && "graph is not a SparseOptimizer"); - assert( - dynamic_cast(parameters.get()) && - "error casting parameters"); + assert(dynamic_cast(¶meters) && + "error casting parameters"); const auto* optimizer = static_cast(&graph); - auto* params = - static_cast(parameters.get()); + const auto* params = + static_cast(¶meters); const_cast(optimizer)->computeActiveErrors(); if (params->iteration < 0) { diff --git a/g2o/core/sparse_optimizer_terminate_action.h b/g2o/core/sparse_optimizer_terminate_action.h index 9d3e5000e..cc0d601bc 100644 --- a/g2o/core/sparse_optimizer_terminate_action.h +++ b/g2o/core/sparse_optimizer_terminate_action.h @@ -27,8 +27,6 @@ #ifndef SPARSE_OPTIMIZER_TERMINATE_ACTION_H #define SPARSE_OPTIMIZER_TERMINATE_ACTION_H -#include - #include "g2o_core_api.h" #include "hyper_graph_action.h" @@ -50,9 +48,7 @@ class HyperGraph; class G2O_CORE_API SparseOptimizerTerminateAction : public HyperGraphAction { public: SparseOptimizerTerminateAction(); - bool operator()( - const HyperGraph& graph, - const std::shared_ptr& parameters = nullptr) override; + bool operator()(const HyperGraph& graph, Parameters& parameters) override; [[nodiscard]] double gainThreshold() const { return gainThreshold_; } void setGainThreshold(double gainThreshold); diff --git a/g2o/types/data/robot_laser.cpp b/g2o/types/data/robot_laser.cpp index 3f48bf402..fcff73dd2 100644 --- a/g2o/types/data/robot_laser.cpp +++ b/g2o/types/data/robot_laser.cpp @@ -36,6 +36,7 @@ #include #include "g2o/core/eigen_types.h" +#include "g2o/core/hyper_graph_action.h" #include "g2o/stuff/macros.h" #include "g2o/types/data/laser_parameters.h" #include "g2o/types/data/raw_laser.h" @@ -120,27 +121,23 @@ RobotLaserDrawAction::RobotLaserDrawAction() pointSize_(nullptr), maxRange_(nullptr) {} -bool RobotLaserDrawAction::refreshPropertyPtrs( - const std::shared_ptr& params_) { - if (!DrawAction::refreshPropertyPtrs(params_)) return false; - if (previousParams_) { - beamsDownsampling_ = previousParams_->makeProperty( +DrawAction::Parameters* RobotLaserDrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(params_); + if (params) { + beamsDownsampling_ = params->makeProperty( typeName_ + "::BEAMS_DOWNSAMPLING", 1); - pointSize_ = previousParams_->makeProperty( - typeName_ + "::POINT_SIZE", 1.0F); - maxRange_ = previousParams_->makeProperty( - typeName_ + "::MAX_RANGE", -1.); - } else { - beamsDownsampling_ = nullptr; - pointSize_ = nullptr; - maxRange_ = nullptr; + pointSize_ = + params->makeProperty(typeName_ + "::POINT_SIZE", 1.0F); + maxRange_ = + params->makeProperty(typeName_ + "::MAX_RANGE", -1.); } - return true; + return params; } bool RobotLaserDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; refreshPropertyPtrs(params_); diff --git a/g2o/types/data/robot_laser.h b/g2o/types/data/robot_laser.h index 010e29967..8048bd6f9 100644 --- a/g2o/types/data/robot_laser.h +++ b/g2o/types/data/robot_laser.h @@ -71,13 +71,11 @@ class G2O_TYPES_DATA_API RobotLaserDrawAction : public DrawAction { public: RobotLaserDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; protected: - bool refreshPropertyPtrs( - const std::shared_ptr& params_) - override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) override; std::shared_ptr beamsDownsampling_; std::shared_ptr pointSize_; std::shared_ptr maxRange_; diff --git a/g2o/types/data/vertex_ellipse.cpp b/g2o/types/data/vertex_ellipse.cpp index a03eb874e..6d4dd2a86 100644 --- a/g2o/types/data/vertex_ellipse.cpp +++ b/g2o/types/data/vertex_ellipse.cpp @@ -26,6 +26,7 @@ #include "vertex_ellipse.h" +#include "g2o/core/hyper_graph_action.h" #include "g2o/stuff/misc.h" #ifdef G2O_HAVE_OPENGL @@ -102,21 +103,18 @@ bool VertexEllipse::write(std::ostream& os) const { VertexEllipseDrawAction::VertexEllipseDrawAction() : DrawAction(typeid(VertexEllipse).name()) {} -bool VertexEllipseDrawAction::refreshPropertyPtrs( - const std::shared_ptr& params_) { - if (!DrawAction::refreshPropertyPtrs(params_)) return false; - if (previousParams_) { - scaleFactor_ = - previousParams_->makeProperty(typeName_ + "::", 1); - } else { - scaleFactor_ = nullptr; +DrawAction::Parameters* VertexEllipseDrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(params_); + if (params) { + scaleFactor_ = params->makeProperty(typeName_ + "::", 1); } - return true; + return params; } bool VertexEllipseDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; refreshPropertyPtrs(params_); diff --git a/g2o/types/data/vertex_ellipse.h b/g2o/types/data/vertex_ellipse.h index dd82f1a7d..0a6a4e24d 100644 --- a/g2o/types/data/vertex_ellipse.h +++ b/g2o/types/data/vertex_ellipse.h @@ -87,13 +87,11 @@ class G2O_TYPES_DATA_API VertexEllipseDrawAction : public DrawAction { public: VertexEllipseDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; protected: - bool refreshPropertyPtrs( - const std::shared_ptr& params_) - override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) override; std::shared_ptr scaleFactor_ = nullptr; }; #endif diff --git a/g2o/types/data/vertex_tag.cpp b/g2o/types/data/vertex_tag.cpp index 8345b2a87..41d299c6d 100644 --- a/g2o/types/data/vertex_tag.cpp +++ b/g2o/types/data/vertex_tag.cpp @@ -30,6 +30,7 @@ #include #include +#include "g2o/core/hyper_graph_action.h" #include "g2o/stuff/macros.h" #ifdef G2O_HAVE_OPENGL @@ -64,21 +65,19 @@ bool VertexTag::write(std::ostream& os) const { VertexTagDrawAction::VertexTagDrawAction() : DrawAction(typeid(VertexTag).name()), textSize_(nullptr) {} -bool VertexTagDrawAction::refreshPropertyPtrs( - const std::shared_ptr& params_) { - if (!DrawAction::refreshPropertyPtrs(params_)) return false; - if (previousParams_) { - textSize_ = previousParams_->makeProperty( - typeName_ + "::TEXT_SIZE", 1); - } else { - textSize_ = nullptr; +DrawAction::Parameters* VertexTagDrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(params_); + if (params) { + textSize_ = + params->makeProperty(typeName_ + "::TEXT_SIZE", 1); } - return true; + return params; } bool VertexTagDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; refreshPropertyPtrs(params_); diff --git a/g2o/types/data/vertex_tag.h b/g2o/types/data/vertex_tag.h index 01034e086..8a6a42d5e 100644 --- a/g2o/types/data/vertex_tag.h +++ b/g2o/types/data/vertex_tag.h @@ -68,13 +68,12 @@ class G2O_TYPES_DATA_API VertexTag : public RobotData { class G2O_TYPES_DATA_API VertexTagDrawAction : public DrawAction { public: VertexTagDrawAction(); - bool operator()( - HyperGraph::HyperGraphElement& element, - const std::shared_ptr&) override; + bool operator()(HyperGraph::HyperGraphElement& element, + HyperGraphElementAction::Parameters&) override; protected: - bool refreshPropertyPtrs( - const std::shared_ptr&) override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters&) override; std::shared_ptr textSize_; }; #endif diff --git a/g2o/types/sclam2d/edge_se2_odom_differential_calib.cpp b/g2o/types/sclam2d/edge_se2_odom_differential_calib.cpp index 556947c1c..d6b311028 100644 --- a/g2o/types/sclam2d/edge_se2_odom_differential_calib.cpp +++ b/g2o/types/sclam2d/edge_se2_odom_differential_calib.cpp @@ -62,7 +62,7 @@ EdgeSE2OdomDifferentialCalibDrawAction::EdgeSE2OdomDifferentialCalibDrawAction() bool EdgeSE2OdomDifferentialCalibDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr&) { + HyperGraphElementAction::Parameters&) { if (typeid(element).name() != typeName_) return false; auto* e = static_cast(&element); auto fromEdge = e->vertexXn<0>(); diff --git a/g2o/types/sclam2d/edge_se2_odom_differential_calib.h b/g2o/types/sclam2d/edge_se2_odom_differential_calib.h index bc29628ff..1fba5e5b1 100644 --- a/g2o/types/sclam2d/edge_se2_odom_differential_calib.h +++ b/g2o/types/sclam2d/edge_se2_odom_differential_calib.h @@ -27,8 +27,6 @@ #ifndef G2O_EDGE_SE2_ODOM_CALIB_DIFFERENTIAL_H #define G2O_EDGE_SE2_ODOM_CALIB_DIFFERENTIAL_H -#include - #include "g2o/config.h" #include "g2o/core/base_fixed_sized_edge.h" #include "g2o/core/hyper_graph.h" @@ -53,8 +51,7 @@ class G2O_TYPES_SCLAM2D_API EdgeSE2OdomDifferentialCalibDrawAction public: EdgeSE2OdomDifferentialCalibDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; }; #endif diff --git a/g2o/types/sclam2d/edge_se2_sensor_calib.cpp b/g2o/types/sclam2d/edge_se2_sensor_calib.cpp index a00de7b2c..84957ab30 100644 --- a/g2o/types/sclam2d/edge_se2_sensor_calib.cpp +++ b/g2o/types/sclam2d/edge_se2_sensor_calib.cpp @@ -83,7 +83,7 @@ EdgeSE2SensorCalibDrawAction::EdgeSE2SensorCalibDrawAction() bool EdgeSE2SensorCalibDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr&) { + HyperGraphElementAction::Parameters&) { if (typeid(element).name() != typeName_) return false; auto* e = static_cast(&element); auto fromEdge = e->vertexXn<0>(); diff --git a/g2o/types/sclam2d/edge_se2_sensor_calib.h b/g2o/types/sclam2d/edge_se2_sensor_calib.h index 4e44f35c6..bf540a6bb 100644 --- a/g2o/types/sclam2d/edge_se2_sensor_calib.h +++ b/g2o/types/sclam2d/edge_se2_sensor_calib.h @@ -27,8 +27,6 @@ #ifndef G2O_EDGE_SE2_SENSOR_CALIB_H #define G2O_EDGE_SE2_SENSOR_CALIB_H -#include - #include "g2o/config.h" #include "g2o/core/base_fixed_sized_edge.h" #include "g2o/core/hyper_graph.h" @@ -64,8 +62,7 @@ class EdgeSE2SensorCalibDrawAction : public DrawAction { public: EdgeSE2SensorCalibDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; }; #endif diff --git a/g2o/types/slam2d/edge_se2.cpp b/g2o/types/slam2d/edge_se2.cpp index e0747a23c..99aa27b28 100644 --- a/g2o/types/slam2d/edge_se2.cpp +++ b/g2o/types/slam2d/edge_se2.cpp @@ -83,24 +83,21 @@ EdgeSE2DrawAction::EdgeSE2DrawAction() triangleX_(nullptr), triangleY_(nullptr) {} -bool EdgeSE2DrawAction::refreshPropertyPtrs( - const std::shared_ptr& params_) { - if (!DrawAction::refreshPropertyPtrs(params_)) return false; - if (previousParams_) { - triangleX_ = previousParams_->makeProperty( +DrawAction::Parameters* EdgeSE2DrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(params_); + if (params) { + triangleX_ = params->makeProperty( typeName_ + "::GHOST_TRIANGLE_X", .2F); - triangleY_ = previousParams_->makeProperty( + triangleY_ = params->makeProperty( typeName_ + "::GHOST_TRIANGLE_Y", .05F); - } else { - triangleX_ = nullptr; - triangleY_ = nullptr; } - return true; + return params; } bool EdgeSE2DrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; refreshPropertyPtrs(params_); diff --git a/g2o/types/slam2d/edge_se2.h b/g2o/types/slam2d/edge_se2.h index 0529dbf4e..2a646f5ee 100644 --- a/g2o/types/slam2d/edge_se2.h +++ b/g2o/types/slam2d/edge_se2.h @@ -87,13 +87,11 @@ class G2O_TYPES_SLAM2D_API EdgeSE2DrawAction : public DrawAction { public: EdgeSE2DrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params) override; protected: - bool refreshPropertyPtrs( - const std::shared_ptr& params_) - override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) override; std::shared_ptr triangleX_, triangleY_; }; #endif diff --git a/g2o/types/slam2d/edge_se2_pointxy.cpp b/g2o/types/slam2d/edge_se2_pointxy.cpp index 267b0d575..09d0f34f3 100644 --- a/g2o/types/slam2d/edge_se2_pointxy.cpp +++ b/g2o/types/slam2d/edge_se2_pointxy.cpp @@ -88,7 +88,7 @@ EdgeSE2PointXYDrawAction::EdgeSE2PointXYDrawAction() bool EdgeSE2PointXYDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; refreshPropertyPtrs(params_); diff --git a/g2o/types/slam2d/edge_se2_pointxy.h b/g2o/types/slam2d/edge_se2_pointxy.h index 1ae972e18..db88e2c71 100644 --- a/g2o/types/slam2d/edge_se2_pointxy.h +++ b/g2o/types/slam2d/edge_se2_pointxy.h @@ -28,7 +28,6 @@ #define G2O_EDGE_SE2_POINT_XY_H #include -#include #include "g2o/config.h" #include "g2o/core/base_binary_edge.h" @@ -78,8 +77,7 @@ class G2O_TYPES_SLAM2D_API EdgeSE2PointXYDrawAction : public DrawAction { public: EdgeSE2PointXYDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; }; #endif diff --git a/g2o/types/slam2d/edge_se2_pointxy_bearing.cpp b/g2o/types/slam2d/edge_se2_pointxy_bearing.cpp index 3498d6f5b..a58cd0621 100644 --- a/g2o/types/slam2d/edge_se2_pointxy_bearing.cpp +++ b/g2o/types/slam2d/edge_se2_pointxy_bearing.cpp @@ -62,7 +62,7 @@ EdgeSE2PointXYBearingDrawAction::EdgeSE2PointXYBearingDrawAction() bool EdgeSE2PointXYBearingDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; refreshPropertyPtrs(params_); diff --git a/g2o/types/slam2d/edge_se2_pointxy_bearing.h b/g2o/types/slam2d/edge_se2_pointxy_bearing.h index aad817809..6077a5d31 100644 --- a/g2o/types/slam2d/edge_se2_pointxy_bearing.h +++ b/g2o/types/slam2d/edge_se2_pointxy_bearing.h @@ -29,7 +29,6 @@ #include #include -#include #include "g2o/config.h" #include "g2o/core/base_binary_edge.h" @@ -78,8 +77,7 @@ class G2O_TYPES_SLAM2D_API EdgeSE2PointXYBearingDrawAction : public DrawAction { public: EdgeSE2PointXYBearingDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; }; #endif diff --git a/g2o/types/slam2d/vertex_point_xy.cpp b/g2o/types/slam2d/vertex_point_xy.cpp index 3cd8d7afc..55c5f9f58 100644 --- a/g2o/types/slam2d/vertex_point_xy.cpp +++ b/g2o/types/slam2d/vertex_point_xy.cpp @@ -26,6 +26,8 @@ #include "vertex_point_xy.h" +#include "g2o/core/hyper_graph_action.h" + #ifdef G2O_HAVE_OPENGL #include "g2o/stuff/opengl_primitives.h" #include "g2o/stuff/opengl_wrapper.h" @@ -42,21 +44,19 @@ VertexPointXY::VertexPointXY() { estimate_.setZero(); } VertexPointXYDrawAction::VertexPointXYDrawAction() : DrawAction(typeid(VertexPointXY).name()), pointSize_(nullptr) {} -bool VertexPointXYDrawAction::refreshPropertyPtrs( - const std::shared_ptr& params) { - if (!DrawAction::refreshPropertyPtrs(params)) return false; - if (previousParams_) { - pointSize_ = previousParams_->makeProperty( - typeName_ + "::POINT_SIZE", 1.); - } else { - pointSize_ = nullptr; +DrawAction::Parameters* VertexPointXYDrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& p) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(p); + if (params) { + pointSize_ = + params->makeProperty(typeName_ + "::POINT_SIZE", 1.); } - return true; + return params; } bool VertexPointXYDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params) { + HyperGraphElementAction::Parameters& params) { if (typeid(element).name() != typeName_) return false; initializeDrawActionsCache(); refreshPropertyPtrs(params); diff --git a/g2o/types/slam2d/vertex_point_xy.h b/g2o/types/slam2d/vertex_point_xy.h index a20e80018..8b90bdfb6 100644 --- a/g2o/types/slam2d/vertex_point_xy.h +++ b/g2o/types/slam2d/vertex_point_xy.h @@ -54,14 +54,12 @@ class G2O_TYPES_SLAM2D_API VertexPointXYDrawAction : public DrawAction { public: VertexPointXYDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params) override; + HyperGraphElementAction::Parameters& params) override; protected: std::shared_ptr pointSize_; - bool refreshPropertyPtrs( - const std::shared_ptr& params) - override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params) override; }; #endif diff --git a/g2o/types/slam2d/vertex_se2.cpp b/g2o/types/slam2d/vertex_se2.cpp index 0d3459465..a7d6a4243 100644 --- a/g2o/types/slam2d/vertex_se2.cpp +++ b/g2o/types/slam2d/vertex_se2.cpp @@ -54,24 +54,21 @@ VertexSE2DrawAction::VertexSE2DrawAction() triangleX_(nullptr), triangleY_(nullptr) {} -bool VertexSE2DrawAction::refreshPropertyPtrs( - const std::shared_ptr& parameters) { - if (!DrawAction::refreshPropertyPtrs(parameters)) return false; - if (previousParams_) { - triangleX_ = previousParams_->makeProperty( - typeName_ + "::TRIANGLE_X", .2F); - triangleY_ = previousParams_->makeProperty( - typeName_ + "::TRIANGLE_Y", .05F); - } else { - triangleX_ = nullptr; - triangleY_ = nullptr; +DrawAction::Parameters* VertexSE2DrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& parameters) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(parameters); + if (params) { + triangleX_ = + params->makeProperty(typeName_ + "::TRIANGLE_X", .2F); + triangleY_ = + params->makeProperty(typeName_ + "::TRIANGLE_Y", .05F); } - return true; + return params; } bool VertexSE2DrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& parameters) { + HyperGraphElementAction::Parameters& parameters) { if (typeid(element).name() != typeName_) return false; initializeDrawActionsCache(); refreshPropertyPtrs(parameters); diff --git a/g2o/types/slam2d/vertex_se2.h b/g2o/types/slam2d/vertex_se2.h index 0d06ec0fd..bbecd70ec 100644 --- a/g2o/types/slam2d/vertex_se2.h +++ b/g2o/types/slam2d/vertex_se2.h @@ -55,14 +55,12 @@ class G2O_TYPES_SLAM2D_API VertexSE2DrawAction : public DrawAction { public: VertexSE2DrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - parameters) override; + HyperGraphElementAction::Parameters& parameters) override; protected: HyperGraphElementAction* drawActions_ = nullptr; - bool refreshPropertyPtrs( - const std::shared_ptr& parameters) - override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& parameters) override; std::shared_ptr triangleX_, triangleY_; }; #endif diff --git a/g2o/types/slam2d_addons/vertex_segment2d.cpp b/g2o/types/slam2d_addons/vertex_segment2d.cpp index f3e4314f9..ab609a33a 100644 --- a/g2o/types/slam2d_addons/vertex_segment2d.cpp +++ b/g2o/types/slam2d_addons/vertex_segment2d.cpp @@ -26,6 +26,8 @@ #include "vertex_segment2d.h" +#include "g2o/core/hyper_graph_action.h" + #ifdef G2O_HAVE_OPENGL #include "g2o/stuff/opengl_wrapper.h" #endif @@ -41,21 +43,19 @@ VertexSegment2D::VertexSegment2D() { estimate_.setZero(); } VertexSegment2DDrawAction::VertexSegment2DDrawAction() : DrawAction(typeid(VertexSegment2D).name()), pointSize_(nullptr) {} -bool VertexSegment2DDrawAction::refreshPropertyPtrs( - const std::shared_ptr& params_) { - if (!DrawAction::refreshPropertyPtrs(params_)) return false; - if (previousParams_) { - pointSize_ = previousParams_->makeProperty( - typeName_ + "::POINT_SIZE", 1.); - } else { - pointSize_ = nullptr; +DrawAction::Parameters* VertexSegment2DDrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(params_); + if (params) { + pointSize_ = + params->makeProperty(typeName_ + "::POINT_SIZE", 1.); } - return true; + return params; } bool VertexSegment2DDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; refreshPropertyPtrs(params_); diff --git a/g2o/types/slam2d_addons/vertex_segment2d.h b/g2o/types/slam2d_addons/vertex_segment2d.h index 68a4f07db..346ed730a 100644 --- a/g2o/types/slam2d_addons/vertex_segment2d.h +++ b/g2o/types/slam2d_addons/vertex_segment2d.h @@ -71,14 +71,12 @@ class G2O_TYPES_SLAM2D_ADDONS_API VertexSegment2DDrawAction public: VertexSegment2DDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; protected: std::shared_ptr pointSize_; - bool refreshPropertyPtrs( - const std::shared_ptr& params_) - override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) override; }; #endif diff --git a/g2o/types/slam3d/edge_se3.cpp b/g2o/types/slam3d/edge_se3.cpp index 9df189c21..78bbe410e 100644 --- a/g2o/types/slam3d/edge_se3.cpp +++ b/g2o/types/slam3d/edge_se3.cpp @@ -86,7 +86,7 @@ EdgeSE3DrawAction::EdgeSE3DrawAction() : DrawAction(typeid(EdgeSE3).name()) {} bool EdgeSE3DrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; refreshPropertyPtrs(params_); if (!previousParams_) return true; diff --git a/g2o/types/slam3d/edge_se3.h b/g2o/types/slam3d/edge_se3.h index cc1f83e55..fafef8bd1 100644 --- a/g2o/types/slam3d/edge_se3.h +++ b/g2o/types/slam3d/edge_se3.h @@ -28,7 +28,6 @@ #define G2O_EDGE_SE3_H_ #include -#include #include "g2o/config.h" #include "g2o/core/base_binary_edge.h" @@ -85,8 +84,7 @@ class G2O_TYPES_SLAM3D_API EdgeSE3DrawAction : public DrawAction { public: EdgeSE3DrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; }; #endif diff --git a/g2o/types/slam3d/edge_se3_pointxyz.cpp b/g2o/types/slam3d/edge_se3_pointxyz.cpp index db98233e9..4107630cf 100644 --- a/g2o/types/slam3d/edge_se3_pointxyz.cpp +++ b/g2o/types/slam3d/edge_se3_pointxyz.cpp @@ -134,7 +134,7 @@ EdgeSE3PointXYZDrawAction::EdgeSE3PointXYZDrawAction() bool EdgeSE3PointXYZDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; refreshPropertyPtrs(params_); if (!previousParams_) return true; diff --git a/g2o/types/slam3d/edge_se3_pointxyz.h b/g2o/types/slam3d/edge_se3_pointxyz.h index f9d99be6d..13799660f 100644 --- a/g2o/types/slam3d/edge_se3_pointxyz.h +++ b/g2o/types/slam3d/edge_se3_pointxyz.h @@ -81,8 +81,7 @@ class EdgeSE3PointXYZDrawAction : public DrawAction { public: EdgeSE3PointXYZDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; }; #endif diff --git a/g2o/types/slam3d/edge_se3_pointxyz_disparity.cpp b/g2o/types/slam3d/edge_se3_pointxyz_disparity.cpp index 9a405ad6c..fc2358241 100644 --- a/g2o/types/slam3d/edge_se3_pointxyz_disparity.cpp +++ b/g2o/types/slam3d/edge_se3_pointxyz_disparity.cpp @@ -162,7 +162,7 @@ EdgeProjectDisparityDrawAction::EdgeProjectDisparityDrawAction() bool EdgeProjectDisparityDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; refreshPropertyPtrs(params_); if (!previousParams_) return true; diff --git a/g2o/types/slam3d/edge_se3_pointxyz_disparity.h b/g2o/types/slam3d/edge_se3_pointxyz_disparity.h index b11f29b66..1675e9cf5 100644 --- a/g2o/types/slam3d/edge_se3_pointxyz_disparity.h +++ b/g2o/types/slam3d/edge_se3_pointxyz_disparity.h @@ -86,8 +86,7 @@ class G2O_TYPES_SLAM3D_API EdgeProjectDisparityDrawAction : public DrawAction { public: EdgeProjectDisparityDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; }; #endif diff --git a/g2o/types/slam3d/parameter_camera.cpp b/g2o/types/slam3d/parameter_camera.cpp index fd5b57458..ca03fbeec 100644 --- a/g2o/types/slam3d/parameter_camera.cpp +++ b/g2o/types/slam3d/parameter_camera.cpp @@ -34,6 +34,7 @@ #include #include +#include "g2o/core/hyper_graph_action.h" #include "g2o/types/slam3d/vertex_se3.h" #ifdef G2O_HAVE_OPENGL @@ -65,25 +66,21 @@ void CacheCamera::updateImpl() { CacheCameraDrawAction::CacheCameraDrawAction() : DrawAction(typeid(CacheCamera).name()) {} -bool CacheCameraDrawAction::refreshPropertyPtrs( - const std::shared_ptr& params_) { - if (!DrawAction::refreshPropertyPtrs(params_)) return false; - if (previousParams_) { - cameraZ_ = previousParams_->makeProperty( - typeName_ + "::CAMERA_Z", .05F); - cameraSide_ = previousParams_->makeProperty( - typeName_ + "::CAMERA_SIDE", .05F); - - } else { - cameraZ_ = nullptr; - cameraSide_ = nullptr; +DrawAction::Parameters* CacheCameraDrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(params_); + if (params) { + cameraZ_ = + params->makeProperty(typeName_ + "::CAMERA_Z", .05F); + cameraSide_ = + params->makeProperty(typeName_ + "::CAMERA_SIDE", .05F); } - return true; + return params; } bool CacheCameraDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params) { + HyperGraphElementAction::Parameters& params) { if (typeid(element).name() != typeName_) return false; auto* that = static_cast(&element); refreshPropertyPtrs(params); diff --git a/g2o/types/slam3d/parameter_camera.h b/g2o/types/slam3d/parameter_camera.h index 9735b6735..b50720c6b 100644 --- a/g2o/types/slam3d/parameter_camera.h +++ b/g2o/types/slam3d/parameter_camera.h @@ -171,13 +171,11 @@ class G2O_TYPES_SLAM3D_API CacheCameraDrawAction : public DrawAction { public: CacheCameraDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; protected: - bool refreshPropertyPtrs( - const std::shared_ptr& params_) - override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) override; std::shared_ptr cameraZ_, cameraSide_; }; #endif diff --git a/g2o/types/slam3d/parameter_se3_offset.cpp b/g2o/types/slam3d/parameter_se3_offset.cpp index a7b156a93..c1f4bc02f 100644 --- a/g2o/types/slam3d/parameter_se3_offset.cpp +++ b/g2o/types/slam3d/parameter_se3_offset.cpp @@ -31,6 +31,7 @@ #include #include "g2o/core/eigen_types.h" +#include "g2o/core/hyper_graph_action.h" #include "vertex_se3.h" #ifdef G2O_HAVE_OPENGL @@ -61,21 +62,19 @@ void CacheSE3Offset::updateImpl() { CacheSE3OffsetDrawAction::CacheSE3OffsetDrawAction() : DrawAction(typeid(CacheSE3Offset).name()) {} -bool CacheSE3OffsetDrawAction::refreshPropertyPtrs( - const std::shared_ptr& params_) { - if (!DrawAction::refreshPropertyPtrs(params_)) return false; +DrawAction::Parameters* CacheSE3OffsetDrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(params_); if (previousParams_) { - cubeSide_ = previousParams_->makeProperty( - typeName_ + "::CUBE_SIDE", .05F); - } else { - cubeSide_ = nullptr; + cubeSide_ = + params->makeProperty(typeName_ + "::CUBE_SIDE", .05F); } - return true; + return params; } bool CacheSE3OffsetDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; auto* that = static_cast(&element); refreshPropertyPtrs(params_); diff --git a/g2o/types/slam3d/parameter_se3_offset.h b/g2o/types/slam3d/parameter_se3_offset.h index f569d4087..e1c2f2743 100644 --- a/g2o/types/slam3d/parameter_se3_offset.h +++ b/g2o/types/slam3d/parameter_se3_offset.h @@ -93,13 +93,11 @@ class G2O_TYPES_SLAM3D_API CacheSE3OffsetDrawAction : public DrawAction { public: CacheSE3OffsetDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; protected: - bool refreshPropertyPtrs( - const std::shared_ptr& params_) - override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) override; std::shared_ptr cubeSide_; }; #endif diff --git a/g2o/types/slam3d/vertex_pointxyz.cpp b/g2o/types/slam3d/vertex_pointxyz.cpp index b99f162f4..eb76a1783 100644 --- a/g2o/types/slam3d/vertex_pointxyz.cpp +++ b/g2o/types/slam3d/vertex_pointxyz.cpp @@ -39,21 +39,19 @@ namespace g2o { VertexPointXYZDrawAction::VertexPointXYZDrawAction() : DrawAction(typeid(VertexPointXYZ).name()), pointSize_(nullptr) {} -bool VertexPointXYZDrawAction::refreshPropertyPtrs( - const std::shared_ptr& params_) { - if (!DrawAction::refreshPropertyPtrs(params_)) return false; - if (previousParams_) { - pointSize_ = previousParams_->makeProperty( - typeName_ + "::POINT_SIZE", 1.); - } else { - pointSize_ = nullptr; +DrawAction::Parameters* VertexPointXYZDrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(params_); + if (params) { + pointSize_ = + params->makeProperty(typeName_ + "::POINT_SIZE", 1.); } - return true; + return params; } bool VertexPointXYZDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params) { + HyperGraphElementAction::Parameters& params) { if (typeid(element).name() != typeName_) return false; initializeDrawActionsCache(); refreshPropertyPtrs(params); diff --git a/g2o/types/slam3d/vertex_pointxyz.h b/g2o/types/slam3d/vertex_pointxyz.h index a5a8738f6..d82d92417 100644 --- a/g2o/types/slam3d/vertex_pointxyz.h +++ b/g2o/types/slam3d/vertex_pointxyz.h @@ -58,14 +58,12 @@ class VertexPointXYZDrawAction : public DrawAction { public: VertexPointXYZDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; protected: std::shared_ptr pointSize_; - bool refreshPropertyPtrs( - const std::shared_ptr& params_) - override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) override; }; #endif diff --git a/g2o/types/slam3d/vertex_se3.cpp b/g2o/types/slam3d/vertex_se3.cpp index 343a278ca..d2e7076a0 100644 --- a/g2o/types/slam3d/vertex_se3.cpp +++ b/g2o/types/slam3d/vertex_se3.cpp @@ -30,6 +30,7 @@ #include #include +#include "g2o/core/hyper_graph_action.h" #include "g2o/types/slam3d/isometry3d_mappings.h" #ifdef G2O_HAVE_OPENGL #include "g2o/stuff/opengl_primitives.h" @@ -82,24 +83,21 @@ VertexSE3DrawAction::VertexSE3DrawAction() cacheDrawActions_ = nullptr; } -bool VertexSE3DrawAction::refreshPropertyPtrs( - const std::shared_ptr& params_) { - if (!DrawAction::refreshPropertyPtrs(params_)) return false; - if (previousParams_) { - triangleX_ = previousParams_->makeProperty( - typeName_ + "::TRIANGLE_X", .2F); - triangleY_ = previousParams_->makeProperty( - typeName_ + "::TRIANGLE_Y", .05F); - } else { - triangleX_ = nullptr; - triangleY_ = nullptr; +DrawAction::Parameters* VertexSE3DrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(params_); + if (params) { + triangleX_ = + params->makeProperty(typeName_ + "::TRIANGLE_X", .2F); + triangleY_ = + params->makeProperty(typeName_ + "::TRIANGLE_Y", .05F); } - return true; + return params; } bool VertexSE3DrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; initializeDrawActionsCache(); refreshPropertyPtrs(params_); diff --git a/g2o/types/slam3d/vertex_se3.h b/g2o/types/slam3d/vertex_se3.h index c9230ecfd..1fe893617 100644 --- a/g2o/types/slam3d/vertex_se3.h +++ b/g2o/types/slam3d/vertex_se3.h @@ -78,13 +78,11 @@ class G2O_TYPES_SLAM3D_API VertexSE3DrawAction : public DrawAction { public: VertexSE3DrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; protected: - bool refreshPropertyPtrs( - const std::shared_ptr& params_) - override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) override; std::shared_ptr triangleX_, triangleY_; }; #endif diff --git a/g2o/types/slam3d_addons/edge_se3_line.cpp b/g2o/types/slam3d_addons/edge_se3_line.cpp index 275f74303..8e1ae862c 100644 --- a/g2o/types/slam3d_addons/edge_se3_line.cpp +++ b/g2o/types/slam3d_addons/edge_se3_line.cpp @@ -67,26 +67,21 @@ EdgeSE3Line3DDrawAction::EdgeSE3Line3DDrawAction() lineLength_(nullptr), lineWidth_(nullptr) {} -bool EdgeSE3Line3DDrawAction::refreshPropertyPtrs( - const std::shared_ptr& params_) { - if (!DrawAction::refreshPropertyPtrs(params_)) { - return false; +DrawAction::Parameters* EdgeSE3Line3DDrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(params_); + if (params) { + lineLength_ = + params->makeProperty(typeName_ + "::LINE_LENGTH", 4.0F); + lineWidth_ = + params->makeProperty(typeName_ + "::LINE_WIDTH", 2.0F); } - if (previousParams_) { - lineLength_ = previousParams_->makeProperty( - typeName_ + "::LINE_LENGTH", 4.0F); - lineWidth_ = previousParams_->makeProperty( - typeName_ + "::LINE_WIDTH", 2.0F); - } else { - lineLength_ = nullptr; - lineWidth_ = nullptr; - } - return true; + return params; } bool EdgeSE3Line3DDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; refreshPropertyPtrs(params_); diff --git a/g2o/types/slam3d_addons/edge_se3_line.h b/g2o/types/slam3d_addons/edge_se3_line.h index 57f9aebbd..a6e59291b 100644 --- a/g2o/types/slam3d_addons/edge_se3_line.h +++ b/g2o/types/slam3d_addons/edge_se3_line.h @@ -32,6 +32,7 @@ #include "g2o/config.h" #include "g2o/core/base_binary_edge.h" #include "g2o/core/eigen_types.h" +#include "g2o/core/hyper_graph_action.h" #include "g2o/types/slam3d/vertex_se3.h" #include "g2o_types_slam3d_addons_api.h" #include "line3d.h" @@ -63,13 +64,11 @@ class G2O_TYPES_SLAM3D_ADDONS_API EdgeSE3Line3DDrawAction : public DrawAction { public: EdgeSE3Line3DDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; protected: - bool refreshPropertyPtrs( - const std::shared_ptr& params_) - override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) override; std::shared_ptr lineLength_, lineWidth_; }; #endif diff --git a/g2o/types/slam3d_addons/edge_se3_plane_calib.cpp b/g2o/types/slam3d_addons/edge_se3_plane_calib.cpp index 9cc7589bf..dff617b0f 100644 --- a/g2o/types/slam3d_addons/edge_se3_plane_calib.cpp +++ b/g2o/types/slam3d_addons/edge_se3_plane_calib.cpp @@ -45,24 +45,21 @@ EdgeSE3PlaneSensorCalibDrawAction::EdgeSE3PlaneSensorCalibDrawAction() planeWidth_(nullptr), planeHeight_(nullptr) {} -bool EdgeSE3PlaneSensorCalibDrawAction::refreshPropertyPtrs( - const std::shared_ptr& params_) { - if (!DrawAction::refreshPropertyPtrs(params_)) return false; - if (previousParams_) { - planeWidth_ = previousParams_->makeProperty( - typeName_ + "::PLANE_WIDTH", 0.5F); - planeHeight_ = previousParams_->makeProperty( - typeName_ + "::PLANE_HEIGHT", 0.5F); - } else { - planeWidth_ = nullptr; - planeHeight_ = nullptr; +DrawAction::Parameters* EdgeSE3PlaneSensorCalibDrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(params_); + if (params) { + planeWidth_ = + params->makeProperty(typeName_ + "::PLANE_WIDTH", 0.5F); + planeHeight_ = + params->makeProperty(typeName_ + "::PLANE_HEIGHT", 0.5F); } - return true; + return params; } bool EdgeSE3PlaneSensorCalibDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; refreshPropertyPtrs(params_); diff --git a/g2o/types/slam3d_addons/edge_se3_plane_calib.h b/g2o/types/slam3d_addons/edge_se3_plane_calib.h index faa8cc8c9..c75104549 100644 --- a/g2o/types/slam3d_addons/edge_se3_plane_calib.h +++ b/g2o/types/slam3d_addons/edge_se3_plane_calib.h @@ -70,13 +70,11 @@ class EdgeSE3PlaneSensorCalibDrawAction : public DrawAction { G2O_TYPES_SLAM3D_ADDONS_API EdgeSE3PlaneSensorCalibDrawAction(); G2O_TYPES_SLAM3D_ADDONS_API bool operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) - override; + HyperGraphElementAction::Parameters& params_) override; protected: - bool refreshPropertyPtrs( - const std::shared_ptr& params_) - override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) override; std::shared_ptr planeWidth_, planeHeight_; }; #endif diff --git a/g2o/types/slam3d_addons/vertex_line3d.cpp b/g2o/types/slam3d_addons/vertex_line3d.cpp index baf54af6f..5d59c1e34 100644 --- a/g2o/types/slam3d_addons/vertex_line3d.cpp +++ b/g2o/types/slam3d_addons/vertex_line3d.cpp @@ -41,26 +41,21 @@ VertexLine3DDrawAction::VertexLine3DDrawAction() lineLength_(nullptr), lineWidth_(nullptr) {} -bool VertexLine3DDrawAction::refreshPropertyPtrs( - const std::shared_ptr& params_) { - if (!DrawAction::refreshPropertyPtrs(params_)) { - return false; - } - if (previousParams_) { - lineLength_ = previousParams_->makeProperty( - typeName_ + "::LINE_LENGTH", 15); - lineWidth_ = previousParams_->makeProperty( - typeName_ + "::LINE_WIDTH", 5); - } else { - lineLength_ = nullptr; - lineWidth_ = nullptr; +DrawAction::Parameters* VertexLine3DDrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(params_); + if (params) { + lineLength_ = + params->makeProperty(typeName_ + "::LINE_LENGTH", 15); + lineWidth_ = + params->makeProperty(typeName_ + "::LINE_WIDTH", 5); } - return true; + return params; } bool VertexLine3DDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) { return false; } diff --git a/g2o/types/slam3d_addons/vertex_line3d.h b/g2o/types/slam3d_addons/vertex_line3d.h index ad29495e5..7d8a5d369 100644 --- a/g2o/types/slam3d_addons/vertex_line3d.h +++ b/g2o/types/slam3d_addons/vertex_line3d.h @@ -57,13 +57,11 @@ class VertexLine3DDrawAction : public DrawAction { public: VertexLine3DDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; protected: - bool refreshPropertyPtrs( - const std::shared_ptr& params_) - override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) override; std::shared_ptr lineLength_, lineWidth_; }; #endif diff --git a/g2o/types/slam3d_addons/vertex_plane.cpp b/g2o/types/slam3d_addons/vertex_plane.cpp index 44dfcc847..dd59b3022 100644 --- a/g2o/types/slam3d_addons/vertex_plane.cpp +++ b/g2o/types/slam3d_addons/vertex_plane.cpp @@ -29,6 +29,7 @@ #include #include +#include "g2o/core/hyper_graph_action.h" #include "g2o/stuff/macros.h" #include "g2o/stuff/misc.h" #include "g2o/stuff/opengl_wrapper.h" @@ -45,24 +46,21 @@ VertexPlaneDrawAction::VertexPlaneDrawAction() planeWidth_(nullptr), planeHeight_(nullptr) {} -bool VertexPlaneDrawAction::refreshPropertyPtrs( - const std::shared_ptr& params_) { - if (!DrawAction::refreshPropertyPtrs(params_)) return false; - if (previousParams_) { - planeWidth_ = previousParams_->makeProperty( - typeName_ + "::PLANE_WIDTH", 3); - planeHeight_ = previousParams_->makeProperty( - typeName_ + "::PLANE_HEIGHT", 3); - } else { - planeWidth_ = nullptr; - planeHeight_ = nullptr; +DrawAction::Parameters* VertexPlaneDrawAction::refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) { + DrawAction::Parameters* params = DrawAction::refreshPropertyPtrs(params_); + if (params) { + planeWidth_ = + params->makeProperty(typeName_ + "::PLANE_WIDTH", 3); + planeHeight_ = + params->makeProperty(typeName_ + "::PLANE_HEIGHT", 3); } - return true; + return params; } bool VertexPlaneDrawAction::operator()( HyperGraph::HyperGraphElement& element, - const std::shared_ptr& params_) { + HyperGraphElementAction::Parameters& params_) { if (typeid(element).name() != typeName_) return false; refreshPropertyPtrs(params_); if (!previousParams_) return true; diff --git a/g2o/types/slam3d_addons/vertex_plane.h b/g2o/types/slam3d_addons/vertex_plane.h index 2bdbf190c..ea853c9a7 100644 --- a/g2o/types/slam3d_addons/vertex_plane.h +++ b/g2o/types/slam3d_addons/vertex_plane.h @@ -56,13 +56,11 @@ class VertexPlaneDrawAction : public DrawAction { public: VertexPlaneDrawAction(); bool operator()(HyperGraph::HyperGraphElement& element, - const std::shared_ptr& - params_) override; + HyperGraphElementAction::Parameters& params_) override; protected: - bool refreshPropertyPtrs( - const std::shared_ptr& params_) - override; + DrawAction::Parameters* refreshPropertyPtrs( + HyperGraphElementAction::Parameters& params_) override; std::shared_ptr planeWidth_, planeHeight_; }; #endif