All state blocks are shared_ptr'ed

src/frame_base.cpp

@@ -115,7 +115,6 @@ void FrameBase::removeStateBlocks()
             {
                 getProblem()->removeStateBlockPtr(sbp);
             }
-//            delete sbp;
             setStateBlockPtr(i, nullptr);
         }
     }

src/frame_imu.cpp

@@ -7,7 +7,7 @@
 
 namespace wolf {
 
-FrameIMU::FrameIMU(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _v_ptr, StateQuaternion* _q_ptr,
+FrameIMU::FrameIMU(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _v_ptr, StateQuaternionPtr _q_ptr,
                    StateBlockPtr _ba_ptr, StateBlockPtr _bg_ptr) :
         FrameBase(_ts, _p_ptr, (StateBlockPtr)((_q_ptr)), _v_ptr)//,
 {
@@ -17,7 +17,7 @@ FrameIMU::FrameIMU(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _v_
 }
 
 FrameIMU::FrameIMU(const FrameKeyType& _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _v_ptr,
-                   StateQuaternion* _q_ptr, StateBlockPtr _ba_ptr, StateBlockPtr _bg_ptr) :
+                   StateQuaternionPtr _q_ptr, StateBlockPtr _ba_ptr, StateBlockPtr _bg_ptr) :
         FrameBase(_tp, _ts, _p_ptr, (StateBlockPtr)((_q_ptr)), _v_ptr)
 {
     setStateBlockPtr(3, std::make_shared<StateBlock>(3)); // acc bias

src/frame_imu.h

@@ -32,7 +32,7 @@ namespace wolf {
            * \param _ba_ptr StateBlock pointer to the acceleration bias (default: nullptr).
            * \param _bg_ptr StateBlock pointer to the gyrometer bias (default: nullptr).
            **/
-        FrameIMU(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _v_ptr = nullptr, StateQuaternion* _q_ptr =
+        FrameIMU(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _v_ptr = nullptr, StateQuaternionPtr _q_ptr =
                          nullptr,
                  StateBlockPtr _ba_ptr = nullptr, StateBlockPtr _bg_ptr = nullptr);
 
@@ -48,7 +48,7 @@ namespace wolf {
            * \param _bg_ptr StateBlock pointer to the gyrometer bias (default: nullptr).
            **/
         FrameIMU(const FrameKeyType& _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _v_ptr = nullptr,
-                 StateQuaternion* _q_ptr = nullptr, StateBlockPtr _ba_ptr = nullptr, StateBlockPtr _bg_ptr = nullptr);
+                 StateQuaternionPtr _q_ptr = nullptr, StateBlockPtr _ba_ptr = nullptr, StateBlockPtr _bg_ptr = nullptr);
 
           /** \brief Constructor with type, time stamp and state vector
           * \param _tp indicates frame type. Generally either NON_KEY_FRAME or KEY_FRAME. (types defined at wolf.h)

src/landmark_AHP.cpp

@@ -11,7 +11,7 @@ LandmarkAHP::LandmarkAHP(Eigen::Vector4s _position_homogeneous,
                          FrameBasePtr _anchor_frame,
                          SensorBasePtr _anchor_sensor,
                          cv::Mat _2D_descriptor) :
-    LandmarkBase(LANDMARK_AHP, "AHP", new StateHomogeneous3D(_position_homogeneous)),
+    LandmarkBase(LANDMARK_AHP, "AHP", std::make_shared<StateHomogeneous3D>(_position_homogeneous)),
     cv_descriptor_(_2D_descriptor.clone()),
     anchor_frame_(_anchor_frame),
     anchor_sensor_(_anchor_sensor)

src/landmark_base.cpp

@@ -152,7 +152,6 @@ void LandmarkBase::removeStateBlocks()
             {
                 getProblem()->removeStateBlockPtr(sbp);
             }
-//            delete sbp;
             setStateBlockPtr(i, nullptr);
         }
     }

src/landmark_polyline_2D.cpp

@@ -24,7 +24,7 @@ LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_pt
     //std::cout << "LandmarkPolyline2D::LandmarkPolyline2D" << std::endl;
     assert(_points.cols() >= 2 && "LandmarkPolyline2D::LandmarkPolyline2D: 2 points at least needed.");
     for (auto i = 0; i < _points.cols(); i++)
-        point_state_ptr_vector_.push_back(new StateBlock(_points.col(i).head<2>()));
+        point_state_ptr_vector_.push_back(std::make_shared<StateBlock>(_points.col(i).head<2>()));
 
     if (!first_defined_)
         point_state_ptr_vector_.front()->setLocalParametrizationPtr(new LocalParametrizationPolylineExtreme(point_state_ptr_vector_[1]));
@@ -106,7 +106,7 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de
     }
     else
     {
-        point_state_ptr_vector_.push_front(new StateBlock(_point.head<2>(), false,
+        point_state_ptr_vector_.push_front(std::make_shared<StateBlock>(_point.head<2>(), false,
                                                           (!_defined ?
                                                                   new LocalParametrizationPolylineExtreme(point_state_ptr_vector_.front()) :
                                                                   nullptr)));
@@ -136,7 +136,7 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne
     {
         for (int i = _idx; i < _points.cols(); i++)
         {
-        	point_state_ptr_vector_.push_back(new StateBlock(_points.block(0,i,2,1),
+        	point_state_ptr_vector_.push_back(std::make_shared<StateBlock>(_points.block(0,i,2,1),
         													 false,
         													 (i == _points.cols()-1 && !_defined ?
         															 new LocalParametrizationPolylineExtreme(point_state_ptr_vector_.back()) :
@@ -151,7 +151,7 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne
     {
         for (int i = _idx; i >= 0; i--)
         {
-        	point_state_ptr_vector_.push_front(new StateBlock(_points.block(0,i,2,1),
+        	point_state_ptr_vector_.push_front(std::make_shared<StateBlock>(_points.block(0,i,2,1),
         													  false,
         													  (i == 0 && !_defined ?
         															  new LocalParametrizationPolylineExtreme(point_state_ptr_vector_.front()) :
@@ -307,7 +307,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
         getProblem()->removeStateBlockPtr(remove_state);
     std::cout << "state removed " << std::endl;
 
-    delete remove_state;
+//    delete remove_state;
     std::cout << "state deleted " << std::endl;
 
     // remove element from deque
@@ -344,7 +344,7 @@ LandmarkBasePtr LandmarkPolyline2D::create(const YAML::Node& _lmk_node)
     }
 
     // Create a new landmark
-    std::shared_ptr<LandmarkPolyline2D> lmk_ptr = std::make_shared<LandmarkPolyline2D>(new StateBlock(pos, pos_fixed), new StateBlock(ori, ori_fixed), points, first_defined, last_defined, first_id, classification);
+    std::shared_ptr<LandmarkPolyline2D> lmk_ptr = std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(pos, pos_fixed), std::make_shared<StateBlock>(ori, ori_fixed), points, first_defined, last_defined, first_id, classification);
     lmk_ptr->setId(id);
 
     // fix all points

src/problem.cpp

@@ -189,22 +189,22 @@ FrameBasePtr Problem::createFrame(FrameKeyType _frame_key_type, const Eigen::Vec
         case FRM_PO_2D:
         {
             assert(_frame_state.size() == 3 && "Wrong state vector size");
-            return trajectory_ptr_->addFrame(std::make_shared<FrameBase>(_frame_key_type, _time_stamp, new StateBlock(_frame_state.head(2)),
-                                  new StateBlock(_frame_state.tail(1))));
+            return trajectory_ptr_->addFrame(std::make_shared<FrameBase>(_frame_key_type, _time_stamp, std::make_shared<StateBlock>(_frame_state.head(2)),
+                                  std::make_shared<StateBlock>(_frame_state.tail(1))));
         }
         case FRM_PO_3D:
         {
             assert(_frame_state.size() == 7 && "Wrong state vector size");
-            return trajectory_ptr_->addFrame(std::make_shared<FrameBase>(_frame_key_type, _time_stamp, new StateBlock(_frame_state.head(3)),
-                                  new StateQuaternion(_frame_state.tail(4))));
+            return trajectory_ptr_->addFrame(std::make_shared<FrameBase>(_frame_key_type, _time_stamp, std::make_shared<StateBlock>(_frame_state.head(3)),
+                                  std::make_shared<StateQuaternion>(_frame_state.tail(4))));
         }
         case FRM_POV_3D:
         {
             assert(_frame_state.size() == 10 && "Wrong state vector size");
             std::cout << __FILE__ << ":" << __FUNCTION__ << "():" << __LINE__ << std::endl;
-            return trajectory_ptr_->addFrame(std::make_shared<FrameBase>(_frame_key_type, _time_stamp, new StateBlock(_frame_state.head(3)),
-                                  new StateQuaternion(_frame_state.segment<4>(3)),
-                                  new StateBlock(_frame_state.tail(3))));
+            return trajectory_ptr_->addFrame(std::make_shared<FrameBase>(_frame_key_type, _time_stamp, std::make_shared<StateBlock>(_frame_state.head(3)),
+                                  std::make_shared<StateQuaternion>(_frame_state.segment<4>(3)),
+                                  std::make_shared<StateBlock>(_frame_state.tail(3))));
         }
         case FRM_PVQBB_3D:
         {

src/processor_tracker_feature_corner.cpp

@@ -103,8 +103,8 @@ ConstraintBasePtr ProcessorTrackerFeatureCorner::createConstraint(FeatureBasePtr
     {
         // Create new landmark
         Eigen::Vector3s feature_global_pose = R_world_sensor_ * _feature_ptr->getMeasurement() + t_world_sensor_;
-        landmark_ptr = std::make_shared<LandmarkCorner2D>(new StateBlock(feature_global_pose.head(2)),
-                                                          new StateBlock(feature_global_pose.tail(1)),
+        landmark_ptr = std::make_shared<LandmarkCorner2D>(std::make_shared<StateBlock>(feature_global_pose.head(2)),
+                                                          std::make_shared<StateBlock>(feature_global_pose.tail(1)),
                                                           _feature_ptr->getMeasurement()(3));
 
         // Add landmark constraint to the other feature

src/processor_tracker_landmark_corner.h

@@ -210,8 +210,8 @@ inline LandmarkBasePtr ProcessorTrackerLandmarkCorner::createLandmark(FeatureBas
     // compute feature global pose
     Eigen::Vector3s feature_global_pose = R_world_sensor_ * _feature_ptr->getMeasurement().head<3>() + t_world_sensor_;
     // Create new landmark
-    return std::make_shared<LandmarkCorner2D>(new StateBlock(feature_global_pose.head(2)),
-                                              new StateBlock(feature_global_pose.tail(1)),
+    return std::make_shared<LandmarkCorner2D>(std::make_shared<StateBlock>(feature_global_pose.head(2)),
+                                              std::make_shared<StateBlock>(feature_global_pose.tail(1)),
                                               _feature_ptr->getMeasurement()(3));
 }
 

src/processor_tracker_landmark_dummy.cpp

@@ -78,7 +78,7 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int
 LandmarkBasePtr ProcessorTrackerLandmarkDummy::createLandmark(FeatureBasePtr _feature_ptr)
 {
     //std::cout << "ProcessorTrackerLandmarkDummy::createLandmark" << std::endl;
-    return std::make_shared<LandmarkCorner2D>(new StateBlock(2), new StateBlock(1), _feature_ptr->getMeasurement(0));
+    return std::make_shared<LandmarkCorner2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), _feature_ptr->getMeasurement(0));
 }
 
 ConstraintBasePtr ProcessorTrackerLandmarkDummy::createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)

src/processor_tracker_landmark_polyline.cpp

@@ -521,7 +521,7 @@ LandmarkBasePtr ProcessorTrackerLandmarkPolyline::createLandmark(FeatureBasePtr
     //std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl;
 
     // Create new landmark
-    return std::make_shared<LandmarkPolyline2D>(new StateBlock(Eigen::Vector2s::Zero(), true), new StateBlock(Eigen::Vector1s::Zero(), true), points_global, polyline_ptr->isFirstDefined(), polyline_ptr->isLastDefined());
+    return std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true), std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true), points_global, polyline_ptr->isFirstDefined(), polyline_ptr->isLastDefined());
 }
 
 ProcessorTrackerLandmarkPolyline::~ProcessorTrackerLandmarkPolyline()

src/sensor_base.cpp

@@ -94,7 +94,6 @@ void SensorBase::removeStateBlocks()
             {
                 getProblem()->removeStateBlockPtr(sbp);
             }
-            delete sbp;
             setStateBlockPtr(i, nullptr);
         }
     }

src/sensor_camera.cpp

@@ -15,16 +15,16 @@ SensorCamera::SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBloc
 }
 
 SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const std::shared_ptr<IntrinsicsCamera> _intrinsics_ptr) :
-                SensorBase(SEN_CAMERA, "CAMERA", new StateBlock(_extrinsics.head(3)), new StateQuaternion(_extrinsics.tail(4)), new StateBlock(_intrinsics_ptr->pinhole_model), 2), // will initialize state blocks later
+                SensorBase(SEN_CAMERA, "CAMERA", std::make_shared<StateBlock>(_extrinsics.head(3)), std::make_shared<StateQuaternion>(_extrinsics.tail(4)), std::make_shared<StateBlock>(_intrinsics_ptr->pinhole_model), 2), // will initialize state blocks later
                 img_width_(_intrinsics_ptr->width), //
                 img_height_(_intrinsics_ptr->height), //
                 distortion_(_intrinsics_ptr->distortion), //
                 correction_(distortion_.size()) // make correction vector of the same size as distortion vector
 {
     assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3D");
-//    p_ptr_ = new StateBlock(_extrinsics.head(3));
-//    o_ptr_ = new StateQuaternion(_extrinsics.tail(4));
-//    intrinsic_ptr_ = new StateBlock(_intrinsics_ptr->pinhole_model);
+//    p_ptr_ = std::make_shared<StateBlock>(_extrinsics.head(3));
+//    o_ptr_ = std::make_shared<StateQuaternion>(_extrinsics.tail(4));
+//    intrinsic_ptr_ = std::make_shared<StateBlock>(_intrinsics_ptr->pinhole_model);
     K_ = setIntrinsicMatrix(_intrinsics_ptr->pinhole_model);
     pinhole::computeCorrectionModel(getIntrinsicPtr()->getVector(), distortion_, correction_);
 //    std::cout << "\tintrinsic_ptr  : " << intrinsic_ptr_->getVector().transpose() << std::endl;

src/sensor_gps.cpp

@@ -40,7 +40,7 @@ SensorBasePtr SensorGPS::create(const std::string& _unique_name, const Eigen::Ve
 {
     // decode extrinsics vector
     assert(_extrinsics_p.size() == 3 && "Bad extrinsics vector length. Should be 3 for 3D.");
-    StateBlockPtr pos_ptr = new StateBlock(_extrinsics_p, true);
+    StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_p, true);
     StateBlockPtr ori_ptr = nullptr;
     SensorBasePtr sen = std::make_shared<SensorGPS>(pos_ptr, ori_ptr, nullptr, nullptr, nullptr);
     sen->setName(_unique_name);

src/sensor_gps_fix.cpp

@@ -26,7 +26,7 @@ SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen:
 {
     assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
             && "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
-    StateBlockPtr pos_ptr = new StateBlock(_extrinsics, true);
+    StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics, true);
     std::shared_ptr<SensorGPSFix> sen = std::make_shared<SensorGPSFix>(pos_ptr, nullptr, 0);
     sen->setName(_unique_name);
     return sen;

src/sensor_imu.cpp

@@ -6,7 +6,7 @@
 namespace wolf {
 
 SensorIMU::SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _a_w_biases_ptr) :
-//                SensorBase(SEN_IMU, "IMU", _p_ptr, _o_ptr, (_a_w_biases_ptr == nullptr) ? new StateBlock(6, false) : _a_w_biases_ptr, 6)
+//                SensorBase(SEN_IMU, "IMU", _p_ptr, _o_ptr, (_a_w_biases_ptr == nullptr) ? std::make_shared<StateBlock>(6, false) : _a_w_biases_ptr, 6)
 SensorBase(SEN_IMU, "IMU", _p_ptr, _o_ptr, _a_w_biases_ptr, 6)
 {
     //
@@ -24,9 +24,9 @@ SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::Ve
     // decode extrinsics vector
     assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
 
-    StateBlockPtr pos_ptr  = new StateBlock(_extrinsics_pq.head(3), true);
-    StateBlockPtr ori_ptr  = new StateQuaternion(_extrinsics_pq.tail(4), true);
-    StateBlockPtr bias_ptr = new StateBlock(6, false); // We'll have the IMU biases here
+    StateBlockPtr pos_ptr  = std::make_shared<StateBlock>(_extrinsics_pq.head(3), true);
+    StateBlockPtr ori_ptr  = std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true);
+    StateBlockPtr bias_ptr = std::make_shared<StateBlock>(6, false); // We'll have the IMU biases here
 
     std::shared_ptr<SensorIMU> sen = std::make_shared<SensorIMU>(pos_ptr, ori_ptr, bias_ptr);
     sen->setName(_unique_name);

src/sensor_laser_2D.cpp

@@ -57,8 +57,8 @@ SensorBasePtr SensorLaser2D::create(const std::string& _unique_name, const Eigen
 {
     // decode extrinsics vector
     assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
-    StateBlockPtr pos_ptr = new StateBlock(_extrinsics_po.head(2), true);
-    StateBlockPtr ori_ptr = new StateBlock(_extrinsics_po.tail(1), true);
+    StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
+    StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true);
     // cast intrinsics into derived type
     IntrinsicsLaser2D::Ptr params = std::static_pointer_cast<IntrinsicsLaser2D>(_intrinsics);
     SensorLaser2D::Ptr sen = std::make_shared<SensorLaser2D>(pos_ptr, ori_ptr, params->scan_params);

src/sensor_odom_2D.cpp

@@ -30,8 +30,8 @@ SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen:
 {
     // decode extrinsics vector
     assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
-    StateBlockPtr pos_ptr = new StateBlock(_extrinsics_po.head(2), true);
-    StateBlockPtr ori_ptr = new StateBlock(_extrinsics_po.tail(1), true);
+    StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
+    StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true);
     // cast intrinsics into derived type
     std::shared_ptr<IntrinsicsOdom2D> params = std::static_pointer_cast<IntrinsicsOdom2D>(_intrinsics);
     std::shared_ptr<SensorOdom2D> odo = std::make_shared<SensorOdom2D>(pos_ptr, ori_ptr, params->k_disp_to_disp, params->k_rot_to_rot);

src/sensor_odom_3D.cpp

@@ -12,7 +12,7 @@
 
 namespace wolf {
 
-SensorOdom3D::SensorOdom3D(StateBlock* _p_ptr, StateQuaternion* _o_ptr, const Scalar& _k_disp_to_disp, const Scalar& _k_disp_to_rot, const Scalar&  _k_rot_to_rot) :
+SensorOdom3D::SensorOdom3D(StateBlockPtr _p_ptr, StateQuaternionPtr _o_ptr, const Scalar& _k_disp_to_disp, const Scalar& _k_disp_to_rot, const Scalar&  _k_rot_to_rot) :
         SensorBase(SEN_ODOM_3D, "ODOM 3D", _p_ptr, _o_ptr, nullptr, 6), k_disp_to_disp_(_k_disp_to_disp), k_disp_to_rot_(_k_disp_to_rot), k_rot_to_rot_(_k_rot_to_rot)
 {
     //
@@ -44,8 +44,8 @@ SensorBasePtr SensorOdom3D::create(const std::string& _unique_name, const Eigen:
 {
     // decode extrinsics vector
     assert(_extrinsics_po.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
-    StateBlock* pos_ptr = new StateBlock(_extrinsics_po.head(3), true);
-    StateQuaternion* ori_ptr = new StateQuaternion(_extrinsics_po.tail(4), true);
+    StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(3), true);
+    StateQuaternionPtr ori_ptr = std::make_shared<StateQuaternion>(_extrinsics_po.tail(4), true);
     // cast intrinsics into derived type
 //    IntrinsicsOdom3D* params = (IntrinsicsOdom3D*)(_intrinsics);
 //    params->k_disp_to_disp = 1.0;

src/sensor_odom_3D.h

@@ -38,7 +38,7 @@ class SensorOdom3D : public SensorBase
          * \param _rot_noise_factor rotation noise factor
          *
          **/
-        SensorOdom3D(StateBlock* _p_ptr, StateQuaternion* _q_ptr, const Scalar& _k_disp_to_disp, const Scalar& _k_disp_to_rot, const Scalar&  _k_rot_to_rot);
+        SensorOdom3D(StateBlockPtr _p_ptr, StateQuaternionPtr _q_ptr, const Scalar& _k_disp_to_disp, const Scalar& _k_disp_to_rot, const Scalar&  _k_rot_to_rot);
 
         /** \brief Default destructor (not recommended)
          *