From 84d82abac575359e3326b20c5092d06dada770a6 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 29 Nov 2018 14:26:37 +0100
Subject: [PATCH] Small updates
---
src/hello_wolf/hello_wolf.cpp | 1 +
src/hello_wolf/processor_range_bearing.cpp | 10 +++++-----
2 files changed, 6 insertions(+), 5 deletions(-)
diff --git a/src/hello_wolf/hello_wolf.cpp b/src/hello_wolf/hello_wolf.cpp
index d3e0aa4f2..cd14a779e 100644
--- a/src/hello_wolf/hello_wolf.cpp
+++ b/src/hello_wolf/hello_wolf.cpp
@@ -139,6 +139,7 @@ int main()
// 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>();
diff --git a/src/hello_wolf/processor_range_bearing.cpp b/src/hello_wolf/processor_range_bearing.cpp
index b2c66924f..5023023b6 100644
--- a/src/hello_wolf/processor_range_bearing.cpp
+++ b/src/hello_wolf/processor_range_bearing.cpp
@@ -115,11 +115,6 @@ 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)) ;
@@ -159,6 +154,11 @@ 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