diff --git a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_constraint.h b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_constraint.h index 032638c2d..3575a0472 100644 --- a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_constraint.h +++ b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_constraint.h @@ -63,10 +63,10 @@ namespace fuse_constraints * * A landmark is represented as a 3D pose (3D position and a 3D orientation). This class takes * the ground truth location of the 3D landmark and applies a reprojection-error based constraint - * an constraint on the position, orientation and calibration of the camera that observed the landmark. + * on the position, orientation and calibration of the camera that observed the landmark. * * In most cases, the camera calibration should be held fixed as a single landmark does not present enough - * points to accurate constrain the pose AND the calibraton. + * points to accurately constrain the pose AND the calibraton. * */ class Fixed3DLandmarkConstraint : public fuse_core::Constraint diff --git a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h index caa76becf..7f7433aad 100644 --- a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h @@ -114,8 +114,6 @@ class Fixed3DLandmarkCostFunctor fuse_core::Vector7d b_; fuse_core::MatrixXd obs_; fuse_core::MatrixXd pts3d_; - - NormalPriorOrientation3DCostFunctor orientation_functor_; }; Fixed3DLandmarkCostFunctor::Fixed3DLandmarkCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector7d& b, @@ -124,7 +122,6 @@ Fixed3DLandmarkCostFunctor::Fixed3DLandmarkCostFunctor(const fuse_core::MatrixXd , b_(b) , obs_(obs) , pts3d_(pts3d.transpose()) // Transpose from Nx3 to 3xN to make math easier. - , orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Delta will not be scaled { assert(pts3d_.rows() == 3); // Check if we have 3xN