Make emplace return derived type pointer

include/core/capture/capture_base.h

@@ -107,7 +107,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
         void move(FrameBasePtr);
         void link(FrameBasePtr);
         template<typename classType, typename... T>
-        static std::shared_ptr<CaptureBase> emplace(FrameBasePtr _frm_ptr, T&&... all);
+        static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
 
     protected:
         SizeEigen computeCalibSize() const;
@@ -130,9 +130,9 @@ 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)
+std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... all)
 {
-    CaptureBasePtr cpt = std::make_shared<classType>(std::forward<T>(all)...);
+    std::shared_ptr<classType> cpt = std::make_shared<classType>(std::forward<T>(all)...);
     cpt->link(_frm_ptr);
     return cpt;
 }

include/core/factor/factor_base.h

@@ -165,7 +165,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
 
         void link(FeatureBasePtr ftr);
         template<typename classType, typename... T>
-        static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all);
+        static std::shared_ptr<classType> emplace(FeatureBasePtr _oth_ptr, T&&... all);
 
     private:
 
@@ -191,9 +191,9 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
 namespace wolf{
 
 template<typename classType, typename... T>
-std::shared_ptr<FactorBase> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... all)
+std::shared_ptr<classType> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... all)
 {
-    FactorBasePtr ctr = std::make_shared<classType>(std::forward<T>(all)...);
+    std::shared_ptr<classType> ctr = std::make_shared<classType>(std::forward<T>(all)...);
     ctr->link(_ftr_ptr);
     return ctr;
 }

include/core/feature/feature_base.h

@@ -98,7 +98,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
 
         void link(CaptureBasePtr cap_ptr);
         template<typename classType, typename... T>
-        static std::shared_ptr<FeatureBase> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
+        static std::shared_ptr<classType> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
 
 
     protected:
@@ -122,9 +122,9 @@ 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)
+    std::shared_ptr<classType> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all)
     {
-        FeatureBasePtr ftr = std::make_shared<classType>(std::forward<T>(all)...);
+        std::shared_ptr<classType> ftr = std::make_shared<classType>(std::forward<T>(all)...);
         ftr->link(_cpt_ptr);
         return ftr;
     }

include/core/frame/frame_base.h

@@ -149,7 +149,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         const FactorBasePtrList& getConstrainedByList() const;
         void link(TrajectoryBasePtr);
         template<typename classType, typename... T>
-        static std::shared_ptr<FrameBase> emplace(TrajectoryBasePtr _ptr, T&&... all);
+        static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all);
 
     private:
 
@@ -173,9 +173,9 @@ 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)
+std::shared_ptr<classType> FrameBase::emplace(TrajectoryBasePtr _ptr, T&&... all)
 {
-    FrameBasePtr frm = std::make_shared<classType>(std::forward<T>(all)...);
+    std::shared_ptr<classType> frm = std::make_shared<classType>(std::forward<T>(all)...);
     frm->link(_ptr);
     return frm;
 }

include/core/landmark/landmark_base.h

@@ -93,7 +93,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
         MapBasePtr getMap();
         void link(MapBasePtr);
         template<typename classType, typename... T>
-        static std::shared_ptr<LandmarkBase> emplace(MapBasePtr _map_ptr, T&&... all);
+        static std::shared_ptr<classType> emplace(MapBasePtr _map_ptr, T&&... all);
 
         /** \brief Creator for Factory<LandmarkBase, YAML::Node>
          * Caution: This creator does not set the landmark's anchor frame and sensor.
@@ -117,9 +117,9 @@ 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)
+std::shared_ptr<classType> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&... all)
 {
-    LandmarkBasePtr lmk = std::make_shared<classType>(std::forward<T>(all)...);
+    std::shared_ptr<classType> lmk = std::make_shared<classType>(std::forward<T>(all)...);
     lmk->link(_map_ptr);
     return lmk;
 }

include/core/processor/processor_base.h

@@ -304,7 +304,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
 
         void link(SensorBasePtr);
         template<typename classType, typename... T>
-        static std::shared_ptr<ProcessorBase> emplace(SensorBasePtr _sen_ptr, T&&... all);
+        static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all);
         void setVotingAuxActive(bool _voting_active = true);
 };
 
@@ -336,9 +336,9 @@ 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)
+std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all)
 {
-    ProcessorBasePtr prc = std::make_shared<classType>(std::forward<T>(all)...);
+    std::shared_ptr<classType> prc = std::make_shared<classType>(std::forward<T>(all)...);
     prc->link(_sen_ptr);
     return prc;
 }

include/core/sensor/sensor_base.h

@@ -199,7 +199,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
 
         void link(HardwareBasePtr);
         template<typename classType, typename... T>
-        static std::shared_ptr<SensorBase> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
+        static std::shared_ptr<classType> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
 
     protected:
         SizeEigen computeCalibSize() const;
@@ -218,9 +218,9 @@ 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)
+std::shared_ptr<classType> SensorBase::emplace(HardwareBasePtr _hwd_ptr, T&&... all)
 {
-    SensorBasePtr sen = std::make_shared<classType>(std::forward<T>(all)...);
+    std::shared_ptr<classType> sen = std::make_shared<classType>(std::forward<T>(all)...);
     sen->link(_hwd_ptr);
     return sen;
 }

test/gtest_emplace.cpp

@@ -55,6 +55,11 @@ TEST(Emplace, Processor)
     ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getProcessorList().front()->getSensor()->getProblem());
     ASSERT_EQ(sen, sen->getProcessorList().front()->getSensor());
     ASSERT_EQ(prc, sen->getProcessorList().front());
+
+    SensorBasePtr sen2 = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
+    ProcessorOdom2DPtr prc2 = ProcessorOdom2D::emplace<ProcessorOdom2D>(sen2, std::make_shared<ProcessorParamsOdom2D>());
+    ASSERT_EQ(sen2, sen2->getProcessorList().front()->getSensor());
+    ASSERT_EQ(prc2, sen2->getProcessorList().front());
 }
 
 TEST(Emplace, Capture)
@@ -119,7 +124,7 @@ TEST(Emplace, EmplaceDerived)
     auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXs(3), IntrinsicsOdom2D());
     auto cov = Eigen::MatrixXs::Identity(2,2);
     auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::VectorXs(2), cov, frm);
-    auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt);
+    // auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt);
     auto m = Eigen::Matrix<Scalar,9,6>();
     for(int i = 0; i < 9; i++)
         for(int j = 0; j < 6; j++)
@@ -133,6 +138,20 @@ TEST(Emplace, Nullpointer)
 {
     CaptureBase::emplace<wolf::CaptureBase>(nullptr, "DUMMY", 1.2, nullptr);
 }
+TEST(Emplace, ReturnDerived)
+{
+    ProblemPtr P = Problem::create("POV", 3);
+
+    ASSERT_NE(P->getTrajectory(), nullptr);
+    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+    auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
+    auto cov = Eigen::MatrixXs::Identity(2,2);
+    auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(2), cov);
+    auto cnt = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm);
+
+    FactorOdom2DPtr fac = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm);
+
+}
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);