Skip to content
Snippets Groups Projects
Commit 32ff61e7 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

[skip ci] minor

parent 1f55b83d
No related branches found
No related tags found
1 merge request!448Draft: Resolve "Implementation of new nodes creation"
Pipeline #12361 skipped
......@@ -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
......@@ -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"
......
......@@ -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
......
......@@ -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_;
// }
// };
......@@ -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;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment