Skip to content
Snippets Groups Projects
Commit 4e13cf99 authored by Pep Martí Saumell's avatar Pep Martí Saumell
Browse files

deleted unnecesary files

parent 7156f8f0
No related branches found
No related tags found
3 merge requests!30Release after RAL,!29After 2nd RAL submission,!2Resolve "autoconf: add create function"
Hide whitespace changes
Inline Side-by-side
Showing
with 1 addition and 846 deletions
src/sensor/CMakeLists.txt deleted 100644 → 0
+ 0
− 27
View file @ 7156f8f0
INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
#=========================================
#=========================================
# Add in this section the CONDITIONAL CLUES [IF/ELSE]
# for external libraries and move inside them the respective lines from above.
#=========================================
#=========================================
SET(HDRS_SENSOR ${HDRS_SENSOR}
${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.h
${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.hpp
${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.h
)
SET(SRCS_SENSOR ${SRCS_SENSOR}
${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.cpp
)
# Forward var to parent scope
# These lines always at the end
SET(HDRS_SENSOR ${HDRS_SENSOR} PARENT_SCOPE)
SET(SRCS_SENSOR ${SRCS_SENSOR} PARENT_SCOPE)
\ No newline at end of file
src/sensor/sensor_GPS.cpp deleted 100644 → 0
+ 0
− 56
View file @ 7156f8f0
#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
src/sensor/sensor_GPS_fix.cpp deleted 100644 → 0
+ 0
− 43
View file @ 7156f8f0
#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
src/sensor/sensor_IMU.cpp deleted 100644 → 0
+ 0
− 49
View file @ 7156f8f0
#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
src/sensor/sensor_base.cpp deleted 100644 → 0
+ 0
− 407
View file @ 7156f8f0
#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
src/sensor/sensor_diff_drive.cpp deleted 100644 → 0
+ 0
− 133
View file @ 7156f8f0
#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
src/sensor/sensor_laser_2D.cpp
+ 1
− 1
View file @ 4e13cf99
...@@ -50,7 +50,7 @@ void SensorLaser2D::setDefaultScanParams() ...@@ -50,7 +50,7 @@ void SensorLaser2D::setDefaultScanParams()
scan_params_.range_min_ = 0.2; scan_params_.range_min_ = 0.2;
scan_params_.range_max_ = 100; scan_params_.range_max_ = 100;
scan_params_.range_std_dev_ = 0.01; scan_params_.range_std_dev_ = 0.01;
setNoiseStd(Eigen::VectorXs::Constant(1,scan_params_.range_std_dev_)); setNoiseStd(Eigen::VectorXs::Constant(1,scan_params_.range_std_dev_));
} }
......
src/sensor/sensor_odom_2D.cpp deleted 100644 → 0
+ 0
− 67
View file @ 7156f8f0
#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
src/sensor/sensor_odom_3D.cpp deleted 100644 → 0
+ 0
− 63
View file @ 7156f8f0
/**
* \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)
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment