Merge branch 'devel' into Apriltag

hello_wolf/hello_wolf.cpp

@@ -104,7 +104,7 @@ int main()
     using namespace wolf;
 
     // Wolf problem and solver
-    ProblemPtr problem                      = Problem::create("PO 2D");
+    ProblemPtr problem                      = Problem::create("PO", 2);
     ceres::Solver::Options options;
     options.max_num_iterations              = 1000; // We depart far from solution, need a lot of iterations
     CeresManagerPtr ceres                   = std::make_shared<CeresManager>(problem, options);

hello_wolf/processor_range_bearing.cpp

@@ -47,7 +47,8 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
 
     // 2. cast incoming capture to the range-and-bearing type, add it to the keyframe
     CaptureRangeBearingPtr capture_rb = std::static_pointer_cast<CaptureRangeBearing>(_capture);
-    kf->addCapture(capture_rb);
+    // kf->addCapture(capture_rb);
+    capture_rb->link(kf);
 
     // 3. explore all observations in the capture
     for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++)
@@ -70,9 +71,10 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
         {
             // new landmark:
             // - create landmark
-            lmk = std::make_shared<LandmarkPoint2D>(id, invObserve(range, bearing));
+            // lmk = std::make_shared<LandmarkPoint2D>(id, invObserve(range, bearing));
+            lmk = std::static_pointer_cast<LandmarkPoint2D>(LandmarkBase::emplace<LandmarkPoint2D>(getProblem()->getMap(), id, invObserve(range, bearing)));
             WOLF_TRACE("new   lmk(", id, "): ", lmk->getP()->getState().transpose());
-            getProblem()->getMap()->addLandmark(lmk);
+            // getProblem()->getMap()->addLandmark(lmk);
             // - add to known landmarks
             known_lmks.emplace(id, lmk);
         }
@@ -81,17 +83,23 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
         Vector2s rb(range,bearing);
         auto ftr = std::make_shared<FeatureRangeBearing>(rb,
                                                          getSensor()->getNoiseCov());
-        capture_rb->addFeature(ftr);
+        // capture_rb->addFeature(ftr);
+        FeatureBase::emplace<FeatureRangeBearing>(capture_rb, rb, getSensor()->getNoiseCov());
 
         // 6. create factor
         auto prc = shared_from_this();
-        auto ctr = std::make_shared<FactorRangeBearing>(capture_rb,
-                                                            lmk,
-                                                            prc,
-                                                            false,
-                                                            FAC_ACTIVE);
-        ftr->addFactor(ctr);
-        lmk->addConstrainedBy(ctr);
+        // auto ctr = std::make_shared<FactorRangeBearing>(capture_rb,
+        //                                                     lmk,
+        //                                                     prc,
+        //                                                     false,
+        //                                                     FAC_ACTIVE);
+        auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr, capture_rb,
+                                                           lmk,
+                                                           prc,
+                                                           false,
+                                                           FAC_ACTIVE);
+        // ftr->addFactor(ctr);
+        // lmk->addConstrainedBy(ctr);
     }
 
 }

include/base/capture/capture_base.h

@@ -99,6 +99,9 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
         SizeEigen getCalibSize() const;
         virtual Eigen::VectorXs getCalibration() const;
         void setCalibration(const Eigen::VectorXs& _calib);
+        void link(FrameBasePtr);
+        template<typename classType, typename... T>
+        static std::shared_ptr<CaptureBase> emplace(FrameBasePtr _frm_ptr, T&&... all);
 
     protected:
         SizeEigen computeCalibSize() const;
@@ -116,6 +119,14 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
 
 namespace wolf{
 
+template<typename classType, typename... T>
+std::shared_ptr<CaptureBase> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... all)
+{
+    CaptureBasePtr cpt = std::make_shared<classType>(std::forward<T>(all)...);
+    cpt->link(_frm_ptr);
+    return cpt;
+}
+
 inline SizeEigen CaptureBase::getCalibSize() const
 {
     return calib_size_;

include/base/common/node_base.h

@@ -81,7 +81,6 @@ class NodeBase
 
         void setType(const std::string& _type);
         void setName(const std::string& _name);
-
         ProblemPtr getProblem() const;
         virtual void setProblem(ProblemPtr _prob_ptr);
 };

include/base/factor/factor_base.h

@@ -171,6 +171,10 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
          */
         void setProcessor(const ProcessorBasePtr& _processor_ptr);
 
+        void link(FeatureBasePtr);
+        template<typename classType, typename... T>
+        static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all);
+
 //    protected:
 //        template<typename D>
 //        void print(const std::string& name, const Eigen::MatrixBase<D>& mat) const; // Do nothing if input Scalar type is ceres::Jet
@@ -209,6 +213,15 @@ namespace wolf{
 //    }
 //}
 
+template<typename classType, typename... T>
+std::shared_ptr<FactorBase> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... all)
+{
+    FactorBasePtr ctr = std::make_shared<classType>(std::forward<T>(all)...);
+    ctr->link(_ftr_ptr);
+    return ctr;
+}
+
+
 inline unsigned int FactorBase::id() const
 {
     return factor_id_;

include/base/feature/feature_base.h

@@ -95,6 +95,10 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
         // all factors
         void getFactorList(FactorBasePtrList & _fac_list);
 
+        void link(CaptureBasePtr);
+        template<typename classType, typename... T>
+        static std::shared_ptr<FeatureBase> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
+
     private:
         Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
 };
@@ -107,6 +111,14 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
 
 namespace wolf{
 
+    template<typename classType, typename... T>
+    std::shared_ptr<FeatureBase> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all)
+    {
+        FeatureBasePtr ftr = std::make_shared<classType>(std::forward<T>(all)...);
+        ftr->link(_cpt_ptr);
+        return ftr;
+    }
+
 inline unsigned int FeatureBase::getHits() const
 {
     return constrained_by_list_.size();

include/base/frame/frame_base.h

@@ -65,6 +65,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
          **/        
         FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr);
 
+        FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x);
+
         virtual ~FrameBase();
         virtual void remove();
 
@@ -144,6 +146,9 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
         unsigned int getHits() const;
         FactorBasePtrList& getConstrainedByList();
