diff --git a/src/hello_wolf/hello_wolf.cpp b/src/hello_wolf/hello_wolf.cpp
index c4f3103e0970af884a5184f39de340d078cd31d1..d3e0aa4f240f34c6d9c17ff0912783ca3131dc9b 100644
--- a/src/hello_wolf/hello_wolf.cpp
+++ b/src/hello_wolf/hello_wolf.cpp
@@ -1,7 +1,7 @@
/**
* \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,6 +136,7 @@ int main()
intrinsics_rb->noise_range_metres_std = 0.1;
intrinsics_rb->noise_bearing_degrees_std = 1.0;
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, since the position is not observable)
// sensor_rb->getOPtr()->unfix();
@@ -270,7 +271,7 @@ int main()
*
* - Observe that all other KFs and Lmks are correct.
*
- * - Try self-calibrating the sensor orientation by uncommenting line 140 (well, around 140)
+ * - Try self-calibrating the sensor orientation by uncommenting line 141 (well, around 141)
*
*/
diff --git a/src/hello_wolf/processor_range_bearing.cpp b/src/hello_wolf/processor_range_bearing.cpp
index 0252e6622cdd48acb65ca51ef2c6b1023805b080..b2c66924f2292f30884b0d3dbebf2f31758bfbf7 100644
--- a/src/hello_wolf/processor_range_bearing.cpp
+++ b/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,31 @@ 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));
+}
+
+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)) ;
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,23 +154,11 @@ 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
-{
- Trf H = Eigen::Translation<Scalar,2>(_position(0), _position(1)) * Eigen::Rotation2D<Scalar>(_orientation(0)) ;
- return H;
-}
-
} /* namespace wolf */
diff --git a/src/hello_wolf/processor_range_bearing.h b/src/hello_wolf/processor_range_bearing.h
index 5a5f1c82fe5f49946a5a212c13bc6068088e9ef0..5d3d2a8dd83b2390c204d2a0d58e9239ca233548 100644
--- a/src/hello_wolf/processor_range_bearing.h
+++ b/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;