diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp index 9c3132b417318ab856095f90dd14a91186507064..c986ebf7cf9cd7fb83df1417ea19c42beed66c74 100644 --- a/src/processor/processor_odom_2d.cpp +++ b/src/processor/processor_odom_2d.cpp @@ -1,4 +1,5 @@ #include "core/processor/processor_odom_2d.h" +#include "core/sensor/sensor_odom_2d.h" #include "core/math/covariance.h" namespace wolf @@ -15,12 +16,12 @@ ProcessorOdom2d::~ProcessorOdom2d() { } -void ProcessorOdom2D::configure(SensorBasePtr _sensor) +void ProcessorOdom2d::configure(SensorBasePtr _sensor) { - auto sensor_ = std::dynamic_pointer_cast<SensorOdom2D>(_sensor); - assert(sensor_ != nullptr && "Sensor is not of type SensorOdom2D"); + auto sensor_ = std::dynamic_pointer_cast<SensorOdom2d>(_sensor); + assert(sensor_ != nullptr && "Sensor is not of type SensorOdom2d"); } -void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov, +void ProcessorOdom2d::computeCurrentDelta(const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov, const Eigen::VectorXd& _calib, const double _dt, Eigen::VectorXd& _delta, Eigen::MatrixXd& _delta_cov, Eigen::MatrixXd& _jacobian_calib) const { diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 1f1fa759fabc9bb3e0bf5faeb645fef08de8188c..40e354ddfef4e98746fc7d3bd5c6e0ca983a3d93 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -288,7 +288,8 @@ TEST(Problem, Covariances) Eigen::Vector3d xs2d; SensorBasePtr Sm = P->installSensor ("SensorOdom3d", "odometer", xs3d, wolf_root + "/test/yaml/sensor_odom_3d.yaml"); - SensorBasePtr St = P->installSensor ("SensorOdom2d", "other odometer", xs2d, wolf_root + "/test/yaml/sensor_odom_2d.yaml"); + // SensorBasePtr St = P->installSensor ("SensorOdom2d", "other odometer", xs2d, wolf_root + "/test/yaml/sensor_odom_2d.yaml"); + SensorBasePtr St = P->installSensor ("SensorOdom3d", "other odometer", xs3d, wolf_root + "/test/yaml/sensor_odom_3d.yaml"); ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>(); params->time_tolerance = 0.1; diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index b4c4d5d590b5ae8eee6342428b54898e1941a1d4..1933da8599234e61c534e9f05008710ac4396661 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -56,10 +56,10 @@ TEST(ProcessorBase, IsMotion) auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk); // Install odometer (sensor and processor) - SensorBasePtr sens_odo = problem->installSensor("SensorOdom2D", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"); - ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>(); + SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml"); + ProcessorParamsOdom2dPtr proc_odo_params = make_shared<ProcessorParamsOdom2d>(); proc_odo_params->time_tolerance = dt/2; - ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2D", "odom processor", sens_odo, proc_odo_params); + ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d", "odom processor", sens_odo, proc_odo_params); ASSERT_FALSE(proc_trk->isMotion()); ASSERT_TRUE (proc_odo->isMotion());