diff --git a/src/constraint_analytic.cpp b/src/constraint_analytic.cpp
index 8b0422380bc4ad0d39f2e7f38955ea3b0bd948a8..1d10e7a06c81a0370eed2df600e9ab0242398264 100644
--- a/src/constraint_analytic.cpp
+++ b/src/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/constraint_analytic.h b/src/constraint_analytic.h
index 17ff4a079a55efc4908611801f11d95b9aa86305..93be1b1af130516c1fd7a0c2b8f0ce822733da2c 100644
--- a/src/constraint_analytic.h
+++ b/src/constraint_analytic.h
@@ -83,7 +83,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
@@ -118,7 +117,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
@@ -132,9 +131,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
@@ -144,11 +143,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/src/constraint_block_absolute.h b/src/constraint_block_absolute.h
index fe8eec7a9f01095838c0572d4517e4493d85fa3a..e3def4493f0444ada07ed097e737610e5459671b 100644
--- a/src/constraint_block_absolute.h
+++ b/src/constraint_block_absolute.h
@@ -9,7 +9,7 @@
#define CONSTRAINT_BLOCK_ABSOLUTE_H_
//Wolf includes
-#include "constraint_autodiff.h"
+#include "constraint_analytic.h"
#include "frame_base.h"
@@ -18,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/src/constraint_relative_2D_analytic.h b/src/constraint_relative_2D_analytic.h
index 32b8b30593a73717be3fccc4459652da17dce616..53dd71ef1037165d6d9efe460bb23867d5ad4f27 100644
--- a/src/constraint_relative_2D_analytic.h
+++ b/src/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
*
**/
@@ -125,11 +124,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/src/problem.cpp b/src/problem.cpp
index fec23b17497b920b575f3f80c05a9db5882e9bc8..f80a99b4dcb9bcabdf2b896c96a531dfdc03ee7c 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -754,7 +754,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_odom_2D.cpp b/src/processor_odom_2D.cpp
index 19818dffcc1aee098c316f1b13bf603703333c39..5041ae985c60770b0c8124f753bcc802574f88ee 100644
--- a/src/processor_odom_2D.cpp
+++ b/src/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/sensor_base.cpp b/src/sensor_base.cpp
index 429aaf9f142d08a3374d8242df8e79379e5c6d90..947984669e5c7cad26a0ca0c9c7d99732f2652f3 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -1,5 +1,8 @@
#include "sensor_base.h"
#include "state_block.h"
+#include "state_quaternion.h"
+#include "constraint_block_absolute.h"
+#include "constraint_quaternion_absolute.h"
namespace wolf {
@@ -63,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++)
@@ -103,8 +82,6 @@ void SensorBase::removeStateBlocks()
}
}
-
-
void SensorBase::fix()
{
for( auto sbp : state_block_vec_)
@@ -165,7 +142,49 @@ 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()
{
@@ -243,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
@@ -322,32 +341,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_base.h b/src/sensor_base.h
index 98a95e2c59e6f0248b83d90ea131a5008cde7d80..992058512a2bd6d29a253653a988ccbe932c8ad1 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -47,6 +47,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
*
@@ -87,7 +89,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();
@@ -108,11 +109,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);
@@ -131,6 +137,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;
@@ -194,6 +226,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;
}
@@ -250,6 +284,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/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 219a8a27999e9a512ab6d1585a0f67d0a425494f..47e53807a4afa37b9fe04e6cb80fb702ad007986 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -168,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})
diff --git a/src/test/gtest_constraint_absolute.cpp b/src/test/gtest_constraint_absolute.cpp
index a0587e29ce96207c1e7bdaa05fb4f7e4a9b5cbc4..de55b4767d7edefdc39f04fb67691b3e5511ed9d 100644
--- a/src/test/gtest_constraint_absolute.cpp
+++ b/src/test/gtest_constraint_absolute.cpp
@@ -27,18 +27,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));
@@ -49,22 +48,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);
@@ -73,24 +63,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();
@@ -100,24 +97,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();
@@ -127,7 +115,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/src/test/gtest_constraint_odom_3D.cpp b/src/test/gtest_constraint_odom_3D.cpp
index 68408277551644f6e04abd3a1bb3894ef0bb57af..17cb8e9e65d4139b7393fb0a40a9e96715f1f783 100644
--- a/src/test/gtest_constraint_odom_3D.cpp
+++ b/src/test/gtest_constraint_odom_3D.cpp
@@ -35,12 +35,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));
@@ -52,7 +52,7 @@ ConstraintBasePtr dummy = frm0->addConstrainedBy(ctr1);
TEST(ConstraintOdom3D, check_tree)
{
- ASSERT_TRUE(problem->check(0));
+ ASSERT_TRUE(problem_ptr->check(0));
}
TEST(ConstraintOdom3D, expectation)
@@ -85,7 +85,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/src/test/gtest_param_prior.cpp b/src/test/gtest_param_prior.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0048fa2ef7f7a2f09505a7d5dc1d616bfafb8d1f
--- /dev/null
+++ b/src/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 "../src/logging.h"
+
+#include "../problem.h"
+#include "../ceres_wrapper/ceres_manager.h"
+#include "../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();
+}
+