Resolve "Implement a Pose sensor"

Closes #395 (closed)

include/core/factor/factor_pose_3d_with_extrinsics.h

@@ -76,12 +76,6 @@ inline bool FactorPose3dWithExtrinsics::operator ()(const T* const _p,
     err.head(3) = w_p_wm - (w_p_wb + w_q_b*b_p_bm);
     err.tail(3) = q2v((w_q_b * b_q_m).conjugate() * w_q_m.cast<T>());
 
-    std::cout << "\n\n\n\nFAC" << std::endl;
-    std::cout << "w_p_wm: " <<  w_p_wm.transpose() << std::endl;
-    std::cout << "(w_p_wb + w_q_b*b_p_bm): " <<  (w_p_wb + w_q_b*b_p_bm).transpose() << std::endl;
-    std::cout << "b_p_bm: " <<  b_p_bm.transpose() << std::endl;
-    std::cout << "err: " <<  err.transpose() << std::endl;
-
     // Residuals
     Eigen::Map<Eigen::Matrix<T,6,1> > res(_residuals);
     res = getMeasurementSquareRootInformationUpper().cast<T>() * err;