Implement isValid w/o tolerance param

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_ */

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;
 };

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

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;

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

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