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

Remove many wolf:: specifiers to avoid duplicates

parent b1431e58
No related branches found
No related tags found
1 merge request!198Typedef enum
Pipeline #
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 329 additions and 326 deletions
src/examples/test_constraint_imu.cpp
+ 7
− 7
View file @ 8c3caeb7
......@@ -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);
......
src/test/gtest_constraint_IMU.cpp
+ 145
− 145
View file @ 8c3caeb7
This diff is collapsed.
src/test/gtest_constraint_absolute.cpp
+ 1
− 1
View file @ 8c3caeb7
......@@ -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);
......
src/test/gtest_constraint_autodiff_trifocal.cpp
+ 4
− 4
View file @ 8c3caeb7
......@@ -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
......
src/test/gtest_feature_IMU.cpp
+ 17
− 15
View file @ 8c3caeb7
......@@ -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)
......
src/test/gtest_local_param.cpp
+ 2
− 2
View file @ 8c3caeb7
......@@ -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);
......
src/test/gtest_processor_IMU.cpp
+ 138
− 137
View file @ 8c3caeb7
This diff is collapsed.
src/test/gtest_processor_IMU_jacobians.cpp
+ 15
− 15
View file @ 8c3caeb7
......@@ -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_)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment