eigen map to avoid useless copies

include/core/factor/factor_pose_3D.h

@@ -34,8 +34,8 @@ inline bool FactorPose3D::operator ()(const T* const _p, const T* const _o, T* _
 {
 
     // states
-    Eigen::Matrix<T, 3, 1>  p(_p);
-    Eigen::Quaternion<T>    q(_o);
+    Eigen::Map<const Eigen::Matrix<T, 3, 1>>  p(_p);
+    Eigen::Map<const Eigen::Quaternion<T>>    q(_o);
 
     // measurements
     Eigen::Vector3d     p_measured(getMeasurement().data() + 0);