diff --git a/src/examples/test_apriltag.cpp b/src/examples/test_apriltag.cpp index 5cd2dc8e225754b0126b906cb5167428e83aadfd..21bdc72c5dc80f1349b9ef9f1c4e1d8c8b5b4f29 100644 --- a/src/examples/test_apriltag.cpp +++ b/src/examples/test_apriltag.cpp @@ -46,7 +46,7 @@ int main(int argc, char *argv[]) // General execution options const bool APPLY_CONTRAST = false; const bool IMAGE_OUTPUT = true; - const bool USEMAP = true; + const bool USEMAP = false; WOLF_INFO( "==================== processor apriltag test ======================" ) @@ -61,9 +61,9 @@ int main(int argc, char *argv[]) WOLF_INFO( "==================== Configure Problem ======================" ) - - SensorBasePtr sen = problem->installSensor("CAMERA", "camera", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/camera_logitech_c300_640_480.yaml"); -// SensorBasePtr sen = problem->installSensor("CAMERA", "camera", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/camera_Dinesh_LAAS_params_notangentrect.yaml"); + Eigen::Vector7s cam_extrinsics; cam_extrinsics << 0,0,0, 0,0,0,1; + SensorBasePtr sen = problem->installSensor("CAMERA", "camera", cam_extrinsics, wolf_root + "/src/examples/camera_logitech_c300_640_480.yaml"); +// SensorBasePtr sen = problem->installSensor("CAMERA", "camera", cam_extrinsics, wolf_root + "/src/examples/camera_Dinesh_LAAS_params_notangentrect.yaml"); SensorCameraPtr sen_cam = std::static_pointer_cast<SensorCamera>(sen); ProcessorBasePtr prc = problem->installProcessor("TRACKER LANDMARK APRILTAG", "apriltags", "camera", wolf_root + "/src/examples/processor_tracker_landmark_apriltag.yaml"); diff --git a/src/processors/processor_tracker_landmark_apriltag.cpp b/src/processors/processor_tracker_landmark_apriltag.cpp index c91887d977b181497a9d6783be6af6caf0b7e96e..916ab0f9717c21f001888aa042e71698ef6c1b53 100644 --- a/src/processors/processor_tracker_landmark_apriltag.cpp +++ b/src/processors/processor_tracker_landmark_apriltag.cpp @@ -145,7 +145,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess() Eigen::Affine3ds M_ippe1, M_ippe2, M_april, M_PnP; Scalar rep_error1; Scalar rep_error2; - ippe_pose_estimation(det, cv_K_, tag_width, M_ippe1, rep_error1, M_ippe2, rep_error2); + ippePoseEstimation(det, cv_K_, tag_width, M_ippe1, rep_error1, M_ippe2, rep_error2); // If not so sure about whether we have the right solution or not, do not create a feature // TODO: use parameters @@ -157,7 +157,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess() // Slower than UMICH (iterative algorithm LM) but yield more precise results // Does not solve the ambiguity on the rotation however ////////////////// - M_PnP = opencv_pose_estimation(det, cv_K_, tag_width); + M_PnP = opencvPoseEstimation(det, cv_K_, tag_width); ////////////////// ////////////////// @@ -165,7 +165,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess() // Implementation found in the original Apriltag c implementation. // Analytical formula but high reprojection error for large angles ////////////////// - // M_april = umich_pose_estimation(det, cv_K_, tag_width); + // M_april = umichPoseEstimation(det, cv_K_, tag_width); ////////////////// WOLF_DEBUG("ippe1\n", M_ippe1 .matrix()); @@ -175,23 +175,34 @@ void ProcessorTrackerLandmarkApriltag::preProcess() c_M_t = M_ippe1; +// if (tag_id == 1){ +// WOLF_INFO("TEST1: change solution of tag 1"); +// c_M_t = M_ippe2; +// } + // set the measured pose vector Eigen::Vector3s translation ( c_M_t.translation() ); // translation vector in apriltag meters Eigen::Vector7s pose; pose << translation, R2q(c_M_t.linear()).coeffs(); // compute the covariance - Eigen::Matrix6s cov = getVarVec().asDiagonal() ; // fixed dummy covariance -// Eigen::Matrix6s info = computeInformation(translation, c_M_t.linear(), K_, tag_width, std_pix_); // Lie jacobians covariance +// Eigen::Matrix6s cov = getVarVec().asDiagonal() ; // fixed dummy covariance + Eigen::Matrix6s info = computeInformation(translation, c_M_t.linear(), K_, tag_width, std_pix_); // Lie jacobians covariance if (is_ambiguous){ - WOLF_INFO("Ambiguity on estimated rotation is likely"); - // Put a very high covariance on angles measurements - cov.bottomRightCorner(3, 3) = 1000000*Eigen::Matrix3s::Identity(); +// WOLF_INFO("Ambiguity on estimated rotation is likely"); +// // Put a very high covariance on angles measurements +// cov.bottomRightCorner(3, 3) = 1000000*Eigen::Matrix3s::Identity(); } - WOLF_TRACE("cov diagonal: [", cov.diagonal(), "]"); +// if (tag_id == 1){ +// // Put a very high covariance on angles measurements +// WOLF_INFO("TEST2: Increase meas cov on tag 1"); +// cov.bottomRightCorner(3, 3) = 1000000*Eigen::Matrix3s::Identity(); +// } + +// WOLF_TRACE("tag ", tag_id, " cov diagonal: [", cov.diagonal().transpose(), "]"); // add to detected features list - detections_incoming_.push_back(std::make_shared<FeatureApriltag>(pose, cov.inverse(), tag_id, *det, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO)); + detections_incoming_.push_back(std::make_shared<FeatureApriltag>(pose, info, tag_id, *det, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO)); WOLF_TRACE("---------------------\n"); } @@ -201,20 +212,20 @@ void ProcessorTrackerLandmarkApriltag::preProcess() // To compare with apriltag implementation // Returned translation is in tag units: needs to be multiplied by tag_width/2 -Eigen::Affine3ds ProcessorTrackerLandmarkApriltag::opencv_pose_estimation(apriltag_detection_t *det, cv::Mat_<Scalar> K, double tag_width){ +Eigen::Affine3ds ProcessorTrackerLandmarkApriltag::opencvPoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar> _K, double _tag_width){ // get corners from det std::vector<cv::Point2d> corners_pix(4); for (int i = 0; i < 4; i++) { - corners_pix[i].x = det->p[i][0]; - corners_pix[i].y = det->p[i][1]; + corners_pix[i].x = _det->p[i][0]; + corners_pix[i].y = _det->p[i][1]; } std::vector<cv::Point3d> obj_pts; // Same order as the 2D corners (anti clockwise, looking at the tag). // Looking at the tag, the reference frame is // X = Right, Y = Down, Z = Inside the plane - Scalar s = tag_width/2; + Scalar s = _tag_width/2; obj_pts.emplace_back(-s, s, 0); // bottom left obj_pts.emplace_back( s, s, 0); // bottom right obj_pts.emplace_back( s, -s, 0); // top right @@ -225,7 +236,7 @@ Eigen::Affine3ds ProcessorTrackerLandmarkApriltag::opencv_pose_estimation(aprilt cv::Mat rvec, tvec; cv::Mat dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<Scalar>::type); // Assuming corrected images - cv::solvePnP(obj_pts, corners_pix, K, dist_coeffs, rvec, tvec); + cv::solvePnP(obj_pts, corners_pix, _K, dist_coeffs, rvec, tvec); // Puts the result in a Eigen affine Transform cv::Matx33d rmat; @@ -239,13 +250,13 @@ Eigen::Affine3ds ProcessorTrackerLandmarkApriltag::opencv_pose_estimation(aprilt return M; } -Eigen::Affine3d ProcessorTrackerLandmarkApriltag::umich_pose_estimation(apriltag_detection_t *det, cv::Mat_<Scalar> K, double tag_width){ +Eigen::Affine3d ProcessorTrackerLandmarkApriltag::umichPoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar> _K, double _tag_width){ // To put in the usual camera frame with Z looking in front (RDF) Eigen::Affine3d c_M_ac; c_M_ac.matrix() = (Eigen::Vector4d() << 1, -1, -1, 1).finished().asDiagonal(); Eigen::Affine3d M_april_raw; - matd_t *pose_matrix = homography_to_pose(det->H, -K(0,0), K(1,1), K(0,2), K(1,2)); // !! fx Negative sign advised by apriltag library commentary + matd_t *pose_matrix = homography_to_pose(_det->H, -_K(0,0), _K(1,1), _K(0,2), _K(1,2)); // !! fx Negative sign advised by apriltag library commentary // write it in Eigen form Eigen::Affine3d ac_M_t; for(int r=0; r<4; r++) @@ -255,16 +266,16 @@ Eigen::Affine3d ProcessorTrackerLandmarkApriltag::umich_pose_estimation(apriltag Eigen::Affine3d c_M_t = c_M_ac * ac_M_t; // Normalize transform - c_M_t.matrix().block(0,3,3,1) *= tag_width/2; + c_M_t.matrix().block(0,3,3,1) *= _tag_width/2; return c_M_t; } -void ProcessorTrackerLandmarkApriltag::ippe_pose_estimation(apriltag_detection_t *det, cv::Mat_<Scalar> K, double tag_width, - Eigen::Affine3d &M1, - double &rep_error1, - Eigen::Affine3d &M2, - double &rep_error2){ +void ProcessorTrackerLandmarkApriltag::ippePoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar> _K, double _tag_width, + Eigen::Affine3d &_M1, + double &_rep_error1, + Eigen::Affine3d &_M2, + double &_rep_error2){ // camera coefficients cv::Mat dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<Scalar>::type); // Assuming corrected images @@ -273,14 +284,14 @@ void ProcessorTrackerLandmarkApriltag::ippe_pose_estimation(apriltag_detection_t std::vector<cv::Point2d> corners_pix(4); for (int i = 0; i < 4; i++) { - corners_pix[i].x = det->p[i][0]; - corners_pix[i].y = det->p[i][1]; + corners_pix[i].x = _det->p[i][0]; + corners_pix[i].y = _det->p[i][1]; } std::vector<cv::Point3d> obj_pts; // Same order as the 2D corners (anti clockwise, looking at the tag). // Looking at the tag, the reference frame is // X = Right, Y = Down, Z = Inside the plane - Scalar s = tag_width/2; + Scalar s = _tag_width/2; obj_pts.emplace_back(-s, s, 0); // bottom left obj_pts.emplace_back( s, s, 0); // bottom right obj_pts.emplace_back( s, -s, 0); // top right @@ -303,26 +314,26 @@ void ProcessorTrackerLandmarkApriltag::ippe_pose_estimation(apriltag_detection_t * @param reprojErr2 Reprojection error of second solution */ float err1, err2; - pose_solver.solveGeneric(obj_pts, corners_pix, K, dist_coeffs, + pose_solver.solveGeneric(obj_pts, corners_pix, _K, dist_coeffs, rvec1, tvec1, err1, rvec2, tvec2, err2); - rep_error1 = err1; - rep_error2 = err2; + _rep_error1 = err1; + _rep_error2 = err2; // Puts the result in a Eigen affine Transform cv::Matx33d rmat1; cv::Rodrigues(rvec1, rmat1); Eigen::Matrix3s R_eigen1; cv2eigen(rmat1, R_eigen1); Eigen::Vector3s t_eigen1; cv2eigen(tvec1, t_eigen1); - M1.matrix().block(0,0,3,3) = R_eigen1; - M1.matrix().block(0,3,3,1) = t_eigen1; + _M1.matrix().block(0,0,3,3) = R_eigen1; + _M1.matrix().block(0,3,3,1) = t_eigen1; cv::Matx33d rmat2; cv::Rodrigues(rvec2, rmat2); Eigen::Matrix3s R_eigen2; cv2eigen(rmat2, R_eigen2); Eigen::Vector3s t_eigen2; cv2eigen(tvec2, t_eigen2); - M2.matrix().block(0,0,3,3) = R_eigen2; - M2.matrix().block(0,3,3,1) = t_eigen2; + _M2.matrix().block(0,0,3,3) = R_eigen2; + _M2.matrix().block(0,3,3,1) = t_eigen2; } diff --git a/src/processors/processor_tracker_landmark_apriltag.h b/src/processors/processor_tracker_landmark_apriltag.h index a8ea31bcf1b1879ecb918fe8c2cbe717a5e6cc9c..888dadf714ae86227774a53902ec6edee035819b 100644 --- a/src/processors/processor_tracker_landmark_apriltag.h +++ b/src/processors/processor_tracker_landmark_apriltag.h @@ -115,20 +115,20 @@ class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark Eigen::Vector6s getVarVec(); FeatureBaseList getIncomingDetections() const; FeatureBaseList getLastDetections() const; - Eigen::Affine3d opencv_pose_estimation(apriltag_detection_t *det, cv::Mat_<Scalar>, double tag_width); - Eigen::Affine3d umich_pose_estimation(apriltag_detection_t *det, cv::Mat_<Scalar>, double tag_width); - void ippe_pose_estimation(apriltag_detection_t *det, cv::Mat_<Scalar>, double tag_width, - Eigen::Affine3d &M1, - double &rep_error1, - Eigen::Affine3d &M2, - double &rep_error2); - Eigen::Matrix6s computeInformation(Eigen::Vector3s const &t, Eigen::Matrix3s const &R, Eigen::Matrix3s const &K, double const &tag_width, double const &sig_q); - void pinholeHomogeneous(Eigen::Matrix3s const & K, Eigen::Vector3s const & t, - Eigen::Matrix3s const & R, Eigen::Vector3s const & p, - Eigen::Vector3s &h, Eigen::Matrix3s &J_h_T, Eigen::Matrix3s &J_h_R); - void cornersToPose(const std::vector<cv::Point2d> &img_pts, - const std::vector<Scalar> &k_vec, - Eigen::Affine3ds &M); + Eigen::Affine3d opencvPoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar>, double _tag_width); + Eigen::Affine3d umichPoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar>, double _tag_width); + void ippePoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar>, double _tag_width, + Eigen::Affine3d &_M1, + double &_rep_error1, + Eigen::Affine3d &_M2, + double &_rep_error2); + Eigen::Matrix6s computeInformation(Eigen::Vector3s const &_t, Eigen::Matrix3s const &_R, Eigen::Matrix3s const &_K, double const &_tag_width, double const &_sig_q); + void pinholeHomogeneous(Eigen::Matrix3s const & _K, Eigen::Vector3s const & _t, + Eigen::Matrix3s const & _R, Eigen::Vector3s const & _p, + Eigen::Vector3s &_h, Eigen::Matrix3s &_J_h_T, Eigen::Matrix3s &_J_h_R); + void cornersToPose(const std::vector<cv::Point2d> &_img_pts, + const std::vector<Scalar> &_k_vec, + Eigen::Affine3ds &_M); protected: void advanceDerived();