From 3c0555d600027a5c1578f0acbc0a5986bd55ac67 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Wed, 15 Jul 2020 12:29:34 +0200
Subject: [PATCH] Add tolerance to isValid
---
include/core/state_block/local_parametrization_angle.h | 4 ++--
include/core/state_block/local_parametrization_base.h | 2 +-
.../core/state_block/local_parametrization_homogeneous.h | 2 +-
include/core/state_block/local_parametrization_quaternion.h | 4 ++--
include/core/state_block/state_block.h | 6 +++---
src/state_block/local_parametrization_homogeneous.cpp | 2 +-
6 files changed, 10 insertions(+), 10 deletions(-)
diff --git a/include/core/state_block/local_parametrization_angle.h b/include/core/state_block/local_parametrization_angle.h
index a57d0de76..460835cb9 100644
--- a/include/core/state_block/local_parametrization_angle.h
+++ b/include/core/state_block/local_parametrization_angle.h
@@ -27,7 +27,7 @@ class LocalParametrizationAngle : public LocalParametrizationBase
bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
Eigen::Map<const Eigen::VectorXd>& _x2,
Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) override;
- bool isValid(const Eigen::VectorXd& state) override;
+ bool isValid(const Eigen::VectorXd& state, double tolerance) override;
};
inline LocalParametrizationAngle::LocalParametrizationAngle() :
@@ -64,7 +64,7 @@ inline bool LocalParametrizationAngle::minus(Eigen::Map<const Eigen::VectorXd>&
return true;
}
-inline bool LocalParametrizationAngle::isValid(const Eigen::VectorXd& _state)
+inline bool LocalParametrizationAngle::isValid(const Eigen::VectorXd& _state, double tolerance)
{
//Any real is a valid angle because we use the pi2pi function. Also
//the types don't match. In this case the argument is
diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h
index 48417d92f..8494ec3a8 100644
--- a/include/core/state_block/local_parametrization_base.h
+++ b/include/core/state_block/local_parametrization_base.h
@@ -31,7 +31,7 @@ class LocalParametrizationBase{
virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
Eigen::Map<const Eigen::VectorXd>& _x2,
Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
- virtual bool isValid(const Eigen::VectorXd& state) = 0;
+ virtual bool isValid(const Eigen::VectorXd& state, double tolerance) = 0;
unsigned int getLocalSize() const;
unsigned int getGlobalSize() const;
};
diff --git a/include/core/state_block/local_parametrization_homogeneous.h b/include/core/state_block/local_parametrization_homogeneous.h
index 834a070e7..b8bbb981c 100644
--- a/include/core/state_block/local_parametrization_homogeneous.h
+++ b/include/core/state_block/local_parametrization_homogeneous.h
@@ -47,7 +47,7 @@ class LocalParametrizationHomogeneous : public LocalParametrizationBase
bool minus(Eigen::Map<const Eigen::VectorXd>& _h1,
Eigen::Map<const Eigen::VectorXd>& _h2,
Eigen::Map<Eigen::VectorXd>& _h2_minus_h1) override;
- bool isValid(const Eigen::VectorXd& state) override;
+ bool isValid(const Eigen::VectorXd& state, double tolerance) override;
};
} // namespace wolf
diff --git a/include/core/state_block/local_parametrization_quaternion.h b/include/core/state_block/local_parametrization_quaternion.h
index 35b905f8d..c637ab6bb 100644
--- a/include/core/state_block/local_parametrization_quaternion.h
+++ b/include/core/state_block/local_parametrization_quaternion.h
@@ -75,9 +75,9 @@ public:
Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) override;
// inline bool isValid(const Eigen::VectorXd& state) override;
// template<QuaternionDeltaReference DeltaReference>
- bool isValid(const Eigen::VectorXd& _state) override
+ bool isValid(const Eigen::VectorXd& _state, double tolerance) override
{
- return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < Constants::EPS;
+ return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < tolerance;
}
};
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 1773daf16..5406fe0c1 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -159,7 +159,7 @@ public:
void plus(const Eigen::VectorXd& _dv);
- bool isValid();
+ bool isValid(double tolerance = Constants::EPS);
static StateBlockPtr create (const Eigen::VectorXd& _state, bool _fixed = false);
@@ -321,9 +321,9 @@ inline StateBlockPtr StateBlock::create (const Eigen::VectorXd& _state, bool _fi
{
return std::make_shared<StateBlock>(_state, _fixed);
}
-inline bool StateBlock::isValid()
+inline bool StateBlock::isValid(double tolerance)
{
- return local_param_ptr_ ? local_param_ptr_->isValid(state_) : true;
+ return local_param_ptr_ ? local_param_ptr_->isValid(state_, tolerance) : true;
}
}// namespace wolf
diff --git a/src/state_block/local_parametrization_homogeneous.cpp b/src/state_block/local_parametrization_homogeneous.cpp
index 284360b72..846b14a49 100644
--- a/src/state_block/local_parametrization_homogeneous.cpp
+++ b/src/state_block/local_parametrization_homogeneous.cpp
@@ -60,7 +60,7 @@ bool LocalParametrizationHomogeneous::minus(Eigen::Map<const Eigen::VectorXd>& _
return true;
}
-bool LocalParametrizationHomogeneous::isValid(const Eigen::VectorXd& _state)
+bool LocalParametrizationHomogeneous::isValid(const Eigen::VectorXd& _state, double tolerance)
{
return _state.size() == global_size_;
}
--
GitLab