Skip to content
Snippets Groups Projects
Commit 7cd5b7d7 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Remove frame's time tolerance from all APIs

parent c0a63ee7
No related branches found
No related tags found
3 merge requests!30Release after RAL,!29After 2nd RAL submission,!28Resolve "Follow core 420"
Hide whitespace changes
Inline Side-by-side
demos/demo_2_lasers_offline.cpp
+ 1
− 1
View file @ 7cd5b7d7
...@@ -189,7 +189,7 @@ int main(int argc, char** argv) ...@@ -189,7 +189,7 @@ int main(int argc, char** argv)
odom_trajectory.head(3) = ground_truth_pose; odom_trajectory.head(3) = ground_truth_pose;
// Origin Key Frame with covariance // Origin Key Frame with covariance
problem.setPrior(ground_truth_pose, Eigen::Matrix3d::Identity() * 0.1, ts, 0.1); problem.setPrior(ground_truth_pose, Eigen::Matrix3d::Identity() * 0.1, ts);
// Ceres wrapper // Ceres wrapper
SolverCeres ceres_manager(&problem); SolverCeres ceres_manager(&problem);
......
test/gtest_processor_odom_icp.cpp
+ 1
− 1
View file @ 7cd5b7d7
...@@ -85,7 +85,7 @@ class ProcessorOdomIcp_Test : public testing::Test ...@@ -85,7 +85,7 @@ class ProcessorOdomIcp_Test : public testing::Test
t0 = 0.0; t0 = 0.0;
x0 = VectorComposite("PO", {Vector2d(0,0), Vector1d(0)}); x0 = VectorComposite("PO", {Vector2d(0,0), Vector1d(0)});
s0 = VectorComposite("PO", {Vector2d(1,1), Vector1d(1)}); s0 = VectorComposite("PO", {Vector2d(1,1), Vector1d(1)});
F0 = problem->setPriorFactor(x0, s0, t0, 0.1); F0 = problem->setPriorFactor(x0, s0, t0);
} }
void TearDown() override{} void TearDown() override{}
}; };
......
test/gtest_processor_tracker_feature_polyline_2d.cpp
+ 19
− 19
View file @ 7cd5b7d7
...@@ -283,7 +283,7 @@ TEST(ProcessorIMU, voteForKeyFrame) ...@@ -283,7 +283,7 @@ TEST(ProcessorIMU, voteForKeyFrame)
TimeStamp t(0); TimeStamp t(0);
x0 << 0,0,0, 0,0,0,1, 0,0,0; x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
//data variable and covariance matrix //data variable and covariance matrix
// since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
Vector6d data; Vector6d data;
...@@ -346,7 +346,7 @@ TEST_F(ProcessorIMUt, interpolate) ...@@ -346,7 +346,7 @@ TEST_F(ProcessorIMUt, interpolate)
t.set(0); t.set(0);
x0 << 0,0,0, 0,0,0,1, 0,0,0; x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
data << 2, 0, 0, 0, 0, 0; // only acc_x data << 2, 0, 0, 0, 0, 0; // only acc_x
cap_imu_ptr->setData(data); cap_imu_ptr->setData(data);
...@@ -392,7 +392,7 @@ TEST_F(ProcessorIMUt, acc_x) ...@@ -392,7 +392,7 @@ TEST_F(ProcessorIMUt, acc_x)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0; x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity! data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
...@@ -418,7 +418,7 @@ TEST_F(ProcessorIMUt, acc_y) ...@@ -418,7 +418,7 @@ TEST_F(ProcessorIMUt, acc_y)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0; x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
data << 0, 20, 9.806, 0, 0, 0; // only acc_y, but measure gravity! data << 0, 20, 9.806, 0, 0, 0; // only acc_y, but measure gravity!
...@@ -455,7 +455,7 @@ TEST_F(ProcessorIMUt, acc_z) ...@@ -455,7 +455,7 @@ TEST_F(ProcessorIMUt, acc_z)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0; x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
data << 0, 0, 9.806 + 2.0, 0, 0, 0; // only acc_z, but measure gravity! data << 0, 0, 9.806 + 2.0, 0, 0, 0; // only acc_z, but measure gravity!
...@@ -492,7 +492,7 @@ TEST_F(ProcessorIMUt, check_Covariance) ...@@ -492,7 +492,7 @@ TEST_F(ProcessorIMUt, check_Covariance)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0; x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity! data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
...@@ -513,7 +513,7 @@ TEST_F(ProcessorIMUt, gyro_x) ...@@ -513,7 +513,7 @@ TEST_F(ProcessorIMUt, gyro_x)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0; x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
double rate_of_turn = 5 * M_PI/180.0; double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
...@@ -565,7 +565,7 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx) ...@@ -565,7 +565,7 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
// init things // init things
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias); problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias); problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
...@@ -618,7 +618,7 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy) ...@@ -618,7 +618,7 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
double abx(0.002), aby(0.01); double abx(0.002), aby(0.01);
x0 << 0,0,0, 0,0,0,1, 0,0,0; x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
Vector6d bias; bias << abx,aby,0, 0,0,0; Vector6d bias; bias << abx,aby,0, 0,0,0;
Vector3d acc_bias = bias.head(3); Vector3d acc_bias = bias.head(3);
...@@ -670,7 +670,7 @@ TEST_F(ProcessorIMUt, gyro_z) ...@@ -670,7 +670,7 @@ TEST_F(ProcessorIMUt, gyro_z)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0; x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
double rate_of_turn = 5 * M_PI/180.0; double rate_of_turn = 5 * M_PI/180.0;
data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity! data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
...@@ -708,7 +708,7 @@ TEST_F(ProcessorIMUt, gyro_xyz) ...@@ -708,7 +708,7 @@ TEST_F(ProcessorIMUt, gyro_xyz)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0; x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
Vector3d tmp_vec; //will be used to store rate of turn data Vector3d tmp_vec; //will be used to store rate of turn data
Quaterniond quat_comp(Quaterniond::Identity()); Quaterniond quat_comp(Quaterniond::Identity());
...@@ -795,7 +795,7 @@ TEST_F(ProcessorIMUt, gyro_z_ConstVelocity) ...@@ -795,7 +795,7 @@ TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0; x0 << 0,0,0, 0,0,0,1, 2,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
double rate_of_turn = 5 * M_PI/180.0; double rate_of_turn = 5 * M_PI/180.0;
data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity! data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
...@@ -834,7 +834,7 @@ TEST_F(ProcessorIMUt, gyro_x_ConstVelocity) ...@@ -834,7 +834,7 @@ TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0; x0 << 0,0,0, 0,0,0,1, 2,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
double rate_of_turn = 5 * M_PI/180.0; double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
...@@ -878,7 +878,7 @@ TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity) ...@@ -878,7 +878,7 @@ TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0; x0 << 0,0,0, 0,0,0,1, 2,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
double rate_of_turn = 5 * M_PI/180.0; double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
...@@ -922,7 +922,7 @@ TEST_F(ProcessorIMUt, gyro_y_ConstVelocity) ...@@ -922,7 +922,7 @@ TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0; x0 << 0,0,0, 0,0,0,1, 2,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
double rate_of_turn = 5 * M_PI/180.0; double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
...@@ -965,7 +965,7 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity) ...@@ -965,7 +965,7 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0; x0 << 0,0,0, 0,0,0,1, 2,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
Vector3d tmp_vec; //will be used to store rate of turn data Vector3d tmp_vec; //will be used to store rate of turn data
Quaterniond quat_comp(Quaterniond::Identity()); Quaterniond quat_comp(Quaterniond::Identity());
...@@ -1054,7 +1054,7 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x) ...@@ -1054,7 +1054,7 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0; x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
double rate_of_turn = 5 * M_PI/180.0; double rate_of_turn = 5 * M_PI/180.0;
data << 1, 0, -wolf::gravity()(2), rate_of_turn, 0, 0; // measure gravity data << 1, 0, -wolf::gravity()(2), rate_of_turn, 0, 0; // measure gravity
...@@ -1103,7 +1103,7 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y) ...@@ -1103,7 +1103,7 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0; x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
double rate_of_turn = 5 * M_PI/180.0; double rate_of_turn = 5 * M_PI/180.0;
data << 0, 1, -wolf::gravity()(2), 0, rate_of_turn, 0; // measure gravity data << 0, 1, -wolf::gravity()(2), 0, rate_of_turn, 0; // measure gravity
...@@ -1152,7 +1152,7 @@ TEST_F(ProcessorIMUt, gyro_z_acc_z) ...@@ -1152,7 +1152,7 @@ TEST_F(ProcessorIMUt, gyro_z_acc_z)
t.set(0); // clock in 0,1 ms ticks t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0; x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXd P0(9,9); P0.setIdentity(); MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01); problem->setPrior(x0, P0, t);
double rate_of_turn = 5 * M_PI/180.0; double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, -wolf::gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity data << 0, 0, -wolf::gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity
......
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