diff --git a/src/sensor_GPS_fix.cpp b/src/sensor_GPS_fix.cpp
index 956e3fb687352c20d609617e0089b6f42e299be7..b97e43c49a8ff74befcd77024e54a37100e64c21 100644
--- a/src/sensor_GPS_fix.cpp
+++ b/src/sensor_GPS_fix.cpp
@@ -10,6 +10,18 @@ SensorGPSFix::SensorGPSFix(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const dou
//
}
+SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics) :
+ SensorBase("GPS FIX", std::make_shared<StateBlock>(_extrinsics.head(3)), std::make_shared<StateQuaternion>(_extrinsics.tail(4)), nullptr, _intrinsics.noise_std)
+{
+ assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D.");
+}
+
+SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr) : SensorGPSFix(_extrinsics, *_intrinsics_ptr)
+{
+ //
+}
+
+
SensorGPSFix::~SensorGPSFix()
{
//
diff --git a/src/sensor_GPS_fix.h b/src/sensor_GPS_fix.h
index b2423e71ac6760716a2d311316d4b8583ee5d413..3291aa538a7f7cd7a8fa838df8190a4b70ab4acb 100644
--- a/src/sensor_GPS_fix.h
+++ b/src/sensor_GPS_fix.h
@@ -12,8 +12,10 @@
namespace wolf {
+WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsGPSFix);
struct IntrinsicsGPSFix : public IntrinsicsBase
{
+ Eigen::Vector3s noise_std;
// Empty -- it acts only as a typedef for IntrinsicsBase, but allows future extension with parameters
virtual ~IntrinsicsGPSFix() = default;
};
@@ -32,6 +34,8 @@ class SensorGPSFix : public SensorBase
*
**/
SensorGPSFix(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _noise);
+ SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics);
+ SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr);
virtual ~SensorGPSFix();
diff --git a/src/sensor_IMU.cpp b/src/sensor_IMU.cpp
index 6e973746eafa75f0642e646e6d1edec9e33a0a49..ed8046ce84a2be818fc989136f621244f3525ed2 100644
--- a/src/sensor_IMU.cpp
+++ b/src/sensor_IMU.cpp
@@ -4,18 +4,42 @@
namespace wolf {
-SensorIMU::SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsIMUPtr params) :
- SensorBase("IMU", _p_ptr, _o_ptr, 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)
+SensorIMU::SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsIMU& _params) :
+ SensorBase("IMU", _p_ptr, _o_ptr, 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)
{
//
}
+SensorIMU::SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsIMUPtr _params) :
+ SensorIMU(_p_ptr, _o_ptr, *_params)
+{
+ //
+}
+
+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()
{
@@ -29,11 +53,8 @@ SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::Ve
// decode extrinsics vector
assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_pq.head(3), true);
- StateBlockPtr ori_ptr = std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true);
-
IntrinsicsIMUPtr params = std::static_pointer_cast<IntrinsicsIMU>(_intrinsics);
- SensorIMUPtr sen = std::make_shared<SensorIMU>(pos_ptr, ori_ptr, params);
+ SensorIMUPtr sen = std::make_shared<SensorIMU>(_extrinsics_pq, params);
sen->setName(_unique_name);
return sen;
}
diff --git a/src/sensor_IMU.h b/src/sensor_IMU.h
index a9509188fc2f091f4c31fc26576a6087eed38b02..0b34016e29025f9ddc43a07582924988095e01a1 100644
--- a/src/sensor_IMU.h
+++ b/src/sensor_IMU.h
@@ -56,6 +56,9 @@ class SensorIMU : public SensorBase
*
**/
SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsIMUPtr _params);
+ SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsIMU& _params);
+ SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params);
+ SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params);
Scalar getGyroNoise() const;
Scalar getAccelNoise() const;
diff --git a/src/sensor_camera.cpp b/src/sensor_camera.cpp
index 36e6c9ad53f3d19c37560eeb9a756398acbab736..7a9a9a2c7904b0ea47143c96a4d0bdd1b36c8c50 100644
--- a/src/sensor_camera.cpp
+++ b/src/sensor_camera.cpp
@@ -15,18 +15,23 @@ SensorCamera::SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBloc
//
}
-SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const std::shared_ptr<IntrinsicsCamera> _intrinsics_ptr) :
- SensorBase("CAMERA", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(_intrinsics_ptr->pinhole_model, true), 1),
- img_width_(_intrinsics_ptr->width), //
- img_height_(_intrinsics_ptr->height), //
- distortion_(_intrinsics_ptr->distortion), //
+SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const IntrinsicsCamera& _intrinsics) :
+ SensorBase("CAMERA", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(_intrinsics.pinhole_model, true), 1),
+ img_width_(_intrinsics.width), //
+ img_height_(_intrinsics.height), //
+ distortion_(_intrinsics.distortion), //
correction_(distortion_.size()+2) // make correction vector of the same size as distortion vector
{
assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3D");
- K_ = setIntrinsicMatrix(_intrinsics_ptr->pinhole_model);
+ K_ = setIntrinsicMatrix(_intrinsics.pinhole_model);
pinhole::computeCorrectionModel(getIntrinsicPtr()->getState(), distortion_, correction_);
}
+SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr) :
+ SensorCamera(_extrinsics, *_intrinsics_ptr)
+{
+ //
+}
SensorCamera::~SensorCamera()
{
@@ -58,13 +63,6 @@ SensorBasePtr SensorCamera::create(const std::string& _unique_name, //
SensorCameraPtr sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr);
sen_ptr->setName(_unique_name);
-// std::cout << "Created camera:\n\tintrinsics : " << sen_ptr->getIntrinsicPtr()->getState().transpose() << std::endl;
-// std::cout << "\tintrinsic matrix : " << K_ << std::endl;
-// std::cout << "\tdistortion : " << distortion_.transpose() << std::endl;
-// std::cout << "\tcorrection : " << correction_.transpose() << std::endl;
-// std::cout << "\tposition : " << sen_ptr->getPPtr()->getState().transpose() << std::endl;
-// std::cout << "\torientation : " << sen_ptr->getOPtr()->getState().transpose() << std::endl;
-
return sen_ptr;
}
diff --git a/src/sensor_camera.h b/src/sensor_camera.h
index 4b3d565da7d8c0095d18cefe3b97789f195190e8..8b5ec2035452d2b9ec21ec6056d00654731466f3 100644
--- a/src/sensor_camera.h
+++ b/src/sensor_camera.h
@@ -7,6 +7,7 @@
namespace wolf
{
+WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsCamera);
/** Struct of intrinsic camera parameters
*/
struct IntrinsicsCamera : public IntrinsicsBase
@@ -38,7 +39,8 @@ class SensorCamera : public SensorBase
**/
SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, int _img_width, int _img_height);
- SensorCamera(const Eigen::VectorXs & _extrinsics, const std::shared_ptr<IntrinsicsCamera> _intrinsics_ptr);
+ SensorCamera(const Eigen::VectorXs & _extrinsics, const IntrinsicsCamera& _intrinsics);
+ SensorCamera(const Eigen::VectorXs & _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr);
virtual ~SensorCamera();
diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp
index 738dc3e6b069b80fb86e3f8915c9f3d249659e05..66239d0d0e65d159f2fd3c12dc4e0bad5319ed5d 100644
--- a/src/sensor_laser_2D.cpp
+++ b/src/sensor_laser_2D.cpp
@@ -23,6 +23,19 @@ SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const l
//
}
+SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params) :
+ SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8),
+ scan_params_(_params.scan_params)
+{
+ //
+}
+
+SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params) :
+ SensorLaser2D(_p_ptr, _o_ptr, *_params)
+{
+ //
+}
+
SensorLaser2D::~SensorLaser2D()
{
//
diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h
index 8c2aa85762d14ac5c50dbccee840de46a1418531..9cdcf5567499c2993d67f53535f0e1d34ed2682d 100644
--- a/src/sensor_laser_2D.h
+++ b/src/sensor_laser_2D.h
@@ -54,6 +54,8 @@ class SensorLaser2D : public SensorBase
*
**/
SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const laserscanutils::LaserScanParams& _params);
+ SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params);
+ SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params);
virtual ~SensorLaser2D();
diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp
index d720c4e66f9df27ee82e67129ea26ba64f1b6954..fdb7ab4804809072cada12774e8af9a08ed9c3f4 100644
--- a/src/sensor_odom_2D.cpp
+++ b/src/sensor_odom_2D.cpp
@@ -12,6 +12,22 @@ SensorOdom2D::SensorOdom2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Sca
//
}
+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()
{
//
@@ -33,17 +49,20 @@ SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen:
{
// decode extrinsics vector
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<StateAngle>(_extrinsics_po(2), true);
- // cast intrinsics into derived type
+
SensorOdom2DPtr odo;
if (_intrinsics)
{
std::shared_ptr<IntrinsicsOdom2D> params = std::static_pointer_cast<IntrinsicsOdom2D>(_intrinsics);
- odo = std::make_shared<SensorOdom2D>(pos_ptr, ori_ptr, params->k_disp_to_disp, params->k_rot_to_rot);
+ odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params);
}
else
- odo = std::make_shared<SensorOdom2D>(pos_ptr, ori_ptr, 1, 1);
+ {
+ 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;
}
diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h
index b906bce9b5d4e72749c3d318aed2ee2d21aa1b89..4ba0fc37dee5a0d4e487548c894f32fff4fc2a86 100644
--- a/src/sensor_odom_2D.h
+++ b/src/sensor_odom_2D.h
@@ -35,7 +35,9 @@ class SensorOdom2D : public SensorBase
* \param _rot_noise_factor rotation noise factor
*
**/
- SensorOdom2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _disp_noise_factor, const Scalar& _rot_noise_factor);
+ SensorOdom2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _disp_noise_factor, const Scalar& _rot_noise_factor);
+ SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics);
+ SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics);
virtual ~SensorOdom2D();
diff --git a/src/sensor_odom_3D.cpp b/src/sensor_odom_3D.cpp
index 21acafbbb484f81cbedf94376e370e6504fa9b5b..36213d9e69b39d1e09c42804b84d3ed4eb060d2e 100644
--- a/src/sensor_odom_3D.cpp
+++ b/src/sensor_odom_3D.cpp
@@ -24,23 +24,46 @@ SensorOdom3D::SensorOdom3D(StateBlockPtr _p_ptr, StateQuaternionPtr _o_ptr, Intr
setNoiseCov(noise_cov_); // sets also noise_std_
}
+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_po,
+SensorBasePtr SensorOdom3D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq,
const IntrinsicsBasePtr _intrinsics)
{
// decode extrinsics vector
- assert(_extrinsics_po.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(3), true);
- StateQuaternionPtr ori_ptr = std::make_shared<StateQuaternion>(_extrinsics_po.tail(4), true);
+ 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);
- SensorBasePtr odo = std::make_shared<SensorOdom3D>(pos_ptr, ori_ptr, params);
+
+ // Call constructor and finish
+ SensorBasePtr odo = std::make_shared<SensorOdom3D>(_extrinsics_pq, params);
odo->setName(_unique_name);
+
return odo;
}
diff --git a/src/sensor_odom_3D.h b/src/sensor_odom_3D.h
index 9ffef8d23eae9cfaa4fdf3d439636985350ab0f6..80575937c8eef5cbab637531f1c508371b8e29e3 100644
--- a/src/sensor_odom_3D.h
+++ b/src/sensor_odom_3D.h
@@ -47,6 +47,8 @@ class SensorOdom3D : public SensorBase
* \param _params shared_ptr to a struct with parameters
**/
SensorOdom3D(StateBlockPtr _p_ptr, StateQuaternionPtr _q_ptr, IntrinsicsOdom3DPtr params);
+ SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsOdom3D& params);
+ SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom3DPtr params);
virtual ~SensorOdom3D();