Skip to content
Snippets Groups Projects
Commit d45fe35f authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Rename Intrinsics -> ParamsSensor

parent d51c2f3b
No related branches found
No related tags found
2 merge requests!28release after RAL,!27After 2nd RAL submission
...@@ -8,10 +8,10 @@ ...@@ -8,10 +8,10 @@
namespace wolf { namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsGnss); WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorGnss);
WOLF_PTR_TYPEDEFS(SensorGnss); WOLF_PTR_TYPEDEFS(SensorGnss);
struct IntrinsicsGnss : public IntrinsicsBase struct ParamsSensorGnss : public ParamsSensorBase
{ {
// add GNSS parameters here // add GNSS parameters here
bool extrinsics_fixed_ = true; bool extrinsics_fixed_ = true;
...@@ -19,10 +19,10 @@ struct IntrinsicsGnss : public IntrinsicsBase ...@@ -19,10 +19,10 @@ struct IntrinsicsGnss : public IntrinsicsBase
bool pitch_fixed_=true; bool pitch_fixed_=true;
bool yaw_fixed_ =false; bool yaw_fixed_ =false;
bool translation_fixed_ = false; bool translation_fixed_ = false;
virtual ~IntrinsicsGnss() = default; virtual ~ParamsSensorGnss() = default;
IntrinsicsGnss() = default; ParamsSensorGnss() = default;
IntrinsicsGnss(std::string _unique_name, const ParamsServer& _server): ParamsSensorGnss(std::string _unique_name, const ParamsServer& _server):
IntrinsicsBase(_unique_name, _server) ParamsSensorBase(_unique_name, _server)
{ {
extrinsics_fixed_ = _server.getParam<bool>(_unique_name + "/extrinsics_fixed"); extrinsics_fixed_ = _server.getParam<bool>(_unique_name + "/extrinsics_fixed");
roll_fixed_ = _server.getParam<bool>(_unique_name + "/roll_fixed"); roll_fixed_ = _server.getParam<bool>(_unique_name + "/roll_fixed");
...@@ -32,7 +32,7 @@ struct IntrinsicsGnss : public IntrinsicsBase ...@@ -32,7 +32,7 @@ struct IntrinsicsGnss : public IntrinsicsBase
} }
std::string print() const std::string print() const
{ {
return "\n" + IntrinsicsBase::print() + "\n" return "\n" + ParamsSensorBase::print() + "\n"
+ "extrinsics_fixed: " + std::to_string(extrinsics_fixed_) + "\n" + "extrinsics_fixed: " + std::to_string(extrinsics_fixed_) + "\n"
+ "roll_fixed: " + std::to_string(roll_fixed_) + "\n" + "roll_fixed: " + std::to_string(roll_fixed_) + "\n"
+ "pitch_fixed: " + std::to_string(pitch_fixed_) + "\n" + "pitch_fixed: " + std::to_string(pitch_fixed_) + "\n"
...@@ -44,14 +44,14 @@ struct IntrinsicsGnss : public IntrinsicsBase ...@@ -44,14 +44,14 @@ struct IntrinsicsGnss : public IntrinsicsBase
class SensorGnss : public SensorBase class SensorGnss : public SensorBase
{ {
protected: protected:
IntrinsicsGnssPtr params_; ParamsSensorGnssPtr params_;
bool ENU_defined_, ENU_MAP_initialized_; bool ENU_defined_, ENU_MAP_initialized_;
Eigen::Matrix3d R_ENU_ECEF_; Eigen::Matrix3d R_ENU_ECEF_;
Eigen::Vector3d t_ENU_ECEF_; Eigen::Vector3d t_ENU_ECEF_;
public: public:
SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, IntrinsicsGnssPtr params); SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, ParamsSensorGnssPtr params);
SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, IntrinsicsGnssPtr params, StateBlockPtr _t_ENU_MAP_ptr, StateBlockPtr _roll_ENU_MAP_ptr, StateBlockPtr _pitch_ENU_MAP_ptr, StateBlockPtr _yaw_ENU_MAP_ptr); SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, ParamsSensorGnssPtr params, StateBlockPtr _t_ENU_MAP_ptr, StateBlockPtr _roll_ENU_MAP_ptr, StateBlockPtr _pitch_ENU_MAP_ptr, StateBlockPtr _yaw_ENU_MAP_ptr);
virtual ~SensorGnss(); virtual ~SensorGnss();
...@@ -85,7 +85,7 @@ class SensorGnss : public SensorBase ...@@ -85,7 +85,7 @@ class SensorGnss : public SensorBase
const Eigen::Vector3d& _v_ECEF_antena1_antena2); const Eigen::Vector3d& _v_ECEF_antena1_antena2);
public: public:
static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics_p, const IntrinsicsBasePtr _intrinsics); static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics_p, const ParamsSensorBasePtr _intrinsics);
}; };
......
...@@ -13,7 +13,7 @@ static double kSecondEccentricitySquared = 6.73949674228 * 0.001; ...@@ -13,7 +13,7 @@ static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D position with respect to the car frame (base frame) SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D position with respect to the car frame (base frame)
StateBlockPtr _bias_ptr, //GNSS sensor time bias StateBlockPtr _bias_ptr, //GNSS sensor time bias
IntrinsicsGnssPtr params) //sensor params ParamsSensorGnssPtr params) //sensor params
: :
SensorBase("SensorGnss", _p_ptr, nullptr, _bias_ptr, 0), // antena orientation has no sense in GNSS SensorBase("SensorGnss", _p_ptr, nullptr, _bias_ptr, 0), // antena orientation has no sense in GNSS
params_(params), params_(params),
...@@ -31,7 +31,7 @@ SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D positi ...@@ -31,7 +31,7 @@ SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D positi
SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS sensor position with respect to the car frame (base frame) SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS sensor position with respect to the car frame (base frame)
StateBlockPtr _bias_ptr, //GNSS sensor time bias StateBlockPtr _bias_ptr, //GNSS sensor time bias
IntrinsicsGnssPtr params, //sensor params ParamsSensorGnssPtr params, //sensor params
StateBlockPtr _t_ENU_MAP_ptr, //ENU_MAP translation StateBlockPtr _t_ENU_MAP_ptr, //ENU_MAP translation
StateBlockPtr _roll_ENU_MAP_ptr, //ENU_MAP Roll StateBlockPtr _roll_ENU_MAP_ptr, //ENU_MAP Roll
StateBlockPtr _pitch_ENU_MAP_ptr, //ENU_MAP Pitch StateBlockPtr _pitch_ENU_MAP_ptr, //ENU_MAP Pitch
...@@ -240,12 +240,12 @@ Eigen::Matrix3d SensorGnss::getREnuMap() const ...@@ -240,12 +240,12 @@ Eigen::Matrix3d SensorGnss::getREnuMap() const
// Define the factory method // Define the factory method
SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics, SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics,
const IntrinsicsBasePtr _intrinsics) const ParamsSensorBasePtr _intrinsics)
{ {
assert((_extrinsics.size() == 3 || _extrinsics.size() == 9) && "Bad extrinsics vector length. Should be 3 (antena position) or 9 (antena position and MAP-ENU initialization: position XYZ and orientation RPY)"); assert((_extrinsics.size() == 3 || _extrinsics.size() == 9) && "Bad extrinsics vector length. Should be 3 (antena position) or 9 (antena position and MAP-ENU initialization: position XYZ and orientation RPY)");
SensorBasePtr sen = nullptr; SensorBasePtr sen = nullptr;
IntrinsicsGnssPtr intrinsics_gnss_ptr = std::static_pointer_cast<IntrinsicsGnss>(_intrinsics); ParamsSensorGnssPtr intrinsics_gnss_ptr = std::static_pointer_cast<ParamsSensorGnss>(_intrinsics);
StateBlockPtr p_ptr = std::make_shared<StateBlock>(_extrinsics.head(3), intrinsics_gnss_ptr->extrinsics_fixed_); StateBlockPtr p_ptr = std::make_shared<StateBlock>(_extrinsics.head(3), intrinsics_gnss_ptr->extrinsics_fixed_);
if (_extrinsics.size() == 3) if (_extrinsics.size() == 3)
......
...@@ -88,7 +88,7 @@ Vector3d t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + ...@@ -88,7 +88,7 @@ Vector3d t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena +
// WOLF // WOLF
ProblemPtr problem_ptr = Problem::create("PO", 2); ProblemPtr problem_ptr = Problem::create("PO", 2);
CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr); CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
SensorGnssPtr gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<IntrinsicsGnss>())); SensorGnssPtr gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorGnss>()));
FrameBasePtr frame_ptr; FrameBasePtr frame_ptr;
//////////////////////////////////////////////////////// ////////////////////////////////////////////////////////
......
...@@ -64,7 +64,7 @@ class FactorGnssSingleDiff2DTest : public testing::Test ...@@ -64,7 +64,7 @@ class FactorGnssSingleDiff2DTest : public testing::Test
ceres_mgr_ptr->getSolverOptions().max_num_iterations = 10; ceres_mgr_ptr->getSolverOptions().max_num_iterations = 10;
// gnss sensor // gnss sensor
gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<IntrinsicsBase>())); gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorBase>()));
gnss_sensor_ptr->getEnuMapRoll()->fix(); gnss_sensor_ptr->getEnuMapRoll()->fix();
gnss_sensor_ptr->getEnuMapPitch()->fix(); gnss_sensor_ptr->getEnuMapPitch()->fix();
gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu)); gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
...@@ -80,7 +80,7 @@ class FactorGnssSingleDiff2DTest : public testing::Test ...@@ -80,7 +80,7 @@ class FactorGnssSingleDiff2DTest : public testing::Test
problem_ptr->installProcessor("ProcessorGnssSingleDiff", "gnss single difference", gnss_sensor_ptr, gnss_params_ptr); problem_ptr->installProcessor("ProcessorGnssSingleDiff", "gnss single difference", gnss_sensor_ptr, gnss_params_ptr);
// odom sensor & processor // odom sensor & processor
IntrinsicsOdom2DPtr odom_intrinsics_ptr = std::make_shared<IntrinsicsOdom2D>(); ParamsSensorOdom2DPtr odom_intrinsics_ptr = std::make_shared<ParamsSensorOdom2D>();
odom_intrinsics_ptr->k_disp_to_disp = 0.1; odom_intrinsics_ptr->k_disp_to_disp = 0.1;
odom_intrinsics_ptr->k_rot_to_rot = 0.1; odom_intrinsics_ptr->k_rot_to_rot = 0.1;
//<<<<<<< HEAD //<<<<<<< HEAD
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment