small

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 6 state blocks by default. Resize in derived constructors if needed.
+            state_block_vec_(3), // allow for 3 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/processor_range_bearing.cpp

@@ -115,6 +115,11 @@ Eigen::Vector2s ProcessorRangeBearing::observe(const Eigen::Vector2s& lmk_w) con
     return polar(toSensor(lmk_w));
 }
 
+Eigen::Vector2s ProcessorRangeBearing::invObserve(Scalar r, Scalar b) const
+{
+    return fromSensor(rect(r, b));
+}
+
 ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector3s& _pose) const
 {
     Trf H = Eigen::Translation<Scalar,2>(_pose(0), _pose(1)) * Eigen::Rotation2D<Scalar>(_pose(2)) ;
@@ -154,11 +159,6 @@ Eigen::Vector2s ProcessorRangeBearing::rect(Scalar range, Scalar bearing) const
     return range * (Vector2s() << cos(bearing), sin(bearing)).finished();
 }
 
-Eigen::Vector2s ProcessorRangeBearing::invObserve(Scalar r, Scalar b) const
-{
-    return fromSensor(rect(r, b));
-}
-
 } /* namespace wolf */