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
View open merge request
Hide whitespace changes
Inline Side-by-side
Showing
with 1644 additions and 1896 deletions
include/core/factor/factor_velocity_local_direction_3d.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------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/>.
#pragma once
#ifndef FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_
#define FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_
//Wolf includes
// Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/math/rotations.h"
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorVelocityLocalDirection3d);
//class
class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalDirection3d,2,3,4>
// class
class FactorVelocityLocalDirection3d : public FactorAutodiff<FactorVelocityLocalDirection3d, 2, 3, 4>
{
protected:
double min_vel_sq_norm_; // stored in squared norm for efficiency
int orthogonal_axis_; // 0: X, 1: Y, 2: Z
public:
FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr,
const double& _min_vel_norm,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
~FactorVelocityLocalDirection3d() override = default;
template<typename T>
bool operator ()(const T* const _v, const T* const _o, T* _residuals) const;
template<typename T>
Eigen::Matrix<T,2,1> computeElevationAzimuth(const Eigen::Matrix<T,3,1> vector) const
{
Eigen::Matrix<T,2,1> elev_azim;
// plane YZ
if (orthogonal_axis_ == 0)
{
elev_azim(0) = asin(vector(0) / vector.norm());
elev_azim(1) = atan2(vector(2), vector(1));
}
// plane XZ
if (orthogonal_axis_ == 1)
{
elev_azim(0) = asin(vector(1) / vector.norm());
elev_azim(1) = atan2(vector(0), vector(2));
}
// plane XY
if (orthogonal_axis_ == 2)
{
elev_azim(0) = asin(vector(2) / vector.norm());
elev_azim(1) = atan2(vector(1), vector(0));
}
return elev_azim;
}
protected:
double min_vel_sq_norm_; // stored in squared norm for efficiency
int orthogonal_axis_; // 0: X, 1: Y, 2: Z
public:
FactorVelocityLocalDirection3d(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_reference,
const double& _min_vel_norm,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
~FactorVelocityLocalDirection3d() override = default;
template <typename T>
bool operator()(const T* const _v, const T* const _o, T* _residuals) const;
template <typename T>
Eigen::Matrix<T, 2, 1> computeElevationAzimuth(const Eigen::Matrix<T, 3, 1> vector,
const int& orthogonal_axis) const;
};
FactorVelocityLocalDirection3d::FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr,
const double& _min_vel_norm,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff<FactorVelocityLocalDirection3d,2,3,4>("FactorVelocityLocalDirection3d",
TOP_ABS,
_ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getV(),
_ftr_ptr->getFrame()->getO()),
min_vel_sq_norm_(_min_vel_norm*_min_vel_norm)
FactorVelocityLocalDirection3d::FactorVelocityLocalDirection3d(
const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_reference,
const double& _min_vel_norm,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status)
: FactorAutodiff<FactorVelocityLocalDirection3d, 2, 3, 4>(
"FactorVelocityLocalDirection3d",
TOP_ABS,
_measurement,
_measurement_sqrt_information_upper,
{_node_reference},
_processor_ptr,
{_node_reference->getStateBlock('V'), _node_reference->getO()},
_apply_loss_function,
_status),
min_vel_sq_norm_(_min_vel_norm * _min_vel_norm)
{
MatrixSizeCheck<3,1>::check(_ftr_ptr->getMeasurement());
MatrixSizeCheck<1,1>::check(_ftr_ptr->getMeasurementSquareRootInformationUpper());
assert(_node_reference->getStateBlock('V') &&
"FactorVelocityLocalDirection3d: _node_reference state V not found!");
assert(_node_reference->getO() && "FactorVelocityLocalDirection3d: _node_reference state O not found!");
MatrixSizeCheck<3, 1>::check(_measurement);
MatrixSizeCheck<1, 1>::check(_measurement_sqrt_information_upper);
/* Decide the plane (two axis A1, A2 from X, Y, Z in local coordinates) in which compute elevation and azimuth
* - elevation w.r.t. the plane
......@@ -110,16 +91,16 @@ FactorVelocityLocalDirection3d::FactorVelocityLocalDirection3d(FeatureBasePtr _f
measurement_.minCoeff(&orthogonal_axis_);
// measurement: elevation & azimuth (w.r.t. selected plane)
measurement_ = computeElevationAzimuth(Eigen::Vector3d(measurement_));
measurement_sqrt_information_upper_ = Matrix2d::Identity() * measurement_sqrt_information_upper_(0,0);
measurement_ = computeElevationAzimuth(Eigen::Vector3d(measurement_), orthogonal_axis_);
measurement_sqrt_information_upper_ = Eigen::Matrix2d::Identity() * measurement_sqrt_information_upper_(0, 0);
}
template<typename T>
inline bool FactorVelocityLocalDirection3d::operator ()(const T* const _v, const T* const _q, T* _residuals) const
template <typename T>
inline bool FactorVelocityLocalDirection3d::operator()(const T* const _v, const T* const _q, T* _residuals) const
{
Eigen::Map<const Eigen::Matrix<T,3,1> > v(_v);
Eigen::Map<const Eigen::Quaternion<T> > q(_q);
Eigen::Map<Eigen::Matrix<T,2,1> > residuals(_residuals);
Eigen::Map<const Eigen::Matrix<T, 3, 1> > v(_v);
Eigen::Map<const Eigen::Quaternion<T> > q(_q);
Eigen::Map<Eigen::Matrix<T, 2, 1> > residuals(_residuals);
if (v.squaredNorm() < min_vel_sq_norm_)
{
......@@ -128,43 +109,70 @@ inline bool FactorVelocityLocalDirection3d::operator ()(const T* const _v, const
return true;
}
// std::cout << "----------\n";
// std::cout << "desired elevation: " << getMeasurement()(0) << "\n";
// std::cout << "desired azimuth: " << getMeasurement()(1) << "\n";
// std::cout << "----------\n";
// std::cout << "desired elevation: " << getMeasurement()(0) << "\n";
// std::cout << "desired azimuth: " << getMeasurement()(1) << "\n";
// std::cout << "v: \n\t" << v(0) << "\n\t"
// << v(1) << "\n\t"
// << v(2) << "\n";
//
// std::cout << "q: \n\t" << q.coeffs()(0) << "\n\t"
// << q.coeffs()(1) << "\n\t"
// << q.coeffs()(2) << "\n\t"
// << q.coeffs()(3) << "\n";
// std::cout << "v: \n\t" << v(0) << "\n\t"
// << v(1) << "\n\t"
// << v(2) << "\n";
//
// std::cout << "q: \n\t" << q.coeffs()(0) << "\n\t"
// << q.coeffs()(1) << "\n\t"
// << q.coeffs()(2) << "\n\t"
// << q.coeffs()(3) << "\n";
Eigen::Matrix<T,3,1> v_local = q.conjugate() * v;
// std::cout << "v (local): \n\t" << v_local(0) << "\n\t"
// << v_local(1) << "\n\t"
// << v_local(2) << "\n";
Eigen::Matrix<T, 3, 1> v_local = q.conjugate() * v;
// std::cout << "v (local): \n\t" << v_local(0) << "\n\t"
// << v_local(1) << "\n\t"
// << v_local(2) << "\n";
// expectation
Eigen::Matrix<T,2,1> exp_elev_azim = computeElevationAzimuth(v_local);
// std::cout << "expected elevation: " << exp_elev_azim(0) << "\n";
// std::cout << "expected azimuth: " << exp_elev_azim(1) << "\n";
Eigen::Matrix<T, 2, 1> exp_elev_azim = computeElevationAzimuth(v_local, orthogonal_axis_);
// std::cout << "expected elevation: " << exp_elev_azim(0) << "\n";
// std::cout << "expected azimuth: " << exp_elev_azim(1) << "\n";
// error
Eigen::Matrix<T,2,1> error = getMeasurement() - exp_elev_azim;
Eigen::Matrix<T, 2, 1> error = getMeasurement() - exp_elev_azim;
pi2pi(error(1));
// std::cout << "error (corrected): \n\t" << error(0) << "\n\t"
// << error(1) << "\n;
// std::cout << "error (corrected): \n\t" << error(0) << "\n\t"
// << error(1) << "\n;
// residuals
residuals = getMeasurementSquareRootInformationUpper() * error;
// std::cout << "residuals: \n\t" << residuals(0) << "\n\t"
// << residuals(1) << "\n;
// std::cout << "residuals: \n\t" << residuals(0) << "\n\t"
// << residuals(1) << "\n;
return true;
}
} // namespace wolf
template <typename T>
inline Eigen::Matrix<T, 2, 1> FactorVelocityLocalDirection3d::computeElevationAzimuth(
const Eigen::Matrix<T, 3, 1> vector,
const int& orthogonal_axis) const
{
Eigen::Matrix<T, 2, 1> elev_azim;
// plane YZ
if (orthogonal_axis == 0)
{
elev_azim(0) = asin(vector(0) / vector.norm());
elev_azim(1) = atan2(vector(2), vector(1));
}
// plane XZ
if (orthogonal_axis == 1)
{
elev_azim(0) = asin(vector(1) / vector.norm());
elev_azim(1) = atan2(vector(0), vector(2));
}
// plane XY
if (orthogonal_axis == 2)
{
elev_azim(0) = asin(vector(2) / vector.norm());
elev_azim(1) = atan2(vector(1), vector(0));
}
return elev_azim;
}
#endif
} // namespace wolf
include/core/feature/feature_base.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef FEATURE_BASE_H_
#define FEATURE_BASE_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Forward declarations for node templates
namespace wolf{
namespace wolf
{
class CaptureBase;
class FactorBase;
}
} // namespace wolf
//Wolf includes
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
//std includes
namespace wolf {
// std includes
//class FeatureBase
namespace wolf
{
// class FeatureBase
class FeatureBase : public NodeBase, public std::enable_shared_from_this<FeatureBase>
{
friend FactorBase;
friend CaptureBase;
friend SensorBase;
friend FactorBase;
private:
CaptureBaseWPtr capture_ptr_;
FactorBasePtrList factor_list_;
FactorBasePtrList constrained_by_list_;
static unsigned int feature_id_count_;
protected:
unsigned int feature_id_;
unsigned int track_id_; // ID of the feature track
unsigned int landmark_id_; // ID of the landmark
Eigen::VectorXd measurement_; ///< the measurement vector
Eigen::MatrixXd measurement_covariance_; ///< the measurement covariance matrix
Eigen::MatrixXd measurement_sqrt_information_upper_; ///< the squared root information matrix
Eigen::VectorXd expectation_; ///< expectation
void setProblem(ProblemPtr _problem) override final;
public:
typedef enum
{
UNCERTAINTY_IS_COVARIANCE,
UNCERTAINTY_IS_INFO,
UNCERTAINTY_IS_STDDEV
} UncertaintyType;
/** \brief Constructor from capture pointer and measure
* \param _tp type of feature -- see wolf.h
* \param _measurement the measurement
* \param _meas_covariance the noise of the measurement
*/
FeatureBase(const std::string& _type,
const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _meas_uncertainty,
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
~FeatureBase() override;
virtual void remove(bool viral_remove_empty_parent=false);
// properties
unsigned int id() const;
unsigned int trackId() const {return track_id_;}
void setTrackId(unsigned int _tr_id){track_id_ = _tr_id;}
unsigned int landmarkId() const {return landmark_id_;}
void setLandmarkId(unsigned int _lmk_id){landmark_id_ = _lmk_id;}
// values
/* \brief Returns _ii component of measurement vector
*
* WARNING: To be fast, it does not check that index _ii is smaller than dimension.
*/
double getMeasurement(unsigned int _ii) const;
const Eigen::VectorXd& getMeasurement() const;
private:
/* \brief Set the measurement and its noise
*
* WARNING: To be used once in the constructor only.
*/
void setMeasurement(const Eigen::VectorXd& _meas);
void setMeasurementCovariance(const Eigen::MatrixXd & _meas_cov);
void setMeasurementInformation(const Eigen::MatrixXd & _meas_info);
public:
const Eigen::MatrixXd& getMeasurementCovariance() const;
Eigen::MatrixXd getMeasurementInformation() const;
const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const;
const Eigen::VectorXd& getExpectation() const;
void setExpectation(const Eigen::VectorXd& expectation);
// wolf tree access
FrameBaseConstPtr getFrame() const;
FrameBasePtr getFrame();
CaptureBaseConstPtr getCapture() const;
CaptureBasePtr getCapture();
FactorBaseConstPtrList getFactorList() const;
FactorBasePtrList getFactorList();
void getFactorList(FactorBaseConstPtrList & _fac_list) const;
void getFactorList(FactorBasePtrList & _fac_list);
bool hasFactor(FactorBaseConstPtr _fac) const;
unsigned int getHits() const;
FactorBaseConstPtrList getConstrainedByList() const;
FactorBasePtrList getConstrainedByList();
bool isConstrainedBy(const FactorBaseConstPtr &_factor) const;
void link(CaptureBasePtr cap_ptr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
virtual void printHeader(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream ,
std::string _tabs = "") const;
void print(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, FeatureBaseConstPtr _feature_ptr, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
FactorBasePtr addFactor(FactorBasePtr _co_ptr);
void removeFactor(FactorBasePtr _co_ptr);
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
private:
CaptureBaseWPtr capture_ptr_;
FactorBasePtrList factor_list_;
static unsigned int feature_id_count_;
protected:
unsigned int feature_id_;
unsigned int track_id_; // ID of the feature track
unsigned int landmark_id_; // ID of the landmark
Eigen::VectorXd measurement_; ///< the measurement vector
Eigen::MatrixXd measurement_covariance_; ///< the measurement covariance matrix
Eigen::MatrixXd measurement_sqrt_information_upper_; ///< the squared root information matrix
Eigen::VectorXd expectation_; ///< expectation
public:
typedef enum
{
UNCERTAINTY_IS_COVARIANCE,
UNCERTAINTY_IS_INFO,
UNCERTAINTY_IS_STDDEV
} UncertaintyType;
/** \brief Constructor from capture pointer and measure
* \param _type type of feature -- see wolf.h
* \param _measurement the measurement
* \param _meas_covariance the noise of the measurement
*/
FeatureBase(const std::string& _type,
const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _meas_uncertainty,
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
~FeatureBase() override = default;
virtual void remove(bool viral_remove_parent_without_children = false);
bool hasChildren() const override
{
return not factor_list_.empty();
};
// properties
unsigned int id() const;
unsigned int trackId() const
{
return track_id_;
}
void setTrackId(unsigned int _tr_id)
{
track_id_ = _tr_id;
}
unsigned int landmarkId() const
{
return landmark_id_;
}
void setLandmarkId(unsigned int _lmk_id)
{
landmark_id_ = _lmk_id;
}
// values
/* \brief Returns _ii component of measurement vector
*
* WARNING: To be fast, it does not check that index _ii is smaller than dimension.
*/
double getMeasurement(unsigned int _ii) const;
const Eigen::VectorXd& getMeasurement() const;
private:
/* \brief Set the measurement and its noise
*
* WARNING: To be used once in the constructor only.
*/
void setMeasurement(const Eigen::VectorXd& _meas);
void setMeasurementCovariance(const Eigen::MatrixXd& _meas_cov);
void setMeasurementInformation(const Eigen::MatrixXd& _meas_info);
public:
const Eigen::MatrixXd& getMeasurementCovariance() const;
Eigen::MatrixXd getMeasurementInformation() const;
const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const;
const Eigen::VectorXd& getExpectation() const;
void setExpectation(const Eigen::VectorXd& expectation);
// wolf tree access
FrameBaseConstPtr getFrame() const;
FrameBasePtr getFrame();
CaptureBaseConstPtr getCapture() const;
CaptureBasePtr getCapture();
FactorBaseConstPtrList getFactorList() const;
FactorBasePtrList getFactorList();
void getFactorList(FactorBaseConstPtrList& _fac_list) const;
void getFactorList(FactorBasePtrList& _fac_list);
bool hasFactor(FactorBaseConstPtr _fac) const;
void link(CaptureBasePtr cap_ptr);
template <typename classType, typename... T>
static std::shared_ptr<classType> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
virtual void printHeader(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const;
void print(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
void setCapture(CaptureBasePtr _cap_ptr)
{
capture_ptr_ = _cap_ptr;
}
FactorBasePtr addFactor(FactorBasePtr _co_ptr);
void removeFactor(FactorBasePtr _co_ptr);
};
}
} // namespace wolf
// IMPLEMENTATION
#include "core/factor/factor_base.h"
namespace wolf{
template<typename classType, typename... T>
namespace wolf
{
template <typename classType, typename... T>
std::shared_ptr<classType> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&... all)
{
std::shared_ptr<classType> ftr = std::make_shared<classType>(std::forward<T>(all)...);
......@@ -177,16 +182,10 @@ std::shared_ptr<classType> FeatureBase::emplace(CaptureBasePtr _cpt_ptr, T&&...
return ftr;
}
inline unsigned int FeatureBase::getHits() const
{
return constrained_by_list_.size();
}
inline FactorBaseConstPtrList FeatureBase::getFactorList() const
{
FactorBaseConstPtrList list_const;
for (auto&& obj_ptr : factor_list_)
list_const.push_back(obj_ptr);
for (auto&& obj_ptr : factor_list_) list_const.push_back(obj_ptr);
return list_const;
}
......@@ -195,19 +194,6 @@ inline FactorBasePtrList FeatureBase::getFactorList()
return factor_list_;
}
inline FactorBaseConstPtrList FeatureBase::getConstrainedByList() const
{
FactorBaseConstPtrList list_const;
for (auto&& obj_ptr : constrained_by_list_)
list_const.push_back(obj_ptr);
return list_const;
}
inline FactorBasePtrList FeatureBase::getConstrainedByList()
{
return constrained_by_list_;
}
inline unsigned int FeatureBase::id() const
{
return feature_id_;
......@@ -258,6 +244,4 @@ inline void FeatureBase::setExpectation(const Eigen::VectorXd& expectation)
expectation_ = expectation;
}
} // namespace wolf
#endif
} // namespace wolf
include/core/feature/feature_diff_drive.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file feature_diff_drive.h
*
* Created on: Oct 27, 2016
* \author: Jeremie Deray
*/
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#ifndef _WOLF_FEATURE_DIFF_DRIVE_H_
#define _WOLF_FEATURE_DIFF_DRIVE_H_
//Wolf includes
// Wolf includes
#include "core/feature/feature_motion.h"
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FeatureDiffDrive)
class FeatureDiffDrive : public FeatureMotion
{
public:
FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated,
const Eigen::MatrixXd& _delta_preintegrated_covariance,
const Eigen::VectorXd& _diff_drive_params,
const Eigen::MatrixXd& _jacobian_diff_drive_params);
virtual ~FeatureDiffDrive() = default;
public:
FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated,
const Eigen::MatrixXd& _delta_preintegrated_covariance,
const Eigen::VectorXd& _diff_drive_params,
const Eigen::MatrixXd& _jacobian_diff_drive_params);
virtual ~FeatureDiffDrive() = default;
};
} /* namespace wolf */
#endif /* _WOLF_FEATURE_DIFF_DRIVE_H_ */
include/core/feature/feature_landmark_external.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------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/>.
#pragma once
// Wolf includes
......@@ -28,7 +25,6 @@
namespace wolf
{
WOLF_PTR_TYPEDEFS(FeatureLandmarkExternal);
// class FeatureLandmarkExternal
......
include/core/feature/feature_match.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef FEATURE_MATCH_H_
#define FEATURE_MATCH_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Wolf includes
#include "core/common/wolf.h"
//wolf nampseace
namespace wolf {
//forward declaration to typedef class pointers
// wolf nampseace
namespace wolf
{
// forward declaration to typedef class pointers
WOLF_STRUCT_PTR_TYPEDEFS(FeatureMatch);
/** \brief Map of feature matches
......@@ -38,8 +34,8 @@ WOLF_STRUCT_PTR_TYPEDEFS(FeatureMatch);
* map<FeatureBasePtr actual_feature, FeatureMatchPtr corresponding_feature_match>
*
*/
typedef std::map<FeatureBasePtr, FeatureMatchPtr> FeatureMatchMap; //a map is also typedefined
typedef std::map<FeatureBasePtr, FeatureMatchPtr> FeatureMatchMap; // a map is also typedefined
/** \brief Match between a feature and a feature
*
* Match between a feature and a feature (feature-feature correspondence)
......@@ -47,11 +43,8 @@ typedef std::map<FeatureBasePtr, FeatureMatchPtr> FeatureMatchMap; //a map is al
*/
struct FeatureMatch
{
FeatureBasePtr feature_ptr_; ///< Corresponding feature
double normalized_score_; ///< normalized similarity score (0 is bad, 1 is good)
FeatureBasePtr feature_ptr_; ///< Corresponding feature
double normalized_score_; ///< normalized similarity score (0 is bad, 1 is good)
};
}//end namespace
#endif
} // namespace wolf
include/core/feature/feature_motion.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* feature_motion.h
*
* Created on: Aug 11, 2017
* Author: jsola
*/
#ifndef FEATURE_MOTION_H_
#define FEATURE_MOTION_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/feature/feature_base.h"
#include "core/capture/capture_motion.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(FeatureMotion);
class FeatureMotion : public FeatureBase
{
public:
FeatureMotion(const std::string& _type,
const VectorXd& _delta_preint,
const MatrixXd _delta_preint_cov,
const VectorXd& _calib_preint,
const MatrixXd& _jacobian_calib);
~FeatureMotion() override;
const Eigen::VectorXd& getDeltaPreint() const; ///< A new name for getMeasurement()
const Eigen::VectorXd& getCalibrationPreint() const;
const Eigen::MatrixXd& getJacobianCalibration() const;
private:
Eigen::VectorXd calib_preint_;
Eigen::MatrixXd jacobian_calib_;
public:
FeatureMotion(const std::string& _type,
const VectorXd& _delta_preint,
const MatrixXd _delta_preint_cov,
const VectorXd& _calib_preint,
const MatrixXd& _jacobian_calib);
~FeatureMotion() override;
const Eigen::VectorXd& getDeltaPreint() const; ///< A new name for getMeasurement()
const Eigen::VectorXd& getCalibrationPreint() const;
const Eigen::MatrixXd& getJacobianCalibration() const;
private:
Eigen::VectorXd calib_preint_;
Eigen::MatrixXd jacobian_calib_;
};
inline const Eigen::VectorXd& FeatureMotion::getDeltaPreint() const
......@@ -71,7 +59,4 @@ inline const Eigen::MatrixXd& FeatureMotion::getJacobianCalibration() const
return jacobian_calib_;
}
} /* namespace wolf */
#endif /* FEATURE_MOTION_H_ */
include/core/feature/feature_odom_2d.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef FEATURE_ODOM_2d_H_
#define FEATURE_ODOM_2d_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
//Wolf includes
// Wolf includes
#include "core/feature/feature_base.h"
//std includes
namespace wolf {
// std includes
namespace wolf
{
WOLF_PTR_TYPEDEFS(FeatureOdom2d);
//class FeatureOdom2d
// class FeatureOdom2d
class FeatureOdom2d : public FeatureBase
{
public:
/** \brief Constructor from capture pointer and measure
*
* \param _measurement the measurement
* \param _meas_covariance the noise of the measurement
*
*/
FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
~FeatureOdom2d() override;
public:
/** \brief Constructor from capture pointer and measure
*
* \param _measurement the measurement
* \param _meas_covariance the noise of the measurement
*
*/
FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
~FeatureOdom2d() override;
};
} // namespace wolf
#endif
} // namespace wolf
include/core/feature/feature_pose.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef FEATURE_POSE_H_
#define FEATURE_POSE_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
//Wolf includes
// Wolf includes
#include "core/feature/feature_base.h"
//std includes
//
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FeaturePose);
//class FeaturePose
// class FeaturePose
class FeaturePose : public FeatureBase
{
public:
/** \brief Constructor from capture pointer and measure
*
* \param _measurement the measurement
* \param _meas_covariance the noise of the measurement
*
*/
FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
~FeaturePose() override;
public:
/** \brief Constructor from capture pointer and measure
*
* \param _measurement the measurement
* \param _meas_covariance the noise of the measurement
*
*/
FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
~FeaturePose() override;
};
} // namespace wolf
#endif
} // namespace wolf
include/core/frame/frame_base.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef FRAME_BASE_H_
#define FRAME_BASE_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Fwd refs
namespace wolf{
namespace wolf
{
class TrajectoryBase;
class CaptureBase;
class StateBlock;
}
} // namespace wolf
//Wolf includes
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/time_stamp.h"
#include "core/common/node_base.h"
#include "core/state_block/has_state_blocks.h"
//std includes
#include "core/common/node_state_blocks.h"
namespace wolf {
//class FrameBase
class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<FrameBase>
namespace wolf
{
// class FrameBase
class FrameBase : public NodeStateBlocks
{
friend CaptureBase;
friend FactorBase;
private:
TrajectoryBaseWPtr trajectory_ptr_;
CaptureBasePtrList capture_list_;
FactorBasePtrList constrained_by_list_;
static unsigned int frame_id_count_;
protected:
unsigned int frame_id_;
TimeStamp time_stamp_; ///< frame time stamp
public:
/** \brief Constructor with type, time stamp and state pointer
*
* Constructor with type, time stamp and state pointer
* \param _ts is the time stamp associated to this frame, provided in seconds
* \param _p_ptr StateBlock pointer to the position (default: nullptr)
* \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
* \param _v_ptr StateBlock pointer to the velocity (default: nullptr).
**/
FrameBase(const TimeStamp& _ts,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _v_ptr = nullptr);
FrameBase(const TimeStamp& _ts,
const StateStructure& _frame_structure,
const VectorComposite& _state);
FrameBase(const TimeStamp& _ts,
const StateStructure& _frame_structure,
const std::list<VectorXd>& _vectors);
~FrameBase() override;
// Add and remove from WOLF tree ---------------------------------
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all);
void link(TrajectoryBasePtr);
void link(ProblemPtr _prb);
virtual void remove(bool viral_remove_empty_parent=false);
// Frame properties -----------------------------------------------
public:
unsigned int id() const;
// Frame values ------------------------------------------------
public:
void setTimeStamp(const TimeStamp& _ts);
TimeStamp getTimeStamp() const;
void getTimeStamp(TimeStamp& _ts) const;
// State blocks ----------------------------------------------------
public:
StateBlockConstPtr getV() const;
StateBlockPtr getV();
protected:
void setProblem(ProblemPtr _problem) override final;
// States
public:
bool getCovariance(Eigen::MatrixXd& _cov) const;
// Wolf tree access ---------------------------------------------------
public:
TrajectoryBaseConstPtr getTrajectory() const;
TrajectoryBasePtr getTrajectory();
FrameBaseConstPtr getPreviousFrame(const unsigned int& i = 1) const;
FrameBaseConstPtr getNextFrame(const unsigned int& i = 1) const;
FrameBasePtr getPreviousFrame(const unsigned int& i = 1);
FrameBasePtr getNextFrame(const unsigned int& i = 1);
CaptureBaseConstPtrList getCaptureList() const;
CaptureBasePtrList getCaptureList();
bool hasCapture(const CaptureBaseConstPtr& _capture) const;
FactorBaseConstPtrList getFactorList() const;
FactorBasePtrList getFactorList();
void getFactorList(FactorBaseConstPtrList& _fac_list) const;
void getFactorList(FactorBasePtrList& _fac_list);
FactorBaseConstPtrList getConstrainedByList() const;
FactorBasePtrList getConstrainedByList();
bool isConstrainedBy(const FactorBaseConstPtr& _factor) const;
template <class C>
std::shared_ptr<const C> getCaptureOfType() const;
template <class C>
std::shared_ptr<C> getCaptureOfType();
CaptureBaseConstPtr getCaptureOfType(const std::string& type) const;
CaptureBasePtr getCaptureOfType(const std::string& type);
template <class C>
std::list<std::shared_ptr<const C>> getCapturesOfType() const;
template <class C>
std::list<std::shared_ptr<C>> getCapturesOfType();
CaptureBaseConstPtrList getCapturesOfType(const std::string& type) const;
CaptureBasePtrList getCapturesOfType(const std::string& type);
CaptureBaseConstPtr getCaptureOf(SensorBaseConstPtr _sensor_ptr) const;
CaptureBasePtr getCaptureOf(SensorBaseConstPtr _sensor_ptr);
CaptureBaseConstPtr getCaptureOf(SensorBaseConstPtr _sensor_ptr, const std::string& type) const;
CaptureBasePtr getCaptureOf(SensorBasePtr _sensor_ptr, const std::string& type);
CaptureBaseConstPtrList getCapturesOf(SensorBaseConstPtr _sensor_ptr) const;
CaptureBasePtrList getCapturesOf(SensorBasePtr _sensor_ptr);
FactorBaseConstPtr getFactorOf(ProcessorBaseConstPtr _processor_ptr) const;
FactorBasePtr getFactorOf(ProcessorBasePtr _processor_ptr);
FactorBaseConstPtr getFactorOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type) const;
FactorBasePtr getFactorOf(ProcessorBasePtr _processor_ptr, const std::string& type);
unsigned int getHits() const;
// Debug and info -------------------------------------------------------
virtual void printHeader(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream ,
std::string _tabs = "") const;
void print(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, FrameBaseConstPtr _frm_ptr, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
void removeCapture(CaptureBasePtr _capt_ptr);
void setTrajectory(TrajectoryBasePtr _trj_ptr);
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
private:
TrajectoryBaseWPtr trajectory_ptr_;
CaptureBasePtrList capture_list_;
static unsigned int frame_id_count_;
protected:
unsigned int frame_id_;
TimeStamp time_stamp_; ///< frame time stamp
public:
/** \brief Constructor time stamp and specs composite
*
* Constructor with time stamp and specs (types, vectors, priors)
* \param _ts is the time stamp associated to this frame, provided in seconds
* \param _frame_types TypeComposite containing the types of the state blocs.
* \param _frame_vectors VectorComposite containing the vectors of the the state blocs.
* \param _frame_priors PriorComposite containing the priors of the the state blocs.
**/
FrameBase(const TimeStamp& _ts,
const TypeComposite& _frame_types,
const VectorComposite& _frame_vectors,
const PriorComposite& _frame_priors = {});
~FrameBase() override = default;
// Add and remove from WOLF tree ---------------------------------
template <typename classType, typename... T>
static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all);
void link(TrajectoryBasePtr);
void link(ProblemPtr _prb);
void remove(bool viral_remove_parent_without_children = false) override;
bool hasChildren() const override;
// Frame properties -----------------------------------------------
public:
unsigned int id() const override;
// Frame values ------------------------------------------------
public:
void setTimeStamp(const TimeStamp& _ts);
TimeStamp getTimeStamp() const;
void getTimeStamp(TimeStamp& _ts) const;
// State blocks ----------------------------------------------------
public:
StateBlockConstPtr getV() const;
StateBlockPtr getV();
protected:
void setProblem(ProblemPtr _problem) override final;
// Wolf tree access ---------------------------------------------------
public:
TrajectoryBaseConstPtr getTrajectory() const;
TrajectoryBasePtr getTrajectory();
FrameBaseConstPtr getPreviousFrame(const unsigned int& i = 1) const;
FrameBaseConstPtr getNextFrame(const unsigned int& i = 1) const;
FrameBasePtr getPreviousFrame(const unsigned int& i = 1);
FrameBasePtr getNextFrame(const unsigned int& i = 1);
CaptureBaseConstPtrList getCaptureList() const;
CaptureBasePtrList getCaptureList();
bool hasCapture(const CaptureBaseConstPtr& _capture) const;
FactorBaseConstPtrList getFactorList() const;
FactorBasePtrList getFactorList();
void getFactorList(FactorBaseConstPtrList& _fac_list) const;
void getFactorList(FactorBasePtrList& _fac_list);
template <class C>
std::shared_ptr<const C> getCaptureOfType() const;
template <class C>
std::shared_ptr<C> getCaptureOfType();
CaptureBaseConstPtr getCaptureOfType(const std::string& type) const;
CaptureBasePtr getCaptureOfType(const std::string& type);
template <class C>
std::list<std::shared_ptr<const C>> getCapturesOfType() const;
template <class C>
std::list<std::shared_ptr<C>> getCapturesOfType();
CaptureBaseConstPtrList getCapturesOfType(const std::string& type) const;
CaptureBasePtrList getCapturesOfType(const std::string& type);
CaptureBaseConstPtr getCaptureOf(SensorBaseConstPtr _sensor_ptr) const;
CaptureBasePtr getCaptureOf(SensorBaseConstPtr _sensor_ptr);
CaptureBaseConstPtr getCaptureOf(SensorBaseConstPtr _sensor_ptr, const std::string& type) const;
CaptureBasePtr getCaptureOf(SensorBasePtr _sensor_ptr, const std::string& type);
CaptureBaseConstPtrList getCapturesOf(SensorBaseConstPtr _sensor_ptr) const;
CaptureBasePtrList getCapturesOf(SensorBasePtr _sensor_ptr);
// Debug and info -------------------------------------------------------
virtual void printHeader(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const;
void print(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
void removeCapture(CaptureBasePtr _capt_ptr);
void setTrajectory(TrajectoryBasePtr _trj_ptr);
public:
FrameBasePtr shared_from_this_frame()
{
return std::static_pointer_cast<FrameBase>(shared_from_this());
};
FrameBaseConstPtr shared_from_this_frame() const
{
return std::static_pointer_cast<const FrameBase>(shared_from_this());
};
};
} // namespace wolf
} // namespace wolf
// IMPLEMENTATION //
......@@ -208,12 +180,13 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
#include "core/factor/factor_base.h"
#include "core/state_block/state_block.h"
namespace wolf {
template<typename classType, typename... T>
namespace wolf
{
template <typename classType, typename... T>
std::shared_ptr<classType> FrameBase::emplace(TrajectoryBasePtr _ptr, T&&... all)
{
std::shared_ptr<classType> frm = std::make_shared<classType>(std::forward<T>(all)...);
frm->emplacePriors();
frm->link(_ptr);
return frm;
}
......@@ -256,8 +229,7 @@ inline TrajectoryBasePtr FrameBase::getTrajectory()
inline CaptureBaseConstPtrList FrameBase::getCaptureList() const
{
CaptureBaseConstPtrList list_const;
for (auto&& obj_ptr : capture_list_)
list_const.push_back(obj_ptr);
for (auto&& obj_ptr : capture_list_) list_const.push_back(obj_ptr);
return list_const;
}
......@@ -266,24 +238,6 @@ inline CaptureBasePtrList FrameBase::getCaptureList()
return capture_list_;
}
inline unsigned int FrameBase::getHits() const
{
return constrained_by_list_.size();
}
inline FactorBaseConstPtrList FrameBase::getConstrainedByList() const
{
FactorBaseConstPtrList list_const;
for (auto&& obj_ptr : constrained_by_list_)
list_const.push_back(obj_ptr);
return list_const;
}
inline FactorBasePtrList FrameBase::getConstrainedByList()
{
return constrained_by_list_;
}
inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
{
trajectory_ptr_ = _trj_ptr;
......@@ -295,8 +249,7 @@ inline std::shared_ptr<const C> FrameBase::getCaptureOfType() const
for (auto capture_ptr : getCaptureList())
{
auto cap_derived = std::dynamic_pointer_cast<const C>(capture_ptr);
if (cap_derived)
return cap_derived;
if (cap_derived) return cap_derived;
}
return nullptr;
}
......@@ -307,8 +260,7 @@ inline std::shared_ptr<C> FrameBase::getCaptureOfType()
for (auto capture_ptr : getCaptureList())
{
auto cap_derived = std::dynamic_pointer_cast<C>(capture_ptr);
if (cap_derived)
return cap_derived;
if (cap_derived) return cap_derived;
}
return nullptr;
}
......@@ -320,8 +272,7 @@ inline std::list<std::shared_ptr<const C>> FrameBase::getCapturesOfType() const
for (auto capture_ptr : getCaptureList())
{
auto cap_derived = std::dynamic_pointer_cast<const C>(capture_ptr);
if (cap_derived)
captures.push_back(cap_derived);
if (cap_derived) captures.push_back(cap_derived);
}
return captures;
}
......@@ -333,12 +284,9 @@ inline std::list<std::shared_ptr<C>> FrameBase::getCapturesOfType()
for (auto capture_ptr : getCaptureList())
{
auto cap_derived = std::dynamic_pointer_cast<C>(capture_ptr);
if (cap_derived)
captures.push_back(cap_derived);
if (cap_derived) captures.push_back(cap_derived);
}
return captures;
}
} // namespace wolf
#endif
} // namespace wolf
include/core/hardware/hardware_base.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef HARDWARE_BASE_H_
#define HARDWARE_BASE_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Fwd dependencies
namespace wolf{
namespace wolf
{
class Problem;
class SensorBase;
}
} // namespace wolf
//Wolf includes
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
namespace wolf {
//class HardwareBase
namespace wolf
{
// class HardwareBase
class HardwareBase : public NodeBase, public std::enable_shared_from_this<HardwareBase>
{
friend SensorBase;
private:
SensorBasePtrList sensor_list_;
public:
HardwareBase();
~HardwareBase() override;
SensorBaseConstPtrList getSensorList() const;
SensorBasePtrList getSensorList();
/** \brief get a sensor pointer by its name
* \param _sensor_name The sensor name, as it was installed with installSensor()
*/
SensorBaseConstPtr getSensor(const std::string& _sensor_name) const;
SensorBasePtr getSensor(const std::string& _sensor_name);
virtual void printHeader(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream ,
std::string _tabs = "") const;
void print(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, HardwareBaseConstPtr _hwd_ptr, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
private:
SensorBasePtrList sensor_list_;
public:
HardwareBase();
~HardwareBase() override;
bool hasChildren() const override;
SensorBaseConstPtrList getSensorList() const;
SensorBasePtrList getSensorList();
/** \brief get a sensor pointer by its name
* \param _sensor_name The sensor name, as it was installed with installSensor()
*/
SensorBaseConstPtr getSensor(const std::string& _sensor_name) const;
SensorBasePtr getSensor(const std::string& _sensor_name);
virtual void printHeader(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const;
void print(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
};
} // namespace wolf
} // namespace wolf
// IMPLEMENTATION
namespace wolf {
namespace wolf
{
inline SensorBaseConstPtrList HardwareBase::getSensorList() const
{
SensorBaseConstPtrList list_const;
for (auto&& obj_ptr : sensor_list_)
list_const.push_back(obj_ptr);
for (auto&& obj_ptr : sensor_list_) list_const.push_back(obj_ptr);
return list_const;
}
......@@ -94,6 +91,9 @@ inline SensorBasePtrList HardwareBase::getSensorList()
return sensor_list_;
}
} // namespace wolf
inline bool HardwareBase::hasChildren() const
{
return not sensor_list_.empty();
}
#endif
} // namespace wolf
include/core/landmark/factory_landmark.h 0 → 100644
Edit View file @ 44e08b4a
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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 General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// wolf
#include "core/common/factory.h"
// yaml
#include "yaml-cpp/yaml.h"
namespace wolf
{
typedef Factory<LandmarkBasePtr, const YAML::Node&, const std::vector<std::string>&> FactoryLandmark;
template <>
inline std::string FactoryLandmark::getClass() const
{
return "FactoryLandmark";
}
#define WOLF_REGISTER_LANDMARK(LandmarkType) \
namespace \
{ \
const bool WOLF_UNUSED LandmarkType##Registered = \
FactoryLandmark::registerCreator(#LandmarkType, LandmarkType::create); \
}
} /* namespace wolf */
include/core/landmark/landmark.h 0 → 100644
Edit View file @ 44e08b4a
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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 General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Wolf includes
#include "core/landmark/landmark_base.h"
namespace wolf
{
template <unsigned int DIM>
class Landmark : public LandmarkBase
{
public:
/** \brief Constructor specs composite
*
* Constructor with specs composite
* \param _state_vectors Composite of keys and vectors of the landmark states.
* All keys ('P' and 'O') are required.
* \param _state_priors Composite of keys and priors (initial_guess, factor or fixed) of the landmark states.
* Not all the keys are required. If not specified, no prior is assumed, i.e. 'initial_guess'.
**/
Landmark(const VectorComposite& _state_vectors,
const PriorComposite& _state_priors = {},
const int _external_id = -1,
const int _external_type = -1);
/** \brief Constructor with YAML node (to be used by the derived classes YAML node constructors)
*
* Constructor with YAML node
* \param _n YAML node containing the necessary information
**/
Landmark(const YAML::Node& _n);
WOLF_LANDMARK_TEMPLATE_DIM_CREATE(Landmark, DIM);
~Landmark() override = default;
};
typedef Landmark<2> Landmark2d;
typedef Landmark<3> Landmark3d;
WOLF_DECLARED_PTR_TYPEDEFS(Landmark2d);
WOLF_DECLARED_PTR_TYPEDEFS(Landmark3d);
////////////////////////////////////////////////////////////////////////////////////////////////
// IMPLEMENTATION //
////////////////////////////////////////////////////////////////////////////////////////////////
template <unsigned int DIM>
inline Landmark<DIM>::Landmark(const VectorComposite& _state_vectors,
const PriorComposite& _state_priors,
const int _external_id,
const int _external_type)
: LandmarkBase(
"Landmark" + std::to_string(DIM) + "d",
{{'P', DIM == 2 ? "StatePoint2d" : "StatePoint3d"}, {'O', DIM == 2 ? "StateAngle" : "StateQuaternion"}},
_state_vectors,
_state_priors,
_external_id,
_external_type)
{
static_assert(DIM == 2 or DIM == 3);
}
template <unsigned int DIM>
inline Landmark<DIM>::Landmark(const YAML::Node& _n)
: LandmarkBase(
"Landmark" + std::to_string(DIM) + "d",
{{'P', DIM == 2 ? "StatePoint2d" : "StatePoint3d"}, {'O', DIM == 2 ? "StateAngle" : "StateQuaternion"}},
_n)
{
static_assert(DIM == 2 or DIM == 3);
}
} // namespace wolf
include/core/landmark/landmark_base.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef LANDMARK_BASE_H_
#define LANDMARK_BASE_H_
// Fwd references
namespace wolf{
class MapBase;
class StateBlock;
}
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
//Wolf includes
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/common/node_state_blocks.h"
#include "core/common/time_stamp.h"
#include "core/state_block/has_state_blocks.h"
//std includes
#include "core/landmark/factory_landmark.h"
// yaml
#include "yaml-cpp/yaml.h"
namespace wolf {
#include "yaml-schema-cpp/yaml_server.hpp"
//class LandmarkBase
class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<LandmarkBase>
namespace wolf
{
/*
* Macro for defining Autoconf landmark creator.
*
* Place a call to this macro inside your class declaration (in the landmark_class.h file),
* preferably just after the constructors.
*
* In order to use this macro, the derived landmark class, LandmarkClass,
* must have two constructors available with the API:
*
* LandmarkClass(const YAML::Node& _node);
*/
#define WOLF_LANDMARK_CREATE(LandmarkClass) \
static LandmarkBasePtr create(const YAML::Node& _node, const std::vector<std::string>& _folders_schema = {}) \
{ \
if (not _folders_schema.empty()) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
server.setYaml(_node); \
if (not server.applySchema(#LandmarkClass)) \
{ \
WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
} \
auto lmk = std::make_shared<LandmarkClass>(_node); \
lmk->emplacePriors(); \
return lmk; \
}
#define WOLF_LANDMARK_TEMPLATE_DIM_CREATE(LandmarkClass, Dim) \
static LandmarkBasePtr create(const YAML::Node& _node, const std::vector<std::string>& _folders_schema = {}) \
{ \
if (not _folders_schema.empty()) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
server.setYaml(_node); \
if (not server.applySchema(#LandmarkClass + std::to_string(Dim) + "d")) \
{ \
WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
} \
auto lmk = std::make_shared<LandmarkClass<Dim>>(_node); \
lmk->emplacePriors(); \
return lmk; \
}
// class LandmarkBase
class LandmarkBase : public NodeStateBlocks
{
friend FactorBase;
private:
MapBaseWPtr map_ptr_;
FactorBasePtrList constrained_by_list_;
static unsigned int landmark_id_count_;
protected:
// Navigate wolf tree
void setProblem(ProblemPtr _problem) override final;
unsigned int landmark_id_; ///< landmark unique id
unsigned int track_id_; ///< associated track id
TimeStamp stamp_; ///< stamp of the creation of the landmark
Eigen::VectorXd descriptor_; //TODO: agree? JS: No: It is not general enough as descriptor to be in LmkBase.
public:
/** \brief Constructor with type, time stamp and the position state pointer (optional orientation state pointer)
*
* Constructor with type, and state pointer
* \param _tp indicates landmark type.(types defined at wolf.h)
* \param _p_ptr StateBlock pointer to the position
* \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
*
**/
LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
~LandmarkBase() override;
virtual void remove(bool viral_remove_empty_parent=false);
virtual YAML::Node saveToYaml() const;
// Properties
unsigned int id() const;
void setId(unsigned int _id);
unsigned int trackId(); // get track ID
void setTrackId(unsigned int _track_id); // set track ID
// State blocks
//std::vector<StateBlockConstPtr> getUsedStateBlockVec() const;
//std::vector<StateBlockPtr> getUsedStateBlockVec();
bool getCovariance(Eigen::MatrixXd& _cov) const;
// Descriptor
public:
const Eigen::VectorXd& getDescriptor() const;
double getDescriptor(unsigned int _ii) const;
void setDescriptor(const Eigen::VectorXd& _descriptor);
unsigned int getHits() const;
FactorBaseConstPtrList getConstrainedByList() const;
FactorBasePtrList getConstrainedByList();
bool isConstrainedBy(const FactorBaseConstPtr &_factor) const;
MapBaseConstPtr getMap() const;
MapBasePtr getMap();
void link(MapBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(MapBasePtr _map_ptr, T&&... all);
/** \brief Creator for Factory<LandmarkBase, YAML::Node>
* Caution: This creator does not set the landmark's anchor frame and sensor.
* These need to be set afterwards.
*/
static LandmarkBasePtr create(const YAML::Node& _node);
virtual void printHeader(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream ,
std::string _tabs = "") const;
void print(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
void setMap(const MapBasePtr _map_ptr);
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
private:
MapBaseWPtr map_ptr_;
static unsigned int landmark_id_count_;
protected:
unsigned int landmark_id_; ///< landmark unique id
unsigned int track_id_; ///< associated track id
int external_id_; ///< Id provided by an external tracker (-1 untracked)
int external_type_; ///< Type provided by an external classifier (-1 unclassified)
TimeStamp stamp_; ///< stamp of the creation of the landmark
// Navigate wolf tree
void setProblem(ProblemPtr _problem) override;
public:
/** \brief Constructor type and state specs composites (types, vectors, priors)
*
* Constructor with type and specs composite
* \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: derived landmark class name
* \param _state_types TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: Composite of types of the landmark states.
* \param _state_vectors Composite of vectors of the states of the node. A 'StateBlock' for each key in
* '_state_vectors' will be emplaced (of the type specified in '_state_types'). Not all keys of '_state_types' are
* required to be specified, but all keys of '_state_vectors' should be in '_state_types'.
* \param _state_priors Composite of priors (initial_guess, factor or fixed) of the states of the node. Not all
* keys of '_state_vectors' are required to be specified, but all keys of '_state_priors' should be in
* '_state_vectors'. If not specified, no prior is assumed, i.e. 'initial_guess'.
* \param _external_id ID provided by an external tracker (default: -1 untracked)
* \param _external_type TYPE provided by an external classifier (default: -1 unclassified)
**/
LandmarkBase(const std::string& _type,
const TypeComposite& _state_types,
const VectorComposite& _state_vectors,
const PriorComposite& _state_priors,
const int _external_id = -1,
const int _external_type = -1);
/** \brief Constructor with type, state types composite and a YAML node (to be used by the derived classes YAML
*node constructors)
*
* Constructor with type, and YAML node
* \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: derived landmark class name
* \param _state_types TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: Composite of types of the landmark states.
* \param _n YAML node containing the necessary information
**/
LandmarkBase(const std::string& _type, const TypeComposite& _state_types, const YAML::Node& _n);
~LandmarkBase() override = default;
void remove(bool viral_remove_parent_without_children = false) override;
YAML::Node toYaml() const override;
// Properties
unsigned int id() const override;
void setId(unsigned int _id);
unsigned int trackId(); // get track ID
void setTrackId(unsigned int _track_id); // set track ID
void setExternalId(const int& _external_id);
int getExternalId() const;
void setExternalType(const int& _external_type);
int getExternalType() const;
MapBaseConstPtr getMap() const;
MapBasePtr getMap();
void link(MapBasePtr);
template <typename classType, typename... T>
static std::shared_ptr<classType> emplace(MapBasePtr _map_ptr, T&&... all);
virtual void printHeader(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const;
virtual void print(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
virtual bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
void setMap(const MapBasePtr _map_ptr);
public:
LandmarkBasePtr shared_from_this_landmark();
LandmarkBaseConstPtr shared_from_this_landmark() const;
};
}
} // namespace wolf
#include "core/map/map_base.h"
#include "core/factor/factor_base.h"
#include "core/state_block/state_block.h"
namespace wolf{
template<typename classType, typename... T>
namespace wolf
{
template <typename classType, typename... T>
std::shared_ptr<classType> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&... all)
{
std::shared_ptr<classType> lmk = std::make_shared<classType>(std::forward<T>(all)...);
lmk->emplacePriors();
lmk->link(_map_ptr);
return lmk;
}
......@@ -174,8 +215,7 @@ inline unsigned int LandmarkBase::id() const
inline void LandmarkBase::setId(unsigned int _id)
{
landmark_id_ = _id;
if (_id > landmark_id_count_)
landmark_id_count_ = _id;
if (_id > landmark_id_count_) landmark_id_count_ = _id;
}
inline unsigned int LandmarkBase::trackId()
......@@ -183,44 +223,39 @@ inline unsigned int LandmarkBase::trackId()
return track_id_;
}
inline void LandmarkBase::setTrackId(unsigned int _track_id)
inline void LandmarkBase::setTrackId(unsigned int _track_id)
{
track_id_ = _track_id;
}
inline unsigned int LandmarkBase::getHits() const
inline void LandmarkBase::setExternalId(const int& _external_id)
{
return constrained_by_list_.size();
external_id_ = _external_id;
}
inline FactorBaseConstPtrList LandmarkBase::getConstrainedByList() const
inline int LandmarkBase::getExternalId() const
{
FactorBaseConstPtrList list_const;
for (auto&& obj_ptr : constrained_by_list_)
list_const.push_back(obj_ptr);
return list_const;
return external_id_;
}
inline FactorBasePtrList LandmarkBase::getConstrainedByList()
inline void LandmarkBase::setExternalType(const int& _external_type)
{
return constrained_by_list_;
external_type_ = _external_type;
}
inline void LandmarkBase::setDescriptor(const Eigen::VectorXd& _descriptor)
inline int LandmarkBase::getExternalType() const
{
descriptor_ = _descriptor;
return external_type_;
}
inline double LandmarkBase::getDescriptor(unsigned int _ii) const
inline LandmarkBasePtr LandmarkBase::shared_from_this_landmark()
{
assert(_ii < descriptor_.size() && "LandmarkBase::getDescriptor: bad index");
return descriptor_(_ii);
return std::static_pointer_cast<LandmarkBase>(shared_from_this());
}
inline const Eigen::VectorXd& LandmarkBase::getDescriptor() const
inline LandmarkBaseConstPtr LandmarkBase::shared_from_this_landmark() const
{
return descriptor_;
return std::static_pointer_cast<const LandmarkBase>(shared_from_this());
}
} // namespace wolf
#endif
} // namespace wolf
include/core/landmark/landmark_external.h deleted 100644 → 0
View file @ c7bb5751
//--------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--------
#ifndef LANDMARK_EXTERNAL_H_
#define LANDMARK_EXTERNAL_H_
// Wolf includes
#include "core/landmark/landmark_base.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(LandmarkExternal);
// class LandmarkExternal
class LandmarkExternal : public LandmarkBase
{
protected:
int external_id_; ///< Id provided by an external tracker (-1 untracked)
int external_type_; ///< Type provided by an external classifier (-1 unclassified)
public:
/** \brief Constructor with the position state pointer (optional orientation state pointer)
*
* Constructor with type, and state pointer
* \param _external_id ID provided by an external tracker (-1 for untracked)
* \param _external_type TYPE provided by an external classifier (-1 for unknown)
* \param _p_ptr StateBlock pointer to the position
* \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
*
**/
LandmarkExternal(const int& _external_id,
const int& _external_type,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr = nullptr);
~LandmarkExternal() = default;
YAML::Node saveToYaml() const override;
/** \brief Creator for Factory<LandmarkExternal, YAML::Node>
* Caution: This creator does not set the landmark's anchor frame and sensor.
* These need to be set afterwards.
*/
static LandmarkBasePtr create(const YAML::Node& _node);
void setExternalId(const int& _external_id);
int getExternalId() const;
void setExternalType(const int& _external_type);
int getExternalType() const;
};
inline void LandmarkExternal::setExternalId(const int& _external_id)
{
external_id_ = _external_id;
}
inline int LandmarkExternal::getExternalId() const
{
return external_id_;
}
inline void LandmarkExternal::setExternalType(const int& _external_type)
{
external_type_ = _external_type;
}
inline int LandmarkExternal::getExternalType() const
{
return external_type_;
}
} // namespace wolf
#endif
\ No newline at end of file
include/core/landmark/landmark_match.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef LANDMARK_MATCH_H_
#define LANDMARK_MATCH_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Wolf includes
#include "core/common/wolf.h"
//wolf nampseace
namespace wolf {
// Map of Feature - Landmark matches
// wolf nampseace
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(LandmarkMatch);
typedef std::map<FeatureBasePtr, LandmarkMatchPtr> LandmarkMatchMap;
......@@ -39,21 +34,14 @@ typedef std::map<FeatureBasePtr, LandmarkMatchPtr> LandmarkMatchMap;
**/
struct LandmarkMatch
{
LandmarkBasePtr landmark_ptr_; ///< Pointer to the matched landmark
double normalized_score_; ///< Similarity measure: 0: no match -- 1: perfect match
LandmarkMatch() :
landmark_ptr_(nullptr), normalized_score_(0)
{
LandmarkBasePtr landmark_ptr_; ///< Pointer to the matched landmark
double normalized_score_; ///< Similarity measure: 0: no match -- 1: perfect match
}
LandmarkMatch(LandmarkBasePtr _landmark_ptr, double _normalized_score) :
landmark_ptr_(_landmark_ptr), normalized_score_(_normalized_score)
LandmarkMatch() : landmark_ptr_(nullptr), normalized_score_(0) {}
LandmarkMatch(LandmarkBasePtr _landmark_ptr, double _normalized_score)
: landmark_ptr_(_landmark_ptr), normalized_score_(_normalized_score)
{
}
};
}//end namespace
#endif
} // namespace wolf
include/core/map/factory_map.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file factory_map.h
*
* Created on: Jul 25, 2016
* \author: jvallve
*/
#ifndef FACTORY_MAP_H_
#define FACTORY_MAP_H_
namespace wolf
{
class MapBase;
struct ParamsMapBase;
}
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// wolf
#include "core/common/factory.h"
#include "core/utils/params_server.h"
namespace wolf
{
/** \brief Map factory class
*
* This factory can create objects of classes deriving from MapBase.
*
* Specific object creation is invoked by `create(TYPE, params ... )`, and the TYPE of map is identified with a string.
* Currently, the following map types are implemented,
* - "MapGrid2dGravity" in plugin 'imu'
*
* among others.
*
* Find general Factory documentation in class Factory:
* - Access the factory
* - Register/unregister creators
* - Invoke object creation
*
* This documentation shows you how to use the FactoryMap specifically:
* - Write map creators.
* - Create maps
*
* #### Write map creators
* Map creators have the following API:
*
* \code
* static MapBasePtr create(ParamsMapBasePtr _params_map);
* \endcode
*
* They follow the general implementation shown below:
*
* \code
* static MapBasePtr create(ParamsMapBasePtr _params_map)
* {
* // cast map parameters to good type --- example: ParamsMapGrid
* auto params_ptr = std::static_pointer_cast<ParamsMapGrid>(_params_map);
*
* // Do create the Map object --- example: MapGrid
* return map_ptr = std::make_shared<MapGrid>(params_ptr);
* }
* \endcode
*
* #### Creating maps
* Note: Prior to invoking the creation of a map of a particular type,
* you must register the creator for this type into the factory.
*
* To create e.g. a MapGrid, you type:
*
* \code
* auto grid_ptr = FactoryMap::create("MapGrid", params_grid);
* \endcode
*
* #### See also
* - FactoryParamsMap: to create parameter structs deriving from ParamsMapBase directly from YAML files.
*
* #### Example 1: writing a MapGrid creator
* Here is an example of MapGrid::create() extracted from map_grid.cpp:
*
* \code
* static MapBasePtr create(const std::string& _unique_name, Eigen::VectorXd& _extrinsics_pq, ParamsMapBasePtr _intrinsics)
* {
* // check extrinsics vector
* assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d.");
*
* // cast instrinsics to good type
* auto intrinsics_ptr = std::static_pointer_cast<ParamsMapGrid>(_intrinsics);
*
* // Do create the MapGrid object, and complete its setup
* auto sen_ptr = std::make_shared<MapGrid>(_extrinsics_pq, intrinsics_ptr);
*
* sen_ptr->setName(_unique_name);
*
* return sen_ptr;
* }
* \endcode
*
* #### Example 2: registering a map creator into the factory
* Registration can be done manually or automatically. It involves the call to static functions.
* It is advisable to put these calls within unnamed namespaces.
*
* - __Manual registration__: you control registration at application level.
* Put the code either at global scope (you must define a dummy variable for this),
* \code
* namespace {
* const bool registered_grid = FactoryMap::registerCreator("MapGrid", MapGrid::create);
* }
* main () { ... }
* \endcode
* or inside your main(), where a direct call is possible:
* \code
* main () {
* FactoryMap::registerCreator("MapGrid", MapGrid::create);
* ...
* }
* \endcode
*
* - __Automatic registration__: registration is performed at library level.
* Put the code at the last line of the map_xxx.cpp file,
* \code
* namespace {
* const bool registered_grid = FactoryMap::registerCreator("MapGrid", MapGrid::create);
* }
* \endcode
* Automatic registration is recommended in wolf, and implemented in the classes shipped with it.
* You are free to comment out these lines and place them wherever you consider it more convenient for your design.
*
* #### Example 2: creating maps
* We finally provide the necessary steps to create a map of class MapGrid in our application:
*
* \code
* #include "core/map/factory_map.h"
* #include "core/map/map_grid.h" // provides MapGrid
*
* // Note: MapGrid::create() is already registered, automatically.
*
* using namespace wolf;
* int main() {
*
* // To create a grid, provide:
* // a type = "MapGrid" and
* // a pointer to the intrinsics struct:
*
* // Create a pointer to the struct of map parameters stored in a YAML file ( see FactoryParamsMap )
* ParamsMapGridPtr params_1 =
* FactoryParamsMap::create("ParamsMapGrid",
* grid_1.yaml);
*
* MapBasePtr grid_1_ptr =
* FactoryMap::create ( "MapGrid" ,
* params_1 );
*
* return 0;
* }
* \endcode
*
*/
typedef Factory<MapBase,
const ParamsMapBasePtr> FactoryMap;
// Yaml
#include "yaml-cpp/yaml.h"
template<>
inline std::string FactoryMap::getClass() const
namespace wolf
{
return "FactoryMap";
}
typedef Factory<MapBasePtr, const YAML::Node&, const std::vector<std::string>&> FactoryMapNode;
// ParamsMap factory
struct ParamsMapBase;
typedef Factory<ParamsMapBase,
const std::string&> FactoryParamsMap;
template<>
inline std::string FactoryParamsMap::getClass() const
template <>
inline std::string FactoryMapNode::getClass() const
{
return "FactoryParamsMap";
return "FactoryMapNode";
}
#define WOLF_REGISTER_MAP(MapType) \
namespace{ const bool WOLF_UNUSED MapType##Registered = \
FactoryMap::registerCreator(#MapType, MapType::create); } \
typedef Factory<MapBasePtr, const std::string&, const std::vector<std::string>&> FactoryMapFile;
typedef Factory<MapBase,
const ParamsServer&> AutoConfFactoryMap;
template<>
inline std::string AutoConfFactoryMap::getClass() const
template <>
inline std::string FactoryMapFile::getClass() const
{
return "AutoConfFactoryMap";
return "FactoryMapFile";
}
#define WOLF_REGISTER_MAP_AUTO(MapType) \
namespace{ const bool WOLF_UNUSED MapType##AutoConfRegistered = \
AutoConfFactoryMap::registerCreator(#MapType, MapType::create); } \
#define WOLF_REGISTER_MAP(MapType) \
namespace \
{ \
const bool WOLF_UNUSED MapType##Registered = FactoryMapNode::registerCreator(#MapType, MapType::create); \
} \
namespace \
{ \
const bool WOLF_UNUSED MapType##RegisteredYaml = FactoryMapFile::registerCreator(#MapType, MapType::create); \
}
} /* namespace wolf */
#endif /* SENSOR_FACTORY_H_ */
include/core/map/map_base.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef MAP_BASE_H_
#define MAP_BASE_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Fwd refs
namespace wolf{
namespace wolf
{
class Problem;
class LandmarkBase;
}
} // namespace wolf
//Wolf includes
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/common/params_base.h"
//std includes
#include "core/map/factory_map.h"
#include "core/landmark/factory_landmark.h"
#include "core/landmark/landmark_base.h"
namespace wolf {
// other includes
#include "yaml-schema-cpp/yaml_server.hpp"
namespace wolf
{
/*
* Macro for defining Autoconf map creator.
*
......@@ -47,103 +45,99 @@ namespace wolf {
* In order to use this macro, the derived map class, MapClass,
* must have a constructor available with the API:
*
* MapClass(const ParamsMapClassPtr _params);
* MapClass(const YAML::Node& _params);
*
* We recommend writing one of such constructors in your derived maps.
*/
#define WOLF_MAP_CREATE(MapClass, ParamsMapClass) \
static \
MapBasePtr create(const ParamsServer& _server) \
{ \
auto params = std::make_shared<ParamsMapClass>(_server); \
\
return std::make_shared<MapClass>(params); \
} \
\
static \
MapBasePtr create(const ParamsMapBasePtr _params) \
{ \
auto params = std::static_pointer_cast<ParamsMapClass>(_params); \
\
return std::make_shared<MapClass>(params); \
} \
/** \brief base struct for map parameters
*
* Derive from this struct to create structs of map parameters.
*/
struct ParamsMapBase: public ParamsBase
{
std::string prefix = "map";
ParamsMapBase(const ParamsServer& _param_server) :
ParamsBase("map", _param_server)
{
};
~ParamsMapBase() override = default;
std::string print() const override
{
return "";
#define WOLF_MAP_CREATE(MapClass) \
static MapBasePtr create(const YAML::Node& _input_node, const std::vector<std::string>& _folders_schema = {}) \
{ \
if (not _folders_schema.empty()) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
server.setYaml(_input_node); \
if (not server.applySchema(#MapClass)) \
{ \
WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
} \
MapBasePtr map = std::shared_ptr<MapClass>(new MapClass(_input_node)); \
if (_input_node["landmarks"]) \
{ \
for (unsigned int i = 0; i < _input_node["landmarks"].size(); i++) \
{ \
LandmarkBasePtr lmk_ptr = \
FactoryLandmark::create(_input_node["landmarks"][i]["type"].as<std::string>(), \
_input_node["landmarks"][i], \
_folders_schema); \
lmk_ptr->link(map); \
} \
} \
return map; \
} \
static MapBasePtr create(const std::string& _yaml_filepath, const std::vector<std::string>& _folders_schema) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
if (not server.applySchema(#MapClass)) \
{ \
WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
return create(server.getNode(), {}); \
}
};
//class MapBase
// class MapBase
class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
{
friend LandmarkBase;
private:
LandmarkBasePtrList landmark_list_;
public:
MapBase();
MapBase(ParamsMapBasePtr _params, const std::string& _type = "Base");
WOLF_MAP_CREATE(MapBase, ParamsMapBase);
~MapBase() override;
protected:
virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr);
virtual void removeLandmark(LandmarkBasePtr _landmark_ptr);
public:
LandmarkBaseConstPtrList getLandmarkList() const;
LandmarkBasePtrList getLandmarkList();
LandmarkBaseConstPtr getLandmark(const unsigned int& _id) const;
LandmarkBasePtr getLandmark(const unsigned int& _id);
void load(std::string _map_file_yaml);
void save(std::string _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf") const;
virtual void printHeader(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream ,
std::string _tabs = "") const;
void print(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, MapBaseConstPtr _map_ptr, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
std::string dateTimeNow() const;
private:
LandmarkBasePtrList landmark_list_;
public:
MapBase();
MapBase(const YAML::Node& _params);
MapBase(const std::string& _type, const YAML::Node& _params);
WOLF_MAP_CREATE(MapBase);
~MapBase() override = default;
bool hasChildren() const override;
protected:
virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr);
virtual void removeLandmark(LandmarkBasePtr _landmark_ptr);
public:
LandmarkBaseConstPtrList getLandmarkList() const;
LandmarkBasePtrList getLandmarkList();
LandmarkBaseConstPtr getLandmark(const unsigned int& _id) const;
LandmarkBasePtr getLandmark(const unsigned int& _id);
virtual void load(std::string _map_file_yaml);
virtual bool save(std::string _map_file_yaml, const std::string& _map_name = "") const;
virtual void printHeader(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const;
virtual void print(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
virtual bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
};
inline LandmarkBaseConstPtrList MapBase::getLandmarkList() const
{
LandmarkBaseConstPtrList list_const;
for (auto&& obj_ptr : landmark_list_)
list_const.push_back(obj_ptr);
for (auto&& obj_ptr : landmark_list_) list_const.push_back(obj_ptr);
return list_const;
}
......@@ -152,6 +146,4 @@ inline LandmarkBasePtrList MapBase::getLandmarkList()
return landmark_list_;
}
} // namespace wolf
#endif
} // namespace wolf
include/core/math/SE2.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file SE2.h
*
* Created on: Jul 27, 2019
* \author: jsola
*/
#ifndef MATH_SE2_H_
#define MATH_SE2_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/common/wolf.h"
#include "core/math/rotations.h"
#include "core/state_block/state_composite.h"
#include "core/composite/vector_composite.h"
// #include "core/composite/matrix_composite.h"
#include <Eigen/Geometry>
#include <Eigen/Dense>
......@@ -39,72 +29,74 @@
/*
* Some of the functions below are based on:
*
* [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018, https://arxiv.org/pdf/1812.01537.pdf
* [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018,
* https://arxiv.org/pdf/1812.01537.pdf
*/
namespace wolf
{
namespace SO2{
template<typename T>
namespace SO2
{
template <typename T>
inline Eigen::Matrix<T, 2, 2> exp(const T theta)
{
return Eigen::Rotation2D<T>(theta).matrix();
}
} // namespace SO2
namespace SE2{
} // namespace SO2
template<class T>
namespace SE2
{
template <class T>
inline Eigen::Matrix<T, 2, 2> skew(const T& t)
{
return (Eigen::Matrix<T, 2, 2>() << (T) 0, -t, t, (T) 0).finished();
return (Eigen::Matrix<T, 2, 2>() << (T)0, -t, t, (T)0).finished();
}
/** \brief Compute [1]_x * t = (-t.y ; t.x)
*/
template<class D>
inline Eigen::Matrix<typename D::Scalar, 2, 1> skewed(const MatrixBase<D>& t)
template <class D>
inline Eigen::Matrix<typename D::Scalar, 2, 1> skewed(const Eigen::MatrixBase<D>& t)
{
return Eigen::Matrix<typename D::Scalar, 2, 1>(-t(1), t(0));
return Eigen::Matrix<typename D::Scalar, 2, 1>(-t(1), t(0));
}
template<typename T>
template <typename T>
inline Eigen::Matrix2d V_helper(const T theta)
{
T s; // sin(theta) / theta
T c_1; // (1-cos(theta)) / theta
T s; // sin(theta) / theta
T c_1; // (1-cos(theta)) / theta
if (fabs(theta) > T(Constants::EPS))
{
// [1] eq. 158
s = sin(theta) / theta;
s = sin(theta) / theta;
c_1 = (T(1.0) - cos(theta)) / theta;
}
else
{
// approx of [1] eq. 158
s = T(1.0); // sin(x) ~= x
c_1 = theta / T(2.0); // cos(x) ~= 1 - x^2/2
s = T(1.0); // sin(x) ~= x
c_1 = theta / T(2.0); // cos(x) ~= 1 - x^2/2
}
return (Matrix<T, 2, 2>() << s, -c_1, c_1, s).finished();
return (Eigen::Matrix<T, 2, 2>() << s, -c_1, c_1, s).finished();
}
inline VectorComposite identity()
{
VectorComposite v;
v['P'] = Vector2d::Zero();
v['O'] = Vector1d::Zero();
v['P'] = Eigen::Vector2d::Zero();
v['O'] = Eigen::Vector1d::Zero();
return v;
}
template<typename D1, typename D2>
inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o,
MatrixBase<D2>& ip, typename D2::Scalar& io)
template <typename D1, typename D2>
inline void inverse(const Eigen::MatrixBase<D1>& p,
const typename D1::Scalar& o,
Eigen::MatrixBase<D2>& ip,
typename D2::Scalar& io)
{
MatrixSizeCheck<2, 1>::check(p);
MatrixSizeCheck<2, 1>::check(ip);
......@@ -113,9 +105,11 @@ inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o,
io = -o;
}
template<typename D1, typename D2, typename D3, typename D4>
inline void inverse(const MatrixBase<D1>& p, const MatrixBase<D2>& o,
MatrixBase<D3>& ip, MatrixBase<D4>& io)
template <typename D1, typename D2, typename D3, typename D4>
inline void inverse(const Eigen::MatrixBase<D1>& p,
const Eigen::MatrixBase<D2>& o,
Eigen::MatrixBase<D3>& ip,
Eigen::MatrixBase<D4>& io)
{
MatrixSizeCheck<2, 1>::check(p);
MatrixSizeCheck<1, 1>::check(o);
......@@ -125,23 +119,22 @@ inline void inverse(const MatrixBase<D1>& p, const MatrixBase<D2>& o,
inverse(p, o(0), ip, io(0));
}
template<typename D1, typename D2>
inline void inverse(const MatrixBase<D1>& d,
MatrixBase<D2>& id)
template <typename D1, typename D2>
inline void inverse(const Eigen::MatrixBase<D1>& d, Eigen::MatrixBase<D2>& id)
{
MatrixSizeCheck<3, 1>::check(d);
MatrixSizeCheck<3, 1>::check(id);
Map<const Matrix<typename D1::Scalar, 2, 1> > p ( & d( 0 ) );
Map<Matrix<typename D2::Scalar, 2, 1> > ip ( & id( 0 ) );
Eigen::Map<const Eigen::Matrix<typename D1::Scalar, 2, 1> > p(&d(0));
Eigen::Map<Eigen::Matrix<typename D2::Scalar, 2, 1> > ip(&id(0));
inverse(p, d(2), ip, id(2));
}
template<typename D>
inline Matrix<typename D::Scalar, 3, 1> inverse(const MatrixBase<D>& d)
template <typename D>
inline Eigen::Matrix<typename D::Scalar, 3, 1> inverse(const Eigen::MatrixBase<D>& d)
{
Matrix<typename D::Scalar, 3, 1> id;
Eigen::Matrix<typename D::Scalar, 3, 1> id;
inverse(d, id);
return id;
}
......@@ -153,12 +146,12 @@ inline void inverse(const VectorComposite& v, VectorComposite& c)
inline VectorComposite inverse(const VectorComposite& v)
{
VectorComposite c("PO", {2,1});
VectorComposite c = identity();
inverse(v, c);
return c;
}
template<class D1, class D2>
template <class D1, class D2>
inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
{
MatrixSizeCheck<3, 1>::check(_tau);
......@@ -166,29 +159,29 @@ inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta
// [1] eq. 156
_delta.head(2) = V_helper(_tau(2)) * _tau.head(2);
_delta(2) = pi2pi(_tau(2));
_delta(2) = pi2pi(_tau(2));
}
template<class D, typename T>
template <class D, typename T>
inline Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T theta)
{
MatrixSizeCheck<2, 1>::check (p);
MatrixSizeCheck<2, 1>::check(p);
// Jacobian of t = V(theta)*p wrt theta -- developed in Matlab symbolic, and copied here.
T x = p(0);
T y = p(1);
Matrix<T, 2, 1> J_Vp_th;
T x = p(0);
T y = p(1);
Eigen::Matrix<T, 2, 1> J_Vp_th;
if (fabs(theta) > T(Constants::EPS))
{
T s_th = sin(theta);
T c_th = cos(theta);
T s_th = sin(theta);
T c_th = cos(theta);
T theta2 = theta * theta;
J_Vp_th << -(y * c_th - y + x * s_th - theta * x * c_th + theta * y * s_th) / theta2,
(x * c_th - x - y * s_th + theta * y * c_th + theta * x * s_th) / theta2;
(x * c_th - x - y * s_th + theta * y * c_th + theta * x * s_th) / theta2;
}
else
{ // sin(x) ~= x
{ // sin(x) ~= x
// cos(x) ~= 1 - x^2/2
J_Vp_th << -y / 2.0, x / 2.0;
}
......@@ -196,8 +189,8 @@ inline Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T
return J_Vp_th;
}
template<class D1, class D2, class D3>
inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_delta_tau)
template <class D1, class D2, class D3>
inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta, Eigen::MatrixBase<D3>& _J_delta_tau)
{
MatrixSizeCheck<3, 1>::check(_tau);
MatrixSizeCheck<3, 1>::check(_delta);
......@@ -206,10 +199,10 @@ inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D
typedef typename D1::Scalar T;
// [1] eq. 156
T theta = pi2pi(_tau(2));
Eigen::Matrix<T, 2, 2> V = V_helper(theta);
_delta.head(2) = V * _tau.head(2);
_delta(2) = theta;
T theta = pi2pi(_tau(2));
Eigen::Matrix<T, 2, 2> V = V_helper(theta);
_delta.head(2) = V * _tau.head(2);
_delta(2) = theta;
// Jacobian is the composite definition [1] eq. 89, with jacobian blocks:
// J_Vp_p = V: see V_helper, eq. 158
......@@ -221,70 +214,73 @@ inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D
inline void exp(const VectorComposite& _tau, VectorComposite& _delta)
{
// [1] eq. 156
const auto &p = _tau.at('P');
const auto &theta = pi2pi(_tau.at('O')(0));
Matrix2d V = V_helper(theta);
const auto& p = _tau.at('P');
const auto& theta = pi2pi(_tau.at('O')(0));
Eigen::Matrix2d V = V_helper(theta);
_delta['P'] = V * p;
_delta['O'] = Matrix1d(theta);
_delta['O'] = Eigen::Matrix1d(theta);
}
inline VectorComposite exp(const VectorComposite& tau)
{
VectorComposite res("PO", {2,1});
VectorComposite res = identity();
exp(tau, res);
return res;
}
inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComposite& _J_delta_tau)
{
// [1] eq. 156
const auto &p = _tau.at('P');
const auto &theta = pi2pi(_tau.at('O')(0));
Matrix2d V = V_helper(theta);
_delta['P'] = V * p;
_delta['O'] = Matrix1d(theta);
// Jacobian follows the composite definition in [1] eq. 89, with jacobian blocks:
// J_Vp_p = V: see V_helper, eq. 158
// J_Vp_theta: see fcn helper
// J_theta_theta = 1; eq. 126
_J_delta_tau.clear();
_J_delta_tau.emplace('P', 'P', V);
_J_delta_tau.emplace('P', 'O', J_Vp_theta(p, theta));
_J_delta_tau.emplace('O', 'P', RowVector2d(0.0, 0.0));
_J_delta_tau.emplace('O', 'O', Matrix1d(1));
}
template<class D1, class D2, class D3>
inline void compose(const MatrixBase<D1> &_delta1, const MatrixBase<D2> &_delta2, MatrixBase<D3> &_delta1_compose_delta2)
// inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComposite& _J_delta_tau)
// {
// // [1] eq. 156
// const auto& p = _tau.at('P');
// const auto& theta = pi2pi(_tau.at('O')(0));
// Matrix2d V = V_helper(theta);
// _delta['P'] = V * p;
// _delta['O'] = Eigen::Matrix1d(theta);
// // Jacobian follows the composite definition in [1] eq. 89, with jacobian blocks:
// // J_Vp_p = V: see V_helper, eq. 158
// // J_Vp_theta: see fcn helper
// // J_theta_theta = 1; eq. 126
// _J_delta_tau.clear();
// _J_delta_tau.emplace('P', 'P', V);
// _J_delta_tau.emplace('P', 'O', J_Vp_theta(p, theta));
// _J_delta_tau.emplace('O', 'P', Eigen::RowVector2d(0.0, 0.0));
// _J_delta_tau.emplace('O', 'O', Eigen::Matrix1d(1));
// }
template <class D1, class D2, class D3>
inline void compose(const Eigen::MatrixBase<D1>& _delta1,
const Eigen::MatrixBase<D2>& _delta2,
Eigen::MatrixBase<D3>& _delta1_compose_delta2)
{
MatrixSizeCheck<3, 1>::check(_delta1);
MatrixSizeCheck<3, 1>::check(_delta2);
MatrixSizeCheck<3, 1>::check(_delta1_compose_delta2);
_delta1_compose_delta2.template head<2>() = _delta1.template head<2>()
+ SO2::exp(_delta1(2)) * _delta2.template head<2>();
_delta1_compose_delta2.template head<2>() =
_delta1.template head<2>() + SO2::exp(_delta1(2)) * _delta2.template head<2>();
_delta1_compose_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
}
template<class D1, class D2>
inline Matrix<typename D1::Scalar,3,1> compose(const MatrixBase<D1> &_delta1, const MatrixBase<D2> &_delta2)
template <class D1, class D2>
inline Eigen::Matrix<typename D1::Scalar, 3, 1> compose(const Eigen::MatrixBase<D1>& _delta1,
const Eigen::MatrixBase<D2>& _delta2)
{
Matrix<typename D1::Scalar,3,1> delta1_compose_delta2;
Eigen::Matrix<typename D1::Scalar, 3, 1> delta1_compose_delta2;
compose(_delta1, _delta2, delta1_compose_delta2);
return delta1_compose_delta2;
}
template<class D1, class D2, class D3, class D4, class D5>
inline void compose(const MatrixBase<D1>& _delta1,
const MatrixBase<D2>& _delta2,
MatrixBase<D3>& _delta1_compose_delta2,
MatrixBase<D4>& _J_compose_delta1,
MatrixBase<D5>& _J_compose_delta2)
template <class D1, class D2, class D3, class D4, class D5>
inline void compose(const Eigen::MatrixBase<D1>& _delta1,
const Eigen::MatrixBase<D2>& _delta2,
Eigen::MatrixBase<D3>& _delta1_compose_delta2,
Eigen::MatrixBase<D4>& _J_compose_delta1,
Eigen::MatrixBase<D5>& _J_compose_delta2)
{
MatrixSizeCheck<3, 1>::check(_delta1);
MatrixSizeCheck<3, 1>::check(_delta2);
......@@ -292,7 +288,7 @@ inline void compose(const MatrixBase<D1>& _delta1,
MatrixSizeCheck<3, 3>::check(_J_compose_delta1);
MatrixSizeCheck<3, 3>::check(_J_compose_delta2);
Matrix2d R1 = SO2::exp(_delta1(2)); // need temporary
Eigen::Matrix2d R1 = SO2::exp(_delta1(2)); // need temporary
// tc = t1 + R1 t2
_delta1_compose_delta2.template head<2>() = _delta1.template head<2>() + R1 * _delta2.template head<2>();
......@@ -334,98 +330,99 @@ inline void compose(const MatrixBase<D1>& _delta1,
_J_compose_delta2.template block<2, 2>(0, 0) = R1;
}
inline void compose(const VectorComposite& _x1,
const VectorComposite& _x2,
VectorComposite& _c)
inline void compose(const VectorComposite& _x1, const VectorComposite& _x2, VectorComposite& _c)
{
const auto& p1 = _x1.at('P');
const auto& a1 = _x1.at('O')(0); // angle
const auto& a1 = _x1.at('O')(0); // angle
const auto& R1 = SO2::exp(a1);
const auto& p2 = _x2.at('P');
const auto& a2 = _x2.at('O')(0); // angle
const auto& a2 = _x2.at('O')(0); // angle
// tc = t1 + R1 t2
_c['P'] = p1 + R1 * p2;
// Rc = R1 R2 --> theta = theta1 + theta2
_c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
_c['O'] = Eigen::Matrix1d(pi2pi(a1 + a2));
}
inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
{
VectorComposite c("PO", {2,1});
VectorComposite c = identity();
compose(x1, x2, c);
return c;
}
inline void compose(const VectorComposite& _x1,
const VectorComposite& _x2,
VectorComposite& _c,
MatrixComposite& _J_c_x1,
MatrixComposite& _J_c_x2)
{
const auto& p1 = _x1.at('P');
const auto& a1 = _x1.at('O')(0); // angle
Matrix2d R1 = SO2::exp(a1); // need temporary
const auto& p2 = _x2.at('P');
const auto& a2 = _x2.at('O')(0); // angle
/* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
*
* resulting delta is:
*
* tc = t1 + R1 t2 (*)
* Rc = R1 R2 (**)
*
* Jacobians have the form:
*
* [ J_t_t J_t_R ]
* [ J_r_t J_R_R ]
*
* Jacobian blocks are:
*
* J_tc_t1 = I trivial from (*)
* J_tc_R1 = R1 [1]x t2 = R1 (-t2.y ; t2.x) see [1]: eq. (129)
*
* J_Rc_t1 = (0 0) trivial from (**)
* J_Rc_R1 = 1 see [1]: eq. (125)
*
* J_tc_t2 = R1 see [1]: eq. (130)
* J_tc_R2 = 0 trivial from (*)
*
* J_Rc_t2 = 0 trivial from (**)
* J_Rc_R2 = 1 see [1]: eq. (125)
*/
_J_c_x1.clear();
_J_c_x1.emplace('P', 'P', Matrix2d::Identity());
_J_c_x1.emplace('P', 'O', MatrixXd(R1 * skewed(p2)) );
_J_c_x1.emplace('O', 'P', RowVector2d(0,0));
_J_c_x1.emplace('O', 'O', Matrix1d(1));
_J_c_x2.clear();
_J_c_x2.emplace('P', 'P', R1);
_J_c_x2.emplace('P', 'O', Vector2d(0,0));
_J_c_x2.emplace('O', 'P', RowVector2d(0,0));
_J_c_x2.emplace('O', 'O', Matrix1d(1));
// Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1' or 'x2'.
// tc = t1 + R1 t2
_c['P'] = p1 + R1 * p2;
// Rc = R1 R2 --> theta = theta1 + theta2
_c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
MatrixBase<D7>& plus_p, MatrixBase<D8>& plus_q)
// inline void compose(const VectorComposite& _x1,
// const VectorComposite& _x2,
// VectorComposite& _c,
// MatrixComposite& _J_c_x1,
// MatrixComposite& _J_c_x2)
// {
// const auto& p1 = _x1.at('P');
// const auto& a1 = _x1.at('O')(0); // angle
// Eigen::Matrix2d R1 = SO2::exp(a1); // need temporary
// const auto& p2 = _x2.at('P');
// const auto& a2 = _x2.at('O')(0); // angle
// /* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
// *
// * resulting delta is:
// *
// * tc = t1 + R1 t2 (*)
// * Rc = R1 R2 (**)
// *
// * Jacobians have the form:
// *
// * [ J_t_t J_t_R ]
// * [ J_r_t J_R_R ]
// *
// * Jacobian blocks are:
// *
// * J_tc_t1 = I trivial from (*)
// * J_tc_R1 = R1 [1]x t2 = R1 (-t2.y ; t2.x) see [1]: eq. (129)
// *
// * J_Rc_t1 = (0 0) trivial from (**)
// * J_Rc_R1 = 1 see [1]: eq. (125)
// *
// * J_tc_t2 = R1 see [1]: eq. (130)
// * J_tc_R2 = 0 trivial from (*)
// *
// * J_Rc_t2 = 0 trivial from (**)
// * J_Rc_R2 = 1 see [1]: eq. (125)
// */
// _J_c_x1.clear();
// _J_c_x1.emplace('P', 'P', Eigen::Matrix2d::Identity());
// _J_c_x1.emplace('P', 'O', Eigen::MatrixXd(R1 * skewed(p2)));
// _J_c_x1.emplace('O', 'P', Eigen::RowVector2d(0, 0));
// _J_c_x1.emplace('O', 'O', Eigen::Matrix1d(1));
// _J_c_x2.clear();
// _J_c_x2.emplace('P', 'P', R1);
// _J_c_x2.emplace('P', 'O', Eigen::Vector2d(0, 0));
// _J_c_x2.emplace('O', 'P', Eigen::RowVector2d(0, 0));
// _J_c_x2.emplace('O', 'O', Eigen::Matrix1d(1));
// // Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1'
// or
// // 'x2'.
// // tc = t1 + R1 t2
// _c['P'] = p1 + R1 * p2;
// // Rc = R1 R2 --> theta = theta1 + theta2
// _c['O'] = Eigen::Matrix1d(pi2pi(a1 + a2));
// }
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void plus(const Eigen::MatrixBase<D1>& p1,
const Eigen::MatrixBase<D2>& q1,
const Eigen::MatrixBase<D4>& p2,
const Eigen::MatrixBase<D5>& o2,
Eigen::MatrixBase<D7>& plus_p,
Eigen::MatrixBase<D8>& plus_q)
{
MatrixSizeCheck<2, 1>::check(p1);
MatrixSizeCheck<1, 1>::check(q1);
......@@ -438,26 +435,23 @@ inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
plus_q(0) = pi2pi(q1(0) + o2(0));
}
template<typename D1, typename D2, typename D3>
inline void plus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& d_plus)
template <typename D1, typename D2, typename D3>
inline void plus(const Eigen::MatrixBase<D1>& d1, const Eigen::MatrixBase<D2>& d2, Eigen::MatrixBase<D3>& d_plus)
{
Map<const Matrix<typename D1::Scalar, 2, 1> > p1 ( & d1(0) );
Map<const Matrix<typename D1::Scalar, 1, 1> > q1 ( & d1(2) );
Map<const Matrix<typename D2::Scalar, 2, 1> > p2 ( & d2(0) );
Map<const Matrix<typename D2::Scalar, 1, 1> > o2 ( & d2(2) );
Map<Matrix<typename D3::Scalar, 2, 1> > p_p ( & d_plus(0) );
Map<Matrix<typename D1::Scalar, 1, 1> > q_p ( & d_plus(2) );
Eigen::Map<const Eigen::Matrix<typename D1::Scalar, 2, 1> > p1(&d1(0));
Eigen::Map<const Eigen::Matrix<typename D1::Scalar, 1, 1> > q1(&d1(2));
Eigen::Map<const Eigen::Matrix<typename D2::Scalar, 2, 1> > p2(&d2(0));
Eigen::Map<const Eigen::Matrix<typename D2::Scalar, 1, 1> > o2(&d2(2));
Eigen::Map<Eigen::Matrix<typename D3::Scalar, 2, 1> > p_p(&d_plus(0));
Eigen::Map<Eigen::Matrix<typename D1::Scalar, 1, 1> > q_p(&d_plus(2));
plus(p1, q1, p2, o2, p_p, q_p);
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 3, 1> plus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2)
template <typename D1, typename D2>
inline Eigen::Matrix<typename D1::Scalar, 3, 1> plus(const Eigen::MatrixBase<D1>& d1, const Eigen::MatrixBase<D2>& d2)
{
Matrix<typename D1::Scalar, 3, 1> d_plus;
Eigen::Matrix<typename D1::Scalar, 3, 1> d_plus;
plus(d1, d2, d_plus);
return d_plus;
}
......@@ -469,33 +463,31 @@ inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorCom
inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
{
VectorComposite res;
res['P'] = Vector2d();
res['O'] = Vector1d();
VectorComposite res = identity();
plus(x, tau, res);
return res;
}
template<typename D1, typename D2, typename D3>
inline void between(const MatrixBase<D1>& p1, const typename D1::Scalar& o1,
const MatrixBase<D2>& p2, const typename D2::Scalar& o2,
MatrixBase<D3>& p12, typename D3::Scalar& o12)
template <typename D1, typename D2, typename D3>
inline void between(const Eigen::MatrixBase<D1>& p1,
const typename D1::Scalar& o1,
const Eigen::MatrixBase<D2>& p2,
const typename D2::Scalar& o2,
Eigen::MatrixBase<D3>& p12,
typename D3::Scalar& o12)
{
MatrixSizeCheck<2, 1>::check(p1);
MatrixSizeCheck<2, 1>::check(p2);
MatrixSizeCheck<2, 1>::check(p12);
MatrixSizeCheck<2, 1>::check(p1);
MatrixSizeCheck<2, 1>::check(p2);
MatrixSizeCheck<2, 1>::check(p12);
p12 = SO2::exp(-o1) * ( p2 - p1 );
o12 = o2 - o1;
p12 = SO2::exp(-o1) * (p2 - p1);
o12 = o2 - o1;
}
template<typename D1, typename D2, typename D3>
inline void between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& d12)
template <typename D1, typename D2, typename D3>
inline void between(const Eigen::MatrixBase<D1>& d1, const Eigen::MatrixBase<D2>& d2, Eigen::MatrixBase<D3>& d12)
{
MatrixSizeCheck<3, 1>::check(d1);
MatrixSizeCheck<3, 1>::check(d2);
......@@ -503,25 +495,23 @@ inline void between(const MatrixBase<D1>& d1,
typedef typename D1::Scalar T;
Map<const Matrix<T, 2, 1> > p1 ( & d1(0) );
Map<const Matrix<T, 2, 1> > p2 ( & d2(0) );
Map<Matrix<T, 2, 1> > p12 ( & d12(0) );
Eigen::Map<const Eigen::Matrix<T, 2, 1> > p1(&d1(0));
Eigen::Map<const Eigen::Matrix<T, 2, 1> > p2(&d2(0));
Eigen::Map<Eigen::Matrix<T, 2, 1> > p12(&d12(0));
between(p1, d1(2), p2, d2(2), p12, d12(2));
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 3, 1> between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2 )
template <typename D1, typename D2>
inline Eigen::Matrix<typename D1::Scalar, 3, 1> between(const Eigen::MatrixBase<D1>& d1,
const Eigen::MatrixBase<D2>& d2)
{
MatrixSizeCheck<3, 1>::check(d1);
MatrixSizeCheck<3, 1>::check(d2);
Matrix<typename D1::Scalar, 3, 1> d12;
Eigen::Matrix<typename D1::Scalar, 3, 1> d12;
between(d1, d2, d12);
return d12;
}
} // namespace SE2
} // namespacs wolf
#endif /* MATH_SE2_H_ */
} // namespace SE2
} // namespace wolf
include/core/math/SE3.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* SE3.h
*
* Created on: Mar 15, 2018
* Author: jsola
*/
#ifndef SE3_H_
#define SE3_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/common/wolf.h"
#include "core/math/rotations.h"
#include "core/state_block/state_composite.h"
#include "core/composite/vector_composite.h"
/*
* The functions in this file are related to manipulations of Delta motion magnitudes used in 3d motion.
......@@ -58,23 +47,25 @@
*
* Some of the functions below are based on:
*
* [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018, https://arxiv.org/pdf/1812.01537.pdf
* [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018,
* https://arxiv.org/pdf/1812.01537.pdf
*/
namespace wolf
{
namespace SE3 {
namespace SE3
{
using namespace Eigen;
template<typename D1, typename D2>
template <typename D1, typename D2>
inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q)
{
MatrixSizeCheck<3, 1>::check(p);
p = MatrixBase<D1>::Zero(3,1);
p = MatrixBase<D1>::Zero(3, 1);
q = QuaternionBase<D2>::Identity();
}
template<typename D1, typename D2>
template <typename D1, typename D2>
inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q)
{
MatrixSizeCheck<3, 1>::check(p);
......@@ -92,42 +83,39 @@ inline void identity(VectorComposite& v)
v['O'] = Quaterniond::Identity().coeffs();
}
template<typename T = double>
template <typename T = double>
inline Matrix<T, 7, 1> identity()
{
Matrix<T, 7, 1> ret;
ret<< T(0), T(0), T(0),
T(0), T(0), T(0), T(1);
ret << T(0), T(0), T(0), T(0), T(0), T(0), T(1);
return ret;
}
template<typename D1, typename D2, typename D4, typename D5>
inline void inverse(const MatrixBase<D1>& p, const QuaternionBase<D2>& q,
MatrixBase<D4>& ip, QuaternionBase<D5>& iq)
template <typename D1, typename D2, typename D4, typename D5>
inline void inverse(const MatrixBase<D1>& p, const QuaternionBase<D2>& q, MatrixBase<D4>& ip, QuaternionBase<D5>& iq)
{
MatrixSizeCheck<3, 1>::check(p);
MatrixSizeCheck<3, 1>::check(ip);
ip = -(q.conjugate() * p) ;
iq = q.conjugate() ;
ip = -(q.conjugate() * p);
iq = q.conjugate();
}
template<typename D1, typename D2>
inline void inverse(const MatrixBase<D1>& d,
MatrixBase<D2>& id)
template <typename D1, typename D2>
inline void inverse(const MatrixBase<D1>& d, MatrixBase<D2>& id)
{
MatrixSizeCheck<7, 1>::check(d);
MatrixSizeCheck<7, 1>::check(id);
Map<const Matrix<typename D1::Scalar, 3, 1> > p ( & d( 0 ) );
Map<const Quaternion<typename D1::Scalar> > q ( & d( 3 ) );
Map<Matrix<typename D2::Scalar, 3, 1> > ip ( & id( 0 ) );
Map<Quaternion<typename D2::Scalar> > iq ( & id( 3 ) );
Map<const Matrix<typename D1::Scalar, 3, 1>> p(&d(0));
Map<const Quaternion<typename D1::Scalar>> q(&d(3));
Map<Matrix<typename D2::Scalar, 3, 1>> ip(&id(0));
Map<Quaternion<typename D2::Scalar>> iq(&id(3));
inverse(p, q, ip, iq);
}
template<typename D>
template <typename D>
inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
{
Matrix<typename D::Scalar, 7, 1> id;
......@@ -137,107 +125,111 @@ inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
inline void inverse(const VectorComposite& v, VectorComposite& c)
{
Map<const Quaternion<double> > qv( & v.at('O')(0) );
Map<Quaternion<double> > qc( & c['O'](0) );
Map<const Quaternion<double>> qv(&v.at('O')(0));
Map<Quaternion<double>> qc(&c['O'](0));
inverse(v.at('P'), qv, c['P'], qc);
}
inline VectorComposite inverse(const VectorComposite& v)
{
VectorComposite c("PO", {3,4});
VectorComposite c(
{{'P', Vector3d::Zero()}, {'O', Vector4d::Zero()}}); // no need to normalize, will be overwritten
inverse(v, c);
return c;
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void compose(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
MatrixBase<D7>& pc, QuaternionBase<D8>& qc )
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void compose(const MatrixBase<D1>& p1,
const QuaternionBase<D2>& q1,
const MatrixBase<D4>& p2,
const QuaternionBase<D5>& q2,
MatrixBase<D7>& pc,
QuaternionBase<D8>& qc)
{
MatrixSizeCheck<3, 1>::check(p1);
MatrixSizeCheck<3, 1>::check(p2);
MatrixSizeCheck<3, 1>::check(pc);
MatrixSizeCheck<3, 1>::check(p1);
MatrixSizeCheck<3, 1>::check(p2);
MatrixSizeCheck<3, 1>::check(pc);
pc = p1 + q1 * p2;
qc = q1 * q2; // q here to avoid possible aliasing between d1 and sum
pc = p1 + q1 * p2;
qc = q1 * q2; // q here to avoid possible aliasing between d1 and sum
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void compose(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
const MatrixBase<D4>& p2, const MatrixBase<D5>& q2,
MatrixBase<D7>& pc, MatrixBase<D8>& qc )
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void compose(const MatrixBase<D1>& p1,
const MatrixBase<D2>& q1,
const MatrixBase<D4>& p2,
const MatrixBase<D5>& q2,
MatrixBase<D7>& pc,
MatrixBase<D8>& qc)
{
MatrixSizeCheck<3, 1>::check(p1);
MatrixSizeCheck<4, 1>::check(q1);
MatrixSizeCheck<3, 1>::check(p2);
MatrixSizeCheck<4, 1>::check(q2);
MatrixSizeCheck<3, 1>::check(pc);
MatrixSizeCheck<4, 1>::check(qc);
MatrixSizeCheck<3, 1>::check(p1);
MatrixSizeCheck<4, 1>::check(q1);
MatrixSizeCheck<3, 1>::check(p2);
MatrixSizeCheck<4, 1>::check(q2);
MatrixSizeCheck<3, 1>::check(pc);
MatrixSizeCheck<4, 1>::check(qc);
Map<const Quaternion<typename D2::Scalar> > mq1 ( & q1( 0 ) );
Map<const Quaternion<typename D5::Scalar> > mq2 ( & q2( 0 ) );
Map< Quaternion<typename D8::Scalar> > mqc ( & qc( 0 ) );
Map<const Quaternion<typename D2::Scalar>> mq1(&q1(0));
Map<const Quaternion<typename D5::Scalar>> mq2(&q2(0));
Map<Quaternion<typename D8::Scalar>> mqc(&qc(0));
compose(p1, mq1, p2, mq2, pc, mqc);
compose(p1, mq1, p2, mq2, pc, mqc);
}
template<typename D1, typename D2, typename D3>
inline void compose(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& sum)
template <typename D1, typename D2, typename D3>
inline void compose(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& sum)
{
MatrixSizeCheck<7, 1>::check(d1);
MatrixSizeCheck<7, 1>::check(d2);
MatrixSizeCheck<7, 1>::check(sum);
Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1( 0 ) );
Map<const Quaternion<typename D1::Scalar> > q1 ( & d1( 3 ) );
Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2( 0 ) );
Map<const Quaternion<typename D2::Scalar> > q2 ( & d2( 3 ) );
Map<Matrix<typename D3::Scalar, 3, 1> > pc ( & sum( 0 ) );
Map<Quaternion<typename D3::Scalar> > qc ( & sum( 3 ) );
Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
Map<const Quaternion<typename D1::Scalar>> q1(&d1(3));
Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
Map<const Quaternion<typename D2::Scalar>> q2(&d2(3));
Map<Matrix<typename D3::Scalar, 3, 1>> pc(&sum(0));
Map<Quaternion<typename D3::Scalar>> qc(&sum(3));
compose(p1, q1, p2, q2, pc, qc);
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> compose(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2 )
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> compose(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
Matrix<typename D1::Scalar, 7, 1> ret;
Matrix<typename D1::Scalar, 7, 1> ret;
compose(d1, d2, ret);
return ret;
}
template<typename D1, typename D2, typename D3, typename D4, typename D5>
template <typename D1, typename D2, typename D3, typename D4, typename D5>
inline void compose(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& sum,
MatrixBase<D4>& J_sum_d1,
MatrixBase<D5>& J_sum_d2)
MatrixBase<D3>& sum,
MatrixBase<D4>& J_sum_d1,
MatrixBase<D5>& J_sum_d2)
{
MatrixSizeCheck<7, 1>::check(d1);
MatrixSizeCheck<7, 1>::check(d2);
MatrixSizeCheck<7, 1>::check(sum);
MatrixSizeCheck< 6, 6>::check(J_sum_d1);
MatrixSizeCheck< 6, 6>::check(J_sum_d2);
MatrixSizeCheck<6, 6>::check(J_sum_d1);
MatrixSizeCheck<6, 6>::check(J_sum_d2);
// Some useful temporaries
Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //q1.matrix(); // First Delta, DR
Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //q2.matrix(); // Second delta, dR
Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3, 4)); // q1.matrix(); // First Delta, DR
Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3, 4)); // q2.matrix(); // Second delta, dR
// Jac wrt first delta
J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
J_sum_d1.block(0,3,3,3).noalias() = - dR1 * skew(d2.head(3)) ; // dDp'/dDo
J_sum_d1.block(3,3,3,3) = dR2.transpose(); // dDo'/dDo
J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
J_sum_d1.block(0, 3, 3, 3).noalias() = -dR1 * skew(d2.head(3)); // dDp'/dDo
J_sum_d1.block(3, 3, 3, 3) = dR2.transpose(); // dDo'/dDo
// Jac wrt second delta
J_sum_d2.setIdentity(); //
J_sum_d2.block(0,0,3,3) = dR1; // dDp'/p
J_sum_d2.setIdentity(); //
J_sum_d2.block(0, 0, 3, 3) = dR1; // dDp'/p
// J_sum_d2.block(3,3,3,3) = Matrix3d::Identity(); // dDo'/ddo = I
// compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
// compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same
// variable
compose(d1, d2, sum);
}
......@@ -248,73 +240,75 @@ inline void compose(const VectorComposite& x1, const VectorComposite& x2, Vector
inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
{
VectorComposite c("PO", {3,4});
VectorComposite c(
{{'P', Vector3d::Zero()}, {'O', Vector4d::Zero()}}); // no need to normalize, will be overwritten
compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']);
return c;
}
inline void compose(const VectorComposite& x1,
const VectorComposite& x2,
VectorComposite& c,
MatrixComposite& J_c_x1,
MatrixComposite& J_c_x2)
// inline void compose(const VectorComposite& x1,
// const VectorComposite& x2,
// VectorComposite& c,
// MatrixComposite& J_c_x1,
// MatrixComposite& J_c_x2)
// {
// // Some useful temporaries
// const auto R1 = q2R(x1.at('O')); // q1.matrix(); // make temporary
// const auto& R2 = q2R(x2.at('O')); // q2.matrix(); // do not make temporary, only reference
// // Jac wrt first delta
// J_c_x1.emplace('P', 'P', Matrix3d::Identity());
// J_c_x1.emplace('P', 'O', -R1 * skew(x2.at('P')));
// J_c_x1.emplace('O', 'P', Matrix3d::Zero());
// J_c_x1.emplace('O', 'O', R2.transpose());
// // Jac wrt second delta
// J_c_x2.emplace('P', 'P', R1);
// J_c_x2.emplace('P', 'O', Matrix3d::Zero());
// J_c_x2.emplace('O', 'P', Matrix3d::Zero());
// J_c_x2.emplace('O', 'O', Matrix3d::Identity());
// // compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the
// same
// // variable
// compose(x1, x2, c);
// }
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void between(const MatrixBase<D1>& p1,
const QuaternionBase<D2>& q1,
const MatrixBase<D4>& p2,
const QuaternionBase<D5>& q2,
MatrixBase<D7>& p12,
QuaternionBase<D8>& q12)
{
MatrixSizeCheck<3, 1>::check(p1);
MatrixSizeCheck<3, 1>::check(p2);
MatrixSizeCheck<3, 1>::check(p12);
// Some useful temporaries
const auto R1 = q2R(x1.at('O')); //q1.matrix(); // make temporary
const auto& R2 = q2R(x2.at('O')); //q2.matrix(); // do not make temporary, only reference
// Jac wrt first delta
J_c_x1.emplace('P', 'P', Matrix3d::Identity() ) ;
J_c_x1.emplace('P', 'O', - R1 * skew(x2.at('P')) ) ;
J_c_x1.emplace('O', 'P', Matrix3d::Zero() ) ;
J_c_x1.emplace('O', 'O', R2.transpose() ) ;
// Jac wrt second delta
J_c_x2.emplace('P', 'P', R1 );
J_c_x2.emplace('P', 'O', Matrix3d::Zero() );
J_c_x2.emplace('O', 'P', Matrix3d::Zero() );
J_c_x2.emplace('O', 'O', Matrix3d::Identity() );
// compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the same variable
compose(x1, x2, c);
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void between(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
MatrixBase<D7>& p12, QuaternionBase<D8>& q12)
{
MatrixSizeCheck<3, 1>::check(p1);
MatrixSizeCheck<3, 1>::check(p2);
MatrixSizeCheck<3, 1>::check(p12);
p12 = q1.conjugate() * ( p2 - p1 );
q12 = q1.conjugate() * q2;
p12 = q1.conjugate() * (p2 - p1);
q12 = q1.conjugate() * q2;
}
template<typename D1, typename D2, typename D3>
inline void between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& d12)
template <typename D1, typename D2, typename D3>
inline void between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d12)
{
MatrixSizeCheck<7, 1>::check(d1);
MatrixSizeCheck<7, 1>::check(d2);
MatrixSizeCheck<7, 1>::check(d12);
Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
Map<Matrix<typename D3::Scalar, 3, 1> > p12 ( & d12(0) );
Map<Quaternion<typename D3::Scalar> > q12 ( & d12(3) );
Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
Map<const Quaternion<typename D1::Scalar>> q1(&d1(3));
Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
Map<const Quaternion<typename D2::Scalar>> q2(&d2(3));
Map<Matrix<typename D3::Scalar, 3, 1>> p12(&d12(0));
Map<Quaternion<typename D3::Scalar>> q12(&d12(3));
between(p1, q1, p2, q2, p12, q12);
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2 )
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
MatrixSizeCheck<7, 1>::check(d1);
MatrixSizeCheck<7, 1>::check(d2);
......@@ -323,28 +317,28 @@ inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
return d12;
}
template<typename Derived>
template <typename Derived>
inline Matrix<typename Derived::Scalar, 6, 1> log(const MatrixBase<Derived>& delta_in)
{
MatrixSizeCheck<7, 1>::check(delta_in);
Matrix<typename Derived::Scalar, 6, 1> ret;
Map<const Matrix<typename Derived::Scalar, 3, 1> > p_in ( & delta_in(0) );
Map<const Quaternion<typename Derived::Scalar> > q_in ( & delta_in(3) );
Map<Matrix<typename Derived::Scalar, 3, 1> > p_ret ( & ret(0) );
Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
Map<const Matrix<typename Derived::Scalar, 3, 1>> p_in(&delta_in(0));
Map<const Quaternion<typename Derived::Scalar>> q_in(&delta_in(3));
Map<Matrix<typename Derived::Scalar, 3, 1>> p_ret(&ret(0));
Map<Matrix<typename Derived::Scalar, 3, 1>> do_ret(&ret(3));
Matrix<typename Derived::Scalar, 3, 3> V_inv;
do_ret = log_q(q_in);
V_inv = jac_SO3_left_inv(do_ret);
do_ret = log_q(q_in);
V_inv = jac_SO3_left_inv(do_ret);
p_ret = V_inv * p_in;
return ret;
}
template<typename Derived>
template <typename Derived>
inline Matrix<typename Derived::Scalar, 7, 1> exp(const MatrixBase<Derived>& d_in)
{
MatrixSizeCheck<6, 1>::check(d_in);
......@@ -355,10 +349,10 @@ inline Matrix<typename Derived::Scalar, 7, 1> exp(const MatrixBase<Derived>& d_i
V = jac_SO3_left(d_in.template tail<3>());
Map<const Matrix<typename Derived::Scalar, 3, 1> > p_in ( & d_in(0) );
Map<const Matrix<typename Derived::Scalar, 3, 1> > o_in ( & d_in(3) );
Map<Matrix<typename Derived::Scalar, 3, 1> > p ( & ret(0) );
Map<Quaternion<typename Derived::Scalar> > q ( & ret(3) );
Map<const Matrix<typename Derived::Scalar, 3, 1>> p_in(&d_in(0));
Map<const Matrix<typename Derived::Scalar, 3, 1>> o_in(&d_in(3));
Map<Matrix<typename Derived::Scalar, 3, 1>> p(&ret(0));
Map<Quaternion<typename Derived::Scalar>> q(&ret(3));
p = V * p_in;
q = exp_q(o_in);
......@@ -371,20 +365,23 @@ inline VectorComposite exp(const VectorComposite& tau)
const auto& p = tau.at('P');
const auto& theta = tau.at('O');
Matrix3d V = jac_SO3_left(theta); // see [1] eqs. (174) and (145)
Matrix3d V = jac_SO3_left(theta); // see [1] eqs. (174) and (145)
VectorComposite res;
VectorComposite res;
res['P'] = V * p ;
res['O'] = exp_q(theta).coeffs() ;
res['P'] = V * p;
res['O'] = exp_q(theta).coeffs();
return res;
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void plus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q)
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void plus(const MatrixBase<D1>& p1,
const QuaternionBase<D2>& q1,
const MatrixBase<D4>& p2,
const MatrixBase<D5>& o2,
MatrixBase<D7>& plus_p,
QuaternionBase<D8>& plus_q)
{
MatrixSizeCheck<3, 1>::check(p1);
MatrixSizeCheck<3, 1>::check(p2);
......@@ -395,38 +392,38 @@ inline void plus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
plus_q = q1 * exp_q(o2);
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
MatrixBase<D7>& plus_p, MatrixBase<D8>& plus_q)
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void plus(const MatrixBase<D1>& p1,
const MatrixBase<D2>& q1,
const MatrixBase<D4>& p2,
const MatrixBase<D5>& o2,
MatrixBase<D7>& plus_p,
MatrixBase<D8>& plus_q)
{
MatrixSizeCheck<4, 1>::check(q1);
MatrixSizeCheck<4, 1>::check(plus_q);
Map<const Quaterniond> q1m ( & q1 (0) );
Map<Quaterniond> plus_qm ( & plus_q(0) );
Map<const Quaterniond> q1m(&q1(0));
Map<Quaterniond> plus_qm(&plus_q(0));
plus(p1, q1m, p2, o2, plus_p, plus_qm);
}
template<typename D1, typename D2, typename D3>
inline void plus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& d_plus)
template <typename D1, typename D2, typename D3>
inline void plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d_plus)
{
Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
Map<const Matrix<typename D2::Scalar, 3, 1> > o2 ( & d2(3) );
Map<Matrix<typename D3::Scalar, 3, 1> > p_p ( & d_plus(0) );
Map<Quaternion<typename D3::Scalar> > q_p ( & d_plus(3) );
Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
Map<const Quaternion<typename D1::Scalar>> q1(&d1(3));
Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
Map<const Matrix<typename D2::Scalar, 3, 1>> o2(&d2(3));
Map<Matrix<typename D3::Scalar, 3, 1>> p_p(&d_plus(0));
Map<Quaternion<typename D3::Scalar>> q_p(&d_plus(3));
plus(p1, q1, p2, o2, p_p, q_p);
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2)
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
Matrix<typename D1::Scalar, 7, 1> d_plus;
plus(d1, d2, d_plus);
......@@ -450,55 +447,60 @@ inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau
return res;
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void minus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
MatrixBase<D7>& pd, MatrixBase<D8>& od )
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void minus(const MatrixBase<D1>& p1,
const QuaternionBase<D2>& q1,
const MatrixBase<D4>& p2,
const QuaternionBase<D5>& q2,
MatrixBase<D7>& pd,
MatrixBase<D8>& od)
{
pd = p2 - p1;
od = log_q(q1.conjugate() * q2);
}
template<typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
inline void minus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
MatrixBase<D6>& pd, MatrixBase<D7>& od,
MatrixBase<D8>& J_do_q1, MatrixBase<D9>& J_do_q2)
template <typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
inline void minus(const MatrixBase<D1>& p1,
const QuaternionBase<D2>& q1,
const MatrixBase<D4>& p2,
const QuaternionBase<D5>& q2,
MatrixBase<D6>& pd,
MatrixBase<D7>& od,
MatrixBase<D8>& J_do_q1,
MatrixBase<D9>& J_do_q2)
{
minus(p1, q1, p2, q2, pd, od);
J_do_q1 = - jac_SO3_left_inv(od);
J_do_q2 = jac_SO3_right_inv(od);
J_do_q1 = -jac_SO3_left_inv(od);
J_do_q2 = jac_SO3_right_inv(od);
}
template<typename D1, typename D2, typename D3>
inline void minus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& err)
template <typename D1, typename D2, typename D3>
inline void minus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& err)
{
Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
Map<Matrix<typename D3::Scalar, 3, 1> > pd ( & err(0) );
Map<Matrix<typename D3::Scalar, 3, 1> > od ( & err(3) );
Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
Map<const Quaternion<typename D1::Scalar>> q1(&d1(3));
Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
Map<const Quaternion<typename D2::Scalar>> q2(&d2(3));
Map<Matrix<typename D3::Scalar, 3, 1>> pd(&err(0));
Map<Matrix<typename D3::Scalar, 3, 1>> od(&err(3));
minus(p1, q1, p2, q2, pd, od);
}
template<typename D1, typename D2, typename D3, typename D4, typename D5>
template <typename D1, typename D2, typename D3, typename D4, typename D5>
inline void minus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& dif,
MatrixBase<D4>& J_diff_d1,
MatrixBase<D5>& J_diff_d2)
MatrixBase<D3>& dif,
MatrixBase<D4>& J_diff_d1,
MatrixBase<D5>& J_diff_d2)
{
Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
Map<Matrix<typename D3::Scalar, 3, 1> > pd ( & dif(0) );
Map<Matrix<typename D3::Scalar, 3, 1> > od ( & dif(3) );
Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&d1(0));
Map<const Quaternion<typename D1::Scalar>> q1(&d1(3));
Map<const Matrix<typename D2::Scalar, 3, 1>> p2(&d2(0));
Map<const Quaternion<typename D2::Scalar>> q2(&d2(3));
Map<Matrix<typename D3::Scalar, 3, 1>> pd(&dif(0));
Map<Matrix<typename D3::Scalar, 3, 1>> od(&dif(3));
Matrix<typename D4::Scalar, 3, 3> J_do_q1, J_do_q2;
......@@ -512,27 +514,26 @@ inline void minus(const MatrixBase<D1>& d1,
* J_do_q1 = - J_l_inv(theta)
* J_do_q2 = J_r_inv(theta)
*/
J_diff_d1 = - Matrix<typename D4::Scalar, 6, 6>::Identity();// d(p2 - p1) / d(p1) = - Identity
J_diff_d1.block(3,3,3,3) = J_do_q1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
J_diff_d1 = -Matrix<typename D4::Scalar, 6, 6>::Identity(); // d(p2 - p1) / d(p1) = - Identity
J_diff_d1.block(3, 3, 3, 3) = J_do_q1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
J_diff_d2.setIdentity(6,6); // d(R1.tr*R2) / d(R2) = Identity
J_diff_d2.block(3,3,3,3) = J_do_q2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
J_diff_d2.setIdentity(6, 6); // d(R1.tr*R2) / d(R2) = Identity
J_diff_d2.block(3, 3, 3, 3) = J_do_q2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2)
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
Matrix<typename D1::Scalar, 6, 1> ret;
minus(d1, d2, ret);
return ret;
}
template<typename D1, typename D2, typename D3>
inline void interpolate(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
template <typename D1, typename D2, typename D3>
inline void interpolate(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
const typename D1::Scalar& t,
MatrixBase<D3>& ret)
MatrixBase<D3>& ret)
{
Matrix<typename D1::Scalar, 7, 1> dd = between(d1, d2);
......@@ -541,90 +542,87 @@ inline void interpolate(const MatrixBase<D1>& d1,
ret = compose(d1, exp(tau));
}
template<typename D1, typename D2>
inline void toSE3(const MatrixBase<D1>& pose,
MatrixBase<D2>& SE3)
template <typename D1, typename D2>
inline void toSE3(const MatrixBase<D1>& pose, MatrixBase<D2>& SE3)
{
MatrixSizeCheck<4,4>::check(SE3);
MatrixSizeCheck<4, 4>::check(SE3);
typedef typename D1::Scalar T;
SE3.template block<3,1>(0,3) = pose.template head<3>();
SE3.template block<3,3>(0,0) = q2R(pose.template tail<4>());
SE3.template block<1,3>(3,0).setZero();
SE3(3,3) = (T)1.0;
SE3.template block<3, 1>(0, 3) = pose.template head<3>();
SE3.template block<3, 3>(0, 0) = q2R(pose.template tail<4>());
SE3.template block<1, 3>(3, 0).setZero();
SE3(3, 3) = (T)1.0;
}
template<typename D1, typename D2>
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 6, 6> Q_helper(const MatrixBase<D1>& v, const MatrixBase<D1>& w)
{
typedef typename D1::Scalar T;
Matrix<T, 3, 3> V = skew(v);
Matrix<T, 3, 3> W = skew(w);
Matrix<T, 3, 3> VW = V * W;
Matrix<T, 3, 3> WV = VW.transpose(); // Note on this change wrt. Barfoot: it happens that V*W = (W*V).transpose() !!!
Matrix<T, 3, 3> V = skew(v);
Matrix<T, 3, 3> W = skew(w);
Matrix<T, 3, 3> VW = V * W;
Matrix<T, 3, 3> WV =
VW.transpose(); // Note on this change wrt. Barfoot: it happens that V*W = (W*V).transpose() !!!
Matrix<T, 3, 3> WVW = WV * W;
Matrix<T, 3, 3> VWW = VW * W;
T th_2 = w.squaredNorm();
T th_2 = w.squaredNorm();
T A(T(0.5)), B, C, D;
// Small angle approximation
if (th_2 <= T(1e-8))
{
B = double(1./6.) + double(1./120.) * th_2;
C = -double(1./24.) + double(1./720.) * th_2;
D = -double(1./60.);
B = double(1. / 6.) + double(1. / 120.) * th_2;
C = -double(1. / 24.) + double(1. / 720.) * th_2;
D = -double(1. / 60.);
}
else
{
T th = sqrt(th_2);
T th_3 = th_2*th;
T th_4 = th_2*th_2;
T th_5 = th_3*th_2;
T sin_th = sin(th);
T cos_th = cos(th);
B = (th-sin_th)/th_3;
C = (T(1.0) - th_2/T(2.0) - cos_th)/th_4;
D = (th - sin_th - th_3/T(6.0))/th_5;
T th = sqrt(th_2);
T th_3 = th_2 * th;
T th_4 = th_2 * th_2;
T th_5 = th_3 * th_2;
T sin_th = sin(th);
T cos_th = cos(th);
B = (th - sin_th) / th_3;
C = (T(1.0) - th_2 / T(2.0) - cos_th) / th_4;
D = (th - sin_th - th_3 / T(6.0)) / th_5;
}
Matrix<T, 3, 3> Q;
Q.noalias() =
+ A * V
+ B * (WV + VW + WVW)
- C * (VWW - VWW.transpose() - double(3) * WVW) // Note on this change wrt. Barfoot: it happens that V*W*W = -(W*W*V).transpose() !!!
- D * WVW * W; // Note on this change wrt. Barfoot: it happens that W*V*W*W = W*W*V*W !!!
+A * V + B * (WV + VW + WVW) -
C * (VWW - VWW.transpose() -
double(3) * WVW) // Note on this change wrt. Barfoot: it happens that V*W*W = -(W*W*V).transpose() !!!
- D * WVW * W; // Note on this change wrt. Barfoot: it happens that W*V*W*W = W*W*V*W !!!
return Q;
}
template<typename D1>
template <typename D1>
inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_left(const MatrixBase<D1>& tangent)
{
typedef typename D1::Scalar T;
Map<Matrix<T, 3, 1>> v(tangent.data() + 0); // linear
Map<Matrix<T, 3, 1>> w(tangent.data() + 3); // angular
Map<Matrix<T, 3, 1>> v(tangent.data() + 0); // linear
Map<Matrix<T, 3, 1>> w(tangent.data() + 3); // angular
Matrix<T, 3, 3> Jl(jac_SO3_left(w));
Matrix<T, 3, 3> Q = Q_helper(v,w);
Matrix<T, 3, 3> Q = Q_helper(v, w);
Matrix<T, 6, 6> Jl_SE3;
Jl_SE3.topLeftCorner(3,3) = Jl;
Jl_SE3.bottomRightCorner(3,3) = Jl;
Jl_SE3.topRightCorner(3,3) = Q;
Jl_SE3.bottomLeftCorner(3,3) .setZero();
Jl_SE3.topLeftCorner(3, 3) = Jl;
Jl_SE3.bottomRightCorner(3, 3) = Jl;
Jl_SE3.topRightCorner(3, 3) = Q;
Jl_SE3.bottomLeftCorner(3, 3).setZero();
}
template<typename D1>
template <typename D1>
inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_right(const MatrixBase<D1>& tangent)
{
return jac_SE3_left(-tangent);
}
} // namespace three_d
} // namespace wolf
#endif /* SE3_H_ */
} // namespace SE3
} // namespace wolf
include/core/math/covariance.h
Edit View file @ 44e08b4a
//--------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)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// 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
// 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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file covariance.h
*
* Created on: Feb 26, 2020
* \author: jsola
*/
#ifndef MATH_COVARIANCE_H_
#define MATH_COVARIANCE_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include <Eigen/Dense>
namespace wolf{
namespace wolf
{
template <typename T, int N, int RC>
inline bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M,
const T eps = wolf::Constants::EPS)
inline bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M, const T eps = wolf::Constants::EPS)
{
return M.isApprox(M.transpose(), eps);
if (M.cols() != M.rows()) return false;
return M.isApprox(M.transpose(), eps);
}
template <typename T, int N, int RC>
inline bool isPositiveSemiDefinite(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
{
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M, Eigen::EigenvaluesOnly);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M, Eigen::EigenvaluesOnly);
if (eigensolver.info() == Eigen::Success)
{
// All eigenvalues must be >= 0:
return (eigensolver.eigenvalues().array() >= eps).all();
}
else
std::cout << "eigen decomposition failed" << std::endl;
if (eigensolver.info() == Eigen::Success)
{
// All eigenvalues must be >= 0:
return (eigensolver.eigenvalues().array() >= eps).all();
}
else
std::cout << "eigen decomposition failed" << std::endl;
return false;
return false;
}
template <typename T, int N, int RC>
inline bool isCovariance(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
{
return isSymmetric(M) && isPositiveSemiDefinite(M, eps);
return isSymmetric(M) && isPositiveSemiDefinite(M, eps);
}
#define WOLF_ASSERT_COVARIANCE_MATRIX(x) \
assert(isCovariance(x, Constants::EPS_SMALL) && "Not a covariance");
#define WOLF_ASSERT_COVARIANCE_MATRIX(x) assert(isCovariance(x, Constants::EPS_SMALL) && "Not a covariance");
#define WOLF_ASSERT_INFORMATION_MATRIX(x) \
assert(isCovariance(x, double(0.0)) && "Not an information matrix");
#define WOLF_ASSERT_INFORMATION_MATRIX(x) assert(isCovariance(x, double(0.0)) && "Not an information matrix");
template <typename T, int N, int RC>
inline bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS)
inline bool makePosDef(Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
{
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T,N,N,RC> > eigensolver(M);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M);
if (eigensolver.info() == Eigen::Success)
{
......@@ -79,17 +66,16 @@ inline bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS
double epsilon = eps;
while ((eigensolver.eigenvalues().array() < eps).any())
{
//std::cout << "----- any negative eigenvalue or too close to zero\n";
//std::cout << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
//std::cout << "previous determinant: " << M.determinant() << std::endl;
M = eigensolver.eigenvectors() *
eigensolver.eigenvalues().cwiseMax(epsilon).asDiagonal() *
// std::cout << "----- any negative eigenvalue or too close to zero\n";
// std::cout << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
// std::cout << "previous determinant: " << M.determinant() << std::endl;
M = eigensolver.eigenvectors() * eigensolver.eigenvalues().cwiseMax(epsilon).asDiagonal() *
eigensolver.eigenvectors().transpose();
eigensolver.compute(M);
//std::cout << "epsilon used: " << epsilon << std::endl;
//std::cout << "posterior eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
//std::cout << "posterior determinant: " << M.determinant() << std::endl;
epsilon *=10;
// std::cout << "epsilon used: " << epsilon << std::endl;
// std::cout << "posterior eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
// std::cout << "posterior determinant: " << M.determinant() << std::endl;
epsilon *= 10;
}
WOLF_ASSERT_COVARIANCE_MATRIX(M);
......@@ -101,31 +87,25 @@ inline bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS
return false;
}
inline Eigen::MatrixXd computeSqrtUpper(const Eigen::MatrixXd & _info)
inline Eigen::MatrixXd computeSqrtUpper(const Eigen::MatrixXd& _info)
{
// impose symmetry
Eigen::MatrixXd info = _info.selfadjointView<Eigen::Upper>();
// Normal Cholesky factorization
Eigen::LLT<Eigen::MatrixXd> llt_of_info(info);
Eigen::MatrixXd R = llt_of_info.matrixU();
Eigen::MatrixXd R = llt_of_info.matrixU();
// Good factorization
if (info.isApprox(R.transpose() * R, Constants::EPS))
return R;
if (info.isApprox(R.transpose() * R, Constants::EPS)) return R;
// Not good factorization: SelfAdjointEigenSolver
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(info);
Eigen::VectorXd eval = es.eigenvalues().real().cwiseMax(Constants::EPS);
Eigen::VectorXd eval = es.eigenvalues().real().cwiseMax(Constants::EPS);
R = eval.cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose();
return R;
}
}
#endif /* MATH_COVARIANCE_H_ */
} // namespace wolf