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