diff --git a/include/base/constraint/constraint_analytic.h b/include/base/constraint/constraint_analytic.h
index 3b5015be0841fab26b3e540dcda757987bdea753..2e3d110b133925f4032370c971e4bd344d8a1ae5 100644
--- a/include/base/constraint/constraint_analytic.h
+++ b/include/base/constraint/constraint_analytic.h
@@ -81,7 +81,6 @@ class ConstraintAnalytic: public ConstraintBase
/** \brief Evaluate the constraint given the input parameters and returning the residuals and jacobians
**/
- // TODO
virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override
{
// load parameters evaluation value
@@ -116,7 +115,7 @@ class ConstraintAnalytic: public ConstraintBase
return true;
};
- /** Returns the residual vetor and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
+ /** Returns the residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
**/
// TODO
virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const override
@@ -130,9 +129,9 @@ class ConstraintAnalytic: public ConstraintBase
**/
virtual Eigen::VectorXs evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector) const = 0;
- /** \brief Returns the jacobians evaluated in the states provided
+ /** \brief Returns the normalized jacobians evaluated in the states
*
- * Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
+ * Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the constraint
@@ -142,11 +141,10 @@ class ConstraintAnalytic: public ConstraintBase
**/
virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector, std::vector<Eigen::Map<Eigen::MatrixXs>>& jacobians, const std::vector<bool>& _compute_jacobian) const = 0;
- /** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
+ /** \brief Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values
*
- * Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
+ * Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values
*
- * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the constraint
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
*
**/
diff --git a/include/base/constraint/constraint_block_absolute.h b/include/base/constraint/constraint_block_absolute.h
index 22b4ee1b1ebb371e6cc76ad5e8bd5392df6e64ee..85a76c664ab5c3113c91ec34ff1df2785e1b910e 100644
--- a/include/base/constraint/constraint_block_absolute.h
+++ b/include/base/constraint/constraint_block_absolute.h
@@ -9,6 +9,7 @@
#define CONSTRAINT_BLOCK_ABSOLUTE_H_
//Wolf includes
+#include "base/constraint/constraint_analytic.h"
#include "base/constraint/constraint_autodiff.h"
#include "base/frame_base.h"
@@ -17,43 +18,122 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(ConstraintBlockAbsolute);
//class
-class ConstraintBlockAbsolute: public ConstraintAutodiff<ConstraintBlockAbsolute,3,3>
+class ConstraintBlockAbsolute : public ConstraintAnalytic
{
+ private:
+ SizeEigen sb_size_; // the size of the state block
+ SizeEigen sb_constrained_start_; // the index of the first state element that is constrained
+ SizeEigen sb_constrained_size_; // the size of the state segment that is constrained
+ Eigen::MatrixXs J_; // Jacobian
+
public:
- ConstraintBlockAbsolute(StateBlockPtr _sb_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintBlockAbsolute,3,3>("BLOCK ABS",
- nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _sb_ptr)
+ /** \brief Constructor
+ *
+ * \param _sb_ptr the constrained state block
+ * \param _start_idx (default=0) the index of the first state element that is constrained
+ * \param _start_idx (default=-1) the size of the state segment that is constrained, -1 = all the
+ *
+ */
+ ConstraintBlockAbsolute(StateBlockPtr _sb_ptr,
+ unsigned int _start_idx = 0,
+ int _size = -1,
+ bool _apply_loss_function = false,
+ ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintAnalytic("BLOCK ABS",
+ _apply_loss_function,
+ _status,
+ _sb_ptr),
+ sb_size_(_sb_ptr->getSize()),
+ sb_constrained_start_(_start_idx),
+ sb_constrained_size_(_size == -1 ? sb_size_ : _size)
{
- //
+ assert(sb_constrained_size_+sb_constrained_start_ <= sb_size_);
+
+ // precompute Jacobian (Identity)
+ if (sb_constrained_start_ == 0)
+ J_ = Eigen::MatrixXs::Identity(sb_constrained_size_, sb_size_);
+ else
+ {
+ J_ = Eigen::MatrixXs::Zero(sb_constrained_size_,sb_size_);
+ J_.middleCols(sb_constrained_start_,sb_constrained_size_) = Eigen::MatrixXs::Identity(sb_constrained_size_, sb_constrained_size_);
+ }
}
virtual ~ConstraintBlockAbsolute() = default;
- template<typename T>
- bool operator ()(const T* const _sb, T* _residuals) const;
-
+ /** \brief Returns the residual evaluated in the states provided
+ *
+ * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXs
+ *
+ **/
+ virtual Eigen::VectorXs evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const;
+
+ /** \brief Returns the normalized jacobians evaluated in the states
+ *
+ * Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
+ * IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
+ *
+ * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the constraint
+ * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+ * \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
+ *
+ **/
+ virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixXs> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const;
+
+ /** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
+ *
+ * Returns the pure jacobians (without measurement noise) evaluated in the current state blocks values
+ *
+ * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+ *
+ **/
+ virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const;
+
+ /** \brief Returns the constraint residual size
+ **/
+ virtual unsigned int getSize() const;
};
-template<typename T>
-inline bool ConstraintBlockAbsolute::operator ()(const T* const _sb, T* _residuals) const
+inline Eigen::VectorXs ConstraintBlockAbsolute::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const
{
+ assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
+ assert(_st_vector.front().size() >= getMeasurement().size() && "Wrong StateBlock size");
- // states
- Eigen::Matrix<T, 3, 1> sb(_sb);
+ // residual
+ if (sb_constrained_size_ == _st_vector.front().size())
+ return getMeasurementSquareRootInformationUpper() * (_st_vector.front() - getMeasurement());
+ else
+ return getMeasurementSquareRootInformationUpper() * (_st_vector.front().segment(sb_constrained_start_,sb_constrained_size_) - getMeasurement());
+}
- // measurements
- Eigen::Vector3s measured_state(getMeasurement().data() + 0);
+inline void ConstraintBlockAbsolute::evaluateJacobians(
+ const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixXs> >& jacobians, const std::vector<bool>& _compute_jacobian) const
+{
+ assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
+ assert(jacobians.size() == 1 && "Wrong number of jacobians!");
+ assert(_compute_jacobian.size() == 1 && "Wrong number of _compute_jacobian booleans!");
+ assert(_st_vector.front().size() == sb_size_ && "Wrong StateBlock size");
+ assert(_st_vector.front().size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
+
+ // normalized jacobian
+ if (_compute_jacobian.front())
+ jacobians.front() = getMeasurementSquareRootInformationUpper() * J_;
+}
- // error
- Eigen::Matrix<T, 3, 1> er;
- er = measured_state.cast<T>() - sb;
+inline void ConstraintBlockAbsolute::evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const
+{
+ assert(jacobians.size() == 1 && "Wrong number of jacobians!");
- // residual
- Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
- res = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ jacobians.front() = J_;
+}
- return true;
+inline unsigned int ConstraintBlockAbsolute::getSize() const
+{
+ return sb_constrained_size_;
}
} // namespace wolf
diff --git a/include/base/constraint/constraint_relative_2D_analytic.h b/include/base/constraint/constraint_relative_2D_analytic.h
index a9655a9146e8f7dce7efc6378effd3c56a59cb66..125537e5a3dde3771d9d7c8da20c97c09b96f520 100644
--- a/include/base/constraint/constraint_relative_2D_analytic.h
+++ b/include/base/constraint/constraint_relative_2D_analytic.h
@@ -85,7 +85,6 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic
const std::vector<bool>& _compute_jacobian) const override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
- * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the constraint
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
*
**/
@@ -124,11 +123,11 @@ inline void ConstraintRelative2DAnalytic::evaluateJacobians(
<< -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0))
+ (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)), -(_st_vector[2](0)
- _st_vector[0](0)) * cos(_st_vector[1](0)) - (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)), -1;
- jacobians[1] = getMeasurementSquareRootInformationUpper() * jacobians[0];
+ jacobians[1] = getMeasurementSquareRootInformationUpper() * jacobians[1];
jacobians[2] << cos(_st_vector[1](0)), sin(_st_vector[1](0)), -sin(_st_vector[1](0)), cos(_st_vector[1](0)), 0, 0;
- jacobians[2] = getMeasurementSquareRootInformationUpper() * jacobians[0];
+ jacobians[2] = getMeasurementSquareRootInformationUpper() * jacobians[2];
jacobians[3] << 0, 0, 1;
- jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[0];
+ jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[3];
}
inline void ConstraintRelative2DAnalytic::evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const
diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
index 6a1d23cfb3fd19a625407c4de20eb6d17784e371..23ac82daa6ff6ea65bc3172d4fa0726890e5dff1 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -110,13 +110,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)
{
//
}
@@ -129,9 +125,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/include/base/sensor/sensor_GPS_fix.h b/include/base/sensor/sensor_GPS_fix.h
index e74ff5e89f31b3fc0759f8d62bd76448c956c593..17db7c12c11574c9be64e7adac312d8a671587af 100644
--- a/include/base/sensor/sensor_GPS_fix.h
+++ b/include/base/sensor/sensor_GPS_fix.h
@@ -22,15 +22,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/include/base/sensor/sensor_IMU.h b/include/base/sensor/sensor_IMU.h
index 17c1c873d921032c05700a558abf6151e3f3255b..2c8efa2c9312f62bca4d9d1c2578a458914403bb 100644
--- a/include/base/sensor/sensor_IMU.h
+++ b/include/base/sensor/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/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h
index 8707d13ac8bcac4b288f139dec53045625f02ac9..01df97dc63db1cbd6a02abce6625dc6b24247d47 100644
--- a/include/base/sensor/sensor_base.h
+++ b/include/base/sensor/sensor_base.h
@@ -24,9 +24,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>
@@ -49,6 +46,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
Eigen::VectorXs noise_std_; // std of sensor noise
Eigen::MatrixXs noise_cov_; // cov matrix of noise
+ std::map<unsigned int,FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by index in state_block_vec_)
+
public:
/** \brief Constructor with noise size
*
@@ -89,7 +88,6 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
const bool _intr_dyn = false);
virtual ~SensorBase();
- virtual void remove();
unsigned int id();
@@ -110,11 +108,16 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
const std::vector<StateBlockPtr>& getStateBlockVec() const;
std::vector<StateBlockPtr>& getStateBlockVec();
StateBlockPtr getStateBlockPtrStatic(unsigned int _i) const;
- StateBlockPtr getStateBlockPtrDynamic(unsigned int _i);
- StateBlockPtr getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts);
+ StateBlockPtr getStateBlockPtr(unsigned int _i);
+ StateBlockPtr getStateBlockPtr(unsigned int _i, const TimeStamp& _ts);
void setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr);
void resizeStateBlockVec(unsigned int _size);
+ bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap);
+ bool isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap);
+ bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts);
+ bool isStateBlockDynamic(unsigned int _i);
+
StateBlockPtr getPPtr(const TimeStamp _ts);
StateBlockPtr getOPtr(const TimeStamp _ts);
StateBlockPtr getIntrinsicPtr(const TimeStamp _ts);
@@ -133,6 +136,32 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
void fixIntrinsics();
void unfixIntrinsics();
+ /** \brief Add an absolute constraint to a parameter
+ *
+ * Add an absolute constraint to a parameter
+ * \param _i state block index (in state_block_vec_) of the parameter to be constrained
+ * \param _x prior value
+ * \param _cov covariance
+ * \param _start_idx state segment starting index (not used in quaternions)
+ * \param _size state segment size (-1: whole state) (not used in quaternions)
+ *
+ **/
+ void addPriorParameter(const unsigned int _i,
+ const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov,
+ unsigned int _start_idx = 0,
+ int _size = -1);
+ void addPriorP(const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov,
+ unsigned int _start_idx = 0,
+ int _size = -1);
+ void addPriorO(const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov);
+ void addPriorIntrinsics(const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov,
+ unsigned int _start_idx = 0,
+ int _size = -1);
+
SizeEigen getCalibSize() const;
Eigen::VectorXs getCalibration() const;
@@ -196,6 +225,8 @@ inline StateBlockPtr SensorBase::getStateBlockPtrStatic(unsigned int _i) const
inline void SensorBase::setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr)
{
+ assert (_i < state_block_vec_.size() && "Setting a state block pointer out of the vector range!");
+ assert((params_prior_map_.find(_i) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute constraint");
state_block_vec_[_i] = _sb_ptr;
}
@@ -252,6 +283,21 @@ inline void SensorBase::setHardwarePtr(const HardwareBasePtr _hw_ptr)
hardware_ptr_ = _hw_ptr;
}
+inline void SensorBase::addPriorP(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+{
+ addPriorParameter(0, _x, _cov, _start_idx, _size);
+}
+
+inline void SensorBase::addPriorO(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov)
+{
+ addPriorParameter(1, _x, _cov);
+}
+
+inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+{
+ addPriorParameter(2, _x, _cov);
+}
+
inline SizeEigen SensorBase::getCalibSize() const
{
return calib_size_;
diff --git a/include/base/sensor/sensor_camera.h b/include/base/sensor/sensor_camera.h
index 2214fc14a317f07c072f64462feac9870388dee6..c7db047c3c28808c9dd40374bdf93adcdd894a2f 100644
--- a/include/base/sensor/sensor_camera.h
+++ b/include/base/sensor/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;
};
@@ -26,26 +27,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_;}
@@ -57,7 +52,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);
@@ -70,6 +67,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/include/base/sensor/sensor_odom_2D.h b/include/base/sensor/sensor_odom_2D.h
index 75110b2cd69595555c9a6969f3e3cbbefbf13065..f1528ec377f88f854ee905fbde29e0db5c381265 100644
--- a/include/base/sensor/sensor_odom_2D.h
+++ b/include/base/sensor/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/include/base/sensor/sensor_odom_3D.h b/include/base/sensor/sensor_odom_3D.h
index 76c9a3d5b9e9314e8e45f50f3520175ba03e893b..be3694b2da8b69ad7201d57251f0246deb6dba92 100644
--- a/include/base/sensor/sensor_odom_3D.h
+++ b/include/base/sensor/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/constraint/constraint_analytic.cpp b/src/constraint/constraint_analytic.cpp
index 23ab82362dc7391747099d82b705c6d13b5896c8..9cfb538b1647487968e1e35e73db4af4603bc857 100644
--- a/src/constraint/constraint_analytic.cpp
+++ b/src/constraint/constraint_analytic.cpp
@@ -7,9 +7,19 @@ ConstraintAnalytic::ConstraintAnalytic(const std::string& _tp,
bool _apply_loss_function, ConstraintStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- ConstraintBase(_tp, _apply_loss_function, _status),
- state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
- _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
+ ConstraintBase(_tp, _apply_loss_function, _status),
+ state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
+ _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
+ state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
+ _state1Ptr ? (unsigned int) _state1Ptr->getSize() : 0,
+ _state2Ptr ? (unsigned int) _state2Ptr->getSize() : 0,
+ _state3Ptr ? (unsigned int) _state3Ptr->getSize() : 0,
+ _state4Ptr ? (unsigned int) _state4Ptr->getSize() : 0,
+ _state5Ptr ? (unsigned int) _state5Ptr->getSize() : 0,
+ _state6Ptr ? (unsigned int) _state6Ptr->getSize() : 0,
+ _state7Ptr ? (unsigned int) _state7Ptr->getSize() : 0,
+ _state8Ptr ? (unsigned int) _state8Ptr->getSize() : 0,
+ _state9Ptr ? (unsigned int) _state9Ptr->getSize() : 0})
{
resizeVectors();
}
@@ -23,9 +33,19 @@ ConstraintAnalytic::ConstraintAnalytic(const std::string& _tp,
bool _apply_loss_function, ConstraintStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
- _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
+ ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
+ _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
+ state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
+ _state1Ptr ? (unsigned int) _state1Ptr->getSize() : 0,
+ _state2Ptr ? (unsigned int) _state2Ptr->getSize() : 0,
+ _state3Ptr ? (unsigned int) _state3Ptr->getSize() : 0,
+ _state4Ptr ? (unsigned int) _state4Ptr->getSize() : 0,
+ _state5Ptr ? (unsigned int) _state5Ptr->getSize() : 0,
+ _state6Ptr ? (unsigned int) _state6Ptr->getSize() : 0,
+ _state7Ptr ? (unsigned int) _state7Ptr->getSize() : 0,
+ _state8Ptr ? (unsigned int) _state8Ptr->getSize() : 0,
+ _state9Ptr ? (unsigned int) _state9Ptr->getSize() : 0})
{
resizeVectors();
}
@@ -58,17 +78,15 @@ JacobianMethod ConstraintAnalytic::getJacobianMethod() const
void ConstraintAnalytic::resizeVectors()
{
- for (unsigned int ii = 1; ii<state_ptr_vector_.size(); ii++)
- {
- if (state_ptr_vector_.at(ii) != nullptr)
- state_block_sizes_vector_.push_back(state_ptr_vector_.at(ii)->getSize());
+ assert(state_ptr_vector_[0] != nullptr && "at least one not null state block pointer required");
- else
+ for (unsigned int ii = 1; ii<state_ptr_vector_.size(); ii++)
+ if (state_ptr_vector_.at(ii) == nullptr)
{
state_ptr_vector_.resize(ii);
+ state_block_sizes_vector_.resize(ii);
break;
}
- }
}
} // namespace wolf
diff --git a/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
index 4108864a7319361776556d02a9efe43277643623..3ada8333437f02034267943e68a4ab2c8bf57517 100644
--- a/src/examples/test_wolf_factories.cpp
+++ b/src/examples/test_wolf_factories.cpp
@@ -62,7 +62,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 40ef35f2f1b2c7a044736b85cce3c5f98a4d492c..d96a1d29065a18815bd99eb65245856e89cde66d 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -4,17 +4,14 @@
#include "base/trajectory_base.h"
#include "base/map_base.h"
#include "base/sensor/sensor_base.h"
-#include "base/factory.h"
+#include "base/processor/processor_motion.h"
+#include "base/processor/processor_tracker.h"
+#include "base/capture/capture_pose.h"
+#include "base/constraint/constraint_base.h"
#include "base/sensor/sensor_factory.h"
#include "base/processor/processor_factory.h"
-#include "base/constraint/constraint_base.h"
#include "base/state_block.h"
-#include "base/processor/processor_motion.h"
-#include "base/sensor/sensor_GPS.h"
-#include "base/processor/processor_tracker.h"
-//#include "processors/processor_tracker_feature_trifocal.h"
-#include "base/capture/capture_pose.h"
// IRI libs includes
@@ -749,7 +746,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
{
if (i==0) cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
if (i==2) cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
- auto sb = S->getStateBlockPtrDynamic(i);
+ auto sb = S->getStateBlockPtr(i);
if (sb)
{
cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index aa259d706262904969e9e27245a50e06456584a0..60569422e15f307d04dc9f6f8c350eea313643af 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/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/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index c375d52faca184c57b3dad8976aac24c9decaca5..2c5bfe456d0d31e1be0460faf0c8df78b2922556 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -21,7 +21,7 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Ei
assert(_data_cov.rows() == data_size_ && "Wrong _data_cov size");
assert(_data_cov.cols() == data_size_ && "Wrong _data_cov size");
- // data is [dtheta, dr]
+ // data is [dr, dtheta]
// delta is [dx, dy, dtheta]
// motion model is 1/2 turn + straight + 1/2 turn
_delta(0) = cos(_data(1) / 2) * _data(0);
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index 03cd93f17f9a21b33493cc1cf76a45046061cb4a..30018991042f601481d15289a0bf008aa9dc6501 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -37,7 +37,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/sensor_GPS_fix.cpp b/src/sensor/sensor_GPS_fix.cpp
index 788dc06dc58023d6502d01a2d01029b0bf96af5f..427dc45182c9c5efa9319a0e56ca9bebf21922eb 100644
--- a/src/sensor/sensor_GPS_fix.cpp
+++ b/src/sensor/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)
{
//
}
@@ -33,8 +28,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/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp
index 94a30bb4347f0f42c137393b7e9df4b08cda1ae0..f97edf645e607b08534ca421db1bd09070bf8d70 100644
--- a/src/sensor/sensor_IMU.cpp
+++ b/src/sensor/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/sensor_base.cpp b/src/sensor/sensor_base.cpp
index a05fec1ceb4b1b90912dd4ec380212848c68f98b..92616d53373d40c081638ddf0433a83692d575ae 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -1,5 +1,9 @@
#include "base/sensor/sensor_base.h"
#include "base/state_block.h"
+#include "base/state_quaternion.h"
+#include "base/constraint/constraint_block_absolute.h"
+#include "base/constraint/constraint_quaternion_absolute.h"
+
namespace wolf {
@@ -62,30 +66,6 @@ SensorBase::~SensorBase()
removeStateBlocks();
}
-void SensorBase::remove()
-{
- if (!is_removing_)
- {
- is_removing_ = true;
- SensorBasePtr this_S = shared_from_this(); // protect it while removing links
-
- // Remove State Blocks
- removeStateBlocks();
-
- // remove from upstream
- auto H = hardware_ptr_.lock();
- if (H)
- H->getSensorList().remove(this_S);
-
- // remove downstream processors
- while (!processor_list_.empty())
- {
- processor_list_.front()->remove();
- }
- }
-
-}
-
void SensorBase::removeStateBlocks()
{
for (unsigned int i = 0; i < state_block_vec_.size(); i++)
@@ -162,6 +142,50 @@ void SensorBase::unfixIntrinsics()
updateCalibSize();
}
+void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+{
+ assert(!isStateBlockDynamic(_i) && "SensorBase::addPriorParameter only allowed for static parameters");
+ assert(_i < state_block_vec_.size() && "State block not found");
+
+ StateBlockPtr _sb = getStateBlockPtrStatic(_i);
+ bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr);
+
+ assert(((!is_quaternion && _x.size() == _cov.rows() && _x.size() == _cov.cols()) ||
+ (is_quaternion && _x.size() == 4 &&_cov.rows() == 3 && _cov.cols() == 3)) && "bad prior/covariance dimensions");
+ assert((_size == -1 && _start_idx == 0) || (_size+_start_idx <= _sb->getSize()));
+ assert(_size == -1 || _size == _x.size());
+ assert(!(_size != -1 && is_quaternion) && "prior of a segment of state not available for quaternion");
+
+ // set StateBlock state
+ if (_size == -1)
+ _sb->setState(_x);
+ else
+ {
+ Eigen::VectorXs new_x = _sb->getState();
+ new_x.segment(_start_idx,_size) = _x;
+ _sb->setState(new_x);
+ }
+
+ // remove previous prior (if any)
+ if (params_prior_map_.find(_i) != params_prior_map_.end())
+ params_prior_map_[_i]->remove();
+
+ // create feature
+ FeatureBasePtr ftr_prior = std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
+
+ // set feature problem
+ ftr_prior->setProblem(getProblem());
+
+ // create & add constraint absolute
+ if (is_quaternion)
+ ftr_prior->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(_sb));
+ else
+ ftr_prior->addConstraint(std::make_shared<ConstraintBlockAbsolute>(_sb, _start_idx, _size));
+
+ // store feature in params_prior_map_
+ params_prior_map_[_i] = ftr_prior;
+}
+
void SensorBase::registerNewStateBlocks()
{
if (getProblem() != nullptr)
@@ -238,32 +262,32 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
StateBlockPtr SensorBase::getPPtr(const TimeStamp _ts)
{
- return getStateBlockPtrDynamic(0, _ts);
+ return getStateBlockPtr(0, _ts);
}
StateBlockPtr SensorBase::getOPtr(const TimeStamp _ts)
{
- return getStateBlockPtrDynamic(1, _ts);
+ return getStateBlockPtr(1, _ts);
}
StateBlockPtr SensorBase::getIntrinsicPtr(const TimeStamp _ts)
{
- return getStateBlockPtrDynamic(2, _ts);
+ return getStateBlockPtr(2, _ts);
}
StateBlockPtr SensorBase::getPPtr()
{
- return getStateBlockPtrDynamic(0);
+ return getStateBlockPtr(0);
}
StateBlockPtr SensorBase::getOPtr()
{
- return getStateBlockPtrDynamic(1);
+ return getStateBlockPtr(1);
}
StateBlockPtr SensorBase::getIntrinsicPtr()
{
- return getStateBlockPtrDynamic(2);
+ return getStateBlockPtr(2);
}
SizeEigen SensorBase::computeCalibSize() const
@@ -315,32 +339,62 @@ ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
return _proc_ptr;
}
-StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i)
+StateBlockPtr SensorBase::getStateBlockPtr(unsigned int _i)
+{
+ CaptureBasePtr cap;
+
+ if (isStateBlockDynamic(_i, cap))
+ return cap->getStateBlockPtr(_i);
+
+ return getStateBlockPtrStatic(_i);
+}
+
+StateBlockPtr SensorBase::getStateBlockPtr(unsigned int _i, const TimeStamp& _ts)
+{
+ CaptureBasePtr cap;
+
+ if (isStateBlockDynamic(_i, _ts, cap))
+ return cap->getStateBlockPtr(_i);
+
+ return getStateBlockPtrStatic(_i);
+}
+
+bool SensorBase::isStateBlockDynamic(unsigned int _i, CaptureBasePtr& cap)
{
if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
{
- CaptureBasePtr cap = lastKeyCapture();
- if (cap)
- return cap->getStateBlockPtr(_i);
- else
- return getStateBlockPtrStatic(_i);
+ cap = lastKeyCapture();
+
+ return cap != nullptr;
}
else
- return getStateBlockPtrStatic(_i);
+ return false;
}
-StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts)
+bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts, CaptureBasePtr& cap)
{
if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
{
- CaptureBasePtr cap = lastCapture(_ts);
- if (cap)
- return cap->getStateBlockPtr(_i);
- else
- return getStateBlockPtrStatic(_i);
+ cap = lastCapture(_ts);
+
+ return cap != nullptr;
}
else
- return getStateBlockPtrStatic(_i);
+ return false;
+}
+
+bool SensorBase::isStateBlockDynamic(unsigned int _i)
+{
+ CaptureBasePtr cap;
+
+ return isStateBlockDynamic(_i,cap);
+}
+
+bool SensorBase::isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts)
+{
+ CaptureBasePtr cap;
+
+ return isStateBlockDynamic(_i,_ts,cap);
}
void SensorBase::setProblem(ProblemPtr _problem)
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 3a3dae9bd4a3064517c150cbe808a4a24b43061f..6e20ff960177e9be75bef2dfc4342cdb96b23bc8 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/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_);
}
@@ -69,7 +64,6 @@ SensorBasePtr SensorCamera::create(const std::string& _unique_name, //
// Register in the SensorFactory
#include "base/sensor/sensor_factory.h"
-//#include "base/factory.h"
namespace wolf
{
WOLF_REGISTER_SENSOR("CAMERA", SensorCamera)
diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp
index 7f0abaed4648650da90136d189c34b9d5a7c66f4..c698db52f69d668ca7bab3c522087cb168047d10 100644
--- a/src/sensor/sensor_odom_2D.cpp
+++ b/src/sensor/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/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3D.cpp
index aa6efc42b82aac106dbb39c509bcb382d77cd2b8..0a5c2d7961b5d36e171e6b9d29fafdc30580a74e 100644
--- a/src/sensor/sensor_odom_3D.cpp
+++ b/src/sensor/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/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp
index 38b83b82f3908983df2f0ad27867bbfe818064a2..2cfc70f46321ea3af69d657b47540258b4b17ebd 100644
--- a/src/yaml/processor_IMU_yaml.cpp
+++ b/src/yaml/processor_IMU_yaml.cpp
@@ -26,18 +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->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 af8934b07d182dab640a70b7f53511ce75abb447..5c2172ea9f26c3561f7fc48c5c56d4be7695ed1c 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 fd110aa0e142745558c4f3ec69ee71ea525c76cb..2337df07c51f3dd1027d5e1b6004ca426221b722 100644
--- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
@@ -33,9 +33,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 c64914c6054aca6cd27513085289903dabdf051c..cc3dbb649b7d2c14c92ec4e78f2d43c0c343b254 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 d1a76b78b6a58c46f68480d8efd1a5b0fe4ce663..5e5e3df19cd91a8735f6625ecd2d551deb7e7b77 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;
//=========================================
// ===== this part for debugging only =====
@@ -83,7 +101,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 6a6b3f4ae06efdec4b8b48750a4e8f3bde22328c..e687c14463b746d147d791338afc5e34120fe8f3 100644
--- a/src/yaml/sensor_laser_2D_yaml.cpp
+++ b/src/yaml/sensor_laser_2D_yaml.cpp
@@ -22,9 +22,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 f423169a9a013c2c2251f8bc10c7597abe13a079..63a7baa2c4cad96e3ca297950df76611fb4ed7ba 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>();
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index cd99f0476dfe9eacbf0b7b63566f9306def8e554..47e53807a4afa37b9fe04e6cb80fb702ad007986 100644
--- a/test/CMakeLists.txt
+++ b/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})
@@ -164,6 +168,10 @@ target_link_libraries(gtest_IMU ${PROJECT_NAME})
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
target_link_libraries(gtest_make_posdef ${PROJECT_NAME})
+# Parameter prior test
+wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
+target_link_libraries(gtest_param_prior ${PROJECT_NAME})
+
# Pinhole test
wolf_add_gtest(gtest_pinhole gtest_pinhole.cpp)
target_link_libraries(gtest_pinhole ${PROJECT_NAME})
@@ -194,9 +202,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/test/gtest_constraint_absolute.cpp b/test/gtest_constraint_absolute.cpp
index 53252fc5fdf43c5c58b29105245038d38be0d748..f5f03b27dc0d5e1a11b5e8301c5178f6d1929088 100644
--- a/test/gtest_constraint_absolute.cpp
+++ b/test/gtest_constraint_absolute.cpp
@@ -25,18 +25,17 @@ Vector10s pose9toPose10(Vector9s _pose9)
// Input pose9 and covariance
Vector9s pose(Vector9s::Random());
Vector10s pose10 = pose9toPose10(pose);
-Vector9s data_var((Vector9s() << 0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2).finished());
-Eigen::Matrix<wolf::Scalar, 9, 9> data_cov = data_var.asDiagonal();
+Eigen::Matrix<wolf::Scalar, 9, 9> data_cov = 0.2 * Eigen::Matrix<Scalar,9,9>::Identity();
// perturbated priors
Vector10s x0 = pose9toPose10(pose + Vector9s::Random()*0.25);
// Problem and solver
-ProblemPtr problem = Problem::create("POV 3D");
-CeresManager ceres_mgr(problem);
+ProblemPtr problem_ptr = Problem::create("POV 3D");
+CeresManager ceres_mgr(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0));
// Capture, feature and constraint
CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS", 0, nullptr, pose10, 10, 9, nullptr));
@@ -47,22 +46,13 @@ CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS"
* Both features and constraints are created in the TEST(). Hence, tests will not interfere each others.
*/
-TEST(ConstraintBlockAbs, ctr_block_abs_p_check)
+TEST(ConstraintBlockAbs, ctr_block_abs_p)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
- ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
- fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr()))
- );
- ASSERT_TRUE(problem->check(0));
-}
+ fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr()));
+
+ ASSERT_TRUE(problem_ptr->check(0));
-TEST(ConstraintBlockAbs, ctr_block_abs_p_solve)
-{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
- ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
- fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr()))
- );
-
// Unfix frame 0, perturb frm0
frm0->unfix();
frm0->setState(x0);
@@ -71,24 +61,31 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p_solve)
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only orientation is constrained
- ASSERT_MATRIX_APPROX(frm0->getState().head<3>(), pose10.head<3>(), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getPPtr()->getState(), pose10.head<3>(), 1e-6);
}
-TEST(ConstraintBlockAbs, ctr_block_abs_v_check)
+TEST(ConstraintBlockAbs, ctr_block_abs_p_tail2)
{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
- ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
- fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr()))
- );
- ASSERT_TRUE(problem->check(0));
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>()));
+ fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr(),1,2));
+
+ // Unfix frame 0, perturb frm0
+ frm0->unfix();
+ frm0->setState(x0);
+
+ // solve for frm0
+ std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+
+ //only orientation is constrained
+ ASSERT_MATRIX_APPROX(frm0->getPPtr()->getState().tail<2>(), pose10.segment<2>(1), 1e-6);
}
-TEST(ConstraintBlockAbs, ctr_block_abs_v_solve)
+TEST(ConstraintBlockAbs, ctr_block_abs_v)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
- ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
- fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr()))
- );
+ fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr()));
+
+ ASSERT_TRUE(problem_ptr->check(0));
// Unfix frame 0, perturb frm0
frm0->unfix();
@@ -98,24 +95,15 @@ TEST(ConstraintBlockAbs, ctr_block_abs_v_solve)
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only velocity is constrained
- ASSERT_MATRIX_APPROX(frm0->getState().tail<3>(), pose10.tail<3>(), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getVPtr()->getState(), pose10.tail<3>(), 1e-6);
}
-TEST(ConstraintQuatAbs, ctr_block_abs_o_check)
+TEST(ConstraintQuatAbs, ctr_block_abs_o)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
- ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
- fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()))
- );
- ASSERT_TRUE(problem->check(0));
-}
+ fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()));
-TEST(ConstraintQuatAbs, ctr_block_abs_o_solve)
-{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
- ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
- fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()))
- );
+ ASSERT_TRUE(problem_ptr->check(0));
// Unfix frame 0, perturb frm0
frm0->unfix();
@@ -125,7 +113,7 @@ TEST(ConstraintQuatAbs, ctr_block_abs_o_solve)
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only velocity is constrained
- ASSERT_MATRIX_APPROX(frm0->getState().segment<4>(3), pose10.segment<4>(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getOPtr()->getState(), pose10.segment<4>(3), 1e-6);
}
int main(int argc, char **argv)
diff --git a/test/gtest_constraint_autodiff.cpp b/test/gtest_constraint_autodiff.cpp
index 5138bb89960eba132e32d9153a53c02bc603e700..9305f71ea64703cc29ecb741c38f017f4afdd85d 100644
--- a/test/gtest_constraint_autodiff.cpp
+++ b/test/gtest_constraint_autodiff.cpp
@@ -23,7 +23,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);
@@ -55,7 +58,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);
@@ -99,7 +105,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);
@@ -178,7 +187,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/test/gtest_constraint_odom_3D.cpp b/test/gtest_constraint_odom_3D.cpp
index d71050ec6534cd1e0ed1c01a94f35fd927ba6e93..945d49fbf7aeb42564f64cc06c45cd4408ffda7e 100644
--- a/test/gtest_constraint_odom_3D.cpp
+++ b/test/gtest_constraint_odom_3D.cpp
@@ -33,12 +33,12 @@ Vector7s x0 = data2delta(Vector6s::Random()*0.25);
Vector7s x1 = data2delta(data + Vector6s::Random()*0.25);
// Problem and solver
-ProblemPtr problem = Problem::create("PO 3D");
-CeresManager ceres_mgr(problem);
+ProblemPtr problem_ptr = Problem::create("PO 3D");
+CeresManager ceres_mgr(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
-FrameBasePtr frm1 = problem->emplaceFrame(KEY_FRAME, delta, TimeStamp(1));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY_FRAME, delta, TimeStamp(1));
// Capture, feature and constraint from frm1 to frm0
CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 1, nullptr, delta, 7, 6, nullptr));
@@ -50,7 +50,7 @@ ConstraintBasePtr dummy = frm0->addConstrainedBy(ctr1);
TEST(ConstraintOdom3D, check_tree)
{
- ASSERT_TRUE(problem->check(0));
+ ASSERT_TRUE(problem_ptr->check(0));
}
TEST(ConstraintOdom3D, expectation)
@@ -83,7 +83,7 @@ TEST(ConstraintOdom3D, fix_1_solve)
// solve for frm0
std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(frm0->getState(), problem->zeroState(), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getState(), problem_ptr->zeroState(), 1e-6);
}
int main(int argc, char **argv)
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index e5a4d4f3c837e855929bd3f24fff98692227194e..98e032cb3ea7b0ba8f5bb975c1ed7a7adc43f4e5 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -52,7 +52,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);
@@ -63,7 +64,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/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6bb1eccbc1cac0070a412b63e7ebcf3a62d238b1
--- /dev/null
+++ b/test/gtest_param_prior.cpp
@@ -0,0 +1,85 @@
+/*
+ * gtest_param_prior.cpp
+ *
+ * Created on: Feb 6, 2019
+ * Author: jvallve
+ */
+
+#include "utils_gtest.h"
+#include "base/logging.h"
+
+#include "base/problem.h"
+#include "base/ceres_wrapper/ceres_manager.h"
+#include "base/sensor/sensor_odom_3D.h"
+
+#include <iostream>
+
+using namespace wolf;
+
+ProblemPtr problem_ptr = Problem::create("PO 3D");
+CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
+Eigen::Vector7s initial_extrinsics((Eigen::Vector7s() << 1, 2, 3, 1, 0, 0, 0).finished());
+Eigen::Vector7s prior_extrinsics((Eigen::Vector7s() << 10, 20, 30, 0, 0, 0, 1).finished());
+Eigen::Vector7s prior2_extrinsics((Eigen::Vector7s() << 100, 200, 300, 0, 0, 0, 1).finished());
+
+SensorOdom3DPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3D>(problem_ptr->installSensor("ODOM 3D", "odometer", initial_extrinsics, std::make_shared<IntrinsicsOdom3D>()));
+
+TEST(ParameterPrior, initial_extrinsics)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+ ASSERT_TRUE(odom_sensor_ptr_->getPPtr());
+ ASSERT_TRUE(odom_sensor_ptr_->getOPtr());
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState(),initial_extrinsics.head(3),1e-9);
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getOPtr()->getState(),initial_extrinsics.tail(4),1e-9);
+}
+
+TEST(ParameterPrior, prior_p)
+{
+ odom_sensor_ptr_->addPriorP(prior_extrinsics.head(3),Eigen::Matrix3s::Identity());
+
+ std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState(),prior_extrinsics.head(3),1e-6);
+}
+
+TEST(ParameterPrior, prior_o)
+{
+ odom_sensor_ptr_->addPriorO(prior_extrinsics.tail(4),Eigen::Matrix3s::Identity());
+
+ std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getOPtr()->getState(),prior_extrinsics.tail(4),1e-6);
+}
+
+TEST(ParameterPrior, prior_p_overwrite)
+{
+ odom_sensor_ptr_->addPriorP(prior2_extrinsics.head(3),Eigen::Matrix3s::Identity());
+
+ std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState(),prior2_extrinsics.head(3),1e-6);
+}
+
+TEST(ParameterPrior, prior_p_segment)
+{
+ odom_sensor_ptr_->addPriorP(prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2);
+
+ std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState().segment(1,2),prior_extrinsics.segment(1,2),1e-6);
+}
+
+TEST(ParameterPrior, prior_o_segment)
+{
+ // constraining segment of quaternion is not allowed
+ ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter(1, prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2),"");
+}
+
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp
index d182bb4f376f579e498e6bc281ecf1d845a39315..d94815f9210090c887bebde5d416ce8fbeaf0e4f 100644
--- a/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -81,7 +81,7 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
intr);
ProcessorParamsTrackerFeatureTrifocalPtr params_tracker_feature_trifocal = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>();
- params_tracker_feature_trifocal->name = "trifocal";
+// 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;
diff --git a/test/gtest_sensor_camera.cpp b/test/gtest_sensor_camera.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..14b24deb1a0e23b39dc9bb55d1cf24355bd1a357
--- /dev/null
+++ b/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 "src/sensor/sensor_camera.cpp"
+#include "base/sensor/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();
+}
+