diff --git a/test/data/kitti/0000000100.bin b/test/data/kitti/0000000100.bin
new file mode 100755
index 0000000000000000000000000000000000000000..e0cd092902fb3f4a82d9b6f74177769f34ecc8f8
Binary files /dev/null and b/test/data/kitti/0000000100.bin differ
diff --git a/test/data/kitti/0000000101.bin b/test/data/kitti/0000000101.bin
new file mode 100755
index 0000000000000000000000000000000000000000..e094e1d4d51b80547d5c98c70cf5e0175a5d4414
Binary files /dev/null and b/test/data/kitti/0000000101.bin differ
diff --git a/test/data/kitti/0000000102.bin b/test/data/kitti/0000000102.bin
new file mode 100755
index 0000000000000000000000000000000000000000..fedf48c5aa17fe3d3bacc32a1f51a82629b6e15f
Binary files /dev/null and b/test/data/kitti/0000000102.bin differ
diff --git a/test/data/kitti/0000000103.bin b/test/data/kitti/0000000103.bin
new file mode 100755
index 0000000000000000000000000000000000000000..57bbf794f2666aafbbdf180ed956b5227f146767
Binary files /dev/null and b/test/data/kitti/0000000103.bin differ
diff --git a/test/data/kitti/0000000104.bin b/test/data/kitti/0000000104.bin
new file mode 100755
index 0000000000000000000000000000000000000000..82a17b5d70d9758a5b58d0a8bf0927cf1c8446cc
Binary files /dev/null and b/test/data/kitti/0000000104.bin differ
diff --git a/test/data/kitti/0000000105.bin b/test/data/kitti/0000000105.bin
new file mode 100755
index 0000000000000000000000000000000000000000..2f1d4dbb1472e3b00c06f2ffe09c6802543cf6fe
Binary files /dev/null and b/test/data/kitti/0000000105.bin differ
diff --git a/test/data/kitti/timestamps.txt b/test/data/kitti/timestamps.txt
new file mode 100755
index 0000000000000000000000000000000000000000..ab89364290787f6c0063638c24cda52ee118f7f3
--- /dev/null
+++ b/test/data/kitti/timestamps.txt
@@ -0,0 +1,154 @@
+2011-09-26 13:04:32.335337762
+2011-09-26 13:04:32.438625628
+2011-09-26 13:04:32.541951663
+2011-09-26 13:04:32.645280345
+2011-09-26 13:04:32.748584715
+2011-09-26 13:04:32.851882576
+2011-09-26 13:04:32.955179251
+2011-09-26 13:04:33.058455038
+2011-09-26 13:04:33.161788132
+2011-09-26 13:04:33.265142234
+2011-09-26 13:04:33.368407119
+2011-09-26 13:04:33.471679129
+2011-09-26 13:04:33.574987540
+2011-09-26 13:04:33.678287762
+2011-09-26 13:04:33.781556152
+2011-09-26 13:04:33.884954722
+2011-09-26 13:04:33.988245317
+2011-09-26 13:04:34.091397260
+2011-09-26 13:04:34.194682360
+2011-09-26 13:04:34.297989343
+2011-09-26 13:04:34.401283278
+2011-09-26 13:04:34.504527128
+2011-09-26 13:04:34.607802417
+2011-09-26 13:04:34.711087760
+2011-09-26 13:04:34.814355620
+2011-09-26 13:04:34.917648931
+2011-09-26 13:04:35.020923233
+2011-09-26 13:04:35.124177585
+2011-09-26 13:04:35.227469163
+2011-09-26 13:04:35.330748917
+2011-09-26 13:04:35.433963723
+2011-09-26 13:04:35.537190894
+2011-09-26 13:04:35.640452958
+2011-09-26 13:04:35.743742739
+2011-09-26 13:04:35.847022176
+2011-09-26 13:04:35.950271780
+2011-09-26 13:04:36.053488779
+2011-09-26 13:04:36.156731687
+2011-09-26 13:04:36.259997486
+2011-09-26 13:04:36.363209987
+2011-09-26 13:04:36.466424567
+2011-09-26 13:04:36.569624701
+2011-09-26 13:04:36.672925196
+2011-09-26 13:04:36.776165764
+2011-09-26 13:04:36.879301220
+2011-09-26 13:04:36.982498156
+2011-09-26 13:04:37.085692252
+2011-09-26 13:04:37.188918035
+2011-09-26 13:04:37.292135658
+2011-09-26 13:04:37.395349614
+2011-09-26 13:04:37.498575284
+2011-09-26 13:04:37.601778062
+2011-09-26 13:04:37.704961679
+2011-09-26 13:04:37.808162495
+2011-09-26 13:04:37.911378801
+2011-09-26 13:04:38.014581331
+2011-09-26 13:04:38.117808833
+2011-09-26 13:04:38.221042124
+2011-09-26 13:04:38.324276085
+2011-09-26 13:04:38.427558211
+2011-09-26 13:04:38.530837254
+2011-09-26 13:04:38.634156788
+2011-09-26 13:04:38.737468483
+2011-09-26 13:04:38.840772177
+2011-09-26 13:04:38.944160820
+2011-09-26 13:04:39.047556446
+2011-09-26 13:04:39.150937092
+2011-09-26 13:04:39.254330815
+2011-09-26 13:04:39.357768089
+2011-09-26 13:04:39.461236131
+2011-09-26 13:04:39.564688516
+2011-09-26 13:04:39.668137104
+2011-09-26 13:04:39.771585858
+2011-09-26 13:04:39.875048796
+2011-09-26 13:04:39.978517137
+2011-09-26 13:04:40.082008601
+2011-09-26 13:04:40.185503214
+2011-09-26 13:04:40.288992178
+2011-09-26 13:04:40.392504444
+2011-09-26 13:04:40.495973075
+2011-09-26 13:04:40.599400084
+2011-09-26 13:04:40.702844822
+2011-09-26 13:04:40.806273004
+2011-09-26 13:04:40.909693242
+2011-09-26 13:04:41.013114779
+2011-09-26 13:04:41.116475689
+2011-09-26 13:04:41.219850175
+2011-09-26 13:04:41.323247270
+2011-09-26 13:04:41.426587321
+2011-09-26 13:04:41.529889840
+2011-09-26 13:04:41.633198896
+2011-09-26 13:04:41.736532013
+2011-09-26 13:04:41.839873160
+2011-09-26 13:04:41.943155264
+2011-09-26 13:04:42.046403723
+2011-09-26 13:04:42.149658272
+2011-09-26 13:04:42.252880524
+2011-09-26 13:04:42.356117971
+2011-09-26 13:04:42.459365092
+2011-09-26 13:04:42.562622210
+2011-09-26 13:04:42.665904890
+2011-09-26 13:04:42.769154732
+2011-09-26 13:04:42.872380298
+2011-09-26 13:04:42.975612623
+2011-09-26 13:04:43.078937121
+2011-09-26 13:04:43.182197917
+2011-09-26 13:04:43.285382470
+2011-09-26 13:04:43.388602972
+2011-09-26 13:04:43.491806052
+2011-09-26 13:04:43.595032884
+2011-09-26 13:04:43.698724998
+2011-09-26 13:04:43.801931003
+2011-09-26 13:04:43.904677436
+2011-09-26 13:04:44.007882621
+2011-09-26 13:04:44.111073932
+2011-09-26 13:04:44.214291904
+2011-09-26 13:04:44.317542613
+2011-09-26 13:04:44.420786651
+2011-09-26 13:04:44.523975691
+2011-09-26 13:04:44.627157621
+2011-09-26 13:04:44.730388368
+2011-09-26 13:04:44.833617097
+2011-09-26 13:04:44.936830345
+2011-09-26 13:04:45.040056894
+2011-09-26 13:04:45.143307359
+2011-09-26 13:04:45.246573491
+2011-09-26 13:04:45.349799334
+2011-09-26 13:04:45.453006344
+2011-09-26 13:04:45.556158193
+2011-09-26 13:04:45.659241506
+2011-09-26 13:04:45.762386345
+2011-09-26 13:04:45.865443920
+2011-09-26 13:04:45.968411521
+2011-09-26 13:04:46.071456700
+2011-09-26 13:04:46.174492262
+2011-09-26 13:04:46.277515679
+2011-09-26 13:04:46.380561323
+2011-09-26 13:04:46.483618987
+2011-09-26 13:04:46.586664270
+2011-09-26 13:04:46.689720187
+2011-09-26 13:04:46.792812032
+2011-09-26 13:04:46.895899467
+2011-09-26 13:04:46.998993630
+2011-09-26 13:04:47.102132861
+2011-09-26 13:04:47.205293239
+2011-09-26 13:04:47.308506549
+2011-09-26 13:04:47.411779993
+2011-09-26 13:04:47.515076291
+2011-09-26 13:04:47.618330665
+2011-09-26 13:04:47.721590862
+2011-09-26 13:04:47.824916201
+2011-09-26 13:04:47.928236690
+2011-09-26 13:04:48.031571686
+2011-09-26 13:04:48.134922273
diff --git a/test/gtest_processor_odom_icp_3d.cpp b/test/gtest_processor_odom_icp_3d.cpp
index 8ebb485410b0f69c9a3f54f6982a6fbe60dc2e8c..6d1dcf6b47ccb9bb6aefaca47dfd481d80320567 100644
--- a/test/gtest_processor_odom_icp_3d.cpp
+++ b/test/gtest_processor_odom_icp_3d.cpp
@@ -44,6 +44,9 @@
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
+// other includes
+#include <string>
+
using namespace wolf;
std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
@@ -118,164 +121,50 @@ TEST_F(Test_ProcessorOdomIcp3D_yaml, process_make_keyframes_always_same_pcl)
ASSERT_MATRIX_APPROX(X.at(0).vector("PO"), X.at(5).vector("PO"), 1e-8); // pointclouds are the same
}
-// // Test to see if the constructor works (not yaml files)
-// TEST_F(Test_FactorForceTorqueInertialDynamics, constructor)
-// {
-// ASSERT_EQ(c1->getCapture(), C1);
-// ASSERT_EQ(c1->getCalibPreint().size(), 13);
-// }
-
-// // Test to see if the constructor works (yaml files)
-// TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, constructor)
-// {
-// ASSERT_EQ(c1->getCapture(), C1);
-// ASSERT_EQ(c1->getCalibPreint().size(), 13);
-// }
-
-// // Test en el que no hi ha moviment (not yaml files)
-// TEST_F(Test_FactorForceTorqueInertialDynamics, residual)
-// {
-// VectorXd res_exp = VectorXd::Zero(18);
-// Matrix<double, 18, 1> res;
-// VectorXd bias = VectorXd::Zero(6);
-
-// P->print(4, 1, 1, 1);
-
-// c1->residual(F0->getStateBlock('P')->getState(), // p0
-// Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
-// F0->getStateBlock('V')->getState(), // v0
-// F0->getStateBlock('L')->getState(), // L0
-// bias, // I
-// F1->getStateBlock('P')->getState(), // p1
-// Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
-// F1->getStateBlock('V')->getState(), // v1
-// F1->getStateBlock('L')->getState(), // L1
-// S->getCom(), // C
-// S->getInertia(), // i
-// S->getMass(), // m
-// res);
-
-// ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
-// }
-
-// // Test en el que no hi ha moviment (yaml files)
-// TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residual)
-// {
-// VectorXd res_exp = VectorXd::Zero(18);
-// Matrix<double, 18, 1> res;
-// VectorXd bias = VectorXd::Zero(6);
-
-// c1->residual(F0->getStateBlock('P')->getState(), // p0
-// Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
-// F0->getStateBlock('V')->getState(), // v0
-// F0->getStateBlock('L')->getState(), // L0
-// bias, // I
-// F1->getStateBlock('P')->getState(), // p1
-// Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
-// F1->getStateBlock('V')->getState(), // v1
-// F1->getStateBlock('L')->getState(), // L1
-// S->getCom(), // C
-// S->getInertia(), // i
-// S->getMass(), // m
-// res);
-
-// ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
-// }
-
-// // Test en el que s'avança 1m en direcció x (not yaml files)
-// TEST_F(Test_FactorForceTorqueInertialDynamics, residualx)
-// {
-// VectorXd res_exp = VectorXd::Zero(18);
-// Matrix<double, 18, 1> res;
-// VectorXd bias = VectorXd::Zero(6);
-
-// // provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
-// F1->getStateBlock('P')->setState(Vector3d(1, 0, 0));
-
-// // Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
-// VectorXd delta_preint(VectorXd::Zero(19));
-// delta_preint.head<3>() = -0.5 * gravity();
-// delta_preint(0) = 1;
-// delta_preint.segment<3>(3) = -gravity();
-// delta_preint.segment<3>(6) = -0.5 * gravity();
-// delta_preint(6) = 1;
-// delta_preint.segment<3>(9) = -gravity();
-// delta_preint(18) = 1;
-// MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
-// VectorXd calib_preint(VectorXd::Zero(13));
-// MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
-// FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(
-// C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
-
-// FactorForceTorqueInertialDynamicsPtr c2 =
-// FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
-
-// c2->residual(F0->getStateBlock('P')->getState(), // p0
-// Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
-// F0->getStateBlock('V')->getState(), // v0
-// F0->getStateBlock('L')->getState(), // L0
-// bias, // I
-// F1->getStateBlock('P')->getState(), // p1
-// Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
-// F1->getStateBlock('V')->getState(), // v1
-// F1->getStateBlock('L')->getState(), // L1
-// S->getCom(), // C
-// S->getInertia(), // i
-// S->getMass(), // m
-// res);
-
-// ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
-// }
-
-// // Test en el que s'avança 1m en direcció x (fitxers yaml)
+TEST_F(Test_ProcessorOdomIcp3D_yaml, process_kitti)
+{ // Loads different kitti frames
+ // source: 2011_09_26_drive_0005_sync/velodyne_points
+ std::vector<VectorComposite> X(15);
+ unsigned int i = 0;
+ std::string fname; //ex: 0000000050.bin
+ TimeStamp t = 0.0; // should be read from velodyne_points/timestamps.txt
+ for (int j = 100; j <= 105; j++) // from scan 100 to 105
+ {
+ // Load pointcloud from file
+ pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref_j = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
-// TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residualx)
-// {
-// VectorXd res_exp = VectorXd::Zero(18);
-// Matrix<double, 18, 1> res;
-// VectorXd bias = VectorXd::Zero(6);
+ fname = "0000000000" + std::to_string(j) + ".bin" ;
+ fname = fname.substr(fname.length() - 14, fname.length());
+ loadData(laser_root_dir + "/test/data/kitti/" + fname, *pcl_ref_j);
+
+ WOLF_INFO("Loaded PointCloud "+fname);
-// // provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
-// F1->getStateBlock('P')->setState(Vector3d(1, 0, 0));
+ // make Capture and process
+ C0 = std::make_shared<CaptureLaser3d>(t, S, pcl_ref_j);
+ C0->process();
-// // Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
-// VectorXd delta_preint(VectorXd::Zero(19));
-// delta_preint.head<3>() = -0.5 * gravity();
-// delta_preint(0) = 1;
-// delta_preint.segment<3>(3) = -gravity();
-// delta_preint.segment<3>(6) = -0.5 * gravity();
-// delta_preint(6) = 1;
-// delta_preint.segment<3>(9) = -gravity();
-// delta_preint(18) = 1;
-// MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
-// VectorXd calib_preint(VectorXd::Zero(13));
-// MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
-// FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(
-// C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
+ // store state at integer timestamps (where the KFs are)
+ if (std::fabs(t.get() - (double)i) < 0.1)
+ {
+ X[i] = p->getLast()->getFrame()->getState("PO");
+ i++;
+ }
-// FactorForceTorqueInertialDynamicsPtr c2 =
-// FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
+ t += 0.1;
+ }
-// c2->residual(F0->getStateBlock('P')->getState(), // p0
-// Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
-// F0->getStateBlock('V')->getState(), // v0
-// F0->getStateBlock('L')->getState(), // L0
-// bias, // I
-// F1->getStateBlock('P')->getState(), // p1
-// Quaterniond(F1->getStateBlock('O')->getState().data()), // q1
-// F1->getStateBlock('V')->getState(), // v1
-// F1->getStateBlock('L')->getState(), // L1
-// S->getCom(), // C
-// S->getInertia(), // i
-// S->getMass(), // m
-// res);
+ P->print(4, 1, 1, 1);
+}
-// ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
-// }
+/*
+TEST(Test_ProcessorOdomIcp3D_yaml, load_single_binary){
+ pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref_i = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+ loadData(laser_root_dir + "/test/data/kitti/0000000101.bin", *pcl_ref_i);
+}*/
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
- //::testing::GTEST_FLAG(filter) = "Test_FactorForceTorqueInertialDynamics_yaml.residual";
+ ::testing::GTEST_FLAG(filter) = "Test_ProcessorOdomIcp3D_yaml.process_kitti";
return RUN_ALL_TESTS();
}
\ No newline at end of file
diff --git a/test/yaml/problem_odom_icp_3d.yaml b/test/yaml/problem_odom_icp_3d.yaml
index 8291a5714b1ff457bee9be7c1bef41a54b10639d..cd8308a82bfbf527c656c0f5fe1074e019d2568b 100644
--- a/test/yaml/problem_odom_icp_3d.yaml
+++ b/test/yaml/problem_odom_icp_3d.yaml
@@ -36,7 +36,7 @@ config:
keyframe_vote:
voting_active: true
min_features_for_keyframe : 0
- max_time_span: 0.99
+ max_time_span: 0.000099 # KF every frame
max_fitness_score: 0.0001 # maximum Euclidean fitness score (e.g., mean of squared distances from the source to the target)
angle_turned: 1.0
state_getter: true