diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index df0d03dade0ea04d825dc928ceb48462cb674e0a..e0cec08b265bbf0cbfc5950d2f7fea8dab36aa46 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -90,6 +90,11 @@ Eigen::VectorXs CaptureBase::getData()
return data_;
}
+Eigen::MatrixXs CaptureBase::getDataCovariance()
+{
+ return data_covariance_;
+}
+
void CaptureBase::setData(unsigned int _size, const WolfScalar *_data)
{
data_.resize(_size);
diff --git a/src/capture_base.h b/src/capture_base.h
index 0719a29478b57f70044f4fcdf3446fb0601ae85d..a19f47213f7d8eb811f589e80f110d36d08fbaeb 100644
--- a/src/capture_base.h
+++ b/src/capture_base.h
@@ -84,6 +84,8 @@ class CaptureBase : public NodeLinked<FrameBase,FeatureBase>
Eigen::VectorXs getData();
+ Eigen::MatrixXs getDataCovariance();
+
void setData(unsigned int _size, const WolfScalar *_data);
void setData(const Eigen::VectorXs& _data);
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index a8d632da7684277c83e9ce04e9a50569ff3d39ac..0626cd2a5a4433f736ee85ff395686bdebaddb1b 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -91,6 +91,7 @@ void CaptureOdom2D::integrateCapture(CaptureRelative* _new_capture)
assert(dynamic_cast<CaptureOdom2D*>(_new_capture) && "Trying to integrate with a CaptureOdom2D a CaptureRelativePtr which is not CaptureOdom2D");
data_(0) += _new_capture->getData()(0);
data_(1) += _new_capture->getData()(1);
+ data_covariance_ += _new_capture->getDataCovariance();
//std::cout << "integrated!" << std::endl;
}
diff --git a/src/constraint_corner_2D_theta.h b/src/constraint_corner_2D_theta.h
index 222769cb7f88acb87f81ce712b729c1803b46316..1ab409b7689ff3dc462b2d44ff37f099e4aa0ca5 100644
--- a/src/constraint_corner_2D_theta.h
+++ b/src/constraint_corner_2D_theta.h
@@ -35,7 +35,7 @@ class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1>
if (lmk_ptr_->getHits() == 1)
getTop()->getMapPtr()->removeLandmark(lmk_ptr_);
else
- lmk_ptr_->unHit();
+ lmk_ptr_->unhit();
}
LandmarkCorner2D* getLandmarkPtr()
diff --git a/src/examples/test_ceres_laser.cpp b/src/examples/test_ceres_laser.cpp
index 9202a97901884fd43de695fcceb200ae04215f71..93bec962d253f6a166c58876c513160a675c873e 100644
--- a/src/examples/test_ceres_laser.cpp
+++ b/src/examples/test_ceres_laser.cpp
@@ -44,21 +44,12 @@
//C includes for sleep, time and main args
#include "unistd.h"
-#include <time.h>
-#include <sys/time.h>
-
-//GLUT
-// #include <GL/glut.h>
//faramotics includes
-//#include "faramotics/window.h"
#include "faramotics/dynamicSceneRender.h"
#include "faramotics/rangeScan2D.h"
#include "btr-headers/pose3d.h"
-//namespaces
-using namespace std;
-
//function travel around
void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_)
{
@@ -117,11 +108,9 @@ class WolfManager
protected:
bool use_complex_angles_;
WolfProblem* problem_;
- std::vector<Eigen::VectorXs> odom_captures_;
- std::vector<Eigen::VectorXs> gps_captures_;
std::queue<CaptureBase*> new_captures_;
SensorBase* sensor_prior_;
- unsigned int last_state_units_, window_size_;
+ unsigned int window_size_;
FrameBaseIter first_window_frame_;
public:
@@ -129,7 +118,6 @@ class WolfManager
use_complex_angles_(_complex_angle),
problem_(new WolfProblem(_state_length)),
sensor_prior_(_sensor_prior),
- last_state_units_(0),
window_size_(_w_size)
{
Eigen::VectorXs init_frame(use_complex_angles_ ? 4 : 3);
@@ -149,30 +137,29 @@ class WolfManager
{
// Create frame and add it to the trajectory
StateBase* new_position = new StatePoint2D(problem_->getNewStatePtr());
- //new_position->setPendingStatus(NOT_PENDING);
problem_->addState(new_position, _frame_state.head(2));
StateBase* new_orientation;
if (use_complex_angles_)
- {
new_orientation = new StateComplexAngle(problem_->getNewStatePtr());
- //new_orientation->setPendingStatus(NOT_PENDING);
- problem_->addState(new_orientation, _frame_state.tail(2));
- }
else
- {
new_orientation = new StateTheta(problem_->getNewStatePtr());
- //new_orientation->setPendingStatus(NOT_PENDING);
- problem_->addState(new_orientation, _frame_state.tail(1));
- }
+
+ problem_->addState(new_orientation, _frame_state.tail(new_orientation->getStateSize()));
problem_->getTrajectoryPtr()->addFrame(new FrameBase(_time_stamp, new_position, new_orientation));
+
+ // add a zero odometry capture (in order to integrate)
+ problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new CaptureOdom2D(_time_stamp,
+ sensor_prior_,
+ Eigen::Vector2s::Zero(),
+ Eigen::Matrix2s::Zero()));
}
void addCapture(CaptureBase* _capture)
{
new_captures_.push(_capture);
- //std::cout << "added new capture: " << _capture->nodeId() << std::endl;
+ std::cout << "added new capture: " << _capture->nodeId() << std::endl;
}
void update()
@@ -198,23 +185,13 @@ class WolfManager
}
}
- // INTEGRATING/ADDING CAPTURE & COMPUTING PRIOR
- Eigen::VectorXs prior;
- if (previous_relative_capture == nullptr)
- {
- problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new_capture);
- //std::cout << "added capture: " << new_capture->nodeId() << std::endl;
- prior = new_capture->computePrior();
- }
- else
- {
- //std::cout << "integrating captures " << previous_relative_capture->nodeId() << " " << new_capture->nodeId() << std::endl;
- previous_relative_capture->integrateCapture((CaptureRelative*)(new_capture));
- prior = previous_relative_capture->computePrior();
- }
+ // INTEGRATING ODOMETRY CAPTURE & COMPUTING PRIOR
+ std::cout << "integrating captures " << previous_relative_capture->nodeId() << " " << new_capture->nodeId() << std::endl;
+ previous_relative_capture->integrateCapture((CaptureRelative*)(new_capture));
+ Eigen::VectorXs prior = previous_relative_capture->computePrior();
// NEW KEY FRAME (if enough time from last frame)
- //std::cout << "new TimeStamp - last Frame TimeStamp = " << new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() << std::endl;
+ std::cout << "new TimeStamp - last Frame TimeStamp = " << new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() << std::endl;
if (new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() > 0.1)
{
auto previous_frame_ptr = problem_->getTrajectoryPtr()->getFrameListPtr()->back();
@@ -230,7 +207,7 @@ class WolfManager
if (problem_->getTrajectoryPtr()->getFrameListPtr()->size() > window_size_)
{
//problem_->getTrajectoryPtr()->removeFrame(problem_->getTrajectoryPtr()->getFrameListPtr()->begin());
- fixFrame(*first_window_frame_);
+ (*first_window_frame_)->fix();
first_window_frame_++;
}
else
@@ -240,7 +217,7 @@ class WolfManager
else
{
// ADD CAPTURE TO THE LAST FRAME (or substitute the same sensor previous capture)
- //std::cout << "adding not odometry capture..." << std::endl;
+ std::cout << "adding not odometry capture..." << std::endl;
for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin(); capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++)
{
if ((*capture_it)->getSensorPtr() == new_capture->getSensorPtr())
@@ -255,23 +232,6 @@ class WolfManager
}
}
}
- // TODO: passar a fix() a frame_base i crear-hi unfix()
- void fixFrame(FrameBase* _fixed_frame)
- {
- //std::cout << "Fixing frame " << _fixed_frame->nodeId() << std::endl;
- //_fixed_frame->print();
- _fixed_frame->setStatus(ST_FIXED);
-
- // Frame State Units
- if (_fixed_frame->getPPtr())
- _fixed_frame->getPPtr()->setStateStatus(ST_FIXED);
- if (_fixed_frame->getOPtr())
- _fixed_frame->getOPtr()->setStateStatus(ST_FIXED);
- if (_fixed_frame->getVPtr())
- _fixed_frame->getVPtr()->setStateStatus(ST_FIXED);
- if (_fixed_frame->getWPtr())
- _fixed_frame->getWPtr()->setStateStatus(ST_FIXED);
- }
const Eigen::VectorXs getState() const
{
diff --git a/src/examples/test_ceres_manager.cpp b/src/examples/test_ceres_manager.cpp
index e8052861a90ae4d9f7594bb38e7388ccb0079f4a..7ed716701b3efef822494be5d9940c21b5971588 100644
--- a/src/examples/test_ceres_manager.cpp
+++ b/src/examples/test_ceres_manager.cpp
@@ -36,11 +36,6 @@
using namespace Eigen;
-class CaptureXBase;
-class ConstraintXBase;
-typedef std::shared_ptr<ConstraintXBase> ConstraintXShPtr;
-typedef std::shared_ptr<CaptureXBase> CaptureXShPtr;
-
class ConstraintXBase
{
protected:
@@ -158,7 +153,7 @@ class ConstraintGPS2D : public ConstraintSparse<2,2>
{
}
- ConstraintGPS2D(WolfScalar* _measurementPtr, const StateBaseShPtr& _statePtr) :
+ ConstraintGPS2D(WolfScalar* _measurementPtr, StateBase* _statePtr) :
ConstraintSparse<2,2>(_measurementPtr, _statePtr->getPtr())
{
}
@@ -197,7 +192,7 @@ class Constraint2DOdometry : public ConstraintSparse<2,2,2,2,2>
{
}
- Constraint2DOdometry(WolfScalar* _measurementPtr, const StateBaseShPtr& _state0Ptr, const StateBaseShPtr& _state1Ptr, const StateBaseShPtr& _state2Ptr, const StateBaseShPtr& _state3Ptr) :
+ Constraint2DOdometry(WolfScalar* _measurementPtr, StateBase* _state0Ptr, StateBase* _state1Ptr, StateBase* _state2Ptr, StateBase* _state3Ptr) :
ConstraintSparse<2,2,2,2,2>(_measurementPtr, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
{
}
@@ -243,7 +238,7 @@ class Constraint2DOdometryTheta : public ConstraintSparse<2,2,1,2,1>
{
}
- Constraint2DOdometryTheta(WolfScalar* _measurementPtr, const StateBaseShPtr& _state0Ptr, const StateBaseShPtr& _state1Ptr, const StateBaseShPtr& _state2Ptr, const StateBaseShPtr& _state3Ptr) :
+ Constraint2DOdometryTheta(WolfScalar* _measurementPtr, StateBase* _state0Ptr, StateBase* _state1Ptr, StateBase* _state2Ptr, StateBase* _state3Ptr) :
ConstraintSparse<2,2,1,2,1>(_measurementPtr, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
{
}
@@ -277,9 +272,9 @@ class CaptureXBase
public:
VectorXs capture;
TimeStamp time_stamp;
- SensorBaseShPtr sensor_ptr_; ///< Pointer to sensor
+ SensorBase* sensor_ptr_; ///< Pointer to sensor
- CaptureXBase(const VectorXs& _capture, const WolfScalar& _time_stamp, const SensorBaseShPtr& _sensor_ptr) :
+ CaptureXBase(const VectorXs& _capture, const WolfScalar& _time_stamp, SensorBase* _sensor_ptr) :
capture(_capture),
time_stamp(_time_stamp),
sensor_ptr_(_sensor_ptr)
@@ -307,12 +302,12 @@ class WolfManager
VectorXs state_;
unsigned int first_empty_state_;
bool use_complex_angles_;
- std::vector<FrameBaseShPtr> frames_;
- std::vector<ConstraintXShPtr> constraints_;
+ std::vector<FrameBase*> frames_;
+ std::vector<ConstraintXBase*> constraints_;
std::vector<VectorXs> odom_captures_;
std::vector<VectorXs> gps_captures_;
- std::queue<CaptureXShPtr> new_captures_;
- std::vector<CaptureXShPtr> captures_;
+ std::queue<CaptureXBase*> new_captures_;
+ std::vector<CaptureXBase*> captures_;
public:
WolfManager(const unsigned int& _state_length=1000, const bool _complex_angle=false) :
@@ -361,29 +356,29 @@ class WolfManager
// frames_.push_back(FrameBaseShPtr(new FrameBase(nullptr, _time_stamp,
// StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
// StateBaseShPtr(new StateComplexAngle(state_.data()+first_empty_state_+2)))));
- frames_.push_back(FrameBaseShPtr(new FrameBase(_time_stamp,
- StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
- StateBaseShPtr(new StateComplexAngle(state_.data()+first_empty_state_+2)))));
+ frames_.push_back(new FrameBase(_time_stamp,
+ new StatePoint2D(state_.data()+first_empty_state_),
+ new StateComplexAngle(state_.data()+first_empty_state_+2)));
else
// frames_.push_back(FrameBaseShPtr(new FrameBase(nullptr, _time_stamp,
// StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
// StateBaseShPtr(new StateTheta(state_.data()+first_empty_state_+2)))));
- frames_.push_back(FrameBaseShPtr(new FrameBase(_time_stamp,
- StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
- StateBaseShPtr(new StateTheta(state_.data()+first_empty_state_+2)))));
+ frames_.push_back(new FrameBase(_time_stamp,
+ new StatePoint2D(state_.data()+first_empty_state_),
+ new StateTheta(state_.data()+first_empty_state_+2)));
// Update first free state location index
first_empty_state_ += use_complex_angles_ ? 4 : 3;
}
- void addCapture(const VectorXs& _odom_capture, const WolfScalar& _time_stamp, const SensorBaseShPtr& _sensor_ptr)
+ void addCapture(const VectorXs& _odom_capture, const WolfScalar& _time_stamp, SensorBase* _sensor_ptr)
{
- new_captures_.push(CaptureXShPtr(new CaptureXBase(_odom_capture, _time_stamp, _sensor_ptr)));
+ new_captures_.push(new CaptureXBase(_odom_capture, _time_stamp, _sensor_ptr));
}
- void computeOdomCapture(const CaptureXShPtr& _odom_capture)
+ void computeOdomCapture(CaptureXBase* _odom_capture)
{
- FrameBaseShPtr prev_frame_ptr = frames_.back();
+ FrameBase* prev_frame_ptr = frames_.back();
// STORE CAPTURE
captures_.push_back(_odom_capture);
@@ -413,30 +408,30 @@ class WolfManager
// CORRESPONDENCE ODOMETRY
if (use_complex_angles_)
- constraints_.push_back(ConstraintXShPtr(new Constraint2DOdometry(_odom_capture->getPtr(),
- prev_frame_ptr->getPPtr()->getPtr(),
- prev_frame_ptr->getOPtr()->getPtr(),
- frames_.back()->getPPtr()->getPtr(),
- frames_.back()->getOPtr()->getPtr())));
+ constraints_.push_back(new Constraint2DOdometry(_odom_capture->getPtr(),
+ prev_frame_ptr->getPPtr()->getPtr(),
+ prev_frame_ptr->getOPtr()->getPtr(),
+ frames_.back()->getPPtr()->getPtr(),
+ frames_.back()->getOPtr()->getPtr()));
else
- constraints_.push_back(ConstraintXShPtr(new Constraint2DOdometryTheta(_odom_capture->getPtr(),
- prev_frame_ptr->getPPtr()->getPtr(),
- prev_frame_ptr->getOPtr()->getPtr(),
- frames_.back()->getPPtr()->getPtr(),
- frames_.back()->getOPtr()->getPtr())));
+ constraints_.push_back(new Constraint2DOdometryTheta(_odom_capture->getPtr(),
+ prev_frame_ptr->getPPtr()->getPtr(),
+ prev_frame_ptr->getOPtr()->getPtr(),
+ frames_.back()->getPPtr()->getPtr(),
+ frames_.back()->getOPtr()->getPtr()));
}
- void computeGPSCapture(const CaptureXShPtr& _gps_capture)
+ void computeGPSCapture(CaptureXBase* _gps_capture)
{
// STORE CAPTURE
captures_.push_back(_gps_capture);
// CORRESPONDENCE GPS
- constraints_.push_back(ConstraintXShPtr(new ConstraintGPS2D(_gps_capture->getPtr(), frames_.back()->getPPtr()->getPtr())));
+ constraints_.push_back(new ConstraintGPS2D(_gps_capture->getPtr(), frames_.back()->getPPtr()->getPtr()));
}
- void update(std::queue<StateBaseShPtr>& new_state_units, std::queue<ConstraintXShPtr>& new_constraints)
+ void update(std::queue<StateBase*>& new_state_units, std::queue<ConstraintXBase*>& new_constraints)
{
while (!new_captures_.empty())
{
@@ -464,14 +459,14 @@ class WolfManager
return state_;
}
- ConstraintXShPtr getConstraintPrt(unsigned int i)
+ ConstraintXBase* getConstraintPrt(unsigned int i)
{
return constraints_.at(i);
}
- std::queue<StateBaseShPtr> getStateUnitsPtrs(unsigned int i)
+ std::queue<StateBase*> getStateUnitsPtrs(unsigned int i)
{
- return std::queue<StateBaseShPtr>({frames_.at(i)->getPPtr(),frames_.at(i)->getOPtr()});
+ return std::queue<StateBase*>({frames_.at(i)->getPPtr(),frames_.at(i)->getOPtr()});
}
};
@@ -479,7 +474,7 @@ class CeresManager
{
protected:
- std::vector<std::pair<ceres::ResidualBlockId, ConstraintXShPtr>> constraint_list_;
+ std::vector<std::pair<ceres::ResidualBlockId, ConstraintXBase*>> constraint_list_;
ceres::Problem* ceres_problem_;
public:
@@ -513,7 +508,7 @@ class CeresManager
return ceres_summary_;
}
- void addConstraints(std::queue<ConstraintXShPtr>& _new_constraints)
+ void addConstraints(std::queue<ConstraintXBase*>& _new_constraints)
{
//std::cout << _new_constraints.size() << " new constraints\n";
while (!_new_constraints.empty())
@@ -533,13 +528,13 @@ class CeresManager
std::cout << ceres_problem_->NumResidualBlocks() << " residual blocks \n";
}
- void addConstraint(const ConstraintXShPtr& _corr_ptr)
+ void addConstraint(ConstraintXBase* _corr_ptr)
{
ceres::ResidualBlockId blockIdx = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), NULL, _corr_ptr->getBlockPtrVector());
- constraint_list_.push_back(std::pair<ceres::ResidualBlockId, ConstraintXShPtr>(blockIdx,_corr_ptr));
+ constraint_list_.push_back(std::pair<ceres::ResidualBlockId, ConstraintXBase*>(blockIdx,_corr_ptr));
}
- void addStateUnits(std::queue<StateBaseShPtr>& _new_state_units)
+ void addStateUnits(std::queue<StateBase*>& _new_state_units)
{
while (!_new_state_units.empty())
{
@@ -553,7 +548,7 @@ class CeresManager
ceres_problem_->RemoveParameterBlock(_st_ptr);
}
- void addStateUnit(const StateBaseShPtr& _st_ptr)
+ void addStateUnit(StateBase* _st_ptr)
{
//std::cout << "Adding a State Unit to wolf_problem... " << std::endl;
//_st_ptr->print();
@@ -564,7 +559,7 @@ class CeresManager
{
//std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
//ceres_problem_->SetParameterization(_st_ptr->getPtr(), new ComplexAngleParameterization);
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr.get())->BLOCK_SIZE, new ComplexAngleParameterization);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
break;
}
// case PARAM_QUATERNION:
@@ -577,19 +572,19 @@ class CeresManager
case ST_POINT_1D: //equivalent case ST_THETA:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr.get())->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_POINT_2D:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint2D*)_st_ptr.get())->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint2D*)_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(), ((StatePoint3D*)_st_ptr.get())->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint3D*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
default:
@@ -597,13 +592,13 @@ class CeresManager
}
}
- ceres::CostFunction* createCostFunction(const ConstraintXShPtr& _corrPtr)
+ ceres::CostFunction* createCostFunction(ConstraintXBase* _corrPtr)
{
switch (_corrPtr->getType())
{
case CTR_GPS_FIX_2D:
{
- ConstraintGPS2D* specific_ptr = (ConstraintGPS2D*)(_corrPtr.get());
+ ConstraintGPS2D* specific_ptr = (ConstraintGPS2D*)(_corrPtr);
return new ceres::AutoDiffCostFunction<ConstraintGPS2D,
specific_ptr->measurementSize,
specific_ptr->block0Size,
@@ -620,7 +615,7 @@ class CeresManager
}
case CTR_ODOM_2D_COMPLEX_ANGLE:
{
- Constraint2DOdometry* specific_ptr = (Constraint2DOdometry*)(_corrPtr.get());
+ Constraint2DOdometry* specific_ptr = (Constraint2DOdometry*)(_corrPtr);
return new ceres::AutoDiffCostFunction<Constraint2DOdometry,
specific_ptr->measurementSize,
specific_ptr->block0Size,
@@ -637,7 +632,7 @@ class CeresManager
}
case CTR_ODOM_2D_THETA:
{
- Constraint2DOdometryTheta* specific_ptr = (Constraint2DOdometryTheta*)(_corrPtr.get());
+ Constraint2DOdometryTheta* specific_ptr = (Constraint2DOdometryTheta*)(_corrPtr);
return new ceres::AutoDiffCostFunction<Constraint2DOdometryTheta,
specific_ptr->measurementSize,
specific_ptr->block0Size,
@@ -712,10 +707,10 @@ 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<StateBaseShPtr> new_state_units; // new state units in wolf that must be added to ceres
- std::queue<ConstraintXShPtr> new_constraints; // new constraints in wolf that must be added to ceres
- SensorBaseShPtr odom_sensor = SensorBaseShPtr(new SensorBase(ODOM_2D, Eigen::MatrixXs::Zero(3,1),0));
- SensorBaseShPtr gps_sensor = SensorBaseShPtr(new SensorBase(GPS_FIX, Eigen::MatrixXs::Zero(3,1),0));
+ std::queue<StateBase*> 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(3,1),0);
+ SensorBase* gps_sensor = new SensorBase(GPS_FIX, Eigen::MatrixXs::Zero(3,1),0);
// Initial pose
pose_true << 0,0,0;
diff --git a/src/examples/test_ceres_manager_tree.cpp b/src/examples/test_ceres_manager_tree.cpp
index 87ee7beca403d27fffec6083d7f58578152312d0..ebaba3b404d1452ae5ba2a929f7bc77b756c52d8 100644
--- a/src/examples/test_ceres_manager_tree.cpp
+++ b/src/examples/test_ceres_manager_tree.cpp
@@ -50,14 +50,14 @@ class WolfManager
VectorXs state_;
unsigned int first_empty_state_;
bool use_complex_angles_;
- TrajectoryBasePtr trajectory_;
+ TrajectoryBase* trajectory_;
std::vector<VectorXs> odom_captures_;
std::vector<VectorXs> gps_captures_;
- std::queue<CaptureBaseShPtr> new_captures_;
- SensorBasePtr sensor_prior_;
+ std::queue<CaptureBase*> new_captures_;
+ SensorBase* sensor_prior_;
public:
- WolfManager(const SensorBasePtr& _sensor_prior, const bool _complex_angle, const unsigned int& _state_length=1000) :
+ WolfManager(SensorBase* _sensor_prior, const bool _complex_angle, const unsigned int& _state_length=1000) :
state_(_state_length),
first_empty_state_(0),
use_complex_angles_(_complex_angle),
@@ -85,18 +85,16 @@ class WolfManager
// Create frame and add it to the trajectory
if (use_complex_angles_)
{
- FrameBaseShPtr new_frame(new FrameBase(
- _time_stamp,
- StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
- StateBaseShPtr(new StateComplexAngle(state_.data()+first_empty_state_+2))));
+ FrameBase* new_frame = new FrameBase(_time_stamp,
+ new StatePoint2D(state_.data()+first_empty_state_),
+ new StateComplexAngle(state_.data()+first_empty_state_+2));
trajectory_->addFrame(new_frame);
}
else
{
- FrameBaseShPtr new_frame(new FrameBase(
- _time_stamp,
- StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
- StateBaseShPtr(new StateTheta(state_.data()+first_empty_state_+2))));
+ FrameBase* new_frame = new FrameBase(_time_stamp,
+ new StatePoint2D(state_.data()+first_empty_state_),
+ new StateTheta(state_.data()+first_empty_state_+2));
trajectory_->addFrame(new_frame);
}
@@ -104,18 +102,18 @@ class WolfManager
first_empty_state_ += use_complex_angles_ ? 4 : 3;
}
- void addCapture(const CaptureBaseShPtr& _capture)
+ void addCapture(CaptureBase* _capture)
{
new_captures_.push(_capture);
}
- void update(std::list<StateBasePtr>& new_state_units, std::list<ConstraintBasePtr>& new_constraints)
+ void update(std::list<StateBase*>& new_state_units, std::list<ConstraintBase*>& new_constraints)
{
// TODO: management due to time stamps
while (!new_captures_.empty())
{
// EXTRACT NEW CAPTURE
- CaptureBaseShPtr new_capture = new_captures_.front();
+ CaptureBase* new_capture = new_captures_.front();
new_captures_.pop();
// NEW FRAME (if the specific sensor)
@@ -132,8 +130,8 @@ class WolfManager
// TODO: Change by something like...
//new_state_units.insert(new_state_units.end(), trajectory_.getFrameList.back()->getStateList().begin(), trajectory_.getFrameList.back()->getStateList().end());
- new_state_units.push_back(trajectory_->getFrameListPtr()->back()->getPPtr().get());
- new_state_units.push_back(trajectory_->getFrameListPtr()->back()->getOPtr().get());
+ new_state_units.push_back(trajectory_->getFrameListPtr()->back()->getPPtr());
+ new_state_units.push_back(trajectory_->getFrameListPtr()->back()->getOPtr());
}
else
{
@@ -149,7 +147,7 @@ class WolfManager
{
for (ConstraintBaseIter constraint_list_iter=(*feature_list_iter)->getConstraintListPtr()->begin(); constraint_list_iter!=(*feature_list_iter)->getConstraintListPtr()->end(); constraint_list_iter++)
{
- new_constraints.push_back((*constraint_list_iter).get());
+ new_constraints.push_back(*constraint_list_iter);
}
}
}
@@ -160,23 +158,23 @@ class WolfManager
return state_;
}
- std::list<StateBasePtr> getStateList()
+ std::list<StateBase*> getStateList()
{
- std::list<StateBasePtr> st_list;
+ std::list<StateBase*> st_list;
for (FrameBaseIter frame_list_iter=trajectory_->getFrameListPtr()->begin(); frame_list_iter!=trajectory_->getFrameListPtr()->end(); frame_list_iter++)
{
//st_list.insert(st_list.end(), (*frame_list_iter)->getStateList().begin(), (*frame_list_iter)->getStateList().end());
- st_list.push_back((*frame_list_iter)->getPPtr().get());
- st_list.push_back((*frame_list_iter)->getOPtr().get());
+ st_list.push_back((*frame_list_iter)->getPPtr());
+ st_list.push_back((*frame_list_iter)->getOPtr());
}
return st_list;
}
- std::list<ConstraintBaseShPtr> getConstraintsList()
+ std::list<ConstraintBase*> getConstraintsList()
{
- std::list<ConstraintBaseShPtr> corr_list;
+ std::list<ConstraintBase*> corr_list;
for (FrameBaseIter frame_list_iter=trajectory_->getFrameListPtr()->begin(); frame_list_iter!=trajectory_->getFrameListPtr()->end(); frame_list_iter++)
{
@@ -251,8 +249,8 @@ 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<StateBasePtr> new_state_units; // new state units in wolf that must be added to ceres
- std::list<ConstraintBasePtr> new_constraints; // new constraints in wolf that must be added to ceres
+ std::list<StateBase*> 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
SensorOdom2D odom_sensor(Eigen::MatrixXs::Zero(3,1), odom_std, odom_std);
@@ -299,8 +297,8 @@ int main(int argc, char** argv)
for (uint step=1; step < n_execution; step++)
{
// adding new sensor captures
- wolf_manager->addCapture(CaptureBaseShPtr(new CaptureOdom2D(TimeStamp(step*0.01), &odom_sensor, odom_readings.segment(step*2,2), odom_std * MatrixXs::Identity(2,2))));
- wolf_manager->addCapture(CaptureBaseShPtr(new CaptureGPSFix(TimeStamp(step*0.01), &gps_sensor, gps_fix_readings.segment(step*3,3), gps_std * MatrixXs::Identity(3,3))));
+ wolf_manager->addCapture(new CaptureOdom2D(TimeStamp(step*0.01), &odom_sensor, odom_readings.segment(step*2,2), odom_std * MatrixXs::Identity(2,2)));
+ wolf_manager->addCapture(new CaptureGPSFix(TimeStamp(step*0.01), &gps_sensor, gps_fix_readings.segment(step*3,3), gps_std * MatrixXs::Identity(3,3)));
// updating problem
wolf_manager->update(new_state_units, new_constraints);
diff --git a/src/examples/test_node_linked.cpp b/src/examples/test_node_linked.cpp
index 0f4ed41c07377c1f7d3611974f03ef4429ae7826..3a1f1bec7383bd258ac61324a00491297d091936 100644
--- a/src/examples/test_node_linked.cpp
+++ b/src/examples/test_node_linked.cpp
@@ -99,14 +99,14 @@ int main()
cout << "========================================================" << endl;
cout << endl << "TEST 1. Constructors" << endl;
- shared_ptr<WolfProblem> problem_(new WolfProblem());
- shared_ptr<TrajectoryN> trajectory_(new TrajectoryN(2));
- shared_ptr<FrameN> frame_1_(new FrameN(1.011));
- shared_ptr<FrameN> frame_2_(new FrameN(2.022));
- shared_ptr<MeasurementN> sensor_data_cam_1_(new MeasurementN(640));
- shared_ptr<MeasurementN> sensor_data_laser_(new MeasurementN(180));
- shared_ptr<MeasurementN> sensor_data_cam_2_(new MeasurementN(480));
- shared_ptr<MeasurementN> sensor_data_radar_(new MeasurementN(90));
+ WolfProblem* problem_(new WolfProblem());
+ TrajectoryN* trajectory_(new TrajectoryN(2));
+ FrameN* frame_1_(new FrameN(1.011));
+ FrameN* frame_2_(new FrameN(2.022));
+ MeasurementN* sensor_data_cam_1_(new MeasurementN(640));
+ MeasurementN* sensor_data_laser_(new MeasurementN(180));
+ MeasurementN* sensor_data_cam_2_(new MeasurementN(480));
+ MeasurementN* sensor_data_radar_(new MeasurementN(90));
trajectory_->print();
cout << "========================================================" << endl;
@@ -116,7 +116,7 @@ int main()
frame_2_->addDownNode(sensor_data_cam_2_);
trajectory_->addDownNode(frame_1_);
trajectory_->addDownNode(frame_2_);
- trajectory_->linkToUpperNode(problem_.get());
+ trajectory_->linkToUpperNode(problem_);
trajectory_->print();
cout << "========================================================" << endl;
@@ -128,9 +128,9 @@ int main()
cout << endl << "TEST 4. Remove nodes" << endl;
//check if resetting ptr previously, effectively removes object when calling removeDownNode()
unsigned int f1_id_ = frame_1_->nodeId();
- frame_1_.reset();
- sensor_data_cam_1_.reset();
- sensor_data_laser_.reset();
+ frame_1_ = nullptr;
+ sensor_data_cam_1_ = nullptr;
+ sensor_data_laser_ = nullptr;;
trajectory_->removeDownNode(f1_id_);
trajectory_->print();
cout << "========================================================" << endl;
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 1780c516f5cb3d01cfc43614049e6d15cbfc10bd..ea848b3954396bb6dc0592c026bec767a2e17cc4 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -55,6 +55,38 @@ void FrameBase::setType(FrameType _ft)
type_ = _ft;
}
+void FrameBase::fix()
+{
+ //std::cout << "Fixing frame " << nodeId() << std::endl;
+ status_ = ST_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 FrameBase::unfix()
+{
+ //std::cout << "Unfixing frame " << nodeId() << std::endl;
+ status_ = 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);
+}
+
void FrameBase::setTimeStamp(const TimeStamp & _ts)
{
time_stamp_ = _ts;
diff --git a/src/frame_base.h b/src/frame_base.h
index e8245fb0cd3ecc4868da82d95f0222f409e6f18e..60d80b9d410ccd50a094eaf4eb7a3861e506e008 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -78,6 +78,10 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
void setType(FrameType _ft);
+ void fix();
+
+ void unfix();
+
void setTimeStamp(const TimeStamp& _ts);
TimeStamp getTimeStamp() const;
diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp
index e156dc0fd915c79f78c7dc97b97cf023341af7e3..2c89acdb4e4879067ccd2c0fe461033c1d617264 100644
--- a/src/landmark_base.cpp
+++ b/src/landmark_base.cpp
@@ -49,11 +49,43 @@ void LandmarkBase::hit()
hit_count_ ++;
}
-void LandmarkBase::unHit()
+void LandmarkBase::unhit()
{
hit_count_ --;
}
+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);
+}
+
unsigned int LandmarkBase::getHits() const
{
return hit_count_;
diff --git a/src/landmark_base.h b/src/landmark_base.h
index d4c2b2f4aba534bb7fdcc6fe0f02f0e159d7de51..c5723e88e61b130c214231e6b9264249785eddf4 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -25,6 +25,7 @@ class LandmarkBase : public NodeLinked<MapBase,NodeTerminus>
LandmarkType type_; //type of landmark. (types defined at wolf.h)
LandmarkStatus status_; //status of the landmark. (types defined at wolf.h)
unsigned int hit_count_; //counts how many features has been associated to this landmark
+ 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
@@ -44,7 +45,7 @@ class LandmarkBase : public NodeLinked<MapBase,NodeTerminus>
* \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 , StateBase* _p_ptr, StateBase* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
/** \brief Constructor with type, time stamp and state list
*
@@ -66,7 +67,11 @@ class LandmarkBase : public NodeLinked<MapBase,NodeTerminus>
void hit();
- void unHit();
+ void unhit();
+
+ void fix();
+
+ void unfix();
unsigned int getHits() const;
diff --git a/src/node_linked.h b/src/node_linked.h
index 3102e79945b41ffbd6d6b8de5a0bd3fd05406d97..fb07972db0fc5b198b530fe56fef2f935b08e95b 100644
--- a/src/node_linked.h
+++ b/src/node_linked.h
@@ -352,10 +352,7 @@ inline void NodeLinked<UpperType, LowerType>::removeDownNode(const LowerNodeIter
template<class UpperType, class LowerType>
WolfProblem* NodeLinked<UpperType, LowerType>::getTop()
{
-// if (location_ == TOP)
-// return this;
-// else
- return up_node_ptr_->getTop();
+ return up_node_ptr_->getTop();
}
//template<class UpperType, class LowerType>
diff --git a/src/node_terminus.cpp b/src/node_terminus.cpp
index 44b6692bccba5113da9798be949316152d86ed78..633626558fbf422d6214bd40c2599d821435e6cf 100644
--- a/src/node_terminus.cpp
+++ b/src/node_terminus.cpp
@@ -1,7 +1,18 @@
#include "node_terminus.h"
-inline NodeTerminus::~NodeTerminus()
+NodeTerminus::NodeTerminus() :
+ NodeBase("TERMINUS")
{
//
}
+
+NodeTerminus::~NodeTerminus()
+{
+ //
+}
+
+WolfProblem* NodeTerminus::getTop()
+{
+ return nullptr;
+}
diff --git a/src/node_terminus.h b/src/node_terminus.h
index 5b28904e9f26f9a56a608f4b24c694d7194110eb..b4d5a98a11e12b174bb896cc4a5fca6a341f86d9 100644
--- a/src/node_terminus.h
+++ b/src/node_terminus.h
@@ -20,6 +20,11 @@
class NodeTerminus : public NodeBase
{
public:
+ NodeTerminus();
+
virtual ~NodeTerminus();
+
+ WolfProblem* getTop();
+
};
#endif /* NODE_TERMINUS_H_ */
diff --git a/src/wolf.h b/src/wolf.h
index fc7c7766075e799b618a03239baf1dd582f1a9ec..5583edd5aa0df28c7864e1d74d8b9ebaefa196a4 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -183,10 +183,10 @@ typedef enum
typedef enum
{
LANDMARK_CANDIDATE, ///< A landmark, just created. Association with it allowed, but not yet stablish an actual constraint for the solver
- LANDMARK_IN_ESTIMATION, ///< A landmark being estimated. Association with it allowed, stablishing actual constraints for the solver where both vehicle and landmar states are being estimated
- LANDMARK_ESTIMATED, ///< A landmark estimated. Association with it allowed, stablishing actual constraints for the solver, but its value remains static, no longer optimized
+ LANDMARK_ESTIMATED, ///< A landmark being estimated. Association with it allowed, stablishing actual constraints for the solver where both vehicle and landmark states are being estimated
+ LANDMARK_FIXED, ///< A landmark estimated. Association with it allowed, stablishing actual constraints for the solver, but its value remains static, no longer optimized
LANDMARK_OUT_OF_VIEW, ///< A landmark out of the field of view. Association with it is not allowed, so does not pose constraints for the solver
- LANDMARK_OLD ///< An old landmark. Just kept for visualization or statistical analysis. Association with it not allowed, so does not pose constraints for the solver.
+ LANDMARK_OLD ///< An old landmark. Association with it not allowed, but old constraints can still be taken into account by the solver.
} LandmarkStatus;
/** \brief Pending status of a node