From 386161dd2df4abdc5da9e6d0e591bac2006220be Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 29 Sep 2016 17:51:26 +0200
Subject: [PATCH] typedef all pointers to base classes
---
src/capture_base.cpp | 2 +-
src/capture_base.h | 20 +++---
src/capture_fix.cpp | 2 +-
src/capture_fix.h | 2 +-
src/capture_gps_fix.cpp | 4 +-
src/capture_gps_fix.h | 4 +-
src/capture_imu.cpp | 4 +-
src/capture_imu.h | 12 +++-
src/capture_laser_2D.cpp | 2 +-
src/capture_laser_2D.h | 2 +-
src/capture_motion.h | 26 +++----
src/capture_void.cpp | 2 +-
src/capture_void.h | 2 +-
src/ceres_wrapper/ceres_manager.cpp | 6 +-
src/ceres_wrapper/ceres_manager.h | 8 +--
.../create_auto_diff_cost_function.cpp | 2 +-
.../create_auto_diff_cost_function.h | 2 +-
.../create_auto_diff_cost_function_ceres.h | 2 +-
.../create_auto_diff_cost_function_wrapper.h | 2 +-
.../create_numeric_diff_cost_function.cpp | 2 +-
.../create_numeric_diff_cost_function.h | 2 +-
.../create_numeric_diff_cost_function_ceres.h | 2 +-
src/constraint_analytic.cpp | 6 +-
src/constraint_analytic.h | 6 +-
src/constraint_base.cpp | 8 +--
src/constraint_base.h | 34 ++++-----
src/constraint_container.h | 4 +-
src/constraint_corner_2D.h | 4 +-
src/constraint_epipolar.h | 10 +--
src/constraint_fix.h | 2 +-
src/constraint_gps_2D.h | 2 +-
src/constraint_gps_pseudorange_2D.h | 2 +-
src/constraint_gps_pseudorange_3D.h | 6 +-
src/constraint_image.h | 8 +--
src/constraint_image_new_landmark.h | 8 +--
src/constraint_imu.h | 2 +-
src/constraint_odom_2D.h | 8 +--
src/constraint_odom_2D_analytic.h | 8 +--
src/constraint_relative_2D_analytic.h | 6 +-
src/constraint_sparse.h | 12 ++--
src/examples/test_2_lasers_offline.cpp | 6 +-
.../test_analytic_odom_constraint.cpp | 30 ++++----
src/examples/test_ceres_2_lasers.cpp | 2 +-
.../test_ceres_2_lasers_polylines.cpp | 2 +-
src/examples/test_image.cpp | 4 +-
src/examples/test_motion_2d.cpp | 10 +--
src/examples/test_node_linked.cpp | 4 +-
.../test_processor_image_landmark.cpp | 4 +-
src/examples/test_processor_imu.cpp | 8 +--
.../test_processor_tracker_feature.cpp | 4 +-
.../test_processor_tracker_landmark.cpp | 4 +-
src/examples/test_sort_keyframes.cpp | 14 ++--
src/examples/test_wolf_autodiffwrapper.cpp | 32 ++++-----
src/examples/test_wolf_factories.cpp | 4 +-
src/examples/test_wolf_imported_graph.cpp | 32 ++++-----
src/examples/test_wolf_prunning.cpp | 32 ++++-----
src/factory.h | 18 ++---
src/feature_base.cpp | 4 +-
src/feature_base.h | 18 ++---
src/frame_base.cpp | 6 +-
src/frame_base.h | 12 ++--
src/hardware_base.cpp | 4 +-
src/hardware_base.h | 10 +--
src/landmark_AHP.cpp | 8 +--
src/landmark_AHP.h | 28 ++++----
src/landmark_base.h | 10 +--
src/landmark_polyline_2D.cpp | 4 +-
src/landmark_polyline_2D.h | 2 +-
src/map_base.cpp | 8 +--
src/map_base.h | 8 +--
src/node_constrained.h | 4 +-
src/node_linked.h | 10 +--
src/problem.cpp | 70 +++++++++----------
src/problem.h | 70 +++++++++----------
src/processor_base.cpp | 4 +-
src/processor_base.h | 24 +++----
src/processor_factory.cpp | 2 +-
src/processor_factory.h | 14 ++--
src/processor_gps.cpp | 8 +--
src/processor_gps.h | 8 +--
src/processor_image.cpp | 10 +--
src/processor_image.h | 10 +--
src/processor_image_landmark.cpp | 8 +--
src/processor_image_landmark.h | 6 +-
src/processor_imu.cpp | 2 +-
src/processor_imu.h | 11 ++-
src/processor_motion.h | 36 +++++-----
src/processor_odom_2D.cpp | 2 +-
src/processor_odom_2D.h | 7 +-
src/processor_odom_3D.cpp | 2 +-
src/processor_odom_3D.h | 12 ++--
src/processor_tracker.cpp | 10 +--
src/processor_tracker.h | 24 +++----
src/processor_tracker_feature.h | 4 +-
src/processor_tracker_feature_corner.cpp | 4 +-
src/processor_tracker_feature_corner.h | 8 +--
src/processor_tracker_feature_dummy.h | 12 ++--
src/processor_tracker_landmark.cpp | 2 +-
src/processor_tracker_landmark.h | 4 +-
src/processor_tracker_landmark2.cpp | 2 +-
src/processor_tracker_landmark2.h | 10 +--
src/processor_tracker_landmark_corner.cpp | 16 ++---
src/processor_tracker_landmark_corner.h | 14 ++--
src/processor_tracker_landmark_dummy.cpp | 4 +-
src/processor_tracker_landmark_dummy.h | 4 +-
src/processor_tracker_landmark_polyline.cpp | 14 ++--
src/processor_tracker_landmark_polyline.h | 8 +--
src/sensor_base.h | 12 ++--
src/sensor_camera.cpp | 4 +-
src/sensor_camera.h | 4 +-
src/sensor_factory.cpp | 2 +-
src/sensor_factory.h | 14 ++--
src/sensor_gps.cpp | 6 +-
src/sensor_gps.h | 2 +-
src/sensor_gps_fix.cpp | 4 +-
src/sensor_gps_fix.h | 2 +-
src/sensor_imu.cpp | 6 +-
src/sensor_imu.h | 2 +-
src/sensor_laser_2D.cpp | 8 +--
src/sensor_laser_2D.h | 4 +-
src/sensor_odom_2D.cpp | 6 +-
src/sensor_odom_2D.h | 2 +-
src/solver/qr_solver.h | 20 +++---
src/solver/solver_manager.cpp | 6 +-
src/solver/solver_manager.h | 6 +-
src/trajectory_base.cpp | 10 +--
src/trajectory_base.h | 28 ++++----
src/wolf.h | 33 +++++++--
src/yaml/processor_image_yaml.cpp | 2 +-
src/yaml/sensor_camera_yaml.cpp | 2 +-
src/yaml/sensor_laser_2D_yaml.cpp | 2 +-
131 files changed, 627 insertions(+), 600 deletions(-)
diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index 0d9ee7f08..c0a5420ae 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -6,7 +6,7 @@ namespace wolf{
unsigned int CaptureBase::capture_id_count_ = 0;
-CaptureBase::CaptureBase(const std::string& _type, const TimeStamp& _ts, SensorBase* _sensor_ptr) :
+CaptureBase::CaptureBase(const std::string& _type, const TimeStamp& _ts, SensorBasePtr _sensor_ptr) :
NodeBase("CAPTURE", _type),
// NodeLinked(MID, "CAPTURE", _type),
capture_id_(++capture_id_count_),
diff --git a/src/capture_base.h b/src/capture_base.h
index f034c9059..2a1e2f930 100644
--- a/src/capture_base.h
+++ b/src/capture_base.h
@@ -30,7 +30,7 @@ class CaptureBase : public NodeBase // NodeLinked<FrameBase, FeatureBase>
protected:
unsigned int capture_id_;
TimeStamp time_stamp_; ///< Time stamp
- SensorBase* sensor_ptr_; ///< Pointer to sensor
+ SensorBasePtr sensor_ptr_; ///< Pointer to sensor
// Allow precomputing global frames for accelerating code.
//Eigen::Vector3s sensor_pose_global_; ///< Sensor pose in world frame: composition of the frame pose and the sensor pose. TODO: use state units
@@ -42,7 +42,7 @@ class CaptureBase : public NodeBase // NodeLinked<FrameBase, FeatureBase>
public:
- CaptureBase(const std::string& _type, const TimeStamp& _ts, SensorBase* _sensor_ptr);
+ CaptureBase(const std::string& _type, const TimeStamp& _ts, SensorBasePtr _sensor_ptr);
/** \brief Default destructor (not recommended)
*
@@ -57,14 +57,14 @@ class CaptureBase : public NodeBase // NodeLinked<FrameBase, FeatureBase>
/** \brief Adds a Feature to the down node list
**/
- FeatureBase* addFeature(FeatureBase* _ft_ptr);
+ FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
void addFeatureList(FeatureBaseList& _new_ft_list);
void removeFeature(FeatureBasePtr _ft_ptr);
/** \brief Gets Frame pointer
**/
- FrameBase* getFramePtr() const;
+ FrameBasePtr getFramePtr() const;
void setFramePtr(const FrameBasePtr _frm_ptr);
void unlinkFromFrame(){frame_ptr_ = nullptr;}
@@ -79,7 +79,7 @@ class CaptureBase : public NodeBase // NodeLinked<FrameBase, FeatureBase>
TimeStamp getTimeStamp() const;
- SensorBase* getSensorPtr() const;
+ SensorBasePtr getSensorPtr() const;
StateBlock* getSensorPPtr() const;
@@ -93,8 +93,8 @@ class CaptureBase : public NodeBase // NodeLinked<FrameBase, FeatureBase>
*/
virtual void process();
- Problem* getProblem(){return problem_ptr_;}
- void setProblem(Problem* _prob_ptr){problem_ptr_ = _prob_ptr;}
+ ProblemPtr getProblem(){return problem_ptr_;}
+ void setProblem(ProblemPtr _prob_ptr){problem_ptr_ = _prob_ptr;}
};
@@ -111,7 +111,7 @@ inline unsigned int CaptureBase::id()
return capture_id_;
}
-inline FeatureBase* CaptureBase::addFeature(FeatureBase* _ft_ptr)
+inline FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
{
//std::cout << "Adding feature" << std::endl;
feature_list_.push_back(_ft_ptr);
@@ -120,7 +120,7 @@ inline FeatureBase* CaptureBase::addFeature(FeatureBase* _ft_ptr)
return _ft_ptr;
}
-inline FrameBase* CaptureBase::getFramePtr() const
+inline FrameBasePtr CaptureBase::getFramePtr() const
{
return frame_ptr_;
// return upperNodePtr();
@@ -142,7 +142,7 @@ inline TimeStamp CaptureBase::getTimeStamp() const
return time_stamp_;
}
-inline SensorBase* CaptureBase::getSensorPtr() const
+inline SensorBasePtr CaptureBase::getSensorPtr() const
{
return sensor_ptr_;
}
diff --git a/src/capture_fix.cpp b/src/capture_fix.cpp
index 11a26d39e..fec67ee4a 100644
--- a/src/capture_fix.cpp
+++ b/src/capture_fix.cpp
@@ -2,7 +2,7 @@
namespace wolf{
-CaptureFix::CaptureFix(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
+CaptureFix::CaptureFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
CaptureBase("FIX", _ts, _sensor_ptr),
data_(_data),
data_covariance_(_data_covariance)
diff --git a/src/capture_fix.h b/src/capture_fix.h
index adb776ef3..a91664446 100644
--- a/src/capture_fix.h
+++ b/src/capture_fix.h
@@ -21,7 +21,7 @@ class CaptureFix : public CaptureBase
public:
- CaptureFix(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
+ CaptureFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
/** \brief Default destructor (not recommended)
*
diff --git a/src/capture_gps_fix.cpp b/src/capture_gps_fix.cpp
index faf5f8cb8..3fccc781f 100644
--- a/src/capture_gps_fix.cpp
+++ b/src/capture_gps_fix.cpp
@@ -3,14 +3,14 @@
namespace wolf {
-CaptureGPSFix::CaptureGPSFix(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data) :
+CaptureGPSFix::CaptureGPSFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data) :
CaptureBase("GPS FIX", _ts, _sensor_ptr),
data_(_data)
{
//
}
-CaptureGPSFix::CaptureGPSFix(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
+CaptureGPSFix::CaptureGPSFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
CaptureBase("GPS FIX", _ts, _sensor_ptr),
data_(_data),
data_covariance_(_data_covariance)
diff --git a/src/capture_gps_fix.h b/src/capture_gps_fix.h
index 1198e0d29..341971931 100644
--- a/src/capture_gps_fix.h
+++ b/src/capture_gps_fix.h
@@ -18,9 +18,9 @@ class CaptureGPSFix : public CaptureBase
Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
public:
- CaptureGPSFix(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data);
+ CaptureGPSFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data);
- CaptureGPSFix(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
+ CaptureGPSFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
/** \brief Default destructor (not recommended)
*
diff --git a/src/capture_imu.cpp b/src/capture_imu.cpp
index 5648efcc6..ce31436ff 100644
--- a/src/capture_imu.cpp
+++ b/src/capture_imu.cpp
@@ -3,14 +3,14 @@
namespace wolf {
-CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, SensorBase* _sensor_ptr,
+CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr,
const Eigen::Vector6s& _acc_gyro_data) :
CaptureMotion(_init_ts, _sensor_ptr, _acc_gyro_data )
{
setType("IMU");
}
-/*CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, SensorBase* _sensor_ptr,
+/*CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr,
const Eigen::Vector6s& _data, const Eigen::Matrix<Scalar,6,3>& _data_covariance) :
CaptureMotion(_init_ts, _sensor_ptr, _data, _data_covariance)
{
diff --git a/src/capture_imu.h b/src/capture_imu.h
index 0dec1715b..4a4038270 100644
--- a/src/capture_imu.h
+++ b/src/capture_imu.h
@@ -10,9 +10,15 @@ class CaptureIMU : public CaptureMotion
{
public:
- CaptureIMU(const TimeStamp& _init_ts, SensorBase* _sensor_ptr, const Eigen::Vector6s& _data);
-
- /*CaptureIMU(const TimeStamp& _init_ts, SensorBase* _sensor_ptr, const Eigen::Vector6s& _data, const Eigen::Matrix<Scalar,6,3>& _data_covariance);*/
+ CaptureIMU(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, const Eigen::Vector6s& _data);
+
+//<<<<<<< e72779277b2cbd56ce81286c43b51ae2b4934110
+// /*CaptureIMU(const TimeStamp& _init_ts, SensorBase* _sensor_ptr, const Eigen::Vector6s& _data, const Eigen::Matrix<Scalar,6,3>& _data_covariance);*/
+//=======
+// /*CaptureIMU(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, const Eigen::Vector6s& _data, const Eigen::Matrix<Scalar,6,3>& _data_covariance);
+//
+// CaptureIMU(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, const Eigen::Vector6s& _data, const Eigen::Matrix<Scalar,6,3>& _data_covariance, FrameBasePtr _origin_frame_ptr);*/
+//>>>>>>> typedef all pointers to base classes
/** \brief Default destructor (not recommended)
*
diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index 14d32b4c0..9771afbde 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -2,7 +2,7 @@
namespace wolf {
-CaptureLaser2D::CaptureLaser2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges) :
+CaptureLaser2D::CaptureLaser2D(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const std::vector<float>& _ranges) :
CaptureBase("LASER 2D", _ts, _sensor_ptr), laser_ptr_((SensorLaser2D*)(sensor_ptr_)), scan_(_ranges)
{
//
diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h
index 7b03dd94d..797f8faa6 100644
--- a/src/capture_laser_2D.h
+++ b/src/capture_laser_2D.h
@@ -17,7 +17,7 @@ class CaptureLaser2D : public CaptureBase
public:
/** \brief Constructor with ranges
**/
- CaptureLaser2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges);
+ CaptureLaser2D(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const std::vector<float>& _ranges);
/** \brief Default destructor (not recommended)
*
diff --git a/src/capture_motion.h b/src/capture_motion.h
index fd8495baf..879b2ef65 100644
--- a/src/capture_motion.h
+++ b/src/capture_motion.h
@@ -43,11 +43,11 @@ class CaptureMotion : public CaptureBase
// public interface:
public:
- CaptureMotion(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data,
- FrameBase* _origin_frame_ptr = nullptr);
+ CaptureMotion(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data,
+ FrameBasePtr _origin_frame_ptr = nullptr);
- CaptureMotion(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov, FrameBase* _origin_frame_ptr = nullptr);
+ CaptureMotion(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data,
+ const Eigen::MatrixXs& _data_cov, FrameBasePtr _origin_frame_ptr = nullptr);
virtual ~CaptureMotion();
@@ -60,19 +60,19 @@ class CaptureMotion : public CaptureBase
const MotionBuffer* getBufferPtr() const;
const Eigen::VectorXs& getDelta() const;
- FrameBase* getOriginFramePtr();
- void setOriginFramePtr(FrameBase* _frame_ptr);
+ FrameBasePtr getOriginFramePtr();
+ void setOriginFramePtr(FrameBasePtr _frame_ptr);
// member data:
private:
Eigen::VectorXs data_; ///< Motion data in form of vector mandatory
Eigen::MatrixXs data_cov_; ///< Motion data in form of vector mandatory
MotionBuffer buffer_; ///< Buffer of motions between this Capture and the next one.
- FrameBase* origin_frame_ptr_; ///< Pointer to the origin frame of the motion
+ FrameBasePtr origin_frame_ptr_; ///< Pointer to the origin frame of the motion
};
-inline CaptureMotion::CaptureMotion(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data,
- FrameBase* _origin_frame_ptr) :
+inline CaptureMotion::CaptureMotion(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data,
+ FrameBasePtr _origin_frame_ptr) :
CaptureBase("MOTION", _ts, _sensor_ptr),
data_(_data),
data_cov_(Eigen::MatrixXs::Identity(_data.rows(), _data.rows())),
@@ -82,8 +82,8 @@ inline CaptureMotion::CaptureMotion(const TimeStamp& _ts, SensorBase* _sensor_pt
//
}
-inline CaptureMotion::CaptureMotion(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov, FrameBase* _origin_frame_ptr) :
+inline CaptureMotion::CaptureMotion(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data,
+ const Eigen::MatrixXs& _data_cov, FrameBasePtr _origin_frame_ptr) :
CaptureBase("MOTION", _ts, _sensor_ptr),
data_(_data),
data_cov_(_data_cov),
@@ -133,12 +133,12 @@ inline const Eigen::VectorXs& CaptureMotion::getDelta() const
return buffer_.get().back().delta_integr_;
}
-inline wolf::FrameBase* CaptureMotion::getOriginFramePtr()
+inline wolf::FrameBasePtr CaptureMotion::getOriginFramePtr()
{
return origin_frame_ptr_;
}
-inline void CaptureMotion::setOriginFramePtr(FrameBase* _frame_ptr)
+inline void CaptureMotion::setOriginFramePtr(FrameBasePtr _frame_ptr)
{
origin_frame_ptr_ = _frame_ptr;
}
diff --git a/src/capture_void.cpp b/src/capture_void.cpp
index b601bf6fd..13b93ffb2 100644
--- a/src/capture_void.cpp
+++ b/src/capture_void.cpp
@@ -2,7 +2,7 @@
namespace wolf {
-CaptureVoid::CaptureVoid(const TimeStamp& _ts, SensorBase* _sensor_ptr) :
+CaptureVoid::CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr) :
CaptureBase("VOID", _ts, _sensor_ptr)
{
//
diff --git a/src/capture_void.h b/src/capture_void.h
index 63c7a5c4b..2d6b23f93 100644
--- a/src/capture_void.h
+++ b/src/capture_void.h
@@ -11,7 +11,7 @@ namespace wolf {
class CaptureVoid : public CaptureBase
{
public:
- CaptureVoid(const TimeStamp& _ts, SensorBase* _sensor_ptr);
+ CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr);
/** \brief Default destructor (not recommended)
*
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 72cf6ef2d..beb49b6fd 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -5,7 +5,7 @@
namespace wolf {
-CeresManager::CeresManager(Problem* _wolf_problem, const ceres::Solver::Options& _ceres_options, const bool _use_wolf_auto_diff) :
+CeresManager::CeresManager(ProblemPtr _wolf_problem, const ceres::Solver::Options& _ceres_options, const bool _use_wolf_auto_diff) :
ceres_options_(_ceres_options),
wolf_problem_(_wolf_problem),
use_wolf_auto_diff_(_use_wolf_auto_diff)
@@ -275,7 +275,7 @@ void CeresManager::update()
assert(ceres_problem_->NumResidualBlocks() == id_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
-void CeresManager::addConstraint(ConstraintBase* _ctr_ptr, unsigned int _id)
+void CeresManager::addConstraint(ConstraintBasePtr _ctr_ptr, unsigned int _id)
{
id_2_costfunction_[_id] = createCostFunction(_ctr_ptr);
@@ -343,7 +343,7 @@ void CeresManager::updateStateBlockStatus(StateBlock* _st_ptr)
ceres_problem_->SetParameterBlockVariable(_st_ptr->getPtr());
}
-ceres::CostFunction* CeresManager::createCostFunction(ConstraintBase* _corrPtr)
+ceres::CostFunction* CeresManager::createCostFunction(ConstraintBasePtr _corrPtr)
{
assert(_corrPtr != nullptr);
//std::cout << "creating cost function for constraint " << _corrPtr->id() << std::endl;
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index aa2ecc6c5..9050ce1ee 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -42,11 +42,11 @@ class CeresManager
ceres::Problem* ceres_problem_;
ceres::Solver::Options ceres_options_;
ceres::Covariance* covariance_;
- Problem* wolf_problem_;
+ ProblemPtr wolf_problem_;
bool use_wolf_auto_diff_;
public:
- CeresManager(Problem* _wolf_problem, const ceres::Solver::Options& _ceres_options = ceres::Solver::Options(), const bool _use_wolf_auto_diff = true);
+ CeresManager(ProblemPtr _wolf_problem, const ceres::Solver::Options& _ceres_options = ceres::Solver::Options(), const bool _use_wolf_auto_diff = true);
~CeresManager();
@@ -62,7 +62,7 @@ class CeresManager
void update();
- void addConstraint(ConstraintBase* _corr_ptr, unsigned int _id);
+ void addConstraint(ConstraintBasePtr _corr_ptr, unsigned int _id);
void removeConstraint(const unsigned int& _corr_idx);
@@ -74,7 +74,7 @@ class CeresManager
void updateStateBlockStatus(StateBlock* _st_ptr);
- ceres::CostFunction* createCostFunction(ConstraintBase* _corrPtr);
+ ceres::CostFunction* createCostFunction(ConstraintBasePtr _corrPtr);
};
inline ceres::Solver::Options& CeresManager::getSolverOptions()
diff --git a/src/ceres_wrapper/create_auto_diff_cost_function.cpp b/src/ceres_wrapper/create_auto_diff_cost_function.cpp
index fe369e47a..4cb0d863d 100644
--- a/src/ceres_wrapper/create_auto_diff_cost_function.cpp
+++ b/src/ceres_wrapper/create_auto_diff_cost_function.cpp
@@ -29,7 +29,7 @@
namespace wolf {
-ceres::CostFunction* createAutoDiffCostFunction(ConstraintBase* _ctr_ptr, bool _use_wolf_autodiff)
+ceres::CostFunction* createAutoDiffCostFunction(ConstraintBasePtr _ctr_ptr, bool _use_wolf_autodiff)
{
switch (_ctr_ptr->getTypeId())
{
diff --git a/src/ceres_wrapper/create_auto_diff_cost_function.h b/src/ceres_wrapper/create_auto_diff_cost_function.h
index 852129d77..d50bdb97f 100644
--- a/src/ceres_wrapper/create_auto_diff_cost_function.h
+++ b/src/ceres_wrapper/create_auto_diff_cost_function.h
@@ -12,7 +12,7 @@
#include "ceres/cost_function.h"
namespace wolf {
- ceres::CostFunction* createAutoDiffCostFunction(ConstraintBase* _ctr_ptr, bool _use_wolf_autodiff);
+ ceres::CostFunction* createAutoDiffCostFunction(ConstraintBasePtr _ctr_ptr, bool _use_wolf_autodiff);
}
diff --git a/src/ceres_wrapper/create_auto_diff_cost_function_ceres.h b/src/ceres_wrapper/create_auto_diff_cost_function_ceres.h
index b7472b5b3..9009326e7 100644
--- a/src/ceres_wrapper/create_auto_diff_cost_function_ceres.h
+++ b/src/ceres_wrapper/create_auto_diff_cost_function_ceres.h
@@ -19,7 +19,7 @@ ceres::AutoDiffCostFunction<CtrType,
CtrType::block2Size,CtrType::block3Size,
CtrType::block4Size,CtrType::block5Size,
CtrType::block6Size,CtrType::block7Size,
- CtrType::block8Size,CtrType::block9Size>* createAutoDiffCostFunctionCeres(ConstraintBase* _constraint_ptr)
+ CtrType::block8Size,CtrType::block9Size>* createAutoDiffCostFunctionCeres(ConstraintBasePtr _constraint_ptr)
{
static_assert(CtrType::measurementSize != 0,"Measurement size cannot be null!");
static_assert(!(CtrType::block0Size == 0 ||
diff --git a/src/ceres_wrapper/create_auto_diff_cost_function_wrapper.h b/src/ceres_wrapper/create_auto_diff_cost_function_wrapper.h
index 6fd8219b7..e3bfb7c3f 100644
--- a/src/ceres_wrapper/create_auto_diff_cost_function_wrapper.h
+++ b/src/ceres_wrapper/create_auto_diff_cost_function_wrapper.h
@@ -19,7 +19,7 @@ AutoDiffCostFunctionWrapperBase<CtrType,
CtrType::block2Size,CtrType::block3Size,
CtrType::block4Size,CtrType::block5Size,
CtrType::block6Size,CtrType::block7Size,
- CtrType::block8Size,CtrType::block9Size>* createAutoDiffCostFunctionWrapper(ConstraintBase* _constraint_ptr)
+ CtrType::block8Size,CtrType::block9Size>* createAutoDiffCostFunctionWrapper(ConstraintBasePtr _constraint_ptr)
{
static_assert(CtrType::measurementSize != 0,"Measurement size cannot be null!");
static_assert(!(CtrType::block0Size == 0 ||
diff --git a/src/ceres_wrapper/create_numeric_diff_cost_function.cpp b/src/ceres_wrapper/create_numeric_diff_cost_function.cpp
index 9aca4f147..cfadab44b 100644
--- a/src/ceres_wrapper/create_numeric_diff_cost_function.cpp
+++ b/src/ceres_wrapper/create_numeric_diff_cost_function.cpp
@@ -15,7 +15,7 @@
namespace wolf {
-ceres::CostFunction* createNumericDiffCostFunction(ConstraintBase* _ctr_ptr, bool _use_wolf_numericdiff)
+ceres::CostFunction* createNumericDiffCostFunction(ConstraintBasePtr _ctr_ptr, bool _use_wolf_numericdiff)
{
if (_use_wolf_numericdiff)
throw std::invalid_argument( "Numeric differentiation not implemented in wolf" );
diff --git a/src/ceres_wrapper/create_numeric_diff_cost_function.h b/src/ceres_wrapper/create_numeric_diff_cost_function.h
index 86619a8d3..e53e84f96 100644
--- a/src/ceres_wrapper/create_numeric_diff_cost_function.h
+++ b/src/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -13,7 +13,7 @@
namespace wolf {
-ceres::CostFunction* createNumericDiffCostFunction(ConstraintBase* _ctr_ptr, bool _use_wolf_numericdiff);
+ceres::CostFunction* createNumericDiffCostFunction(ConstraintBasePtr _ctr_ptr, bool _use_wolf_numericdiff);
} // namespace wolf
diff --git a/src/ceres_wrapper/create_numeric_diff_cost_function_ceres.h b/src/ceres_wrapper/create_numeric_diff_cost_function_ceres.h
index c94133263..551e28bfe 100644
--- a/src/ceres_wrapper/create_numeric_diff_cost_function_ceres.h
+++ b/src/ceres_wrapper/create_numeric_diff_cost_function_ceres.h
@@ -19,7 +19,7 @@ ceres::NumericDiffCostFunction<CtrType, ceres::CENTRAL,
CtrType::block2Size,CtrType::block3Size,
CtrType::block4Size,CtrType::block5Size,
CtrType::block6Size,CtrType::block7Size,
- CtrType::block8Size,CtrType::block9Size>* createNumericDiffCostFunctionCeres(ConstraintBase* _constraint_ptr)
+ CtrType::block8Size,CtrType::block9Size>* createNumericDiffCostFunctionCeres(ConstraintBasePtr _constraint_ptr)
{
static_assert(CtrType::measurementSize != 0,"Measurement size cannot be null!");
static_assert(!(CtrType::block0Size == 0 ||
diff --git a/src/constraint_analytic.cpp b/src/constraint_analytic.cpp
index 1772a1aba..f008dba4a 100644
--- a/src/constraint_analytic.cpp
+++ b/src/constraint_analytic.cpp
@@ -13,7 +13,7 @@ ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, bool _apply_loss_func
resizeVectors();
}
-ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, FrameBase* _frame_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, FrameBasePtr _frame_ptr, bool _apply_loss_function, ConstraintStatus _status,
StateBlock* _state0Ptr, StateBlock* _state1Ptr, StateBlock* _state2Ptr, StateBlock* _state3Ptr, StateBlock* _state4Ptr,
StateBlock* _state5Ptr, StateBlock* _state6Ptr, StateBlock* _state7Ptr, StateBlock* _state8Ptr, StateBlock* _state9Ptr ) :
ConstraintBase(_tp, _frame_ptr, _apply_loss_function, _status),
@@ -23,7 +23,7 @@ ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, FrameBase* _frame_ptr
resizeVectors();
}
-ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, FeatureBase* _feature_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, FeatureBasePtr _feature_ptr, bool _apply_loss_function, ConstraintStatus _status,
StateBlock* _state0Ptr, StateBlock* _state1Ptr, StateBlock* _state2Ptr, StateBlock* _state3Ptr, StateBlock* _state4Ptr,
StateBlock* _state5Ptr, StateBlock* _state6Ptr, StateBlock* _state7Ptr, StateBlock* _state8Ptr, StateBlock* _state9Ptr ) :
ConstraintBase( _tp, _feature_ptr, _apply_loss_function, _status),
@@ -34,7 +34,7 @@ ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, FeatureBase* _feature
}
-ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, LandmarkBase* _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, LandmarkBasePtr _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status,
StateBlock* _state0Ptr, StateBlock* _state1Ptr, StateBlock* _state2Ptr, StateBlock* _state3Ptr, StateBlock* _state4Ptr,
StateBlock* _state5Ptr, StateBlock* _state6Ptr, StateBlock* _state7Ptr, StateBlock* _state8Ptr, StateBlock* _state9Ptr ) :
ConstraintBase( _tp, _landmark_ptr, _apply_loss_function, _status),
diff --git a/src/constraint_analytic.h b/src/constraint_analytic.h
index 381cd8285..ea4e66805 100644
--- a/src/constraint_analytic.h
+++ b/src/constraint_analytic.h
@@ -38,7 +38,7 @@ class ConstraintAnalytic: public ConstraintBase
* Constructor of category CTR_FRAME
*
**/
- ConstraintAnalytic(ConstraintType _tp, FrameBase* _frame_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ ConstraintAnalytic(ConstraintType _tp, FrameBasePtr _frame_ptr, bool _apply_loss_function, ConstraintStatus _status,
StateBlock* _state0Ptr,
StateBlock* _state1Ptr = nullptr,
StateBlock* _state2Ptr = nullptr,
@@ -55,7 +55,7 @@ class ConstraintAnalytic: public ConstraintBase
* Constructor of category CTR_FEATURE
*
**/
- ConstraintAnalytic(ConstraintType _tp, FeatureBase* _feature_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ ConstraintAnalytic(ConstraintType _tp, FeatureBasePtr _feature_ptr, bool _apply_loss_function, ConstraintStatus _status,
StateBlock* _state0Ptr,
StateBlock* _state1Ptr = nullptr,
StateBlock* _state2Ptr = nullptr,
@@ -72,7 +72,7 @@ class ConstraintAnalytic: public ConstraintBase
* Constructor of category CTR_LANDMARK
*
**/
- ConstraintAnalytic(ConstraintType _tp, LandmarkBase* _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ ConstraintAnalytic(ConstraintType _tp, LandmarkBasePtr _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status,
StateBlock* _state0Ptr,
StateBlock* _state1Ptr = nullptr,
StateBlock* _state2Ptr = nullptr,
diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp
index 0084c20ca..1e452afda 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -21,7 +21,7 @@ ConstraintBase::ConstraintBase(ConstraintType _tp, bool _apply_loss_function, Co
}
-ConstraintBase::ConstraintBase(ConstraintType _tp, FrameBase* _frame_ptr, bool _apply_loss_function, ConstraintStatus _status) :
+ConstraintBase::ConstraintBase(ConstraintType _tp, FrameBasePtr _frame_ptr, bool _apply_loss_function, ConstraintStatus _status) :
NodeBase("CONSTRAINT", "Base"),
constraint_id_(++constraint_id_count_),
type_id_(_tp),
@@ -37,7 +37,7 @@ ConstraintBase::ConstraintBase(ConstraintType _tp, FrameBase* _frame_ptr, bool _
}
-ConstraintBase::ConstraintBase(ConstraintType _tp, FeatureBase* _feature_ptr, bool _apply_loss_function, ConstraintStatus _status) :
+ConstraintBase::ConstraintBase(ConstraintType _tp, FeatureBasePtr _feature_ptr, bool _apply_loss_function, ConstraintStatus _status) :
NodeBase("CONSTRAINT"),
constraint_id_(++constraint_id_count_),
type_id_(_tp),
@@ -53,7 +53,7 @@ ConstraintBase::ConstraintBase(ConstraintType _tp, FeatureBase* _feature_ptr, bo
}
-ConstraintBase::ConstraintBase(ConstraintType _tp, LandmarkBase* _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status) :
+ConstraintBase::ConstraintBase(ConstraintType _tp, LandmarkBasePtr _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status) :
NodeBase("CONSTRAINT"),
constraint_id_(++constraint_id_count_),
type_id_(_tp),
@@ -105,7 +105,7 @@ const Eigen::MatrixXs& ConstraintBase::getMeasurementSquareRootInformation() con
return getFeaturePtr()->getMeasurementSquareRootInformation();
}
-CaptureBase* ConstraintBase::getCapturePtr() const
+CaptureBasePtr ConstraintBase::getCapturePtr() const
{
return getFeaturePtr()->getCapturePtr();
// return upperNodePtr()->upperNodePtr();
diff --git a/src/constraint_base.h b/src/constraint_base.h
index 6404a344b..028d1dff6 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -32,9 +32,9 @@ class ConstraintBase : public NodeBase // NodeLinked<FeatureBase, NodeTerminus>
ConstraintCategory category_; ///< category of constraint (types defined at wolf.h)
ConstraintStatus status_; ///< status of constraint (types defined at wolf.h)
bool apply_loss_function_; ///< flag for applying loss function to this constraint
- FrameBase* frame_other_ptr_; ///< FrameBase pointer (for category CTR_FRAME)
- FeatureBase* feature_other_ptr_; ///< FeatureBase pointer (for category CTR_FEATURE)
- LandmarkBase* landmark_other_ptr_; ///< LandmarkBase pointer (for category CTR_LANDMARK)
+ FrameBasePtr frame_other_ptr_; ///< FrameBase pointer (for category CTR_FRAME)
+ FeatureBasePtr feature_other_ptr_; ///< FeatureBase pointer (for category CTR_FEATURE)
+ LandmarkBasePtr landmark_other_ptr_; ///< LandmarkBase pointer (for category CTR_LANDMARK)
public:
@@ -44,15 +44,15 @@ class ConstraintBase : public NodeBase // NodeLinked<FeatureBase, NodeTerminus>
/** \brief Constructor of category CTR_FRAME
**/
- ConstraintBase(ConstraintType _tp, FrameBase* _frame_ptr, bool _apply_loss_function, ConstraintStatus _status);
+ ConstraintBase(ConstraintType _tp, FrameBasePtr _frame_ptr, bool _apply_loss_function, ConstraintStatus _status);
/** \brief Constructor of category CTR_FEATURE
**/
- ConstraintBase(ConstraintType _tp, FeatureBase* _feature_ptr, bool _apply_loss_function, ConstraintStatus _status);
+ ConstraintBase(ConstraintType _tp, FeatureBasePtr _feature_ptr, bool _apply_loss_function, ConstraintStatus _status);
/** \brief Constructor of category CTR_LANDMARK
**/
- ConstraintBase(ConstraintType _tp, LandmarkBase* _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status);
+ ConstraintBase(ConstraintType _tp, LandmarkBasePtr _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status);
/** \brief Default destructor (not recommended)
*
@@ -95,12 +95,12 @@ class ConstraintBase : public NodeBase // NodeLinked<FeatureBase, NodeTerminus>
/** \brief Returns a pointer to the feature constrained from
**/
- FeatureBase* getFeaturePtr() const;
+ FeatureBasePtr getFeaturePtr() const;
void setFeaturePtr(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
/** \brief Returns a pointer to its capture
**/
- CaptureBase* getCapturePtr() const;
+ CaptureBasePtr getCapturePtr() const;
/** \brief Returns the constraint residual size
**/
@@ -128,18 +128,18 @@ class ConstraintBase : public NodeBase // NodeLinked<FeatureBase, NodeTerminus>
/** \brief Returns a pointer to the frame constrained to
**/
- FrameBase* getFrameOtherPtr();
+ FrameBasePtr getFrameOtherPtr();
/** \brief Returns a pointer to the feature constrained to
**/
- FeatureBase* getFeatureOtherPtr();
+ FeatureBasePtr getFeatureOtherPtr();
/** \brief Returns a pointer to the landmark constrained to
**/
- LandmarkBase* getLandmarkOtherPtr();
+ LandmarkBasePtr getLandmarkOtherPtr();
- Problem* getProblem(){return problem_ptr_;}
- void setProblem(Problem* _prob_ptr){problem_ptr_ = _prob_ptr;}
+ ProblemPtr getProblem(){return problem_ptr_;}
+ void setProblem(ProblemPtr _prob_ptr){problem_ptr_ = _prob_ptr;}
};
@@ -163,7 +163,7 @@ inline ConstraintType ConstraintBase::getTypeId() const
return type_id_;
}
-inline FeatureBase* ConstraintBase::getFeaturePtr() const
+inline FeatureBasePtr ConstraintBase::getFeaturePtr() const
{
return feature_ptr_;
// return upperNodePtr();
@@ -198,12 +198,12 @@ inline void ConstraintBase::setApplyLossFunction(const bool _apply)
}
}
-inline FrameBase* ConstraintBase::getFrameOtherPtr()
+inline FrameBasePtr ConstraintBase::getFrameOtherPtr()
{
return frame_other_ptr_;
}
-inline FeatureBase* ConstraintBase::getFeatureOtherPtr()
+inline FeatureBasePtr ConstraintBase::getFeatureOtherPtr()
{
return feature_other_ptr_;
}
@@ -226,7 +226,7 @@ inline void ConstraintBase::destruct()
}
}
-inline LandmarkBase* ConstraintBase::getLandmarkOtherPtr()
+inline LandmarkBasePtr ConstraintBase::getLandmarkOtherPtr()
{
return landmark_other_ptr_;
}
diff --git a/src/constraint_container.h b/src/constraint_container.h
index fe5e3f1e7..d478a56d5 100644
--- a/src/constraint_container.h
+++ b/src/constraint_container.h
@@ -17,7 +17,7 @@ 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, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintContainer(FeatureBasePtr _ftr_ptr, LandmarkContainer* _lmk_ptr, const unsigned int _corner, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintSparse<3,2,1,2,1>(CTR_CONTAINER, _lmk_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()),
lmk_ptr_(_lmk_ptr),
corner_(_corner)
@@ -131,7 +131,7 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
public:
- static wolf::ConstraintBase* create(FeatureBase* _feature_ptr, NodeBase* _correspondant_ptr)
+ static wolf::ConstraintBasePtr create(FeatureBasePtr _feature_ptr, NodeBasePtr _correspondant_ptr)
{
unsigned int corner = 0; // Hard-coded, but this class is nevertheless deprecated.
diff --git a/src/constraint_corner_2D.h b/src/constraint_corner_2D.h
index 67978812c..cba05601b 100644
--- a/src/constraint_corner_2D.h
+++ b/src/constraint_corner_2D.h
@@ -12,7 +12,7 @@ class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1>
public:
// static const unsigned int N_BLOCKS = 4;
- ConstraintCorner2D(FeatureBase* _ftr_ptr, LandmarkCorner2D* _lmk_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintCorner2D(FeatureBasePtr _ftr_ptr, LandmarkCorner2D* _lmk_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintSparse<3,2,1,2,1>(CTR_CORNER_2D, _lmk_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr())
{
setType("CORNER 2D");
@@ -48,7 +48,7 @@ class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1>
}
public:
- static ConstraintBase* create(FeatureBase* _feature_ptr, NodeBase* _correspondant_ptr)
+ static ConstraintBasePtr create(FeatureBasePtr _feature_ptr, NodeBasePtr _correspondant_ptr)
{
return new ConstraintCorner2D(_feature_ptr, (LandmarkCorner2D*) _correspondant_ptr);
}
diff --git a/src/constraint_epipolar.h b/src/constraint_epipolar.h
index 4343fd79a..a59151a40 100644
--- a/src/constraint_epipolar.h
+++ b/src/constraint_epipolar.h
@@ -8,7 +8,7 @@ namespace wolf {
class ConstraintEpipolar : public ConstraintBase
{
public:
- ConstraintEpipolar(FeatureBase* _feature_ptr, FeatureBase* _feature_other_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE);
+ ConstraintEpipolar(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE);
virtual ~ConstraintEpipolar();
@@ -29,15 +29,15 @@ class ConstraintEpipolar : public ConstraintBase
virtual unsigned int getSize() const{return 0;}
public:
- static wolf::ConstraintBase* create(FeatureBase* _feature_ptr, //
- NodeBase* _correspondant_ptr)
+ static wolf::ConstraintBasePtr create(FeatureBasePtr _feature_ptr, //
+ NodeBasePtr _correspondant_ptr)
{
- return new ConstraintEpipolar(_feature_ptr, (FeatureBase*)_correspondant_ptr);
+ return new ConstraintEpipolar(_feature_ptr, (FeatureBasePtr)_correspondant_ptr);
}
};
-inline ConstraintEpipolar::ConstraintEpipolar(FeatureBase* _feature_ptr, FeatureBase* _feature_other_ptr, bool _apply_loss_function, ConstraintStatus _status) :
+inline ConstraintEpipolar::ConstraintEpipolar(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr, bool _apply_loss_function, ConstraintStatus _status) :
ConstraintBase(CTR_EPIPOLAR, _feature_other_ptr, _apply_loss_function, _status)
{
setType("EPIPOLAR");
diff --git a/src/constraint_fix.h b/src/constraint_fix.h
index 89cd3b71b..dd8aab33d 100644
--- a/src/constraint_fix.h
+++ b/src/constraint_fix.h
@@ -14,7 +14,7 @@ class ConstraintFix: public ConstraintSparse<3,2,1>
public:
// static const unsigned int N_BLOCKS = 2;
- ConstraintFix(FeatureBase* _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintFix(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintSparse<3, 2, 1>(CTR_FIX, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),
_ftr_ptr->getFramePtr()->getOPtr())
{
diff --git a/src/constraint_gps_2D.h b/src/constraint_gps_2D.h
index 91af883a7..09d5c78f8 100644
--- a/src/constraint_gps_2D.h
+++ b/src/constraint_gps_2D.h
@@ -14,7 +14,7 @@ class ConstraintGPS2D : public ConstraintSparse<2, 2>
public:
// static const unsigned int N_BLOCKS = 1;
- ConstraintGPS2D(FeatureBase* _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintGPS2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintSparse<2, 2>(CTR_GPS_FIX_2D, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr())
{
setType("GPS FIX 2D");
diff --git a/src/constraint_gps_pseudorange_2D.h b/src/constraint_gps_pseudorange_2D.h
index 6e007f658..021b2438c 100644
--- a/src/constraint_gps_pseudorange_2D.h
+++ b/src/constraint_gps_pseudorange_2D.h
@@ -28,7 +28,7 @@ namespace wolf {
class ConstraintGPSPseudorange2D : public ConstraintSparse<1, 2, 1, 3, 1, 3, 1>
{
public:
- ConstraintGPSPseudorange2D(FeatureBase* _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintGPSPseudorange2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintSparse<1, 2, 1, 3, 1, 3, 1>(CTR_GPS_PR_2D,
_apply_loss_function,
_status,
diff --git a/src/constraint_gps_pseudorange_3D.h b/src/constraint_gps_pseudorange_3D.h
index 2edf0aa3c..a97abadcf 100644
--- a/src/constraint_gps_pseudorange_3D.h
+++ b/src/constraint_gps_pseudorange_3D.h
@@ -24,7 +24,7 @@ class ConstraintGPSPseudorange3D: public ConstraintSparse<1, 3, 4, 3, 1, 3, 4>
public:
- ConstraintGPSPseudorange3D(FeatureBase* _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintGPSPseudorange3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintSparse<1, 3, 4, 3, 1, 3, 4>(CTR_GPS_PR_3D, _apply_loss_function, _status,
_ftr_ptr->getFramePtr()->getPPtr(), // position of the vehicle's frame with respect to map frame
_ftr_ptr->getFramePtr()->getOPtr(), // orientation of the vehicle's frame wrt map frame
@@ -77,8 +77,8 @@ protected:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
- static wolf::ConstraintBase* create(FeatureBase* _feature_ptr, //
- NodeBase* _correspondant_ptr = nullptr)
+ static wolf::ConstraintBasePtr create(FeatureBasePtr _feature_ptr, //
+ NodeBasePtr _correspondant_ptr = nullptr)
{
return new ConstraintGPSPseudorange3D(_feature_ptr);
}
diff --git a/src/constraint_image.h b/src/constraint_image.h
index 2840aa9d0..bb555aa73 100644
--- a/src/constraint_image.h
+++ b/src/constraint_image.h
@@ -23,7 +23,7 @@ class ConstraintImage : public ConstraintSparse<2, 3, 4, 3, 4, 4>
public:
// static const unsigned int N_BLOCKS = 5; //TODO: Prueba a comentarlo
- ConstraintImage(FeatureBase* _ftr_ptr, FrameBase* _frame_ptr, LandmarkAHP* _landmark_ptr,
+ ConstraintImage(FeatureBasePtr _ftr_ptr, FrameBasePtr _frame_ptr, LandmarkAHP* _landmark_ptr,
bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintSparse<2, 3, 4, 3, 4, 4>(CTR_AHP, _landmark_ptr, _apply_loss_function, _status,
_frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _landmark_ptr->getAnchorFrame()->getPPtr()
@@ -60,10 +60,10 @@ class ConstraintImage : public ConstraintSparse<2, 3, 4, 3, 4, 4>
}
// public:
-// static wolf::ConstraintBase* create(FeatureBase* _feature_ptr, //
-// NodeBase* _correspondant_ptr)
+// static wolf::ConstraintBasePtr create(FeatureBasePtr _feature_ptr, //
+// NodeBasePtr _correspondant_ptr)
// {
-// return new ConstraintImage(_feature_ptr, (FrameBase*)_correspondant_ptr);
+// return new ConstraintImage(_feature_ptr, (FrameBasePtr)_correspondant_ptr);
// }
};
diff --git a/src/constraint_image_new_landmark.h b/src/constraint_image_new_landmark.h
index 5368ceb5f..3779598d0 100644
--- a/src/constraint_image_new_landmark.h
+++ b/src/constraint_image_new_landmark.h
@@ -21,7 +21,7 @@ class ConstraintImageNewLandmark : public ConstraintSparse<2, 3, 4, 4>
public:
// static const unsigned int N_BLOCKS = 3; //TODO: Prueba a comentarlo
- ConstraintImageNewLandmark(FeatureBase* _ftr_ptr, FrameBase* _frame_ptr, LandmarkAHP* _landmark_ptr,
+ ConstraintImageNewLandmark(FeatureBasePtr _ftr_ptr, FrameBasePtr _frame_ptr, LandmarkAHP* _landmark_ptr,
bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintSparse<2, 3, 4, 4>(CTR_AHP_NL, _landmark_ptr, _apply_loss_function, _status,
_frame_ptr->getPPtr(), _frame_ptr->getOPtr(),_landmark_ptr->getPPtr()),
@@ -55,10 +55,10 @@ class ConstraintImageNewLandmark : public ConstraintSparse<2, 3, 4, 4>
}
// public:
-// static wolf::ConstraintBase* create(FeatureBase* _feature_ptr, //
-// NodeBase* _correspondant_ptr)
+// static wolf::ConstraintBasePtr create(FeatureBasePtr _feature_ptr, //
+// NodeBasePtr _correspondant_ptr)
// {
-// return new ConstraintImage(_feature_ptr, (FrameBase*)_correspondant_ptr);
+// return new ConstraintImage(_feature_ptr, (FrameBasePtr)_correspondant_ptr);
// }
};
diff --git a/src/constraint_imu.h b/src/constraint_imu.h
index ccc5e58b0..f42eaf726 100644
--- a/src/constraint_imu.h
+++ b/src/constraint_imu.h
@@ -74,7 +74,7 @@ inline ConstraintIMU::ConstraintIMU(FeatureIMU* _ftr_ptr, FrameIMU* _frame_ptr,
dDp_dwb_(_ftr_ptr->dDp_dwb_),
dDv_dwb_(_ftr_ptr->dDv_dwb_),
dDq_dwb_(_ftr_ptr->dDq_dwb_),
- dt_(frame_ptr_->getTimeStamp()-feature_ptr_->getFramePtr()->getTimeStamp()),
+ dt_(_frame_ptr->getTimeStamp() - getFeaturePtr()->getFramePtr()->getTimeStamp()),
dt_2_(dt_*dt_),
g_(wolf::gravity())
diff --git a/src/constraint_odom_2D.h b/src/constraint_odom_2D.h
index 4fc09e471..d8238475f 100644
--- a/src/constraint_odom_2D.h
+++ b/src/constraint_odom_2D.h
@@ -12,7 +12,7 @@ class ConstraintOdom2D : public ConstraintSparse<3, 2, 1, 2, 1>
public:
// static const unsigned int N_BLOCKS = 4;
- ConstraintOdom2D(FeatureBase* _ftr_ptr, FrameBase* _frame_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintOdom2D(FeatureBasePtr _ftr_ptr, FrameBasePtr _frame_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintSparse<3, 2, 1, 2, 1>(CTR_ODOM_2D, _frame_ptr, _apply_loss_function, _status, _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
{
setType("ODOM 2D");
@@ -40,10 +40,10 @@ class ConstraintOdom2D : public ConstraintSparse<3, 2, 1, 2, 1>
}
public:
- static wolf::ConstraintBase* create(FeatureBase* _feature_ptr, //
- NodeBase* _correspondant_ptr)
+ static wolf::ConstraintBasePtr create(FeatureBasePtr _feature_ptr, //
+ NodeBasePtr _correspondant_ptr)
{
- return new ConstraintOdom2D(_feature_ptr, (FrameBase*)_correspondant_ptr);
+ return new ConstraintOdom2D(_feature_ptr, (FrameBasePtr)_correspondant_ptr);
}
};
diff --git a/src/constraint_odom_2D_analytic.h b/src/constraint_odom_2D_analytic.h
index df4ef4792..756d91bee 100644
--- a/src/constraint_odom_2D_analytic.h
+++ b/src/constraint_odom_2D_analytic.h
@@ -10,7 +10,7 @@ namespace wolf {
class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic
{
public:
- ConstraintOdom2DAnalytic(FeatureBase* _ftr_ptr, FrameBase* _frame_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintOdom2DAnalytic(FeatureBasePtr _ftr_ptr, FrameBasePtr _frame_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintRelative2DAnalytic(_ftr_ptr, CTR_ODOM_2D, _frame_ptr, _apply_loss_function, _status)
{
setType("ODOM 2D ANALYTIC");
@@ -105,10 +105,10 @@ class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic
public:
- static wolf::ConstraintBase* create(FeatureBase* _feature_ptr, //
- NodeBase* _correspondant_ptr)
+ static wolf::ConstraintBasePtr create(FeatureBasePtr _feature_ptr, //
+ NodeBasePtr _correspondant_ptr)
{
- return new ConstraintOdom2DAnalytic(_feature_ptr, (FrameBase*)_correspondant_ptr);
+ return new ConstraintOdom2DAnalytic(_feature_ptr, (FrameBasePtr)_correspondant_ptr);
}
};
diff --git a/src/constraint_relative_2D_analytic.h b/src/constraint_relative_2D_analytic.h
index c85735d2e..469ee9140 100644
--- a/src/constraint_relative_2D_analytic.h
+++ b/src/constraint_relative_2D_analytic.h
@@ -14,7 +14,7 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic
/** \brief Constructor of category CTR_FRAME
**/
- ConstraintRelative2DAnalytic(FeatureBase* _ftr_ptr, ConstraintType _tp, FrameBase* _frame_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintRelative2DAnalytic(FeatureBasePtr _ftr_ptr, ConstraintType _tp, FrameBasePtr _frame_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintAnalytic(_tp, _frame_ptr, _apply_loss_function, _status, _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
{
//
@@ -22,7 +22,7 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic
/** \brief Constructor of category CTR_FEATURE
**/
- ConstraintRelative2DAnalytic(FeatureBase* _ftr_ptr, ConstraintType _tp, FeatureBase* _ftr_other_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintRelative2DAnalytic(FeatureBasePtr _ftr_ptr, ConstraintType _tp, FeatureBasePtr _ftr_other_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintAnalytic(_tp, _ftr_other_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _ftr_other_ptr->getFramePtr()->getPPtr(), _ftr_other_ptr->getFramePtr()->getOPtr() )
{
//
@@ -30,7 +30,7 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic
/** \brief Constructor of category CTR_LANDMARK
**/
- ConstraintRelative2DAnalytic(FeatureBase* _ftr_ptr, ConstraintType _tp, LandmarkBase* _landmark_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintRelative2DAnalytic(FeatureBasePtr _ftr_ptr, ConstraintType _tp, LandmarkBasePtr _landmark_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintAnalytic(_tp, _landmark_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _landmark_ptr->getPPtr(), _landmark_ptr->getOPtr())
{
//
diff --git a/src/constraint_sparse.h b/src/constraint_sparse.h
index 1cd1d8875..7204835e0 100644
--- a/src/constraint_sparse.h
+++ b/src/constraint_sparse.h
@@ -62,7 +62,7 @@ class ConstraintSparse: public ConstraintBase
* Constructor of category CTR_FRAME
*
**/
- ConstraintSparse(ConstraintType _tp, FrameBase* _frame_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ ConstraintSparse(ConstraintType _tp, FrameBasePtr _frame_ptr, bool _apply_loss_function, ConstraintStatus _status,
StateBlock* _state0Ptr,
StateBlock* _state1Ptr = nullptr,
StateBlock* _state2Ptr = nullptr,
@@ -79,7 +79,7 @@ class ConstraintSparse: public ConstraintBase
* Constructor of category CTR_FEATURE
*
**/
- ConstraintSparse(ConstraintType _tp, FeatureBase* _feature_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ ConstraintSparse(ConstraintType _tp, FeatureBasePtr _feature_ptr, bool _apply_loss_function, ConstraintStatus _status,
StateBlock* _state0Ptr,
StateBlock* _state1Ptr = nullptr,
StateBlock* _state2Ptr = nullptr,
@@ -96,7 +96,7 @@ class ConstraintSparse: public ConstraintBase
* Constructor of category CTR_LANDMARK
*
**/
- ConstraintSparse(ConstraintType _tp, LandmarkBase* _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ ConstraintSparse(ConstraintType _tp, LandmarkBasePtr _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status,
StateBlock* _state0Ptr,
StateBlock* _state1Ptr = nullptr,
StateBlock* _state2Ptr = nullptr,
@@ -204,7 +204,7 @@ ConstraintSparse<MEASUREMENT_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
- BLOCK_9_SIZE>::ConstraintSparse(ConstraintType _tp, FrameBase* _frame_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ BLOCK_9_SIZE>::ConstraintSparse(ConstraintType _tp, FrameBasePtr _frame_ptr, bool _apply_loss_function, ConstraintStatus _status,
StateBlock* _state0Ptr,
StateBlock* _state1Ptr,
StateBlock* _state2Ptr,
@@ -243,7 +243,7 @@ ConstraintSparse<MEASUREMENT_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
- BLOCK_9_SIZE>::ConstraintSparse(ConstraintType _tp, FeatureBase* _feature_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ BLOCK_9_SIZE>::ConstraintSparse(ConstraintType _tp, FeatureBasePtr _feature_ptr, bool _apply_loss_function, ConstraintStatus _status,
StateBlock* _state0Ptr,
StateBlock* _state1Ptr,
StateBlock* _state2Ptr,
@@ -282,7 +282,7 @@ ConstraintSparse<MEASUREMENT_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
- BLOCK_9_SIZE>::ConstraintSparse(ConstraintType _tp, LandmarkBase* _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ BLOCK_9_SIZE>::ConstraintSparse(ConstraintType _tp, LandmarkBasePtr _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status,
StateBlock* _state0Ptr,
StateBlock* _state1Ptr,
StateBlock* _state2Ptr,
diff --git a/src/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp
index 5409005c8..13e77a268 100644
--- a/src/examples/test_2_lasers_offline.cpp
+++ b/src/examples/test_2_lasers_offline.cpp
@@ -154,9 +154,9 @@ int main(int argc, char** argv)
odom_params.dist_traveled_th_ = 5;
odom_params.elapsed_time_th_ = 10;
ProcessorOdom2D* odom_processor = (ProcessorOdom2D*)problem.installProcessor("ODOM 2D", "main odometry", odom_sensor, &odom_params);
- //SensorBase* gps_sensor = problem.installSensor("GPS FIX", "GPS fix", gps_position);
- SensorBase* laser_1_sensor = problem.installSensor("LASER 2D", "front laser", laser_1_pose2D, &laser_1_intrinsics);
- SensorBase* laser_2_sensor = problem.installSensor("LASER 2D", "rear laser", laser_2_pose2D, &laser_2_intrinsics);
+ //SensorBasePtr gps_sensor = problem.installSensor("GPS FIX", "GPS fix", gps_position);
+ SensorBasePtr laser_1_sensor = problem.installSensor("LASER 2D", "front laser", laser_1_pose2D, &laser_1_intrinsics);
+ SensorBasePtr laser_2_sensor = problem.installSensor("LASER 2D", "rear laser", laser_2_pose2D, &laser_2_intrinsics);
problem.installProcessor("LASER 2D", "front laser processor", laser_1_sensor, &laser_1_processor_params);
problem.installProcessor("LASER 2D", "rear laser processor", laser_2_sensor, &laser_2_processor_params);
diff --git a/src/examples/test_analytic_odom_constraint.cpp b/src/examples/test_analytic_odom_constraint.cpp
index 55c5ffbc8..fa284f2cb 100644
--- a/src/examples/test_analytic_odom_constraint.cpp
+++ b/src/examples/test_analytic_odom_constraint.cpp
@@ -55,13 +55,13 @@ int main(int argc, char** argv)
ceres::Solver::Summary summary_autodiff, summary_analytic;
// loading variables
- std::map<unsigned int, FrameBase*> index_2_frame_ptr_autodiff;
- std::map<unsigned int, FrameBase*> index_2_frame_ptr_analytic;
+ std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_autodiff;
+ std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_analytic;
// Wolf problem
- Problem* wolf_problem_autodiff = new Problem(FRM_PO_2D);
- Problem* wolf_problem_analytic = new Problem(FRM_PO_2D);
- SensorBase* sensor = new SensorBase(SEN_ODOM_2D, "ODOM 2D", new StateBlock(Eigen::VectorXs::Zero(2)), new StateBlock(Eigen::VectorXs::Zero(1)), new StateBlock(Eigen::VectorXs::Zero(2)), 2);
+ ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2D);
+ ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2D);
+ SensorBasePtr sensor = new SensorBase(SEN_ODOM_2D, "ODOM 2D", new StateBlock(Eigen::VectorXs::Zero(2)), new StateBlock(Eigen::VectorXs::Zero(1)), new StateBlock(Eigen::VectorXs::Zero(2)), 2);
// Ceres wrapper
ceres::Solver::Options ceres_options;
@@ -128,8 +128,8 @@ int main(int argc, char** argv)
bNum.clear();
// add frame to problem
- FrameBase* vertex_frame_ptr_autodiff = new FrameBase(TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
- FrameBase* vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_autodiff = new FrameBase(TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
wolf_problem_autodiff->getTrajectoryPtr()->addFrame(vertex_frame_ptr_autodiff);
wolf_problem_analytic->getTrajectoryPtr()->addFrame(vertex_frame_ptr_analytic);
// store
@@ -238,12 +238,12 @@ int main(int argc, char** argv)
bNum.clear();
// add capture, feature and constraint to problem
- FeatureBase* feature_ptr_autodiff = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_autodiff = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_autodiff = new CaptureVoid(TimeStamp(0), sensor);
assert(index_2_frame_ptr_autodiff.find(edge_old) != index_2_frame_ptr_autodiff.end() && "edge from vertex not added!");
- FrameBase* frame_old_ptr_autodiff = index_2_frame_ptr_autodiff[edge_old];
+ FrameBasePtr frame_old_ptr_autodiff = index_2_frame_ptr_autodiff[edge_old];
assert(index_2_frame_ptr_autodiff.find(edge_new) != index_2_frame_ptr_autodiff.end() && "edge to vertex not added!");
- FrameBase* frame_new_ptr_autodiff = index_2_frame_ptr_autodiff[edge_new];
+ FrameBasePtr frame_new_ptr_autodiff = index_2_frame_ptr_autodiff[edge_new];
frame_new_ptr_autodiff->addCapture(capture_ptr_autodiff);
capture_ptr_autodiff->addFeature(feature_ptr_autodiff);
ConstraintOdom2D* constraint_ptr_autodiff = new ConstraintOdom2D(feature_ptr_autodiff, frame_old_ptr_autodiff);
@@ -251,12 +251,12 @@ int main(int argc, char** argv)
//std::cout << "Added autodiff edge! " << constraint_ptr_autodiff->nodeId() << " from vertex " << constraint_ptr_autodiff->getCapturePtr()->getFramePtr()->nodeId() << " to " << constraint_ptr_autodiff->getFrameOtherPtr()->nodeId() << std::endl;
// add capture, feature and constraint to problem
- FeatureBase* feature_ptr_analytic = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_analytic = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_analytic = new CaptureVoid(TimeStamp(0), sensor);
assert(index_2_frame_ptr_analytic.find(edge_old) != index_2_frame_ptr_analytic.end() && "edge from vertex not added!");
- FrameBase* frame_old_ptr_analytic = index_2_frame_ptr_analytic[edge_old];
+ FrameBasePtr frame_old_ptr_analytic = index_2_frame_ptr_analytic[edge_old];
assert(index_2_frame_ptr_analytic.find(edge_new) != index_2_frame_ptr_analytic.end() && "edge to vertex not added!");
- FrameBase* frame_new_ptr_analytic = index_2_frame_ptr_analytic[edge_new];
+ FrameBasePtr frame_new_ptr_analytic = index_2_frame_ptr_analytic[edge_new];
frame_new_ptr_analytic->addCapture(capture_ptr_analytic);
capture_ptr_analytic->addFeature(feature_ptr_analytic);
ConstraintOdom2DAnalytic* constraint_ptr_analytic = new ConstraintOdom2DAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
@@ -276,8 +276,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBase* first_frame_autodiff = wolf_problem_autodiff->getTrajectoryPtr()->getFrameListPtr()->front();
- FrameBase* first_frame_analytic = wolf_problem_analytic->getTrajectoryPtr()->getFrameListPtr()->front();
+ FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectoryPtr()->getFrameListPtr()->front();
+ FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectoryPtr()->getFrameListPtr()->front();
CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase(SEN_ABSOLUTE_POSE, "ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3s::Identity() * 0.01);
CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase(SEN_ABSOLUTE_POSE, "ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3s::Identity() * 0.01);
first_frame_autodiff->addCapture(initial_covariance_autodiff);
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index af38f3519..eecb272ae 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -249,7 +249,7 @@ int main(int argc, char** argv)
odom_trajectory.head(3) = ground_truth_pose;
// Origin Key Frame
- FrameBase* origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts);
+ FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts);
// Prior covariance
CaptureFix* initial_covariance = new CaptureFix(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1);
diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
index 97505d961..fa0e6990f 100644
--- a/src/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -256,7 +256,7 @@ int main(int argc, char** argv)
odom_trajectory.head(3) = ground_truth_pose;
// Origin Key Frame
- FrameBase* origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts);
+ FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts);
// Prior covariance
CaptureFix* initial_covariance = new CaptureFix(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1);
diff --git a/src/examples/test_image.cpp b/src/examples/test_image.cpp
index 7b0c350e4..0743e4dd7 100644
--- a/src/examples/test_image.cpp
+++ b/src/examples/test_image.cpp
@@ -70,7 +70,7 @@ int main(int argc, char** argv)
std::cout << "Wolf path: " << wolf_path << std::endl;
- Problem* wolf_problem_ = new Problem(FRM_PO_3D);
+ ProblemPtr wolf_problem_ = new Problem(FRM_PO_3D);
//=====================================================
// Method 1: Use data generated here for sensor and processor
@@ -118,7 +118,7 @@ int main(int argc, char** argv)
// SENSOR
// one-liner API
- SensorBase* sensor_ptr = wolf_problem_->installSensor("CAMERA", "PinHole", Eigen::VectorXs::Zero(7), wolf_path + "/src/examples/camera_params.yaml");
+ SensorBasePtr sensor_ptr = wolf_problem_->installSensor("CAMERA", "PinHole", Eigen::VectorXs::Zero(7), wolf_path + "/src/examples/camera_params.yaml");
SensorCamera* camera_ptr = (SensorCamera*)sensor_ptr;
// PROCESSOR
diff --git a/src/examples/test_motion_2d.cpp b/src/examples/test_motion_2d.cpp
index 2a0691a2e..caf5bd53d 100644
--- a/src/examples/test_motion_2d.cpp
+++ b/src/examples/test_motion_2d.cpp
@@ -50,11 +50,11 @@ int main()
Eigen::MatrixXs data_cov = Eigen::MatrixXs::Identity(2, 2) * 0.01;
// Create Wolf tree nodes
- Problem* problem_ptr = new Problem(FRM_PO_2D);
- SensorBase* sensor_odom_ptr = new SensorBase(SEN_ODOM_2D, "ODOM 2D", new StateBlock(Eigen::Vector2s::Zero(), true),
+ ProblemPtr problem_ptr = new Problem(FRM_PO_2D);
+ SensorBasePtr sensor_odom_ptr = new SensorBase(SEN_ODOM_2D, "ODOM 2D", new StateBlock(Eigen::Vector2s::Zero(), true),
new StateBlock(Eigen::Vector1s::Zero(), true),
new StateBlock(Eigen::VectorXs::Zero(0), true), 0);
- SensorBase* sensor_fix_ptr = new SensorBase(SEN_ABSOLUTE_POSE, "ABSOLUTE POSE", nullptr, nullptr, nullptr, 0);
+ SensorBasePtr sensor_fix_ptr = new SensorBase(SEN_ABSOLUTE_POSE, "ABSOLUTE POSE", nullptr, nullptr, nullptr, 0);
ProcessorOdom2D* odom2d_ptr = new ProcessorOdom2D(100,100,100);
// Assemble Wolf tree by linking the nodes
sensor_odom_ptr->addProcessor(odom2d_ptr);
@@ -70,7 +70,7 @@ int main()
// Origin Key Frame
- FrameBase* origin_frame = problem_ptr->createFrame(KEY_FRAME, x0, t0);
+ FrameBasePtr origin_frame = problem_ptr->createFrame(KEY_FRAME, x0, t0);
// Prior covariance
CaptureFix* initial_covariance = new CaptureFix(TimeStamp(0), sensor_fix_ptr, x0, init_cov);
@@ -206,7 +206,7 @@ int main()
TimeStamp t_split = t0 + 0.13;
std::cout << "Split time: " << t_split - t0 << std::endl;
- FrameBase* new_keyframe_ptr = problem_ptr->createFrame(KEY_FRAME, odom2d_ptr->getState(t_split), t_split);
+ FrameBasePtr new_keyframe_ptr = problem_ptr->createFrame(KEY_FRAME, odom2d_ptr->getState(t_split), t_split);
odom2d_ptr->keyFrameCallback(new_keyframe_ptr, 0);
diff --git a/src/examples/test_node_linked.cpp b/src/examples/test_node_linked.cpp
index 9e70fac29..54fdaf075 100644
--- a/src/examples/test_node_linked.cpp
+++ b/src/examples/test_node_linked.cpp
@@ -103,7 +103,7 @@ int main()
cout << "========================================================" << endl;
cout << endl << "TEST 1. Constructors" << endl;
- Problem* problem_(new Problem(FRM_PO_2D));
+ ProblemPtr problem_(new Problem(FRM_PO_2D));
TrajectoryN* trajectory_(new TrajectoryN(2));
FrameN* frame_1_(new FrameN(1.011));
FrameN* frame_2_(new FrameN(2.022));
@@ -136,7 +136,7 @@ int main()
cout << "========================================================" << endl;
cout << endl << "TEST 5. getWolfProblem()" << endl;
- Problem* nb_ptr = sensor_data_radar_->getProblem();
+ ProblemPtr nb_ptr = sensor_data_radar_->getProblem();
//shared_ptr<TrajectoryN> nb_shptr((TrajectoryN*)nb_ptr);
// cout << "TOP node is: " << nb_ptr->nodeId() << endl;
//cout << "nb_shptr.use_count(): " << nb_shptr.use_count() << "; value: " << nb_shptr.get() << endl;
diff --git a/src/examples/test_processor_image_landmark.cpp b/src/examples/test_processor_image_landmark.cpp
index 9b74d316f..340115afe 100644
--- a/src/examples/test_processor_image_landmark.cpp
+++ b/src/examples/test_processor_image_landmark.cpp
@@ -63,7 +63,7 @@ int main(int argc, char** argv)
// Wolf problem
- Problem* wolf_problem_ptr_ = new Problem(FRM_PO_3D);
+ ProblemPtr wolf_problem_ptr_ = new Problem(FRM_PO_3D);
//=====================================================
// Method 1: Use data generated here for sensor and processor
@@ -125,7 +125,7 @@ int main(int argc, char** argv)
const Eigen::VectorXs extr = extrinsic_cam;
/* Do this while there aren't extrinsic parameters on the yaml */
- SensorBase* sensor_ptr = wolf_problem_ptr_->installSensor("CAMERA", "PinHole", extr, wolf_path + "/src/examples/camera_params.yaml");
+ SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("CAMERA", "PinHole", extr, wolf_path + "/src/examples/camera_params.yaml");
SensorCamera* camera_ptr_ = (SensorCamera*)sensor_ptr;
diff --git a/src/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp
index 8e45e2237..6822ea3cb 100644
--- a/src/examples/test_processor_imu.cpp
+++ b/src/examples/test_processor_imu.cpp
@@ -70,10 +70,10 @@ int main(int argc, char** argv)
}
// Wolf problem
- Problem* wolf_problem_ptr_ = new Problem(FRM_PVQBB_3D);
- Eigen::VectorXs IMU_extrinsics(7);
- IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
- SensorBase* sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, nullptr);
+ ProblemPtr wolf_problem_ptr_ = new Problem(FRM_PVQBB_3D);
+ Eigen::VectorXs extrinsics(7);
+ extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
+ SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", extrinsics, nullptr);
wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
// Time and data variables
diff --git a/src/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp
index e8cbbc0cc..f32266a81 100644
--- a/src/examples/test_processor_tracker_feature.cpp
+++ b/src/examples/test_processor_tracker_feature.cpp
@@ -23,8 +23,8 @@ int main()
std::cout << std::endl << "==================== processor tracker feature test ======================" << std::endl;
// Wolf problem
- Problem* wolf_problem_ptr_ = new Problem(FRM_PO_2D);
- SensorBase* sensor_ptr_ = new SensorBase(SEN_ODOM_2D, "ODOM 2D", new StateBlock(Eigen::VectorXs::Zero(2)),
+ ProblemPtr wolf_problem_ptr_ = new Problem(FRM_PO_2D);
+ SensorBasePtr sensor_ptr_ = new SensorBase(SEN_ODOM_2D, "ODOM 2D", new StateBlock(Eigen::VectorXs::Zero(2)),
new StateBlock(Eigen::VectorXs::Zero(1)),
new StateBlock(Eigen::VectorXs::Zero(2)), 2);
diff --git a/src/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp
index ce44397c3..ed74152ce 100644
--- a/src/examples/test_processor_tracker_landmark.cpp
+++ b/src/examples/test_processor_tracker_landmark.cpp
@@ -23,8 +23,8 @@ int main()
std::cout << std::endl << "==================== processor tracker landmark test ======================" << std::endl;
// Wolf problem
- Problem* wolf_problem_ptr_ = new Problem(FRM_PO_2D);
- SensorBase* sensor_ptr_ = new SensorBase(SEN_ODOM_2D, "ODOM 2D", new StateBlock(Eigen::VectorXs::Zero(2)),
+ ProblemPtr wolf_problem_ptr_ = new Problem(FRM_PO_2D);
+ SensorBasePtr sensor_ptr_ = new SensorBase(SEN_ODOM_2D, "ODOM 2D", new StateBlock(Eigen::VectorXs::Zero(2)),
new StateBlock(Eigen::VectorXs::Zero(1)),
new StateBlock(Eigen::VectorXs::Zero(2)), 2);
diff --git a/src/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp
index 9fc2876ea..732864263 100644
--- a/src/examples/test_sort_keyframes.cpp
+++ b/src/examples/test_sort_keyframes.cpp
@@ -19,7 +19,7 @@
using namespace wolf;
-void printFrames(Problem* _problem_ptr)
+void printFrames(ProblemPtr _problem_ptr)
{
std::cout << "TRAJECTORY:" << std::endl;
for (auto frm : *(_problem_ptr->getTrajectoryPtr()->getFrameListPtr()))
@@ -31,11 +31,11 @@ int main()
Problem problem(FRM_PO_2D);
problem.createFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.1));
- FrameBase* frm2 = problem.createFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.2));
- FrameBase* frm3 = problem.createFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.3));
+ FrameBasePtr frm2 = problem.createFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.2));
+ FrameBasePtr frm3 = problem.createFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.3));
problem.createFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.4));
- FrameBase* frm5 = problem.createFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.5));
- FrameBase* frm6 = problem.createFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.6));
+ FrameBasePtr frm5 = problem.createFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.5));
+ FrameBasePtr frm6 = problem.createFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.6));
printFrames(&problem);
@@ -64,12 +64,12 @@ int main()
printFrames(&problem);
- FrameBase* frm7 = problem.createFrame(KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.7));
+ FrameBasePtr frm7 = problem.createFrame(KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.7));
std::cout << std::endl << "created Key Frame " << frm7->id() << " TS: " << 0.7 << std::endl;
printFrames(&problem);
- FrameBase* frm8 = problem.createFrame(KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.35));
+ FrameBasePtr frm8 = problem.createFrame(KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.35));
std::cout << std::endl << "created Key Frame " << frm8->id() << " TS: " << 0.35 << std::endl;
printFrames(&problem);
diff --git a/src/examples/test_wolf_autodiffwrapper.cpp b/src/examples/test_wolf_autodiffwrapper.cpp
index b5a47c675..88ed84384 100644
--- a/src/examples/test_wolf_autodiffwrapper.cpp
+++ b/src/examples/test_wolf_autodiffwrapper.cpp
@@ -40,14 +40,14 @@ int main(int argc, char** argv)
ceres::Solver::Summary summary_ceres_diff, summary_wolf_diff;
// loading variables
- std::map<unsigned int, FrameBase*> index_2_frame_ptr_ceres_diff;
- std::map<unsigned int, FrameBase*> index_2_frame_ptr_wolf_diff;
- std::map<FrameBase*, unsigned int> frame_ptr_2_index_wolf_diff;
+ std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_ceres_diff;
+ std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_wolf_diff;
+ std::map<FrameBasePtr, unsigned int> frame_ptr_2_index_wolf_diff;
// Wolf problem
- Problem* wolf_problem_ceres_diff = new Problem(FRM_PO_2D);
- Problem* wolf_problem_wolf_diff = new Problem(FRM_PO_2D);
- SensorBase* sensor = new SensorBase(SEN_ODOM_2D, "ODOM 2D", new StateBlock(Eigen::VectorXs::Zero(2)), new StateBlock(Eigen::VectorXs::Zero(1)), new StateBlock(Eigen::VectorXs::Zero(2)), 2);
+ ProblemPtr wolf_problem_ceres_diff = new Problem(FRM_PO_2D);
+ ProblemPtr wolf_problem_wolf_diff = new Problem(FRM_PO_2D);
+ SensorBasePtr sensor = new SensorBase(SEN_ODOM_2D, "ODOM 2D", new StateBlock(Eigen::VectorXs::Zero(2)), new StateBlock(Eigen::VectorXs::Zero(1)), new StateBlock(Eigen::VectorXs::Zero(2)), 2);
// Ceres wrappers
ceres::Solver::Options ceres_options;
@@ -114,8 +114,8 @@ int main(int argc, char** argv)
bNum.clear();
// add frame to problem
- FrameBase* vertex_frame_ptr_ceres_diff = new FrameBase(TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
- FrameBase* vertex_frame_ptr_wolf_diff = new FrameBase(TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_ceres_diff = new FrameBase(TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_wolf_diff = new FrameBase(TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
wolf_problem_ceres_diff->getTrajectoryPtr()->addFrame(vertex_frame_ptr_ceres_diff);
wolf_problem_wolf_diff->getTrajectoryPtr()->addFrame(vertex_frame_ptr_wolf_diff);
// store
@@ -225,18 +225,18 @@ int main(int argc, char** argv)
bNum.clear();
// add capture, feature and constraint to problem
- FeatureBase* feature_ptr_ceres_diff = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
- FeatureBase* feature_ptr_wolf_diff = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_ceres_diff = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_wolf_diff = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_ceres_diff = new CaptureVoid(TimeStamp(0), sensor);
CaptureVoid* capture_ptr_wolf_diff = new CaptureVoid(TimeStamp(0), sensor);
assert(index_2_frame_ptr_ceres_diff.find(edge_old) != index_2_frame_ptr_ceres_diff.end() && "edge from vertex not added!");
assert(index_2_frame_ptr_wolf_diff.find(edge_old) != index_2_frame_ptr_wolf_diff.end() && "edge from vertex not added!");
- FrameBase* frame_old_ptr_ceres_diff = index_2_frame_ptr_ceres_diff[edge_old];
- FrameBase* frame_old_ptr_wolf_diff = index_2_frame_ptr_wolf_diff[edge_old];
+ FrameBasePtr frame_old_ptr_ceres_diff = index_2_frame_ptr_ceres_diff[edge_old];
+ FrameBasePtr frame_old_ptr_wolf_diff = index_2_frame_ptr_wolf_diff[edge_old];
assert(index_2_frame_ptr_ceres_diff.find(edge_new) != index_2_frame_ptr_ceres_diff.end() && "edge to vertex not added!");
assert(index_2_frame_ptr_wolf_diff.find(edge_new) != index_2_frame_ptr_wolf_diff.end() && "edge to vertex not added!");
- FrameBase* frame_new_ptr_ceres_diff = index_2_frame_ptr_ceres_diff[edge_new];
- FrameBase* frame_new_ptr_wolf_diff = index_2_frame_ptr_wolf_diff[edge_new];
+ FrameBasePtr frame_new_ptr_ceres_diff = index_2_frame_ptr_ceres_diff[edge_new];
+ FrameBasePtr frame_new_ptr_wolf_diff = index_2_frame_ptr_wolf_diff[edge_new];
frame_new_ptr_ceres_diff->addCapture(capture_ptr_ceres_diff);
frame_new_ptr_wolf_diff->addCapture(capture_ptr_wolf_diff);
capture_ptr_ceres_diff->addFeature(feature_ptr_ceres_diff);
@@ -260,8 +260,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBase* first_frame_ceres_diff = wolf_problem_ceres_diff->getTrajectoryPtr()->getFrameListPtr()->front();
- FrameBase* first_frame_wolf_diff = wolf_problem_wolf_diff->getTrajectoryPtr()->getFrameListPtr()->front();
+ FrameBasePtr first_frame_ceres_diff = wolf_problem_ceres_diff->getTrajectoryPtr()->getFrameListPtr()->front();
+ FrameBasePtr first_frame_wolf_diff = wolf_problem_wolf_diff->getTrajectoryPtr()->getFrameListPtr()->front();
CaptureFix* initial_covariance_ceres_diff = new CaptureFix(TimeStamp(0), new SensorBase(SEN_ABSOLUTE_POSE, "ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_ceres_diff->getState(), Eigen::Matrix3s::Identity() * 0.01);
CaptureFix* initial_covariance_wolf_diff = new CaptureFix(TimeStamp(0), new SensorBase(SEN_ABSOLUTE_POSE, "ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_wolf_diff->getState(), Eigen::Matrix3s::Identity() * 0.01);
first_frame_ceres_diff->addCapture(initial_covariance_ceres_diff);
diff --git a/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
index 4228102a7..77b2a6d67 100644
--- a/src/examples/test_wolf_factories.cpp
+++ b/src/examples/test_wolf_factories.cpp
@@ -70,8 +70,8 @@ int main(void)
cout << "\n================== Intrinsics Factory ===================" << endl;
// Use params factory for camera intrinsics
- IntrinsicsBase* intr_cam_ptr = IntrinsicsFactory::get().create("CAMERA", WOLF_CONFIG + "/camera.yaml");
- ProcessorParamsBase* params_ptr = ProcessorParamsFactory::get().create("IMAGE", WOLF_CONFIG + "/processor_image_ORB.yaml");
+ IntrinsicsBasePtr intr_cam_ptr = IntrinsicsFactory::get().create("CAMERA", WOLF_CONFIG + "/camera.yaml");
+ ProcessorParamsBasePtr params_ptr = ProcessorParamsFactory::get().create("IMAGE", WOLF_CONFIG + "/processor_image_ORB.yaml");
cout << "CAMERA with intrinsics : " << ((IntrinsicsCamera*)intr_cam_ptr)->pinhole_model.transpose() << endl;
cout << "Processor IMAGE image width : " << ((ProcessorParamsImage*)params_ptr)->image.width << endl;
diff --git a/src/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp
index 635e54828..a86d6c373 100644
--- a/src/examples/test_wolf_imported_graph.cpp
+++ b/src/examples/test_wolf_imported_graph.cpp
@@ -57,14 +57,14 @@ int main(int argc, char** argv)
Scalar xi, yi, thi, si, ci, xj, yj;
// loading variables
- std::map<unsigned int, FrameBase*> index_2_frame_ptr_full;
- std::map<unsigned int, FrameBase*> index_2_frame_ptr_prun;
- std::map<FrameBase*, unsigned int> frame_ptr_2_index_prun;
+ std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_full;
+ std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_prun;
+ std::map<FrameBasePtr, unsigned int> frame_ptr_2_index_prun;
// Wolf problem
- Problem* wolf_problem_full = new Problem(FRM_PO_2D);
- Problem* wolf_problem_prun = new Problem(FRM_PO_2D);
- SensorBase* sensor = new SensorBase(SEN_ODOM_2D, "ODOM 2D", new StateBlock(Eigen::VectorXs::Zero(2)), new StateBlock(Eigen::VectorXs::Zero(1)), new StateBlock(Eigen::VectorXs::Zero(2)), 2);
+ ProblemPtr wolf_problem_full = new Problem(FRM_PO_2D);
+ ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2D);
+ SensorBasePtr sensor = new SensorBase(SEN_ODOM_2D, "ODOM 2D", new StateBlock(Eigen::VectorXs::Zero(2)), new StateBlock(Eigen::VectorXs::Zero(1)), new StateBlock(Eigen::VectorXs::Zero(2)), 2);
Eigen::SparseMatrix<Scalar> Lambda(0,0);
@@ -133,8 +133,8 @@ int main(int argc, char** argv)
bNum.clear();
// add frame to problem
- FrameBase* vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
- FrameBase* vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
wolf_problem_full->getTrajectoryPtr()->addFrame(vertex_frame_ptr_full);
wolf_problem_prun->getTrajectoryPtr()->addFrame(vertex_frame_ptr_prun);
// store
@@ -246,18 +246,18 @@ int main(int argc, char** argv)
bNum.clear();
// add capture, feature and constraint to problem
- FeatureBase* feature_ptr_full = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
- FeatureBase* feature_ptr_prun = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_full = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_prun = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_full = new CaptureVoid(TimeStamp(0), sensor);
CaptureVoid* capture_ptr_prun = new CaptureVoid(TimeStamp(0), sensor);
assert(index_2_frame_ptr_full.find(edge_old) != index_2_frame_ptr_full.end() && "edge from vertex not added!");
assert(index_2_frame_ptr_prun.find(edge_old) != index_2_frame_ptr_prun.end() && "edge from vertex not added!");
- FrameBase* frame_old_ptr_full = index_2_frame_ptr_full[edge_old];
- FrameBase* frame_old_ptr_prun = index_2_frame_ptr_prun[edge_old];
+ FrameBasePtr frame_old_ptr_full = index_2_frame_ptr_full[edge_old];
+ FrameBasePtr frame_old_ptr_prun = index_2_frame_ptr_prun[edge_old];
assert(index_2_frame_ptr_full.find(edge_new) != index_2_frame_ptr_full.end() && "edge to vertex not added!");
assert(index_2_frame_ptr_prun.find(edge_new) != index_2_frame_ptr_prun.end() && "edge to vertex not added!");
- FrameBase* frame_new_ptr_full = index_2_frame_ptr_full[edge_new];
- FrameBase* frame_new_ptr_prun = index_2_frame_ptr_prun[edge_new];
+ FrameBasePtr frame_new_ptr_full = index_2_frame_ptr_full[edge_new];
+ FrameBasePtr frame_new_ptr_prun = index_2_frame_ptr_prun[edge_new];
frame_new_ptr_full->addCapture(capture_ptr_full);
frame_new_ptr_prun->addCapture(capture_ptr_prun);
capture_ptr_full->addFeature(feature_ptr_full);
@@ -304,8 +304,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBase* first_frame_full = wolf_problem_full->getTrajectoryPtr()->getFrameListPtr()->front();
- FrameBase* first_frame_prun = wolf_problem_prun->getTrajectoryPtr()->getFrameListPtr()->front();
+ FrameBasePtr first_frame_full = wolf_problem_full->getTrajectoryPtr()->getFrameListPtr()->front();
+ FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectoryPtr()->getFrameListPtr()->front();
CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase(SEN_ABSOLUTE_POSE, "ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3s::Identity() * 0.01);
CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase(SEN_ABSOLUTE_POSE, "ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3s::Identity() * 0.01);
first_frame_full->addCapture(initial_covariance_full);
diff --git a/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp
index 0034b361b..4b310ba5c 100644
--- a/src/examples/test_wolf_prunning.cpp
+++ b/src/examples/test_wolf_prunning.cpp
@@ -71,14 +71,14 @@ int main(int argc, char** argv)
double t_sigma_manual = 0;
// loading variables
- std::map<unsigned int, FrameBase*> index_2_frame_ptr_full;
- std::map<unsigned int, FrameBase*> index_2_frame_ptr_prun;
- std::map<FrameBase*, unsigned int> frame_ptr_2_index_prun;
+ std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_full;
+ std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_prun;
+ std::map<FrameBasePtr, unsigned int> frame_ptr_2_index_prun;
// Wolf problem
- Problem* wolf_problem_full = new Problem(FRM_PO_2D);
- Problem* wolf_problem_prun = new Problem(FRM_PO_2D);
- SensorBase* sensor = new SensorBase(SEN_ODOM_2D, "ODOM 2D", new StateBlock(Eigen::VectorXs::Zero(2)), new StateBlock(Eigen::VectorXs::Zero(1)), new StateBlock(Eigen::VectorXs::Zero(2)), 2);
+ ProblemPtr wolf_problem_full = new Problem(FRM_PO_2D);
+ ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2D);
+ SensorBasePtr sensor = new SensorBase(SEN_ODOM_2D, "ODOM 2D", new StateBlock(Eigen::VectorXs::Zero(2)), new StateBlock(Eigen::VectorXs::Zero(1)), new StateBlock(Eigen::VectorXs::Zero(2)), 2);
Eigen::SparseMatrix<Scalar> Lambda(0,0);
@@ -149,8 +149,8 @@ int main(int argc, char** argv)
bNum.clear();
// add frame to problem
- FrameBase* vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
- FrameBase* vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
wolf_problem_full->getTrajectoryPtr()->addFrame(vertex_frame_ptr_full);
wolf_problem_prun->getTrajectoryPtr()->addFrame(vertex_frame_ptr_prun);
// store
@@ -263,18 +263,18 @@ int main(int argc, char** argv)
//std::cout << "Adding edge... " << std::endl;
// add capture, feature and constraint to problem
- FeatureBase* feature_ptr_full = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
- FeatureBase* feature_ptr_prun = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_full = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_prun = new FeatureBase(FEATURE_FIX, "FIX", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_full = new CaptureVoid(TimeStamp(0), sensor);
CaptureVoid* capture_ptr_prun = new CaptureVoid(TimeStamp(0), sensor);
assert(index_2_frame_ptr_full.find(edge_old) != index_2_frame_ptr_full.end() && "edge from vertex not added!");
assert(index_2_frame_ptr_prun.find(edge_old) != index_2_frame_ptr_prun.end() && "edge from vertex not added!");
- FrameBase* frame_old_ptr_full = index_2_frame_ptr_full[edge_old];
- FrameBase* frame_old_ptr_prun = index_2_frame_ptr_prun[edge_old];
+ FrameBasePtr frame_old_ptr_full = index_2_frame_ptr_full[edge_old];
+ FrameBasePtr frame_old_ptr_prun = index_2_frame_ptr_prun[edge_old];
assert(index_2_frame_ptr_full.find(edge_new) != index_2_frame_ptr_full.end() && "edge to vertex not added!");
assert(index_2_frame_ptr_prun.find(edge_new) != index_2_frame_ptr_prun.end() && "edge to vertex not added!");
- FrameBase* frame_new_ptr_full = index_2_frame_ptr_full[edge_new];
- FrameBase* frame_new_ptr_prun = index_2_frame_ptr_prun[edge_new];
+ FrameBasePtr frame_new_ptr_full = index_2_frame_ptr_full[edge_new];
+ FrameBasePtr frame_new_ptr_prun = index_2_frame_ptr_prun[edge_new];
frame_new_ptr_full->addCapture(capture_ptr_full);
frame_new_ptr_prun->addCapture(capture_ptr_prun);
capture_ptr_full->addFeature(feature_ptr_full);
@@ -323,8 +323,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBase* first_frame_full = wolf_problem_full->getTrajectoryPtr()->getFrameListPtr()->front();
- FrameBase* first_frame_prun = wolf_problem_prun->getTrajectoryPtr()->getFrameListPtr()->front();
+ FrameBasePtr first_frame_full = wolf_problem_full->getTrajectoryPtr()->getFrameListPtr()->front();
+ FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectoryPtr()->getFrameListPtr()->front();
CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase(SEN_ABSOLUTE_POSE, "ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3s::Identity() * 0.01);
CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase(SEN_ABSOLUTE_POSE, "ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3s::Identity() * 0.01);
first_frame_full->addCapture(initial_covariance_full);
diff --git a/src/factory.h b/src/factory.h
index f750cd047..7ba9d2b2a 100644
--- a/src/factory.h
+++ b/src/factory.h
@@ -38,9 +38,9 @@ namespace wolf
* The returned data is always a pointer to TypeBase.
*
* For example, you may use as __TypeBase__ the following types:
- * - LandmarkBase: the Factory creates landmarks deriving from LandmarkBase and returns base pointers ````LandmarkBase*```` to them
- * - IntrinsicsBase: the Factory creates intrinsic parameters deriving from IntrinsicsBase and returns base pointers ````IntrinsicsBase*```` to them
- * - XxxBase: the Factory creates objects deriving from XxxBase and returns pointers ````XxxBase*```` to them.
+ * - LandmarkBase: the Factory creates landmarks deriving from LandmarkBase and returns base pointers ````LandmarkBasePtr```` to them
+ * - IntrinsicsBase: the Factory creates intrinsic parameters deriving from IntrinsicsBase and returns base pointers ````IntrinsicsBasePtr```` to them
+ * - XxxBase: the Factory creates objects deriving from XxxBase and returns pointers ````XxxBasePtr```` to them.
*
* - The class in also templatized on the type of the input parameter of the creator, __TypeInput__:
* - ````std::string```` is used when the input parameter is a file name from which to read data (typically a YAML file).
@@ -119,8 +119,8 @@ namespace wolf
* Two examples:
*
* \code
- * static IntrinsicsBase* create(const std::string& _intrinsics_dot_yaml)
- * static LandmarkBase* create(const YAML::Node& _lmk_yaml_node)
+ * static IntrinsicsBasePtr create(const std::string& _intrinsics_dot_yaml)
+ * static LandmarkBasePtr create(const YAML::Node& _lmk_yaml_node)
* \endcode
*
* See further down for an implementation example.
@@ -163,13 +163,13 @@ namespace wolf
* To create e.g. a LandmarkPolyline2D from a YAML node you type:
*
* \code
- * LandmarkBase* lmk_ptr = Factory<LandmarkBase*, YAML::Node>::get().create("POLYLINE 2D", lmk_yaml_node);
+ * LandmarkBasePtr lmk_ptr = Factory<LandmarkBasePtr, YAML::Node>::get().create("POLYLINE 2D", lmk_yaml_node);
* \endcode
*
* or even better, make use of the convenient typedefs:
*
* \code
- * LandmarkBase* lmk_ptr = LandmarkFactory::get().create("POLYLINE 2D", lmk_yaml_node);
+ * LandmarkBasePtr lmk_ptr = LandmarkFactory::get().create("POLYLINE 2D", lmk_yaml_node);
* \endcode
*
* ### Examples
@@ -179,7 +179,7 @@ namespace wolf
*
* \code
* // Creator (this method is static):
- * LandmarkBase* LandmarkPolyline2D::create(const YAML::Node& _lmk_node)
+ * LandmarkBasePtr LandmarkPolyline2D::create(const YAML::Node& _lmk_node)
* {
* // Parse YAML node with lmk info and data
* unsigned int id = _lmk_node["id"].as<unsigned int>();
@@ -194,7 +194,7 @@ namespace wolf
* }
*
* // Create a new landmark
- * LandmarkBase* lmk_ptr = new LandmarkPolyline2D(points, first_defined, last_defined, first_id);
+ * LandmarkBasePtr lmk_ptr = new LandmarkPolyline2D(points, first_defined, last_defined, first_id);
* lmk_ptr->setId(id);
*
* return lmk_ptr;
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index e3825e3e8..931c8e379 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -42,7 +42,7 @@ FeatureBase::~FeatureBase()
//std::cout << "constraints deleted" << std::endl;
}
-ConstraintBase* FeatureBase::addConstraint(ConstraintBase* _co_ptr)
+ConstraintBasePtr FeatureBase::addConstraint(ConstraintBasePtr _co_ptr)
{
constraint_list_.push_back(_co_ptr);
_co_ptr->setFeaturePtr(this);
@@ -55,7 +55,7 @@ ConstraintBase* FeatureBase::addConstraint(ConstraintBase* _co_ptr)
return _co_ptr;
}
-FrameBase* FeatureBase::getFramePtr() const
+FrameBasePtr FeatureBase::getFramePtr() const
{
return capture_ptr_->getFramePtr();
// return upperNodePtr()->upperNodePtr();
diff --git a/src/feature_base.h b/src/feature_base.h
index 827010954..4c1a5ef47 100644
--- a/src/feature_base.h
+++ b/src/feature_base.h
@@ -67,7 +67,7 @@ class FeatureBase : public NodeBase // NodeConstrained<CaptureBase,ConstraintBas
/** \brief Adds a constraint from this feature (as a down node)
*/
- ConstraintBase* addConstraint(ConstraintBase* _co_ptr);
+ ConstraintBasePtr addConstraint(ConstraintBasePtr _co_ptr);
/** \brief Removes a constraint (as a down node)
*/
@@ -75,12 +75,12 @@ class FeatureBase : public NodeBase // NodeConstrained<CaptureBase,ConstraintBas
/** \brief Gets the capture pointer
*/
- CaptureBase* getCapturePtr() const;
- void setCapturePtr(CaptureBase* _cap_ptr){capture_ptr_ = _cap_ptr;}
+ CaptureBasePtr getCapturePtr() const;
+ void setCapturePtr(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
/** \brief Gets the frame pointer
*/
- FrameBase* getFramePtr() const;
+ FrameBasePtr getFramePtr() const;
/** \brief Gets the constraint list (down nodes) pointer
*/
@@ -108,11 +108,11 @@ class FeatureBase : public NodeBase // NodeConstrained<CaptureBase,ConstraintBas
void setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov);
- virtual void addConstrainedBy(ConstraintBase* _ctr_ptr)
+ virtual void addConstrainedBy(ConstraintBasePtr _ctr_ptr)
{
constrained_by_list_.push_back(_ctr_ptr);
}
- virtual void removeConstrainedBy(ConstraintBase* _ctr_ptr)
+ virtual void removeConstrainedBy(ConstraintBasePtr _ctr_ptr)
{
constrained_by_list_.remove(_ctr_ptr);
}
@@ -125,8 +125,8 @@ class FeatureBase : public NodeBase // NodeConstrained<CaptureBase,ConstraintBas
return &constrained_by_list_;
}
- Problem* getProblem(){return problem_ptr_;}
- void setProblem(Problem* _prob_ptr){problem_ptr_ = _prob_ptr;}
+ ProblemPtr getProblem(){return problem_ptr_;}
+ void setProblem(ProblemPtr _prob_ptr){problem_ptr_ = _prob_ptr;}
};
@@ -150,7 +150,7 @@ inline void FeatureBase::removeConstraint(ConstraintBasePtr _co_ptr)
delete _co_ptr;
}
-inline CaptureBase* FeatureBase::getCapturePtr() const
+inline CaptureBasePtr FeatureBase::getCapturePtr() const
{
return capture_ptr_;
// return upperNodePtr();
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 59922b28a..7b3b08f9f 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -163,7 +163,7 @@ void FrameBase::getState(Eigen::VectorXs& state) const
}
}
-CaptureBasePtr FrameBase::hasCaptureOf(const SensorBase* _sensor_ptr)
+CaptureBasePtr FrameBase::hasCaptureOf(const SensorBasePtr _sensor_ptr)
{
for (auto capture_ptr : *getCaptureListPtr())
if (capture_ptr->getSensorPtr() == _sensor_ptr)
@@ -177,7 +177,7 @@ void FrameBase::getConstraintList(ConstraintBaseList & _ctr_list)
(*c_it)->getConstraintList(_ctr_list);
}
-FrameBase* FrameBase::getPreviousFrame() const
+FrameBasePtr FrameBase::getPreviousFrame() const
{
//std::cout << "finding previous frame of " << this->node_id_ << std::endl;
if (getTrajectoryPtr() == nullptr)
@@ -207,7 +207,7 @@ FrameBase* FrameBase::getPreviousFrame() const
return nullptr;
}
-FrameBase* FrameBase::getNextFrame() const
+FrameBasePtr FrameBase::getNextFrame() const
{
//std::cout << "finding next frame of " << this->node_id_ << std::endl;
auto f_it = getTrajectoryPtr()->getFrameListPtr()->rbegin();
diff --git a/src/frame_base.h b/src/frame_base.h
index 243db8c56..f27aacea1 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -110,22 +110,22 @@ class FrameBase : public NodeBase // NodeConstrained<TrajectoryBase,CaptureBase>
FrameBasePtr getNextFrame() const;
CaptureBaseList* getCaptureListPtr();
- CaptureBasePtr addCapture(CaptureBase* _capt_ptr);
+ CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
void removeCapture(const CaptureBaseIter& _capt_iter);
void removeCapture(const CaptureBasePtr _capt_ptr);
- CaptureBasePtr hasCaptureOf(const SensorBase* _sensor_ptr);
+ CaptureBasePtr hasCaptureOf(const SensorBasePtr _sensor_ptr);
void unlinkCapture(CaptureBasePtr _cap_ptr);
void getConstraintList(ConstraintBaseList & _ctr_list);
ProblemPtr getProblem(){return problem_ptr_;}
- void setProblem(Problem* _prob_ptr){problem_ptr_ = _prob_ptr;}
+ void setProblem(ProblemPtr _prob_ptr){problem_ptr_ = _prob_ptr;}
- virtual void addConstrainedBy(ConstraintBase* _ctr_ptr)
+ virtual void addConstrainedBy(ConstraintBasePtr _ctr_ptr)
{
constrained_by_list_.push_back(_ctr_ptr);
}
- virtual void removeConstrainedBy(ConstraintBase* _ctr_ptr)
+ virtual void removeConstrainedBy(ConstraintBasePtr _ctr_ptr)
{
constrained_by_list_.remove(_ctr_ptr);
}
@@ -223,7 +223,7 @@ inline StateBlock* FrameBase::getVPtr() const
return v_ptr_;
}
-inline TrajectoryBase* FrameBase::getTrajectoryPtr() const
+inline TrajectoryBasePtr FrameBase::getTrajectoryPtr() const
{
return trajectory_ptr_;
// return upperNodePtr();
diff --git a/src/hardware_base.cpp b/src/hardware_base.cpp
index ee81e40d2..2b92b3cb3 100644
--- a/src/hardware_base.cpp
+++ b/src/hardware_base.cpp
@@ -16,7 +16,7 @@ HardwareBase::~HardwareBase()
//std::cout << "deleting HardwareBase " << nodeId() << std::endl;
}
-SensorBase* HardwareBase::addSensor(SensorBase* _sensor_ptr)
+SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
{
//std::cout << "adding sensor..." << std::endl;
sensor_list_.push_back(_sensor_ptr);
@@ -30,7 +30,7 @@ SensorBase* HardwareBase::addSensor(SensorBase* _sensor_ptr)
}
-void HardwareBase::removeSensor(SensorBase* _sensor_ptr)
+void HardwareBase::removeSensor(SensorBasePtr _sensor_ptr)
{
sensor_list_.remove(_sensor_ptr);
delete _sensor_ptr;
diff --git a/src/hardware_base.h b/src/hardware_base.h
index cbda6d695..e3bcaf1b5 100644
--- a/src/hardware_base.h
+++ b/src/hardware_base.h
@@ -17,7 +17,7 @@ namespace wolf {
class HardwareBase : public NodeBase //: public NodeLinked<Problem, SensorBase>
{
private:
- Problem* problem_ptr_;
+ ProblemPtr problem_ptr_;
SensorBaseList sensor_list_;
public:
@@ -36,16 +36,16 @@ class HardwareBase : public NodeBase //: public NodeLinked<Problem, SensorBase>
delete this;
}
- virtual SensorBase* addSensor(SensorBase* _sensor_ptr);
+ virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
void removeSensor(const SensorBaseIter& _sensor_iter);
- void removeSensor(SensorBase* _sensor_ptr);
+ void removeSensor(SensorBasePtr _sensor_ptr);
SensorBaseList* getSensorListPtr();
- Problem* getProblem(){return problem_ptr_;}
- void setProblem(Problem* _prob_ptr){problem_ptr_ = _prob_ptr;}
+ ProblemPtr getProblem(){return problem_ptr_;}
+ void setProblem(ProblemPtr _prob_ptr){problem_ptr_ = _prob_ptr;}
};
} // namespace wolf
diff --git a/src/landmark_AHP.cpp b/src/landmark_AHP.cpp
index 6f28f7c8c..2e40d6888 100644
--- a/src/landmark_AHP.cpp
+++ b/src/landmark_AHP.cpp
@@ -8,8 +8,8 @@ namespace wolf {
/* Landmark - Anchored Homogeneous Point*/
LandmarkAHP::LandmarkAHP(Eigen::Vector4s _position_homogeneous,
- FrameBase* _anchor_frame,
- SensorBase* _anchor_sensor,
+ FrameBasePtr _anchor_frame,
+ SensorBasePtr _anchor_sensor,
cv::Mat _2D_descriptor) :
LandmarkBase(LANDMARK_AHP, "AHP", new StateHomogeneous3D(_position_homogeneous)),
cv_descriptor_(_2D_descriptor.clone()),
@@ -36,14 +36,14 @@ YAML::Node LandmarkAHP::saveToYaml() const
}
-wolf::LandmarkBase* LandmarkAHP::create(const YAML::Node& _node)
+wolf::LandmarkBasePtr LandmarkAHP::create(const YAML::Node& _node)
{
unsigned int id = _node["id"] .as< unsigned int >();
Eigen::VectorXs pos_homog = _node["position"] .as< Eigen::VectorXs >();
std::vector<int> v = _node["descriptor"] .as< std::vector<int> >();
cv::Mat desc(v);
- LandmarkBase* lmk = new LandmarkAHP(pos_homog, nullptr, nullptr, desc);
+ LandmarkBasePtr lmk = new LandmarkAHP(pos_homog, nullptr, nullptr, desc);
lmk->setId(id);
return lmk;
}
diff --git a/src/landmark_AHP.h b/src/landmark_AHP.h
index 3fc8bad4d..23d707b60 100644
--- a/src/landmark_AHP.h
+++ b/src/landmark_AHP.h
@@ -17,23 +17,23 @@ class LandmarkAHP : public LandmarkBase
{
protected:
cv::Mat cv_descriptor_;
- FrameBase* anchor_frame_;
- SensorBase* anchor_sensor_;
+ FrameBasePtr anchor_frame_;
+ SensorBasePtr anchor_sensor_;
public:
- LandmarkAHP(Eigen::Vector4s _position_homogeneous, FrameBase* _anchor_frame, SensorBase* _anchor_sensor, cv::Mat _2D_descriptor);
+ LandmarkAHP(Eigen::Vector4s _position_homogeneous, FrameBasePtr _anchor_frame, SensorBasePtr _anchor_sensor, cv::Mat _2D_descriptor);
virtual ~LandmarkAHP();
const cv::Mat& getCvDescriptor() const;
void setCvDescriptor(const cv::Mat& _descriptor);
- const FrameBase* getAnchorFrame () const;
- const SensorBase* getAnchorSensor() const;
+ const FrameBasePtr getAnchorFrame () const;
+ const SensorBasePtr getAnchorSensor() const;
- void setAnchorFrame (FrameBase* _anchor_frame );
- void setAnchorSensor (SensorBase* _anchor_sensor);
- void setAnchor (FrameBase* _anchor_frame , SensorBase* _anchor_sensor);
+ void setAnchorFrame (FrameBasePtr _anchor_frame );
+ void setAnchorSensor (SensorBasePtr _anchor_sensor);
+ void setAnchor (FrameBasePtr _anchor_frame , SensorBasePtr _anchor_sensor);
YAML::Node saveToYaml() const;
@@ -41,7 +41,7 @@ class LandmarkAHP : public LandmarkBase
* Caution: This creator does not set the landmark's anchor frame and sensor.
* These need to be set afterwards.
*/
- static LandmarkBase* create(const YAML::Node& _node);
+ static LandmarkBasePtr create(const YAML::Node& _node);
};
inline const cv::Mat& LandmarkAHP::getCvDescriptor() const
@@ -54,27 +54,27 @@ inline void LandmarkAHP::setCvDescriptor(const cv::Mat& _descriptor)
cv_descriptor_ = _descriptor;
}
-inline const FrameBase* LandmarkAHP::getAnchorFrame() const
+inline const FrameBasePtr LandmarkAHP::getAnchorFrame() const
{
return anchor_frame_;
}
-inline void LandmarkAHP::setAnchorFrame(FrameBase* _anchor_frame)
+inline void LandmarkAHP::setAnchorFrame(FrameBasePtr _anchor_frame)
{
anchor_frame_ = _anchor_frame;
}
-inline const SensorBase* LandmarkAHP::getAnchorSensor() const
+inline const SensorBasePtr LandmarkAHP::getAnchorSensor() const
{
return anchor_sensor_;
}
-inline void LandmarkAHP::setAnchorSensor(SensorBase* _anchor_sensor)
+inline void LandmarkAHP::setAnchorSensor(SensorBasePtr _anchor_sensor)
{
anchor_sensor_ = _anchor_sensor;
}
-inline void LandmarkAHP::setAnchor(FrameBase* _anchor_frame, SensorBase* _anchor_sensor)
+inline void LandmarkAHP::setAnchor(FrameBasePtr _anchor_frame, SensorBasePtr _anchor_sensor)
{
anchor_frame_ = _anchor_frame;
anchor_sensor_ = _anchor_sensor;
diff --git a/src/landmark_base.h b/src/landmark_base.h
index cfad3c362..5be7a0156 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -85,7 +85,7 @@ class LandmarkBase : public NodeBase // NodeConstrained<MapBase, NodeTerminus>
/** \brief Remove the given constraint from the list.
* If list becomes empty, deletes this object by calling destruct()
**/
- void removeConstrainedBy(ConstraintBase* _ctr_ptr);
+ void removeConstrainedBy(ConstraintBasePtr _ctr_ptr);
/** \brief Adds all stateBlocks of the frame to the wolfProblem list of new stateBlocks
**/
@@ -129,7 +129,7 @@ class LandmarkBase : public NodeBase // NodeConstrained<MapBase, NodeTerminus>
virtual YAML::Node saveToYaml() const;
- void addConstrainedBy(ConstraintBase* _ctr_ptr)
+ void addConstrainedBy(ConstraintBasePtr _ctr_ptr)
{
constrained_by_list_.push_back(_ctr_ptr);
}
@@ -144,8 +144,8 @@ class LandmarkBase : public NodeBase // NodeConstrained<MapBase, NodeTerminus>
void setMapPtr(MapBasePtr _map_ptr){map_ptr_ = _map_ptr;}
- Problem* getProblem(){return problem_ptr_;}
- void setProblem(Problem* _prob_ptr){problem_ptr_ = _prob_ptr;}
+ ProblemPtr getProblem(){return problem_ptr_;}
+ void setProblem(ProblemPtr _prob_ptr){problem_ptr_ = _prob_ptr;}
};
@@ -180,7 +180,7 @@ inline void LandmarkBase::unfix()
this->setStatus(LANDMARK_ESTIMATED);
}
-inline void LandmarkBase::removeConstrainedBy(ConstraintBase* _ctr_ptr)
+inline void LandmarkBase::removeConstrainedBy(ConstraintBasePtr _ctr_ptr)
{
constrained_by_list_.remove(_ctr_ptr);
// NodeConstrained::removeConstrainedBy(_ctr_ptr);
diff --git a/src/landmark_polyline_2D.cpp b/src/landmark_polyline_2D.cpp
index 1d26f4c91..654455964 100644
--- a/src/landmark_polyline_2D.cpp
+++ b/src/landmark_polyline_2D.cpp
@@ -244,7 +244,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
// Change constraints from remove_state to remain_state
ConstraintBaseList old_constraints_list = *getConstrainedByListPtr();
std::cout << "changing constraints: " << old_constraints_list.size() << std::endl;
- ConstraintBase* new_ctr_ptr = nullptr;
+ ConstraintBasePtr new_ctr_ptr = nullptr;
for (auto ctr_ptr : old_constraints_list)
{
if (ctr_ptr->getTypeId() == CTR_POINT_2D)
@@ -324,7 +324,7 @@ void LandmarkPolyline2D::registerNewStateBlocks()
}
// static
-LandmarkBase* LandmarkPolyline2D::create(const YAML::Node& _lmk_node)
+LandmarkBasePtr LandmarkPolyline2D::create(const YAML::Node& _lmk_node)
{
// Parse YAML node with lmk info and data
unsigned int id = _lmk_node["id"].as<unsigned int>();
diff --git a/src/landmark_polyline_2D.h b/src/landmark_polyline_2D.h
index ee432874b..7decd3079 100644
--- a/src/landmark_polyline_2D.h
+++ b/src/landmark_polyline_2D.h
@@ -117,7 +117,7 @@ class LandmarkPolyline2D : public LandmarkBase
/** Factory method to create new landmarks from YAML nodes
*/
- static LandmarkBase* create(const YAML::Node& _lmk_node);
+ static LandmarkBasePtr create(const YAML::Node& _lmk_node);
YAML::Node saveToYaml() const;
};
diff --git a/src/map_base.cpp b/src/map_base.cpp
index 7029fa00d..24ad6d363 100644
--- a/src/map_base.cpp
+++ b/src/map_base.cpp
@@ -29,7 +29,7 @@ MapBase::~MapBase()
//std::cout << "deleting MapBase " << nodeId() << std::endl;
}
-LandmarkBase* MapBase::addLandmark(LandmarkBase* _landmark_ptr)
+LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
{
//std::cout << "MapBase::addLandmark" << std::endl;
landmark_list_.push_back(_landmark_ptr);
@@ -52,7 +52,7 @@ void MapBase::addLandmarkList(LandmarkBaseList _landmark_list)
landmark_list_.splice(landmark_list_.end(), _landmark_list);
}
-void MapBase::removeLandmark(LandmarkBase* _landmark_ptr)
+void MapBase::removeLandmark(LandmarkBasePtr _landmark_ptr)
{
landmark_list_.remove(_landmark_ptr);
delete _landmark_ptr;
@@ -77,7 +77,7 @@ void MapBase::load(const std::string& _map_file_dot_yaml)
for (unsigned int i = 0; i < nlandmarks; i++)
{
YAML::Node lmk_node = map["landmarks"][i];
- LandmarkBase* lmk_ptr = LandmarkFactory::get().create(lmk_node["type"].as<std::string>(), lmk_node);
+ LandmarkBasePtr lmk_ptr = LandmarkFactory::get().create(lmk_node["type"].as<std::string>(), lmk_node);
addLandmark(lmk_ptr);
}
@@ -95,7 +95,7 @@ void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_na
emitter << "landmarks" << YAML::BeginSeq;
- for (LandmarkBase* lmk_ptr : *getLandmarkListPtr())
+ for (LandmarkBasePtr lmk_ptr : *getLandmarkListPtr())
{
emitter << YAML::Flow << lmk_ptr->saveToYaml();
}
diff --git a/src/map_base.h b/src/map_base.h
index 8772a3045..e303e4c60 100644
--- a/src/map_base.h
+++ b/src/map_base.h
@@ -42,7 +42,7 @@ class MapBase : public NodeBase //: public NodeLinked<Problem,LandmarkBase>
*
* Adds a landmark to the Map. It also updates the lists of StateBlocks that are used by the solver.
**/
- virtual LandmarkBase* addLandmark(LandmarkBase* _landmark_ptr);
+ virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr);
/** \brief Adds a landmark
*
@@ -51,15 +51,15 @@ class MapBase : public NodeBase //: public NodeLinked<Problem,LandmarkBase>
virtual void addLandmarkList(LandmarkBaseList _landmark_list);
void removeLandmark(const LandmarkBaseIter& _landmark_iter);
- void removeLandmark(LandmarkBase* _landmark_ptr);
+ void removeLandmark(LandmarkBasePtr _landmark_ptr);
LandmarkBaseList* getLandmarkListPtr();
void load(const std::string& _map_file_yaml);
void save(const std::string& _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf");
- Problem* getProblem(){return problem_ptr_;}
- void setProblem(Problem* _prob_ptr){problem_ptr_ = _prob_ptr;}
+ ProblemPtr getProblem(){return problem_ptr_;}
+ void setProblem(ProblemPtr _prob_ptr){problem_ptr_ = _prob_ptr;}
private:
std::string dateTimeNow();
diff --git a/src/node_constrained.h b/src/node_constrained.h
index 3d170f793..8b61afc82 100644
--- a/src/node_constrained.h
+++ b/src/node_constrained.h
@@ -38,11 +38,11 @@ class NodeConstrained : public NodeLinked<UpperType, LowerType>
{
}
- virtual void addConstrainedBy(ConstraintBase* _ctr_ptr)
+ virtual void addConstrainedBy(ConstraintBasePtr _ctr_ptr)
{
constrained_by_list_.push_back(_ctr_ptr);
}
- virtual void removeConstrainedBy(ConstraintBase* _ctr_ptr)
+ virtual void removeConstrainedBy(ConstraintBasePtr _ctr_ptr)
{
constrained_by_list_.remove(_ctr_ptr);
}
diff --git a/src/node_linked.h b/src/node_linked.h
index 5493de214..15dca8ee3 100644
--- a/src/node_linked.h
+++ b/src/node_linked.h
@@ -81,7 +81,7 @@ class NodeLinked : public NodeBase
bool is_deleting_; ///< This node is being deleted.
private:
- Problem* problem_ptr_;
+ ProblemPtr problem_ptr_;
public:
@@ -197,11 +197,11 @@ class NodeLinked : public NodeBase
/** \brief Gets a pointer to the tree top node - direct method
**/
- Problem* getProblem();
+ ProblemPtr getProblem();
/** \brief Gets a pointer to the tree top node - recursive method
**/
- Problem* getTop();
+ ProblemPtr getTop();
};
@@ -425,7 +425,7 @@ inline void NodeLinked<UpperType, LowerType>::unlinkDownNode(const LowerNodeIter
}
template<class UpperType, class LowerType>
-Problem* NodeLinked<UpperType, LowerType>::getProblem()
+ProblemPtr NodeLinked<UpperType, LowerType>::getProblem()
{
if (problem_ptr_ == nullptr)
problem_ptr_ = getTop();
@@ -433,7 +433,7 @@ Problem* NodeLinked<UpperType, LowerType>::getProblem()
}
template<class UpperType, class LowerType>
-inline Problem* NodeLinked<UpperType, LowerType>::getTop()
+inline ProblemPtr NodeLinked<UpperType, LowerType>::getTop()
{
if (up_node_ptr_ != nullptr)
return up_node_ptr_->getTop();
diff --git a/src/problem.cpp b/src/problem.cpp
index 3b5a7df1b..eab58ede8 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -50,36 +50,36 @@ void Problem::destruct()
delete this;
}
-void Problem::addSensor(SensorBase* _sen_ptr)
+void Problem::addSensor(SensorBasePtr _sen_ptr)
{
getHardwarePtr()->addSensor(_sen_ptr);
}
-SensorBase* Problem::installSensor(const std::string& _sen_type, //
+SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const Eigen::VectorXs& _extrinsics, //
- IntrinsicsBase* _intrinsics)
+ IntrinsicsBasePtr _intrinsics)
{
- SensorBase* sen_ptr = SensorFactory::get().create(uppercase(_sen_type), _unique_sensor_name, _extrinsics, _intrinsics);
+ SensorBasePtr sen_ptr = SensorFactory::get().create(uppercase(_sen_type), _unique_sensor_name, _extrinsics, _intrinsics);
addSensor(sen_ptr);
return sen_ptr;
}
-SensorBase* Problem::installSensor(const std::string& _sen_type, //
+SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const Eigen::VectorXs& _extrinsics, //
const std::string& _intrinsics_filename)
{
- IntrinsicsBase* intr_ptr = IntrinsicsFactory::get().create(_sen_type, _intrinsics_filename);
+ IntrinsicsBasePtr intr_ptr = IntrinsicsFactory::get().create(_sen_type, _intrinsics_filename);
return installSensor(_sen_type, _unique_sensor_name, _extrinsics, intr_ptr);
}
-ProcessorBase* Problem::installProcessor(const std::string& _prc_type, //
+ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
- SensorBase* _corresponding_sensor_ptr, //
- ProcessorParamsBase* _prc_params)
+ SensorBasePtr _corresponding_sensor_ptr, //
+ ProcessorParamsBasePtr _prc_params)
{
- ProcessorBase* prc_ptr = ProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _prc_params);
+ ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _prc_params);
_corresponding_sensor_ptr->addProcessor(prc_ptr);
// setting the origin in all processor motion if origin already setted
@@ -98,14 +98,14 @@ void Problem::installProcessor(const std::string& _prc_type, //
const std::string& _corresponding_sensor_name, //
const std::string& _params_filename)
{
- SensorBase* sen_ptr = getSensorPtr(_corresponding_sensor_name);
+ SensorBasePtr sen_ptr = getSensorPtr(_corresponding_sensor_name);
if (sen_ptr == nullptr)
throw std::runtime_error("Sensor not found. Cannot bind Processor.");
if (_params_filename == "")
installProcessor(_prc_type, _unique_processor_name, sen_ptr, nullptr);
else
{
- ProcessorParamsBase* prc_params = ProcessorParamsFactory::get().create(_prc_type, _params_filename);
+ ProcessorParamsBasePtr prc_params = ProcessorParamsFactory::get().create(_prc_type, _params_filename);
installProcessor(_prc_type, _unique_processor_name, sen_ptr, prc_params);
}
}
@@ -115,12 +115,12 @@ void Problem::setProcessorMotion(ProcessorMotion* _processor_motion_ptr)
processor_motion_ptr_ = _processor_motion_ptr;
}
-FrameBase* Problem::createFrame(FrameKeyType _frame_type, const TimeStamp& _time_stamp)
+FrameBasePtr Problem::createFrame(FrameKeyType _frame_type, const TimeStamp& _time_stamp)
{
return createFrame(_frame_type, getStateAtTimeStamp(_time_stamp), _time_stamp);
}
-FrameBase* Problem::createFrame(FrameKeyType _frame_key_type, const Eigen::VectorXs& _frame_state,
+FrameBasePtr Problem::createFrame(FrameKeyType _frame_key_type, const Eigen::VectorXs& _frame_state,
const TimeStamp& _time_stamp)
{
//std::cout << "Problem::createFrame" << std::endl;
@@ -206,7 +206,7 @@ void Problem::getStateAtTimeStamp(const TimeStamp& _ts, Eigen::VectorXs& state)
processor_motion_ptr_->getState(_ts, state);
else
{
- FrameBase* closest_frame = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
+ FrameBasePtr closest_frame = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
if (closest_frame != nullptr)
closest_frame->getState(state);
else
@@ -245,12 +245,12 @@ Eigen::VectorXs Problem::zeroState()
return state;
}
-bool Problem::permitKeyFrame(ProcessorBase* _processor_ptr)
+bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr)
{
return true;
}
-void Problem::keyFrameCallback(FrameBase* _keyframe_ptr, ProcessorBase* _processor_ptr, const Scalar& _time_tolerance)
+void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const Scalar& _time_tolerance)
{
//std::cout << "Problem::keyFrameCallback: processor " << _processor_ptr->getName() << std::endl;
for (auto sensor : (*hardware_ptr_->getSensorListPtr()))
@@ -259,7 +259,7 @@ void Problem::keyFrameCallback(FrameBase* _keyframe_ptr, ProcessorBase* _process
processor->keyFrameCallback(_keyframe_ptr, _time_tolerance);
}
-LandmarkBase* Problem::addLandmark(LandmarkBase* _lmk_ptr)
+LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr)
{
getMapPtr()->addLandmark(_lmk_ptr);
return _lmk_ptr;
@@ -312,7 +312,7 @@ void Problem::removeStateBlockPtr(StateBlock* _state_ptr)
}
-ConstraintBase* Problem::addConstraintPtr(ConstraintBase* _constraint_ptr)
+ConstraintBasePtr Problem::addConstraintPtr(ConstraintBasePtr _constraint_ptr)
{
//std::cout << "addConstraintPtr" << std::endl;
// queue for solver manager
@@ -321,7 +321,7 @@ ConstraintBase* Problem::addConstraintPtr(ConstraintBase* _constraint_ptr)
return _constraint_ptr;
}
-void Problem::removeConstraintPtr(ConstraintBase* _constraint_ptr)
+void Problem::removeConstraintPtr(ConstraintBasePtr _constraint_ptr)
{
// Check if the constraint addition is still as a notification
auto ctr_found_it = constraint_notification_list_.end();
@@ -382,7 +382,7 @@ bool Problem::getCovarianceBlock(StateBlock* _state1, StateBlock* _state2, Eigen
return true;
}
-bool Problem::getFrameCovariance(FrameBase* _frame_ptr, Eigen::MatrixXs& _covariance)
+bool Problem::getFrameCovariance(FrameBasePtr _frame_ptr, Eigen::MatrixXs& _covariance)
{
return getCovarianceBlock(_frame_ptr->getPPtr(), _frame_ptr->getPPtr(), _covariance, 0, 0) &&
getCovarianceBlock(_frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _covariance, 0,_frame_ptr->getPPtr()->getSize()) &&
@@ -390,14 +390,14 @@ bool Problem::getFrameCovariance(FrameBase* _frame_ptr, Eigen::MatrixXs& _covari
getCovarianceBlock(_frame_ptr->getOPtr(), _frame_ptr->getOPtr(), _covariance, _frame_ptr->getPPtr()->getSize() ,_frame_ptr->getPPtr()->getSize());
}
-Eigen::MatrixXs Problem::getFrameCovariance(FrameBase* _frame_ptr)
+Eigen::MatrixXs Problem::getFrameCovariance(FrameBasePtr _frame_ptr)
{
Eigen::MatrixXs covariance = Eigen::MatrixXs::Zero(_frame_ptr->getPPtr()->getSize()+_frame_ptr->getOPtr()->getSize(), _frame_ptr->getPPtr()->getSize()+_frame_ptr->getOPtr()->getSize());
getFrameCovariance(_frame_ptr, covariance);
return covariance;
}
-bool Problem::getLandmarkCovariance(LandmarkBase* _landmark_ptr, Eigen::MatrixXs& _covariance)
+bool Problem::getLandmarkCovariance(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& _covariance)
{
return getCovarianceBlock(_landmark_ptr->getPPtr(), _landmark_ptr->getPPtr(), _covariance, 0, 0) &&
getCovarianceBlock(_landmark_ptr->getPPtr(), _landmark_ptr->getOPtr(), _covariance, 0,_landmark_ptr->getPPtr()->getSize()) &&
@@ -405,14 +405,14 @@ bool Problem::getLandmarkCovariance(LandmarkBase* _landmark_ptr, Eigen::MatrixXs
getCovarianceBlock(_landmark_ptr->getOPtr(), _landmark_ptr->getOPtr(), _covariance, _landmark_ptr->getPPtr()->getSize() ,_landmark_ptr->getPPtr()->getSize());
}
-Eigen::MatrixXs Problem::getLandmarkCovariance(LandmarkBase* _landmark_ptr)
+Eigen::MatrixXs Problem::getLandmarkCovariance(LandmarkBasePtr _landmark_ptr)
{
Eigen::MatrixXs covariance = Eigen::MatrixXs::Zero(_landmark_ptr->getPPtr()->getSize()+_landmark_ptr->getOPtr()->getSize(), _landmark_ptr->getPPtr()->getSize()+_landmark_ptr->getOPtr()->getSize());
getLandmarkCovariance(_landmark_ptr, covariance);
return covariance;
}
-MapBase* Problem::addMap(MapBase* _map_ptr)
+MapBasePtr Problem::addMap(MapBasePtr _map_ptr)
{
// TODO: not necessary but update map maybe..
map_ptr_ = _map_ptr;
@@ -422,7 +422,7 @@ MapBase* Problem::addMap(MapBase* _map_ptr)
return map_ptr_;
}
-TrajectoryBase* Problem::addTrajectory(TrajectoryBase* _trajectory_ptr)
+TrajectoryBasePtr Problem::addTrajectory(TrajectoryBasePtr _trajectory_ptr)
{
trajectory_ptr_ = _trajectory_ptr;
trajectory_ptr_->setProblem(this);
@@ -431,27 +431,27 @@ TrajectoryBase* Problem::addTrajectory(TrajectoryBase* _trajectory_ptr)
return trajectory_ptr_;
}
-MapBase* Problem::getMapPtr()
+MapBasePtr Problem::getMapPtr()
{
return map_ptr_;
}
-TrajectoryBase* Problem::getTrajectoryPtr()
+TrajectoryBasePtr Problem::getTrajectoryPtr()
{
return trajectory_ptr_;
}
-HardwareBase* Problem::getHardwarePtr()
+HardwareBasePtr Problem::getHardwarePtr()
{
return hardware_ptr_;
}
-FrameBase* Problem::getLastFramePtr()
+FrameBasePtr Problem::getLastFramePtr()
{
return trajectory_ptr_->getLastFramePtr();
}
-FrameBase* Problem::getLastKeyFramePtr()
+FrameBasePtr Problem::getLastKeyFramePtr()
{
return trajectory_ptr_->getLastKeyFramePtr();;
}
@@ -461,10 +461,10 @@ StateBlockList* Problem::getStateListPtr()
return &state_block_ptr_list_;
}
-wolf::SensorBase* Problem::getSensorPtr(const std::string& _sensor_name)
+wolf::SensorBasePtr Problem::getSensorPtr(const std::string& _sensor_name)
{
auto sen_it = std::find_if(getHardwarePtr()->getSensorListPtr()->begin(),
- getHardwarePtr()->getSensorListPtr()->end(), [&](SensorBase* sb)
+ getHardwarePtr()->getSensorListPtr()->end(), [&](SensorBasePtr sb)
{
return sb->getName() == _sensor_name;
}); // lambda function for the find_if
@@ -479,10 +479,10 @@ void Problem::setOrigin(const Eigen::VectorXs& _origin_pose, const Eigen::Matrix
if (!origin_setted_)
{
// Create origin frame
- FrameBase* origin_frame_ptr = createFrame(KEY_FRAME, _origin_pose, _ts);
+ FrameBasePtr origin_frame_ptr = createFrame(KEY_FRAME, _origin_pose, _ts);
// FIXME: create a fix sensor
IntrinsicsBase fix_instrinsics;
- SensorBase* fix_sensor_ptr = installSensor("GPS", "initial pose", Eigen::VectorXs::Zero(3), &fix_instrinsics );
+ SensorBasePtr fix_sensor_ptr = installSensor("GPS", "initial pose", Eigen::VectorXs::Zero(3), &fix_instrinsics );
CaptureFix* init_capture = new CaptureFix(_ts, fix_sensor_ptr, _origin_pose, _origin_cov);
origin_frame_ptr->addCapture(init_capture);
init_capture->process();
diff --git a/src/problem.h b/src/problem.h
index 179bd9ae3..4ff47b5be 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -39,7 +39,7 @@ struct StateBlockNotification
struct ConstraintNotification
{
Notification notification_;
- ConstraintBase* constraint_ptr_;
+ ConstraintBasePtr constraint_ptr_;
unsigned int id_;
};
@@ -57,14 +57,14 @@ struct ConstraintNotification
class Problem //: public NodeBase
{
public:
-// typedef NodeBase* LowerNodePtr; // Necessatry for destruct() of node_linked
+// typedef NodeBasePtr LowerNodePtr; // Necessatry for destruct() of node_linked
protected:
std::map<std::pair<StateBlock*, StateBlock*>, Eigen::MatrixXs> covariances_;
NodeLocation location_; // TODO: should it be in node_base?
- HardwareBase* hardware_ptr_;
- TrajectoryBase* trajectory_ptr_;
- MapBase* map_ptr_;
+ HardwareBasePtr hardware_ptr_;
+ TrajectoryBasePtr trajectory_ptr_;
+ MapBasePtr map_ptr_;
ProcessorMotion* processor_motion_ptr_;
StateBlockList state_block_ptr_list_;
std::list<StateBlockNotification> state_block_notification_list_;
@@ -104,7 +104,7 @@ class Problem //: public NodeBase
/** \brief add sensor to hardware
* \param _sen_ptr pointer to the sensor to add
*/
- void addSensor(SensorBase* _sen_ptr);
+ void addSensor(SensorBasePtr _sen_ptr);
/** \brief Factory method to install (create and add) sensors only from its properties
* \param _sen_type type of sensor
@@ -112,10 +112,10 @@ class Problem //: public NodeBase
* \param _extrinsics a vector of extrinsic parameters: size 2 for 2D position, 3 for 2D pose, 3 for 3D position, 7 for 3D pose.
* \param _intrinsics a base-pointer to a derived struct defining the intrinsic parameters.
*/
- SensorBase* installSensor(const std::string& _sen_type, //
+ SensorBasePtr installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const Eigen::VectorXs& _extrinsics, //
- IntrinsicsBase* _intrinsics = nullptr);
+ IntrinsicsBasePtr _intrinsics = nullptr);
/** \brief Factory method to install (create and add) sensors only from its properties -- Helper method loading parameters from file
* \param _sen_type type of sensor
@@ -123,7 +123,7 @@ class Problem //: public NodeBase
* \param _extrinsics a vector of extrinsic parameters: size 2 for 2D position, 3 for 2D pose, 3 for 3D position, 7 for 3D pose.
* \param _intrinsics_filename the name of a file containing the intrinsic parameters in a format compatible with the intrinsics creator registered in IntrinsicsFactory under the key _sen_type.
*/
- SensorBase* installSensor(const std::string& _sen_type, //
+ SensorBasePtr installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const Eigen::VectorXs& _extrinsics, //
const std::string& _intrinsics_filename);
@@ -136,10 +136,10 @@ class Problem //: public NodeBase
* \param _corresponding_sensor_ptr pointer to the sensor where the processor will be installed.
* \param _prc_params a base-pointer to a derived struct defining the processor parameters.
*/
- ProcessorBase* installProcessor(const std::string& _prc_type, //
+ ProcessorBasePtr installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
- SensorBase* _corresponding_sensor_ptr, //
- ProcessorParamsBase* _prc_params = nullptr);
+ SensorBasePtr _corresponding_sensor_ptr, //
+ ProcessorParamsBasePtr _prc_params = nullptr);
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
@@ -171,13 +171,13 @@ class Problem //: public NodeBase
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem
*/
- FrameBase* createFrame(FrameKeyType _frame_key_type, const TimeStamp& _time_stamp);
+ FrameBasePtr createFrame(FrameKeyType _frame_key_type, const TimeStamp& _time_stamp);
/** \brief Create Frame from vector
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem
*/
- FrameBase* createFrame(FrameKeyType _frame_key_type, const Eigen::VectorXs& _frame_state,
+ FrameBasePtr createFrame(FrameKeyType _frame_key_type, const Eigen::VectorXs& _frame_state,
const TimeStamp& _time_stamp);
ProcessorMotion* getProcessorMotionPtr();
@@ -199,15 +199,15 @@ class Problem //: public NodeBase
/** \brief Give the permission to a processor to create a new keyFrame
*/
- bool permitKeyFrame(ProcessorBase* _processor_ptr);
+ bool permitKeyFrame(ProcessorBasePtr _processor_ptr);
/** \brief New key frame callback
*
* New key frame callback: It should be called by any processor that creates a new keyframe. It calls the keyFrameCallback of the rest of processors.
*/
- void keyFrameCallback(FrameBase* _keyframe_ptr, ProcessorBase* _processor_ptr, const Scalar& _time_tolerance);
+ void keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const Scalar& _time_tolerance);
- LandmarkBase* addLandmark(LandmarkBase* _lmk_ptr);
+ LandmarkBasePtr addLandmark(LandmarkBasePtr _lmk_ptr);
void addLandmarkList(LandmarkBaseList _lmk_list);
@@ -225,11 +225,11 @@ class Problem //: public NodeBase
/** \brief Adds a new constraint to be added to solver manager
*/
- ConstraintBase* addConstraintPtr(ConstraintBase* _constraint_ptr);
+ ConstraintBasePtr addConstraintPtr(ConstraintBasePtr _constraint_ptr);
/** \brief Adds a constraint to be removed to solver manager
*/
- void removeConstraintPtr(ConstraintBase* _constraint_ptr);
+ void removeConstraintPtr(ConstraintBasePtr _constraint_ptr);
/** \brief Clear covariance
*/
@@ -246,41 +246,41 @@ class Problem //: public NodeBase
/** \brief Gets the covariance of a frame
*/
- bool getFrameCovariance(FrameBase* _frame_ptr, Eigen::MatrixXs& _covariance);
- Eigen::MatrixXs getFrameCovariance(FrameBase* _frame_ptr);
+ bool getFrameCovariance(FrameBasePtr _frame_ptr, Eigen::MatrixXs& _covariance);
+ Eigen::MatrixXs getFrameCovariance(FrameBasePtr _frame_ptr);
/** \brief Gets the covariance of a landmark
*/
- bool getLandmarkCovariance(LandmarkBase* _landmark_ptr, Eigen::MatrixXs& _covariance);
- Eigen::MatrixXs getLandmarkCovariance(LandmarkBase* _landmark_ptr);
+ bool getLandmarkCovariance(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& _covariance);
+ Eigen::MatrixXs getLandmarkCovariance(LandmarkBasePtr _landmark_ptr);
/** \brief Adds a map
*/
- MapBase* addMap(MapBase* _map_ptr);
+ MapBasePtr addMap(MapBasePtr _map_ptr);
/** \brief Adds a trajectory
*/
- TrajectoryBase* addTrajectory(TrajectoryBase* _trajectory_ptr);
+ TrajectoryBasePtr addTrajectory(TrajectoryBasePtr _trajectory_ptr);
/** \brief Gets a pointer to map
*/
- MapBase* getMapPtr();
+ MapBasePtr getMapPtr();
/** \brief Gets a pointer to trajectory
*/
- TrajectoryBase* getTrajectoryPtr();
+ TrajectoryBasePtr getTrajectoryPtr();
/** \brief Gets a pointer to Hardware
*/
- HardwareBase* getHardwarePtr();
+ HardwareBasePtr getHardwarePtr();
/** \brief Returns a pointer to last frame
**/
- FrameBase* getLastFramePtr();
+ FrameBasePtr getLastFramePtr();
/** \brief Returns a pointer to last key frame
*/
- FrameBase* getLastKeyFramePtr();
+ FrameBasePtr getLastKeyFramePtr();
/** \brief Gets a pointer to the state units list
*/
@@ -296,11 +296,11 @@ class Problem //: public NodeBase
/** \brief get top node (this)
*/
- Problem* getTop();
+ ProblemPtr getTop();
/** \brief get this node
*/
- Problem* getProblem();
+ ProblemPtr getProblem();
/** \brief Returns a true (is top)
*/
@@ -315,7 +315,7 @@ class Problem //: public NodeBase
/** \brief get a sensor pointer by its name
* \param _sensor_name The sensor name, as it was installed with installSensor()
*/
- SensorBase* getSensorPtr(const std::string& _sensor_name);
+ SensorBasePtr getSensorPtr(const std::string& _sensor_name);
};
@@ -341,12 +341,12 @@ inline std::list<ConstraintNotification>& Problem::getConstraintNotificationList
return constraint_notification_list_;
}
-inline Problem* Problem::getProblem()
+inline ProblemPtr Problem::getProblem()
{
return this;
}
-inline Problem* Problem::getTop()
+inline ProblemPtr Problem::getTop()
{
return this;
}
diff --git a/src/processor_base.cpp b/src/processor_base.cpp
index 97477e323..f05c5cdbf 100644
--- a/src/processor_base.cpp
+++ b/src/processor_base.cpp
@@ -25,10 +25,10 @@ bool ProcessorBase::permittedKeyFrame()
return getProblem()->permitKeyFrame(this);
}
-void ProcessorBase::makeFrame(CaptureBase* _capture_ptr, FrameKeyType _type)
+void ProcessorBase::makeFrame(CaptureBasePtr _capture_ptr, FrameKeyType _type)
{
// We need to create the new free Frame to hold what will become the last Capture
- FrameBase* new_frame_ptr = getProblem()->createFrame(_type, _capture_ptr->getTimeStamp());
+ FrameBasePtr new_frame_ptr = getProblem()->createFrame(_type, _capture_ptr->getTimeStamp());
new_frame_ptr->addCapture(_capture_ptr); // Add incoming Capture to the new Frame
if (_type == KEY_FRAME)
// Keyframe callback in order to let the other processors to establish their constraints
diff --git a/src/processor_base.h b/src/processor_base.h
index bef09ad2d..2aeadba49 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -28,8 +28,8 @@ struct ProcessorParamsBase
class ProcessorBase : public NodeBase // NodeLinked<SensorBase, NodeTerminus>
{
private:
- Problem* problem_ptr_;
- SensorBase* sensor_ptr_;
+ ProblemPtr problem_ptr_;
+ SensorBasePtr sensor_ptr_;
public:
ProcessorBase(ProcessorType _tp, const std::string& _type, const Scalar& _time_tolerance = 0);
@@ -43,7 +43,7 @@ class ProcessorBase : public NodeBase // NodeLinked<SensorBase, NodeTerminus>
unsigned int id();
- virtual void process(CaptureBase* _capture_ptr) = 0;
+ virtual void process(CaptureBasePtr _capture_ptr) = 0;
/** \brief Vote for KeyFrame generation
*
@@ -58,18 +58,18 @@ class ProcessorBase : public NodeBase // NodeLinked<SensorBase, NodeTerminus>
/**\brief make a Frame with the provided Capture
*/
- virtual void makeFrame(CaptureBase* _capture_ptr, FrameKeyType _type = NON_KEY_FRAME);
+ virtual void makeFrame(CaptureBasePtr _capture_ptr, FrameKeyType _type = NON_KEY_FRAME);
- virtual bool keyFrameCallback(FrameBase* _keyframe_ptr, const Scalar& _time_tolerance) = 0;
+ virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) = 0;
- SensorBase* getSensorPtr();
- const SensorBase* getSensorPtr() const;
- void setSensorPtr(SensorBase* _sen_ptr){sensor_ptr_ = _sen_ptr;}
+ SensorBasePtr getSensorPtr();
+ const SensorBasePtr getSensorPtr() const;
+ void setSensorPtr(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
virtual bool isMotion();
- Problem* getProblem(){return problem_ptr_;}
- void setProblem(Problem* _prob_ptr){problem_ptr_ = _prob_ptr;}
+ ProblemPtr getProblem(){return problem_ptr_;}
+ void setProblem(ProblemPtr _prob_ptr){problem_ptr_ = _prob_ptr;}
private:
static unsigned int processor_id_count_;
@@ -115,13 +115,13 @@ inline unsigned int ProcessorBase::id()
return processor_id_;
}
-inline SensorBase* ProcessorBase::getSensorPtr()
+inline SensorBasePtr ProcessorBase::getSensorPtr()
{
return sensor_ptr_;
// return upperNodePtr();
}
-inline const SensorBase* ProcessorBase::getSensorPtr() const
+inline const SensorBasePtr ProcessorBase::getSensorPtr() const
{
return sensor_ptr_;
// return upperNodePtr();
diff --git a/src/processor_factory.cpp b/src/processor_factory.cpp
index 956051d49..c9af58f8e 100644
--- a/src/processor_factory.cpp
+++ b/src/processor_factory.cpp
@@ -29,7 +29,7 @@ bool ProcessorFactory::unregisterCreator(const std::string& _processor_type)
return callbacks_.erase(_processor_type) == 1;
}
-ProcessorBase* ProcessorFactory::create(const std::string& _processor_type, const std::string& _name, const ProcessorParamsBase* _params)
+ProcessorBasePtr ProcessorFactory::create(const std::string& _processor_type, const std::string& _name, const ProcessorParamsBasePtr _params)
{
CallbackMap::const_iterator creator_callback_it = callbacks_.find(_processor_type);
if (creator_callback_it == callbacks_.end())
diff --git a/src/processor_factory.h b/src/processor_factory.h
index 620fe1854..e3c2fcdb9 100644
--- a/src/processor_factory.h
+++ b/src/processor_factory.h
@@ -76,18 +76,18 @@ namespace wolf
*
* The method ProcessorOdom2D::create() exists in the ProcessorOdom2D class as a static method.
* All these ProcessorXxx::create() methods need to have exactly the same API, regardless of the processor type.
- * This API includes a processor name, and a pointer to a base struct of parameters, ProcessorParamsBase*,
+ * This API includes a processor name, and a pointer to a base struct of parameters, ProcessorParamsBasePtr,
* that can be derived for each derived processor.
*
* Here is an example of ProcessorOdom2D::create() extracted from processor_odom_2D.h:
*
* \code
- * static ProcessorBase* create(const std::string& _name, ProcessorParamsBase* _params)
+ * static ProcessorBasePtr create(const std::string& _name, ProcessorParamsBasePtr _params)
* {
* // cast _params to good type
* ProcessorParamsOdom2D* params = (ProcessorParamsOdom2D*)_params;
*
- * ProcessorBase* prc = new ProcessorOdom2D(params);
+ * ProcessorBasePtr prc = new ProcessorOdom2D(params);
* prc->setName(_name); // pass the name to the created ProcessorOdom2D.
* return prc;
* }
@@ -134,13 +134,13 @@ namespace wolf
* // Note: ProcessorOdom2D::create() is already registered, automatically.
*
* // First create the sensor (See SensorFactory for details)
- * SensorBase* sensor_ptr = SensorFactory::get().create ( "ODOM 2D" , "Main odometer" , extrinsics , &intrinsics );
+ * SensorBasePtr sensor_ptr = SensorFactory::get().create ( "ODOM 2D" , "Main odometer" , extrinsics , &intrinsics );
*
* // To create a odometry integrator, provide a type="ODOM 2D", a name="main odometry", and a pointer to the parameters struct:
*
* ProcessorParamsOdom2D params({...}); // fill in the derived struct (note: ProcessorOdom2D actually has no input params)
*
- * ProcessorBase* processor_ptr =
+ * ProcessorBasePtr processor_ptr =
* ProcessorFactory::get().create ( "ODOM 2D" , "main odometry" , ¶ms );
*
* // Bind processor to sensor
@@ -169,13 +169,13 @@ namespace wolf
class ProcessorFactory
{
public:
- typedef ProcessorBase* (*CreateProcessorCallback)(const std::string& _unique_name, const ProcessorParamsBase* _params);
+ typedef ProcessorBasePtr (*CreateProcessorCallback)(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
private:
typedef std::map<std::string, CreateProcessorCallback> CallbackMap;
public:
bool registerCreator(const std::string& _processor_type, CreateProcessorCallback createFn);
bool unregisterCreator(const std::string& _processor_type);
- ProcessorBase* create(const std::string& _processor_type, const std::string& _unique_name, const ProcessorParamsBase* _params = nullptr);
+ ProcessorBasePtr create(const std::string& _processor_type, const std::string& _unique_name, const ProcessorParamsBasePtr _params = nullptr);
private:
CallbackMap callbacks_;
diff --git a/src/processor_gps.cpp b/src/processor_gps.cpp
index 1f695db2e..f7054fd54 100644
--- a/src/processor_gps.cpp
+++ b/src/processor_gps.cpp
@@ -21,11 +21,11 @@ ProcessorGPS::~ProcessorGPS()
{
}
-void ProcessorGPS::init(CaptureBase* _capture_ptr)
+void ProcessorGPS::init(CaptureBasePtr _capture_ptr)
{
}
-void ProcessorGPS::process(CaptureBase* _capture_ptr)
+void ProcessorGPS::process(CaptureBasePtr _capture_ptr)
{
std::cout << "ProcessorGPS::process(GPScapture)" << std::endl;
//TODO add assert with dynamic_cast when it will be ready
@@ -53,12 +53,12 @@ bool ProcessorGPS::voteForKeyFrame()
return false;
}
-bool ProcessorGPS::keyFrameCallback(wolf::FrameBase*, const Scalar& _time_tol)
+bool ProcessorGPS::keyFrameCallback(wolf::FrameBasePtr, const Scalar& _time_tol)
{
return false;
}
-wolf::ProcessorBase* ProcessorGPS::create(const std::string& _unique_name, const ProcessorParamsBase* _params)
+wolf::ProcessorBasePtr ProcessorGPS::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params)
{
ProcessorGPS* prc_ptr = new ProcessorGPS();
prc_ptr->setName(_unique_name);
diff --git a/src/processor_gps.h b/src/processor_gps.h
index 9b731bc59..e15360118 100644
--- a/src/processor_gps.h
+++ b/src/processor_gps.h
@@ -31,13 +31,13 @@ class ProcessorGPS : public ProcessorBase
public:
ProcessorGPS();
virtual ~ProcessorGPS();
- virtual void init(CaptureBase* _capture_ptr);
- virtual void process(CaptureBase* _capture_ptr);
+ virtual void init(CaptureBasePtr _capture_ptr);
+ virtual void process(CaptureBasePtr _capture_ptr);
virtual bool voteForKeyFrame();
- virtual bool keyFrameCallback(wolf::FrameBase*, const Scalar& _time_tol);
+ virtual bool keyFrameCallback(wolf::FrameBasePtr, const Scalar& _time_tol);
public:
- static ProcessorBase* create(const std::string& _unique_name, const ProcessorParamsBase* _params);
+ static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
};
diff --git a/src/processor_image.cpp b/src/processor_image.cpp
index 5198f99e5..6e1318c9a 100644
--- a/src/processor_image.cpp
+++ b/src/processor_image.cpp
@@ -109,7 +109,7 @@ void ProcessorImage::postProcess()
// drawTrackingFeatures(image_last_,tracker_target_,tracker_candidates_);
}
-bool ProcessorImage::correctFeatureDrift(const FeatureBase* _origin_feature, const FeatureBase* _last_feature, FeatureBase* _incoming_feature)
+bool ProcessorImage::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
{
std::vector<cv::DMatch> matches_mat;
FeaturePointImage* feat_incoming_ptr = (FeaturePointImage*)_incoming_feature;
@@ -277,14 +277,14 @@ void ProcessorImage::filterFeatureLists(FeatureBaseList _original_list, FeatureB
unsigned int counter_repeated_features = 0;
unsigned int counter_total_repeated_features = 0;
- for (std::list<FeatureBase*>::const_iterator first_feature_iterator = _original_list.begin();
+ for (std::list<FeatureBasePtr>::const_iterator first_feature_iterator = _original_list.begin();
first_feature_iterator != _original_list.end();
first_feature_iterator++)
{
FeaturePointImage* tested_feature_ptr = (FeaturePointImage*)*first_feature_iterator;
bool repeated_feature = false;
- for (std::list<FeatureBase*>::const_iterator second_feature_iterator = first_feature_iterator;
+ for (std::list<FeatureBasePtr>::const_iterator second_feature_iterator = first_feature_iterator;
second_feature_iterator != _original_list.begin();
second_feature_iterator--)
{
@@ -494,7 +494,7 @@ void ProcessorImage::drawRoi(cv::Mat _image, std::list<cv::Rect> _roi_list, cv::
cv::imshow("Feature tracker", _image);
}
-void ProcessorImage::drawFeatures(CaptureBase* const _last_ptr)
+void ProcessorImage::drawFeatures(CaptureBasePtr const _last_ptr)
{
for (auto feature_ptr : *(last_ptr_->getFeatureListPtr()))
{
@@ -513,7 +513,7 @@ void ProcessorImage::drawFeatures(CaptureBase* const _last_ptr)
}
-ProcessorBase* ProcessorImage::create(const std::string& _unique_name, const ProcessorParamsBase* _params)
+ProcessorBasePtr ProcessorImage::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params)
{
ProcessorImage* prc_ptr = new ProcessorImage(*((ProcessorParamsImage*)_params));
prc_ptr->setName(_unique_name);
diff --git a/src/processor_image.h b/src/processor_image.h
index ae113d0cf..15697a7e2 100644
--- a/src/processor_image.h
+++ b/src/processor_image.h
@@ -164,7 +164,7 @@ class ProcessorImage : public ProcessorTrackerFeature
* \param _incoming_feature input/output feature in incoming capture to be corrected
* \return false if the the process discards the correspondence with origin's feature
*/
- virtual bool correctFeatureDrift(const FeatureBase* _origin_feature, const FeatureBase* _last_feature, FeatureBase* _incoming_feature);
+ virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature);
/** \brief Vote for KeyFrame generation
*
@@ -189,7 +189,7 @@ class ProcessorImage : public ProcessorTrackerFeature
*
* Creates a constraint from feature to feature
*/
- virtual ConstraintBase* createConstraint(FeatureBase* _feature_ptr, FeatureBase* _feature_other_ptr);
+ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
private:
@@ -234,7 +234,7 @@ class ProcessorImage : public ProcessorTrackerFeature
// These only to debug, will disappear one day soon
public:
- virtual void drawFeatures(CaptureBase* const _last_ptr);
+ virtual void drawFeatures(CaptureBasePtr const _last_ptr);
virtual void drawTrackingFeatures(cv::Mat _image, std::list<cv::Point> _target_list, std::list<cv::Point> _candidates_list);
@@ -243,7 +243,7 @@ class ProcessorImage : public ProcessorTrackerFeature
virtual void resetVisualizationFlag(FeatureBaseList& _feature_list_last);
public:
- static ProcessorBase* create(const std::string& _unique_name, const ProcessorParamsBase* _params);
+ static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
};
@@ -255,7 +255,7 @@ inline bool ProcessorImage::voteForKeyFrame()
return (incoming_ptr_->getFeatureListPtr()->size() < params_.algorithm.min_features_for_keyframe);
}
-inline ConstraintBase* ProcessorImage::createConstraint(FeatureBase* _feature_ptr, FeatureBase* _feature_other_ptr)
+inline ConstraintBasePtr ProcessorImage::createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
{
ConstraintEpipolar* const_epipolar_ptr = new ConstraintEpipolar(_feature_ptr, _feature_other_ptr);
return const_epipolar_ptr; // TODO Crear constraint
diff --git a/src/processor_image_landmark.cpp b/src/processor_image_landmark.cpp
index 5f5a943d8..a6ab659c6 100644
--- a/src/processor_image_landmark.cpp
+++ b/src/processor_image_landmark.cpp
@@ -284,7 +284,7 @@ unsigned int ProcessorImageLandmark::detectNewFeatures(const unsigned int& _max_
return n_new_features;
}
-LandmarkBase* ProcessorImageLandmark::createLandmark(FeatureBase* _feature_ptr)
+LandmarkBasePtr ProcessorImageLandmark::createLandmark(FeatureBasePtr _feature_ptr)
{
FeaturePointImage* feat_point_image_ptr = (FeaturePointImage*) _feature_ptr;
@@ -317,7 +317,7 @@ LandmarkBase* ProcessorImageLandmark::createLandmark(FeatureBase* _feature_ptr)
unitary_vector.normalize();
// std::cout << "unitary_vector: " << unitary_vector(0) << "\t" << unitary_vector(1) << "\t" << unitary_vector(2) << std::endl;
- FrameBase* anchor_frame = getProblem()->getTrajectoryPtr()->getLastFramePtr();
+ FrameBasePtr anchor_frame = getProblem()->getTrajectoryPtr()->getLastFramePtr();
// TODO: Poner el anchor del punto (ahora mismo está en el 0 del world, pero no hay código por si cambia)
@@ -328,7 +328,7 @@ LandmarkBase* ProcessorImageLandmark::createLandmark(FeatureBase* _feature_ptr)
return new LandmarkAHP(vec_homogeneous, anchor_frame, getSensorPtr(), feat_point_image_ptr->getDescriptor());
}
-ConstraintBase* ProcessorImageLandmark::createConstraint(FeatureBase* _feature_ptr, LandmarkBase* _landmark_ptr)
+ConstraintBasePtr ProcessorImageLandmark::createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
{
// std::cout << "\nProcessorImageLandmark::createConstraint" << std::endl;
if (((LandmarkAHP*)_landmark_ptr)->getAnchorFrame() == last_ptr_->getFramePtr())
@@ -594,7 +594,7 @@ void ProcessorImageLandmark::drawFeatures(cv::Mat& _image)
//namespace wolf{
-ProcessorBase* ProcessorImageLandmark::create(const std::string& _unique_name, const ProcessorParamsBase* _params)
+ProcessorBasePtr ProcessorImageLandmark::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params)
{
ProcessorImageLandmark* prc_ptr = new ProcessorImageLandmark(*((ProcessorParamsImage*)_params));
prc_ptr->setName(_unique_name);
diff --git a/src/processor_image_landmark.h b/src/processor_image_landmark.h
index b18853f13..73ef024c1 100644
--- a/src/processor_image_landmark.h
+++ b/src/processor_image_landmark.h
@@ -189,10 +189,10 @@ class ProcessorImageLandmark : public ProcessorTrackerLandmark
*
* Implement in derived classes to build the type of landmark you need for this tracker.
*/
- virtual LandmarkBase* createLandmark(FeatureBase* _feature_ptr);
+ virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
public:
- static ProcessorBase* create(const std::string& _unique_name, const ProcessorParamsBase* _params);
+ static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
@@ -205,7 +205,7 @@ class ProcessorImageLandmark : public ProcessorTrackerLandmark
* TODO: Make a general ConstraintFactory, and put it in WolfProblem.
* This factory only needs to know the two derived pointers to decide on the actual Constraint created.
*/
- virtual ConstraintBase* createConstraint(FeatureBase* _feature_ptr, LandmarkBase* _landmark_ptr);
+ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
diff --git a/src/processor_imu.cpp b/src/processor_imu.cpp
index 4f1d1a4f5..9d6a01bfe 100644
--- a/src/processor_imu.cpp
+++ b/src/processor_imu.cpp
@@ -23,7 +23,7 @@ ProcessorIMU::~ProcessorIMU()
{
}
-ProcessorBase* ProcessorIMU::create(const std::string& _unique_name, const ProcessorParamsBase* _params)
+ProcessorBasePtr ProcessorIMU::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params)
{
ProcessorIMU* prc_ptr = new ProcessorIMU();
prc_ptr->setName(_unique_name);
diff --git a/src/processor_imu.h b/src/processor_imu.h
index 9cda294dd..2716492b4 100644
--- a/src/processor_imu.h
+++ b/src/processor_imu.h
@@ -35,8 +35,8 @@ class ProcessorIMU : public ProcessorMotion{
virtual Motion interpolate(const Motion& _motion_ref,
Motion& _motion,
TimeStamp& _ts);
- virtual ConstraintBase* createConstraint(FeatureBase* _feature_motion,
- FrameBase* _frame_origin);
+ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_motion,
+ FrameBasePtr _frame_origin);
void resetDerived();
@@ -77,7 +77,7 @@ class ProcessorIMU : public ProcessorMotion{
void remapData(const Eigen::VectorXs& _data);
public:
- static ProcessorBase* create(const std::string& _unique_name, const ProcessorParamsBase* _params);
+ static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
};
}
@@ -346,10 +346,9 @@ inline void ProcessorIMU::resetDerived()
dDq_dwb_.setZero();
}
-
-inline ConstraintBase* ProcessorIMU::createConstraint(FeatureBase* _feature_motion, FrameBase* _frame_origin)
+inline ConstraintBasePtr ProcessorIMU::createConstraint(FeatureBasePtr _feature_motion, FrameBasePtr _frame_origin)
{
- // return new ConstraintIMU(_feature_motion, _frame_origin);
+ // TODO: return new ConstraintIMU(_feature_motion, _frame_origin);
return nullptr;
}
diff --git a/src/processor_motion.h b/src/processor_motion.h
index 15e6808dc..b31363512 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -87,7 +87,7 @@ class ProcessorMotion : public ProcessorBase
// Instructions to the processor:
- virtual void process(CaptureBase* _incoming_ptr);
+ virtual void process(CaptureBasePtr _incoming_ptr);
virtual void resetDerived();
// Queries to the processor:
@@ -157,7 +157,7 @@ class ProcessorMotion : public ProcessorBase
/** Set the origin of all motion for this processor
* \param _origin_frame the key frame to be the origin
*/
- void setOrigin(FrameBase* _origin_frame);
+ void setOrigin(FrameBasePtr _origin_frame);
/** Set the origin of all motion for this processor
* \param _x_origin the state at the origin
@@ -165,7 +165,7 @@ class ProcessorMotion : public ProcessorBase
*/
void setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin);
- virtual bool keyFrameCallback(FrameBase* _keyframe_ptr, const Scalar& _time_tol);
+ virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol);
// Helper functions:
public:
@@ -175,7 +175,7 @@ class ProcessorMotion : public ProcessorBase
// void reset(CaptureMotion2* _capture_ptr);
- FrameBase* makeFrame(CaptureBase* _capture_ptr, const Eigen::VectorXs& _state, FrameKeyType _type);
+ FrameBasePtr makeFrame(CaptureBasePtr _capture_ptr, const Eigen::VectorXs& _state, FrameKeyType _type);
MotionBuffer* getBufferPtr();
@@ -312,7 +312,7 @@ class ProcessorMotion : public ProcessorBase
virtual Motion interpolate(const Motion& _motion_ref, Motion& _motion, TimeStamp& _ts) = 0;
- virtual ConstraintBase* createConstraint(FeatureBase* _feature_motion, FrameBase* _frame_origin) = 0;
+ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_motion, FrameBasePtr _frame_origin) = 0;
Motion motionZero(const TimeStamp& _ts);
@@ -322,7 +322,7 @@ class ProcessorMotion : public ProcessorBase
Size delta_size_; ///< the size of the deltas
Size delta_cov_size_; ///< the size of the delta covariances matrix
Size data_size_; ///< the size of the incoming data
- CaptureBase* origin_ptr_;
+ CaptureBasePtr origin_ptr_;
CaptureMotion* last_ptr_;
CaptureMotion* incoming_ptr_;
@@ -370,12 +370,12 @@ inline ProcessorMotion::~ProcessorMotion()
inline void ProcessorMotion::setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin)
{
// make a new key frame
- FrameBase* key_frame_ptr = getProblem()->createFrame(KEY_FRAME, _x_origin, _ts_origin);
+ FrameBasePtr key_frame_ptr = getProblem()->createFrame(KEY_FRAME, _x_origin, _ts_origin);
// set the key frame as origin
setOrigin(key_frame_ptr);
}
-inline void ProcessorMotion::setOrigin(FrameBase* _origin_frame)
+inline void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
{
assert(_origin_frame->getTrajectoryPtr() != nullptr && "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
@@ -405,7 +405,7 @@ inline void ProcessorMotion::setOrigin(FrameBase* _origin_frame)
resetDerived();
}
-inline void ProcessorMotion::process(CaptureBase* _incoming_ptr)
+inline void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
{
incoming_ptr_ = (CaptureMotion*)(_incoming_ptr);
preProcess();
@@ -415,7 +415,7 @@ inline void ProcessorMotion::process(CaptureBase* _incoming_ptr)
{
// key_capture
CaptureMotion* key_capture_ptr = last_ptr_;
- FrameBase* key_frame_ptr = key_capture_ptr->getFramePtr();
+ FrameBasePtr key_frame_ptr = key_capture_ptr->getFramePtr();
// Set the frame as key
key_frame_ptr->setState(getCurrentState());
@@ -423,7 +423,7 @@ inline void ProcessorMotion::process(CaptureBase* _incoming_ptr)
key_frame_ptr->setKey();
// create motion constraint and add it to the new keyframe
- FeatureBase* key_feature_ptr = new FeatureBase(FEATURE_MOTION, "MOTION",
+ FeatureBasePtr key_feature_ptr = new FeatureBase(FEATURE_MOTION, "MOTION",
key_capture_ptr->getBufferPtr()->get().back().delta_integr_,
key_capture_ptr->getBufferPtr()->get().back().delta_integr_cov_.determinant() > 0 ?
key_capture_ptr->getBufferPtr()->get().back().delta_integr_cov_ :
@@ -523,7 +523,7 @@ inline void ProcessorMotion::reintegrate(CaptureMotion* _capture_ptr)
}
}
-inline bool ProcessorMotion::keyFrameCallback(FrameBase* _keyframe_ptr, const Scalar& _time_tol)
+inline bool ProcessorMotion::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol)
{
assert(_keyframe_ptr->getTrajectoryPtr() != nullptr && "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
//std::cout << "ProcessorMotion::keyFrameCallback: ts = " << _keyframe_ptr->getTimeStamp().getSeconds() << "." << _keyframe_ptr->getTimeStamp().getNanoSeconds() << std::endl;
@@ -537,7 +537,7 @@ inline bool ProcessorMotion::keyFrameCallback(FrameBase* _keyframe_ptr, const Sc
CaptureMotion* capture_ptr = findCaptureContainingTimeStamp(ts);
assert(capture_ptr != nullptr && "ProcessorMotion::keyFrameCallback: no motion capture containing the required TimeStamp found");
- FrameBase* key_capture_origin = capture_ptr->getOriginFramePtr();
+ FrameBasePtr key_capture_origin = capture_ptr->getOriginFramePtr();
// create motion capture
CaptureMotion* key_capture_ptr = new CaptureMotion(ts, this->getSensorPtr(), Eigen::VectorXs::Zero(data_size_),
@@ -564,7 +564,7 @@ inline bool ProcessorMotion::keyFrameCallback(FrameBase* _keyframe_ptr, const Sc
//std::cout << "\tinterpolated state: " << interpolated_state.transpose() << std::endl;
// create motion constraint and add it to the new keyframe
- FeatureBase* key_feature_ptr = new FeatureBase(FEATURE_MOTION, "MOTION",
+ FeatureBasePtr key_feature_ptr = new FeatureBase(FEATURE_MOTION, "MOTION",
key_capture_ptr->getBufferPtr()->get().back().delta_integr_,
key_capture_ptr->getBufferPtr()->get().back().delta_integr_cov_.determinant() > 0 ?
key_capture_ptr->getBufferPtr()->get().back().delta_integr_cov_ :
@@ -585,7 +585,7 @@ inline bool ProcessorMotion::keyFrameCallback(FrameBase* _keyframe_ptr, const Sc
// modify feature and constraint (if they exist)
if (!capture_ptr->getFeatureListPtr()->empty())
{
- FeatureBase* feature_ptr = capture_ptr->getFeatureListPtr()->front();
+ FeatureBasePtr feature_ptr = capture_ptr->getFeatureListPtr()->front();
// modify feature
feature_ptr->setMeasurement(capture_ptr->getBufferPtr()->get().back().delta_integr_);
feature_ptr->setMeasurementCovariance(capture_ptr->getBufferPtr()->get().back().delta_integr_cov_.determinant() > 0 ?
@@ -607,10 +607,10 @@ inline void ProcessorMotion::splitBuffer(const TimeStamp& _t_split, MotionBuffer
last_ptr_->getBufferPtr()->split(_t_split, _oldest_part);
}
-inline FrameBase* ProcessorMotion::makeFrame(CaptureBase* _capture_ptr, const Eigen::VectorXs& _state, FrameKeyType _type)
+inline FrameBasePtr ProcessorMotion::makeFrame(CaptureBasePtr _capture_ptr, const Eigen::VectorXs& _state, FrameKeyType _type)
{
// We need to create the new free Frame to hold what will become the last Capture
- FrameBase* new_frame_ptr = getProblem()->createFrame(_type, _state, _capture_ptr->getTimeStamp());
+ FrameBasePtr new_frame_ptr = getProblem()->createFrame(_type, _state, _capture_ptr->getTimeStamp());
new_frame_ptr->addCapture(_capture_ptr); // Add incoming Capture to the new Frame
return new_frame_ptr;
}
@@ -703,7 +703,7 @@ inline CaptureMotion* ProcessorMotion::findCaptureContainingTimeStamp(const Time
return nullptr;
else
{
- CaptureBase* capture_base_ptr = capture_ptr->getOriginFramePtr()->hasCaptureOf(getSensorPtr());
+ CaptureBasePtr capture_base_ptr = capture_ptr->getOriginFramePtr()->hasCaptureOf(getSensorPtr());
if (capture_base_ptr == nullptr)
return nullptr;
else
diff --git a/src/processor_odom_2D.cpp b/src/processor_odom_2D.cpp
index 6891646a1..0d94900ae 100644
--- a/src/processor_odom_2D.cpp
+++ b/src/processor_odom_2D.cpp
@@ -2,7 +2,7 @@
namespace wolf
{
-ProcessorBase* ProcessorOdom2D::create(const std::string& _unique_name, const ProcessorParamsBase* _params)
+ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params)
{
ProcessorParamsOdom2D* params = (ProcessorParamsOdom2D*)_params;
ProcessorOdom2D* prc_ptr = new ProcessorOdom2D(params->dist_traveled_th_, params->cov_det_th_, params->elapsed_time_th_);
diff --git a/src/processor_odom_2D.h b/src/processor_odom_2D.h
index 461705648..745938f14 100644
--- a/src/processor_odom_2D.h
+++ b/src/processor_odom_2D.h
@@ -51,8 +51,7 @@ class ProcessorOdom2D : public ProcessorMotion
Motion& _motion,
TimeStamp& _ts);
- virtual ConstraintBase* createConstraint(FeatureBase* _feature_motion,
- FrameBase* _frame_origin);
+ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_motion, FrameBasePtr _frame_origin);
protected:
Scalar dist_traveled_th_;
@@ -61,7 +60,7 @@ class ProcessorOdom2D : public ProcessorMotion
// Factory method
public:
- static ProcessorBase* create(const std::string& _unique_name, const ProcessorParamsBase* _params);
+ static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
};
inline ProcessorOdom2D::ProcessorOdom2D(const Scalar& _traveled_dist_th, const Scalar& _cov_det_th, const Scalar& _elapsed_time_th) :
@@ -207,7 +206,7 @@ inline Eigen::VectorXs ProcessorOdom2D::deltaZero() const
return Eigen::VectorXs::Zero(delta_size_);
}
-inline ConstraintBase* ProcessorOdom2D::createConstraint(FeatureBase* _feature_motion, FrameBase* _frame_origin)
+inline ConstraintBasePtr ProcessorOdom2D::createConstraint(FeatureBasePtr _feature_motion, FrameBasePtr _frame_origin)
{
return new ConstraintOdom2D(_feature_motion, _frame_origin);
}
diff --git a/src/processor_odom_3D.cpp b/src/processor_odom_3D.cpp
index 7871192dc..502c6c383 100644
--- a/src/processor_odom_3D.cpp
+++ b/src/processor_odom_3D.cpp
@@ -2,7 +2,7 @@
namespace wolf
{
-ProcessorBase* ProcessorOdom3D::create(const std::string& _unique_name, const ProcessorParamsBase* _params)
+ProcessorBasePtr ProcessorOdom3D::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params)
{
ProcessorOdom3D* prc_ptr = new ProcessorOdom3D();
prc_ptr->setName(_unique_name);
diff --git a/src/processor_odom_3D.h b/src/processor_odom_3D.h
index 4e3657eaf..0787fde11 100644
--- a/src/processor_odom_3D.h
+++ b/src/processor_odom_3D.h
@@ -66,8 +66,12 @@ class ProcessorOdom3D : public ProcessorMotion
Motion& _motion,
TimeStamp& _ts);
- virtual ConstraintBase* createConstraint(FeatureBase* _feature_motion,
- FrameBase* _frame_origin);
+//<<<<<<< e72779277b2cbd56ce81286c43b51ae2b4934110
+// virtual ConstraintBase* createConstraint(FeatureBase* _feature_motion,
+// FrameBase* _frame_origin);
+//=======
+ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_motion, FrameBasePtr _frame_origin);
+//>>>>>>> typedef all pointers to base classes
private:
Eigen::Map<const Eigen::Vector3s> p1_, p2_;
@@ -78,7 +82,7 @@ class ProcessorOdom3D : public ProcessorMotion
// Factory method
public:
- static ProcessorBase* create(const std::string& _unique_name, const ProcessorParamsBase* _params);
+ static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
};
@@ -164,7 +168,7 @@ inline Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref, Motion& _m
return tmp;
}
-inline ConstraintBase* ProcessorOdom3D::createConstraint(FeatureBase* _feature_motion, FrameBase* _frame_origin)
+inline ConstraintBasePtr ProcessorOdom3D::createConstraint(FeatureBasePtr _feature_motion, FrameBasePtr _frame_origin)
{
return new ConstraintOdom2D(_feature_motion, _frame_origin);
}
diff --git a/src/processor_tracker.cpp b/src/processor_tracker.cpp
index 6b85c5632..8915cb823 100644
--- a/src/processor_tracker.cpp
+++ b/src/processor_tracker.cpp
@@ -37,7 +37,7 @@ ProcessorTracker::~ProcessorTracker()
}
}
-void ProcessorTracker::process(CaptureBase* const _incoming_ptr)
+void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
{
//std::cout << "-----nProcessorTracker::process():" << std::endl;
//std::cout << "\tlast features: " << (last_ptr_ == nullptr ? 0 : last_ptr_->getFeatureListPtr()->size()) << std::endl;
@@ -153,7 +153,7 @@ void ProcessorTracker::process(CaptureBase* const _incoming_ptr)
//std::cout << "\tincoming new features: " << new_features_incoming_.size() << std::endl;
}
-bool ProcessorTracker::keyFrameCallback(FrameBase* _keyframe_ptr, const Scalar& _time_tol)
+bool ProcessorTracker::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol)
{
assert((last_ptr_ == nullptr || last_ptr_->getFramePtr() != nullptr) && "ProcessorTracker::keyFrameCallback: last_ptr_ must have a frame allways");
Scalar time_tol = std::max(time_tolerance_, _time_tol);
@@ -168,7 +168,7 @@ bool ProcessorTracker::keyFrameCallback(FrameBase* _keyframe_ptr, const Scalar&
//std::cout << "ProcessorTracker::keyFrameCallback in sensor " << getSensorPtr()->id() << std::endl;
// Capture last_ is added to the new keyframe
- FrameBase* last_old_frame = last_ptr_->getFramePtr();
+ FrameBasePtr last_old_frame = last_ptr_->getFramePtr();
last_old_frame->unlinkCapture(last_ptr_);
last_old_frame->destruct();
_keyframe_ptr->addCapture(last_ptr_);
@@ -185,7 +185,7 @@ bool ProcessorTracker::keyFrameCallback(FrameBase* _keyframe_ptr, const Scalar&
return true;
}
-void ProcessorTracker::setKeyFrame(CaptureBase* _capture_ptr)
+void ProcessorTracker::setKeyFrame(CaptureBasePtr _capture_ptr)
{
assert(_capture_ptr != nullptr && _capture_ptr->getFramePtr() != nullptr && "ProcessorTracker::setKeyFrame: null capture or capture without frame");
if (!_capture_ptr->getFramePtr()->isKey())
@@ -195,7 +195,7 @@ void ProcessorTracker::setKeyFrame(CaptureBase* _capture_ptr)
// Set state to the keyframe
_capture_ptr->getFramePtr()->setState(getProblem()->getStateAtTimeStamp(_capture_ptr->getTimeStamp()));
// Call the new keyframe callback in order to let the other processors to establish their constraints
- getProblem()->keyFrameCallback(_capture_ptr->getFramePtr(), (ProcessorBase*)((this)), time_tolerance_);
+ getProblem()->keyFrameCallback(_capture_ptr->getFramePtr(), (ProcessorBasePtr)((this)), time_tolerance_);
}
}
diff --git a/src/processor_tracker.h b/src/processor_tracker.h
index c29132d9f..fc4cec350 100644
--- a/src/processor_tracker.h
+++ b/src/processor_tracker.h
@@ -66,9 +66,9 @@ struct ProcessorParamsTracker : public ProcessorParamsBase
class ProcessorTracker : public ProcessorBase
{
protected:
- CaptureBase* origin_ptr_; ///< Pointer to the origin of the tracker.
- CaptureBase* last_ptr_; ///< Pointer to the last tracked capture.
- CaptureBase* incoming_ptr_; ///< Pointer to the incoming capture being processed.
+ CaptureBasePtr origin_ptr_; ///< Pointer to the origin of the tracker.
+ CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture.
+ CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed.
FeatureBaseList new_features_last_; ///< List of new features in \b last for landmark initialization and new key-frame creation.
FeatureBaseList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
unsigned int max_new_features_; ///< max features allowed to detect in one iteration. 0 = no limit
@@ -82,14 +82,14 @@ class ProcessorTracker : public ProcessorBase
* Usually you do not need to overload this method in derived classes.
* Overload it only if you want to alter this algorithm.
*/
- virtual void process(CaptureBase* const _incoming_ptr);
+ virtual void process(CaptureBasePtr const _incoming_ptr);
void setMaxNewFeatures(const unsigned int& _max_new_features);
const unsigned int getMaxNewFeatures();
- virtual bool keyFrameCallback(FrameBase* _keyframe_ptr, const Scalar& _dt);
+ virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _dt);
- virtual CaptureBase* getLastPtr();
+ virtual CaptureBasePtr getLastPtr();
protected:
/** Pre-process incoming Capture
@@ -169,7 +169,7 @@ class ProcessorTracker : public ProcessorBase
/**\brief set key Frame to the provided Capture's frame
*/
- virtual void setKeyFrame(CaptureBase* _capture_ptr);
+ virtual void setKeyFrame(CaptureBasePtr _capture_ptr);
/** \brief Reset the tracker using the \b last Capture as the new \b origin.
*/
@@ -181,11 +181,11 @@ class ProcessorTracker : public ProcessorBase
protected:
- void addNewFeatureLast(FeatureBase* _feature_ptr);
+ void addNewFeatureLast(FeatureBasePtr _feature_ptr);
FeatureBaseList& getNewFeaturesListIncoming();
- void addNewFeatureIncoming(FeatureBase* _feature_ptr);
+ void addNewFeatureIncoming(FeatureBasePtr _feature_ptr);
};
inline void ProcessorTracker::setMaxNewFeatures(const unsigned int& _max_new_features)
@@ -203,7 +203,7 @@ inline FeatureBaseList& ProcessorTracker::getNewFeaturesListLast()
return new_features_last_;
}
-inline void ProcessorTracker::addNewFeatureLast(FeatureBase* _feature_ptr)
+inline void ProcessorTracker::addNewFeatureLast(FeatureBasePtr _feature_ptr)
{
new_features_last_.push_back(_feature_ptr);
}
@@ -213,12 +213,12 @@ inline FeatureBaseList& ProcessorTracker::getNewFeaturesListIncoming()
return new_features_incoming_;
}
-inline void ProcessorTracker::addNewFeatureIncoming(FeatureBase* _feature_ptr)
+inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
{
new_features_incoming_.push_back(_feature_ptr);
}
-inline CaptureBase* ProcessorTracker::getLastPtr()
+inline CaptureBasePtr ProcessorTracker::getLastPtr()
{
return last_ptr_;
}
diff --git a/src/processor_tracker_feature.h b/src/processor_tracker_feature.h
index 237ce9ba5..76c5d43c9 100644
--- a/src/processor_tracker_feature.h
+++ b/src/processor_tracker_feature.h
@@ -112,7 +112,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
* \param _incoming_feature input/output feature in incoming capture to be corrected
* \return false if the the process discards the correspondence with origin's feature
*/
- virtual bool correctFeatureDrift(const FeatureBase* _origin_feature, const FeatureBase* _last_feature, FeatureBase* _incoming_feature) = 0;
+ virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature) = 0;
/** \brief Vote for KeyFrame generation
*
@@ -154,7 +154,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
* TODO: Make a general ConstraintFactory, and put it in WolfProblem.
* This factory only needs to know the two derived types to decide on the actual Constraint created.
*/
- virtual ConstraintBase* createConstraint(FeatureBase* _feature_ptr, FeatureBase* _feature_other_ptr) = 0;
+ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0;
/** \brief Establish constraints between features in Captures \b last and \b origin
*/
diff --git a/src/processor_tracker_feature_corner.cpp b/src/processor_tracker_feature_corner.cpp
index 25c1b6c0f..a3f26c982 100644
--- a/src/processor_tracker_feature_corner.cpp
+++ b/src/processor_tracker_feature_corner.cpp
@@ -90,8 +90,8 @@ unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int
return new_features_last_.size();
}
-ConstraintBase* ProcessorTrackerFeatureCorner::createConstraint(FeatureBase* _feature_ptr,
- FeatureBase* _feature_other_ptr)
+ConstraintBasePtr ProcessorTrackerFeatureCorner::createConstraint(FeatureBasePtr _feature_ptr,
+ FeatureBasePtr _feature_other_ptr)
{
// Getting landmark ptr
LandmarkCorner2D* landmark_ptr = nullptr;
diff --git a/src/processor_tracker_feature_corner.h b/src/processor_tracker_feature_corner.h
index ae64b60f3..9c1e118a4 100644
--- a/src/processor_tracker_feature_corner.h
+++ b/src/processor_tracker_feature_corner.h
@@ -89,7 +89,7 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
* \param _incoming_feature input/output feature in incoming capture to be corrected
* \return false if the the process discards the correspondence with origin's feature
*/
- virtual bool correctFeatureDrift(const FeatureBase* _last_feature, FeatureBase* _incoming_feature);
+ virtual bool correctFeatureDrift(const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature);
/** \brief Vote for KeyFrame generation
*
@@ -122,7 +122,7 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
* TODO: Make a general ConstraintFactory, and put it in WolfProblem.
* This factory only needs to know the two derived pointers to decide on the actual Constraint created.
*/
- virtual ConstraintBase* createConstraint(FeatureBase* _feature_ptr, FeatureBase* _feature_other_ptr);
+ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
private:
@@ -149,8 +149,8 @@ inline void ProcessorTrackerFeatureCorner::postProcess()
{
}
-inline bool ProcessorTrackerFeatureCorner::correctFeatureDrift(const FeatureBase* _last_feature,
- FeatureBase* _incoming_feature)
+inline bool ProcessorTrackerFeatureCorner::correctFeatureDrift(const FeatureBasePtr _last_feature,
+ FeatureBasePtr _incoming_feature)
{
return true;
}
diff --git a/src/processor_tracker_feature_dummy.h b/src/processor_tracker_feature_dummy.h
index ff62e9190..b76b8788a 100644
--- a/src/processor_tracker_feature_dummy.h
+++ b/src/processor_tracker_feature_dummy.h
@@ -42,7 +42,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
* \param _incoming_feature input/output feature in incoming capture to be corrected
* \return false if the the process discards the correspondence with origin's feature
*/
- virtual bool correctFeatureDrift(const FeatureBase* _origin_feature, const FeatureBase* _last_feature, FeatureBase* _incoming_feature);
+ virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature);
/** \brief Vote for KeyFrame generation
*
@@ -75,7 +75,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
* TODO: Make a general ConstraintFactory, and put it in WolfProblem.
* This factory only needs to know the two derived pointers to decide on the actual Constraint created.
*/
- virtual ConstraintBase* createConstraint(FeatureBase* _feature_ptr, FeatureBase* _feature_other_ptr);
+ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
};
@@ -91,14 +91,14 @@ inline ProcessorTrackerFeatureDummy::~ProcessorTrackerFeatureDummy()
//
}
-inline bool ProcessorTrackerFeatureDummy::correctFeatureDrift(const FeatureBase* _origin_feature, const FeatureBase* _last_feature,
- FeatureBase* _incoming_feature)
+inline bool ProcessorTrackerFeatureDummy::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature,
+ FeatureBasePtr _incoming_feature)
{
return true;
}
-inline ConstraintBase* ProcessorTrackerFeatureDummy::createConstraint(FeatureBase* _feature_ptr,
- FeatureBase* _feature_other_ptr)
+inline ConstraintBasePtr ProcessorTrackerFeatureDummy::createConstraint(FeatureBasePtr _feature_ptr,
+ FeatureBasePtr _feature_other_ptr)
{
std::cout << "creating constraint: last feature " << _feature_ptr->getMeasurement()
<< " with origin feature " << _feature_other_ptr->getMeasurement() << std::endl;
diff --git a/src/processor_tracker_landmark.cpp b/src/processor_tracker_landmark.cpp
index ae90440c7..5d749dc3e 100644
--- a/src/processor_tracker_landmark.cpp
+++ b/src/processor_tracker_landmark.cpp
@@ -89,7 +89,7 @@ void ProcessorTrackerLandmark::createNewLandmarks(LandmarkBaseList& _new_landmar
for (auto new_feature_ptr : new_features_last_)
{
// create new landmark
- LandmarkBase* new_lmk_ptr = createLandmark(new_feature_ptr);
+ LandmarkBasePtr new_lmk_ptr = createLandmark(new_feature_ptr);
//std::cout << "\tnew_landmark: " << new_lmk_ptr->id() << std::endl;
_new_landmarks.push_back(new_lmk_ptr);
// create new correspondence
diff --git a/src/processor_tracker_landmark.h b/src/processor_tracker_landmark.h
index 09d432171..8ccc521df 100644
--- a/src/processor_tracker_landmark.h
+++ b/src/processor_tracker_landmark.h
@@ -143,7 +143,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker
*
* Implement in derived classes to build the type of landmark you need for this tracker.
*/
- virtual LandmarkBase* createLandmark(FeatureBase* _feature_ptr) = 0;
+ virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr) = 0;
/** \brief Create a new constraint
* \param _feature_ptr pointer to the Feature to constrain
@@ -154,7 +154,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker
* TODO: Make a general ConstraintFactory, and put it in WolfProblem.
* This factory only needs to know the two derived pointers to decide on the actual Constraint created.
*/
- virtual ConstraintBase* createConstraint(FeatureBase* _feature_ptr, LandmarkBase* _landmark_ptr) = 0;
+ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) = 0;
/** \brief Establish constraints between features in Captures \b last and \b origin
*/
diff --git a/src/processor_tracker_landmark2.cpp b/src/processor_tracker_landmark2.cpp
index de51c9183..87e5bc611 100644
--- a/src/processor_tracker_landmark2.cpp
+++ b/src/processor_tracker_landmark2.cpp
@@ -42,7 +42,7 @@ unsigned int ProcessorTrackerLandmark2::processNew(const unsigned int& _max_feat
for (auto new_feature_ptr : new_features_last_)
{
// create new landmark
- LandmarkBase* new_lmk_ptr = createLandmark(new_feature_ptr);
+ LandmarkBasePtr new_lmk_ptr = createLandmark(new_feature_ptr);
new_landmarks.push_back(new_lmk_ptr);
// create new correspondence
matches_landmark_from_last_[new_feature_ptr] = LandmarkMatch(new_lmk_ptr, 1); // max score
diff --git a/src/processor_tracker_landmark2.h b/src/processor_tracker_landmark2.h
index 93b52f36f..54bf5a371 100644
--- a/src/processor_tracker_landmark2.h
+++ b/src/processor_tracker_landmark2.h
@@ -17,14 +17,14 @@ namespace wolf
// Match Feature - Landmark
struct LandmarkMatch
{
- LandmarkBase* landmark_ptr_;
+ LandmarkBasePtr landmark_ptr_;
Scalar normalized_score_;
LandmarkMatch() :
landmark_ptr_(nullptr), normalized_score_(0.0)
{
}
- LandmarkMatch(LandmarkBase* _landmark_ptr, const Scalar& _normalized_score) :
+ LandmarkMatch(LandmarkBasePtr _landmark_ptr, const Scalar& _normalized_score) :
landmark_ptr_(_landmark_ptr), normalized_score_(_normalized_score)
{
@@ -32,7 +32,7 @@ struct LandmarkMatch
};
// Match map Feature - Landmark
-typedef std::map<FeatureBase*, LandmarkMatch> LandmarkMatchMap;
+typedef std::map<FeatureBasePtr, LandmarkMatch> LandmarkMatchMap;
/** \brief Landmark tracker processor
*
@@ -167,7 +167,7 @@ class ProcessorTrackerLandmark2 : public ProcessorTracker
*
* Implement in derived classes to build the type of landmark you need for this tracker.
*/
- virtual LandmarkBase* createLandmark(FeatureBase* _feature_ptr) = 0;
+ virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr) = 0;
/** \brief Create a new constraint
* \param _feature_ptr pointer to the Feature to constrain
@@ -178,7 +178,7 @@ class ProcessorTrackerLandmark2 : public ProcessorTracker
* TODO: Make a general ConstraintFactory, and put it in WolfProblem.
* This factory only needs to know the two derived pointers to decide on the actual Constraint created.
*/
- virtual ConstraintBase* createConstraint(FeatureBase* _feature_ptr, LandmarkBase* _landmark_ptr) = 0;
+ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) = 0;
/** \brief Establish constraints between features in Captures \b last and \b origin
*/
diff --git a/src/processor_tracker_landmark_corner.cpp b/src/processor_tracker_landmark_corner.cpp
index ea8df7a53..6ad3c1550 100644
--- a/src/processor_tracker_landmark_corner.cpp
+++ b/src/processor_tracker_landmark_corner.cpp
@@ -51,8 +51,8 @@ unsigned int ProcessorTrackerLandmarkCorner::findLandmarks(const LandmarkBaseLis
Scalar dm2;
// COMPUTING ALL EXPECTED FEATURES
- std::map<LandmarkBase*, Eigen::Vector4s, std::less<LandmarkBase*>, Eigen::aligned_allocator<std::pair<LandmarkBase*, Eigen::Vector4s> > > expected_features;
- std::map<LandmarkBase*, Eigen::Matrix3s, std::less<LandmarkBase*>, Eigen::aligned_allocator<std::pair<LandmarkBase*, Eigen::Matrix3s> > > expected_features_covs;
+ std::map<LandmarkBasePtr, Eigen::Vector4s, std::less<LandmarkBasePtr>, Eigen::aligned_allocator<std::pair<LandmarkBasePtr, Eigen::Vector4s> > > expected_features;
+ std::map<LandmarkBasePtr, Eigen::Matrix3s, std::less<LandmarkBasePtr>, Eigen::aligned_allocator<std::pair<LandmarkBasePtr, Eigen::Matrix3s> > > expected_features_covs;
for (auto landmark : not_matched_landmarks)
expectedFeature(landmark, expected_features[landmark], expected_features_covs[landmark]);
@@ -104,8 +104,8 @@ unsigned int ProcessorTrackerLandmarkCorner::findLandmarks(const LandmarkBaseLis
unsigned int ii, jj;
// COMPUTING ALL EXPECTED FEATURES
- std::map<LandmarkBase*, Eigen::Vector4s, std::less<LandmarkBase*>, Eigen::aligned_allocator<std::pair<LandmarkBase*, Eigen::Vector4s> > > expected_features;
- std::map<LandmarkBase*, Eigen::Matrix3s, std::less<LandmarkBase*>, Eigen::aligned_allocator<std::pair<LandmarkBase*, Eigen::Matrix3s> > > expected_features_covs;
+ std::map<LandmarkBasePtr, Eigen::Vector4s, std::less<LandmarkBasePtr>, Eigen::aligned_allocator<std::pair<LandmarkBasePtr, Eigen::Vector4s> > > expected_features;
+ std::map<LandmarkBasePtr, Eigen::Matrix3s, std::less<LandmarkBasePtr>, Eigen::aligned_allocator<std::pair<LandmarkBasePtr, Eigen::Matrix3s> > > expected_features_covs;
for (auto landmark : _landmarks_corner_searched)
expectedFeature(landmark, expected_features[landmark], expected_features_covs[landmark]);
//std::cout << "expected features!" << std::endl;
@@ -279,7 +279,7 @@ void ProcessorTrackerLandmarkCorner::extractCorners(CaptureLaser2D* _capture_las
}*/
}
-void ProcessorTrackerLandmarkCorner::expectedFeature(LandmarkBase* _landmark_ptr, Eigen::Vector4s& expected_feature_,
+void ProcessorTrackerLandmarkCorner::expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::Vector4s& expected_feature_,
Eigen::Matrix3s& expected_feature_cov_)
{
//std::cout << "ProcessorTrackerLandmarkCorner::expectedFeature: " << std::endl;
@@ -296,7 +296,7 @@ void ProcessorTrackerLandmarkCorner::expectedFeature(LandmarkBase* _landmark_ptr
// ------------ expected feature covariance
Eigen::MatrixXs Sigma = Eigen::MatrixXs::Zero(6, 6);
// closer keyframe with computed covariance
- FrameBase* key_frame_ptr = (origin_ptr_ != nullptr ? origin_ptr_->getFramePtr() : nullptr);
+ FrameBasePtr key_frame_ptr = (origin_ptr_ != nullptr ? origin_ptr_->getFramePtr() : nullptr);
// If all covariance blocks are stored wolfproblem (filling upper diagonal only)
if (key_frame_ptr != nullptr &&
// Sigma_rr
@@ -328,7 +328,7 @@ void ProcessorTrackerLandmarkCorner::expectedFeature(LandmarkBase* _landmark_ptr
expected_feature_cov_ = Eigen::Matrix3s::Identity()*0.1;
}
-Eigen::VectorXs ProcessorTrackerLandmarkCorner::computeSquaredMahalanobisDistances(const FeatureBase* _feature_ptr,
+Eigen::VectorXs ProcessorTrackerLandmarkCorner::computeSquaredMahalanobisDistances(const FeatureBasePtr _feature_ptr,
const Eigen::Vector4s& _expected_feature,
const Eigen::Matrix3s& _expected_feature_cov,
const Eigen::MatrixXs& _mu)
@@ -357,7 +357,7 @@ Eigen::VectorXs ProcessorTrackerLandmarkCorner::computeSquaredMahalanobisDistanc
return squared_mahalanobis_distances;
}
-ProcessorBase* ProcessorTrackerLandmarkCorner::create(const std::string& _unique_name, const ProcessorParamsBase* _params)
+ProcessorBasePtr ProcessorTrackerLandmarkCorner::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params)
{
ProcessorParamsLaser* params = (ProcessorParamsLaser*)_params;
ProcessorTrackerLandmarkCorner* prc_ptr = new ProcessorTrackerLandmarkCorner(params->line_finder_params_, params->new_corners_th, params->loop_frames_th);
diff --git a/src/processor_tracker_landmark_corner.h b/src/processor_tracker_landmark_corner.h
index 7dcc7d7bd..3340946ed 100644
--- a/src/processor_tracker_landmark_corner.h
+++ b/src/processor_tracker_landmark_corner.h
@@ -145,7 +145,7 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
*
* Implement in derived classes to build the type of landmark you need for this tracker.
*/
- virtual LandmarkBase* createLandmark(FeatureBase* _feature_ptr);
+ virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
/** \brief Create a new constraint
* \param _feature_ptr pointer to the Feature to constrain
@@ -156,22 +156,22 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
* TODO: Make a general ConstraintFactory, and put it in WolfProblem.
* This factory only needs to know the two derived pointers to decide on the actual Constraint created.
*/
- virtual ConstraintBase* createConstraint(FeatureBase* _feature_ptr, LandmarkBase* _landmark_ptr);
+ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
private:
void extractCorners(CaptureLaser2D* _capture_laser_ptr, FeatureBaseList& _corner_list);
- void expectedFeature(LandmarkBase* _landmark_ptr, Eigen::Vector4s& expected_feature_,
+ void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::Vector4s& expected_feature_,
Eigen::Matrix3s& expected_feature_cov_);
- Eigen::VectorXs computeSquaredMahalanobisDistances(const FeatureBase* _feature_ptr,
+ Eigen::VectorXs computeSquaredMahalanobisDistances(const FeatureBasePtr _feature_ptr,
const Eigen::Vector4s& _expected_feature,
const Eigen::Matrix3s& _expected_feature_cov,
const Eigen::MatrixXs& _mu);
// Factory method
public:
- static ProcessorBase* create(const std::string& _unique_name, const ProcessorParamsBase* _params);
+ static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
};
inline ProcessorTrackerLandmarkCorner::ProcessorTrackerLandmarkCorner(const laserscanutils::LineFinderIterativeParams& _line_finder_params,
@@ -201,7 +201,7 @@ inline unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsi
return new_features_last_.size();
}
-inline LandmarkBase* ProcessorTrackerLandmarkCorner::createLandmark(FeatureBase* _feature_ptr)
+inline LandmarkBasePtr ProcessorTrackerLandmarkCorner::createLandmark(FeatureBasePtr _feature_ptr)
{
//std::cout << "ProcessorTrackerLandmarkCorner::createLandmark" << std::endl;
@@ -212,7 +212,7 @@ inline LandmarkBase* ProcessorTrackerLandmarkCorner::createLandmark(FeatureBase*
new StateBlock(feature_global_pose.tail(1)), _feature_ptr->getMeasurement()(3));
}
-inline ConstraintBase* ProcessorTrackerLandmarkCorner::createConstraint(FeatureBase* _feature_ptr, LandmarkBase* _landmark_ptr)
+inline ConstraintBasePtr ProcessorTrackerLandmarkCorner::createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
{
assert(_feature_ptr != nullptr && _landmark_ptr != nullptr && "ProcessorTrackerLandmarkCorner::createConstraint: feature and landmark pointers can not be nullptr!");
diff --git a/src/processor_tracker_landmark_dummy.cpp b/src/processor_tracker_landmark_dummy.cpp
index e966d9d88..c78b2bb43 100644
--- a/src/processor_tracker_landmark_dummy.cpp
+++ b/src/processor_tracker_landmark_dummy.cpp
@@ -71,13 +71,13 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int
return new_features_last_.size();
}
-LandmarkBase* ProcessorTrackerLandmarkDummy::createLandmark(FeatureBase* _feature_ptr)
+LandmarkBasePtr ProcessorTrackerLandmarkDummy::createLandmark(FeatureBasePtr _feature_ptr)
{
//std::cout << "ProcessorTrackerLandmarkDummy::createLandmark" << std::endl;
return new LandmarkCorner2D(new StateBlock(2), new StateBlock(1), _feature_ptr->getMeasurement(0));
}
-ConstraintBase* ProcessorTrackerLandmarkDummy::createConstraint(FeatureBase* _feature_ptr, LandmarkBase* _landmark_ptr)
+ConstraintBasePtr ProcessorTrackerLandmarkDummy::createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
{
std::cout << "\tProcessorTrackerLandmarkDummy::createConstraint" << std::endl;
std::cout << "\t\tfeature " << _feature_ptr->getMeasurement() << std::endl;
diff --git a/src/processor_tracker_landmark_dummy.h b/src/processor_tracker_landmark_dummy.h
index f793846b7..92a21ffdb 100644
--- a/src/processor_tracker_landmark_dummy.h
+++ b/src/processor_tracker_landmark_dummy.h
@@ -59,7 +59,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
*
* Implement in derived classes to build the type of landmark you need for this tracker.
*/
- virtual LandmarkBase* createLandmark(FeatureBase* _feature_ptr);
+ virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
/** \brief Create a new constraint
* \param _feature_ptr pointer to the Feature to constrain
@@ -70,7 +70,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
* TODO: Make a general ConstraintFactory, and put it in WolfProblem.
* This factory only needs to know the two derived pointers to decide on the actual Constraint created.
*/
- virtual ConstraintBase* createConstraint(FeatureBase* _feature_ptr, LandmarkBase* _landmark_ptr);
+ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
};
inline void ProcessorTrackerLandmarkDummy::postProcess()
diff --git a/src/processor_tracker_landmark_polyline.cpp b/src/processor_tracker_landmark_polyline.cpp
index 874120d09..29342a637 100644
--- a/src/processor_tracker_landmark_polyline.cpp
+++ b/src/processor_tracker_landmark_polyline.cpp
@@ -58,8 +58,8 @@ unsigned int ProcessorTrackerLandmarkPolyline::findLandmarks(const LandmarkBaseL
//std::cout << "ProcessorTrackerLandmarkPolyline::findLandmarks: " << _landmarks_searched.size() << " features: " << polylines_incoming_.size() << std::endl;
// COMPUTING ALL EXPECTED FEATURES
- std::map<LandmarkBase*, Eigen::MatrixXs> expected_features;
- std::map<LandmarkBase*, Eigen::MatrixXs> expected_features_covs;
+ std::map<LandmarkBasePtr, Eigen::MatrixXs> expected_features;
+ std::map<LandmarkBasePtr, Eigen::MatrixXs> expected_features_covs;
for (auto landmark : _landmarks_searched)
if (landmark->getTypeId() == LANDMARK_POLYLINE_2D)
{
@@ -334,7 +334,7 @@ void ProcessorTrackerLandmarkPolyline::extractPolylines(CaptureLaser2D* _capture
//std::cout << _polyline_list.size() << " polylines extracted" << std::endl;
}
-void ProcessorTrackerLandmarkPolyline::expectedFeature(LandmarkBase* _landmark_ptr, Eigen::MatrixXs& expected_feature_,
+void ProcessorTrackerLandmarkPolyline::expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
Eigen::MatrixXs& expected_feature_cov_)
{
assert(_landmark_ptr->getTypeId() == LANDMARK_POLYLINE_2D && "ProcessorTrackerLandmarkPolyline::expectedFeature: Bad landmark type");
@@ -482,7 +482,7 @@ void ProcessorTrackerLandmarkPolyline::createNewLandmarks(LandmarkBaseList& _new
{
//std::cout << "ProcessorTrackerLandmarkPolyline::createNewLandmarks" << std::endl;
FeaturePolyline2D* polyline_ft_ptr;
- LandmarkBase* new_lmk_ptr;
+ LandmarkBasePtr new_lmk_ptr;
for (auto new_feature_ptr : new_features_last_)
{
// create new landmark
@@ -504,7 +504,7 @@ void ProcessorTrackerLandmarkPolyline::createNewLandmarks(LandmarkBaseList& _new
}
}
-LandmarkBase* ProcessorTrackerLandmarkPolyline::createLandmark(FeatureBase* _feature_ptr)
+LandmarkBasePtr ProcessorTrackerLandmarkPolyline::createLandmark(FeatureBasePtr _feature_ptr)
{
assert(_feature_ptr->getTypeId() == FEATURE_POLYLINE_2D);
//std::cout << "ProcessorTrackerLandmarkPolyline::createLandmark" << std::endl;
@@ -1001,13 +1001,13 @@ void ProcessorTrackerLandmarkPolyline::postProcess()
classifyPolilines(getProblem()->getMapPtr()->getLandmarkListPtr());
}
-ConstraintBase* ProcessorTrackerLandmarkPolyline::createConstraint(FeatureBase* _feature_ptr, LandmarkBase* _landmark_ptr)
+ConstraintBasePtr ProcessorTrackerLandmarkPolyline::createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
{
// not used
return nullptr;
}
-ProcessorBase* ProcessorTrackerLandmarkPolyline::create(const std::string& _unique_name, const ProcessorParamsBase* _params)
+ProcessorBasePtr ProcessorTrackerLandmarkPolyline::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params)
{
ProcessorParamsPolyline* params = (ProcessorParamsPolyline*)_params;
ProcessorTrackerLandmarkPolyline* prc_ptr = new ProcessorTrackerLandmarkPolyline(*params);
diff --git a/src/processor_tracker_landmark_polyline.h b/src/processor_tracker_landmark_polyline.h
index eb4211c74..1652cc05d 100644
--- a/src/processor_tracker_landmark_polyline.h
+++ b/src/processor_tracker_landmark_polyline.h
@@ -134,7 +134,7 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
*
* Implement in derived classes to build the type of landmark you need for this tracker.
*/
- virtual LandmarkBase* createLandmark(FeatureBase* _feature_ptr);
+ virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
/** \brief Establish constraints between features in Captures \b last and \b origin
*/
@@ -153,13 +153,13 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
* TODO: Make a general ConstraintFactory, and put it in WolfProblem. JV: I disagree..
* This factory only needs to know the two derived pointers to decide on the actual Constraint created.
*/
- virtual ConstraintBase* createConstraint(FeatureBase* _feature_ptr, LandmarkBase* _landmark_ptr);
+ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
private:
void extractPolylines(CaptureLaser2D* _capture_laser_ptr, FeatureBaseList& _polyline_list);
- void expectedFeature(LandmarkBase* _landmark_ptr, Eigen::MatrixXs& expected_feature_,
+ void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
Eigen::MatrixXs& expected_feature_cov_);
Eigen::VectorXs computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
@@ -171,7 +171,7 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
bool _A_extreme, bool _B_extreme);
// Factory method
public:
- static ProcessorBase* create(const std::string& _unique_name, const ProcessorParamsBase* _params);
+ static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
};
inline ProcessorTrackerLandmarkPolyline::ProcessorTrackerLandmarkPolyline(const ProcessorParamsPolyline& _params) :
diff --git a/src/sensor_base.h b/src/sensor_base.h
index a1957f7e6..d735c76e3 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -30,8 +30,8 @@ struct IntrinsicsBase
class SensorBase : public NodeBase // NodeLinked<HardwareBase, ProcessorBase>
{
private:
- Problem* problem_ptr_;
- HardwareBase* hardware_ptr_;
+ ProblemPtr problem_ptr_;
+ HardwareBasePtr hardware_ptr_;
ProcessorBaseList processor_list_;
static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory)
@@ -97,7 +97,7 @@ class SensorBase : public NodeBase // NodeLinked<HardwareBase, ProcessorBase>
SensorType typeId();
- ProcessorBase* addProcessor(ProcessorBase* _proc_ptr);
+ ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
void removeProcessor(ProcessorBasePtr _prc_ptr);
ProcessorBaseList* getProcessorListPtr();
@@ -127,8 +127,8 @@ class SensorBase : public NodeBase // NodeLinked<HardwareBase, ProcessorBase>
Eigen::MatrixXs getNoiseCov();
- Problem* getProblem(){return problem_ptr_;}
- void setProblem(Problem* _prob_ptr){problem_ptr_ = _prob_ptr;}
+ ProblemPtr getProblem(){return problem_ptr_;}
+ void setProblem(ProblemPtr _prob_ptr){problem_ptr_ = _prob_ptr;}
};
@@ -155,7 +155,7 @@ inline wolf::SensorType SensorBase::typeId()
return type_id_;
}
-inline ProcessorBase* SensorBase::addProcessor(ProcessorBase* _proc_ptr)
+inline ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
{
processor_list_.push_back(_proc_ptr);
_proc_ptr->setSensorPtr(this);
diff --git a/src/sensor_camera.cpp b/src/sensor_camera.cpp
index f0869d5b5..5c5d75f6f 100644
--- a/src/sensor_camera.cpp
+++ b/src/sensor_camera.cpp
@@ -40,9 +40,9 @@ SensorCamera::~SensorCamera()
}
// Define the factory method
-SensorBase* SensorCamera::create(const std::string& _unique_name, //
+SensorBasePtr SensorCamera::create(const std::string& _unique_name, //
const Eigen::VectorXs& _extrinsics_pq, //
- const IntrinsicsBase* _intrinsics)
+ const IntrinsicsBasePtr _intrinsics)
{
assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
diff --git a/src/sensor_camera.h b/src/sensor_camera.h
index d03a615b3..844ecfdc5 100644
--- a/src/sensor_camera.h
+++ b/src/sensor_camera.h
@@ -57,9 +57,9 @@ class SensorCamera : public SensorBase
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
- static SensorBase* create(const std::string & _unique_name, //
+ static SensorBasePtr create(const std::string & _unique_name, //
const Eigen::VectorXs& _extrinsics, //
- const IntrinsicsBase* _intrinsics);
+ const IntrinsicsBasePtr _intrinsics);
};
diff --git a/src/sensor_factory.cpp b/src/sensor_factory.cpp
index ef7012390..0aa2def25 100644
--- a/src/sensor_factory.cpp
+++ b/src/sensor_factory.cpp
@@ -28,7 +28,7 @@ bool SensorFactory::unregisterCreator(const std::string& _sensor_type)
return callbacks_.erase(_sensor_type) == 1;
}
-SensorBase* SensorFactory::create(const std::string& _sensor_type, const std::string& _name, const Eigen::VectorXs& _extrinsics, const IntrinsicsBase* _intrinsics)
+SensorBasePtr SensorFactory::create(const std::string& _sensor_type, const std::string& _name, const Eigen::VectorXs& _extrinsics, const IntrinsicsBasePtr _intrinsics)
{
CallbackMap::const_iterator creator_callback_it = callbacks_.find(_sensor_type);
if (creator_callback_it == callbacks_.end())
diff --git a/src/sensor_factory.h b/src/sensor_factory.h
index 62c3ef6ce..f2d957cf1 100644
--- a/src/sensor_factory.h
+++ b/src/sensor_factory.h
@@ -78,11 +78,11 @@ namespace wolf
* The method SensorCamera::create() exists in the SensorCamera class as a static method.
* All these ````SensorXxx::create()```` methods need to have exactly the same API, regardless of the sensor type.
* This API includes a sensor name, a vector of extrinsic parameters,
- * and a pointer to a base struct of intrinsic parameters, IntrinsicsBase*,
+ * and a pointer to a base struct of intrinsic parameters, IntrinsicsBasePtr,
* that can be derived for each derived sensor:
*
* \code
- * static SensorBase* create(const std::string& _name, Eigen::VectorXs& _extrinsics_pq, IntrinsicsBase* _intrinsics)
+ * static SensorBasePtr create(const std::string& _name, Eigen::VectorXs& _extrinsics_pq, IntrinsicsBasePtr _intrinsics)
* \endcode
*
* See further down for an implementation example.
@@ -127,7 +127,7 @@ namespace wolf
* Here is an example of SensorCamera::create() extracted from sensor_camera.cpp:
*
* \code
- * static SensorBase* create(const std::string& _name, Eigen::VectorXs& _extrinsics_pq, IntrinsicsBase* _intrinsics)
+ * static SensorBasePtr create(const std::string& _name, Eigen::VectorXs& _extrinsics_pq, IntrinsicsBasePtr _intrinsics)
* {
* // check extrinsics vector
* assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
@@ -194,7 +194,7 @@ namespace wolf
* Eigen::VectorXs extrinsics_1(7); // give it some values...
* IntrinsicsCamera intrinsics_1({...}); // see IntrinsicsFactory to fill in the derived struct
*
- * SensorBase* camera_1_ptr =
+ * SensorBasePtr camera_1_ptr =
* SensorFactory::get().create ( "CAMERA" , "Front-left camera" , extrinsics_1 , &intrinsics_1 );
*
* // A second camera... with a different name!
@@ -202,7 +202,7 @@ namespace wolf
* Eigen::VectorXs extrinsics_2(7);
* IntrinsicsCamera intrinsics_2({...});
*
- * SensorBase* camera_2_ptr =
+ * SensorBasePtr camera_2_ptr =
* SensorFactory::get().create( "CAMERA" , "Front-right camera" , extrinsics_2 , &intrinsics_2 );
*
* return 0;
@@ -214,13 +214,13 @@ namespace wolf
class SensorFactory
{
public:
- typedef SensorBase* (*CreateSensorCallback)(const std::string & _unique_name, const Eigen::VectorXs& _extrinsics, const IntrinsicsBase* _intrinsics);
+ typedef SensorBasePtr (*CreateSensorCallback)(const std::string & _unique_name, const Eigen::VectorXs& _extrinsics, const IntrinsicsBasePtr _intrinsics);
private:
typedef std::map<std::string, CreateSensorCallback> CallbackMap;
public:
bool registerCreator(const std::string& _sensor_type, CreateSensorCallback createFn);
bool unregisterCreator(const std::string& _sensor_type);
- SensorBase* create(const std::string& _sensor_type, const std::string& _unique_name, const Eigen::VectorXs& _extrinsics, const IntrinsicsBase* _intrinsics = nullptr);
+ SensorBasePtr create(const std::string& _sensor_type, const std::string& _unique_name, const Eigen::VectorXs& _extrinsics, const IntrinsicsBasePtr _intrinsics = nullptr);
private:
CallbackMap callbacks_;
diff --git a/src/sensor_gps.cpp b/src/sensor_gps.cpp
index f9bfbbdb4..8fb5ba78a 100644
--- a/src/sensor_gps.cpp
+++ b/src/sensor_gps.cpp
@@ -55,14 +55,14 @@ void SensorGPS::registerNewStateBlocks()
}
// Define the factory method
-SensorBase* SensorGPS::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p,
- const IntrinsicsBase* _intrinsics)
+SensorBasePtr SensorGPS::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p,
+ const IntrinsicsBasePtr _intrinsics)
{
// decode extrinsics vector
assert(_extrinsics_p.size() == 3 && "Bad extrinsics vector length. Should be 3 for 3D.");
StateBlock* pos_ptr = new StateBlock(_extrinsics_p, true);
StateBlock* ori_ptr = nullptr;
- SensorBase* sen = new SensorGPS(pos_ptr, ori_ptr, nullptr, nullptr, nullptr); // TODO: how to init these last three pointers?
+ SensorBasePtr sen = new SensorGPS(pos_ptr, ori_ptr, nullptr, nullptr, nullptr); // TODO: how to init these last three pointers?
sen->setName(_unique_name);
return sen;
}
diff --git a/src/sensor_gps.h b/src/sensor_gps.h
index f812da30e..48b4500e7 100644
--- a/src/sensor_gps.h
+++ b/src/sensor_gps.h
@@ -48,7 +48,7 @@ public:
virtual void registerNewStateBlocks();
public:
- static SensorBase* create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p, const IntrinsicsBase* _intrinsics);
+ static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p, const IntrinsicsBasePtr _intrinsics);
};
diff --git a/src/sensor_gps_fix.cpp b/src/sensor_gps_fix.cpp
index 16c38d1f3..6de1dca6f 100644
--- a/src/sensor_gps_fix.cpp
+++ b/src/sensor_gps_fix.cpp
@@ -21,8 +21,8 @@ Scalar SensorGPSFix::getNoise() const
}
// Define the factory method
-SensorBase* SensorGPSFix::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics,
- const IntrinsicsBase* _intrinsics)
+SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics,
+ const IntrinsicsBasePtr _intrinsics)
{
assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
&& "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
diff --git a/src/sensor_gps_fix.h b/src/sensor_gps_fix.h
index 335a7b8b4..8336e7fa5 100644
--- a/src/sensor_gps_fix.h
+++ b/src/sensor_gps_fix.h
@@ -45,7 +45,7 @@ class SensorGPSFix : public SensorBase
Scalar getNoise() const;
public:
- static SensorBase* create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBase* _intrinsics);
+ static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics);
};
diff --git a/src/sensor_imu.cpp b/src/sensor_imu.cpp
index 80f98a4f6..b1a744f3c 100644
--- a/src/sensor_imu.cpp
+++ b/src/sensor_imu.cpp
@@ -18,8 +18,8 @@ SensorIMU::~SensorIMU()
}
// Define the factory method
-SensorBase* SensorIMU::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq,
- const IntrinsicsBase* _intrinsics)
+SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq,
+ const IntrinsicsBasePtr _intrinsics)
{
// decode extrinsics vector
assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
@@ -29,7 +29,7 @@ SensorBase* SensorIMU::create(const std::string& _unique_name, const Eigen::Vect
// cast instrinsics to good type and extract intrinsic vector
// IntrinsicsIMU* intrinsics = (IntrinsicsIMU*)((_intrinsics));
StateBlock* bias_ptr = new StateBlock(6, false); // We'll have the IMU biases here
- SensorBase* sen = new SensorIMU(pos_ptr, ori_ptr, bias_ptr);
+ SensorBasePtr sen = new SensorIMU(pos_ptr, ori_ptr, bias_ptr);
sen->setName(_unique_name);
return sen;
}
diff --git a/src/sensor_imu.h b/src/sensor_imu.h
index b6333282d..f5463dd06 100644
--- a/src/sensor_imu.h
+++ b/src/sensor_imu.h
@@ -37,7 +37,7 @@ class SensorIMU : public SensorBase
virtual ~SensorIMU();
public:
- static SensorBase* create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBase* _intrinsics = nullptr);
+ static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics = nullptr);
};
diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp
index 92e5cf14a..11a395eca 100644
--- a/src/sensor_laser_2D.cpp
+++ b/src/sensor_laser_2D.cpp
@@ -52,8 +52,8 @@ const laserscanutils::LaserScanParams& SensorLaser2D::getScanParams() const
}
// Define the factory method
-SensorBase* SensorLaser2D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po,
- const IntrinsicsBase* _intrinsics)
+SensorBasePtr SensorLaser2D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po,
+ const IntrinsicsBasePtr _intrinsics)
{
// decode extrinsics vector
assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
@@ -80,9 +80,9 @@ SensorBase* SensorLaser2D::create(const std::string& _unique_name, const Eigen::
//#include "yaml-cpp/yaml.h"
namespace wolf {
//// Yaml parser here !
-//IntrinsicsBase* createIntrinsicsLaser2D(const std::string& _filename_dot_yaml)
+//IntrinsicsBasePtr createIntrinsicsLaser2D(const std::string& _filename_dot_yaml)
//{
-// IntrinsicsBase* params; // dummy
+// IntrinsicsBasePtr params; // dummy
// return params;
//}
namespace
diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h
index 0d20cde0a..8020f8962 100644
--- a/src/sensor_laser_2D.h
+++ b/src/sensor_laser_2D.h
@@ -72,8 +72,8 @@ class SensorLaser2D : public SensorBase
const laserscanutils::LaserScanParams & getScanParams() const;
public:
- static SensorBase* create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po, const IntrinsicsBase* _intrinsics);
- static IntrinsicsBase* createParams(const std::string& _filename_dot_yaml);
+ static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po, const IntrinsicsBasePtr _intrinsics);
+ static IntrinsicsBasePtr createParams(const std::string& _filename_dot_yaml);
};
diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp
index e113d9c65..401e06c50 100644
--- a/src/sensor_odom_2D.cpp
+++ b/src/sensor_odom_2D.cpp
@@ -25,8 +25,8 @@ Scalar SensorOdom2D::getRotVarToRotNoiseFactor() const
}
// Define the factory method
-SensorBase* SensorOdom2D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po,
- const IntrinsicsBase* _intrinsics)
+SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po,
+ const IntrinsicsBasePtr _intrinsics)
{
// decode extrinsics vector
assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
@@ -34,7 +34,7 @@ SensorBase* SensorOdom2D::create(const std::string& _unique_name, const Eigen::V
StateBlock* ori_ptr = new StateBlock(_extrinsics_po.tail(1), true);
// cast intrinsics into derived type
IntrinsicsOdom2D* params = (IntrinsicsOdom2D*)(_intrinsics);
- SensorBase* odo = new SensorOdom2D(pos_ptr, ori_ptr, params->k_disp_to_disp, params->k_rot_to_rot);
+ SensorBasePtr odo = new SensorOdom2D(pos_ptr, ori_ptr, params->k_disp_to_disp, params->k_rot_to_rot);
odo->setName(_unique_name);
return odo;
}
diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h
index 4c583f1db..99dec762f 100644
--- a/src/sensor_odom_2D.h
+++ b/src/sensor_odom_2D.h
@@ -55,7 +55,7 @@ class SensorOdom2D : public SensorBase
public:
- static SensorBase* create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBase* _intrinsics);
+ static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics);
};
diff --git a/src/solver/qr_solver.h b/src/solver/qr_solver.h
index f9ff2d396..80f9e5954 100644
--- a/src/solver/qr_solver.h
+++ b/src/solver/qr_solver.h
@@ -38,13 +38,13 @@ namespace wolf
class SolverQR
{
protected:
- Problem* problem_ptr_;
+ ProblemPtr problem_ptr_;
Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::NaturalOrdering<int>> solver_;
Eigen::SparseMatrix<double> A_, R_;
Eigen::VectorXd b_, x_incr_;
std::vector<StateBlock*> nodes_;
- std::vector<ConstraintBase*> constraints_;
- std::vector<CostFunctionBase*> cost_functions_;
+ std::vector<ConstraintBasePtr> constraints_;
+ std::vector<CostFunctionBasePtr> cost_functions_;
// ordering
Eigen::SparseMatrix<int> A_nodes_;
@@ -61,7 +61,7 @@ class SolverQR
double time_ordering_, time_solving_, time_managing_;
public:
- SolverQR(Problem* problem_ptr_) :
+ SolverQR(ProblemPtr problem_ptr_) :
problem_ptr_(problem_ptr_), A_(0, 0), R_(0, 0), A_nodes_(0, 0), acc_node_permutation_(0), n_new_constraints_(
0), time_ordering_(0), time_solving_(0), time_managing_(0)
{
@@ -154,7 +154,7 @@ class SolverQR
//TODO
}
- void addConstraint(ConstraintBase* _constraint_ptr)
+ void addConstraint(ConstraintBasePtr _constraint_ptr)
{
std::cout << "adding constraint " << _constraint_ptr->nodeId() << std::endl;
t_managing_ = clock();
@@ -282,7 +282,7 @@ class SolverQR
unsigned int first_ordered_idx;
for (unsigned int i = 0; i < n_new_constraints_; i++)
{
- ConstraintBase* ct_ptr = constraints_.at(constraints_.size() - 1 - i);
+ ConstraintBasePtr ct_ptr = constraints_.at(constraints_.size() - 1 - i);
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++)
@@ -530,7 +530,7 @@ class SolverQR
return nodes_.size();
}
- CostFunctionBase* createCostFunction(ConstraintBase* _corrPtr)
+ CostFunctionBasePtr createCostFunction(ConstraintBasePtr _corrPtr)
{
//std::cout << "adding ctr " << _corrPtr->nodeId() << std::endl;
//_corrPtr->print();
@@ -540,7 +540,7 @@ class SolverQR
case CTR_GPS_FIX_2D:
{
ConstraintGPS2D* specific_ptr = (ConstraintGPS2D*)(_corrPtr);
- return (CostFunctionBase*)(new CostFunctionSparse<ConstraintGPS2D, specific_ptr->measurementSize,
+ return (CostFunctionBasePtr)(new CostFunctionSparse<ConstraintGPS2D, 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,
@@ -550,7 +550,7 @@ class SolverQR
case CTR_ODOM_2D:
{
ConstraintOdom2D* specific_ptr = (ConstraintOdom2D*)(_corrPtr);
- return (CostFunctionBase*)new CostFunctionSparse<ConstraintOdom2D, specific_ptr->measurementSize,
+ return (CostFunctionBasePtr)new CostFunctionSparse<ConstraintOdom2D, 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,
@@ -560,7 +560,7 @@ class SolverQR
case CTR_CORNER_2D:
{
ConstraintCorner2D* specific_ptr = (ConstraintCorner2D*)(_corrPtr);
- return (CostFunctionBase*)new CostFunctionSparse<ConstraintCorner2D, specific_ptr->measurementSize,
+ return (CostFunctionBasePtr)new CostFunctionSparse<ConstraintCorner2D, 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,
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 7e953d9db..cba97a342 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -15,7 +15,7 @@ void SolverManager::solve()
}
-//void SolverManager::computeCovariances(WolfProblem* _problem_ptr)
+//void SolverManager::computeCovariances(WolfProblemPtr _problem_ptr)
//{
//}
@@ -59,7 +59,7 @@ void SolverManager::update(const WolfProblemPtr _problem_ptr)
}
}
-void SolverManager::addConstraint(ConstraintBase* _corr_ptr)
+void SolverManager::addConstraint(ConstraintBasePtr _corr_ptr)
{
//TODO MatrixXs J; Vector e;
// getResidualsAndJacobian(_corr_ptr, J, e);
@@ -145,7 +145,7 @@ void SolverManager::updateStateUnitStatus(StateBlock* _st_ptr)
// _st_ptr->setPendingStatus(NOT_PENDING);
}
-ceres::CostFunction* SolverManager::createCostFunction(ConstraintBase* _corrPtr)
+ceres::CostFunction* SolverManager::createCostFunction(ConstraintBasePtr _corrPtr)
{
//std::cout << "adding ctr " << _corrPtr->nodeId() << std::endl;
//_corrPtr->print();
diff --git a/src/solver/solver_manager.h b/src/solver/solver_manager.h
index 05cd23265..5d3c3163e 100644
--- a/src/solver/solver_manager.h
+++ b/src/solver/solver_manager.h
@@ -29,11 +29,11 @@ class SolverManager
ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
- //void computeCovariances(WolfProblem* _problem_ptr);
+ //void computeCovariances(WolfProblemPtr _problem_ptr);
void update(const WolfProblemPtr _problem_ptr);
- void addConstraint(ConstraintBase* _corr_ptr);
+ void addConstraint(ConstraintBasePtr _corr_ptr);
void removeConstraint(const unsigned int& _corr_idx);
@@ -43,7 +43,7 @@ class SolverManager
void updateStateUnitStatus(StateBlock* _st_ptr);
- ceres::CostFunction* createCostFunction(ConstraintBase* _corrPtr);
+ ceres::CostFunction* createCostFunction(ConstraintBasePtr _corrPtr);
};
#endif
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index 78d8532da..e0b6d2df9 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -15,7 +15,7 @@ TrajectoryBase::~TrajectoryBase()
//std::cout << "deleting TrajectoryBase " << nodeId() << std::endl;
}
-FrameBase* TrajectoryBase::addFrame(FrameBase* _frame_ptr)
+FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
{
if (_frame_ptr->isKey())
{
@@ -42,12 +42,12 @@ void TrajectoryBase::getConstraintList(ConstraintBaseList & _ctr_list)
(*fr_it)->getConstraintList(_ctr_list);
}
-void TrajectoryBase::sortFrame(FrameBase* _frame_ptr)
+void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
{
moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr));
}
-FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBase* _frame_ptr)
+FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
{
for (auto frm_rit = getFrameListPtr()->rbegin(); frm_rit != getFrameListPtr()->rend(); frm_rit++)
if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKey() && (*frm_rit)->getTimeStamp() < _frame_ptr->getTimeStamp())
@@ -55,9 +55,9 @@ FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBase* _frame_ptr)
return getFrameListPtr()->begin();
}
-FrameBase* TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
+FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
{
- FrameBase* closest_kf = nullptr;
+ FrameBasePtr closest_kf = nullptr;
Scalar min_dt = 1e9;
for (auto frm_rit = getFrameListPtr()->rbegin(); frm_rit != getFrameListPtr()->rend(); frm_rit++)
diff --git a/src/trajectory_base.h b/src/trajectory_base.h
index cedf122a9..43ee6bd8a 100644
--- a/src/trajectory_base.h
+++ b/src/trajectory_base.h
@@ -23,11 +23,11 @@ class TrajectoryBase : public NodeBase //: public NodeBase // NodeLinked<Problem
{
private:
ProblemPtr problem_ptr_;
- std::list<FrameBase*> frame_list_;
+ std::list<FrameBasePtr> frame_list_;
protected:
FrameStructure frame_structure_; // Defines the structure of the Frames in the Trajectory.
- FrameBase* last_key_frame_ptr_; // keeps pointer to the last key frame
+ FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame
public:
TrajectoryBase(FrameStructure _frame_sturcture);
@@ -42,7 +42,7 @@ class TrajectoryBase : public NodeBase //: public NodeBase // NodeLinked<Problem
/** \brief Add a frame to the trajectory
**/
- FrameBase* addFrame(FrameBase* _frame_ptr);
+ FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
/** \brief Remove a frame to the trajectory
**/
@@ -55,15 +55,15 @@ class TrajectoryBase : public NodeBase //: public NodeBase // NodeLinked<Problem
/** \brief Returns a pointer to last frame
**/
- FrameBase* getLastFramePtr();
+ FrameBasePtr getLastFramePtr();
/** \brief Returns a pointer to last key frame
*/
- FrameBase* getLastKeyFramePtr();
+ FrameBasePtr getLastKeyFramePtr();
/** \brief Sets the pointer to last key frame
*/
- void setLastKeyFramePtr(FrameBase* _key_frame_ptr);
+ void setLastKeyFramePtr(FrameBasePtr _key_frame_ptr);
/** \brief Returns a list of all constraints in the trajectory thru reference
**/
@@ -75,7 +75,7 @@ class TrajectoryBase : public NodeBase //: public NodeBase // NodeLinked<Problem
/** \brief Sorts the frame by timestamp
**/
- void sortFrame(FrameBase* _frame_iter);
+ void sortFrame(FrameBasePtr _frame_iter);
void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place)
{
@@ -88,14 +88,14 @@ class TrajectoryBase : public NodeBase //: public NodeBase // NodeLinked<Problem
/** \brief Compute the position where the frame should be
**/
- FrameBaseIter computeFrameOrder(FrameBase* _frame_ptr);
+ FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr);
/** \brief Finds the closes key frame to a given timestamp
**/
- FrameBase* closestKeyFrameToTimeStamp(const TimeStamp& _ts);
+ FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
- Problem* getProblem(){return problem_ptr_;}
- void setProblem(Problem* _prob_ptr){problem_ptr_ = _prob_ptr;}
+ ProblemPtr getProblem(){return problem_ptr_;}
+ void setProblem(ProblemPtr _prob_ptr){problem_ptr_ = _prob_ptr;}
};
@@ -124,18 +124,18 @@ inline FrameBaseList* TrajectoryBase::getFrameListPtr()
return & frame_list_;
}
-inline FrameBase* TrajectoryBase::getLastFramePtr()
+inline FrameBasePtr TrajectoryBase::getLastFramePtr()
{
// return getDownNodeListPtr()->back();
return frame_list_.back();
}
-inline FrameBase* TrajectoryBase::getLastKeyFramePtr()
+inline FrameBasePtr TrajectoryBase::getLastKeyFramePtr()
{
return last_key_frame_ptr_;
}
-inline void TrajectoryBase::setLastKeyFramePtr(FrameBase* _key_frame_ptr)
+inline void TrajectoryBase::setLastKeyFramePtr(FrameBasePtr _key_frame_ptr)
{
last_key_frame_ptr_ = _key_frame_ptr;
}
diff --git a/src/wolf.h b/src/wolf.h
index 84547344c..f19dd7eb9 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -283,8 +283,10 @@ typedef enum
/////////////////////////////////////////////////////////////////////////
// - forwards for pointers
-class NodeTerminus;
class Problem;
+class HardwareBase;
+class IntrinsicsBase;
+class ProcessorParamsBase;
class MapBase;
class LandmarkBase;
class LandmarkCorner2D;
@@ -302,21 +304,29 @@ class SensorLaser2D;
class TransSensor;
class ProcessorBase;
class StateBlock;
+class LocalParametrizationBase;
+class NodeBase;
// TODO: No seria millor que cada classe es defineixi aquests typedefs?
+// NodeBase
+typedef NodeBase* NodeBasePtr;
+
//Problem
typedef Problem* ProblemPtr;
+// Hardware
+typedef HardwareBase* HardwareBasePtr;
+
//Map
typedef MapBase* MapBasePtr;
-typedef std::list<MapBase*> MapBaseList;
+typedef std::list<MapBasePtr> MapBaseList;
typedef MapBaseList::iterator MapBaseIter;
//Landmark
typedef LandmarkBase* LandmarkBasePtr;
-typedef std::list<LandmarkBase*> LandmarkBaseList;
+typedef std::list<LandmarkBasePtr> LandmarkBaseList;
typedef LandmarkBaseList::iterator LandmarkBaseIter;
//Landmark corner 2D
@@ -363,6 +373,9 @@ typedef SensorBase* SensorBasePtr;
typedef std::list<SensorBase*> SensorBaseList;
typedef SensorBaseList::iterator SensorBaseIter;
+// Intrinsics
+typedef IntrinsicsBase* IntrinsicsBasePtr;
+
// - transSensor
typedef std::map<unsigned int, TransSensor*> TransSensorMap;
typedef TransSensorMap::iterator TransSensorIter;
@@ -372,30 +385,36 @@ typedef ProcessorBase* ProcessorBasePtr;
typedef std::list<ProcessorBase*> ProcessorBaseList;
typedef ProcessorBaseList::iterator ProcessorBaseIter;
+// Processor params
+typedef ProcessorParamsBase* ProcessorParamsBasePtr;
+
// - State
typedef std::list<StateBlock*> StateBlockList;
typedef StateBlockList::iterator StateBlockIter;
+// Local Parametrization
+typedef LocalParametrizationBase* LocalParametrizationBasePtr;
+
// Match Feature - Landmark
struct LandmarkMatch
{
- LandmarkBase* landmark_ptr_;
+ LandmarkBasePtr landmark_ptr_;
Scalar normalized_score_;
};
// Match map Feature - Landmark
-typedef std::map<FeatureBase*, LandmarkMatch*> LandmarkMatchMap;
+typedef std::map<FeatureBasePtr, LandmarkMatch*> LandmarkMatchMap;
// Feature-Feature correspondence
struct FeatureMatch
{
- FeatureBase* feature_ptr_;
+ FeatureBasePtr feature_ptr_;
Scalar normalized_score_;
};
-typedef std::map<FeatureBase*, FeatureMatch> FeatureMatchMap;
+typedef std::map<FeatureBasePtr, FeatureMatch> FeatureMatchMap;
diff --git a/src/yaml/processor_image_yaml.cpp b/src/yaml/processor_image_yaml.cpp
index acfb53a4a..e1b43731e 100644
--- a/src/yaml/processor_image_yaml.cpp
+++ b/src/yaml/processor_image_yaml.cpp
@@ -19,7 +19,7 @@ namespace wolf
{
namespace
{
-static ProcessorParamsBase* createProcessorParamsImage(const std::string & _filename_dot_yaml)
+static ProcessorParamsBasePtr createProcessorParamsImage(const std::string & _filename_dot_yaml)
{
using std::string;
using YAML::Node;
diff --git a/src/yaml/sensor_camera_yaml.cpp b/src/yaml/sensor_camera_yaml.cpp
index 4eb0813e2..72c13444d 100644
--- a/src/yaml/sensor_camera_yaml.cpp
+++ b/src/yaml/sensor_camera_yaml.cpp
@@ -20,7 +20,7 @@ namespace wolf
namespace
{
-static IntrinsicsBase* createIntrinsicsCamera(const std::string & _filename_dot_yaml)
+static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_dot_yaml)
{
YAML::Node camera_config = YAML::LoadFile(_filename_dot_yaml);
diff --git a/src/yaml/sensor_laser_2D_yaml.cpp b/src/yaml/sensor_laser_2D_yaml.cpp
index 1d55f3e6c..7be17f29c 100644
--- a/src/yaml/sensor_laser_2D_yaml.cpp
+++ b/src/yaml/sensor_laser_2D_yaml.cpp
@@ -21,7 +21,7 @@ namespace wolf
{
namespace {
// intrinsics creator
-IntrinsicsBase* createIntrinsicsLaser2D(const std::string& _filename_dot_yaml)
+IntrinsicsBasePtr createIntrinsicsLaser2D(const std::string& _filename_dot_yaml)
{
// TODO: Parse YAML <-- maybe we want this out of this file?
IntrinsicsLaser2D* params; // dummy
--
GitLab