diff --git a/CMakeLists.txt b/CMakeLists.txt index 15a8dc9ace99da7483746403cebc3a41760dcfeb..2c4eacec4547808ce5bf3b11d42b3dbf155a9270 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -262,10 +262,8 @@ SET(HDRS_CAPTURE include/base/capture/capture_pose.h include/base/capture/capture_void.h include/base/capture/capture_motion.h - include/base/capture/capture_GPS_fix.h include/base/capture/capture_odom_2D.h include/base/capture/capture_odom_3D.h - include/base/capture/capture_GPS_fix.h include/base/capture/capture_odom_2D.h include/base/capture/capture_odom_3D.h include/base/capture/capture_velocity.h @@ -277,9 +275,6 @@ SET(HDRS_FACTOR include/base/factor/factor_corner_2D.h include/base/factor/factor_epipolar.h include/base/factor/factor_fix_bias.h - include/base/factor/factor_GPS_2D.h - include/base/factor/factor_GPS_pseudorange_3D.h - include/base/factor/factor_GPS_pseudorange_2D.h include/base/factor/factor_odom_2D.h include/base/factor/factor_odom_2D_analytic.h include/base/factor/factor_odom_3D.h @@ -294,9 +289,6 @@ SET(HDRS_FACTOR include/base/factor/factor_diff_drive.h include/base/factor/factor_epipolar.h include/base/factor/factor_fix_bias.h - include/base/factor/factor_GPS_2D.h - include/base/factor/factor_GPS_pseudorange_3D.h - include/base/factor/factor_GPS_pseudorange_2D.h include/base/factor/factor_odom_2D.h include/base/factor/factor_odom_2D_analytic.h include/base/factor/factor_odom_3D.h @@ -310,13 +302,9 @@ SET(HDRS_FACTOR ) SET(HDRS_FEATURE include/base/feature/feature_corner_2D.h - include/base/feature/feature_GPS_fix.h - include/base/feature/feature_GPS_pseudorange.h include/base/feature/feature_odom_2D.h include/base/feature/feature_corner_2D.h include/base/feature/feature_diff_drive.h - include/base/feature/feature_GPS_fix.h - include/base/feature/feature_GPS_pseudorange.h include/base/feature/feature_odom_2D.h include/base/feature/feature_base.h include/base/feature/feature_match.h @@ -358,12 +346,8 @@ SET(HDRS_PROCESSOR SET(HDRS_SENSOR include/base/sensor/sensor_base.h include/base/sensor/sensor_diff_drive.h - include/base/sensor/sensor_GPS.h - include/base/sensor/sensor_GPS_fix.h include/base/sensor/sensor_odom_2D.h include/base/sensor/sensor_odom_3D.h - include/base/sensor/sensor_GPS.h - include/base/sensor/sensor_GPS_fix.h include/base/sensor/sensor_odom_2D.h include/base/sensor/sensor_odom_3D.h include/base/sensor/sensor_factory.h @@ -464,7 +448,6 @@ SET(SRCS_BASE SET(SRCS ) SET(SRCS_CAPTURE - src/capture/capture_GPS_fix.cpp src/capture/capture_odom_2D.cpp src/capture/capture_odom_3D.cpp src/capture/capture_velocity.cpp @@ -473,8 +456,6 @@ SET(SRCS_CAPTURE SET(SRCS_FEATURE src/feature/feature_corner_2D.cpp src/feature/feature_diff_drive.cpp - src/feature/feature_GPS_fix.cpp - src/feature/feature_GPS_pseudorange.cpp src/feature/feature_odom_2D.cpp ) SET(SRCS_LANDMARK @@ -494,8 +475,6 @@ SET(SRCS_PROCESSOR ) SET(SRCS_SENSOR src/sensor/sensor_diff_drive.cpp - src/sensor/sensor_GPS.cpp - src/sensor/sensor_GPS_fix.cpp src/sensor/sensor_odom_2D.cpp src/sensor/sensor_odom_3D.cpp ) diff --git a/include/base/sensor/sensor_camera.h b/include/base/sensor/sensor_camera.h deleted file mode 100644 index c5b8f4423661a0b4dc2e5dc00cd17bd3392455cb..0000000000000000000000000000000000000000 --- a/include/base/sensor/sensor_camera.h +++ /dev/null @@ -1,90 +0,0 @@ -#ifndef SENSOR_CAMERA_H -#define SENSOR_CAMERA_H - -//wolf includes -#include "base/sensor/sensor_base.h" - -namespace wolf -{ - -WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsCamera); -/** Struct of intrinsic camera parameters - */ -struct IntrinsicsCamera : public IntrinsicsBase -{ - 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; -}; - -WOLF_PTR_TYPEDEFS(SensorCamera); -/**Pin-hole camera sensor - */ -class SensorCamera : public SensorBase -{ - public: - - 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_; } - - bool isUsingRawImages() { return using_raw_; } - bool useRawImages(); - bool useRectifiedImages(); - - - int getImgWidth(){return img_width_;} - int getImgHeight(){return img_height_;} - void setImgWidth(int _w){img_width_ = _w;} - void setImgHeight(int _h){img_height_ = _h;} - - private: - int img_width_; - 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); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html) - - static SensorBasePtr create(const std::string & _unique_name, // - const Eigen::VectorXs& _extrinsics, // - const IntrinsicsBasePtr _intrinsics); - -}; - -inline bool SensorCamera::useRawImages() -{ - getIntrinsic()->setState(pinhole_model_raw_); - K_ = setIntrinsicMatrix(pinhole_model_raw_); - using_raw_ = true; - - return true; -} - -inline bool SensorCamera::useRectifiedImages() -{ - getIntrinsic()->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/sensor_camera.cpp b/src/sensor/sensor_camera.cpp deleted file mode 100644 index 54c08c69f3a263b6d71a1b461baa393e62db0d10..0000000000000000000000000000000000000000 --- a/src/sensor/sensor_camera.cpp +++ /dev/null @@ -1,71 +0,0 @@ -#include "base/sensor/sensor_camera.h" - -#include "base/pinhole_tools.h" -#include "base/state_block.h" -#include "base/state_quaternion.h" - -namespace wolf -{ - -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_raw, true), 1), - img_width_(_intrinsics.width), // - img_height_(_intrinsics.height), // - distortion_(_intrinsics.distortion), // - 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"); - useRawImages(); - pinhole::computeCorrectionModel(getIntrinsic()->getState(), distortion_, correction_); -} - -SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr) : - SensorCamera(_extrinsics, *_intrinsics_ptr) -{ - // -} - -SensorCamera::~SensorCamera() -{ - // -} - -Eigen::Matrix3s SensorCamera::setIntrinsicMatrix(Eigen::Vector4s _pinhole_model) -{ - Eigen::Matrix3s K; - K(0, 0) = _pinhole_model(2); - K(0, 1) = 0; - K(0, 2) = _pinhole_model(0); - K(1, 0) = 0; - K(1, 1) = _pinhole_model(3); - K(1, 2) = _pinhole_model(1); - K.row(2) << 0, 0, 1; - return K; -} - -// Define the factory method -SensorBasePtr SensorCamera::create(const std::string& _unique_name, // - const Eigen::VectorXs& _extrinsics_pq, // - const IntrinsicsBasePtr _intrinsics) -{ - assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D."); - - std::shared_ptr<IntrinsicsCamera> intrinsics_ptr = std::static_pointer_cast<IntrinsicsCamera>(_intrinsics); - SensorCameraPtr sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr); - sen_ptr->setName(_unique_name); - - return sen_ptr; -} - -} // namespace wolf - -// Register in the SensorFactory -#include "base/sensor/sensor_factory.h" -namespace wolf -{ -WOLF_REGISTER_SENSOR("CAMERA", SensorCamera) -} // namespace wolf - diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 8ee7623dc68b7f637677fd24ceb6f90b083a4b39..b175cb9d07eb344b347e9fbcb266d04371674140 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -210,6 +210,7 @@ TEST(Problem, StateBlocks) ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) 2); ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) 2); + //TODO: TO BE FIXED: Since the splitting the core has no camera sensor, so this test fails. // 3 state blocks, fixed SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml"); ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) (2 + 3));