Skip to content
GitLab
Commit b51f1491 authored by mederic_fourmy's avatar mederic_fourmy
Browse files

Remove wrong gtest on votefor keyframe, clean up some files, comments

parent cbe1390d
Hide whitespace changes
Inline Side-by-side
include/apriltag/factor/factor_apriltag.h
View file @ b51f1491
......@@ -134,58 +134,6 @@ template<typename T> bool FactorApriltag::operator ()( const T* const _p_camera,
// Residual
residuals = getMeasurementSquareRootInformationUpper().cast<T>() * err;
/*//states
Eigen::Translation<T,3> p_camera (_p_camera[0] , _p_camera[1] , _p_camera[2]),
p_keyframe (_p_keyframe[0], _p_keyframe[1], _p_keyframe[2]),
p_landmark (_p_landmark[0], _p_landmark[1], _p_landmark[2]);
Eigen::Quaternion<T> q_camera (_o_camera),
q_keyframe (_o_keyframe),
q_landmark (_o_landmark);
//Measurements T and Q
Eigen::Translation3d p_measured(getMeasurement().head(3));
Eigen::Quaterniond q_measured(getMeasurement().data() + 3 );
// landmark wrt camera, measure
Eigen::Transform<T, 3, Eigen::Isometry> c_M_l_meas = p_c_l_meas.cast<T>() * q_measured.cast<T>();
// Create transformation matrices to compose
// robot wrt world
Eigen::Transform<T, 3, Eigen::Isometry> w_M_r = p_keyframe * q_keyframe;
// camera wrt robot
Eigen::Transform<T, 3, Eigen::Isometry> r_M_c = p_camera * q_camera;
// landmark wrt world
Eigen::Transform<T, 3, Eigen::Isometry> w_M_l = p_landmark * q_landmark;
// landmark wrt camera, estimated
Eigen::Transform<T, 3, Eigen::Isometry> c_M_l_est = (w_M_r * r_M_c).inverse() * w_M_l;
// expectation error, in camera frame
// right-minus
Eigen::Transform<T, 3, Eigen::Isometry> c_M_err = c_M_l_meas.inverse() * c_M_l_est;
// opposite of the previous formula and therefore equivalent
// Eigen::Transform<T, 3, Eigen::Isometry> c_M_err = c_M_l_est.inverse() * c_M_l_meas;
// error
Eigen::Matrix<T, 6, 1> err;
err.block(0,0,3,1) = c_M_err.translation();
Eigen::Matrix<T, 3, 3> R_err(c_M_err.linear());
err.block(3,0,3,1) = wolf::log_R(R_err);
// debug stuff
// int kf = getFeature()->getCapture()->getFrame()->id();
// int lmk = getLandmarkOther()->id();
//
// print(kf, lmk, "w_M_c : \n", (w_M_r*r_M_c).matrix());
// print(kf, lmk, "w_M_l : \n", w_M_l.matrix());
// print(kf, lmk, "c_M_l_e: \n", c_M_l_est.matrix());
// print(kf, lmk, "c_M_l_m: \n", c_M_l_meas.matrix());
// print(kf, lmk, "error : \n", err.transpose().eval());
// residual
Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals);
res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * err;
*/
return true;
}
......
src/processor/processor_tracker_landmark_apriltag.cpp
View file @ b51f1491
......@@ -364,7 +364,7 @@ unsigned int ProcessorTrackerLandmarkApriltag::findLandmarks(const LandmarkBaseP
{
_features_out.push_back(feature_in_image);
double score(1.0);
LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(landmark_in_ptr, score); //TODO: smarter score
LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(landmark_in_ptr, score);
_feature_landmark_correspondences.emplace ( feature_in_image, matched_landmark );
feature_in_image->link(_capture); // since all features are created in preProcess() are unlinked
break;
......
test/camera_params_canonical.yaml 0 → 100644
View file @ b51f1491
image_width: 2
image_height: 2
camera_name: canonical
camera_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
distortion_model: none
distortion_coefficients:
rows: 1
cols: 5
data: [0, 0, 0, 0, 0]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0]
\ No newline at end of file
test/gtest_processor_tracker_landmark_apriltag.cpp
View file @ b51f1491
......@@ -72,8 +72,8 @@ class ProcessorTrackerLandmarkApriltag_class : public testing::Test{
// configure wolf problem
problem = Problem::create("PO", 3);
sen = problem->installSensor("SensorCamera", "camera", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/camera_params_canonical.yaml");
prc = problem->installProcessor("ProcessorTrackerLandmarkApriltag_Wrapper", "apriltags_wrapper", "camera", wolf_root + "/demos/processor_tracker_landmark_apriltag.yaml");
sen = problem->installSensor("SensorCamera", "camera", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/camera_params_canonical.yaml");
prc = problem->installProcessor("ProcessorTrackerLandmarkApriltag_Wrapper", "apriltags_wrapper", "camera", wolf_root + "/test/processor_tracker_landmark_apriltag.yaml");
prc_apr = std::static_pointer_cast<ProcessorTrackerLandmarkApriltag_Wrapper>(prc);
// set prior
......@@ -171,51 +171,56 @@ TEST(ProcessorTrackerLandmarkApriltag, Constructor)
ASSERT_DEATH( { std::make_shared<ProcessorTrackerLandmarkApriltag>(params); }, "" );
}
TEST_F(ProcessorTrackerLandmarkApriltag_class, voteForKeyFrame)
{
double min_time_vote = prc_apr->getMinTimeVote();
unsigned int min_features_for_keyframe = prc_apr->getMinFeaturesForKeyframe();
double start_ts = 2.0;
CaptureBasePtr Ca = CaptureBase::emplace<CapturePose>(F1, start_ts, sen, Vector7d(), Matrix6d());
CaptureBasePtr Cb = CaptureBase::emplace<CapturePose>(F1, start_ts + min_time_vote/2, sen, Vector7d(), Matrix6d());
CaptureBasePtr Cc = CaptureBase::emplace<CapturePose>(F1, start_ts + 2*min_time_vote, sen, Vector7d(), Matrix6d());
CaptureBasePtr Cd = CaptureBase::emplace<CapturePose>(F1, start_ts + 2.5*min_time_vote, sen, Vector7d(), Matrix6d());
CaptureBasePtr Ce = CaptureBase::emplace<CapturePose>(F1, start_ts + 3*min_time_vote, sen, Vector7d(), Matrix6d());
for (int i=0; i < min_features_for_keyframe; i++){
det.id = i;
FeatureBase::emplace<FeatureApriltag>(Ca, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation);
FeatureBase::emplace<FeatureApriltag>(Cc, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation);
if (i != min_features_for_keyframe-1){
FeatureBase::emplace<FeatureApriltag>(Cd, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation);
FeatureBase::emplace<FeatureApriltag>(Ce, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation);
}
}
// CASE 1: Not enough time between origin and incoming
prc_apr->setOriginPtr(Ca);
prc_apr->setIncomingPtr(Cb);
ASSERT_FALSE(prc_apr->voteForKeyFrame());
// CASE 2: Enough time but still too many features in image to trigger a KF
prc_apr->setOriginPtr(Ca);
prc_apr->setLastPtr(Cb);
prc_apr->setIncomingPtr(Cc);
ASSERT_FALSE(prc_apr->voteForKeyFrame());
// CASE 3: Enough time, enough features in last, not enough features in incoming
prc_apr->setOriginPtr(Ca);
prc_apr->setLastPtr(Cc);
prc_apr->setIncomingPtr(Cd);
ASSERT_TRUE(prc_apr->voteForKeyFrame());
// CASE 4: Enough time, not enough features in last, not enough features in incoming
prc_apr->setOriginPtr(Ca);
prc_apr->setLastPtr(Cd);
prc_apr->setIncomingPtr(Ce);
ASSERT_FALSE(prc_apr->voteForKeyFrame());
}
// TEST_F(ProcessorTrackerLandmarkApriltag_class, voteForKeyFrame)
// {
// //////////////////////////////////////
// //////////////////////////////////////
// // TODO: GTEST IMPLEMENTATION IS WRONG
// //////////////////////////////////////
// //////////////////////////////////////
// double min_time_vote = prc_apr->getMinTimeVote();
// unsigned int min_features_for_keyframe = prc_apr->getMinFeaturesForKeyframe();
// double start_ts = 2.0;
// CaptureBasePtr Ca = CaptureBase::emplace<CapturePose>(F1, start_ts, sen, Vector7d(), Matrix6d());
// CaptureBasePtr Cb = CaptureBase::emplace<CapturePose>(F1, start_ts + min_time_vote/2, sen, Vector7d(), Matrix6d());
// CaptureBasePtr Cc = CaptureBase::emplace<CapturePose>(F1, start_ts + 2*min_time_vote, sen, Vector7d(), Matrix6d());
// CaptureBasePtr Cd = CaptureBase::emplace<CapturePose>(F1, start_ts + 2.5*min_time_vote, sen, Vector7d(), Matrix6d());
// CaptureBasePtr Ce = CaptureBase::emplace<CapturePose>(F1, start_ts + 3*min_time_vote, sen, Vector7d(), Matrix6d());
// for (int i=0; i < min_features_for_keyframe; i++){
// det.id = i;
// FeatureBase::emplace<FeatureApriltag>(Ca, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation);
// FeatureBase::emplace<FeatureApriltag>(Cc, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation);
// if (i != min_features_for_keyframe-1){
// FeatureBase::emplace<FeatureApriltag>(Cd, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation);
// FeatureBase::emplace<FeatureApriltag>(Ce, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation);
// }
// }
// // CASE 1: Not enough time between origin and incoming
// prc_apr->setOriginPtr(Ca);
// prc_apr->setIncomingPtr(Cb);
// ASSERT_FALSE(prc_apr->voteForKeyFrame());
// // CASE 2: Enough time but still too many features in image to trigger a KF
// prc_apr->setOriginPtr(Ca);
// prc_apr->setLastPtr(Cb);
// prc_apr->setIncomingPtr(Cc);
// ASSERT_FALSE(prc_apr->voteForKeyFrame());
// // // CASE 3: Enough time, enough features in last, not enough features in incoming
// // prc_apr->setOriginPtr(Ca);
// // prc_apr->setLastPtr(Cc);
// // prc_apr->setIncomingPtr(Cd);
// // ASSERT_TRUE(prc_apr->voteForKeyFrame());
// // // CASE 4: Enough time, not enough features in last, not enough features in incoming
// // prc_apr->setOriginPtr(Ca);
// // prc_apr->setLastPtr(Cd);
// // prc_apr->setIncomingPtr(Ce);
// // ASSERT_FALSE(prc_apr->voteForKeyFrame());
// }
TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeaturesDuplicated)
{
......
test/processor_tracker_landmark_apriltag.yaml 0 → 100644
View file @ b51f1491
type: "ProcessorTrackerLandmarkApriltag"
detector parameters:
quad_decimate: 1.5 # doing quad detection at lower resolution to speed things up (see end of file)
quad_sigma: 0.8 # gaussian blur good for noisy images, may be recommended with quad_decimate. Kernel size adapted (see end of this file)
nthreads: 8 # how many thread during tag detection (does not change much?)
debug: false # write some debugging images
refine_edges: true # better edge detection if quad_decimate > 1 (quite inexpensive, almost no diff)
ippe_min_ratio: 3.0 # quite arbitrary, always > 1 (to deactive, set at 0 for example)
ippe_max_rep_error: 2.0 # to deactivate, set at something big (100)
tag widths:
0: 0.055
1: 0.055
2: 0.055
3: 0.055
tag parameters:
tag_family: "tag36h11"
# tag_black_border: 1
tag_width_default: 0.165 # used if tag width not specified
noise:
std_xy : 0.1 # m
std_z : 0.1 # m
std_rpy_degrees : 5 # degrees
std_pix: 20 # pixel error
vote:
voting active: true
min_time_vote: 0 # s
max_time_vote: 0 # s
min_features_for_keyframe: 12
nb_vote_for_every_first: 50
reestimate_last_frame: true # for a better prior on the new keyframe: use only if no motion processor
add_3d_cstr: true # add 3D constraints between the KF so that they do not jump when using apriltag only
max_new_features: -1
apply_loss_function: true
# Annexes:
### Quad decimate: the higher, the lower the resolution
# Does nothing if <= 1.0
# Only values taken into account:
# 1.5, 2, 3, 4
# 1.5 -> ~*2 speed up
# Higher values use a "bad" code according to commentaries in the library, smaller do nothing
### Gaussian blur table:
# max sigma kernel size
# 0.499 1 (disabled)
# 0.999 3
# 1.499 5
# 1.999 7
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment