diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 84449032ae000d3f008236521ec96325ef8321d4..8e9b97fcd99909926fcb12ddbcab4309bc3cc62f 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -152,6 +152,8 @@ deploy_imu:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH != "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
DEPLOY_CI_ROS: "false"
@@ -164,6 +166,8 @@ deploy_gnss:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH != "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
@@ -177,6 +181,8 @@ deploy_vision:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH != "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
DEPLOY_CI_ROS: "false"
@@ -189,6 +195,8 @@ deploy_laser:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH != "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
@@ -202,6 +210,8 @@ deploy_apriltag:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH != "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
@@ -215,6 +225,8 @@ deploy_bodydynamics:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH != "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
@@ -229,6 +241,8 @@ deploy_imu_main:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: main
DEPLOY_CI_ROS: "false"
@@ -241,6 +255,8 @@ deploy_gnss_main:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: main
GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
@@ -254,6 +270,8 @@ deploy_vision_main:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: main
DEPLOY_CI_ROS: "false"
@@ -266,6 +284,8 @@ deploy_laser_main:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: main
LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
@@ -279,6 +299,8 @@ deploy_apriltag_main:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: main
WOLF_VISION_BRANCH: main
@@ -292,6 +314,8 @@ deploy_bodydynamics_main:
stage: deploy_plugins
rules:
- if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: main
WOLF_IMU_BRANCH: main
@@ -312,6 +336,8 @@ deploy_wolf_ros_node:
stage: deploy_ros
rules:
- if: $CI_COMMIT_BRANCH != "main" && $DEPLOY_CI_ROS == "true"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
@@ -337,6 +363,8 @@ deploy_wolf_ros_node_main:
stage: deploy_ros
rules:
- if: $CI_COMMIT_BRANCH == "main" && $DEPLOY_CI_ROS == "true"
+ - if: $CI_PIPELINE_SOURCE == "merge_request_event"
+ when: never
variables:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
WOLF_IMU_BRANCH: main
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 482dff8644d9ade38612bfd29cab879e63955275..454fb800063780f68b64f3e2564e63a0f59e4c87 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -26,7 +26,6 @@ namespace wolf{
class HardwareBase;
class ProcessorBase;
class StateBlock;
-class ParamsServer;
}
//Wolf includes
@@ -98,7 +97,7 @@ struct ParamsSensorBase: public ParamsBase
name = _input_node["name"].as<std::string>();
}
~ParamsSensorBase() override = default;
-
+ using ParamsBase::ParamsBase;
std::string print() const override
{
return "name: " + name;
diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h
index 4bafeb5ff67cfc36d5eccea46fa4c549ec275542..c303e3cfb8c9f8e2aae62ff4355642064bf75314 100644
--- a/include/core/sensor/sensor_pose.h
+++ b/include/core/sensor/sensor_pose.h
@@ -31,21 +31,24 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPose);
struct ParamsSensorPose : public ParamsSensorBase
{
- double std_p = 1; // m
- double std_o = 1; // rad
+ Eigen::VectorXd std_noise;
+ // double std_p = 1; // m
+ // double std_o = 1; // rad
ParamsSensorPose() = default;
ParamsSensorPose(const YAML::Node& _input_node):
ParamsSensorBase(_input_node)
{
- std_p = _input_node["std_p"].as<double>();
- std_o = _input_node["std_o"].as<double>();
+ std_noise = _input_node["std_noise"].as<Eigen::VectorXd>();
+ // std_p = _input_node["std_p"].as<double>();
+ // std_o = _input_node["std_o"].as<double>();
}
~ParamsSensorPose() override = default;
std::string print() const override
{
return ParamsSensorBase::print() + "\n"
- + "std_p: " + toString(std_p) + "\n"
- + "std_o: " + toString(std_o) + "\n";
+ + "std_p: " + toString(std_noise) + "\n";
+ // + "std_p: " + toString(std_p) + "\n"
+ // + "std_o: " + toString(std_o) + "\n";
}
};
@@ -65,45 +68,22 @@ class SensorPose : public SensorBase
params_pose_(_params)
{
static_assert(DIM == 2 or DIM == 3);
+ if (DIM == 2 and params_pose_->std_noise.size() != 3)
+ throw std::runtime_error("SensorBase: wrong std_noise size for 2D: " + toString(params_pose_->std_noise.size()) + "(should be 3)");
+ if (DIM == 3 and params_pose_->std_noise.size() != 6)
+ throw std::runtime_error("SensorBase: wrong std_noise size for 3D: " + toString(params_pose_->std_noise.size()) + "(should be 6)");
}
WOLF_SENSOR_CREATE(SensorPose<DIM>, ParamsSensorPose);
~SensorPose() override = default;
- double getStdP() const;
- double getStdO() const;
-
Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override;
};
template <unsigned int DIM>
-inline double SensorPose<DIM>::getStdP() const
-{
- return params_pose_->std_p;
-}
-
-template <unsigned int DIM>
-inline double SensorPose<DIM>::getStdO() const
-{
- return params_pose_->std_o;
-}
-
-template <>
-inline Eigen::MatrixXd SensorPose<2>::computeNoiseCov(const Eigen::VectorXd & _data) const
-{
- return (Eigen::Vector3d() << params_pose_->std_p,
- params_pose_->std_p,
- params_pose_->std_o).finished().cwiseAbs2().asDiagonal();
-}
-template <>
-inline Eigen::MatrixXd SensorPose<3>::computeNoiseCov(const Eigen::VectorXd & _data) const
+inline Eigen::MatrixXd SensorPose<DIM>::computeNoiseCov(const Eigen::VectorXd & _data) const
{
- return (Eigen::Vector6d() << params_pose_->std_p,
- params_pose_->std_p,
- params_pose_->std_p,
- params_pose_->std_o,
- params_pose_->std_o,
- params_pose_->std_o).finished().cwiseAbs2().asDiagonal();
+ return params_pose_->std_noise.cwiseAbs2().asDiagonal();
}
typedef SensorPose<2> SensorPose2d;
diff --git a/schema/sensor/SensorPose2d.schema b/schema/sensor/SensorPose2d.schema
index 3e8557e3737c1541e0a6731dcef12aa0e5db3732..5863bd1bdcd83c96b2ef109913633ebb97309665 100644
--- a/schema/sensor/SensorPose2d.schema
+++ b/schema/sensor/SensorPose2d.schema
@@ -1,14 +1,9 @@
follow: SensorBase.schema
-std_p:
+std_noise:
_mandatory: true
- _type: double
- _doc: standard deviation of the position measurement.
-
-std_o:
- _mandatory: true
- _type: double
- _doc: standard deviation of the orientation measurement.
+ _type: Eigen::Vector3d
+ _doc: Vector containing the standard deviation of the position and orientation measurements (square root of the covariance matrix diagonal).
states:
P:
diff --git a/schema/sensor/SensorPose3d.schema b/schema/sensor/SensorPose3d.schema
index f00a9ef433301bff8b49835a7a03e5affac8f045..8c1b7d2081825abc24c7177fc3ba70ce58fdcd4e 100644
--- a/schema/sensor/SensorPose3d.schema
+++ b/schema/sensor/SensorPose3d.schema
@@ -1,14 +1,9 @@
follow: SensorBase.schema
-std_p:
+std_noise:
_mandatory: true
- _type: double
- _doc: standard deviation of the position measurement.
-
-std_o:
- _mandatory: true
- _type: double
- _doc: standard deviation of the orientation measurement.
+ _type: Eigen::Vector6d
+ _doc: Vector containing the standard deviation of the position and orientation measurements (square root of the covariance matrix diagonal).
states:
P:
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index d5f2b0b04782ec0e2c45f9a4ccbbee35b2048c9f..b35f00900c03eafbd1bde420ff02439a53367537 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -116,8 +116,10 @@ class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
solver = std::make_shared<SolverCeres>(problem);
// Create sensor to be able to initialize
+ auto S_params = std::make_shared<ParamsSensorPose>();
+ S_params->std_noise = Eigen::Vector6d::Ones();
S = SensorBase::emplace<SensorPose3d>(problem->getHardware(),
- std::make_shared<ParamsSensorPose>(),
+ S_params,
SpecSensorComposite{{'P',SpecStateSensor("StatePoint3d",pos_camera)},
{'O',SpecStateSensor("StateQuaternion",vquat_camera)}});
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 b5e8188673f1ba77d38e984a14472da22d7b628f..de8205cd0b1dd9084bbdbf2fa1677038a81823ef 100644
--- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
@@ -120,8 +120,7 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
// pose sensor and proc (to get extrinsics in the prb)
auto params_sp = std::make_shared<ParamsSensorPose>();
params_sp->name = "pose sensor";
- params_sp->std_p = 1;
- params_sp->std_o = 1;
+ params_sp->std_noise = Vector6d::Ones();
sensor_pose_ = SensorBase::emplace<SensorPose3d>(problem_->getHardware(),
params_sp,
SpecSensorComposite{{'P',SpecStateSensor("StatePoint3d",b_p_bm_,"initial_guess")},
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
index e2d17acbcd62c7f9413328913aa7ceaceed874cf..2f57e54a57d3311d1f4fce3386525650b92ead2c 100644
--- a/test/gtest_sensor_pose.cpp
+++ b/test/gtest_sensor_pose.cpp
@@ -19,12 +19,6 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file gtest_sensor_pose.cpp
- *
- * Created on: Feb 18, 2020
- * \author: mfourmy
- */
#include "core/sensor/sensor_pose.h"
#include "core/sensor/factory_sensor.h"
@@ -42,17 +36,16 @@ Vector2d p_state_2D = (Vector2d() << 1,2).finished();
Vector3d p_state_3D = (Vector3d() << 1,2,3).finished();
Vector1d o_state_2D = (Vector1d() << 3).finished();
Vector4d o_state_3D = (Vector4d() << .5,.5,.5,.5).finished();
-double std_p = 0.2;
-double std_o = 0.1;
-Matrix3d noise_cov_2D = (Vector3d() << std_p, std_p, std_o).finished().cwiseAbs2().asDiagonal();
-Matrix6d noise_cov_3D = (Vector6d() << std_p, std_p, std_p, std_o, std_o, std_o).finished().cwiseAbs2().asDiagonal();
+Vector3d std_noise_2D = (Vector3d() << 0.1, 0.2, 0.3).finished();
+Vector6d std_noise_3D = (Vector6d() << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6).finished();
+Matrix3d noise_cov_2D = std_noise_2D.cwiseAbs2().asDiagonal();
+Matrix6d noise_cov_3D = std_noise_3D.cwiseAbs2().asDiagonal();
TEST(Pose, constructor_priors_2D)
{
auto param = std::make_shared<ParamsSensorPose>();
param->name = "a not so cool sensor";
- param->std_p = std_p;
- param->std_o = std_o;
+ param->std_noise = std_noise_2D;
SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint2d", p_state_2D)}, //default "fix", not dynamic
{'O',SpecStateSensor("StateAngle", o_state_2D)}}; //default "fix", not dynamic
@@ -67,9 +60,6 @@ TEST(Pose, constructor_priors_2D)
ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
- ASSERT_EQ(sen->getStdP(), std_p);
- ASSERT_EQ(sen->getStdO(), std_o);
-
ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
}
@@ -77,8 +67,7 @@ TEST(Pose, constructor_priors_3D)
{
auto param = std::make_shared<ParamsSensorPose>();
param->name = "a not so cool sensor";
- param->std_p = std_p;
- param->std_o = std_o;
+ param->std_noise = std_noise_3D;
SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint3d", p_state_3D)}, //default "fix", not dynamic
{'O',SpecStateSensor("StateQuaternion", o_state_3D)}}; //default "fix", not dynamic
@@ -93,9 +82,6 @@ TEST(Pose, constructor_priors_3D)
ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
- ASSERT_EQ(sen->getStdP(), std_p);
- ASSERT_EQ(sen->getStdO(), std_o);
-
ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
}
@@ -117,9 +103,6 @@ TEST(Pose, factory_2D)
ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
- ASSERT_EQ(sen->getStdP(), std_p);
- ASSERT_EQ(sen->getStdO(), std_o);
-
ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
}
@@ -141,9 +124,6 @@ TEST(Pose, factory_3D)
ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
- ASSERT_EQ(sen->getStdP(), std_p);
- ASSERT_EQ(sen->getStdO(), std_o);
-
ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
}
@@ -161,9 +141,6 @@ TEST(Pose, factory_yaml_2D)
ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
- ASSERT_EQ(sen->getStdP(), std_p);
- ASSERT_EQ(sen->getStdO(), std_o);
-
ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
}
@@ -181,9 +158,6 @@ TEST(Pose, factory_yaml_3D)
ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
- ASSERT_EQ(sen->getStdP(), std_p);
- ASSERT_EQ(sen->getStdO(), std_o);
-
ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
}
diff --git a/test/yaml/sensor_pose_2d.yaml b/test/yaml/sensor_pose_2d.yaml
index 10c6003e44afdd3a6d3d5762f3318be380fffc35..78d84857f7d3309cc12ca02eb7196d8367ddbefd 100644
--- a/test/yaml/sensor_pose_2d.yaml
+++ b/test/yaml/sensor_pose_2d.yaml
@@ -9,6 +9,4 @@ states:
state: [3]
dynamic: false
-std_p: 0.2
-std_o: 0.1
-
+std_noise: [0.1, 0.2, 0.3]
diff --git a/test/yaml/sensor_pose_3d.yaml b/test/yaml/sensor_pose_3d.yaml
index 249af7bfb925cc97f943e6136b39c2dc08b5b1d3..128461a53abd981d9b85ee9a339e6e3fb99c79b2 100644
--- a/test/yaml/sensor_pose_3d.yaml
+++ b/test/yaml/sensor_pose_3d.yaml
@@ -9,5 +9,4 @@ states:
state: [0.5, 0.5, 0.5, 0.5]
dynamic: false
-std_p: 0.2
-std_o: 0.1
+std_noise: [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]