Skip to content
Snippets Groups Projects

Compare revisions

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

Source

Select target project
No results found

Target

Select target project
  • mobile_robotics/wolf_projects/wolf_lib/wolf
1 result
Show changes
Showing
with 5103 additions and 6206 deletions
source diff could not be displayed: it is too large. Options to address this: view the blob.
//--------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 FACTOR_BASE_H_
#define FACTOR_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 FeatureBase;
//class TrajectoryIter;
//class TrajectoryRevIter;
}
} // namespace wolf
//Wolf includes
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
//std includes
namespace wolf {
// std includes
namespace wolf
{
/** \brief Enumeration of factor status
*
* You may add items to this list as needed. Be concise with names, and document your entries.
*/
typedef enum
{
FAC_INACTIVE = 0, ///< Inactive factor, it is not included in the SLAM problem.
FAC_ACTIVE = 1 ///< Normal active factor, playing its role in the solution.
FAC_INACTIVE = 0, ///< Inactive factor, it is not included in the SLAM problem.
FAC_ACTIVE = 1 ///< Normal active factor, playing its role in the solution.
} FactorStatus;
/** \brief Enumeration of factor topologies
......@@ -53,12 +48,12 @@ typedef enum
*/
typedef enum
{
TOP_ABS, ///< absolute factor
TOP_MOTION, ///<motion factor, e.g. odometry, IMU
TOP_LOOP, ///<loop closure factor
TOP_LMK, ///<landmark observation factor
TOP_GEOM, ///<some geometric relation, e.g. distance
TOP_OTHER ///<other topologies
TOP_ABS, ///< absolute factor
TOP_MOTION, ///< motion factor, e.g. odometry, IMU
TOP_LOOP, ///< loop closure factor
TOP_LMK, ///< landmark observation factor
TOP_GEOM, ///< some geometric relation, e.g. distance
TOP_OTHER ///< other topologies
} FactorTopology;
/** \brief Enumeration of jacobian computation method
......@@ -67,66 +62,86 @@ typedef enum
*/
typedef enum
{
JAC_AUTO = 1, ///< Auto differentiation (AutoDiffCostFunctionWrapper or ceres::NumericDiffCostFunction).
JAC_NUMERIC, ///< Numeric differentiation (ceres::NumericDiffCostFunction).
JAC_ANALYTIC ///< Analytic jacobians.
JAC_AUTO = 1, ///< Auto differentiation (AutoDiffCostFunctionWrapper or ceres::NumericDiffCostFunction).
JAC_NUMERIC, ///< Numeric differentiation (ceres::NumericDiffCostFunction).
JAC_ANALYTIC ///< Analytic jacobians.
} JacobianMethod;
//class FactorBase
// class FactorBase
class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBase>
{
friend FeatureBase;
private:
FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
ProcessorBaseWPtr processor_ptr_; ///< Processor pointer
static unsigned int factor_id_count_;
FrameBaseWPtrList frame_other_list_; ///< FrameBase pointer list
CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list
FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list
LandmarkBaseWPtrList landmark_other_list_; ///< LandmarkBase pointer list
protected:
unsigned int factor_id_;
FactorTopology topology_; ///< topology of factor
FactorStatus status_; ///< status of factor
bool apply_loss_function_; ///< flag for applying loss function to this factor
Eigen::VectorXd measurement_; ///< the measurement vector
Eigen::MatrixXd measurement_sqrt_information_upper_; ///< the squared root information matrix
void setProblem(ProblemPtr) override final;
public:
FactorBase(const std::string& _tp,
const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
const LandmarkBasePtrList& _landmark_other_list,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
~FactorBase() override = default;
virtual void remove(bool viral_remove_empty_parent=false);
unsigned int id() const;
/** \brief get the main topological characteristic
**/
virtual FactorTopology getTopology() const final
{
return topology_;
}
std::string getTopologyString() const
friend FeatureBase;
private:
FeatureBaseWPtr feature_ptr_; ///< Feature from which the factor was created (optional)
ProcessorBaseWPtr processor_ptr_; ///< Processor pointer
static unsigned int factor_id_count_;
NodeStateBlocksWPtrList node_state_blocks_list_; ///< NodeStateBlocks pointer list
FrameBaseWPtrList frame_list_; ///< FrameBase pointer list
CaptureBaseWPtrList capture_list_; ///< CaptureBase pointer list
LandmarkBaseWPtrList landmark_list_; ///< LandmarkBase pointer list
SensorBaseWPtrList sensor_list_; ///< SensorBase pointer list
protected:
unsigned int factor_id_;
FactorTopology topology_; ///< topology of factor
JacobianMethod jacobian_method_; ///< jacobian method
FactorStatus status_; ///< status of factor
bool apply_loss_function_; ///< flag for applying loss function to this factor
Eigen::VectorXd measurement_; ///< the measurement vector
Eigen::MatrixXd measurement_sqrt_information_upper_; ///< the squared root information matrix
std::vector<unsigned int> state_block_sizes_; ///< vector containing sizes of the state blocks involved
std::vector<StateBlockPtr> state_block_vector_; ///< vector containing the state blocks involved
void setProblem(ProblemPtr) override final;
public:
/** \brief constructor from measurement and noise square root information upper matrix (cholesky)
**/
FactorBase(const std::string& _type,
const FactorTopology& _top,
const JacobianMethod& _jac_method,
const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtrList& _node_state_block_list,
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
~FactorBase() override = default;
virtual void remove(bool viral_remove_parent_without_children = false);
bool hasChildren() const override
{
return false;
};
unsigned int id() const;
/** \brief get the main topological characteristic
**/
virtual FactorTopology getTopology() const final
{
return topology_;
}
/** \brief set the main topological characteristic
**/
virtual void getTopology(FactorTopology _top) final
{
topology_ = _top;
}
/** \brief get the main topological characteristic as string
**/
std::string getTopologyString() const
{
switch (topology_)
{
switch (topology_)
{
case TOP_GEOM:
return "GEOM";
break;
......@@ -144,150 +159,133 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
break;
default:
return "UNDEFINED";
}
}
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
virtual bool evaluate(double const* const* _parameters, double* _residuals, double** _jacobians) const = 0;
/** Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr and the residual vector
**/
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& _residual, std::vector<Eigen::MatrixXd>& _jacobians) const = 0;
/** \brief Returns the jacobians computation method
**/
virtual JacobianMethod getJacobianMethod() const = 0;
/** \brief Returns a vector of pointers to the states in which this factor depends
**/
virtual std::vector<StateBlockConstPtr> getStateBlockPtrVector() const = 0;
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() = 0;
/** \brief Returns a vector of the states sizes
**/
virtual std::vector<unsigned int> getStateSizes() const = 0;
/** \brief Returns a reference to the measurement
**/
virtual const Eigen::VectorXd& getMeasurement() const;
/** \brief Returns a reference to the measurement square root information
**/
virtual const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const;
/** \brief Returns a pointer to the feature constrained from
**/
FeatureBaseConstPtr getFeature() const;
FeatureBasePtr getFeature();
/** \brief Returns a pointer to its capture
**/
CaptureBaseConstPtr getCapture() const;
CaptureBasePtr getCapture();
/** \brief Returns a pointer to its frame
**/
FrameBaseConstPtr getFrame() const;
FrameBasePtr getFrame();
/** \brief Returns a pointer to its capture's sensor
**/
SensorBaseConstPtr getSensor() const;
SensorBasePtr getSensor();
/** \brief Returns the factor residual size
**/
virtual unsigned int getSize() const = 0;
/** \brief Gets the status
*/
FactorStatus getStatus() const;
/** \brief Sets the status
*/
void setStatus(FactorStatus _status);
/** \brief Gets the apply loss function flag
*/
bool getApplyLossFunction() const;
/** \brief Returns a pointer to the first frame constrained to
**/
FrameBaseConstPtr getFrameOther() const;
FrameBasePtr getFrameOther();
/** \brief Returns a pointer to the first capture constrained to
**/
CaptureBaseConstPtr getCaptureOther() const;
CaptureBasePtr getCaptureOther();
/** \brief Returns a pointer to the first feature constrained to
**/
FeatureBaseConstPtr getFeatureOther() const;
FeatureBasePtr getFeatureOther();
/** \brief Returns a pointer to the first landmark constrained to
**/
LandmarkBaseConstPtr getLandmarkOther() const;
LandmarkBasePtr getLandmarkOther();
// get pointer lists to other nodes
FrameBaseWPtrList getFrameOtherList() { return frame_other_list_; }
CaptureBaseWPtrList getCaptureOtherList() { return capture_other_list_; }
FeatureBaseWPtrList getFeatureOtherList() { return feature_other_list_; }
LandmarkBaseWPtrList getLandmarkOtherList() { return landmark_other_list_; }
FrameBaseConstWPtrList getFrameOtherList() const;
CaptureBaseConstWPtrList getCaptureOtherList() const;
FeatureBaseConstWPtrList getFeatureOtherList() const;
LandmarkBaseConstWPtrList getLandmarkOtherList() const;
bool hasFrameOther(const FrameBaseConstPtr& _frm_other) const;
bool hasCaptureOther(const CaptureBaseConstPtr& _cap_other) const;
bool hasFeatureOther(const FeatureBaseConstPtr& _ftr_other) const;
bool hasLandmarkOther(const LandmarkBaseConstPtr& _lmk_other) const;
/**
* @brief getProcessor
* @return
*/
ProcessorBaseConstPtr getProcessor() const;
ProcessorBasePtr getProcessor();
void link(FeatureBasePtr ftr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(FeatureBasePtr _oth_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, FactorBaseConstPtr _fac_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 setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
/**
* @brief setProcessor
* @param _processor_ptr
*/
void setProcessor(const ProcessorBasePtr& _processor_ptr);
}
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
virtual bool evaluate(double const* const* _parameters, double* _residuals, double** _jacobians) const = 0;
/** Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in
*_states_ptr and the residual vector
**/
virtual bool evaluate(const std::vector<const double*>& _states_ptr,
Eigen::VectorXd& _residual,
std::vector<Eigen::MatrixXd>& _jacobians) const = 0;
/** \brief Evaluate the factor residual with the current state block values
**/
virtual Eigen::VectorXd residual() const;
/** \brief Evaluate the cost of the factor with the current state block values
**/
virtual double cost() const;
/** \brief Returns the jacobians computation method
**/
virtual JacobianMethod getJacobianMethod() const final
{
return jacobian_method_;
}
/** \brief Returns a vector of pointers to the states in which this factor depends
**/
virtual std::vector<StateBlockConstPtr> getStateBlockPtrVector() const;
virtual std::vector<StateBlockPtr> getStateBlockPtrVector();
/** \brief Returns a vector of the states sizes
**/
virtual std::vector<unsigned int> getStateSizes() const;
/** \brief Returns a reference to the measurement
**/
virtual const Eigen::VectorXd& getMeasurement() const;
/** \brief Returns a reference to the measurement square root information
**/
virtual const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const;
/** \brief Returns a pointer to the feature constrained from
**/
FeatureBaseConstPtr getFeature() const;
FeatureBasePtr getFeature();
/** \brief Returns a pointer to its capture
**/
CaptureBaseConstPtr getCapture() const;
CaptureBasePtr getCapture();
/** \brief Returns a pointer to its frame
**/
FrameBaseConstPtr getFrame() const;
FrameBasePtr getFrame();
/** \brief getProcessor
*/
ProcessorBaseConstPtr getProcessor() const;
ProcessorBasePtr getProcessor();
/** \brief Returns the factor residual size
**/
virtual unsigned int getSize() const = 0;
/** \brief Gets the status
*/
FactorStatus getStatus() const;
/** \brief Sets the status
*/
void setStatus(FactorStatus _status);
/** \brief Gets the apply loss function flag
*/
bool getApplyLossFunction() const;
// get pointer lists to factored nodes
NodeStateBlocksWPtrList getNodesFactored();
FrameBaseWPtrList getFramesFactored();
CaptureBaseWPtrList getCapturesFactored();
LandmarkBaseWPtrList getLandmarksFactored();
SensorBaseWPtrList getSensorsFactored();
NodeStateBlocksConstWPtrList getNodesFactored() const;
FrameBaseConstWPtrList getFramesFactored() const;
CaptureBaseConstWPtrList getCapturesFactored() const;
LandmarkBaseConstWPtrList getLandmarksFactored() const;
SensorBaseConstWPtrList getSensorsFactored() const;
bool hasNode(const NodeStateBlocksConstPtr& _nsb) const;
bool hasFrame(const FrameBaseConstPtr& _frm) const;
bool hasCapture(const CaptureBaseConstPtr& _cap) const;
bool hasLandmark(const LandmarkBaseConstPtr& _lmk) const;
bool hasSensor(const SensorBaseConstPtr& _sen) const;
void link(FeatureBasePtr ftr);
template <typename classType, typename... T>
static std::shared_ptr<classType> emplace(FeatureBasePtr _oth_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 setFeature(const FeatureBasePtr _ft_ptr);
void setProcessor(const ProcessorBasePtr& _processor_ptr);
};
}
} // namespace wolf
// IMPLEMENTATION //
......@@ -297,9 +295,9 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
#include "core/sensor/sensor_base.h"
#include "core/landmark/landmark_base.h"
namespace wolf{
template<typename classType, typename... T>
namespace wolf
{
template <typename classType, typename... T>
std::shared_ptr<classType> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... all)
{
std::shared_ptr<classType> ctr = std::make_shared<classType>(std::forward<T>(all)...);
......@@ -312,103 +310,71 @@ inline unsigned int FactorBase::id() const
return factor_id_;
}
inline FeatureBaseConstPtr FactorBase::getFeature() const
inline NodeStateBlocksWPtrList FactorBase::getNodesFactored()
{
return feature_ptr_.lock();
return node_state_blocks_list_;
}
inline FeatureBasePtr FactorBase::getFeature()
inline FrameBaseWPtrList FactorBase::getFramesFactored()
{
return feature_ptr_.lock();
}
inline FactorStatus FactorBase::getStatus() const
{
return status_;
}
inline bool FactorBase::getApplyLossFunction() const
{
return apply_loss_function_;
return frame_list_;
}
inline FrameBaseConstPtr FactorBase::getFrameOther() const
inline CaptureBaseWPtrList FactorBase::getCapturesFactored()
{
if (frame_other_list_.empty()) return nullptr;
if (frame_other_list_.front().expired()) return nullptr;
return frame_other_list_.front().lock();
return capture_list_;
}
inline FrameBasePtr FactorBase::getFrameOther()
inline LandmarkBaseWPtrList FactorBase::getLandmarksFactored()
{
if (frame_other_list_.empty()) return nullptr;
if (frame_other_list_.front().expired()) return nullptr;
return frame_other_list_.front().lock();
return landmark_list_;
}
inline CaptureBaseConstPtr FactorBase::getCaptureOther() const
inline SensorBaseWPtrList FactorBase::getSensorsFactored()
{
if (capture_other_list_.empty()) return nullptr;
if (capture_other_list_.front().expired()) return nullptr;
return capture_other_list_.front().lock();
return sensor_list_;
}
inline CaptureBasePtr FactorBase::getCaptureOther()
inline FactorStatus FactorBase::getStatus() const
{
if (capture_other_list_.empty()) return nullptr;
if (capture_other_list_.front().expired()) return nullptr;
return capture_other_list_.front().lock();
return status_;
}
inline FeatureBaseConstPtr FactorBase::getFeatureOther() const
inline bool FactorBase::getApplyLossFunction() const
{
if (feature_other_list_.empty()) return nullptr;
if (feature_other_list_.front().expired()) return nullptr;
return feature_other_list_.front().lock();
return apply_loss_function_;
}
inline FeatureBasePtr FactorBase::getFeatureOther()
inline FeatureBaseConstPtr FactorBase::getFeature() const
{
if (feature_other_list_.empty()) return nullptr;
if (feature_other_list_.front().expired()) return nullptr;
return feature_other_list_.front().lock();
if (feature_ptr_.expired()) return nullptr;
return feature_ptr_.lock();
}
inline LandmarkBaseConstPtr FactorBase::getLandmarkOther() const
inline FeatureBasePtr FactorBase::getFeature()
{
if (landmark_other_list_.empty()) return nullptr;
if (landmark_other_list_.front().expired()) return nullptr;
return landmark_other_list_.front().lock();
if (feature_ptr_.expired()) return nullptr;
return feature_ptr_.lock();
}
inline LandmarkBasePtr FactorBase::getLandmarkOther()
inline ProcessorBaseConstPtr FactorBase::getProcessor() const
{
if (landmark_other_list_.empty()) return nullptr;
if (landmark_other_list_.front().expired()) return nullptr;
return landmark_other_list_.front().lock();
return processor_ptr_.lock();
}
inline ProcessorBaseConstPtr FactorBase::getProcessor() const
inline ProcessorBasePtr FactorBase::getProcessor()
{
return processor_ptr_.lock();
return processor_ptr_.lock();
}
inline ProcessorBasePtr FactorBase::getProcessor()
inline void FactorBase::setFeature(const FeatureBasePtr _ft_ptr)
{
return processor_ptr_.lock();
feature_ptr_ = _ft_ptr;
}
inline void FactorBase::setProcessor(const ProcessorBasePtr& _processor_ptr)
{
processor_ptr_ = _processor_ptr;
processor_ptr_ = _processor_ptr;
}
inline const Eigen::VectorXd& FactorBase::getMeasurement() const
......@@ -421,37 +387,63 @@ inline const Eigen::MatrixXd& FactorBase::getMeasurementSquareRootInformationUpp
return measurement_sqrt_information_upper_;
}
inline FrameBaseConstWPtrList FactorBase::getFrameOtherList() const
inline NodeStateBlocksConstWPtrList FactorBase::getNodesFactored() const
{
NodeStateBlocksConstWPtrList set_const;
for (auto&& obj_ptr : node_state_blocks_list_) set_const.push_back(obj_ptr);
return set_const;
}
inline FrameBaseConstWPtrList FactorBase::getFramesFactored() const
{
FrameBaseConstWPtrList list_const;
for (auto&& obj_ptr : frame_other_list_)
list_const.push_back(obj_ptr);
for (auto&& obj_ptr : frame_list_) list_const.push_back(obj_ptr);
return list_const;
}
inline CaptureBaseConstWPtrList FactorBase::getCaptureOtherList() const
inline CaptureBaseConstWPtrList FactorBase::getCapturesFactored() const
{
CaptureBaseConstWPtrList list_const;
for (auto&& obj_ptr : capture_other_list_)
list_const.push_back(obj_ptr);
for (auto&& obj_ptr : capture_list_) list_const.push_back(obj_ptr);
return list_const;
}
inline FeatureBaseConstWPtrList FactorBase::getFeatureOtherList() const
inline LandmarkBaseConstWPtrList FactorBase::getLandmarksFactored() const
{
FeatureBaseConstWPtrList list_const;
for (auto&& obj_ptr : feature_other_list_)
list_const.push_back(obj_ptr);
LandmarkBaseConstWPtrList list_const;
for (auto&& obj_ptr : landmark_list_) list_const.push_back(obj_ptr);
return list_const;
}
inline LandmarkBaseConstWPtrList FactorBase::getLandmarkOtherList() const
inline SensorBaseConstWPtrList FactorBase::getSensorsFactored() const
{
LandmarkBaseConstWPtrList list_const;
for (auto&& obj_ptr : landmark_other_list_)
list_const.push_back(obj_ptr);
SensorBaseConstWPtrList list_const;
for (auto&& obj_ptr : sensor_list_) list_const.push_back(obj_ptr);
return list_const;
}
} // namespace wolf
#endif
inline std::vector<StateBlockConstPtr> FactorBase::getStateBlockPtrVector() const
{
std::vector<StateBlockConstPtr> vec_const;
for (auto&& obj_ptr : state_block_vector_) vec_const.push_back(obj_ptr);
return vec_const;
}
inline std::vector<StateBlockPtr> FactorBase::getStateBlockPtrVector()
{
return state_block_vector_;
}
inline std::vector<unsigned int> FactorBase::getStateSizes() const
{
std::vector<unsigned int> sizes;
for (auto&& obj_ptr : state_block_vector_) sizes.push_back(obj_ptr->getSize());
return sizes;
}
inline double FactorBase::cost() const
{
return residual().squaredNorm();
}
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// 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 factor_block_absolute.h
*
* Created on: Dec 15, 2017
* \author: AtDinesh
*/
#ifndef FACTOR_BLOCK_ABSOLUTE_H_
#define FACTOR_BLOCK_ABSOLUTE_H_
//Wolf includes
// 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/factor/factor_analytic.h"
#include "core/factor/factor_autodiff.h"
#include "core/common/node_state_blocks.h"
#include "core/frame/frame_base.h"
#include "core/capture/capture_base.h"
#include "core/landmark/landmark_base.h"
#include "core/sensor/sensor_base.h"
#include "core/state_block/state_angle.h"
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorBlockAbsolute);
//class
// class
class FactorBlockAbsolute : public FactorAnalytic
{
private:
SizeEigen sb_size_; // the size of the state block
SizeEigen sb_constrained_start_; // the index of the first state element that is constrained
SizeEigen sb_constrained_size_; // the size of the state segment that is constrained
Eigen::MatrixXd J_; // Jacobian
public:
/** \brief Constructor
*
* \param _sb_ptr the constrained state block
*
*/
FactorBlockAbsolute(FeatureBasePtr _feature_ptr,
StateBlockPtr _sb_ptr,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorBlockAbsolute",
TOP_ABS,
_feature_ptr,
nullptr,
nullptr,
nullptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_sb_ptr),
sb_size_(_sb_ptr->getSize()),
sb_constrained_start_(0),
sb_constrained_size_(sb_size_)
{
assert(sb_constrained_size_+sb_constrained_start_ <= sb_size_);
// precompute Jacobian (Identity)
private:
SizeEigen sb_size_; // the size of the state block
SizeEigen sb_constrained_start_; // the index of the first state element that is constrained
SizeEigen sb_constrained_size_; // the size of the state segment that is constrained
Eigen::MatrixXd J_; // Jacobian
bool is_angle_; // Stateblock is StateAngle
public:
/** \brief Constructor for segment of state block
*
* \param _measurement the measurement
* \param _measurement_sqrt_information_upper the square root factorization of the measurement noise information matrix
* \param _node_state_blocks the NodeStateBlocks (Frame,Capture,Landmark,Sensor) containing the factored state block
* \param _key the key of the factored state block
* \param _start_idx the index of the first state element that is constrained
* \param _size the size of the state segment that is constrained, -1 = all
* \param _processor_ptr processor that created the factor
* \param _apply_loss_function if the factor should have loss function
* \param _status factor status
*
*/
FactorBlockAbsolute(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_state_blocks,
const char& _key,
unsigned int _start_idx,
int _size,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE)
: FactorAnalytic("FactorBlockAbsolute",
TOP_ABS,
_measurement,
_measurement_sqrt_information_upper,
{_node_state_blocks},
_processor_ptr,
{_node_state_blocks->getStateBlock(_key)},
_apply_loss_function,
_status),
sb_constrained_start_(_start_idx)
{
assert(_node_state_blocks->getStateBlock(_key) &&
"FactorBlockAbsolute: _node_state_blocks does not have _key");
// store fields after asserts
sb_size_ = _node_state_blocks->getStateBlock(_key)->getSize();
sb_constrained_size_ = _size == -1 ? sb_size_ : _size;
is_angle_ = (std::dynamic_pointer_cast<StateAngle>(_node_state_blocks->getStateBlock(_key)) != nullptr);
// more asserts
assert(_node_state_blocks->getStateBlock(_key)->getLocalSize() ==
_node_state_blocks->getStateBlock(_key)->getSize());
assert(sb_constrained_size_ + sb_constrained_start_ <= sb_size_);
assert(sb_constrained_size_ > 0);
assert(_measurement.size() == sb_constrained_size_ and "FactorBlockAbsolute: _measurement wrong size");
assert(_measurement_sqrt_information_upper.rows() == sb_constrained_size_ and
_measurement_sqrt_information_upper.cols() == sb_constrained_size_ and
"FactorBlockAbsolute: _measurement_sqrt_information_upper wrong size");
// precompute Jacobian (Identity)
if (sb_constrained_start_ == 0)
J_ = Eigen::MatrixXd::Identity(sb_constrained_size_, sb_size_);
}
/** \brief Constructor for segment of state block
*
* \param _sb_ptr the constrained state block
* \param _start_idx the index of the first state element that is constrained
* \param _start_idx the size of the state segment that is constrained, -1 = all the
*
*/
FactorBlockAbsolute(FeatureBasePtr _feature_ptr,
StateBlockPtr _sb_ptr,
unsigned int _start_idx,
int _size,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorBlockAbsolute",
TOP_ABS,
_feature_ptr,
nullptr,
nullptr,
nullptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_sb_ptr),
sb_size_(_sb_ptr->getSize()),
sb_constrained_start_(_start_idx),
sb_constrained_size_(_size == -1 ? sb_size_ : _size)
else
{
assert(sb_constrained_size_+sb_constrained_start_ <= sb_size_);
// precompute Jacobian (Identity)
if (sb_constrained_start_ == 0)
J_ = Eigen::MatrixXd::Identity(sb_constrained_size_, sb_size_);
else
{
J_ = Eigen::MatrixXd::Zero(sb_constrained_size_,sb_size_);
J_.middleCols(sb_constrained_start_,sb_constrained_size_) = Eigen::MatrixXd::Identity(sb_constrained_size_, sb_constrained_size_);
}
J_ = Eigen::MatrixXd::Zero(sb_constrained_size_, sb_size_);
J_.middleCols(sb_constrained_start_, sb_constrained_size_) =
Eigen::MatrixXd::Identity(sb_constrained_size_, sb_constrained_size_);
}
~FactorBlockAbsolute() override = default;
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
*
**/
Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const override;
/** \brief Returns the normalized jacobians evaluated in the states
*
* Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
*
* Returns the pure jacobians (without measurement noise) evaluated in the current state blocks values
*
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
*
**/
void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
/** \brief Returns the factor residual size
**/
unsigned int getSize() const override;
}
/** \brief Constructor for the whole state block
*
* \param _measurement the measurement
* \param _measurement_sqrt_information_upper the square root factorization of the measurement noise information matrix
* \param _node_state_blocks the NodeStateBlocks (Frame,Capture,Landmark,Sensor) containing the factored state block
* \param _key the key of the factored state block
* \param _processor_ptr processor that created the factor
* \param _apply_loss_function if the factor should have loss function
* \param _status factor status
*
*/
FactorBlockAbsolute(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_state_blocks,
const char& _key,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE)
: FactorBlockAbsolute(_measurement,
_measurement_sqrt_information_upper,
_node_state_blocks,
_key,
0,
-1,
_processor_ptr,
_apply_loss_function,
_status)
{
}
~FactorBlockAbsolute() override = default;
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
*
**/
Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector) const override;
/** \brief Returns the normalized jacobians evaluated in the states
*
* Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in
*std::vector of mapped Eigen::VectorXd. IMPORTANT: only fill the jacobians of the state blocks specified in
*_compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state
*block \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd>>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
*
* Returns the pure jacobians (without measurement noise) evaluated in the current state blocks values
*
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state
*block
*
**/
void evaluateUnweightedJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
/** \brief Returns the factor residual size
**/
unsigned int getSize() const override;
};
inline Eigen::VectorXd FactorBlockAbsolute::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
inline Eigen::VectorXd FactorBlockAbsolute::evaluateResiduals(
const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector) const
{
assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
assert(_st_vector.front().size() >= getMeasurement().size() && "Wrong StateBlock size");
// residual
if (sb_constrained_size_ == _st_vector.front().size())
if (is_angle_)
{
return getMeasurementSquareRootInformationUpper() *
Eigen::Vector1d(pi2pi(_st_vector.front()(0) - getMeasurement()(0)));
}
else if (sb_constrained_size_ == _st_vector.front().size())
{
return getMeasurementSquareRootInformationUpper() * (_st_vector.front() - getMeasurement());
}
else
return getMeasurementSquareRootInformationUpper() * (_st_vector.front().segment(sb_constrained_start_,sb_constrained_size_) - getMeasurement());
{
return getMeasurementSquareRootInformationUpper() *
(_st_vector.front().segment(sb_constrained_start_, sb_constrained_size_) - getMeasurement());
}
}
inline void FactorBlockAbsolute::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
inline void FactorBlockAbsolute::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd>>& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
assert(jacobians.size() == 1 && "Wrong number of jacobians!");
assert(_compute_jacobian.size() == 1 && "Wrong number of _compute_jacobian booleans!");
assert(_st_vector.front().size() == sb_size_ && "Wrong StateBlock size");
assert(_st_vector.front().size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
assert(_st_vector.front().size() >= getMeasurement().size() &&
"StateBlock size and measurement size should match");
// normalized jacobian
if (_compute_jacobian.front())
jacobians.front() = getMeasurementSquareRootInformationUpper() * J_;
if (_compute_jacobian.front()) jacobians.front() = getMeasurementSquareRootInformationUpper() * J_;
}
inline void FactorBlockAbsolute::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
assert(jacobians.size() == 1 && "Wrong number of jacobians!");
assert(_compute_jacobian.size() == 1 && "Wrong number of _compute_jacobian booleans!");
assert(_st_vector.front().size() == sb_size_ && "Wrong StateBlock size");
assert(_st_vector.front().size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
assert(_st_vector.front().size() >= getMeasurement().size() &&
"StateBlock size and measurement size should match");
// normalized jacobian
if (_compute_jacobian.front())
jacobians.front() = getMeasurementSquareRootInformationUpper() * J_;
if (_compute_jacobian.front()) jacobians.front() = getMeasurementSquareRootInformationUpper() * J_;
}
inline void FactorBlockAbsolute::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
inline void FactorBlockAbsolute::evaluateUnweightedJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
{
assert(jacobians.size() == 1 && "Wrong number of jacobians!");
......@@ -215,6 +241,4 @@ inline unsigned int FactorBlockAbsolute::getSize() const
return sb_constrained_size_;
}
} // namespace wolf
#endif
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// 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 factor_block_difference.h
*
* Created on: Feb 28, 2020
* \author: mfourmy
*/
#ifndef FACTOR_BLOCK_DIFFERENCE_H_
#define FACTOR_BLOCK_DIFFERENCE_H_
//Wolf includes
// 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/factor/factor_analytic.h"
#include "core/frame/frame_base.h"
#include "core/math/rotations.h"
#include "core/state_block/state_angle.h"
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorBlockDifference);
//class
// class
class FactorBlockDifference : public FactorAnalytic
{
private:
SizeEigen sb1_size_; // the size of the state blocks
SizeEigen sb2_size_; // the size of the state blocks
SizeEigen sb1_constrained_start_; // the index of the first state element of sb1 that is constrained
SizeEigen sb1_constrained_size_; // the size of sb1 the state segment that is constrained
SizeEigen sb2_constrained_start_; // the index of the first state element of sb2 that is constrained
SizeEigen sb2_constrained_size_; // the size of sb2 the state segment that is constrained
Eigen::MatrixXd J_res_sb1_; // Jacobian
Eigen::MatrixXd J_res_sb2_; // Jacobian
public:
/** \brief Constructor
*
* \param _sb_ptr the constrained state block
*
*/
FactorBlockDifference(const FeatureBasePtr& _feature_ptr,
const StateBlockPtr& _sb1_ptr,
const StateBlockPtr& _sb2_ptr,
const FrameBasePtr& _frame_other = nullptr,
const CaptureBasePtr& _cap_other = nullptr,
const FeatureBasePtr& _feat_other = nullptr,
const LandmarkBasePtr& _lmk_other = nullptr,
unsigned int _start_idx1 = 0,
int _size1 = -1,
unsigned int _start_idx2 = 0,
int _size2 = -1,
ProcessorBasePtr _processor_ptr = nullptr,
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorBlockDifference",
TOP_GEOM,
_feature_ptr,
_frame_other,
_cap_other,
_feat_other,
_lmk_other,
_processor_ptr,
_apply_loss_function,
_status,
_sb1_ptr,
_sb2_ptr),
sb1_size_(_sb1_ptr->getSize()),
sb2_size_(_sb2_ptr->getSize()),
sb1_constrained_start_(_start_idx1),
sb1_constrained_size_(_size1 == -1 ? sb1_size_ : _size1),
sb2_constrained_start_(_start_idx2),
sb2_constrained_size_(_size2 == -1 ? sb2_size_ : _size2)
{
// Check if constrained portion is in the stateblock vector range
assert(sb1_constrained_size_+sb1_constrained_start_ <= sb1_size_);
assert(sb2_constrained_size_+sb2_constrained_start_ <= sb2_size_);
assert(sb1_constrained_size_ == sb2_constrained_size_);
// precompute Jacobian (Identity with zero columns if the stateblock part is not constrained)
J_res_sb1_ = Eigen::MatrixXd::Zero(sb1_constrained_size_,sb1_size_);
J_res_sb1_.middleCols(sb1_constrained_start_,sb1_constrained_size_) = Eigen::MatrixXd::Identity(sb1_constrained_size_,sb1_constrained_size_);
J_res_sb2_ = Eigen::MatrixXd::Zero(sb2_constrained_size_,sb2_size_);
J_res_sb2_.middleCols(sb2_constrained_start_,sb1_constrained_size_) = - Eigen::MatrixXd::Identity(sb2_constrained_size_,sb2_constrained_size_);
}
~FactorBlockDifference() override = default;
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
*
**/
Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const override;
/** \brief Returns the normalized jacobians evaluated in the states
*
* Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
*
* Returns the pure jacobians (without measurement noise) evaluated in the current state blocks values
*
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
*
**/
void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
/** \brief Returns the factor residual size
**/
unsigned int getSize() const override;
private:
SizeEigen sb1_size_; // the size of the state blocks
SizeEigen sb2_size_; // the size of the state blocks
SizeEigen sb1_constrained_start_; // the index of the first state element of sb1 that is constrained
SizeEigen sb2_constrained_start_; // the index of the first state element of sb2 that is constrained
SizeEigen sb_constrained_size_; // the size of sb1 the state segment that is constrained
Eigen::MatrixXd J_res_sb1_; // Jacobian
Eigen::MatrixXd J_res_sb2_; // Jacobian
bool is_angle_; // Stateblock is StateAngle
public:
/** \brief Constructor for segment of state blocks
*
* \param _measurement the measurement
* \param _measurement_sqrt_information_upper the square root factorization of the measurement noise information matrix
* \param _node_state_blocks1 the NodeStateBlocks (Frame,Capture,Landmark,Sensor) containing the factored state block 1
* \param _node_state_blocks2 the NodeStateBlocks (Frame,Capture,Landmark,Sensor) containing the factored state block 2
* \param _key1 the key of the factored state block 1
* \param _key2 the key of the factored state block 2
* \param _start_idx1 the index of the first state element that is constrained in state block 1
* \param _start_idx2 the index of the first state element that is constrained in state block 2
* \param _size the size of the state segment that is constrained, -1 = all
* \param _processor_ptr processor that created the factor
* \param _apply_loss_function if the factor should have loss function
* \param _status factor status
*
*/
FactorBlockDifference(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_state_blocks1,
const NodeStateBlocksPtr& _node_state_blocks2,
const char& _key1,
const char& _key2,
unsigned int _start_idx1,
unsigned int _start_idx2,
int _size,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE)
: FactorAnalytic("FactorBlockDifference",
TOP_GEOM,
_measurement,
_measurement_sqrt_information_upper,
_node_state_blocks1 == _node_state_blocks2
? NodeStateBlocksPtrList{_node_state_blocks1}
: NodeStateBlocksPtrList{_node_state_blocks1, _node_state_blocks2},
_processor_ptr,
{_node_state_blocks1->getStateBlock(_key1), _node_state_blocks2->getStateBlock(_key2)},
_apply_loss_function,
_status),
sb1_constrained_start_(_start_idx1),
sb2_constrained_start_(_start_idx2)
{
assert(_node_state_blocks1->getStateBlock(_key1) &&
"FactorBlockDifference: _node_state_blocks1 does not have _key1");
assert(_node_state_blocks2->getStateBlock(_key2) &&
"FactorBlockDifference: _node_state_blocks2 does not have _key2");
// store fields after asserts of keys existence
sb1_size_ = _node_state_blocks1->getStateBlock(_key1)->getSize();
sb2_size_ = _node_state_blocks2->getStateBlock(_key2)->getSize();
assert(not(_size == -1 and sb1_size_ != sb2_size_) and
"FactorBlockDifference: _size == -1 but state blocks have different size");
sb_constrained_size_ = _size == -1 ? sb2_size_ : _size;
is_angle_ = std::dynamic_pointer_cast<StateAngle>(_node_state_blocks1->getStateBlock(_key1)) != nullptr;
assert(_measurement.size() == sb_constrained_size_ and "FactorBlockDifference: _measurement wrong size");
assert(_measurement_sqrt_information_upper.rows() == sb_constrained_size_ and
_measurement_sqrt_information_upper.cols() == sb_constrained_size_ and
"FactorBlockDifference: _measurement_sqrt_information_upper wrong size");
// Check if constrained portion is in the stateblock vector range
assert(sb_constrained_size_ + sb1_constrained_start_ <= sb1_size_);
assert(sb_constrained_size_ + sb2_constrained_start_ <= sb2_size_);
// precompute Jacobian (Identity with zero columns if the stateblock part is not constrained)
J_res_sb1_ = Eigen::MatrixXd::Zero(sb_constrained_size_, sb1_size_);
J_res_sb1_.middleCols(sb1_constrained_start_, sb_constrained_size_) =
Eigen::MatrixXd::Identity(sb_constrained_size_, sb_constrained_size_);
J_res_sb2_ = Eigen::MatrixXd::Zero(sb_constrained_size_, sb2_size_);
J_res_sb2_.middleCols(sb2_constrained_start_, sb_constrained_size_) =
-Eigen::MatrixXd::Identity(sb_constrained_size_, sb_constrained_size_);
}
/** \brief Constructor for the whole state blocks
*
* \param _measurement the measurement
* \param _measurement_sqrt_information_upper the square root factorization of the measurement noise information matrix
* \param _node_state_blocks1 the NodeStateBlocks (Frame,Capture,Landmark,Sensor) containing the factored state block 1
* \param _node_state_blocks2 the NodeStateBlocks (Frame,Capture,Landmark,Sensor) containing the factored state block 2
* \param _key1 the key of the factored state block 1
* \param _key2 the key of the factored state block 2
* \param _processor_ptr processor that created the factor
* \param _apply_loss_function if the factor should have loss function
* \param _status factor status
*
*/
FactorBlockDifference(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_state_blocks1,
const NodeStateBlocksPtr& _node_state_blocks2,
const char& _key1,
const char& _key2,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE)
: FactorBlockDifference(_measurement,
_measurement_sqrt_information_upper,
_node_state_blocks1,
_node_state_blocks2,
_key1,
_key2,
0,
0,
-1,
_processor_ptr,
_apply_loss_function,
_status)
{
}
~FactorBlockDifference() override = default;
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
*
**/
Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector) const override;
/** \brief Returns the normalized jacobians evaluated in the states
*
* Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in
*std::vector of mapped Eigen::VectorXd. IMPORTANT: only fill the jacobians of the state blocks specified in
*_compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd>>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
*
* Returns the pure jacobians (without measurement noise) evaluated in the current state blocks values
*
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state
*block
*
**/
void evaluateUnweightedJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
/** \brief Returns the factor residual size
**/
unsigned int getSize() const override;
};
inline Eigen::VectorXd FactorBlockDifference::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
inline Eigen::VectorXd FactorBlockDifference::evaluateResiduals(
const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector) const
{
// Check measurement and cov sizes
// Check measurement and cov sizes
assert(_st_vector.size() == 2 && "Wrong number of state blocks!");
assert(getMeasurement().size() == getMeasurementSquareRootInformationUpper().cols());
assert(getMeasurement().size() == getMeasurementSquareRootInformationUpper().rows());
assert(getMeasurement().size() == sb1_constrained_size_);
assert(getMeasurement().size() == sb2_constrained_size_);
assert(getMeasurement().size() == sb_constrained_size_);
// residual: meas -> measurement of the difference between partial stateblocks
return getMeasurementSquareRootInformationUpper() * (getMeasurement() - (_st_vector[1].segment(sb2_constrained_start_, sb2_constrained_size_)
- _st_vector[0].segment(sb1_constrained_start_, sb1_constrained_size_)));
return getMeasurementSquareRootInformationUpper() *
(getMeasurement() - (_st_vector[1].segment(sb2_constrained_start_, sb_constrained_size_) -
_st_vector[0].segment(sb1_constrained_start_, sb_constrained_size_)));
// residual
if (is_angle_)
{
return getMeasurementSquareRootInformationUpper() *
Eigen::Vector1d(pi2pi(getMeasurement()(0) - (_st_vector[1](0) - _st_vector[0](0))));
}
else
{
return getMeasurementSquareRootInformationUpper() *
(getMeasurement() - (_st_vector[1].segment(sb2_constrained_start_, sb_constrained_size_) -
_st_vector[0].segment(sb1_constrained_start_, sb_constrained_size_)));
}
}
inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& _jacobians,
const std::vector<bool>& _compute_jacobian) const
inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd>>& _jacobians,
const std::vector<bool>& _compute_jacobian) const
{
assert(_st_vector.size() == 2 && "Wrong number of state blocks!");
assert(_jacobians.size() == 2 && "Wrong number of jacobians!");
......@@ -168,21 +232,25 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() &&
"Wrong jacobian sb1 or covariance size");
assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() &&
"Wrong jacobian sb2 or covariance size");
// normalized jacobian, computed according to the _compute_jacobian flag
if (_compute_jacobian[0]){
if (_compute_jacobian[0])
{
_jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
}
if (_compute_jacobian[1]){
if (_compute_jacobian[1])
{
_jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
}
}
inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& _jacobians,
const std::vector<bool>& _compute_jacobian) const
std::vector<Eigen::MatrixXd>& _jacobians,
const std::vector<bool>& _compute_jacobian) const
{
assert(_st_vector.size() == 2 && "Wrong number of state blocks!");
assert(_jacobians.size() == 2 && "Wrong number of jacobians!");
......@@ -191,19 +259,23 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() &&
"Wrong jacobian sb1 or covariance size");
assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() &&
"Wrong jacobian sb2 or covariance size");
// normalized jacobian, computed according to the _compute_jacobian flag
if (_compute_jacobian[0]){
if (_compute_jacobian[0])
{
_jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
}
if (_compute_jacobian[1]){
if (_compute_jacobian[1])
{
_jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
}
}
inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& _pure_jacobians) const
inline void FactorBlockDifference::evaluateUnweightedJacobians(std::vector<Eigen::MatrixXd>& _pure_jacobians) const
{
assert(_pure_jacobians.size() == 1 && "Wrong number of jacobians!");
......@@ -213,9 +285,7 @@ inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::Matr
inline unsigned int FactorBlockDifference::getSize() const
{
return sb1_constrained_size_;
return sb_constrained_size_;
}
} // namespace wolf
#endif
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// 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 factor_diff_drive.h
*
* Created on: Oct 27, 2017
* \author: Jeremie Deray
* \adapted: Joan Sola - July 2019
*/
#ifndef WOLF_FACTOR_DIFF_DRIVE_H_
#define WOLF_FACTOR_DIFF_DRIVE_H_
//Wolf includes
// 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/capture/capture_diff_drive.h"
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
......@@ -39,17 +27,15 @@
namespace
{
constexpr std::size_t POSITION_SIZE = 2;
constexpr std::size_t ORIENTATION_SIZE = 1;
constexpr std::size_t INTRINSICS_SIZE = 3;
constexpr std::size_t RESIDUAL_SIZE = 3;
constexpr std::size_t POSITION_SIZE = 2;
constexpr std::size_t ORIENTATION_SIZE = 1;
constexpr std::size_t INTRINSICS_SIZE = 3;
constexpr std::size_t RESIDUAL_SIZE = 3;
}
} // namespace
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorDiffDrive);
class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
......@@ -60,65 +46,66 @@ class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
ORIENTATION_SIZE,
INTRINSICS_SIZE>
{
using Base = FactorAutodiff<FactorDiffDrive, // @suppress("Type cannot be resolved")
RESIDUAL_SIZE,
POSITION_SIZE,
ORIENTATION_SIZE,
POSITION_SIZE,
ORIENTATION_SIZE,
INTRINSICS_SIZE>;
public:
FactorDiffDrive(const FeatureMotionPtr& _ftr_ptr,
const CaptureBasePtr& _capture_origin_ptr,
const ProcessorBasePtr& _processor_ptr,
const bool _apply_loss_function,
const FactorStatus _status = FAC_ACTIVE) :
Base("FactorDiffDrive",
TOP_MOTION,
_ftr_ptr,
_capture_origin_ptr->getFrame(),
_capture_origin_ptr,
nullptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_capture_origin_ptr->getFrame()->getP(),
_capture_origin_ptr->getFrame()->getO(),
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_capture_origin_ptr->getSensorIntrinsic()),
calib_preint_(_ftr_ptr->getCalibrationPreint()),
J_delta_calib_(_ftr_ptr->getJacobianCalibration())
{
//
}
/**
* \brief Default destructor (not recommended)
*
* Default destructor.
*
**/
~FactorDiffDrive() override = default;
template<typename T>
bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
const T* const _o2, const T* const _c, T* _residuals) const;
VectorXd residual();
protected:
Eigen::Vector3d calib_preint_;
Eigen::MatrixXd J_delta_calib_;
public:
FactorDiffDrive(const FeatureMotionPtr& _ftr_ptr,
const CaptureBasePtr& _capture_origin_ptr,
const ProcessorBasePtr& _processor_ptr,
const bool _apply_loss_function,
const FactorStatus _status = FAC_ACTIVE)
: FactorAutodiff("FactorDiffDrive",
TOP_MOTION,
_ftr_ptr->getMeasurement(),
_ftr_ptr->getMeasurementSquareRootInformationUpper(),
{std::static_pointer_cast<NodeStateBlocks>(_capture_origin_ptr->getFrame()),
std::static_pointer_cast<NodeStateBlocks>(_ftr_ptr->getFrame()),
_capture_origin_ptr->hasStateBlock(_capture_origin_ptr->getSensorIntrinsic())
? std::static_pointer_cast<NodeStateBlocks>(_capture_origin_ptr)
: std::static_pointer_cast<NodeStateBlocks>(_capture_origin_ptr->getSensor())},
_processor_ptr,
{_capture_origin_ptr->getFrame()->getP(),
_capture_origin_ptr->getFrame()->getO(),
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_capture_origin_ptr->getSensorIntrinsic()},
_apply_loss_function,
_status),
calib_preint_(_ftr_ptr->getCalibrationPreint()),
J_delta_calib_(_ftr_ptr->getJacobianCalibration())
{
assert(_capture_origin_ptr->hasStateBlock(_capture_origin_ptr->getSensorIntrinsic()) or
_capture_origin_ptr->getSensor()->hasStateBlock(_capture_origin_ptr->getSensorIntrinsic()));
}
/**
* \brief Default destructor (not recommended)
*
* Default destructor.
*
**/
~FactorDiffDrive() override = default;
template <typename T>
bool operator()(const T* const _p1,
const T* const _o1,
const T* const _p2,
const T* const _o2,
const T* const _c,
T* _residuals) const;
// VectorXd residual();
protected:
Eigen::Vector3d calib_preint_;
Eigen::MatrixXd J_delta_calib_;
};
template<typename T>
inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
const T* const _o2, const T* const _c, T* _residuals) const
template <typename T>
inline bool FactorDiffDrive::operator()(const T* const _p1,
const T* const _o1,
const T* const _p2,
const T* const _o2,
const T* const _c,
T* _residuals) const
{
// MAPS
Eigen::Map<Eigen::Matrix<T, RESIDUAL_SIZE, 1> > residuals(_residuals);
......@@ -128,11 +115,9 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
Eigen::Map<const Eigen::Matrix<T, INTRINSICS_SIZE, 1> > c(_c);
// Correct delta due to changes in the calibration parameters
auto delta_corrected = getMeasurement() + J_delta_calib_ * (c - calib_preint_);
// Predict delta from states
Eigen::Matrix<T, 3, 1> delta_predicted;
// position
......@@ -152,18 +137,4 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
return true;
}
inline Eigen::VectorXd FactorDiffDrive::residual()
{
VectorXd residual(3);
operator ()(getFrameOther() ->getP() ->getState() .data(),
getFrameOther() ->getO() ->getState() .data(),
getFrame() ->getP() ->getState() .data(),
getFrame() ->getO() ->getState() .data(),
getCaptureOther() ->getSensorIntrinsic() ->getState() .data(),
residual.data());
return residual;
}
} // namespace wolf
#endif /* WOLF_FACTOR_DIFF_DRIVE_H_ */
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// 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 factor_autodiff_distance_3d.h
*
* Created on: Apr 10, 2018
* \author: jsola
*/
#ifndef FACTOR_DISTANCE_3d_H_
#define FACTOR_DISTANCE_3d_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/factor/factor_autodiff.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorDistance3d);
class FactorDistance3d : public FactorAutodiff<FactorDistance3d, 1, 3, 3>
{
public:
FactorDistance3d(const FeatureBasePtr& _feat,
const FrameBasePtr& _frm_other,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff("FactorAutodiffDistance3d",
TOP_GEOM,
_feat,
_frm_other,
nullptr,
nullptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_feat->getFrame()->getP(),
_frm_other->getP())
{
}
public:
FactorDistance3d(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_reference,
const NodeStateBlocksPtr& _node_target,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE)
: FactorAutodiff("FactorDistance3d",
TOP_GEOM,
_measurement,
_measurement_sqrt_information_upper,
{_node_reference, _node_target},
_processor_ptr,
{_node_reference->getP(), _node_target->getP()},
_apply_loss_function,
_status)
{
assert(_node_reference->getP() && "FactorDistance3d: _node_reference state P not found!");
assert(_node_target->getP() && "FactorDistance3d: _node_target state P not found!");
MatrixSizeCheck<1, 1>::check(_measurement);
MatrixSizeCheck<1, 1>::check(_measurement_sqrt_information_upper);
}
~FactorDistance3d() override { /* nothing */ }
~FactorDistance3d() override
{ /* nothing */
}
template<typename T>
bool operator () (const T* const _pos1,
const T* const _pos2,
T* _residual) const
{
using namespace Eigen;
template <typename T>
bool operator()(const T* const _pos1, const T* const _pos2, T* _residual) const
{
using namespace Eigen;
Map<const Matrix<T,3,1>> pos1(_pos1);
Map<const Matrix<T,3,1>> pos2(_pos2);
Map<Matrix<T,1,1>> res(_residual);
Map<const Matrix<T, 3, 1>> pos1(_pos1);
Map<const Matrix<T, 3, 1>> pos2(_pos2);
Map<Matrix<T, 1, 1>> res(_residual);
// If pos2 and pos1 are the same, undefined behavior when computing the jacobian
T norm_squared = ( pos2 - pos1 ).squaredNorm();
if (norm_squared < (T)1e-8){
norm_squared += (T)1e-8;
}
Matrix<T,1,1> dist_exp ( sqrt(norm_squared) );
// If pos2 and pos1 are the same, undefined behavior when computing the jacobian
T norm_squared = (pos2 - pos1).squaredNorm();
if (norm_squared < (T)1e-8)
{
norm_squared += (T)1e-8;
}
Matrix<T, 1, 1> dist_exp(sqrt(norm_squared));
res = getMeasurementSquareRootInformationUpper() * (getMeasurement() - dist_exp);
res = getMeasurementSquareRootInformationUpper() * (getMeasurement() - dist_exp);
return true;
}
return true;
}
};
} /* namespace wolf */
#endif /* FACTOR_DISTANCE_3d_H_ */
// 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/factor/factor_autodiff.h"
#include "core/state_block/state_homogeneous_3d.h"
#include "core/common/node_state_blocks.h"
#include "core/math/rotations.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorHomogeneous3dAbsolute);
// class
class FactorHomogeneous3dAbsolute : public FactorAutodiff<FactorHomogeneous3dAbsolute, 3, 4>
{
public:
/** \brief Constructor
*
* \param _measurement the measurement (3D euclidean point or 4D homogeneous point)
* \param _measurement_sqrt_information_upper the square root factorization of the measurement noise information matrix
* \param _node_state_blocks the NodeStateBlocks (Frame,Capture,Landmark,Sensor) containing the factored state block
* \param _key the key of the factored state block
* \param _processor_ptr processor that created the factor
* \param _apply_loss_function if the factor should have loss function
* \param _status factor status
*
*/
FactorHomogeneous3dAbsolute(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_state_blocks,
const char& _key,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE)
: FactorAutodiff("FactorHomogeneous3dAbsolute",
TOP_ABS,
_measurement.size() == 4 ? _measurement.head<3>() / _measurement(3) : _measurement,
_measurement_sqrt_information_upper,
{_node_state_blocks},
_processor_ptr,
{_node_state_blocks->getStateBlock(_key)},
_apply_loss_function,
_status)
{
assert(_node_state_blocks->getStateBlock(_key) &&
"FactorHomogeneous3dAbsolute: _node_state_blocks does not have _key");
assert(std::dynamic_pointer_cast<StateHomogeneous3d>(_node_state_blocks->getStateBlock(_key)) &&
"FactorHomogeneous3dAbsolute: state block should be of type StateHomogeneous3d");
assert(_measurement.size() == 3 or _measurement.size() == 4);
MatrixSizeCheck<3, 1>::check(measurement_);
MatrixSizeCheck<3, 3>::check(_measurement_sqrt_information_upper);
}
~FactorHomogeneous3dAbsolute() override = default;
template <typename T>
bool operator()(const T* const _h, T* _residuals) const;
};
template <typename T>
inline bool FactorHomogeneous3dAbsolute::operator()(const T* const _h, T* _residuals) const
{
// residual (measurement is 3D point)
Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
res = getMeasurementSquareRootInformationUpper() *
(getMeasurement() - Eigen::Matrix<T, 3, 1>(_h[0] / _h[3], _h[1] / _h[3], _h[2] / _h[3]));
return true;
}
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// 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_POSE_2d_H_
#define FACTOR_POSE_2d_H_
//Wolf includes
// Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/math/rotations.h"
//#include "ceres/jet.h"
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorPose2d);
//class
class FactorPose2d: public FactorAutodiff<FactorPose2d,3,2,1>
// class
class FactorPose2d : public FactorAutodiff<FactorPose2d, 3, 2, 1>
{
public:
FactorPose2d(FeatureBasePtr _ftr_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPose2d, 3, 2, 1>("FactorPose2d",
TOP_ABS,
_ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
// std::cout << "created FactorPose2d " << std::endl;
}
~FactorPose2d() override = default;
public:
FactorPose2d(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_state_blocks,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE)
: FactorAutodiff("FactorPose2d",
TOP_ABS,
_measurement,
_measurement_sqrt_information_upper,
{_node_state_blocks},
_processor_ptr,
{_node_state_blocks->getP(), _node_state_blocks->getO()},
_apply_loss_function,
_status)
{
assert(_node_state_blocks->getP() && "FactorPose2d: _node_state_blocks state P not found!");
assert(_node_state_blocks->getO() && "FactorPose2d: _node_state_blocks state O not found!");
MatrixSizeCheck<3, 1>::check(_measurement);
MatrixSizeCheck<3, 3>::check(_measurement_sqrt_information_upper);
}
template<typename T>
bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
~FactorPose2d() override = default;
template <typename T>
bool operator()(const T* const _p, const T* const _o, T* _residuals) const;
};
template<typename T>
inline bool FactorPose2d::operator ()(const T* const _p, const T* const _o, T* _residuals) const
template <typename T>
inline bool FactorPose2d::operator()(const T* const _p, const T* const _o, T* _residuals) const
{
// measurement
// Eigen::Matrix<T,3,1> meas = getMeasurement().cast<T>();
auto& meas = getMeasurement();
auto& meas = getMeasurement();
// error
Eigen::Matrix<T,3,1> er;
Eigen::Matrix<T, 3, 1> er;
er(0) = meas(0) - _p[0];
er(1) = meas(1) - _p[1];
er(2) = pi2pi(meas(2) - _o[0]);
// residual
Eigen::Map<Eigen::Matrix<T,3,1>> res(_residuals);
Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
res = getMeasurementSquareRootInformationUpper() * er;
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
// using ceres::Jet;
// Eigen::MatrixXd J(3,3);
// J.row(0) = ((Jet<double, 3>)(er(0))).v;
// J.row(1) = ((Jet<double, 3>)(er(1))).v;
// J.row(2) = ((Jet<double, 3>)(er(2))).v;
// J.row(0) = ((Jet<double, 3>)(res(0))).v;
// J.row(1) = ((Jet<double, 3>)(res(1))).v;
// J.row(2) = ((Jet<double, 3>)(res(2))).v;
// if (sizeof(er(0)) != sizeof(double))
// {
// std::cout << "FactorPose2d::Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "FactorPose2d::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "Sqrt Info(c" << id() << ") = \n " << getMeasurementSquareRootInformationUpper() << std::endl;
// }
// using ceres::Jet;
// Eigen::MatrixXd J(3,3);
// J.row(0) = ((Jet<double, 3>)(er(0))).v;
// J.row(1) = ((Jet<double, 3>)(er(1))).v;
// J.row(2) = ((Jet<double, 3>)(er(2))).v;
// J.row(0) = ((Jet<double, 3>)(res(0))).v;
// J.row(1) = ((Jet<double, 3>)(res(1))).v;
// J.row(2) = ((Jet<double, 3>)(res(2))).v;
// if (sizeof(er(0)) != sizeof(double))
// {
// std::cout << "FactorPose2d::Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "FactorPose2d::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "Sqrt Info(c" << id() << ") = \n " << getMeasurementSquareRootInformationUpper() <<
// std::endl;
// }
////////////////////////////////////////////////////////
return true;
}
} // namespace wolf
#endif
} // namespace wolf
// 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/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/math/rotations.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorPose2dWithExtrinsics);
// class
class FactorPose2dWithExtrinsics : public FactorAutodiff<FactorPose2dWithExtrinsics, 3, 2, 1, 2, 1>
{
public:
FactorPose2dWithExtrinsics(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const FrameBasePtr& _frame_ptr,
const SensorBasePtr& _sensor_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE)
: FactorAutodiff("FactorPose2dWithExtrinsics",
TOP_ABS,
_measurement,
_measurement_sqrt_information_upper,
{_frame_ptr, _sensor_ptr},
_processor_ptr,
{_frame_ptr->getP(), _frame_ptr->getO(), _sensor_ptr->getP(), _sensor_ptr->getO()},
_apply_loss_function,
_status)
{
assert(_frame_ptr->getP() && "FactorPose2dWithExtrinsics: _frame_ptr state P not found!");
assert(_frame_ptr->getO() && "FactorPose2dWithExtrinsics: _frame_ptr state O not found!");
assert(_sensor_ptr->getP() && "FactorPose2dWithExtrinsics: _sensor_ptr state P not found!");
assert(_sensor_ptr->getP() && "FactorPose2dWithExtrinsics: _sensor_ptr state P not found!");
assert(not _sensor_ptr->isStateBlockDynamic('P') and not _sensor_ptr->isStateBlockDynamic('O') &&
"FactorPose2dWithExtrinsics: extrinsics cannot be dynamic!");
MatrixSizeCheck<3, 1>::check(_measurement);
MatrixSizeCheck<3, 3>::check(_measurement_sqrt_information_upper);
}
~FactorPose2dWithExtrinsics() override = default;
template <typename T>
bool operator()(const T* const _p, const T* const _o, const T* const _sp, const T* const _so, T* _residuals) const;
};
template <typename T>
inline bool FactorPose2dWithExtrinsics::operator()(const T* const _p,
const T* const _o,
const T* const _sp,
const T* const _so,
T* _residuals) const
{
// MAPS
Eigen::Map<const Eigen::Matrix<T, 2, 1> > p(_p);
Eigen::Map<const Eigen::Matrix<T, 2, 1> > sp(_sp);
Eigen::Matrix<T, 3, 1> err; // error
err.head(2) = getMeasurement().head(2) - (p.head(2) + Eigen::Rotation2D<T>(_o[0]) * sp.head(2));
err(2) = pi2pi(getMeasurement()(2) - (_o[0] + _so[0]));
// Residuals
Eigen::Map<Eigen::Matrix<T, 3, 1> > res(_residuals);
res = getMeasurementSquareRootInformationUpper().cast<T>() * err;
return true;
}
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// 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 FACTOR_POSE_3d_H_
#define FACTOR_POSE_3d_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/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/math/rotations.h"
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorPose3d);
//class
class FactorPose3d: public FactorAutodiff<FactorPose3d,6,3,4>
// class
class FactorPose3d : public FactorAutodiff<FactorPose3d, 6, 3, 4>
{
public:
FactorPose3d(FeatureBasePtr _ftr_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPose3d,6,3,4>("FactorPose3d",
TOP_ABS,
_ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
//
}
~FactorPose3d() override = default;
template<typename T>
bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
public:
FactorPose3d(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_state_blocks,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE)
: FactorAutodiff("FactorPose3d",
TOP_ABS,
_measurement,
_measurement_sqrt_information_upper,
{_node_state_blocks},
_processor_ptr,
{_node_state_blocks->getP(), _node_state_blocks->getO()},
_apply_loss_function,
_status)
{
assert(_node_state_blocks->getP() && "FactorPose3d: _node_state_blocks state P not found!");
assert(_node_state_blocks->getO() && "FactorPose3d: _node_state_blocks state O not found!");
MatrixSizeCheck<7, 1>::check(_measurement);
MatrixSizeCheck<6, 6>::check(_measurement_sqrt_information_upper);
}
~FactorPose3d() override = default;
template <typename T>
bool operator()(const T* const _p, const T* const _o, T* _residuals) const;
};
template<typename T>
inline bool FactorPose3d::operator ()(const T* const _p, const T* const _o, T* _residuals) const
template <typename T>
inline bool FactorPose3d::operator()(const T* const _p, const T* const _o, T* _residuals) const
{
// states
Eigen::Map<const Eigen::Matrix<T, 3, 1>> p(_p);
Eigen::Map<const Eigen::Quaternion<T>> q(_o);
Eigen::Map<const Eigen::Matrix<T, 3, 1>> p(_p);
Eigen::Map<const Eigen::Quaternion<T>> q(_o);
// measurements
Eigen::Vector3d p_measured(getMeasurement().data() + 0);
Eigen::Quaterniond q_measured(getMeasurement().data() + 3);
Eigen::Vector3d p_measured(getMeasurement().data() + 0);
Eigen::Quaterniond q_measured(getMeasurement().data() + 3);
// error
Eigen::Matrix<T, 6, 1> er;
er.head(3) = p_measured - p;
er.tail(3) = q2v(q.conjugate() * q_measured.cast<T>());
er.head(3) = p_measured - p;
er.tail(3) = q2v(q.conjugate() * q_measured.cast<T>());
// residual
Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals);
res = getMeasurementSquareRootInformationUpper() * er;
res = getMeasurementSquareRootInformationUpper() * er;
return true;
}
} // namespace wolf
#endif
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// 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 FACTOR_POSE_3D_WITH_EXTRINSICS_THETA_H_
#define FACTOR_POSE_3D_WITH_EXTRINSICS_THETA_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/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/math/rotations.h"
//#include "ceres/jet.h"
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorPose3dWithExtrinsics);
//class
// class
class FactorPose3dWithExtrinsics : public FactorAutodiff<FactorPose3dWithExtrinsics, 6, 3, 4, 3, 4>
{
public:
FactorPose3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPose3dWithExtrinsics, 6, 3, 4, 3, 4>("FactorPose3dWithExtrinsics",
_top,
_ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_ftr_ptr->getCapture()->getSensorP(),
_ftr_ptr->getCapture()->getSensorO())
{
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
//
}
~FactorPose3dWithExtrinsics() override = default;
template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
inline void error(const Eigen::MatrixBase<D1> & w_p_wb,
const Eigen::QuaternionBase<D2> & w_q_b,
const Eigen::MatrixBase<D3> & b_p_bm,
const Eigen::QuaternionBase<D4> & b_q_m,
const Eigen::MatrixBase<D5> & w_p_wm,
const Eigen::QuaternionBase<D6> & w_q_m,
Eigen::MatrixBase<D7> & p_err,
Eigen::MatrixBase<D7> & o_err) const;
inline Eigen::Vector6d error() const;
template<typename T>
bool operator ()(const T* const _p,
const T* const _q,
const T* const _sp,
const T* const _sq,
T* _residuals) const;
public:
FactorPose3dWithExtrinsics(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const FrameBasePtr& _frame_ptr,
const SensorBasePtr& _sensor_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE)
: FactorAutodiff("FactorPose3dWithExtrinsics",
TOP_ABS,
_measurement,
_measurement_sqrt_information_upper,
{_frame_ptr, _sensor_ptr},
_processor_ptr,
{_frame_ptr->getP(), _frame_ptr->getO(), _sensor_ptr->getP(), _sensor_ptr->getO()},
_apply_loss_function,
_status)
{
assert(_frame_ptr->getP() && "FactorPose3dWithExtrinsics: _frame_ptr state P not found!");
assert(_frame_ptr->getO() && "FactorPose3dWithExtrinsics: _frame_ptr state O not found!");
assert(_sensor_ptr->getP() && "FactorPose3dWithExtrinsics: _sensor_ptr state P not found!");
assert(_sensor_ptr->getP() && "FactorPose3dWithExtrinsics: _sensor_ptr state P not found!");
assert(not _sensor_ptr->isStateBlockDynamic('P') and not _sensor_ptr->isStateBlockDynamic('O') &&
"FactorPose3dWithExtrinsics: extrinsics cannot be dynamic!");
MatrixSizeCheck<7, 1>::check(_measurement);
MatrixSizeCheck<6, 6>::check(_measurement_sqrt_information_upper);
}
~FactorPose3dWithExtrinsics() override = default;
template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
inline void error(const Eigen::MatrixBase<D1>& w_p_wb,
const Eigen::QuaternionBase<D2>& w_q_b,
const Eigen::MatrixBase<D3>& b_p_bm,
const Eigen::QuaternionBase<D4>& b_q_m,
const Eigen::MatrixBase<D5>& w_p_wm,
const Eigen::QuaternionBase<D6>& w_q_m,
Eigen::MatrixBase<D7>& p_err,
Eigen::MatrixBase<D7>& o_err) const;
inline Eigen::Vector6d error() const;
template <typename T>
bool operator()(const T* const _p, const T* const _q, const T* const _sp, const T* const _sq, T* _residuals) const;
};
template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
inline void FactorPose3dWithExtrinsics::error(const Eigen::MatrixBase<D1> & w_p_wb,
const Eigen::QuaternionBase<D2> & w_q_b,
const Eigen::MatrixBase<D3> & b_p_bm,
const Eigen::QuaternionBase<D4> & b_q_m,
const Eigen::MatrixBase<D5> & w_p_wm,
const Eigen::QuaternionBase<D6> & w_q_m,
Eigen::MatrixBase<D7> & p_err,
Eigen::MatrixBase<D7> & o_err) const
template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
inline void FactorPose3dWithExtrinsics::error(const Eigen::MatrixBase<D1>& w_p_wb,
const Eigen::QuaternionBase<D2>& w_q_b,
const Eigen::MatrixBase<D3>& b_p_bm,
const Eigen::QuaternionBase<D4>& b_q_m,
const Eigen::MatrixBase<D5>& w_p_wm,
const Eigen::QuaternionBase<D6>& w_q_m,
Eigen::MatrixBase<D7>& p_err,
Eigen::MatrixBase<D7>& o_err) const
{
// Transformation definitions:
// w_p_wm, w_q_m: world to measurement frame (robot attached mocap frame)
// w_p_wb, w_q_b: world to robot base frame
// w_p_wm, w_q_m: world to measurement frame (robot attached mocap frame)
// w_p_wb, w_q_b: world to robot base frame
// b_p_bm, b_q_m: base to measurement frame (extrinsics)
// Error function:
// err = w_T_m |-| (w_T_b*b_T_m)
p_err = w_p_wm - (w_p_wb + w_q_b*b_p_bm);
p_err = w_p_wm - (w_p_wb + w_q_b * b_p_bm);
o_err = q2v((w_q_b * b_q_m).conjugate() * w_q_m);
}
inline Eigen::Vector6d FactorPose3dWithExtrinsics::error() const
{
// get current frame and extrinsics estimates
const Eigen::Vector3d w_p_wb = (getFeature()->getFrame()->getP()->getState());
const Eigen::Quaterniond w_q_b (getFeature()->getFrame()->getO()->getState().data());
const Eigen::Vector3d b_p_bm = (getFeature()->getCapture()->getSensorP()->getState());
const Eigen::Quaterniond b_q_m (getFeature()->getCapture()->getSensorO()->getState().data());
const Eigen::Vector3d w_p_wb = (getFeature()->getFrame()->getP()->getState());
const Eigen::Quaterniond w_q_b(getFeature()->getFrame()->getO()->getState().data());
const Eigen::Vector3d b_p_bm = (getFeature()->getCapture()->getSensorP()->getState());
const Eigen::Quaterniond b_q_m(getFeature()->getCapture()->getSensorO()->getState().data());
// measurements
Eigen::Vector3d w_p_wm(getMeasurement().data() + 0); // measurements
Eigen::Quaterniond w_q_m (getMeasurement().data() + 3); // measurements
Eigen::Quaterniond w_q_m(getMeasurement().data() + 3); // measurements
Vector6d err;
Eigen::Map<Vector3d> p_err(err.data() + 0);
Eigen::Map<Vector3d> o_err(err.data() + 3);
Eigen::Vector6d err;
Eigen::Map<Eigen::Vector3d> p_err(err.data() + 0);
Eigen::Map<Eigen::Vector3d> o_err(err.data() + 3);
error(w_p_wb, w_q_b, b_p_bm, b_q_m, w_p_wm, w_q_m, p_err, o_err);
return err;
}
template<typename T>
inline bool FactorPose3dWithExtrinsics::operator ()(const T* const _p,
const T* const _q,
const T* const _sp,
const T* const _sq,
T* _residuals) const
template <typename T>
inline bool FactorPose3dWithExtrinsics::operator()(const T* const _p,
const T* const _q,
const T* const _sp,
const T* const _sq,
T* _residuals) const
{
// MAPS
Eigen::Map<const Eigen::Matrix<T,3,1> > w_p_wb(_p);
Eigen::Map<const Eigen::Quaternion<T> > w_q_b(_q);
Eigen::Map<const Eigen::Matrix<T,3,1> > b_p_bm(_sp);
Eigen::Map<const Eigen::Quaternion<T> > b_q_m(_sq);
Eigen::Map<const Eigen::Matrix<T, 3, 1> > w_p_wb(_p);
Eigen::Map<const Eigen::Quaternion<T> > w_q_b(_q);
Eigen::Map<const Eigen::Matrix<T, 3, 1> > b_p_bm(_sp);
Eigen::Map<const Eigen::Quaternion<T> > b_q_m(_sq);
// measurements
Eigen::Vector3d w_p_wm(getMeasurement().data() + 0); // measurements
Eigen::Quaterniond w_q_m (getMeasurement().data() + 3); // measurements
Eigen::Quaterniond w_q_m(getMeasurement().data() + 3); // measurements
Eigen::Matrix<T, 6, 1> err; // error
Eigen::Map<Matrix<T, 3, 1> > p_err(err.data() + 0);
Eigen::Map<Matrix<T, 3, 1> > o_err(err.data() + 3);
Eigen::Matrix<T, 6, 1> err; // error
Eigen::Map<Eigen::Matrix<T, 3, 1> > p_err(err.data() + 0);
Eigen::Map<Eigen::Matrix<T, 3, 1> > o_err(err.data() + 3);
error(w_p_wb, w_q_b, b_p_bm, b_q_m, w_p_wm.cast<T>(), w_q_m.cast<T>(), p_err, o_err);
// Residuals
Eigen::Map<Eigen::Matrix<T,6,1> > res(_residuals);
Eigen::Map<Eigen::Matrix<T, 6, 1> > res(_residuals);
res = getMeasurementSquareRootInformationUpper().cast<T>() * err;
return true;
}
} // namespace wolf
#endif
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// 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 factor_quaternion_absolute.h
*
* Created on: Dec 15, 2017
* \author: AtDinesh
*/
// 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_QUATERNION_ABSOLUTE_H_
#define FACTOR_QUATERNION_ABSOLUTE_H_
//Wolf includes
// Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/state_block/local_parametrization_quaternion.h"
#include "core/frame/frame_base.h"
#include "core/state_block/state_quaternion.h"
#include "core/common/node_state_blocks.h"
#include "core/math/rotations.h"
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorQuaternionAbsolute);
//class
class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3,4>
// class
class FactorQuaternionAbsolute : public FactorAutodiff<FactorQuaternionAbsolute, 3, 4>
{
public:
FactorQuaternionAbsolute(FeatureBasePtr _ftr_ptr,
StateBlockPtr _sb_ptr,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorQuaternionAbsolute,3,4>("FactorQuaternionAbsolute",
TOP_ABS,
_ftr_ptr,
nullptr,
nullptr,
nullptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_sb_ptr)
{
//
}
~FactorQuaternionAbsolute() override = default;
template<typename T>
bool operator ()(const T* const _o, T* _residuals) const;
public:
/** \brief Constructor
*
* \param _measurement the measurement
* \param _measurement_sqrt_information_upper the square root factorization of the measurement noise information matrix
* \param _node_state_blocks the NodeStateBlocks (Frame,Capture,Landmark,Sensor) containing the factored state block
* \param _key the key of the factored state block
* \param _processor_ptr processor that created the factor
* \param _apply_loss_function if the factor should have loss function
* \param _status factor status
*
*/
FactorQuaternionAbsolute(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_state_blocks,
const char& _key,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE)
: FactorAutodiff("FactorQuaternionAbsolute",
TOP_ABS,
_measurement,
_measurement_sqrt_information_upper,
{_node_state_blocks},
_processor_ptr,
{_node_state_blocks->getStateBlock(_key)},
_apply_loss_function,
_status)
{
assert(_node_state_blocks->getStateBlock(_key) &&
"FactorQuaternionAbsolute: _node_state_blocks does not have _key");
assert(std::dynamic_pointer_cast<StateQuaternion>(_node_state_blocks->getStateBlock(_key)) &&
"FactorQuaternionAbsolute: state block should be of type StateQuaternion");
MatrixSizeCheck<4, 1>::check(_measurement);
MatrixSizeCheck<3, 3>::check(_measurement_sqrt_information_upper);
}
~FactorQuaternionAbsolute() override = default;
template <typename T>
bool operator()(const T* const _o, T* _residuals) const;
};
template<typename T>
inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residuals) const
template <typename T>
inline bool FactorQuaternionAbsolute::operator()(const T* const _o, T* _residuals) const
{
// states
Eigen::Quaternion<T> q1(_o);
Eigen::Quaternion<T> q1(_o);
// measurements
Eigen::Quaterniond q2(getMeasurement().data() + 0); //q_measured
Eigen::Quaterniond q2(getMeasurement().data() + 0); // q_measured
/* error
* to compute the difference between both quaternions, we do
* to compute the difference between both quaternions, we do
* diff = log(q2 * q1.conj)
* isolating q2 we get
* isolating q2 we get
* q2 = exp(diff) * q1 ==> exp on the left means global.
*
* In rotations.h, we have
* minus(q1,q2) = minus_right(q1,q2) = log_q(q1.conjugate() * q2); --> this is a local 'minus'
* minus_left(q1,q2) = log_q(q2 * q1.conjugate()); --> this is a global 'minus'
*/
*/
Eigen::Matrix<T, 3, 1> er;
er = minus_left( q1, q2.cast<T>() );
er = minus_left(q1, q2.cast<T>());
// residual
Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
......@@ -102,6 +104,4 @@ inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residua
return true;
}
} // namespace wolf
#endif
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// 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 FACTOR_RELATIVE_POSE_2d_H_
#define FACTOR_RELATIVE_POSE_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/factor/factor_analytic.h"
#include "core/landmark/landmark_base.h"
#include "core/math/rotations.h"
#include <Eigen/StdVector>
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorRelativePose2d);
//class
// class
class FactorRelativePose2d : public FactorAnalytic
{
public:
/** \brief Constructor of category FAC_FRAME
**/
FactorRelativePose2d(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorRelativePose2d",
_top,
_ftr_ptr,
_frame_other_ptr,
nullptr,
nullptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_frame_other_ptr->getP(),
_frame_other_ptr->getO(),
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO())
{
//
}
/** \brief Constructor of category FAC_FEATURE
**/
FactorRelativePose2d(const FeatureBasePtr& _ftr_ptr,
const FeatureBasePtr& _ftr_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorRelativePose2d",
_top,
_ftr_ptr,
nullptr,
nullptr,
_ftr_other_ptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_ftr_other_ptr->getFrame()->getP(),
_ftr_other_ptr->getFrame()->getO() )
{
//
}
/** \brief Constructor of category FAC_LANDMARK
**/
FactorRelativePose2d(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorRelativePose2d",
_top,
_ftr_ptr,
nullptr,
nullptr,
nullptr,
_landmark_other_ptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_landmark_other_ptr->getP(),
_landmark_other_ptr->getO())
{
//
}
~FactorRelativePose2d() override = default;
/** \brief Returns the factor residual size
**/
unsigned int getSize() const override
{
return 3;
}
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in a std::vector of mapped Eigen::VectorXd
**/
Eigen::VectorXd evaluateResiduals(
const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const override;
public:
/** \brief Constructor
**/
FactorRelativePose2d(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_reference,
const NodeStateBlocksPtr& _node_target,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE)
: FactorAnalytic(
"FactorRelativePose2d",
_top,
_measurement,
_measurement_sqrt_information_upper,
{_node_reference, _node_target},
_processor_ptr,
{_node_reference->getP(), _node_reference->getO(), _node_target->getP(), _node_target->getO()},
_apply_loss_function,
_status)
{
assert(_node_reference->getP() && "FactorRelativePose2d: _node_reference state P not found!");
assert(_node_reference->getO() && "FactorRelativePose2d: _node_reference state O not found!");
assert(_node_target->getP() && "FactorRelativePose2d: _node_target state P not found!");
assert(_node_target->getO() && "FactorRelativePose2d: _node_target state O not found!");
MatrixSizeCheck<3, 1>::check(_measurement);
MatrixSizeCheck<3, 3>::check(_measurement_sqrt_information_upper);
}
/** \brief Returns the jacobians evaluated in the states provided
*
* Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
~FactorRelativePose2d() override = default;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
*
**/
void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
/** \brief Returns the factor residual size
**/
unsigned int getSize() const override
{
return 3;
}
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in a std::vector of mapped Eigen::VectorXd
**/
Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector) const override;
/** \brief Returns the jacobians evaluated in the states provided
*
* Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state
*block \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd>>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state
*block
*
**/
void evaluateUnweightedJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
};
/// IMPLEMENTATION ///
inline Eigen::VectorXd FactorRelativePose2d::evaluateResiduals(
const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector) const
{
Eigen::VectorXd residual(3);
Eigen::VectorXd expected_measurement(3);
// Expected measurement
Eigen::Matrix2d R = Eigen::Rotation2D<double>(-_st_vector[1](0)).matrix();
expected_measurement.head(2) = R * (_st_vector[2] - _st_vector[0]); // rotar menys l'angle de primer (-_o1)
expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0);
Eigen::Matrix2d R = Eigen::Rotation2D<double>(-_st_vector[1](0)).matrix();
expected_measurement.head(2) = R * (_st_vector[2] - _st_vector[0]); // rotar menys l'angle de primer (-_o1)
expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0);
// Residual
residual = expected_measurement - getMeasurement();
residual = expected_measurement - getMeasurement();
residual(2) = pi2pi(residual(2));
residual = getMeasurementSquareRootInformationUpper() * residual;
residual = getMeasurementSquareRootInformationUpper() * residual;
return residual;
}
inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd>>& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
assert(jacobians.size() == 4);
......@@ -184,34 +129,43 @@ inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map
if (_compute_jacobian[0])
{
jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,2) <<
-cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
sin(_st_vector[1](0)), -cos(_st_vector[1](0)),
0, 0).finished();
jacobians[0] =
getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3, 2) << -cos(_st_vector[1](0)),
-sin(_st_vector[1](0)),
sin(_st_vector[1](0)),
-cos(_st_vector[1](0)),
0,
0)
.finished();
}
if (_compute_jacobian[1])
{
jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,1) <<
-(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) + (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
-(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) - (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)),
-1).finished();
jacobians[1] = getMeasurementSquareRootInformationUpper() *
(Eigen::MatrixRowXd(3, 1) << -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) +
(_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
-(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) -
(_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)),
-1)
.finished();
}
if (_compute_jacobian[2])
{
jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,2) <<
cos(_st_vector[1](0)), sin(_st_vector[1](0)),
-sin(_st_vector[1](0)), cos(_st_vector[1](0)),
0, 0).finished();
jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3, 2) << cos(_st_vector[1](0)),
sin(_st_vector[1](0)),
-sin(_st_vector[1](0)),
cos(_st_vector[1](0)),
0,
0)
.finished();
}
if (_compute_jacobian[3])
{
jacobians[3] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,1) <<
0, 0, 1).finished();
jacobians[3] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3, 1) << 0, 0, 1).finished();
}
}
inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
assert(jacobians.size() == 4);
......@@ -220,60 +174,74 @@ inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map
if (_compute_jacobian[0])
{
jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,2) <<
-cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
sin(_st_vector[1](0)), -cos(_st_vector[1](0)),
0, 0).finished();
jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3, 2) << -cos(_st_vector[1](0)),
-sin(_st_vector[1](0)),
sin(_st_vector[1](0)),
-cos(_st_vector[1](0)),
0,
0)
.finished();
}
if (_compute_jacobian[1])
{
jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,1) <<
-(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) + (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
-(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) - (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)),
-1).finished();
jacobians[1] = getMeasurementSquareRootInformationUpper() *
(Eigen::MatrixXd(3, 1) << -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) +
(_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
-(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) -
(_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)),
-1)
.finished();
}
if (_compute_jacobian[2])
{
jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,2) <<
cos(_st_vector[1](0)), sin(_st_vector[1](0)),
-sin(_st_vector[1](0)), cos(_st_vector[1](0)),
0, 0).finished();
jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3, 2) << cos(_st_vector[1](0)),
sin(_st_vector[1](0)),
-sin(_st_vector[1](0)),
cos(_st_vector[1](0)),
0,
0)
.finished();
}
if (_compute_jacobian[3])
{
jacobians[3] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,1) <<
0, 0, 1).finished();
jacobians[3] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3, 1) << 0, 0, 1).finished();
}
}
inline void FactorRelativePose2d::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
inline void FactorRelativePose2d::evaluateUnweightedJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
{
assert(jacobians.size() == 4);
jacobians[0] = (Eigen::MatrixXd(3,2) <<
-cos(getStateBlockPtrVector()[1]->getState()(0)), -sin(getStateBlockPtrVector()[1]->getState()(0)),
sin(getStateBlockPtrVector()[1]->getState()(0)), -cos(getStateBlockPtrVector()[1]->getState()(0)),
0, 0).finished();
jacobians[1] = (Eigen::MatrixXd(3,1) <<
-(getStateBlockPtrVector()[2]->getState()(0)
- getStateBlockPtrVector()[0]->getState()(0)) * sin(getStateBlockPtrVector()[1]->getState()(0))
+ (getStateBlockPtrVector()[2]->getState()(1)
- getStateBlockPtrVector()[0]->getState()(1)) * cos(getStateBlockPtrVector()[1]->getState()(0)),
-(getStateBlockPtrVector()[2]->getState()(0)
- getStateBlockPtrVector()[0]->getState()(0)) * cos(getStateBlockPtrVector()[1]->getState()(0))
- (getStateBlockPtrVector()[2]->getState()(1)
- getStateBlockPtrVector()[0]->getState()(1)) * sin(getStateBlockPtrVector()[1]->getState()(0)),
-1).finished();
jacobians[2] = (Eigen::MatrixXd(3,2) <<
cos(getStateBlockPtrVector()[1]->getState()(0)), sin(getStateBlockPtrVector()[1]->getState()(0)),
-sin(getStateBlockPtrVector()[1]->getState()(0)), cos(getStateBlockPtrVector()[1]->getState()(0)),
0, 0).finished();
jacobians[3] = (Eigen::MatrixXd(3,1) << 0, 0, 1).finished();
jacobians[0] = (Eigen::MatrixXd(3, 2) << -cos(getStateBlockPtrVector()[1]->getState()(0)),
-sin(getStateBlockPtrVector()[1]->getState()(0)),
sin(getStateBlockPtrVector()[1]->getState()(0)),
-cos(getStateBlockPtrVector()[1]->getState()(0)),
0,
0)
.finished();
jacobians[1] = (Eigen::MatrixXd(3, 1)
<< -(getStateBlockPtrVector()[2]->getState()(0) - getStateBlockPtrVector()[0]->getState()(0)) *
sin(getStateBlockPtrVector()[1]->getState()(0)) +
(getStateBlockPtrVector()[2]->getState()(1) -
getStateBlockPtrVector()[0]->getState()(1)) *
cos(getStateBlockPtrVector()[1]->getState()(0)),
-(getStateBlockPtrVector()[2]->getState()(0) - getStateBlockPtrVector()[0]->getState()(0)) *
cos(getStateBlockPtrVector()[1]->getState()(0)) -
(getStateBlockPtrVector()[2]->getState()(1) - getStateBlockPtrVector()[0]->getState()(1)) *
sin(getStateBlockPtrVector()[1]->getState()(0)),
-1)
.finished();
jacobians[2] = (Eigen::MatrixXd(3, 2) << cos(getStateBlockPtrVector()[1]->getState()(0)),
sin(getStateBlockPtrVector()[1]->getState()(0)),
-sin(getStateBlockPtrVector()[1]->getState()(0)),
cos(getStateBlockPtrVector()[1]->getState()(0)),
0,
0)
.finished();
jacobians[3] = (Eigen::MatrixXd(3, 1) << 0, 0, 1).finished();
}
} // namespace wolf
#endif
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// 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 FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_H_
#define FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_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/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/math/rotations.h"
//#include "ceres/jet.h"
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorRelativePose2dWithExtrinsics);
//class
class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>
// class
class FactorRelativePose2dWithExtrinsics
: public FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>
{
public:
/** \brief Class constructor Frame-Frame
*/
FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
/** \brief Class constructor Frame-Landmark
*/
FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _lmk_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top = TOP_LMK,
FactorStatus _status = FAC_ACTIVE);
~FactorRelativePose2dWithExtrinsics() override = default;
template<typename T>
bool operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _o_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
Eigen::Vector3d residual() const;
public:
/** \brief Class constructor
*/
FactorRelativePose2dWithExtrinsics(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_reference,
const NodeStateBlocksPtr& _node_target,
const SensorBasePtr& _sensor,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
~FactorRelativePose2dWithExtrinsics() override = default;
template <typename T>
bool operator()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _o_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
};
inline FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff("FactorRelativePose2dWithExtrinsics",
_top,
_ftr_ptr,
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_frame_other_ptr->getP(),
_frame_other_ptr->getO(),
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_ftr_ptr->getCapture()->getSensorP(),
_ftr_ptr->getCapture()->getSensorO())
inline FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(
const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_reference,
const NodeStateBlocksPtr& _node_target,
const SensorBasePtr& _sensor,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status)
: FactorAutodiff("FactorRelativePose2dWithExtrinsics",
_top,
_measurement,
_measurement_sqrt_information_upper,
{_node_reference, _node_target, _sensor},
_processor_ptr,
{_node_reference->getP(),
_node_reference->getO(),
_node_target->getP(),
_node_target->getO(),
_sensor->getP(),
_sensor->getO()},
_apply_loss_function,
_status)
{
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
//
assert(_node_reference->getP() && "FactorRelativePose2dWithExtrinsics: _node_reference state P not found!");
assert(_node_reference->getO() && "FactorRelativePose2dWithExtrinsics: _node_reference state O not found!");
assert(_node_target->getP() && "FactorRelativePose2dWithExtrinsics: _node_target state P not found!");
assert(_node_target->getO() && "FactorRelativePose2dWithExtrinsics: _node_target state O not found!");
assert(_sensor->getP() && "FactorRelativePose2dWithExtrinsics: _sensor state P not found!");
assert(_sensor->getP() && "FactorRelativePose2dWithExtrinsics: _sensor state P not found!");
assert(not _sensor->isStateBlockDynamic('P') and not _sensor->isStateBlockDynamic('O') &&
"FactorRelativePose2dWithExtrinsics: extrinsics cannot be dynamic!");
MatrixSizeCheck<3, 1>::check(_measurement);
MatrixSizeCheck<3, 3>::check(_measurement_sqrt_information_upper);
}
inline FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _lmk_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff("FactorRelativePose2dWithExtrinsics",
_top,
_ftr_ptr,
nullptr, nullptr, nullptr, _lmk_other_ptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_lmk_other_ptr->getP(),
_lmk_other_ptr->getO(),
_ftr_ptr->getCapture()->getSensorP(),
_ftr_ptr->getCapture()->getSensorO())
template <typename T>
inline bool FactorRelativePose2dWithExtrinsics::operator()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _o_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const
{
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
//
}
template<typename T>
inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _o_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const
{
// MAPS
Eigen::Map<Eigen::Matrix<T,3,1> > res(_residuals);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_target(_p_target);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_sensor(_p_sensor);
const T& o_ref = _o_ref[0];
const T& o_target = _o_target[0];
const T& o_sensor = _o_sensor[0];
Eigen::Map<Eigen::Matrix<T, 3, 1> > res(_residuals);
Eigen::Map<const Eigen::Matrix<T, 2, 1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Matrix<T, 2, 1> > p_target(_p_target);
Eigen::Map<const Eigen::Matrix<T, 2, 1> > p_sensor(_p_sensor);
const T& o_ref = _o_ref[0];
const T& o_target = _o_target[0];
const T& o_sensor = _o_sensor[0];
Eigen::Matrix<T, 3, 1> expected_measurement;
Eigen::Matrix<T, 3, 1> er; // error
Eigen::Matrix<T, 3, 1> er; // error
// FRAME CASE
if (not getFrameOtherList().empty())
// FRAME-FRAME CASE
if (getFramesFactored().size() == 2)
{
// Expected measurement
// r1_p_s1 = p_sensor
......@@ -156,11 +129,14 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re
// ----------------------------------------
// s1_p_s2 = s1_R_r1*(r1_R_w*(w_p_r2 + w_R_r2*r2_p_s2 - w_p_r1) - r1_p_s1)
expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target + Eigen::Rotation2D<T>(o_target)*p_sensor));
expected_measurement.head(2) =
Eigen::Rotation2D<T>(-o_sensor) *
(-p_sensor +
Eigen::Rotation2D<T>(-o_ref) * (-p_ref + p_target + Eigen::Rotation2D<T>(o_target) * p_sensor));
expected_measurement(2) = o_target - o_ref;
}
// LANDMARK CASE
else if (not getLandmarkOtherList().empty())
// FRAME-LANDMARK CASE
else if (getLandmarksFactored().size() == 1)
{
// Expected measurement
// r1_p_s1 = p_sensor
......@@ -174,12 +150,16 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re
// ----------------------------------------
// s1_p_s2 = s1_R_r1*(r1_R_w*(w_p_r2 - w_p_r1) - r1_p_s1)
expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target));
expected_measurement.head(2) =
Eigen::Rotation2D<T>(-o_sensor) * (-p_sensor + Eigen::Rotation2D<T>(-o_ref) * (-p_ref + p_target));
expected_measurement(2) = o_target - o_ref - o_sensor;
}
else
throw std::runtime_error(
"FactorRelativePose2dWithExtrinsics::operator(): unknown case! not FRAME-FRAME or FRAME-LANDMARK");
// Error
er = expected_measurement - getMeasurement().cast<T>();
er = expected_measurement - getMeasurement().cast<T>();
er(2) = pi2pi(er(2));
// Residuals
......@@ -187,53 +167,24 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
// using ceres::Jet;
// Eigen::MatrixXs J(3,6);
// J.row(0) = ((Jet<Scalar, 6>)(er(0))).v;
// J.row(1) = ((Jet<Scalar, 6>)(er(1))).v;
// J.row(2) = ((Jet<Scalar, 6>)(er(2))).v;
// J.row(0) = ((Jet<Scalar, 6>)(res(0))).v;
// J.row(1) = ((Jet<Scalar, 6>)(res(1))).v;
// J.row(2) = ((Jet<Scalar, 6>)(res(2))).v;
// if (sizeof(er(0)) != sizeof(double))
// {
// std::cout << "FactorRelativePose2dWithExtrinsics::Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "FactorRelativePose2dWithExtrinsics::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "FactorRelativePose2dWithExtrinsics::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
// }
// using ceres::Jet;
// Eigen::MatrixXs J(3,6);
// J.row(0) = ((Jet<Scalar, 6>)(er(0))).v;
// J.row(1) = ((Jet<Scalar, 6>)(er(1))).v;
// J.row(2) = ((Jet<Scalar, 6>)(er(2))).v;
// J.row(0) = ((Jet<Scalar, 6>)(res(0))).v;
// J.row(1) = ((Jet<Scalar, 6>)(res(1))).v;
// J.row(2) = ((Jet<Scalar, 6>)(res(2))).v;
// if (sizeof(er(0)) != sizeof(double))
// {
// std::cout << "FactorRelativePose2dWithExtrinsics::Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "FactorRelativePose2dWithExtrinsics::Weighted Jacobian(c" << id() << ") = \n " << J <<
// std::endl; std::cout << "FactorRelativePose2dWithExtrinsics::Sqrt Info(c" << id() << ") = \n" <<
// getMeasurementSquareRootInformationUpper() << std::endl;
// }
////////////////////////////////////////////////////////
return true;
}
inline Eigen::Vector3d FactorRelativePose2dWithExtrinsics::residual() const
{
Eigen::Vector3d res;
Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_target;
p_sensor = getCapture()->getSensorP()->getState();
o_sensor = getCapture()->getSensorO()->getState();
// FRAME CASE
if (not getFrameOtherList().empty())
{
p_ref = getFrameOther()->getP()->getState();
o_ref = getFrameOther()->getO()->getState();
p_target = getCapture()->getFrame()->getP()->getState();
o_target = getCapture()->getFrame()->getO()->getState();
}
// LANDMARK CASE
else if (not getLandmarkOtherList().empty())
{
p_ref = getCapture()->getFrame()->getP()->getState();
o_ref = getCapture()->getFrame()->getO()->getState();
p_target = getLandmarkOther()->getP()->getState();
o_target = getLandmarkOther()->getO()->getState();
}
operator() (p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data());
return res;
}
} // namespace wolf
#endif
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// 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 FACTOR_RELATIVE_POSE_3D_H_
#define FACTOR_RELATIVE_POSE_3D_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/factor/factor_autodiff.h"
#include "core/math/rotations.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorRelativePose3d);
//class
class FactorRelativePose3d : public FactorAutodiff<FactorRelativePose3d,6,3,4,3,4>
{
public:
FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
const FrameBasePtr& _frame_past_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
const LandmarkBasePtr& _landmark_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
~FactorRelativePose3d() override = default;
template<typename T>
bool operator ()(const T* const _p_target,
const T* const _q_target,
const T* const _p_ref,
const T* const _q_ref,
T* _residuals) const;
template<typename T>
bool expectation(const T* const _p_target,
const T* const _q_target,
const T* const _p_ref,
const T* const _q_ref,
T* _expectation_dp,
T* _expectation_dq) const;
Eigen::Vector7d expectation() const;
private:
template<typename T>
void printRes(const Eigen::Matrix<T, 6, 1>& r) const;
std::string topology_;
// class
class FactorRelativePose3d : public FactorAutodiff<FactorRelativePose3d, 6, 3, 4, 3, 4>
{
public:
FactorRelativePose3d(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_reference,
const NodeStateBlocksPtr& _node_target,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
~FactorRelativePose3d() override = default;
template <typename T>
bool operator()(const T* const _p_target,
const T* const _q_target,
const T* const _p_ref,
const T* const _q_ref,
T* _residuals) const;
template <typename T>
bool expectation(const T* const _p_target,
const T* const _q_target,
const T* const _p_ref,
const T* const _q_ref,
T* _expectation_dp,
T* _expectation_dq) const;
Eigen::Vector7d expectation() const;
private:
template <typename T>
void printRes(const Eigen::Matrix<T, 6, 1>& r) const;
std::string topology_;
};
template<typename T>
template <typename T>
inline void FactorRelativePose3d::printRes(const Eigen::Matrix<T, 6, 1>& r) const
{
std::cout << "Jet residual = " << std::endl;
......@@ -87,104 +75,80 @@ inline void FactorRelativePose3d::printRes(const Eigen::Matrix<T, 6, 1>& r) cons
std::cout << r(5) << std::endl;
}
template<>
inline void FactorRelativePose3d::printRes (const Eigen::Matrix<double,6,1> & r) const
template <>
inline void FactorRelativePose3d::printRes(const Eigen::Matrix<double, 6, 1>& r) const
{
std::cout << "Scalar residual = " << std::endl;
std::cout << r.transpose() << std::endl;
}
inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
const FrameBasePtr& _frame_past_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff<FactorRelativePose3d, 6, 3, 4, 3, 4>("FactorRelativePose3d", // type
_top, // topology
_ftr_current_ptr, // feature
_frame_past_ptr, // frame other
nullptr, // capture other
nullptr, // feature other
nullptr, // landmark other
_processor_ptr, // processor
_apply_loss_function,
_status,
_ftr_current_ptr->getFrame()->getP(), // current frame P
_ftr_current_ptr->getFrame()->getO(), // current frame Q
_frame_past_ptr->getP(), // past frame P
_frame_past_ptr->getO()) // past frame Q
{
MatrixSizeCheck<7,1>::check(_ftr_current_ptr->getMeasurement());
MatrixSizeCheck<6,6>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
}
inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
const LandmarkBasePtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff<FactorRelativePose3d, 6, 3, 4, 3, 4>("FactorRelativePose3d", // type
_top, // topology
_ftr_current_ptr, // feature
nullptr, // frame other
nullptr, // capture other
nullptr, // feature other
_lmk_ptr, // landmark other
_processor_ptr, // processor
_apply_loss_function,
_status,
_lmk_ptr->getP(), // landmark P
_lmk_ptr->getO(), // landmark Q
_ftr_current_ptr->getFrame()->getP(), // current frame P
_ftr_current_ptr->getFrame()->getO()) // current frame Q
inline FactorRelativePose3d::FactorRelativePose3d(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_reference,
const NodeStateBlocksPtr& _node_target,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status)
: FactorAutodiff("FactorRelativePose3d",
_top,
_measurement,
_measurement_sqrt_information_upper,
{_node_reference, _node_target},
_processor_ptr,
{_node_reference->getP(), _node_reference->getO(), _node_target->getP(), _node_target->getO()},
_apply_loss_function,
_status)
{
MatrixSizeCheck<7,1>::check(_ftr_current_ptr->getMeasurement());
MatrixSizeCheck<6,6>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
assert(_node_reference->getP() && "FactorRelativePose3d: _node_reference state P not found!");
assert(_node_reference->getO() && "FactorRelativePose3d: _node_reference state O not found!");
assert(_node_target->getP() && "FactorRelativePose3d: _node_target state P not found!");
assert(_node_target->getO() && "FactorRelativePose3d: _node_target state O not found!");
MatrixSizeCheck<7, 1>::check(_measurement);
MatrixSizeCheck<6, 6>::check(_measurement_sqrt_information_upper);
}
template<typename T>
inline bool FactorRelativePose3d::expectation(const T* const _p_target,
const T* const _q_target,
const T* const _p_ref,
template <typename T>
inline bool FactorRelativePose3d::expectation(const T* const _p_ref,
const T* const _q_ref,
T* _expectation_dp,
T* _expectation_dq) const
const T* const _p_target,
const T* const _q_target,
T* _expectation_dp,
T* _expectation_dq) const
{
Eigen::Map<const Eigen::Matrix<T,3,1> > p_target(_p_target);
Eigen::Map<const Eigen::Quaternion<T> > q_target(_q_target);
Eigen::Map<const Eigen::Matrix<T,3,1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Quaternion<T> > q_ref(_q_ref);
Eigen::Map<Eigen::Matrix<T,3,1> > expectation_dp(_expectation_dp);
Eigen::Map<Eigen::Quaternion<T> > expectation_dq(_expectation_dq);
// std::cout << "p_target: " << p_target(0) << " "
// << p_target(1) << " "
// << p_target(2) << "\n";
// std::cout << "q_target: " << q_target.coeffs()(0) << " "
// << q_target.coeffs()(1) << " "
// << q_target.coeffs()(2) << " "
// << q_target.coeffs()(3) << "\n";
// std::cout << "p_ref: " << p_ref(0) << " "
// << p_ref(1) << " "
// << p_ref(2) << "\n";
// std::cout << "q_ref: " << q_ref.coeffs()(0) << " "
// << q_ref.coeffs()(1) << " "
// << q_ref.coeffs()(2) << " "
// << q_ref.coeffs()(3) << "\n";
Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_target(_p_target);
Eigen::Map<const Eigen::Quaternion<T> > q_target(_q_target);
Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Quaternion<T> > q_ref(_q_ref);
Eigen::Map<Eigen::Matrix<T, 3, 1> > expectation_dp(_expectation_dp);
Eigen::Map<Eigen::Quaternion<T> > expectation_dq(_expectation_dq);
// std::cout << "p_target: " << p_target(0) << " "
// << p_target(1) << " "
// << p_target(2) << "\n";
// std::cout << "q_target: " << q_target.coeffs()(0) << " "
// << q_target.coeffs()(1) << " "
// << q_target.coeffs()(2) << " "
// << q_target.coeffs()(3) << "\n";
// std::cout << "p_ref: " << p_ref(0) << " "
// << p_ref(1) << " "
// << p_ref(2) << "\n";
// std::cout << "q_ref: " << q_ref.coeffs()(0) << " "
// << q_ref.coeffs()(1) << " "
// << q_ref.coeffs()(2) << " "
// << q_ref.coeffs()(3) << "\n";
// estimate motion increment, dp, dq;
expectation_dp = q_ref.conjugate() * (p_target - p_ref);
expectation_dq = q_ref.conjugate() * q_target;
// std::cout << "exp_p: " << expectation_dp(0) << " "
// << expectation_dp(1) << " "
// << expectation_dp(2) << "\n";
// std::cout << "exp_q: " << expectation_dq.coeffs()(0) << " "
// << expectation_dq.coeffs()(1) << " "
// << expectation_dq.coeffs()(2) << " "
// << expectation_dq.coeffs()(3) << "\n";
// std::cout << "exp_p: " << expectation_dp(0) << " "
// << expectation_dp(1) << " "
// << expectation_dp(2) << "\n";
// std::cout << "exp_q: " << expectation_dq.coeffs()(0) << " "
// << expectation_dq.coeffs()(1) << " "
// << expectation_dq.coeffs()(2) << " "
// << expectation_dq.coeffs()(3) << "\n";
return true;
}
......@@ -192,37 +156,39 @@ inline bool FactorRelativePose3d::expectation(const T* const _p_target,
inline Eigen::Vector7d FactorRelativePose3d::expectation() const
{
Eigen::Vector7d exp;
auto frm_current = getFeature()->getFrame();
auto frm_past = getFrameOther();
auto lmk = getLandmarkOther();
const Eigen::VectorXd target_pos = ( frm_past ? frm_current->getP()->getState() : lmk->getP()->getState());
const Eigen::VectorXd target_ori = ( frm_past ? frm_current->getO()->getState() : lmk->getO()->getState());
const Eigen::VectorXd ref_pos = ( frm_past ? frm_past->getP()->getState() : frm_current->getP()->getState());
const Eigen::VectorXd ref_ori = ( frm_past ? frm_past->getO()->getState() : frm_current->getO()->getState());
// std::cout << "target_pos: " << target_pos.transpose() << std::endl;
// std::cout << "target_ori: " << target_ori.coeffs().transpose() << std::endl;
// std::cout << "ref_pos: " << ref_pos.transpose() << std::endl;
// std::cout << "ref_ori: " << ref_ori.coeffs().transpose() << std::endl;
expectation(target_pos.data(),
target_ori.data(),
ref_pos.data(),
ref_ori.data(),
// auto frm_current = getFeature()->getFrame();
// auto frm_past = getFrameOther();
// auto lmk = getLandmarkOther();
// const Eigen::VectorXd target_pos = (frm_past ? frm_current->getP()->getState() : lmk->getP()->getState());
// const Eigen::VectorXd target_ori = (frm_past ? frm_current->getO()->getState() : lmk->getO()->getState());
// const Eigen::VectorXd ref_pos = (frm_past ? frm_past->getP()->getState() : frm_current->getP()->getState());
// const Eigen::VectorXd ref_ori = (frm_past ? frm_past->getO()->getState() : frm_current->getO()->getState());
// std::cout << "target_pos: " << target_pos.transpose() << std::endl;
// std::cout << "target_ori: " << target_ori.coeffs().transpose() << std::endl;
// std::cout << "ref_pos: " << ref_pos.transpose() << std::endl;
// std::cout << "ref_ori: " << ref_ori.coeffs().transpose() << std::endl;
// expectation(target_pos.data(), target_ori.data(), ref_pos.data(), ref_ori.data(), exp.data(), exp.data() + 3);
expectation(state_block_vector_[0]->getStateData(),
state_block_vector_[1]->getStateData(),
state_block_vector_[2]->getStateData(),
state_block_vector_[3]->getStateData(),
exp.data(),
exp.data()+3);
exp.data() + 3);
return exp;
}
template<typename T>
inline bool FactorRelativePose3d::operator ()(const T* const _p_target,
const T* const _q_target,
const T* const _p_ref,
const T* const _q_ref,
T* _residuals) const
template <typename T>
inline bool FactorRelativePose3d::operator()(const T* const _p_ref,
const T* const _q_ref,
const T* const _p_target,
const T* const _q_target,
T* _residuals) const
{
/* Residual expression
* -------------------
*
......@@ -259,24 +225,25 @@ inline bool FactorRelativePose3d::operator ()(const T* const _p_target,
* x_j = x_current
*/
Eigen::Map<Eigen::Matrix<T,6,1> > residuals(_residuals);
Eigen::Map<Eigen::Matrix<T, 6, 1> > residuals(_residuals);
Eigen::Matrix<T, Eigen::Dynamic, 1> expected(7) ;
expectation(_p_target, _q_target, _p_ref, _q_ref, expected.data(), expected.data()+3);
Eigen::Matrix<T, Eigen::Dynamic, 1> expected(7);
expectation(_p_ref, _q_ref, _p_target, _q_target, expected.data(), expected.data() + 3);
// measured motion increment, dp_m, dq_m
// Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
auto& dp_m = getMeasurement().head<3>();
auto& dp_m = getMeasurement().head<3>();
Eigen::Quaternion<T> dq_m(getMeasurement().tail<4>().cast<T>());
Eigen::Matrix<T,3,1> dp = expected.head(3);
Eigen::Quaternion<T> dq;
Eigen::Matrix<T, 3, 1> dp = expected.head(3);
Eigen::Quaternion<T> dq;
dq.x() = expected(3);
dq.y() = expected(4);
dq.z() = expected(5);
dq.w() = expected(6);
residuals.head(3) = dp_m - dp; // being a residual, rotating it has no implications, so we skip the product by dq.conj
residuals.head(3) =
dp_m - dp; // being a residual, rotating it has no implications, so we skip the product by dq.conj
residuals.tail(3) = q2v(dq.conjugate() * dq_m);
residuals = getMeasurementSquareRootInformationUpper() * residuals;
......@@ -285,5 +252,3 @@ inline bool FactorRelativePose3d::operator ()(const T* const _p_target,
}
} /* namespace wolf */
#endif /* FACTOR_RELATIVE_POSE_3D_H_ */
//--------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 _FACTOR_TELATIVE_POSE_3D_WITH_EXTRINSICS
#define _FACTOR_TELATIVE_POSE_3D_WITH_EXTRINSICS
// 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_state_blocks.h"
#include "core/math/rotations.h"
#include "core/factor/factor_autodiff.h"
#include "core/sensor/sensor_base.h"
#include "core/landmark/landmark_base.h"
#include "core/feature/feature_base.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorRelativePose3dWithExtrinsics);
class FactorRelativePose3dWithExtrinsics : public FactorAutodiff<FactorRelativePose3dWithExtrinsics, 6, 3, 4, 3, 4, 3, 4>
class FactorRelativePose3dWithExtrinsics
: public FactorAutodiff<FactorRelativePose3dWithExtrinsics, 6, 3, 4, 3, 4, 3, 4>
{
public:
/** \brief Class constructor Frame-Frame
*/
FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
/** \brief Class constructor Frame-Landmark
*/
FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top = TOP_LMK,
FactorStatus _status = FAC_ACTIVE);
/** \brief Class Destructor
*/
~FactorRelativePose3dWithExtrinsics() override = default;
/** brief : compute the residual from the state blocks being iterated by the solver.
**/
template<typename T>
bool operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _o_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
Eigen::Vector6d residual() const;
double cost() const;
// print function only for double (not Jet)
template<typename T, int Rows, int Cols>
void print(int kf, int lmk, const std::string s, const Eigen::Matrix<T, Rows, Cols> _M) const
{
// jet prints nothing
}
template<int Rows, int Cols>
void print(int kf, int lmk, const std::string s, const Eigen::Matrix<double, Rows, Cols> _M) const
{
// double prints stuff
WOLF_TRACE("KF", kf, " L", lmk, "; ", s, _M);
}
public:
/** \brief Class constructor
*/
FactorRelativePose3dWithExtrinsics(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_reference,
const NodeStateBlocksPtr& _node_target,
const SensorBasePtr& _sensor,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
/** \brief Class Destructor
*/
~FactorRelativePose3dWithExtrinsics() override = default;
/** brief : compute the residual from the state blocks being iterated by the solver.
**/
template <typename T>
bool operator()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _o_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
// print function only for double (not Jet)
template <typename T, int Rows, int Cols>
void print(int kf, int lmk, const std::string s, const Eigen::Matrix<T, Rows, Cols> _M) const
{
// jet prints nothing
}
template <int Rows, int Cols>
void print(int kf, int lmk, const std::string s, const Eigen::Matrix<double, Rows, Cols> _M) const
{
// double prints stuff
WOLF_TRACE("KF", kf, " L", lmk, "; ", s, _M);
}
};
} // namespace wolf
namespace wolf
{
} // namespace wolf
inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff("FactorRelativePose3dWithExtrinsics",
_top,
_feature_ptr,
_frame_other_ptr,
nullptr,
nullptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_frame_other_ptr->getP(),
_frame_other_ptr->getO(),
_feature_ptr->getFrame()->getP(),
_feature_ptr->getFrame()->getO(),
_feature_ptr->getCapture()->getSensorP(),
_feature_ptr->getCapture()->getSensorO())
namespace wolf
{
assert(_feature_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_feature_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
}
inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff("FactorRelativePose3dWithExtrinsics",
_top,
_feature_ptr,
nullptr,
nullptr,
nullptr,
_landmark_other_ptr,
_processor_ptr,
_apply_loss_function,
_status,
_feature_ptr->getFrame()->getP(),
_feature_ptr->getFrame()->getO(),
_landmark_other_ptr->getP(),
_landmark_other_ptr->getO(),
_feature_ptr->getCapture()->getSensorP(),
_feature_ptr->getCapture()->getSensorO())
inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(
const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_reference,
const NodeStateBlocksPtr& _node_target,
const SensorBasePtr& _sensor,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status)
: FactorAutodiff("FactorRelativePose3dWithExtrinsics",
_top,
_measurement,
_measurement_sqrt_information_upper,
{_node_reference, _node_target, _sensor},
_processor_ptr,
{_node_reference->getP(),
_node_reference->getO(),
_node_target->getP(),
_node_target->getO(),
_sensor->getP(),
_sensor->getO()},
_apply_loss_function,
_status)
{
assert(_node_reference->getP() && "FactorRelativePose3dWithExtrinsics: _node_reference state P not found!");
assert(_node_reference->getO() && "FactorRelativePose3dWithExtrinsics: _node_reference state O not found!");
assert(_node_target->getP() && "FactorRelativePose3dWithExtrinsics: _node_target state P not found!");
assert(_node_target->getO() && "FactorRelativePose3dWithExtrinsics: _node_target state O not found!");
assert(_sensor->getP() && "FactorRelativePose3dWithExtrinsics: _sensor state P not found!");
assert(_sensor->getP() && "FactorRelativePose3dWithExtrinsics: _sensor state P not found!");
assert(not _sensor->isStateBlockDynamic('P') and not _sensor->isStateBlockDynamic('O') &&
"FactorRelativePose3dWithExtrinsics: extrinsics cannot be dynamic!");
MatrixSizeCheck<7, 1>::check(_measurement);
MatrixSizeCheck<6, 6>::check(_measurement_sqrt_information_upper);
}
template<typename T>
inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _o_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const
template <typename T>
inline bool FactorRelativePose3dWithExtrinsics::operator()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _o_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const
{
// Maps
Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_s(_p_sensor);
Eigen::Map<const Eigen::Quaternion<T>> q_r_s(_o_sensor);
Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_ref);
Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_ref);
Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_t(_p_target);
Eigen::Map<const Eigen::Quaternion<T>> q_w_t(_o_target);
Eigen::Map<Eigen::Matrix<T,6,1>> residuals(_residuals);
Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_r_s(_p_sensor);
Eigen::Map<const Eigen::Quaternion<T>> q_r_s(_o_sensor);
Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_w_r(_p_ref);
Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_ref);
Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_w_t(_p_target);
Eigen::Map<const Eigen::Quaternion<T>> q_w_t(_o_target);
Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(_residuals);
// WOLF_INFO("p_sensor: ", p_r_s.transpose());
// WOLF_INFO("o_sensor: ", q_r_s.coeffs().transpose());
......@@ -171,33 +141,39 @@ inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_re
// WOLF_INFO("o_target: ", q_w_t.coeffs().transpose());
// Expected measurement
Eigen::Quaternion<T> expected_q;
Eigen::Matrix<T,3,1> expected_p;
// FRAME CASE
if (not getFrameOtherList().empty())
Eigen::Quaternion<T> expected_q;
Eigen::Matrix<T, 3, 1> expected_p;
// FRAME-FRAME CASE
if (getFramesFactored().size() == 2)
{
expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t + q_w_t * p_r_s - p_w_r) - p_r_s);
expected_q = q_r_s.conjugate() * q_w_r.conjugate() * q_w_t * q_r_s;
}
// LANDMARK CASE
else if (not getLandmarkOtherList().empty())
// FRAME-LANDMARK CASE
else if (getLandmarksFactored().size() == 1)
{
expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t - p_w_r) - p_r_s);
expected_q = q_r_s.conjugate() * q_w_r.conjugate() * q_w_t;
}
else
throw std::runtime_error(
"FactorRelativePose3dWithExtrinsics::operator(): unknown case! not FRAME-FRAME or FRAME-LANDMARK");
// Measurement
Eigen::Vector3d p_meas(getMeasurement().head<3>());
Eigen::Quaterniond q_c_l_meas(getMeasurement().data() + 3 );
Eigen::Quaterniond q_c_l_meas(getMeasurement().data() + 3);
Eigen::Quaternion<T> q_l_c_meas = q_c_l_meas.conjugate().cast<T>();
//Eigen::Matrix<T,3,1> p_l_c_meas = -q_l_c_meas * p_c_l_meas.cast<T>();
// Eigen::Matrix<T,3,1> p_l_c_meas = -q_l_c_meas * p_c_l_meas.cast<T>();
// Error
Eigen::Matrix<T, 6, 1> err;
// err.head(3) = q_l_c_meas * (p_meas.cast<T>() - expected_p);
err.head(3) = (p_meas - expected_p);
err.tail(3) = wolf::log_q(q_l_c_meas * expected_q); // true error function for which the covariance should be computed
//err.tail(3) = T(2)*(q_l_c_meas * expected_q).vec(); // 1rst order approximation of sin function ( 2*sin(aa/2) ~ aa )
err.tail(3) =
wolf::log_q(q_l_c_meas * expected_q); // true error function for which the covariance should be computed
// err.tail(3) = T(2)*(q_l_c_meas * expected_q).vec(); // 1rst order approximation of sin function (
// 2*sin(aa/2) ~ aa )
// Residual
residuals = getMeasurementSquareRootInformationUpper().cast<T>() * err;
......@@ -205,39 +181,4 @@ inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_re
return true;
}
inline Eigen::Vector6d FactorRelativePose3dWithExtrinsics::residual() const
{
Eigen::Vector6d res;
Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_target;
p_sensor = getCapture()->getSensorP()->getState();
o_sensor = getCapture()->getSensorO()->getState();
// FRAME CASE
if (not getFrameOtherList().empty())
{
p_ref = getFrameOther()->getP()->getState();
o_ref = getFrameOther()->getO()->getState();
p_target = getCapture()->getFrame()->getP()->getState();
o_target = getCapture()->getFrame()->getO()->getState();
}
// LANDMARK CASE
else if (not getLandmarkOtherList().empty())
{
p_ref = getCapture()->getFrame()->getP()->getState();
o_ref = getCapture()->getFrame()->getO()->getState();
p_target = getLandmarkOther()->getP()->getState();
o_target = getLandmarkOther()->getO()->getState();
}
operator() (p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data());
return res;
}
inline double FactorRelativePose3dWithExtrinsics::cost() const
{
return residual().squaredNorm();
}
} // namespace wolf
#endif /* _FACTOR_TELATIVE_POSE_3D_WITH_EXTRINSICS */
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// 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
// Wolf includes
#include "core/factor/factor_analytic.h"
#include "core/landmark/landmark_base.h"
#include "core/math/rotations.h"
#include <Eigen/StdVector>
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorRelativePosition2d);
//class
// class
class FactorRelativePosition2d : public FactorAnalytic
{
public:
/** \brief Constructor of category FAC_FRAME
**/
FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorRelativePosition2d",
_top,
_ftr_ptr,
_frame_other_ptr,
nullptr,
nullptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_frame_other_ptr->getP(),
_frame_other_ptr->getO(),
_ftr_ptr->getFrame()->getP())
{
//
}
/** \brief Constructor of category FAC_FEATURE
**/
FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
const FeatureBasePtr& _ftr_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorRelativePosition2d",
_top,
_ftr_ptr,
nullptr,
nullptr,
_ftr_other_ptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_ftr_other_ptr->getFrame()->getP() )
{
//
}
/** \brief Constructor of category FAC_LANDMARK
**/
FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorRelativePosition2d",
_top,
_ftr_ptr,
nullptr,
nullptr,
nullptr,
_landmark_other_ptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_landmark_other_ptr->getP())
{
//
}
~FactorRelativePosition2d() override = default;
/** \brief Returns the factor residual size
**/
unsigned int getSize() const override
{
return 2;
}
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in a std::vector of mapped Eigen::VectorXd
**/
Eigen::VectorXd evaluateResiduals(
const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const override;
public:
/** \brief Constructor of category FAC_FRAME
**/
FactorRelativePosition2d(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_reference,
const NodeStateBlocksPtr& _node_target,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE)
: FactorAnalytic("FactorRelativePosition2d",
_top,
_measurement,
_measurement_sqrt_information_upper,
{_node_reference, _node_target},
_processor_ptr,
{_node_reference->getP(), _node_reference->getO(), _node_target->getP()},
_apply_loss_function,
_status)
{
assert(_node_reference->getP() && "FactorRelativePosition2d: _node_reference state P not found!");
assert(_node_reference->getO() && "FactorRelativePosition2d: _node_reference state O not found!");
assert(_node_target->getP() && "FactorRelativePosition2d: _node_target state P not found!");
MatrixSizeCheck<2, 1>::check(_measurement);
MatrixSizeCheck<2, 2>::check(_measurement_sqrt_information_upper);
}
/** \brief Returns the jacobians evaluated in the states provided
*
* Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
~FactorRelativePosition2d() override = default;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
*
**/
void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
/** \brief Returns the factor residual size
**/
unsigned int getSize() const override
{
return 2;
}
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in a std::vector of mapped Eigen::VectorXd
**/
Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector) const override;
/** \brief Returns the jacobians evaluated in the states provided
*
* Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state
*block \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd>>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state
*block
*
**/
void evaluateUnweightedJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
};
/// IMPLEMENTATION ///
inline Eigen::VectorXd FactorRelativePosition2d::evaluateResiduals(
const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector) const
{
Eigen::VectorXd residual(2);
Eigen::VectorXd expected_measurement(2);
// Expected measurement
Eigen::Matrix2d R = Eigen::Rotation2D<double>(-_st_vector[1](0)).matrix();
expected_measurement = R * (_st_vector[2] - _st_vector[0]); // rotar menys l'angle de primer (-_o1)
Eigen::Matrix2d R = Eigen::Rotation2D<double>(-_st_vector[1](0)).matrix();
expected_measurement = R * (_st_vector[2] - _st_vector[0]); // rotar menys l'angle de primer (-_o1)
// Residual
residual = expected_measurement - getMeasurement();
residual = getMeasurementSquareRootInformationUpper() * residual;
return residual;
}
inline void FactorRelativePosition2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const
inline void FactorRelativePosition2d::evaluateJacobians(
const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd>>& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
assert(jacobians.size() == 3);
assert(_st_vector.size() == 3);
......@@ -178,29 +126,36 @@ inline void FactorRelativePosition2d::evaluateJacobians(const std::vector<Eigen:
if (_compute_jacobian[0])
{
jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
-cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
sin(_st_vector[1](0)), -cos(_st_vector[1](0))).finished();
jacobians[0] =
getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2, 2) << -cos(_st_vector[1](0)),
-sin(_st_vector[1](0)),
sin(_st_vector[1](0)),
-cos(_st_vector[1](0)))
.finished();
}
if (_compute_jacobian[1])
{
jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,1) <<
-(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) +
(_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
-(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) -
(_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0))).finished();
jacobians[1] = getMeasurementSquareRootInformationUpper() *
(Eigen::MatrixRowXd(2, 1) << -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) +
(_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
-(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) -
(_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)))
.finished();
}
if (_compute_jacobian[2])
{
jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
cos(_st_vector[1](0)), sin(_st_vector[1](0)),
-sin(_st_vector[1](0)), cos(_st_vector[1](0))).finished();
jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2, 2) << cos(_st_vector[1](0)),
sin(_st_vector[1](0)),
-sin(_st_vector[1](0)),
cos(_st_vector[1](0)))
.finished();
}
}
inline void FactorRelativePosition2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const
inline void FactorRelativePosition2d::evaluateJacobians(
const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
assert(jacobians.size() == 3);
assert(_st_vector.size() == 3);
......@@ -208,51 +163,59 @@ inline void FactorRelativePosition2d::evaluateJacobians(const std::vector<Eigen:
if (_compute_jacobian[0])
{
jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
-cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
sin(_st_vector[1](0)), -cos(_st_vector[1](0))).finished();
jacobians[0] =
getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2, 2) << -cos(_st_vector[1](0)),
-sin(_st_vector[1](0)),
sin(_st_vector[1](0)),
-cos(_st_vector[1](0)))
.finished();
}
if (_compute_jacobian[1])
{
jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,1) <<
-(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) +
(_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
-(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) -
(_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0))).finished();
jacobians[1] = getMeasurementSquareRootInformationUpper() *
(Eigen::MatrixRowXd(2, 1) << -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) +
(_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
-(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) -
(_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)))
.finished();
}
if (_compute_jacobian[2])
{
jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
cos(_st_vector[1](0)), sin(_st_vector[1](0)),
-sin(_st_vector[1](0)), cos(_st_vector[1](0))).finished();
jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2, 2) << cos(_st_vector[1](0)),
sin(_st_vector[1](0)),
-sin(_st_vector[1](0)),
cos(_st_vector[1](0)))
.finished();
}
}
inline void FactorRelativePosition2d::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
inline void FactorRelativePosition2d::evaluateUnweightedJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
{
assert(jacobians.size() == 3);
jacobians[0] = (Eigen::MatrixXd(2,2) <<
-cos(getStateBlockPtrVector()[1]->getState()(0)),
jacobians[0] = (Eigen::MatrixXd(2, 2) << -cos(getStateBlockPtrVector()[1]->getState()(0)),
-sin(getStateBlockPtrVector()[1]->getState()(0)),
sin(getStateBlockPtrVector()[1]->getState()(0)),
-cos(getStateBlockPtrVector()[1]->getState()(0))).finished();
jacobians[1] = (Eigen::MatrixXd(2,1) <<
-(getStateBlockPtrVector()[2]->getState()(0)
- getStateBlockPtrVector()[0]->getState()(0)) * sin(getStateBlockPtrVector()[1]->getState()(0))
+ (getStateBlockPtrVector()[2]->getState()(1)
- getStateBlockPtrVector()[0]->getState()(1)) * cos(getStateBlockPtrVector()[1]->getState()(0)),
-(getStateBlockPtrVector()[2]->getState()(0)
- getStateBlockPtrVector()[0]->getState()(0)) * cos(getStateBlockPtrVector()[1]->getState()(0))
- (getStateBlockPtrVector()[2]->getState()(1)
- getStateBlockPtrVector()[0]->getState()(1)) * sin(getStateBlockPtrVector()[1]->getState()(0))).finished();
jacobians[2] = (Eigen::MatrixXd(2,2) <<
cos(getStateBlockPtrVector()[1]->getState()(0)),
sin(getStateBlockPtrVector()[1]->getState()(0)),
sin(getStateBlockPtrVector()[1]->getState()(0)),
-cos(getStateBlockPtrVector()[1]->getState()(0)))
.finished();
jacobians[1] = (Eigen::MatrixXd(2, 1)
<< -(getStateBlockPtrVector()[2]->getState()(0) - getStateBlockPtrVector()[0]->getState()(0)) *
sin(getStateBlockPtrVector()[1]->getState()(0)) +
(getStateBlockPtrVector()[2]->getState()(1) -
getStateBlockPtrVector()[0]->getState()(1)) *
cos(getStateBlockPtrVector()[1]->getState()(0)),
-(getStateBlockPtrVector()[2]->getState()(0) - getStateBlockPtrVector()[0]->getState()(0)) *
cos(getStateBlockPtrVector()[1]->getState()(0)) -
(getStateBlockPtrVector()[2]->getState()(1) - getStateBlockPtrVector()[0]->getState()(1)) *
sin(getStateBlockPtrVector()[1]->getState()(0)))
.finished();
jacobians[2] = (Eigen::MatrixXd(2, 2) << cos(getStateBlockPtrVector()[1]->getState()(0)),
sin(getStateBlockPtrVector()[1]->getState()(0)),
-sin(getStateBlockPtrVector()[1]->getState()(0)),
cos(getStateBlockPtrVector()[1]->getState()(0))).finished();
cos(getStateBlockPtrVector()[1]->getState()(0)))
.finished();
}
} // namespace wolf
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// 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
// Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/math/rotations.h"
//#include "ceres/jet.h"
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorRelativePosition2dWithExtrinsics);
//class
class FactorRelativePosition2dWithExtrinsics : public FactorAutodiff<FactorRelativePosition2dWithExtrinsics, 2, 2, 1, 2, 2, 1>
// class
class FactorRelativePosition2dWithExtrinsics
: public FactorAutodiff<FactorRelativePosition2dWithExtrinsics, 2, 2, 1, 2, 2, 1>
{
public:
/** \brief Class constructor Frame-Landmark
*/
FactorRelativePosition2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _lmk_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top = TOP_LMK,
FactorStatus _status = FAC_ACTIVE);
public:
/** \brief Class constructor Frame-Landmark
*/
FactorRelativePosition2dWithExtrinsics(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const FrameBasePtr& _frame_reference,
const LandmarkBasePtr& _lmk_target,
const SensorBasePtr& _sensor,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
~FactorRelativePosition2dWithExtrinsics() override = default;
~FactorRelativePosition2dWithExtrinsics() override = default;
template<typename T>
bool operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
template <typename T>
bool operator()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
};
inline FactorRelativePosition2dWithExtrinsics::FactorRelativePosition2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _lmk_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff("FactorRelativePosition2dWithExtrinsics",
_top,
_ftr_ptr,
nullptr, nullptr, nullptr, _lmk_other_ptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_lmk_other_ptr->getP(),
_ftr_ptr->getCapture()->getSensorP(),
_ftr_ptr->getCapture()->getSensorO())
inline FactorRelativePosition2dWithExtrinsics::FactorRelativePosition2dWithExtrinsics(
const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const FrameBasePtr& _frame_reference,
const LandmarkBasePtr& _lmk_target,
const SensorBasePtr& _sensor,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status)
: FactorAutodiff(
"FactorRelativePosition2dWithExtrinsics",
TOP_LMK,
_measurement,
_measurement_sqrt_information_upper,
{_frame_reference, _lmk_target, _sensor},
_processor_ptr,
{_frame_reference->getP(), _frame_reference->getO(), _lmk_target->getP(), _sensor->getP(), _sensor->getO()},
_apply_loss_function,
_status)
{
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
//
assert(_frame_reference->getP() && "FactorRelativePosition2dWithExtrinsics: _frame_reference state P not found!");
assert(_frame_reference->getO() && "FactorRelativePosition2dWithExtrinsics: _frame_reference state O not found!");
assert(_lmk_target->getP() && "FactorRelativePosition2dWithExtrinsics: _lmk_target state P not found!");
assert(_sensor->getP() && "FactorRelativePosition2dWithExtrinsics: _sensor state P not found!");
assert(_sensor->getP() && "FactorRelativePosition2dWithExtrinsics: _sensor state P not found!");
assert(not _sensor->isStateBlockDynamic('P') and not _sensor->isStateBlockDynamic('O') &&
"FactorRelativePosition2dWithExtrinsics: extrinsics cannot be dynamic!");
MatrixSizeCheck<2, 1>::check(_measurement);
MatrixSizeCheck<2, 2>::check(_measurement_sqrt_information_upper);
}
template<typename T>
inline bool FactorRelativePosition2dWithExtrinsics::operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const
template <typename T>
inline bool FactorRelativePosition2dWithExtrinsics::operator()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const
{
// MAPS
Eigen::Map<Eigen::Matrix<T,2,1> > res(_residuals);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_target(_p_target);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_sensor(_p_sensor);
T o_ref = _o_ref[0];
T o_sensor = _o_sensor[0];
Eigen::Map<Eigen::Matrix<T, 2, 1> > res(_residuals);
Eigen::Map<const Eigen::Matrix<T, 2, 1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Matrix<T, 2, 1> > p_target(_p_target);
Eigen::Map<const Eigen::Matrix<T, 2, 1> > p_sensor(_p_sensor);
T o_ref = _o_ref[0];
T o_sensor = _o_sensor[0];
// Expected measurement
Eigen::Matrix<T, 2, 1> expected_measurement = Eigen::Rotation2D<T>(-o_sensor) *
(-p_sensor + Eigen::Rotation2D<T>(-o_ref) * (p_target - p_ref));
Eigen::Matrix<T, 2, 1> expected_measurement =
Eigen::Rotation2D<T>(-o_sensor) * (-p_sensor + Eigen::Rotation2D<T>(-o_ref) * (p_target - p_ref));
// Residuals
res = getMeasurementSquareRootInformationUpper() * (expected_measurement - getMeasurement());
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
// using ceres::Jet;
// Eigen::MatrixXs J(3,6);
// J.row(0) = ((Jet<Scalar, 6>)(er(0))).v;
// J.row(1) = ((Jet<Scalar, 6>)(er(1))).v;
// J.row(2) = ((Jet<Scalar, 6>)(er(2))).v;
// J.row(0) = ((Jet<Scalar, 6>)(res(0))).v;
// J.row(1) = ((Jet<Scalar, 6>)(res(1))).v;
// J.row(2) = ((Jet<Scalar, 6>)(res(2))).v;
// if (sizeof(er(0)) != sizeof(double))
// {
// std::cout << "FactorRelativePosition2dWithExtrinsics::Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "FactorRelativePosition2dWithExtrinsics::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "FactorRelativePosition2dWithExtrinsics::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
// }
// using ceres::Jet;
// Eigen::MatrixXs J(3,6);
// J.row(0) = ((Jet<Scalar, 6>)(er(0))).v;
// J.row(1) = ((Jet<Scalar, 6>)(er(1))).v;
// J.row(2) = ((Jet<Scalar, 6>)(er(2))).v;
// J.row(0) = ((Jet<Scalar, 6>)(res(0))).v;
// J.row(1) = ((Jet<Scalar, 6>)(res(1))).v;
// J.row(2) = ((Jet<Scalar, 6>)(res(2))).v;
// if (sizeof(er(0)) != sizeof(double))
// {
// std::cout << "FactorRelativePosition2dWithExtrinsics::Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "FactorRelativePosition2dWithExtrinsics::Weighted Jacobian(c" << id() << ") = \n " << J <<
// std::endl; std::cout << "FactorRelativePosition2dWithExtrinsics::Sqrt Info(c" << id() << ") = \n" <<
// getMeasurementSquareRootInformationUpper() << std::endl;
// }
////////////////////////////////////////////////////////
return true;
}
} // namespace wolf
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// 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
#include "core/factor/factor_autodiff.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorRelativePosition3d);
//class
class FactorRelativePosition3d : public FactorAutodiff<FactorRelativePosition3d,3,3,4,3>
{
public:
FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
const FrameBasePtr& _frame_past_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
const LandmarkBasePtr& _landmark_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
~FactorRelativePosition3d() override = default;
template<typename T>
bool operator ()(const T* const _p_ref,
const T* const _q_ref,
const T* const _p_target,
T* _residuals) const;
private:
template<typename T>
void printRes(const Eigen::Matrix<T, 3, 1>& r) const;
// class
class FactorRelativePosition3d : public FactorAutodiff<FactorRelativePosition3d, 3, 3, 4, 3>
{
public:
FactorRelativePosition3d(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_reference,
const NodeStateBlocksPtr& _node_target,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
~FactorRelativePosition3d() override = default;
template <typename T>
bool operator()(const T* const _p_ref, const T* const _q_ref, const T* const _p_target, T* _residuals) const;
private:
template <typename T>
void printRes(const Eigen::Matrix<T, 3, 1>& r) const;
};
template<typename T>
template <typename T>
inline void FactorRelativePosition3d::printRes(const Eigen::Matrix<T, 3, 1>& r) const
{
std::cout << "Jet residual = " << std::endl;
......@@ -69,84 +56,62 @@ inline void FactorRelativePosition3d::printRes(const Eigen::Matrix<T, 3, 1>& r)
std::cout << r(2) << std::endl;
}
template<>
inline void FactorRelativePosition3d::printRes (const Eigen::Matrix<double,3,1> & r) const
template <>
inline void FactorRelativePosition3d::printRes(const Eigen::Matrix<double, 3, 1>& r) const
{
std::cout << "Scalar residual = " << std::endl;
std::cout << r.transpose() << std::endl;
}
inline FactorRelativePosition3d::FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
const FrameBasePtr& _frame_past_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff("FactorRelativePosition3d", // type
_top, // topology
_ftr_current_ptr, // feature
_frame_past_ptr, // frame other
nullptr, // capture other
nullptr, // feature other
nullptr, // landmark other
_processor_ptr, // processor
_apply_loss_function,
_status,
_frame_past_ptr->getP(), // past frame P
_frame_past_ptr->getO(), // past frame Q
_ftr_current_ptr->getFrame()->getP()) // current frame P
inline FactorRelativePosition3d::FactorRelativePosition3d(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtr& _node_reference,
const NodeStateBlocksPtr& _node_target,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status)
: FactorAutodiff("FactorRelativePosition3d",
_top,
_measurement,
_measurement_sqrt_information_upper,
{_node_reference, _node_target},
_processor_ptr,
{_node_reference->getP(), _node_reference->getO(), _node_target->getP()},
_apply_loss_function,
_status)
{
MatrixSizeCheck<3,1>::check(_ftr_current_ptr->getMeasurement());
MatrixSizeCheck<3,3>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
assert(_node_reference->getP() && "FactorRelativePosition3d: _node_reference state P not found!");
assert(_node_reference->getO() && "FactorRelativePosition3d: _node_reference state O not found!");
assert(_node_target->getP() && "FactorRelativePosition3d: _node_target state P not found!");
MatrixSizeCheck<3, 1>::check(_measurement);
MatrixSizeCheck<3, 3>::check(_measurement_sqrt_information_upper);
}
inline FactorRelativePosition3d::FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
const LandmarkBasePtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff("FactorRelativePosition3d", // type
_top, // topology
_ftr_current_ptr, // feature
nullptr, // frame other
nullptr, // capture other
nullptr, // feature other
_lmk_ptr, // landmark other
_processor_ptr, // processor
_apply_loss_function,
_status,
_ftr_current_ptr->getFrame()->getP(), // frame P
_ftr_current_ptr->getFrame()->getO(), // frame Q
_lmk_ptr->getP()) // landmark P
template <typename T>
inline bool FactorRelativePosition3d::operator()(const T* const _p_ref,
const T* const _q_ref,
const T* const _p_target,
T* _residuals) const
{
MatrixSizeCheck<3,1>::check(_ftr_current_ptr->getMeasurement());
MatrixSizeCheck<3,3>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
}
template<typename T>
inline bool FactorRelativePosition3d::operator ()(const T* const _p_ref,
const T* const _q_ref,
const T* const _p_target,
T* _residuals) const
{
Eigen::Map<const Eigen::Matrix<T,3,1> > p_target(_p_target);
Eigen::Map<const Eigen::Matrix<T,3,1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Quaternion<T> > q_ref(_q_ref);
Eigen::Map<Eigen::Matrix<T,3,1> > residuals(_residuals);
// std::cout << "p_ref: " << p_ref(0) << " "
// << p_ref(1) << " "
// << p_ref(2) << "\n";
// std::cout << "q_ref: " << q_ref.coeffs()(0) << " "
// << q_ref.coeffs()(1) << " "
// << q_ref.coeffs()(2) << " "
// << q_ref.coeffs()(3) << "\n";
// std::cout << "p_target: " << p_target(0) << " "
// << p_target(1) << " "
// << p_target(2) << "\n";
residuals = getMeasurementSquareRootInformationUpper() * (getMeasurement() - (q_ref.conjugate() * (p_target - p_ref)));
Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_target(_p_target);
Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Quaternion<T> > q_ref(_q_ref);
Eigen::Map<Eigen::Matrix<T, 3, 1> > residuals(_residuals);
// std::cout << "p_ref: " << p_ref(0) << " "
// << p_ref(1) << " "
// << p_ref(2) << "\n";
// std::cout << "q_ref: " << q_ref.coeffs()(0) << " "
// << q_ref.coeffs()(1) << " "
// << q_ref.coeffs()(2) << " "
// << q_ref.coeffs()(3) << "\n";
// std::cout << "p_target: " << p_target(0) << " "
// << p_target(1) << " "
// << p_target(2) << "\n";
residuals =
getMeasurementSquareRootInformationUpper() * (getMeasurement() - (q_ref.conjugate() * (p_target - p_ref)));
return true;
}
......
//--------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
// Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/math/rotations.h"
//#include "ceres/jet.h"
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorRelativePosition3dWithExtrinsics);
//class
class FactorRelativePosition3dWithExtrinsics : public FactorAutodiff<FactorRelativePosition3dWithExtrinsics, 3, 3, 4, 3, 3, 4>
// class
class FactorRelativePosition3dWithExtrinsics
: public FactorAutodiff<FactorRelativePosition3dWithExtrinsics, 3, 3, 4, 3, 3, 4>
{
public:
/** \brief Class constructor Frame-Landmark
*/
FactorRelativePosition3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _lmk_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top = TOP_LMK,
FactorStatus _status = FAC_ACTIVE);
~FactorRelativePosition3dWithExtrinsics() override = default;
template<typename T>
bool operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
Eigen::Vector3d residual() const;
double cost() const;
public:
/** \brief Class constructor Frame-Landmark
*/
FactorRelativePosition3dWithExtrinsics(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const FrameBasePtr& _frame_reference,
const LandmarkBasePtr& _lmk_target,
const SensorBasePtr& _sensor,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
~FactorRelativePosition3dWithExtrinsics() override = default;
template <typename T>
bool operator()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
};
inline FactorRelativePosition3dWithExtrinsics::FactorRelativePosition3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _lmk_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff("FactorRelativePosition3dWithExtrinsics",
_top,
_ftr_ptr,
nullptr, nullptr, nullptr, _lmk_other_ptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_lmk_other_ptr->getP(),
_ftr_ptr->getCapture()->getSensorP(),
_ftr_ptr->getCapture()->getSensorO())
inline FactorRelativePosition3dWithExtrinsics::FactorRelativePosition3dWithExtrinsics(
const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const FrameBasePtr& _frame_reference,
const LandmarkBasePtr& _lmk_target,
const SensorBasePtr& _sensor,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status)
: FactorAutodiff(
"FactorRelativePosition3dWithExtrinsics",
TOP_LMK,
_measurement,
_measurement_sqrt_information_upper,
{_frame_reference, _lmk_target, _sensor},
_processor_ptr,
{_frame_reference->getP(), _frame_reference->getO(), _lmk_target->getP(), _sensor->getP(), _sensor->getO()},
_apply_loss_function,
_status)
{
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
assert(_frame_reference->getP() && "FactorRelativePosition3dWithExtrinsics: _frame_reference state P not found!");
assert(_frame_reference->getO() && "FactorRelativePosition3dWithExtrinsics: _frame_reference state O not found!");
assert(_lmk_target->getP() && "FactorRelativePosition3dWithExtrinsics: _lmk_target state P not found!");
assert(_sensor->getP() && "FactorRelativePosition3dWithExtrinsics: _sensor state P not found!");
assert(_sensor->getP() && "FactorRelativePosition3dWithExtrinsics: _sensor state P not found!");
assert(not _sensor->isStateBlockDynamic('P') and not _sensor->isStateBlockDynamic('O') &&
"FactorRelativePosition3dWithExtrinsics: extrinsics cannot be dynamic!");
MatrixSizeCheck<3, 1>::check(_measurement);
MatrixSizeCheck<3, 3>::check(_measurement_sqrt_information_upper);
}
template<typename T>
inline bool FactorRelativePosition3dWithExtrinsics::operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const
template <typename T>
inline bool FactorRelativePosition3dWithExtrinsics::operator()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const
{
// Maps
Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_s(_p_sensor);
Eigen::Map<const Eigen::Quaternion<T>> q_r_s(_o_sensor);
Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_ref);
Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_ref);
Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_t(_p_target);
Eigen::Map<Eigen::Matrix<T,3,1>> residuals(_residuals);
Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_r_s(_p_sensor);
Eigen::Map<const Eigen::Quaternion<T>> q_r_s(_o_sensor);
Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_w_r(_p_ref);
Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_ref);
Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_w_t(_p_target);
Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals(_residuals);
// WOLF_INFO("p_sensor: ", p_r_s.transpose());
// WOLF_INFO("o_sensor: ", q_r_s.coeffs().transpose());
......@@ -106,7 +107,7 @@ inline bool FactorRelativePosition3dWithExtrinsics::operator ()(const T* const _
// WOLF_INFO("p_target: ", p_w_t.transpose());
// Expected measurement
Eigen::Matrix<T,3,1> expected_p;
Eigen::Matrix<T, 3, 1> expected_p;
expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t - p_w_r) - p_r_s);
// Residual
......@@ -115,35 +116,4 @@ inline bool FactorRelativePosition3dWithExtrinsics::operator ()(const T* const _
return true;
}
inline Eigen::Vector3d FactorRelativePosition3dWithExtrinsics::residual() const
{
Eigen::Vector3d res;
Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target;
p_sensor = getCapture()->getSensorP()->getState();
o_sensor = getCapture()->getSensorO()->getState();
// FRAME CASE
if (not getFrameOtherList().empty())
{
p_ref = getFrameOther()->getP()->getState();
o_ref = getFrameOther()->getO()->getState();
p_target = getCapture()->getFrame()->getP()->getState();
}
// LANDMARK CASE
else if (not getLandmarkOtherList().empty())
{
p_ref = getCapture()->getFrame()->getP()->getState();
o_ref = getCapture()->getFrame()->getO()->getState();
p_target = getLandmarkOther()->getP()->getState();
}
operator() (p_ref.data(), o_ref.data(), p_target.data(), p_sensor.data(), o_sensor.data(), res.data());
return res;
}
inline double FactorRelativePosition3dWithExtrinsics::cost() const
{
return residual().squaredNorm();
}
} // namespace wolf
} // namespace wolf