local param using rowmajor map for Jacobians

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;

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;

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

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

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

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.");

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.");

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;