Merge branch 'hello_wolf' into 'master'

Hello wolf

See merge request mobile_robotics/wolf!214

src/capture_base.cpp

@@ -57,7 +57,7 @@ CaptureBase::CaptureBase(const std::string& _type,
     }
     updateCalibSize();
 
-    WOLF_TRACE("New Capture ", id(), " -- type ", getType(), " -- t = ", getTimeStamp(), " s");
+//    WOLF_TRACE("New Capture ", id(), " -- type ", getType(), " -- t = ", getTimeStamp(), " s");
 }
 
 

src/frame_base.cpp

@@ -23,9 +23,6 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
     state_block_vec_[0] = _p_ptr;
     state_block_vec_[1] = _o_ptr;
     state_block_vec_[2] = _v_ptr;
-
-    if ( isKey() )
-        registerNewStateBlocks();
 }
 
 FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
@@ -40,9 +37,6 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
     state_block_vec_[0] = _p_ptr;
     state_block_vec_[1] = _o_ptr;
     state_block_vec_[2] = _v_ptr;
-
-    if ( isKey() )
-        registerNewStateBlocks();
 }
                 
 FrameBase::~FrameBase()

src/hello_wolf/hello_wolf.cpp

 /**
  * \file hello_wolf.cpp
  *
- *  Created on: Dec 1, 2017 -- two months exactly after Oct-1st, we still have 4 political prisoners.
+ *  Created on: Dec 1, 2017  -- two months exactly after Oct-1st, we still have 4 political prisoners.
  *
  *     ###
  *    ## ##
@@ -100,7 +100,7 @@ int main()
      *   - Second, using random values
      * Both solutions must produce the same exact values as in the sketches above.
      *
-     * Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line 139)
+     * Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line 141)
      *
      * (c) 2017 Joan Sola @ IRI-CSIC
      */
@@ -136,8 +136,10 @@ int main()
     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)
+
+    // NOTE: uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
     // sensor_rb->getOPtr()->unfix();
+    // sensor_rb->getPPtr()->unfix(); // The position is however not observable, and thus self-calibration would not work. You can try uncommenting it too.
 
     // processor Range and Bearing
     ProcessorParamsRangeBearingPtr params_rb = std::make_shared<ProcessorParamsRangeBearing>();
@@ -157,6 +159,7 @@ int main()
 
 
     // SET OF EVENTS =======================================================
+    std::cout << std::endl;
     WOLF_TRACE("======== BUILD PROBLEM =======")
 
     // We'll do 3 steps of motion and landmark observations.
@@ -205,9 +208,9 @@ 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);
+    sensor_rb   ->process(cap_rb);          // L1 : (1,2), L2 : (2,2), L3 : (3,2)
 
-    problem->print(4,1,1,1);
+    problem->print(1,0,1,0);
 
 
     // SOLVE ================================================================
@@ -216,7 +219,7 @@ int main()
     WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
     std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
     WOLF_TRACE(report);                     // should show a very low iteration number (possibly 1)
-    problem->print(4,1,1,1);
+    problem->print(1,0,1,0);
 
     // PERTURB initial guess
     WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
@@ -227,14 +230,16 @@ int main()
     for (auto kf : problem->getTrajectoryPtr()->getFrameList())
         kf->setState(Vector3s::Random() * 0.5);                 // We perturb A LOT !
     for (auto lmk : problem->getMapPtr()->getLandmarkList())
-        lmk->getPPtr()->setState(Vector2s::Random());           // We perturb A LOT !
-    problem->print(4,1,1,1);
+        for (auto sb : lmk->getStateBlockVec())
+            if (sb && !sb->isFixed())
+                sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);       // We perturb A LOT !
+    problem->print(1,0,1,0);
 
     // SOLVE again
     WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
     report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
     WOLF_TRACE(report);                     // should show a very high iteration number (more than 10, or than 100!)
-    problem->print(4,1,1,1);
+    problem->print(1,0,1,0);
 
     // GET COVARIANCES of all states
     WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
@@ -244,6 +249,10 @@ int main()
             WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance());
     for (auto lmk : problem->getMapPtr()->getLandmarkList())
         WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance());
