Skip to content
Snippets Groups Projects
Commit 32b11bac authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Merge branch 'devel' of... 

Merge branch 'devel' of ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/laser.git into devel
parents ade1cbca 80801857
No related branches found
No related tags found
2 merge requests!30Release after RAL,!29After 2nd RAL submission
Hide whitespace changes
Inline Side-by-side
demos/sensor_imu.yaml deleted 100644 → 0
+ 0
− 9
View file @ ade1cbca
sensor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
sensor name: "Main IMU" # This is ignored. The name provided to the SensorFactory prevails
motion variances:
a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2
w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
ab_initial_stdev: 0.800 # m/s2 - initial bias
wb_initial_stdev: 0.350 # rad/sec - initial bias
ab_rate_stdev: 0.1 # m/s2/sqrt(s)
wb_rate_stdev: 0.0400 # rad/s/sqrt(s)
\ No newline at end of file
demos/sensor_odom_3D.yaml deleted 100644 → 0
+ 0
− 8
View file @ ade1cbca
sensor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
sensor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
motion variances:
disp_to_disp: 0.02 # m^2 / m
disp_to_rot: 0.02 # rad^2 / m
rot_to_rot: 0.01 # rad^2 / rad
min_disp_var: 0.01 # m^2
min_rot_var: 0.01 # rad^2
demos/sensor_odom_3D_HQ.yaml deleted 100644 → 0
+ 0
− 8
View file @ ade1cbca
sensor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
sensor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
motion variances:
disp_to_disp: 0.000001 # m^2 / m
disp_to_rot: 0.000001 # rad^2 / m
rot_to_rot: 0.000001 # rad^2 / rad
min_disp_var: 0.00000001 # m^2
min_rot_var: 0.00000001 # rad^2
\ No newline at end of file
test/gtest_processor_tracker_landmark_apriltag.cpp deleted 100644 → 0
+ 0
− 434
View file @ ade1cbca
#include <core/utils/utils_gtest.h>
#include "base/common/wolf.h"
#include "base/utils/logging.h"
#include "base/processor/processor_tracker_landmark_apriltag.h"
#include "base/feature/feature_apriltag.h"
#include "base/landmark/landmark_apriltag.h"
#include "base/capture/capture_pose.h"
#include "base/processor/processor_factory.h"
using namespace Eigen;
using namespace wolf;
using std::static_pointer_cast;
////////////////////////////////////////////////////////////////
/*
* Wrapper class to be able to have setOriginPtr() and setLastPtr() in ProcessorTrackerLandmarkApriltag
*/
WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkApriltag_Wrapper);
class ProcessorTrackerLandmarkApriltag_Wrapper : public ProcessorTrackerLandmarkApriltag
{
public:
ProcessorTrackerLandmarkApriltag_Wrapper(ProcessorParamsTrackerLandmarkApriltagPtr _params_tracker_landmark_apriltag) :
ProcessorTrackerLandmarkApriltag(_params_tracker_landmark_apriltag)
{
setType("TRACKER LANDMARK APRILTAG WRAPPER");
};
~ProcessorTrackerLandmarkApriltag_Wrapper(){}
void setOriginPtr(const CaptureBasePtr _origin_ptr) { origin_ptr_ = _origin_ptr; }
void setLastPtr (const CaptureBasePtr _last_ptr) { last_ptr_ = _last_ptr; }
void setIncomingPtr (const CaptureBasePtr _incoming_ptr) { incoming_ptr_ = _incoming_ptr; }
unsigned int getMinFeaturesForKeyframe (){return min_features_for_keyframe_;}
Scalar getMinTimeVote (){return min_time_vote_;}
void setIncomingDetections(const FeatureBasePtrList _incoming_detections) { detections_incoming_ = _incoming_detections; }
void setLastDetections(const FeatureBasePtrList _last_detections) { detections_last_ = _last_detections; }
// for factory
static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr)
{
std::shared_ptr<ProcessorParamsTrackerLandmarkApriltag> prc_apriltag_params_;
if (_params)
prc_apriltag_params_ = std::static_pointer_cast<ProcessorParamsTrackerLandmarkApriltag>(_params);
else
prc_apriltag_params_ = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>();
ProcessorTrackerLandmarkApriltag_WrapperPtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkApriltag_Wrapper>(prc_apriltag_params_);
prc_ptr->setName(_unique_name);
return prc_ptr;
}
};
namespace wolf{
// Register in the Factories
WOLF_REGISTER_PROCESSOR("TRACKER LANDMARK APRILTAG WRAPPER", ProcessorTrackerLandmarkApriltag_Wrapper);
}
////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////
/*
* Test class to prepare a little wolf problem to test the class ProcessorTrackerLandmarkApriltag
*
* The class ProcessorTrackerLandmarkApriltag is sometimes tested via the wrapper ProcessorTrackerLandmarkApriltag_Wrapper
*/
// Use the following in case you want to initialize tests with predefined variables or methods.
class ProcessorTrackerLandmarkApriltag_class : public testing::Test{
public:
virtual void SetUp()
{
wolf_root = _WOLF_ROOT_DIR;
// configure wolf problem
problem = Problem::create("PO", 3);
sen = problem->installSensor("CAMERA", "camera", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/camera_params_canonical.yaml");
prc = problem->installProcessor("TRACKER LANDMARK APRILTAG WRAPPER", "apriltags_wrapper", "camera", wolf_root + "/src/examples/processor_tracker_landmark_apriltag.yaml");
prc_apr = std::static_pointer_cast<ProcessorTrackerLandmarkApriltag_Wrapper>(prc);
// set prior
F1 = problem->setPrior((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity(), 0.0, 0.1);
// minimal config for the processor to be operative
C1 = std::make_shared<CapturePose>(1.0, sen, Vector7s(), Matrix6s());
F1->addCapture(C1);
prc_apr->setOriginPtr(C1);
prc_apr->setLastPtr(C1);
det.p[0][0] = 1.0;
det.p[0][1] = -1.0;
det.p[1][0] = 1.0;
det.p[1][1] = 1.0;
det.p[2][0] = -1.0;
det.p[2][1] = 1.0;
det.p[3][0] = -1.0;
det.p[3][1] = -1.0;
rep_error1 = 0.01;
rep_error2 = 0.1;
use_rotation = true;
}
public:
ProcessorTrackerLandmarkApriltag_WrapperPtr prc_apr;
std::string wolf_root;
ProblemPtr problem;
SensorBasePtr sen;
ProcessorBasePtr prc;
FrameBasePtr F1;
CaptureBasePtr C1;
apriltag_detection_t det;
Scalar rep_error1;
Scalar rep_error2;
bool use_rotation;
};
////////////////////////////////////////////////////////////////
/////////////////// TESTS START HERE ///////////////////////////
// //
TEST(ProcessorTrackerLandmarkApriltag, Constructor)
{
std::string s1;
std::string s2;
ProcessorParamsTrackerLandmarkApriltagPtr params = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>();
ProcessorTrackerLandmarkApriltagPtr p;
params->tag_family_ = "tag16h5";
p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
params->tag_family_ = "tag25h9";
p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
params->tag_family_ = "tag36h11";
p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
params->tag_family_ = "tagCircle21h7";
p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
params->tag_family_ = "tagStandard41h12";
p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
// ! At the time of this test rewriting, Apriltag3 bugs on the creation of the
// subsequent family's detector
// NOPE
// params->tag_family_ = "tagCircle49h12";
// p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
// ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
// NOPE
// params->tag_family_ = "tagCustom48h12";
// p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
// ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
// NOPE
// params->tag_family_ = "tagStandard52h13";
// p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
// ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
params->tag_family_ = "wrong_family";
WOLF_INFO("The following runtime error \"Unrecognized tag family name. Use e.g. \"tag36h11\".\" is expected and does not imply a failed test:");
ASSERT_DEATH( { std::make_shared<ProcessorTrackerLandmarkApriltag>(params); }, "" );
}
TEST_F(ProcessorTrackerLandmarkApriltag_class, voteForKeyFrame)
{
Scalar min_time_vote = prc_apr->getMinTimeVote();
unsigned int min_features_for_keyframe = prc_apr->getMinFeaturesForKeyframe();
Scalar start_ts = 2.0;
CaptureBasePtr Ca = std::make_shared<CapturePose>(start_ts, sen, Vector7s(), Matrix6s());
CaptureBasePtr Cb = std::make_shared<CapturePose>(start_ts + min_time_vote/2, sen, Vector7s(), Matrix6s());
CaptureBasePtr Cc = std::make_shared<CapturePose>(start_ts + 2*min_time_vote, sen, Vector7s(), Matrix6s());
CaptureBasePtr Cd = std::make_shared<CapturePose>(start_ts + 2.5*min_time_vote, sen, Vector7s(), Matrix6s());
CaptureBasePtr Ce = std::make_shared<CapturePose>(start_ts + 3*min_time_vote, sen, Vector7s(), Matrix6s());
for (int i=0; i < min_features_for_keyframe; i++){
det.id = i;
FeatureApriltagPtr f = std::make_shared<FeatureApriltag>((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity(), i, det, rep_error1, rep_error2, use_rotation);
Ca->addFeature(f);
Ca->addFeature(f);
Cc->addFeature(f);
if (i != min_features_for_keyframe-1){
Cd->addFeature(f);
Ce->addFeature(f);
}
}
F1->addCapture(Ca);
F1->addCapture(Cb);
F1->addCapture(Cc);
F1->addCapture(Cd);
F1->addCapture(Ce);
// 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, detectNewFeatures)
{
// No detected features
FeatureBasePtrList features_out;
prc_apr->detectNewFeatures(1, features_out);
ASSERT_EQ(features_out.size(), 0);
// Some detected features TODO
FeatureBasePtrList features_in;
Eigen::Vector3s pos;
Eigen::Vector3s ori; //Euler angles in rad
Eigen::Quaternions quat;
Eigen::Vector7s pose;
Eigen::Matrix6s meas_cov( (prc_apr->getVarVec()).asDiagonal() );
int tag_id;
// feature 0
pos << 0,2,0;
ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
quat = e2q(ori);
pose << pos, quat.coeffs();
tag_id = 0;
det.id = tag_id;
FeatureBasePtr f0 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
// feature 1
pos << 1,2,0;
ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
quat = e2q(ori);
pose << pos, quat.coeffs();
tag_id = 1;
det.id = tag_id;
FeatureBasePtr f1 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
// feature 2
pos << 0,2,1;
ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
quat = e2q(ori);
pose << pos, quat.coeffs();
tag_id = 2;
det.id = tag_id;
FeatureBasePtr f2 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
features_in.push_back(f0);
features_in.push_back(f0);
// We just added twice the same feature in the list.
prc_apr->setLastDetections(features_in);
// at this point we have 0 detections in last, 2 detections in incoming with same id. We should keep only one in the final list of new detected features
prc_apr->detectNewFeatures(2, features_out);
ASSERT_EQ(features_out.size(), 1);
//we add new different features in the list
features_in.clear();
features_in.push_back(f0);
features_in.push_back(f1);
//these features are set as the incoming detections due to processing an image
prc_apr->setLastDetections(features_in);
// at this point we have 0 detections in last, 2 detections in incoming with different ids, thus we should have 2 new detected features (if max_features set to >= 2)
prc_apr->detectNewFeatures(2, features_out);
ASSERT_EQ(features_out.size(), 2);
// Put some of the features in the graph with createLandmark() and detect some of them as well as others with detectNewFeatures() running again.
WOLF_WARN("call to function createLandmark() in unit test for detectNewFeatures().")
C1->addFeature(f0);
LandmarkBasePtr lmk0 = prc_apr->createLandmark(f0);
C1->addFeature(f1);
LandmarkBasePtr lmk1 = prc_apr->createLandmark(f1);
// Add landmarks to the map
LandmarkBasePtrList landmark_list;
landmark_list.push_back(lmk0);
landmark_list.push_back(lmk1);
problem->addLandmarkList(landmark_list);
//problem->print(4,1,1,1);
// Add 1 one more new feature to the detection list
features_in.push_back(f2);
prc_apr->setLastDetections(features_in);
// At this point we have 2 landmarks (for f0 and f1), and 3 detections (f0, f1 and f2).
// Hence we should 1 new detected feature : f2
features_out.clear();
prc_apr->detectNewFeatures(2, features_out);
ASSERT_EQ(features_out.size(), 1);
ASSERT_EQ(std::static_pointer_cast<FeatureApriltag>(features_out.front())->getTagId(), 2);
}
TEST_F(ProcessorTrackerLandmarkApriltag_class, createLandmark)
{
Vector7s pose_landmark((Vector7s()<<0,0,0,0,0,0,1).finished());
det.id = 1;
FeatureApriltagPtr f1 = std::make_shared<FeatureApriltag>(pose_landmark, Matrix6s::Identity(), 1, det, rep_error1, rep_error2, use_rotation);
C1->addFeature(f1);
LandmarkBasePtr lmk = prc_apr->createLandmark(f1);
LandmarkApriltagPtr lmk_april = std::static_pointer_cast<LandmarkApriltag>(lmk);
ASSERT_TRUE(lmk_april->getType() == "APRILTAG");
ASSERT_MATRIX_APPROX(lmk_april->getState(), pose_landmark, 1e-6);
}
TEST_F(ProcessorTrackerLandmarkApriltag_class, createFactor)
{
det.id = 1;
FeatureApriltagPtr f1 = std::make_shared<FeatureApriltag>((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity(), 1, det, rep_error1, rep_error2, use_rotation);
C1->addFeature(f1);
LandmarkBasePtr lmk = prc_apr->createLandmark(f1);
LandmarkApriltagPtr lmk_april = std::static_pointer_cast<LandmarkApriltag>(lmk);
FactorBasePtr ctr = prc_apr->createFactor(f1, lmk);
ASSERT_TRUE(ctr->getType() == "AUTODIFF APRILTAG");
}
TEST_F(ProcessorTrackerLandmarkApriltag_class, computeInformation)
{
Scalar cx = 320;
Scalar cy = 240;
Scalar fx = 320;
Scalar fy = 320;
Eigen::Matrix3s K;
K << fx, 0, cx,
0, fy, cy,
0, 0, 1;
Eigen::Vector3s t; t << 0.0, 0.0, 0.4;
Eigen::Vector3s v; v << 0.2, 0.0, 0.0;
Scalar tag_width = 0.05;
Scalar s = tag_width/2;
Eigen::Vector3s p1; p1 << s, s, 0; // bottom right
Eigen::Vector3s p2; p2 << -s, s, 0; // bottom left
// Got from Matlab code:
// Top left corner
Eigen::Vector3s h1_matlab; h1_matlab << 137.5894, 105.0325, 0.4050;
Eigen::Matrix3s J_h_T1_matlab;
J_h_T1_matlab << 320, 0, 320,
0, 320, 240,
0, 0, 1;
Eigen::Matrix3s J_h_R1_matlab;
J_h_R1_matlab << 7.8405, -7.8405, -6.4106,
4.2910, -4.2910, 9.0325,
0.0245, -0.0245, 0.0050;
// Top right corner
Eigen::Vector3s h2_matlab; h2_matlab << 121.5894, 105.0325, 0.4050;
Eigen::Matrix3s J_h_T2_matlab;
J_h_T2_matlab << 320, 0, 320,
0, 320, 240,
0, 0, 1;
Eigen::Matrix3s J_h_R2_matlab;
J_h_R2_matlab << 7.8405, 7.8405, -9.5894,
4.2910, 4.2910, -9.0325,
0.0245, 0.0245, -0.0050;
Eigen::Vector3s h1;
Eigen::Matrix3s J_h_T1;
Eigen::Matrix3s J_h_R1;
Eigen::Vector3s h2;
Eigen::Matrix3s J_h_T2;
Eigen::Matrix3s J_h_R2;
prc_apr->pinholeHomogeneous(K, t, v2R(v), p1, h1, J_h_T1, J_h_R1);
prc_apr->pinholeHomogeneous(K, t, v2R(v), p2, h2, J_h_T2, J_h_R2);
ASSERT_MATRIX_APPROX(h1, h1_matlab, 1e-3);
ASSERT_MATRIX_APPROX(J_h_T1, J_h_T1_matlab, 1e-3);
ASSERT_MATRIX_APPROX(J_h_R1, J_h_R1_matlab, 1e-3);
ASSERT_MATRIX_APPROX(h2, h2_matlab, 1e-3);
ASSERT_MATRIX_APPROX(J_h_T2, J_h_T2_matlab, 1e-3);
ASSERT_MATRIX_APPROX(J_h_R2, J_h_R2_matlab, 1e-3);
Scalar sig_q = 2;
Eigen::Matrix6s transformation_info = prc_apr->computeInformation(t, v2R(v), K, tag_width, sig_q);
// From Matlab
// Eigen::Matrix6s transformation_cov_matlab;
// transformation_cov_matlab <<
// 0.0000, 0.0000, -0.0000, 0.0000, -0.0002, 0.0000,
// 0.0000, 0.0000, -0.0000, 0.0002, 0.0000, 0.0000,
// -0.0000, -0.0000, 0.0004, -0.0040, -0.0000, 0.0000,
// 0.0000, 0.0002, -0.0040, 0.1027, 0.0000, 0.0000,
// -0.0002, 0.0000, -0.0000, 0.0000, 0.1074, -0.0106,
// 0.0000, 0.0000, 0.0000, 0.0000, -0.0106, 0.0023;
Eigen::Matrix6s transformation_info_matlab;
transformation_info_matlab <<
6.402960973553990, 0, 0.000000000000000, -0.000000000000000, 0.009809735541319, 0.001986080274985,
0, 6.402960973553990, 0.014610695222409, -0.008824560412472, 0.000000000000000, 0.000000000000000,
0.000000000000000, 0.014610695222409, 0.049088870761338, 0.001889201771982, 0.000000000000000, 0.000000000000000,
-0.000000000000000, -0.008824560412472, 0.001889201771982, 0.000183864607538, -0.000000000000000, 0.000000000000000,
0.009809735541319, 0.000000000000000, 0.000000000000000, -0.000000000000000, 0.000183864607538, 0.000773944077821,
0.001986080274985, 0.000000000000000, 0.000000000000000, -0.000000000000000, 0.000773944077821, 0.007846814985446;
transformation_info_matlab = transformation_info_matlab*100000.0;
ASSERT_MATRIX_APPROX(transformation_info, transformation_info_matlab, 1e-3);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
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