Merge branch 'master' into hello_wolf

src/ceres_wrapper/ceres_manager.h

@@ -26,60 +26,62 @@ WOLF_PTR_TYPEDEFS(CeresManager);
 
 class CeresManager : public SolverManager
 {
-protected:
+    protected:
 
-  std::map<ConstraintBasePtr, ceres::ResidualBlockId> ctr_2_residual_idx_;
-  std::map<ConstraintBasePtr, ceres::CostFunctionPtr> ctr_2_costfunction_;
+        std::map<ConstraintBasePtr, ceres::ResidualBlockId> ctr_2_residual_idx_;
+        std::map<ConstraintBasePtr, ceres::CostFunctionPtr> ctr_2_costfunction_;
 
-  std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
+        std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
 
-  ceres::Solver::Options ceres_options_;
-  ceres::Solver::Summary summary_;
-  std::unique_ptr<ceres::Problem> ceres_problem_;
-  std::unique_ptr<ceres::Covariance> covariance_;
+        ceres::Solver::Options ceres_options_;
+        ceres::Solver::Summary summary_;
+        std::unique_ptr<ceres::Problem> ceres_problem_;
+        std::unique_ptr<ceres::Covariance> covariance_;
 
-public:
+    public:
 
-  CeresManager(const ProblemPtr& _wolf_problem,
-               const ceres::Solver::Options& _ceres_options
-                = ceres::Solver::Options());
+        CeresManager(const ProblemPtr& _wolf_problem,
+                     const ceres::Solver::Options& _ceres_options
+                     = ceres::Solver::Options());
 
-  ~CeresManager();
+        ~CeresManager();
 
-  ceres::Solver::Summary getSummary();
+        ceres::Solver::Summary getSummary();
 
-  virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
-                                  = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
+        virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
+                                        = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
 
-  virtual void computeCovariances(const StateBlockList& st_list) override;
+        virtual void computeCovariances(const StateBlockList& st_list) override;
 
-  ceres::Solver::Options& getSolverOptions();
+        ceres::Solver::Options& getSolverOptions();
 
-private:
+    private:
 
-  std::string solveImpl(const ReportVerbosity report_level) override;
+        std::string solveImpl(const ReportVerbosity report_level) override;
 
-  void addConstraint(const ConstraintBasePtr& ctr_ptr) override;
+        void addConstraint(const ConstraintBasePtr& ctr_ptr) override;
 
-  void removeConstraint(const ConstraintBasePtr& ctr_ptr) override;
+        void removeConstraint(const ConstraintBasePtr& ctr_ptr) override;
 
-  void addStateBlock(const StateBlockPtr& state_ptr) override;
+        void addStateBlock(const StateBlockPtr& state_ptr) override;
 
-  void removeStateBlock(const StateBlockPtr& state_ptr) override;
+        void removeStateBlock(const StateBlockPtr& state_ptr) override;
 
-  void updateStateBlockStatus(const StateBlockPtr& state_ptr) override;
+        void updateStateBlockStatus(const StateBlockPtr& state_ptr) override;
 
-  ceres::CostFunctionPtr createCostFunction(const ConstraintBasePtr& _ctr_ptr);
+        ceres::CostFunctionPtr createCostFunction(const ConstraintBasePtr& _ctr_ptr);
+
+        void check();
 };
 
 inline ceres::Solver::Summary CeresManager::getSummary()
 {
-  return summary_;
+    return summary_;
 }
 
 inline ceres::Solver::Options& CeresManager::getSolverOptions()
 {
-  return ceres_options_;
+    return ceres_options_;
 }
 
 } // namespace wolf

src/ceres_wrapper/cost_function_wrapper.h