+    std::cout << std::endl;
+
+    WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
+    problem->print(4,1,1,1);
 
     /*
      * ============= FIRST COMMENT ON THE RESULTS ==================
@@ -260,9 +269,7 @@ int main()
      *
      *  - Observe that all other KFs and Lmks are correct.
      *
-     *  - Observe that F4 is not correct. Since it is not a KF, is has not been estimated.
-     *    But this is a no-issue because F4 is just an inner frame used by the odometer processor,
-     *    with no role in the problem itself, nor in the optimization process.
+     *  - Try self-calibrating the sensor orientation by uncommenting line 141 (well, around 141)
      *
      */
 

src/hello_wolf/processor_range_bearing.cpp

@@ -23,38 +23,39 @@ ProcessorRangeBearing::ProcessorRangeBearing(const SensorRangeBearingPtr _sensor
 void ProcessorRangeBearing::process(CaptureBasePtr _capture)
 {
 
+    // 1. get KF
+    FrameBasePtr kf(nullptr);
     if ( !kf_pack_buffer_.empty() )
     {
-        // Select using incoming_ptr
+        // KeyFrame Callback received
         KFPackPtr pack = kf_pack_buffer_.selectPack( _capture->getTimeStamp(), params_->time_tolerance );
 
         if (pack!=nullptr)
-            keyFrameCallback(pack->key_frame,pack->time_tolerance);
+            kf = pack->key_frame;
 
         kf_pack_buffer_.removeUpTo( _capture->getTimeStamp() );
-    }
 
-    CaptureRangeBearingPtr capture = std::static_pointer_cast<CaptureRangeBearing>(_capture);
+        assert( kf && "Callback KF is not close enough to _capture!");
+    }
 
-    // 1. get KF -- we assume a KF is available to hold this _capture (checked in assert below)
-    auto kf = getProblem()->closestKeyFrameToTimeStamp(capture->getTimeStamp());
-    assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
+    if (!kf)
+    {
+        // No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below)
+        kf = getProblem()->closestKeyFrameToTimeStamp(_capture->getTimeStamp());
+        assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
+    }
 
-    // 2. create Capture
-    auto cap = std::make_shared<CaptureRangeBearing>(capture->getTimeStamp(),
-                                                     getSensorPtr(),
-                                                     capture->getIds(),
-                                                     capture->getRanges(),
-                                                     capture->getBearings());
-    kf->addCapture(cap);
+    // 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);
 
     // 3. explore all observations in the capture
-    for (SizeEigen i = 0; i < capture->getIds().size(); i++)
+    for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++)
     {
         // extract info
-        int     id      = capture->getId     (i);
-        Scalar  range   = capture->getRange  (i);
-        Scalar  bearing = capture->getBearing(i);
+        int     id      = capture_rb->getId     (i);
+        Scalar  range   = capture_rb->getRange  (i);
+        Scalar  bearing = capture_rb->getBearing(i);
 
         // 4. create or recover landmark
         LandmarkPoint2DPtr lmk;
@@ -80,11 +81,11 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
         Vector2s rb(range,bearing);
         auto ftr = std::make_shared<FeatureRangeBearing>(rb,
                                                          getSensorPtr()->getNoiseCov());
-        cap->addFeature(ftr);
+        capture_rb->addFeature(ftr);
 
         // 6. create constraint
         auto prc = shared_from_this();
-        auto ctr = std::make_shared<ConstraintRangeBearing>(cap,
+        auto ctr = std::make_shared<ConstraintRangeBearing>(capture_rb,
                                                             lmk,
                                                             prc,
                                                             false,
@@ -95,16 +96,6 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
 
 }
 
-Eigen::Vector2s ProcessorRangeBearing::observe(const Eigen::Vector2s& lmk_w) const
-{
-    return polar(toSensor(lmk_w));
-}
-
-Eigen::Vector2s ProcessorRangeBearing::invObserve(Scalar r, Scalar b) const
-{
-    return fromSensor(rect(r, b));
-}
-
 ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
 {
     SensorRangeBearingPtr       sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sen_ptr);
@@ -119,17 +110,26 @@ ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name,
     return prc;
 }
 
+Eigen::Vector2s ProcessorRangeBearing::observe(const Eigen::Vector2s& lmk_w) const
+{
+    return polar(toSensor(lmk_w));
+}
+
 ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector3s& _pose) const
 {
     Trf H = Eigen::Translation<Scalar,2>(_pose(0), _pose(1)) * Eigen::Rotation2D<Scalar>(_pose(2)) ;
     return H;
 }
 
+ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2s& _position,
+                                                            const Eigen::Vector1s& _orientation) const
+{
+    Trf H = Eigen::Translation<Scalar,2>(_position(0), _position(1)) * Eigen::Rotation2D<Scalar>(_orientation(0)) ;
+    return H;
+}
+
 Eigen::Vector2s ProcessorRangeBearing::fromSensor(const Eigen::Vector2s& lmk_s) const
 {
-//    Eigen::Vector2s pos_s = getSensorPtr()->getPPtr()->getState();
-//    Eigen::Vector1s ori_s = getSensorPtr()->getOPtr()->getState();
-//    Trf H_w_r = transform(pos_s, ori_s);
     Trf H_w_r = transform(getProblem()->getCurrentState());
     return H_w_r * H_r_s * lmk_s;
 }
@@ -149,21 +149,14 @@ Eigen::Vector2s ProcessorRangeBearing::polar(const Eigen::Vector2s& rect) const
     return polar;
 }
 
-void ProcessorRangeBearing::keyFrameCallback(FrameBasePtr _key_frame, const Scalar& _time_tolerance)
-{
-    //
-}
-
 Eigen::Vector2s ProcessorRangeBearing::rect(Scalar range, Scalar bearing) const
 {
     return range * (Vector2s() << cos(bearing), sin(bearing)).finished();
 }
 
-ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2s& _position,
-                                                            const Eigen::Vector1s& _orientation) const
+Eigen::Vector2s ProcessorRangeBearing::invObserve(Scalar r, Scalar b) const
 {
-    Trf H = Eigen::Translation<Scalar,2>(_position(0), _position(1)) * Eigen::Rotation2D<Scalar>(_orientation(0)) ;
-    return H;
+    return fromSensor(rect(r, b));
 }
 
 } /* namespace wolf */

src/hello_wolf/processor_range_bearing.h

@@ -21,7 +21,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsRangeBearing);
 
 struct ProcessorParamsRangeBearing : public ProcessorParamsBase
 {
-        //        Eigen::Vector3s pose0, delta;
+        // We do not need special parameters, but in case you need they should be defined here.
 };
 
 
@@ -46,17 +46,16 @@ class ProcessorRangeBearing : public ProcessorBase
         // Implementation of pure virtuals from ProcessorBase
         virtual void process            (CaptureBasePtr _capture) override;
         virtual bool voteForKeyFrame    () override {return false;}
-        virtual void keyFrameCallback   (FrameBasePtr _key_frame, const Scalar& _time_tolerance) override;
-
-        // landmark observation models -- they would be better off in a separate library e.g. range_bearing_tools.h
-        Eigen::Vector2s observe     (const Eigen::Vector2s& lmk_w) const;
-        Eigen::Vector2s invObserve  (Scalar r, Scalar b) const;
 
     private:
         // control variables
         Trf H_r_s; // transformation matrix, robot to sensor
         std::map<int, LandmarkBasePtr> known_lmks; // all observed lmks so far
 
+    protected:
+        // helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h
+        Eigen::Vector2s observe     (const Eigen::Vector2s& lmk_w) const;
+        Eigen::Vector2s invObserve  (Scalar r, Scalar b) const;
     private:
         // helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h
         Trf             transform   (const Eigen::Vector3s& _pose) const;

src/sensors/sensor_diff_drive.h

@@ -15,7 +15,7 @@
 namespace wolf {
 
 WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsDiffDrive);
-WOLF_STRUCT_PTR_TYPEDEFS(SensorDiffDrive);
+WOLF_PTR_TYPEDEFS(SensorDiffDrive);
 
 struct IntrinsicsDiffDrive : public IntrinsicsBase
 {

src/trajectory_base.cpp

@@ -89,7 +89,6 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
         if ((*frm_rit)->isKey())
         {
             Scalar dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
-            std::cout << "dt " << dt << std::endl;
             if (dt < min_dt)
             {
                 min_dt = dt;