Initialize covs and jacs in ProcMotion constructors, remove from derived.

src/processor/processor_motion.cpp

@@ -67,7 +67,13 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
         jacobian_delta_(delta_cov_size_, delta_cov_size_),
         jacobian_calib_(delta_cov_size_, calib_size_)
 {   
-    //
+    jacobian_delta_preint_          .setIdentity(delta_cov_size_,delta_cov_size_);                                    // dDp'/dDp, dDv'/dDv, all zeros
+    jacobian_delta_                 .setIdentity(delta_cov_size_,delta_cov_size_);                                           //
+    jacobian_calib_                 .setZero(delta_cov_size_,calib_size_);
+    unmeasured_perturbation_cov_ =
+              params_motion_->unmeasured_perturbation_std
+            * params_motion_->unmeasured_perturbation_std
+            * MatrixXd::Identity(delta_cov_size_, delta_cov_size_);
 }
 
 ProcessorMotion::~ProcessorMotion()

src/processor/processor_odom_2d.cpp

@@ -32,7 +32,7 @@ ProcessorOdom2d::ProcessorOdom2d(ParamsProcessorOdom2dPtr _params) :
                 ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params),
                 params_odom_2d_(_params)
 {
-    unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3d::Identity();
+    //
 }
 
 ProcessorOdom2d::~ProcessorOdom2d()

src/processor/processor_odom_3d.cpp

@@ -29,11 +29,7 @@ ProcessorOdom3d::ProcessorOdom3d(ParamsProcessorOdom3dPtr _params) :
                         ProcessorMotion("ProcessorOdom3d", "PO", 3, 7, 7, 6, 6, 0, _params),
                         params_odom_3d_ (_params)
 {
-     // Set constant parts of Jacobians
-    jacobian_delta_preint_.setIdentity(6,6);
-    jacobian_delta_.setIdentity(6,6);
-    jacobian_calib_.setZero(6,0);
-    unmeasured_perturbation_cov_ = pow(params_odom_3d_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix6d::Identity();   
+    //
 }
 
 ProcessorOdom3d::~ProcessorOdom3d()