diff --git a/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp index 3b2b68c18cf8b8c347ce78037e38d00d4f990406..cde8790298ad7e3709c1e2dc2a0ecd16f1c398a1 100644 --- a/src/examples/test_wolf_factories.cpp +++ b/src/examples/test_wolf_factories.cpp @@ -65,7 +65,7 @@ int main(void) IntrinsicsBasePtr intr_cam_ptr = IntrinsicsFactory::get().create("CAMERA", wolf_config + "/camera_params_ueye_sim.yaml"); ProcessorParamsBasePtr params_ptr = ProcessorParamsFactory::get().create("IMAGE FEATURE", wolf_config + "/processor_image_feature.yaml"); - cout << "CAMERA with intrinsics : " << (static_pointer_cast<IntrinsicsCamera>(intr_cam_ptr))->pinhole_model.transpose() << endl; + cout << "CAMERA with intrinsics : " << (static_pointer_cast<IntrinsicsCamera>(intr_cam_ptr))->pinhole_model_raw.transpose() << endl; // cout << "Processor IMAGE image width : " << (static_pointer_cast<ProcessorParamsImage>(params_ptr))->image.width << endl; cout << "\n==================== Install Sensors ====================" << endl; diff --git a/src/problem.cpp b/src/problem.cpp index 658bd033dad8e177330dfededc49fb4fbe259d90..2b7ac27841a195836a492cd83462aed4e37313a1 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -4,17 +4,14 @@ #include "trajectory_base.h" #include "map_base.h" #include "sensor_base.h" -#include "factory.h" +#include "processor_motion.h" +#include "processor_tracker.h" +#include "capture_pose.h" +#include "constraint_base.h" #include "sensor_factory.h" #include "processor_factory.h" -#include "constraint_base.h" #include "state_block.h" -#include "processor_motion.h" -#include "sensor_GPS.h" -#include "processor_tracker.h" -//#include "processors/processor_tracker_feature_trifocal.h" -#include "capture_pose.h" // IRI libs includes diff --git a/src/processor_base.cpp b/src/processor_base.cpp index ea4bdc4e82e1edbcb83c9500656f0b0d05de6ab8..90943540e929a22bf24c12d55ca89329b5f3c935 100644 --- a/src/processor_base.cpp +++ b/src/processor_base.cpp @@ -8,7 +8,7 @@ namespace wolf { unsigned int ProcessorBase::processor_id_count_ = 0; ProcessorBase::ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _params) : - NodeBase("PROCESSOR", _type, _params->name), + NodeBase("PROCESSOR", _type), processor_id_(++processor_id_count_), params_(_params), sensor_ptr_() diff --git a/src/processor_base.h b/src/processor_base.h index bcf5c46b62730d593165f99fdb8d021296964812..2b996e17f3e58bacf694f8485f71444bb96cafd5 100644 --- a/src/processor_base.h +++ b/src/processor_base.h @@ -112,13 +112,9 @@ struct ProcessorParamsBase ProcessorParamsBase() = default; ProcessorParamsBase(bool _voting_active, - Scalar _time_tolerance, - const std::string& _type, - const std::string& _name) + Scalar _time_tolerance) : voting_active(_voting_active) , time_tolerance(_time_tolerance) - , type(_type) - , name(_name) { // } @@ -131,9 +127,6 @@ struct ProcessorParamsBase /// a particular Capture of this processor to allow assigning /// this Capture to the Keyframe. Scalar time_tolerance = Scalar(0); - - std::string type; - std::string name; }; //class ProcessorBase diff --git a/src/processors/processor_tracker_feature_trifocal.cpp b/src/processors/processor_tracker_feature_trifocal.cpp index 340f4ec153ff972be60b44731e7f6feddb23c348..162e42b23da84ed9ce9f899c3510e8eb03960b1d 100644 --- a/src/processors/processor_tracker_feature_trifocal.cpp +++ b/src/processors/processor_tracker_feature_trifocal.cpp @@ -39,7 +39,6 @@ ProcessorTrackerFeatureTrifocal::ProcessorTrackerFeatureTrifocal(ProcessorParams prev_origin_ptr_(nullptr), initialized_(false) { - setName(_params_tracker_feature_trifocal->name); assert(!(params_tracker_feature_trifocal_->yaml_file_params_vision_utils.empty()) && "Missing YAML file with vision_utils parameters!"); assert(file_exists(params_tracker_feature_trifocal_->yaml_file_params_vision_utils) && "Cannot setup processor: vision_utils' YAML file does not exist."); diff --git a/src/sensor_GPS_fix.cpp b/src/sensor_GPS_fix.cpp index fa1f46eb16bbeef20789cfdaf7165433910e21d9..311ec92924f34554cce36398b05eea2e1d680fa2 100644 --- a/src/sensor_GPS_fix.cpp +++ b/src/sensor_GPS_fix.cpp @@ -4,12 +4,6 @@ namespace wolf { -SensorGPSFix::SensorGPSFix(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _noise) : - SensorBase("GPS FIX", _p_ptr, _o_ptr, nullptr, Eigen::VectorXs::Constant(1,_noise)) -{ - // -} - SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics) : SensorBase("GPS FIX", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), nullptr, _intrinsics.noise_std) { @@ -17,7 +11,8 @@ SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, const Intrinsics && "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D."); } -SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr) : SensorGPSFix(_extrinsics, *_intrinsics_ptr) +SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr) : + SensorGPSFix(_extrinsics, *_intrinsics_ptr) { // } @@ -34,8 +29,8 @@ SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen: { assert((_extrinsics.size() == 2 || _extrinsics.size() == 3) && "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D."); - StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics, true); - SensorGPSFixPtr sen = std::make_shared<SensorGPSFix>(pos_ptr, nullptr, 0); + SensorGPSFixPtr sen = std::make_shared<SensorGPSFix>(_extrinsics, std::static_pointer_cast<IntrinsicsGPSFix>(_intrinsics)); + sen->getPPtr()->fix(); sen->setName(_unique_name); return sen; } diff --git a/src/sensor_GPS_fix.h b/src/sensor_GPS_fix.h index bef5bb861629a5c3c8894488f2fe902aa18f5cee..9d5bdb01ac5a0bfd826ad536acd149c3905ef7f6 100644 --- a/src/sensor_GPS_fix.h +++ b/src/sensor_GPS_fix.h @@ -26,15 +26,6 @@ WOLF_PTR_TYPEDEFS(SensorGPSFix); class SensorGPSFix : public SensorBase { public: - /** \brief Constructor with arguments - * - * Constructor with arguments - * \param _p_ptr StateBlock pointer to the sensor position - * \param _o_ptr StateBlock pointer to the sensor orientation - * \param _noise noise standard deviation - * - **/ - 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); diff --git a/src/sensor_IMU.cpp b/src/sensor_IMU.cpp index 9e5da5f0ddba954357e7c58492a82947423ff5af..0189ca2d2e8d4dd84aab43ec656a9f4e95a29b36 100644 --- a/src/sensor_IMU.cpp +++ b/src/sensor_IMU.cpp @@ -4,24 +4,6 @@ namespace wolf { -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) { diff --git a/src/sensor_IMU.h b/src/sensor_IMU.h index 2f5d9baa576f697772800359a46a239170f39144..e809d79fffc923f731a4e477e49dfe292665cb9b 100644 --- a/src/sensor_IMU.h +++ b/src/sensor_IMU.h @@ -46,17 +46,6 @@ class SensorIMU : public SensorBase public: - /** \brief Constructor with arguments - * - * Constructor with arguments - * \param _p_ptr StateBlock pointer to the sensor position wrt vehicle base - * \param _o_ptr StateBlock pointer to the sensor orientation wrt vehicle base - * \param params IntrinsicsIMU pointer to sensor properties - * \param _a_w_biases_ptr StateBlock pointer to the vector of acc and gyro biases - * - **/ - 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); diff --git a/src/sensor_base.h b/src/sensor_base.h index f01c8ce0b533ad838b33e9a980e92b80bc691d08..992058512a2bd6d29a253653a988ccbe932c8ad1 100644 --- a/src/sensor_base.h +++ b/src/sensor_base.h @@ -25,9 +25,6 @@ namespace wolf { struct IntrinsicsBase { virtual ~IntrinsicsBase() = default; - - 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 7a9a9a2c7904b0ea47143c96a4d0bdd1b36c8c50..403ba8dd86676848c1a3ce3148782ccd773f5197 100644 --- a/src/sensor_camera.cpp +++ b/src/sensor_camera.cpp @@ -7,23 +7,18 @@ namespace wolf { -SensorCamera::SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, // - int _img_width, int _img_height) : - SensorBase("CAMERA", _p_ptr, _o_ptr, _intr_ptr, 2), // - img_width_(_img_width), img_height_(_img_height) -{ - // -} - 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), + 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_raw, 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 + correction_(distortion_.size() + 1), // make correction vector slightly larger in size than the distortion vector + pinhole_model_raw_(_intrinsics.pinhole_model_raw), // + pinhole_model_rectified_(_intrinsics.pinhole_model_rectified), // + using_raw_(true) { assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3D"); - K_ = setIntrinsicMatrix(_intrinsics.pinhole_model); + useRawImages(); pinhole::computeCorrectionModel(getIntrinsicPtr()->getState(), distortion_, correction_); } @@ -71,7 +66,6 @@ SensorBasePtr SensorCamera::create(const std::string& _unique_name, // // Register in the SensorFactory #include "sensor_factory.h" -//#include "factory.h" namespace wolf { WOLF_REGISTER_SENSOR("CAMERA", SensorCamera) diff --git a/src/sensor_camera.h b/src/sensor_camera.h index d8641510db8225e874e06526665199e8bc316d2f..9bdee32534c2d9b752bc6a988c6d4d0b76a0f7f9 100644 --- a/src/sensor_camera.h +++ b/src/sensor_camera.h @@ -12,10 +12,11 @@ WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsCamera); */ struct IntrinsicsCamera : public IntrinsicsBase { - unsigned int width; ///< Image width in pixels - 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 + unsigned int width; ///< Image width in pixels + unsigned int height; ///< Image height in pixels + Eigen::Vector4s pinhole_model_raw; ///< k = [u_0, v_0, alpha_u, alpha_v] vector of pinhole intrinsic parameters + Eigen::Vector4s pinhole_model_rectified;///< 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; }; @@ -27,26 +28,20 @@ WOLF_PTR_TYPEDEFS(SensorCamera); class SensorCamera : public SensorBase { public: - /** \brief Constructor with arguments - * - * Constructor with arguments - * \param _p_ptr StateBlock pointer to the sensor position wrt vehicle base - * \param _o_ptr StateBlock pointer to the sensor orientation wrt vehicle base - * \param _intr_ptr contains intrinsic values for the camera - * \param _img_width image height in pixels - * \param _img_height image width in pixels - * - **/ - SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, int _img_width, int _img_height); SensorCamera(const Eigen::VectorXs & _extrinsics, const IntrinsicsCamera& _intrinsics); SensorCamera(const Eigen::VectorXs & _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr); virtual ~SensorCamera(); - Eigen::VectorXs getDistortionVector(){return distortion_;} - Eigen::VectorXs getCorrectionVector(){return correction_;} - Eigen::Matrix3s getIntrinsicMatrix() {return K_;} + Eigen::VectorXs getDistortionVector() { return distortion_; } + Eigen::VectorXs getCorrectionVector() { return correction_; } + Eigen::Matrix3s getIntrinsicMatrix() { return K_; } + + bool isUsingRawImages() { return using_raw_; } + bool useRawImages(); + bool useRectifiedImages(); + int getImgWidth(){return img_width_;} int getImgHeight(){return img_height_;} @@ -58,7 +53,9 @@ class SensorCamera : public SensorBase int img_height_; Eigen::VectorXs distortion_; Eigen::VectorXs correction_; + Eigen::Vector4s pinhole_model_raw_, pinhole_model_rectified_; Eigen::Matrix3s K_; + bool using_raw_; virtual Eigen::Matrix3s setIntrinsicMatrix(Eigen::Vector4s _pinhole_model); @@ -71,6 +68,24 @@ class SensorCamera : public SensorBase }; +inline bool SensorCamera::useRawImages() +{ + getIntrinsicPtr()->setState(pinhole_model_raw_); + K_ = setIntrinsicMatrix(pinhole_model_raw_); + using_raw_ = true; + + return true; +} + +inline bool SensorCamera::useRectifiedImages() +{ + getIntrinsicPtr()->setState(pinhole_model_rectified_); + K_ = setIntrinsicMatrix(pinhole_model_rectified_); + using_raw_ = false; + + return true; +} + } // namespace wolf #endif // SENSOR_CAMERA_H diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp index fdb7ab4804809072cada12774e8af9a08ed9c3f4..7a03676337872f829653d6cd56f7daaf0e5446fd 100644 --- a/src/sensor_odom_2D.cpp +++ b/src/sensor_odom_2D.cpp @@ -4,14 +4,6 @@ namespace wolf { -SensorOdom2D::SensorOdom2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _disp_noise_factor, const Scalar& _rot_noise_factor) : - SensorBase("ODOM 2D", _p_ptr, _o_ptr, nullptr, 2), - k_disp_to_disp_(_disp_noise_factor), - k_rot_to_rot_(_rot_noise_factor) -{ - // -} - 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), diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h index 4ba0fc37dee5a0d4e487548c894f32fff4fc2a86..f66b9eea4b643f65eca471ef82906e1d0c1ad4e4 100644 --- a/src/sensor_odom_2D.h +++ b/src/sensor_odom_2D.h @@ -26,16 +26,6 @@ class SensorOdom2D : public SensorBase Scalar k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation public: - /** \brief Constructor with arguments - * - * Constructor with arguments - * \param _p_ptr StateBlock pointer to the sensor position wrt vehicle base - * \param _o_ptr StateBlock pointer to the sensor orientation wrt vehicle base - * \param _disp_noise_factor displacement noise factor - * \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(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics); SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics); diff --git a/src/sensor_odom_3D.cpp b/src/sensor_odom_3D.cpp index c5f3fb388c1ab12635dd8bd0d64bbc506fab3207..5cf426b294ad2241a0cace34ede0b14b8e1080fd 100644 --- a/src/sensor_odom_3D.cpp +++ b/src/sensor_odom_3D.cpp @@ -12,18 +12,6 @@ namespace wolf { -SensorOdom3D::SensorOdom3D(StateBlockPtr _p_ptr, StateQuaternionPtr _o_ptr, IntrinsicsOdom3DPtr _params) : - SensorBase("ODOM 3D", _p_ptr, _o_ptr, nullptr, 6), - k_disp_to_disp_(_params->k_disp_to_disp), - k_disp_to_rot_(_params->k_disp_to_rot), - k_rot_to_rot_(_params->k_rot_to_rot), - min_disp_var_(_params->min_disp_var), - min_rot_var_(_params->min_rot_var) -{ - 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, 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), diff --git a/src/sensor_odom_3D.h b/src/sensor_odom_3D.h index 19f037162677691f08b2deff390c42b49ab706fd..b366b99884997fe3cd73a132f435e53086b131db 100644 --- a/src/sensor_odom_3D.h +++ b/src/sensor_odom_3D.h @@ -38,14 +38,6 @@ class SensorOdom3D : public SensorBase Scalar min_rot_var_; public: - /** \brief Constructor with arguments - * - * Constructor with arguments - * \param _p_ptr StateBlock pointer to the sensor position wrt vehicle base - * \param _o_ptr StateBlock pointer to the sensor orientation wrt vehicle base - * \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); diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index bfa53c590ea89d99b569acdd088f93356b796ba8..484793812a146123926cfcc6388184c8b822743a 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -102,6 +102,10 @@ wolf_add_gtest(gtest_SE3 gtest_SE3.cpp) wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp) target_link_libraries(gtest_sensor_base ${PROJECT_NAME}) +# shared_from_this test +wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp) +target_link_libraries(gtest_shared_from_this ${PROJECT_NAME}) + # SolverManager test wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp) target_link_libraries(gtest_solver_manager ${PROJECT_NAME}) @@ -210,9 +214,10 @@ target_link_libraries(gtest_odom_2D ${PROJECT_NAME}) wolf_add_gtest(gtest_odom_3D gtest_odom_3D.cpp) target_link_libraries(gtest_odom_3D ${PROJECT_NAME}) -# shared_from_this test -wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp) -target_link_libraries(gtest_shared_from_this ${PROJECT_NAME}) +# SensorBase test +wolf_add_gtest(gtest_sensor_camera gtest_sensor_camera.cpp) +target_link_libraries(gtest_sensor_camera ${PROJECT_NAME}) + # ------- Now Core classes Serialization ---------- diff --git a/src/test/gtest_constraint_autodiff.cpp b/src/test/gtest_constraint_autodiff.cpp index c5820537f54d0aaf8e251cc4867a88b82f1d0e33..37754df0a2dfcc492195653699ea058949d16c8d 100644 --- a/src/test/gtest_constraint_autodiff.cpp +++ b/src/test/gtest_constraint_autodiff.cpp @@ -24,7 +24,10 @@ TEST(CaptureAutodiff, ConstructorOdom2d) FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1))); // SENSOR - SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1); + IntrinsicsOdom2D intrinsics_odo; + intrinsics_odo.k_disp_to_disp = 0.1; + intrinsics_odo.k_rot_to_rot = 0.1; + SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); // CAPTURE CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); @@ -56,7 +59,10 @@ TEST(CaptureAutodiff, ResidualOdom2d) FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); // SENSOR - SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1); + IntrinsicsOdom2D intrinsics_odo; + intrinsics_odo.k_disp_to_disp = 0.1; + intrinsics_odo.k_rot_to_rot = 0.1; + SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); // CAPTURE CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); @@ -100,7 +106,10 @@ TEST(CaptureAutodiff, JacobianOdom2d) FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); // SENSOR - SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1); + IntrinsicsOdom2D intrinsics_odo; + intrinsics_odo.k_disp_to_disp = 0.1; + intrinsics_odo.k_rot_to_rot = 0.1; + SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); // CAPTURE CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); @@ -179,7 +188,10 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic) FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); // SENSOR - SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1); + IntrinsicsOdom2D intrinsics_odo; + intrinsics_odo.k_disp_to_disp = 0.1; + intrinsics_odo.k_rot_to_rot = 0.1; + SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); // CAPTURE CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr); diff --git a/src/test/gtest_constraint_gnss_fix_2D.cpp b/src/test/gtest_constraint_gnss_fix_2D.cpp index 5c1d3201d10bd410bf2ec53b2edd106cde09cb43..5900482eeabfe29b9cbe64bfe334b3726a3c7f20 100644 --- a/src/test/gtest_constraint_gnss_fix_2D.cpp +++ b/src/test/gtest_constraint_gnss_fix_2D.cpp @@ -75,8 +75,6 @@ class ConstraintGnssFix2DTest : public testing::Test gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu)); std::cout << "gnss sensor installed" << std::endl; ProcessorParamsGnssFixPtr gnss_params_ptr = std::make_shared<ProcessorParamsGnssFix>(); - gnss_params_ptr->name = "cool GNSS fix processor"; - gnss_params_ptr->type = "GNSS FIX"; gnss_params_ptr->time_tolerance = -1.0; gnss_params_ptr->voting_active = true; problem_ptr->installProcessor("GNSS FIX", "gnss fix", gnss_sensor_ptr, gnss_params_ptr); diff --git a/src/test/gtest_constraint_gnss_single_diff_2D.cpp b/src/test/gtest_constraint_gnss_single_diff_2D.cpp index 19eaf5ae192372c9865a2b8e8d3251bd49acdd59..743b8506ee399ba8e5d91fd42bfab423c56b8d8b 100644 --- a/src/test/gtest_constraint_gnss_single_diff_2D.cpp +++ b/src/test/gtest_constraint_gnss_single_diff_2D.cpp @@ -71,7 +71,7 @@ class ConstraintGnssSingleDiff2DTest : public testing::Test std::cout << "gnss sensor installed" << std::endl; // gnss processor - ProcessorParamsBasePtr gnss_params_ptr = std::make_shared<ProcessorParamsBase>(true, 1.0, "GNSS SINGLE DIFFERENCES", "cool GNSS single differences processor"); + ProcessorParamsBasePtr gnss_params_ptr = std::make_shared<ProcessorParamsBase>(true, 1.0); problem_ptr->installProcessor("GNSS SINGLE DIFFERENCES", "gnss single difference", gnss_sensor_ptr, gnss_params_ptr); std::cout << "gnss processor installed" << std::endl; diff --git a/src/test/gtest_frame_base.cpp b/src/test/gtest_frame_base.cpp index ae6b78068133529dcf9045572bba5f9715b9a62c..92068e3920ee3c887dd6fd979db131e03e0f4015 100644 --- a/src/test/gtest_frame_base.cpp +++ b/src/test/gtest_frame_base.cpp @@ -55,7 +55,8 @@ TEST(FrameBase, LinksBasic) ASSERT_FALSE(F->getProblem()); // ASSERT_THROW(f->getPreviousFrame(), std::runtime_error); // protected by assert() // ASSERT_EQ(f->getStatus(), ST_ESTIMATED); // protected - ASSERT_FALSE(F->getCaptureOf(make_shared<SensorOdom2D>(nullptr, nullptr, 1,1))); + SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), IntrinsicsOdom2D()); + ASSERT_FALSE(F->getCaptureOf(S)); ASSERT_TRUE(F->getCaptureList().empty()); ASSERT_TRUE(F->getConstrainedByList().empty()); ASSERT_EQ(F->getHits() , (unsigned int) 0); @@ -67,7 +68,10 @@ TEST(FrameBase, LinksToTree) // Problem with 2 frames and one motion constraint between them ProblemPtr P = Problem::create("PO 2D"); TrajectoryBasePtr T = P->getTrajectoryPtr(); - SensorOdom2DPtr S = make_shared<SensorOdom2D>(make_shared<StateBlock>(2), make_shared<StateBlock>(1), 1,1); + IntrinsicsOdom2D intrinsics_odo; + intrinsics_odo.k_disp_to_disp = 1; + intrinsics_odo.k_rot_to_rot = 1; + SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo); P->getHardwarePtr()->addSensor(S); FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); T->addFrame(F1); diff --git a/src/test/gtest_processor_tracker_feature_trifocal.cpp b/src/test/gtest_processor_tracker_feature_trifocal.cpp index fb5625ccc2d662c31cac0bc9af3b8e52bc5a2db3..941cf0376702941b066e855b446150c9da6e4e42 100644 --- a/src/test/gtest_processor_tracker_feature_trifocal.cpp +++ b/src/test/gtest_processor_tracker_feature_trifocal.cpp @@ -82,8 +82,11 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback) intr); ProcessorParamsTrackerFeatureTrifocalPtr params_tracker_feature_trifocal = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>(); - params_tracker_feature_trifocal->name = "trifocal"; - params_tracker_feature_trifocal->voting_active = false; + + // params_tracker_feature_trifocal->name = "trifocal"; + params_tracker_feature_trifocal->pixel_noise_std = 1.0; + params_tracker_feature_trifocal->voting_active = true; + params_tracker_feature_trifocal->min_features_for_keyframe = 5; params_tracker_feature_trifocal->time_tolerance = dt/2; params_tracker_feature_trifocal->min_features_for_keyframe = 5; params_tracker_feature_trifocal->pixel_noise_std = 1.0; diff --git a/src/test/gtest_sensor_camera.cpp b/src/test/gtest_sensor_camera.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a75431e50f596372604ad49bec5618a9fb0664f1 --- /dev/null +++ b/src/test/gtest_sensor_camera.cpp @@ -0,0 +1,123 @@ +/** + * \file gtest_sensor_camera.cpp + * + * Created on: Feb 7, 2019 + * \author: jsola + */ + + +#include "utils_gtest.h" + +#include "sensor_camera.cpp" +#include "sensor_factory.h" + +using namespace wolf; + +TEST(SensorCamera, Img_size) +{ + Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1; + IntrinsicsCamera params; + params.width = 640; + params.height = 480; + SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params); + + ASSERT_EQ(cam->getImgWidth() , 640); + ASSERT_EQ(cam->getImgHeight(), 480); + + cam->setImgWidth(100); + ASSERT_EQ(cam->getImgWidth() , 100); + + cam->setImgHeight(100); + ASSERT_EQ(cam->getImgHeight(), 100); +} + +TEST(SensorCamera, Intrinsics_Raw_Rectified) +{ + Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1; + IntrinsicsCamera params; + params.pinhole_model_raw << 321, 241, 321, 321; + params.pinhole_model_rectified << 320, 240, 320, 320; + SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params); + + Eigen::Matrix3s K_raw, K_rectified; + K_raw << 321, 0, 321, 0, 321, 241, 0, 0, 1; + K_rectified << 320, 0, 320, 0, 320, 240, 0, 0, 1; + Eigen::Vector4s k_raw(321,241,321,321); + Eigen::Vector4s k_rectified(320,240,320,320); + + // default is raw + ASSERT_TRUE(cam->isUsingRawImages()); + ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8); + ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsicPtr()->getState(), 1e-8); + + cam->useRectifiedImages(); + ASSERT_FALSE(cam->isUsingRawImages()); + ASSERT_MATRIX_APPROX(K_rectified, cam->getIntrinsicMatrix(), 1e-8); + ASSERT_MATRIX_APPROX(k_rectified, cam->getIntrinsicPtr()->getState(), 1e-8); + + cam->useRawImages(); + ASSERT_TRUE(cam->isUsingRawImages()); + ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8); + ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsicPtr()->getState(), 1e-8); +} + +TEST(SensorCamera, Distortion) +{ + Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1; + IntrinsicsCamera params; + params.width = 640; + params.height = 480; + params.pinhole_model_raw << 321, 241, 321, 321; + params.pinhole_model_rectified << 320, 240, 320, 320; + params.distortion = Eigen::Vector3s( -0.3, 0.096, 0 ); + SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params); + + Eigen::Vector3s d(-0.3, 0.096, 0); + + ASSERT_MATRIX_APPROX(d, cam->getDistortionVector(), 1e-8); +} + +TEST(SensorCamera, Correction_zero) +{ + Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1; + IntrinsicsCamera params; + params.width = 640; + params.height = 480; + params.pinhole_model_raw << 321, 241, 321, 321; + params.pinhole_model_rectified << 320, 240, 320, 320; + params.distortion = Eigen::Vector3s( 0, 0, 0 ); + SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params); + + Eigen::MatrixXs c(cam->getCorrectionVector().size(), 1); + c.setZero(); + + ASSERT_GE(cam->getCorrectionVector().size(), cam->getDistortionVector().size()); + ASSERT_MATRIX_APPROX(c, cam->getCorrectionVector(), 1e-8); +} + +TEST(SensorCamera, create) +{ + Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1; + IntrinsicsCameraPtr params = std::make_shared<IntrinsicsCamera>(); + params->width = 640; + params->height = 480; + params->pinhole_model_raw << 321, 241, 321, 321; + params->pinhole_model_rectified << 320, 240, 320, 320; + params->distortion = Eigen::Vector3s( 0, 0, 0 ); + + SensorBasePtr sen = SensorCamera::create("camera", extrinsics, params); + + ASSERT_NE(sen, nullptr); + + SensorCameraPtr cam = std::static_pointer_cast<SensorCamera>(sen); + + ASSERT_NE(cam, nullptr); + ASSERT_EQ(cam->getImgWidth(), 640); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + diff --git a/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp index 75e2e405e9d7dacc0ac57f927701fdf4d970878b..e31c957f86b2eac07b3df83e4c9199118eaf60b3 100644 --- a/src/yaml/processor_IMU_yaml.cpp +++ b/src/yaml/processor_IMU_yaml.cpp @@ -26,20 +26,10 @@ static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _file if (config["processor type"].as<std::string>() == "IMU") { - - // YAML:: to Eigen:: - using namespace Eigen; - std::string processor_type = config["processor type"] .as<std::string>(); - std::string processor_name = config["processor name"] .as<std::string>(); - YAML::Node kf_vote = config["keyframe vote"]; ProcessorParamsIMUPtr params = std::make_shared<ProcessorParamsIMU>(); - params->time_tolerance = config["time tolerance"].as<Scalar>(); - - params->type = processor_type; - params->name = processor_name; params->max_time_span = kf_vote["max time span"] .as<Scalar>(); params->max_buff_length = kf_vote["max buffer length"] .as<SizeEigen >(); params->dist_traveled = kf_vote["dist traveled"] .as<Scalar>(); diff --git a/src/yaml/processor_odom_3D_yaml.cpp b/src/yaml/processor_odom_3D_yaml.cpp index 1b5a3f40db3589a8e51201273a93872cc827d8ce..047e42463a5466b66079ad9dfff9072f9566abac 100644 --- a/src/yaml/processor_odom_3D_yaml.cpp +++ b/src/yaml/processor_odom_3D_yaml.cpp @@ -26,18 +26,10 @@ static ProcessorParamsBasePtr createProcessorOdom3DParams(const std::string & _f if (config["processor type"].as<std::string>() == "ODOM 3D") { - - // YAML:: to Eigen:: - using namespace Eigen; - std::string processor_type = config["processor type"] .as<std::string>(); - std::string processor_name = config["processor name"] .as<std::string>(); - YAML::Node kf_vote = config["keyframe vote"]; ProcessorParamsOdom3DPtr params = std::make_shared<ProcessorParamsOdom3D>(); - params->type = processor_type; - params->name = processor_name; params->max_time_span = kf_vote["max time span"] .as<Scalar>(); params->max_buff_length = kf_vote["max buffer length"] .as<SizeEigen >(); params->dist_traveled = kf_vote["dist traveled"] .as<Scalar>(); diff --git a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp index 09b543541855b481ee954242a73686f25a08c3d8..438f0de2fa5e3660c9e22f6d61b27b8cecb303f8 100644 --- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp +++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp @@ -34,9 +34,6 @@ static ProcessorParamsBasePtr createProcessorParamsTrackerFeatureTrifocal(const { ProcessorParamsTrackerFeatureTrifocalPtr params = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>(); - params->type = config["processor type"].as<std::string>(); - params->name = config["processor name"].as<std::string>(); - YAML::Node vision_utils = config ["vision_utils"]; params->yaml_file_params_vision_utils = vision_utils["YAML file params"].as<std::string>(); diff --git a/src/yaml/sensor_IMU_yaml.cpp b/src/yaml/sensor_IMU_yaml.cpp index 658becb6bf8dd50722454d2c83947357a3e57a2b..a29331becfebfc0d132ac78d13b0fcb7d83ab6f3 100644 --- a/src/yaml/sensor_IMU_yaml.cpp +++ b/src/yaml/sensor_IMU_yaml.cpp @@ -27,23 +27,17 @@ static IntrinsicsBasePtr createIntrinsicsIMU(const std::string & _filename_dot_y if (config["sensor type"].as<std::string>() == "IMU") { - - // YAML:: to Eigen:: - using namespace Eigen; - std::string sensor_type = config["sensor type"] .as<std::string>(); - std::string sensor_name = config["sensor name"] .as<std::string>(); - - YAML::Node variances = config["motion variances"]; - YAML::Node kf_vote = config["keyframe vote"]; + YAML::Node variances = config["motion variances"]; + YAML::Node kf_vote = config["keyframe vote"]; IntrinsicsIMUPtr params = std::make_shared<IntrinsicsIMU>(); - params->a_noise = variances["a_noise"] .as<Scalar>(); - params->w_noise = variances["w_noise"] .as<Scalar>(); - params->ab_initial_stdev = variances["ab_initial_stdev"] .as<Scalar>(); - params->wb_initial_stdev = variances["wb_initial_stdev"] .as<Scalar>(); - params->ab_rate_stdev = variances["ab_rate_stdev"] .as<Scalar>(); - params->wb_rate_stdev = variances["wb_rate_stdev"] .as<Scalar>(); + params->a_noise = variances["a_noise"] .as<Scalar>(); + params->w_noise = variances["w_noise"] .as<Scalar>(); + params->ab_initial_stdev = variances["ab_initial_stdev"] .as<Scalar>(); + params->wb_initial_stdev = variances["wb_initial_stdev"] .as<Scalar>(); + params->ab_rate_stdev = variances["ab_rate_stdev"] .as<Scalar>(); + params->wb_rate_stdev = variances["wb_rate_stdev"] .as<Scalar>(); return params; } diff --git a/src/yaml/sensor_camera_yaml.cpp b/src/yaml/sensor_camera_yaml.cpp index 852adc977ac61c465613ed08a6dfd267f2cceedd..22e2b85b8a688ecb5e4067ac3c55e0e68d062cc3 100644 --- a/src/yaml/sensor_camera_yaml.cpp +++ b/src/yaml/sensor_camera_yaml.cpp @@ -24,40 +24,58 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do { YAML::Node camera_config = YAML::LoadFile(_filename_dot_yaml); - if ("CAMERA") //camera_config["sensor type"]) + // if (camera_config["sensor type"].as<std::string>() == "CAMERA") // this does not work: camera YAML files are ROS-styled + if (camera_config["camera_matrix"]) // check that at least this field exists to validate YAML file of the correct type { // YAML:: to Eigen:: using namespace Eigen; - std::string sensor_type = "CAMERA"; //camera_config["sensor type"] .as<std::string>(); - std::string sensor_name = camera_config["camera_name"] .as<std::string>(); unsigned int width = camera_config["image_width"] .as<unsigned int>(); unsigned int height = camera_config["image_height"] .as<unsigned int>(); - VectorXd intrinsic = camera_config["camera_matrix"]["data"] .as<VectorXd>(); VectorXd distortion = camera_config["distortion_coefficients"]["data"] .as<VectorXd>(); + VectorXd intrinsic = camera_config["camera_matrix"]["data"] .as<VectorXd>(); + VectorXd projection = camera_config["projection_matrix"]["data"] .as<VectorXd>(); // Eigen:: to wolf:: std::shared_ptr<IntrinsicsCamera> intrinsics_cam = std::make_shared<IntrinsicsCamera>(); - intrinsics_cam->type = sensor_type; - intrinsics_cam->name = sensor_name; - intrinsics_cam->pinhole_model[0] = intrinsic[2]; - intrinsics_cam->pinhole_model[1] = intrinsic[5]; - intrinsics_cam->pinhole_model[2] = intrinsic[0]; - intrinsics_cam->pinhole_model[3] = intrinsic[4]; + + intrinsics_cam->width = width; + intrinsics_cam->height = height; + + intrinsics_cam->pinhole_model_raw[0] = intrinsic[2]; + intrinsics_cam->pinhole_model_raw[1] = intrinsic[5]; + intrinsics_cam->pinhole_model_raw[2] = intrinsic[0]; + intrinsics_cam->pinhole_model_raw[3] = intrinsic[4]; + + intrinsics_cam->pinhole_model_rectified[0] = projection[2]; + intrinsics_cam->pinhole_model_rectified[1] = projection[6]; + intrinsics_cam->pinhole_model_rectified[2] = projection[0]; + intrinsics_cam->pinhole_model_rectified[3] = projection[5]; + assert (distortion.size() == 5 && "Distortion size must be size 5!"); - assert (distortion(2) == 0 && distortion(3) == 0 && "Cannot handle tangential distortion. Please re-calibrate without tangential distortion!"); + + WOLF_WARN_COND( distortion(2) != 0 || distortion(3) != 0 , "Wolf does not handle tangential distortion. Please consider re-calibrating without tangential distortion!"); + if (distortion(4) == 0) - intrinsics_cam->distortion = distortion.head<2>(); + if (distortion(1) == 0) + if (distortion(0) == 0) + intrinsics_cam->distortion.resize(0); + else + { + intrinsics_cam->distortion.resize(1); + intrinsics_cam->distortion = distortion.head<1>(); + } + else + { + intrinsics_cam->distortion.resize(2); + intrinsics_cam->distortion = distortion.head<2>(); + } else { - unsigned int dist_size = distortion.size() - 2; - unsigned int dist_tail_size = dist_size - 2; - intrinsics_cam->distortion.resize(dist_size); + intrinsics_cam->distortion.resize(3); intrinsics_cam->distortion.head<2>() = distortion.head<2>(); - intrinsics_cam->distortion.tail(dist_tail_size) = distortion.tail(dist_tail_size); + intrinsics_cam->distortion.tail<1>() = distortion.tail<1>(); } - intrinsics_cam->width = width; - intrinsics_cam->height = height; //========================================= @@ -87,7 +105,7 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do return intrinsics_cam; } - std::cout << "Bad configuration file. No sensor type found." << std::endl; + std::cout << "Bad configuration file. No or bad sensor type found." << std::endl; return nullptr; } diff --git a/src/yaml/sensor_laser_2D_yaml.cpp b/src/yaml/sensor_laser_2D_yaml.cpp index 160386e6ea9f3b005ab1ea073ffe6bcf269c3d4c..110c3aa950157b83da46467f3a7d332688e78650 100644 --- a/src/yaml/sensor_laser_2D_yaml.cpp +++ b/src/yaml/sensor_laser_2D_yaml.cpp @@ -23,9 +23,9 @@ namespace { // intrinsics creator IntrinsicsBasePtr createIntrinsicsLaser2D(const std::string& _filename_dot_yaml) { - // TODO: Parse YAML <-- maybe we want this out of this file? + // If required: Parse YAML + IntrinsicsLaser2DPtr params; // dummy - params->type = "LASER 2D"; // fill this one just for the fun of it return params; } diff --git a/src/yaml/sensor_odom_3D_yaml.cpp b/src/yaml/sensor_odom_3D_yaml.cpp index a6012aeef037ba22493f562969614ff885341aa4..8d1e391c2c061454e11b6b558ca133ed7fe3ea7d 100644 --- a/src/yaml/sensor_odom_3D_yaml.cpp +++ b/src/yaml/sensor_odom_3D_yaml.cpp @@ -26,14 +26,7 @@ static IntrinsicsBasePtr createIntrinsicsOdom3D(const std::string & _filename_do if (config["sensor type"].as<std::string>() == "ODOM 3D") { - - // YAML:: to Eigen:: - using namespace Eigen; - std::string sensor_type = config["sensor type"] .as<std::string>(); - std::string sensor_name = config["sensor name"] .as<std::string>(); - YAML::Node variances = config["motion variances"]; - YAML::Node kf_vote = config["keyframe vote"]; IntrinsicsOdom3DPtr params = std::make_shared<IntrinsicsOdom3D>();