diff --git a/doc/doxygen.conf b/doc/doxygen.conf
index 531ce42173f96f516b5b4e853edd4c1e83ed565d..26f73408b82bb0169b28539acfa1be0b6367a56c 100644
--- a/doc/doxygen.conf
+++ b/doc/doxygen.conf
@@ -791,7 +791,8 @@ RECURSIVE = YES
# Note that relative paths are relative to the directory from which doxygen is
# run.
-EXCLUDE = ../src/old
+EXCLUDE = ../src/old \
+ ../src/examples
# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
# directories that are symbolic links (a Unix file system feature) are excluded
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b27d30c1778425c86670b85478cbc49d0cbdbdb3..491c04098b7b2476137cc73b9e8b534bd5d36dba 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -78,6 +78,7 @@ SET(HDRS
feature_fix.h
feature_odom_2D.h
frame_base.h
+ hardware_base.h
landmark_base.h
landmark_corner_2D.h
landmark_container.h
@@ -89,7 +90,7 @@ SET(HDRS
sensor_laser_2D.h
sensor_odom_2D.h
sensor_gps_fix.h
- state_base.h
+ state_block.h
time_stamp.h
trajectory_base.h
wolf_problem.h
@@ -116,6 +117,7 @@ SET(SRCS
feature_fix.cpp
feature_odom_2D.cpp
frame_base.cpp
+ hardware_base.cpp
landmark_base.cpp
landmark_corner_2D.cpp
landmark_container.cpp
@@ -125,10 +127,11 @@ SET(SRCS
sensor_base.cpp
sensor_odom_2D.cpp
sensor_gps_fix.cpp
- state_base.cpp
+ state_block.cpp
time_stamp.cpp
trajectory_base.cpp
wolf_problem.cpp
+ wolf_manager.cpp
data_association/association_solver.cpp
data_association/association_node.cpp
data_association/association_tree.cpp
diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index f2ce6f2a75e454823eff881ed78dbf2ea6aa47c6..7bf1be5cb49af8ac121f090bf57b237a2627c521 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -57,7 +57,7 @@ FeatureBaseList* CaptureBase::getFeatureListPtr()
void CaptureBase::getConstraintList(ConstraintBaseList & _ctr_list)
{
for(auto f_it = getFeatureListPtr()->begin(); f_it != getFeatureListPtr()->end(); ++f_it)
- (*f_it)->getConstraintList(_ctr_list);
+ (*f_it)->getConstraintFromList(_ctr_list);
}
TimeStamp CaptureBase::getTimeStamp() const
diff --git a/src/capture_fix.cpp b/src/capture_fix.cpp
index 7f3323aaafeb689e0f2c44047592ff05e928450b..d18d2c731739f0048e0c3bc9bed8f9f2c017fccb 100644
--- a/src/capture_fix.cpp
+++ b/src/capture_fix.cpp
@@ -16,9 +16,11 @@ void CaptureFix::processCapture()
{
// EXTRACT AND ADD FEATURES
addFeature(new FeatureFix(data_,data_covariance_));
+ std::cout << "FeatureFix extracted " << std::endl;
// ADD CONSTRAINT
- getFeatureListPtr()->front()->addConstraint(new ConstraintFix(getFeatureListPtr()->front(), getFramePtr()));
+ getFeatureListPtr()->front()->addConstraintFrom(new ConstraintFix(getFeatureListPtr()->front()));
+ std::cout << "ConstraintFix added " << std::endl;
}
Eigen::VectorXs CaptureFix::computePrior(const TimeStamp& _now) const
diff --git a/src/capture_gps_fix.cpp b/src/capture_gps_fix.cpp
index 55b1cbd98f80a74f4c33cda677023aa0902a1045..7d35acc4329fd9f4e1dd3ca6a17567894685f324 100644
--- a/src/capture_gps_fix.cpp
+++ b/src/capture_gps_fix.cpp
@@ -29,7 +29,7 @@ void CaptureGPSFix::processCapture()
addFeature(new FeatureGPSFix(data_,data_covariance_));
// ADD CONSTRAINT
- getFeatureListPtr()->front()->addConstraint(new ConstraintGPS2D(getFeatureListPtr()->front(), getFramePtr()));
+ getFeatureListPtr()->front()->addConstraintFrom(new ConstraintGPS2D(getFeatureListPtr()->front(), getFramePtr()));
}
Eigen::VectorXs CaptureGPSFix::computePrior(const TimeStamp& _now) const
diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index f3818272944f321a835e6a891fd0d192f1d2693c..13663b9a2016cb2c5ded78c6ef117888ed7e96b3 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -260,13 +260,13 @@ void CaptureLaser2D::establishConstraintsMHTree()
unsigned int associed_landmark_index = landmarks_index_map[ft_lk_pairs[ii]];
if (associed_landmark->getType() == LANDMARK_CORNER)
- associed_feature->addConstraint(new ConstraintCorner2D(associed_feature, // feature pointer
- (LandmarkCorner2D*)(associed_landmark))); // landmark pointer
+ associed_feature->addConstraintFrom(new ConstraintCorner2D(associed_feature, // feature pointer
+ (LandmarkCorner2D*)(associed_landmark))); // landmark pointer
else if (associed_landmark->getType() == LANDMARK_CONTAINER)
- associed_feature->addConstraint(new ConstraintContainer(associed_feature, //feature pointer
- (LandmarkContainer*)(associed_landmark), //landmark pointer
- associed_landmark_index )); // corner index
+ associed_feature->addConstraintFrom(new ConstraintContainer(associed_feature, //feature pointer
+ (LandmarkContainer*)(associed_landmark), //landmark pointer
+ associed_landmark_index )); // corner index
}
}
@@ -561,13 +561,16 @@ bool CaptureLaser2D::fitNewContainer(FeatureCorner2D* _corner_ptr, LandmarkCorne
void CaptureLaser2D::createCornerLandmark(FeatureCorner2D* _corner_ptr, const Eigen::Vector3s& _feature_global_pose)
{
- //create new landmark at global coordinates
- StateBase* new_landmark_state_position = new StateBase(getTop()->getNewStatePtr(), 2);
- getTop()->addState(new_landmark_state_position, _feature_global_pose.head(2));
- StateBase* new_landmark_state_orientation = new StateBase(getTop()->getNewStatePtr(), 1);
- getTop()->addState(new_landmark_state_orientation, _feature_global_pose.tail(1));
+ //Create new landmark
+ LandmarkCorner2D* new_landmark = new LandmarkCorner2D(new StateBlock(_feature_global_pose.head(2)),
+ new StateBlock(_feature_global_pose.tail(1)),
+ _corner_ptr->getMeasurement()(3));
+ //Constraint with the new landmark
+ _corner_ptr->addConstraintFrom(new ConstraintCorner2D(_corner_ptr, new_landmark));
+ //Add it to the map
+ getTop()->getMapPtr()->addLandmark((LandmarkBase*)new_landmark);
- // Initialize landmark covariance
+ // Initialize landmark covariance // TODO: has it sense???
Eigen::MatrixXs Sigma_robot = Eigen::MatrixXs::Zero(3,3);
getTop()->getCovarianceBlock(getFramePtr()->getPPtr(), getFramePtr()->getPPtr(), Sigma_robot, 0,0);
getTop()->getCovarianceBlock(getFramePtr()->getPPtr(), getFramePtr()->getOPtr(), Sigma_robot, 0,2);
@@ -578,58 +581,55 @@ void CaptureLaser2D::createCornerLandmark(FeatureCorner2D* _corner_ptr, const Ei
R_robot3D.block<2,2>(0,0) = Eigen::Rotation2D<WolfScalar>(*(getFramePtr()->getOPtr()->getPtr())).matrix();
Eigen::Matrix3s Sigma_landmark = Sigma_robot + R_robot3D.transpose() * _corner_ptr->getMeasurementCovariance().topLeftCorner<3,3>() * R_robot3D;
- getTop()->addCovarianceBlock(new_landmark_state_position, new_landmark_state_position, Sigma_landmark.topLeftCorner<2,2>());
- getTop()->addCovarianceBlock(new_landmark_state_position, (StateBase*)new_landmark_state_orientation, Sigma_landmark.block<2,1>(0,2));
- getTop()->addCovarianceBlock((StateBase*)new_landmark_state_orientation, (StateBase*)new_landmark_state_orientation, Sigma_landmark.block<1,1>(2,2));
-
- //add it to the slam map as a new landmark
- LandmarkCorner2D* new_landmark = new LandmarkCorner2D(new_landmark_state_position, new_landmark_state_orientation, _corner_ptr->getMeasurement()(3));
- _corner_ptr->addConstraint(new ConstraintCorner2D(_corner_ptr, new_landmark));
- getTop()->getMapPtr()->addLandmark((LandmarkBase*)new_landmark);
+ getTop()->addCovarianceBlock(new_landmark->getPPtr(), new_landmark->getPPtr(), Sigma_landmark.topLeftCorner<2,2>());
+ getTop()->addCovarianceBlock(new_landmark->getPPtr(), new_landmark->getOPtr(), Sigma_landmark.block<2,1>(0,2));
+ getTop()->addCovarianceBlock(new_landmark->getOPtr(), new_landmark->getOPtr(), Sigma_landmark.block<1,1>(2,2));
}
void CaptureLaser2D::createContainerLandmark(FeatureCorner2D* _corner_ptr, const Eigen::Vector3s& _feature_global_pose, LandmarkCorner2D* _old_corner_landmark_ptr, int& _feature_idx, int& _corner_idx)
{
assert(_old_corner_landmark_ptr != nullptr && "fitting result = true but corner found is nullptr");
- // CREATING LANDMARK CONTAINER
- // create new landmark state units
- StateBase* new_container_position = new StateBase(getTop()->getNewStatePtr(), 2);
- getTop()->addState(new_container_position, Eigen::Vector2s::Zero());
- StateBase* new_container_orientation = new StateBase(getTop()->getNewStatePtr(), 1);
- getTop()->addState(new_container_orientation, Eigen::Vector1s::Zero());
-
// create new landmark
Eigen::Vector3s corner_pose;
corner_pose.head(2) = _old_corner_landmark_ptr->getPPtr()->getVector();
corner_pose(2) = *(_old_corner_landmark_ptr->getOPtr()->getPtr());
- LandmarkContainer* new_landmark = new LandmarkContainer(new_container_position, new_container_orientation, _feature_global_pose, corner_pose, _feature_idx, _corner_idx, CONTAINER_WIDTH, CONTAINER_LENGTH);
+ LandmarkContainer* new_landmark = new LandmarkContainer(new StateBlock(Eigen::Vector2s::Zero()),
+ new StateBlock(Eigen::Vector1s::Zero()),
+ _feature_global_pose,
+ corner_pose,
+ _feature_idx,
+ _corner_idx,
+ CONTAINER_WIDTH,
+ CONTAINER_LENGTH);
+ // create new constraint (feature to container)
+ _corner_ptr->addConstraintFrom(new ConstraintContainer(_corner_ptr, new_landmark,_feature_idx));
// add new landmark in the map
getTop()->getMapPtr()->addLandmark((LandmarkBase*)new_landmark);
- // initialize container covariance with landmark corner covariance
+ // initialize container covariance with landmark corner covariance // TODO: has it sense???
Eigen::MatrixXs Sigma_landmark = Eigen::MatrixXs::Zero(3,3);
getTop()->getCovarianceBlock(_old_corner_landmark_ptr->getPPtr(), _old_corner_landmark_ptr->getPPtr(), Sigma_landmark, 0,0);
getTop()->getCovarianceBlock(_old_corner_landmark_ptr->getPPtr(), _old_corner_landmark_ptr->getOPtr(), Sigma_landmark, 0,2);
getTop()->getCovarianceBlock(_old_corner_landmark_ptr->getOPtr(), _old_corner_landmark_ptr->getOPtr(), Sigma_landmark, 2,2);
Sigma_landmark.block<1,2>(2,0) = Sigma_landmark.block<2,1>(0,2).transpose();
- getTop()->addCovarianceBlock(new_container_position, new_container_position, Sigma_landmark.topLeftCorner<2,2>());
- getTop()->addCovarianceBlock(new_container_position, (StateBase*)new_container_orientation, Sigma_landmark.block<2,1>(0,2));
- getTop()->addCovarianceBlock((StateBase*)new_container_orientation, (StateBase*)new_container_orientation, Sigma_landmark.block<1,1>(2,2));
+ getTop()->addCovarianceBlock(new_landmark->getPPtr(), new_landmark->getPPtr(), Sigma_landmark.topLeftCorner<2,2>());
+ getTop()->addCovarianceBlock(new_landmark->getPPtr(), new_landmark->getOPtr(), Sigma_landmark.block<2,1>(0,2));
+ getTop()->addCovarianceBlock(new_landmark->getOPtr(), new_landmark->getOPtr(), Sigma_landmark.block<1,1>(2,2));
// create new constraint (feature to container)
- _corner_ptr->addConstraint(new ConstraintContainer(_corner_ptr, new_landmark,_feature_idx));
+ _corner_ptr->addConstraintFrom(new ConstraintContainer(_corner_ptr, new_landmark,_feature_idx));
// ERASING LANDMARK CORNER
// change all constraints from corner landmark by new corner container
- for (auto ctr_it = _old_corner_landmark_ptr->getConstraints()->begin(); ctr_it != _old_corner_landmark_ptr->getConstraints()->end(); ctr_it++)
+ for (auto ctr_it = _old_corner_landmark_ptr->getConstraintToListPtr()->begin(); ctr_it != _old_corner_landmark_ptr->getConstraintToListPtr()->end(); ctr_it++)
{
// create new constraint to landmark container
- (*ctr_it)->getFeaturePtr()->addConstraint(new ConstraintContainer((*ctr_it)->getFeaturePtr(), new_landmark, _corner_idx));
+ (*ctr_it)->getFeaturePtr()->addConstraintFrom(new ConstraintContainer((*ctr_it)->getFeaturePtr(), new_landmark, _corner_idx));
}
// Remove corner landmark (it will remove all old constraints)
getTop()->getMapPtr()->removeLandmark(_old_corner_landmark_ptr);
diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h
index 4389191efe59189cb7e20cd7f50f90143be97f2c..95c402510687690e11f5efae515af959a4d3ed40 100644
--- a/src/capture_laser_2D.h
+++ b/src/capture_laser_2D.h
@@ -19,7 +19,7 @@
#include "laser_scan_utils/corner_detector.h"
//wolf includes
-#include "state_base.h"
+#include "state_block.h"
#include "constraint_corner_2D.h"
#include "constraint_container.h"
#include "capture_base.h"
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index d49c0f3526bfefefa7128c825f6447bf9a36fb30..64b6fc3ba53fb4b4bed1c9353bf7cfc9654d150b 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -55,10 +55,10 @@ Eigen::VectorXs CaptureOdom2D::computePrior(const TimeStamp& _now) const
void CaptureOdom2D::addConstraints()
{
- assert(getFramePtr()->getNextFrame() != nullptr && "Trying to add odometry constraint in the last frame (there is no next frame)");
+ assert(getFramePtr()->getPreviousFrame() != nullptr && "Trying to add odometry constraint in the last frame (there is no next frame)");
- getFeatureListPtr()->front()->addConstraint(new ConstraintOdom2D(getFeatureListPtr()->front(),
- getFramePtr()->getPreviousFrame()));
+ getFeatureListPtr()->front()->addConstraintFrom(new ConstraintOdom2D(getFeatureListPtr()->front(),
+ getFramePtr()->getPreviousFrame()));
}
void CaptureOdom2D::integrateCapture(CaptureMotion* _new_capture)
@@ -66,12 +66,25 @@ void CaptureOdom2D::integrateCapture(CaptureMotion* _new_capture)
assert(dynamic_cast<CaptureOdom2D*>(_new_capture) && "Trying to integrate with a CaptureOdom2D a CaptureRelativePtr which is not CaptureOdom2D");
//std::cout << "Integrate odoms: " << std::endl << data_.transpose() << std::endl << _new_capture->getData().transpose() << std::endl;
- data_(0) += (_new_capture->getData()(0) * cos(data_(2)) - _new_capture->getData()(1) * sin(data_(2)));
- data_(1) += (_new_capture->getData()(0) * sin(data_(2)) + _new_capture->getData()(1) * cos(data_(2)));
- data_(2) += _new_capture->getData()(2);
-
- // TODO Jacobians!
- data_covariance_ += _new_capture->getDataCovariance();
+ // turn/2 + straight + turn/2
+ data_(2) += _new_capture->getData()(2)/2;
+ data_(0) += _new_capture->getData()(0) * cos(data_(2)) - _new_capture->getData()(1) * sin(data_(2));
+ data_(1) += _new_capture->getData()(0) * sin(data_(2)) + _new_capture->getData()(1) * cos(data_(2));
+ data_(2) += _new_capture->getData()(2)/2;
+
+ // Covariance propagation
+ Eigen::Matrix3s Jx = Eigen::Matrix3s::Identity();
+ Jx(0,2) = -_new_capture->getData()(0)*sin(data_(2)+_new_capture->getData()(2)/2);
+ Jx(1,2) = _new_capture->getData()(0)*cos(data_(2)+_new_capture->getData()(2)/2);
+
+ Eigen::Matrix3s Ju = Eigen::Matrix3s::Zero();
+ Ju(0,0) = cos(data_(2)+_new_capture->getData()(2)/2);
+ Ju(1,0) = sin(data_(2)+_new_capture->getData()(2)/2);
+ Ju(0,2) = -0.5*_new_capture->getData()(0)*cos(data_(2)+_new_capture->getData()(2)/2);
+ Ju(1,2) = 0.5*_new_capture->getData()(0)*cos(data_(2)+_new_capture->getData()(2)/2);
+ Ju(2,2) = 1;
+
+ data_covariance_ = Jx*data_covariance_*Jx.transpose() + Ju*_new_capture->getDataCovariance()*Ju.transpose();
this->final_time_stamp_ = _new_capture->getFinalTimeStamp();
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 18f17bef7cf3994cecdb2d13657d438531de3ba1..d6a058582e1819180e10fbb26c3a02397cd09acb 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -1,7 +1,8 @@
#include "ceres_manager.h"
-CeresManager::CeresManager(ceres::Problem::Options _options) :
- ceres_problem_(new ceres::Problem(_options))
+CeresManager::CeresManager(WolfProblem* _wolf_problem, ceres::Problem::Options _options) :
+ ceres_problem_(new ceres::Problem(_options)),
+ wolf_problem_(_wolf_problem)
{
ceres::Covariance::Options covariance_options;
//covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD;
@@ -12,7 +13,7 @@ CeresManager::CeresManager(ceres::Problem::Options _options) :
CeresManager::~CeresManager()
{
- removeAllStateUnits();
+ removeAllStateBlocks();
std::cout << "all state units removed! \n";
std::cout << "residual blocks: " << ceres_problem_->NumResidualBlocks() << "\n";
@@ -36,7 +37,7 @@ ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_
return ceres_summary_;
}
-void CeresManager::computeCovariances(WolfProblem* _problem_ptr)
+void CeresManager::computeCovariances()
{
//std::cout << "computing covariances..." << std::endl;
@@ -44,8 +45,8 @@ void CeresManager::computeCovariances(WolfProblem* _problem_ptr)
std::vector<std::pair<const double*, const double*>> covariance_blocks;
// Last frame
- StateBase* current_position = _problem_ptr->getTrajectoryPtr()->getFrameListPtr()->back()->getPPtr();
- StateBase* current_orientation = _problem_ptr->getTrajectoryPtr()->getFrameListPtr()->back()->getOPtr();
+ StateBlock* current_position = wolf_problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getPPtr();
+ StateBlock* current_orientation = wolf_problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getOPtr();
double* current_position_ptr = current_position->getPtr();
double* current_orientation_ptr = current_orientation->getPtr();
covariance_blocks.push_back(std::make_pair(current_position_ptr,current_position_ptr));
@@ -53,7 +54,7 @@ void CeresManager::computeCovariances(WolfProblem* _problem_ptr)
covariance_blocks.push_back(std::make_pair(current_orientation_ptr,current_orientation_ptr));
// Landmarks and cross-covariance with current frame
- for(auto landmark_it = _problem_ptr->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it!=_problem_ptr->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
+ for(auto landmark_it = wolf_problem_->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it!=wolf_problem_->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
{
double* landmark_position_ptr = (*landmark_it)->getPPtr()->getPtr();
double* landmark_orientation_ptr = (*landmark_it)->getOPtr()->getPtr();
@@ -72,9 +73,9 @@ void CeresManager::computeCovariances(WolfProblem* _problem_ptr)
{
// STORE DESIRED COVARIANCES
// Last frame
- Eigen::MatrixXs m_pp(current_position->getStateSize(),current_position->getStateSize());
- Eigen::MatrixXs m_oo(current_orientation->getStateSize(),current_orientation->getStateSize());
- Eigen::MatrixXs m_po(current_position->getStateSize(),current_orientation->getStateSize());
+ Eigen::MatrixXs m_pp(current_position->getSize(),current_position->getSize());
+ Eigen::MatrixXs m_oo(current_orientation->getSize(),current_orientation->getSize());
+ Eigen::MatrixXs m_po(current_position->getSize(),current_orientation->getSize());
//std::cout << "getting m_pp covariance block... " << m_pp.rows() << "x" << m_pp.cols() << std::endl;
covariance_->GetCovarianceBlock(current_position_ptr, current_position_ptr, m_pp.data());
@@ -83,151 +84,135 @@ void CeresManager::computeCovariances(WolfProblem* _problem_ptr)
//std::cout << "getting m_po covariance block... " << m_po.rows() << "x" << m_po.cols() << std::endl;
covariance_->GetCovarianceBlock(current_orientation_ptr, current_orientation_ptr, m_oo.data());
- _problem_ptr->addCovarianceBlock(current_position, current_position, m_pp);
- _problem_ptr->addCovarianceBlock(current_orientation, current_orientation, m_oo);
- _problem_ptr->addCovarianceBlock(current_position, current_orientation, m_po);
+ wolf_problem_->addCovarianceBlock(current_position, current_position, m_pp);
+ wolf_problem_->addCovarianceBlock(current_orientation, current_orientation, m_oo);
+ wolf_problem_->addCovarianceBlock(current_position, current_orientation, m_po);
// Landmarks and cross-covariance with current frame
- for(auto landmark_it = _problem_ptr->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it!=_problem_ptr->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
+ for(auto landmark_it = wolf_problem_->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it!=wolf_problem_->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
{
- StateBase* landmark_position = (*landmark_it)->getPPtr();
- StateBase* landmark_orientation = (*landmark_it)->getOPtr();
- double* landmark_position_ptr = landmark_position->getPtr();
- double* landmark_orientation_ptr = landmark_orientation->getPtr();
-
- Eigen::MatrixXs m_landmark_pp(landmark_position->getStateSize(),landmark_position->getStateSize());
- Eigen::MatrixXs m_landmark_po(landmark_position->getStateSize(),landmark_orientation->getStateSize());
- Eigen::MatrixXs m_landmark_oo(landmark_orientation->getStateSize(),landmark_orientation->getStateSize());
- Eigen::MatrixXs m_landmark_p_frame_p(landmark_position->getStateSize(),current_position->getStateSize());
- Eigen::MatrixXs m_landmark_p_frame_o(landmark_position->getStateSize(),current_orientation->getStateSize());
- Eigen::MatrixXs m_landmark_o_frame_p(landmark_orientation->getStateSize(),current_position->getStateSize());
- Eigen::MatrixXs m_landmark_o_frame_o(landmark_orientation->getStateSize(),current_orientation->getStateSize());
-
- //std::cout << "getting m_landmark_pp covariance block... " << m_landmark_pp.rows() << "x" << m_landmark_pp.cols() << std::endl;
- covariance_->GetCovarianceBlock(landmark_position_ptr, landmark_position_ptr, m_landmark_pp.data());
- //std::cout << "getting m_landmark_po covariance block... " << m_landmark_po.rows() << "x" << m_landmark_po.cols() << std::endl;
- covariance_->GetCovarianceBlock(landmark_position_ptr, landmark_orientation_ptr, m_landmark_po.data());
- //std::cout << "getting m_landmark_oo covariance block... " << m_landmark_oo.rows() << "x" << m_landmark_oo.cols() << std::endl;
- covariance_->GetCovarianceBlock(landmark_orientation_ptr, landmark_orientation_ptr, m_landmark_oo.data());
- //std::cout << "getting m_landmark_p_frame_p covariance block... " << m_landmark_p_frame_p.rows() << "x" << m_landmark_p_frame_p.cols() << std::endl;
- covariance_->GetCovarianceBlock(landmark_position_ptr, current_position_ptr, m_landmark_p_frame_p.data());
- //std::cout << "getting m_landmark_p_frame_o covariance block... " << m_landmark_p_frame_o.rows() << "x" << m_landmark_p_frame_o.cols() << std::endl;
- covariance_->GetCovarianceBlock(landmark_position_ptr, current_orientation_ptr, m_landmark_p_frame_o.data());
- //std::cout << "getting m_landmark_o_frame_p covariance block... " << m_landmark_o_frame_p.rows() << "x" << m_landmark_o_frame_p.cols() << std::endl;
- covariance_->GetCovarianceBlock(landmark_orientation_ptr, current_position_ptr, m_landmark_o_frame_p.data());
- //std::cout << "getting m_landmark_o_frame_o covariance block... " << m_landmark_o_frame_o.rows() << "x" << m_landmark_o_frame_o.cols() << std::endl;
- covariance_->GetCovarianceBlock(landmark_orientation_ptr, current_orientation_ptr, m_landmark_o_frame_o.data());
-
- _problem_ptr->addCovarianceBlock(landmark_position, landmark_position, m_landmark_pp);
- _problem_ptr->addCovarianceBlock(landmark_position, landmark_orientation, m_landmark_po);
- _problem_ptr->addCovarianceBlock(landmark_orientation, landmark_orientation, m_landmark_oo);
- _problem_ptr->addCovarianceBlock(landmark_position, current_position, m_landmark_p_frame_p);
- _problem_ptr->addCovarianceBlock(landmark_position, current_orientation, m_landmark_p_frame_o);
- _problem_ptr->addCovarianceBlock(landmark_orientation, current_position, m_landmark_o_frame_p);
- _problem_ptr->addCovarianceBlock(landmark_orientation, current_orientation, m_landmark_o_frame_o);
+ StateBlock* landmark_position = (*landmark_it)->getPPtr();
+ StateBlock* landmark_orientation = (*landmark_it)->getOPtr();
+ double* landmark_position_ptr = landmark_position->getPtr();
+ double* landmark_orientation_ptr = landmark_orientation->getPtr();
+
+ Eigen::MatrixXs m_landmark_pp(landmark_position->getSize(),landmark_position->getSize());
+ Eigen::MatrixXs m_landmark_po(landmark_position->getSize(),landmark_orientation->getSize());
+ Eigen::MatrixXs m_landmark_oo(landmark_orientation->getSize(),landmark_orientation->getSize());
+ Eigen::MatrixXs m_landmark_p_frame_p(landmark_position->getSize(),current_position->getSize());
+ Eigen::MatrixXs m_landmark_p_frame_o(landmark_position->getSize(),current_orientation->getSize());
+ Eigen::MatrixXs m_landmark_o_frame_p(landmark_orientation->getSize(),current_position->getSize());
+ Eigen::MatrixXs m_landmark_o_frame_o(landmark_orientation->getSize(),current_orientation->getSize());
+
+ //std::cout << "getting m_landmark_pp covariance block... " << m_landmark_pp.rows() << "x" << m_landmark_pp.cols() << std::endl;
+ covariance_->GetCovarianceBlock(landmark_position_ptr, landmark_position_ptr, m_landmark_pp.data());
+ //std::cout << "getting m_landmark_po covariance block... " << m_landmark_po.rows() << "x" << m_landmark_po.cols() << std::endl;
+ covariance_->GetCovarianceBlock(landmark_position_ptr, landmark_orientation_ptr, m_landmark_po.data());
+ //std::cout << "getting m_landmark_oo covariance block... " << m_landmark_oo.rows() << "x" << m_landmark_oo.cols() << std::endl;
+ covariance_->GetCovarianceBlock(landmark_orientation_ptr, landmark_orientation_ptr, m_landmark_oo.data());
+ //std::cout << "getting m_landmark_p_frame_p covariance block... " << m_landmark_p_frame_p.rows() << "x" << m_landmark_p_frame_p.cols() << std::endl;
+ covariance_->GetCovarianceBlock(landmark_position_ptr, current_position_ptr, m_landmark_p_frame_p.data());
+ //std::cout << "getting m_landmark_p_frame_o covariance block... " << m_landmark_p_frame_o.rows() << "x" << m_landmark_p_frame_o.cols() << std::endl;
+ covariance_->GetCovarianceBlock(landmark_position_ptr, current_orientation_ptr, m_landmark_p_frame_o.data());
+ //std::cout << "getting m_landmark_o_frame_p covariance block... " << m_landmark_o_frame_p.rows() << "x" << m_landmark_o_frame_p.cols() << std::endl;
+ covariance_->GetCovarianceBlock(landmark_orientation_ptr, current_position_ptr, m_landmark_o_frame_p.data());
+ //std::cout << "getting m_landmark_o_frame_o covariance block... " << m_landmark_o_frame_o.rows() << "x" << m_landmark_o_frame_o.cols() << std::endl;
+ covariance_->GetCovarianceBlock(landmark_orientation_ptr, current_orientation_ptr, m_landmark_o_frame_o.data());
+
+ wolf_problem_->addCovarianceBlock(landmark_position, landmark_position, m_landmark_pp);
+ wolf_problem_->addCovarianceBlock(landmark_position, landmark_orientation, m_landmark_po);
+ wolf_problem_->addCovarianceBlock(landmark_orientation, landmark_orientation, m_landmark_oo);
+ wolf_problem_->addCovarianceBlock(landmark_position, current_position, m_landmark_p_frame_p);
+ wolf_problem_->addCovarianceBlock(landmark_position, current_orientation, m_landmark_p_frame_o);
+ wolf_problem_->addCovarianceBlock(landmark_orientation, current_position, m_landmark_o_frame_p);
+ wolf_problem_->addCovarianceBlock(landmark_orientation, current_orientation, m_landmark_o_frame_o);
}
}
else
std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
}
-void CeresManager::update(WolfProblem* _problem_ptr)
+void CeresManager::update()
{
- // IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
- if (_problem_ptr->isReallocated())
- {
- // Remove all parameter blocks (residual blocks will be also removed)
- removeAllStateUnits();
-
- // Add all parameter blocks
- for(auto state_unit_it = _problem_ptr->getStateListPtr()->begin(); state_unit_it!=_problem_ptr->getStateListPtr()->end(); state_unit_it++)
- addStateUnit(*state_unit_it);
-
- // Add all residual blocks
- ConstraintBaseList ctr_list;
- _problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list);
- for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
- addConstraint(*ctr_it);
-
- // set the wolf problem reallocation flag to false
- _problem_ptr->reallocationDone();
- }
- else
- {
- // ADD/UPDATE STATE UNITS
- for(auto state_unit_it = _problem_ptr->getStateListPtr()->begin(); state_unit_it!=_problem_ptr->getStateListPtr()->end(); state_unit_it++)
- {
- if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
- addStateUnit(*state_unit_it);
-
- else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
- updateStateUnitStatus(*state_unit_it);
- }
- //std::cout << "state units updated!" << std::endl;
-
- // REMOVE STATE UNITS
- while (!_problem_ptr->getRemovedStateListPtr()->empty())
- {
- ceres_problem_->RemoveParameterBlock(_problem_ptr->getRemovedStateListPtr()->front());
- _problem_ptr->getRemovedStateListPtr()->pop_front();
- }
- //std::cout << "state units removed!" << std::endl;
-
- // ADD CONSTRAINTS
- ConstraintBaseList ctr_list;
- _problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list);
- //std::cout << "ctr_list.size() = " << ctr_list.size() << std::endl;
- for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
- if ((*ctr_it)->getPendingStatus() == ADD_PENDING)
- addConstraint(*ctr_it);
-
- //std::cout << "constraints updated!" << std::endl;
- }
+ // REMOVE CONSTRAINTS
+ while (!wolf_problem_->getConstraintRemoveList()->empty())
+ {
+ removeConstraint(wolf_problem_->getConstraintRemoveList()->front());
+ wolf_problem_->getConstraintRemoveList()->pop_front();
+ }
+ // REMOVE STATE BLOCKS
+ while (!wolf_problem_->getStateBlockRemoveList()->empty())
+ {
+ removeStateBlock((double *)(wolf_problem_->getStateBlockRemoveList()->front()));
+ wolf_problem_->getStateBlockRemoveList()->pop_front();
+ }
+ // ADD STATE BLOCKS
+ while (!wolf_problem_->getStateBlockAddList()->empty())
+ {
+ addStateBlock(wolf_problem_->getStateBlockAddList()->front());
+ wolf_problem_->getStateBlockAddList()->pop_front();
+ }
+ // UPDATE STATE BLOCKS
+ while (!wolf_problem_->getStateBlockUpdateList()->empty())
+ {
+ updateStateBlockStatus(wolf_problem_->getStateBlockUpdateList()->front());
+ wolf_problem_->getStateBlockUpdateList()->pop_front();
+ }
+ // ADD CONSTRAINTS
+ while (!wolf_problem_->getConstraintAddList()->empty())
+ {
+ addConstraint(wolf_problem_->getConstraintAddList()->front(), true);
+ wolf_problem_->getConstraintAddList()->pop_front();
+ }
}
-void CeresManager::addConstraint(ConstraintBase* _corr_ptr)
+void CeresManager::addConstraint(ConstraintBase* _corr_ptr, const bool _apply_loss)
{
- ceres::ResidualBlockId blockIdx = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), NULL, _corr_ptr->getStateBlockPtrVector());
-// constraint_map_[_corr_ptr->nodeId()] = blockIdx;
- _corr_ptr->setPendingStatus(NOT_PENDING);
+ if (_apply_loss)
+ constraint_map_[_corr_ptr->nodeId()] = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), new ceres::CauchyLoss(0.5), _corr_ptr->getStateBlockPtrVector());
+ else
+ constraint_map_[_corr_ptr->nodeId()] = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), NULL, _corr_ptr->getStateBlockPtrVector());
}
void CeresManager::removeConstraint(const unsigned int& _corr_idx)
{
- // TODO: necessari? outliers?
-// ceres_problem_->RemoveResidualBlock(constraint_map_[_corr_idx]);
-// constraint_map_.erase(_corr_idx);
+ // TODO: check if not already removed by removing blocks
+ ceres_problem_->RemoveResidualBlock(constraint_map_[_corr_idx]);
+ constraint_map_.erase(_corr_idx);
}
-void CeresManager::addStateUnit(StateBase* _st_ptr)
+void CeresManager::addStateBlock(StateBlock* _st_ptr)
{
//std::cout << "Adding State Unit " << _st_ptr->nodeId() << std::endl;
//_st_ptr->print();
- switch (_st_ptr->getStateType())
+ switch (_st_ptr->getType())
{
case ST_VECTOR:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getStateSize(), nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getSize(), nullptr);
break;
}
case ST_QUATERNION:
{
//TODO: change nullptr below by quaternion parametrization following method in complex_angle_parametrization.cpp
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getStateSize(), nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getSize(), nullptr);
break;
}
default:
std::cout << "Unknown state type!" << std::endl;
}
- if (_st_ptr->getStateStatus() != ST_ESTIMATED)
- updateStateUnitStatus(_st_ptr);
+ if (_st_ptr->isFixed())
+ updateStateBlockStatus(_st_ptr);
+}
- _st_ptr->setPendingStatus(NOT_PENDING);
+void CeresManager::removeStateBlock(double* _st_ptr)
+{
+ ceres_problem_->RemoveParameterBlock(_st_ptr);
}
-void CeresManager::removeAllStateUnits()
+void CeresManager::removeAllStateBlocks()
{
std::vector<double*> parameter_blocks;
@@ -237,16 +222,12 @@ void CeresManager::removeAllStateUnits()
ceres_problem_->RemoveParameterBlock(parameter_blocks[i]);
}
-void CeresManager::updateStateUnitStatus(StateBase* _st_ptr)
+void CeresManager::updateStateBlockStatus(StateBlock* _st_ptr)
{
- if (_st_ptr->getStateStatus() == ST_ESTIMATED)
- ceres_problem_->SetParameterBlockVariable(_st_ptr->getPtr());
- else if (_st_ptr->getStateStatus() == ST_FIXED)
+ if (_st_ptr->isFixed())
ceres_problem_->SetParameterBlockConstant(_st_ptr->getPtr());
else
- printf("\nERROR: Update state unit status with unknown status");
-
- _st_ptr->setPendingStatus(NOT_PENDING);
+ ceres_problem_->SetParameterBlockVariable(_st_ptr->getPtr());
}
ceres::CostFunction* CeresManager::createCostFunction(ConstraintBase* _corrPtr)
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index da1095d0eff72490d025a0dce41c0bf7d23ebaf9..926996fc80cd92e76405bd262e030603c82efd92 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -8,7 +8,7 @@
//wof includes
#include "../wolf.h"
-#include "../state_base.h"
+#include "../state_block.h"
#include "../constraint_sparse.h"
#include "../constraint_fix.h"
#include "../constraint_gps_2D.h"
@@ -26,31 +26,33 @@
class CeresManager
{
protected:
-// std::map<unsigned int, ceres::ResidualBlockId> constraint_map_;
+ std::map<unsigned int, ceres::ResidualBlockId> constraint_map_;
ceres::Problem* ceres_problem_;
ceres::Covariance* covariance_;
+ WolfProblem* wolf_problem_;
public:
- CeresManager(ceres::Problem::Options _options);
+ CeresManager(WolfProblem* _wolf_problem, ceres::Problem::Options _options);
~CeresManager();
ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
- void computeCovariances(WolfProblem* _problem_ptr);
+ void computeCovariances();
- void update(const WolfProblemPtr _problem_ptr);
+ void update();
- void addConstraint(ConstraintBase* _corr_ptr);
+ void addConstraint(ConstraintBase* _corr_ptr, const bool _apply_loss);
- // TODO: not necessary?
void removeConstraint(const unsigned int& _corr_idx);
- void addStateUnit(StateBase* _st_ptr);
+ void addStateBlock(StateBlock* _st_ptr);
- void removeAllStateUnits();
+ void removeStateBlock(double* _st_ptr);
- void updateStateUnitStatus(StateBase* _st_ptr);
+ void removeAllStateBlocks();
+
+ void updateStateBlockStatus(StateBlock* _st_ptr);
ceres::CostFunction* createCostFunction(ConstraintBase* _corrPtr);
};
diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp
index babb3de7d75bafc93aad8891163924368185f1a3..05fb9a30a3610917661e20a6b559fec2b3706434 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -1,18 +1,98 @@
#include "constraint_base.h"
-ConstraintBase::ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp) :
+
+ConstraintBase::ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp, ConstraintStatus _status) :
+ NodeLinked(BOTTOM, "CONSTRAINT"),
+ type_(_tp),
+ category_(CTR_ABSOLUTE),
+ status_(_status),
+ measurement_(_ftr_ptr->getMeasurement()),
+ measurement_covariance_(_ftr_ptr->getMeasurementCovariance())
+{
+ std::cout << "creating ConstraintBase " << std::endl;
+}
+
+
+ConstraintBase::ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp, FrameBase* _frame_ptr, ConstraintStatus _status) :
+ NodeLinked(BOTTOM, "CONSTRAINT"),
+ type_(_tp),
+ category_(CTR_FRAME),
+ status_(_status),
+ measurement_(_ftr_ptr->getMeasurement()),
+ measurement_covariance_(_ftr_ptr->getMeasurementCovariance()),
+ frame_ptr_(_frame_ptr),
+ feature_ptr_(nullptr),
+ landmark_ptr_(nullptr)
+{
+ // add constraint to frame
+ frame_ptr_->addConstraintTo(this);
+}
+
+
+ConstraintBase::ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp, FeatureBase* _feature_ptr, ConstraintStatus _status) :
+ NodeLinked(BOTTOM, "CONSTRAINT"),
+ type_(_tp),
+ category_(CTR_FEATURE),
+ status_(_status),
+ measurement_(_ftr_ptr->getMeasurement()),
+ measurement_covariance_(_ftr_ptr->getMeasurementCovariance()),
+ frame_ptr_(nullptr),
+ feature_ptr_(_feature_ptr),
+ landmark_ptr_(nullptr)
+{
+ // add constraint to feature
+ feature_ptr_->addConstraintTo(this);
+}
+
+
+ConstraintBase::ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp, LandmarkBase* _landmark_ptr, ConstraintStatus _status) :
NodeLinked(BOTTOM, "CONSTRAINT"),
type_(_tp),
- measurement_(_ftr_ptr->getMeasurement()),
- measurement_covariance_(_ftr_ptr->getMeasurementCovariance()),
- pending_status_(ADD_PENDING)
+ category_(CTR_LANDMARK),
+ status_(_status),
+ measurement_(_ftr_ptr->getMeasurement()),
+ measurement_covariance_(_ftr_ptr->getMeasurementCovariance()),
+ frame_ptr_(nullptr),
+ feature_ptr_(nullptr),
+ landmark_ptr_(_landmark_ptr)
{
- //
+ // add constraint to landmark
+ landmark_ptr_->addConstraintTo(this);
}
ConstraintBase::~ConstraintBase()
{
//std::cout << "deleting ConstraintBase " << nodeId() << std::endl;
+ is_deleting_ = true;
+
+ // add constraint to be removed from solver
+ getTop()->removeConstraintPtr(this);
+
+ // remove constraint to frame/landmark/feature
+ switch(category_)
+ {
+ case CTR_FRAME:
+ {
+ frame_ptr_->removeConstraintTo(this);
+ break;
+ }
+ case CTR_FEATURE:
+ {
+ feature_ptr_->removeConstraintTo(this);
+ break;
+ }
+ case CTR_LANDMARK:
+ {
+ landmark_ptr_->removeConstraintTo(this);
+ break;
+ }
+ }
+}
+
+void ConstraintBase::destruct()
+{
+ if (!is_deleting_)
+ up_node_ptr_->removeDownNode(this);
}
ConstraintType ConstraintBase::getConstraintType() const
@@ -35,12 +115,32 @@ CaptureBase* ConstraintBase::getCapturePtr() const
return upperNodePtr()->upperNodePtr();
}
-PendingStatus ConstraintBase::getPendingStatus() const
+ConstraintCategory ConstraintBase::getCategory() const
+{
+ return category_;
+}
+
+ConstraintStatus ConstraintBase::getStatus() const
+{
+ return status_;
+}
+
+void ConstraintBase::setStatus(ConstraintStatus _status)
+{
+ status_ = _status;
+}
+
+FrameBase* ConstraintBase::getFrameToPtr()
+{
+ return frame_ptr_;
+}
+
+FeatureBase* ConstraintBase::getFeatureToPtr()
{
- return pending_status_;
+ return feature_ptr_;
}
-void ConstraintBase::setPendingStatus(PendingStatus _pending)
+LandmarkBase* ConstraintBase::getLandmarkToPtr()
{
- pending_status_ = _pending;
+ return landmark_ptr_;
}
diff --git a/src/constraint_base.h b/src/constraint_base.h
index 721d339c1d3fbf4b2d91da79135b70bf739f7c41..a023c2f1ea6f4c934825c497997fd28821728920 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -19,19 +19,43 @@ class NodeTerminus;
class ConstraintBase : public NodeLinked<FeatureBase, NodeTerminus>
{
protected:
- ConstraintType type_; ///< type of constraint (types defined at wolf.h)
- const Eigen::VectorXs& measurement_; ///< Direct access to the measurement
+ ConstraintType type_; ///< type of constraint (types defined at wolf.h)
+ ConstraintCategory category_; ///< category of constraint (types defined at wolf.h)
+ ConstraintStatus status_; ///< status of constraint (types defined at wolf.h)
+ const Eigen::VectorXs& measurement_; ///< Direct access to the measurement
const Eigen::MatrixXs& measurement_covariance_; ///< Direct access to the measurement's covariance
- PendingStatus pending_status_; ///< Pending status
-
+ FrameBase* frame_ptr_; ///< FrameBase pointer (for category CTR_FRAME)
+ FeatureBase* feature_ptr_; ///< FeatureBase pointer (for category CTR_FEATURE)
+ LandmarkBase* landmark_ptr_; ///< LandmarkBase pointer (for category CTR_LANDMARK)
public:
- /** \brief Constructor
+ /** \brief Constructor of category CTR_ABSOLUTE
+ *
+ * Constructor of category CTR_ABSOLUTE
+ *
+ **/
+ ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp, ConstraintStatus _status);
+
+ /** \brief Constructor of category CTR_FRAME
+ *
+ * Constructor of category CTR_FRAME
+ *
+ **/
+ ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp, FrameBase* _frame_ptr, ConstraintStatus _status);
+
+ /** \brief Constructor of category CTR_FEATURE
+ *
+ * Constructor of category CTR_FEATURE
+ *
+ **/
+ ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp, FeatureBase* _feature_ptr, ConstraintStatus _status);
+
+ /** \brief Constructor of category CTR_LANDMARK
*
- * Constructor
+ * Constructor of category CTR_LANDMARK
*
**/
- ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp);
+ ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp, LandmarkBase* _landmark_ptr, ConstraintStatus _status);
/** \brief Destructor
*
@@ -40,6 +64,13 @@ class ConstraintBase : public NodeLinked<FeatureBase, NodeTerminus>
**/
virtual ~ConstraintBase();
+ /** \brief Destructor call if is not already deleting
+ *
+ * Destructor call if is not already deleting
+ *
+ */
+ virtual void destruct();
+
/** \brief Returns the constraint type
*
* Returns the constraint type
@@ -59,7 +90,7 @@ class ConstraintBase : public NodeLinked<FeatureBase, NodeTerminus>
* Returns a vector of pointers to the state in which this constraint depends
*
**/
- virtual const std::vector<StateBase*> getStatePtrVector() const = 0;
+ virtual const std::vector<StateBlock*> getStatePtrVector() const = 0;
/** \brief Returns a pointer to the feature measurement
*
@@ -68,9 +99,9 @@ class ConstraintBase : public NodeLinked<FeatureBase, NodeTerminus>
**/
const Eigen::VectorXs& getMeasurement();
- /** \brief Returns a pointer to its capture
+ /** \brief Returns a pointer to the feature constrained from
*
- * Returns a pointer to its capture
+ * Returns a pointer to the feature constrained from
*
**/
FeatureBase* getFeaturePtr() const;
@@ -89,20 +120,46 @@ class ConstraintBase : public NodeLinked<FeatureBase, NodeTerminus>
**/
virtual unsigned int getSize() const = 0;
- /** \brief Gets the node pending status (pending or not to be added/updated in the filter or optimizer)
+ /** \brief Gets the category
+ *
+ * Gets the category
+ *
+ */
+ ConstraintCategory getCategory() const;
+
+ /** \brief Gets the status
*
- * Gets the node pending status (pending or not to be added/updated in the filter or optimizer)
+ * Gets the status
*
*/
- PendingStatus getPendingStatus() const;
+ ConstraintStatus getStatus() const;
- /** \brief Sets the node pending status (pending or not to be added/updated in the filter or optimizer)
+ /** \brief Sets the status
*
- * Sets the node pending status (pending or not to be added/updated in the filter or optimizer)
+ * Sets the status
*
*/
- void setPendingStatus(PendingStatus _pending);
+ void setStatus(ConstraintStatus _status);
+
+ /** \brief Returns a pointer to the frame constrained to
+ *
+ * Returns a pointer to the frame constrained to
+ *
+ **/
+ FrameBase* getFrameToPtr();
+ /** \brief Returns a pointer to the feature constrained to
+ *
+ * Returns a pointer to the feature constrained to
+ *
+ **/
+ FeatureBase* getFeatureToPtr();
+ /** \brief Returns a pointer to the landmark constrained to
+ *
+ * Returns a pointer to the landmark constrained to
+ *
+ **/
+ LandmarkBase* getLandmarkToPtr();
};
#endif
diff --git a/src/constraint_container.h b/src/constraint_container.h
index 481a3b3a663ac7aae0340b38d4c7dce85156d6e1..acc323fe61aae8458653fb825800b71f6cc291a3 100644
--- a/src/constraint_container.h
+++ b/src/constraint_container.h
@@ -15,19 +15,19 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
public:
static const unsigned int N_BLOCKS = 4;
- ConstraintContainer(FeatureBase* _ftr_ptr, LandmarkContainer* _lmk_ptr, const unsigned int _corner) :
- ConstraintSparse<3,2,1,2,1>(_ftr_ptr,CTR_CONTAINER, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()),
+ ConstraintContainer(FeatureBase* _ftr_ptr, LandmarkContainer* _lmk_ptr, const unsigned int _corner, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintSparse<3,2,1,2,1>(_ftr_ptr, CTR_CONTAINER, _lmk_ptr, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()),
lmk_ptr_(_lmk_ptr),
corner_(_corner)
{
assert(_corner >= 0 && _corner <= 3 && "Wrong corner id in constraint container constructor");
- lmk_ptr_->hit(this);
+ lmk_ptr_->addConstraintTo(this);
}
virtual ~ConstraintContainer()
{
//std::cout << "deleting ConstraintContainer " << nodeId() << std::endl;
- lmk_ptr_->unhit(this);
+ lmk_ptr_->removeConstraintTo(this);
}
LandmarkContainer* getLandmarkPtr()
diff --git a/src/constraint_corner_2D.h b/src/constraint_corner_2D.h
index d651fcb9b55c184bbc326751efbdafed6f2adbdf..073fc543e66d02219f89c2027b7126d3961564c3 100644
--- a/src/constraint_corner_2D.h
+++ b/src/constraint_corner_2D.h
@@ -8,28 +8,24 @@
class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1>
{
- protected:
- LandmarkCorner2D* lmk_ptr_;
-
public:
static const unsigned int N_BLOCKS = 4;
- ConstraintCorner2D(FeatureBase* _ftr_ptr, LandmarkCorner2D* _lmk_ptr) :
- ConstraintSparse<3,2,1,2,1>(_ftr_ptr,CTR_CORNER_2D, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()),
- lmk_ptr_(_lmk_ptr)
+ ConstraintCorner2D(FeatureBase* _ftr_ptr, LandmarkCorner2D* _lmk_ptr, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintSparse<3,2,1,2,1>(_ftr_ptr, CTR_CORNER_2D, _lmk_ptr, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr())
{
- lmk_ptr_->hit(this);
+ landmark_ptr_->addConstraintTo(this);
}
virtual ~ConstraintCorner2D()
{
//std::cout << "deleting ConstraintCorner2D " << nodeId() << std::endl;
- lmk_ptr_->unhit(this);
+ landmark_ptr_->removeConstraintTo(this);
}
LandmarkCorner2D* getLandmarkPtr()
{
- return lmk_ptr_;
+ return (LandmarkCorner2D*) landmark_ptr_;
}
template <typename T>
diff --git a/src/constraint_fix.h b/src/constraint_fix.h
index cabe1420891b34a9d681edfc322ebeb5c8e0678f..c0686159c5944120367dcde34bda66e71049be21 100644
--- a/src/constraint_fix.h
+++ b/src/constraint_fix.h
@@ -11,10 +11,10 @@ class ConstraintFix: public ConstraintSparse<3,2,1>
public:
static const unsigned int N_BLOCKS = 2;
- ConstraintFix(FeatureBase* _ftr_ptr, FrameBase* _frame_ptr):
- ConstraintSparse<3,2,1>(_ftr_ptr, CTR_FIX, _frame_ptr->getPPtr(), _frame_ptr->getOPtr())
+ ConstraintFix(FeatureBase* _ftr_ptr, ConstraintStatus _status = CTR_ACTIVE):
+ ConstraintSparse<3,2,1>(_ftr_ptr, CTR_FIX, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
{
- //
+ std::cout << "creating ConstraintFix " << std::endl;
}
virtual ~ConstraintFix()
diff --git a/src/constraint_gps_2D.h b/src/constraint_gps_2D.h
index 11ef68a9ea16ecb9d6d029ade6489aaa9a2af8e4..fd17abba7589513b939b3fe20a15235f5317860f 100644
--- a/src/constraint_gps_2D.h
+++ b/src/constraint_gps_2D.h
@@ -11,8 +11,8 @@ class ConstraintGPS2D: public ConstraintSparse<2,2>
public:
static const unsigned int N_BLOCKS = 1;
- ConstraintGPS2D(FeatureBase* _ftr_ptr, FrameBase* _frame_ptr):
- ConstraintSparse<2,2>(_ftr_ptr,CTR_GPS_FIX_2D, _frame_ptr->getPPtr())
+ ConstraintGPS2D(FeatureBase* _ftr_ptr, FrameBase* _frame_ptr, ConstraintStatus _status = CTR_ACTIVE):
+ ConstraintSparse<2,2>(_ftr_ptr,CTR_GPS_FIX_2D, _status, _frame_ptr->getPPtr())
{
//
}
diff --git a/src/constraint_odom_2D.h b/src/constraint_odom_2D.h
index eaefa6d5ba5b4f62c98d1e001105068fcc6e3da2..780b7c98c3ad22be2bf5f5d38139aeaabff0efbb 100644
--- a/src/constraint_odom_2D.h
+++ b/src/constraint_odom_2D.h
@@ -10,8 +10,8 @@ class ConstraintOdom2D : public ConstraintSparse<3, 2, 1, 2, 1>
public:
static const unsigned int N_BLOCKS = 4;
- ConstraintOdom2D(FeatureBase* _ftr_ptr, FrameBase* _frame_ptr) :
- ConstraintSparse<3, 2, 1, 2, 1>(_ftr_ptr, CTR_ODOM_2D, _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+ ConstraintOdom2D(FeatureBase* _ftr_ptr, FrameBase* _frame_ptr, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintSparse<3, 2, 1, 2, 1>(_ftr_ptr, CTR_ODOM_2D, _frame_ptr, _status, _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
{
//
}
diff --git a/src/constraint_sparse.h b/src/constraint_sparse.h
index 8823716cf7c57954ff424145c395c9181bdfccc1..6cbd33fbb06dbb20308376fd6b4c411fbc625eb4 100644
--- a/src/constraint_sparse.h
+++ b/src/constraint_sparse.h
@@ -7,7 +7,7 @@
#include "constraint_base.h"
-//template class ConstraintBase
+//template class ConstraintSparse
template <const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE = 0,
@@ -22,7 +22,7 @@ template <const unsigned int MEASUREMENT_SIZE,
class ConstraintSparse: public ConstraintBase
{
protected:
- std::vector<StateBase*> state_ptr_vector_;
+ std::vector<StateBlock*> state_ptr_vector_;
std::vector<unsigned int> state_block_sizes_vector_;
public:
@@ -38,184 +38,533 @@ class ConstraintSparse: public ConstraintBase
static const unsigned int block8Size = BLOCK_8_SIZE;
static const unsigned int block9Size = BLOCK_9_SIZE;
- /** \brief Contructor with state pointer separated
+ /** \brief Constructor of category CTR_ABSOLUTE
*
- * Constructor with state pointers separated
+ * Constructor of category CTR_ABSOLUTE
*
**/
- ConstraintSparse(FeatureBase* _ftr_ptr,
- ConstraintType _tp,
- StateBase* _state0Ptr,
- StateBase* _state1Ptr = nullptr,
- StateBase* _state2Ptr = nullptr,
- StateBase* _state3Ptr = nullptr,
- StateBase* _state4Ptr = nullptr,
- StateBase* _state5Ptr = nullptr,
- StateBase* _state6Ptr = nullptr,
- StateBase* _state7Ptr = nullptr,
- StateBase* _state8Ptr = nullptr,
- StateBase* _state9Ptr = nullptr ) :
- ConstraintBase(_ftr_ptr,_tp),
- state_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
- state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
- {
- for (unsigned int ii = 1; ii<state_block_sizes_vector_.size(); ii++)
- {
- if (state_ptr_vector_.at(ii) != nullptr)
- assert(state_block_sizes_vector_.at(ii) != 0 && "Too many non-null state pointers in ConstraintSparse constructor");
-
- else
- {
- assert(state_block_sizes_vector_.at(ii) == 0 && "No non-null state pointers enough in ConstraintSparse constructor");
- state_ptr_vector_.resize(ii);
- state_block_sizes_vector_.resize(ii);
- break;
- }
- }
- }
+ ConstraintSparse(FeatureBase* _ftr_ptr, ConstraintType _tp, ConstraintStatus _status,
+ StateBlock* _state0Ptr,
+ StateBlock* _state1Ptr = nullptr,
+ StateBlock* _state2Ptr = nullptr,
+ StateBlock* _state3Ptr = nullptr,
+ StateBlock* _state4Ptr = nullptr,
+ StateBlock* _state5Ptr = nullptr,
+ StateBlock* _state6Ptr = nullptr,
+ StateBlock* _state7Ptr = nullptr,
+ StateBlock* _state8Ptr = nullptr,
+ StateBlock* _state9Ptr = nullptr ) ;
+
+ /** \brief Constructor of category CTR_FRAME
+ *
+ * Constructor of category CTR_FRAME
+ *
+ **/
+ ConstraintSparse(FeatureBase* _ftr_ptr, ConstraintType _tp, FrameBase* _frame_ptr, ConstraintStatus _status,
+ StateBlock* _state0Ptr,
+ StateBlock* _state1Ptr = nullptr,
+ StateBlock* _state2Ptr = nullptr,
+ StateBlock* _state3Ptr = nullptr,
+ StateBlock* _state4Ptr = nullptr,
+ StateBlock* _state5Ptr = nullptr,
+ StateBlock* _state6Ptr = nullptr,
+ StateBlock* _state7Ptr = nullptr,
+ StateBlock* _state8Ptr = nullptr,
+ StateBlock* _state9Ptr = nullptr );
+
+ /** \brief Constructor of category CTR_FEATURE
+ *
+ * Constructor of category CTR_FEATURE
+ *
+ **/
+ ConstraintSparse(FeatureBase* _ftr_ptr, ConstraintType _tp, FeatureBase* _feature_ptr, ConstraintStatus _status,
+ StateBlock* _state0Ptr,
+ StateBlock* _state1Ptr = nullptr,
+ StateBlock* _state2Ptr = nullptr,
+ StateBlock* _state3Ptr = nullptr,
+ StateBlock* _state4Ptr = nullptr,
+ StateBlock* _state5Ptr = nullptr,
+ StateBlock* _state6Ptr = nullptr,
+ StateBlock* _state7Ptr = nullptr,
+ StateBlock* _state8Ptr = nullptr,
+ StateBlock* _state9Ptr = nullptr ) ;
+
+ /** \brief Constructor of category CTR_LANDMARK
+ *
+ * Constructor of category CTR_LANDMARK
+ *
+ **/
+ ConstraintSparse(FeatureBase* _ftr_ptr, ConstraintType _tp, LandmarkBase* _landmark_ptr, ConstraintStatus _status,
+ StateBlock* _state0Ptr,
+ StateBlock* _state1Ptr = nullptr,
+ StateBlock* _state2Ptr = nullptr,
+ StateBlock* _state3Ptr = nullptr,
+ StateBlock* _state4Ptr = nullptr,
+ StateBlock* _state5Ptr = nullptr,
+ StateBlock* _state6Ptr = nullptr,
+ StateBlock* _state7Ptr = nullptr,
+ StateBlock* _state8Ptr = nullptr,
+ StateBlock* _state9Ptr = nullptr ) ;
/** \brief Destructor
*
* Destructor
*
**/
- virtual ~ConstraintSparse()
- {
- //
- }
+ virtual ~ConstraintSparse();
/** \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 const std::vector<WolfScalar*> getStateBlockPtrVector()
- {
- assert(state_ptr_vector_.size() > 0 && state_ptr_vector_.size() <= 10 && "Wrong state vector size in constraint, it should be between 1 and 10");
-
- switch (state_ptr_vector_.size())
- {
- case 1:
- {
- return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr()});
- }
- case 2:
- {
- return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr()});
- }
- case 3:
- {
- return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr()});
- }
- case 4:
- {
- return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr()});
- }
- case 5:
- {
- return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr()});
- }
- case 6:
- {
- return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr()});
- }
- case 7:
- {
- return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr(),
- state_ptr_vector_[6]->getPtr()});
- }
- case 8:
- {
- return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr(),
- state_ptr_vector_[6]->getPtr(),
- state_ptr_vector_[7]->getPtr()});
- }
- case 9:
- {
- return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr(),
- state_ptr_vector_[6]->getPtr(),
- state_ptr_vector_[7]->getPtr(),
- state_ptr_vector_[8]->getPtr()});
- }
- case 10:
- {
- return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
- state_ptr_vector_[1]->getPtr(),
- state_ptr_vector_[2]->getPtr(),
- state_ptr_vector_[3]->getPtr(),
- state_ptr_vector_[4]->getPtr(),
- state_ptr_vector_[5]->getPtr(),
- state_ptr_vector_[6]->getPtr(),
- state_ptr_vector_[7]->getPtr(),
- state_ptr_vector_[8]->getPtr(),
- state_ptr_vector_[9]->getPtr()});
- }
- }
-
- return std::vector<WolfScalar*>(0); //Not going to happen
- }
+ virtual const std::vector<WolfScalar*> getStateBlockPtrVector();
/** \brief Returns a vector of pointers to the states
*
* Returns a vector of pointers to the state in which this constraint depends
*
**/
- virtual const std::vector<StateBase*> getStatePtrVector() const
- {
- return state_ptr_vector_;
- }
+ virtual const std::vector<StateBlock*> getStatePtrVector() const;
/** \brief Returns the constraint residual size
*
* Returns the constraint residual size
*
**/
- virtual unsigned int getSize() const
+ virtual unsigned int getSize() const;
+
+ virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
+
+ private:
+ void resizeVectors();
+};
+
+
+//////////////////////////////////////////
+// IMPLEMENTATION
+//////////////////////////////////////////
+template <const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+ConstraintSparse<MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::ConstraintSparse(FeatureBase* _ftr_ptr, ConstraintType _tp, ConstraintStatus _status,
+ StateBlock* _state0Ptr,
+ StateBlock* _state1Ptr,
+ StateBlock* _state2Ptr,
+ StateBlock* _state3Ptr,
+ StateBlock* _state4Ptr,
+ StateBlock* _state5Ptr,
+ StateBlock* _state6Ptr,
+ StateBlock* _state7Ptr,
+ StateBlock* _state8Ptr,
+ StateBlock* _state9Ptr ) :
+ ConstraintBase(_ftr_ptr,_tp, _status),
+ state_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
+ state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
{
- return MEASUREMENT_SIZE;
+ resizeVectors();
}
- virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const
+template <const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+ConstraintSparse<MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::ConstraintSparse(FeatureBase* _ftr_ptr, ConstraintType _tp, FrameBase* _frame_ptr, ConstraintStatus _status,
+ StateBlock* _state0Ptr,
+ StateBlock* _state1Ptr,
+ StateBlock* _state2Ptr,
+ StateBlock* _state3Ptr,
+ StateBlock* _state4Ptr,
+ StateBlock* _state5Ptr,
+ StateBlock* _state6Ptr,
+ StateBlock* _state7Ptr,
+ StateBlock* _state8Ptr,
+ StateBlock* _state9Ptr ) :
+ ConstraintBase(_ftr_ptr, _tp, _frame_ptr, _status),
+ state_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
+ state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
{
- NodeLinked::printSelf(_ntabs, _ost);
- for (unsigned int ii = 0; ii<state_block_sizes_vector_.size(); ii++)
- {
- printTabs(_ntabs);
- _ost << "block " << ii << ": ";
- for (unsigned int jj = 0; jj<state_block_sizes_vector_.at(ii); jj++)
- _ost << *(state_ptr_vector_.at(ii)->getPtr()+jj) << " ";
- _ost << std::endl;
- }
+ resizeVectors();
}
-};
+
+template <const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+ConstraintSparse<MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::ConstraintSparse(FeatureBase* _ftr_ptr, ConstraintType _tp, FeatureBase* _feature_ptr, ConstraintStatus _status,
+ StateBlock* _state0Ptr,
+ StateBlock* _state1Ptr,
+ StateBlock* _state2Ptr,
+ StateBlock* _state3Ptr,
+ StateBlock* _state4Ptr,
+ StateBlock* _state5Ptr,
+ StateBlock* _state6Ptr,
+ StateBlock* _state7Ptr,
+ StateBlock* _state8Ptr,
+ StateBlock* _state9Ptr ) :
+ ConstraintBase(_ftr_ptr, _tp, _feature_ptr, _status),
+ state_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
+ state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
+ {
+ resizeVectors();
+ }
+
+template <const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+ConstraintSparse<MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::ConstraintSparse(FeatureBase* _ftr_ptr, ConstraintType _tp, LandmarkBase* _landmark_ptr, ConstraintStatus _status,
+ StateBlock* _state0Ptr,
+ StateBlock* _state1Ptr,
+ StateBlock* _state2Ptr,
+ StateBlock* _state3Ptr,
+ StateBlock* _state4Ptr,
+ StateBlock* _state5Ptr,
+ StateBlock* _state6Ptr,
+ StateBlock* _state7Ptr,
+ StateBlock* _state8Ptr,
+ StateBlock* _state9Ptr ) :
+ ConstraintBase(_ftr_ptr, _tp, _landmark_ptr, _status),
+ state_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
+ state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
+ {
+ resizeVectors();
+ }
+
+template <const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+ConstraintSparse<MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::~ConstraintSparse()
+{
+ //
+}
+
+template <const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+const std::vector<WolfScalar*> ConstraintSparse<MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::getStateBlockPtrVector()
+{
+ assert(state_ptr_vector_.size() > 0 && state_ptr_vector_.size() <= 10 && "Wrong state vector size in constraint, it should be between 1 and 10");
+
+ switch (state_ptr_vector_.size())
+ {
+ case 1:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr()});
+ }
+ case 2:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr()});
+ }
+ case 3:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr()});
+ }
+ case 4:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr(),
+ state_ptr_vector_[3]->getPtr()});
+ }
+ case 5:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr(),
+ state_ptr_vector_[3]->getPtr(),
+ state_ptr_vector_[4]->getPtr()});
+ }
+ case 6:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr(),
+ state_ptr_vector_[3]->getPtr(),
+ state_ptr_vector_[4]->getPtr(),
+ state_ptr_vector_[5]->getPtr()});
+ }
+ case 7:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr(),
+ state_ptr_vector_[3]->getPtr(),
+ state_ptr_vector_[4]->getPtr(),
+ state_ptr_vector_[5]->getPtr(),
+ state_ptr_vector_[6]->getPtr()});
+ }
+ case 8:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr(),
+ state_ptr_vector_[3]->getPtr(),
+ state_ptr_vector_[4]->getPtr(),
+ state_ptr_vector_[5]->getPtr(),
+ state_ptr_vector_[6]->getPtr(),
+ state_ptr_vector_[7]->getPtr()});
+ }
+ case 9:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr(),
+ state_ptr_vector_[3]->getPtr(),
+ state_ptr_vector_[4]->getPtr(),
+ state_ptr_vector_[5]->getPtr(),
+ state_ptr_vector_[6]->getPtr(),
+ state_ptr_vector_[7]->getPtr(),
+ state_ptr_vector_[8]->getPtr()});
+ }
+ case 10:
+ {
+ return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
+ state_ptr_vector_[1]->getPtr(),
+ state_ptr_vector_[2]->getPtr(),
+ state_ptr_vector_[3]->getPtr(),
+ state_ptr_vector_[4]->getPtr(),
+ state_ptr_vector_[5]->getPtr(),
+ state_ptr_vector_[6]->getPtr(),
+ state_ptr_vector_[7]->getPtr(),
+ state_ptr_vector_[8]->getPtr(),
+ state_ptr_vector_[9]->getPtr()});
+ }
+ }
+
+ return std::vector<WolfScalar*>(0); //Not going to happen
+}
+
+template <const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+const std::vector<StateBlock*> ConstraintSparse<MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::getStatePtrVector() const
+{
+ return state_ptr_vector_;
+}
+
+template <const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+unsigned int ConstraintSparse<MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::getSize() const
+{
+ return MEASUREMENT_SIZE;
+}
+
+template <const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+void ConstraintSparse<MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::print(unsigned int _ntabs, std::ostream& _ost) const
+{
+ NodeLinked::printSelf(_ntabs, _ost);
+ for (unsigned int ii = 0; ii<state_block_sizes_vector_.size(); ii++)
+ {
+ printTabs(_ntabs);
+ _ost << "block " << ii << ": ";
+ for (unsigned int jj = 0; jj<state_block_sizes_vector_.at(ii); jj++)
+ _ost << *(state_ptr_vector_.at(ii)->getPtr()+jj) << " ";
+ _ost << std::endl;
+ }
+}
+
+template <const unsigned int MEASUREMENT_SIZE,
+ unsigned int BLOCK_0_SIZE,
+ unsigned int BLOCK_1_SIZE,
+ unsigned int BLOCK_2_SIZE,
+ unsigned int BLOCK_3_SIZE,
+ unsigned int BLOCK_4_SIZE,
+ unsigned int BLOCK_5_SIZE,
+ unsigned int BLOCK_6_SIZE,
+ unsigned int BLOCK_7_SIZE,
+ unsigned int BLOCK_8_SIZE,
+ unsigned int BLOCK_9_SIZE>
+void ConstraintSparse<MEASUREMENT_SIZE,
+ BLOCK_0_SIZE,
+ BLOCK_1_SIZE,
+ BLOCK_2_SIZE,
+ BLOCK_3_SIZE,
+ BLOCK_4_SIZE,
+ BLOCK_5_SIZE,
+ BLOCK_6_SIZE,
+ BLOCK_7_SIZE,
+ BLOCK_8_SIZE,
+ BLOCK_9_SIZE>::resizeVectors()
+{
+ for (unsigned int ii = 1; ii<state_block_sizes_vector_.size(); ii++)
+ {
+ if (state_ptr_vector_.at(ii) != nullptr)
+ assert(state_block_sizes_vector_.at(ii) != 0 && "Too many non-null state pointers in ConstraintSparse constructor");
+
+ else
+ {
+ assert(state_block_sizes_vector_.at(ii) == 0 && "No non-null state pointers enough in ConstraintSparse constructor");
+ state_ptr_vector_.resize(ii);
+ state_block_sizes_vector_.resize(ii);
+ break;
+ }
+ }
+}
+
#endif
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 7e94ea9a624d9f6d50da99b578e3dee4e2b1fdc4..482dddd4cb62b4dbc82335621b56edf5fddbfbe3 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -114,19 +114,7 @@ int main(int argc, char** argv)
//init google log
//google::InitGoogleLogging(argv[0]);
- // Ceres initialization
- 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;
- ceres::Problem::Options problem_options;
- problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
- problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
- problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
- CeresManager* ceres_manager = new CeresManager(problem_options);
- std::ofstream log_file, landmark_file; //output file
+
// Faramotics stuff
Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
@@ -168,18 +156,32 @@ int main(int argc, char** argv)
Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
laser_1_pose << 1.2, 0, 0, 0; //laser 1
laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
- SensorOdom2D odom_sensor(new StateBase(odom_pose.data()), new StateBase(&odom_pose(2)), odom_std_factor, odom_std_factor);
- SensorGPSFix gps_sensor(new StateBase(gps_pose.data()), new StateBase(&gps_pose(2)), gps_std);
- SensorLaser2D laser_1_sensor(new StateBase(laser_1_pose.data()), new StateBase(&laser_1_pose(3)));
- SensorLaser2D laser_2_sensor(new StateBase(laser_2_pose.data()), new StateBase(&laser_2_pose(3)));
+ SensorOdom2D odom_sensor(new StateBlock(odom_pose.head(2)), new StateBlock(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
+ SensorGPSFix gps_sensor(new StateBlock(gps_pose.head(2)), new StateBlock(gps_pose.tail(1)), gps_std);
+ SensorLaser2D laser_1_sensor(new StateBlock(laser_1_pose.head(2)), new StateBlock(laser_1_pose.tail(1)));
+ SensorLaser2D laser_2_sensor(new StateBlock(laser_2_pose.head(2)), new StateBlock(laser_2_pose.tail(1)));
// Initial pose
pose_odom << 2, 8, 0;
ground_truth.head(3) = pose_odom;
odom_trajectory.head(3) = pose_odom;
- WolfManager<StateBase, StateBase>* wolf_manager = new WolfManager<StateBase, StateBase>(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3);
+ WolfManager* wolf_manager = new WolfManager(PO_2D, 1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3);
+ // 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;
+ ceres::Problem::Options problem_options;
+ problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+ problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+ problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+ CeresManager* ceres_manager = new CeresManager(wolf_manager->getProblemPtr(), problem_options);
+ std::ofstream log_file, landmark_file; //output file
+
//std::cout << "START TRAJECTORY..." << std::endl;
// START TRAJECTORY ============================================================================================
for (unsigned int step = 1; step < n_execution; step++)
@@ -247,7 +249,7 @@ int main(int argc, char** argv)
std::cout << "UPDATING CERES..." << std::endl;
t1 = clock();
// update state units and constraints in ceres
- ceres_manager->update(wolf_manager->getProblemPtr());
+ ceres_manager->update();
mean_times(2) += ((double) clock() - t1) / CLOCKS_PER_SEC;
// SOLVE OPTIMIZATION ---------------------------
@@ -260,7 +262,7 @@ int main(int argc, char** argv)
// COMPUTE COVARIANCES ---------------------------
std::cout << "COMPUTING COVARIANCES..." << std::endl;
t1 = clock();
- ceres_manager->computeCovariances(wolf_manager->getProblemPtr());
+ ceres_manager->computeCovariances();
mean_times(4) += ((double) clock() - t1) / CLOCKS_PER_SEC;
// DRAWING STUFF ---------------------------
diff --git a/src/examples/test_ceres_laser.cpp b/src/examples/test_ceres_laser.cpp
index 8bef1334bf70ba384467f0d6a821e2fde72d681b..a63dd899579855f3fb4c699051ef87aad7f3981b 100644
--- a/src/examples/test_ceres_laser.cpp
+++ b/src/examples/test_ceres_laser.cpp
@@ -140,16 +140,16 @@ class WolfManager
void createFrame(const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp)
{
// Create frame and add it to the trajectory
- StateBase* new_position = new StateBase(problem_->getNewStatePtr());
+ StateBlock* new_position = new StateBlock(problem_->getNewStatePtr());
problem_->addState(new_position, _frame_state.head(2));
- StateBase* new_orientation;
+ StateBlock* new_orientation;
if (use_complex_angles_)
new_orientation = new StateComplexAngle(problem_->getNewStatePtr());
else
- new_orientation = new StateBase(problem_->getNewStatePtr());
+ new_orientation = new StateBlock(problem_->getNewStatePtr());
- problem_->addState(new_orientation, _frame_state.tail(new_orientation->getStateSize()));
+ problem_->addState(new_orientation, _frame_state.tail(new_orientation->getSize()));
problem_->getTrajectoryPtr()->addFrame(new FrameBase(_time_stamp, new_position, new_orientation));
@@ -242,7 +242,7 @@ class WolfManager
return problem_->getState();
}
- StateBaseList* getStateListPtr()
+ StateBlockList* getStateListPtr()
{
return problem_->getStateListPtr();
}
diff --git a/src/examples/test_ceres_manager.cpp b/src/examples/test_ceres_manager.cpp
index 20560388261908152a303ccf57bee98006abbca6..65221225f234122e5d6f2f2c3e56fce9610fc7b4 100644
--- a/src/examples/test_ceres_manager.cpp
+++ b/src/examples/test_ceres_manager.cpp
@@ -154,7 +154,7 @@ class ConstraintGPS2D : public ConstraintSparse<2,2>
{
}
- ConstraintGPS2D(WolfScalar* _measurementPtr, StateBase* _statePtr) :
+ ConstraintGPS2D(WolfScalar* _measurementPtr, StateBlock* _statePtr) :
ConstraintSparse<2,2>(_measurementPtr, _statePtr->getPtr())
{
}
@@ -193,7 +193,7 @@ class Constraint2DOdometry : public ConstraintSparse<2,2,2,2,2>
{
}
- Constraint2DOdometry(WolfScalar* _measurementPtr, StateBase* _state0Ptr, StateBase* _state1Ptr, StateBase* _state2Ptr, StateBase* _state3Ptr) :
+ Constraint2DOdometry(WolfScalar* _measurementPtr, StateBlock* _state0Ptr, StateBlock* _state1Ptr, StateBlock* _state2Ptr, StateBlock* _state3Ptr) :
ConstraintSparse<2,2,2,2,2>(_measurementPtr, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
{
}
@@ -239,7 +239,7 @@ class Constraint2DOdometryTheta : public ConstraintSparse<2,2,1,2,1>
{
}
- Constraint2DOdometryTheta(WolfScalar* _measurementPtr, StateBase* _state0Ptr, StateBase* _state1Ptr, StateBase* _state2Ptr, StateBase* _state3Ptr) :
+ Constraint2DOdometryTheta(WolfScalar* _measurementPtr, StateBlock* _state0Ptr, StateBlock* _state1Ptr, StateBlock* _state2Ptr, StateBlock* _state3Ptr) :
ConstraintSparse<2,2,1,2,1>(_measurementPtr, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
{
}
@@ -358,7 +358,7 @@ class WolfManager
// StateBaseShPtr(new StateBase(state_.data()+first_empty_state_)),
// StateBaseShPtr(new StateComplexAngle(state_.data()+first_empty_state_+2)))));
frames_.push_back(new FrameBase(_time_stamp,
- new StateBase(state_.data()+first_empty_state_),
+ new StateBlock(state_.data()+first_empty_state_),
new StateComplexAngle(state_.data()+first_empty_state_+2)));
else
@@ -366,8 +366,8 @@ class WolfManager
// StateBaseShPtr(new StateBase(state_.data()+first_empty_state_)),
// StateBaseShPtr(new StateBase(state_.data()+first_empty_state_+2)))));
frames_.push_back(new FrameBase(_time_stamp,
- new StateBase(state_.data()+first_empty_state_),
- new StateBase(state_.data()+first_empty_state_+2)));
+ new StateBlock(state_.data()+first_empty_state_),
+ new StateBlock(state_.data()+first_empty_state_+2)));
// Update first free state location index
first_empty_state_ += use_complex_angles_ ? 4 : 3;
}
@@ -432,7 +432,7 @@ class WolfManager
constraints_.push_back(new ConstraintGPS2D(_gps_capture->getPtr(), frames_.back()->getPPtr()->getPtr()));
}
- void update(std::queue<StateBase*>& new_state_units, std::queue<ConstraintXBase*>& new_constraints)
+ void update(std::queue<StateBlock*>& new_state_units, std::queue<ConstraintXBase*>& new_constraints)
{
while (!new_captures_.empty())
{
@@ -465,9 +465,9 @@ class WolfManager
return constraints_.at(i);
}
- std::queue<StateBase*> getStateUnitsPtrs(unsigned int i)
+ std::queue<StateBlock*> getStateUnitsPtrs(unsigned int i)
{
- return std::queue<StateBase*>({frames_.at(i)->getPPtr(),frames_.at(i)->getOPtr()});
+ return std::queue<StateBlock*>({frames_.at(i)->getPPtr(),frames_.at(i)->getOPtr()});
}
};
@@ -561,11 +561,11 @@ class CeresManager
constraint_list_.push_back(std::pair<ceres::ResidualBlockId, ConstraintXBase*>(blockIdx,_corr_ptr));
}
- void addStateUnits(std::queue<StateBase*>& _new_state_units)
+ void addStateUnits(std::queue<StateBlock*>& _new_state_units)
{
while (!_new_state_units.empty())
{
- addStateUnit(_new_state_units.front());
+ addStateBlock(_new_state_units.front());
_new_state_units.pop();
}
}
@@ -575,12 +575,12 @@ class CeresManager
ceres_problem_->RemoveParameterBlock(_st_ptr);
}
- void addStateUnit(StateBase* _st_ptr)
+ void addStateBlock(StateBlock* _st_ptr)
{
//std::cout << "Adding a State Unit to wolf_problem... " << std::endl;
//_st_ptr->print();
- switch (_st_ptr->getStateType())
+ switch (_st_ptr->getType())
{
case ST_COMPLEX_ANGLE:
{
@@ -605,13 +605,13 @@ class CeresManager
case ST_POINT_2D:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBase*)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlock*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_POINT_3D:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBase*)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlock*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
default:
@@ -734,7 +734,7 @@ int main(int argc, char** argv)
Eigen::VectorXs odom_trajectory(n_execution*3); //all true poses
Eigen::VectorXs odom_readings(n_execution*2); // all odometry readings
Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings
- std::queue<StateBase*> new_state_units; // new state units in wolf that must be added to ceres
+ std::queue<StateBlock*> new_state_units; // new state units in wolf that must be added to ceres
std::queue<ConstraintXBase*> new_constraints; // new constraints in wolf that must be added to ceres
SensorBase* odom_sensor = new SensorBase(ODOM_2D, Eigen::MatrixXs::Zero(6,1),0);
SensorBase* gps_sensor = new SensorBase(GPS_FIX, Eigen::MatrixXs::Zero(6,1),0);
diff --git a/src/examples/test_ceres_manager_tree.cpp b/src/examples/test_ceres_manager_tree.cpp
index d33a4bf4b9e870b13e6d303047147442860b0abd..9773203764a742873075e2c515b0566915595647 100644
--- a/src/examples/test_ceres_manager_tree.cpp
+++ b/src/examples/test_ceres_manager_tree.cpp
@@ -86,15 +86,15 @@ class WolfManager
if (use_complex_angles_)
{
FrameBase* new_frame = new FrameBase(_time_stamp,
- new StateBase(state_.data()+first_empty_state_),
+ new StateBlock(state_.data()+first_empty_state_),
new StateComplexAngle(state_.data()+first_empty_state_+2));
trajectory_->addFrame(new_frame);
}
else
{
FrameBase* new_frame = new FrameBase(_time_stamp,
- new StateBase(state_.data()+first_empty_state_),
- new StateBase(state_.data()+first_empty_state_+2));
+ new StateBlock(state_.data()+first_empty_state_),
+ new StateBlock(state_.data()+first_empty_state_+2));
trajectory_->addFrame(new_frame);
}
@@ -107,7 +107,7 @@ class WolfManager
new_captures_.push(_capture);
}
- void update(std::list<StateBase*>& new_state_units, std::list<ConstraintBase*>& new_constraints)
+ void update(std::list<StateBlock*>& new_state_units, std::list<ConstraintBase*>& new_constraints)
{
// TODO: management due to time stamps
while (!new_captures_.empty())
@@ -159,9 +159,9 @@ class WolfManager
return state_;
}
- std::list<StateBase*> getStateList()
+ std::list<StateBlock*> getStateList()
{
- std::list<StateBase*> st_list;
+ std::list<StateBlock*> st_list;
for (FrameBaseIter frame_list_iter=trajectory_->getFrameListPtr()->begin(); frame_list_iter!=trajectory_->getFrameListPtr()->end(); frame_list_iter++)
{
@@ -250,7 +250,7 @@ int main(int argc, char** argv)
Eigen::VectorXs odom_trajectory(n_execution*3); //all true poses
Eigen::VectorXs odom_readings(n_execution*2); // all odometry readings
Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings
- std::list<StateBase*> new_state_units; // new state units in wolf that must be added to ceres
+ std::list<StateBlock*> new_state_units; // new state units in wolf that must be added to ceres
std::list<ConstraintBase*> new_constraints; // new constraints in wolf that must be added to ceres
// Wolf manager initialization
diff --git a/src/examples/test_ceres_wrapper_jet.cpp b/src/examples/test_ceres_wrapper_jet.cpp
index cf8dd52d54696496dc4e45d39df8d0af1f40ec2c..e2dbd1e85efe6f44040c19b1cffd6074b0966dcd 100644
--- a/src/examples/test_ceres_wrapper_jet.cpp
+++ b/src/examples/test_ceres_wrapper_jet.cpp
@@ -21,7 +21,7 @@
#include "sensor_base.h"
#include "frame_base.h"
#include "capture_base.h"
-#include "state_base.h"
+#include "state_block.h"
#include "wolf.h"
// ceres wrapper includes
diff --git a/src/examples/test_iQR_wolf2.cpp b/src/examples/test_iQR_wolf2.cpp
index 05ecf64df6c9989c6385546a2d6f4ef233d6472f..17db520b4767a2239a9d8a58508d85e716b06949 100644
--- a/src/examples/test_iQR_wolf2.cpp
+++ b/src/examples/test_iQR_wolf2.cpp
@@ -17,7 +17,7 @@
#include <queue>
//Wolf includes
-#include "state_base.h"
+#include "state_block.h"
#include "constraint_base.h"
#include "wolf_manager.h"
@@ -113,8 +113,7 @@ int main(int argc, char *argv[])
unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
// INITIALIZATION ============================================================================================
- // Problems
- SolverQR solver_;
+
//init random generators
WolfScalar odom_std_factor = 0.1;
@@ -122,20 +121,6 @@ int main(int argc, char *argv[])
std::default_random_engine generator(1);
std::normal_distribution<WolfScalar> gaussian_distribution(0.0, 1);
- // Ceres initialization
- 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;
- ceres::Problem::Options problem_options;
- problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
- problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
- problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
- CeresManager* ceres_manager = new CeresManager(problem_options);
- std::ofstream log_file, landmark_file; //output file
-
// Faramotics stuff
Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
vector < Cpose3d > devicePoses;
@@ -176,18 +161,35 @@ int main(int argc, char *argv[])
Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
laser_1_pose << 1.2, 0, 0, 0; //laser 1
laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
- SensorOdom2D odom_sensor(new StateBase(odom_pose.data()), new StateBase(&odom_pose(2)), odom_std_factor, odom_std_factor);
- SensorGPSFix gps_sensor(new StateBase(gps_pose.data()), new StateBase(&gps_pose(2)), gps_std);
- SensorLaser2D laser_1_sensor(new StateBase(laser_1_pose.data()), new StateBase(&laser_1_pose(3)));
- SensorLaser2D laser_2_sensor(new StateBase(laser_2_pose.data()), new StateBase(&laser_2_pose(3)));
+ SensorOdom2D odom_sensor(new StateBlock(odom_pose.head(2)), new StateBlock(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
+ SensorGPSFix gps_sensor(new StateBlock(gps_pose.head(2)), new StateBlock(gps_pose.tail(1)), gps_std);
+ SensorLaser2D laser_1_sensor(new StateBlock(laser_1_pose.head(2)), new StateBlock(laser_1_pose.tail(1)));
+ SensorLaser2D laser_2_sensor(new StateBlock(laser_2_pose.head(2)), new StateBlock(laser_2_pose.tail(1)));
// Initial pose
pose_odom << 2, 8, 0;
ground_truth.head(3) = pose_odom;
odom_trajectory.head(3) = pose_odom;
- WolfManager<StateBase, StateBase>* wolf_manager_QR = new WolfManager<StateBase, StateBase>(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
- WolfManager<StateBase, StateBase>* wolf_manager_ceres = new WolfManager<StateBase, StateBase>(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
+ WolfManager* wolf_manager_QR = new WolfManager(PO_2D, 1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
+ WolfManager* wolf_manager_ceres = new WolfManager(PO_2D, 1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
+
+ // Ceres initialization
+ 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;
+ ceres::Problem::Options problem_options;
+ problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+ problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+ problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+ CeresManager* ceres_manager = new CeresManager(wolf_manager_ceres->getProblemPtr(), problem_options);
+ std::ofstream log_file, landmark_file; //output file
+
+ // Own solver
+ SolverQR solver_(wolf_manager_QR->getProblemPtr());
std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
@@ -257,8 +259,8 @@ int main(int argc, char *argv[])
// UPDATING SOLVER ---------------------------
//std::cout << "UPDATING..." << std::endl;
// update state units and constraints in ceres
- solver_.update(wolf_manager_QR->getProblemPtr());
- ceres_manager->update(wolf_manager_ceres->getProblemPtr());
+ solver_.update();
+ ceres_manager->update();
// PRINT PROBLEM
//solver_.printProblem();
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index 351e045e712fb47b4fd6580f1c7144849d60ea59..f3d31e1678df77dcadc97e8f301a3bd416bbf69b 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -18,11 +18,42 @@ FeatureBase::FeatureBase(const Eigen::VectorXs& _measurement, const Eigen::Matri
FeatureBase::~FeatureBase()
{
//std::cout << "deleting FeatureBase " << nodeId() << std::endl;
+ is_deleting_ = true;
+
+ while (!constraint_to_list_.empty())
+ {
+ //std::cout << "deleting constraint " << (*constraints_list_.begin())->nodeId() << std::endl;
+ constraint_to_list_.front()->destruct();
+ //std::cout << "deleted " << std::endl;
+ }
+ //std::cout << "constraints deleted" << std::endl;
}
-void FeatureBase::addConstraint(ConstraintBase* _co_ptr)
+void FeatureBase::addConstraintFrom(ConstraintBase* _co_ptr)
{
addDownNode(_co_ptr);
+ // add constraint to be added in solver
+ getTop()->addConstraintPtr(_co_ptr);
+}
+
+void FeatureBase::addConstraintTo(ConstraintBase* _ctr_ptr)
+{
+ constraint_to_list_.push_back(_ctr_ptr);
+}
+
+void FeatureBase::removeConstraintTo(ConstraintBase* _ctr_ptr)
+{
+ constraint_to_list_.remove(_ctr_ptr);
+}
+
+unsigned int FeatureBase::getHits() const
+{
+ return constraint_to_list_.size();
+}
+
+std::list<ConstraintBase*>* FeatureBase::getConstraintToListPtr()
+{
+ return &constraint_to_list_;
}
CaptureBase* FeatureBase::getCapturePtr() const
@@ -35,14 +66,14 @@ FrameBase* FeatureBase::getFramePtr() const
return upperNodePtr()->upperNodePtr();
}
-ConstraintBaseList* FeatureBase::getConstraintListPtr()
+ConstraintBaseList* FeatureBase::getConstraintFromListPtr()
{
return getDownNodeListPtr();
}
-void FeatureBase::getConstraintList(ConstraintBaseList & _ctr_list)
+void FeatureBase::getConstraintFromList(ConstraintBaseList & _ctr_list)
{
- for(auto c_it = getConstraintListPtr()->begin(); c_it != getConstraintListPtr()->end(); ++c_it)
+ for(auto c_it = getConstraintFromListPtr()->begin(); c_it != getConstraintFromListPtr()->end(); ++c_it)
_ctr_list.push_back((*c_it));
}
diff --git a/src/feature_base.h b/src/feature_base.h
index ae5db87ddb03fe6c52902e3dcbb51d4192d6251d..d541679b23e05efc21e53b2cf74f1902bd5b0398 100644
--- a/src/feature_base.h
+++ b/src/feature_base.h
@@ -17,8 +17,9 @@ class ConstraintBase;
class FeatureBase : public NodeLinked<CaptureBase,ConstraintBase>
{
protected:
- Eigen::VectorXs measurement_;
- Eigen::MatrixXs measurement_covariance_; ///< Noise of the measurement
+ Eigen::VectorXs measurement_; ///< Reference to the measurement
+ Eigen::MatrixXs measurement_covariance_; ///< Reference to the measurement covariance
+ std::list<ConstraintBase*> constraint_to_list_; ///< List of constraints linked TO this feature
public:
/** \brief Constructor from capture pointer and measure dim
@@ -36,21 +37,70 @@ class FeatureBase : public NodeLinked<CaptureBase,ConstraintBase>
*/
FeatureBase(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
+ /** \brief Destructor
+ *
+ * Destructor
+ *
+ */
virtual ~FeatureBase();
-
- void addConstraint(ConstraintBase* _co_ptr);
+ /** \brief Adds a constraint from this feature (as a down node)
+ *
+ * Adds a constraint from this feature (as a down node)
+ *
+ */
+ void addConstraintFrom(ConstraintBase* _co_ptr);
+
+ /** \brief Adds a constraint to this feature (down node from other feature)
+ *
+ * Adds a constraint to this feature (down node from other feature)
+ *
+ */
+ void addConstraintTo(ConstraintBase* _co_ptr);
+
+ /** \brief Remove a constraint to this feature
+ *
+ * Remove a constraint to this feature
+ *
+ **/
+ void removeConstraintTo(ConstraintBase* _ctr_ptr);
+
+ /** \brief Gets the number of constraints linked with this frame
+ *
+ * Gets the number of constraints linked with this frame
+ *
+ **/
+ unsigned int getHits() const;
+
+ /** \brief Gets the list of constraints linked with this frame
+ *
+ * Gets the list of constraints linked with this frame
+ *
+ **/
+ std::list<ConstraintBase*>* getConstraintToListPtr();
+
+ /** \brief Gets the capture pointer
+ *
+ * Gets the capture pointer
+ *
+ */
CaptureBase* getCapturePtr() const;
+ /** \brief Gets the frame pointer
+ *
+ * Gets the frame pointer
+ *
+ */
FrameBase* getFramePtr() const;
-
- ConstraintBaseList* getConstraintListPtr();
-
- void getConstraintList(ConstraintBaseList & _ctr_list);
-// Eigen::VectorXs * getMeasurementPtr();
-//
-// Eigen::MatrixXs * getMeasurementCovariancePtr();
+ /** \brief Gets the constraint list (down nodes) pointer
+ *
+ * Gets the constraint list (down nodes) pointer
+ *
+ */
+ ConstraintBaseList* getConstraintFromListPtr();
+
+ void getConstraintFromList(ConstraintBaseList & _ctr_list);
const Eigen::VectorXs & getMeasurement() const;
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 30be6810a386522b4d74981cad6c36500391ef27..fd3664e8675abc13517a14009e63a48b4f960849 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -1,30 +1,24 @@
#include "frame_base.h"
-FrameBase::FrameBase(const TimeStamp& _ts, StateBase* _p_ptr, StateBase* _o_ptr, StateBase* _v_ptr, StateBase* _w_ptr) :
- //NodeLinked(MID, "FRAME", _traj_ptr),
+FrameBase::FrameBase(const TimeStamp& _ts, StateBlock* _p_ptr, StateBlock* _o_ptr) :
NodeLinked(MID, "FRAME"),
type_(REGULAR_FRAME),
time_stamp_(_ts),
status_(ST_ESTIMATED),
p_ptr_(_p_ptr),
- o_ptr_(_o_ptr),
- v_ptr_(_v_ptr),
- w_ptr_(_w_ptr)
+ o_ptr_(_o_ptr)
{
- //
+ //
}
-FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBase* _p_ptr, StateBase* _o_ptr, StateBase* _v_ptr, StateBase* _w_ptr) :
- //NodeLinked(MID, "FRAME", _traj_ptr),
+FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlock* _p_ptr, StateBlock* _o_ptr) :
NodeLinked(MID, "FRAME"),
type_(_tp),
time_stamp_(_ts),
status_(ST_ESTIMATED),
p_ptr_(_p_ptr),
- o_ptr_(_o_ptr),
- v_ptr_(_v_ptr),
- w_ptr_(_w_ptr)
+ o_ptr_(_o_ptr)
{
//
}
@@ -32,59 +26,114 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBase* _p_
FrameBase::~FrameBase()
{
//std::cout << "deleting FrameBase " << nodeId() << std::endl;
+ is_deleting_ = true;
- // Remove Frame State Units
+ // Remove Frame State Blocks
if (p_ptr_ != nullptr)
- getTop()->removeState(p_ptr_);
+ {
+ getTop()->removeStateBlockPtr(p_ptr_);
+ delete p_ptr_;
+ }
if (o_ptr_ != nullptr)
- getTop()->removeState(o_ptr_);
- if (v_ptr_ != nullptr)
- getTop()->removeState(v_ptr_);
- if (w_ptr_ != nullptr)
- getTop()->removeState(w_ptr_);
+ {
+ getTop()->removeStateBlockPtr(o_ptr_);
+ delete o_ptr_;
+ }
+ //std::cout << "states deleted" << std::endl;
+
+
+ while (!constraint_to_list_.empty())
+ {
+ //std::cout << "deleting constraint " << (*constraints_list_.begin())->nodeId() << std::endl;
+ constraint_to_list_.front()->destruct();
+ //std::cout << "deleted " << std::endl;
+ }
+ //std::cout << "constraints deleted" << std::endl;
}
-bool FrameBase::isKey() const
+void FrameBase::destruct()
{
- if ( type_ == KEY_FRAME ) return true;
- else return false;
+ if (!is_deleting_)
+ up_node_ptr_->removeDownNode(this);
}
-void FrameBase::setType(FrameType _ft)
+void FrameBase::addConstraintTo(ConstraintBase* _ctr_ptr)
{
- type_ = _ft;
+ constraint_to_list_.push_back(_ctr_ptr);
}
-void FrameBase::fix()
+void FrameBase::removeConstraintTo(ConstraintBase* _ctr_ptr)
{
- //std::cout << "Fixing frame " << nodeId() << std::endl;
- status_ = ST_FIXED;
+ constraint_to_list_.remove(_ctr_ptr);
- // Frame State Units
- if (p_ptr_!=nullptr)
- p_ptr_->setStateStatus(ST_FIXED);
- if (o_ptr_!=nullptr)
- o_ptr_->setStateStatus(ST_FIXED);
- if (v_ptr_!=nullptr)
- v_ptr_->setStateStatus(ST_FIXED);
- if (w_ptr_!=nullptr)
- w_ptr_->setStateStatus(ST_FIXED);
+ if (constraint_to_list_.empty())
+ this->destruct();
+}
+
+unsigned int FrameBase::getHits() const
+{
+ return constraint_to_list_.size();
+}
+
+std::list<ConstraintBase*>* FrameBase::getConstraintToListPtr()
+{
+ return &constraint_to_list_;
+}
+
+void FrameBase::setStatus(StateStatus _st)
+{
+ status_ = _st;
+
+ // State Blocks
+ if (status_ == ST_FIXED)
+ {
+ if (p_ptr_!=nullptr)
+ {
+ p_ptr_->fix();
+ getTop()->updateStateBlockPtr(p_ptr_);
+ }
+ if (o_ptr_!=nullptr)
+ {
+ o_ptr_->fix();
+ getTop()->updateStateBlockPtr(o_ptr_);
+ }
+ }
+ else if(status_ == ST_ESTIMATED)
+ {
+ if (p_ptr_!=nullptr)
+ {
+ p_ptr_->unfix();
+ getTop()->updateStateBlockPtr(p_ptr_);
+ }
+ if (o_ptr_!=nullptr)
+ {
+ o_ptr_->unfix();
+ getTop()->updateStateBlockPtr(o_ptr_);
+ }
+ }
+}
+
+void FrameBase::fix()
+{
+ //std::cout << "Fixing frame " << nodeId() << std::endl;
+ this->setStatus(ST_FIXED);
}
void FrameBase::unfix()
{
- //std::cout << "Unfixing frame " << nodeId() << std::endl;
- status_ = ST_ESTIMATED;
+ //std::cout << "Unfixing frame " << nodeId() << std::endl;
+ this->setStatus(ST_ESTIMATED);
+}
- // Frame State Units
- if (p_ptr_!=nullptr)
- p_ptr_->setStateStatus(ST_ESTIMATED);
- if (o_ptr_!=nullptr)
- o_ptr_->setStateStatus(ST_ESTIMATED);
- if (v_ptr_!=nullptr)
- v_ptr_->setStateStatus(ST_ESTIMATED);
- if (w_ptr_!=nullptr)
- w_ptr_->setStateStatus(ST_ESTIMATED);
+bool FrameBase::isKey() const
+{
+ if ( type_ == KEY_FRAME ) return true;
+ else return false;
+}
+
+void FrameBase::setType(FrameType _ft)
+{
+ type_ = _ft;
}
void FrameBase::setTimeStamp(const TimeStamp & _ts)
@@ -102,11 +151,6 @@ void FrameBase::getTimeStamp(TimeStamp & _ts) const
_ts = time_stamp_.get();
}
-void FrameBase::setStatus(const StateStatus& _status)
-{
- status_ = _status;
-}
-
StateStatus FrameBase::getStatus() const
{
return status_;
@@ -115,23 +159,23 @@ StateStatus FrameBase::getStatus() const
void FrameBase::setState(const Eigen::VectorXs& _st)
{
- assert(_st.size() == ((p_ptr_==nullptr ? 0 : p_ptr_->getStateSize()) +
- (o_ptr_==nullptr ? 0 : o_ptr_->getStateSize()) +
- (v_ptr_==nullptr ? 0 : v_ptr_->getStateSize()) +
- (w_ptr_==nullptr ? 0 : w_ptr_->getStateSize())) &&
- "In FrameBase::setState wrong state size");
- assert(p_ptr_!=nullptr && "in FrameBase::setState(), p_ptr_ is nullptr");
-
- Eigen::Map<Eigen::VectorXs> state_map(p_ptr_->getPtr(), _st.size());
- //std::cout << "setting state\noriginal: " << state_map.transpose() << "\nnew: " << _st.transpose() << std::endl;
- state_map = _st;
- //std::cout << "setted state: " << *p_ptr_->getPtr() << " " << *(p_ptr_->getPtr()+1) << std::endl;
+ assert(_st.size() == ((p_ptr_==nullptr ? 0 : p_ptr_->getSize()) +
+ (o_ptr_==nullptr ? 0 : o_ptr_->getSize())) &&
+ "In FrameBase::setState wrong state size");
+
+ if (p_ptr_!=nullptr)
+ p_ptr_->setVector(_st.head(p_ptr_->getSize()));
+ if (o_ptr_!=nullptr)
+ o_ptr_->setVector(_st.segment((p_ptr_==nullptr ? 0 : p_ptr_->getSize()),
+ o_ptr_->getSize()));
}
-Eigen::Map<Eigen::VectorXs> FrameBase::getState() const
+Eigen::VectorXs FrameBase::getState() const
{
- return Eigen::Map<Eigen::VectorXs>(p_ptr_->getPtr(),
- p_ptr_->getStateSize() + o_ptr_->getStateSize() + v_ptr_->getStateSize() + w_ptr_->getStateSize());
+ Eigen::VectorXs state((p_ptr_==nullptr ? 0 : p_ptr_->getSize()) +
+ (o_ptr_==nullptr ? 0 : o_ptr_->getSize()));
+ state << p_ptr_->getVector(), o_ptr_->getVector();
+ return state;
}
void FrameBase::addCapture(CaptureBase* _capt_ptr)
@@ -198,26 +242,16 @@ FrameBase* FrameBase::getNextFrame() const
return nullptr;
}
-StateBase* FrameBase::getPPtr() const
+StateBlock* FrameBase::getPPtr() const
{
return p_ptr_;
}
-StateBase* FrameBase::getOPtr() const
+StateBlock* FrameBase::getOPtr() const
{
return o_ptr_;
}
-StateBase* FrameBase::getVPtr() const
-{
- return v_ptr_;
-}
-
-StateBase* FrameBase::getWPtr() const
-{
- return w_ptr_;
-}
-
CaptureBaseIter FrameBase::hasCaptureOf(const SensorBase* _sensor_ptr)
{
for (auto capture_it = getCaptureListPtr()->begin(); capture_it != getCaptureListPtr()->end(); capture_it++)
@@ -244,18 +278,4 @@ void FrameBase::printSelf(unsigned int _ntabs, std::ostream& _ost) const
printTabs(_ntabs);
o_ptr_->print(_ntabs,_ost);
}
- if (v_ptr_)
- {
- printTabs(_ntabs);
- _ost << "\tVelocity : \n";
- printTabs(_ntabs);
- v_ptr_->print(_ntabs,_ost);
- }
- if (w_ptr_)
- {
- printTabs(_ntabs);
- _ost << "\tAngular velocity : \n";
- printTabs(_ntabs);
- v_ptr_->print(_ntabs,_ost);
- }
}
diff --git a/src/frame_base.h b/src/frame_base.h
index fec673a1237ed3f720d838de1a8c7be337362f02..a17d93c86111cedcef91910bf6a4364eef040fae 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -18,48 +18,40 @@ class CaptureBase;
#include "node_linked.h"
#include "trajectory_base.h"
#include "capture_base.h"
-#include "state_base.h"
+#include "state_block.h"
//class FrameBase
class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
{
protected:
- FrameType type_; //type of frame. Either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h)
- TimeStamp time_stamp_; //frame time stamp
- StateStatus status_; // status of the estimation of the frame state
- StateBase* p_ptr_; // Position state unit pointer
- StateBase* o_ptr_; // Orientation state unit pointer
- StateBase* v_ptr_; // Velocity state unit pointer
- StateBase* w_ptr_; // Angular velocity state unit pointer
- //TODO: accelerations?
+ FrameType type_; ///< type of frame. Either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h)
+ TimeStamp time_stamp_; ///< frame time stamp
+ StateStatus status_; ///< status of the estimation of the frame state
+ StateBlock* p_ptr_; ///< Position state block pointer
+ StateBlock* o_ptr_; ///< Orientation state block pointer
+ std::list<ConstraintBase*> constraint_to_list_; ///> List of constraints TO this frame
public:
/** \brief Constructor with only time stamp
*
* Constructor with only time stamp
- * \param _traj_ptr a pointer to the Trajectory up node
- * \param _tp indicates frame type. Generally either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h)
- * \param _p_ptr StateBase pointer to the position (default: nullptr)
- * \param _o_ptr StateBase pointer to the orientation (default: nullptr)
- * \param _v_ptr StateBase pointer to the velocity (default: nullptr)
- * \param _w_ptr StateBase pointer to the angular velocity (default: nullptr)
+ * \param _ts is the time stamp associated to this frame, provided in seconds
+ * \param _p_ptr StateBlock pointer to the position (default: nullptr)
+ * \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
*
**/
- FrameBase(const TimeStamp& _ts, StateBase* _p_ptr, StateBase* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr); //ACM: Either remove all pointer arguments from this header, or merge both constructors in a single one
+ FrameBase(const TimeStamp& _ts, StateBlock* _p_ptr, StateBlock* _o_ptr = nullptr);
/** \brief Constructor with type, time stamp and state pointer
*
* Constructor with type, time stamp and state pointer
- * \param _traj_ptr a pointer to the Trajectory up node
* \param _tp indicates frame type. Generally either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h)
* \param _ts is the time stamp associated to this frame, provided in seconds
- * \param _p_ptr StateBase pointer to the position (default: nullptr)
- * \param _o_ptr StateBase pointer to the orientation (default: nullptr)
- * \param _v_ptr StateBase pointer to the velocity (default: nullptr)
- * \param _w_ptr StateBase pointer to the angular velocity (default: nullptr)
+ * \param _p_ptr StateBlock pointer to the position (default: nullptr)
+ * \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
*
**/
- FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBase* _p_ptr, StateBase* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
+ FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlock* _p_ptr, StateBlock* _o_ptr = nullptr);
/** \brief Destructor
*
@@ -67,6 +59,62 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
*
**/
virtual ~FrameBase();
+
+ /** \brief Destructor call if is not already deleting
+ *
+ * Destructor call if is not already deleting
+ *
+ */
+ virtual void destruct();
+
+ /** \brief Link with a constraint
+ *
+ * Link with a constraint
+ *
+ **/
+ void addConstraintTo(ConstraintBase* _ctr_ptr);
+
+ /** \brief Remove a constraint to this frame
+ *
+ * Remove a constraint to this frame
+ *
+ **/
+ void removeConstraintTo(ConstraintBase* _ctr_ptr);
+
+ /** \brief Gets the number of constraints linked with this frame
+ *
+ * Gets the number of constraints linked with this frame
+ *
+ **/
+ unsigned int getHits() const;
+
+ /** \brief Gets the list of constraints linked with this frame
+ *
+ * Gets the list of constraints linked with this frame
+ *
+ **/
+ std::list<ConstraintBase*>* getConstraintToListPtr();
+
+ /** \brief Sets the Frame status
+ *
+ * Sets the Frame status (see wolf.h)
+ *
+ **/
+ void setStatus(StateStatus _st);
+
+ /** \brief Sets all the states status to fixed
+ *
+ * Sets all the states status to fixed
+ *
+ **/
+ void fix();
+
+ /** \brief Sets all the states status to estimated
+ *
+ * Sets all the states status to estimated
+ *
+ **/
+ void unfix();
/** \brief Checks if this frame is KEY_FRAME
*
@@ -74,12 +122,14 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
*
**/
bool isKey() const;
+
+ /** \brief Sets the Frame type
+ *
+ * Sets the frame type (see wolf.h)
+ *
+ **/
void setType(FrameType _ft);
-
- void fix();
-
- void unfix();
void setTimeStamp(const TimeStamp& _ts); //ACM: if all constructors require a timestamp, do we need a set? Should we allow to change TS?
@@ -87,15 +137,13 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
void getTimeStamp(TimeStamp & _ts) const;
- void setStatus(const StateStatus& _status);
-
StateStatus getStatus() const;
void setState(const Eigen::VectorXs& _st);
- void addCapture(CaptureBase* _capt_ptr);
+ Eigen::VectorXs getState() const;
- Eigen::Map<Eigen::VectorXs> getState() const;
+ void addCapture(CaptureBase* _capt_ptr);
void removeCapture(CaptureBaseIter& _capt_ptr);
@@ -109,13 +157,9 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
FrameBase* getNextFrame() const;
- StateBase* getPPtr() const;
-
- StateBase* getOPtr() const;
-
- StateBase* getVPtr() const;
+ StateBlock* getPPtr() const;
- StateBase* getWPtr() const;
+ StateBlock* getOPtr() const;
const Eigen::Matrix4s * getTransformationMatrix() const; //ACM: Who owns this return matrix ?
diff --git a/src/hardware_base.cpp b/src/hardware_base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7dfe37fd15accad3004af7a829460b7d7091fa19
--- /dev/null
+++ b/src/hardware_base.cpp
@@ -0,0 +1,34 @@
+#include "hardware_base.h"
+
+HardwareBase::HardwareBase() :
+ NodeLinked(MID, "HARDWARE")
+{
+ //std::cout << "HardwareBase::HardwareBase(): " << __LINE__ << std::endl;
+}
+
+HardwareBase::~HardwareBase()
+{
+ //std::cout << "deleting HardwareBase " << nodeId() << std::endl;
+}
+
+void HardwareBase::addSensor(SensorBase* _sensor_ptr)
+{
+ std::cout << "adding sensor..." << std::endl;
+ addDownNode(_sensor_ptr);
+ std::cout << "added!" << std::endl;
+}
+
+void HardwareBase::removeSensor(const SensorBaseIter& _sensor_iter)
+{
+ removeDownNode(_sensor_iter);
+}
+
+void HardwareBase::removeSensor(SensorBase* _sensor_ptr)
+{
+ removeDownNode(_sensor_ptr->nodeId());
+}
+
+SensorBaseList* HardwareBase::getSensorListPtr()
+{
+ return getDownNodeListPtr();
+}
diff --git a/src/hardware_base.h b/src/hardware_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..eb12aabb3631f8bccc6680294fede3e467370bc1
--- /dev/null
+++ b/src/hardware_base.h
@@ -0,0 +1,59 @@
+
+#ifndef HARDWARE_BASE_H_
+#define HARDWARE_BASE_H_
+
+//Wolf includes
+#include "wolf.h"
+#include "wolf_problem.h"
+#include "node_linked.h"
+#include "node_terminus.h"
+#include "sensor_base.h"
+
+//class HardwareBase
+class HardwareBase : public NodeLinked<WolfProblem,SensorBase>
+{
+ public:
+ /** \brief Constructor
+ *
+ * Constructor
+ *
+ **/
+ HardwareBase();
+
+ /** \brief Destructor
+ *
+ * Destructor
+ *
+ **/
+ ~HardwareBase();
+
+ /** \brief Adds a sensor
+ *
+ * Adds a sensor
+ *
+ **/
+ virtual void addSensor(SensorBase* _sensor_ptr);
+
+ /** \brief Removes a sensor
+ *
+ * Removes a sensor
+ *
+ **/
+ void removeSensor(const SensorBaseIter& _sensor_iter);
+
+ /** \brief Removes a sensor
+ *
+ * Removes a sensor
+ *
+ **/
+ void removeSensor(SensorBase* _sensor_ptr);
+
+ /** \brief Returns Frame list
+ *
+ * Returns FrameBase list
+ *
+ **/
+ SensorBaseList* getSensorListPtr();
+
+};
+#endif
diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp
index 54608d9a3a90d7d83ec6d98e6aa049891e5e5145..42725137dffb8b7eed6eee99fd8c3bc0de2cd513 100644
--- a/src/landmark_base.cpp
+++ b/src/landmark_base.cpp
@@ -1,15 +1,13 @@
#include "landmark_base.h"
-LandmarkBase::LandmarkBase(const LandmarkType & _tp, StateBase* _p_ptr, StateBase* _o_ptr, StateBase* _v_ptr, StateBase* _w_ptr) :
+LandmarkBase::LandmarkBase(const LandmarkType & _tp, StateBlock* _p_ptr, StateBlock* _o_ptr) :
NodeLinked(MID, "LANDMARK"),
type_(_tp),
status_(LANDMARK_CANDIDATE),
p_ptr_(_p_ptr),
o_ptr_(_o_ptr),
- v_ptr_(_v_ptr),
- w_ptr_(_w_ptr),
- constraints_list_({})
+ constraint_to_list_({})
{
//
}
@@ -17,125 +15,122 @@ LandmarkBase::LandmarkBase(const LandmarkType & _tp, StateBase* _p_ptr, StateBas
LandmarkBase::~LandmarkBase()
{
//std::cout << "deleting LandmarkBase " << nodeId() << std::endl;
-
- // Remove Landmark State Units
- if (p_ptr_ != nullptr)
- getTop()->removeState(p_ptr_);
- if (o_ptr_ != nullptr)
- getTop()->removeState(o_ptr_);
- if (v_ptr_ != nullptr)
- getTop()->removeState(v_ptr_);
- if (w_ptr_ != nullptr)
- getTop()->removeState(w_ptr_);
-
- //std::cout << "state deleted" << std::endl;
-
- auto ctr_it = constraints_list_.begin();
- while (ctr_it != constraints_list_.end())
+ is_deleting_ = true;
+
+ // Remove Frame State Blocks
+ if (p_ptr_ != nullptr)
+ {
+ getTop()->removeStateBlockPtr(p_ptr_);
+ delete p_ptr_;
+ }
+ if (o_ptr_ != nullptr)
+ {
+ getTop()->removeStateBlockPtr(o_ptr_);
+ delete o_ptr_;
+ }
+ //std::cout << "states deleted" << std::endl;
+
+ while (!constraint_to_list_.empty())
{
- //std::cout << "deleting constraint " << (*ctr_it)->nodeId() << std::endl;
- (*ctr_it)->getFeaturePtr()->removeDownNode((*ctr_it));
+ //std::cout << "deleting constraint " << (*constraints_list_.begin())->nodeId() << std::endl;
+ constraint_to_list_.front()->destruct();
//std::cout << "deleted " << std::endl;
- ctr_it = constraints_list_.begin();
}
//std::cout << "constraints deleted" << std::endl;
}
-void LandmarkBase::setStatus(LandmarkStatus _st)
+void LandmarkBase::destruct()
{
- status_ = _st;
+ if (!is_deleting_)
+ up_node_ptr_->removeDownNode(this);
}
-void LandmarkBase::hit(ConstraintBase* _ctr_ptr)
+void LandmarkBase::addConstraintTo(ConstraintBase* _ctr_ptr)
{
- constraints_list_.push_back(_ctr_ptr);
+ constraint_to_list_.push_back(_ctr_ptr);
}
-void LandmarkBase::unhit(ConstraintBase* _ctr_ptr)
+void LandmarkBase::removeConstraintTo(ConstraintBase* _ctr_ptr)
{
- constraints_list_.remove(_ctr_ptr);
-}
+ constraint_to_list_.remove(_ctr_ptr);
-void LandmarkBase::fix()
-{
- //std::cout << "Fixing frame " << nodeId() << std::endl;
- status_ = LANDMARK_FIXED;
-
- // Frame State Units
- if (p_ptr_!=nullptr)
- p_ptr_->setStateStatus(ST_FIXED);
- if (o_ptr_!=nullptr)
- o_ptr_->setStateStatus(ST_FIXED);
- if (v_ptr_!=nullptr)
- v_ptr_->setStateStatus(ST_FIXED);
- if (w_ptr_!=nullptr)
- w_ptr_->setStateStatus(ST_FIXED);
-}
-
-void LandmarkBase::unfix()
-{
- //std::cout << "Unfixing frame " << nodeId() << std::endl;
- status_ = LANDMARK_ESTIMATED;
-
- // Frame State Units
- if (p_ptr_!=nullptr)
- p_ptr_->setStateStatus(ST_ESTIMATED);
- if (o_ptr_!=nullptr)
- o_ptr_->setStateStatus(ST_ESTIMATED);
- if (v_ptr_!=nullptr)
- v_ptr_->setStateStatus(ST_ESTIMATED);
- if (w_ptr_!=nullptr)
- w_ptr_->setStateStatus(ST_ESTIMATED);
+ if (constraint_to_list_.empty())
+ this->destruct();
}
unsigned int LandmarkBase::getHits() const
{
- return constraints_list_.size();
+ return constraint_to_list_.size();
}
-std::list<ConstraintBase*>* LandmarkBase::getConstraints()
+std::list<ConstraintBase*>* LandmarkBase::getConstraintToListPtr()
{
- return &constraints_list_;
+ return &constraint_to_list_;
}
-StateBase* LandmarkBase::getPPtr() const
+void LandmarkBase::setStatus(LandmarkStatus _st)
{
- return p_ptr_;
-}
+ status_ = _st;
-StateBase* LandmarkBase::getOPtr() const
-{
- return o_ptr_;
+ // State Blocks
+ if (status_ == LANDMARK_FIXED)
+ {
+ if (p_ptr_!=nullptr)
+ {
+ p_ptr_->fix();
+ getTop()->updateStateBlockPtr(p_ptr_);
+ }
+ if (o_ptr_!=nullptr)
+ {
+ o_ptr_->fix();
+ getTop()->updateStateBlockPtr(o_ptr_);
+ }
+ }
+ else if(status_ == LANDMARK_ESTIMATED)
+ {
+ if (p_ptr_!=nullptr)
+ {
+ p_ptr_->unfix();
+ getTop()->updateStateBlockPtr(p_ptr_);
+ }
+ if (o_ptr_!=nullptr)
+ {
+ o_ptr_->unfix();
+ getTop()->updateStateBlockPtr(o_ptr_);
+ }
+ }
}
-StateBase* LandmarkBase::getVPtr() const
+void LandmarkBase::fix()
{
- return v_ptr_;
+ //std::cout << "Fixing frame " << nodeId() << std::endl;
+ this->setStatus(LANDMARK_FIXED);
}
-StateBase* LandmarkBase::getWPtr() const
+void LandmarkBase::unfix()
{
- return w_ptr_;
+ //std::cout << "Unfixing frame " << nodeId() << std::endl;
+ this->setStatus(LANDMARK_ESTIMATED);
}
-void LandmarkBase::setPPtr(StateBase* _st_ptr)
+StateBlock* LandmarkBase::getPPtr() const
{
- p_ptr_ = _st_ptr;
+ return p_ptr_;
}
-void LandmarkBase::setOPtr(StateBase* _st_ptr)
+StateBlock* LandmarkBase::getOPtr() const
{
- o_ptr_ = _st_ptr;
+ return o_ptr_;
}
-void LandmarkBase::setVPtr(StateBase* _st_ptr)
+void LandmarkBase::setPPtr(StateBlock* _st_ptr)
{
- v_ptr_ = _st_ptr;
+ p_ptr_ = _st_ptr;
}
-void LandmarkBase::setWPtr(StateBase* _st_ptr)
+void LandmarkBase::setOPtr(StateBlock* _st_ptr)
{
- w_ptr_ = _st_ptr;
+ o_ptr_ = _st_ptr;
}
void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor)
@@ -157,38 +152,3 @@ const LandmarkType LandmarkBase::getType() const
{
return type_;
}
-
-//const StateBasePtr LandmarkBase::getStatePtr() const
-//{
-// return st_list_.front();
-//}
-
-//const StateBasePtrList* LandmarkBase::getStateListPtr() const
-//{
-// return &st_list_;
-//}
-
-//void LandmarkBase::printSelf(unsigned int _ntabs, std::ostream& _ost) const
-//{
-// NodeLinked::printSelf(_ntabs, _ost);
-// if (p_ptr_.get() != nullptr)
-// {
-// _ost << "\tPosition : \n";
-// p_ptr_->printSelf(_ntabs,_ost);
-// }
-// if (o_ptr_.get() != nullptr)
-// {
-// _ost << "\tOrientation : \n";
-// o_ptr_->printSelf(_ntabs,_ost);
-// }
-// if (v_ptr_.get() != nullptr)
-// {
-// _ost << "\tVelocity : \n";
-// v_ptr_->printSelf(_ntabs,_ost);
-// }
-// if (w_ptr_.get() != nullptr)
-// {
-// _ost << "\tAngular velocity : \n";
-// v_ptr_->printSelf(_ntabs,_ost);
-// }
-//}
diff --git a/src/landmark_base.h b/src/landmark_base.h
index 39c7ead8d9ac93db92b2ae892da6a3f80f6f1862..5f7527dc4a634062afee4746cdf7d7612501b223 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -16,30 +16,23 @@ class NodeTerminus;
#include "time_stamp.h"
#include "node_linked.h"
#include "map_base.h"
-#include "state_base.h"
+#include "state_block.h"
#include "constraint_base.h"
-// why v, w and a ?
-// add descriptor as a StateBase -> Could be estimated or not. Aperture could be one case of "descriptor"that can be estimated or not
-// orientation ? -> Is it also a descriptor ?
-// init and end Time stamps
-//
+// TODO: add descriptor as a StateBase -> Could be estimated or not. Aperture could be one case of "descriptor"that can be estimated or not
+// TODO: init and end Time stamps
//class LandmarkBase
class LandmarkBase : public NodeLinked<MapBase, NodeTerminus>
{
protected:
- LandmarkType type_; //type of landmark. (types defined at wolf.h)
- LandmarkStatus status_; //status of the landmark. (types defined at wolf.h)
- TimeStamp stamp_; // stamp of the creation of the landmark (and stamp of destruction when status is LANDMARK_OLD)
- //StateBaseList st_list_; //List of pointers to the state corresponding to the landmark estimation
- StateBase* p_ptr_; // Position state unit pointer
- StateBase* o_ptr_; // Orientation state unit pointer
- StateBase* v_ptr_; // Velocity state unit pointer
- StateBase* w_ptr_; // Angular velocity state unit pointer
- //TODO: accelerations?
+ LandmarkType type_; ///< type of landmark. (types defined at wolf.h)
+ LandmarkStatus status_; ///< status of the landmark. (types defined at wolf.h)
+ TimeStamp stamp_; ///< stamp of the creation of the landmark (and stamp of destruction when status is LANDMARK_OLD)
+ StateBlock* p_ptr_; ///< Position state unit pointer
+ StateBlock* o_ptr_; ///< Orientation state unit pointer
Eigen::VectorXs descriptor_; //TODO: agree?
- std::list<ConstraintBase*> constraints_list_;
+ std::list<ConstraintBase*> constraint_to_list_; ///< List of constraints linked to this landmark
public:
/** \brief Constructor with type, time stamp and the position state pointer
@@ -48,20 +41,10 @@ class LandmarkBase : public NodeLinked<MapBase, NodeTerminus>
* \param _tp indicates landmark type.(types defined at wolf.h)
* \param _p_ptr StateBase pointer to the position
* \param _o_ptr StateBase pointer to the orientation (default: nullptr)
- * \param _v_ptr StateBase pointer to the velocity (default: nullptr)
- * \param _w_ptr StateBase pointer to the angular velocity (default: nullptr)
*
**/
- LandmarkBase(const LandmarkType & _tp, StateBase* _p_ptr, StateBase* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
+ LandmarkBase(const LandmarkType & _tp, StateBlock* _p_ptr, StateBlock* _o_ptr = nullptr);
- /** \brief Constructor with type, time stamp and state list
- *
- * Constructor with type and state pointer list
- * \param _tp indicates frame type. Generally either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h)
- * \param _stp_list StateBase list of the landmark estimation
- *
- **/
- //LandmarkBase(const LandmarkType & _tp, const StateBaseList& _stp_list);
/** \brief Destructor
*
* Destructor
@@ -69,38 +52,102 @@ class LandmarkBase : public NodeLinked<MapBase, NodeTerminus>
**/
virtual ~LandmarkBase();
- void setStatus(LandmarkStatus _st);
-
- void hit(ConstraintBase* _ctr_ptr);
-
- void unhit(ConstraintBase* _ctr_ptr);
+ /** \brief Destructor call if is not already deleting
+ *
+ * Destructor call if is not already deleting
+ *
+ */
+ virtual void destruct();
- void fix();
+ /** \brief Link with a constraint
+ *
+ * Link with a constraint
+ *
+ **/
+ void addConstraintTo(ConstraintBase* _ctr_ptr);
- void unfix();
+ /** \brief Remove a constraint to this landmark
+ *
+ * Remove a constraint to this landmark
+ *
+ **/
+ void removeConstraintTo(ConstraintBase* _ctr_ptr);
+ /** \brief Gets the number of constraints linked with this landmark
+ *
+ * Gets the number of constraints linked with this landmark
+ *
+ **/
unsigned int getHits() const;
- std::list<ConstraintBase*>* getConstraints();
-
- StateBase* getPPtr() const;
+ /** \brief Gets the list of constraints linked with this landmark
+ *
+ * Gets the list of constraints linked with this landmark
+ *
+ **/
+ std::list<ConstraintBase*>* getConstraintToListPtr();
- StateBase* getOPtr() const;
+ /** \brief Sets the Landmark status
+ *
+ * Sets the Landmark status (see wolf.h)
+ *
+ **/
+ void setStatus(LandmarkStatus _st);
- StateBase* getVPtr() const;
+ /** \brief Sets the Landmark status to fixed
+ *
+ * Sets the Landmark status to fixed
+ *
+ **/
+ void fix();
- StateBase* getWPtr() const;
+ /** \brief Sets the Landmark status to estimated
+ *
+ * Sets the Landmark status to estimated
+ *
+ **/
+ void unfix();
- void setPPtr(StateBase* _st_ptr);
+ /** \brief Gets the position state block pointer
+ *
+ * Gets the position state block pointer
+ *
+ **/
+ StateBlock* getPPtr() const;
- void setOPtr(StateBase* _st_ptr);
+ /** \brief Gets the orientation state block pointer
+ *
+ * Gets the orientation state block pointer
+ *
+ **/
+ StateBlock* getOPtr() const;
- void setVPtr(StateBase* _st_ptr);
+ /** \brief Sets the position state block pointer
+ *
+ * Sets the position state block pointer
+ *
+ **/
+ void setPPtr(StateBlock* _st_ptr);
- void setWPtr(StateBase* _st_ptr);
+ /** \brief Sets the orientation state block pointer
+ *
+ * Sets the orientation state block pointer
+ *
+ **/
+ void setOPtr(StateBlock* _st_ptr);
+ /** \brief Sets the descriptor
+ *
+ * Sets the descriptor
+ *
+ **/
void setDescriptor(const Eigen::VectorXs& _descriptor);
-
+
+ /** \brief Gets the descriptor
+ *
+ * Gets the descriptor
+ *
+ **/
const Eigen::VectorXs& getDescriptor() const;
/** \brief Returns _ii component of descriptor vector
@@ -111,10 +158,11 @@ class LandmarkBase : public NodeLinked<MapBase, NodeTerminus>
**/
WolfScalar getDescriptor(unsigned int _ii) const;
+ /** \brief Return the type of the landmark
+ *
+ * Return the type of the landmark (see wolf.h)
+ *
+ **/
const LandmarkType getType() const;
-
- //const StateBase* getStatePtr() const;
-
- //const StateBaseList* getStateListPtr() const;
};
#endif
diff --git a/src/landmark_container.cpp b/src/landmark_container.cpp
index d484500012449e2e15ebfdce5dd749c767aa6e68..e39b62cbc0d9abd6faa1dde698a76f73a728d99c 100644
--- a/src/landmark_container.cpp
+++ b/src/landmark_container.cpp
@@ -1,7 +1,7 @@
#include "landmark_container.h"
-LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _witdh, const WolfScalar& _length) :
+LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _witdh, const WolfScalar& _length) :
LandmarkBase(LANDMARK_CONTAINER, _p_ptr, _o_ptr),
corners_(3,4)
{
@@ -14,7 +14,7 @@ LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, const
0, M_PI/2, M_PI, -M_PI/2;
}
-LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const WolfScalar& _witdh, const WolfScalar& _length) :
+LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const WolfScalar& _witdh, const WolfScalar& _length) :
LandmarkBase(LANDMARK_CONTAINER, _p_ptr, _o_ptr),
corners_(3,4)
{
@@ -29,7 +29,7 @@ LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, const
// Computing center
std::cout << "Container constructor: Computing center pose... " << std::endl;
Eigen::Map<Eigen::Vector2s> container_position(_p_ptr->getPtr());
- Eigen::Map<Eigen::VectorXs> container_orientation(_o_ptr->getPtr(), _o_ptr->getStateSize());
+ Eigen::Map<Eigen::VectorXs> container_orientation(_o_ptr->getPtr(), _o_ptr->getSize());
std::cout << "Container constructor: _corner_1_idx " << _corner_1_idx << "_corner_2_idx " << _corner_2_idx << std::endl;
@@ -75,7 +75,7 @@ LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, const
std::cout << "Container center pose... " << container_position.transpose() << " " << container_orientation.transpose() << std::endl;
}
-LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const WolfScalar& _witdh, const WolfScalar& _length) :
+LandmarkContainer::LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const WolfScalar& _witdh, const WolfScalar& _length) :
LandmarkBase(LANDMARK_CONTAINER, _p_ptr, _o_ptr),
corners_(3,4)
{
@@ -92,7 +92,7 @@ LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, Landm
// Computing center
//std::cout << "Container constructor: Computing center position... " << std::endl;
Eigen::Map<Eigen::Vector2s> container_position(_p_ptr->getPtr());
- Eigen::Map<Eigen::VectorXs> container_orientation(_o_ptr->getPtr(), _o_ptr->getStateSize());
+ Eigen::Map<Eigen::VectorXs> container_orientation(_o_ptr->getPtr(), _o_ptr->getSize());
container_position = Eigen::Vector2s::Zero();
diff --git a/src/landmark_container.h b/src/landmark_container.h
index 09c5cc448211c2adfb98413a634283c3733c4fd2..a85a6b7b3e631a34a0f694c7fe2d456238e3a29d 100644
--- a/src/landmark_container.h
+++ b/src/landmark_container.h
@@ -30,7 +30,7 @@ class LandmarkContainer : public LandmarkBase
* \param _length descriptor of the landmark: container length
*
**/
- LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
+ LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
/** \brief Constructor with type, time stamp and the position state pointer
*
@@ -45,7 +45,7 @@ class LandmarkContainer : public LandmarkBase
* \param _length descriptor of the landmark: container length
*
**/
- LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
+ LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
/** \brief Constructor with type, time stamp and the position state pointer
*
@@ -58,7 +58,7 @@ class LandmarkContainer : public LandmarkBase
* \param _length descriptor of the landmark: container length
*
**/
- LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
+ LandmarkContainer(StateBlock* _p_ptr, StateBlock* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
/** \brief Destructor
*
diff --git a/src/landmark_corner_2D.cpp b/src/landmark_corner_2D.cpp
index 176effa1820cfb32c61f20a432e6910981322c93..ca4b6c85bb02701f2aed9cbf1210e6e49ddd69a4 100644
--- a/src/landmark_corner_2D.cpp
+++ b/src/landmark_corner_2D.cpp
@@ -1,7 +1,7 @@
#include "landmark_corner_2D.h"
-LandmarkCorner2D::LandmarkCorner2D(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _aperture) :
+LandmarkCorner2D::LandmarkCorner2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _aperture) :
LandmarkBase(LANDMARK_CORNER, _p_ptr, _o_ptr)
{
setDescriptor(Eigen::VectorXs::Constant(1,_aperture));
diff --git a/src/landmark_corner_2D.h b/src/landmark_corner_2D.h
index cb6645840316f6aa803928c5cb8987a637d2836b..aba3f4922b55b25669f2cc9d113ab774808a2249 100644
--- a/src/landmark_corner_2D.h
+++ b/src/landmark_corner_2D.h
@@ -28,7 +28,7 @@ class LandmarkCorner2D : public LandmarkBase
* \param _aperture descriptor of the landmark: aperture of the corner
*
**/
- LandmarkCorner2D(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _aperture=0);
+ LandmarkCorner2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _aperture=0);
/** \brief Destructor
*
diff --git a/src/map_base.cpp b/src/map_base.cpp
index fc2ec1e6bc3cb666773f6def7cd9e863e16bcb41..090be46328264002d009411b93bddf257cfffaa5 100644
--- a/src/map_base.cpp
+++ b/src/map_base.cpp
@@ -14,6 +14,11 @@ MapBase::~MapBase()
void MapBase::addLandmark(LandmarkBase* _landmark_ptr)
{
addDownNode(_landmark_ptr);
+
+ if (_landmark_ptr->getPPtr() != nullptr)
+ getTop()->addStateBlockPtr(_landmark_ptr->getPPtr());
+ if (_landmark_ptr->getOPtr() != nullptr)
+ getTop()->addStateBlockPtr(_landmark_ptr->getOPtr());
}
void MapBase::removeLandmark(const LandmarkBaseIter& _landmark_iter)
diff --git a/src/node_base.cpp b/src/node_base.cpp
index 685d67bebbaf5d8cefc749ac758534b8dd875b3d..d8fd39c6e3f3fc7e6b9d00c93927e29c76e1f2a8 100644
--- a/src/node_base.cpp
+++ b/src/node_base.cpp
@@ -6,7 +6,7 @@ unsigned int NodeBase::node_id_count_ = 0;
NodeBase::NodeBase(std::string _label, bool _verbose) :
label_(_label),
node_id_(++node_id_count_),
- node_pending_(ADD_PENDING),
+ //node_pending_(ADD_PENDING),
verbose_(_verbose)
{
if (verbose_) std::cout << "NodeBase::NodeBase(). Id: " << node_id_ << " Label: " << label_ << std::endl;
diff --git a/src/node_base.h b/src/node_base.h
index fbdfe2ab4e90a018a950eecc0f61369f39138238..1c46a6fa467d65174158bad02ac44b3af143dcd1 100644
--- a/src/node_base.h
+++ b/src/node_base.h
@@ -18,7 +18,7 @@ class NodeBase
std::string label_; ///< Text label identifying the node
unsigned int node_id_; ///< Node id. It is unique over the whole Wolf Tree
static unsigned int node_id_count_; ///< Object counter (acts as simple ID factory)
- PendingStatus node_pending_; ///< Pending to be added/updated in the filter or optimizer
+ //PendingStatus node_pending_; ///< Pending to be added/updated in the filter or optimizer
bool verbose_;
public:
diff --git a/src/node_linked.h b/src/node_linked.h
index 6a9f9e1b810dd2c0019cf25476118436b6a0042c..63663df49b1b0f3e9acd8a0fdac4ce59621d9a82 100644
--- a/src/node_linked.h
+++ b/src/node_linked.h
@@ -47,6 +47,7 @@ class NodeLinked : public NodeBase
NodeLocation location_;
UpperNodePtr up_node_ptr_; //it is not a shared pointer because the ownership of upper node should not be shared by lower nodes
LowerNodeList down_node_list_;
+ bool is_deleting_;
public:
@@ -57,13 +58,6 @@ class NodeLinked : public NodeBase
*/
NodeLinked(const NodeLocation _loc, const std::string& _label);
- /** \brief Constructor specifying up node
- *
- * Constructor specifying up node
- *
- */
- //NodeLinked(const NodeLocation _loc, const std::string& _label, const UpperNodePtr& _up_node_ptr);
-
/** \brief Default destructor
*
* Default destructor
@@ -71,6 +65,13 @@ class NodeLinked : public NodeBase
*/
virtual ~NodeLinked();
+ /** \brief Checks if the destructor has been called already
+ *
+ * Checks if the destructor has been called already
+ *
+ */
+ virtual const bool isDeleting() const;
+
/** \brief Checks if node is on the top of Wolf tree
*
* Check if node is on the top of Wolf tree
@@ -129,13 +130,6 @@ class NodeLinked : public NodeBase
*/
LowerNodeList& downNodeList() const;
- /** \brief Gets a constant reference to down node list
- *
- * Gets constant reference to down node list
- *
- */
-// const LowerNodeList& downNodeList() const;
-
/** \brief Gets a pointer to down node list
*
* Gets a pointer to down node list
@@ -143,13 +137,6 @@ class NodeLinked : public NodeBase
*/
LowerNodeList* getDownNodeListPtr();
- /** \brief Gets a const pointer to down node list
- *
- * Gets a const pointer to down node list
- *
- */
-// const LowerNodeList* downNodeListPtr() const;
-
/** \brief Removes a down node from list, given an iterator
*
* Removes a down node from the list
@@ -228,28 +215,27 @@ NodeLinked<UpperType, LowerType>::NodeLinked(const NodeLocation _loc, const std:
NodeBase(_label), //
location_(_loc), //
up_node_ptr_(nullptr),
- down_node_list_()
+ down_node_list_(),
+ is_deleting_(false)
{
}
-// template<class UpperType, class LowerType>
-// NodeLinked<UpperType, LowerType>::NodeLinked(const NodeLocation _loc, const std::string& _label, const UpperNodePtr& _up_node_ptr) :
-// NodeBase(_label), //
-// location_(_loc),
-// down_node_list_()
-// {
-// linkToUpperNode(_up_node_ptr);
-// }
-
template<class UpperType, class LowerType>
NodeLinked<UpperType, LowerType>::~NodeLinked()
{
//std::cout << "deleting Nodelinked " << node_id_ << " down_node_list_.size() " << down_node_list_.size() << std::endl;
+ is_deleting_ = true;
while (down_node_list_.begin()!= down_node_list_.end())
removeDownNode(down_node_list_.begin());
}
+template<class UpperType, class LowerType>
+inline const bool NodeLinked<UpperType, LowerType>::isDeleting() const
+{
+ return is_deleting_;
+}
+
template<class UpperType, class LowerType>
inline bool NodeLinked<UpperType, LowerType>::isTop() const
{
@@ -316,24 +302,12 @@ inline typename NodeLinked<UpperType, LowerType>::LowerNodeList& NodeLinked<Uppe
return down_node_list_;
}
-// template<class UpperType, class LowerType>
-// inline const typename NodeLinked<UpperType, LowerType>::LowerNodeList& NodeLinked<UpperType, LowerType>::downNodeList() const
-// {
-// return down_node_list_;
-// }
-
template<class UpperType, class LowerType>
inline typename NodeLinked<UpperType, LowerType>::LowerNodeList* NodeLinked<UpperType, LowerType>::getDownNodeListPtr()
{
return &down_node_list_;
}
-// template<class UpperType, class LowerType>
-// inline const typename NodeLinked<UpperType, LowerType>::LowerNodeList* NodeLinked<UpperType, LowerType>::downNodeListPtr() const
-// {
-// return &down_node_list_;
-// }
-
template<class UpperType, class LowerType>
inline void NodeLinked<UpperType, LowerType>::removeDownNode(const unsigned int _id)
{
@@ -362,25 +336,12 @@ inline void NodeLinked<UpperType, LowerType>::removeDownNode(const LowerNodeIter
delete *_iter;
}
-//TODO: confirm this change by the others :)
template<class UpperType, class LowerType>
WolfProblem* NodeLinked<UpperType, LowerType>::getTop()
{
return up_node_ptr_->getTop();
}
-//template<class UpperType, class LowerType>
-//WolfProblem* NodeLinked<UpperType, LowerType>::getTop() //TODO: confirm by the others :)
-//{
-// return up_node_ptr_->getTop();
-//}
-//
-//template<class LowerType>
-//WolfProblem* NodeLinked<WolfProblem, LowerType>::getTop() //TODO: confirm by the others :)
-//{
-// return up_node_ptr_;
-//}
-
template<class UpperType, class LowerType>
void NodeLinked<UpperType, LowerType>::print(unsigned int _ntabs, std::ostream& _ost) const
{
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index f186de66204ad9a71e4706533f1c129363ed4838..d6d93bab0ffc640979f2308db11384ffa6e531fb 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -1,7 +1,7 @@
#include "sensor_base.h"
-SensorBase::SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateBase* _o_ptr, const Eigen::VectorXs & _params) :
- NodeBase("SENSOR"),
+SensorBase::SensorBase(const SensorType & _tp, StateBlock* _p_ptr, StateBlock* _o_ptr, const Eigen::VectorXs & _params) :
+ NodeLinked(MID, "SENSOR"),
type_(_tp),
p_ptr_(_p_ptr),
o_ptr_(_o_ptr),
@@ -10,8 +10,8 @@ SensorBase::SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateBase* _o_
params_ = _params;
}
-SensorBase::SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateBase* _o_ptr, unsigned int _params_size) :
- NodeBase("SENSOR"),
+SensorBase::SensorBase(const SensorType & _tp, StateBlock* _p_ptr, StateBlock* _o_ptr, unsigned int _params_size) :
+ NodeLinked(MID, "SENSOR"),
type_(_tp),
p_ptr_(_p_ptr),
o_ptr_(_o_ptr),
@@ -23,6 +23,18 @@ SensorBase::SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateBase* _o_
SensorBase::~SensorBase()
{
//std::cout << "deleting SensorBase " << nodeId() << std::endl;
+
+ // Remove State Blocks
+ if (p_ptr_ != nullptr)
+ {
+ getTop()->removeStateBlockPtr(p_ptr_);
+ delete p_ptr_;
+ }
+ if (o_ptr_ != nullptr)
+ {
+ getTop()->removeStateBlockPtr(o_ptr_);
+ delete o_ptr_;
+ }
}
const SensorType SensorBase::getSensorType() const
@@ -30,27 +42,29 @@ const SensorType SensorBase::getSensorType() const
return type_;
}
-StateBase* SensorBase::getPPtr() const
+StateBlock* SensorBase::getPPtr() const
{
return p_ptr_;
}
-StateBase* SensorBase::getOPtr() const
+StateBlock* SensorBase::getOPtr() const
{
return o_ptr_;
}
-Eigen::Matrix2s SensorBase::getRotationMatrix2D() {
+Eigen::Matrix2s SensorBase::getRotationMatrix2D()
+{
// TODO: move this code somewhere else and do a real get()
- assert ( o_ptr_->getStateType() != ST_QUATERNION && "2D rot matrix not defined for quaternions." );
- return Eigen::Rotation2D<WolfScalar>(*(o_ptr_->getPtr())).matrix();
+ assert ( o_ptr_->getType() != ST_QUATERNION && "2D rot matrix not defined for quaternions." );
+ return Eigen::Rotation2D<WolfScalar>(*(o_ptr_->getPtr())).matrix();
}
-Eigen::Matrix3s SensorBase::getRotationMatrix3D() {
+Eigen::Matrix3s SensorBase::getRotationMatrix3D()
+{
// TODO: move this code somewhere else and do a real get()
Eigen::Matrix3s R = Eigen::Matrix3s::Identity();
- if ( o_ptr_->getStateType() == ST_QUATERNION )
+ if ( o_ptr_->getType() == ST_VECTOR)
R.block<2,2>(0,0) = Eigen::Rotation2D<WolfScalar>(*(o_ptr_->getPtr())).matrix();
else
@@ -58,3 +72,33 @@ Eigen::Matrix3s SensorBase::getRotationMatrix3D() {
return R;
}
+
+void SensorBase::fix()
+{
+ // State Blocks
+ if (p_ptr_!=nullptr)
+ {
+ p_ptr_->fix();
+ getTop()->updateStateBlockPtr(p_ptr_);
+ }
+ if (o_ptr_!=nullptr)
+ {
+ o_ptr_->fix();
+ getTop()->updateStateBlockPtr(o_ptr_);
+ }
+}
+
+void SensorBase::unfix()
+{
+ // State Blocks
+ if (p_ptr_!=nullptr)
+ {
+ p_ptr_->unfix();
+ getTop()->updateStateBlockPtr(p_ptr_);
+ }
+ if (o_ptr_!=nullptr)
+ {
+ o_ptr_->unfix();
+ getTop()->updateStateBlockPtr(o_ptr_);
+ }
+}
diff --git a/src/sensor_base.h b/src/sensor_base.h
index d16c82608a2c5d9a69b1920f7c81a7c97a1fc628..2e4521b1f08b2b49f82574ac9bad959f46174cfa 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -1,65 +1,66 @@
#ifndef SENSOR_BASE_H_
#define SENSOR_BASE_H_
+class HardwareBase;
+class NodeTerminus;
+
//std includes
#include <iostream>
//Wolf includes
#include "wolf.h"
-#include "node_base.h"
-#include "state_base.h"
+#include "node_linked.h"
+#include "state_block.h"
+#include "hardware_base.h"
-class SensorBase : public NodeBase
+class SensorBase : public NodeLinked<HardwareBase,NodeTerminus>
{
protected:
SensorType type_; //indicates sensor type. Enum defined at wolf.h
- StateBase* p_ptr_; // sensor position state unit pointer
- StateBase* o_ptr_; // sensor orientation state unit pointer
+ StateBlock* p_ptr_; // sensor position state block pointer
+ StateBlock* o_ptr_; // sensor orientation state block pointer
Eigen::VectorXs params_;//sensor intrinsic params: offsets, scale factors, sizes, ...
public:
- /** \brief Constructor without parameters
- *
- * Constructor without parameters.
- * Allows allocation of containers of StateBase, such as std::vector<SensorBase>
- *
- **/
- //SensorBase();
/** \brief Constructor with parameter vector
*
* Constructor with parameter vector
* \param _tp Type of the sensor (types defined at wolf.h)
- * \param _p_ptr StateBase pointer to the sensor position
- * \param _o_ptr StateBase pointer to the sensor orientation
+ * \param _p_ptr StateBlock pointer to the sensor position
+ * \param _o_ptr StateBlock pointer to the sensor orientation
* \param _params Vector containing the sensor parameters
*
**/
- SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateBase* _o_ptr, const Eigen::VectorXs & _params);
+ SensorBase(const SensorType & _tp, StateBlock* _p_ptr, StateBlock* _o_ptr, const Eigen::VectorXs & _params);
/** \brief Constructor with parameter size
*
* Constructor with parameter vector
* \param _tp Type of the sensor (types defined at wolf.h)
- * \param _p_ptr StateBase pointer to the sensor position
- * \param _o_ptr StateBase pointer to the sensor orientation
+ * \param _p_ptr StateBlock pointer to the sensor position
+ * \param _o_ptr StateBlock pointer to the sensor orientation
* \param _params_size size of the vector containing the sensor parameters
*
**/
- SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateBase* _o_ptr, unsigned int _params_size);
+ SensorBase(const SensorType & _tp, StateBlock* _p_ptr, StateBlock* _o_ptr, unsigned int _params_size);
~SensorBase();
const SensorType getSensorType() const;
- StateBase* getPPtr() const;
+ StateBlock* getPPtr() const;
- StateBase* getOPtr() const;
+ StateBlock* getOPtr() const;
Eigen::Matrix2s getRotationMatrix2D();
Eigen::Matrix3s getRotationMatrix3D();
+ void fix();
+
+ void unfix();
+
};
#endif
diff --git a/src/sensor_gps_fix.cpp b/src/sensor_gps_fix.cpp
index fffa2d51304c4edfc3d78955835888e143a2662b..35558aee39f917e8c35a7c268e5eeb45ee108d03 100644
--- a/src/sensor_gps_fix.cpp
+++ b/src/sensor_gps_fix.cpp
@@ -1,6 +1,6 @@
#include "sensor_gps_fix.h"
-SensorGPSFix::SensorGPSFix(StateBase* _p_ptr, StateBase* _o_ptr, const double& _noise) :
+SensorGPSFix::SensorGPSFix(StateBlock* _p_ptr, StateBlock* _o_ptr, const double& _noise) :
SensorBase(GPS_FIX, _p_ptr, _o_ptr, Eigen::VectorXs::Constant(1,_noise))
{
//
diff --git a/src/sensor_gps_fix.h b/src/sensor_gps_fix.h
index 5eedff65bbdbbcd64624ce7004e05ef1f0c1b5b1..2a45bf05cdc6a6005fee20fe8f50f6adf7fc9f6c 100644
--- a/src/sensor_gps_fix.h
+++ b/src/sensor_gps_fix.h
@@ -16,7 +16,7 @@ class SensorGPSFix : public SensorBase
* \param _noise noise standard deviation
*
**/
- SensorGPSFix(StateBase* _p_ptr, StateBase* _o_ptr, const double& _noise);
+ SensorGPSFix(StateBlock* _p_ptr, StateBlock* _o_ptr, const double& _noise);
/** \brief Destructor
*
diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp
index b3f2b9820d99bbb861809f3cde146c40ed54b95c..559d7b2009ac523f9b9993844cd1b935ce9e9e89 100644
--- a/src/sensor_laser_2D.cpp
+++ b/src/sensor_laser_2D.cpp
@@ -7,7 +7,7 @@
// params_ << _angle_min, _angle_max, _angle_increment, _range_min, _range_max, _range_stdev, _time_increment, _scan_time;
// }
-SensorLaser2D::SensorLaser2D(StateBase* _p_ptr, StateBase* _o_ptr) :
+SensorLaser2D::SensorLaser2D(StateBlock* _p_ptr, StateBlock* _o_ptr) :
SensorBase(LIDAR, _p_ptr, _o_ptr, 8)
{
setDefaultScanParams();
diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h
index c8a3ce0c5f3f009ad5bcd84998440b05e92b5ed2..07b3061d7594778407a2b60689ae574941f655fb 100644
--- a/src/sensor_laser_2D.h
+++ b/src/sensor_laser_2D.h
@@ -29,7 +29,7 @@ class SensorLaser2D : public SensorBase
* \param _params struct with scan parameters. See laser_scan_utils library API for reference
*
**/
- SensorLaser2D(StateBase* _p_ptr, StateBase* _o_ptr);
+ SensorLaser2D(StateBlock* _p_ptr, StateBlock* _o_ptr);
//SensorLaser2D(const Eigen::VectorXs & _sp, const laserscanutils::ScanParams & _params);
/** \brief Destructor
diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp
index 6359f6236b29b15a8adad67c08cdb6ee46fbf5ba..3943f7a05c5bd1917bd69e62de05a0919d5abc23 100644
--- a/src/sensor_odom_2D.cpp
+++ b/src/sensor_odom_2D.cpp
@@ -1,6 +1,6 @@
#include "sensor_odom_2D.h"
-SensorOdom2D::SensorOdom2D(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor) :
+SensorOdom2D::SensorOdom2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor) :
SensorBase(ODOM_2D, _p_ptr, _o_ptr, 2)
{
params_ << _disp_noise_factor, _rot_noise_factor;
diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h
index 24e67daff68173b8a620bcb754f12f4db98d4e60..b250dd0ccc22c258cc3b12b1d84310c6044b238c 100644
--- a/src/sensor_odom_2D.h
+++ b/src/sensor_odom_2D.h
@@ -17,7 +17,7 @@ class SensorOdom2D : public SensorBase
* \param _rot_noise_factor rotation noise factor
*
**/
- SensorOdom2D(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor);
+ SensorOdom2D(StateBlock* _p_ptr, StateBlock* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor);
/** \brief Destructor
*
diff --git a/src/solver/qr_solver.h b/src/solver/qr_solver.h
index b32be207a5fd633ea7fb013e06054f5f5440bbe6..aa20367e6afa6509003b8dcd658662ee8570907e 100644
--- a/src/solver/qr_solver.h
+++ b/src/solver/qr_solver.h
@@ -13,7 +13,7 @@
#include <ctime>
//Wolf includes
-#include "state_base.h"
+#include "state_block.h"
#include "constraint_base.h"
#include "sparse_utils.h"
@@ -31,17 +31,18 @@ using namespace Eigen;
class SolverQR
{
protected:
+ WolfProblem* problem_ptr_;
SparseQR < SparseMatrix<double>, NaturalOrdering<int>> solver_;
SparseMatrix<double> A_, R_;
VectorXd b_, x_incr_;
- std::vector<StateBase*> nodes_;
+ std::vector<StateBlock*> nodes_;
std::vector<ConstraintBase*> constraints_;
std::vector<CostFunctionBase*> cost_functions_;
// ordering
SparseMatrix<int> A_nodes_;
PermutationMatrix<Dynamic, Dynamic, int> acc_node_permutation_;
- std::map<unsigned int, unsigned int> id_2_idx_;
+ std::map<double*, unsigned int> id_2_idx_;
CCOLAMDOrdering<int> orderer_;
VectorXi node_ordering_restrictions_;
ArrayXi node_locations_;
@@ -54,7 +55,8 @@ class SolverQR
public:
- SolverQR() :
+ SolverQR(WolfProblem* problem_ptr_) :
+ problem_ptr_(problem_ptr_),
A_(0,0),
R_(0,0),
A_nodes_(0,0),
@@ -73,66 +75,67 @@ class SolverQR
}
- void update(WolfProblem* _problem_ptr)
+ void update()
{
- // ADD/UPDATE STATE UNITS
- for(auto state_unit_it = _problem_ptr->getStateListPtr()->begin(); state_unit_it!=_problem_ptr->getStateListPtr()->end(); state_unit_it++)
+ // REMOVE CONSTRAINTS
+ while (!problem_ptr_->getConstraintRemoveList()->empty())
{
- if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
- addStateUnit(*state_unit_it);
-
- else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
- updateStateUnitStatus(*state_unit_it);
+ // TODO: removeConstraint(problem_ptr_->getConstraintRemoveList()->front());
+ problem_ptr_->getConstraintRemoveList()->pop_front();
}
- //std::cout << "state units updated!" << std::endl;
-
- // REMOVE STATE UNITS
- while (!_problem_ptr->getRemovedStateListPtr()->empty())
+ // REMOVE STATE BLOCKS
+ while (!problem_ptr_->getStateBlockRemoveList()->empty())
{
- // TODO
- _problem_ptr->getRemovedStateListPtr()->pop_front();
+ // TODO removeStateBlock((double *)(problem_ptr_->getStateBlockRemoveList()->front()));
+ problem_ptr_->getStateBlockRemoveList()->pop_front();
+ }
+ // ADD STATE BLOCKS
+ while (!problem_ptr_->getStateBlockAddList()->empty())
+ {
+ addStateBlock(problem_ptr_->getStateBlockAddList()->front());
+ problem_ptr_->getStateBlockAddList()->pop_front();
+ }
+ // UPDATE STATE BLOCKS
+ while (!problem_ptr_->getStateBlockUpdateList()->empty())
+ {
+ updateStateBlockStatus(problem_ptr_->getStateBlockUpdateList()->front());
+ problem_ptr_->getStateBlockUpdateList()->pop_front();
}
- //std::cout << "state units removed!" << std::endl;
-
// ADD CONSTRAINTS
- ConstraintBaseList ctr_list;
- _problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list);
- //std::cout << "ctr_list.size() = " << ctr_list.size() << std::endl;
- for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
- if ((*ctr_it)->getPendingStatus() == ADD_PENDING)
- addConstraint(*ctr_it);
-
- //std::cout << "constraints updated!" << std::endl;
+ while (!problem_ptr_->getConstraintAddList()->empty())
+ {
+ addConstraint(problem_ptr_->getConstraintAddList()->front());
+ problem_ptr_->getConstraintAddList()->pop_front();
+ }
}
- void addStateUnit(StateBase* _state_ptr)
+ void addStateBlock(StateBlock* _state_ptr)
{
t_managing_ = clock();
- std::cout << "adding state unit " << _state_ptr->nodeId() << std::endl;
- if (_state_ptr->getStateStatus() == ST_ESTIMATED)
+ std::cout << "adding state unit " << _state_ptr->getPtr() << std::endl;
+ if (!_state_ptr->isFixed() )
{
nodes_.push_back(_state_ptr);
- id_2_idx_[_state_ptr->nodeId()] = nodes_.size()-1;
+ id_2_idx_[_state_ptr->getPtr()] = nodes_.size()-1;
- std::cout << "idx " << id_2_idx_[_state_ptr->nodeId()] << std::endl;
+ std::cout << "idx " << id_2_idx_[_state_ptr->getPtr()] << std::endl;
// Resize accumulated permutations
augmentPermutation(acc_node_permutation_, nNodes());
// Resize state
- x_incr_.conservativeResize(x_incr_.size() + _state_ptr->getStateSize());
+ x_incr_.conservativeResize(x_incr_.size() + _state_ptr->getSize());
// Resize problem
- A_.conservativeResize(A_.rows(), A_.cols() + _state_ptr->getStateSize());
- R_.conservativeResize(R_.cols() + _state_ptr->getStateSize(), R_.cols() + _state_ptr->getStateSize());
+ A_.conservativeResize(A_.rows(), A_.cols() + _state_ptr->getSize());
+ R_.conservativeResize(R_.cols() + _state_ptr->getSize(), R_.cols() + _state_ptr->getSize());
}
- _state_ptr->setPendingStatus(NOT_PENDING);
time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC;
}
- void updateStateUnitStatus(StateBase* _state_ptr)
+ void updateStateBlockStatus(StateBlock* _state_ptr)
{
//TODO
}
@@ -156,8 +159,8 @@ class SolverQR
std::vector<unsigned int> idxs;
for (unsigned int i = 0; i < _constraint_ptr->getStatePtrVector().size(); i++)
- if (_constraint_ptr->getStatePtrVector().at(i)->getStateStatus() == ST_ESTIMATED)
- idxs.push_back(id_2_idx_[_constraint_ptr->getStatePtrVector().at(i)->nodeId()]);
+ if (!_constraint_ptr->getStatePtrVector().at(i)->isFixed() )
+ idxs.push_back(id_2_idx_[_constraint_ptr->getStatePtrVector().at(i)->getPtr()]);
n_new_constraints_++;
constraint_locations_.push_back(A_.rows());
@@ -182,8 +185,6 @@ class SolverQR
// error
b_.tail(meas_dim) = error;
- _constraint_ptr->setPendingStatus(NOT_PENDING);
-
time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC;
}
@@ -268,9 +269,9 @@ class SolverQR
std::cout << "constraint: " << i << " id: " << constraints_.at(constraints_.size()-1-i)->nodeId() << std::endl;
for (unsigned int j = 0; j < ct_ptr->getStatePtrVector().size(); j++)
{
- if (ct_ptr->getStatePtrVector().at(j)->getStateStatus() == ST_ESTIMATED)
+ if (!ct_ptr->getStatePtrVector().at(j)->isFixed())
{
- unsigned int idx = id_2_idx_[ct_ptr->getStatePtrVector().at(j)->nodeId()];
+ unsigned int idx = id_2_idx_[ct_ptr->getStatePtrVector().at(j)->getPtr()];
//std::cout << "estimated idx " << idx << std::endl;
//std::cout << "node_order(idx) " << node_order(idx) << std::endl;
//std::cout << "first_ordered_node " << first_ordered_node << std::endl;
@@ -404,8 +405,8 @@ class SolverQR
// UPDATE X VALUE
for (unsigned int i = 0; i<nodes_.size(); i++)
{
- Map<VectorXs> x_i(nodes_.at(i)->getPtr(), nodes_.at(i)->getStateSize());
- x_i += x_incr_.segment(nodeLocation(i), nodes_.at(i)->getStateSize());
+ Map<VectorXs> x_i(nodes_.at(i)->getPtr(), nodes_.at(i)->getSize());
+ x_i += x_incr_.segment(nodeLocation(i), nodes_.at(i)->getSize());
}
// Zero the error
b_.setZero();
@@ -483,7 +484,7 @@ class SolverQR
unsigned int nodeDim(const unsigned int _idx)
{
assert(_idx < nNodes());
- return nodes_.at(_idx)->getStateSize();
+ return nodes_.at(_idx)->getSize();
}
unsigned int nodeOrder(const unsigned int _idx)
@@ -529,23 +530,6 @@ class SolverQR
specific_ptr->block9Size>(specific_ptr));
break;
}
- case CTR_ODOM_2D_COMPLEX_ANGLE:
- {
- ConstraintOdom2DComplexAngle* specific_ptr = (ConstraintOdom2DComplexAngle*)(_corrPtr);
- return (CostFunctionBase*)new CostFunctionSparse<ConstraintOdom2DComplexAngle,
- specific_ptr->measurementSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
case CTR_ODOM_2D:
{
ConstraintOdom2D* specific_ptr = (ConstraintOdom2D*)(_corrPtr);
diff --git a/src/solver/solver_manager.h b/src/solver/solver_manager.h
index 32604ed2af330152f4e5724ac55a7264cc0134e5..05cd232650c029e687534cad84927b3a27b8e79b 100644
--- a/src/solver/solver_manager.h
+++ b/src/solver/solver_manager.h
@@ -3,7 +3,7 @@
//wolf includes
#include "../wolf.h"
-#include "../state_base.h"
+#include "../state_block.h"
#include "../state_point.h"
#include "../state_complex_angle.h"
#include "../state_theta.h"
@@ -37,11 +37,11 @@ class SolverManager
void removeConstraint(const unsigned int& _corr_idx);
- void addStateUnit(StateBase* _st_ptr);
+ void addStateUnit(StateBlock* _st_ptr);
void removeAllStateUnits();
- void updateStateUnitStatus(StateBase* _st_ptr);
+ void updateStateUnitStatus(StateBlock* _st_ptr);
ceres::CostFunction* createCostFunction(ConstraintBase* _corrPtr);
};
diff --git a/src/state_base.cpp b/src/state_base.cpp
deleted file mode 100644
index cdf449b868f74728f3315e61c09880d4526523fa..0000000000000000000000000000000000000000
--- a/src/state_base.cpp
+++ /dev/null
@@ -1,64 +0,0 @@
-
-#include "state_base.h"
-
-StateBase::StateBase(WolfScalar* _st_ptr, unsigned int _size, StateType _type, StateStatus _status) :
- type_(_type),
- state_ptr_(_st_ptr),
- size_(_size),
- status_(_status),
- pending_status_(ADD_PENDING)
-{
- //
-}
-
-StateBase::~StateBase()
-{
- //std::cout << "deleting StateBase " << nodeId() << std::endl;
-}
-
-WolfScalar* StateBase::getPtr()
-{
- return state_ptr_;
-}
-
-void StateBase::setPtr(WolfScalar* _new_ptr)
-{
- state_ptr_ = _new_ptr;
-}
-
-StateStatus StateBase::getStateStatus() const
-{
- return status_;
-}
-
-void StateBase::setStateStatus(const StateStatus& _status)
-{
- status_=_status;
- if (getPendingStatus() != ADD_PENDING)
- setPendingStatus(UPDATE_PENDING);
-}
-
-PendingStatus StateBase::getPendingStatus() const
-{
- return pending_status_;
-}
-
-unsigned int StateBase::getStateSize() const {
- return size_;
-}
-
-void StateBase::setPendingStatus(PendingStatus _pending)
-{
- pending_status_ = _pending;
-}
-
-Eigen::Map<const Eigen::VectorXs> StateBase::getVector() const {
- return Eigen::Map<const Eigen::VectorXs> (state_ptr_,size_);
-}
-
-StateType StateBase::getStateType() const {
- return type_;
-}
-
-void StateBase::print(unsigned int _ntabs, std::ostream& _ost) const {
-}
diff --git a/src/state_base.h b/src/state_base.h
deleted file mode 100644
index 7e1d77cf072e5fc5d4f34c3b3b96db9e54cee670..0000000000000000000000000000000000000000
--- a/src/state_base.h
+++ /dev/null
@@ -1,111 +0,0 @@
-
-#ifndef STATE_BASE_H_
-#define STATE_BASE_H_
-
-//std includes
-#include <iostream>
-#include <vector>
-#include <cmath>
-
-//Wolf includes
-#include "wolf.h"
-#include "node_base.h"
-
-//class StateBase
-class StateBase
-{
- protected:
- StateType type_;
- WolfScalar* state_ptr_;
- unsigned int size_;
- StateStatus status_;
- PendingStatus pending_status_;
-
- public:
- /** \brief Constructor with scalar pointer
- *
- * Constructor with scalar pointer
- * \param _st_ptr is the pointer of the first element of the state unit
- *
- **/
- StateBase(WolfScalar* _st_ptr, unsigned int _size, StateType _type = ST_VECTOR, StateStatus _status = ST_ESTIMATED);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~StateBase();
-
- /** \brief Returns the pointer to the first element of the state
- *
- * Returns the scalar pointer to the first element of the state
- *
- **/
- virtual WolfScalar* getPtr();
-
- /** \brief Returns a (mapped) vector of the state unit
- *
- * Returns a (mapped) vector of the state unit
- *
- **/
- Eigen::Map<const Eigen::VectorXs> getVector() const;
-
- /** \brief Set the pointer of the first element of the state
- *
- * Set the pointer of the first element of the state
- *
- **/
- virtual void setPtr(WolfScalar* _new_ptr);
-
- /** \brief Returns the parametrization of the state unit
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual StateType getStateType() const;
-
- /** \brief Returns the status of the state unit
- *
- * Returns the stateStatus (see wolf.h) of the state unit
- *
- **/
- StateStatus getStateStatus() const;
-
- /** \brief Set the status of the state unit
- *
- * Set tthe stateStatus (see wolf.h) of the state unit
- *
- **/
- void setStateStatus(const StateStatus& _status);
-
- /** \brief Returns the state unit size
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual unsigned int getStateSize() const;
-
- /** \brief Gets the node pending status (pending or not to be added/updated in the filter or optimizer)
- *
- * Gets the node pending status (pending or not to be added/updated in the filter or optimizer)
- *
- */
- PendingStatus getPendingStatus() const;
-
- /** \brief Sets the node pending status (pending or not to be added/updated in the filter or optimizer)
- *
- * Sets the node pending status (pending or not to be added/updated in the filter or optimizer)
- *
- */
- void setPendingStatus(PendingStatus _pending);
-
-
- /** \brief Prints all the elements of the state unit
- *
- * Prints all the elements of the state unit
- *
- **/
- virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
-};
-#endif
diff --git a/src/state_block.cpp b/src/state_block.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0aa6ff8f7a8d86a8869e4d12aee3ffaae54f8d96
--- /dev/null
+++ b/src/state_block.cpp
@@ -0,0 +1,59 @@
+
+#include "state_block.h"
+
+StateBlock::StateBlock(const Eigen::VectorXs _state, StateType _type) :
+ type_(_type),
+ state_(_state)
+{
+ //
+}
+
+StateBlock::~StateBlock()
+{
+ //std::cout << "deleting StateBase " << nodeId() << std::endl;
+}
+
+WolfScalar* StateBlock::getPtr()
+{
+ return state_.data();
+}
+
+Eigen::VectorXs StateBlock::getVector()
+{
+ return state_;
+}
+
+void StateBlock::setVector(const Eigen::VectorXs& _state)
+{
+ assert(_state.size() == state_.size());
+ state_ = _state;
+}
+
+StateType StateBlock::getType() const
+{
+ return type_;
+}
+
+unsigned int StateBlock::getSize() const
+{
+ return state_.size();
+}
+
+bool StateBlock::isFixed() const
+{
+ return fixed_;
+}
+
+void StateBlock::fix()
+{
+ fixed_ = true;
+}
+
+void StateBlock::unfix()
+{
+ fixed_ = false;
+}
+
+void StateBlock::print(unsigned int _ntabs, std::ostream& _ost) const
+{
+}
diff --git a/src/state_block.h b/src/state_block.h
new file mode 100644
index 0000000000000000000000000000000000000000..7fe56d86e44326c14fb5249a76753300e9a83450
--- /dev/null
+++ b/src/state_block.h
@@ -0,0 +1,102 @@
+
+#ifndef STATE_BLOCK_H_
+#define STATE_BLOCK_H_
+
+//std includes
+#include <iostream>
+#include <vector>
+#include <cmath>
+
+//Wolf includes
+#include "wolf.h"
+#include "node_base.h"
+
+//class StateBlock
+class StateBlock
+{
+ protected:
+ StateType type_;
+ Eigen::VectorXs state_;
+ bool fixed_;
+
+ public:
+ /** \brief Constructor with scalar pointer
+ *
+ * Constructor with scalar pointer
+ * \param _state is state vector
+ * \param _type parametrization of the state
+ *
+ **/
+ StateBlock(const Eigen::VectorXs _state, StateType _type = ST_VECTOR);
+
+ /** \brief Destructor
+ *
+ * Destructor
+ *
+ **/
+ virtual ~StateBlock();
+
+ /** \brief Returns the pointer to the first element of the state
+ *
+ * Returns the scalar pointer to the first element of the state
+ *
+ **/
+ WolfScalar* getPtr();
+
+ /** \brief Returns the state vector
+ *
+ * Returns the state vector
+ *
+ **/
+ Eigen::VectorXs getVector();
+
+ /** \brief Sets the state vector
+ *
+ * Sets the state vector
+ *
+ **/
+ void setVector(const Eigen::VectorXs& _state);
+
+ /** \brief Returns the parametrization of the state
+ *
+ * Returns the parametrizationType (see wolf.h) of the state
+ *
+ **/
+ virtual StateType getType() const;
+
+ /** \brief Returns the state size
+ *
+ * Returns the state size
+ *
+ **/
+ unsigned int getSize() const;
+
+ /** \brief Returns if the state is fixed (not estimated)
+ *
+ * Returns if the state is fixed (not estimated)
+ *
+ **/
+ bool isFixed() const;
+
+ /** \brief Sets the state as fixed
+ *
+ * Sets the state as fixed
+ *
+ **/
+ void fix();
+
+ /** \brief Sets the state as estimated
+ *
+ * Sets the state as estimated
+ *
+ **/
+ void unfix();
+
+ /** \brief Prints all the elements of the state unit
+ *
+ * Prints all the elements of the state unit
+ *
+ **/
+ virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
+};
+#endif
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index c76955a382900cd7c37473bff3118cf4b8c84ace..905fcc1295fe73f3b03a8edae7a2c2326ab217b4 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -14,6 +14,11 @@ TrajectoryBase::~TrajectoryBase()
void TrajectoryBase::addFrame(FrameBase* _frame_ptr)
{
addDownNode(_frame_ptr);
+
+ if (_frame_ptr->getPPtr() != nullptr)
+ getTop()->addStateBlockPtr(_frame_ptr->getPPtr());
+ if (_frame_ptr->getOPtr() != nullptr)
+ getTop()->addStateBlockPtr(_frame_ptr->getOPtr());
}
void TrajectoryBase::removeFrame(const FrameBaseIter& _frame_iter)
diff --git a/src/trajectory_base.h b/src/trajectory_base.h
index 709c164c996d5eba3f96f98fc21306288d7752ab..c041bcd895599baf90a92cab8bacec56bd18a221 100644
--- a/src/trajectory_base.h
+++ b/src/trajectory_base.h
@@ -18,7 +18,7 @@ class FrameBase;
#include "node_linked.h"
#include "node_terminus.h"
#include "frame_base.h"
-#include "state_base.h"
+#include "state_block.h"
//class TrajectoryBase
class TrajectoryBase : public NodeLinked<WolfProblem,FrameBase>
diff --git a/src/wolf.h b/src/wolf.h
index d972a671451e452c759c9a728397715a3d5e40bf..950996c753b89d9b5c0ebb18697cc2b5e7757d4f 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -103,6 +103,19 @@ typedef enum
REGULAR_FRAME ///< marks a regular frame. It does play at optimizations but it will be discarded from the window once a newer frame arrives.
} FrameType;
+/** \brief Enumeration of all possible frames
+ *
+ * Enumeration of all possible frames.
+ *
+ * You may add items to this list as needed. Be concise with names, and document your entries.
+ *
+ */
+typedef enum
+{
+ PO_2D, ///< marks a 2D frame containing position (x,y) and orientation angle.
+ PO_3D ///< marks a 3D frame containing position (x,y,z) and orientation quaternion.
+} FrameStructure;
+
/** \brief Enumeration of all possible constraints
*
* Enumeration of all possible constraints.
@@ -114,14 +127,42 @@ typedef enum
{
CTR_GPS_FIX_2D, ///< marks a 2D GPS Fix constraint.
CTR_FIX, ///< marks a Fix constraint (for priors).
- CTR_ODOM_2D_COMPLEX_ANGLE, ///< marks a 2D Odometry constraint using complex angles.
- CTR_ODOM_2D, ///< marks a 2D Odometry constraint using theta angles.
- CTR_TWIST_2D_THETA, ///< marks a 2D Twist constraint using theta angles.
- CTR_CORNER_2D, ///< marks a 2D corner constraint using theta angles.
- CTR_CONTAINER ///< marks a 2D container constraint using theta angles.
+ CTR_ODOM_2D, ///< marks a 2D Odometry constraint .
+ CTR_CORNER_2D, ///< marks a 2D corner constraint .
+ CTR_CONTAINER ///< marks a 2D container constraint .
} ConstraintType;
+/** \brief Enumeration of constraint categories
+ *
+ * Enumeration of constraint categories.
+ *
+ * You may add items to this list as needed. Be concise with names, and document your entries.
+ *
+ */
+typedef enum
+{
+ CTR_ABSOLUTE, ///< Constraint established with absolute reference.
+ CTR_FRAME, ///< Constraint established with a frame (odometry).
+ CTR_FEATURE, ///< Constraint established with a feature (correspondence).
+ CTR_LANDMARK ///< Constraint established with a landmark (correpondence).
+
+} ConstraintCategory;
+
+/** \brief Enumeration of constraint status
+ *
+ * Enumeration of constraint status.
+ *
+ * You may add items to this list as needed. Be concise with names, and document your entries.
+ *
+ */
+typedef enum
+{
+ CTR_ACTIVE, ///< Constraint established with absolute reference.
+ CTR_INACTIVE ///< Constraint established with a frame (odometry).
+
+} ConstraintStatus;
+
/** \brief Enumeration of all possible state parametrizations
*
* Enumeration of all possible state parametrizations.
@@ -146,7 +187,6 @@ typedef enum
{
ST_ESTIMATED, ///< State in estimation (default)
ST_FIXED, ///< State fixed, estimated enough or fixed infrastructure.
- ST_REMOVED ///< Removed state. TODO: is it useful?
} StateStatus;
/** \brief Enumeration of all possible sensor types
@@ -199,12 +239,12 @@ typedef enum
* You may add items to this list as needed. Be concise with names, and document your entries.
*
*/
-typedef enum
+/*typedef enum
{
NOT_PENDING, ///< A point landmark, either 3D or 2D
ADD_PENDING, ///< A corner landmark (2D)
UPDATE_PENDING ///< A container landmark
-} PendingStatus;
+} PendingStatus;*/
/////////////////////////////////////////////////////////////////////////
// TYPEDEFS FOR POINTERS AND ITERATORS IN THE WOLF TREE
@@ -230,7 +270,7 @@ class RawLaser2D;
class SensorBase;
class SensorLaser2D;
class TransSensor;
-class StateBase;
+class StateBlock;
template<unsigned int SIZE> class StatePoint;
class PinHole;
@@ -289,14 +329,16 @@ typedef ConstraintBaseList::iterator ConstraintBaseIter;
// - Raw
// - Sensors
+typedef std::list<SensorBase*> SensorBaseList;
+typedef SensorBaseList::iterator SensorBaseIter;
// - transSensor
typedef std::map<unsigned int, TransSensor*> TransSensorMap;
typedef TransSensorMap::iterator TransSensorIter;
// - State
-typedef std::list<StateBase*> StateBaseList;
-typedef StateBaseList::iterator StateBaseIter;
+typedef std::list<StateBlock*> StateBlockList;
+typedef StateBlockList::iterator StateBaseIter;
// - Pin hole
diff --git a/src/wolf_manager.h b/src/wolf_manager.h
index 396c2e5e6e950493208013463aea40f60d4f10c4..2ca3563f63b3086d6a79f5de6aac520990d8aac9 100644
--- a/src/wolf_manager.h
+++ b/src/wolf_manager.h
@@ -29,7 +29,7 @@
#include "capture_odom_2D.h"
#include "capture_gps_fix.h"
#include "capture_laser_2D.h"
-#include "state_base.h"
+#include "state_block.h"
#include "constraint_sparse.h"
#include "constraint_gps_2D.h"
#include "constraint_odom_2D.h"
@@ -37,12 +37,12 @@
#include "map_base.h"
#include "wolf_problem.h"
-template <class StatePositionT, class StateOrientationT, class StateVelocityT=NoState, class StateOmegaT=NoState>
class WolfManager
{
protected:
//sets the problem
WolfProblem* problem_;
+ FrameStructure frame_structure_;
//pointer to a sensor providing predictions
SensorBase* sensor_prior_;
@@ -50,21 +50,22 @@ class WolfManager
//auxiliar/temporary iterators, frames and captures
FrameBaseIter first_window_frame_;
FrameBase* current_frame_;
- FrameBase* previous_frame_;
+ FrameBase* last_key_frame_;
CaptureMotion* last_capture_relative_;
CaptureMotion* second_last_capture_relative_;
std::queue<CaptureBase*> new_captures_;
//Manager parameters
- unsigned int window_size_;
+ unsigned int trajectory_size_;
WolfScalar new_frame_elapsed_time_;
public:
- WolfManager(const unsigned int& _state_length,
- SensorBase* _sensor_prior,
- const Eigen::VectorXs& _init_frame,
- const Eigen::MatrixXs& _init_frame_cov,
- const unsigned int& _w_size = 10,
+ WolfManager(const FrameStructure _frame_structure,
+ const unsigned int& _state_length,
+ SensorBase* _sensor_prior_ptr,
+ const Eigen::VectorXs& _prior,
+ const Eigen::MatrixXs& _prior_cov,
+ const unsigned int& _trajectory_size = 10,
const WolfScalar& _new_frame_elapsed_time = 0.1);
virtual ~WolfManager();
@@ -73,6 +74,8 @@ class WolfManager
void createFrame(const TimeStamp& _time_stamp);
+ void addSensor(SensorBase* _sensor_ptr);
+
void addCapture(CaptureBase* _capture);
void manageWindow();
@@ -89,243 +92,3 @@ class WolfManager
void setNewFrameElapsedTime(const WolfScalar& _dt);
};
-
-
-//////////////////////////////////////////
-// IMPLEMENTATION
-//////////////////////////////////////////
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::WolfManager(const unsigned int& _state_length,
- SensorBase* _sensor_prior,
- const Eigen::VectorXs& _init_frame,
- const Eigen::MatrixXs& _init_frame_cov,
- const unsigned int& _w_size,
- const WolfScalar& _new_frame_elapsed_time) :
-
- problem_(new WolfProblem(_state_length)),
- sensor_prior_(_sensor_prior),
- current_frame_(nullptr),
- previous_frame_(nullptr),
- last_capture_relative_(nullptr),
- window_size_(_w_size),
- new_frame_elapsed_time_(_new_frame_elapsed_time)
-{
- assert( _init_frame.size() == StatePositionT::BLOCK_SIZE + StateOrientationT::BLOCK_SIZE &&
- _init_frame_cov.cols() == StatePositionT::BLOCK_SIZE + StateOrientationT::BLOCK_SIZE &&
- _init_frame_cov.rows() == StatePositionT::BLOCK_SIZE + StateOrientationT::BLOCK_SIZE &&
- "Wrong init_frame state vector or covariance matrix size");
-
- std::cout << "initializing wolfmanager" << std::endl;
-
- // Initial frame
- createFrame(_init_frame, TimeStamp(0));
- first_window_frame_ = problem_->getTrajectoryPtr()->getFrameListPtr()->begin();
-
- // Initial covariance
- CaptureFix* initial_covariance = new CaptureFix(TimeStamp(0), _init_frame, _init_frame_cov);
- current_frame_->addCapture(initial_covariance);
- initial_covariance->processCapture();
-
- // Current robot frame
- createFrame(_init_frame, TimeStamp(0));
-
- std::cout << " wolfmanager initialized" << std::endl;
-}
-
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::~WolfManager()
-{
- delete problem_;
-}
-
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::createFrame(const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp)
-{
- //std::cout << "creating new frame..." << std::endl;
-
- assert(_frame_state.size() == StatePositionT::BLOCK_SIZE + StateOrientationT::BLOCK_SIZE + StateVelocityT::BLOCK_SIZE + StateOmegaT::BLOCK_SIZE && "Wrong frame vector when creating a new frame");
-
- // Process current frame non-odometry captures
- if (current_frame_ != nullptr)
- {
- //std::cout << "Processing last frame non-odometry captures " << current_frame_->getCaptureListPtr()->size() << std::endl;
- for (auto capture_it = current_frame_->getCaptureListPtr()->begin(); capture_it != current_frame_->getCaptureListPtr()->end(); capture_it++)
- if ((*capture_it)->getSensorPtr() != sensor_prior_)
- {
- //std::cout << "processing capture " << (*capture_it)->nodeId() << std::endl;
- (*capture_it)->processCapture();
- }
- }
- //std::cout << "Last frame non-odometry captures processed" << std::endl;
-
- // Store last frame
- previous_frame_ = current_frame_;
-
- // Create frame and add it to the trajectory
- StatePositionT* new_position = new StatePositionT(problem_->getNewStatePtr());
- problem_->addState(new_position, _frame_state.head<StatePositionT::BLOCK_SIZE>());
- //std::cout << "StatePosition created" << std::endl;
-
- StateOrientationT* new_orientation = new StateOrientationT(problem_->getNewStatePtr());
- problem_->addState(new_orientation, _frame_state.segment<StateOrientationT::BLOCK_SIZE>(new_position->getStateSize()));
- //std::cout << "StateBase created" << std::endl;
-
- StateVelocityT* new_velocity = nullptr;
- if (StateVelocityT::BLOCK_SIZE != 0)
- {
- new_velocity = new StateVelocityT(problem_->getNewStatePtr());
- problem_->addState(new_velocity, _frame_state.segment<StateVelocityT::BLOCK_SIZE>(new_position->getStateSize()+new_orientation->getStateSize()));
- }
-
- StateOmegaT* new_omega = nullptr;
- if (StateOmegaT::BLOCK_SIZE != 0)
- {
- new_omega = new StateOmegaT(problem_->getNewStatePtr());
- problem_->addState(new_omega, _frame_state.segment<StateOmegaT::BLOCK_SIZE>(new_position->getStateSize()+new_orientation->getStateSize()+new_velocity->getStateSize()));
- }
-
- problem_->getTrajectoryPtr()->addFrame(new FrameBase(_time_stamp, new_position, new_orientation, new_velocity, new_omega));
- //std::cout << "frame created" << std::endl;
-
- // Store new current frame
- current_frame_ = problem_->getLastFramePtr();
- //std::cout << "current_frame_" << std::endl;
-
- // empty last capture relative
- if (previous_frame_ != nullptr)
- {
- CaptureMotion* empty_odom = new CaptureOdom2D(_time_stamp, _time_stamp, sensor_prior_, Eigen::Vector3s::Zero());
- previous_frame_->addCapture(empty_odom);
- empty_odom->processCapture();
- last_capture_relative_ = empty_odom;
- }
- //std::cout << "last_frame_" << std::endl;
-
- // Fixing or removing old frames
- manageWindow();
- //std::cout << "new frame created" << std::endl;
-}
-
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::createFrame(const TimeStamp& _time_stamp)
-{
- //std::cout << "creating new frame from prior..." << std::endl;
- createFrame(last_capture_relative_->computePrior(_time_stamp), _time_stamp);
-}
-
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::addCapture(CaptureBase* _capture)
-{
- new_captures_.push(_capture);
- //std::cout << "added new capture: " << _capture->nodeId() << " stamp: ";
- //_capture->getTimeStamp().print();
- //std::cout << std::endl;
-}
-
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::manageWindow()
-{
- //std::cout << "managing window..." << std::endl;
- // WINDOW of FRAMES (remove or fix old frames)
- if (problem_->getTrajectoryPtr()->getFrameListPtr()->size() > window_size_+1)
- {
- //std::cout << "first_window_frame_ " << (*first_window_frame_)->nodeId() << std::endl;
- //problem_->getTrajectoryPtr()->removeFrame(problem_->getTrajectoryPtr()->getFrameListPtr()->begin());
- (*first_window_frame_)->fix();
- first_window_frame_++;
- }
- //std::cout << "window managed" << std::endl;
-}
-
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-bool WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::checkNewFrame(CaptureBase* new_capture)
-{
- //std::cout << "checking if new frame..." << std::endl;
- // TODO: not only time, depending on the sensor...
- //std::cout << new_capture->getTimeStamp().get() - last_frame_->getTimeStamp().get() << std::endl;
- return new_capture->getTimeStamp().get() - previous_frame_->getTimeStamp().get() > new_frame_elapsed_time_;
-}
-
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::update()
-{
- //std::cout << "updating..." << std::endl;
- while (!new_captures_.empty())
- {
- // EXTRACT NEW CAPTURE
- CaptureBase* new_capture = new_captures_.front();
- new_captures_.pop();
-
- // OVERWRITE CURRENT STAMP
- current_frame_->setTimeStamp(new_capture->getTimeStamp());
-
- // INITIALIZE FIRST FRAME STAMP
- if (previous_frame_->getTimeStamp().get() == 0)
- previous_frame_->setTimeStamp(new_capture->getTimeStamp());
-
- // ODOMETRY SENSOR
- if (new_capture->getSensorPtr() == sensor_prior_)
- {
- //std::cout << "adding odometry capture..." << new_capture->nodeId() << std::endl;
- // NEW KEY FRAME ?
- if (checkNewFrame(new_capture))
- createFrame(new_capture->getTimeStamp());
-
- // ADD/INTEGRATE NEW ODOMETRY TO THE LAST FRAME
- last_capture_relative_->integrateCapture((CaptureMotion*) (new_capture));
- current_frame_->setState(last_capture_relative_->computePrior(new_capture->getTimeStamp()));
- current_frame_->setTimeStamp(new_capture->getTimeStamp());
- }
- else
- {
- //std::cout << "adding not odometry capture..." << new_capture->nodeId() << std::endl;
- // NEW KEY FRAME ?
- if (checkNewFrame(new_capture))
- createFrame(new_capture->getTimeStamp());
-
- // ADD CAPTURE TO THE CURRENT FRAME (or substitute the same sensor previous capture)
- //std::cout << "searching repeated capture..." << new_capture->nodeId() << std::endl;
- CaptureBaseIter repeated_capture_it = current_frame_->hasCaptureOf(new_capture->getSensorPtr());
-
- if (repeated_capture_it != current_frame_->getCaptureListPtr()->end()) // repeated capture
- {
- //std::cout << "repeated capture, keeping new capture" << new_capture->nodeId() << std::endl;
- current_frame_->removeCapture(repeated_capture_it);
- current_frame_->addCapture(new_capture);
- }
- else
- {
- //std::cout << "not repeated, adding capture..." << new_capture->nodeId() << std::endl;
- current_frame_->addCapture(new_capture);
- }
- }
- }
- //std::cout << "updated" << std::endl;
-}
-
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-Eigen::VectorXs WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::getVehiclePose(const TimeStamp& _now)
-{
- if (last_capture_relative_ == nullptr)
- return Eigen::Map<Eigen::Vector3s>(current_frame_->getPPtr()->getPtr());
- else
- return last_capture_relative_->computePrior(_now);
-}
-
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-WolfProblem* WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::getProblemPtr()
-{
- return problem_;
-}
-
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::setWindowSize(const unsigned int& _size)
-{
- window_size_ = _size;
-}
-
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::setNewFrameElapsedTime(const WolfScalar& _dt)
-{
- new_frame_elapsed_time_ = _dt;
-}
diff --git a/src/wolf_problem.cpp b/src/wolf_problem.cpp
index 7421882952e6802f11fb31ce2ff3ccfae89a3eb9..d1f8d36ca742cd330f75e509105a1546936378e1 100644
--- a/src/wolf_problem.cpp
+++ b/src/wolf_problem.cpp
@@ -2,96 +2,96 @@
WolfProblem::WolfProblem(unsigned int _size) :
NodeBase("WOLF_PROBLEM"), //
- state_(_size),
covariance_(_size,_size),
- state_idx_last_(0),
- location_(TOP),
+ location_(TOP),
+ trajectory_ptr_(new TrajectoryBase),
map_ptr_(new MapBase),
- trajectory_ptr_(new TrajectoryBase),
- reallocated_(false)
+ hardware_ptr_(new HardwareBase)
{
-// map_ptr_ = new MapBase;
-// trajectory_ptr_ = new TrajectoryBase;
- map_ptr_->linkToUpperNode( this );
- trajectory_ptr_->linkToUpperNode( this );
+ trajectory_ptr_->linkToUpperNode( this );
+ map_ptr_->linkToUpperNode( this );
+ hardware_ptr_->linkToUpperNode( this );
}
-WolfProblem::WolfProblem(TrajectoryBase* _trajectory_ptr, MapBase* _map_ptr, unsigned int _size) :
+WolfProblem::WolfProblem(TrajectoryBase* _trajectory_ptr, MapBase* _map_ptr, HardwareBase* _hardware_ptr, unsigned int _size) :
NodeBase("WOLF_PROBLEM"), //
- state_(_size),
- covariance_(_size,_size),
- state_idx_last_(0),
- location_(TOP),
+ covariance_(_size,_size),
+ location_(TOP),
+ trajectory_ptr_(_trajectory_ptr==nullptr ? new TrajectoryBase : _trajectory_ptr),
map_ptr_(_map_ptr==nullptr ? new MapBase : _map_ptr),
- trajectory_ptr_(_trajectory_ptr==nullptr ? new TrajectoryBase : _trajectory_ptr),
- reallocated_(false)
+ hardware_ptr_(_hardware_ptr==nullptr ? new HardwareBase : _hardware_ptr)
{
+ trajectory_ptr_->linkToUpperNode( this );
map_ptr_->linkToUpperNode( this );
- trajectory_ptr_->linkToUpperNode( this );
+ hardware_ptr_->linkToUpperNode( this );
}
WolfProblem::~WolfProblem()
{
delete trajectory_ptr_;
- delete map_ptr_;
+ delete map_ptr_;
+ delete hardware_ptr_;
}
-bool WolfProblem::addState(StateBase* _new_state_ptr, const Eigen::VectorXs& _new_state_values)
+void WolfProblem::addStateBlockPtr(StateBlock* _state_ptr)
{
- // Check if resize should be done
- if (state_idx_last_+_new_state_ptr->getStateSize() > state_.size())
- {
- std::cout << "Resizing state and updating asl state units pointers..." << std::endl;
- std::cout << "\nState size: " << state_.size() << " last idx: " << state_idx_last_ << " last idx + new state size: " << state_idx_last_+_new_state_ptr->getStateSize() << std::endl;
- WolfScalar* old_first_pointer = state_.data();
- state_.conservativeResize(state_.size()*2);
- covariance_.conservativeResize(state_.size()*2,state_.size()*2);
- for (auto state_units_it = state_list_.begin(); state_units_it != state_list_.end(); state_units_it++)
- {
- //std::cout << "state unit: " << (*state_units_it)->nodeId() << std::endl;
- (*state_units_it)->setPtr(state_.data() + ( (*state_units_it)->getPtr() - old_first_pointer) );
- }
- std::cout << "----------------------------- difference of location: " << old_first_pointer - state_.data() << std::endl;
- _new_state_ptr->setPtr(state_.data()+state_idx_last_);
- std::cout << "New state size: " << state_.size() << " last idx: " << state_idx_last_ << std::endl;
- reallocated_ = true;
- }
- //std::cout << "\nnew state unit: " << _new_state_values.transpose() << std::endl;
- //std::cout << "\nPrev state: " << state_.segment(0,state_idx_last_).transpose() << std::endl;
-
- // copy the values of the new state
- assert(_new_state_values.size() == _new_state_ptr->getStateSize() && "Different state unit and vector sizes");
- state_.segment(state_idx_last_,_new_state_ptr->getStateSize()) = _new_state_values;
-
// add the state unit to the list
- state_list_.push_back(_new_state_ptr);
+ state_block_ptr_list_.push_back(_state_ptr);
+ state_idx_map_[_state_ptr] = covariance_.rows();
- // update the last state index
- state_idx_last_ += _new_state_ptr->getStateSize();
+ // Resize Covariance
+ covariance_.conservativeResize(covariance_.rows() + _state_ptr->getSize(), covariance_.cols() + _state_ptr->getSize());
+ // queue for solver manager
+ state_block_add_list_.push_back(_state_ptr);
+}
+
+void WolfProblem::updateStateBlockPtr(StateBlock* _state_ptr)
+{
+ // queue for solver manager
+ state_block_update_list_.push_back(_state_ptr);
+}
+
+void WolfProblem::removeStateBlockPtr(StateBlock* _state_ptr)
+{
+ // add the state unit to the list
+ state_block_ptr_list_.remove(_state_ptr);
+ state_idx_map_.erase(_state_ptr);
- //std::cout << "\nPost state: " << state_.segment(0,state_idx_last_).transpose() << std::endl;
- return reallocated_;
+ // Resize Covariance
+ covariance_.conservativeResize(covariance_.rows() - _state_ptr->getSize(), covariance_.cols() - _state_ptr->getSize());
+ // queue for solver manager
+ state_block_remove_list_.push_back(_state_ptr->getPtr());
}
-void WolfProblem::addCovarianceBlock(StateBase* _state1, StateBase* _state2, const Eigen::MatrixXs& _cov)
+void WolfProblem::addConstraintPtr(ConstraintBase* _constraint_ptr)
+{
+ // queue for solver manager
+ constraint_add_list_.push_back(_constraint_ptr);
+}
+
+void WolfProblem::removeConstraintPtr(ConstraintBase* _constraint_ptr)
+{
+ // queue for solver manager
+ constraint_remove_list_.remove(_constraint_ptr->nodeId());
+}
+
+void WolfProblem::addCovarianceBlock(StateBlock* _state1, StateBlock* _state2, const Eigen::MatrixXs& _cov)
{
assert(_state1 != nullptr);
- assert(_state1->getPtr() != nullptr);
- assert(_state1->getPtr() < state_.data() + state_idx_last_);
- assert(_state1->getPtr() > state_.data());
+ assert(state_idx_map_.find(_state1) != state_idx_map_.end());
+ assert(state_idx_map_.at(_state1) + _state1->getSize() <= (unsigned int) covariance_.rows());
assert(_state2 != nullptr);
- assert(_state2->getPtr() != nullptr);
- assert(_state2->getPtr() < state_.data() + state_idx_last_);
- assert(_state2->getPtr() > state_.data());
+ assert(state_idx_map_.find(_state2) != state_idx_map_.end());
+ assert(state_idx_map_.at(_state2) + _state2->getSize() <= (unsigned int) covariance_.rows());
// Guarantee that we are updating the top triangular matrix (in cross covariance case)
- bool flip = _state1->getPtr() > _state2->getPtr();
- StateBase* stateA = (flip ? _state2 : _state1);
- StateBase* stateB = (flip ? _state1 : _state2);
- unsigned int row = (stateA->getPtr() - state_.data());
- unsigned int col = (stateB->getPtr() - state_.data());
- unsigned int block_rows = stateA->getStateSize();
- unsigned int block_cols = stateB->getStateSize();
+ bool flip = state_idx_map_.at(_state1) > state_idx_map_.at(_state2);
+ StateBlock* stateA = (flip ? _state2 : _state1);
+ StateBlock* stateB = (flip ? _state1 : _state2);
+ unsigned int row = state_idx_map_.at(stateA);
+ unsigned int col = state_idx_map_.at(stateB);
+ unsigned int block_rows = stateA->getSize();
+ unsigned int block_cols = stateB->getSize();
assert( block_rows == (flip ? _cov.cols() : _cov.rows()) && block_cols == (flip ? _cov.rows() : _cov.cols()) && "Bad covariance size in WolfProblem::addCovarianceBlock");
@@ -101,50 +101,46 @@ void WolfProblem::addCovarianceBlock(StateBase* _state1, StateBase* _state2, con
covariance_.coeffRef(i+row,j+col) = (flip ? _cov(j,i) : _cov(i,j));
}
-void WolfProblem::getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eigen::MatrixXs& _cov_block) const
+void WolfProblem::getCovarianceBlock(StateBlock* _state1, StateBlock* _state2, Eigen::MatrixXs& _cov_block) const
{
assert(_state1 != nullptr);
- assert(_state1->getPtr() != nullptr);
- assert(_state1->getPtr() < state_.data() + state_idx_last_);
- assert(_state1->getPtr() > state_.data());
+ assert(state_idx_map_.find(_state1) != state_idx_map_.end());
+ assert(state_idx_map_.at(_state1) + _state1->getSize() <= (unsigned int) covariance_.rows());
assert(_state2 != nullptr);
- assert(_state2->getPtr() != nullptr);
- assert(_state2->getPtr() < state_.data() + state_idx_last_);
- assert(_state2->getPtr() > state_.data());
-
- // Guarantee that we are getting the top triangular matrix (in cross covariance case)
- bool flip = _state1->getPtr() > _state2->getPtr();
- StateBase* stateA = (flip ? _state2 : _state1);
- StateBase* stateB = (flip ? _state1 : _state2);
- unsigned int row = (stateA->getPtr() - state_.data());
- unsigned int col = (stateB->getPtr() - state_.data());
- unsigned int block_rows = stateA->getStateSize();
- unsigned int block_cols = stateB->getStateSize();
+ assert(state_idx_map_.find(_state2) != state_idx_map_.end());
+ assert(state_idx_map_.at(_state2) + _state2->getSize() <= (unsigned int) covariance_.rows());
+
+ // Guarantee that we are updating the top triangular matrix (in cross covariance case)
+ bool flip = state_idx_map_.at(_state1) > state_idx_map_.at(_state2);
+ StateBlock* stateA = (flip ? _state2 : _state1);
+ StateBlock* stateB = (flip ? _state1 : _state2);
+ unsigned int row = state_idx_map_.at(stateA);
+ unsigned int col = state_idx_map_.at(stateB);
+ unsigned int block_rows = stateA->getSize();
+ unsigned int block_cols = stateB->getSize();
assert(_cov_block.rows() == (flip ? block_cols : block_rows) && _cov_block.cols() == (flip ? block_rows : block_cols) && "Bad _cov_block matrix sizes");
_cov_block = (flip ? Eigen::MatrixXs(covariance_.block(row, col, block_rows, block_cols)) : Eigen::MatrixXs(covariance_.block(row, col, block_rows, block_cols)).transpose() );
}
-void WolfProblem::getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eigen::MatrixXs& _cov, const int _row, const int _col) const
+void WolfProblem::getCovarianceBlock(StateBlock* _state1, StateBlock* _state2, Eigen::MatrixXs& _cov, const int _row, const int _col) const
{
assert(_state1 != nullptr);
- assert(_state1->getPtr() != nullptr);
- assert(_state1->getPtr() < state_.data() + state_idx_last_);
- assert(_state1->getPtr() > state_.data());
+ assert(state_idx_map_.find(_state1) != state_idx_map_.end());
+ assert(state_idx_map_.at(_state1) + _state1->getSize() <= (unsigned int) covariance_.rows());
assert(_state2 != nullptr);
- assert(_state2->getPtr() != nullptr);
- assert(_state2->getPtr() < state_.data() + state_idx_last_);
- assert(_state2->getPtr() > state_.data());
-
- // Guarantee that we are getting the top triangular matrix (in cross covariance case)
- bool flip = _state1->getPtr() > _state2->getPtr();
- StateBase* stateA = (flip ? _state2 : _state1);
- StateBase* stateB = (flip ? _state1 : _state2);
- unsigned int row = (stateA->getPtr() - state_.data());
- unsigned int col = (stateB->getPtr() - state_.data());
- unsigned int block_rows = stateA->getStateSize();
- unsigned int block_cols = stateB->getStateSize();
+ assert(state_idx_map_.find(_state2) != state_idx_map_.end());
+ assert(state_idx_map_.at(_state2) + _state2->getSize() <= (unsigned int) covariance_.rows());
+
+ // Guarantee that we are updating the top triangular matrix (in cross covariance case)
+ bool flip = state_idx_map_.at(_state1) > state_idx_map_.at(_state2);
+ StateBlock* stateA = (flip ? _state2 : _state1);
+ StateBlock* stateB = (flip ? _state1 : _state2);
+ unsigned int row = state_idx_map_.at(stateA);
+ unsigned int col = state_idx_map_.at(stateB);
+ unsigned int block_rows = stateA->getSize();
+ unsigned int block_cols = stateB->getSize();
// std::cout << "flip " << flip << std::endl;
// std::cout << "_row " << _row << std::endl;
@@ -162,31 +158,9 @@ void WolfProblem::getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eig
_cov.block(_row,_col,block_cols,block_rows) = Eigen::MatrixXs(covariance_.block(row, col, block_rows, block_cols)).transpose();
}
-void WolfProblem::removeState(StateBase* _state_ptr)
-{
- // TODO: Reordering? Mandatory for filtering...
- state_list_.remove(_state_ptr);
- removed_state_ptr_list_.push_back(_state_ptr->getPtr());
- delete _state_ptr;
-}
-
-WolfScalar* WolfProblem::getStatePtr()
-{
- return state_.data();
-}
-
-WolfScalar* WolfProblem::getNewStatePtr()
-{
- return state_.data()+state_idx_last_;
-}
-
-const unsigned int WolfProblem::getStateSize() const
-{
- return state_idx_last_;
-}
-
void WolfProblem::addMap(MapBase* _map_ptr)
{
+ // TODO: not necessary but update map maybe..
map_ptr_ = _map_ptr;
map_ptr_->linkToUpperNode( this );
}
@@ -207,19 +181,19 @@ TrajectoryBase* WolfProblem::getTrajectoryPtr()
return trajectory_ptr_;
}
-FrameBase* WolfProblem::getLastFramePtr()
+HardwareBase* WolfProblem::getHardwarePtr()
{
- return trajectory_ptr_->getLastFramePtr();
+ return hardware_ptr_;
}
-StateBaseList* WolfProblem::getStateListPtr()
+FrameBase* WolfProblem::getLastFramePtr()
{
- return &state_list_;
+ return trajectory_ptr_->getLastFramePtr();
}
-std::list<WolfScalar*>* WolfProblem::getRemovedStateListPtr()
+StateBlockList* WolfProblem::getStateListPtr()
{
- return &removed_state_ptr_list_;
+ return &state_block_ptr_list_;
}
void WolfProblem::print(unsigned int _ntabs, std::ostream& _ost) const
@@ -235,20 +209,29 @@ void WolfProblem::printSelf(unsigned int _ntabs, std::ostream& _ost) const
_ost << "TOP" << std::endl;
}
-const Eigen::VectorXs WolfProblem::getState() const
+std::list<StateBlock*>* WolfProblem::getStateBlockAddList()
{
- return state_;
+ return &state_block_add_list_;
}
-bool WolfProblem::isReallocated() const
+std::list<StateBlock*>* WolfProblem::getStateBlockUpdateList()
{
- return reallocated_;
+ return &state_block_update_list_;
}
+std::list<WolfScalar*>* WolfProblem::getStateBlockRemoveList()
+{
+ return &state_block_remove_list_;
+}
+
+std::list<ConstraintBase*>* WolfProblem::getConstraintAddList()
+{
+ return &constraint_add_list_;
+}
-void WolfProblem::reallocationDone()
+std::list<unsigned int>* WolfProblem::getConstraintRemoveList()
{
- reallocated_ = false;
+ return &constraint_remove_list_;
}
WolfProblem* WolfProblem::getTop()
diff --git a/src/wolf_problem.h b/src/wolf_problem.h
index 88c5176ab99ba6820011ab6a118ac54955117ef8..b276edfef15a062ea19a3016e0362aa2cbe5bcc3 100644
--- a/src/wolf_problem.h
+++ b/src/wolf_problem.h
@@ -1,10 +1,12 @@
#ifndef WOLF_PROBLEM_H_
#define WOLF_PROBLEM_H_
-//wof includes
+
+//wolf includes
#include "node_base.h"
#include "map_base.h"
#include "trajectory_base.h"
+#include "hardware_base.h"
#include "wolf.h"
/** \brief Wolf problem node element in the Wolf Tree
@@ -20,16 +22,18 @@
class WolfProblem: public NodeBase
{
protected:
- Eigen::VectorXs state_;
Eigen::SparseMatrix<WolfScalar> covariance_;
- unsigned int state_idx_last_;
NodeLocation location_;// TODO: should it be in node_base?
- MapBase* map_ptr_;
TrajectoryBase* trajectory_ptr_;
- //TODO: SensorBaseList sensor_list_;
- StateBaseList state_list_;
- std::list<WolfScalar*> removed_state_ptr_list_;
- bool reallocated_;
+ MapBase* map_ptr_;
+ HardwareBase* hardware_ptr_;
+ StateBlockList state_block_ptr_list_;
+ std::list<StateBlock*> state_block_add_list_;
+ std::list<StateBlock*> state_block_update_list_;
+ std::list<WolfScalar*> state_block_remove_list_;
+ std::list<ConstraintBase*> constraint_add_list_;
+ std::list<unsigned int> constraint_remove_list_;
+ std::map<StateBlock*,unsigned int> state_idx_map_;
public:
@@ -45,7 +49,7 @@ class WolfProblem: public NodeBase
* Constructor from map and trajectory shared pointers
*
*/
- WolfProblem(TrajectoryBase* _trajectory_ptr, MapBase* _map_ptr=nullptr, unsigned int _size=1e3);
+ WolfProblem(TrajectoryBase* _trajectory_ptr, MapBase* _map_ptr=nullptr, HardwareBase* _hardware_ptr=nullptr, unsigned int _size=1e3);
/** \brief Default destructor
*
@@ -54,48 +58,55 @@ class WolfProblem: public NodeBase
*/
virtual ~WolfProblem();
- /** \brief Adds a new state unit to the state
+ /** \brief Adds a new state block to be added to solver manager
*
- * Adds a new state unit to the state. Returns true if a remapping has been done.
+ * Adds a new state block to be added to solver manager
*
*/
- bool addState(StateBase* _new_state, const Eigen::VectorXs& _new_state_values);
+ void addStateBlockPtr(StateBlock* _state_ptr);
- /** \brief Adds a new covariance block
+ /** \brief Adds a new state block to be updated to solver manager
*
- * Adds a new covariance block
+ * Adds a new state block to be updated to solver manager
*
*/
- void addCovarianceBlock(StateBase* _state1, StateBase* _state2, const Eigen::MatrixXs& _cov);
+ void updateStateBlockPtr(StateBlock* _state_ptr);
- /** \brief Gets a covariance block
+ /** \brief Adds a state block to be removed to solver manager
*
- * Gets a covariance block
+ * Adds a state block to be removed to solver manager
*
*/
- void getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eigen::MatrixXs& _cov_block) const;
- void getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eigen::MatrixXs& _cov, const int _row, const int _col) const;
+ void removeStateBlockPtr(StateBlock* _state_ptr);
- /** \brief Removes a new state unit of the state
- *
- * Removes a new state unit of the state
- *
- */
- void removeState(StateBase* _state);
+ /** \brief Adds a new constraint to be added to solver manager
+ *
+ * Adds a new constraint to be added to solver manager
+ *
+ */
+ void addConstraintPtr(ConstraintBase* _constraint_ptr);
+
+ /** \brief Adds a constraint to be removed to solver manager
+ *
+ * Adds a constraint to be removed to solver manager
+ *
+ */
+ void removeConstraintPtr(ConstraintBase* _constraint_ptr);
- /** \brief Gets a pointer to the state first position
+ /** \brief Adds a new covariance block
*
- * Gets a pointer to the state first position
+ * Adds a new covariance block
*
*/
- WolfScalar* getStatePtr();
+ void addCovarianceBlock(StateBlock* _state1, StateBlock* _state2, const Eigen::MatrixXs& _cov);
- /** \brief Gets a pointer where a new state unit should be located
+ /** \brief Gets a covariance block
*
- * Gets a pointer where a new state unit should be located
+ * Gets a covariance block
*
*/
- WolfScalar* getNewStatePtr();
+ void getCovarianceBlock(StateBlock* _state1, StateBlock* _state2, Eigen::MatrixXs& _cov_block) const;
+ void getCovarianceBlock(StateBlock* _state1, StateBlock* _state2, Eigen::MatrixXs& _cov, const int _row, const int _col) const;
/** \brief Gets state size
*
@@ -104,13 +115,6 @@ class WolfProblem: public NodeBase
*/
const unsigned int getStateSize() const;
- /** \brief Sets the index of the last occupied position of the state
- *
- * Sets the index of the last occupied position of the state
- *
- */
- //void setStateIdx(unsigned int _idx);
-
/** \brief Adds a map
*
* Adds a map
@@ -123,21 +127,14 @@ class WolfProblem: public NodeBase
* Adds a trajectory
*
*/
- void addTrajectory(TrajectoryBase* _Trajectory_ptr);
-
- /** \brief Gets a reference to map
- *
- * Gets a reference to map
- *
- */
- //MapBase& getMap() const;
+ void addTrajectory(TrajectoryBase* _trajectory_ptr);
- /** \brief Gets a reference to Trajectory
+ /** \brief Adds a hardware
*
- * Gets a reference to Trajectory
+ * Adds a hardware
*
*/
- //TrajectoryBase& getTrajectory() const;
+ void addHarware(HardwareBase* _hardware_ptr);
/** \brief Gets a pointer to map
*
@@ -146,13 +143,20 @@ class WolfProblem: public NodeBase
*/
MapBase* getMapPtr();
- /** \brief Gets a pointer to map
+ /** \brief Gets a pointer to trajectory
*
- * Gets a pointer to map
+ * Gets a pointer to trajectory
*
*/
TrajectoryBase* getTrajectoryPtr();
+ /** \brief Gets a pointer to Hardware
+ *
+ * Gets a pointer to Hardware
+ *
+ */
+ HardwareBase* getHardwarePtr();
+
/** \brief Returns a pointer to last Frame
*
* Returns a pointer to last Frame
@@ -165,35 +169,42 @@ class WolfProblem: public NodeBase
* Gets a pointer to the state units list
*
*/
- StateBaseList* getStateListPtr();
+ StateBlockList* getStateListPtr();
- /** \brief Gets a list of wolfscalar pointers contained by removed state units (in order to delete them in ceres)
+ /** \brief Gets a queue of state blocks to be added in the solver
*
- * Gets a list of wolfscalar pointers contained by removed state units (in order to delete them in ceres)
+ * Gets a queue of state blocks to be added in the solver
*
*/
- std::list<WolfScalar*>* getRemovedStateListPtr();
+ std::list<StateBlock*>* getStateBlockAddList();
- /** \brief Gets the state vector
- *
- * Gets the state vector
- *
- */
- const Eigen::VectorXs getState() const;
+ /** \brief Gets a queue of state blocks to be updated in the solver
+ *
+ * Gets a queue of state blocks to be updated in the solver
+ *
+ */
+ std::list<StateBlock*>* getStateBlockUpdateList();
- /** \brief Gets if the state has been reallocated
- *
- * Gets if the state has been reallocated
- *
- */
- bool isReallocated() const;
+ /** \brief Gets a queue of state blocks to be removed from the solver
+ *
+ * Gets a queue of state blocks to be removed from the solver
+ *
+ */
+ std::list<WolfScalar*>* getStateBlockRemoveList();
- /** \brief Turn off the reallocation flag
- *
- * Turn off the reallocation flag
- *
- */
- void reallocationDone();
+ /** \brief Gets a queue of constraint ids to be added in the solver
+ *
+ * Gets a queue of constraint ids to be added in the solver
+ *
+ */
+ std::list<ConstraintBase*>* getConstraintAddList();
+
+ /** \brief Gets a queue of constraint ids to be removed from the solver
+ *
+ * Gets a queue of constraint ids to be removed from the solver
+ *
+ */
+ std::list<unsigned int>* getConstraintRemoveList();
/** \brief get top node
*