@@ -17,10 +17,8 @@ WOLF_PTR_TYPEDEFS(CostFunctionWrapper);
 
 class CostFunctionWrapper : public ceres::CostFunction
 {
-    protected:
-        ConstraintBasePtr constraint_ptr_;
-
     public:
+        ConstraintBasePtr constraint_ptr_;
 
         CostFunctionWrapper(ConstraintBasePtr _constraint_ptr);
 

src/feature_base.cpp

@@ -59,7 +59,10 @@ ConstraintBasePtr FeatureBase::addConstraint(ConstraintBasePtr _co_ptr)
     _co_ptr->setProblem(getProblem());
     // add constraint to be added in solver
     if (getProblem() != nullptr)
-        getProblem()->addConstraintPtr(_co_ptr);
+    {
+        if (_co_ptr->getStatus() == CTR_ACTIVE)
+            getProblem()->addConstraintPtr(_co_ptr);
+    }
     else
         WOLF_TRACE("WARNING: ADDING CONSTRAINT ", _co_ptr->id(), " TO FEATURE ", this->id(), " NOT CONNECTED WITH PROBLEM.");
     return _co_ptr;

src/frame_base.cpp

@@ -14,7 +14,7 @@ unsigned int FrameBase::frame_id_count_ = 0;
 FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
             NodeBase("FRAME", "Base"),
             trajectory_ptr_(),
-            state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
+            state_block_vec_(3), // allow for 6 state blocks by default. Resize in derived constructors if needed.
             is_removing_(false),
             frame_id_(++frame_id_count_),
             type_(NON_KEY_FRAME),

src/hello_wolf/hello_wolf.cpp

@@ -27,7 +27,7 @@
 #include "ceres_wrapper/ceres_manager.h"
 
 int main()
- {
+{
     /*
      * ============= PROBLEM DEFINITION ==================
      *
@@ -117,12 +117,12 @@ int main()
 
     // sensor odometer 2D
     IntrinsicsOdom2DPtr intrinsics_odo      = std::make_shared<IntrinsicsOdom2D>();
+    intrinsics_odo->k_disp_to_disp          = 0.1;
+    intrinsics_odo->k_rot_to_rot            = 0.1;
     SensorBasePtr sensor_odo                = problem->installSensor("ODOM 2D", "sensor odo", Vector3s(0,0,0), intrinsics_odo);
 
     // processor odometer 2D
     ProcessorParamsOdom2DPtr params_odo     = std::make_shared<ProcessorParamsOdom2D>();
-    params_odo->voting_active               = true;
-    params_odo->time_tolerance              = 0.1;
     params_odo->max_time_span               = 999;
     params_odo->dist_traveled               = 0.95; // Will make KFs automatically every 1m displacement
     params_odo->angle_turned                = 999;
@@ -133,8 +133,8 @@ int main()
 
     // sensor Range and Bearing
     IntrinsicsRangeBearingPtr intrinsics_rb = std::make_shared<IntrinsicsRangeBearing>();
-    intrinsics_rb->noise_range_metres_std   = 0.1;
     intrinsics_rb->noise_bearing_degrees_std = 1.0;
+    intrinsics_rb->noise_range_metres_std   = 0.1;
     SensorBasePtr sensor_rb                 = problem->installSensor("RANGE BEARING", "sensor RB", Vector3s(1,1,0), intrinsics_rb);
 
     // NOTE: uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
@@ -143,7 +143,6 @@ int main()
 
     // processor Range and Bearing
     ProcessorParamsRangeBearingPtr params_rb = std::make_shared<ProcessorParamsRangeBearing>();
-    params_rb->voting_active                = false;
     params_rb->time_tolerance               = 0.01;
     ProcessorBasePtr processor_rb           = problem->installProcessor("RANGE BEARING", "processor RB", sensor_rb, params_rb);
 
@@ -168,10 +167,10 @@ int main()
     // STEP 1 --------------------------------------------------------------
 
     // initialize
-    TimeStamp   t(0.0);                     // t : 0.0
+    TimeStamp   t(0.0);
     Vector3s    x(0,0,0);
     Matrix3s    P = Matrix3s::Identity() * 0.1;
-    problem->setPrior(x, P, t, 0.5);             // KF1 : (0,0,0)
+    problem->setPrior(x, P, t, 0.5);             // KF1
 
     // observe lmks
     ids.resize(1); ranges.resize(1); bearings.resize(1);
@@ -179,14 +178,14 @@ int main()
     ranges      << 1.0;                     // see drawing
     bearings    << M_PI/2;
     CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
-    sensor_rb   ->process(cap_rb);          // L1 : (1,2)
+    sensor_rb   ->process(cap_rb);
 
     // STEP 2 --------------------------------------------------------------
-    t += 1.0;                     // t : 1.0
+    t += 1.0;
 
     // motion
     CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odo, motion_data, motion_cov);
-    sensor_odo  ->process(cap_motion);      // KF2 : (1,0,0)
+    sensor_odo  ->process(cap_motion);      // KF2
 
     // observe lmks
     ids.resize(2); ranges.resize(2); bearings.resize(2);
@@ -194,14 +193,14 @@ int main()
     ranges      << sqrt(2.0), 1.0;          // see drawing
     bearings    << 3*M_PI/4, M_PI/2;
     cap_rb      = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
-    sensor_rb   ->process(cap_rb);          // L1 : (1,2), L2 : (2,2)
+    sensor_rb   ->process(cap_rb);
 
     // STEP 3 --------------------------------------------------------------
-    t += 1.0;                     // t : 2.0
+    t += 1.0;
 
     // motion
     cap_motion  ->setTimeStamp(t);
-    sensor_odo  ->process(cap_motion);      // KF3 : (2,0,0)
+    sensor_odo  ->process(cap_motion);      // KF3
 
     // observe lmks
     ids.resize(2); ranges.resize(2); bearings.resize(2);
@@ -210,6 +209,7 @@ int main()
     bearings    << 3*M_PI/4, M_PI/2;
     cap_rb      = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
     sensor_rb   ->process(cap_rb);          // L1 : (1,2), L2 : (2,2), L3 : (3,2)
+
     problem->print(1,0,1,0);
 
 
@@ -226,12 +226,9 @@ int main()
     for (auto sen : problem->getHardwarePtr()->getSensorList())
         for (auto sb : sen->getStateBlockVec())
             if (sb && !sb->isFixed())
-                sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);       // We perturb A LOT !
+                sb->setState(VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
     for (auto kf : problem->getTrajectoryPtr()->getFrameList())
-        if (kf->isKey())
-            for (auto sb : kf->getStateBlockVec())
-                if (sb && !sb->isFixed())
-                    sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);   // We perturb A LOT !
+        kf->setState(Vector3s::Random() * 0.5);                 // We perturb A LOT !
     for (auto lmk : problem->getMapPtr()->getLandmarkList())
         for (auto sb : lmk->getStateBlockVec())
             if (sb && !sb->isFixed())

src/solver/solver_manager.cpp

@@ -6,179 +6,179 @@
 namespace wolf {
 
 SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
-  wolf_problem_(_wolf_problem)
+          wolf_problem_(_wolf_problem)
 {
-  assert(_wolf_problem != nullptr && "Passed a nullptr ProblemPtr.");
+    assert(_wolf_problem != nullptr && "Passed a nullptr ProblemPtr.");
 }
 
 void SolverManager::update()
 {
-  // REMOVE CONSTRAINTS
-  auto ctr_notification_it = wolf_problem_->getConstraintNotificationList().begin();
-  while ( ctr_notification_it != wolf_problem_->getConstraintNotificationList().end() )
-  {
-    if (ctr_notification_it->notification_ == REMOVE)
+    // REMOVE CONSTRAINTS
+    auto ctr_notification_it = wolf_problem_->getConstraintNotificationList().begin();
+    while ( ctr_notification_it != wolf_problem_->getConstraintNotificationList().end() )
     {
-      removeConstraint(ctr_notification_it->constraint_ptr_);
-      ctr_notification_it = wolf_problem_->getConstraintNotificationList().erase(ctr_notification_it);
-    }
-    else
-      ctr_notification_it++;
-  }
-
-  StateBlockList& states = wolf_problem_->getNotifiedStateBlockList();
-
-  for (StateBlockPtr& state : states)
-  {
-    const auto notifications = state->consumeNotifications();
-
-    for (const auto notif : notifications)
-    {
-      switch (notif)
-      {
-      case StateBlock::Notification::ADD:
-      {
-        const bool registered = state_blocks_.find(state)!=state_blocks_.end();
-
-        // call addStateBlock only if first time added.
-        if (!registered)
+        if (ctr_notification_it->notification_ == REMOVE)
         {
-          state_blocks_.emplace(state, state->getState());
-          addStateBlock(state);
+            removeConstraint(ctr_notification_it->constraint_ptr_);
+            ctr_notification_it = wolf_problem_->getConstraintNotificationList().erase(ctr_notification_it);
         }
+        else
+            ctr_notification_it++;
+    }
 
-        WOLF_DEBUG_COND(registered, "Tried adding an already registered StateBlock.");
-
-        break;
-      }
-      case StateBlock::Notification::UPDATE_STATE:
-      {
-        WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
-                        "Updating the state of an unregistered StateBlock !");
-
-        assert(state_blocks_.find(state)!=state_blocks_.end() &&
-            "Updating the state of an unregistered StateBlock !");
-
-        Eigen::VectorXs new_state = state->getState();
-        // We assume the same size for the states in both WOLF and the solver.
-        std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state));
-
-        break;
-      }
-      case StateBlock::Notification::UPDATE_FIX:
-      {
-        WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
-                        "Updating the fix state of an unregistered StateBlock !");
+    StateBlockList& states = wolf_problem_->getNotifiedStateBlockList();
 
-        assert(state_blocks_.find(state)!=state_blocks_.end() &&
-            "Updating the fix state of an unregistered StateBlock !");
+    for (StateBlockPtr& state : states)
+    {
+        const auto notifications = state->consumeNotifications();
 
-        if (state_blocks_.find(state)!=state_blocks_.end())
+        for (const auto notif : notifications)
         {
-            updateStateBlockStatus(state);
+            switch (notif)
+            {
+                case StateBlock::Notification::ADD:
+                {
+                    const bool registered = state_blocks_.find(state)!=state_blocks_.end();
+
+                    // call addStateBlock only if first time added.
+                    if (!registered)
+                    {
+                        state_blocks_.emplace(state, state->getState());
+                        addStateBlock(state);
+                    }
+
+                    WOLF_DEBUG_COND(registered, "Tried adding an already registered StateBlock.");
+
+                    break;
+                }
+                case StateBlock::Notification::UPDATE_STATE:
+                {
+                    WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
+                                    "Updating the state of an unregistered StateBlock !");
+
+                    assert(state_blocks_.find(state)!=state_blocks_.end() &&
+                           "Updating the state of an unregistered StateBlock !");
+
+                    Eigen::VectorXs new_state = state->getState();
+                    // We assume the same size for the states in both WOLF and the solver.
+                    std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state));
+
+                    break;
+                }
+                case StateBlock::Notification::UPDATE_FIX:
+                {
+                    WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
+                                    "Updating the fix state of an unregistered StateBlock !");
+
+                    assert(state_blocks_.find(state)!=state_blocks_.end() &&
+                           "Updating the fix state of an unregistered StateBlock !");
+
+                    if (state_blocks_.find(state)!=state_blocks_.end())
+                    {
+                        updateStateBlockStatus(state);
+                    }
+
+                    break;
+                }
+                case StateBlock::Notification::REMOVE:
+                {
+                    WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
+                                    "Tried to remove a StateBlock that was not added !");
+
+                    if (state_blocks_.find(state)!=state_blocks_.end())
+                    {
+                        removeStateBlock(state);
+                        state_blocks_.erase(state);
+                    }
+
+                    break;
+                }
+                default:
+                    throw std::runtime_error("SolverManager::update: State Block notification "
+                            "must be ADD, STATE_UPDATE, FIX_UPDATE or REMOVE.");
+            }
         }
+    }
 
-        break;
-      }
-      case StateBlock::Notification::REMOVE:
-      {
-        WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
-                        "Tried to remove a StateBlock that was not added !");
+    states.clear();
 
-        if (state_blocks_.find(state)!=state_blocks_.end())
+    // ADD CONSTRAINTS
+    while (!wolf_problem_->getConstraintNotificationList().empty())
+    {
+        switch (wolf_problem_->getConstraintNotificationList().front().notification_)
         {
-            removeStateBlock(state);
-            state_blocks_.erase(state);
+            case Notification::ADD:
+            {
+                addConstraint(wolf_problem_->getConstraintNotificationList().front().constraint_ptr_);
+
+                break;
+            }
+            default:
+                throw std::runtime_error("SolverManager::update:"
+                        " Constraint notification must be ADD or REMOVE.");
         }
 
-        break;
-      }
-      default:
-        throw std::runtime_error("SolverManager::update: State Block notification "
-                                 "must be ADD, STATE_UPDATE, FIX_UPDATE or REMOVE.");
-      }
+        wolf_problem_->getConstraintNotificationList().pop_front();
     }
-  }
 
-  states.clear();
+    assert(wolf_problem_->getConstraintNotificationList().empty() &&
+           "wolf problem's constraints notification list not empty after update");
+    assert(wolf_problem_->getNotifiedStateBlockList().empty() &&
+           "wolf problem's state_blocks notification list not empty after update");
 
-  // ADD CONSTRAINTS
-  while (!wolf_problem_->getConstraintNotificationList().empty())
-  {
-    switch (wolf_problem_->getConstraintNotificationList().front().notification_)
-    {
-    case Notification::ADD:
+    // UPDATE ALL STATES
+    for (StateBlockPtr& state : states)
     {
-      addConstraint(wolf_problem_->getConstraintNotificationList().front().constraint_ptr_);
-
-      break;
-    }
-    default:
-      throw std::runtime_error("SolverManager::update:"
-                               " Constraint notification must be ADD or REMOVE.");
+        Eigen::VectorXs new_state = state->getState();
+        std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state));
     }
-
-    wolf_problem_->getConstraintNotificationList().pop_front();
-  }
-
-  assert(wolf_problem_->getConstraintNotificationList().empty() &&
-         "wolf problem's constraints notification list not empty after update");
-  assert(wolf_problem_->getNotifiedStateBlockList().empty() &&
-         "wolf problem's state_blocks notification list not empty after update");
-
-  // UPDATE ALL STATES
-  for (StateBlockPtr& state : states)
-  {
-      Eigen::VectorXs new_state = state->getState();
-      std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state));
-  }
 }
 
 wolf::ProblemPtr SolverManager::getProblemPtr()
 {
-  return wolf_problem_;
+    return wolf_problem_;
 }
 
 std::string SolverManager::solve(const ReportVerbosity report_level)
 {
-  // update problem
-  update();
+    // update problem
+    update();
 
-  std::string report = solveImpl(report_level);
+    std::string report = solveImpl(report_level);
 
-  // update StateBlocks with optimized state value.
-  /// @todo whatif someone has changed the state notification during opti ??
+    // update StateBlocks with optimized state value.
+    /// @todo whatif someone has changed the state notification during opti ??
 
-  std::map<StateBlockPtr, Eigen::VectorXs>::iterator it = state_blocks_.begin(),
-                                                     it_end = state_blocks_.end();
-  for (; it != it_end; ++it)
-  {
-    // Avoid usuless copies
-    if (!it->first->isFixed())
-      it->first->setState(it->second, false);
-  }
+    std::map<StateBlockPtr, Eigen::VectorXs>::iterator it = state_blocks_.begin(),
+            it_end = state_blocks_.end();
+    for (; it != it_end; ++it)
+    {
+        // Avoid usuless copies
+        if (!it->first->isFixed())
+            it->first->setState(it->second, false);
+    }
 
-  return report;
+    return report;
 }
 
 Eigen::VectorXs& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state_ptr)
 {
-  auto it = state_blocks_.find(state_ptr);
+    auto it = state_blocks_.find(state_ptr);
 
-  if (it == state_blocks_.end())
-    throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
+    if (it == state_blocks_.end())
+        throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
 
-  return it->second;
+    return it->second;
 }
 
 Scalar* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
 {
-  auto it = state_blocks_.find(state_ptr);
+    auto it = state_blocks_.find(state_ptr);
 
-  if (it == state_blocks_.end())
-    throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
+    if (it == state_blocks_.end())
+        throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
 
-  return it->second.data();
+    return it->second.data();
 }
 
 } // namespace wolf