diff --git a/src/problem.cpp b/src/problem.cpp
index f80a99b4dcb9bcabdf2b896c96a531dfdc03ee7c..be130ffa9f1ab4189d6814af96ef53e8ed5a62b9 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -4,17 +4,14 @@
#include "trajectory_base.h"
#include "map_base.h"
#include "sensor_base.h"
-#include "factory.h"
+#include "processor_motion.h"
+#include "processor_tracker.h"
+#include "capture_pose.h"
+#include "constraint_base.h"
#include "sensor_factory.h"
#include "processor_factory.h"
-#include "constraint_base.h"
#include "state_block.h"
-#include "processor_motion.h"
-#include "sensor_GPS.h"
-#include "processor_tracker.h"
-//#include "processors/processor_tracker_feature_trifocal.h"
-#include "capture_pose.h"
// IRI libs includes
diff --git a/src/sensor_GPS_fix.cpp b/src/sensor_GPS_fix.cpp
index fa1f46eb16bbeef20789cfdaf7165433910e21d9..311ec92924f34554cce36398b05eea2e1d680fa2 100644
--- a/src/sensor_GPS_fix.cpp
+++ b/src/sensor_GPS_fix.cpp
@@ -4,12 +4,6 @@
namespace wolf {
-SensorGPSFix::SensorGPSFix(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _noise) :
- SensorBase("GPS FIX", _p_ptr, _o_ptr, nullptr, Eigen::VectorXs::Constant(1,_noise))
-{
- //
-}
-
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)
{
@@ -17,7 +11,8 @@ SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, const Intrinsics
&& "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(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr) :
+ SensorGPSFix(_extrinsics, *_intrinsics_ptr)
{
//
}
@@ -34,8 +29,8 @@ SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen:
{
assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
&& "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics, true);
- SensorGPSFixPtr sen = std::make_shared<SensorGPSFix>(pos_ptr, nullptr, 0);
+ SensorGPSFixPtr sen = std::make_shared<SensorGPSFix>(_extrinsics, std::static_pointer_cast<IntrinsicsGPSFix>(_intrinsics));
+ sen->getPPtr()->fix();
sen->setName(_unique_name);
return sen;
}
diff --git a/src/sensor_GPS_fix.h b/src/sensor_GPS_fix.h
index c319d159f0adce4734f832a24c40ba361c8ba64b..25f3f052af7bf0254985ee56904455f4bcb22dba 100644
--- a/src/sensor_GPS_fix.h
+++ b/src/sensor_GPS_fix.h
@@ -25,15 +25,6 @@ WOLF_PTR_TYPEDEFS(SensorGPSFix);
class SensorGPSFix : public SensorBase
{
public:
- /** \brief Constructor with arguments
- *
- * Constructor with arguments
- * \param _p_ptr StateBlock pointer to the sensor position
- * \param _o_ptr StateBlock pointer to the sensor orientation
- * \param _noise noise standard deviation
- *
- **/
- 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);
diff --git a/src/sensor_IMU.cpp b/src/sensor_IMU.cpp
index 9e5da5f0ddba954357e7c58492a82947423ff5af..0189ca2d2e8d4dd84aab43ec656a9f4e95a29b36 100644
--- a/src/sensor_IMU.cpp
+++ b/src/sensor_IMU.cpp
@@ -4,24 +4,6 @@
namespace wolf {
-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)
{
diff --git a/src/sensor_IMU.h b/src/sensor_IMU.h
index 2f5d9baa576f697772800359a46a239170f39144..e809d79fffc923f731a4e477e49dfe292665cb9b 100644
--- a/src/sensor_IMU.h
+++ b/src/sensor_IMU.h
@@ -46,17 +46,6 @@ class SensorIMU : public SensorBase
public:
- /** \brief Constructor with arguments
- *
- * Constructor with arguments
- * \param _p_ptr StateBlock pointer to the sensor position wrt vehicle base
- * \param _o_ptr StateBlock pointer to the sensor orientation wrt vehicle base
- * \param params IntrinsicsIMU pointer to sensor properties
- * \param _a_w_biases_ptr StateBlock pointer to the vector of acc and gyro biases
- *
- **/
- 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);
diff --git a/src/sensor_camera.cpp b/src/sensor_camera.cpp
index ec41c09faf89385b3c43f4ea90f528194775e486..403ba8dd86676848c1a3ce3148782ccd773f5197 100644
--- a/src/sensor_camera.cpp
+++ b/src/sensor_camera.cpp
@@ -7,15 +7,6 @@
namespace wolf
{
-SensorCamera::SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, //
- int _img_width, int _img_height) :
- SensorBase("CAMERA", _p_ptr, _o_ptr, _intr_ptr, 2), //
- img_width_(_img_width), img_height_(_img_height),
- using_raw_(true)
-{
- useRawImages();
-}
-
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_raw, true), 1),
img_width_(_intrinsics.width), //
@@ -75,7 +66,6 @@ SensorBasePtr SensorCamera::create(const std::string& _unique_name, //
// Register in the SensorFactory
#include "sensor_factory.h"
-//#include "factory.h"
namespace wolf
{
WOLF_REGISTER_SENSOR("CAMERA", SensorCamera)
diff --git a/src/sensor_camera.h b/src/sensor_camera.h
index 6e5e422ed7e82bed5df6de8c1a1da5786e170dbe..9bdee32534c2d9b752bc6a988c6d4d0b76a0f7f9 100644
--- a/src/sensor_camera.h
+++ b/src/sensor_camera.h
@@ -28,17 +28,6 @@ WOLF_PTR_TYPEDEFS(SensorCamera);
class SensorCamera : public SensorBase
{
public:
- /** \brief Constructor with arguments
- *
- * Constructor with arguments
- * \param _p_ptr StateBlock pointer to the sensor position wrt vehicle base
- * \param _o_ptr StateBlock pointer to the sensor orientation wrt vehicle base
- * \param _intr_ptr contains intrinsic values for the camera
- * \param _img_width image height in pixels
- * \param _img_height image width in pixels
- *
- **/
- SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, int _img_width, int _img_height);
SensorCamera(const Eigen::VectorXs & _extrinsics, const IntrinsicsCamera& _intrinsics);
SensorCamera(const Eigen::VectorXs & _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr);
diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp
index fdb7ab4804809072cada12774e8af9a08ed9c3f4..7a03676337872f829653d6cd56f7daaf0e5446fd 100644
--- a/src/sensor_odom_2D.cpp
+++ b/src/sensor_odom_2D.cpp
@@ -4,14 +4,6 @@
namespace wolf {
-SensorOdom2D::SensorOdom2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _disp_noise_factor, const Scalar& _rot_noise_factor) :
- SensorBase("ODOM 2D", _p_ptr, _o_ptr, nullptr, 2),
- k_disp_to_disp_(_disp_noise_factor),
- k_rot_to_rot_(_rot_noise_factor)
-{
- //
-}
-
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),
diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h
index 4ba0fc37dee5a0d4e487548c894f32fff4fc2a86..f66b9eea4b643f65eca471ef82906e1d0c1ad4e4 100644
--- a/src/sensor_odom_2D.h
+++ b/src/sensor_odom_2D.h
@@ -26,16 +26,6 @@ class SensorOdom2D : public SensorBase
Scalar k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
public:
- /** \brief Constructor with arguments
- *
- * Constructor with arguments
- * \param _p_ptr StateBlock pointer to the sensor position wrt vehicle base
- * \param _o_ptr StateBlock pointer to the sensor orientation wrt vehicle base
- * \param _disp_noise_factor displacement noise factor
- * \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(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics);
SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics);
diff --git a/src/sensor_odom_3D.cpp b/src/sensor_odom_3D.cpp
index c5f3fb388c1ab12635dd8bd0d64bbc506fab3207..5cf426b294ad2241a0cace34ede0b14b8e1080fd 100644
--- a/src/sensor_odom_3D.cpp
+++ b/src/sensor_odom_3D.cpp
@@ -12,18 +12,6 @@
namespace wolf {
-SensorOdom3D::SensorOdom3D(StateBlockPtr _p_ptr, StateQuaternionPtr _o_ptr, IntrinsicsOdom3DPtr _params) :
- SensorBase("ODOM 3D", _p_ptr, _o_ptr, nullptr, 6),
- k_disp_to_disp_(_params->k_disp_to_disp),
- k_disp_to_rot_(_params->k_disp_to_rot),
- k_rot_to_rot_(_params->k_rot_to_rot),
- min_disp_var_(_params->min_disp_var),
- min_rot_var_(_params->min_rot_var)
-{
- 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, 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),
diff --git a/src/sensor_odom_3D.h b/src/sensor_odom_3D.h
index 19f037162677691f08b2deff390c42b49ab706fd..b366b99884997fe3cd73a132f435e53086b131db 100644
--- a/src/sensor_odom_3D.h
+++ b/src/sensor_odom_3D.h
@@ -38,14 +38,6 @@ class SensorOdom3D : public SensorBase
Scalar min_rot_var_;
public:
- /** \brief Constructor with arguments
- *
- * Constructor with arguments
- * \param _p_ptr StateBlock pointer to the sensor position wrt vehicle base
- * \param _o_ptr StateBlock pointer to the sensor orientation wrt vehicle base
- * \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);
diff --git a/src/test/gtest_constraint_autodiff.cpp b/src/test/gtest_constraint_autodiff.cpp
index c5820537f54d0aaf8e251cc4867a88b82f1d0e33..37754df0a2dfcc492195653699ea058949d16c8d 100644
--- a/src/test/gtest_constraint_autodiff.cpp
+++ b/src/test/gtest_constraint_autodiff.cpp
@@ -24,7 +24,10 @@ TEST(CaptureAutodiff, ConstructorOdom2d)
FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
// SENSOR
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1);
+ IntrinsicsOdom2D intrinsics_odo;
+ intrinsics_odo.k_disp_to_disp = 0.1;
+ intrinsics_odo.k_rot_to_rot = 0.1;
+ SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
// CAPTURE
CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
@@ -56,7 +59,10 @@ TEST(CaptureAutodiff, ResidualOdom2d)
FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
// SENSOR
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1);
+ IntrinsicsOdom2D intrinsics_odo;
+ intrinsics_odo.k_disp_to_disp = 0.1;
+ intrinsics_odo.k_rot_to_rot = 0.1;
+ SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
// CAPTURE
CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
@@ -100,7 +106,10 @@ TEST(CaptureAutodiff, JacobianOdom2d)
FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
// SENSOR
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1);
+ IntrinsicsOdom2D intrinsics_odo;
+ intrinsics_odo.k_disp_to_disp = 0.1;
+ intrinsics_odo.k_rot_to_rot = 0.1;
+ SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
// CAPTURE
CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
@@ -179,7 +188,10 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
// SENSOR
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1);
+ IntrinsicsOdom2D intrinsics_odo;
+ intrinsics_odo.k_disp_to_disp = 0.1;
+ intrinsics_odo.k_rot_to_rot = 0.1;
+ SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
// CAPTURE
CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
diff --git a/src/test/gtest_frame_base.cpp b/src/test/gtest_frame_base.cpp
index ae6b78068133529dcf9045572bba5f9715b9a62c..92068e3920ee3c887dd6fd979db131e03e0f4015 100644
--- a/src/test/gtest_frame_base.cpp
+++ b/src/test/gtest_frame_base.cpp
@@ -55,7 +55,8 @@ TEST(FrameBase, LinksBasic)
ASSERT_FALSE(F->getProblem());
// ASSERT_THROW(f->getPreviousFrame(), std::runtime_error); // protected by assert()
// ASSERT_EQ(f->getStatus(), ST_ESTIMATED); // protected
- ASSERT_FALSE(F->getCaptureOf(make_shared<SensorOdom2D>(nullptr, nullptr, 1,1)));
+ SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), IntrinsicsOdom2D());
+ ASSERT_FALSE(F->getCaptureOf(S));
ASSERT_TRUE(F->getCaptureList().empty());
ASSERT_TRUE(F->getConstrainedByList().empty());
ASSERT_EQ(F->getHits() , (unsigned int) 0);
@@ -67,7 +68,10 @@ TEST(FrameBase, LinksToTree)
// Problem with 2 frames and one motion constraint between them
ProblemPtr P = Problem::create("PO 2D");
TrajectoryBasePtr T = P->getTrajectoryPtr();
- SensorOdom2DPtr S = make_shared<SensorOdom2D>(make_shared<StateBlock>(2), make_shared<StateBlock>(1), 1,1);
+ IntrinsicsOdom2D intrinsics_odo;
+ intrinsics_odo.k_disp_to_disp = 1;
+ intrinsics_odo.k_rot_to_rot = 1;
+ SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
P->getHardwarePtr()->addSensor(S);
FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
T->addFrame(F1);