+        void link(TrajectoryBasePtr);
+        template<typename classType, typename... T>
+        static std::shared_ptr<FrameBase> emplace(TrajectoryBasePtr _ptr, T&&... all);
 
     public:
         static FrameBasePtr create_PO_2D (const FrameType & _tp,
@@ -168,6 +173,14 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
 
 namespace wolf {
 
+template<typename classType, typename... T>
+std::shared_ptr<FrameBase> FrameBase::emplace(TrajectoryBasePtr _ptr, T&&... all)
+{
+    FrameBasePtr frm = std::make_shared<classType>(std::forward<T>(all)...);
+    frm->link(_ptr);
+    return frm;
+}
+
 inline unsigned int FrameBase::id()
 {
     return frame_id_;

include/base/landmark/landmark_base.h

@@ -44,7 +44,8 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
          * \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
          *
          **/
-        LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
+    LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
+    LandmarkBase(MapBaseWPtr _ptr, const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
         virtual ~LandmarkBase();
         virtual void remove();
         virtual YAML::Node saveToYaml() const;
@@ -78,7 +79,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
 
         // Descriptor
     public:
-        const Eigen::VectorXs& getDescriptor() const;        
+        const Eigen::VectorXs& getDescriptor() const;
         Scalar getDescriptor(unsigned int _ii) const;
         void setDescriptor(const Eigen::VectorXs& _descriptor);
 
@@ -91,6 +92,9 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
 
         void setMap(const MapBasePtr _map_ptr);
         MapBasePtr getMap();
+        void link(MapBasePtr);
+        template<typename classType, typename... T>
+        static std::shared_ptr<LandmarkBase> emplace(MapBasePtr _map_ptr, T&&... all);
 
 };
 
@@ -102,6 +106,13 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
 
 namespace wolf{
 
+template<typename classType, typename... T>
+std::shared_ptr<LandmarkBase> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&... all)
+{
+    LandmarkBasePtr lmk = std::make_shared<classType>(std::forward<T>(all)...);
+    lmk->link(_map_ptr);
+    return lmk;
+}
 inline void LandmarkBase::setProblem(ProblemPtr _problem)
 {
     NodeBase::setProblem(_problem);

include/base/problem/problem.h

@@ -49,19 +49,22 @@ class Problem : public std::enable_shared_from_this<Problem>
         mutable std::mutex mut_state_block_notifications_;
         mutable std::mutex mut_covariances_;
         bool prior_is_set_;
+        std::string frame_structure_;
 
     private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
         Problem(const std::string& _frame_structure); // USE create() below !!
+        Problem(const std::string& _frame_structure, SizeEigen _dim); // USE create() below !!
         void setup();
 
     public:
-        static ProblemPtr create(const std::string& _frame_structure); // USE THIS AS A CONSTRUCTOR!
+        static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim); // USE THIS AS A CONSTRUCTOR!
         virtual ~Problem();
 
         // Properties -----------------------------------------
         SizeEigen getFrameStructureSize() const;
         void getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const;
         SizeEigen getDim() const;
+        std::string getFrameStructure() const;
 
         // Hardware branch ------------------------------------
         HardwareBasePtr getHardware();
@@ -140,6 +143,7 @@ class Problem : public std::enable_shared_from_this<Problem>
 
         /** \brief Emplace frame from string frame_structure
          * \param _frame_structure String indicating the frame structure.
+         * \param _dim variable indicating the dimension of the problem
          * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
          * \param _frame_state State vector; must match the size and format of the chosen frame structure
          * \param _time_stamp Time stamp of the frame
@@ -150,12 +154,14 @@ class Problem : public std::enable_shared_from_this<Problem>
          *   - If it is key-frame, update state-block lists in Problem
          */
         FrameBasePtr emplaceFrame(const std::string& _frame_structure, //
+                                  const SizeEigen _dim, //
                                   FrameType _frame_key_type, //
                                   const Eigen::VectorXs& _frame_state, //
                                   const TimeStamp& _time_stamp);
 
         /** \brief Emplace frame from string frame_structure without state
          * \param _frame_structure String indicating the frame structure.
+         * \param _dim variable indicating the dimension of the problem
          * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
          * \param _time_stamp Time stamp of the frame
          *
@@ -165,6 +171,7 @@ class Problem : public std::enable_shared_from_this<Problem>
          *   - If it is key-frame, update state-block lists in Problem
          */
         FrameBasePtr emplaceFrame(const std::string& _frame_structure, //
+                                  const SizeEigen _dim, //
                                   FrameType _frame_key_type, //
                                   const TimeStamp& _time_stamp);
 

include/base/processor/processor_base.h

@@ -210,6 +210,9 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
 
         void setVotingActive(bool _voting_active = true);
 
+        void link(SensorBasePtr);
+        template<typename classType, typename... T>
+        static std::shared_ptr<ProcessorBase> emplace(SensorBasePtr _sen_ptr, T&&... all);
         void setVotingAuxActive(bool _voting_active = true);
 };
 
@@ -240,6 +243,14 @@ inline void ProcessorBase::setVotingAuxActive(bool _voting_active)
 
 namespace wolf {
 
+template<typename classType, typename... T>
+std::shared_ptr<ProcessorBase> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all)
+{
+    ProcessorBasePtr prc = std::make_shared<classType>(std::forward<T>(all)...);
+    prc->link(_sen_ptr);
+    return prc;
+}
+
 inline bool ProcessorBase::isMotion()
 {
     return false;

include/base/sensor/sensor_base.h

@@ -180,6 +180,9 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         Eigen::MatrixXs getNoiseCov();
         void setExtrinsicDynamic(bool _extrinsic_dynamic);
         void setIntrinsicDynamic(bool _intrinsic_dynamic);
+        void link(HardwareBasePtr);
+        template<typename classType, typename... T>
+        static std::shared_ptr<SensorBase> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
 
     protected:
         SizeEigen computeCalibSize() const;
@@ -197,6 +200,14 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
 
 namespace wolf{
 
+template<typename classType, typename... T>
+std::shared_ptr<SensorBase> SensorBase::emplace(HardwareBasePtr _hwd_ptr, T&&... all)
+{
+    SensorBasePtr sen = std::make_shared<classType>(std::forward<T>(all)...);
+    sen->link(_hwd_ptr);
+    return sen;
+}
+
 inline unsigned int SensorBase::id()
 {
     return sensor_id_;

src/capture/capture_GPS_fix.cpp

@@ -25,10 +25,11 @@ CaptureGPSFix::~CaptureGPSFix()
 void CaptureGPSFix::process()
 {
 	// EXTRACT AND ADD FEATURES
-    addFeature(std::make_shared<FeatureGPSFix>(data_,data_covariance_));
+    // addFeature(std::make_shared<FeatureGPSFix>(data_,data_covariance_));
 
     // ADD CONSTRAINT
-    getFeatureList().front()->addFactor(std::make_shared <FactorGPS2D>(getFeatureList().front()));
+    // getFeatureList().front()->addFactor(std::make_shared <FactorGPS2D>(getFeatureList().front()));
+    FactorBase::emplace<FactorGPS2D>(getFeatureList().front(), getFeatureList().front());
 }
 
 } //namespace wolf

src/capture/capture_base.cpp

@@ -99,8 +99,6 @@ FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
 {
     //std::cout << "Adding feature" << std::endl;
     feature_list_.push_back(_ft_ptr);
-    _ft_ptr->setCapture(shared_from_this());
-    _ft_ptr->setProblem(getProblem());
     return _ft_ptr;
 }
 
@@ -294,5 +292,20 @@ void CaptureBase::setCalibration(const VectorXs& _calib)
     }
 }
 
+void CaptureBase::link(FrameBasePtr _frm_ptr)
+{
+    if(_frm_ptr)
+    {
+        _frm_ptr->addCapture(shared_from_this());
+        this->setFrame(_frm_ptr);
+        this->setProblem(_frm_ptr->getProblem());
+        this->registerNewStateBlocks();
+    }
+    else
+    {
+        WOLF_WARN("Linking with a nullptr");
+    }
+}
+
 } // namespace wolf
 

src/capture/capture_pose.cpp

@@ -18,14 +18,16 @@ CapturePose::~CapturePose()
 void CapturePose::emplaceFeatureAndFactor()
 {
     // Emplace feature
-    FeaturePosePtr feature_pose = std::make_shared<FeaturePose>(data_,data_covariance_);
-    addFeature(feature_pose);
+    // FeaturePosePtr feature_pose = std::make_shared<FeaturePose>(data_,data_covariance_);
+    // addFeature(feature_pose);
+    auto feature_pose = FeatureBase::emplace<FeaturePose>(shared_from_this(), data_, data_covariance_);
 
+    std::cout << data_.size() << " ~~ " << data_covariance_.rows() << "x" << data_covariance_.cols() << std::endl;
     // Emplace factor
     if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3 )
-        feature_pose->addFactor(std::make_shared<FactorPose2D>(feature_pose));
+        FactorBase::emplace<FactorPose2D>(feature_pose, feature_pose);
     else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6 )
