diff --git a/include/laser/sensor/sensor_laser_2D.h b/include/laser/sensor/sensor_laser_2D.h
index 89d3ce7687247615237b4bfcd0f1055d047c3ce7..64caa44e3fd96f374c90b08faa7f1b0336a3c218 100644
--- a/include/laser/sensor/sensor_laser_2D.h
+++ b/include/laser/sensor/sensor_laser_2D.h
@@ -31,11 +31,11 @@ class SensorLaser2D : public SensorBase
public:
/** \brief Constructor with extrinsics
- *
+ *
* \param _p_ptr StateBlock pointer to the sensor position
* \param _o_ptr StateBlock pointer to the sensor orientation
- *
- **/
+ *
+ **/
SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr);
/** \brief Constructor with extrinsics and scan parameters
@@ -58,25 +58,26 @@ class SensorLaser2D : public SensorBase
SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params);
virtual ~SensorLaser2D();
-
+
void setDefaultScanParams();
-
+
/** \brief Set scanner intrinsic parameters
- *
+ *
* \param _params struct with scanner intrinsic parameters. See laser_scan_utils library API for reference.
- *
- **/
+ *
+ **/
void setScanParams(const laserscanutils::LaserScanParams & _params);
/** \brief Get scanner intrinsic parameters
- *
+ *
* Get scanner intrinsic parameters
- *
- **/
+ *
+ **/
const laserscanutils::LaserScanParams & getScanParams() const;
public:
static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po, const IntrinsicsBasePtr _intrinsics);
+ static SensorBasePtr createAutoConf(const std::string& _unique_name, const paramsServer& _server);
static IntrinsicsBasePtr createParams(const std::string& _filename_dot_yaml);
};
diff --git a/src/sensor/sensor_GPS.cpp b/src/sensor/sensor_GPS.cpp
deleted file mode 100644
index 666788225c5c7b3c1a35ae8004569f38053e1573..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_GPS.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-
-#include "core/sensor/sensor_GPS.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-
-namespace wolf {
-
-SensorGPS::SensorGPS(StateBlockPtr _p_ptr, //GPS sensor position with respect to the car frame (base frame)
- StateBlockPtr _o_ptr, //GPS sensor orientation with respect to the car frame (base frame)
- StateBlockPtr _bias_ptr, //GPS sensor time bias
- StateBlockPtr _map_p_ptr, //initial position of vehicle's frame with respect to starting point frame
- StateBlockPtr _map_o_ptr) //initial orientation of vehicle's frame with respect to the starting point frame
- :
- SensorBase("GPS", _p_ptr, _o_ptr, _bias_ptr, 0)
-{
- getStateBlockVec().resize(5);
- setStateBlockPtrStatic(3, _map_p_ptr); // Map position
- setStateBlockPtrStatic(4, _map_o_ptr); // Map orientation
- //
-}
-
-SensorGPS::~SensorGPS()
-{
- //
-}
-
-StateBlockPtr SensorGPS::getMapP() const
-{
- return getStateBlockPtrStatic(3);
-}
-
-StateBlockPtr SensorGPS::getMapO() const
-{
- return getStateBlockPtrStatic(4);
-}
-
-// Define the factory method
-SensorBasePtr SensorGPS::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_p.size() == 3 && "Bad extrinsics vector length. Should be 3 for 3D.");
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_p, true);
- StateBlockPtr ori_ptr = nullptr;
- SensorBasePtr sen = std::make_shared<SensorGPS>(pos_ptr, ori_ptr, nullptr, nullptr, nullptr);
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("GPS",SensorGPS)
-} // namespace wolf
diff --git a/src/sensor/sensor_GPS_fix.cpp b/src/sensor/sensor_GPS_fix.cpp
deleted file mode 100644
index ca204f36449afda97c38c4e5278859a3e752a036..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_GPS_fix.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-#include "core/sensor/sensor_GPS_fix.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-
-namespace wolf {
-
-SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics) :
- SensorBase("GPS FIX", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), nullptr, _intrinsics.noise_std)
-{
- assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
- && "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
-}
-
-SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr) :
- SensorGPSFix(_extrinsics, *_intrinsics_ptr)
-{
- //
-}
-
-SensorGPSFix::~SensorGPSFix()
-{
- //
-}
-
-// Define the factory method
-SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics,
- const IntrinsicsBasePtr _intrinsics)
-{
- assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
- && "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
- SensorGPSFixPtr sen = std::make_shared<SensorGPSFix>(_extrinsics, std::static_pointer_cast<IntrinsicsGPSFix>(_intrinsics));
- sen->getP()->fix();
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("GPS FIX", SensorGPSFix)
-} // namespace wolf
diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp
deleted file mode 100644
index de0146784289081c2adb55bd272982d9c86bd797..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_IMU.cpp
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "core/sensor/sensor_IMU.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-
-namespace wolf {
-
-SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params) :
- SensorIMU(_extrinsics, *_params)
-{
- //
-}
-
-SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params) :
- SensorBase("IMU", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6s()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, true),
- a_noise(_params.a_noise),
- w_noise(_params.w_noise),
- ab_initial_stdev(_params.ab_initial_stdev),
- wb_initial_stdev(_params.wb_initial_stdev),
- ab_rate_stdev(_params.ab_rate_stdev),
- wb_rate_stdev(_params.wb_rate_stdev)
-{
- assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D.");
-}
-
-SensorIMU::~SensorIMU()
-{
- //
-}
-
-// Define the factory method
-SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
-
- IntrinsicsIMUPtr params = std::static_pointer_cast<IntrinsicsIMU>(_intrinsics);
- SensorIMUPtr sen = std::make_shared<SensorIMU>(_extrinsics_pq, params);
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("IMU", SensorIMU)
-} // namespace wolf
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
deleted file mode 100644
index 26238eb8dcdf059a440b82ce149f5daef8ae20d7..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_base.cpp
+++ /dev/null
@@ -1,407 +0,0 @@
-#include "core/sensor/sensor_base.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/factor/factor_block_absolute.h"
-#include "core/factor/factor_quaternion_absolute.h"
-
-
-namespace wolf {
-
-unsigned int SensorBase::sensor_id_count_ = 0;
-
-SensorBase::SensorBase(const std::string& _type,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr,
- const unsigned int _noise_size,
- const bool _extr_dyn,
- const bool _intr_dyn) :
- NodeBase("SENSOR", _type),
- hardware_ptr_(),
- state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
- calib_size_(0),
- sensor_id_(++sensor_id_count_), // simple ID factory
- extrinsic_dynamic_(_extr_dyn),
- intrinsic_dynamic_(_intr_dyn),
- has_capture_(false),
- noise_std_(_noise_size),
- noise_cov_(_noise_size, _noise_size)
-{
- noise_std_.setZero();
- noise_cov_.setZero();
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _intr_ptr;
- updateCalibSize();
-}
-
-SensorBase::SensorBase(const std::string& _type,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr,
- const Eigen::VectorXs & _noise_std,
- const bool _extr_dyn,
- const bool _intr_dyn) :
- NodeBase("SENSOR", _type),
- hardware_ptr_(),
- state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
- calib_size_(0),
- sensor_id_(++sensor_id_count_), // simple ID factory
- extrinsic_dynamic_(_extr_dyn),
- intrinsic_dynamic_(_intr_dyn),
- has_capture_(false),
- noise_std_(_noise_std),
- noise_cov_(_noise_std.size(), _noise_std.size())
-{
- state_block_vec_[0] = _p_ptr;
- state_block_vec_[1] = _o_ptr;
- state_block_vec_[2] = _intr_ptr;
- setNoiseStd(_noise_std);
- updateCalibSize();
-}
-
-SensorBase::~SensorBase()
-{
- // Remove State Blocks
- removeStateBlocks();
-}
-
-void SensorBase::removeStateBlocks()
-{
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
- {
- auto sbp = getStateBlockPtrStatic(i);
- if (sbp != nullptr)
- {
- if (getProblem() != nullptr && !extrinsic_dynamic_)
- {
- getProblem()->removeStateBlock(sbp);
- }
- setStateBlockPtrStatic(i, nullptr);
- }
- }
-}
-
-void SensorBase::fix()
-{
- for( auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->fix();
- updateCalibSize();
-}
-
-void SensorBase::unfix()
-{
- for( auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->unfix();
- updateCalibSize();
-}
-
-void SensorBase::fixExtrinsics()
-{
- for (unsigned int i = 0; i < 2; i++)
- {
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->fix();
- }
- updateCalibSize();
-}
-
-void SensorBase::unfixExtrinsics()
-{
- for (unsigned int i = 0; i < 2; i++)
- {
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->unfix();
- }
- updateCalibSize();
-}
-
-void SensorBase::fixIntrinsics()
-{
- for (unsigned int i = 2; i < state_block_vec_.size(); i++)
- {
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->fix();
- }
- updateCalibSize();
-}
-
-void SensorBase::unfixIntrinsics()
-{
- for (unsigned int i = 2; i < state_block_vec_.size(); i++)
- {
- auto sbp = state_block_vec_[i];
- if (sbp != nullptr)
- sbp->unfix();
- }
- updateCalibSize();
-}
-
-void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
-{
- assert(!isStateBlockDynamic(_i) && "SensorBase::addPriorParameter only allowed for static parameters");
- assert(_i < state_block_vec_.size() && "State block not found");
-
- StateBlockPtr _sb = getStateBlockPtrStatic(_i);
- bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr);
-
- assert(((!is_quaternion && _x.size() == _cov.rows() && _x.size() == _cov.cols()) ||
- (is_quaternion && _x.size() == 4 &&_cov.rows() == 3 && _cov.cols() == 3)) && "bad prior/covariance dimensions");
- assert((_size == -1 && _start_idx == 0) || (_size+_start_idx <= _sb->getSize()));
- assert(_size == -1 || _size == _x.size());
- assert(!(_size != -1 && is_quaternion) && "prior of a segment of state not available for quaternion");
-
- // set StateBlock state
- if (_size == -1)
- _sb->setState(_x);
- else
- {
- Eigen::VectorXs new_x = _sb->getState();
- new_x.segment(_start_idx,_size) = _x;
- _sb->setState(new_x);
- }
-
- // remove previous prior (if any)
- if (params_prior_map_.find(_i) != params_prior_map_.end())
- params_prior_map_[_i]->remove();
-
- // create feature
- FeatureBasePtr ftr_prior = std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
-
- // set feature problem
- ftr_prior->setProblem(getProblem());
-
- // create & add factor absolute
- if (is_quaternion)
- ftr_prior->addFactor(std::make_shared<FactorQuaternionAbsolute>(_sb));
- else
- ftr_prior->addFactor(std::make_shared<FactorBlockAbsolute>(_sb, _start_idx, _size));
-
- // store feature in params_prior_map_
- params_prior_map_[_i] = ftr_prior;
-}
-
-void SensorBase::registerNewStateBlocks()
-{
- if (getProblem() != nullptr)
- {
- for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
- {
- if (i < 2 && !isExtrinsicDynamic())
- {
- auto sbp = getStateBlockPtrStatic(i);
- if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
- }
- if (i >= 2 && !isIntrinsicDynamic())
- {
- auto sbp = getStateBlockPtrStatic(i);
- if (sbp != nullptr)
- getProblem()->addStateBlock(sbp);
- }
- }
- }
-}
-
-void SensorBase::setNoiseStd(const Eigen::VectorXs& _noise_std) {
- noise_std_ = _noise_std;
- noise_cov_ = _noise_std.array().square().matrix().asDiagonal();
-}
-
-void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
- noise_std_ = _noise_cov.diagonal().array().sqrt();
- noise_cov_ = _noise_cov;
-}
-
-CaptureBasePtr SensorBase::lastKeyCapture(void)
-{
- // we search for the most recent Capture of this sensor which belongs to a KeyFrame
- CaptureBasePtr capture = nullptr;
- FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
- FrameBaseRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
- {
- if ((*frame_rev_it)->isKey())
- {
- capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
- if (capture)
- // found the most recent Capture made by this sensor !
- break;
- }
- frame_rev_it++;
- }
- return capture;
-}
-
-CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
-{
- // we search for the most recent Capture of this sensor before _ts
- CaptureBasePtr capture = nullptr;
- FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
-
- // We iterate in reverse since we're likely to find it close to the rbegin() place.
- FrameBaseRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
- {
- if ((*frame_rev_it)->getTimeStamp() <= _ts)
- {
- CaptureBasePtr capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
- if (capture)
- // found the most recent Capture made by this sensor !
- break;
- }
- frame_rev_it++;
- }
- return capture;
-}
-
-StateBlockPtr SensorBase::getP(const TimeStamp _ts)
-{
- return getStateBlock(0, _ts);
-}
-
-StateBlockPtr SensorBase::getO(const TimeStamp _ts)
-{
- return getStateBlock(1, _ts);
-}
-
-StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts)
-{
- return getStateBlock(2, _ts);
-}
-
-StateBlockPtr SensorBase::getP()
-{
- return getStateBlock(0);
-}
-
-StateBlockPtr SensorBase::getO()
-{
- return getStateBlock(1);
-}
-
-StateBlockPtr SensorBase::getIntrinsic()
-{
- return getStateBlock(2);
-}
-
-SizeEigen SensorBase::computeCalibSize() const
-{
- SizeEigen sz = 0;
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
- {
- auto sb = state_block_vec_[i];
- if (sb && !sb->isFixed())
- sz += sb->getSize();
- }
- return sz;
-}
-
-Eigen::VectorXs SensorBase::getCalibration() const
-{
- SizeEigen index = 0;
- SizeEigen sz = getCalibSize();
- Eigen::VectorXs calib(sz);
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
- {
- auto sb = getStateBlockPtrStatic(i);
- if (sb && !sb->isFixed())
- {
- calib.segment(index, sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
- }
- return calib;
-}
-
-bool SensorBase::process(const CaptureBasePtr capture_ptr)
-{
- capture_ptr->setSensor(shared_from_this());
-
- for (const auto processor : processor_list_)
- {
- processor->process(capture_ptr);
- }
-
- return true;
-}
-
-ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
-{
- processor_list_.push_back(_proc_ptr);
- _proc_ptr->setSensor(shared_from_this());
- _proc_ptr->setProblem(getProblem());
- return _proc_ptr;
-}
-
-StateBlockPtr SensorBase::getStateBlock(unsigned int _i)
-{
- CaptureBasePtr cap;
-
- if (isStateBlockDynamic(_i, cap))
- return cap->getStateBlock(_i);
-
- return getStateBlockPtrStatic(_i);
-}
-
-StateBlockPtr SensorBase::getStateBlock(unsigned int _i, const TimeStamp& _ts)
-{
- CaptureBasePtr cap;
-
- if (isStateBlockDynamic(_i, _ts, cap))
- return cap->getStateBlock(_i);
-
- return getStateBlockPtrStatic(_i);
-}
-
-bool SensorBase::isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap)
-{
- if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
- {
- cap = lastKeyCapture();
-
- return cap != nullptr;
- }
- else
- return false;
-}
-
-bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap)
-{
- if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
- {
- cap = lastCapture(_ts);
-
- return cap != nullptr;
- }
- else
- return false;
-}
-
-bool SensorBase::isStateBlockDynamic(unsigned int _i)
-{
- CaptureBasePtr cap;
-
- return isStateBlockDynamic(_i,cap);
-}
-
-bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts)
-{
- CaptureBasePtr cap;
-
- return isStateBlockDynamic(_i,_ts,cap);
-}
-
-void SensorBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
- for (auto prc : processor_list_)
- prc->setProblem(_problem);
-}
-
-} // namespace wolf
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
deleted file mode 100644
index 3e07aea5750b77c76f3abdb2bddab92ddb69d2f9..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_diff_drive.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-#include "core/sensor/sensor_diff_drive.h"
-#include "core/state_block/state_block.h"
-#include "core/capture/capture_motion.h"
-#include "core/utils/eigen_assert.h"
-
-namespace wolf {
-
-SensorDiffDrive::SensorDiffDrive(const StateBlockPtr& _p_ptr,
- const StateBlockPtr& _o_ptr,
- const StateBlockPtr& _i_ptr,
- const IntrinsicsDiffDrivePtr& _intrinsics) :
- SensorBase("DIFF DRIVE", _p_ptr, _o_ptr, _i_ptr, 2, false, false),
- intrinsics_ptr_{_intrinsics}
-{
- //
-}
-
-// Define the factory method
-SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
- const Eigen::VectorXs& _extrinsics_po,
- const IntrinsicsBasePtr _intrinsics)
-{
- Eigen::VectorSizeCheck<3>::check(_extrinsics_po);
-
- // cast intrinsics into derived type
- IntrinsicsDiffDrivePtr params = std::dynamic_pointer_cast<IntrinsicsDiffDrive>(_intrinsics);
-
- if (params == nullptr)
- {
- WOLF_ERROR("SensorDiffDrive::create expected IntrinsicsDiffDrive !");
- return nullptr;
- }
-
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
- StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true);
- StateBlockPtr int_ptr = std::make_shared<StateBlock>(params->factors_, true);
-
- SensorBasePtr odo = std::make_shared<SensorDiffDrive>(pos_ptr, ori_ptr, int_ptr, params);
-
- odo->setName(_unique_name);
-
- /// @todo make calibration optional at creation
- //if (calibrate)
- // odo->unfixIntrinsics();
-
- return odo;
-}
-
-/// @todo Further work to enforce wheel model
-
-//const IntrinsicsDiffDrive& SensorDiffDrive::getIntrinsics()
-//{
-//// if (intrinsic_ptr_)
-//// {
-//// const auto& intrinsics = intrinsic_ptr_->getVector();
-
-//// intrinsics_.left_radius_factor_ = intrinsics(0);
-//// intrinsics_.right_radius_factor_ = intrinsics(1);
-//// intrinsics_.separation_factor_ = intrinsics(2);
-//// }
-
-// return intrinsics_;
-//}
-
-//void SensorDiffDrive::initIntrisics()
-//{
-// assert(intrinsic_ptr_ == nullptr &&
-// "SensorDiffDrive::initIntrisicsPtr should only be called once at construction.");
-
-// Eigen::Vector3s state;
-// state << intrinsics_.left_radius_factor_,
-// intrinsics_.right_radius_factor_,
-// intrinsics_.separation_factor_;
-
-// assert(state(0) > 0 && "The left_radius_factor should be rather close to 1.");
-// assert(state(1) > 0 && "The right_radius_factor should be rather close to 1.");
-// assert(state(2) > 0 && "The separation_factor should be rather close to 1.");
-
-// intrinsic_ptr_ = new StateBlock(state);
-//}
-
-//void SensorDiffDrive::computeDataCov(const Eigen::Vector2s &_data, Eigen::Matrix2s &_data_cov)
-//{
-// const double dp_std_l = intrinsics_.left_gain_ * _data(0);
-// const double dp_std_r = intrinsics_.right_gain_ * _data(1);
-
-// const double dp_var_l = dp_std_l * dp_std_l;
-// const double dp_var_r = dp_std_r * dp_std_r;
-
-// /// Wheel resolution covariance, which is like a DC offset equal to half of
-// /// the resolution, which is the theoretical average error:
-// const double dp_std_avg = Scalar(0.5) * intrinsics_.left_resolution_;
-// const double dp_var_avg = dp_std_avg * dp_std_avg;
-
-// /// Set covariance matrix (diagonal):
-// _data_cov.diagonal() << dp_var_l + dp_var_avg,
-// dp_var_r + dp_var_avg;
-//}
-
-// This overload is probably not the best solution as it forces
-// me to call addCapture from a SensorDiffDrivePtr whereas
-// problem->installSensor() return a SensorBasePtr.
-//bool SensorDiffDrive::addCapture(CaptureBasePtr _capture_ptr)
-//{
-// if (intrinsics_.data_is_position_)
-// {
-// Eigen::Vector2s data = _capture_ptr->getData();
-
-// // dt is set to one as we are dealing with wheel position
-// data = pose_inc_(data, intrinsics_.left_radius_, intrinsics_.right_radius_,
-// intrinsics_.separation_, 1);
-
-// _capture_ptr->setData(data);
-
-// Eigen::Matrix2s data_cov;
-// data_cov << 0.00001, 0, 0, 0.00001; // Todo
-
-// computeDataCov(data, data_cov);
-
-// _capture_ptr->setDataCovariance(data_cov);
-// }
-
-// /// @todo tofix
-// return false;//SensorBase::addCapture(_capture_ptr);
-//}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("DIFF DRIVE", SensorDiffDrive)
-} // namespace wolf
diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp
index 41c5fb00058dacf4172dc1ed8ee741a1dde50768..f1ddf33abe02f9d732a62e6182f7dcec8d1b1dbc 100644
--- a/src/sensor/sensor_laser_2D.cpp
+++ b/src/sensor/sensor_laser_2D.cpp
@@ -50,7 +50,7 @@ void SensorLaser2D::setDefaultScanParams()
scan_params_.range_min_ = 0.2;
scan_params_.range_max_ = 100;
scan_params_.range_std_dev_ = 0.01;
-
+
setNoiseStd(Eigen::VectorXs::Constant(1,scan_params_.range_std_dev_));
}
@@ -79,6 +79,23 @@ SensorBasePtr SensorLaser2D::create(const std::string& _unique_name, const Eigen
return sen;
}
+SensorBasePtr SensorLaser2D::createAutoConf(const std::string& _unique_name, const paramsServer& _server)
+{
+ // decode extrinsics vector
+ Eigen::VectorXs _extrinsics_po = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pos", "[0,0,0]");
+ assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
+ StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
+ StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true);
+
+ SensorLaser2DPtr sen;
+ IntrinsicsLaser2D params;
+
+ sen = std::make_shared<SensorLaser2D>(pos_ptr, ori_ptr, params.scan_params);
+
+ sen->setName(_unique_name);
+ return sen;
+}
+
} // namespace wolf
// Register in the SensorFactory and the ParameterFactory
@@ -88,3 +105,7 @@ namespace wolf {
WOLF_REGISTER_SENSOR("LASER 2D", SensorLaser2D)
//const bool registered_laser_params = IntrinsicsFactory::get().registerCreator("LASER 2D", createIntrinsicsLaser2D);
} // namespace wolf
+#include "core/sensor/autoconf_sensor_factory.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR_AUTO("LASER 2D", SensorLaser2D)
+} // namespace wolf
diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp
deleted file mode 100644
index e3e5d860f2960f8775a292c371b345d51d2dd70f..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_odom_2D.cpp
+++ /dev/null
@@ -1,67 +0,0 @@
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_angle.h"
-
-namespace wolf {
-
-SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics) :
- SensorBase("ODOM 2D", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
- k_disp_to_disp_(_intrinsics.k_disp_to_disp),
- k_rot_to_rot_(_intrinsics.k_rot_to_rot)
-{
- assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2D.");
- //
-}
-
-SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics) :
- SensorOdom2D(_extrinsics, *_intrinsics)
-{
- //
-}
-
-SensorOdom2D::~SensorOdom2D()
-{
- //
-}
-
-Scalar SensorOdom2D::getDispVarToDispNoiseFactor() const
-{
- return k_disp_to_disp_;
-}
-
-Scalar SensorOdom2D::getRotVarToRotNoiseFactor() const
-{
- return k_rot_to_rot_;
-}
-
-// Define the factory method
-SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
-
- SensorOdom2DPtr odo;
- if (_intrinsics)
- {
- std::shared_ptr<IntrinsicsOdom2D> params = std::static_pointer_cast<IntrinsicsOdom2D>(_intrinsics);
- odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params);
- }
- else
- {
- IntrinsicsOdom2D params;
- params.k_disp_to_disp = 1;
- params.k_rot_to_rot = 1;
- odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params);
- }
- odo->setName(_unique_name);
- return odo;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("ODOM 2D", SensorOdom2D)
-} // namespace wolf
diff --git a/src/sensor/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3D.cpp
deleted file mode 100644
index cf6431489efb45c0e0bf4599899bc9c850ebf902..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_odom_3D.cpp
+++ /dev/null
@@ -1,63 +0,0 @@
-/**
- * \file sensor_odom_3D.cpp
- *
- * Created on: Oct 7, 2016
- * \author: jsola
- */
-
-#include "core/sensor/sensor_odom_3D.h"
-
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-
-namespace wolf {
-
-SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsOdom3D& _intrinsics) :
- SensorBase("ODOM 3D", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
- k_disp_to_disp_(_intrinsics.k_disp_to_disp),
- k_disp_to_rot_(_intrinsics.k_disp_to_rot),
- k_rot_to_rot_(_intrinsics.k_rot_to_rot),
- min_disp_var_(_intrinsics.min_disp_var),
- min_rot_var_(_intrinsics.min_rot_var)
-{
- assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3D.");
-
- noise_cov_ = (Eigen::Vector6s() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal();
- setNoiseCov(noise_cov_); // sets also noise_std_
-}
-
-SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom3DPtr _intrinsics) :
- SensorOdom3D(_extrinsics_pq, *_intrinsics)
-{
- //
-}
-
-SensorOdom3D::~SensorOdom3D()
-{
- //
-}
-
-// Define the factory method
-SensorBasePtr SensorOdom3D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
-
- // cast intrinsics into derived type
- IntrinsicsOdom3DPtr params = std::static_pointer_cast<IntrinsicsOdom3D>(_intrinsics);
-
- // Call constructor and finish
- SensorBasePtr odo = std::make_shared<SensorOdom3D>(_extrinsics_pq, params);
- odo->setName(_unique_name);
-
- return odo;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("ODOM 3D", SensorOdom3D)
-}