diff --git a/src/hello_wolf/hello_wolf.cpp b/src/hello_wolf/hello_wolf.cpp index d3e0aa4f240f34c6d9c17ff0912783ca3131dc9b..cd14a779efb7fa38be48b1815a86d1cf09a61214 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 b2c66924f2292f30884b0d3dbebf2f31758bfbf7..5023023b603d5aa160cf84ddb5719b47550e39b2 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 */