diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index f996584d4963da450daf0dda49ad89721824de7e..8d6f178c40197778c3aecc4bf02d07ddd61dd05f 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -257,7 +257,7 @@ void CeresManager::update()
{
case ADD:
{
- //std::cout << "adding constraint" << std::endl;
+ std::cout << "adding constraint" << std::endl;
addConstraint(wolf_problem_->getConstraintNotificationList().front().constraint_ptr_,wolf_problem_->getConstraintNotificationList().front().id_);
//std::cout << "added" << std::endl;
break;
@@ -268,9 +268,9 @@ void CeresManager::update()
wolf_problem_->getConstraintNotificationList().pop_front();
}
//std::cout << "all constraints added" << std::endl;
- //std::cout << "ceres residual blocks: " << ceres_problem_->NumResidualBlocks() << std::endl;
- //std::cout << "wrapper residual blocks: " << id_2_residual_idx_.size() << std::endl;
- //std::cout << "parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
+ std::cout << "ceres residual blocks: " << ceres_problem_->NumResidualBlocks() << std::endl;
+ std::cout << "wrapper residual blocks: " << id_2_residual_idx_.size() << std::endl;
+ std::cout << "parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
assert(ceres_problem_->NumResidualBlocks() == id_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
@@ -300,18 +300,18 @@ void CeresManager::removeConstraint(const unsigned int& _corr_id)
void CeresManager::addStateBlock(StateBlockPtr _st_ptr)
{
-// std::cout << "Adding State Block " << _st_ptr->getPtr() << std::endl;
-// std::cout << " size: " << _st_ptr->getSize() << std::endl;
-// std::cout << " vector: " << _st_ptr->getVector().transpose() << std::endl;
+ std::cout << "Adding State Block " << _st_ptr->getPtr() << std::endl;
+ std::cout << " size: " << _st_ptr->getSize() << std::endl;
+ std::cout << " vector: " << _st_ptr->getVector().transpose() << std::endl;
if (_st_ptr->hasLocalParametrization())
{
- //std::cout << "Local Parametrization to be added:" << _st_ptr->getLocalParametrizationPtr() << std::endl;
+ std::cout << "Local Parametrization to be added:" << _st_ptr->getLocalParametrizationPtr() << std::endl;
ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getSize(), new LocalParametrizationWrapper(_st_ptr->getLocalParametrizationPtr()));
}
else
{
- //std::cout << "No Local Parametrization to be added" << std::endl;
+ std::cout << "No Local Parametrization to be added" << std::endl;
ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getSize(), nullptr);
}
if (_st_ptr->isFixed())
diff --git a/src/constraint_AHP.h b/src/constraint_AHP.h
index f74eaf2e9a413a9c43baf13843de6af6319ea569..5b27da72e10d2ea3fbd9ebedb8f92f42297146c5 100644
--- a/src/constraint_AHP.h
+++ b/src/constraint_AHP.h
@@ -28,7 +28,7 @@ class ConstraintAHP : public ConstraintSparse<2, 3, 4, 3, 4, 4>
public:
- ConstraintAHP(FeatureBasePtr _ftr_ptr, FrameBasePtr _current_frame_ptr, std::shared_ptr<LandmarkAHP> _landmark_ptr,
+ ConstraintAHP(FeatureBasePtr _ftr_ptr, FrameBasePtr _current_frame_ptr, LandmarkAHP::Ptr _landmark_ptr,
bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintSparse<2, 3, 4, 3, 4, 4>(CTR_AHP, _landmark_ptr, _apply_loss_function, _status,
_current_frame_ptr->getPPtr(), _current_frame_ptr->getOPtr(), _landmark_ptr->getAnchorFrame()->getPPtr()
@@ -39,6 +39,8 @@ class ConstraintAHP : public ConstraintSparse<2, 3, 4, 3, 4, 4>
{
setType("AHP");
+ frame_other_ptr_ = _landmark_ptr->getAnchorFrame();
+
K_ = (std::static_pointer_cast<SensorCamera>(_ftr_ptr->getCapturePtr()->getSensorPtr()))->getIntrinsicMatrix();
distortion_ = (std::static_pointer_cast<SensorCamera>(_ftr_ptr->getCapturePtr()->getSensorPtr()))->getDistortionVector();
}
@@ -52,6 +54,7 @@ class ConstraintAHP : public ConstraintSparse<2, 3, 4, 3, 4, 4>
void expectation(const T* const _current_frame_p, const T* const _current_frame_o, const T* const _anchor_frame_p,
const T* const _anchor_frame_o, const T* const _lmk_hmg, T* _expectation) const
{
+ // Maps over the input pointers
Eigen::Matrix<T, 3, 3> K = K_.cast<T>();
Eigen::Map<const Eigen::Matrix<T, 4, 1> > landmark_hmg_c0(_lmk_hmg);
@@ -80,72 +83,77 @@ class ConstraintAHP : public ConstraintSparse<2, 3, 4, 3, 4, 4>
T_R0_C0 = trc * qrc;
T_R1_C1 = T_R0_C0;
+ // hmg point in C1 frame (current frame)
Eigen::Matrix<T,4,1> landmark_hmg_c1;
landmark_hmg_c1 = T_R1_C1.inverse(Eigen::Affine) * T_W_R1.inverse(Eigen::Affine) * T_W_R0 * T_R0_C0 * landmark_hmg_c0;
-// std::cout << "\nlandmark_hmg_c1:\n" << landmark_hmg_c1(0) << "\t" << landmark_hmg_c1(1) << "\t" << landmark_hmg_c1(2) << "\t" << landmark_hmg_c1(3) << std::endl;
+ // std::cout << "\nlandmark_hmg_c1:\n" << landmark_hmg_c1(0) << "\t" << landmark_hmg_c1(1) << "\t" << landmark_hmg_c1(2) << "\t" << landmark_hmg_c1(3) << std::endl;
- Eigen::Matrix<T,3,1> v_normalized;
- v_normalized = landmark_hmg_c1.head(3);// /landmark_hmg_c1(3);
-// T inverse_dist_c1 = landmark_hmg_c1(3); // inverse distance
+ // lmk direction vector
+ Eigen::Matrix<T,3,1> v_dir = landmark_hmg_c1.head(3);
-// std::cout << "\nv_normalized:\n" << v_normalized(0) << "\t" << v_normalized(1) << "\t"
-// << v_normalized(2) << "\t" << landmark_hmg_c1(3) << std::endl;
+// std::cout << "\nv_normalized:\n" << v_dir(0) << "\t" << v_dir(1) << "\t"
+// << v_dir(2) << "\t" << landmark_hmg_c1(3) << std::endl;
- Eigen::Matrix<T,2,1> v;
- v = v_normalized.head(2)/v_normalized(2);
-// std::cout << "\nv:\n" << v(0) << "\t" << v(1) << std::endl;
+ // projected point in canonical sensor
+ Eigen::Matrix<T,2,1> point_undistorted;
+ point_undistorted = v_dir.head(2)/v_dir(2);
+ // std::cout << "\nv:\n" << point_undistorted(0) << "\t" << point_undistorted(1) << std::endl;
- Eigen::Matrix<T,2,1> distored_point;
Eigen::Matrix<T,Eigen::Dynamic,1> distortion_vector = distortion_.cast<T>();
-// std::cout << "\ndistortion_vector:\n" << distortion_vector(0) << "\t" << distortion_vector(1) << std::endl;
-
- Eigen::Matrix<T,2,1> distored_point_pinhole;
- distored_point_pinhole = pinhole::distortPoint(distortion_vector, v);
-
-// T r2 = v.squaredNorm(); // this is the norm squared: r2 = ||u||^2
-//
-// T s = (T)1.0;
-// T r2i = (T)1.0;
-// for (int i = 0; i < distortion_vector.size() ; i++) { // here we are doing:
-// r2i = r2i * r2; // r2i = r^(2*(i+1))
-// s = s + (distortion_vector(i) * r2i); // s = 1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + ...
-// }
-// if (s < (T)0.6) s = (T)1.0;
-// distored_point(0) = s * v(0);
-// distored_point(1) = s * v(1);
-// std::cout << "\ndistored_point:\n" << distored_point(0) << "\t" << distored_point(1) << std::endl;
-// std::cout << "\ndistored_point_pinhole:\n" << distored_point_pinhole(0) << "\t" << distored_point_pinhole(1) << std::endl;
-
-// std::cout << "K: " << K << std::endl;
+ // std::cout << "\ndistortion_vector:\n" << distortion_vector(0) << "\t" << distortion_vector(1) << std::endl;
+
+ // distort point
+ Eigen::Matrix<T,2,1> point_distorted = pinhole::distortPoint(distortion_vector, point_undistorted);
+ // pixellize
Eigen::Map<Eigen::Matrix<T, 2, 1> > expectation(_expectation);
- expectation(0) = K(0,0)*distored_point_pinhole(0)+K(0,2);
- expectation(1) = K(1,1)*distored_point_pinhole(1)+K(1,2);
+ expectation(0) = K(0,0) * point_distorted(0) + K(0,2);
+ expectation(1) = K(1,1) * point_distorted(1) + K(1,2);
-// std::cout << "constraint n[" << id() << "] _expectation: " << _expectation(0) << "\t" << _expectation(1) << std::endl;
+ // std::cout << "constraint n[" << id() << "] _expectation: " << _expectation(0) << "\t" << _expectation(1) << std::endl;
-// return u;
+ }
+
+ Eigen::VectorXs expectation() const
+ {
+ Eigen::VectorXs exp(2);
+ FrameBasePtr frm_current = getFeaturePtr()->getCapturePtr()->getFramePtr();
+ FrameBasePtr frm_anchor = getFrameOtherPtr();
+ LandmarkBasePtr lmk = getLandmarkOtherPtr();
+ const Scalar * const frame_current_pos = frm_current->getPPtr()->getVector().data();
+ const Scalar * const frame_current_ori = frm_current->getOPtr()->getVector().data();
+ const Scalar * const frame_anchor_pos = frm_anchor->getPPtr()->getVector().data();
+ const Scalar * const frame_anchor_ori = frm_anchor->getOPtr()->getVector().data();
+ const Scalar * const lmk_pos_hmg = lmk->getPPtr()->getVector().data();
+ expectation(frame_current_pos,
+ frame_current_ori,
+ frame_anchor_pos,
+ frame_anchor_ori,
+ lmk_pos_hmg,
+ exp.data());
+ return exp;
}
template<typename T>
bool operator ()(const T* const _current_frame_p, const T* const _current_frame_o, const T* const _anchor_frame_p,
const T* const _anchor_frame_o, const T* const _lmk_hmg, T* _residuals) const
{
-
- Eigen::Matrix<T, Eigen::Dynamic, 1> u(2) ;
+// std::cout << "operator: " << id() << std::endl;
+ Eigen::Matrix<T, Eigen::Dynamic, 1> expected(2) ;
expectation(_current_frame_p, _current_frame_o, _anchor_frame_p,
- _anchor_frame_o, _lmk_hmg, u.data()) ;
- // ==================================================
+ _anchor_frame_o, _lmk_hmg, expected.data()) ;
+
+ Eigen::Matrix<T, 2, 1> measured = getMeasurement().cast<T>();
- // std::cout << "\nCONSTRAINT INFO" << std::endl;
+ Eigen::Map<Eigen::Matrix<T, 2, 1> > residuals(_residuals);
- Eigen::Matrix<T, 2, 1> feature_pos = getMeasurement().cast<T>();
+ residuals = getMeasurementSquareRootInformation().cast<T>() * (expected - measured);
- Eigen::Map<Eigen::Matrix<T, 2, 1> > residualsmap(_residuals);
-// std::cout << "SquareRootInformation: " << getMeasurementSquareRootInformation() << std::endl;
- residualsmap = getMeasurementSquareRootInformation().cast<T>() * (u - feature_pos);
-// std::cout << "\nRESIDUALS:\n" << residualsmap[0] << "\t" << residualsmap[1] << std::endl;
+ // debug info:
+ Eigen::Map<const Eigen::Matrix< T, 4, 1> > landmark(_lmk_hmg);
+// std::cout << "\nLANDMARK L" << getLandmarkOtherPtr()->id() << ":\n\t" << landmark[0] << "\n\t" << landmark[1] << "\n\t" << landmark[2] << "\n\t" << landmark[3] << std::endl;
+// std::cout << "\nRESIDUALS c" << id() << ":\n\t" << residuals[0] << "\n\t" << residuals[1] << std::endl;
return true;
}
@@ -157,6 +165,19 @@ class ConstraintAHP : public ConstraintSparse<2, 3, 4, 3, 4, 4>
return JAC_AUTO;
}
+ static ConstraintAHP::Ptr create(FeatureBasePtr _ftr_ptr, FrameBasePtr _frm_current_ptr, LandmarkAHP::Ptr _lmk_ahp_ptr)
+ {
+ // construct constraint
+ Ptr ctr_ahp = std::make_shared<ConstraintAHP>(_ftr_ptr, _frm_current_ptr, _lmk_ahp_ptr);
+
+ // link it to wolf tree
+// _ftr_ptr->addConstraint(ctr_ahp);
+ ctr_ahp->setFrameOtherPtr(_lmk_ahp_ptr->getAnchorFrame());
+ _lmk_ahp_ptr->getAnchorFrame()->addConstrainedBy(ctr_ahp);
+ _lmk_ahp_ptr->addConstrainedBy(ctr_ahp);
+ return ctr_ahp;
+ }
+
};
diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp
index 04cc5475b5cd64ef68d5135bcfad500f584fc0eb..cf0cf4862229000de839ce108a0e18d6c95eddfb 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -133,7 +133,7 @@ void ConstraintBase::remove()
default:
break;
}
- std::cout << "Removed c" << id() << std::endl;
+ std::cout << "Removed c" << id() << std::endl;
}
}
diff --git a/src/constraint_base.h b/src/constraint_base.h
index ea6992127479804d5dd7278cb805b48d3d5836f5..e01a3c6879ebdb7e39c84dee531085532b66174a 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -119,17 +119,17 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
/** \brief Returns a pointer to the frame constrained to
**/
- FrameBasePtr getFrameOtherPtr();
+ FrameBasePtr getFrameOtherPtr() const;
void setFrameOtherPtr(FrameBasePtr _frm_o){frame_other_ptr_ = _frm_o;}
/** \brief Returns a pointer to the feature constrained to
**/
- FeatureBasePtr getFeatureOtherPtr();
+ FeatureBasePtr getFeatureOtherPtr() const;
void setFeatureOtherPtr(FeatureBasePtr _ftr_o){feature_other_ptr_ = _ftr_o;}
/** \brief Returns a pointer to the landmark constrained to
**/
- LandmarkBasePtr getLandmarkOtherPtr();
+ LandmarkBasePtr getLandmarkOtherPtr() const;
void setLandmarkOtherPtr(LandmarkBasePtr _lmk_o){landmark_other_ptr_ = _lmk_o;}
ProblemPtr getProblem();
@@ -209,17 +209,17 @@ inline void ConstraintBase::setApplyLossFunction(const bool _apply)
}
}
-inline FrameBasePtr ConstraintBase::getFrameOtherPtr()
+inline FrameBasePtr ConstraintBase::getFrameOtherPtr() const
{
return frame_other_ptr_.lock();
}
-inline FeatureBasePtr ConstraintBase::getFeatureOtherPtr()
+inline FeatureBasePtr ConstraintBase::getFeatureOtherPtr() const
{
return feature_other_ptr_.lock();
}
-inline LandmarkBasePtr ConstraintBase::getLandmarkOtherPtr()
+inline LandmarkBasePtr ConstraintBase::getLandmarkOtherPtr() const
{
return landmark_other_ptr_.lock();
}
diff --git a/src/constraint_odom_3D.h b/src/constraint_odom_3D.h
index 7c07105da7bbbe56c43dc9ee7d7ad0c66757cb63..7dd7ea6e37d43f99302565b8f38f406c92539ad6 100644
--- a/src/constraint_odom_3D.h
+++ b/src/constraint_odom_3D.h
@@ -31,6 +31,16 @@ class ConstraintOdom3D : public ConstraintSparse<6,3,4,3,4>
const T* const _q2,
T* _residuals) const;
+ template<typename T>
+ bool expectation(const T* const _p1,
+ const T* const _q1,
+ const T* const _p2,
+ const T* const _q2,
+ T* _expectation_dp,
+ T* _expectation_dq) const;
+
+ Eigen::VectorXs expectation() const;
+
private:
template<typename T>
void printRes(const Eigen::Matrix<T, 6, 1>& r) const;
@@ -75,6 +85,54 @@ inline ConstraintOdom3D::~ConstraintOdom3D()
//
}
+template<typename T>
+inline bool wolf::ConstraintOdom3D::expectation(const T* const _p1, const T* const _q1, const T* const _p2,
+ const T* const _q2, T* _expectation_dp, T* _expectation_dq) const
+{
+ Eigen::Map<const Eigen::Matrix<T,3,1> > p1(_p1);
+ Eigen::Map<const Eigen::Quaternion<T> > q1(_q1);
+// Eigen::Map<const Eigen::Matrix<T,4,1> > q1_v(_q1);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > p2(_p2);
+ Eigen::Map<const Eigen::Quaternion<T> > q2(_q2);
+// Eigen::Map<const Eigen::Matrix<T,4,1> > q2_v(_q2);
+ Eigen::Map<Eigen::Matrix<T,3,1> > expectation_dp(_expectation_dp);
+// Eigen::Map<Eigen::Matrix<T,4,1> > expectation_dq_v(_expectation_dq);
+ Eigen::Map<Eigen::Quaternion<T> > expectation_dq(_expectation_dq);
+
+
+ // std::cout << "p1: " << p1(0) << std::endl << p1(1) << std::endl << p1(2) << std::endl;
+ // std::cout << "q1: " << q1_v(0) << std::endl << q1_v(1) << std::endl << q1_v(2) << std::endl << q1_v(3) << std::endl;
+ // std::cout << "p2: " << p2(0) << std::endl << p2(1) << std::endl << p2(2) << std::endl;
+ // std::cout << "q2: " << q2_v(0) << std::endl << q2_v(1) << std::endl << q2_v(2) << std::endl << q2_v(3) << std::endl;
+
+ // estimate motion increment, dp, dq;
+ expectation_dp = q1.conjugate() * (p2 - p1);
+ expectation_dq = q1.conjugate() * q2;
+
+ std::cout << "exp_p: " << expectation_dp(0) << std::endl << expectation_dp(1) << std::endl << expectation_dp(2) << std::endl;
+// std::cout << "exp_q: " << expectation(3) << std::endl << expectation(4) << std::endl << expectation(5) << std::endl << expectation(6) << std::endl;
+
+ return true;
+}
+
+inline Eigen::VectorXs wolf::ConstraintOdom3D::expectation() const
+{
+ Eigen::VectorXs exp(7);
+ FrameBasePtr frm_current = getFeaturePtr()->getFramePtr();
+ FrameBasePtr frm_origin = getFrameOtherPtr();
+ const Scalar * const frame_current_pos = frm_current->getPPtr()->getVector().data();
+ const Scalar * const frame_current_ori = frm_current->getOPtr()->getVector().data();
+ const Scalar * const frame_origin_pos = frm_origin->getPPtr()->getVector().data();
+ const Scalar * const frame_origin_ori = frm_origin->getOPtr()->getVector().data();
+ expectation(frame_current_pos,
+ frame_current_ori,
+ frame_origin_pos,
+ frame_origin_ori,
+ exp.data(),
+ exp.data()+3);
+ return exp;
+}
+
template<typename T>
inline bool wolf::ConstraintOdom3D::operator ()(const T* const _p1, const T* const _q1, const T* const _p2,
const T* const _q2, T* _residuals) const
@@ -112,22 +170,27 @@ inline bool wolf::ConstraintOdom3D::operator ()(const T* const _p1, const T* con
*/
// MAPS
- Eigen::Map<const Eigen::Matrix<T,3,1> > p1(_p1);
- Eigen::Map<const Eigen::Quaternion<T> > q1(_q1);
- Eigen::Map<const Eigen::Matrix<T,4,1> > q1_v(_q1);
- Eigen::Map<const Eigen::Matrix<T,3,1> > p2(_p2);
- Eigen::Map<const Eigen::Quaternion<T> > q2(_q2);
- Eigen::Map<const Eigen::Matrix<T,4,1> > q2_v(_q2);
+// Eigen::Map<const Eigen::Matrix<T,3,1> > p1(_p1);
+// Eigen::Map<const Eigen::Quaternion<T> > q1(_q1);
+// Eigen::Map<const Eigen::Matrix<T,4,1> > q1_v(_q1);
+// Eigen::Map<const Eigen::Matrix<T,3,1> > p2(_p2);
+// Eigen::Map<const Eigen::Quaternion<T> > q2(_q2);
+// Eigen::Map<const Eigen::Matrix<T,4,1> > q2_v(_q2);
Eigen::Map<Eigen::Matrix<T,6,1> > residuals(_residuals);
+//
+// // std::cout << "p1: " << p1(0) << std::endl << p1(1) << std::endl << p1(2) << std::endl;
+// // std::cout << "q1: " << q1_v(0) << std::endl << q1_v(1) << std::endl << q1_v(2) << std::endl << q1_v(3) << std::endl;
+// // std::cout << "p2: " << p2(0) << std::endl << p2(1) << std::endl << p2(2) << std::endl;
+// // std::cout << "q2: " << q2_v(0) << std::endl << q2_v(1) << std::endl << q2_v(2) << std::endl << q2_v(3) << std::endl;
+//
+// // estimate motion increment, dp, dq;
+// Eigen::Matrix<T,3,1> dp = q1.conjugate() * (p2 - p1);
+// Eigen::Quaternion<T> dq = q1.conjugate() * q2;
+
+ Eigen::Matrix<T, Eigen::Dynamic, 1> expected(7) ;
+ expectation(_p1, _q1, _p2, _q2, expected.data(), expected.data()+3);
- // std::cout << "p1: " << p1(0) << std::endl << p1(1) << std::endl << p1(2) << std::endl;
- // std::cout << "q1: " << q1_v(0) << std::endl << q1_v(1) << std::endl << q1_v(2) << std::endl << q1_v(3) << std::endl;
- // std::cout << "p2: " << p2(0) << std::endl << p2(1) << std::endl << p2(2) << std::endl;
- // std::cout << "q2: " << q2_v(0) << std::endl << q2_v(1) << std::endl << q2_v(2) << std::endl << q2_v(3) << std::endl;
- // estimate motion increment, dp, dq;
- Eigen::Matrix<T,3,1> dp = q1.conjugate() * (p2 - p1);
- Eigen::Quaternion<T> dq = q1.conjugate() * q2;
// std::cout << "dp: " << dp(0) << std::endl << dp(1) << std::endl<< dp(2) << std::endl;
// std::cout << "dq: " << dq.x() << std::endl << dq.y() << std::endl << dq.z() << std::endl << dq.w() << std::endl;
@@ -140,6 +203,19 @@ inline bool wolf::ConstraintOdom3D::operator ()(const T* const _p1, const T* con
// residual
// residuals.head<3>() = dq.conjugate() * (dp_m - dp); // see note below
+// residuals.head(3) = dp_m - dp; // being a residual, rotating it has no implications, so we skip the product by dq.conj
+// residuals.tail(3) = q2v(dq.conjugate() * dq_m);
+
+ Eigen::Matrix<T,3,1> dp = expected.head(3);
+ Eigen::Quaternion<T> dq;// = expected.tail(4);
+ dq.x() = expected(3);
+ dq.y() = expected(4);
+ dq.z() = expected(5);
+ dq.w() = expected(6);
+
+ std::cout << "operator dp: " << dp(0) << std::endl << dp(1) << std::endl << dp(2) << std::endl;
+ std::cout << "operator dq: " << dq.x() << std::endl << dq.y() << std::endl << dq.z() << std::endl << dq.w() << std::endl;
+
residuals.head(3) = dp_m - dp; // being a residual, rotating it has no implications, so we skip the product by dq.conj
residuals.tail(3) = q2v(dq.conjugate() * dq_m);
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 4804687231cd80de6bbbf188da6fe517c63c9076..9877404f2a42fe199c43af103c365e9e44eca83d 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -10,6 +10,9 @@ ADD_EXECUTABLE(test_sh_ptr test_sh_ptr.cpp)
ADD_EXECUTABLE(test_wolf_root test_wolf_root.cpp)
+ADD_EXECUTABLE(test_kf_callback test_kf_callback.cpp)
+TARGET_LINK_LIBRARIES(test_kf_callback ${PROJECT_NAME})
+
ADD_EXECUTABLE(test_wolf_logging test_wolf_logging.cpp)
TARGET_LINK_LIBRARIES(test_wolf_logging ${PROJECT_NAME})
@@ -146,6 +149,10 @@ IF(OpenCV_FOUND)
TARGET_LINK_LIBRARIES(test_ROI_ORB ${PROJECT_NAME})
ENDIF(OpenCV_FOUND)
+# Simple AHP test
+ADD_EXECUTABLE(test_simple_AHP test_simple_AHP.cpp)
+TARGET_LINK_LIBRARIES(test_simple_AHP ${PROJECT_NAME})
+
# Processor Tracker Feature test
ADD_EXECUTABLE(test_processor_tracker_feature test_processor_tracker_feature.cpp)
TARGET_LINK_LIBRARIES(test_processor_tracker_feature ${PROJECT_NAME})
diff --git a/src/examples/camera_params_ueye_sim.yaml b/src/examples/camera_params_ueye_sim.yaml
index 61016aec6d9fe03e7bfca52029b2e35f6477cfd6..789aa54b2147d279a3303657b6b82dbbfb744177 100644
--- a/src/examples/camera_params_ueye_sim.yaml
+++ b/src/examples/camera_params_ueye_sim.yaml
@@ -4,7 +4,7 @@ camera_name: camera
camera_matrix:
rows: 3
cols: 3
- data: [392.796383, 0, 350.175772, 0, 392.779002, 255.209917, 0, 0, 1]
+ data: [320, 0, 320, 0, 320, 240, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
diff --git a/src/examples/odom_3D.yaml b/src/examples/odom_3D.yaml
index c05a9f7b08c6e085d585206d9ccf50f6bb20f11c..6f2ba202f2ea861eea637898f5b340134fa9c352 100644
--- a/src/examples/odom_3D.yaml
+++ b/src/examples/odom_3D.yaml
@@ -1,9 +1,9 @@
sensor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
sensor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
motion variances:
- disp_to_disp: 0.2 # m^2 / m
- disp_to_rot: 0.2 # rad^2 / m
- rot_to_rot: 0.1 # rad^2 / rad
- min_disp_var: 0.1 # m^2
- min_rot_var: 0.1 # rad^2
+ disp_to_disp: 0.002 # m^2 / m
+ disp_to_rot: 0.002 # rad^2 / m
+ rot_to_rot: 0.001 # rad^2 / rad
+ min_disp_var: 0.001 # m^2
+ min_rot_var: 0.001 # rad^2
\ No newline at end of file
diff --git a/src/examples/processor_image_ORB.yaml b/src/examples/processor_image_ORB.yaml
index 178d444c1508a3b968adb0fa52bcbc822718c787..bff6d5394cc544efa7da802d457da44717a4ae94 100644
--- a/src/examples/processor_image_ORB.yaml
+++ b/src/examples/processor_image_ORB.yaml
@@ -25,11 +25,11 @@ active search:
separation: 5
algorithm:
- maximum new features: 10
+ maximum new features: 40
minimum features for new keyframe: 40
minimum response for new features: 80 #0.0005
time tolerance: 0.01
- distance: 10
+ distance: 20
draw: # Used to control drawing options
primary draw: true
diff --git a/src/examples/test_constraint_AHP.cpp b/src/examples/test_constraint_AHP.cpp
index f7a38da43de0a17c6ae927906785f8d77d169d47..40c6130960f6c8a673ce81a7a6db094ea2ce1762 100644
--- a/src/examples/test_constraint_AHP.cpp
+++ b/src/examples/test_constraint_AHP.cpp
@@ -16,8 +16,7 @@ int main()
//=====================================================
// Environment variable for configuration files
-// GET_WOLF_ROOT
- std::string wolf_root("/home/jtarraso/dev/wolf");
+ std::string wolf_root = _WOLF_ROOT_DIR;
std::cout << wolf_root << std::endl;
//=====================================================
diff --git a/src/examples/test_image.cpp b/src/examples/test_image.cpp
index 58fd4cb882195a55e1d342ff71735d39d87164aa..6072dd720f5e774a6cf2f944a54ac76bc4b7dc64 100644
--- a/src/examples/test_image.cpp
+++ b/src/examples/test_image.cpp
@@ -66,11 +66,7 @@ int main(int argc, char** argv)
TimeStamp t = 1;
- char const* tmp = std::getenv( "WOLF_ROOT" );
- if ( tmp == nullptr )
- throw std::runtime_error("WOLF_ROOT environment not loaded.");
-
- std::string wolf_root( tmp );
+ std::string wolf_root = _WOLF_ROOT_DIR;
std::cout << "Wolf root: " << wolf_root << std::endl;
ProblemPtr wolf_problem_ = Problem::create(FRM_PO_3D);
diff --git a/src/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..73dd8ee480699b55d0aa585e8df214b637f79243
--- /dev/null
+++ b/src/examples/test_kf_callback.cpp
@@ -0,0 +1,68 @@
+/*
+ * test_kf_callback.cpp
+ *
+ * Created on: Nov 6, 2016
+ * Author: jsola
+ */
+
+
+
+#include "../sensor_odom_2D.h"
+#include "../processor_odom_2D.h"
+#include "../processor_tracker_feature_dummy.h"
+#include "../capture_void.h"
+
+int main()
+{
+ using namespace wolf;
+ using namespace Eigen;
+ using namespace std;
+
+ ProblemPtr problem = Problem::create(FRM_PO_2D);
+
+ SensorBasePtr sen_odo = problem->installSensor("ODOM 2D", "main odometer", (Vector3s()<<0,0,0).finished(),"");
+ ProcessorBasePtr prc_odo = problem->installProcessor("ODOM 2D", "odometry integrator", "main odometer", "");
+ prc_odo->setTimeTolerance(0.1);
+
+ SensorBasePtr sen_ftr = problem->installSensor("ODOM 2D", "other odometer", (Vector3s()<<0,0,0).finished(),"");
+ shared_ptr<ProcessorTrackerFeatureDummy> prc_ftr = make_shared<ProcessorTrackerFeatureDummy>(7, 4);
+ prc_ftr->setName("tracker");
+ sen_ftr->addProcessor(prc_ftr);
+ prc_ftr->setTimeTolerance(0.1);
+
+ cout << "Motion sensor : " << problem->getProcessorMotionPtr()->getSensorPtr()->getName() << endl;
+ cout << "Motion processor : " << problem->getProcessorMotionPtr()->getName() << endl;
+
+ TimeStamp t(0);
+ Vector3s x; x << 0,0,0;
+// problem->setOrigin(x, (Matrix3s::Identity()), t);
+ problem->getProcessorMotionPtr()->setOrigin(x, t);
+
+ cout << "x(0) = " << problem->getCurrentState().transpose() << endl;
+
+ Vector2s odo_data; odo_data << .1, (M_PI / 2);
+
+ problem->print(2, false, true, false); // print(level, constr_by, metric, state_blocks)
+
+ Scalar dt = 1;
+ for (auto i = 0; i < 4; i++)
+ {
+ t += dt;
+ cout << "=======================\n>> TIME: " << t.get() << endl;
+
+ cout << "Tracker----------------" << endl;
+ sen_ftr->addCapture(make_shared<CaptureVoid>(t, sen_ftr));
+ problem->print(2, false, true, false); // print(level, constr_by, metric, state_blocks)
+
+ cout << "Motion-----------------" << endl;
+ sen_odo->addCapture(make_shared<CaptureMotion>(t, sen_odo, odo_data));
+ cout << "x(" << t.get() << ") = " << problem->getCurrentState().transpose() << endl;
+ problem->print(2, false, true, false); // print(level, constr_by, metric, state_blocks)
+
+ }
+
+ problem->print();
+
+
+ return 0;
+}
diff --git a/src/examples/test_local_param.cpp b/src/examples/test_local_param.cpp
index 408ab004112a88b1f7dc0434b4171c9181dea91d..6176ed0de964dc21b251624ce258a8c6edd1099b 100644
--- a/src/examples/test_local_param.cpp
+++ b/src/examples/test_local_param.cpp
@@ -14,10 +14,10 @@
#include <iostream>
#define JAC_NUMERIC(T, _x0, _J, dx) \
-{ Eigen::VectorXs dv(3); \
- Eigen::Map<const Eigen::VectorXs> _dv (dv.data(), 3); \
- Eigen::VectorXs xo(3); \
- Eigen::Map<Eigen::VectorXs> _xo (xo.data(), 4); \
+{ VectorXs dv(3); \
+ Map<const VectorXs> _dv (dv.data(), 3); \
+ VectorXs xo(3); \
+ Map<VectorXs> _xo (xo.data(), 4); \
for (int i = 0; i< 3; i++) {\
dv.setZero();\
dv(i) = dx;\
@@ -34,6 +34,14 @@ int main(){
using namespace std;
using namespace wolf;
+ cout << endl << endl;
+ cout << "=========== Test Local Parametrization ==========" << endl;
+ cout << "---- Quaternion and Homogeneous 3D vector ----" << endl << endl;
+
+ // test result
+ bool all_tests_passed = true;
+ bool pass;
+
// Storage
VectorXs x(22);
MatrixXs M(1,12); // matrix dimensions do not matter, just storage size.
@@ -49,73 +57,93 @@ int main(){
da /= 10.0;
Map<VectorXs> qo(&x(7),4);
Map<MatrixXs> J(&M(0,0),4,3);
+ MatrixXs J_num(4,3);
+ cout << "\n--------------- QUATERNION plus() --------------- " << endl;
cout << "Initial values:" << endl;
cout << "q = " << q.transpose() << " with norm = " << q.norm() << "\nda = " << da.transpose() << endl;
- cout << "qo = " << qo.transpose() << " with norm = " << qo.norm() << endl;
- LocalParametrizationQuaternion<DQ_GLOBAL> Qpar;
+ LocalParametrizationQuaternion<DQ_GLOBAL> Qpar_glob;
LocalParametrizationQuaternion<DQ_LOCAL> Qpar_loc;
- bool pass;
- cout << "\nGLOBAL D_QUAT plus()" << endl;
+ // Global --------------------
+ cout << "\n---- DQ_GLOBAL: qo = Exp(da) * q ----" << endl;
+ cout << "Results:" << endl;
Map<const VectorXs> q_m(q.data(),4);
Map<const VectorXs> da_m(da.data(),3);
- Qpar.plus(q_m,da_m,qo);
+ Qpar_glob.plus(q_m,da_m,qo);
cout << "qo = " << qo.transpose() << " with norm = " << qo.norm() << endl;
+ pass = (q_m.norm()-qo.norm()) < 1e-9;
+ all_tests_passed = all_tests_passed && pass;
+ cout << "-------------------- Norm test " << (pass ? "PASSED" : "FAILED") << endl;
- Qpar.computeJacobian(q_m,J);
- cout << " J = \n" << J << endl << endl;
+ Qpar_glob.computeJacobian(q_m,J);
+ cout << " J = \n" << J << endl;
- MatrixXs J_num(4,3);
- JAC_NUMERIC(Qpar, q_m, J_num, 1e-9)
- cout << " J_num = \n" << J_num;
+ JAC_NUMERIC(Qpar_glob, q_m, J_num, 1e-9)
+ cout << " J_num = \n" << J_num << endl;
pass = (J-J_num).isMuchSmallerThan(1,1e-6);
- std::cout << "Jacobians test " << (pass ? "PASSED" : "FAIL") << std::endl;
+ all_tests_passed = all_tests_passed && pass;
+ cout << "-------------------- Jacobians test " << (pass ? "PASSED" : "FAILED") << endl;
- cout << "\nLOCAL D_QUAT plus()" << endl;
+ // Local -------------------------
+ cout << "\n---- DQ_LOCAL: qo = q * Exp(da) ----" << endl;
+ cout << "Results:" << endl;
Qpar_loc.plus(q_m,da_m,qo);
cout << "qo = " << qo.transpose() << " with norm = " << qo.norm() << endl;
+ pass = (q_m.norm()-qo.norm()) < 1e-9;
+ all_tests_passed = all_tests_passed && pass;
+ cout << "-------------------- Norm test " << (pass ? "PASSED" : "FAILED") << endl;
Qpar_loc.computeJacobian(q_m,J);
- cout << " J = " << J << endl;
+ cout << " J = \n" << J << endl;
JAC_NUMERIC(Qpar_loc, q_m, J_num, 1e-9)
cout << " J_num = \n" << J_num << endl;
pass = (J-J_num).isMuchSmallerThan(1,1e-6);
- std::cout << "Jacobians test " << (pass ? "PASSED" : "FAIL") << std::endl;
+ all_tests_passed = all_tests_passed && pass;
+ cout << "-------------------- Jacobians test " << (pass ? "PASSED" : "FAILED") << endl;
// HOMOGENEOUS ----------------------------------------
- cout << "\nHOMOGENEOUS plus()" << endl;
+ cout << "\n--------------- HOMOGENEOUS plus() --------------- " << endl;
Map<VectorXs> h(&x(11),4);
h.setRandom();
Map<VectorXs> d(&x(15),3);
d << .1,.2,.3;
- Map<VectorXs> h_out(&x(18),4);
+ Map<VectorXs> ho(&x(18),4);
cout << "Initial values:" << endl;
cout << "h = " << h.transpose() << " with norm: " << h.norm() << endl;
cout << "d = " << d.transpose() << endl;
LocalParametrizationHomogeneous Hpar;
- Map<const VectorXs> h_const(h.data(),4);
- Map<const VectorXs> d_const(d.data(),3);
+ Map<const VectorXs> h_m(h.data(),4);
+ Map<const VectorXs> d_m(d.data(),3);
- Hpar.plus(h_const,d_const,h_out);
- cout << "\nh_out = " << h_out.transpose() << " with norm: " << h_out.norm() << endl;
+ cout << "\nResults:" << endl;
+ Hpar.plus(h_m,d_m,ho);
+ cout << "ho = " << ho.transpose() << " with norm: " << ho.norm() << endl;
+ pass = (h_m.norm()-ho.norm()) < 1e-9;
+ all_tests_passed = all_tests_passed && pass;
+ cout << "-------------------- Norm test " << (pass ? "PASSED" : "FAILED") << endl;
- Hpar.computeJacobian(h_const,J);
- cout << " J = " << J << "\n" << endl;
+ Hpar.computeJacobian(h_m,J);
+ cout << " J = \n" << J << endl;
- JAC_NUMERIC(Hpar, q_m, J_num, 1e-9)
+ JAC_NUMERIC(Hpar, h_m, J_num, 1e-9)
cout << " J_num = \n" << J_num << endl;
pass = (J-J_num).isMuchSmallerThan(1,1e-6);
- std::cout << "Jacobians test " << (pass ? "PASSED" : "FAIL") << std::endl;
+ all_tests_passed = all_tests_passed && pass;
+ cout << "-------------------- Jacobians test " << (pass ? "PASSED" : "FAILED") << endl;
+ cout << endl << "---------------- " << (all_tests_passed ? "All tests PASSED " : "Some tests FAILED") << " --------------" << endl;
+ cout << "=================================================" << endl;
+ if (!all_tests_passed)
+ return -1;
return 0;
}
diff --git a/src/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp
index c76b99e0741c1f8d1d4bb5c16e5e0d3ec3418546..8a899872d03325519f878db820bf7a40ecf62d02 100644
--- a/src/examples/test_map_yaml.cpp
+++ b/src/examples/test_map_yaml.cpp
@@ -71,22 +71,19 @@ int main()
std::string filename;
- char* w = std::getenv("WOLF_ROOT");
- if (w == NULL)
- throw std::runtime_error("Environment variable WOLF_ROOT not found");
- std::string WOLF_ROOT = w;
- std::string WOLF_CONFIG = WOLF_ROOT + "/src/examples";
- std::cout << "\nWolf directory for configuration files: " << WOLF_CONFIG << std::endl;
+ std::string wolf_root = _WOLF_ROOT_DIR;
+ std::string wolf_config = wolf_root + "/src/examples";
+ std::cout << "\nWolf directory for configuration files: " << wolf_config << std::endl;
ProblemPtr problem = Problem::create(FRM_PO_2D);
- filename = WOLF_CONFIG + "/map_polyline_example.yaml";
+ filename = wolf_config + "/map_polyline_example.yaml";
std::cout << "Reading map from file: " << filename << std::endl;
problem->loadMap(filename);
std::cout << "Printing map..." << std::endl;
print(*(problem->getMapPtr()));
- filename = WOLF_CONFIG + "/map_polyline_example_write.yaml";
+ filename = wolf_config + "/map_polyline_example_write.yaml";
std::cout << "Writing map to file: " << filename << std::endl;
std::string thisfile = __FILE__;
problem->getMapPtr()->save(filename, "Example generated by test file " + thisfile);
diff --git a/src/examples/test_processor_image_landmark.cpp b/src/examples/test_processor_image_landmark.cpp
index 67948941cbe89f56d2519bbd63d5c35244f7f6c7..2d7ce8adb470377da468bf8f033b7ce0310b17b3 100644
--- a/src/examples/test_processor_image_landmark.cpp
+++ b/src/examples/test_processor_image_landmark.cpp
@@ -37,7 +37,7 @@ int main(int argc, char** argv)
{
// filename = "/home/jtarraso/Videos/House_interior.mp4";
// filename = "/home/jtarraso/VÃdeos/gray1.mp4";
- filename = "/home/jtarraso/test_video/output2.mpg";
+ filename = "/home/jtarraso/test_video/output6.mpg";
capture.open(filename);
}
else if (std::string(argv[1]) == "0")
@@ -125,7 +125,7 @@ int main(int argc, char** argv)
// running CAPTURES preallocated
CaptureImage::Ptr image_ptr;
Vector6s data(Vector6s::Zero()); // will integrate this data repeatedly
- CaptureMotion::Ptr cap_odo = std::make_shared<CaptureMotion>(TimeStamp(0), sen_odo_ptr, data);
+ CaptureMotion::Ptr cap_odo = std::make_shared<CaptureMotion>(TimeStamp(t), sen_odo_ptr, data);
//=====================================================
@@ -197,13 +197,13 @@ int main(int argc, char** argv)
sen_odo_ptr->addCapture(cap_odo);
-// wolf_problem_ptr_->print(2);
+ wolf_problem_ptr_->print(2);
// Image ------------------------------------------------
- clock_t t1 = clock();
+// clock_t t1 = clock();
// Preferred method with factory objects:
image_ptr = std::make_shared<CaptureImage>(t, camera_ptr, frame[f % buffer_size]);
@@ -212,26 +212,47 @@ int main(int argc, char** argv)
//image_ptr->process();
camera_ptr->addCapture(image_ptr);
- std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
+// std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
ceres::Solver::Summary summary = ceres_manager.solve();
std::cout << summary.BriefReport() << std::endl;
+ wolf_problem_ptr_->print(2);
+
+// CaptureMotion::Ptr cap = wolf_problem_ptr_->getProcessorMotionPtr()->getOriginPtr();
+// if(cap)
+// {
+// FeatureBasePtr ftr = cap->getFeatureList().front();
+// if(ftr)
+// {
+// ConstraintBasePtr ctr = ftr->getConstraintList().front();
+// if(ctr)
+// {
+// ConstraintOdom3D::Ptr ctr_odom = std::static_pointer_cast<ConstraintOdom3D>(ctr);
+// if(ctr_odom)
+// {
+// std::cout << "ctr_odom3D expectation: " << ctr_odom->expectation().transpose() << std::endl;
+// std::cout << "odom3D measurement: " << cap->getFeatureList().front()->getMeasurement().transpose() << std::endl;
+// }
+// }
+// }
+// }
+
// std::cout << "Last key frame pose: "
// << wolf_problem_ptr_->getLastKeyFramePtr()->getPPtr()->getVector().transpose() << std::endl;
// std::cout << "Last key frame orientation: "
// << wolf_problem_ptr_->getLastKeyFramePtr()->getOPtr()->getVector().transpose() << std::endl;
- cv::waitKey(20);
+ cv::waitKey(0);
- std::cout << "END OF ITERATION\n=================================" << std::endl;
+ std::cout << "=================================================================================================" << std::endl;
f++;
capture >> frame[f % buffer_size];
}
- wolf_problem_ptr_->print();
+ // wolf_problem_ptr_->print(2);
wolf_problem_ptr_.reset();
return 0;
diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index 6f6d7c9dd4c3dee7d3e062d23e1ce4bdb456baa0..a2fa0d85d52a846004d5f90eff073bfc5b54dc28 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -33,7 +33,6 @@ int main (int argc, char** argv)
std::string wolf_root = _WOLF_ROOT_DIR;
-
TimeStamp tf;
if (argc == 1)
tf = 1.0;
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..959319e125c7ab0a31e9cbcdcb7cfca2dcb86820
--- /dev/null
+++ b/src/examples/test_simple_AHP.cpp
@@ -0,0 +1,279 @@
+/**
+ * \file test_simple_AHP.cpp
+ *
+ * Created on: 2 nov. 2016
+ * \author: jtarraso
+ */
+
+#include "wolf.h"
+#include "frame_base.h"
+#include "sensor_camera.h"
+#include "pinholeTools.h"
+#include "rotations.h"
+#include "capture_image.h"
+#include "landmark_AHP.h"
+#include "constraint_AHP.h"
+#include "ceres_wrapper/ceres_manager.h"
+
+/**
+ * This test simulates the following situation:
+ * - A kf at the origin, we use it as anchor: kf1
+ * - A kf at the origin, we use it to project lmks onto the anchor frame: kf2
+ * - A kf at 1m to the right of the origin, kf3
+ * - A kf at 1m to the left of the origin, kf4
+ * - A lmk at 1m distance in front of the anchor: L1
+ * - we use this lmk to project it onto kf2, kf3 and kf4 and obtain measured pixels p0, p1 and p2
+ * - A lmk initialized at kf1, with measurement p0, at a distance of 2m: L2
+ * - we project the pixels p3 and p4: we observe that they do not match p1 and p2
+ * - we use the measurements p1 and p2 to solve the optimization problem on L2: L2 should converge to L1.
+ * - This is a sketch of the situation:
+ * - X, Y are the axes in world frame
+ * - x, z are the axes in anchor camera frame. We have that X=z, Y=-x.
+ * - Axes Z and y are perpendicular to the drawing; they have no effect.
+ *
+ * X,z
+ * ^
+ * |
+ * L2 * 2
+ * .|.
+ * . | .
+ * . | .
+ * . | .
+ * . L1 * 1 .
+ * . . | . .
+ * . . | . .
+ * p4 . . | . . p3
+ * .. p2 | p1 ..
+ * Y <--+---------+---------+
+ * -x +1 0 -1
+ * kf4 kf1 kf3
+ * kf2
+ *
+ * camera: (uo,vo) = (320,240)
+ * (au,av) = (320,320)
+ *
+ * projection geometry:
+ *
+ * 1:1 2:1 1:0 2:1 1:1
+ * 0 160 320 480 640
+ * +----+----+----+----+
+ * |
+ * |
+ * | 320
+ * |
+ * *
+ *
+ *
+ * projected pixels:
+ * p0 = (320,240) // at optical axis or relation 1:0
+ * p1 = ( 0 ,240) // at 45 deg or relation 1:1
+ * p2 = (640,240) // at 45 deg or relation 1:1
+ * p3 = (160,240) // at a relation 2:1
+ * p4 = (480,240) // at a relation 2:1
+ *
+ */
+int main(int argc, char** argv)
+{
+ using namespace wolf;
+ using namespace Eigen;
+
+ /* 1. crear 2 kf, fixed
+ * 2. crear 1 sensor
+ * 3. crear 1 lmk1
+ * 4. projectar lmk sobre sensor a fk1
+ * 5. projectar lmk sobre sensor a kf4
+ * 6. // esborrar lmk lmk_ptr->remove() no cal
+ * 7. crear nous kf
+ * 8. crear captures
+ * 9. crear features amb les mesures de 4 i 5
+ * 10. crear lmk2 des de kf3
+ * 11. crear constraint des del kf4 a lmk2, amb ancora al kf3
+ * 12. solve
+ * 13. lmk1 == lmk2 ?
+ */
+
+ // ============================================================================================================
+ /* 1 */
+ ProblemPtr problem = Problem::create(FRM_PO_3D);
+ // One anchor frame to define the lmk, and a copy to make a constraint
+ FrameBasePtr kf_1 = problem->createFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
+ FrameBasePtr kf_2 = problem->createFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
+ // and two other frames to observe the lmk
+ FrameBasePtr kf_3 = problem->createFrame(KEY_FRAME,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0));
+ FrameBasePtr kf_4 = problem->createFrame(KEY_FRAME,(Vector7s()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0));
+
+ kf_1->fix();
+ kf_2->fix();
+ kf_3->fix();
+ kf_4->fix();
+ // ============================================================================================================
+
+ // ============================================================================================================
+ /* 2 */
+ std::string wolf_root = _WOLF_ROOT_DIR;
+ SensorBasePtr sensor_ptr = problem->installSensor("CAMERA", "PinHole", (Vector7s()<<0,0,0,-0.5,0.5,-0.5,0.5).finished(), wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
+ SensorCamera::Ptr camera = std::static_pointer_cast<SensorCamera>(sensor_ptr);
+ // ============================================================================================================
+
+ // ============================================================================================================
+ /* 3 */
+ Eigen::Vector3s lmk_dir = {0,0,1}; // in the optical axis of the anchor camera at kf1
+ std::cout << std::endl << "lmk: " << lmk_dir.transpose() << std::endl;
+ lmk_dir.normalize();
+ Eigen::Vector4s lmk_hmg_c;
+ Scalar distance = 1.0; // from anchor at kf1
+ lmk_hmg_c = {lmk_dir(0),lmk_dir(1),lmk_dir(2),(1/distance)};
+// std::cout << "lmk hmg in C frame: " << lmk_hmg_c.transpose() << std::endl;
+ // ============================================================================================================
+
+
+ // Captures------------------
+ cv::Mat cv_image;
+ cv_image.zeros(2,2,0);
+ CaptureImage::Ptr image_0 = std::make_shared<CaptureImage>(TimeStamp(0), camera, cv_image);
+ CaptureImage::Ptr image_1 = std::make_shared<CaptureImage>(TimeStamp(1), camera, cv_image);
+ CaptureImage::Ptr image_2 = std::make_shared<CaptureImage>(TimeStamp(2), camera, cv_image);
+ kf_2->addCapture(image_0);
+ kf_3->addCapture(image_1);
+ kf_4->addCapture(image_2);
+
+ // Features-----------------
+ cv::Mat desc;
+
+ cv::KeyPoint kp_0;
+ std::shared_ptr<FeaturePointImage> feat_0 = std::make_shared<FeaturePointImage>(kp_0, desc, Eigen::Matrix2s::Identity());
+ image_0->addFeature(feat_0);
+
+ cv::KeyPoint kp_1;
+ std::shared_ptr<FeaturePointImage> feat_1 = std::make_shared<FeaturePointImage>(kp_1, desc, Eigen::Matrix2s::Identity());
+ image_1->addFeature(feat_1);
+
+ cv::KeyPoint kp_2;
+ std::shared_ptr<FeaturePointImage> feat_2 = std::make_shared<FeaturePointImage>(kp_2, desc, Eigen::Matrix2s::Identity());
+ image_2->addFeature(feat_2);
+
+ // Landmark--------------------
+ std::shared_ptr<LandmarkAHP> lmk_1 = std::make_shared<LandmarkAHP>(lmk_hmg_c, kf_1, camera, desc);
+ problem->addLandmark(lmk_1);
+ lmk_1->setStatus(LANDMARK_FIXED);
+ std::cout << "Landmark 1: " << lmk_1->point().transpose() << std::endl;
+
+ // Constraints------------------
+ ConstraintAHP::Ptr ctr_0 = ConstraintAHP::create(feat_0, kf_2, lmk_1 );
+ feat_0->addConstraint(ctr_0);
+ ConstraintAHP::Ptr ctr_1 = ConstraintAHP::create(feat_1, kf_3, lmk_1 );
+ feat_1->addConstraint(ctr_1);
+ ConstraintAHP::Ptr ctr_2 = ConstraintAHP::create(feat_2, kf_4, lmk_1 );
+ feat_2->addConstraint(ctr_2);
+
+ // Projections----------------------------
+ Eigen::VectorXs pix_0 = ctr_0->expectation();
+ kp_0 = cv::KeyPoint(pix_0(0), pix_0(1), 0);
+ feat_0->setKeypoint(kp_0);
+
+ Eigen::VectorXs pix_1 = ctr_1->expectation();
+ kp_1 = cv::KeyPoint(pix_1(0), pix_1(1), 0);
+ feat_1->setKeypoint(kp_1);
+
+ Eigen::VectorXs pix_2 = ctr_2->expectation();
+ kp_2 = cv::KeyPoint(pix_2(0), pix_2(1), 0);
+ feat_2->setKeypoint(kp_2);
+
+ std::cout << "pixel 0: " << pix_0.transpose() << std::endl;
+ std::cout << "pixel 1: " << pix_1.transpose() << std::endl;
+ std::cout << "pixel 2: " << pix_2.transpose() << std::endl;
+ //
+ //======== up to here the initial projections ==============
+
+ std::cout << "\n";
+
+ //======== now we want to estimate a new lmk ===============
+ //
+ // Features -----------------
+ std::shared_ptr<FeaturePointImage> feat_3 = std::make_shared<FeaturePointImage>(kp_1, desc, Eigen::Matrix2s::Identity());
+ image_1->addFeature(feat_3);
+
+ std::shared_ptr<FeaturePointImage> feat_4 = std::make_shared<FeaturePointImage>(kp_2, desc, Eigen::Matrix2s::Identity());
+ image_2->addFeature(feat_4);
+
+
+ // New landmark with measured pixels from kf2 (anchor) kf3 and kf4 (measurements)
+ Scalar unknown_distance = 2; // the real distance is 1
+ Matrix3s K = camera->getIntrinsicMatrix();
+ Vector3s pix_0_hmg;
+ pix_0_hmg << pix_0, 1;
+ Eigen::Vector3s dir_0 = K.inverse() * pix_0_hmg;
+ Eigen::Vector4s pnt_hmg_0;
+ pnt_hmg_0 << dir_0, 1/unknown_distance;
+ LandmarkAHP::Ptr lmk_2( std::make_shared<LandmarkAHP>(pnt_hmg_0, kf_2, camera, desc) );
+ problem->addLandmark(lmk_2);
+ std::cout << "Landmark 2: " << lmk_2->point().transpose() << std::endl;
+
+ // New constraints from kf3 and kf4
+ ConstraintAHP::Ptr ctr_3 = ConstraintAHP::create(feat_3, kf_3, lmk_2 );
+ feat_3->addConstraint(ctr_3);
+ ConstraintAHP::Ptr ctr_4 = ConstraintAHP::create(feat_4, kf_4, lmk_2 );
+ feat_4->addConstraint(ctr_4);
+
+ Eigen::Vector2s pix_3 = ctr_3->expectation();
+ Eigen::Vector2s pix_4 = ctr_4->expectation();
+
+ std::cout << "pix 3: " << pix_3.transpose() << std::endl;
+ std::cout << "pix 4: " << pix_4.transpose() << std::endl;
+
+ // Wolf tree status ----------------------
+ problem->print(3);
+// problem->check();
+
+
+
+
+
+
+ // ========== solve ==================================================================================================
+ /* 12 */
+ // Ceres wrapper
+ ceres::Solver::Options ceres_options;
+ ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
+ ceres_options.max_line_search_step_contraction = 1e-3;
+ // ceres_options.minimizer_progress_to_stdout = false;
+ // ceres_options.line_search_direction_type = ceres::LBFGS;
+ // ceres_options.max_num_iterations = 100;
+ google::InitGoogleLogging(argv[0]);
+
+ CeresManager ceres_manager(problem, ceres_options);
+
+
+ ceres::Solver::Summary summary = ceres_manager.solve();
+ std::cout << summary.FullReport() << std::endl;
+
+ // Test of convergence over the lmk params
+ bool pass = (lmk_2->point() - lmk_1->point()).isMuchSmallerThan(1,Constants::EPS);
+
+ std::cout << "Landmark 2 below should have converged to Landmark 1:" << std::endl;
+ std::cout << "Landmark 1: " << lmk_1->point().transpose() << std::endl;
+ std::cout << "Landmark 2: " << lmk_2->point().transpose() << std::endl;
+ std::cout << "Landmark convergence test " << (pass ? "PASSED" : "FAILED") << std::endl;
+ std::cout << std::endl;
+
+ if (!pass)
+ return -1;
+ return 0;
+
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
index 8878f39dc32d6ab149bebbb266cf9a0deb2530dd..c34616753a174a249f54cb5dbf75825ac4e8b843 100644
--- a/src/examples/test_wolf_factories.cpp
+++ b/src/examples/test_wolf_factories.cpp
@@ -36,21 +36,11 @@ int main(void)
using std::static_pointer_cast;
- /**=============================================================================================
- * Get wolf root directory from the environment variable WOLF_ROOT
- * To make this work, you need to set the environment variable WOLF_ROOT:
- * - To run from terminal, edit your ~/.bashrc, or ~/.bash_profile and add this line:
- * export WOLF_ROOT="/path/to/wolf"
- * - To run from eclipse, open the 'run configuration' of this executable, tab 'Environment'
- * and add variable WOLF_ROOT set to /path/to/wolf
- */
- char* w = std::getenv("WOLF_ROOT");
- if (w == NULL)
- throw std::runtime_error("Environment variable WOLF_ROOT not found");
-
- std::string WOLF_ROOT = w;
- std::string WOLF_CONFIG = WOLF_ROOT + "/src/examples";
- std::cout << "\nwolf directory for configuration files: " << WOLF_CONFIG << std::endl;
+
+ //==============================================================================================
+ std::string wolf_root = _WOLF_ROOT_DIR;
+ std::string wolf_config = wolf_root + "/src/examples";
+ std::cout << "\nwolf directory for configuration files: " << wolf_config << std::endl;
//==============================================================================================
cout << "\n====== Registering creators in the Wolf Factories =======" << endl;
@@ -59,7 +49,7 @@ int main(void)
"There is only one attempt per class, and it is successful!\n"
"We do this by registering in the class\'s .cpp file.\n"
"\n"
- "- See \'" << WOLF_ROOT << "/src/examples/test_wolf_factories.cpp\'\n"
+ "- See \'" << wolf_root << "/src/examples/test_wolf_factories.cpp\'\n"
" for the way to install sensors and processors to wolf::Problem." << endl;
// Start creating the problem
@@ -68,13 +58,13 @@ int main(void)
// define some useful parameters
Eigen::VectorXs pq_3d(7), po_2d(3), p_3d(3);
- shared_ptr<IntrinsicsOdom2D> intr_odom2d_ptr;
+ shared_ptr<IntrinsicsOdom2D> intr_odom2d_ptr = nullptr;
cout << "\n================== Intrinsics Factory ===================" << endl;
// Use params factory for camera intrinsics
- IntrinsicsBasePtr intr_cam_ptr = IntrinsicsFactory::get().create("CAMERA", WOLF_CONFIG + "/camera.yaml");
- ProcessorParamsBasePtr params_ptr = ProcessorParamsFactory::get().create("IMAGE", WOLF_CONFIG + "/processor_image_ORB.yaml");
+ 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_ORB.yaml");
cout << "CAMERA with intrinsics : " << (static_pointer_cast<IntrinsicsCamera>(intr_cam_ptr))->pinhole_model.transpose() << endl;
// cout << "Processor IMAGE image width : " << (static_pointer_cast<ProcessorParamsImage>(params_ptr))->image.width << endl;
@@ -83,13 +73,13 @@ int main(void)
// Install sensors
problem->installSensor("CAMERA", "front left camera", pq_3d, intr_cam_ptr);
- problem->installSensor("CAMERA", "front right camera", pq_3d, WOLF_CONFIG + "/camera.yaml");
+ problem->installSensor("CAMERA", "front right camera", pq_3d, wolf_config + "/camera_params_ueye_sim.yaml");
problem->installSensor("ODOM 2D", "main odometer", po_2d, intr_odom2d_ptr);
problem->installSensor("GPS FIX", "GPS fix", p_3d);
problem->installSensor("IMU", "inertial", pq_3d);
- problem->installSensor("GPS", "GPS raw", p_3d);
+// problem->installSensor("GPS", "GPS raw", p_3d);
problem->installSensor("ODOM 2D", "aux odometer", po_2d, intr_odom2d_ptr);
- problem->installSensor("CAMERA", "rear camera", pq_3d, WOLF_ROOT + "/src/examples/camera.yaml");
+ problem->installSensor("CAMERA", "rear camera", pq_3d, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
// print available sensors
for (auto sen : problem->getHardwarePtr()->getSensorList())
@@ -103,9 +93,9 @@ int main(void)
// Install processors and bind them to sensors -- by sensor name!
problem->installProcessor("ODOM 2D", "main odometry", "main odometer");
- problem->installProcessor("ODOM 3D", "sec. odometry", "aux odometer");
+ problem->installProcessor("ODOM 2D", "sec. odometry", "aux odometer");
problem->installProcessor("IMU", "pre-integrated", "inertial");
- problem->installProcessor("IMAGE", "ORB", "front left camera", WOLF_CONFIG + "/processor_image_ORB.yaml");
+ problem->installProcessor("IMAGE FEATURE", "ORB", "front left camera", wolf_config + "/processor_image_ORB.yaml");
// problem->createProcessor("GPS", "GPS pseudoranges", "GPS raw");
// print installed processors
diff --git a/src/examples/test_yaml.cpp b/src/examples/test_yaml.cpp
index 2b9dac224ad71cc0bdbf41d5800b49226547db25..5b4f9f553fb4f0bb86a91a3947a4511270f59100 100644
--- a/src/examples/test_yaml.cpp
+++ b/src/examples/test_yaml.cpp
@@ -21,21 +21,9 @@
int main()
{
- /**=============================================================================================
- * Get wolf root directory from the environment variable WOLF_ROOT
- * To make this work, you need to set the environment variable WOLF_ROOT:
- * - To run from terminal, edit your ~/.bashrc, or ~/.bash_profile and add this line:
- * export WOLF_ROOT="/path/to/wolf"
- * - To run from eclipse, open the 'run configuration' of this executable, tab 'Environment'
- * and add variable WOLF_ROOT set to /path/to/wolf
- */
- std::string WOLF_ROOT;
- char* w = std::getenv("WOLF_ROOT");
- if (w != NULL)
- WOLF_ROOT = w;
- else
- throw std::runtime_error("Environment variable WOLF_ROOT not found");
- std::cout << "\nwolf root directory: " << WOLF_ROOT << std::endl;
+ //=============================================================================================
+ std::string wolf_root = _WOLF_ROOT_DIR;
+ std::cout << "\nwolf root directory: " << wolf_root << std::endl;
//=============================================================================================
using namespace Eigen;
@@ -45,7 +33,7 @@ int main()
// Camera parameters
- YAML::Node camera_config = YAML::LoadFile(WOLF_ROOT + "/src/examples/camera.yaml");
+ YAML::Node camera_config = YAML::LoadFile(wolf_root + "/src/examples/camera.yaml");
if (camera_config["sensor type"])
{
@@ -87,7 +75,7 @@ int main()
ProcessorParamsImage p;
- Node params = YAML::LoadFile(WOLF_ROOT + "/src/examples/processor_image_ORB.yaml");
+ Node params = YAML::LoadFile(wolf_root + "/src/examples/processor_image_ORB.yaml");
if (params["processor type"])
{
diff --git a/src/feature_point_image.cpp b/src/feature_point_image.cpp
index aa1d9e81ab564dcd50dc580808fc9fd49e4a41bf..9bef25d2b5d101e0505c75b717ab0360933bd558 100644
--- a/src/feature_point_image.cpp
+++ b/src/feature_point_image.cpp
@@ -6,6 +6,8 @@ namespace wolf {
FeaturePointImage::FeaturePointImage(const Eigen::Vector2s & _measurement) :
FeatureBase("POINT IMAGE", _measurement,Eigen::MatrixXs::Zero(0,0)), is_known_(false)
{
+ keypoint_.pt.x = float(measurement_(0));
+ keypoint_.pt.y = float(measurement_(1));
//
}
diff --git a/src/feature_point_image.h b/src/feature_point_image.h
index 4c7a451de27e50567f0e2ca539723e62a99181a5..6a7486a7e16992802d193e6ecaa3ddf8066f7278 100644
--- a/src/feature_point_image.h
+++ b/src/feature_point_image.h
@@ -66,6 +66,8 @@ class FeaturePointImage : public FeatureBase
void setKeypoint(const cv::KeyPoint& _kp)
{
keypoint_ = _kp;
+ measurement_(0) = _kp.pt.x;
+ measurement_(1) = _kp.pt.y;
}
const cv::Mat& getDescriptor() const;
diff --git a/src/landmark_AHP.cpp b/src/landmark_AHP.cpp
index c66dd056f00991ebb71c91007efadbb76b25c056..d0ae0765fa4db43bd3cb59975c1ee27df98de7d2 100644
--- a/src/landmark_AHP.cpp
+++ b/src/landmark_AHP.cpp
@@ -35,12 +35,26 @@ YAML::Node LandmarkAHP::saveToYaml() const
return node;
}
-Eigen::Vector3s LandmarkAHP::getPoint3D() const
+Eigen::Vector3s LandmarkAHP::getPointInAnchorSensor() const
{
Eigen::Map<const Eigen::Vector4s> hmg_point(getPPtr()->getVector().data());
return hmg_point.head<3>()/hmg_point(3);
}
+Eigen::Vector3s LandmarkAHP::point() const
+{
+ using namespace Eigen;
+ Transform<Scalar,3,Affine> T_w_r
+ = Translation<Scalar,3>(getAnchorFrame()->getPPtr()->getVector())
+ * Quaternions(getAnchorFrame()->getOPtr()->getVector().data());
+ Transform<Scalar,3,Affine> T_r_c
+ = Translation<Scalar,3>(getAnchorSensor()->getPPtr()->getVector())
+ * Quaternions(getAnchorSensor()->getOPtr()->getVector().data());
+ Vector4s point_hmg_c = getPPtr()->getVector();
+ Vector4s point_hmg = T_w_r * T_r_c * point_hmg_c;
+ return point_hmg.head<3>()/point_hmg(3);
+}
+
wolf::LandmarkBasePtr LandmarkAHP::create(const YAML::Node& _node)
{
unsigned int id = _node["id"] .as< unsigned int >();
diff --git a/src/landmark_AHP.h b/src/landmark_AHP.h
index c9d02c651018a9995d203953cb9112afb4f564c3..7c85e1bc7545a7015c6ad7426d60b9932f5a10d3 100644
--- a/src/landmark_AHP.h
+++ b/src/landmark_AHP.h
@@ -38,7 +38,8 @@ class LandmarkAHP : public LandmarkBase
void setAnchorFrame (FrameBasePtr _anchor_frame );
void setAnchorSensor (SensorBasePtr _anchor_sensor);
void setAnchor (FrameBasePtr _anchor_frame , SensorBasePtr _anchor_sensor);
- Eigen::Vector3s getPoint3D() const;
+ Eigen::Vector3s getPointInAnchorSensor() const;
+ Eigen::Vector3s point() const;
YAML::Node saveToYaml() const;
diff --git a/src/local_parametrization_homogeneous.cpp b/src/local_parametrization_homogeneous.cpp
index 431f77a23e10c8c1f301029e7b8552a8e761a656..987c9e45612d47072f30ef9d8e4d4f30b3c18a84 100644
--- a/src/local_parametrization_homogeneous.cpp
+++ b/src/local_parametrization_homogeneous.cpp
@@ -31,22 +31,27 @@ bool LocalParametrizationHomogeneous::plus(const Eigen::Map<const Eigen::VectorX
using namespace Eigen;
- double norm_delta = _delta.norm();
- if (norm_delta > Constants::EPS)
+ double angle = _delta.norm();
+ Quaternions dq;
+ if (angle > Constants::EPS_SMALL)
{
// compute rotation axis -- this guarantees unity norm
- Vector3s axis = _delta / norm_delta;
+ Vector3s axis = _delta.normalized();
- // express delta as a quaternion -- this is exp(delta)
- Quaternions dq(AngleAxis<Scalar>(norm_delta, axis));
+ // express delta as a quaternion using the angle-axis helper
+ dq = AngleAxis<Scalar>(angle, axis);
- // result as a homogeneous point -- we use the quaternion product for keeping in the 4-sphere
- _h_plus_delta = (dq * Map<const Quaternions>(_h.data())).coeffs();
}
- else
+ else // Consider small angle approx
{
- _h_plus_delta = _h;
+ dq.w() = 1;
+ dq.vec() = _delta/2;
+ dq.normalize();
}
+
+ // result as a homogeneous point -- we use the quaternion product for keeping in the 4-sphere
+ _h_plus_delta = (dq * Map<const Quaternions>(_h.data())).coeffs();
+
return true;
}
@@ -56,11 +61,11 @@ bool LocalParametrizationHomogeneous::computeJacobian(const Eigen::Map<const Eig
assert(_h.size() == global_size_ && "Wrong size of input quaternion.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
- _jacobian << -_h(0), -_h(1), -_h(2),
- _h(3), _h(2), -_h(1),
- -_h(2), _h(3), _h(0),
- _h(1), -_h(0), _h(3);
- _jacobian /= 2;
+ Eigen::Vector4s hh = _h/2;
+ _jacobian << hh(3), hh(2), -hh(1),
+ -hh(2), hh(3), hh(0),
+ hh(1), -hh(0), hh(3),
+ -hh(0), -hh(1), -hh(2) ;
return true;
}
diff --git a/src/local_parametrization_quaternion.cpp b/src/local_parametrization_quaternion.cpp
index 1e7faeb3b96838b512398db17d611556847c70b6..08b8ea1431cdced786eace58221af8d8b7f2850f 100644
--- a/src/local_parametrization_quaternion.cpp
+++ b/src/local_parametrization_quaternion.cpp
@@ -1,5 +1,5 @@
#include "local_parametrization_quaternion.h"
-
+#include <iostream>
namespace wolf {
template <>
@@ -17,23 +17,27 @@ bool LocalParametrizationQuaternion<wolf::DQ_LOCAL>::plus(const Eigen::Map<const
using namespace Eigen;
double angle = _delta_theta.norm();
+ Quaternions dq;
if (angle > Constants::EPS_SMALL)
{
// compute rotation axis -- this guarantees unity norm
Vector3s axis = _delta_theta / angle;
// express delta_theta as a quaternion using the angle-axis helper
- Quaternions dq(AngleAxis<Scalar>(angle, axis));
-
- // result as a quaternion
- // the delta is in local reference: q * dq
- _q_plus_delta_theta = (Map<const Quaternions>(_q.data()) * dq).coeffs();
+ dq = AngleAxis<Scalar>(angle, axis);
}
- else // Consider null rotation
+ else // Consider small angle approx
{
- _q_plus_delta_theta = _q;
+ dq.w() = 1;
+ dq.vec() = _delta_theta/2;
+ dq.normalize();
}
+
+ // result as a quaternion
+ // the delta is in global reference: dq * q
+ _q_plus_delta_theta = (Map<const Quaternions>(_q.data()) * dq).coeffs();
+
return true;
}
@@ -52,22 +56,27 @@ bool LocalParametrizationQuaternion<wolf::DQ_GLOBAL>::plus(const Eigen::Map<cons
using namespace Eigen;
double angle = _delta_theta.norm();
+ Quaternions dq;
if (angle > Constants::EPS_SMALL)
{
// compute rotation axis -- this guarantees unity norm
Vector3s axis = _delta_theta / angle;
// express delta_theta as a quaternion using the angle-axis helper
- Quaternions dq(AngleAxis<Scalar>(angle, axis));
+ dq = AngleAxis<Scalar>(angle, axis);
- // result as a quaternion
- // the delta is in global reference: dq * q
- _q_plus_delta_theta = (dq * Map<const Quaternions>(_q.data())).coeffs();
}
- else // Consider null rotation
+ else // Consider small angle approx
{
- _q_plus_delta_theta = _q;
+ dq.w() = 1;
+ dq.vec() = _delta_theta/2;
+ dq.normalize();
}
+
+ // result as a quaternion
+ // the delta is in global reference: dq * q
+ _q_plus_delta_theta = (dq * Map<const Quaternions>(_q.data())).coeffs();
+
return true;
}
@@ -78,12 +87,11 @@ bool LocalParametrizationQuaternion<wolf::DQ_LOCAL>::computeJacobian(const Eigen
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
- using namespace Eigen;
- _jacobian << -_q(0), -_q(1), -_q(2),
- _q(3), -_q(2), _q(1),
- _q(2), _q(3), -_q(0),
- -_q(1), _q(0), _q(3);
- _jacobian /= 2;
+ Eigen::Vector4s qq = _q/2;
+ _jacobian << qq(3), -qq(2), qq(1),
+ qq(2), qq(3), -qq(0),
+ -qq(1), qq(0), qq(3),
+ -qq(0), -qq(1), -qq(2) ;
return true;
}
@@ -95,12 +103,11 @@ bool LocalParametrizationQuaternion<wolf::DQ_GLOBAL>::computeJacobian(const Eige
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
- using namespace Eigen;
- _jacobian << -_q(0), -_q(1), -_q(2),
- _q(3), _q(2), -_q(1),
- -_q(2), _q(3), _q(0),
- _q(1), -_q(0), _q(3);
- _jacobian /= 2;
+ Eigen::Vector4s qq = _q/2;
+ _jacobian << qq(3), qq(2), -qq(1),
+ -qq(2), qq(3), qq(0),
+ qq(1), -qq(0), qq(3),
+ -qq(0), -qq(1), -qq(2);
return true;
}
diff --git a/src/problem.cpp b/src/problem.cpp
index 4d6f338ded1ff6e262593d93116319622e51aa72..c1104595ccda1ad9b732afe5edf19110e42d6d01 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -556,197 +556,335 @@ void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string&
getMapPtr()->save(_filename_dot_yaml, _map_name);
}
-void Problem::print(int level)
+void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
{
std::cout << std::endl;
std::cout << "P: wolf tree status ---------------------" << std::endl;
- std::cout << "H" << std::endl;
- for (auto S : getHardwarePtr()->getSensorList() )
+ std::cout << "Hardware" << std::endl;
+ if (depth >= 1)
{
- std::cout << " S" << S->id() << std::endl;
- for (auto p : S->getProcessorList() )
+ for (auto S : getHardwarePtr()->getSensorList())
{
- if (p->isMotion())
+ std::cout << " S" << S->id() << std::endl;
+ if (state_blocks)
{
- std::cout << " pm" << p->id() << std::endl;
- ProcessorMotion::Ptr pm = std::static_pointer_cast<ProcessorMotion>(p);
- if (pm->getOriginPtr())
- std::cout << " o: C" << pm->getOriginPtr()->id() << std::endl;
- if (pm->getLastPtr())
- std::cout << " l: C" << pm->getLastPtr()->id() << std::endl;
- if (pm->getIncomingPtr())
- std::cout << " i: C" << pm->getIncomingPtr()->id() << std::endl;
+ std::cout << " sb:";
+ for (auto sb : S->getStateBlockVec())
+ if (sb != nullptr)
+ std::cout << " " << (sb->isFixed() ? "Fix" : "Est");
+ std::cout << std::endl;
}
- else
+ if (depth >= 2)
{
- try
+ for (auto p : S->getProcessorList())
{
- std::cout << " pt" << p->id() << std::endl;
- ProcessorTracker::Ptr pt = std::static_pointer_cast<ProcessorTracker>(p);
- if (pt->getOriginPtr())
- std::cout << " o: C" << pt->getOriginPtr()->id() << std::endl;
- if (pt->getLastPtr())
- std::cout << " l: C" << pt->getLastPtr()->id() << std::endl;
- if (pt->getIncomingPtr())
- std::cout << " i: C" << pt->getIncomingPtr()->id() << std::endl;
- }
- catch (std::runtime_error& e)
- {
- std::cout << "Unknown processor type!" << std::endl;
- }
+ if (p->isMotion())
+ {
+ std::cout << " pm" << p->id() << std::endl;
+ if (depth >= 2)
+ {
+ ProcessorMotion::Ptr pm = std::static_pointer_cast<ProcessorMotion>(p);
+ if (pm->getOriginPtr())
+ std::cout << " o: C" << pm->getOriginPtr()->id() << " - F"
+ << pm->getOriginPtr()->getFramePtr()->id() << std::endl;
+ if (pm->getLastPtr())
+ std::cout << " l: C" << pm->getLastPtr()->id() << " - F"
+ << pm->getLastPtr()->getFramePtr()->id() << std::endl;
+ if (pm->getIncomingPtr())
+ std::cout << " i: C" << pm->getIncomingPtr()->id() << std::endl;
+ }
+ }
+ else
+ {
+ try
+ {
+ std::cout << " pt" << p->id() << std::endl;
+ if (depth >= 2)
+ {
+ ProcessorTracker::Ptr pt = std::static_pointer_cast<ProcessorTracker>(p);
+ if (pt->getOriginPtr())
+ std::cout << " o: C" << pt->getOriginPtr()->id() << " - F"
+ << pt->getOriginPtr()->getFramePtr()->id() << std::endl;
+ if (pt->getLastPtr())
+ std::cout << " l: C" << pt->getLastPtr()->id() << " - F"
+ << pt->getLastPtr()->getFramePtr()->id() << std::endl;
+ if (pt->getIncomingPtr())
+ std::cout << " i: C" << pt->getIncomingPtr()->id() << std::endl;
+ }
+ }
+ catch (std::runtime_error& e)
+ {
+ std::cout << "Unknown processor type!" << std::endl;
+ }
+ }
+ }
}
+ else
+ std::cout << " " << S->getProcessorList().size() << "p" << std::endl;
}
}
- std::cout << "T" << std::endl;
- for (auto F : getTrajectoryPtr()->getFrameList() )
+ else
+ std::cout << " " << getHardwarePtr()->getSensorList().size() << "S" << std::endl;
+ std::cout << "Trajectory" << std::endl;
+ if (depth >= 1)
{
- std::cout << (F->isKey() ? " KF" : " F") << F->id();
- if (level > 0)
- {
- std::cout << " <-- ";
- for (auto cby : F->getConstrainedByList())
- std::cout << "c" << cby->id() << " \t";
- }
- std::cout << std::endl;
- if (level > 1)
+ for (auto F : getTrajectoryPtr()->getFrameList())
{
- std::cout << (F->isFixed() ? " Fixed" : " Estim") << ", ts=" << std::setprecision(5) << F->getTimeStamp().get();
- std::cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << ")";
+ std::cout << (F->isKey() ? " KF" : " F") << F->id();
+ if (constr_by)
+ {
+ std::cout << " <-- ";
+ for (auto cby : F->getConstrainedByList())
+ std::cout << "c" << cby->id() << " \t";
+ }
std::cout << std::endl;
- }
- for (auto C : F->getCaptureList() )
- {
- std::cout << " C" << C->id() << " -> S" << C->getSensorPtr()->id() << std::endl;
- for (auto f : C->getFeatureList() )
+ if (metric)
{
- std::cout << " f" << f->id();
- if (level > 0)
- {
- std::cout<< " <--\t";
- for (auto cby : f->getConstrainedByList())
- std::cout << "c" << cby->id() << " \t";
- }
+ std::cout << (F->isFixed() ? " Fixed" : " Estim") << ", ts=" << std::setprecision(5)
+ << F->getTimeStamp().get();
+ std::cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << ")";
+ std::cout << std::endl;
+ }
+ if (state_blocks)
+ {
+ std::cout << " sb:";
+ for (auto sb : F->getStateBlockVec())
+ if (sb != nullptr)
+ std::cout << " " << (sb->isFixed() ? "Fix" : "Est");
std::cout << std::endl;
- if (level > 1)
- std::cout << " m = ( " << std::setprecision(3) << f->getMeasurement().transpose() << ")" << std::endl;
- for (auto c : f->getConstraintList() )
+ }
+ if (depth >= 2)
+ {
+ for (auto C : F->getCaptureList())
{
- std::cout << " c" << c->id();
- switch (c->getCategory())
+ std::cout << " C" << C->id() << " -> S" << C->getSensorPtr()->id() << std::endl;
+ if (depth >= 3)
{
- case CTR_ABSOLUTE:
- std::cout << " --> A" << std::endl;
- break;
- case CTR_FRAME:
- std::cout << " --> F" << c->getFrameOtherPtr()->id() << std::endl;
- break;
- case CTR_FEATURE:
- std::cout << " --> f" << c->getFeatureOtherPtr()->id() << std::endl;
- break;
- case CTR_LANDMARK:
- std::cout << " --> L" << c->getLandmarkOtherPtr()->id() << std::endl;
- break;
+ for (auto f : C->getFeatureList())
+ {
+ std::cout << " f" << f->id();
+ if (constr_by)
+ {
+ std::cout << " <--\t";
+ for (auto cby : f->getConstrainedByList())
+ std::cout << "c" << cby->id() << " \t";
+ }
+ std::cout << std::endl;
+ if (metric)
+ std::cout << " m = ( " << std::setprecision(3) << f->getMeasurement().transpose()
+ << ")" << std::endl;
+ if (depth >= 4)
+ {
+ for (auto c : f->getConstraintList())
+ {
+ std::cout << " c" << c->id() << " -->";
+ if (c->getCategory() == CTR_ABSOLUTE)
+ std::cout << " A";
+ if (c->getFrameOtherPtr() != nullptr)
+ std::cout << " F" << c->getFrameOtherPtr()->id();
+ if (c->getFeatureOtherPtr() != nullptr)
+ std::cout << " f" << c->getFeatureOtherPtr()->id();
+ if (c->getLandmarkOtherPtr() != nullptr)
+ std::cout << " L" << c->getLandmarkOtherPtr()->id();
+ std::cout << std::endl;
+ }
+ }
+ else
+ std::cout << " " << f->getConstraintList().size() << "c" << std::endl;
+ }
}
+ else
+ std::cout << " " << C->getFeatureList().size() << "f" << std::endl;
}
}
+ else
+ std::cout << " " << F->getCaptureList().size() << "C" << std::endl;
}
}
- std::cout << "M" << std::endl;
- for (auto L : getMapPtr()->getLandmarkList() )
+ else
+ std::cout << " " << getTrajectoryPtr()->getFrameList().size() << "F" << std::endl;
+ std::cout << "Map" << std::endl;
+ if (depth >= 1)
{
- std::cout << " L" << L->id();
- if (level > 0)
+ for (auto L : getMapPtr()->getLandmarkList())
+ {
+ std::cout << " L" << L->id();
+ if (constr_by)
{
- std::cout << "\t<-- ";
- for (auto cby : L->getConstrainedByList())
- std::cout << "c" << cby->id() << " \t";
+ std::cout << "\t<-- ";
+ for (auto cby : L->getConstrainedByList())
+ std::cout << "c" << cby->id() << " \t";
}
- std::cout << std::endl;
+ std::cout << std::endl;
+ if (state_blocks)
+ {
+ std::cout << " sb:";
+ for (auto sb : L->getStateBlockVec())
+ if (sb != nullptr)
+ std::cout << " " << (sb->isFixed() ? "Fix" : "Est");
+ std::cout << std::endl;
+ }
+ }
}
+ else
+ std::cout << " " << getMapPtr()->getLandmarkList().size() << "L" << std::endl;
std::cout << "-----------------------------------------" << std::endl;
std::cout << std::endl;
}
-bool Problem::check()
+bool Problem::check(int verbose_level)
{
bool is_consistent = true;
std::cout << std::endl;
std::cout << "Wolf tree integrity ---------------------" << std::endl;
auto P_raw = this;
- std::cout << "P @ " << P_raw << std::endl;
auto H = hardware_ptr_;
- std::cout << "H @ " << H.get() << std::endl;
+ if (verbose_level > 0)
+ {
+ std::cout << "P @ " << P_raw << std::endl;
+ std::cout << "H @ " << H.get() << std::endl;
+ }
is_consistent = is_consistent && (H->getProblem().get() == P_raw);
- for (auto S : H->getSensorList() )
+ for (auto S : H->getSensorList())
{
- std::cout << " S" << S->id() << " @ " << S.get() << std::endl;
- std::cout << " -> P @ " << S->getProblem().get() << std::endl;
- std::cout << " -> H @ " << S->getHardwarePtr().get() << std::endl;
+ if (verbose_level > 0)
+ {
+ std::cout << " S" << S->id() << " @ " << S.get() << std::endl;
+ std::cout << " -> P @ " << S->getProblem().get() << std::endl;
+ std::cout << " -> H @ " << S->getHardwarePtr().get() << std::endl;
+ }
is_consistent = is_consistent && (S->getProblem().get() == P_raw);
is_consistent = is_consistent && (S->getHardwarePtr() == H);
- for (auto p : S->getProcessorList() )
+ for (auto p : S->getProcessorList())
{
- std::cout << " p" << p->id() << " @ " << p.get() << " -> S" << p->getSensorPtr()->id() << std::endl;
- std::cout << " -> P @ " << p->getProblem().get() << std::endl;
- std::cout << " -> S" << p->getSensorPtr()->id() << " @ " << p->getSensorPtr().get() << std::endl;
+ if (verbose_level > 0)
+ {
+ std::cout << " p" << p->id() << " @ " << p.get() << " -> S" << p->getSensorPtr()->id() << std::endl;
+ std::cout << " -> P @ " << p->getProblem().get() << std::endl;
+ std::cout << " -> S" << p->getSensorPtr()->id() << " @ " << p->getSensorPtr().get() << std::endl;
+ }
is_consistent = is_consistent && (p->getProblem().get() == P_raw);
is_consistent = is_consistent && (p->getSensorPtr() == S);
}
}
auto T = trajectory_ptr_;
- std::cout << "T @ " << T.get() << std::endl;
+ if (verbose_level > 0)
+ {
+ std::cout << "T @ " << T.get() << std::endl;
+ }
is_consistent = is_consistent && (T->getProblem().get() == P_raw);
- for (auto F : T->getFrameList() )
+ for (auto F : T->getFrameList())
{
- std::cout << (F->isKey() ? " KF" : " F") << F->id() << " @ " << F.get() << std::endl;
- std::cout << " -> P @ " << F->getProblem().get() << std::endl;
- std::cout << " -> T @ " << F->getTrajectoryPtr().get() << std::endl;
- is_consistent = is_consistent && (F->getProblem().get() == P_raw);
- is_consistent = is_consistent && (F->getTrajectoryPtr() == T);
- for (auto c : F->getConstrainedByList())
+ if (verbose_level > 0)
{
- std::cout << " <- c" << c->id() << " -> F" << c->getFrameOtherPtr()->id() << std::endl;
+ std::cout << (F->isKey() ? " KF" : " F") << F->id() << " @ " << F.get() << std::endl;
+ std::cout << " -> P @ " << F->getProblem().get() << std::endl;
+ std::cout << " -> T @ " << F->getTrajectoryPtr().get() << std::endl;
+ for (auto c : F->getConstrainedByList())
+ {
+ std::cout << " <- c" << c->id() << " -> F" << c->getFrameOtherPtr()->id() << std::endl;
+ }
}
- for (auto C : F->getCaptureList() )
+ is_consistent = is_consistent && (F->getProblem().get() == P_raw);
+ is_consistent = is_consistent && (F->getTrajectoryPtr() == T);
+ for (auto C : F->getCaptureList())
{
- std::cout << " C" << C->id() << " @" << C.get() << " -> S" << C->getSensorPtr()->id() << std::endl;
- std::cout << " -> P @ " << C->getProblem().get() << std::endl;
- std::cout << " -> F" << C->getFramePtr()->id() << " @ " << C->getFramePtr().get() << std::endl;
+ if (verbose_level > 0)
+ {
+ std::cout << " C" << C->id() << " @" << C.get() << " -> S" << C->getSensorPtr()->id() << std::endl;
+ std::cout << " -> P @ " << C->getProblem().get() << std::endl;
+ std::cout << " -> F" << C->getFramePtr()->id() << " @ " << C->getFramePtr().get() << std::endl;
+ }
is_consistent = is_consistent && (C->getProblem().get() == P_raw);
is_consistent = is_consistent && (C->getFramePtr() == F);
- for (auto f : C->getFeatureList() )
+ for (auto f : C->getFeatureList())
{
- std::cout << " f" << f->id() << " @" << f.get() << std::endl;
- std::cout << " -> P @ " << f->getProblem().get() << std::endl;
- std::cout << " -> C" << f->getCapturePtr()->id() << " @ " << f->getCapturePtr().get() << std::endl;
+ if (verbose_level > 0)
+ {
+ std::cout << " f" << f->id() << " @" << f.get() << std::endl;
+ std::cout << " -> P @ " << f->getProblem().get() << std::endl;
+ std::cout << " -> C" << f->getCapturePtr()->id() << " @ " << f->getCapturePtr().get()
+ << std::endl;
+ }
is_consistent = is_consistent && (f->getProblem().get() == P_raw);
is_consistent = is_consistent && (f->getCapturePtr() == C);
- for (auto c : f->getConstrainedByList())
+ if (verbose_level > 0)
{
- std::cout << " <- c" << c->id() << " -> f" << c->getFeatureOtherPtr()->id() << std::endl;
+ for (auto c : f->getConstrainedByList())
+ {
+ std::cout << " <- c" << c->id() << " -> f" << c->getFeatureOtherPtr()->id() << std::endl;
+ }
}
- for (auto c : f->getConstraintList() )
+ for (auto c : f->getConstraintList())
{
- std::cout << " c" << c->id() << " @" << C.get();
+ if (verbose_level > 0)
+ std::cout << " c" << c->id() << " @" << C.get();
switch (c->getCategory())
{
case CTR_ABSOLUTE:
- std::cout << " --> A" << std::endl;
+ if (verbose_level > 0)
+ std::cout << " --> A" << std::endl;
break;
case CTR_FRAME:
- std::cout << " --> F" << c->getFrameOtherPtr()->id() << std::endl;
+ {
+ auto Fo = c->getFrameOtherPtr();
+ if (verbose_level > 0)
+ std::cout << " --> F" << Fo->id() << " <- ";
+ bool found = false;
+ for (auto cby : Fo->getConstrainedByList())
+ {
+ if (verbose_level > 0)
+ std::cout << " c" << cby->id();
+ found = found || (c == cby);
+ }
+ if (verbose_level > 0)
+ std::cout << std::endl;
+ is_consistent = is_consistent && found;
break;
+ }
case CTR_FEATURE:
- std::cout << " --> f" << c->getFeatureOtherPtr()->id() << std::endl;
+ {
+ auto fo = c->getFeatureOtherPtr();
+ if (verbose_level > 0)
+ std::cout << " --> f" << fo->id() << " <- ";
+ bool found = false;
+ for (auto cby : fo->getConstrainedByList())
+ {
+ if (verbose_level > 0)
+ std::cout << " c" << cby->id();
+ found = found || (c == cby);
+ }
+ if (verbose_level > 0)
+ std::cout << std::endl;
+ is_consistent = is_consistent && found;
break;
+ }
case CTR_LANDMARK:
- std::cout << " --> L" << c->getLandmarkOtherPtr()->id() << std::endl;
+ {
+ auto Lo = c->getLandmarkOtherPtr();
+ if (verbose_level > 0)
+ std::cout << " --> L" << Lo->id() << " <- ";
+ bool found = false;
+ for (auto cby : Lo->getConstrainedByList())
+ {
+ if (verbose_level > 0)
+ std::cout << " c" << cby->id();
+ found = found || (c == cby);
+ }
+ if (verbose_level > 0)
+ std::cout << std::endl;
+ is_consistent = is_consistent && found;
break;
+ }
+ }
+ if (verbose_level > 0)
+ {
+ std::cout << " -> P @ " << c->getProblem().get() << std::endl;
+ std::cout << " -> f" << c->getFeaturePtr()->id() << " @ " << c->getFeaturePtr().get()
+ << std::endl;
}
- std::cout << " -> P @ " << c->getProblem().get() << std::endl;
- std::cout << " -> f" << c->getFeaturePtr()->id() << " @ " << c->getFeaturePtr().get() << std::endl;
is_consistent = is_consistent && (c->getProblem().get() == P_raw);
is_consistent = is_consistent && (c->getFeaturePtr() == f);
}
@@ -754,24 +892,28 @@ bool Problem::check()
}
}
auto M = map_ptr_;
- std::cout << "M @ " << M.get() << std::endl;
+ if (verbose_level > 0)
+ std::cout << "M @ " << M.get() << std::endl;
is_consistent = is_consistent && (M->getProblem().get() == P_raw);
- for (auto L : M->getLandmarkList() )
+ for (auto L : M->getLandmarkList())
{
- std::cout << " L" << L->id() << " @" << L.get() << std::endl;
+ if (verbose_level > 0)
+ std::cout << " L" << L->id() << " @" << L.get() << std::endl;
is_consistent = is_consistent && (L->getProblem().get() == P_raw);
is_consistent = is_consistent && (L->getMapPtr() == M);
- for (auto c : L->getConstrainedByList())
+ bool found = false;
+ for (auto cby : L->getConstrainedByList())
{
- std::cout << " <- c" << c->id() << " -> L" << c->getLandmarkOtherPtr()->id() << std::endl;
+ if (verbose_level > 0)
+ std::cout << " <- c" << cby->id() << " -> L" << cby->getLandmarkOtherPtr()->id() << std::endl;
+ found = found || (cby->getLandmarkOtherPtr() && L == cby->getLandmarkOtherPtr());
}
+ is_consistent = is_consistent && found;
}
-// std::cout << std::endl;
- std::cout << "--------------------------- Wolf tree " << (is_consistent ? " OK" : "NOK") << std::endl;
+ std::cout << "--------------------------- Wolf tree " << (is_consistent ? " OK" : "Not OK !!") << std::endl;
std::cout << std::endl;
-
return is_consistent;
}
diff --git a/src/problem.h b/src/problem.h
index 62b8bef241e096e885f9295262227e161c59c23f..21f88aaa03d69b9209b50a3a2d27371e37b212c6 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -245,10 +245,13 @@ class Problem : public std::enable_shared_from_this<Problem>
// Print and check ---------------------------------------
/**
* \brief print wolf tree
- * \param level : 0: Basic links; 1: with Constrained_by; with 2: Metrics; default: 1.
+ * \param depth : levels to show ( 0: H, T, M : 1: H:S:p, T:F, M:L ; 2: T:F:C ; 3: T:F:C:f ; 4: T:F:C:f:c )
+ * \param constr_by: show constraints pointing to F, f and L.
+ * \param metric : show metric info (status, time stamps, state vectors, measurements)
+ * \param state_blocks : show state blocks
*/
- void print(int level = 1);
- bool check();
+ void print(int depth = 4, bool constr_by = false, bool metric = true, bool state_blocks = false);
+ bool check(int verbose_level = 0);
};
diff --git a/src/processor_base.cpp b/src/processor_base.cpp
index 886d959c00935812b87c005774a896e6bb738076..89849f3d7c706320f808b43f5b77ab04bc998fbe 100644
--- a/src/processor_base.cpp
+++ b/src/processor_base.cpp
@@ -12,6 +12,7 @@ ProcessorBase::ProcessorBase(const std::string& _type, const Scalar& _time_toler
processor_id_(++processor_id_count_),
time_tolerance_(_time_tolerance)
{
+ setType(_type);
std::cout << "constructed +p" << id() << std::endl;
//
diff --git a/src/processor_base.h b/src/processor_base.h
index fc8405c3a2260adb65f4869561da8f2b80fe081e..af502ae8640eaaaea81549f1631c3a43cdc68bf7 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -67,6 +67,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
ProblemPtr getProblem();
+ void setTimeTolerance(Scalar _time_tolerance);
+
private:
static unsigned int processor_id_count_;
@@ -117,6 +119,11 @@ inline const SensorBasePtr ProcessorBase::getSensorPtr() const
return sensor_ptr_.lock();
}
+inline void ProcessorBase::setTimeTolerance(Scalar _time_tolerance)
+{
+ time_tolerance_ = _time_tolerance;
+}
+
} // namespace wolf
#endif
diff --git a/src/processor_image_feature.cpp b/src/processor_image_feature.cpp
index 1c03d30179b86441fd301bc83080274c2565e279..8b4de59670ef39ab6cf7967faa52cdec3e9cf237 100644
--- a/src/processor_image_feature.cpp
+++ b/src/processor_image_feature.cpp
@@ -160,7 +160,7 @@ unsigned int ProcessorImageFeature::trackFeatures(const FeatureBaseList& _featur
tracker_roi_.pop_back();
}
}
- std::cout << "TrackFeatures - Number of Features tracked: " << _feature_list_out.size() << std::endl;
+// std::cout << "TrackFeatures - Number of Features tracked: " << _feature_list_out.size() << std::endl;
return _feature_list_out.size();
}
diff --git a/src/processor_image_landmark.cpp b/src/processor_image_landmark.cpp
index c6e9bb2446e2379a8c14eb56fe4fd44c89867b39..5b1d6c4da5f7ad3ee467af0a1a1e7181c34a00ea 100644
--- a/src/processor_image_landmark.cpp
+++ b/src/processor_image_landmark.cpp
@@ -111,7 +111,6 @@ unsigned int ProcessorImageLandmark::findLandmarks(const LandmarkBaseList& _land
FeatureBaseList& _feature_list_out,
LandmarkMatchMap& _feature_landmark_correspondences)
{
- WOLF_DEBUG_HERE
unsigned int roi_width = params_.matcher.roi_width;
unsigned int roi_heigth = params_.matcher.roi_height;
@@ -190,14 +189,13 @@ unsigned int ProcessorImageLandmark::findLandmarks(const LandmarkBaseList& _land
lmk_nbr++;
}
- std::cout << "\tNumber of Features tracked: " << _feature_list_out.size() << std::endl;
+// std::cout << "\tNumber of Features tracked: " << _feature_list_out.size() << std::endl;
landmarks_tracked_ = _feature_list_out.size();
return _feature_list_out.size();
}
bool ProcessorImageLandmark::voteForKeyFrame()
{
- WOLF_DEBUG_HERE
return false;
// return landmarks_tracked_ < params_.algorithm.min_features_for_keyframe;
@@ -205,7 +203,6 @@ bool ProcessorImageLandmark::voteForKeyFrame()
unsigned int ProcessorImageLandmark::detectNewFeatures(const unsigned int& _max_features)
{
- WOLF_DEBUG_HERE
cv::Rect roi;
std::vector<cv::KeyPoint> new_keypoints;
@@ -253,7 +250,6 @@ unsigned int ProcessorImageLandmark::detectNewFeatures(const unsigned int& _max_
LandmarkBasePtr ProcessorImageLandmark::createLandmark(FeatureBasePtr _feature_ptr)
{
- WOLF_DEBUG_HERE
std::shared_ptr<FeaturePointImage> feat_point_image_ptr = std::static_pointer_cast<FeaturePointImage>( _feature_ptr);
FrameBasePtr anchor_frame = getLastPtr()->getFramePtr();
@@ -284,7 +280,6 @@ LandmarkBasePtr ProcessorImageLandmark::createLandmark(FeatureBasePtr _feature_p
ConstraintBasePtr ProcessorImageLandmark::createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
{
- WOLF_DEBUG_HERE
if ((std::static_pointer_cast<LandmarkAHP>(_landmark_ptr))->getAnchorFrame() == last_ptr_->getFramePtr())
{
@@ -439,7 +434,7 @@ void ProcessorImageLandmark::drawFeaturesFromLandmarks(cv::Mat _image)
//candidate - cyan
cv::Point2f point = (std::static_pointer_cast<FeaturePointImage>(feature_point))->getKeypoint().pt;
cv::circle(_image, point, 2, cv::Scalar(255.0, 255.0, 0.0), -1, 8, 0);
- std::cout << "feature of landmark [" << (feature_point)->landmarkId() << "] in: " << point << std::endl;
+// std::cout << "feature of landmark [" << (feature_point)->landmarkId() << "] in: " << point << std::endl;
cv::Point2f point2 = point;
point2.x = point2.x - 16;
@@ -503,7 +498,7 @@ void ProcessorImageLandmark::drawLandmarks(cv::Mat _image)
cv::putText(_image, std::to_string(counter), label_for_landmark_point2,
cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255.0, 0.0, 255.0));
- std::cout << "\t\tTotal landmarks: " << counter << std::endl;
+// std::cout << "\t\tTotal landmarks: " << counter << std::endl;
cv::imshow("Feature tracker", _image);
}
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index f7317b31d15f2f3007e146d550524eb7b3f35fbc..cdf5bf891f95c291f4e859a475efee143b6e492b 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -20,26 +20,41 @@ ProcessorMotion::~ProcessorMotion()
void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
{
- WOLF_DEBUG_HERE
+
+ /* Status:
+ * KF --- KF --- F ----
+ * o l i
+ */
incoming_ptr_ = std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
+ /* Status:
+ * KF --- KF --- F ---- *
+ * o l i
+ */
+
preProcess();
integrate();
if (voteForKeyFrame() && permittedKeyFrame())
{
+ WOLF_DEBUG_HERE
// key_capture
CaptureMotion::Ptr key_capture_ptr = last_ptr_;
FrameBasePtr key_frame_ptr = key_capture_ptr->getFramePtr();
- if (key_frame_ptr == nullptr) {WOLF_DEBUG_HERE; std::runtime_error("bad pointer");}
- // Set the frame as key
+ // Set the frame of key_capture as key
key_frame_ptr->setState(getCurrentState());
key_frame_ptr->setTimeStamp(getBuffer().get().back().ts_);
key_frame_ptr->setKey();
- // create motion feature and add it to the capture
+ /* Status:
+ * KF --- KF --- KF ---
+ * o l i
+ */
+
+
+ // create motion feature and add it to the key_capture
FeatureBasePtr key_feature_ptr = std::make_shared<FeatureBase>(
"MOTION",
key_capture_ptr->getBuffer().get().back().delta_integr_,
@@ -54,19 +69,27 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
origin_ptr_->getFramePtr() -> addConstrainedBy(ctr_ptr);
// new last capture
- last_ptr_ = std::make_shared<CaptureMotion>(key_frame_ptr->getTimeStamp(), getSensorPtr(),
+ last_ptr_ = std::make_shared<CaptureMotion>(key_frame_ptr->getTimeStamp(),
+ getSensorPtr(),
Eigen::VectorXs::Zero(data_size_),
- Eigen::MatrixXs::Zero(data_size_, data_size_), key_frame_ptr);
+ Eigen::MatrixXs::Zero(data_size_, data_size_),
+ key_frame_ptr);
// create a new last frame
FrameBasePtr new_frame_ptr = getProblem()->createFrame(NON_KEY_FRAME, key_frame_ptr->getState(), last_ptr_->getTimeStamp());
new_frame_ptr->addCapture(last_ptr_); // Add Capture to the new Frame
- // reset processor origin
+ // reset processor origin to the new keyframe's capture
origin_ptr_ = key_capture_ptr;
getBuffer().get().push_back(Motion( {key_frame_ptr->getTimeStamp(), deltaZero(), deltaZero(),
Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_),
Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_)}));
+
+ /* Status:
+ * KF --- KF --- KF --- F ----
+ * o l i
+ */
+
delta_integrated_ = deltaZero();
delta_integrated_cov_.setZero();
@@ -74,12 +97,13 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
resetDerived();
getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), time_tolerance_);
- //// debug cout
- //Eigen::VectorXs interpolated_state(3);
- //xPlusDelta(origin_ptr_->getFramePtr()->getState(), key_capture_ptr->getBufferPtr()->get().back().delta_integr_, interpolated_state);
- //std::cout << "\tinterpolated state: " << interpolated_state.transpose() << std::endl;
}
+
+
postProcess();
+
+ // clear incoming just in case
+ incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
}
CaptureMotion::Ptr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
@@ -113,12 +137,16 @@ CaptureMotion::Ptr ProcessorMotion::findCaptureContainingTimeStamp(const TimeSta
void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
{
- WOLF_DEBUG_HERE
assert(_origin_frame->getTrajectoryPtr() != nullptr
&& "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
+ /* Status:
+ * * --- * ---
+ * o l i
+ */
+
// make (empty) origin Capture
origin_ptr_ = std::make_shared<CaptureMotion>(_origin_frame->getTimeStamp(), getSensorPtr(),
Eigen::VectorXs::Zero(data_size_),
@@ -126,18 +154,28 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// Add origin capture to origin frame
_origin_frame->addCapture(origin_ptr_);
+ /* Status:
+ * KF --- * ---
+ * o l i
+ */
+
// make (emtpy) last Capture
last_ptr_ = std::make_shared<CaptureMotion>(_origin_frame->getTimeStamp(),
getSensorPtr(),
Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_),
_origin_frame);
- // Make frame at last Capture
+ // Make non-key-frame at last Capture
// makeFrame(last_ptr_, _origin_frame->getState(), NON_KEY_FRAME);
FrameBasePtr new_frame_ptr = getProblem()->createFrame(NON_KEY_FRAME,
_origin_frame->getState(),
last_ptr_->getTimeStamp());
- new_frame_ptr->addCapture(last_ptr_); // Add last Capture to the new Frame
+ new_frame_ptr->addCapture(last_ptr_);
+
+ /* Status:
+ * KF --- F ---
+ * o l i
+ */
// Reset deltas
delta_ = deltaZero();
@@ -155,19 +193,22 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
bool ProcessorMotion::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other)
{
- WOLF_DEBUG_HERE
- Scalar time_tol = std::min(time_tolerance_, _time_tol_other);
- std::cout << " Time tol this " << time_tolerance_ << std::endl;
- std::cout << " Time tol other " << _time_tol_other << std::endl;
- std::cout << " Time tol eff " << time_tol << std::endl;
-
- std::cout << " Time stamp input F " << _keyframe_ptr->getTimeStamp().get() << std::endl;
- std::cout << " Time stamp orig F " << getOriginPtr()->getFramePtr()->getTimeStamp().get() << std::endl;
- std::cout << " Time stamp orig C " << getOriginPtr()->getTimeStamp().get() << std::endl;
- std::cout << " Time stamp last F " << getLastPtr()->getFramePtr()->getTimeStamp().get() << std::endl;
- std::cout << " Time stamp last C " << getLastPtr()->getTimeStamp().get() << std::endl;
+ // Scalar time_tol = std::min(time_tolerance_, _time_tol_other);
+ // std::cout << " Time tol this " << time_tolerance_ << std::endl;
+ // std::cout << " Time tol other " << _time_tol_other << std::endl;
+ // std::cout << " Time tol eff " << time_tol << std::endl;
+ //
+ // std::cout << " Time stamp input F " << _keyframe_ptr->getTimeStamp().get() << std::endl;
+ // std::cout << " Time stamp orig F " << getOriginPtr()->getFramePtr()->getTimeStamp().get() << std::endl;
+ // std::cout << " Time stamp orig C " << getOriginPtr()->getTimeStamp().get() << std::endl;
+ // std::cout << " Time stamp last F " << getLastPtr()->getFramePtr()->getTimeStamp().get() << std::endl;
+ // std::cout << " Time stamp last C " << getLastPtr()->getTimeStamp().get() << std::endl;
+ WOLF_DEBUG_HERE
+ std::cout << "PM: KF" << _keyframe_ptr->id() << " callback received at ts= " << _keyframe_ptr->getTimeStamp().get() << std::endl;
+ std::cout << " while last ts= " << last_ptr_->getTimeStamp().get() << std::endl;
+ std::cout << " while last's frame ts= " << last_ptr_->getFramePtr()->getTimeStamp().get() << std::endl;
assert(_keyframe_ptr->getTrajectoryPtr() != nullptr
&& "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
@@ -226,7 +267,7 @@ bool ProcessorMotion::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar&
// modify feature and constraint (if they exist)
if (!capture_ptr->getFeatureList().empty())
{
- FeatureBasePtr feature_ptr = capture_ptr->getFeatureList().front();
+ FeatureBasePtr feature_ptr = capture_ptr->getFeatureList().back(); // there is only one feature!
// modify feature
feature_ptr->setMeasurement(capture_ptr->getBuffer().get().back().delta_integr_);
@@ -236,14 +277,18 @@ bool ProcessorMotion::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar&
Eigen::MatrixXs::Identity(delta_size_, delta_size_) * 1e-8);
// modify constraint
- if (!feature_ptr->getConstraintList().empty())
- {
- auto ctr = feature_ptr->getConstraintList().front();
- feature_ptr->getConstraintList().remove(ctr);
+ // Instead of modifying, we remove one ctr, and create a new one.
+
+ // get the constraint to be removed later
+ auto ctr_to_remove = feature_ptr->getConstraintList().back(); // there is only one constraint!
+
+ // create and append new constraint
+ auto new_ctr = feature_ptr->addConstraint(createConstraint(feature_ptr, _keyframe_ptr));
+ _keyframe_ptr->addConstrainedBy(new_ctr);
+
+ // remove old constraint now (otherwise c->remove() gets propagated to f, C, F, etc.)
+ ctr_to_remove->remove();
- // feature_ptr->getConstraintList().front()->remove(); // XXX What happens here?
- feature_ptr->addConstraint(createConstraint(feature_ptr, _keyframe_ptr));
- }
}
return true;
@@ -251,7 +296,6 @@ bool ProcessorMotion::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar&
void ProcessorMotion::integrate()
{
- WOLF_DEBUG_HERE
// Set dt
updateDt();
@@ -269,11 +313,15 @@ void ProcessorMotion::integrate()
// then push it into buffer
getBuffer().get().push_back(Motion( {incoming_ptr_->getTimeStamp(), delta_, delta_integrated_, delta_cov_,
delta_integrated_cov_}));
+
+ // Update state and time stamps
+ last_ptr_->getFramePtr()->setState(getCurrentState());
+ last_ptr_->setTimeStamp(incoming_ptr_->getTimeStamp());
+ last_ptr_->getFramePtr()->setTimeStamp(last_ptr_->getTimeStamp());
}
void ProcessorMotion::reintegrate(CaptureMotion::Ptr _capture_ptr)
{
- WOLF_DEBUG_HERE
// start with empty motion
_capture_ptr->getBuffer().get().push_front(motionZero(_capture_ptr->getOriginFramePtr()->getTimeStamp()));
diff --git a/src/processor_motion.h b/src/processor_motion.h
index 35a3de7b5d96130f5c7e1ceffd76f80391377eb1..95eed1f740b41c16a6dec2ff8fa63a0b1c72e6d5 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -369,7 +369,6 @@ inline void ProcessorMotion::resetDerived()
inline bool ProcessorMotion::voteForKeyFrame()
{
- WOLF_DEBUG_HERE
return false;
}
diff --git a/src/processor_odom_2D.cpp b/src/processor_odom_2D.cpp
index 24ec3019102ee3a6f6fd897b96d79ca273fea7b6..283c340f0c136ea0f9fbd060ef6e30c43c8f7c33 100644
--- a/src/processor_odom_2D.cpp
+++ b/src/processor_odom_2D.cpp
@@ -4,8 +4,24 @@ namespace wolf
ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
{
- std::shared_ptr<ProcessorParamsOdom2D> params = std::static_pointer_cast<ProcessorParamsOdom2D>(_params);
- std::shared_ptr<ProcessorOdom2D> prc_ptr = std::make_shared<ProcessorOdom2D>(params->dist_traveled_th_, params->cov_det_th_, params->elapsed_time_th_);
+ Scalar dist_traveled_th ;
+ Scalar cov_det_th ;
+ Scalar elapsed_time_th ;
+ if (_params)
+ {
+ std::shared_ptr<ProcessorParamsOdom2D> params = std::static_pointer_cast<ProcessorParamsOdom2D>(_params);
+ dist_traveled_th = params->dist_traveled_th_;
+ cov_det_th = params->cov_det_th_;
+ elapsed_time_th = params->elapsed_time_th_;
+ }
+ else
+ {
+ std::cout << __FILE__ << ":" << __FUNCTION__ << "() : No parameters provided. Using dummy set." << std::endl;
+ dist_traveled_th = 1;
+ cov_det_th = 1;
+ elapsed_time_th = 1;
+ }
+ std::shared_ptr<ProcessorOdom2D> prc_ptr = std::make_shared<ProcessorOdom2D>(dist_traveled_th, cov_det_th, elapsed_time_th);
prc_ptr->setName(_unique_name);
return prc_ptr;
}
diff --git a/src/processor_odom_2D.h b/src/processor_odom_2D.h
index 21f52a3c4e904a7eb59c23ad2032189ed83f1c54..105bd97e5a7cbd004d5c53730962d3736a9543c9 100644
--- a/src/processor_odom_2D.h
+++ b/src/processor_odom_2D.h
@@ -105,8 +105,11 @@ inline void ProcessorOdom2D::data2delta(const Eigen::VectorXs& _data, const Eige
delta_cov_ = J * _data_cov * J.transpose();
- //std::cout << "data cov:" << std::endl << _data_cov << std::endl;
- //std::cout << "delta cov:" << std::endl << _delta_cov << std::endl;
+ //WOLF_DEBUG_HERE
+ //std::cout << "data :" << _data.transpose() << std::endl;
+ //std::cout << "data cov :" << std::endl << _data_cov << std::endl;
+ //std::cout << "delta :" << delta_.transpose() << std::endl;
+ //std::cout << "delta cov :" << std::endl << delta_cov << std::endl;
}
inline void ProcessorOdom2D::xPlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const Scalar _Dt, Eigen::VectorXs& _x_plus_delta)
@@ -230,21 +233,21 @@ inline Motion ProcessorOdom2D::interpolate(const Motion& _motion_ref, Motion& _m
inline bool ProcessorOdom2D::voteForKeyFrame()
{
//std::cout << "ProcessorOdom2D::voteForKeyFrame: traveled distance " << getBufferPtr()->get().back().delta_integr_.norm() << std::endl;
- if (getBuffer().get().back().delta_integr_.norm() > dist_traveled_th_)
+ if (getBuffer().get().back().delta_integr_.head<2>().norm() > dist_traveled_th_)
{
- std::cout << "ProcessorOdom2D:: " << this->id() << "VOTE FOR KEY FRAME traveled distance "
- << getBuffer().get().back().delta_integr_.norm() << std::endl;
+ std::cout << "ProcessorOdom2D:: " << id() << " - VOTE FOR KEY FRAME traveled distance "
+ << getBuffer().get().back().delta_integr_.head<2>().norm() << std::endl;
return true;
}
if (getBuffer().get().back().delta_integr_cov_.determinant() > cov_det_th_)
{
- std::cout << "ProcessorOdom2D:: " << this->id() << "VOTE FOR KEY FRAME covariance det "
+ std::cout << "ProcessorOdom2D:: " << id() << " - VOTE FOR KEY FRAME covariance det "
<< getBuffer().get().back().delta_integr_cov_.determinant() << std::endl;
return true;
}
if (getBuffer().get().back().ts_.get() - origin_ptr_->getFramePtr()->getTimeStamp().get() > elapsed_time_th_)
{
- std::cout << "ProcessorOdom2D:: " << this->id() << "VOTE FOR KEY FRAME elapsed time "
+ std::cout << "ProcessorOdom2D:: " << id() << " - VOTE FOR KEY FRAME elapsed time "
<< getBuffer().get().back().ts_.get() - origin_ptr_->getFramePtr()->getTimeStamp().get()
<< std::endl;
return true;
diff --git a/src/processor_odom_3D.h b/src/processor_odom_3D.h
index 39bf8bcfac8b60e70b29cf27632e1b1a16fb974a..5f799e854aacf2d665f8c643cc547eb6f93717f6 100644
--- a/src/processor_odom_3D.h
+++ b/src/processor_odom_3D.h
@@ -213,7 +213,16 @@ inline Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref,
inline bool ProcessorOdom3D::voteForKeyFrame()
{
- return true;
+ if (getBuffer().get().size() > 3)
+ {
+ std::cout << "PM::vote" << std::endl;
+ return true;
+ }
+ else
+ {
+ std::cout << "PM::do not vote" << std::endl;
+ return false;
+ }
}
inline ConstraintBasePtr ProcessorOdom3D::createConstraint(FeatureBasePtr _feature_motion,
diff --git a/src/processor_tracker.cpp b/src/processor_tracker.cpp
index 1cf3d933ad4f66b3cb01d339b9129fd659cbd8e5..12b841fe90d0aff9dd40c9994c0e9cebca231b29 100644
--- a/src/processor_tracker.cpp
+++ b/src/processor_tracker.cpp
@@ -28,7 +28,6 @@ ProcessorTracker::~ProcessorTracker()
void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
{
- WOLF_DEBUG_HERE
using std::abs;
@@ -41,7 +40,11 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
if (origin_ptr_ == nullptr && last_ptr_ == nullptr)
{
std::cout << "FIRST TIME" << std::endl;
- //std::cout << "Features in origin: " << 0 << "; in last: " << 0 << std::endl;
+
+ /* Status:
+ * * ---- * ---- * ---- KF: keyframes; F: frame
+ * o l i captures: origin, last, incoming
+ */
// advance
advance();
@@ -50,30 +53,56 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
+ /* Status:
+ * * ---- * ---- KF: keyframes; F: frame
+ * o l i captures: origin, last, incoming
+ */
+
// keyframe creation on last
FrameBasePtr closest_key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
if (closest_key_frm && abs(closest_key_frm->getTimeStamp() - last_ptr_->getTimeStamp()) <= time_tolerance_)
{
- std::cout << "Setting key-frame to last_" << std::endl;
+ WOLF_DEBUG_HERE
closest_key_frm->addCapture(last_ptr_);
closest_key_frm->setKey();
+ std::cout << "Last appended to existing F, set KF" << closest_key_frm->id() << std::endl;
+
+ /* Status:
+ * * ---- KF --- KF: keyframes; F: frame
+ * o l i captures: origin, last, incoming
+ */
+
}
else
{
+ WOLF_DEBUG_HERE
//makeFrame(last_ptr_, KEY_FRAME);
//makeFrame(last_ptr_, getProblem()->getStateAtTimeStamp(last_ptr_->getTimeStamp()), KEY_FRAME);
- std::cout << "Making key-frame with last_" << std::endl;
FrameBasePtr new_frame_ptr = getProblem()->createFrame(KEY_FRAME,
getProblem()->getStateAtTimeStamp(last_ptr_->getTimeStamp()),
last_ptr_->getTimeStamp());
new_frame_ptr->addCapture(last_ptr_); // Add incoming Capture to the new Frame
+ std::cout << "Last appended to new KF" << new_frame_ptr->id() << std::endl;
+
+ getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), time_tolerance_);
+
+ /* Status:
+ * * ---- KF --- KF: keyframes; F: frame
+ * o l i captures: origin, last, incoming
+ */
}
// Detect new Features, initialize Landmarks, create Constraints, ...
processNew(max_new_features_);
+ /* Status:
+ * * ---- KF --- KF: keyframes; F: frame
+ * o l i captures: origin, last, incoming
+ * n new features
+ */
+
// Establish constraints from last
establishConstraints();
@@ -85,23 +114,49 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
{
std::cout << "SECOND TIME or after KEY FRAME CALLBACK" << std::endl;
+ /* Status:
+ * * ---- KF --- KF: keyframes; F: frame
+ * o l i captures: origin, last, incoming
+ * n new features
+ */
+
// First we track the known Features
processKnown();
+ /* Status:
+ * * ---- KF --- KF: keyframes; F: frame
+ * o l i captures: origin, last, incoming
+ * k k known features
+ */
+
// Create a new non-key Frame in the Trajectory with the incoming Capture
FrameBasePtr closest_key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(incoming_ptr_->getTimeStamp());
if (closest_key_frm && abs(closest_key_frm->getTimeStamp() - incoming_ptr_->getTimeStamp()) <= time_tolerance_)
{
// Just append the Capture to the existing keyframe
closest_key_frm->addCapture(incoming_ptr_);
- std::cout << "Appended to frame" << closest_key_frm->id() << std::endl;
- }
+ std::cout << "Incoming appended to F" << closest_key_frm->id() << std::endl;
+
+ /* Status:
+ * * ---- KF --- KF KF: keyframes; F: frame
+ * o l i captures: origin, last, incoming
+ * k k known features
+ */
+
+ }
else
{
// Create a frame to hold what will become the last Capture
FrameBasePtr new_frame_ptr = getProblem()->createFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
new_frame_ptr->addCapture(incoming_ptr_); // Add incoming Capture to the new Frame
- std::cout << "Made frame" << new_frame_ptr->id() << std::endl;
+ std::cout << "Incoming in new F" << new_frame_ptr->id() << std::endl;
+
+ /* Status:
+ * * ---- KF --- F KF: keyframes; F: frame
+ * o l i captures: origin, last, incoming
+ * k k known features
+ */
+
}
// Reset the derived Tracker
@@ -112,6 +167,14 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
+ std::cout << "origin <-- last <-- incoming" << std::endl;
+
+ /* Status:
+ * KF -- F/KF -- KF: keyframes; F: frame
+ * o l i captures: origin, last, incoming
+ * k k known features
+ */
+
}
// OTHER TIMES
else
@@ -120,16 +183,16 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// 1. First we track the known Features and create new constraints as needed
- /**
- * KF --- KF --- F KF: keyframes; F: frame
+ /* Status:
+ * KF --- KF --- F ---- KF: keyframes; F: frame
* o l i captures: origin, last, incoming
* k k known features
*/
processKnown();
- /**
- * KF --- KF --- F KF: keyframes; F: frame
+ /* Status:
+ * KF --- KF --- F ---- KF: keyframes; F: frame
* o l i captures: origin, last, incoming
* k k k known features
*/
@@ -150,13 +213,12 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
if ( !( (voteForKeyFrame() && permittedKeyFrame() ) || closest_key_frm_to_last ) )
{
- WOLF_DEBUG_HERE
// 2.a. We did not create a KeyFrame:
- /**
- * KF --- KF --- F ---- * frames
- * o l i captures: incoming
+ /* Status:
+ * KF --- KF --- F ---- *
+ * o l i
* k k k known features
*/
@@ -164,13 +226,15 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
advance();
// Advance this
- last_ptr_->getFramePtr()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's Frame
+ last_ptr_->getFramePtr()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
last_ptr_->remove();
incoming_ptr_->getFramePtr()->setTimeStamp(incoming_ptr_->getTimeStamp());
last_ptr_ = incoming_ptr_; // Incoming Capture takes the place of last Capture
incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
- /**
+ std::cout << "last <-- incoming" << std::endl;
+
+ /* Status:
* KF --- KF ---------- F
* o - l - : deleted capture
* k - k - : deleted features
@@ -178,11 +242,10 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
}
else
{
- WOLF_DEBUG_HERE
// 2.b. We create a KF
- /**
+ /* Status:
* KF --- KF --- F ---- * frames
* o l i captures: incoming
* k k k known features
@@ -191,7 +254,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// Detect new Features, initialize Landmarks, create Constraints, ...
processNew(max_new_features_);
- /**
+ /* Status:
* After keyframe vote and re-processing last and incoming
* KF --- KF --- F ---- *
* o l i
@@ -199,28 +262,27 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
* n n new features
*/
- // Create a new non-key Frame in the Trajectory with the incoming Capture
FrameBasePtr key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(incoming_ptr_->getTimeStamp());
if ( abs(key_frm->getTimeStamp() - incoming_ptr_->getTimeStamp() ) < time_tolerance_)
{
- WOLF_DEBUG_HERE
// Append incoming to existing key-frame
key_frm->addCapture(incoming_ptr_); // TODO I think it should be last_ptr_ not incoming!
- std::cout << "Adhered to existing KF" << key_frm->id() << std::endl;
+ std::cout << "Incoming adhered to existing KF" << key_frm->id() << std::endl;
}
else
{
- WOLF_DEBUG_HERE
+ // Create a new non-key Frame in the Trajectory with the incoming Capture
// Make a non-key-frame to hold incoming
FrameBasePtr new_frame_ptr = getProblem()->createFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
new_frame_ptr->addCapture(incoming_ptr_); // Add incoming Capture to the new Frame
+ std::cout << "Incoming adhered to new F" << key_frm->id() << std::endl;
// Make the last Capture's Frame a KeyFrame
setKeyFrame(last_ptr_);
- std::cout << "Adhered to new created KF" << key_frm->id() << std::endl;
+ std::cout << "Set KEY to last F" << key_frm->id() << std::endl;
}
- /**
+ /* Status:
* KF --- KF --- KF --- F
* o l i
* k k k
@@ -239,7 +301,9 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
- /**
+ std::cout << "origin <-- last <-- incoming" << std::endl;
+
+ /* Status:
* KF --- KF --- KF --- F
* o l
* k k
@@ -256,34 +320,14 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
bool ProcessorTracker::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other)
{
WOLF_DEBUG_HERE
+ std::cout << "PT: KF" << _keyframe_ptr->id() << " callback received at ts= " << _keyframe_ptr->getTimeStamp().get() << std::endl;
+ std::cout << " while last ts= " << last_ptr_->getTimeStamp().get() << std::endl;
+ std::cout << " while last's frame ts= " << last_ptr_->getFramePtr()->getTimeStamp().get() << std::endl;
assert((last_ptr_ == nullptr || last_ptr_->getFramePtr() != nullptr) && "ProcessorTracker::keyFrameCallback: last_ptr_ must have a frame always");
- //======================
- // Start Debug info
Scalar time_tol = std::min(time_tolerance_, _time_tol_other);
- std::cout << " Time tol this " << time_tolerance_ << std::endl;
- std::cout << " Time tol other " << _time_tol_other << std::endl;
- std::cout << " Time tol eff " << time_tol << std::endl;
-
- if (_keyframe_ptr == nullptr) {WOLF_DEBUG_HERE; std::runtime_error("bad pointer");}
-
- std::cout << " Time stamp input F " << _keyframe_ptr->getTimeStamp().get() << std::endl;
- if (getOriginPtr()){
- if (getOriginPtr() == nullptr) {WOLF_DEBUG_HERE; std::runtime_error("bad pointer");}
- if (getOriginPtr()->getFramePtr() == nullptr) {WOLF_DEBUG_HERE; std::runtime_error("bad pointer");}
- std::cout << " Time stamp orig F " << getOriginPtr()->getFramePtr()->getTimeStamp().get() << std::endl;
- std::cout << " Time stamp orig C " << getOriginPtr()->getTimeStamp().get() << std::endl;
- }
- if (getLastPtr())
- {
- if (getLastPtr() == nullptr) {WOLF_DEBUG_HERE; std::runtime_error("bad pointer");}
- if (getLastPtr()->getFramePtr() == nullptr) {WOLF_DEBUG_HERE; std::runtime_error("bad pointer");}
- std::cout << " Time stamp last F " << getLastPtr()->getFramePtr()->getTimeStamp().get() << std::endl;
- std::cout << " Time stamp last C " << getLastPtr()->getTimeStamp().get() << std::endl;
- }
- // End Debug info
- //======================
+
// Nothing to do if:
// - there is no last
@@ -291,7 +335,6 @@ bool ProcessorTracker::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar
// - last frame is too far in time from keyframe
if (last_ptr_ == nullptr || last_ptr_->getFramePtr()->isKey() || std::abs(last_ptr_->getTimeStamp() - _keyframe_ptr->getTimeStamp()) > time_tol)
{
- WOLF_DEBUG_HERE
return false;
}
@@ -317,7 +360,6 @@ bool ProcessorTracker::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar
void ProcessorTracker::setKeyFrame(CaptureBasePtr _capture_ptr)
{
- WOLF_DEBUG_HERE
assert(_capture_ptr != nullptr && _capture_ptr->getFramePtr() != nullptr && "ProcessorTracker::setKeyFrame: null capture or capture without frame");
if (!_capture_ptr->getFramePtr()->isKey())
diff --git a/src/processor_tracker_feature.cpp b/src/processor_tracker_feature.cpp
index 0a733c3feff3f5183a7151d8b9a465fb261ea8cf..a9342814d436e0e2e2c9c02b86f626dc4d85efbe 100644
--- a/src/processor_tracker_feature.cpp
+++ b/src/processor_tracker_feature.cpp
@@ -21,7 +21,6 @@ ProcessorTrackerFeature::~ProcessorTrackerFeature()
unsigned int ProcessorTrackerFeature::processKnown()
{
- WOLF_DEBUG_HERE
// std::cout << "ProcessorTrackerFeature::processKnown()" << std::endl;
@@ -73,7 +72,6 @@ unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_fe
* At the end, all new Features are appended to the lists of known Features in
* the last and incoming Captures.
*/
- WOLF_DEBUG_HERE
// Populate the last Capture with new Features. The result is in new_features_last_.
diff --git a/src/processor_tracker_feature.h b/src/processor_tracker_feature.h
index fe50cc42008d896960e0929a4129bdd2ca1bae4e..1e76748b157a9ba618988938b5745014acfe0ae8 100644
--- a/src/processor_tracker_feature.h
+++ b/src/processor_tracker_feature.h
@@ -172,7 +172,6 @@ namespace wolf {
inline void ProcessorTrackerFeature::establishConstraints()
{
- WOLF_DEBUG_HERE
for (auto match : matches_origin_from_last_)
{
@@ -184,7 +183,6 @@ inline void ProcessorTrackerFeature::establishConstraints()
inline void ProcessorTrackerFeature::advance()
{
- WOLF_DEBUG_HERE
// std::cout << "ProcessorTrackerFeature::advance()" << std::endl;
@@ -206,7 +204,6 @@ inline void ProcessorTrackerFeature::advance()
inline void ProcessorTrackerFeature::reset()
{
- WOLF_DEBUG_HERE
// std::cout << "ProcessorTrackerFeature::reset()" << std::endl;
diff --git a/src/processor_tracker_feature_corner.cpp b/src/processor_tracker_feature_corner.cpp
index de4d2bb4e02d91b85cfee67e09cf5e7d9fb23a7f..c130221f67aec245c615a6d21f2ac3ac30f20a9f 100644
--- a/src/processor_tracker_feature_corner.cpp
+++ b/src/processor_tracker_feature_corner.cpp
@@ -111,9 +111,9 @@ ConstraintBasePtr ProcessorTrackerFeatureCorner::createConstraint(FeatureBasePtr
_feature_other_ptr->addConstraint(std::make_shared<ConstraintCorner2D>(_feature_other_ptr, landmark_ptr));
}
- std::cout << "creating constraint: last feature " << _feature_ptr->getMeasurement()
- << " with origin feature " << _feature_other_ptr->getMeasurement() << std::endl
- << " corresponding to landmark " << landmark_ptr->nodeId() << std::endl;
+// std::cout << "creating constraint: last feature " << _feature_ptr->getMeasurement()
+// << " with origin feature " << _feature_other_ptr->getMeasurement() << std::endl
+// << " corresponding to landmark " << landmark_ptr->nodeId() << std::endl;
return std::make_shared<ConstraintCorner2D>(_feature_ptr, landmark_ptr);
}
diff --git a/src/processor_tracker_feature_dummy.cpp b/src/processor_tracker_feature_dummy.cpp
index f245f831fabb0af8f058adee79e447153d9a8a1e..d8b5de1a0a23d253bc3d02e82ed4b1612c9e4fa5 100644
--- a/src/processor_tracker_feature_dummy.cpp
+++ b/src/processor_tracker_feature_dummy.cpp
@@ -39,7 +39,6 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList&
bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
{
- WOLF_DEBUG_HERE
std::cout << "N features: " << incoming_ptr_->getFeatureList().size() << std::endl;
bool vote = incoming_ptr_->getFeatureList().size() < min_feat_for_keyframe_;
std::cout << (vote ? "Vote ": "Not vote ") << "for KF" << std::endl;
@@ -57,8 +56,9 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int&
n_feature_++;
new_features_last_.push_back(
std::make_shared<FeatureBase>("POINT IMAGE", n_feature_* Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1)));
- std::cout << "feature " << new_features_last_.back()->getMeasurement() << " detected!" << std::endl;
+ //std::cout << "feature " << new_features_last_.back()->getMeasurement() << " detected!" << std::endl;
}
+ std::cout << new_features_last_.size() << " features detected!" << std::endl;
return new_features_last_.size();
}
diff --git a/src/processor_tracker_feature_dummy.h b/src/processor_tracker_feature_dummy.h
index 3cab6acd9ed241c58c00c3df15b71c95b8fd9390..a62695b3355ee51e5b54c684c8af20b7ee34138d 100644
--- a/src/processor_tracker_feature_dummy.h
+++ b/src/processor_tracker_feature_dummy.h
@@ -100,8 +100,8 @@ inline bool ProcessorTrackerFeatureDummy::correctFeatureDrift(const FeatureBaseP
inline ConstraintBasePtr ProcessorTrackerFeatureDummy::createConstraint(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr)
{
- std::cout << "creating constraint: last feature " << _feature_ptr->getMeasurement()
- << " with origin feature " << _feature_other_ptr->getMeasurement() << std::endl;
+// std::cout << "creating constraint: last feature " << _feature_ptr->getMeasurement()
+// << " with origin feature " << _feature_other_ptr->getMeasurement() << std::endl;
return std::make_shared<ConstraintEpipolar>(_feature_ptr, _feature_other_ptr);
}
diff --git a/src/processor_tracker_landmark.cpp b/src/processor_tracker_landmark.cpp
index 1897f16f7966871c016f2c052d29ade8dfc8bf8a..8a55a508f2bd2b83d11a9427f018a42a38ba5878 100644
--- a/src/processor_tracker_landmark.cpp
+++ b/src/processor_tracker_landmark.cpp
@@ -30,7 +30,6 @@ ProcessorTrackerLandmark::~ProcessorTrackerLandmark()
unsigned int ProcessorTrackerLandmark::processNew(const unsigned int& _max_features)
{
- WOLF_DEBUG_HERE
//std::cout << "\tlast correspondences: " << matches_landmark_from_last_.size() << std::endl;
//std::cout << "\tlast features: " << (last_ptr_ == nullptr ? 0 : last_ptr_->getFeatureList().size()) << std::endl;
//std::cout << "\tlast new features: " << new_features_last_.size() << std::endl;
@@ -64,11 +63,11 @@ unsigned int ProcessorTrackerLandmark::processNew(const unsigned int& _max_featu
// Append all new Features to the Capture's list of Features
last_ptr_->addFeatureList(new_features_last_);
- std::cout << "\tnew last features added " << std::endl;
+// std::cout << "\tnew last features added " << std::endl;
// Append new landmarks to the map
getProblem()->addLandmarkList(new_landmarks);
- std::cout << "\tnew landmarks added: " << getProblem()->getMapPtr()->getLandmarkList().size() << std::endl;
+// std::cout << "\tnew landmarks added: " << getProblem()->getMapPtr()->getLandmarkList().size() << std::endl;
//std::cout << "end of processNew:" << std::endl;
//std::cout << "\tlast correspondences: " << matches_landmark_from_last_.size() << std::endl;
@@ -84,24 +83,22 @@ unsigned int ProcessorTrackerLandmark::processNew(const unsigned int& _max_featu
void ProcessorTrackerLandmark::createNewLandmarks(LandmarkBaseList& _new_landmarks)
{
- WOLF_DEBUG_HERE
for (auto new_feature_ptr : new_features_last_)
{
// create new landmark
LandmarkBasePtr new_lmk_ptr = createLandmark(new_feature_ptr);
- std::cout << "\tnew_landmark: " << new_lmk_ptr->id() << std::endl;
+// std::cout << "\tnew_landmark: " << new_lmk_ptr->id() << std::endl;
_new_landmarks.push_back(new_lmk_ptr);
// create new correspondence
matches_landmark_from_last_[new_feature_ptr] = std::make_shared<LandmarkMatch>(new_lmk_ptr, 1); // max score
}
- std::cout << "\tnew_landmarks: " << _new_landmarks.size() << std::endl;
- std::cout << "\tlast correspondences: " << matches_landmark_from_last_.size() << std::endl;
+// std::cout << "\tnew_landmarks: " << _new_landmarks.size() << std::endl;
+// std::cout << "\tlast correspondences: " << matches_landmark_from_last_.size() << std::endl;
}
unsigned int ProcessorTrackerLandmark::processKnown()
{
- WOLF_DEBUG_HERE
//std::cout << "ProcessorTrackerLandmark::processKnown:" << std::endl;
//std::cout << "\tlast correspondences: " << matches_landmark_from_last_.size() << std::endl;
@@ -131,7 +128,6 @@ unsigned int ProcessorTrackerLandmark::processKnown()
void ProcessorTrackerLandmark::establishConstraints()
{
- WOLF_DEBUG_HERE
//std::cout << "\tfeatures:" << last_ptr_->getFeatureList().size() << std::endl;
//std::cout << "\tcorrespondences: " << matches_landmark_from_last_.size() << std::endl;
diff --git a/src/processor_tracker_landmark.h b/src/processor_tracker_landmark.h
index 5b39952fbe2604d6a17b57443fe9e34d5a855955..915ec82f73e3b8426e065c6d9e1215d29c77fca8 100644
--- a/src/processor_tracker_landmark.h
+++ b/src/processor_tracker_landmark.h
@@ -174,7 +174,6 @@ namespace wolf
{
inline void ProcessorTrackerLandmark::advance()
{
- WOLF_DEBUG_HERE
for ( auto match : matches_landmark_from_last_)
{
@@ -191,7 +190,6 @@ inline void ProcessorTrackerLandmark::advance()
inline void ProcessorTrackerLandmark::reset()
{
- WOLF_DEBUG_HERE
//std::cout << "ProcessorTrackerLandmark::reset" << std::endl;
for ( auto match : matches_landmark_from_last_)
diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp
index 82689b6aad7b5b78a7ea46814cf6d9528461c064..972a468d843a535845741d23c8c947cc81c6114f 100644
--- a/src/sensor_odom_2D.cpp
+++ b/src/sensor_odom_2D.cpp
@@ -35,8 +35,14 @@ SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen:
StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true);
// cast intrinsics into derived type
- std::shared_ptr<IntrinsicsOdom2D> params = std::static_pointer_cast<IntrinsicsOdom2D>(_intrinsics);
- std::shared_ptr<SensorOdom2D> odo = std::make_shared<SensorOdom2D>(pos_ptr, ori_ptr, params->k_disp_to_disp, params->k_rot_to_rot);
+ std::shared_ptr<SensorOdom2D> odo;
+ if (_intrinsics)
+ {
+ std::shared_ptr<IntrinsicsOdom2D> params = std::static_pointer_cast<IntrinsicsOdom2D>(_intrinsics);
+ odo = std::make_shared<SensorOdom2D>(pos_ptr, ori_ptr, params->k_disp_to_disp, params->k_rot_to_rot);
+ }
+ else
+ odo = std::make_shared<SensorOdom2D>(pos_ptr, ori_ptr, 1, 1);
odo->setName(_unique_name);
return odo;
}
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index f537b27848ec4a8b4505c8a7f92fc590b853312c..6a6e544cad3953f9a379abe012131d6b4b5f0edf 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -54,7 +54,7 @@ void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
{
for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
- if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKey() && (*frm_rit)->getTimeStamp() < _frame_ptr->getTimeStamp())
+ if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKey() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp())
return frm_rit.base();
return getFrameList().begin();
}
diff --git a/src/yaml/sensor_camera_yaml.cpp b/src/yaml/sensor_camera_yaml.cpp
index 7d1e5980131cc584e517fc05c14656fdd0f48b54..a2feb6fe4a68d55fb9c2074e46c21566f9700f0d 100644
--- a/src/yaml/sensor_camera_yaml.cpp
+++ b/src/yaml/sensor_camera_yaml.cpp
@@ -62,9 +62,9 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do
//=========================================
// ===== this part for debugging only =====
//=========================================
- std::cout << "\n--- Parameters Parsed from YAML file ---" << std::endl;
- std::cout << "sensor type: " << sensor_type << std::endl;
- std::cout << "sensor name: " << sensor_name << std::endl;
+// std::cout << "\n--- Parameters Parsed from YAML file ---" << std::endl;
+// std::cout << "sensor type: " << sensor_type << std::endl;
+// std::cout << "sensor name: " << sensor_name << std::endl;
// // extrinsics discarded in this creator
// Vector3d pos = camera_config["extrinsic"]["position"].as<Vector3d>();
@@ -74,10 +74,10 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do
// std::cout << "\tposition : " << pos.transpose() << std::endl;
// std::cout << "\torientation : " << ori.transpose() << std::endl;
- std::cout << "sensor intrinsics: " << std::endl;
- std::cout << "\timage size : " << width << "x" << height << std::endl;
- std::cout << "\tintrinsic : " << intrinsic.transpose() << std::endl;
- std::cout << "\tdistoriton : " << distortion.transpose() << std::endl;
+// std::cout << "sensor intrinsics: " << std::endl;
+// std::cout << "\timage size : " << width << "x" << height << std::endl;
+// std::cout << "\tintrinsic : " << intrinsic.transpose() << std::endl;
+// std::cout << "\tdistoriton : " << distortion.transpose() << std::endl;
//=========================================
//=========================================
diff --git a/src/yaml/sensor_odom_3D_yaml.cpp b/src/yaml/sensor_odom_3D_yaml.cpp
index e2b281948647c3c2a66a730bf817dc6a2ac100cc..136b29b639e1868ed21aef9644d223eb73277951 100644
--- a/src/yaml/sensor_odom_3D_yaml.cpp
+++ b/src/yaml/sensor_odom_3D_yaml.cpp
@@ -45,9 +45,9 @@ static IntrinsicsBasePtr createIntrinsicsOdom3D(const std::string & _filename_do
//=========================================
// ===== this part for debugging only =====
//=========================================
- std::cout << "\n--- Parameters Parsed from YAML file ---" << std::endl;
- std::cout << "sensor type: " << sensor_type << std::endl;
- std::cout << "sensor name: " << sensor_name << std::endl;
+// std::cout << "\n--- Parameters Parsed from YAML file ---" << std::endl;
+// std::cout << "sensor type: " << sensor_type << std::endl;
+// std::cout << "sensor name: " << sensor_name << std::endl;
//=========================================
//=========================================