Simplify template usage in localization/mapping
Description
Our current code is quite template-heavy, which makes configuring localization/mapping classes quite complex.
To illustrate this, here is the definition of the ndt localization node as seen in the code:
// Alias common types.
using CloudMsg = sensor_msgs::msg::PointCloud2;
template<typename ... Types>
using P2DNDTLocalizer = ndt::P2DNDTLocalizer<Types...>;
template<typename ... Types>
using P2DNDTConfig = ndt::P2DNDTLocalizerConfig<Types...>;
// TODO(yunus.caliskan) remove the hard-coded optimizer set up and make it fully configurable
using Optimizer_ =
common::optimization::NewtonsMethodOptimizer<common::optimization::MoreThuenteLineSearch>;
using OptimizerOptions_ = common::optimization::OptimizationOptions;
using PoseInitializer_ = localization_common::BestEffortInitializer;
/// P2D NDT localizer node. Currently uses the hard coded optimizer and pose initializers.
/// \tparam OptimizerT Hard coded for Newton optimizer. TODO(yunus.caliskan): Make Configurable
/// \tparam OptimizerOptionsT Hard coded for Newton optimizer. TODO(yunus.caliskan): Make
/// Configurable
/// \tparam PoseInitializerT Hard coded for Best effort. TODO(yunus.caliskan): Make Configurable
template<typename OptimizerT = Optimizer_,
typename OptimizerOptionsT = OptimizerOptions_,
typename PoseInitializerT = PoseInitializer_>
class NDT_NODES_PUBLIC P2DNDTLocalizerNode
: public localization_nodes::RelativeLocalizerNode<CloudMsg, CloudMsg,
P2DNDTLocalizer<OptimizerT, OptimizerOptionsT>,
PoseInitializerT>
{
public:
using Localizer = P2DNDTLocalizer<OptimizerT, OptimizerOptionsT>;
using RegistrationSummary = typename Localizer::RegistrationSummary;
using LocalizerBasePtr = std::unique_ptr<localization_common::RelativeLocalizerBase<CloudMsg,
CloudMsg, RegistrationSummary>>;
using ParentT = localization_nodes::RelativeLocalizerNode<CloudMsg, CloudMsg,
Localizer, PoseInitializerT>;
using PoseWithCovarianceStamped = typename Localizer::PoseWithCovarianceStamped;
using Transform = typename Localizer::Transform;
using EigTranslation = Eigen::Vector3d;
using EigRotation = Eigen::Quaterniond;
static constexpr auto EPS = std::numeric_limits<ndt::Real>::epsilon();
...
};
This ends up being quite verbose.
This is in a strong contrast with quite simple interface of what the code should actually do:
- configure itself with proper messages and a localizer
- when a new observation is received - register it to map - publish the pose
- when a new map is received - update the map
I believe there is a number of things we can try to make the configuration of our classes a bit simpler:
- Make sure the classes know only enough to function.
- Make sure the classes are always configured with their parameters upon creation.
- We can think of ways to reduce mixing of static and dynamic polymorphism. Here we are bound by nodes relying on dynamic polymorphism, while some classes like
OptimizationProblem
should stick with static polymorphism due to runtime constraints. - This is not a full list and might be updated as we go.
Expected behavior
The classes should become simpler to configure in the code and allow for easier parametrization from ros parameters.
Definition of Done
This is quite an open-ended issue, so it is hard to define a clear DOD. I will leave this open for now.
Edited by Igor Bogoslavskyi