diff --git a/src/constraint_AHP.h b/src/constraint_AHP.h
index 73c84a9570db2bf4a7b2a5fe7e0742801f05006c..e7acfc0a354713bc2f408effb4907f6576a15d58 100644
--- a/src/constraint_AHP.h
+++ b/src/constraint_AHP.h
@@ -92,14 +92,15 @@ inline Eigen::VectorXs ConstraintAHP::expectation() const
FrameBasePtr frm_anchor = getFrameOtherPtr();
LandmarkBasePtr lmk = getLandmarkOtherPtr();
- const Scalar* const frame_current_pos = frm_current->getPPtr()->getPtr();
- const Scalar* const frame_current_ori = frm_current->getOPtr()->getPtr();
- const Scalar* const frame_anchor_pos = frm_anchor ->getPPtr()->getPtr();
- const Scalar* const frame_anchor_ori = frm_anchor ->getOPtr()->getPtr();
- const Scalar* const lmk_pos_hmg = lmk ->getPPtr()->getPtr();
+ const Eigen::MatrixXs frame_current_pos = frm_current->getPPtr()->getState();
+ const Eigen::MatrixXs frame_current_ori = frm_current->getOPtr()->getState();
+ const Eigen::MatrixXs frame_anchor_pos = frm_anchor ->getPPtr()->getState();
+ const Eigen::MatrixXs frame_anchor_ori = frm_anchor ->getOPtr()->getState();
+ const Eigen::MatrixXs lmk_pos_hmg = lmk ->getPPtr()->getState();
Eigen::Vector2s exp;
- expectation(frame_current_pos, frame_current_ori, frame_anchor_pos, frame_anchor_ori, lmk_pos_hmg, exp.data());
+ expectation(frame_current_pos.data(), frame_current_ori.data(), frame_anchor_pos.data(),
+ frame_anchor_ori.data(), lmk_pos_hmg.data(), exp.data());
return exp;
}
@@ -138,7 +139,7 @@ inline void ConstraintAHP::expectation(const T* const _current_frame_p,
// current robot to current camera transform
CaptureBasePtr current_capture = this->getFeaturePtr()->getCapturePtr();
Translation<T, 3> t_r1_c1 (current_capture->getSensorPPtr()->getState().cast<T>());
- Quaternions q_r1_c1_s(current_capture->getSensorOPtr()->getPtr());
+ Quaternions q_r1_c1_s(Eigen::Vector4s(current_capture->getSensorOPtr()->getState()));
Quaternion<T> q_r1_c1 = q_r1_c1_s.cast<T>();
TransformType T_R1_C1 = t_r1_c1 * q_r1_c1;
diff --git a/src/constraint_analytic.cpp b/src/constraint_analytic.cpp
index c2b0268e3f819162050a7404346a9a70ce388642..8b0422380bc4ad0d39f2e7f38955ea3b0bd948a8 100644
--- a/src/constraint_analytic.cpp
+++ b/src/constraint_analytic.cpp
@@ -29,20 +29,18 @@ ConstraintAnalytic::ConstraintAnalytic(const std::string& _tp,
{
resizeVectors();
}
-
-
-std::vector<Scalar*> ConstraintAnalytic::getStateScalarPtrVector() const
+/*
+ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, const LandmarkBasePtr& _landmark_ptr, const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function, ConstraintStatus _status,
+ StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
+ ConstraintBase( _tp, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
+ _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
{
- assert(state_ptr_vector_.size() > 0 && state_ptr_vector_.size() <= 10 && "Wrong state vector size in constraint, it should be between 1 and 10");
-
- std::vector<Scalar*> state_scalar_ptr_vector(state_ptr_vector_.size());
-
- for (auto i = 0; i < state_scalar_ptr_vector.size(); i++)
- state_scalar_ptr_vector[i] = state_ptr_vector_[i]->getPtr();
-
- return state_scalar_ptr_vector;
+ resizeVectors();
}
-
+*/
std::vector<StateBlockPtr> ConstraintAnalytic::getStateBlockPtrVector() const
{
return state_ptr_vector_;
diff --git a/src/constraint_analytic.h b/src/constraint_analytic.h
index 9384afd71f23b46773dc59d1166830ce6a6d0272..5fb1495bfbb8963c8e1f0252aaac1a75719821d8 100644
--- a/src/constraint_analytic.h
+++ b/src/constraint_analytic.h
@@ -60,13 +60,6 @@ class ConstraintAnalytic: public ConstraintBase
virtual ~ConstraintAnalytic() = default;
- /** \brief Returns a vector of pointers to the state blocks
- *
- * Returns a vector of pointers to the state blocks in which this constraint depends
- *
- **/
- virtual std::vector<Scalar*> getStateScalarPtrVector() const override;
-
/** \brief Returns a vector of pointers to the states
*
* Returns a vector of pointers to the state in which this constraint depends
diff --git a/src/constraint_autodiff.h b/src/constraint_autodiff.h
index 7e6f227a44da539cb80a2d4a97f1e114372b93c5..09472aa90b31d8b1f000b6db7de1aaa65fcd442f 100644
--- a/src/constraint_autodiff.h
+++ b/src/constraint_autodiff.h
@@ -270,26 +270,6 @@ class ConstraintAutodiff : public ConstraintBase
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- /** \brief Returns a vector of pointers to the state blocks
- *
- * Returns a vector of pointers to the state blocks in which this constraint depends
- *
- **/
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr(),
- state_ptrs_[3]->getPtr(),
- state_ptrs_[4]->getPtr(),
- state_ptrs_[5]->getPtr(),
- state_ptrs_[6]->getPtr(),
- state_ptrs_[7]->getPtr(),
- state_ptrs_[8]->getPtr(),
- state_ptrs_[9]->getPtr()
- });
- }
-
/** \brief Returns a vector of pointers to the states
*
* Returns a vector of pointers to the state in which this constraint depends
@@ -556,20 +536,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public Constra
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr(),
- state_ptrs_[3]->getPtr(),
- state_ptrs_[4]->getPtr(),
- state_ptrs_[5]->getPtr(),
- state_ptrs_[6]->getPtr(),
- state_ptrs_[7]->getPtr(),
- state_ptrs_[8]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -811,19 +777,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public Constrai
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr(),
- state_ptrs_[3]->getPtr(),
- state_ptrs_[4]->getPtr(),
- state_ptrs_[5]->getPtr(),
- state_ptrs_[6]->getPtr(),
- state_ptrs_[7]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -1054,18 +1007,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public Constrain
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr(),
- state_ptrs_[3]->getPtr(),
- state_ptrs_[4]->getPtr(),
- state_ptrs_[5]->getPtr(),
- state_ptrs_[6]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -1281,17 +1222,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public Constraint
jacobians_[i].data() + row * state_block_sizes_.at(i));
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr(),
- state_ptrs_[3]->getPtr(),
- state_ptrs_[4]->getPtr(),
- state_ptrs_[5]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -1495,16 +1425,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public ConstraintB
jacobians_[i].data() + row * state_block_sizes_.at(i));
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr(),
- state_ptrs_[3]->getPtr(),
- state_ptrs_[4]->getPtr(),
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -1701,15 +1621,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public ConstraintBa
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr(),
- state_ptrs_[3]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -1895,14 +1806,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public ConstraintBas
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr(),
- state_ptrs_[2]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -2077,13 +1980,6 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public ConstraintBase
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
- state_ptrs_[1]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
@@ -2247,12 +2143,6 @@ class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase
// std::cout << jacobians_[i] << std::endl << std::endl;
}
- virtual std::vector<Scalar*> getStateScalarPtrVector() const
- {
- return std::vector<Scalar*>({state_ptrs_[0]->getPtr()
- });
- }
-
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
{
return state_ptrs_;
diff --git a/src/constraint_base.h b/src/constraint_base.h
index c5b2a756480a8f2a75ac3c55fcb49971d3a4ae9b..c90ade9153062c8e871ef450452208c50a8dcf72 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -91,10 +91,6 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
**/
virtual JacobianMethod getJacobianMethod() const = 0;
- /** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint
- **/
- virtual std::vector<Scalar*> getStateScalarPtrVector() const = 0;
-
/** \brief Returns a vector of pointers to the states in which this constraint depends
**/
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const = 0;
diff --git a/src/constraint_epipolar.h b/src/constraint_epipolar.h
index 2d7d2a14214be26a6ee19fd858c86a72ea269a6e..262b3bc7858922e57e2e4f14cc453d6ad3c86fde 100644
--- a/src/constraint_epipolar.h
+++ b/src/constraint_epipolar.h
@@ -32,10 +32,6 @@ class ConstraintEpipolar : public ConstraintBase
**/
virtual JacobianMethod getJacobianMethod() const override {return JAC_ANALYTIC;}
- /** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint
- **/
- virtual std::vector<Scalar*> getStateScalarPtrVector() const override {return std::vector<Scalar*>(0);}
-
/** \brief Returns a vector of pointers to the states in which this constraint depends
**/
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
diff --git a/src/landmark_container.cpp b/src/landmark_container.cpp
index 0979387a6139c7afb2393d713da2722f04559385..a1ef7ed100e31b72619507bcb4d0663e1d50fe0c 100644
--- a/src/landmark_container.cpp
+++ b/src/landmark_container.cpp
@@ -30,16 +30,17 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
M_PI / 4, 3 * M_PI / 4,-3 * M_PI / 4,-M_PI / 4;
// Computing center
- std::cout << "Container constructor: Computing center pose... " << std::endl;
- Eigen::Map<Eigen::Vector2s> container_position(getPPtr()->getPtr());
- Eigen::Map<Eigen::Vector1s> container_orientation(getOPtr()->getPtr());
+ WOLF_DEBUG( "Container constructor: Computing center pose... " );
+ Eigen::Vector2s container_position(getPPtr()->getState());
+ Eigen::Vector1s container_orientation(getOPtr()->getState());
- std::cout << "Container constructor: _corner_1_idx " << _corner_1_idx << "_corner_2_idx " << _corner_2_idx << std::endl;
+ WOLF_DEBUG( "Container constructor: _corner_1_idx ", _corner_1_idx,
+ "_corner_2_idx ", _corner_2_idx );
// Large side detected (A & B)
if ( (_corner_1_idx == 0 && _corner_2_idx == 1) || (_corner_1_idx == 1 && _corner_2_idx == 0) )
{
- std::cout << "Large side detected" << std::endl;
+ WOLF_DEBUG( "Large side detected" );
Eigen::Vector2s AB = (_corner_1_idx == 0 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
Eigen::Vector2s perpendicularAB;
perpendicularAB << -AB(1)/AB.norm(), AB(0)/AB.norm();
@@ -50,7 +51,7 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
// Short side detected (B & C)
else if ( (_corner_1_idx == 1 && _corner_2_idx == 2) || (_corner_1_idx == 2 && _corner_2_idx == 1) )
{
- std::cout << "Short side detected" << std::endl;
+ WOLF_DEBUG( "Short side detected" );
Eigen::Vector2s BC = (_corner_1_idx == 1 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
Eigen::Vector2s perpendicularBC;
perpendicularBC << -BC(1)/BC.norm(), BC(0)/BC.norm();
@@ -62,7 +63,7 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
// A & C
else if ( (_corner_1_idx == 0 && _corner_2_idx == 2) || (_corner_1_idx == 2 && _corner_2_idx == 0) )
{
- std::cout << "diagonal AC detected" << std::endl;
+ WOLF_DEBUG( "diagonal AC detected" );
Eigen::Vector2s AC = (_corner_1_idx == 0 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
container_position = (_corner_1_idx == 0 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + AC / 2;
container_orientation(0) = atan2(AC(1),AC(0)) - atan2(_witdh,_length);
@@ -70,7 +71,7 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
// B & D
else if ( (_corner_1_idx == 1 && _corner_2_idx == 3) || (_corner_1_idx == 3 && _corner_2_idx == 1) )
{
- std::cout << "diagonal BD detected" << std::endl;
+ WOLF_DEBUG( "diagonal BD detected" );
Eigen::Vector2s BD = (_corner_1_idx == 1 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
container_position = (_corner_1_idx == 1 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + BD / 2;
container_orientation(0) = atan2(BD(1),BD(0)) + atan2(_witdh,_length);
@@ -78,9 +79,12 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
else
assert(0 && "index corners combination not implemented!");
- std::cout << "_corner_1_pose... " << _corner_1_pose.transpose() << std::endl;
- std::cout << "_corner_2_pose... " << _corner_2_pose.transpose() << std::endl;
- std::cout << "Container center pose... " << container_position.transpose() << " " << container_orientation.transpose() << std::endl;
+ WOLF_DEBUG( "_corner_1_pose... ", _corner_1_pose.transpose() );
+ WOLF_DEBUG( "_corner_2_pose... ", _corner_2_pose.transpose() );
+ WOLF_DEBUG( "Container center pose... ", container_position.transpose(), " ", container_orientation.transpose() );
+
+ getPPtr()->setState(container_position);
+ getOPtr()->setState(container_orientation);
}
//LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const Scalar& _witdh, const Scalar& _length) :