diff --git a/src/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp index 75d0c293951a09c467096297062d60ba1b0ce9f0..d8becd8210492f1d07cf3826d4b29bbaa28fb9d9 100644 --- a/src/examples/test_kf_callback.cpp +++ b/src/examples/test_kf_callback.cpp @@ -21,7 +21,10 @@ int main() ProblemPtr problem = Problem::create("PO 2D"); SensorBasePtr sen_odo = problem->installSensor ("ODOM 2D", "main odometer", (Vector3s()<<0,0,0).finished(),""); - ProcessorBasePtr prc_odo = problem->installProcessor("ODOM 2D", "odometry integrator", "main odometer", ""); + ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>(); + params_odo->elapsed_time_th_ = 2; + params_odo->theta_traveled_th_ = M_PI; // 180 degrees turn + ProcessorBasePtr prc_odo = problem->installProcessor("ODOM 2D", "odometry integrator", sen_odo, params_odo); prc_odo->setTimeTolerance(0.1); SensorBasePtr sen_ftr = problem->installSensor ("ODOM 2D", "other odometer", (Vector3s()<<0,0,0).finished(),""); @@ -34,29 +37,32 @@ int main() cout << "Motion processor : " << problem->getProcessorMotionPtr()->getName() << endl; TimeStamp t(0); + cout << "=======================\n>> TIME: " << t.get() << endl; Vector3s x({0,0,0}); - problem->getProcessorMotionPtr()->setOrigin(x, t); + Matrix3s P; P.setZero(); + problem->setPrior(x, P, t, 0.01); - cout << "x(0) = " << problem->getCurrentState().transpose() << endl; + cout << "x(" << t.get() << ") = " << problem->getCurrentState().transpose() << endl; - Vector2s odo_data; odo_data << .1, (M_PI / 2); + Vector2s odo_data; odo_data << .1, (M_PI / 10); - problem->print(2, false, true, false); // print(level, constr_by, metric, state_blocks) + problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks) Scalar dt = 1; for (auto i = 0; i < 4; i++) { - t += dt; - cout << "=======================\n>> TIME: " << t.get() << endl; cout << "Tracker----------------" << endl; sen_ftr->process(make_shared<CaptureVoid>(t, sen_ftr)); - problem->print(2, false, true, false); // print(level, constr_by, metric, state_blocks) + problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks) + + t += dt; + cout << "=======================\n>> TIME: " << t.get() << endl; cout << "Motion-----------------" << endl; sen_odo->process(make_shared<CaptureMotion>(t, sen_odo, odo_data, 3, 3, nullptr)); cout << "x(" << t.get() << ") = " << problem->getCurrentState().transpose() << endl; - problem->print(2, false, true, false); // print(level, constr_by, metric, state_blocks) + problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks) } diff --git a/src/frame_base.cpp b/src/frame_base.cpp index e5f42afb0981a19d3fd6c803c29fcbf565cb576e..92467b61d78ac4c681aa1aafef06cbe63f79baf5 100644 --- a/src/frame_base.cpp +++ b/src/frame_base.cpp @@ -136,7 +136,7 @@ void FrameBase::setKey() getTrajectoryPtr()->sortFrame(shared_from_this()); - WOLF_DEBUG("Set KF", this->id()); +// WOLF_DEBUG("Set KF", this->id()); } } diff --git a/src/processor_tracker_feature.cpp b/src/processor_tracker_feature.cpp index 9c1efa23330838108b68d07e4d0f0b7f95ebcab5..b411ff64832d968b8e82b30a69e4f3ce1270a351 100644 --- a/src/processor_tracker_feature.cpp +++ b/src/processor_tracker_feature.cpp @@ -33,7 +33,7 @@ unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_fe for (auto ftr : new_features_last_) { ftr->setTrackId( ftr->id() ); - WOLF_DEBUG("Det track: ", ftr->trackId(), ", last: ", ftr->id()); +// WOLF_DEBUG("Det track: ", ftr->trackId(), ", last: ", ftr->id()); } // Track new features from last to incoming. This will append new correspondences to matches_last_incoming @@ -43,7 +43,7 @@ unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_fe ftr->setTrackId(matches_last_from_incoming_[ftr]->feature_ptr_->trackId()); // Print new tracked features - WOLF_DEBUG("New track: ", ftr->trackId(), ", last: ", matches_last_from_incoming_[ftr]->feature_ptr_->id(), ", inc: ", ftr->id()); +// WOLF_DEBUG("New track: ", ftr->trackId(), ", last: ", matches_last_from_incoming_[ftr]->feature_ptr_->id(), ", inc: ", ftr->id()); } // Append all new Features to the incoming Captures' list of Features @@ -94,7 +94,7 @@ unsigned int ProcessorTrackerFeature::processKnown() // Print resulting list of matches for (auto match : matches_last_from_incoming_) { - WOLF_DEBUG("Known track: ", match.first->trackId(), ", last: ", match.second->feature_ptr_->id(), ", inc: ", match.first->id()); +// WOLF_DEBUG("Known track: ", match.first->trackId(), ", last: ", match.second->feature_ptr_->id(), ", inc: ", match.first->id()); } return matches_last_from_incoming_.size(); @@ -117,7 +117,7 @@ void ProcessorTrackerFeature::advanceDerived() // Print resulting list for (auto match: matches_origin_from_last_) { - WOLF_DEBUG("Matches advanced: track: ", match.first->trackId(), "-", match.second->feature_ptr_->trackId(), " origin: ", match.second->feature_ptr_->id(), " last: ", match.first->id()); +// WOLF_DEBUG("Matches advanced: track: ", match.first->trackId(), "-", match.second->feature_ptr_->trackId(), " origin: ", match.second->feature_ptr_->id(), " last: ", match.first->id()); } } @@ -133,7 +133,7 @@ void ProcessorTrackerFeature::resetDerived() // Print resulting list for (auto match: matches_origin_from_last_) { - WOLF_DEBUG("Matches reset: track: ", match.first->trackId(), "-", match.second->feature_ptr_->trackId(), " origin: ", match.second->feature_ptr_->id(), " last: ", match.first->id()); +// WOLF_DEBUG("Matches reset: track: ", match.first->trackId(), "-", match.second->feature_ptr_->trackId(), " origin: ", match.second->feature_ptr_->id(), " last: ", match.first->id()); } } @@ -147,9 +147,9 @@ void ProcessorTrackerFeature::establishConstraints() } for (auto match : matches_origin_from_last_) { - WOLF_DEBUG( "Constraint: track: " , match.second->feature_ptr_->trackId() , - " origin: " , match.second->feature_ptr_->id() , - " from last: " , match.first->id() ); +// WOLF_DEBUG( "Constraint: track: " , match.second->feature_ptr_->trackId() , +// " origin: " , match.second->feature_ptr_->id() , +// " from last: " , match.first->id() ); } } diff --git a/src/sensor_GPS.h b/src/sensor_GPS.h index b36d09ae0055c177d42f05b6d3fb7517a4e9fd0a..c43cd13cb8a766fd6cfea67000d16d4262ee5f2a 100644 --- a/src/sensor_GPS.h +++ b/src/sensor_GPS.h @@ -20,6 +20,7 @@ namespace wolf { struct IntrinsicsGPS : public IntrinsicsBase { // add GPS parameters here + virtual ~IntrinsicsGPS() = default; }; WOLF_PTR_TYPEDEFS(SensorGPS); diff --git a/src/sensor_GPS_fix.cpp b/src/sensor_GPS_fix.cpp index 956e3fb687352c20d609617e0089b6f42e299be7..b97e43c49a8ff74befcd77024e54a37100e64c21 100644 --- a/src/sensor_GPS_fix.cpp +++ b/src/sensor_GPS_fix.cpp @@ -10,6 +10,18 @@ SensorGPSFix::SensorGPSFix(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const dou // } +SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics) : + SensorBase("GPS FIX", std::make_shared<StateBlock>(_extrinsics.head(3)), std::make_shared<StateQuaternion>(_extrinsics.tail(4)), nullptr, _intrinsics.noise_std) +{ + assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D."); +} + +SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr) : SensorGPSFix(_extrinsics, *_intrinsics_ptr) +{ + // +} + + SensorGPSFix::~SensorGPSFix() { // diff --git a/src/sensor_GPS_fix.h b/src/sensor_GPS_fix.h index 9cb5583a025eaab3264faa8943aaafbb4c720fd6..3291aa538a7f7cd7a8fa838df8190a4b70ab4acb 100644 --- a/src/sensor_GPS_fix.h +++ b/src/sensor_GPS_fix.h @@ -12,9 +12,12 @@ namespace wolf { +WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsGPSFix); struct IntrinsicsGPSFix : public IntrinsicsBase { + Eigen::Vector3s noise_std; // Empty -- it acts only as a typedef for IntrinsicsBase, but allows future extension with parameters + virtual ~IntrinsicsGPSFix() = default; }; WOLF_PTR_TYPEDEFS(SensorGPSFix); @@ -31,6 +34,8 @@ class SensorGPSFix : public SensorBase * **/ SensorGPSFix(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _noise); + SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics); + SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr); virtual ~SensorGPSFix(); diff --git a/src/sensor_IMU.cpp b/src/sensor_IMU.cpp index 6e973746eafa75f0642e646e6d1edec9e33a0a49..ed8046ce84a2be818fc989136f621244f3525ed2 100644 --- a/src/sensor_IMU.cpp +++ b/src/sensor_IMU.cpp @@ -4,18 +4,42 @@ namespace wolf { -SensorIMU::SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsIMUPtr params) : - SensorBase("IMU", _p_ptr, _o_ptr, std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6s()<<params->a_noise,params->a_noise,params->a_noise,params->w_noise,params->w_noise,params->w_noise).finished(), false, true), - a_noise(params->a_noise), - w_noise(params->w_noise), - ab_initial_stdev(params->ab_initial_stdev), - wb_initial_stdev(params->wb_initial_stdev), - ab_rate_stdev(params->ab_rate_stdev), - wb_rate_stdev(params->wb_rate_stdev) +SensorIMU::SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsIMU& _params) : + SensorBase("IMU", _p_ptr, _o_ptr, std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6s()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, true), + a_noise(_params.a_noise), + w_noise(_params.w_noise), + ab_initial_stdev(_params.ab_initial_stdev), + wb_initial_stdev(_params.wb_initial_stdev), + ab_rate_stdev(_params.ab_rate_stdev), + wb_rate_stdev(_params.wb_rate_stdev) { // } +SensorIMU::SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsIMUPtr _params) : + SensorIMU(_p_ptr, _o_ptr, *_params) +{ + // +} + +SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params) : + SensorIMU(_extrinsics, *_params) +{ + // +} + +SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params) : + SensorBase("IMU", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6s()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, true), + a_noise(_params.a_noise), + w_noise(_params.w_noise), + ab_initial_stdev(_params.ab_initial_stdev), + wb_initial_stdev(_params.wb_initial_stdev), + ab_rate_stdev(_params.ab_rate_stdev), + wb_rate_stdev(_params.wb_rate_stdev) +{ + assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D."); +} + SensorIMU::~SensorIMU() { @@ -29,11 +53,8 @@ SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::Ve // decode extrinsics vector assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D."); - StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_pq.head(3), true); - StateBlockPtr ori_ptr = std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true); - IntrinsicsIMUPtr params = std::static_pointer_cast<IntrinsicsIMU>(_intrinsics); - SensorIMUPtr sen = std::make_shared<SensorIMU>(pos_ptr, ori_ptr, params); + SensorIMUPtr sen = std::make_shared<SensorIMU>(_extrinsics_pq, params); sen->setName(_unique_name); return sen; } diff --git a/src/sensor_IMU.h b/src/sensor_IMU.h index 823c18507efe6976f002d310f6edf9c89de2b4da..0b34016e29025f9ddc43a07582924988095e01a1 100644 --- a/src/sensor_IMU.h +++ b/src/sensor_IMU.h @@ -15,25 +15,18 @@ WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsIMU); struct IntrinsicsIMU : public IntrinsicsBase { //noise std dev - wolf::Scalar w_noise; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s) - wolf::Scalar a_noise; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s) + wolf::Scalar w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s) + wolf::Scalar a_noise = 0.04; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s) //Initial biases std dev - wolf::Scalar ab_initial_stdev; //accelerometer micro_g/sec - wolf::Scalar wb_initial_stdev; //gyroscope rad/sec + wolf::Scalar ab_initial_stdev = 0.01; //accelerometer micro_g/sec + wolf::Scalar wb_initial_stdev = 0.01; //gyroscope rad/sec // bias rate of change std dev - Scalar ab_rate_stdev; - Scalar wb_rate_stdev; - - IntrinsicsIMU() : - w_noise(0.001), - a_noise(0.04), - ab_initial_stdev(0.00001), - wb_initial_stdev(0.00001), - ab_rate_stdev(0.00001), - wb_rate_stdev(0.00001) - {} + Scalar ab_rate_stdev = 0.00001; + Scalar wb_rate_stdev = 0.00001; + + virtual ~IntrinsicsIMU() = default; }; WOLF_PTR_TYPEDEFS(SensorIMU); @@ -63,6 +56,9 @@ class SensorIMU : public SensorBase * **/ SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsIMUPtr _params); + SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsIMU& _params); + SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params); + SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params); Scalar getGyroNoise() const; Scalar getAccelNoise() const; diff --git a/src/sensor_base.h b/src/sensor_base.h index 3a2de9957d572026fb637f0b1d19932974171686..a1befcf792bcb81cca58d972ff1ea33bf4e2b9df 100644 --- a/src/sensor_base.h +++ b/src/sensor_base.h @@ -24,11 +24,10 @@ namespace wolf { */ struct IntrinsicsBase { - IntrinsicsBase() = default; - virtual ~IntrinsicsBase() = default; + virtual ~IntrinsicsBase() = default; - std::string type; - std::string name; + std::string type; + std::string name; }; class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBase> diff --git a/src/sensor_camera.cpp b/src/sensor_camera.cpp index 36e6c9ad53f3d19c37560eeb9a756398acbab736..7a9a9a2c7904b0ea47143c96a4d0bdd1b36c8c50 100644 --- a/src/sensor_camera.cpp +++ b/src/sensor_camera.cpp @@ -15,18 +15,23 @@ SensorCamera::SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBloc // } -SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const std::shared_ptr<IntrinsicsCamera> _intrinsics_ptr) : - SensorBase("CAMERA", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(_intrinsics_ptr->pinhole_model, true), 1), - img_width_(_intrinsics_ptr->width), // - img_height_(_intrinsics_ptr->height), // - distortion_(_intrinsics_ptr->distortion), // +SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const IntrinsicsCamera& _intrinsics) : + SensorBase("CAMERA", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(_intrinsics.pinhole_model, true), 1), + img_width_(_intrinsics.width), // + img_height_(_intrinsics.height), // + distortion_(_intrinsics.distortion), // correction_(distortion_.size()+2) // make correction vector of the same size as distortion vector { assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3D"); - K_ = setIntrinsicMatrix(_intrinsics_ptr->pinhole_model); + K_ = setIntrinsicMatrix(_intrinsics.pinhole_model); pinhole::computeCorrectionModel(getIntrinsicPtr()->getState(), distortion_, correction_); } +SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr) : + SensorCamera(_extrinsics, *_intrinsics_ptr) +{ + // +} SensorCamera::~SensorCamera() { @@ -58,13 +63,6 @@ SensorBasePtr SensorCamera::create(const std::string& _unique_name, // SensorCameraPtr sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr); sen_ptr->setName(_unique_name); -// std::cout << "Created camera:\n\tintrinsics : " << sen_ptr->getIntrinsicPtr()->getState().transpose() << std::endl; -// std::cout << "\tintrinsic matrix : " << K_ << std::endl; -// std::cout << "\tdistortion : " << distortion_.transpose() << std::endl; -// std::cout << "\tcorrection : " << correction_.transpose() << std::endl; -// std::cout << "\tposition : " << sen_ptr->getPPtr()->getState().transpose() << std::endl; -// std::cout << "\torientation : " << sen_ptr->getOPtr()->getState().transpose() << std::endl; - return sen_ptr; } diff --git a/src/sensor_camera.h b/src/sensor_camera.h index 7c61c98e2a04f5bd437820b6e3f5d17b79f3b8cb..8b5ec2035452d2b9ec21ec6056d00654731466f3 100644 --- a/src/sensor_camera.h +++ b/src/sensor_camera.h @@ -7,6 +7,7 @@ namespace wolf { +WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsCamera); /** Struct of intrinsic camera parameters */ struct IntrinsicsCamera : public IntrinsicsBase @@ -15,6 +16,8 @@ struct IntrinsicsCamera : public IntrinsicsBase unsigned int height; ///< Image height in pixels Eigen::Vector4s pinhole_model; ///< k = [u_0, v_0, alpha_u, alpha_v] vector of pinhole intrinsic parameters Eigen::VectorXs distortion; ///< d = [d_1, d_2, d_3, ...] radial distortion coefficients + + virtual ~IntrinsicsCamera() = default; }; WOLF_PTR_TYPEDEFS(SensorCamera); @@ -36,7 +39,8 @@ class SensorCamera : public SensorBase **/ SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, int _img_width, int _img_height); - SensorCamera(const Eigen::VectorXs & _extrinsics, const std::shared_ptr<IntrinsicsCamera> _intrinsics_ptr); + SensorCamera(const Eigen::VectorXs & _extrinsics, const IntrinsicsCamera& _intrinsics); + SensorCamera(const Eigen::VectorXs & _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr); virtual ~SensorCamera(); diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp index 738dc3e6b069b80fb86e3f8915c9f3d249659e05..66239d0d0e65d159f2fd3c12dc4e0bad5319ed5d 100644 --- a/src/sensor_laser_2D.cpp +++ b/src/sensor_laser_2D.cpp @@ -23,6 +23,19 @@ SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const l // } +SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params) : + SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8), + scan_params_(_params.scan_params) +{ + // +} + +SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params) : + SensorLaser2D(_p_ptr, _o_ptr, *_params) +{ + // +} + SensorLaser2D::~SensorLaser2D() { // diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h index b1e3e9b2af5ed8fbe33a354264e018493178cddb..9cdcf5567499c2993d67f53535f0e1d34ed2682d 100644 --- a/src/sensor_laser_2D.h +++ b/src/sensor_laser_2D.h @@ -17,6 +17,8 @@ WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsLaser2D); struct IntrinsicsLaser2D : public IntrinsicsBase { + virtual ~IntrinsicsLaser2D() = default; + laserscanutils::LaserScanParams scan_params; }; @@ -52,6 +54,8 @@ class SensorLaser2D : public SensorBase * **/ SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const laserscanutils::LaserScanParams& _params); + SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params); + SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params); virtual ~SensorLaser2D(); diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp index d720c4e66f9df27ee82e67129ea26ba64f1b6954..fdb7ab4804809072cada12774e8af9a08ed9c3f4 100644 --- a/src/sensor_odom_2D.cpp +++ b/src/sensor_odom_2D.cpp @@ -12,6 +12,22 @@ SensorOdom2D::SensorOdom2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Sca // } +SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics) : + SensorBase("ODOM 2D", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2), + k_disp_to_disp_(_intrinsics.k_disp_to_disp), + k_rot_to_rot_(_intrinsics.k_rot_to_rot) +{ + assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2D."); + // +} + +SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics) : + SensorOdom2D(_extrinsics, *_intrinsics) +{ + // +} + + SensorOdom2D::~SensorOdom2D() { // @@ -33,17 +49,20 @@ SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen: { // decode extrinsics vector assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D."); - StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true); - StateBlockPtr ori_ptr = std::make_shared<StateAngle>(_extrinsics_po(2), true); - // cast intrinsics into derived type + SensorOdom2DPtr odo; if (_intrinsics) { std::shared_ptr<IntrinsicsOdom2D> params = std::static_pointer_cast<IntrinsicsOdom2D>(_intrinsics); - odo = std::make_shared<SensorOdom2D>(pos_ptr, ori_ptr, params->k_disp_to_disp, params->k_rot_to_rot); + odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params); } else - odo = std::make_shared<SensorOdom2D>(pos_ptr, ori_ptr, 1, 1); + { + IntrinsicsOdom2D params; + params.k_disp_to_disp = 1; + params.k_rot_to_rot = 1; + odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params); + } odo->setName(_unique_name); return odo; } diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h index ad3b02ae557b023be9c4b279d106ce650d299a2f..4ba0fc37dee5a0d4e487548c894f32fff4fc2a86 100644 --- a/src/sensor_odom_2D.h +++ b/src/sensor_odom_2D.h @@ -7,10 +7,12 @@ namespace wolf { -WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsOdom2D) +WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsOdom2D); struct IntrinsicsOdom2D : public IntrinsicsBase { + virtual ~IntrinsicsOdom2D() = default; + Scalar k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation Scalar k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation }; @@ -33,7 +35,9 @@ class SensorOdom2D : public SensorBase * \param _rot_noise_factor rotation noise factor * **/ - SensorOdom2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _disp_noise_factor, const Scalar& _rot_noise_factor); + SensorOdom2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _disp_noise_factor, const Scalar& _rot_noise_factor); + SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics); + SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics); virtual ~SensorOdom2D(); diff --git a/src/sensor_odom_3D.cpp b/src/sensor_odom_3D.cpp index 21acafbbb484f81cbedf94376e370e6504fa9b5b..36213d9e69b39d1e09c42804b84d3ed4eb060d2e 100644 --- a/src/sensor_odom_3D.cpp +++ b/src/sensor_odom_3D.cpp @@ -24,23 +24,46 @@ SensorOdom3D::SensorOdom3D(StateBlockPtr _p_ptr, StateQuaternionPtr _o_ptr, Intr setNoiseCov(noise_cov_); // sets also noise_std_ } +SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsOdom3D& _intrinsics) : + SensorBase("ODOM 3D", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6), + k_disp_to_disp_(_intrinsics.k_disp_to_disp), + k_disp_to_rot_(_intrinsics.k_disp_to_rot), + k_rot_to_rot_(_intrinsics.k_rot_to_rot), + min_disp_var_(_intrinsics.min_disp_var), + min_rot_var_(_intrinsics.min_rot_var) +{ + assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3D."); + + noise_cov_ = (Eigen::Vector6s() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal(); + setNoiseCov(noise_cov_); // sets also noise_std_ +} + +SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom3DPtr _intrinsics) : + SensorOdom3D(_extrinsics_pq, *_intrinsics) +{ + // +} + + SensorOdom3D::~SensorOdom3D() { // } // Define the factory method -SensorBasePtr SensorOdom3D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po, +SensorBasePtr SensorOdom3D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics) { // decode extrinsics vector - assert(_extrinsics_po.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D."); - StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(3), true); - StateQuaternionPtr ori_ptr = std::make_shared<StateQuaternion>(_extrinsics_po.tail(4), true); + assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D."); + // cast intrinsics into derived type IntrinsicsOdom3DPtr params = std::static_pointer_cast<IntrinsicsOdom3D>(_intrinsics); - SensorBasePtr odo = std::make_shared<SensorOdom3D>(pos_ptr, ori_ptr, params); + + // Call constructor and finish + SensorBasePtr odo = std::make_shared<SensorOdom3D>(_extrinsics_pq, params); odo->setName(_unique_name); + return odo; } diff --git a/src/sensor_odom_3D.h b/src/sensor_odom_3D.h index c676edc95a66756a7d28b68299127dbd09a60d30..80575937c8eef5cbab637531f1c508371b8e29e3 100644 --- a/src/sensor_odom_3D.h +++ b/src/sensor_odom_3D.h @@ -23,13 +23,8 @@ struct IntrinsicsOdom3D : public IntrinsicsBase Scalar min_disp_var; Scalar min_rot_var; - IntrinsicsOdom3D() : - k_disp_to_disp(0), - k_disp_to_rot(0), - k_rot_to_rot(0), - min_disp_var(0), - min_rot_var(0) - {} + virtual ~IntrinsicsOdom3D() = default; + }; WOLF_PTR_TYPEDEFS(SensorOdom3D); @@ -52,6 +47,8 @@ class SensorOdom3D : public SensorBase * \param _params shared_ptr to a struct with parameters **/ SensorOdom3D(StateBlockPtr _p_ptr, StateQuaternionPtr _q_ptr, IntrinsicsOdom3DPtr params); + SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsOdom3D& params); + SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom3DPtr params); virtual ~SensorOdom3D(); diff --git a/src/sensors/sensor_diff_drive.h b/src/sensors/sensor_diff_drive.h index 334fea3895676f923f84625997caf79b4637bafc..f4556a84a16c7a347f27a6f7f51ce913c02dc477 100644 --- a/src/sensors/sensor_diff_drive.h +++ b/src/sensors/sensor_diff_drive.h @@ -16,30 +16,6 @@ namespace wolf { struct IntrinsicsDiffDrive : public IntrinsicsBase { - IntrinsicsDiffDrive() = default; - - IntrinsicsDiffDrive(const Scalar _left_radius, - const Scalar _right_radius, - const Scalar _separation, - const DiffDriveModel _model, - const Eigen::VectorXs& _factors, - const Scalar _left_resolution, - const Scalar _right_resolution, - const Scalar _left_gain, - const Scalar _right_gain) : - left_radius_(_left_radius), - right_radius_(_right_radius), - separation_(_separation), - model_(_model), - factors_(_factors), - left_resolution_(_left_resolution), - right_resolution_(_right_resolution), - left_gain_(_left_gain), - right_gain_(_right_gain) - { - // - } - Scalar left_radius_; Scalar right_radius_; Scalar separation_; @@ -53,6 +29,8 @@ struct IntrinsicsDiffDrive : public IntrinsicsBase Scalar left_gain_ = 0.01; Scalar right_gain_ = 0.01; + + virtual ~IntrinsicsDiffDrive() = default; }; typedef std::shared_ptr<IntrinsicsDiffDrive> IntrinsicsDiffDrivePtr;