From cd71e44f18894b224bbd6a712e866f0ed6c9b10f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Thu, 16 Jan 2020 11:49:23 +0100 Subject: [PATCH] local param using rowmajor map for Jacobians --- .../state_block/local_parametrization_angle.h | 4 ++-- .../state_block/local_parametrization_base.h | 2 +- .../local_parametrization_homogeneous.h | 2 +- .../local_parametrization_quaternion.h | 2 +- .../local_parametrization_wrapper.cpp | 2 +- .../local_parametrization_homogeneous.cpp | 2 +- .../local_parametrization_quaternion.cpp | 20 +++++++++---------- test/gtest_local_param.cpp | 6 +++--- 8 files changed, 20 insertions(+), 20 deletions(-) diff --git a/include/core/state_block/local_parametrization_angle.h b/include/core/state_block/local_parametrization_angle.h index 6c063da65..4cec6ba23 100644 --- a/include/core/state_block/local_parametrization_angle.h +++ b/include/core/state_block/local_parametrization_angle.h @@ -23,7 +23,7 @@ class LocalParametrizationAngle : public LocalParametrizationBase virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<const Eigen::VectorXd>& _delta, Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const; virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, - Eigen::Map<Eigen::MatrixXd>& _jacobian) const; + Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const; virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1, Eigen::Map<const Eigen::VectorXd>& _x2, Eigen::Map<Eigen::VectorXd>& _x2_minus_x1); @@ -50,7 +50,7 @@ inline bool LocalParametrizationAngle::plus(Eigen::Map<const Eigen::VectorXd>& _ } inline bool LocalParametrizationAngle::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, - Eigen::Map<Eigen::MatrixXd>& _jacobian) const + Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const { _jacobian(0) = 1.0; return true; diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h index e7a8d2200..32599d9dc 100644 --- a/include/core/state_block/local_parametrization_base.h +++ b/include/core/state_block/local_parametrization_base.h @@ -25,7 +25,7 @@ class LocalParametrizationBase{ virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x, Eigen::Map<const Eigen::VectorXd>& _delta, Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0; - virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x, Eigen::Map<Eigen::MatrixXd>& _jacobian) const = 0; + virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x, Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const = 0; virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1, Eigen::Map<const Eigen::VectorXd>& _x2, Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0; diff --git a/include/core/state_block/local_parametrization_homogeneous.h b/include/core/state_block/local_parametrization_homogeneous.h index 31b3a82b9..f91bdacc6 100644 --- a/include/core/state_block/local_parametrization_homogeneous.h +++ b/include/core/state_block/local_parametrization_homogeneous.h @@ -43,7 +43,7 @@ class LocalParametrizationHomogeneous : public LocalParametrizationBase virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<const Eigen::VectorXd>& _delta, Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const; - virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<Eigen::MatrixXd>& _jacobian) const; + virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const; virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _h1, Eigen::Map<const Eigen::VectorXd>& _h2, Eigen::Map<Eigen::VectorXd>& _h2_minus_h1); diff --git a/include/core/state_block/local_parametrization_quaternion.h b/include/core/state_block/local_parametrization_quaternion.h index 32799f565..1d43ace31 100644 --- a/include/core/state_block/local_parametrization_quaternion.h +++ b/include/core/state_block/local_parametrization_quaternion.h @@ -69,7 +69,7 @@ public: Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const; virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q, - Eigen::Map<Eigen::MatrixXd>& _jacobian) const; + Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const; virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _q1, Eigen::Map<const Eigen::VectorXd>& _q2, Eigen::Map<Eigen::VectorXd>& _q2_minus_q1); diff --git a/src/ceres_wrapper/local_parametrization_wrapper.cpp b/src/ceres_wrapper/local_parametrization_wrapper.cpp index 460716197..1cb1425c0 100644 --- a/src/ceres_wrapper/local_parametrization_wrapper.cpp +++ b/src/ceres_wrapper/local_parametrization_wrapper.cpp @@ -13,7 +13,7 @@ bool LocalParametrizationWrapper::Plus(const double* x_raw, const double* delta_ bool LocalParametrizationWrapper::ComputeJacobian(const double* x, double* jacobian) const { Eigen::Map<const Eigen::VectorXd> x_map((double*)x, GlobalSize()); - Eigen::Map<Eigen::MatrixXd> jacobian_map((double*)jacobian, GlobalSize(), LocalSize()); + Eigen::Map<Eigen::MatrixRowXd> jacobian_map((double*)jacobian, GlobalSize(), LocalSize()); return local_parametrization_ptr_->computeJacobian(x_map, jacobian_map); }; diff --git a/src/state_block/local_parametrization_homogeneous.cpp b/src/state_block/local_parametrization_homogeneous.cpp index 8fbf3ce24..b2fa1c111 100644 --- a/src/state_block/local_parametrization_homogeneous.cpp +++ b/src/state_block/local_parametrization_homogeneous.cpp @@ -38,7 +38,7 @@ bool LocalParametrizationHomogeneous::plus(Eigen::Map<const Eigen::VectorXd>& _h } bool LocalParametrizationHomogeneous::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, - Eigen::Map<Eigen::MatrixXd>& _jacobian) const + Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const { assert(_h.size() == global_size_ && "Wrong size of input quaternion."); assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix."); diff --git a/src/state_block/local_parametrization_quaternion.cpp b/src/state_block/local_parametrization_quaternion.cpp index bc9d70273..727fa0de8 100644 --- a/src/state_block/local_parametrization_quaternion.cpp +++ b/src/state_block/local_parametrization_quaternion.cpp @@ -9,8 +9,8 @@ namespace wolf { template <> bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::VectorXd>& _q, - Eigen::Map<const Eigen::VectorXd>& _delta_theta, - Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const + Eigen::Map<const Eigen::VectorXd>& _delta_theta, + Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const { assert(_q.size() == global_size_ && "Wrong size of input quaternion."); @@ -28,7 +28,7 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::Vect template <> bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q, - Eigen::Map<Eigen::MatrixXd>& _jacobian) const + Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const { assert(_q.size() == global_size_ && "Wrong size of input quaternion."); assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix."); @@ -44,8 +44,8 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const template <> bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::VectorXd>& _q1, - Eigen::Map<const Eigen::VectorXd>& _q2, - Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) + Eigen::Map<const Eigen::VectorXd>& _q2, + Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) { assert(_q1.size() == global_size_ && "Wrong size of input quaternion."); assert(_q2.size() == global_size_ && "Wrong size of input quaternion."); @@ -61,8 +61,8 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::Vec template <> bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::VectorXd>& _q, - Eigen::Map<const Eigen::VectorXd>& _delta_theta, - Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const + Eigen::Map<const Eigen::VectorXd>& _delta_theta, + Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const { assert(_q.size() == global_size_ && "Wrong size of input quaternion."); @@ -80,7 +80,7 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::Vec template <> bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q, - Eigen::Map<Eigen::MatrixXd>& _jacobian) const + Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const { assert(_q.size() == global_size_ && "Wrong size of input quaternion."); assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix."); @@ -96,8 +96,8 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const template <> bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::VectorXd>& _q1, - Eigen::Map<const Eigen::VectorXd>& _q2, - Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) + Eigen::Map<const Eigen::VectorXd>& _q2, + Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) { assert(_q1.size() == global_size_ && "Wrong size of input quaternion."); assert(_q2.size() == global_size_ && "Wrong size of input quaternion."); diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp index bcb7d3428..39684eaf1 100644 --- a/test/gtest_local_param.cpp +++ b/test/gtest_local_param.cpp @@ -51,7 +51,7 @@ TEST(TestLocalParametrization, QuaternionLocal) Map<VectorXd> da(&x_storage(4),3); da /= 10.0; Map<VectorXd> qo_m(&x_storage(7),4); - Map<MatrixXd> J(&M_storage(0,0),4,3); + Map<MatrixRowXd> J(&M_storage(0,0),4,3); MatrixXd J_num(4,3); LocalParametrizationQuaternion<DQ_LOCAL> Qpar_loc; @@ -96,7 +96,7 @@ TEST(TestLocalParametrization, QuaternionGlobal) Map<VectorXd> da(&x_storage(4),3); da /= 10.0; Map<VectorXd> qo_m(&x_storage(7),4); - Map<MatrixXd> J(&M_storage(0,0),4,3); + Map<MatrixRowXd> J(&M_storage(0,0),4,3); MatrixXd J_num(4,3); LocalParametrizationQuaternion<DQ_GLOBAL> Qpar_glob; @@ -138,7 +138,7 @@ TEST(TestLocalParametrization, Homogeneous) Map<VectorXd> d(&x_storage(15),3); d << .1,.2,.3; Map<VectorXd> ho_m(&x_storage(18),4); - Map<MatrixXd> J(&M_storage(0,0),4,3); + Map<MatrixRowXd> J(&M_storage(0,0),4,3); MatrixXd J_num(4,3); LocalParametrizationHomogeneous Hpar; -- GitLab