From 290088a1249876430d61069cb34100f7ab33ec2f Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 29 Nov 2018 17:14:24 +0100
Subject: [PATCH] small
---
src/frame_base.cpp | 2 +-
src/hello_wolf/processor_range_bearing.cpp | 10 +++++-----
2 files changed, 6 insertions(+), 6 deletions(-)
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 0891b05cd..453e54cb5 100644
--- a/src/frame_base.cpp
+++ b/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),
diff --git a/src/hello_wolf/processor_range_bearing.cpp b/src/hello_wolf/processor_range_bearing.cpp
index 5023023b6..b2c66924f 100644
--- a/src/hello_wolf/processor_range_bearing.cpp
+++ b/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 */
--
GitLab