Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • mobile_robotics/wolf_projects/wolf_lib/wolf
1 result
Show changes
Showing
with 641 additions and 790 deletions
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. // Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of WOLF // This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify // WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// //
//--------LICENSE_END-------- // You should have received a copy of the GNU General Public License
/* // along with this program. If not, see <http://www.gnu.org/licenses/>.
* FeatureRangeBearing2d.cpp
*
* Created on: Nov 30, 2017
* Author: jsola
*/
#include "feature_range_bearing.h" #include "feature_range_bearing.h"
namespace wolf namespace wolf
{ {
FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance)
FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) : : FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance)
FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance)
{ {
// //
} }
...@@ -42,5 +31,4 @@ FeatureRangeBearing::~FeatureRangeBearing() ...@@ -42,5 +31,4 @@ FeatureRangeBearing::~FeatureRangeBearing()
// //
} }
} // namespace wolf } // namespace wolf
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. // Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of WOLF // This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify // WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// //
//--------LICENSE_END-------- // You should have received a copy of the GNU General Public License
/* // along with this program. If not, see <http://www.gnu.org/licenses/>.
* FeatureRangeBearing2d.h #pragma once
*
* Created on: Nov 30, 2017
* Author: jsola
*/
#ifndef HELLO_WOLF_FEATURE_RANGE_BEARING_H_
#define HELLO_WOLF_FEATURE_RANGE_BEARING_H_
#include "core/feature/feature_base.h" #include "core/feature/feature_base.h"
namespace wolf namespace wolf
{ {
WOLF_PTR_TYPEDEFS(FeatureRangeBearing); WOLF_PTR_TYPEDEFS(FeatureRangeBearing);
class FeatureRangeBearing : public FeatureBase class FeatureRangeBearing : public FeatureBase
{ {
public: public:
FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance); FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
~FeatureRangeBearing() override; ~FeatureRangeBearing() override;
}; };
} // namespace wolf } // namespace wolf
#endif /* HELLO_WOLF_FEATURE_RANGE_BEARING_H_ */
This diff is collapsed.
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/**
* \file landmark_point_2d.cpp
*
* Created on: Dec 4, 2017
* \author: jsola
*/
#include "landmark_point_2d.h"
#include "core/state_block/state_block_derived.h"
namespace wolf
{
LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) :
LandmarkBase("LandmarkPoint2d", std::make_shared<StatePoint2d>(_xy))
{
setId(_id);
}
LandmarkPoint2d::~LandmarkPoint2d()
{
//
}
} /* namespace wolf */
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/**
* \file landmark_point_2d.h
*
* Created on: Dec 4, 2017
* \author: jsola
*/
#ifndef HELLO_WOLF_LANDMARK_POINT_2d_H_
#define HELLO_WOLF_LANDMARK_POINT_2d_H_
#include "core/landmark/landmark_base.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(LandmarkPoint2d);
class LandmarkPoint2d : public LandmarkBase
{
public:
LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy);
~LandmarkPoint2d() override;
};
} /* namespace wolf */
#endif /* HELLO_WOLF_LANDMARK_POINT_2d_H_ */
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. // Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of WOLF // This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify // WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// //
//--------LICENSE_END-------- // You should have received a copy of the GNU General Public License
/* // along with this program. If not, see <http://www.gnu.org/licenses/>.
* processor_range_bearing.cpp
*
* Created on: Nov 30, 2017
* Author: jsola
*/
#include "processor_range_bearing.h" #include "processor_range_bearing.h"
#include "capture_range_bearing.h" #include "capture_range_bearing.h"
#include "landmark_point_2d.h" #include "core/landmark/landmark.h"
#include "core/state_block/state_block_derived.h"
#include "feature_range_bearing.h" #include "feature_range_bearing.h"
#include "factor_range_bearing.h" #include "factor_range_bearing.h"
namespace wolf namespace wolf
{ {
ProcessorRangeBearing::ProcessorRangeBearing(const YAML::Node& _params)
ProcessorRangeBearing::ProcessorRangeBearing(ParamsProcessorBasePtr _params) : : ProcessorBase("ProcessorRangeBearing", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params)
ProcessorBase("ProcessorRangeBearing", 2, _params)
{ {
// //
} }
...@@ -51,21 +41,22 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture) ...@@ -51,21 +41,22 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
// 1. get KF // 1. get KF
FrameBasePtr keyframe(nullptr); FrameBasePtr keyframe(nullptr);
if ( !buffer_frame_.empty() ) if (!buffer_frame_.empty())
{ {
// KeyFrame Callback received // KeyFrame Callback received
keyframe = buffer_frame_.select( _capture->getTimeStamp(), params_->time_tolerance ); keyframe = buffer_frame_.select(_capture->getTimeStamp(), getTimeTolerance());
buffer_frame_.removeUpTo( _capture->getTimeStamp() ); buffer_frame_.removeUpTo(_capture->getTimeStamp());
assert( keyframe && "Callback KF is not close enough to _capture!"); assert(keyframe && "Callback KF is not close enough to _capture!");
} }
if (!keyframe) if (!keyframe)
{ {
// No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below) // No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below)
keyframe = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp()); keyframe = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
assert( (fabs(keyframe->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!"); assert((fabs(keyframe->getTimeStamp() - _capture->getTimeStamp()) < getTimeTolerance()) &&
"Could not find a KF close enough to _capture!");
} }
// 2. cast incoming capture to the range-and-bearing type, add it to the keyframe // 2. cast incoming capture to the range-and-bearing type, add it to the keyframe
...@@ -76,49 +67,42 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture) ...@@ -76,49 +67,42 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++) for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++)
{ {
// extract info // extract info
int id = capture_rb->getId (i); int id = capture_rb->getId(i);
double range = capture_rb->getRange (i); double range = capture_rb->getRange(i);
double bearing = capture_rb->getBearing(i); double bearing = capture_rb->getBearing(i);
// 4. create or recover landmark // 4. create or recover landmark
LandmarkPoint2dPtr lmk; Landmark2dPtr lmk;
auto lmk_it = known_lmks.find(id); auto lmk_it = known_lmks.find(id);
if ( lmk_it != known_lmks.end() ) if (lmk_it != known_lmks.end())
{ {
// known landmarks : recover landmark // known landmarks : recover landmark
lmk = std::static_pointer_cast<LandmarkPoint2d>(lmk_it->second); lmk = std::static_pointer_cast<Landmark2d>(lmk_it->second);
WOLF_TRACE("known lmk(", id, "): ", lmk->getP()->getState().transpose()); WOLF_TRACE("known lmk(", id, "): ", lmk->getP()->getState().transpose());
} }
else else
{ {
// new landmark: // new landmark:
// - create landmark // - create landmark
lmk = LandmarkBase::emplace<LandmarkPoint2d>(getProblem()->getMap(), id, invObserve(range, bearing)); lmk = LandmarkBase::emplace<Landmark2d>(
getProblem()->getMap(), VectorComposite{{'P', invObserve(range, bearing)}}, PriorComposite());
lmk->setId(id);
WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose()); WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose());
// - add to known landmarks // - add to known landmarks
known_lmks.emplace(id, lmk); known_lmks.emplace(id, lmk);
} }
// 5. create feature // 5. create feature
Vector2d measurement_rb(range,bearing); Vector2d measurement_rb(range, bearing);
auto ftr = FeatureBase::emplace<FeatureRangeBearing>(capture_rb, auto ftr = FeatureBase::emplace<FeatureRangeBearing>(
measurement_rb, capture_rb, measurement_rb, getSensor()->computeNoiseCov(measurement_rb));
getSensor()->getNoiseCov());
// 6. create factor // 6. create factor
auto prc = shared_from_this(); auto prc = shared_from_this();
auto fac = FactorBase::emplace<FactorRangeBearing>(ftr, auto fac = FactorBase::emplace<FactorRangeBearing>(ftr, capture_rb, ftr, lmk, prc, false, FAC_ACTIVE);
capture_rb,
ftr,
lmk,
prc,
false,
FAC_ACTIVE);
} }
} }
Eigen::Vector2d ProcessorRangeBearing::observe(const Eigen::Vector2d& lmk_w) const Eigen::Vector2d ProcessorRangeBearing::observe(const Eigen::Vector2d& lmk_w) const
{ {
return polar(toSensor(lmk_w)); return polar(toSensor(lmk_w));
...@@ -131,27 +115,26 @@ Eigen::Vector2d ProcessorRangeBearing::invObserve(double r, double b) const ...@@ -131,27 +115,26 @@ Eigen::Vector2d ProcessorRangeBearing::invObserve(double r, double b) const
ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector3d& _pose) const ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector3d& _pose) const
{ {
Trf H = Eigen::Translation<double,2>(_pose(0), _pose(1)) * Eigen::Rotation2D<double>(_pose(2)) ; Trf H = Eigen::Translation<double, 2>(_pose(0), _pose(1)) * Eigen::Rotation2D<double>(_pose(2));
return H; return H;
} }
ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2d& _position, ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2d& _position,
const Eigen::Vector1d& _orientation) const const Eigen::Vector1d& _orientation) const
{ {
Trf H = Eigen::Translation<double,2>(_position(0), _position(1)) * Eigen::Rotation2D<double>(_orientation(0)) ; Trf H = Eigen::Translation<double, 2>(_position(0), _position(1)) * Eigen::Rotation2D<double>(_orientation(0));
return H; return H;
} }
Eigen::Vector2d ProcessorRangeBearing::fromSensor(const Eigen::Vector2d& lmk_s) const Eigen::Vector2d ProcessorRangeBearing::fromSensor(const Eigen::Vector2d& lmk_s) const
{ {
Trf H_w_r = transform(getProblem()->getState().vector("PO")); Trf H_w_r = transform(getProblem()->getState("PO").vector("PO"));
return H_w_r * H_r_s * lmk_s; return H_w_r * H_r_s * lmk_s;
} }
Eigen::Vector2d ProcessorRangeBearing::toSensor(const Eigen::Vector2d& lmk_w) const Eigen::Vector2d ProcessorRangeBearing::toSensor(const Eigen::Vector2d& lmk_w) const
{ {
// Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState()); Trf H_w_r = transform(getProblem()->getState("PO").vector("PO"));
Trf H_w_r = transform(getProblem()->getState().vector("PO"));
return (H_w_r * H_r_s).inverse() * lmk_w; return (H_w_r * H_r_s).inverse() * lmk_w;
} }
...@@ -170,19 +153,12 @@ Eigen::Vector2d ProcessorRangeBearing::rect(double range, double bearing) const ...@@ -170,19 +153,12 @@ Eigen::Vector2d ProcessorRangeBearing::rect(double range, double bearing) const
bool ProcessorRangeBearing::storeKeyFrame(FrameBasePtr _frame_ptr) bool ProcessorRangeBearing::storeKeyFrame(FrameBasePtr _frame_ptr)
{ {
return true; return true;
} }
bool ProcessorRangeBearing::storeCapture(CaptureBasePtr _cap_ptr) bool ProcessorRangeBearing::storeCapture(CaptureBasePtr _cap_ptr)
{ {
return false; return false;
} }
} /* namespace wolf */
// Register in the SensorFactory
#include "core/processor/factory_processor.h"
namespace wolf
{
WOLF_REGISTER_PROCESSOR(ProcessorRangeBearing) WOLF_REGISTER_PROCESSOR(ProcessorRangeBearing)
WOLF_REGISTER_PROCESSOR_AUTO(ProcessorRangeBearing) } // namespace wolf
} // namespace wolf
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. // Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of WOLF // This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify // WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// //
//--------LICENSE_END-------- // You should have received a copy of the GNU General Public License
/* // along with this program. If not, see <http://www.gnu.org/licenses/>.
* ProcessorRangeBearing.h #pragma once
*
* Created on: Nov 30, 2017
* Author: jsola
*/
#ifndef HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_
#define HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_
#include "core/processor/processor_base.h" #include "core/processor/processor_base.h"
#include "sensor_range_bearing.h" #include "sensor_range_bearing.h"
...@@ -37,79 +26,72 @@ ...@@ -37,79 +26,72 @@
namespace wolf namespace wolf
{ {
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorRangeBearing);
struct ParamsProcessorRangeBearing : public ParamsProcessorBase
{
// We do not need special parameters, but in case you need they should be defined here.
ParamsProcessorRangeBearing()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
}
ParamsProcessorRangeBearing(std::string _unique_name, const ParamsServer& _server) :
ParamsProcessorBase(_unique_name, _server)
{
//
}
std::string print() const
{
return ParamsProcessorBase::print();
}
};
using namespace Eigen; using namespace Eigen;
WOLF_PTR_TYPEDEFS(ProcessorRangeBearing); WOLF_PTR_TYPEDEFS(ProcessorRangeBearing);
class ProcessorRangeBearing : public ProcessorBase class ProcessorRangeBearing : public ProcessorBase
{ {
public: public:
typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf; typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf;
ProcessorRangeBearing(ParamsProcessorBasePtr _params); ProcessorRangeBearing(const YAML::Node& _params);
~ProcessorRangeBearing() override {/* empty */} ~ProcessorRangeBearing() override
void configure(SensorBasePtr _sensor) override; { /* empty */
}
// Factory method for high level API void configure(SensorBasePtr _sensor) override;
WOLF_PROCESSOR_CREATE(ProcessorRangeBearing, ParamsProcessorRangeBearing);
// Factory method for high level API
protected: WOLF_PROCESSOR_CREATE(ProcessorRangeBearing);
// Implementation of pure virtuals from ProcessorBase
void processCapture (CaptureBasePtr _capture) override; protected:
void processKeyFrame (FrameBasePtr _keyframe_ptr) override {}; // Implementation of pure virtuals from ProcessorBase
bool triggerInCapture (CaptureBasePtr) const override { return true;}; void processCapture(CaptureBasePtr _capture) override;
bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr) const override {return false;} void processKeyFrame(FrameBasePtr _keyframe_ptr) override{};
bool voteForKeyFrame () const override {return false;} bool triggerInCapture(CaptureBasePtr) const override
{
/** \brief store key frame return true;
* };
* Returns true if the key frame should be stored bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override
*/ {
bool storeKeyFrame(FrameBasePtr) override; return false;
}
/** \brief store capture bool voteForKeyFrame() const override
* {
* Returns true if the capture should be stored return false;
*/ }
bool storeCapture(CaptureBasePtr) override;
/** \brief store key frame
private: *
// control variables * Returns true if the key frame should be stored
Trf H_r_s; // transformation matrix, robot to sensor */
std::map<int, LandmarkBasePtr> known_lmks; // all observed lmks so far bool storeKeyFrame(FrameBasePtr) override;
protected: /** \brief store capture
// helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h *
Eigen::Vector2d observe (const Eigen::Vector2d& lmk_w) const; * Returns true if the capture should be stored
Eigen::Vector2d invObserve (double r, double b) const; */
private: bool storeCapture(CaptureBasePtr) override;
// 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::Vector3d& _pose) const; private:
Trf transform (const Eigen::Vector2d& _position, const Eigen::Vector1d& _orientation) const; // control variables
Eigen::Vector2d fromSensor (const Eigen::Vector2d& lmk_s) const; Trf H_r_s; // transformation matrix, robot to sensor
Eigen::Vector2d toSensor (const Eigen::Vector2d& lmk_w) const; std::map<int, LandmarkBasePtr> known_lmks; // all observed lmks so far
Eigen::Vector2d polar (const Eigen::Vector2d& rect) const;
Eigen::Vector2d rect (double range, double bearing) const; protected:
// helper methods -- to be used only here -- they would be better off in a separate library e.g.
// range_bearing_tools.h
Eigen::Vector2d observe(const Eigen::Vector2d& lmk_w) const;
Eigen::Vector2d invObserve(double r, double 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::Vector3d& _pose) const;
Trf transform(const Eigen::Vector2d& _position, const Eigen::Vector1d& _orientation) const;
Eigen::Vector2d fromSensor(const Eigen::Vector2d& lmk_s) const;
Eigen::Vector2d toSensor(const Eigen::Vector2d& lmk_w) const;
Eigen::Vector2d polar(const Eigen::Vector2d& rect) const;
Eigen::Vector2d rect(double range, double bearing) const;
}; };
inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor) inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor)
...@@ -118,5 +100,3 @@ inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor) ...@@ -118,5 +100,3 @@ inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor)
} }
} /* namespace wolf */ } /* namespace wolf */
#endif /* HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_ */
follow: ProcessorBase.schema
\ No newline at end of file
follow: SensorBase.schema
noise_range_metres_std:
_mandatory: true
_type: double
_doc: Standard deviation of the noise of the range measurements (meters)
noise_bearing_degrees_std:
_mandatory: true
_type: double
_doc: Standard deviation of the noise of the bearing measurements (degrees)
states:
P:
follow: StateSensorP2d.schema
O:
follow: StateSensorO2d.schema
\ No newline at end of file
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. // Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of WOLF // This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify // WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// //
//--------LICENSE_END-------- // You should have received a copy of the GNU General Public License
/* // along with this program. If not, see <http://www.gnu.org/licenses/>.
* SensorRangeBearing.cpp
*
* Created on: Nov 30, 2017
* Author: jsola
*/
#include "sensor_range_bearing.h" #include "sensor_range_bearing.h"
#include "core/state_block/state_angle.h"
#include "core/state_block/state_block_derived.h"
namespace wolf namespace wolf
{ {
WOLF_PTR_TYPEDEFS(SensorRangeBearing); WOLF_PTR_TYPEDEFS(SensorRangeBearing);
SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std) : SensorRangeBearing::SensorRangeBearing(const YAML::Node& _params)
SensorBase("SensorRangeBearing", : SensorBase("SensorRangeBearing", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params),
std::make_shared<StatePoint2d>(_extrinsics.head<2>(), true), noise_range_metres_std_(_params["noise_range_metres_std"].as<double>()),
std::make_shared<StateAngle>(_extrinsics(2), true), noise_bearing_degrees_std_(_params["noise_bearing_degrees_std"].as<double>())
nullptr,
_noise_std)
{ {
assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2d");
} }
SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr) : SensorRangeBearing::~SensorRangeBearing()
SensorRangeBearing(_extrinsics, Eigen::Vector2d(_intr->noise_range_metres_std, toRad(_intr->noise_bearing_degrees_std)))
{ {
// //
} }
SensorRangeBearing::~SensorRangeBearing() Eigen::MatrixXd SensorRangeBearing::computeNoiseCov(const Eigen::VectorXd& _data) const
{ {
// return (Eigen::Vector2d() << noise_range_metres_std_, noise_bearing_degrees_std_)
.finished()
.cwiseAbs2()
.asDiagonal();
} }
} /* namespace wolf */
// Register in the SensorFactory
#include "core/sensor/factory_sensor.h"
namespace wolf
{
WOLF_REGISTER_SENSOR(SensorRangeBearing) WOLF_REGISTER_SENSOR(SensorRangeBearing)
WOLF_REGISTER_SENSOR_AUTO(SensorRangeBearing) } // namespace wolf
} // namespace wolf
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. // Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // All rights reserved.
// //
// This file is part of WOLF // This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify // WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// //
//--------LICENSE_END-------- // You should have received a copy of the GNU General Public License
/* // along with this program. If not, see <http://www.gnu.org/licenses/>.
* SensorRangeBearing.h #pragma once
*
* Created on: Nov 30, 2017
* Author: jsola
*/
#ifndef HELLO_WOLF_SENSOR_RANGE_BEARING_H_
#define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
#include "core/sensor/sensor_base.h" #include "core/sensor/sensor_base.h"
#include "core/utils/params_server.h" #include "yaml-cpp/yaml.h"
namespace wolf namespace wolf
{ {
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorRangeBearing);
struct ParamsSensorRangeBearing : public ParamsSensorBase
{
double noise_range_metres_std = 0.05;
double noise_bearing_degrees_std = 0.5;
ParamsSensorRangeBearing()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
}
ParamsSensorRangeBearing(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
noise_range_metres_std = _server.getParam<double>(prefix + _unique_name + "/noise_range_metres_std");
noise_bearing_degrees_std = _server.getParam<double>(prefix + _unique_name + "/noise_bearing_degrees_std");
}
std::string print() const
{
return ParamsSensorBase::print() + "\n"
+ "noise_range_metres_std: " + std::to_string(noise_range_metres_std) + "\n"
+ "noise_bearing_degrees_std: " + std::to_string(noise_bearing_degrees_std) + "\n";
}
};
WOLF_PTR_TYPEDEFS(SensorRangeBearing) WOLF_PTR_TYPEDEFS(SensorRangeBearing)
class SensorRangeBearing : public SensorBase class SensorRangeBearing : public SensorBase
{ {
public: private:
SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std); double noise_range_metres_std_ = 0.05;
SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr); double noise_bearing_degrees_std_ = 0.5;
WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing, 3);
~SensorRangeBearing() override; SensorRangeBearing(const YAML::Node& _params);
public:
WOLF_SENSOR_CREATE(SensorRangeBearing);
~SensorRangeBearing() override;
Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
}; };
} /* namespace wolf */ } /* namespace wolf */
#endif /* HELLO_WOLF_SENSOR_RANGE_BEARING_H_ */
config: problem:
problem: dimension: 2 # space is 2d
frame_structure: "PO" # keyframes have position and orientation
first_frame:
P:
value: [0,0]
prior:
mode: "factor"
factor_std: [0.31, 0.31]
O:
value: [0]
prior:
mode: "factor"
factor_std: [0.31]
tree_manager:
type: "none"
solver:
type: SolverCeres
plugin: core
max_num_iterations: 100
verbose: 0
period: 0.2
interrupt_on_problem_change: false
n_threads: 2
compute_cov: false
minimizer: levenberg_marquardt
use_nonmonotonic_steps: false # only for levenberg_marquardt and DOGLEG
parameter_tolerance: 1e-6
function_tolerance: 1e-6
gradient_tolerance: 1e-10
dimension: 2 # space is 2d sensors:
frame_structure: "PO" # keyframes have position and orientation
prior: - type: "SensorOdom2d"
mode: "factor" plugin: "core"
$state: name: "sen odom"
P: [0,0] follow: "sensor_odom_2d.yaml" # config parameters in this file
O: [0]
$sigma:
P: [0.31, 0.31]
O: [0.31]
time_tolerance: 0.5
tree_manager: - type: "SensorRangeBearing"
type: "none" plugin: "core"
plugin: "core" name: "sen rb"
noise_range_metres_std: 0.2 # parser only considers first appearence so the following file parsing will not overwrite this param
follow: sensor_range_bearing.yaml # config parameters in this file
processors:
solver: - type: "ProcessorOdom2d"
max_num_iterations: 100 plugin: "core"
verbose: 0 name: "prc odom"
period: 0.2 sensor_name: "sen odom" # attach processor to this sensor
interrupt_on_problem_change: false follow: processor_odom_2d.yaml # config parameters in this file
n_threads: 2
compute_cov: false - type: "ProcessorRangeBearing"
minimizer: LEVENBERG_MARQUARDT plugin: "core"
use_nonmonotonic_steps: false # only for LEVENBERG_MARQUARDT and DOGLEG name: "prc rb"
function_tolerance: 0.000001 sensor_name: "sen rb" # attach processor to this sensor
gradient_tolerance: 0.0000000001 follow: processor_range_bearing.yaml # config parameters in this file
sensors:
- type: "SensorOdom2d"
plugin: "core"
name: "sen odom"
extrinsic:
pose: [0,0, 0]
follow: "demos/hello_wolf/yaml/sensor_odom_2d.yaml" # config parameters in this file
- type: "SensorRangeBearing"
plugin: "core"
name: "sen rb"
extrinsic:
pose: [1,1, 0]
noise_range_metres_std: 0.2 # parser only considers first appearence so the following file parsing will not overwrite this param
follow: demos/hello_wolf/yaml/sensor_range_bearing.yaml # config parameters in this file
processors:
- type: "ProcessorOdom2d"
plugin: "core"
name: "prc odom"
sensor_name: "sen odom" # attach processor to this sensor
follow: demos/hello_wolf/yaml/processor_odom_2d.yaml # config parameters in this file
- type: "ProcessorRangeBearing"
plugin: "core"
name: "prc rb"
sensor_name: "sen rb" # attach processor to this sensor
follow: demos/hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file
#type: "ProcessorOdom2d"
time_tolerance: 0.1 time_tolerance: 0.1
unmeasured_perturbation_std: 0.001 unmeasured_perturbation_std: 0.001
keyframe_vote: keyframe_vote:
voting_active: true voting_active: true
max_time_span: 999 max_time_span: 999
dist_traveled: 0.95 max_dist_traveled: 0.95
angle_turned: 999 max_angle_turned: 999
max_buff_length: 999 max_buff_length: 999
cov_det: 999 max_cov_det: 999
apply_loss_function: true apply_loss_function: true
state_getter: true state_provider: true
state_priority: 1 state_provider_order: 1
#type: "ProcessorRangeBearing"
time_tolerance: 0.1 time_tolerance: 0.1
keyframe_vote: keyframe_vote:
......
#type: "SensorOdom2d" # This must match the KEY used in the FactorySensor. Otherwise it is an error.
k_disp_to_disp: 0.1 # m^2 / m k_disp_to_disp: 0.1 # m^2 / m
k_rot_to_rot: 0.1 # rad^2 / rad k_rot_to_rot: 0.1 # rad^2 / rad
k_disp_to_rot: 0.0 # m^2 / rad
min_disp_var: 0.0
min_rot_var: 0.0
apply_loss_function: true apply_loss_function: true
states:
P:
prior:
mode: fix
value: [0,0]
dynamic: false
O:
prior:
mode: fix
value: [0]
dynamic: false
\ No newline at end of file
#type: "SensorRangeBearing"
noise_range_metres_std: 0.1 noise_range_metres_std: 0.1
noise_bearing_degrees_std: 0.5 noise_bearing_degrees_std: 0.5
apply_loss_function: true apply_loss_function: true
states:
P:
prior:
mode: fix
value: [1,1]
dynamic: false
O:
prior:
mode: fix
value: [0]
dynamic: false
\ No newline at end of file
type: "SensorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
k_disp_to_disp: 0.02 # m^2 / m
k_disp_to_rot: 0.02 # rad^2 / m
k_rot_to_rot: 0.01 # rad^2 / rad
min_disp_var: 0.01 # m^2
min_rot_var: 0.01 # rad^2
type: "SensorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
k_disp_to_disp: 0.000001 # m^2 / m
k_disp_to_rot: 0.000001 # rad^2 / m
k_rot_to_rot: 0.000001 # rad^2 / rad
min_disp_var: 0.00000001 # m^2
min_rot_var: 0.00000001 # rad^2
ADD_EXECUTABLE(demo_wolf_imported_graph demo_wolf_imported_graph.cpp)
TARGET_LINK_LIBRARIES(demo_wolf_imported_graph ${PLUGIN_NAME})
ADD_EXECUTABLE(demo_analytic_autodiff_comparison demo_analytic_autodiff_comparison.cpp)
TARGET_LINK_LIBRARIES(demo_analytic_autodiff_comparison ${PLUGIN_NAME})