Skip to content
Snippets Groups Projects
Commit b79ddca8 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Merge branch '477-sensorpose-with-anisotropic-noise-specs' into 'devel' 

Resolve "SensorPose with anisotropic noise specs"

Closes #477

See merge request !461
parents 86fb963b 10096d6f
No related branches found
No related tags found
2 merge requests!466devel->main,!461Resolve "SensorPose with anisotropic noise specs"
Pipeline #13481 passed
Hide whitespace changes
Inline Side-by-side
include/core/sensor/sensor_base.h
+ 0
− 9
View file @ b79ddca8
...@@ -58,15 +58,10 @@ static ...@@ -58,15 +58,10 @@ static
SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _server) \ SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _server) \
{ \ { \
Eigen::VectorXd extrinsics = _server.template getParam<Eigen::VectorXd>("sensor/" + _unique_name + "/extrinsic/pose"); \ Eigen::VectorXd extrinsics = _server.template getParam<Eigen::VectorXd>("sensor/" + _unique_name + "/extrinsic/pose"); \
\
assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \ assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \
\
auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server); \ auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server); \
\
auto sensor = std::make_shared<SensorClass>(extrinsics, params); \ auto sensor = std::make_shared<SensorClass>(extrinsics, params); \
\
sensor ->setName(_unique_name); \ sensor ->setName(_unique_name); \
\
return sensor; \ return sensor; \
} \ } \
\ \
...@@ -74,13 +69,9 @@ static ...@@ -74,13 +69,9 @@ static
SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics, const ParamsSensorBasePtr _intrinsics)\ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics, const ParamsSensorBasePtr _intrinsics)\
{ \ { \
assert(_extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \ assert(_extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \
\
auto params = std::static_pointer_cast<ParamsSensorClass>(_intrinsics); \ auto params = std::static_pointer_cast<ParamsSensorClass>(_intrinsics); \
\
auto sensor = std::make_shared<SensorClass>(_extrinsics, params); \ auto sensor = std::make_shared<SensorClass>(_extrinsics, params); \
\
sensor ->setName(_unique_name); \ sensor ->setName(_unique_name); \
\
return sensor; \ return sensor; \
} \ } \
......
include/core/sensor/sensor_pose.h
+ 63
− 18
View file @ b79ddca8
...@@ -39,8 +39,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPose); ...@@ -39,8 +39,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPose);
struct ParamsSensorPose : public ParamsSensorBase struct ParamsSensorPose : public ParamsSensorBase
{ {
double std_p = 1; // m double std_px = 1; // m
double std_o = 1; // rad 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() ParamsSensorPose()
{ {
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE. //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
...@@ -48,14 +52,56 @@ struct ParamsSensorPose : public ParamsSensorBase ...@@ -48,14 +52,56 @@ struct ParamsSensorPose : public ParamsSensorBase
ParamsSensorPose(std::string _unique_name, const ParamsServer& _server): ParamsSensorPose(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server) ParamsSensorBase(_unique_name, _server)
{ {
std_p = _server.getParam<double>(prefix + _unique_name + "/std_p"); /* Here we allow isotropic specs for position and orientation
std_o = _server.getParam<double>(prefix + _unique_name + "/std_o"); * 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 std::string print() const override
{ {
return ParamsSensorBase::print() + "\n" return ParamsSensorBase::print() + "\n"
+ "std_p: " + std::to_string(std_p) + "\n" + "std_px: " + std::to_string(std_px) + "\n"
+ "std_o: " + std::to_string(std_o) + "\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; ~ParamsSensorPose() override = default;
}; };
...@@ -65,8 +111,7 @@ WOLF_PTR_TYPEDEFS(SensorPose); ...@@ -65,8 +111,7 @@ WOLF_PTR_TYPEDEFS(SensorPose);
class SensorPose : public SensorBase class SensorPose : public SensorBase
{ {
protected: protected:
double std_p_; // standard deviation of translation measurements ParamsSensorPosePtr params_pose_;
double std_o_; // standard deviation of orientation measurements
public: public:
SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& params); SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& params);
...@@ -75,20 +120,20 @@ class SensorPose : public SensorBase ...@@ -75,20 +120,20 @@ class SensorPose : public SensorBase
~SensorPose() override; ~SensorPose() override;
double getStdP() const; // double getStdP() const;
double getStdO() const; // double getStdO() const;
}; };
inline double SensorPose::getStdP() const // inline double SensorPose::getStdP() const
{ // {
return std_p_; // return std_p_;
} // }
inline double SensorPose::getStdO() const // inline double SensorPose::getStdO() const
{ // {
return std_o_; // return std_o_;
} // }
// inline Matrix6d SensorPose::computeDataCov() const // inline Matrix6d SensorPose::computeDataCov() const
// { // {
......
src/sensor/sensor_pose.cpp
+ 10
− 5
View file @ b79ddca8
...@@ -35,14 +35,19 @@ ...@@ -35,14 +35,19 @@
namespace wolf { namespace wolf {
SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& _intrinsics) : 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), SensorBase("SensorPose",
std_p_(_intrinsics.std_p), std::make_shared<StatePoint3d>(_extrinsics_pq.head(3), true),
std_o_(_intrinsics.std_o) 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."); 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(); params_pose_ = std::make_shared<ParamsSensorPose>(_intrinsics);
setNoiseCov(noise_cov_); // sets also noise_std_
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) : SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorPosePtr _intrinsics) :
......
src/yaml/sensor_pose_yaml.cpp
+ 6
− 2
View file @ b79ddca8
...@@ -49,8 +49,12 @@ static ParamsSensorBasePtr createParamsSensorPose(const std::string & _filename_ ...@@ -49,8 +49,12 @@ static ParamsSensorBasePtr createParamsSensorPose(const std::string & _filename_
{ {
ParamsSensorPosePtr params = std::make_shared<ParamsSensorPose>(); ParamsSensorPosePtr params = std::make_shared<ParamsSensorPose>();
params->std_p = config["std_p"].as<double>(); params->std_px = config["std_p"].as<double>();
params->std_o = config["std_o"].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; return params;
} }
......
test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
+ 6
− 2
View file @ b79ddca8
...@@ -127,8 +127,12 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test ...@@ -127,8 +127,12 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
// pose sensor and proc (to get extrinsics in the prb) // pose sensor and proc (to get extrinsics in the prb)
auto intr_sp = std::make_shared<ParamsSensorPose>(); auto intr_sp = std::make_shared<ParamsSensorPose>();
intr_sp->std_p = 1; intr_sp->std_px = 1;
intr_sp->std_o = 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(); Vector7d extr; extr << b_p_bm_, b_q_m_.coeffs();
sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp); sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp);
auto params_proc = std::make_shared<ParamsProcessorPose>(); auto params_proc = std::make_shared<ParamsProcessorPose>();
......
test/gtest_sensor_pose.cpp
+ 20
− 12
View file @ b79ddca8
...@@ -51,24 +51,32 @@ TEST(Pose, constructor) ...@@ -51,24 +51,32 @@ TEST(Pose, constructor)
TEST(Pose, getParams) TEST(Pose, getParams)
{ {
auto intr = std::make_shared<ParamsSensorPose>(); auto intr = std::make_shared<ParamsSensorPose>();
intr->std_p = 2; intr->std_px = 2;
intr->std_o = 3; 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; Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
auto sen = std::make_shared<SensorPose>(extr, intr); auto sen = std::make_shared<SensorPose>(extr, intr);
ASSERT_EQ(sen->getStdP(), intr->std_p); // ASSERT_EQ(sen->getStdP(), intr->std_p);
ASSERT_EQ(sen->getStdO(), intr->std_o); // ASSERT_EQ(sen->getStdO(), intr->std_o);
// ASSERT_EQ(sen->getNoiseCov()(0,0), (Vector1d() << 4).finished()); ASSERT_EQ(sen->getNoiseCov()(0,0), 4);
// ASSERT_EQ(sen->getNoiseCov()(3,3), (Vector1d() << 9).finished()); ASSERT_EQ(sen->getNoiseCov()(3,3), 9);
} }
TEST(Pose, create) TEST(Pose, create)
{ {
auto intr = std::make_shared<ParamsSensorPose>(); auto intr = std::make_shared<ParamsSensorPose>();
intr->std_p = 2; intr->std_px = 2;
intr->std_o = 3; 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; Vector7d extr; extr << 1,2,3,.5,.5,.5,.5;
...@@ -76,10 +84,10 @@ TEST(Pose, create) ...@@ -76,10 +84,10 @@ TEST(Pose, create)
auto sen = std::static_pointer_cast<SensorPose>(sen_base); auto sen = std::static_pointer_cast<SensorPose>(sen_base);
ASSERT_EQ(sen->getStdP(), intr->std_p); // ASSERT_EQ(sen->getStdP(), intr->std_p);
ASSERT_EQ(sen->getStdO(), intr->std_o); // ASSERT_EQ(sen->getStdO(), intr->std_o);
// ASSERT_EQ(sen->getNoiseCov()(0,0), (Vector1d() << 4).finished()); ASSERT_EQ(sen->getNoiseCov()(0,0), 4);
// ASSERT_EQ(sen->getNoiseCov()(3,3), (Vector1d() << 9).finished()); ASSERT_EQ(sen->getNoiseCov()(3,3), 9);
} }
int main(int argc, char **argv) int main(int argc, char **argv)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment