diff --git a/include/core/processor/processor_pose.h b/include/core/processor/processor_pose.h index e3cb9e497570d7684acd2257779553f0830218b2..3ee3b2315356bff7025ec476d6bf332cfbdfc712 100644 --- a/include/core/processor/processor_pose.h +++ b/include/core/processor/processor_pose.h @@ -22,13 +22,8 @@ #pragma once // Wolf -#include "core/sensor/sensor_base.h" #include "core/processor/processor_base.h" -#include "core/capture/capture_pose.h" -#include "core/sensor/sensor_pose.h" -#include "core/factor/factor_pose_3d_with_extrinsics.h" - namespace wolf { WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorPose); @@ -68,6 +63,7 @@ class ProcessorPose : public ProcessorBase{ protected: ParamsProcessorPosePtr params_pfnomove_; + bool is_2d_; }; } /* namespace wolf */ \ No newline at end of file diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 1d95026379e6ffc8284ae3e675847e473f590a77..6602a4e9cf24245df8febd5abf56dd6785e285fb 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -282,7 +282,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh } -// #include "core/problem/problem.h" #include "core/hardware/hardware_base.h" #include "core/capture/capture_base.h" #include "core/processor/processor_base.h" diff --git a/src/processor/processor_pose.cpp b/src/processor/processor_pose.cpp index 1cc44ea0d3004bc025b7c38c7ce5c5b8c451ddff..10a20636ef40cfb0bfca5e8019fb90d31b40cb1c 100644 --- a/src/processor/processor_pose.cpp +++ b/src/processor/processor_pose.cpp @@ -28,12 +28,16 @@ #include "core/processor/processor_pose.h" +#include "core/sensor/sensor_pose.h" +#include "core/capture/capture_pose.h" +#include "core/factor/factor_pose_3d_with_extrinsics.h" -namespace wolf{ +namespace wolf +{ -inline ProcessorPose::ProcessorPose(ParamsProcessorPosePtr _params_pfnomove) : - ProcessorBase("ProcessorPose", 3, _params_pfnomove), +ProcessorPose::ProcessorPose(ParamsProcessorPosePtr _params_pfnomove) : + ProcessorBase("ProcessorPose", 0, _params_pfnomove), params_pfnomove_(std::make_shared<ParamsProcessorPose>(*_params_pfnomove)) { @@ -41,10 +45,18 @@ inline ProcessorPose::ProcessorPose(ParamsProcessorPosePtr _params_pfnomove) : void ProcessorPose::configure(SensorBasePtr _sensor) { + auto sen_pose_2d = std::dynamic_pointer_cast<SensorPose2d>(_sensor); + auto sen_pose_3d = std::dynamic_pointer_cast<SensorPose3d>(_sensor); + + if (not sen_pose_2d and not sen_pose_3d) + { + throw std::runtime_error("Configuring a ProcessorPose with a sensor " + _sensor->getType() + ", should be SensorPose2d or SensorPose3d"); + } + is_2d_ = sen_pose_2d != nullptr; } -void ProcessorPose::createFactorIfNecessary(){ - auto sensor_pose = std::static_pointer_cast<SensorPose3d>(getSensor()); +void ProcessorPose::createFactorIfNecessary() +{ auto kf_it_last = buffer_frame_.getContainer().end(); auto kf_it = buffer_frame_.getContainer().begin(); while (kf_it != buffer_frame_.getContainer().end()) @@ -59,8 +71,16 @@ void ProcessorPose::createFactorIfNecessary(){ auto cap = std::static_pointer_cast<CapturePose>(cap_it->second); cap->link(kf_it->second); FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap, "Pose", cap->getData(), cap->getDataCovariance()); - FactorPose3dWithExtrinsicsPtr fac = FactorBase::emplace<FactorPose3dWithExtrinsics>(feat, feat, shared_from_this(), false, TOP_MOTION); - + + if (is_2d_) + { + // TODO + throw std::runtime_error("FactorPose2dWithExtrinsics not implemented"); + } + else + { + FactorBase::emplace<FactorPose3dWithExtrinsics>(feat, feat, shared_from_this(), false, TOP_ABS); + } // erase removes range [first, last): it does not removes last // so increment the iterator so that it points to the next element in the container buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), std::next(cap_it)); @@ -82,7 +102,6 @@ void ProcessorPose::createFactorIfNecessary(){ } - inline void ProcessorPose::processCapture(CaptureBasePtr _capture) { if (!_capture) @@ -92,11 +111,11 @@ inline void ProcessorPose::processCapture(CaptureBasePtr _capture) } // nothing to do if any of the two buffer is empty if(buffer_frame_.empty()){ - WOLF_DEBUG("PInertialKinematic: Frame buffer empty, time ", _capture->getTimeStamp()); + WOLF_DEBUG("processCapture: Frame buffer empty, time ", _capture->getTimeStamp()); return; } if(buffer_frame_.empty()){ - WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp()); + WOLF_DEBUG("processCapture: Capture buffer empty, time ", _capture->getTimeStamp()); return; } @@ -125,9 +144,6 @@ inline void ProcessorPose::processKeyFrame(FrameBasePtr _keyframe_ptr) } - - - } /* namespace wolf */ // Register in the FactoryProcessor diff --git a/src/utils/loader.cpp b/src/utils/loader.cpp index c11bb26671bdf6368de27b4637b708bc6b54e87b..1b9e0b7520f74cfbbf7e8a949de264470a44cde8 100644 --- a/src/utils/loader.cpp +++ b/src/utils/loader.cpp @@ -47,12 +47,3 @@ void LoaderRaw::close() if (resource_) dlclose(resource_); } -// class LoaderPlugin: public Loader{ -// ClassLoader* resource_; -// void load(){ -// resource_ = new ClassLoader(path_); -// } -// void close(){ -// delete resource_; -// } -// }; diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp index 8635bf8b48002c5631ebc1fbdb033847519be214..8898eda8a5426e8170e56de5254b49d87cbf404e 100644 --- a/test/gtest_map_yaml.cpp +++ b/test/gtest_map_yaml.cpp @@ -36,7 +36,6 @@ #include "core/state_block/state_angle.h" #include "core/state_block/state_quaternion.h" #include "core/state_block/local_parametrization_quaternion.h" -// #include "core/yaml/yaml_conversion.h" #include <iostream> using namespace wolf;