From 1f9720d066e74204e45e2d798c0d1a80dd0ea600 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Mon, 13 Jul 2020 11:32:30 +0200
Subject: [PATCH] Implement isValid w/o tolerance param
---
include/core/state_block/local_parametrization_angle.h | 9 ++++++++-
include/core/state_block/local_parametrization_base.h | 2 +-
.../core/state_block/local_parametrization_homogeneous.h | 1 +
.../core/state_block/local_parametrization_quaternion.h | 6 ++++++
include/core/state_block/state_block.h | 7 ++++++-
src/state_block/local_parametrization_homogeneous.cpp | 4 ++++
6 files changed, 26 insertions(+), 3 deletions(-)
diff --git a/include/core/state_block/local_parametrization_angle.h b/include/core/state_block/local_parametrization_angle.h
index 2236c1d62..a57d0de76 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;
};
inline LocalParametrizationAngle::LocalParametrizationAngle() :
@@ -64,6 +64,13 @@ inline bool LocalParametrizationAngle::minus(Eigen::Map<const Eigen::VectorXd>&
return true;
}
+inline bool LocalParametrizationAngle::isValid(const Eigen::VectorXd& _state)
+{
+ //Any real is a valid angle because we use the pi2pi function. Also
+ //the types don't match. In this case the argument is
+ //Eigen::Map not Eigen::VectorXd
+ return true;
+}
} /* namespace wolf */
#endif /* LOCAL_PARAMETRIZATION_ANGLE_H_ */
diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h
index 14f2ce38f..48417d92f 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;
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 30785bb2c..834a070e7 100644
--- a/include/core/state_block/local_parametrization_homogeneous.h
+++ b/include/core/state_block/local_parametrization_homogeneous.h
@@ -47,6 +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;
};
} // namespace wolf
diff --git a/include/core/state_block/local_parametrization_quaternion.h b/include/core/state_block/local_parametrization_quaternion.h
index 75804f85d..35b905f8d 100644
--- a/include/core/state_block/local_parametrization_quaternion.h
+++ b/include/core/state_block/local_parametrization_quaternion.h
@@ -73,6 +73,12 @@ public:
bool minus(Eigen::Map<const Eigen::VectorXd>& _q1,
Eigen::Map<const Eigen::VectorXd>& _q2,
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
+ {
+ return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < Constants::EPS;
+ }
};
typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionGlobal;
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index c8706e2b6..1773daf16 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -159,6 +159,8 @@ public:
void plus(const Eigen::VectorXd& _dv);
+ bool isValid();
+
static StateBlockPtr create (const Eigen::VectorXd& _state, bool _fixed = false);
};
@@ -319,7 +321,10 @@ inline StateBlockPtr StateBlock::create (const Eigen::VectorXd& _state, bool _fi
{
return std::make_shared<StateBlock>(_state, _fixed);
}
-
+inline bool StateBlock::isValid()
+{
+ return local_param_ptr_ ? local_param_ptr_->isValid(state_) : true;
+}
}// namespace wolf
#endif
diff --git a/src/state_block/local_parametrization_homogeneous.cpp b/src/state_block/local_parametrization_homogeneous.cpp
index b2fa1c111..284360b72 100644
--- a/src/state_block/local_parametrization_homogeneous.cpp
+++ b/src/state_block/local_parametrization_homogeneous.cpp
@@ -60,5 +60,9 @@ bool LocalParametrizationHomogeneous::minus(Eigen::Map<const Eigen::VectorXd>& _
return true;
}
+bool LocalParametrizationHomogeneous::isValid(const Eigen::VectorXd& _state)
+{
+ return _state.size() == global_size_;
+}
} // namespace wolf
--
GitLab