It seems test_processor_imu working!

src/capture_base.cpp

@@ -12,8 +12,8 @@ CaptureBase::CaptureBase(const std::string& _type, const TimeStamp& _ts, SensorB
         capture_id_(++capture_id_count_),
         time_stamp_(_ts),
         sensor_ptr_(_sensor_ptr),
-        sensor_p_ptr_(sensor_ptr_->getPPtr()),
-        sensor_o_ptr_(sensor_ptr_->getOPtr())
+        sensor_p_ptr_(sensor_ptr_.lock()->getPPtr()),
+        sensor_o_ptr_(sensor_ptr_.lock()->getOPtr())
 {
     //
     std::cout << "constructed     C" << id() << std::endl;
@@ -41,15 +41,16 @@ void CaptureBase::process()
 {
     // Call all processors assigned to the sensor that captured this data
     auto cap = shared_from_this();
-    for (auto processor_iter = sensor_ptr_->getProcessorList().begin(); processor_iter != sensor_ptr_->getProcessorList().end(); ++processor_iter)
-    {
-        (*processor_iter)->process(cap);
-    }
+    auto sen = sensor_ptr_.lock();
+    if (sen)
+        for (auto prc : sen->getProcessorList())
+            prc->process(cap);
 }
 
 void CaptureBase::remove()
 {
     std::cout << "Remove          C" << id() << std::endl;
+//    std::cout << "C" <<  this_C->id() << " count: " << this_C.use_count() << std::endl;
     if (!is_removing_)
     {
         std::cout << "Removing        C" << id() << std::endl;

src/capture_base.h

@@ -28,7 +28,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
     protected:
         unsigned int capture_id_;
         TimeStamp time_stamp_; ///< Time stamp
-        SensorBasePtr sensor_ptr_; ///< Pointer to sensor
+        SensorBaseWPtr sensor_ptr_; ///< Pointer to sensor
 
         // Deal with sensors with dynamic extrinsics (check dynamic_extrinsic_ in SensorBase)
         StateBlock* sensor_p_ptr_;
@@ -157,7 +157,7 @@ inline TimeStamp CaptureBase::getTimeStamp() const
 
 inline SensorBasePtr CaptureBase::getSensorPtr() const
 {
-    return sensor_ptr_;
+    return sensor_ptr_.lock();
 }
 
 inline void CaptureBase::setTimeStamp(const TimeStamp& _ts)

src/examples/test_processor_imu.cpp

@@ -191,7 +191,7 @@ int main(int argc, char** argv)
     std::cout << "s/integr  : " << elapsed_secs/(N-1)*1e6 << " us" << std::endl;
     std::cout << "integr/s  : " << (N-1)/elapsed_secs << " ips" << std::endl;
 
-    problem_ptr_->print();
+    problem_ptr_->check();
 
 //    imu_ptr->remove();
 

src/feature_base.cpp

@@ -82,7 +82,7 @@ void FeatureBase::remove()
         CaptureBasePtr cap = capture_ptr_.lock();
         if (cap)
         {
-            cap->getFeatureList().remove(shared_from_this()); // remove from upstream
+            cap->getFeatureList().remove(this_f); // remove from upstream
             if (cap->getFeatureList().empty())
                 cap->remove(); // remove upstream
         }

src/frame_base.cpp

@@ -71,8 +71,12 @@ void FrameBase::remove()
 
         while (!capture_list_.empty())
         {
+//            auto C = capture_list_.front();
+//            std::cout << "C" <<  C->id() << " count: " << C.use_count() << std::endl;
             capture_list_.front()->remove(); // remove downstream
+//            std::cout << "C" <<  C->id() << " count: " << C.use_count() << std::endl;
             capture_list_.pop_front();
+//            std::cout << "C" <<  C->id() << " count: " << C.use_count() << std::endl;
         }
         while (!constrained_by_list_.empty())
         {

src/landmark_base.h

@@ -173,7 +173,7 @@ inline void LandmarkBase::removeConstrainedBy(ConstraintBasePtr _ctr_ptr)
 {
     constrained_by_list_.remove(_ctr_ptr);
     if (constrained_by_list_.empty())
-        this->remove();
+        remove();
 }
 
 inline StateBlock* LandmarkBase::getPPtr() const

src/problem.cpp

@@ -576,5 +576,69 @@ void Problem::print()
     }
 }
 
+void Problem::check()
+{
+    std::cout << "P: wolf tree integrity:" << std::endl;
+    std::cout << "H" << std::endl;
+    for (auto S : getHardwarePtr()->getSensorList() )
+    {
+        std::cout << "  S" << S->id() << std::endl;
+        for (auto p : S->getProcessorList() )
+        {
+            std::cout << "    p" << p->id() << " -> S" << p->getSensorPtr()->id() << std::endl;
+        }
+    }
+    std::cout << "T" << std::endl;
+    for (auto F : getTrajectoryPtr()->getFrameList() )
+    {
+        std::cout << (F->isKey() ?  "  KF" : "  F") << F->id() << (F->isFixed() ?  ", fixed" : ", estim") << ", ts=" << std::setprecision(5) << F->getTimeStamp().get();
+        std::cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << ")";
+        std::cout << " T @ " << F->getTrajectoryPtr().get() << std::endl;
+        for (auto c : F->getConstrainedByList())
+        {
+            std::cout << "    <- c" << c->id() << " -> F" << c->getFrameOtherPtr()->id() << std::endl;
+        }
+        for (auto C : F->getCaptureList() )
+        {
+            std::cout << "    C" << C->id() << " -> S" << C->getSensorPtr()->id() << std::endl;
+            for (auto f : C->getFeatureList() )
+            {
+                std::cout << "      f" << f->id() << ",            \t m = ( " << std::setprecision(3) << f->getMeasurement().transpose() << ")" << std::endl;
+                for (auto c : f->getConstrainedByList())
+                {
+                    std::cout << "     <- c" << c->id() << " -> f" << c->getFeatureOtherPtr()->id() << std::endl;
+                }
+                for (auto c : f->getConstraintList() )
+                {
+                    std::cout << "        c" << c->id();
+                    switch (c->getCategory())
+                    {
+                        case CTR_ABSOLUTE:
+                            std::cout << " --> A" << std::endl;
+                            break;
+                        case CTR_FRAME:
+                            std::cout << " --> F" << c->getFrameOtherPtr()->id() << std::endl;
+                            break;
+                        case CTR_FEATURE:
+                            std::cout << " --> f" << c->getFeatureOtherPtr()->id() << std::endl;
+                            break;
+                        case CTR_LANDMARK:
+                            std::cout << " --> L" << c->getLandmarkOtherPtr()->id() << std::endl;
+                            break;
+                    }
+                }
+            }
+        }
+    }
+    std::cout << "M" << std::endl;
+    for (auto L : getMapPtr()->getLandmarkList() )
+    {
+        std::cout << "  L" << L->id() << std::endl;
+        for (auto c : L->getConstrainedByList())
+        {
+            std::cout << "      <- c" << c->id() << " -> L" << c->getLandmarkOtherPtr()->id() << std::endl;
+        }
+    }
+}
 
 } // namespace wolf

src/problem.h

@@ -240,8 +240,9 @@ class Problem : public std::enable_shared_from_this<Problem>
          */
         void removeConstraintPtr(ConstraintBasePtr _constraint_ptr);
 
-        // Print ---------------------------------------
+        // Print and check ---------------------------------------
         void print();
+        void check();
 
 };
 

src/trajectory_base.cpp

@@ -15,6 +15,7 @@ TrajectoryBase::~TrajectoryBase()
 {
     while (!frame_list_.empty())
     {
+//        frame_list_.front()->remove();
         frame_list_.pop_front();
     }
     std::cout << "destructed T" << std::endl;