-        feature_pose->addFactor(std::make_shared<FactorPose3D>(feature_pose));
+        FactorBase::emplace<FactorPose3D>(feature_pose, feature_pose);
     else
         throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2D. Use 7 for 3D.");
 }

src/examples/test_diff_drive.cpp

@@ -163,7 +163,7 @@ int main(int argc, char** argv)
   }
 
   // Wolf problem
-  ProblemPtr wolf_problem_ptr_ = Problem::create("PO 2D");
+  ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2);
 
   const std::string sensor_name("Main Odometer");
   Eigen::VectorXs extrinsics(3);

src/examples/test_factor_AHP.cpp

@@ -21,7 +21,7 @@ int main()
     //=====================================================
 
     // Wolf problem
-    ProblemPtr wolf_problem_ptr_ = Problem::create("PO 3D");
+    ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 3);
 
     /* Do this while there aren't extrinsic parameters on the yaml */
     Eigen::Vector7s extrinsic_cam;

src/examples/test_image.cpp

@@ -52,7 +52,7 @@ int main(int argc, char** argv)
     std::string wolf_root = _WOLF_ROOT_DIR;
     std::cout << "Wolf root: " << wolf_root << std::endl;
 
-    ProblemPtr wolf_problem_ = Problem::create("PO 3D");
+    ProblemPtr wolf_problem_ = Problem::create("PO", 3);
 
     //=====================================================
     // Method 1: Use data generated here for sensor and processor

src/examples/test_kf_callback.cpp

@@ -16,7 +16,7 @@ int main()
     using namespace Eigen;
     using namespace std;
 
-    ProblemPtr problem = Problem::create("PO 2D");
+    ProblemPtr problem = Problem::create("PO", 2);
 
     SensorBasePtr sen_odo    = problem->installSensor   ("ODOM 2D", "main odometer", (Vector3s()<<0,0,0).finished(),"");
     ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();

src/examples/test_map_yaml.cpp

@@ -75,7 +75,7 @@ int main()
     std::string wolf_config     = wolf_root + "/src/examples";
     std::cout << "\nWolf directory for configuration files: " << wolf_config << std::endl;
 
-    ProblemPtr problem = Problem::create("PO 2D");
+    ProblemPtr problem = Problem::create("PO", 2);
     filename = wolf_config + "/map_polyline_example.yaml";
     std::cout << "Reading map from file: " << filename << std::endl;
     problem->loadMap(filename);

src/examples/test_processor_odom_3D.cpp

@@ -40,7 +40,7 @@ int main (int argc, char** argv)
     }
     cout << "Final timestamp tf = " << tf.get() << " s" << endl;
 
-    ProblemPtr problem = Problem::create("PO 3D");
+    ProblemPtr problem = Problem::create("PO", 3);
     ceres::Solver::Options ceres_options;
 //    ceres_options.max_num_iterations = 1000;
 //    ceres_options.function_tolerance = 1e-10;