diff --git a/CMakeLists.txt b/CMakeLists.txt
index ee6fdaf753eaf33d6ade4ad0b9c50cdfb6fb7cca..6cb77a6347addf6697a652e7022148ae852a5518 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -177,47 +177,47 @@ SET(HDRS_PROBLEM
include/${PROJECT_NAME}/problem/problem.h
)
SET(HDRS_PROCESSOR
- include/core/processor/factory_processor.h
- include/core/processor/motion_buffer.h
- include/core/processor/motion_provider.h
- include/core/processor/processor_base.h
- # include/core/processor/processor_diff_drive.h
- # include/core/processor/processor_fixed_wing_model.h
- # include/core/processor/processor_loop_closure.h
- include/core/processor/processor_motion.h
- include/core/processor/processor_odom_2d.h
- # include/core/processor/processor_odom_3d.h
- include/core/processor/processor_pose.h
- include/core/processor/processor_tracker.h
- include/core/processor/processor_tracker_feature.h
- include/core/processor/processor_tracker_landmark.h
- include/core/processor/track_matrix.h
+ include/${PROJECT_NAME}/processor/factory_processor.h
+ include/${PROJECT_NAME}/processor/motion_buffer.h
+ include/${PROJECT_NAME}/processor/motion_provider.h
+ include/${PROJECT_NAME}/processor/processor_base.h
+ include/${PROJECT_NAME}/processor/processor_diff_drive.h
+ # include/${PROJECT_NAME}/processor/processor_fixed_wing_model.h
+ # include/${PROJECT_NAME}/processor/processor_loop_closure.h
+ include/${PROJECT_NAME}/processor/processor_motion.h
+ include/${PROJECT_NAME}/processor/processor_odom_2d.h
+ include/${PROJECT_NAME}/processor/processor_odom_3d.h
+ include/${PROJECT_NAME}/processor/processor_pose.h
+ include/${PROJECT_NAME}/processor/processor_tracker.h
+ include/${PROJECT_NAME}/processor/processor_tracker_feature.h
+ include/${PROJECT_NAME}/processor/processor_tracker_landmark.h
+ include/${PROJECT_NAME}/processor/track_matrix.h
)
SET(HDRS_SENSOR
- include/core/sensor/factory_sensor.h
- include/core/sensor/sensor_base.h
- # include/core/sensor/sensor_diff_drive.h
- # include/core/sensor/sensor_motion_model.h
- include/core/sensor/sensor_odom.h
- include/core/sensor/sensor_pose.h
+ include/${PROJECT_NAME}/sensor/factory_sensor.h
+ include/${PROJECT_NAME}/sensor/sensor_base.h
+ include/${PROJECT_NAME}/sensor/sensor_diff_drive.h
+ # include/${PROJECT_NAME}/sensor/sensor_motion_model.h
+ include/${PROJECT_NAME}/sensor/sensor_odom.h
+ include/${PROJECT_NAME}/sensor/sensor_pose.h
)
SET(HDRS_SOLVER
include/${PROJECT_NAME}/solver/solver_manager.h
include/${PROJECT_NAME}/solver/factory_solver.h
)
SET(HDRS_STATE_BLOCK
- include/core/state_block/factory_state_block.h
- include/core/state_block/has_state_blocks.h
- include/core/state_block/local_parametrization_angle.h
- include/core/state_block/local_parametrization_base.h
- include/core/state_block/local_parametrization_homogeneous.h
- include/core/state_block/local_parametrization_quaternion.h
- include/core/state_block/prior.h
- include/core/state_block/state_angle.h
- include/core/state_block/state_block.h
- include/core/state_block/state_composite.h
- include/core/state_block/state_homogeneous_3d.h
- include/core/state_block/state_quaternion.h
+ include/${PROJECT_NAME}/state_block/factory_state_block.h
+ include/${PROJECT_NAME}/state_block/has_state_blocks.h
+ include/${PROJECT_NAME}/state_block/local_parametrization_angle.h
+ include/${PROJECT_NAME}/state_block/local_parametrization_base.h
+ include/${PROJECT_NAME}/state_block/local_parametrization_homogeneous.h
+ include/${PROJECT_NAME}/state_block/local_parametrization_quaternion.h
+ include/${PROJECT_NAME}/state_block/prior.h
+ include/${PROJECT_NAME}/state_block/state_angle.h
+ include/${PROJECT_NAME}/state_block/state_block.h
+ include/${PROJECT_NAME}/state_block/state_composite.h
+ include/${PROJECT_NAME}/state_block/state_homogeneous_3d.h
+ include/${PROJECT_NAME}/state_block/state_quaternion.h
)
SET(HDRS_TRAJECTORY
include/${PROJECT_NAME}/trajectory/trajectory_base.h
@@ -289,7 +289,7 @@ SET(SRCS_PROCESSOR
src/processor/motion_buffer.cpp
src/processor/motion_provider.cpp
src/processor/processor_base.cpp
- # src/processor/processor_diff_drive.cpp
+ src/processor/processor_diff_drive.cpp
# src/processor/processor_fixed_wing_model.cpp
# src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp
@@ -303,7 +303,7 @@ SET(SRCS_PROCESSOR
)
SET(SRCS_SENSOR
src/sensor/sensor_base.cpp
- # src/sensor/sensor_diff_drive.cpp
+ src/sensor/sensor_diff_drive.cpp
# src/sensor/sensor_motion_model.cpp
src/sensor/sensor_odom.cpp
src/sensor/sensor_pose.cpp
@@ -336,7 +336,7 @@ SET(SRCS_UTILS
)
SET(SRCS_YAML
src/yaml/parser_yaml.cpp
- # src/yaml/processor_odom_3d_yaml.cpp
+ src/yaml/processor_odom_3d_yaml.cpp
# src/yaml/sensor_odom_2d_yaml.cpp
# src/yaml/sensor_odom_3d_yaml.cpp
# src/yaml/sensor_pose_yaml.cpp
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index f435b2569b2c64659d3a87633a29cef67dd0d78a..0a42e1275ef5e96f505837774d0c75842c6e81cd 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -329,7 +329,13 @@ class Factory
template<class TypeBase, typename... TypeInput>
inline Factory<TypeBase, TypeInput...>::Factory()
{
- //std::cout << " Factory destructor " << this->getClass() << std::endl;
+ //std::cout << " Factory constructor " << this->getClass() << std::endl;
+}
+
+template<class TypeBase, typename... TypeInput>
+inline Factory<TypeBase, TypeInput...>::~Factory()
+{
+ //std::cout << " Factory destructor " << this->getClass() << std::endl;}
}
template<class TypeBase, typename... TypeInput>
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 52eab3b6b239922b5afbfe08e84361acd2c0e98e..b5c4cf7ce81994831e7e8fe09d9e83affb29f4cc 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -157,7 +157,6 @@ public:
*/
bool empty() const;
-protected:
/**\brief Check time tolerance
*
* Check if the time distance between two time stamps is smaller than
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 6550a00aa5f72b7648919eae1d07e3dbfb21aa9f..8c56d51e599aa79251baf881d2822cd0b365592b 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -113,6 +113,25 @@ struct ParamsSensorBase: public ParamsBase
}
};
+struct SpecState
+{
+ std::string type;
+ SizeEigen size;
+ std::string doc;
+
+ SpecState(const std::string& _type, SizeEigen _size, const std::string _doc) :
+ type(_type), size(_size), doc(_doc)
+ {
+ assert(not(_type == "StateQuaternion" and _size != 4));
+ assert(not(_type == "StateAngle" and _size != 1));
+ assert(not(_type == "O" and _size != 1 and _size != 4));
+ assert(not(_type == "P" and _size != 1 and _size != 4));
+ assert(not(_type == "V" and _size != 1 and _size != 4));
+ };
+};
+
+typedef std::unordered_map<char,SpecState> SpecStates;
+
WOLF_PTR_TYPEDEFS(SensorBase);
class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<SensorBase>
{
@@ -139,7 +158,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
std::map<char, Eigen::MatrixXd> drift_covs_; // covariance of the drift of dynamic state blocks [unit²/s]
void setProblem(ProblemPtr _problem) override final;
- virtual void loadPriors(const Priors& _priors, SizeEigen _dim);
+ virtual void loadPriors(const Priors& _priors, SizeEigen _dim, const SpecStates& _state_specs);
public:
@@ -151,7 +170,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
* \param _dim Problem dimension
* \param _params params struct
* \param _priors priors of the sensor state blocks
- * \param _keys_types_apart_from_PO Map containing the keys and the types of the state blocks (apart from extrinsics: P, O)
+ * \param _state_specs_apart_from_PO Unordered map containing the keys and the specs for each of the state blocks (apart from extrinsics: P, O)
*
**/
SensorBase(const std::string& _type,
@@ -159,7 +178,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
const SizeEigen& _dim,
ParamsSensorBasePtr _params,
const Priors& _priors,
- const std::unordered_map<char, std::string>& _keys_types_apart_from_PO={});
+ SpecStates _state_specs_apart_from_PO={});
/** \brief Constructor with ParamServer and keys
*
@@ -170,7 +189,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
* \param _dim Problem dimension
* \param _params params struct
* \param _server parameter server
- * \param _keys_types_apart_from_PO Map containing the keys and the types of the state blocks (apart from extrinsics: P, O)
+ * \param _state_specs_apart_from_PO Unordered map containing the keys and the specs for each of the state blocks (apart from extrinsics: P, O)
*
**/
SensorBase(const std::string& _type,
@@ -178,7 +197,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
const SizeEigen& _dim,
ParamsSensorBasePtr _params,
const ParamsServer& _server,
- const std::unordered_map<char, std::string>& _keys_types_apart_from_PO={});
+ SpecStates _state_specs_apart_from_PO={});
~SensorBase() override;
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index 5d1f2bb0c3ef4e9068673ea1a855289993bdcebd..c122164275a29675a1f617363dc6b74ccc036b90 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -19,12 +19,6 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file sensor_diff_drive.h
- *
- * Created on: Jul 22, 2019
- * \author: jsola
- */
#ifndef SENSOR_SENSOR_DIFF_DRIVE_H_
#define SENSOR_SENSOR_DIFF_DRIVE_H_
@@ -38,36 +32,22 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDiffDrive);
struct ParamsSensorDiffDrive : public ParamsSensorBase
{
- double radius_left;
- double radius_right;
- double wheel_separation;
double ticks_per_wheel_revolution;
- bool set_intrinsics_prior;
- Eigen::Vector3d prior_cov_diag;
- double ticks_cov_factor;
+ double ticks_std_factor;
ParamsSensorDiffDrive() = default;
ParamsSensorDiffDrive(std::string _unique_name, const wolf::ParamsServer& _server)
: ParamsSensorBase(_unique_name, _server)
{
- radius_left = _server.getParam<double>(prefix + _unique_name + "/radius_left");
- radius_right = _server.getParam<double>(prefix + _unique_name + "/radius_right");
- wheel_separation = _server.getParam<double>(prefix + _unique_name + "/wheel_separation");
ticks_per_wheel_revolution = _server.getParam<double>(prefix + _unique_name + "/ticks_per_wheel_revolution");
- set_intrinsics_prior = _server.getParam<bool>(prefix + _unique_name + "/set_intrinsics_prior");
- prior_cov_diag = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/prior_cov_diag");
- ticks_cov_factor = _server.getParam<double>(prefix + _unique_name + "/ticks_cov_factor");
+ ticks_std_factor = _server.getParam<double>(prefix + _unique_name + "/ticks_std_factor");
}
+ ~ParamsSensorDiffDrive() = default;
std::string print() const override
{
return ParamsSensorBase::print() + "\n"
- + "radius_left: " + std::to_string(radius_left) + "\n"
- + "radius_right: " + std::to_string(radius_right) + "\n"
- + "wheel_separation: " + std::to_string(wheel_separation) + "\n"
- + "ticks_per_wheel_revolution: " + std::to_string(ticks_per_wheel_revolution)+ "\n"
- + "set_intrinsics_prior: " + std::to_string(set_intrinsics_prior) + "\n"
- + "prior_cov_diag: to_string not implemented yet! \n"
- + "ticks_cov_factor: " + std::to_string(ticks_cov_factor) + "\n";
+ + "ticks_per_wheel_revolution: " + toString(ticks_per_wheel_revolution) + "\n"
+ + "ticks_std_factor: " + toString(ticks_std_factor) + "\n";
}
};
@@ -76,28 +56,48 @@ WOLF_PTR_TYPEDEFS(SensorDiffDrive);
class SensorDiffDrive : public SensorBase
{
public:
- SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
- ParamsSensorDiffDrivePtr _intrinsics);
- WOLF_SENSOR_CREATE(SensorDiffDrive, ParamsSensorDiffDrive, 3);
+
+
+ SensorDiffDrive(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorDiffDrivePtr _params,
+ const Priors& _priors);
+
+ SensorDiffDrive(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorDiffDrivePtr _params,
+ const ParamsServer& _server);
+
+ WOLF_SENSOR_CREATE(SensorDiffDrive, ParamsSensorDiffDrive);
~SensorDiffDrive() override;
+
ParamsSensorDiffDriveConstPtr getParams() const;
+ ParamsSensorDiffDrivePtr getParams();
+
+ Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override;
- double getRadiansPerTick() const
- {
- return radians_per_tick;
- }
+ double getRadiansPerTick() const;
protected:
ParamsSensorDiffDrivePtr params_diff_drive_;
- double radians_per_tick; ///< Not user-definable -- DO NOT PRETEND TO USE YAML TO SET THIS PARAM.
};
-inline wolf::ParamsSensorDiffDriveConstPtr SensorDiffDrive::getParams() const
+inline ParamsSensorDiffDriveConstPtr SensorDiffDrive::getParams() const
+{
+ return params_diff_drive_;
+}
+
+inline ParamsSensorDiffDrivePtr SensorDiffDrive::getParams()
{
return params_diff_drive_;
}
+inline double SensorDiffDrive::getRadiansPerTick() const
+{
+ return 2.0 * M_PI / params_diff_drive_->ticks_per_wheel_revolution;
+}
+
} /* namespace wolf */
#endif /* SENSOR_SENSOR_DIFF_DRIVE_H_ */
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index db06d0078261369000aed77e8d8f4d290cd2a355..beb1c7a4bd4e8837329bb0ef74dbb651bbd01ebd 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -56,9 +56,10 @@ ProcessorDiffDrive::~ProcessorDiffDrive()
void ProcessorDiffDrive::configure(SensorBasePtr _sensor)
{
- assert(_sensor && "Provided sensor is nullptr");
-
- SensorDiffDriveConstPtr sensor_diff_drive = std::static_pointer_cast<SensorDiffDrive>(_sensor);
+ SensorDiffDriveConstPtr sensor_diff_drive = std::dynamic_pointer_cast<SensorDiffDrive>(_sensor);
+
+ if ( not _sensor)
+ throw std::runtime_error("ProcessorDiffDrive::configure Provided sensor is nullptr or not of type SensorDiffDrive");
radians_per_tick_ = sensor_diff_drive->getRadiansPerTick();
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 3f9b4901757503b36287962b06f570c38b822431..abd76885b291b4af5a3315f773da2ddbf028047b 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -33,12 +33,17 @@ namespace wolf {
unsigned int SensorBase::sensor_id_count_ = 0;
+SpecStates specs_po_2d({{'P',SpecState("StateBlock",2,"Sensor extrinsics in 2D: position (x, y) of the sensor in robot coordinates [m].")},
+ {'O',SpecState("StateAngle",1,"Sensor extrinsics in 2D: orientation (yaw) of the sensor in robot coordinates [rad].")}});
+SpecStates specs_po_3d({{'P',SpecState("StateBlock",3,"Sensor extrinsics in 2D: position (x, y, z) of the sensor in robot coordinates [m].")},
+ {'O',SpecState("StateQuaternion",4,"Sensor extrinsics in 2D: orientation (quaternion) of the sensor in robot coordinates.")}});
+
SensorBase::SensorBase(const std::string& _type,
const std::string& _unique_name,
const SizeEigen& _dim,
ParamsSensorBasePtr _params,
const Priors& _priors,
- const std::unordered_map<char, std::string>& _keys_types_apart_from_PO) :
+ SpecStates _state_specs) :
NodeBase("SENSOR", _type, _unique_name),
HasStateBlocks(""),
hardware_ptr_(),
@@ -49,28 +54,22 @@ SensorBase::SensorBase(const std::string& _type,
last_capture_(nullptr)
{
assert(_dim == 2 or _dim == 3);
- assert(_keys_types_apart_from_PO.count('P') == 0 and
- _keys_types_apart_from_PO.count('O') == 0 and
- "SensorBase: _keys_types_apart_from_PO should not contain 'P' or 'O' keys");
+ assert(_state_specs.count('P') == 0 and
+ _state_specs.count('O') == 0 and
+ "SensorBase: '_state_specs' should not contain 'P' or 'O' keys");
// check params valid
if (not params_)
throw std::runtime_error("SensorBase: in constructor, ParamsSensorBase pointer is null");
- // check priors have all _keys_types_apart_from_PO (PO will be checked in loadPriors())
- for (auto key_type : _keys_types_apart_from_PO)
- {
- if (_priors.count(key_type.first) == 0)
- throw std::runtime_error("SensorBase: In constructor, provided '_priors' does not contain key " + std::string(1,key_type.first));
-
- if (_priors.at(key_type.first).getType() != key_type.second)
- throw std::runtime_error("SensorBase: In constructor, provided '_priors' for key '" +
- std::string(1,key_type.first) + "' has wrong type: " +
- _priors.at(key_type.first).getType() +
- " and should be: " + key_type.second);
- }
+ // append P, O to _state_specs
+ if (_dim == 2)
+ _state_specs.insert(specs_po_2d.begin(),specs_po_2d.end());
+ else
+ _state_specs.insert(specs_po_3d.begin(),specs_po_3d.end());
+
// load priors
- loadPriors(_priors, _dim);
+ loadPriors(_priors, _dim, _state_specs);
}
SensorBase::SensorBase(const std::string& _type,
@@ -78,7 +77,7 @@ SensorBase::SensorBase(const std::string& _type,
const SizeEigen& _dim,
ParamsSensorBasePtr _params,
const ParamsServer& _server,
- const std::unordered_map<char, std::string>& _keys_types_apart_from_PO) :
+ SpecStates _state_specs) :
NodeBase("SENSOR", _type, _unique_name),
HasStateBlocks(""),
hardware_ptr_(),
@@ -89,24 +88,27 @@ SensorBase::SensorBase(const std::string& _type,
last_capture_(nullptr)
{
assert(_dim == 2 or _dim == 3);
- assert(_keys_types_apart_from_PO.count('P') == 0 and
- _keys_types_apart_from_PO.count('O') == 0 and
- "SensorBase: _keys_types_apart_from_PO should not contain 'P' or 'O' keys");
+ assert(_state_specs.count('P') == 0 and
+ _state_specs.count('O') == 0 and
+ "SensorBase: _state_specs should not contain 'P' or 'O' keys");
// check params valid
if (not params_)
throw std::runtime_error("SensorBase: in constructor, ParamsSensorBase pointer is null");
+ // append P, O to _state_specs
+ if (_dim == 2)
+ _state_specs.insert(specs_po_2d.begin(),specs_po_2d.end());
+ else
+ _state_specs.insert(specs_po_3d.begin(),specs_po_3d.end());
+
// create priors
- std::unordered_map<char, std::string> keys_types = _keys_types_apart_from_PO;
- keys_types['P'] = "P"; // equivalent to "StateBlock";
- keys_types['O'] = "O"; // equivalent to (_dim == 2 ? "StateAngle" : "StateQuaternion");
Priors priors;
- for (auto pair : keys_types)
- priors[pair.first] = Prior("sensor/" + _unique_name + "/states/" + std::string(1, pair.first), pair.second, _server);
+ for (auto pair : _state_specs)
+ priors[pair.first] = Prior("sensor/" + _unique_name + "/states/" + std::string(1, pair.first), pair.second.type, _server);
// load priors
- loadPriors(priors, _dim);
+ loadPriors(priors, _dim, _state_specs);
}
SensorBase::~SensorBase()
@@ -115,39 +117,78 @@ SensorBase::~SensorBase()
removeStateBlocks(getProblem());
}
-void SensorBase::loadPriors(const Priors& _priors, SizeEigen _dim)
+void SensorBase::loadPriors(const Priors& _priors,
+ SizeEigen _dim,
+ const SpecStates& _state_specs)
{
assert(_dim == 2 or _dim == 3);
+ // check not specified priors
for (auto&& prior_pair : _priors)
{
- auto key = prior_pair.first;
- const Prior& prior = prior_pair.second;
-
- // Check consistency of keys
- if (key == 'P' and prior.getType() != "P" and prior.getType() != "StateBlock")
- throw std::runtime_error("Prior type for key P has to be 'P' or 'StateBlock'");
- if (key == 'V' and prior.getType() != "V" and prior.getType() != "StateBlock")
- throw std::runtime_error("Prior type for key V has to be 'V' or 'StateBlock'");
- if (key == 'O' and _dim == 2 and prior.getType() != "O" and prior.getType() != "StateAngle")
- throw std::runtime_error("Prior type for key O in 2D has to be 'O' or 'StateAngle'");
- if (key == 'O' and _dim == 3 and prior.getType() != "O" and prior.getType() != "StateQuaternion")
- throw std::runtime_error("Prior type for key O in 3D has to be 'O' or 'StateQuaternion'");
-
- // Checks state size (P, O, V)
+ if (_state_specs.count(prior_pair.first) == 0)
+ throw std::runtime_error("SensorBase::loadPriors: Given a prior for key " + std::string(1,prior_pair.first) + " that is not required");
+ }
+
+ // Iterate all keys in _state_specs
+ for (auto state_spec_pair : _state_specs)
+ {
+ auto key = state_spec_pair.first;
+ auto state_spec = state_spec_pair.second;
+
+ // --- CHECKS ---
+ // Existence
+ if (_priors.count(key) == 0)
+ throw std::runtime_error("SensorBase: No prior found for key '"
+ + std::string(1,key) +
+ "' - should have type: " + state_spec.type +
+ "\nDoc:\n" + state_spec.doc);
+
+ const Prior& prior = _priors.at(key);
+
+ // type
+ if (key != 'P' and key != 'O' and key != 'V')
+ {
+ if (prior.getType() != state_spec.type)
+ throw std::runtime_error("SensorBase: The provided prior for key '" +
+ std::string(1,key) + "' has wrong type: " +
+ prior.getType() +
+ " and should be: " + state_spec.type +
+ "\nDoc:\n" + state_spec.doc);
+ }
+ else
+ {
+ if (key == 'P' and prior.getType() != "P" and prior.getType() != "StateBlock")
+ throw std::runtime_error("Prior type for key P has to be 'P' or 'StateBlock'");
+ if (key == 'V' and prior.getType() != "V" and prior.getType() != "StateBlock")
+ throw std::runtime_error("Prior type for key V has to be 'V' or 'StateBlock'");
+ if (key == 'O' and _dim == 2 and prior.getType() != "O" and prior.getType() != "StateAngle")
+ throw std::runtime_error("Prior type for key O in 2D has to be 'O' or 'StateAngle'");
+ if (key == 'O' and _dim == 3 and prior.getType() != "O" and prior.getType() != "StateQuaternion")
+ throw std::runtime_error("Prior type for key O in 3D has to be 'O' or 'StateQuaternion'");
+ }
+
+ // size
+ if (_priors.at(key).getState().size() != state_spec.size)
+ throw std::runtime_error("SensorBase: In constructor, provided '_priors' for key '" +
+ std::string(1,key) + "' has wrong size: " +
+ toString(prior.getState()) +
+ " and should be of size: " + toString(state_spec.size) +
+ "\nDoc:\n" + state_spec.doc);
+ // additional size checks for P, O, V
if ((key == 'P' or key == 'V') and prior.getState().size() != _dim)
throw std::runtime_error("Prior state for P or V has wrong size: " + toString(prior.getState().size()) + " in " + (_dim == 2 ? "2D" : "3D"));
if (key == '0' and prior.getState().size() != (_dim == 2 ? 1 : 4))
throw std::runtime_error("Prior state for O has wrong size: " + toString(prior.getState().size()) + " in " + (_dim == 2 ? "2D" : "3D"));
- // create state block
+ // --- CREATE STATE BLOCK ---
auto sb = FactoryStateBlock::create(prior.getType(), prior.getState(), prior.isFixed());
// check local param
- if (not sb->isValid()) // always false I think...
+ if (not sb->isValid()) // always false I think, the stateblock constructor would have failed...
throw std::runtime_error("The created stateblock " + prior.getType() + " for key " + std::string(1,key) + " is not valid (local param violation)");
- // Add state block
+ // --- ADD STATE BLOCK ---
addStateBlock(key, sb, prior.isDynamic());
// Factor
@@ -158,9 +199,6 @@ void SensorBase::loadPriors(const Priors& _priors, SizeEigen _dim)
if (prior.isDynamic())
setDriftStd(key, prior.getDriftStd());
}
-
- if (not hasStateBlock('P') or not hasStateBlock('O'))
- throw std::runtime_error("SensorBase prior should have at least 'P' and 'O' keys defined");
}
void SensorBase::fixExtrinsics()
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 4620caea4e169aa3ef64d233e6883fdea53bed02..4ed4d49589bc92b8a1e5976b8217df0796adb621 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -32,35 +32,54 @@
namespace wolf
{
-SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
- ParamsSensorDiffDrivePtr _intrinsics) :
- SensorBase("SensorDiffDrive",
- std::make_shared<StateBlock>(_extrinsics.head(2), true),
- std::make_shared<StateAngle>(_extrinsics(2), true),
- std::make_shared<StateBlock>(3, false), 2),
- params_diff_drive_(_intrinsics)
+SpecStates specs_states_diff_drive({{'I',SpecState("StateBlock",
+ 3,
+ "0: left wheel radius (m), 1: right wheel radius (m), 2: wheel separation (m)")}});
+
+SensorDiffDrive::SensorDiffDrive(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorDiffDrivePtr _params,
+ const Priors& _priors) :
+ SensorBase("SensorDiffDrive",
+ _unique_name,
+ _dim,
+ _params,
+ _priors,
+ specs_states_diff_drive),
+ params_diff_drive_(_params)
{
- radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
- getIntrinsic()->setState(Eigen::Vector3d(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
- unfixIntrinsics();
+ assert(_dim == 2 and "SensorDiffDrive only defined for 2D problems");
- if(params_diff_drive_->set_intrinsics_prior)
- addPriorIntrinsics(getIntrinsic()->getState(), params_diff_drive_->prior_cov_diag.asDiagonal());
+ if (this->getStateBlockDynamic('I')->getState().size() != 3)
+ throw std::runtime_error("SensorDiffDrive in constructor, provided state for 'I' of wrong size. Is should be 3");
+}
- // compute noise covariance
- // 1. measured wheel revolutions: sigma = 2*radians_per_tick
- double sigma_rev = 2*radians_per_tick;
- Eigen::Vector2d noise_sigma; noise_sigma << sigma_rev, sigma_rev;
+SensorDiffDrive::SensorDiffDrive(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorDiffDrivePtr _params,
+ const ParamsServer& _server) :
+ SensorBase("SensorDiffDrive",
+ _unique_name,
+ _dim,
+ _params,
+ _server,
+ specs_states_diff_drive),
+ params_diff_drive_(_params)
+{
+ assert(_dim == 2 and "SensorDiffDrive only defined for 2D problems");
- setNoiseStd(noise_sigma);
-
+ if (this->getStateBlockDynamic('I')->getState().size() != 3)
+ throw std::runtime_error("SensorDiffDrive in constructor, provided state for 'I' of wrong size. It should be 3");
}
SensorDiffDrive::~SensorDiffDrive()
{
- // Auto-generated destructor stub
}
+Eigen::MatrixXd SensorDiffDrive::computeNoiseCov(const Eigen::VectorXd & _data) const
+{
+ return (params_diff_drive_->ticks_std_factor * _data).cwiseAbs2().asDiagonal();
+}
} /* namespace wolf */
@@ -68,6 +87,5 @@ SensorDiffDrive::~SensorDiffDrive()
#include "core/sensor/factory_sensor.h"
namespace wolf {
WOLF_REGISTER_SENSOR(SensorDiffDrive);
-WOLF_REGISTER_SENSOR_AUTO(SensorDiffDrive);
} // namespace wolf
diff --git a/src/sensor/sensor_odom.cpp b/src/sensor/sensor_odom.cpp
index c78ae346e835e8588554953e4938885525957565..4d4a1ed6a44a9fa6af1c8bfcd08b5f69e08c452e 100644
--- a/src/sensor/sensor_odom.cpp
+++ b/src/sensor/sensor_odom.cpp
@@ -88,8 +88,8 @@ Eigen::MatrixXd SensorOdom::computeNoiseCov(const Eigen::VectorXd & _data) const
}
// variances
- double dvar = params_odom_->min_disp_var + params_odom_->k_disp_to_disp * d;
- double rvar = params_odom_->min_rot_var + params_odom_->k_disp_to_rot * d + params_odom_->k_rot_to_rot * r;
+ double dvar = std::max(params_odom_->min_disp_var, params_odom_->k_disp_to_disp * d);
+ double rvar = std::max(params_odom_->min_rot_var, params_odom_->k_disp_to_rot * d + params_odom_->k_rot_to_rot * r);
// return
if (dim_ == 2)
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 5dcc60de67b16bc5c280648979dafc438fae542a..c5693f17b43bd94e5b8aea0f3bc1563122c710f4 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -87,7 +87,6 @@ wolf_add_gtest(gtest_param_server gtest_param_server.cpp)
# Parameters server
wolf_add_gtest(gtest_prior gtest_prior.cpp)
-target_link_libraries(gtest_prior ${PLUGIN_NAME})
# YAML parser
wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
@@ -101,7 +100,7 @@ wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
target_link_libraries(gtest_processor_base PUBLIC dummy)
# ProcessorMotion class test
-wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
+# wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
# Rotation test
wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
@@ -154,7 +153,7 @@ wolf_add_gtest(gtest_factor_autodiff_distance_3d gtest_factor_autodiff_distance_
wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
# FactorDiffDrive class test
-wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
+# wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
# FactorOdom2dAutodiff class test
wolf_add_gtest(gtest_factor_odom_2d_autodiff gtest_factor_odom_2d_autodiff.cpp)
@@ -169,13 +168,13 @@ wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
# FactorRelativePose2dWithExtrinsics class test
-wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
+# wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
# FactorRelativePose3d class test
wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
# FactorRelativePose3dWithExtrinsics class test
-wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
+# wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
# FactorVelocityLocalDirection3d class test
wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
@@ -187,16 +186,16 @@ wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
# Parameter prior test
-wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
+# wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
# ProcessorFixedWingModel class test
-wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
+# wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
# ProcessorDiffDrive class test
-wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
+# wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
# ProcessorLoopClosure class test
-wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
+# wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
# ProcessorFrameNearestNeighborFilter class test
# wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2d gtest_processor_frame_nearest_neighbor_filter_2d.cpp)
@@ -208,7 +207,7 @@ wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
# FactorPose3dWithExtrinsics class test
-wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
+# wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
# ProcessorTrackerFeatureDummy class test
wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
@@ -223,28 +222,23 @@ wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
# SensorOdom class test
wolf_add_gtest(gtest_sensor_odom gtest_sensor_odom.cpp)
-target_link_libraries(gtest_sensor_odom ${PLUGIN_NAME})
-
-# # SensorOdom2d class test
-# wolf_add_gtest(gtest_sensor_odom_2d gtest_sensor_odom_2d.cpp)
-# target_link_libraries(gtest_sensor_odom_2d ${PLUGIN_NAME})
# SensorPose class test
wolf_add_gtest(gtest_sensor_pose gtest_sensor_pose.cpp)
IF (Ceres_FOUND)
# SolverCeres test
- wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp)
+ # wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp)
# SolverCeresMultithread test
- wolf_add_gtest(gtest_solver_ceres_multithread gtest_solver_ceres_multithread.cpp)
+ # wolf_add_gtest(gtest_solver_ceres_multithread gtest_solver_ceres_multithread.cpp)
ENDIF(Ceres_FOUND)
# TreeManagerSlidingWindow class test
-wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp)
+# wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp)
# TreeManagerSlidingWindowDualRate class test
-wolf_add_gtest(gtest_tree_manager_sliding_window_dual_rate gtest_tree_manager_sliding_window_dual_rate.cpp)
+# wolf_add_gtest(gtest_tree_manager_sliding_window_dual_rate gtest_tree_manager_sliding_window_dual_rate.cpp)
# yaml conversions
-wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
+# wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
diff --git a/test/dummy/sensor_dummy_poia.h b/test/dummy/sensor_dummy_poia.h
index 849231de5b010b324a733ae91481cb7e59f4b6e1..c311e9cea73ac4faac86ccaac0d77976d072faea 100644
--- a/test/dummy/sensor_dummy_poia.h
+++ b/test/dummy/sensor_dummy_poia.h
@@ -30,6 +30,9 @@
namespace wolf {
+SpecStates specs_states_dummy_poia({{'I',SpecState("StateBlock", 5, "some doc for state I")},
+ {'A',SpecState("StateQuaternion", 4, "some doc for state A")}});
+
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDummyPoia);
struct ParamsSensorDummyPoia : public ParamsSensorDummy
@@ -64,7 +67,7 @@ class SensorDummyPoia : public SensorBase
_dim,
_params,
_server,
- std::unordered_map<char, std::string>{{'I',"StateBlock"},{'A',"StateQuaternion"}}),
+ specs_states_dummy_poia),
params_dummy_poia_(_params)
{
}
@@ -80,7 +83,7 @@ class SensorDummyPoia : public SensorBase
_dim,
_params,
_priors,
- std::unordered_map<char, std::string>{{'I',"StateBlock"},{'A',"StateQuaternion"}}),
+ specs_states_dummy_poia),
params_dummy_poia_(_params)
{
}
diff --git a/test/gtest_buffer_frame.cpp b/test/gtest_buffer_frame.cpp
index ebc17ca1312c50ddea210985cd9fa71c6e43113a..484cd006e556f8f70c3d825f259e1f000cb20432 100644
--- a/test/gtest_buffer_frame.cpp
+++ b/test/gtest_buffer_frame.cpp
@@ -19,21 +19,11 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/*
- * gtest_buffer_frame.cpp
- *
- * Created on: Mar 5, 2018
- * Author: jsola
- */
-//Wolf
-#include "core/utils/utils_gtest.h"
-#include "core/processor/processor_odom_2d.h"
-#include "core/sensor/sensor_odom_2d.h"
+#include "core/utils/utils_gtest.h"
-#include "dummy/processor_tracker_feature_dummy.h"
+#include "core/processor/processor_base.h"
#include "core/capture/capture_void.h"
-
#include "core/problem/problem.h"
// STL
@@ -89,24 +79,16 @@ TEST_F(BufferFrameTest, clear)
ASSERT_TRUE(buffer_kf.empty());
}
-//TEST_F(BufferFrameTest, print)
-//{
-// kfpackbuffer.clear();
-// kfpackbuffer.emplace(f10, tt10);
-// kfpackbuffer.emplace(f20, tt20);
-// kfpackbuffer.print();
-//}
-
-//TEST_F(BufferFrameTest, doubleCheckTimeTolerance)
-//{
-// buffer_kf.clear();
-// buffer_kf.emplace(10, f10);
-// buffer_kf.emplace(20, f20);
-// // min time tolerance > diff between time stamps. It should return true
-// ASSERT_TRUE(buffer_kf.doubleCheckTimeTolerance(10, 20, 20, 20));
-// // min time tolerance < diff between time stamps. It should return true
-// ASSERT_FALSE(buffer_kf.doubleCheckTimeTolerance(10, 1, 20, 20));
-//}
+TEST_F(BufferFrameTest, doubleCheckTimeTolerance)
+{
+ buffer_kf.clear();
+ buffer_kf.emplace(10, f10);
+ buffer_kf.emplace(20, f20);
+ // min time tolerance > diff between time stamps. It should return true
+ ASSERT_TRUE(buffer_kf.doubleCheckTimeTolerance(TimeStamp(10), 20, TimeStamp(20), 20));
+ // min time tolerance < diff between time stamps. It should return true
+ ASSERT_FALSE(buffer_kf.doubleCheckTimeTolerance(TimeStamp(10), 1, TimeStamp(20), 20));
+}
//TEST_F(BufferFrameTest, select)
//{
@@ -167,67 +149,67 @@ TEST_F(BufferFrameTest, clear)
// }
//}
-//TEST_F(BufferFrameTest, selectFirstBefore)
-//{
-// buffer_kf.clear();
-//
-// buffer_kf.emplace(10, f10);
-// buffer_kf.emplace(20, f20);
-// buffer_kf.emplace(21, f21);
-//
-// // input time stamps
-// std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5};
-// double tt = 0.01;
-//
-// // Solution matrix
-// // q_ts | pack
-// //=================
-// // first time
-// //-----------------
-// // 9.5 nullptr
-// // 9.995 10
-// // 10,005 10
-// // 19.5 10
-// // 20.5 10
-// // 21.5 10
-// // second time
-// //-----------------
-// // 9.5 nullptr
-// // 9.995 null
-// // 10,005 null
-// // 19.5 null
-// // 20.5 20
-// // 21.5 20
-// // third time
-// //-----------------
-// // 9.5 nullptr
-// // 9.995 null
-// // 10,005 null
-// // 19.5 null
-// // 20.5 null
-// // 21.5 21
-//
-// Eigen::MatrixXd truth(3,6), res(3,6);
-// truth << 0,10,10,10,10,10, 0,0,0,0,20,20, 0,0,0,0,0,21;
-// res.setZero();
-//
-// for (int i=0; i<3; i++)
-// {
-// PackKeyFramePtr packQ;
-// int j = 0;
-// for (auto ts : q_ts)
-// {
-// packQ = buffer_kf.selectFirstPackBefore(ts, tt);
-// if (packQ)
-// res(i,j) = packQ->key_frame->getTimeStamp().get();
-//
-// j++;
-// }
-// buffer_kf.removeUpTo(packQ->key_frame->getTimeStamp());
-// }
-//
-// ASSERT_MATRIX_APPROX(res, truth, 1e-6);
-//}
+TEST_F(BufferFrameTest, selectFirstBefore)
+{
+ buffer_kf.clear();
+
+ buffer_kf.emplace(10, f10);
+ buffer_kf.emplace(20, f20);
+ buffer_kf.emplace(21, f21);
+
+ // input time stamps
+ std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5};
+ double tt = 0.01;
+
+ // Solution matrix
+ // q_ts | pack
+ //=================
+ // first time
+ //-----------------
+ // 9.5 nullptr
+ // 9.995 10
+ // 10,005 10
+ // 19.5 10
+ // 20.5 10
+ // 21.5 10
+ // second time
+ //-----------------
+ // 9.5 nullptr
+ // 9.995 null
+ // 10,005 null
+ // 19.5 null
+ // 20.5 20
+ // 21.5 20
+ // third time
+ //-----------------
+ // 9.5 nullptr
+ // 9.995 null
+ // 10,005 null
+ // 19.5 null
+ // 20.5 null
+ // 21.5 21
+
+ Eigen::MatrixXd truth(3,6), res(3,6);
+ truth << 0,10,10,10,10,10, 0,0,0,0,20,20, 0,0,0,0,0,21;
+ res.setZero();
+
+ for (int i=0; i<3; i++)
+ {
+ FrameBasePtr packQ;
+ int j = 0;
+ for (auto ts : q_ts)
+ {
+ packQ = buffer_kf.selectFirstBefore(ts, tt);
+ if (packQ)
+ res(i,j) = packQ->getTimeStamp().get();
+
+ j++;
+ }
+ buffer_kf.removeUpTo(packQ->getTimeStamp());
+ }
+
+ ASSERT_MATRIX_APPROX(res, truth, 1e-6);
+}
TEST_F(BufferFrameTest, removeUpTo)
{
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index aa36078108a936109484bd4a8d761c258105d236..893236d5a42e0f45f53f390bbfd79577eabe3ee9 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -29,8 +29,11 @@
#include "core/utils/utils_gtest.h"
#include "core/capture/capture_base.h"
+#include "core/sensor/factory_sensor.h"
#include "core/state_block/state_angle.h"
+std::string wolf_root = _WOLF_ROOT_DIR;
+
using namespace wolf;
using namespace Eigen;
@@ -46,48 +49,33 @@ TEST(CaptureBase, ConstructorNoSensor)
TEST(CaptureBase, ConstructorWithSensor)
{
- SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2));
+ SensorBasePtr S = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
ASSERT_EQ(C->getTimeStamp(), 1.5);
ASSERT_FALSE(C->getFrame());
ASSERT_FALSE(C->getProblem());
ASSERT_TRUE(C->getSensor());
- ASSERT_FALSE(C->getSensorP());
- ASSERT_FALSE(C->getSensorO());
}
TEST(CaptureBase, Static_sensor_params)
{
- StateBlockPtr p(std::make_shared<StateBlock>(2));
- StateBlockPtr o(std::make_shared<StateAngle>() );
- StateBlockPtr i(std::make_shared<StateBlock>(2));
- SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", p, o, i, 2));
+ SensorBasePtr S = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
- // query sensor blocks
- ASSERT_EQ(S->getP(), p);
- ASSERT_EQ(S->getO(), o);
- ASSERT_EQ(S->getIntrinsic(), i);
-
// query capture blocks
- ASSERT_EQ(C->getSensorP(), p);
- ASSERT_EQ(C->getSensorO(), o);
- ASSERT_EQ(C->getSensorIntrinsic(), i);
+ ASSERT_EQ(C->getSensorP(), S->getP());
+ ASSERT_EQ(C->getSensorO(), S->getO());
+ ASSERT_EQ(C->getSensorIntrinsic(), nullptr);
}
TEST(CaptureBase, Dynamic_sensor_params)
{
+ SensorBasePtr S = FactorySensorYaml::create("SensorDiffDrive", "sensor_1", 2, wolf_root + "/test/yaml/sensor_diff_drive_dynamic.yaml");
StateBlockPtr p(std::make_shared<StateBlock>(2));
StateBlockPtr o(std::make_shared<StateAngle>() );
- StateBlockPtr i(std::make_shared<StateBlock>(2));
- SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", std::make_shared<StateBlock>(2), std::make_shared<StateAngle>(), std::make_shared<StateBlock>(2), 2, true, true, true)); // last 3 'true' mark dynamic
+ StateBlockPtr i(std::make_shared<StateBlock>(3));
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5
- // query sensor blocks -- need KFs to find some Capture with the params
- // ASSERT_EQ(S->getP(), p);
- // ASSERT_EQ(S->getO(), o);
- // ASSERT_EQ(S->getIntrinsic(), i);
-
// query capture blocks
ASSERT_EQ(C->getSensorP(), p);
ASSERT_EQ(C->getSensorO(), o);
@@ -114,7 +102,7 @@ TEST(CaptureBase, print)
TEST(CaptureBase, process)
{
- SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2));
+ SensorBasePtr S = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, nullptr));
ASSERT_DEATH({C->process();},""); // No sensor in the capture should fail
C->setSensor(S);
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 0aa294035d9f712b706883636e46cd1e4ae62593..ef45a2d9d33f7fb35bb921f59e4e6b84a6b1cadb 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -31,8 +31,9 @@
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
-#include "core/sensor/sensor_odom_2d.h"
-#include "core/sensor/sensor_odom_3d.h"
+#include "core/sensor/sensor_odom.h"
+#include "dummy/sensor_dummy.h"
+#include "core/sensor/sensor_odom.h"
#include "core/processor/processor_odom_3d.h"
#include "core/processor/processor_odom_2d.h"
#include "core/feature/feature_odom_2d.h"
@@ -56,7 +57,13 @@ TEST(Emplace, Sensor)
{
ProblemPtr P = Problem::create("POV", 3);
- SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
+ auto sen = SensorBase::emplace<SensorDummy>(P->getHardware(), // SensorBase is abstract
+ "dummy_name",
+ 2,
+ std::make_shared<ParamsSensorDummy>(),
+ Priors({{'P',Prior("P",Vector2d::Zero())},
+ {'O',Prior("O",Vector1d::Zero())}}));
+
ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getHardware()->getProblem());
}
TEST(Emplace, Frame)
@@ -72,13 +79,23 @@ TEST(Emplace, Processor)
{
ProblemPtr P = Problem::create("PO", 2);
- auto sen = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
+ auto sen = SensorBase::emplace<SensorDummy>(P->getHardware(), // SensorBase is abstract
+ "dummy_name",
+ 2,
+ std::make_shared<ParamsSensorDummy>(),
+ Priors({{'P',Prior("P",Vector2d::Zero())},
+ {'O',Prior("O",Vector1d::Zero())}}));
auto prc = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen, std::make_shared<ParamsProcessorOdom2d>());
ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getProcessorList().front()->getSensor()->getProblem());
ASSERT_EQ(sen, sen->getProcessorList().front()->getSensor());
ASSERT_EQ(prc, sen->getProcessorList().front());
- SensorBasePtr sen2 = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
+ SensorBasePtr sen2 = SensorBase::emplace<SensorDummy>(P->getHardware(), // SensorBase is abstract
+ "dummy_name",
+ 2,
+ std::make_shared<ParamsSensorDummy>(),
+ Priors({{'P',Prior("P",Vector2d::Zero())},
+ {'O',Prior("O",Vector1d::Zero())}}));
ProcessorOdom2dPtr prc2 = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen2, std::make_shared<ParamsProcessorOdom2d>());
ASSERT_EQ(sen2, sen2->getProcessorList().front()->getSensor());
ASSERT_EQ(prc2, sen2->getProcessorList().front());
@@ -142,7 +159,12 @@ TEST(Emplace, EmplaceDerived)
ProblemPtr P = Problem::create("POV", 3);
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- auto sen = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Eigen::VectorXd(3), ParamsSensorOdom2d());
+ auto sen = SensorBase::emplace<SensorOdom>(P->getHardware(),
+ "dummy_name",
+ 2,
+ std::make_shared<ParamsSensorOdom>(),
+ Priors({{'P',Prior("P",Vector2d::Zero())},
+ {'O',Prior("O",Vector1d::Zero())}}));
auto cov = Eigen::MatrixXd::Identity(2,2);
auto cpt = CaptureBase::emplace<CaptureOdom2d>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
auto m = Eigen::Matrix<double,9,6>();
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 7a1a330916e5ba4a5e7c28f784bd6bfd21278da7..ce5058b1b6489c137588b086f3acdf7e4f9496c9 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -32,11 +32,12 @@
#include "core/factor/factor_relative_pose_2d.h"
#include "core/utils/utils_gtest.h"
-#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/factory_sensor.h"
#include "core/capture/capture_void.h"
#include "core/feature/feature_odom_2d.h"
#include "core/factor/factor_autodiff.h"
+std::string wolf_root = _WOLF_ROOT_DIR;
using namespace wolf;
using namespace Eigen;
@@ -379,10 +380,7 @@ TEST(FactorAutodiff, EmplaceOdom2d)
FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2));
// SENSOR
- ParamsSensorOdom2d 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>(Vector3d::Zero(), intrinsics_odo);
+ auto sensor_ptr = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
@@ -412,15 +410,11 @@ TEST(FactorAutodiff, ResidualOdom2d)
FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
// SENSOR
- ParamsSensorOdom2d 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>(Vector3d::Zero(), intrinsics_odo);
+ auto sensor_ptr = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
-
// FEATURE
Eigen::Vector3d d;
d << -1, -4, M_PI/2;
@@ -458,10 +452,7 @@ TEST(FactorAutodiff, JacobianOdom2d)
FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
// SENSOR
- ParamsSensorOdom2d 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>(Vector3d::Zero(), intrinsics_odo);
+ auto sensor_ptr = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
@@ -538,10 +529,7 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
// SENSOR
- ParamsSensorOdom2d 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>(Vector3d::Zero(), intrinsics_odo);
+ auto sensor_ptr = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index eb61f798d41b2aec792b864dffb39cb1cdb7d098..2b2da613e168a941d58b2de6fe2d4af0bdf7ea9e 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -30,7 +30,8 @@
#include "core/factor/factor_relative_pose_2d.h"
#include "core/frame/frame_base.h"
-#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/factory_sensor.h"
+#include "core/sensor/sensor_odom.h"
#include "core/processor/processor_odom_2d.h"
#include "core/capture/capture_motion.h"
@@ -40,6 +41,8 @@ using namespace Eigen;
using namespace std;
using namespace wolf;
+std::string wolf_root = _WOLF_ROOT_DIR;
+
TEST(FrameBase, GettersAndSetters)
{
FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
@@ -69,7 +72,7 @@ TEST(FrameBase, LinksBasic)
ASSERT_FALSE(F->getTrajectory());
ASSERT_FALSE(F->getProblem());
- SensorOdom2dPtr S = make_shared<SensorOdom2d>(Vector3d::Zero(), ParamsSensorOdom2d());
+ auto S = FactorySensorYaml::create("SensorOdom", "odometer", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
ASSERT_FALSE(F->getCaptureOf(S));
ASSERT_TRUE(F->getCaptureList().empty());
ASSERT_TRUE(F->getConstrainedByList().empty());
@@ -81,10 +84,7 @@ TEST(FrameBase, LinksToTree)
// Problem with 2 frames and one motion factor between them
ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
- ParamsSensorOdom2d intrinsics_odo;
- intrinsics_odo.k_disp_to_disp = 1;
- intrinsics_odo.k_rot_to_rot = 1;
- auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
+ auto S = P->installSensor("SensorOdom", "odometer", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
@@ -135,10 +135,7 @@ TEST(FrameBase, Frames)
// Problem with 10 frames
ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
- ParamsSensorOdom2d intrinsics_odo;
- intrinsics_odo.k_disp_to_disp = 1;
- intrinsics_odo.k_rot_to_rot = 1;
- auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
+ auto S = P->installSensor("SensorOdom", "odometer", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
auto F0 = FrameBase::emplace<FrameBase>(T, 0, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
diff --git a/test/gtest_motion_provider.cpp b/test/gtest_motion_provider.cpp
index 982bc05a33ee7e998c823f9759533aad259b0792..bcec7f0baa74374d77a21648bb1d2e1842bb0dd1 100644
--- a/test/gtest_motion_provider.cpp
+++ b/test/gtest_motion_provider.cpp
@@ -25,7 +25,7 @@
#include "core/processor/motion_provider.h"
#include "core/utils/utils_gtest.h"
-#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/sensor_odom.h"
#include "core/problem/problem.h"
@@ -54,9 +54,8 @@ class MotionProviderTest : public testing::Test
problem = Problem::create("POV", 2);
// Install odom sensor
- sen = problem->installSensor("SensorOdom2d",
+ sen = problem->installSensor("SensorOdom",
"odometer",
- Vector3d(0,0,0),
wolf_root + "/test/yaml/sensor_odom_2d.yaml");
// Install 3 odom processors
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 522246910cc6bd3944f6924a832fbf028cb2b75c..26d939c134adcbf5c8e739cf6595f23d4d6c8126 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -28,11 +28,10 @@
#include "core/utils/utils_gtest.h"
-
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
-#include "core/sensor/sensor_odom_2d.h"
-#include "core/sensor/sensor_odom_3d.h"
+#include "core/sensor/sensor_odom.h"
+#include "dummy/sensor_dummy.h"
#include "core/processor/processor_odom_3d.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/solver/solver_manager.h"
@@ -50,6 +49,7 @@
using namespace wolf;
using namespace Eigen;
+std::string wolf_root = _WOLF_ROOT_DIR;
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
@@ -72,8 +72,12 @@ TEST(Problem, Sensors)
ProblemPtr P = Problem::create("POV", 3);
// add a dummy sensor
- auto S = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
-
+ auto S = SensorBase::emplace<SensorDummy>(P->getHardware(),
+ "dummy_name",
+ 3,
+ std::make_shared<ParamsSensorDummy>(),
+ Priors({{'P',Prior("P",Vector3d::Zero())},
+ {'O',Prior("O",Vector4d::Random().normalized())}}));
// check pointers
ASSERT_EQ(P, S->getProblem());
ASSERT_EQ(P->getHardware(), S->getHardware());
@@ -88,7 +92,12 @@ TEST(Problem, Processor)
ASSERT_TRUE(P->getMotionProviderMap().empty());
// add a motion sensor and processor
- auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), ParamsSensorOdom3d());
+ auto Sm = SensorBase::emplace<SensorOdom>(P->getHardware(),
+ "odometer",
+ 3,
+ std::make_shared<ParamsSensorOdom>(),
+ Priors({{'P',Prior("P",Vector3d::Zero())},
+ {'O',Prior("O",Vector4d::Random().normalized())}}));
// add motion processor
auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ParamsProcessorOdom3d>());
@@ -103,7 +112,7 @@ TEST(Problem, Installers)
ProblemPtr P = Problem::create("PO", 3);
Eigen::Vector7d xs; xs.setRandom(); xs.tail(4).normalize();
- SensorBasePtr S = P->installSensor ("SensorOdom3d", "odometer", xs, wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+ SensorBasePtr S = P->installSensor ("SensorOdom", "odometer", wolf_root + "/test/yaml/sensor_odom_3d.yaml");
// install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer");
@@ -276,13 +285,9 @@ TEST(Problem, StateBlocks)
Eigen::Vector3d xs2d;
// 2 state blocks, fixed
- SensorBasePtr Sm = P->installSensor ("SensorOdom3d", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+ SensorBasePtr Sm = P->installSensor ("SensorOdom", "odometer", wolf_root + "/test/yaml/sensor_odom_3d.yaml");
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
-// // 2 state blocks, fixed
-// SensorBasePtr St = P->installSensor ("SensorOdom2d", "other odometer", xs2d, "");
-// ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2 + 2));
-
auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer");
auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
@@ -333,9 +338,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 ("SensorOdom3d", "other odometer", xs3d, wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+ SensorBasePtr Sm = P->installSensor ("SensorOdom", "odometer", wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+ SensorBasePtr St = P->installSensor ("SensorOdom", "other odometer", wolf_root + "/test/yaml/sensor_odom_3d.yaml");
ParamsProcessorTrackerFeaturePtr params = std::make_shared<ParamsProcessorTrackerFeature>();
params->time_tolerance = 0.1;
@@ -343,7 +347,7 @@ TEST(Problem, Covariances)
params->min_features_for_keyframe = 10;
auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", Sm, params);
- auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "other odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
+ auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "other odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
// 4 state blocks, estimated
St->unfixExtrinsics();
@@ -369,14 +373,14 @@ TEST(Problem, perturb)
auto problem = Problem::create("PO", 2);
// make a sensor first
- auto intr = std::make_shared<ParamsSensorDiffDrive>();
- intr->radius_left = 1.0;
- intr->radius_right = 1.0;
- intr->wheel_separation = 1.0;
- intr->ticks_per_wheel_revolution = 100;
- Vector3d extr(0,0,0);
- auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
- sensor->setStateBlockDynamic('I', true);
+ auto param = std::make_shared<ParamsSensorDiffDrive>();
+ param->ticks_per_wheel_revolution = 100;
+ auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive",
+ "sensor diff drive",
+ param,
+ Priors({{'P',Prior("P",Vector2d::Zero())},
+ {'O',Prior("O",Vector1d::Zero())},
+ {'I',Prior("StateBlock",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}})));
Vector3d pose; pose << 0,0,0;
@@ -412,7 +416,7 @@ TEST(Problem, perturb)
//// CHECK ALL STATE BLOCKS ///
-#define prec 1e-2
+#define prec 1e-3
for (auto S : problem->getHardware()->getSensorList())
{
@@ -460,14 +464,14 @@ TEST(Problem, check)
auto problem = Problem::create("PO", 2);
// make a sensor first
- auto intr = std::make_shared<ParamsSensorDiffDrive>();
- intr->radius_left = 1.0;
- intr->radius_right = 1.0;
- intr->wheel_separation = 1.0;
- intr->ticks_per_wheel_revolution = 100;
- Vector3d extr(0,0,0);
- auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
- sensor->setStateBlockDynamic('I', true);
+ auto param = std::make_shared<ParamsSensorDiffDrive>();
+ param->ticks_per_wheel_revolution = 100;
+ auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive",
+ "sensor diff drive",
+ param,
+ Priors({{'P',Prior("P",Vector2d::Zero())},
+ {'O',Prior("O",Vector1d::Zero())},
+ {'I',Prior("StateBlock",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}})));
Vector3d pose; pose << 0,0,0;
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 89341815653b03ed0d189b7d91c8e1c759f9bd9f..58d13348145372cff58688b8df43d8a864ea4fbd 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -30,7 +30,8 @@
#include "core/utils/utils_gtest.h"
#include "core/processor/processor_odom_2d.h"
-#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/sensor_odom.h"
+#include "dummy/sensor_dummy.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/capture/capture_void.h"
@@ -60,18 +61,14 @@ TEST(ProcessorBase, MotionProvider)
ProblemPtr problem = Problem::create("PO", 2);
// Install tracker (sensor and processor)
- auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
- "SensorDummy",
- nullptr,
- nullptr,
- nullptr,
- 2);
+ auto sens_trk = problem->installSensor("SensorDummy",
+ "dummy_sensor",
+ wolf_root + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml");
auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk);
// Install odometer (sensor and processor)
- SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d",
+ SensorBasePtr sens_odo = problem->installSensor("SensorOdom",
"odometer",
- Vector3d(0,0,0),
wolf_root + "/test/yaml/sensor_odom_2d.yaml");
ParamsProcessorOdom2dPtr proc_odo_params = make_shared<ParamsProcessorOdom2d>();
proc_odo_params->time_tolerance = dt/2;
@@ -102,17 +99,15 @@ TEST(ProcessorBase, KeyFrameCallback)
ProblemPtr problem = Problem::create("PO", 2);
// Install tracker (sensor and processor)
- auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
- "SensorTrackerDummy",
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)),
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)),
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
+ auto sens_trk = problem->installSensor("SensorDummy",
+ "dummy_sensor",
+ wolf_root + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml");
auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
proc_trk_params->time_tolerance = dt/2;
auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk, proc_trk_params);
// Install odometer (sensor and processor)
- SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ SensorBasePtr sens_odo = problem->installSensor("SensorOdom", "odometer", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
ParamsProcessorOdom2dPtr proc_odo_params = make_shared<ParamsProcessorOdom2d>();
proc_odo_params->time_tolerance = dt/2;
ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d", "odom processor", sens_odo, proc_odo_params);
@@ -144,9 +139,6 @@ TEST(ProcessorBase, KeyFrameCallback)
capt_odo->setTimeStamp(t);
std::cout << "2\n";
-// auto proc_odo_motion = std::static_pointer_cast<ProcessorMotion>(proc_odo);
-// auto last_ptr = proc_odo_motion->last_ptr_;
-// auto last_ptr_frame = last_ptr->getFrame();
proc_odo->captureCallback(capt_odo);
std::cout << "3\n";
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index a0d840d113f4d145ff4f0f323eb1a81642b9b01f..ee8c33e5f0bfd9fe7d25e1e006b927a96f830645 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -31,7 +31,7 @@
#include "core/common/wolf.h"
-#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/sensor_odom.h"
#include "core/processor/processor_odom_2d.h"
using namespace Eigen;
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 95af593518002badf85ec15a337b439b6a1703ed..7f7c15e7f030cba20e63b15d5dbb48a8f8dec609 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -105,7 +105,7 @@ class ProcessorTrackerFeatureDummyTest : public testing::Test
problem = Problem::create("PO", 2);
// Install camera
- sensor = problem->installSensor("SensorOdom2d", "auxiliar sensor", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ sensor = problem->installSensor("SensorOdom", "auxiliar sensor", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
// Install processor
params = std::make_shared<ParamsProcessorTrackerFeatureDummy>();
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 68f8147d38f23a8dc3a712b46fc2207d393baa71..64ab59ce841a85ac94c2f63dbc77ac1f51b80aba 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -124,7 +124,7 @@ class ProcessorTrackerLandmarkDummyTest : public testing::Test
problem = Problem::create("PO", 2);
// Install camera
- sensor = problem->installSensor("SensorOdom2d", "auxiliar sensor", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ sensor = problem->installSensor("SensorOdom", "auxiliar sensor", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
// Install processor
params = std::make_shared<ParamsProcessorTrackerLandmarkDummy>();
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 75412f792c6907be50aa661f12aecb71a6c2bdb3..27b0bea3aaa52efbe1a4d1930480ef91b0b6eb07 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -386,10 +386,12 @@ TEST(SensorBase, makeshared_server_PO)
std::vector<bool> drifts({false, true});
std::vector<bool> wrongs({false, true});
- VectorXd p_state(4), o_state(4), po_std(4);
+ VectorXd p_state(4), o_state(4), po_std(4), i_state(5), i_std(5);
p_state << 1, 2, 3, 4;
o_state << 1, 0, 0, 0;
po_std << 0.1, 0.2, 0.3, 0.4;
+ i_state << 1, 2, 3, 4, 5;
+ i_std << 0.1, 0.2, 0.3, 0.4, 0.5;
// P & O
for (auto dim : dims)
@@ -473,7 +475,7 @@ TEST(SensorBase, makeshared_server_PO)
// check
checkSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0);
checkSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
- checkSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
+ checkSensor(S, 'I', i_state, false, vector0, true, i_std);
checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
}
// POIA - 3D - INCORRECT YAML
@@ -502,10 +504,12 @@ TEST(SensorBase, factory)
std::vector<bool> drifts({false, true});
std::vector<bool> wrongs({false, true});
- VectorXd p_state(4), o_state(4), po_std(4);
+ VectorXd p_state(4), o_state(4), po_std(4), i_state(5), i_std(5);
p_state << 1, 2, 3, 4;
o_state << 1, 0, 0, 0;
po_std << 0.1, 0.2, 0.3, 0.4;
+ i_state << 1, 2, 3, 4, 5;
+ i_std << 0.1, 0.2, 0.3, 0.4, 0.5;
// P & O
for (auto dim : dims)
@@ -585,7 +589,7 @@ TEST(SensorBase, factory)
// check
checkSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0);
checkSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
- checkSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
+ checkSensor(S, 'I', i_state, false, vector0, true, i_std);
checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
}
// POIA - 3D - INCORRECT YAML
@@ -612,10 +616,12 @@ TEST(SensorBase, factory_yaml)
std::vector<bool> drifts({false, true});
std::vector<bool> wrongs({false, true});
- VectorXd p_state(4), o_state(4), po_std(4);
+ VectorXd p_state(4), o_state(4), po_std(4), i_state(5), i_std(5);
p_state << 1, 2, 3, 4;
o_state << 1, 0, 0, 0;
po_std << 0.1, 0.2, 0.3, 0.4;
+ i_state << 1, 2, 3, 4, 5;
+ i_std << 0.1, 0.2, 0.3, 0.4, 0.5;
// P & O
for (auto dim : dims)
@@ -693,7 +699,7 @@ TEST(SensorBase, factory_yaml)
// check
checkSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0);
checkSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
- checkSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
+ checkSensor(S, 'I', i_state, false, vector0, true, i_std);
checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
}
// POIA - 3D - INCORRECT YAML
diff --git a/test/gtest_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp
index a28d52df1b18090bb903c564b4daff1839e4036e..016476db811865bb38bbafb0e475fb3ed415dee0 100644
--- a/test/gtest_sensor_diff_drive.cpp
+++ b/test/gtest_sensor_diff_drive.cpp
@@ -27,6 +27,7 @@
*/
#include "core/sensor/sensor_diff_drive.h"
+#include "core/sensor/factory_sensor.h"
#include "core/utils/utils_gtest.h"
#include <cstdio>
@@ -34,62 +35,135 @@
using namespace wolf;
using namespace Eigen;
-TEST(DiffDrive, constructor)
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+Vector2d p_state = (Vector2d() << 1,2).finished();
+Vector1d o_state = (Vector1d() << 3).finished();
+Vector3d i_state = (Vector3d() << 0.1, 0.2, 0.3).finished();
+
+TEST(SensorDiffDrive, constructor_priors)
{
+ auto param = std::make_shared<ParamsSensorDiffDrive>();
- auto intr = std::make_shared<ParamsSensorDiffDrive>();
- Vector3d extr(1,2,3);
+ Priors priors{{'P',Prior("P", p_state)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state)}, //default "fix", not dynamic
+ {'I',Prior("StateBlock", i_state)}}; //default "fix", not dynamic
- auto sen = std::make_shared<SensorDiffDrive>(extr, intr);
+ auto sen = std::make_shared<SensorDiffDrive>("sensor_1", 2, param, priors);
ASSERT_NE(sen, nullptr);
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), Vector2d(1,2), 1e-12);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), Vector1d(3), 1e-12);
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS);
}
-TEST(DiffDrive, getParams)
+TEST(SensorDiffDrive, constructor_server)
{
- auto intr = std::make_shared<ParamsSensorDiffDrive>();
- intr->radius_left = 2;
- intr->radius_right = 3;
- intr->ticks_per_wheel_revolution = 4;
- intr->wheel_separation = 5;
+ ParserYaml parser = ParserYaml("test/yaml/sensor_diff_drive.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
- Vector3d extr(1,2,3);
+ auto params = std::make_shared<ParamsSensorDiffDrive>("sensor_1", server);
+ auto sen = std::make_shared<SensorDiffDrive>("sensor_1", 2, params, server);
- auto sen = std::make_shared<SensorDiffDrive>(extr, intr);
+ ASSERT_NE(sen, nullptr);
- ASSERT_NE(sen->getParams(), nullptr);
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
- ASSERT_EQ(sen->getRadiansPerTick(), 2.0*M_PI/intr->ticks_per_wheel_revolution); // this is dependent on 'ticks_per_wheel_revolution'
- ASSERT_EQ(sen->getParams()->radius_left, 2);
- ASSERT_EQ(sen->getParams()->radius_right, 3);
- ASSERT_EQ(sen->getParams()->ticks_per_wheel_revolution, 4);
- ASSERT_EQ(sen->getParams()->wheel_separation, 5);
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS);
}
-TEST(DiffDrive, create)
+TEST(SensorDiffDrive, factory)
{
- auto intr = std::make_shared<ParamsSensorDiffDrive>();
- intr->radius_left = 2;
- intr->radius_right = 3;
- intr->ticks_per_wheel_revolution = 4;
- intr->wheel_separation = 5;
+ ParserYaml parser = ParserYaml("test/yaml/sensor_diff_drive.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
- Vector3d extr(1,2,3);
+ auto sb = FactorySensor::create("SensorDiffDrive","sensor_1", 2, server);
- auto sen_base = SensorDiffDrive::create("name", extr, intr);
+ SensorDiffDrivePtr sen = std::dynamic_pointer_cast<SensorDiffDrive>(sb);
+
+ ASSERT_NE(sen, nullptr);
+
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS);
+}
- auto sen = std::static_pointer_cast<SensorDiffDrive>(sen_base);
+TEST(SensorDiffDrive, factory_yaml)
+{
+ auto sb = FactorySensorYaml::create("SensorDiffDrive","sensor_1", 2, wolf_root + "/test/yaml/sensor_diff_drive.yaml");
+
+ SensorDiffDrivePtr sen = std::dynamic_pointer_cast<SensorDiffDrive>(sb);
+
+ ASSERT_NE(sen, nullptr);
+
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS);
+}
+
+TEST(SensorDiffDrive, factory_priors)
+{
+ auto param = std::make_shared<ParamsSensorDiffDrive>();
+
+ Priors priors{{'P',Prior("P", p_state)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state)}, //default "fix", not dynamic
+ {'I',Prior("StateBlock", i_state)}}; //default "fix", not dynamic
+
+ auto sb = FactorySensorPriors::create("SensorDiffDrive","sensor_1", 2, param, priors);
+
+ SensorDiffDrivePtr sen = std::dynamic_pointer_cast<SensorDiffDrive>(sb);
+
+ ASSERT_NE(sen, nullptr);
+
+ ASSERT_TRUE(sen->getP()->isFixed());
+ ASSERT_TRUE(sen->getO()->isFixed());
+
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS);
+}
+
+TEST(SensorDiffDrive, getParams)
+{
+ auto param = std::make_shared<ParamsSensorDiffDrive>();
+ param->ticks_per_wheel_revolution = 400;
+ param->ticks_std_factor = 2;
+
+ Priors priors{{'P',Prior("P", p_state)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state)}, //default "fix", not dynamic
+ {'I',Prior("StateBlock", i_state)}}; //default "fix", not dynamic
+
+ auto sen = std::make_shared<SensorDiffDrive>("sensor_1", 2, param, priors);
ASSERT_NE(sen->getParams(), nullptr);
- ASSERT_EQ(sen->getRadiansPerTick(), 2.0*M_PI/intr->ticks_per_wheel_revolution); // this is dependent on 'ticks_per_wheel_revolution'
- ASSERT_EQ(sen->getParams()->radius_left, 2);
- ASSERT_EQ(sen->getParams()->radius_right, 3);
- ASSERT_EQ(sen->getParams()->ticks_per_wheel_revolution, 4);
- ASSERT_EQ(sen->getParams()->wheel_separation, 5);
+ ASSERT_NEAR(sen->getParams()->ticks_per_wheel_revolution, 400, Constants::EPS);
+ ASSERT_NEAR(sen->getRadiansPerTick(), 2*M_PI/400, Constants::EPS);
+ ASSERT_NEAR(sen->getParams()->ticks_std_factor, 2, Constants::EPS);
+}
+
+TEST(SensorDiffDrive, computeNoiseCov)
+{
+ auto param = std::make_shared<ParamsSensorDiffDrive>();
+ param->ticks_per_wheel_revolution = 400;
+ param->ticks_std_factor = 2;
+
+ Priors priors{{'P',Prior("P", p_state)}, //default "fix", not dynamic
+ {'O',Prior("O", o_state)}, //default "fix", not dynamic
+ {'I',Prior("StateBlock", i_state)}}; //default "fix", not dynamic
+
+ auto sen = std::make_shared<SensorDiffDrive>("sensor_1", 2, param, priors);
+
+ ASSERT_MATRIX_APPROX(sen->computeNoiseCov(Eigen::Vector2d::Zero()), Eigen::Matrix2d::Zero(), Constants::EPS);
+ ASSERT_MATRIX_APPROX(sen->computeNoiseCov(Eigen::Vector2d::Ones()), Eigen::Matrix2d::Identity() * 4, Constants::EPS);
}
int main(int argc, char **argv)
diff --git a/test/gtest_sensor_odom.cpp b/test/gtest_sensor_odom.cpp
index 9416dceb34a31ecdcaaa1d34fe6c557078e292c0..3cb35fdb1ee5e82862af75eb42a52f4b71afa508 100644
--- a/test/gtest_sensor_odom.cpp
+++ b/test/gtest_sensor_odom.cpp
@@ -776,22 +776,40 @@ TEST(SensorOdom, compute_noise_cov_3D)
// compute with displacement (3m) but not rotation (size 6)
ASSERT_MATRIX_APPROX(S->computeNoiseCov((Vector6d() << 3, 0, 0, 0, 0, 0).finished()),
- Matrix6d((min_var + disp_elements*k_disp_to_disp*3 + rot_elements*k_disp_to_rot*3).asDiagonal()),
+ Matrix6d((disp_elements*k_disp_to_disp*3 +
+ rot_elements*k_disp_to_rot*3).asDiagonal()),
Constants::EPS);
// compute with displacement (3m) but not rotation (size 7)
ASSERT_MATRIX_APPROX(S->computeNoiseCov((Vector7d() << 0, -3, 0, 0, 0, 0, 1).finished()),
- Matrix6d((min_var + disp_elements*k_disp_to_disp*3 + rot_elements*k_disp_to_rot*3).asDiagonal()),
+ Matrix6d((disp_elements*k_disp_to_disp*3 +
+ rot_elements*k_disp_to_rot*3).asDiagonal()),
Constants::EPS);
// compute with rotation (M_PI) but not displacement (size 6)
ASSERT_MATRIX_APPROX(S->computeNoiseCov((Vector6d() << 0, 0, 0, -M_PI, 0, 0).finished()),
- Matrix6d((min_var + rot_elements*k_rot_to_rot*M_PI).asDiagonal()),
+ Matrix6d((disp_elements*min_disp_var +
+ rot_elements*k_rot_to_rot*M_PI).asDiagonal()),
Constants::EPS);
// compute with rotation (M_PI) but not displacement (size 7)
ASSERT_MATRIX_APPROX(S->computeNoiseCov((Vector7d() << 0, 0, 0, 0, 0, 1, 0).finished()),
- Matrix6d((min_var + rot_elements*k_rot_to_rot*M_PI).asDiagonal()),
+ Matrix6d((disp_elements*min_disp_var +
+ rot_elements*k_rot_to_rot*M_PI).asDiagonal()),
+ Constants::EPS);
+
+ // compute with rotation (M_PI) and displacement (3m) (size 6)
+ ASSERT_MATRIX_APPROX(S->computeNoiseCov((Vector6d() << 0, 0, -3, 0, 0, M_PI).finished()),
+ Matrix6d((disp_elements*k_disp_to_disp*3 +
+ rot_elements*k_disp_to_rot*3 +
+ rot_elements*k_rot_to_rot*M_PI).asDiagonal()),
+ Constants::EPS);
+
+ // compute with rotation (M_PI) and displacement (3m) (size 7)
+ ASSERT_MATRIX_APPROX(S->computeNoiseCov((Vector7d() << 0, 3, 0, 0, 0, -1, 0).finished()),
+ Matrix6d((disp_elements*k_disp_to_disp*3 +
+ rot_elements*k_disp_to_rot*3 +
+ rot_elements*k_rot_to_rot*M_PI).asDiagonal()),
Constants::EPS);
}
diff --git a/test/yaml/params_problem_autosetup.yaml b/test/yaml/params_problem_autosetup.yaml
index c61eab58146a9a5917c1130a14e234afa08a01b6..8af72b6347377f3737d2e5a4222bd9862da37a81 100644
--- a/test/yaml/params_problem_autosetup.yaml
+++ b/test/yaml/params_problem_autosetup.yaml
@@ -18,7 +18,7 @@ config:
plugin: "core"
sensors:
-
- type: "SensorOdom3d"
+ type: "SensorOdom"
name: "odom"
plugin: "core"
k_disp_to_disp: 0.1
@@ -26,8 +26,15 @@ config:
k_rot_to_rot: 0.1
min_disp_var: 0.1
min_rot_var: 0.1
- extrinsic:
- pose: [1,2,3,0,0,0,1]
+ states:
+ P:
+ state: [1,2,3]
+ mode: fix
+ dynamic: false
+ O:
+ state: [0,0,0,1]
+ mode: "fix"
+ dynamic: false
processors:
-
type: "ProcessorOdom3d"
diff --git a/test/yaml/params_problem_autosetup_no_map.yaml b/test/yaml/params_problem_autosetup_no_map.yaml
index 6c5ed47c2efc2afc3ba960b075e60cea73b58a73..46c678eb21f897f5dfeb2b0f19050e86c94abed5 100644
--- a/test/yaml/params_problem_autosetup_no_map.yaml
+++ b/test/yaml/params_problem_autosetup_no_map.yaml
@@ -13,9 +13,9 @@ config:
time_tolerance: 0.1
tree_manager:
type: "None"
- sensors:
+ sensors:
-
- type: "SensorOdom3d"
+ type: "SensorOdom"
name: "odom"
plugin: "core"
k_disp_to_disp: 0.1
@@ -23,8 +23,15 @@ config:
k_rot_to_rot: 0.1
min_disp_var: 0.1
min_rot_var: 0.1
- extrinsic:
- pose: [1,2,3,0,0,0,1]
+ states:
+ P:
+ state: [1,2,3]
+ mode: fix
+ dynamic: false
+ O:
+ state: [0,0,0,1]
+ mode: "fix"
+ dynamic: false
processors:
-
type: "ProcessorOdom3d"
diff --git a/test/yaml/sensor_diff_drive.yaml b/test/yaml/sensor_diff_drive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5276c399284196b063651b8e7862f546f8b77ac4
--- /dev/null
+++ b/test/yaml/sensor_diff_drive.yaml
@@ -0,0 +1,16 @@
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: false
+ O:
+ mode: fix
+ state: [3]
+ dynamic: false
+ I:
+ mode: fix
+ state: [0.1, 0.2, 0.3]
+ dynamic: false
+
+ticks_per_wheel_revolution: 4
+ticks_std_factor: 2
diff --git a/test/yaml/sensor_diff_drive_dynamic.yaml b/test/yaml/sensor_diff_drive_dynamic.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e09cb6031d493d5936a7e664a563e260b86acffe
--- /dev/null
+++ b/test/yaml/sensor_diff_drive_dynamic.yaml
@@ -0,0 +1,16 @@
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+ O:
+ mode: fix
+ state: [3]
+ dynamic: true
+ I:
+ mode: fix
+ state: [0.1, 0.2, 0.3]
+ dynamic: true
+
+ticks_per_wheel_revolution: 4
+ticks_std_factor: 2
diff --git a/test/yaml/sensor_tests/sensor_POIA_3D.yaml b/test/yaml/sensor_tests/sensor_POIA_3D.yaml
index 397f1d2795b704bfce9f0d8c7078f83200788e0f..abe47172093a4e409660180baa507a469ea813f6 100644
--- a/test/yaml/sensor_tests/sensor_POIA_3D.yaml
+++ b/test/yaml/sensor_tests/sensor_POIA_3D.yaml
@@ -11,9 +11,9 @@ states:
dynamic: false
I:
mode: initial_guess
- state: [1, 2, 3, 4]
+ state: [1, 2, 3, 4, 5]
dynamic: true
- drift_std: [0.1, 0.2, 0.3, 0.4]
+ drift_std: [0.1, 0.2, 0.3, 0.4, 0.5]
A:
mode: factor
state: [1, 0, 0, 0]
diff --git a/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml b/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
index fab3cf9b19291061da5e1b96f315db02c7f249aa..5cf6bffb495b7cb98ba739c6de6581668541348b 100644
--- a/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
@@ -11,9 +11,9 @@ states:
dynamic: false
I:
mode: initial_guess
- state: [1, 2, 3, 4]
+ state: [1, 2, 3, 4, 5]
dynamic: true
- drift_std: [0.1, 0.2, 0.3, 0.4]
+ drift_std: [0.1, 0.2, 0.3, 0.4, 0.5]
# A:
# mode: factor
# state: [1, 0, 0, 0]