diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 8e6c6db7c6657037e7df0eddeea9b683635e5b71..a8572e7d66887ea60d6d38ed427f268c748662c2 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -58,15 +58,10 @@ static
SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _server) \
{ \
Eigen::VectorXd extrinsics = _server.template getParam<Eigen::VectorXd>("sensor/" + _unique_name + "/extrinsic/pose"); \
- \
assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \
- \
auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server); \
- \
auto sensor = std::make_shared<SensorClass>(extrinsics, params); \
- \
sensor ->setName(_unique_name); \
- \
return sensor; \
} \
\
@@ -74,13 +69,9 @@ static
SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics, const ParamsSensorBasePtr _intrinsics)\
{ \
assert(_extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \
- \
auto params = std::static_pointer_cast<ParamsSensorClass>(_intrinsics); \
- \
auto sensor = std::make_shared<SensorClass>(_extrinsics, params); \
- \
sensor ->setName(_unique_name); \
- \
return sensor; \
} \
diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h
index e653fe458d39107c143da729a006c727df3959e3..1fbf559364af15d85a74f8b53a13f0c74355b188 100644
--- a/include/core/sensor/sensor_pose.h
+++ b/include/core/sensor/sensor_pose.h
@@ -39,8 +39,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPose);
struct ParamsSensorPose : public ParamsSensorBase
{
- double std_p = 1; // m
- double std_o = 1; // rad
+ double std_px = 1; // m
+ double std_py = 1; // m
+ double std_pz = 1; // m
+ double std_ox = 1; // rad
+ double std_oy = 1; // rad
+ double std_oz = 1; // rad
ParamsSensorPose()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
@@ -48,14 +52,56 @@ struct ParamsSensorPose : public ParamsSensorBase
ParamsSensorPose(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
- std_p = _server.getParam<double>(prefix + _unique_name + "/std_p");
- std_o = _server.getParam<double>(prefix + _unique_name + "/std_o");
+ /* Here we allow isotropic specs for position and orientation
+ * and also particularizations for eaxh axis
+ * depending on how the info was ptovided in the YAML file.
+ * Combinations are possible.
+ *
+ * E.g. if the yaml file contains
+ *
+ * std_p : 1
+ *
+ * then the result is std_px = 1, std_py = 1, std_pz = 1
+ *
+ * If the yaml file contains (in any order)
+ *
+ * std_p : 1
+ * std_pz : 0.1
+ *
+ * then the result is std_px = 1, std_py = 1, std_pz = 0.1
+ */
+
+ // first the isotropic options
+ if (_server.hasParam(prefix + _unique_name + "/std_p"))
+ std_px = std_py = std_pz = _server.getParam<double>(prefix + _unique_name + "/std_p");
+ if (_server.hasParam(prefix + _unique_name + "/std_o"))
+ std_ox = std_oy = std_oz = _server.getParam<double>(prefix + _unique_name + "/std_o");
+
+ // then individual axes specifications
+ if (_server.hasParam(prefix + _unique_name + "/std_px"))
+ std_px = _server.getParam<double>(prefix + _unique_name + "/std_px");
+ if (_server.hasParam(prefix + _unique_name + "/std_py"))
+ std_py = _server.getParam<double>(prefix + _unique_name + "/std_py");
+ if (_server.hasParam(prefix + _unique_name + "/std_pz"))
+ std_pz = _server.getParam<double>(prefix + _unique_name + "/std_pz");
+
+ if (_server.hasParam(prefix + _unique_name + "/std_ox"))
+ std_ox = _server.getParam<double>(prefix + _unique_name + "/std_ox");
+ if (_server.hasParam(prefix + _unique_name + "/std_oy"))
+ std_oy = _server.getParam<double>(prefix + _unique_name + "/std_oy");
+ if (_server.hasParam(prefix + _unique_name + "/std_oz"))
+ std_oz = _server.getParam<double>(prefix + _unique_name + "/std_oz");
+
}
std::string print() const override
{
return ParamsSensorBase::print() + "\n"
- + "std_p: " + std::to_string(std_p) + "\n"
- + "std_o: " + std::to_string(std_o) + "\n";
+ + "std_px: " + std::to_string(std_px) + "\n"
+ + "std_py: " + std::to_string(std_py) + "\n"
+ + "std_pz: " + std::to_string(std_pz) + "\n"
+ + "std_ox: " + std::to_string(std_ox) + "\n";
+ + "std_oy: " + std::to_string(std_oy) + "\n";
+ + "std_oz: " + std::to_string(std_oz) + "\n";
}
~ParamsSensorPose() override = default;
};
@@ -65,8 +111,7 @@ WOLF_PTR_TYPEDEFS(SensorPose);
class SensorPose : public SensorBase
{
protected:
- double std_p_; // standard deviation of translation measurements
- double std_o_; // standard deviation of orientation measurements
+ ParamsSensorPosePtr params_pose_;
public:
SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& params);
@@ -75,20 +120,20 @@ class SensorPose : public SensorBase
~SensorPose() override;
- double getStdP() const;
- double getStdO() const;
+ // double getStdP() const;
+ // double getStdO() const;
};
-inline double SensorPose::getStdP() const
-{
- return std_p_;
-}
+// inline double SensorPose::getStdP() const
+// {
+// return std_p_;
+// }
-inline double SensorPose::getStdO() const
-{
- return std_o_;
-}
+// inline double SensorPose::getStdO() const
+// {
+// return std_o_;
+// }
// inline Matrix6d SensorPose::computeDataCov() const
// {
diff --git a/src/sensor/sensor_pose.cpp b/src/sensor/sensor_pose.cpp
index 0d12027c9d6b880a98931b4b0e3939a3f2150f0d..c9c0c90db18737e6052e83a2050ee9fd3a1ed71f 100644
--- a/src/sensor/sensor_pose.cpp
+++ b/src/sensor/sensor_pose.cpp
@@ -35,14 +35,19 @@
namespace wolf {
SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& _intrinsics) :
- SensorBase("SensorPose", std::make_shared<StatePoint3d>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
- std_p_(_intrinsics.std_p),
- std_o_(_intrinsics.std_o)
+ SensorBase("SensorPose",
+ std::make_shared<StatePoint3d>(_extrinsics_pq.head(3), true),
+ std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true),
+ nullptr,
+ 6)
{
assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
- noise_cov_ = (Eigen::Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal();
- setNoiseCov(noise_cov_); // sets also noise_std_
+ params_pose_ = std::make_shared<ParamsSensorPose>(_intrinsics);
+
+ noise_std_ << params_pose_->std_px, params_pose_->std_py, params_pose_->std_pz, params_pose_->std_ox, params_pose_->std_oy, params_pose_->std_oz;
+
+ setNoiseStd(noise_std_); // sets also noise_cov_
}
SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorPosePtr _intrinsics) :
diff --git a/src/yaml/sensor_pose_yaml.cpp b/src/yaml/sensor_pose_yaml.cpp
index bfbe151c43315c21e5f636e2632fa9ceb614c9d4..4ce97752fc8d5a59aed7c7b0a4b0c3f89de07405 100644
--- a/src/yaml/sensor_pose_yaml.cpp
+++ b/src/yaml/sensor_pose_yaml.cpp
@@ -49,8 +49,12 @@ static ParamsSensorBasePtr createParamsSensorPose(const std::string & _filename_
{
ParamsSensorPosePtr params = std::make_shared<ParamsSensorPose>();
- params->std_p = config["std_p"].as<double>();
- params->std_o = config["std_o"].as<double>();
+ params->std_px = config["std_p"].as<double>();
+ params->std_py = config["std_p"].as<double>();
+ params->std_pz = config["std_p"].as<double>();
+ params->std_ox = config["std_o"].as<double>();
+ params->std_oy = config["std_o"].as<double>();
+ params->std_oz = config["std_o"].as<double>();
return params;
}
diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
index 7e1e8624ec6c9d10711875ca85fe256adcdf2ba6..1b287910cdb0ed08015ffd1831159af6325d88bd 100644
--- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
@@ -127,8 +127,12 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
// pose sensor and proc (to get extrinsics in the prb)
auto intr_sp = std::make_shared<ParamsSensorPose>();
- intr_sp->std_p = 1;
- intr_sp->std_o = 1;
+ intr_sp->std_px = 1;
+ intr_sp->std_py = 1;
+ intr_sp->std_pz = 1;
+ intr_sp->std_ox = 1;
+ intr_sp->std_oy = 1;
+ intr_sp->std_oz = 1;
Vector7d extr; extr << b_p_bm_, b_q_m_.coeffs();
sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp);
auto params_proc = std::make_shared<ParamsProcessorPose>();
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
index d37c700a7daf6a45862a4d8b830b8b8e67767282..ad32fad28a18655ac16993733e6fa371ef805d92 100644
--- a/test/gtest_sensor_pose.cpp
+++ b/test/gtest_sensor_pose.cpp
@@ -51,24 +51,32 @@ TEST(Pose, constructor)
TEST(Pose, getParams)
{
auto intr = std::make_shared<ParamsSensorPose>();
- intr->std_p = 2;
- intr->std_o = 3;
+ intr->std_px = 2;
+ intr->std_py = 2;
+ intr->std_pz = 2;
+ intr->std_ox = 3;
+ intr->std_oy = 3;
+ intr->std_oz = 3;
Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
auto sen = std::make_shared<SensorPose>(extr, intr);
- ASSERT_EQ(sen->getStdP(), intr->std_p);
- ASSERT_EQ(sen->getStdO(), intr->std_o);
- // ASSERT_EQ(sen->getNoiseCov()(0,0), (Vector1d() << 4).finished());
- // ASSERT_EQ(sen->getNoiseCov()(3,3), (Vector1d() << 9).finished());
+ // ASSERT_EQ(sen->getStdP(), intr->std_p);
+ // ASSERT_EQ(sen->getStdO(), intr->std_o);
+ ASSERT_EQ(sen->getNoiseCov()(0,0), 4);
+ ASSERT_EQ(sen->getNoiseCov()(3,3), 9);
}
TEST(Pose, create)
{
auto intr = std::make_shared<ParamsSensorPose>();
- intr->std_p = 2;
- intr->std_o = 3;
+ intr->std_px = 2;
+ intr->std_py = 2;
+ intr->std_pz = 2;
+ intr->std_ox = 3;
+ intr->std_oy = 3;
+ intr->std_oz = 3;
Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
@@ -76,10 +84,10 @@ TEST(Pose, create)
auto sen = std::static_pointer_cast<SensorPose>(sen_base);
- ASSERT_EQ(sen->getStdP(), intr->std_p);
- ASSERT_EQ(sen->getStdO(), intr->std_o);
- // ASSERT_EQ(sen->getNoiseCov()(0,0), (Vector1d() << 4).finished());
- // ASSERT_EQ(sen->getNoiseCov()(3,3), (Vector1d() << 9).finished());
+ // ASSERT_EQ(sen->getStdP(), intr->std_p);
+ // ASSERT_EQ(sen->getStdO(), intr->std_o);
+ ASSERT_EQ(sen->getNoiseCov()(0,0), 4);
+ ASSERT_EQ(sen->getNoiseCov()(3,3), 9);
}
int main(int argc, char **argv)