diff --git a/src/examples/test_constraint_imu.cpp b/src/examples/test_constraint_imu.cpp
index e4a3b8f2b37a5669bf10b32c16a2e8246bc9e52e..9ea5093077a271fcf2a14b5a4dda7f9f8997ee0d 100644
--- a/src/examples/test_constraint_imu.cpp
+++ b/src/examples/test_constraint_imu.cpp
@@ -62,7 +62,7 @@ int main(int argc, char** argv)
Eigen::VectorXs state_vec;
Eigen::VectorXs delta_preint;
//FrameIMUPtr last_frame;
- Eigen::Matrix<wolf::Scalar,9,9> delta_preint_cov;
+ Eigen::Matrix<Scalar,9,9> delta_preint_cov;
//process data
mpu_clock = 0.001003;
@@ -97,7 +97,7 @@ int main(int argc, char** argv)
FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFramePtr()));
- Eigen::Matrix<wolf::Scalar, 10, 1> expect;
+ Eigen::Matrix<Scalar, 10, 1> expect;
Eigen::Vector3s ref_frame_p = ref_frame_ptr->getPPtr()->getVector();
Eigen::Quaternions ref_frame_o(ref_frame_ptr->getOPtr()->getVector().data());
Eigen::Vector3s ref_frame_v = ref_frame_ptr->getVPtr()->getVector();
@@ -105,7 +105,7 @@ int main(int argc, char** argv)
Eigen::Quaternions current_frame_o(last_frame->getOPtr()->getVector().data());
Eigen::Vector3s current_frame_v = last_frame->getVPtr()->getVector();
Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBiasPtr()->getVector()), gyro_bias(ref_frame_ptr->getGyroBiasPtr()->getVector());
- Eigen::Matrix<wolf::Scalar, 9, 1> residu;
+ Eigen::Matrix<Scalar, 9, 1> residu;
residu << 0,0,0, 0,0,0, 0,0,0;
constraint_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, current_frame_p, current_frame_o, current_frame_v, expect);
@@ -158,7 +158,7 @@ int main(int argc, char** argv)
FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFramePtr()));
- Eigen::Matrix<wolf::Scalar, 10, 1> expect;
+ Eigen::Matrix<Scalar, 10, 1> expect;
Eigen::Vector3s ref_frame_p = ref_frame_ptr->getPPtr()->getVector();
Eigen::Quaternions ref_frame_o(ref_frame_ptr->getOPtr()->getVector().data());
Eigen::Vector3s ref_frame_v = ref_frame_ptr->getVPtr()->getVector();
@@ -166,7 +166,7 @@ int main(int argc, char** argv)
Eigen::Quaternions current_frame_o(last_frame->getOPtr()->getVector().data());
Eigen::Vector3s current_frame_v = last_frame->getVPtr()->getVector();
Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBiasPtr()->getVector()), gyro_bias(ref_frame_ptr->getGyroBiasPtr()->getVector());
- Eigen::Matrix<wolf::Scalar, 9, 1> residu;
+ Eigen::Matrix<Scalar, 9, 1> residu;
residu << 0,0,0, 0,0,0, 0,0,0;
constraint_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v, expect);
@@ -216,7 +216,7 @@ int main(int argc, char** argv)
FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFramePtr()));
- Eigen::Matrix<wolf::Scalar, 10, 1> expect;
+ Eigen::Matrix<Scalar, 10, 1> expect;
Eigen::Vector3s ref_frame_p = ref_frame_ptr->getPPtr()->getVector();
Eigen::Quaternions ref_frame_o(ref_frame_ptr->getOPtr()->getVector().data());
Eigen::Vector3s ref_frame_v = ref_frame_ptr->getVPtr()->getVector();
@@ -224,7 +224,7 @@ int main(int argc, char** argv)
Eigen::Quaternions current_frame_o(last_frame->getOPtr()->getVector().data());
Eigen::Vector3s current_frame_v = last_frame->getVPtr()->getVector();
Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBiasPtr()->getVector()), gyro_bias(ref_frame_ptr->getGyroBiasPtr()->getVector());
- Eigen::Matrix<wolf::Scalar, 9, 1> residu;
+ Eigen::Matrix<Scalar, 9, 1> residu;
residu << 0,0,0, 0,0,0, 0,0,0;
constraint_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v, expect);
diff --git a/src/test/gtest_constraint_IMU.cpp b/src/test/gtest_constraint_IMU.cpp
index d0dfc5595d89f261fd7758a8cca9c1de21a0f3b6..a0b8bfd9231a0f4c94ad70e833f85644319f58c1 100644
--- a/src/test/gtest_constraint_IMU.cpp
+++ b/src/test/gtest_constraint_IMU.cpp
@@ -35,7 +35,7 @@ using namespace wolf;
class ConstraintIMU_biasTest_Static_NullBias : public testing::Test
{
public:
- wolf::TimeStamp t;
+ TimeStamp t;
SensorIMUPtr sen_imu;
ProblemPtr wolf_problem_ptr_;
CeresManager* ceres_manager_wolf_diff;
@@ -94,9 +94,9 @@ class ConstraintIMU_biasTest_Static_NullBias : public testing::Test
// PROCESS DATA
Eigen::Vector6s data_imu;
- data_imu << -wolf::gravity(), 0,0,0;
+ data_imu << -gravity(), 0,0,0;
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on IMU (measure only gravity here)
+ CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on IMU (measure only gravity here)
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -122,7 +122,7 @@ class ConstraintIMU_biasTest_Static_NullBias : public testing::Test
class ConstraintIMU_biasTest_Static_NonNullAccBias : public testing::Test
{
public:
- wolf::TimeStamp t;
+ TimeStamp t;
SensorIMUPtr sen_imu;
ProblemPtr wolf_problem_ptr_;
CeresManager* ceres_manager_wolf_diff;
@@ -178,10 +178,10 @@ class ConstraintIMU_biasTest_Static_NonNullAccBias : public testing::Test
// PROCESS DATA
Eigen::Vector6s data_imu;
- data_imu << -wolf::gravity(), 0,0,0;
+ data_imu << -gravity(), 0,0,0;
data_imu = data_imu + origin_bias;
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on IMU (measure only gravity here)
+ CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on IMU (measure only gravity here)
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -206,7 +206,7 @@ class ConstraintIMU_biasTest_Static_NonNullAccBias : public testing::Test
class ConstraintIMU_biasTest_Static_NonNullGyroBias : public testing::Test
{
public:
- wolf::TimeStamp t;
+ TimeStamp t;
SensorIMUPtr sen_imu;
ProblemPtr wolf_problem_ptr_;
CeresManager* ceres_manager_wolf_diff;
@@ -262,10 +262,10 @@ class ConstraintIMU_biasTest_Static_NonNullGyroBias : public testing::Test
// PROCESS DATA
Eigen::Vector6s data_imu;
- data_imu << -wolf::gravity(), 0,0,0;
+ data_imu << -gravity(), 0,0,0;
data_imu = data_imu + origin_bias;
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov() );//, origin_bias); //set data on IMU (measure only gravity here)
+ CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov() );//, origin_bias); //set data on IMU (measure only gravity here)
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -290,7 +290,7 @@ class ConstraintIMU_biasTest_Static_NonNullGyroBias : public testing::Test
class ConstraintIMU_biasTest_Static_NonNullBias : public testing::Test
{
public:
- wolf::TimeStamp t;
+ TimeStamp t;
SensorIMUPtr sen_imu;
ProblemPtr wolf_problem_ptr_;
CeresManager* ceres_manager_wolf_diff;
@@ -348,10 +348,10 @@ class ConstraintIMU_biasTest_Static_NonNullBias : public testing::Test
// PROCESS DATA
Eigen::Vector6s data_imu;
- data_imu << -wolf::gravity(), 0,0,0;
+ data_imu << -gravity(), 0,0,0;
data_imu = data_imu + origin_bias;
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero()); //set data on IMU (measure only gravity here)
+ CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero()); //set data on IMU (measure only gravity here)
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -376,7 +376,7 @@ class ConstraintIMU_biasTest_Static_NonNullBias : public testing::Test
class ConstraintIMU_biasTest_Move_NullBias : public testing::Test
{
public:
- wolf::TimeStamp t;
+ TimeStamp t;
SensorIMUPtr sen_imu;
ProblemPtr wolf_problem_ptr_;
CeresManager* ceres_manager_wolf_diff;
@@ -423,7 +423,7 @@ class ConstraintIMU_biasTest_Move_NullBias : public testing::Test
t.set(0);
Eigen::Vector6s data_imu;
- data_imu << 0,10,-wolf::gravity()(2), 0,0,0; //10m/s on y direction
+ data_imu << 0,10,-gravity()(2), 0,0,0; //10m/s on y direction
expected_final_state << 0,5,0, 0,0,0,1, 0,10,0; // advanced at a=10m/s2 during 1s ==> dx = 0.5*10*1^2 = 5; dvx = 10*1 = 10
origin_bias<< 0,0,0,0,0,0;
@@ -439,7 +439,7 @@ class ConstraintIMU_biasTest_Move_NullBias : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
+ CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -464,7 +464,7 @@ class ConstraintIMU_biasTest_Move_NullBias : public testing::Test
class ConstraintIMU_biasTest_Move_NonNullBias : public testing::Test
{
public:
- wolf::TimeStamp t;
+ TimeStamp t;
SensorIMUPtr sen_imu;
ProblemPtr wolf_problem_ptr_;
CeresManager* ceres_manager_wolf_diff;
@@ -511,7 +511,7 @@ class ConstraintIMU_biasTest_Move_NonNullBias : public testing::Test
Eigen::Vector6s data_imu;
origin_bias = Eigen::Vector6s::Random() * 0.001;
- data_imu << 0,10,-wolf::gravity()(2), 0,0,0; //10m/s on y direction
+ data_imu << 0,10,-gravity()(2), 0,0,0; //10m/s on y direction
data_imu = data_imu + origin_bias;
expected_final_state << 0,5,0, 0,0,0,1, 0,10,0; // advanced at a=10m/s2 during 1s ==> dx = 0.5*10*1^2 = 5; dvx = 10*1 = 10
@@ -522,7 +522,7 @@ class ConstraintIMU_biasTest_Move_NonNullBias : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
+ CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -547,7 +547,7 @@ class ConstraintIMU_biasTest_Move_NonNullBias : public testing::Test
class ConstraintIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
{
public:
- wolf::TimeStamp t;
+ TimeStamp t;
SensorIMUPtr sen_imu;
ProblemPtr wolf_problem_ptr_;
CeresManager* ceres_manager_wolf_diff;
@@ -594,13 +594,13 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
Eigen::Vector6s data_imu, data_imu_initial;
origin_bias = Eigen::Vector6s::Random() * 0.001;
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0; // rad/s
- data_imu << -wolf::gravity(), rate_of_turn,0,0; //rotation only
+ Scalar rate_of_turn = 5 * M_PI/180.0; // rad/s
+ data_imu << -gravity(), rate_of_turn,0,0; //rotation only
data_imu_initial = data_imu;
// Expected state after one second integration
Eigen::Quaternions quat_comp(Eigen::Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data_imu.tail(3)*1);
+ quat_comp = quat_comp * v2q(data_imu.tail(3)*1);
expected_final_state << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.1s = 5 deg => 5 * M_PI/180
data_imu = data_imu + origin_bias; // bias measurements
@@ -611,7 +611,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
//===================================================== END{INITIALIZATION}
//===================================================== PROCESS DATA
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
+ CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -640,7 +640,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
class ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
{
public:
- wolf::TimeStamp t;
+ TimeStamp t;
SensorIMUPtr sen_imu;
ProblemPtr wolf_problem_ptr_;
CeresManager* ceres_manager_wolf_diff;
@@ -688,13 +688,13 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
Eigen::Vector6s data_imu, data_imu_initial;
origin_bias = Eigen::Vector6s::Random();
origin_bias << 0,0,0, 0,0,0;
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0; // rad/s
- data_imu << -wolf::gravity(), rate_of_turn,0,0; //rotation only
+ Scalar rate_of_turn = 5 * M_PI/180.0; // rad/s
+ data_imu << -gravity(), rate_of_turn,0,0; //rotation only
data_imu_initial = data_imu;
// Expected state after one second integration
Eigen::Quaternions quat_comp(Eigen::Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data_imu.tail(3)*1);
+ quat_comp = quat_comp * v2q(data_imu.tail(3)*1);
// rotated at 5 deg/s for 1s = 5 deg => 5 * M_PI/180
// no other acceleration : we should still be moving at initial velocity
@@ -710,7 +710,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
+ CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -741,7 +741,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
class ConstraintIMU_biasTest_Move_NonNullBiasRot : public testing::Test
{
public:
- wolf::TimeStamp t;
+ TimeStamp t;
SensorIMUPtr sen_imu;
ProblemPtr wolf_problem_ptr_;
CeresManager* ceres_manager_wolf_diff;
@@ -800,7 +800,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRot : public testing::Test
Scalar dt(0.001);
TimeStamp ts(0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
+ CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
while( ts.get() < 1 )
{
@@ -809,11 +809,11 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRot : public testing::Test
ts += dt;
rateOfTurn << .1, .2, .3; //to have rate of turn > 0 deg/s
- data_imu.head(3) = quat_current.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
+ data_imu.head(3) = quat_current.conjugate() * (- gravity()); //gravity measured, we have no other translation movement
data_imu.tail(3) = rateOfTurn;
//compute current orientaton taking this measure into account
- quat_current = quat_current * wolf::v2q(rateOfTurn*dt);
+ quat_current = quat_current * v2q(rateOfTurn*dt);
//set timestamp, add bias, set data and process
imu_ptr->setTimeStamp(ts);
@@ -838,7 +838,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRot : public testing::Test
class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
{
public:
- wolf::TimeStamp t;
+ TimeStamp t;
ProblemPtr problem;
CeresManagerPtr ceres_manager;
SensorBasePtr sensor;
@@ -944,11 +944,11 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
// rateOfTurn = Eigen::Vector3s::Random()*10; //to have rate of turn > 0.99 deg/s
rateOfTurn << 5, 10, 15; // deg/s
data_imu.tail<3>() = rateOfTurn * M_PI/180.0;
- data_imu.head<3>() = quat_imu.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
+ data_imu.head<3>() = quat_imu.conjugate() * (- gravity()); //gravity measured, we have no other translation movement
//compute odometry + current orientaton taking this measure into account
- quat_odo = quat_odo * wolf::v2q(data_imu.tail(3)*dt_imu);
- quat_imu = quat_imu * wolf::v2q(data_imu.tail(3)*dt_imu);
+ quat_odo = quat_odo * v2q(data_imu.tail(3)*dt_imu);
+ quat_imu = quat_imu * v2q(data_imu.tail(3)*dt_imu);
//set timestamp, add bias, set data and process
capture_imu->setTimeStamp(t_imu);
@@ -1023,7 +1023,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
{
public:
- wolf::TimeStamp t;
+ TimeStamp t;
SensorIMUPtr sen_imu;
SensorOdom3DPtr sen_odom3D;
ProblemPtr wolf_problem_ptr_;
@@ -1101,8 +1101,8 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
Scalar dt(0.0010), dt_odom(1.0);
TimeStamp ts(0.0), t_odom(0.0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
- wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, sen_odom3D->getNoiseCov(), 7, 6, nullptr);
+ CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
+ CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, sen_odom3D->getNoiseCov(), 7, 6, nullptr);
sen_odom3D->process(mot_ptr);
//first odometry data will be processed at this timestamp
t_odom.set(t_odom.get() + dt_odom);
@@ -1115,11 +1115,11 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
// PROCESS IMU DATA
// Time and data variables
ts.set(i*dt);
- data_imu.head<3>() = current_quatState.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
+ data_imu.head<3>() = current_quatState.conjugate() * (- gravity()); //gravity measured, we have no other translation movement
//compute odometry + current orientaton taking this measure into account
- odom_quat = odom_quat * wolf::v2q(data_imu.tail(3)*dt);
- current_quatState = current_quatState * wolf::v2q(data_imu.tail(3)*dt);
+ odom_quat = odom_quat * v2q(data_imu.tail(3)*dt);
+ current_quatState = current_quatState * v2q(data_imu.tail(3)*dt);
//set timestamp, add bias, set data and process
imu_ptr->setTimeStamp(ts);
@@ -1164,7 +1164,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
{
public:
- wolf::TimeStamp t;
+ TimeStamp t;
SensorIMUPtr sen_imu;
SensorOdom3DPtr sen_odom3D;
ProblemPtr wolf_problem_ptr_;
@@ -1240,8 +1240,8 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
Scalar dt(0.0010), dt_odom(1.0);
TimeStamp ts(0.0), t_odom(1.0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
- wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, 7, 6, nullptr);
+ CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
+ CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, 7, 6, nullptr);
sen_odom3D->process(mot_ptr);
//first odometry data will be processed at this timestamp
//t_odom.set(t_odom.get() + dt_odom);
@@ -1256,11 +1256,11 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
// Time and data variables
ts.set(i*dt);
- data_imu.head<3>() = current_quatState.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
+ data_imu.head<3>() = current_quatState.conjugate() * (- gravity()); //gravity measured, we have no other translation movement
//compute odometry + current orientaton taking this measure into account
- odom_quat = odom_quat * wolf::v2q(data_imu.tail(3)*dt);
- current_quatState = current_quatState * wolf::v2q(data_imu.tail(3)*dt);
+ odom_quat = odom_quat * v2q(data_imu.tail(3)*dt);
+ current_quatState = current_quatState * v2q(data_imu.tail(3)*dt);
//set timestamp, add bias, set data and process
imu_ptr->setTimeStamp(ts);
@@ -1295,11 +1295,11 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
/*data_imu.tail<3>() = rateOfTurn* M_PI/180.0;
randomPart = Eigen::Vector2s::Random(); //to have rate of turn > 0.99 deg/s
data_imu.segment<2>(3) += randomPart* M_PI/180.0;*/
- data_imu.head<3>() = current_quatState.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
+ data_imu.head<3>() = current_quatState.conjugate() * (- gravity()); //gravity measured, we have no other translation movement
//compute odometry + current orientaton taking this measure into account
- odom_quat = odom_quat * wolf::v2q(data_imu.tail(3)*dt);
- current_quatState = current_quatState * wolf::v2q(data_imu.tail(3)*dt);
+ odom_quat = odom_quat * v2q(data_imu.tail(3)*dt);
+ current_quatState = current_quatState * v2q(data_imu.tail(3)*dt);
//set timestamp, add bias, set data and process
imu_ptr->setTimeStamp(ts);
@@ -1359,11 +1359,11 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
}
TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
@@ -1376,7 +1376,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
KF1->getOPtr()->fix();
KF1->getVPtr()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
+ Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
std::string report;
@@ -1393,11 +1393,11 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
//==============================================================
//WOLF_INFO("Starting error bias 1e-4")
@@ -1411,11 +1411,11 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
//==============================================================
//WOLF_INFO("Starting error bias 1e-2")
@@ -1429,11 +1429,11 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
//==============================================================
//WOLF_INFO("Starting error bias 1e-1")
@@ -1449,11 +1449,11 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
}
}
@@ -1475,11 +1475,11 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_in
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
}
TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
@@ -1492,7 +1492,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er
KF1->getOPtr()->fix();
KF1->getVPtr()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
+ Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
std::string report;
@@ -1509,11 +1509,11 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er
report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
//==============================================================
//WOLF_INFO("Starting error bias 1e-4")
@@ -1527,11 +1527,11 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er
report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
//==============================================================
//WOLF_INFO("Starting error bias 1e-2")
@@ -1545,11 +1545,11 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er
report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
//==============================================================
//WOLF_INFO("Starting error bias 1e-1")
@@ -1565,11 +1565,11 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er
report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS) //Acc bias
+ ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS) //Gyro bias
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), Constants::EPS)
+ ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), Constants::EPS)
}
}
@@ -1605,7 +1605,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
+ Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
std::string report;
@@ -1686,7 +1686,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
+ Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
std::string report;
@@ -1789,7 +1789,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
+ Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
//==============================================================
@@ -1888,7 +1888,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
+ Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
//==============================================================
@@ -1988,7 +1988,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
+ Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
//==============================================================
@@ -2088,7 +2088,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
+ Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
//==============================================================
@@ -2224,7 +2224,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::Matrix<Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
@@ -2268,9 +2268,9 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::Matrix<Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
@@ -2314,9 +2314,9 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
}
//jacobian matrix rank deficient here - estimating both initial and final velocity
@@ -2342,11 +2342,11 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
+ ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, Constants::EPS*100)
}
//jacobian matrix rank deficient here - estimating both initial and final velocity
@@ -2370,11 +2370,11 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
}
//jacobian matrix rank deficient here - estimating both initial and final velocity
@@ -2398,13 +2398,13 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
+ ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), Constants::EPS*10000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
+ ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), Constants::EPS*10000)
Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
+ ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, Constants::EPS*100)
}
TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
@@ -2428,16 +2428,16 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
+ ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::Matrix<Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
@@ -2486,16 +2486,16 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
+ ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::Matrix<Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
@@ -2544,16 +2544,16 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
+ ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::Matrix<Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
@@ -2604,7 +2604,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::Matrix<Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
@@ -2648,9 +2648,9 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::Matrix<Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
@@ -2690,11 +2690,11 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
}
TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
@@ -2717,13 +2717,13 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
+ ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, Constants::EPS*100)
}
TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
@@ -2746,11 +2746,11 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
}
TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
@@ -2773,13 +2773,13 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
+ ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), Constants::EPS*10000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
+ ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), Constants::EPS*10000)
Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
+ ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, Constants::EPS*100)
}
TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
@@ -2803,16 +2803,16 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager->computeCovariances(ALL);
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), Constants::EPS*1000)
Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
+ ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::Matrix<Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
Eigen::MatrixXs covX(10,10);
//get data from covariance blocks
diff --git a/src/test/gtest_constraint_absolute.cpp b/src/test/gtest_constraint_absolute.cpp
index 52adcd099403218395990b69ed857da62259d507..b363cbbc6434975f88a1090d965e4ef2a51a1e2f 100644
--- a/src/test/gtest_constraint_absolute.cpp
+++ b/src/test/gtest_constraint_absolute.cpp
@@ -28,7 +28,7 @@ Vector10s pose9toPose10(Vector9s _pose9)
Vector9s pose(Vector9s::Random());
Vector10s pose10 = pose9toPose10(pose);
Vector9s data_var((Vector9s() << 0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2).finished());
-Eigen::Matrix<wolf::Scalar, 9, 9> data_cov = data_var.asDiagonal();
+Eigen::Matrix<Scalar, 9, 9> data_cov = data_var.asDiagonal();
// perturbated priors
Vector10s x0 = pose9toPose10(pose + Vector9s::Random()*0.25);
diff --git a/src/test/gtest_constraint_autodiff_trifocal.cpp b/src/test/gtest_constraint_autodiff_trifocal.cpp
index 76c53c106e353f6cd0cee63b7de461d015f8b325..135a3c0e82ea26a2cd9f103e4f82547f611daf2d 100644
--- a/src/test/gtest_constraint_autodiff_trifocal.cpp
+++ b/src/test/gtest_constraint_autodiff_trifocal.cpp
@@ -184,7 +184,7 @@ TEST_F(ConstraintAutodiffTrifocalTest, expectation)
Vector3s _c1Tc2 = _c1Hc2.block(0,3,3,1);
// Essential matrix, ground truth (fwd and bkwd)
- Matrix3s _c1Ec2 = wolf::skew(_c1Tc2) * _c1Rc2;
+ Matrix3s _c1Ec2 = skew(_c1Tc2) * _c1Rc2;
Matrix3s _c2Ec1 = _c1Ec2.transpose();
// Expected values
@@ -218,15 +218,15 @@ TEST_F(ConstraintAutodiffTrifocalTest, expectation)
ASSERT_TRUE(pll(0)<1e-5);
// Point-line-point
- Eigen::Vector3s plp = wolf::skew(_m3) * m1Tt * _p2;
+ Eigen::Vector3s plp = skew(_m3) * m1Tt * _p2;
ASSERT_MATRIX_APPROX(plp, Vector3s::Zero(), 1e-8);
// Point-point-line
- Eigen::Vector3s ppl = _p3.transpose() * m1Tt * wolf::skew(_m2);
+ Eigen::Vector3s ppl = _p3.transpose() * m1Tt * skew(_m2);
ASSERT_MATRIX_APPROX(ppl, Vector3s::Zero(), 1e-8);
// Point-point-point
- Eigen::Matrix3s ppp = wolf::skew(_m3) * m1Tt * wolf::skew(_m2);
+ Eigen::Matrix3s ppp = skew(_m3) * m1Tt * skew(_m2);
ASSERT_MATRIX_APPROX(ppp, Matrix3s::Zero(), 1e-8);
// check epipolars
diff --git a/src/test/gtest_feature_IMU.cpp b/src/test/gtest_feature_IMU.cpp
index 9f7e9c444d28dbc28efa29d8a7c01806731a9f26..46aa51421709c64d25c91737e952646fd66e911c 100644
--- a/src/test/gtest_feature_IMU.cpp
+++ b/src/test/gtest_feature_IMU.cpp
@@ -9,21 +9,23 @@
#include "utils_gtest.h"
#include "../src/logging.h"
+using namespace wolf;
+
class FeatureIMU_test : public testing::Test
{
public: //These can be accessed in fixtures
- wolf::ProblemPtr problem;
- wolf::TimeStamp ts;
- wolf::CaptureIMUPtr imu_ptr;
+ ProblemPtr problem;
+ TimeStamp ts;
+ CaptureIMUPtr imu_ptr;
Eigen::VectorXs state_vec;
Eigen::VectorXs delta_preint;
- Eigen::Matrix<wolf::Scalar,9,9> delta_preint_cov;
- std::shared_ptr<wolf::FeatureIMU> feat_imu;
- wolf::FrameBasePtr last_frame;
- wolf::FrameBasePtr origin_frame;
+ Eigen::Matrix<Scalar,9,9> delta_preint_cov;
+ std::shared_ptr<FeatureIMU> feat_imu;
+ FrameBasePtr last_frame;
+ FrameBasePtr origin_frame;
Eigen::MatrixXs dD_db_jacobians;
- wolf::ProcessorBasePtr processor_ptr_;
+ ProcessorBasePtr processor_ptr_;
//a new of this will be created for each new test
virtual void SetUp()
@@ -110,16 +112,16 @@ TEST_F(FeatureIMU_test, check_frame)
using namespace wolf;
FrameBasePtr left_frame = feat_imu->getFramePtr();
- wolf::TimeStamp t;
+ TimeStamp t;
left_frame->getTimeStamp(t);
origin_frame->getTimeStamp(ts);
- ASSERT_NEAR(t.get(), ts.get(), wolf::Constants::EPS_SMALL) << "t != ts \t t=" << t << "\t ts=" << ts << std::endl;
+ ASSERT_NEAR(t.get(), ts.get(), Constants::EPS_SMALL) << "t != ts \t t=" << t << "\t ts=" << ts << std::endl;
ASSERT_TRUE(origin_frame->isKey());
ASSERT_TRUE(last_frame->isKey());
ASSERT_TRUE(left_frame->isKey());
- wolf::StateBlockPtr origin_pptr, origin_optr, origin_vptr, left_pptr, left_optr, left_vptr;
+ StateBlockPtr origin_pptr, origin_optr, origin_vptr, left_pptr, left_optr, left_vptr;
origin_pptr = origin_frame->getPPtr();
origin_optr = origin_frame->getOPtr();
origin_vptr = origin_frame->getVPtr();
@@ -127,10 +129,10 @@ TEST_F(FeatureIMU_test, check_frame)
left_optr = left_frame->getOPtr();
left_vptr = left_frame->getVPtr();
- ASSERT_MATRIX_APPROX(origin_pptr->getState(), left_pptr->getState(), wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(origin_pptr->getState(), left_pptr->getState(), Constants::EPS_SMALL);
Eigen::Map<const Eigen::Quaternions> origin_Quat(origin_optr->getState().data()), left_Quat(left_optr->getState().data());
- ASSERT_QUATERNION_APPROX(origin_Quat, left_Quat, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(origin_vptr->getState(), left_vptr->getState(), wolf::Constants::EPS_SMALL);
+ ASSERT_QUATERNION_APPROX(origin_Quat, left_Quat, Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(origin_vptr->getState(), left_vptr->getState(), Constants::EPS_SMALL);
ASSERT_EQ(origin_frame->id(), left_frame->id());
}
@@ -142,7 +144,7 @@ TEST_F(FeatureIMU_test, access_members)
Eigen::VectorXs delta(10);
//dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2; dz = 0.5*9.8*0.1^2 = 0.049; dvz = 9.8*0.1 = 0.98
delta << 0.01,0,0.049, 0,0,0,1, 0.2,0,0.98;
- ASSERT_MATRIX_APPROX(feat_imu->getMeasurement().transpose(), delta.transpose(), wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(feat_imu->getMeasurement().transpose(), delta.transpose(), Constants::EPS);
}
//TEST_F(FeatureIMU_test, addConstraint)
diff --git a/src/test/gtest_local_param.cpp b/src/test/gtest_local_param.cpp
index bdd413a8223216c5c394683b2d9a82844900ccee..828b53d0f707b8949e1b711644f73613e077fb70 100644
--- a/src/test/gtest_local_param.cpp
+++ b/src/test/gtest_local_param.cpp
@@ -65,7 +65,7 @@ TEST(TestLocalParametrization, QuaternionLocal)
ASSERT_DOUBLE_EQ(qo_m.norm(), 1);
- Quaternions qref = Map<Quaternions>(q.data()) * wolf::v2q(da);
+ Quaternions qref = Map<Quaternions>(q.data()) * v2q(da);
ASSERT_TRUE(qo_m.isApprox( qref.coeffs() ) );
Qpar_loc.computeJacobian(q_m,J);
@@ -110,7 +110,7 @@ TEST(TestLocalParametrization, QuaternionGlobal)
ASSERT_DOUBLE_EQ(qo_m.norm(), 1);
- Quaternions qref = wolf::v2q(da) * Map<Quaternions>(q.data());
+ Quaternions qref = v2q(da) * Map<Quaternions>(q.data());
ASSERT_TRUE(qo_m.isApprox( qref.coeffs() ) );
Qpar_glob.computeJacobian(q_m,J);
diff --git a/src/test/gtest_processor_IMU.cpp b/src/test/gtest_processor_IMU.cpp
index 38552b3334d5e0798f29aed417355e11d4ce94f7..08ce49a3caa2c607232a9d61dcf56046e2d5e3d9 100644
--- a/src/test/gtest_processor_IMU.cpp
+++ b/src/test/gtest_processor_IMU.cpp
@@ -20,20 +20,21 @@
#include <iostream>
using namespace Eigen;
+using namespace wolf;
class ProcessorIMUt : public testing::Test
{
public: //These can be accessed in fixtures
- wolf::ProblemPtr problem;
- wolf::SensorBasePtr sensor_ptr;
- wolf::TimeStamp t;
- wolf::Scalar mti_clock, tmp;
- wolf::Scalar dt;
+ ProblemPtr problem;
+ SensorBasePtr sensor_ptr;
+ TimeStamp t;
+ Scalar mti_clock, tmp;
+ Scalar dt;
Vector6s data;
Matrix6s data_cov;
VectorXs x0;
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr;
+ std::shared_ptr<CaptureIMU> cap_imu_ptr;
//a new of this will be created for each new test
virtual void SetUp()
@@ -43,7 +44,7 @@ class ProcessorIMUt : public testing::Test
using std::shared_ptr;
using std::make_shared;
using std::static_pointer_cast;
- using namespace wolf::Constants;
+ using namespace Constants;
std::string wolf_root = _WOLF_ROOT_DIR;
@@ -130,7 +131,7 @@ TEST(ProcessorIMU, voteForKeyFrame)
using std::shared_ptr;
using std::make_shared;
using std::static_pointer_cast;
- using namespace wolf::Constants;
+ using namespace Constants;
std::string wolf_root = _WOLF_ROOT_DIR;
@@ -161,7 +162,7 @@ TEST(ProcessorIMU, voteForKeyFrame)
data_cov(0,0) = 0.5;
// Create the captureIMU to store the IMU data arriving from (sensor / callback / file / etc.)
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
+ std::shared_ptr<CaptureIMU> cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
// Time
// we want more than one data to integrate otherwise covariance will be 0
@@ -238,7 +239,7 @@ TEST_F(ProcessorIMUt, interpolate)
// problem->getProcessorMotionPtr()->getBuffer().print(0,1,1,0);
-class P : wolf::ProcessorIMU
+class P : ProcessorIMU
{
public:
Motion interp(const Motion& ref, Motion& sec, TimeStamp ts)
@@ -274,14 +275,14 @@ TEST_F(ProcessorIMUt, acc_x)
x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
Vector6s b; b << 0,0,0, 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, Constants::EPS_SMALL);
}
TEST_F(ProcessorIMUt, acc_y)
{
- // last part of this test fails with precision wolf::Constants::EPS_SMALL beacause error is in 1e-12
+ // last part of this test fails with precision Constants::EPS_SMALL beacause error is in 1e-12
// difference hier is that we integrate over 1ms periods
t.set(0); // clock in 0,1 ms ticks
@@ -300,9 +301,9 @@ TEST_F(ProcessorIMUt, acc_y)
x << 0,0.00001,0, 0,0,0,1, 0,0.02,0; // advanced at a=20m/s2 during 0.001s ==> dx = 0.5*20*0.001^2 = 0.00001; dvx = 20*0.001 = 0.02
Vector6s b; b<< 0,0,0, 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, Constants::EPS_SMALL);
//do so for 5s
const unsigned int iter = 1000; //how many ms
@@ -314,9 +315,9 @@ TEST_F(ProcessorIMUt, acc_y)
// advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20
x << 0,10,0, 0,0,0,1, 0,20,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, Constants::EPS);
}
TEST_F(ProcessorIMUt, acc_z)
@@ -337,9 +338,9 @@ TEST_F(ProcessorIMUt, acc_z)
x << 0,0,0.01, 0,0,0,1, 0,0,0.2; // advanced at a=2m/s2 during 0.1s ==> dz = 0.5*2*0.1^2 = 0.01; dvz = 2*0.1 = 0.2
Vector6s b; b<< 0,0,0, 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, Constants::EPS_SMALL);
//do so for 5s
const unsigned int iter = 50; //how 0.1s
@@ -351,9 +352,9 @@ TEST_F(ProcessorIMUt, acc_z)
// advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10
x << 0,0,25, 0,0,0,1, 0,0,10;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, Constants::EPS);
}
@@ -371,21 +372,21 @@ TEST_F(ProcessorIMUt, check_Covariance)
sensor_ptr->process(cap_imu_ptr);
VectorXs delta_preint(problem->getProcessorMotionPtr()->getMotion().delta_integr_);
-// Matrix<wolf::Scalar,9,9> delta_preint_cov = problem->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
+// Matrix<Scalar,9,9> delta_preint_cov = problem->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
- ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
-// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXs::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
+ ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, Constants::EPS_SMALL));
+// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXs::Zero(9,9), Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
}
TEST_F(ProcessorIMUt, gyro_x)
{
- wolf::Scalar dt(0.001);
+ Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXs P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ Scalar rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -394,23 +395,23 @@ TEST_F(ProcessorIMUt, gyro_x)
// Expected state after one integration
Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
VectorXs x(10);
x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS_SMALL);
//do so for 5s
const unsigned int iter = 1000; //how many ms
for(int i = 1; i < iter; i++) //already did one integration above
{
// quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
+ data.head(3) = rot.conjugate() * (- gravity());
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
cap_imu_ptr->setData(data);
@@ -418,17 +419,17 @@ TEST_F(ProcessorIMUt, gyro_x)
}
x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS);
}
TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
{
// time
- wolf::Scalar dt(0.001);
+ Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
// bias
- wolf::Scalar abx = 0.002;
+ Scalar abx = 0.002;
Vector6s bias; bias << abx,0,0, 0,0,0;
Vector3s acc_bias = bias.head(3);
// state
@@ -443,8 +444,8 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
// data
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << acc_bias - wolf::gravity(), rate_of_turn, 0, 0; // measure gravity
+ Scalar rate_of_turn = 5 * M_PI/180.0;
+ data << acc_bias - gravity(), rate_of_turn, 0, 0; // measure gravity
cap_imu_ptr->setData(data);
cap_imu_ptr->setTimeStamp(0.001);
@@ -452,7 +453,7 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
// Expected state after one integration
Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
VectorXs x_true(10);
x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
@@ -460,17 +461,17 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
VectorXs x_est(10);
x_est = problem->getCurrentState().head(10);
- ASSERT_MATRIX_APPROX(x_est.transpose() , x_true.transpose(), wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(x_est.transpose() , x_true.transpose(), Constants::EPS);
//do so for 5s
const unsigned int iter = 1000; //how many ms
for(int i = 1; i < iter; i++) //already did one integration above
{
// quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
+ data.head(3) = rot.conjugate() * (- gravity()) + acc_bias;
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
cap_imu_ptr->setData(data);
@@ -478,15 +479,15 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
}
x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x_true, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x_true, Constants::EPS);
}
TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
{
- wolf::Scalar dt(0.001);
+ Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
- wolf::Scalar abx(0.002), aby(0.01);
+ Scalar abx(0.002), aby(0.01);
x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXs P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
@@ -497,9 +498,9 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ Scalar rate_of_turn = 5 * M_PI/180.0;
// data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity
- data << acc_bias - wolf::gravity(), rate_of_turn*1.5, 0, 0; // measure gravity
+ data << acc_bias - gravity(), rate_of_turn*1.5, 0, 0; // measure gravity
cap_imu_ptr->setData(data);
cap_imu_ptr->setTimeStamp(0.001);
@@ -507,12 +508,12 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
// Expected state after one integration
Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
VectorXs x(10);
x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;//, abx,aby,0, 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);// << "expected state : " << problem->getCurrentState().transpose()
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS_SMALL);// << "expected state : " << problem->getCurrentState().transpose()
// << "\n estimated state : " << x.transpose();
//do so for 5s
@@ -520,10 +521,10 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
for(int i = 1; i < iter; i++) //already did one integration above
{
// quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
+ data.head(3) = rot.conjugate() * (- gravity()) + acc_bias;
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
cap_imu_ptr->setData(data);
@@ -531,20 +532,20 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
}
x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);// << "estimated state is : \n" << problem->getCurrentState().transpose() <<
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS);// << "estimated state is : \n" << problem->getCurrentState().transpose() <<
// "\n expected state is : \n" << x.transpose() << std::endl;
}
TEST_F(ProcessorIMUt, gyro_z)
{
- wolf::Scalar dt(0.001);
+ Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXs P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
+ Scalar rate_of_turn = 5 * M_PI/180.0;
+ data << -gravity(), 0, 0, rate_of_turn; // measure gravity!
cap_imu_ptr->setData(data);
cap_imu_ptr->setTimeStamp(0.001);
@@ -552,26 +553,26 @@ TEST_F(ProcessorIMUt, gyro_z)
// Expected state after one integration
Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
VectorXs x(10);
x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS_SMALL);
//do so for 5s
const unsigned int iter = 1000; //how many ms
for(int i = 1; i < iter; i++) //already did one integration above
{
// quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
sensor_ptr->process(cap_imu_ptr);
}
x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS);
}
TEST_F(ProcessorIMUt, gyro_xyz)
@@ -584,12 +585,12 @@ TEST_F(ProcessorIMUt, gyro_xyz)
Vector3s tmp_vec; //will be used to store rate of turn data
Quaternions quat_comp(Quaternions::Identity());
Matrix3s R0(Matrix3s::Identity());
- wolf::Scalar time = 0;
+ Scalar time = 0;
const unsigned int x_rot_vel = 5;
const unsigned int y_rot_vel = 6;
const unsigned int z_rot_vel = 13;
- wolf::Scalar tmpx, tmpy, tmpz;
+ Scalar tmpx, tmpy, tmpz;
/*
ox oy oz evolution in degrees (for understanding) --> converted in rad
@@ -605,7 +606,7 @@ TEST_F(ProcessorIMUt, gyro_xyz)
wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
*/
- const wolf::Scalar dt = 0.001;
+ const Scalar dt = 0.001;
for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++)
{
@@ -617,11 +618,11 @@ TEST_F(ProcessorIMUt, gyro_xyz)
tmp_vec << tmpx, tmpy, tmpz;
// quaternion composition
- quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
- R0 = R0 * wolf::v2R(tmp_vec*dt);
+ quat_comp = quat_comp * v2q(tmp_vec*dt);
+ R0 = R0 * v2R(tmp_vec*dt);
// use processorIMU
Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
+ data.head(3) = rot.conjugate() * (- gravity()); //gravity measured
data.tail(3) = tmp_vec;
cap_imu_ptr->setData(data);
@@ -638,38 +639,38 @@ TEST_F(ProcessorIMUt, gyro_xyz)
*/
// validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
- Quaternions R2quat(wolf::v2q(wolf::R2v(R0)));
+ Quaternions R2quat(v2q(R2v(R0)));
Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
- ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS);// << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
+ ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, Constants::EPS);// << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
VectorXs x(10);
x << 0,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0,0,0;
Quaternions result_quat(problem->getCurrentState().data() + 3);
- //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
+ //std::cout << "final orientation : " << q2v(result_quat).transpose() << std::endl;
//check position part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), Constants::EPS);
//check orientation part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , Constants::EPS);
//check velocity and bias parts
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), Constants::EPS);
}
TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
{
- wolf::Scalar dt(0.001);
+ Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
MatrixXs P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
+ Scalar rate_of_turn = 5 * M_PI/180.0;
+ data << -gravity(), 0, 0, rate_of_turn; // measure gravity!
cap_imu_ptr->setData(data);
cap_imu_ptr->setTimeStamp(0.001);
@@ -677,37 +678,37 @@ TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
// Expected state after one integration
Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
VectorXs x(10);
x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS_SMALL);
//do so for 1s
const unsigned int iter = 1000; //how many ms
for(int i = 1; i < iter; i++) //already did one integration above
{
// quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 5.000
sensor_ptr->process(cap_imu_ptr);
}
x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS);
}
TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
{
- wolf::Scalar dt(0.001);
+ Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
MatrixXs P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ Scalar rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -716,23 +717,23 @@ TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
// Expected state after one integration
Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
VectorXs x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS_SMALL);
//do so for 1s
const unsigned int iter = 1000; //how many ms
for(int i = 1; i < iter; i++) //already did one integration above
{
// quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
+ data.head(3) = rot.conjugate() * (- gravity());
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
cap_imu_ptr->setData(data);
@@ -740,18 +741,18 @@ TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
}
x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS);
}
TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
{
- wolf::Scalar dt(0.001);
+ Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
MatrixXs P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ Scalar rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -760,23 +761,23 @@ TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
// Expected state after one integration
Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
VectorXs x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS_SMALL);
//do so for 1s
const unsigned int iter = 1000; //how many ms
for(int i = 1; i < iter; i++) //already did one integration above
{
// quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
+ data.head(3) = rot.conjugate() * (- gravity());
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
cap_imu_ptr->setData(data);
@@ -784,18 +785,18 @@ TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
}
x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS);
}
TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
{
- wolf::Scalar dt(0.001);
+ Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
MatrixXs P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ Scalar rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -804,23 +805,23 @@ TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
// Expected state after one integration
Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
VectorXs x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS_SMALL);
//do so for 1s
const unsigned int iter = 1000; //how many ms
for(int i = 1; i < iter; i++) //already did one integration above
{
// quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
+ data.head(3) = rot.conjugate() * (- gravity());
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
cap_imu_ptr->setData(data);
@@ -828,7 +829,7 @@ TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
}
x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS);
}
TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
@@ -841,12 +842,12 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
Vector3s tmp_vec; //will be used to store rate of turn data
Quaternions quat_comp(Quaternions::Identity());
Matrix3s R0(Matrix3s::Identity());
- wolf::Scalar time = 0;
+ Scalar time = 0;
const unsigned int x_rot_vel = 5;
const unsigned int y_rot_vel = 6;
const unsigned int z_rot_vel = 13;
- wolf::Scalar tmpx, tmpy, tmpz;
+ Scalar tmpx, tmpy, tmpz;
/*
ox oy oz evolution in degrees (for understanding) --> converted in rad
@@ -862,7 +863,7 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
*/
- const wolf::Scalar dt = 0.001;
+ const Scalar dt = 0.001;
for(unsigned int data_iter = 0; data_iter < 1000; data_iter ++)
{
@@ -874,11 +875,11 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
tmp_vec << tmpx, tmpy, tmpz;
// quaternion composition
- quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
- R0 = R0 * wolf::v2R(tmp_vec*dt);
+ quat_comp = quat_comp * v2q(tmp_vec*dt);
+ R0 = R0 * v2R(tmp_vec*dt);
// use processorIMU
Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
+ data.head(3) = rot.conjugate() * (- gravity()); //gravity measured
data.tail(3) = tmp_vec;
cap_imu_ptr->setData(data);
@@ -895,40 +896,40 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
*/
// validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
- Quaternions R2quat(wolf::v2q(wolf::R2v(R0)));
+ Quaternions R2quat(v2q(R2v(R0)));
Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
- ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS); // << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
+ ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, Constants::EPS); // << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
VectorXs x(10);
//rotation + translation due to initial velocity
x << 2,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 2,0,0;
Quaternions result_quat(problem->getCurrentState().data() + 3);
- //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
+ //std::cout << "final orientation : " << q2v(result_quat).transpose() << std::endl;
//check position part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), Constants::EPS);
//check orientation part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , Constants::EPS);
//check velocity
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), Constants::EPS);
}
TEST_F(ProcessorIMUt, gyro_x_acc_x)
{
- wolf::Scalar dt(0.001);
+ Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXs P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 1, 0, -wolf::gravity()(2), rate_of_turn, 0, 0; // measure gravity
+ Scalar rate_of_turn = 5 * M_PI/180.0;
+ data << 1, 0, -gravity()(2), rate_of_turn, 0, 0; // measure gravity
cap_imu_ptr->setData(data);
cap_imu_ptr->setTimeStamp(0.001);
@@ -936,7 +937,7 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
// Expected state after one integration
Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
VectorXs x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on x axis
@@ -944,7 +945,7 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
// v = a*dt = 0.001
x << 0.0000005,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0.001,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
// "\n current x is : \n" << x.transpose() << std::endl;
//do so for 1s
@@ -952,10 +953,10 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
for(int i = 2; i <= iter; i++) //already did one integration above
{
// quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<1,0,0).finished();
+ data.head(3) = rot.conjugate() * (- gravity()) + (Vector3s()<<1,0,0).finished();
cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
cap_imu_ptr->setData(data);
@@ -965,19 +966,19 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
// translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on x axis
// v = a*dt = 1
x << 0.5,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 1,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS);
}
TEST_F(ProcessorIMUt, gyro_y_acc_y)
{
- wolf::Scalar dt(0.001);
+ Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXs P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 1, -wolf::gravity()(2), 0, rate_of_turn, 0; // measure gravity
+ Scalar rate_of_turn = 5 * M_PI/180.0;
+ data << 0, 1, -gravity()(2), 0, rate_of_turn, 0; // measure gravity
cap_imu_ptr->setData(data);
cap_imu_ptr->setTimeStamp(0.001);
@@ -985,7 +986,7 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
// Expected state after one integration
Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
VectorXs x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on y axis
@@ -993,7 +994,7 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
// v = a*dt = 0.001
x << 0,0.0000005,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0.001,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
// "\n current x is : \n" << x.transpose() << std::endl;
//do so for 1s
@@ -1001,10 +1002,10 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
for(int i = 2; i <= iter; i++) //already did one integration above
{
// quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,1,0).finished();
+ data.head(3) = rot.conjugate() * (- gravity()) + (Vector3s()<<0,1,0).finished();
cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
cap_imu_ptr->setData(data);
@@ -1014,19 +1015,19 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
// translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on y axis
// v = a*dt = 1
x << 0,0.5,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,1,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS);
}
TEST_F(ProcessorIMUt, gyro_z_acc_z)
{
- wolf::Scalar dt(0.001);
+ Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXs P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, -wolf::gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity
+ Scalar rate_of_turn = 5 * M_PI/180.0;
+ data << 0, 0, -gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity
cap_imu_ptr->setData(data);
cap_imu_ptr->setTimeStamp(0.001);
@@ -1034,7 +1035,7 @@ TEST_F(ProcessorIMUt, gyro_z_acc_z)
// Expected state after one integration
Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
VectorXs x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on z axis
@@ -1042,17 +1043,17 @@ TEST_F(ProcessorIMUt, gyro_z_acc_z)
// v = a*dt = 0.001
x << 0,0,0.0000005, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0.001;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS);
//do so for 1s
const unsigned int iter = 1000; //how many ms
for(int i = 2; i <= iter; i++) //already did one integration above
{
// quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+ quat_comp = quat_comp * v2q(data.tail(3)*dt);
Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,0,1).finished();
+ data.head(3) = rot.conjugate() * (- gravity()) + (Vector3s()<<0,0,1).finished();
cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
cap_imu_ptr->setData(data);
@@ -1062,7 +1063,7 @@ TEST_F(ProcessorIMUt, gyro_z_acc_z)
// translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on z axis
// v = a*dt = 1
x << 0,0,0.5, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,1;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, Constants::EPS);
}
int main(int argc, char **argv)
diff --git a/src/test/gtest_processor_IMU_jacobians.cpp b/src/test/gtest_processor_IMU_jacobians.cpp
index dc8677e169f6fa957915668176de2c6b195e377c..6a7472a150652a0414cd0ec57f11cf8452c9ab26 100644
--- a/src/test/gtest_processor_IMU_jacobians.cpp
+++ b/src/test/gtest_processor_IMU_jacobians.cpp
@@ -33,11 +33,11 @@ class ProcessorIMU_jacobians : public testing::Test
public:
TimeStamp t;
Eigen::Vector6s data_;
- Eigen::Matrix<wolf::Scalar,10,1> Delta0;
- wolf::Scalar ddelta_bias;
- wolf::Scalar dt;
- Eigen::Matrix<wolf::Scalar,9,1> Delta_noise;
- Eigen::Matrix<wolf::Scalar,9,1> delta_noise;
+ Eigen::Matrix<Scalar,10,1> Delta0;
+ Scalar ddelta_bias;
+ Scalar dt;
+ Eigen::Matrix<Scalar,9,1> Delta_noise;
+ Eigen::Matrix<Scalar,9,1> delta_noise;
struct IMU_jac_bias bias_jac;
struct IMU_jac_deltas deltas_jac;
@@ -57,7 +57,7 @@ class ProcessorIMU_jacobians : public testing::Test
virtual void SetUp()
{
//SetUp for jacobians
- wolf::Scalar deg_to_rad = M_PI/180.0;
+ Scalar deg_to_rad = M_PI/180.0;
data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad;
// Wolf problem
@@ -70,8 +70,8 @@ class ProcessorIMU_jacobians : public testing::Test
dt = 0.001;
//defining a random Delta to begin with (not to use Origin point)
- Eigen::Matrix<wolf::Scalar,10,1> Delta0;
- Delta0 = Eigen::Matrix<wolf::Scalar,10,1>::Random();
+ Eigen::Matrix<Scalar,10,1> Delta0;
+ Delta0 = Eigen::Matrix<Scalar,10,1>::Random();
Delta0.head<3>() = Delta0.head<3>()*100;
Delta0.tail<3>() = Delta0.tail<3>()*10;
Eigen::Vector3s ang0, ang;
@@ -116,9 +116,9 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
public:
TimeStamp t;
Eigen::Vector6s data_;
- Eigen::Matrix<wolf::Scalar,10,1> Delta0;
- wolf::Scalar ddelta_bias2;
- wolf::Scalar dt;
+ Eigen::Matrix<Scalar,10,1> Delta0;
+ Scalar ddelta_bias2;
+ Scalar dt;
struct IMU_jac_bias bias_jac2;
void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq0, Eigen::Map<Eigen::Quaternions>& _dq0){
@@ -137,7 +137,7 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
virtual void SetUp()
{
//SetUp for jacobians
- wolf::Scalar deg_to_rad = M_PI/180.0;
+ Scalar deg_to_rad = M_PI/180.0;
data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad;
// Wolf problem
@@ -150,8 +150,8 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
dt = 0.001;
//defining a random Delta to begin with (not to use Origin point)
- Eigen::Matrix<wolf::Scalar,10,1> Delta0;
- Delta0 = Eigen::Matrix<wolf::Scalar,10,1>::Random();
+ Eigen::Matrix<Scalar,10,1> Delta0;
+ Delta0 = Eigen::Matrix<Scalar,10,1>::Random();
Delta0.head<3>() = Delta0.head<3>()*100;
Delta0.tail<3>() = Delta0.tail<3>()*10;
Eigen::Vector3s ang0, ang;
@@ -276,7 +276,7 @@ TEST_F(ProcessorIMU_jacobians, dDq_dab)
dDq_dab.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
}
- ASSERT_TRUE(dDq_dab.isZero(wolf::Constants::EPS)) << "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
+ ASSERT_TRUE(dDq_dab.isZero(Constants::EPS)) << "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
}
TEST_F(ProcessorIMU_jacobians, dDq_dwb_noise_1Em8_)