diff --git a/demos/demo_factor_imu.cpp b/demos/demo_factor_imu.cpp
index 36e6bfa52385e37e51a8d1616e3c20640f35b371..f99b44448fb07d337a0319b3bb451560daf19c76 100644
--- a/demos/demo_factor_imu.cpp
+++ b/demos/demo_factor_imu.cpp
@@ -24,7 +24,7 @@ int main(int argc, char** argv)
bool c0(false), c1(false);// c2(true), c3(true), c4(true);
// Wolf problem
ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
- Eigen::VectorXs IMU_extrinsics(7);
+ Eigen::VectorXd IMU_extrinsics(7);
IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, shared_ptr<IntrinsicsBase>());
wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
@@ -38,30 +38,30 @@ int main(int argc, char** argv)
// Time and data variables
TimeStamp t;
- Eigen::Vector6s data_;
- Scalar mpu_clock = 0;
+ Eigen::Vector6d data_;
+ double mpu_clock = 0;
t.set(mpu_clock);
// Set the origin
- Eigen::VectorXs x0(16);
+ Eigen::VectorXd x0(16);
x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,.001, 0,0,.002; // Try some non-zero biases
wolf_problem_ptr_->getProcessorMotion()->setOrigin(x0, t); //this also creates a keyframe at origin
wolf_problem_ptr_->getTrajectory()->getFrameList().front()->fix(); //fix the keyframe at origin
TimeStamp ts(0);
- Eigen::VectorXs origin_state = x0;
+ Eigen::VectorXd origin_state = x0;
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity()) );
+ CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6d::Identity()) );
imu_ptr->setFrame(wolf_problem_ptr_->getTrajectory()->getFrameList().back());
// set variables
using namespace std;
- Eigen::VectorXs state_vec;
- Eigen::VectorXs delta_preint;
+ Eigen::VectorXd state_vec;
+ Eigen::VectorXd delta_preint;
//FrameIMUPtr last_frame;
- Eigen::Matrix<wolf::Scalar,9,9> delta_preint_cov;
+ Eigen::Matrix<double,9,9> delta_preint_cov;
//process data
mpu_clock = 0.001003;
@@ -96,15 +96,15 @@ int main(int argc, char** argv)
FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
- Eigen::Matrix<wolf::Scalar, 10, 1> expect;
- Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
- Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
- Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
- Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
- Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
- Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
- Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
- Eigen::Matrix<wolf::Scalar, 9, 1> residu;
+ Eigen::Matrix<double, 10, 1> expect;
+ Eigen::Vector3d ref_frame_p = ref_frame_ptr->getP()->getVector();
+ Eigen::Quaterniond ref_frame_o(ref_frame_ptr->getO()->getVector().data());
+ Eigen::Vector3d ref_frame_v = ref_frame_ptr->getV()->getVector();
+ Eigen::Vector3d current_frame_p = last_frame->getP()->getVector();
+ Eigen::Quaterniond current_frame_o(last_frame->getO()->getVector().data());
+ Eigen::Vector3d current_frame_v = last_frame->getV()->getVector();
+ Eigen::Vector3d acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
+ Eigen::Matrix<double, 9, 1> residu;
residu << 0,0,0, 0,0,0, 0,0,0;
factor_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, current_frame_p, current_frame_o, current_frame_v, expect);
@@ -157,15 +157,15 @@ int main(int argc, char** argv)
FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
- Eigen::Matrix<wolf::Scalar, 10, 1> expect;
- Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
- Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
- Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
- Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
- Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
- Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
- Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
- Eigen::Matrix<wolf::Scalar, 9, 1> residu;
+ Eigen::Matrix<double, 10, 1> expect;
+ Eigen::Vector3d ref_frame_p = ref_frame_ptr->getP()->getVector();
+ Eigen::Quaterniond ref_frame_o(ref_frame_ptr->getO()->getVector().data());
+ Eigen::Vector3d ref_frame_v = ref_frame_ptr->getV()->getVector();
+ Eigen::Vector3d current_frame_p = last_frame->getP()->getVector();
+ Eigen::Quaterniond current_frame_o(last_frame->getO()->getVector().data());
+ Eigen::Vector3d current_frame_v = last_frame->getV()->getVector();
+ Eigen::Vector3d acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
+ Eigen::Matrix<double, 9, 1> residu;
residu << 0,0,0, 0,0,0, 0,0,0;
factor_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);
@@ -215,15 +215,15 @@ int main(int argc, char** argv)
FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
- Eigen::Matrix<wolf::Scalar, 10, 1> expect;
- Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
- Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
- Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
- Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
- Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
- Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
- Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
- Eigen::Matrix<wolf::Scalar, 9, 1> residu;
+ Eigen::Matrix<double, 10, 1> expect;
+ Eigen::Vector3d ref_frame_p = ref_frame_ptr->getP()->getVector();
+ Eigen::Quaterniond ref_frame_o(ref_frame_ptr->getO()->getVector().data());
+ Eigen::Vector3d ref_frame_v = ref_frame_ptr->getV()->getVector();
+ Eigen::Vector3d current_frame_p = last_frame->getP()->getVector();
+ Eigen::Quaterniond current_frame_o(last_frame->getO()->getVector().data());
+ Eigen::Vector3d current_frame_v = last_frame->getV()->getVector();
+ Eigen::Vector3d acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
+ Eigen::Matrix<double, 9, 1> residu;
residu << 0,0,0, 0,0,0, 0,0,0;
factor_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);
@@ -237,7 +237,7 @@ int main(int argc, char** argv)
}
///having a look at covariances
- Eigen::MatrixXs predelta_cov;
+ Eigen::MatrixXd predelta_cov;
predelta_cov.resize(9,9);
predelta_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
//std::cout << "predelta_cov : \n" << predelta_cov << std::endl;
@@ -247,7 +247,7 @@ int main(int argc, char** argv)
//FrameBasePtr first_frame = wolf_problem_ptr_->getTrajectory()->getFrameList().front();
wolf_problem_ptr_->getProcessorMotion()->setOrigin(wolf_problem_ptr_->getTrajectory()->getFrameList().front());
//SensorBasePtr sensorbase = std::make_shared<SensorBase>("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0);
- //CapturePosePtr initial_covariance = std::make_shared<CaptureFix>(TimeStamp(0), sensorbase, first_frame->getState().head(7), Eigen::Matrix6s::Identity() * 0.01);
+ //CapturePosePtr initial_covariance = std::make_shared<CaptureFix>(TimeStamp(0), sensorbase, first_frame->getState().head(7), Eigen::Matrix6d::Identity() * 0.01);
//first_frame->addCapture(initial_covariance);
//initial_covariance->process();
//std::cout << "initial covariance: factor " << initial_covariance->getFeatureList().front()->getConstrainedByList().front()->id() << std::endl << initial_covariance->getFeatureList().front()->getMeasurementCovariance() << std::endl;
@@ -261,9 +261,9 @@ int main(int argc, char** argv)
// SOLVING PROBLEMS
ceres::Solver::Summary summary_diff;
std::cout << "solving..." << std::endl;
- Eigen::VectorXs prev_wolf_state = wolf_problem_ptr_->getTrajectory()->getFrameList().back()->getState();
+ Eigen::VectorXd prev_wolf_state = wolf_problem_ptr_->getTrajectory()->getFrameList().back()->getState();
summary_diff = ceres_manager_wolf_diff->solve();
- Eigen::VectorXs post_wolf_state = wolf_problem_ptr_->getTrajectory()->getFrameList().back()->getState();
+ Eigen::VectorXd post_wolf_state = wolf_problem_ptr_->getTrajectory()->getFrameList().back()->getState();
std::cout << " prev_wolf_state : " << prev_wolf_state.transpose() << "\n post_wolf_state : " << post_wolf_state.transpose() << std::endl;
//std::cout << summary_wolf_diff.BriefReport() << std::endl;
std::cout << "solved!" << std::endl;
diff --git a/demos/demo_imuDock.cpp b/demos/demo_imuDock.cpp
index c05fbbf9c4e676c5611c7073b94b9f55385a9267..6c3d190af92ff9606d20ee1bb2cf7a879b2b16fd 100644
--- a/demos/demo_imuDock.cpp
+++ b/demos/demo_imuDock.cpp
@@ -85,17 +85,17 @@ int main(int argc, char** argv)
#endif
// ___initialize variabes that will be used through the code___
- Eigen::VectorXs problem_origin(16);
- Eigen::Vector7s imu_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished()), odom_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished());
+ Eigen::VectorXd problem_origin(16);
+ Eigen::Vector7d imu_pose((Eigen::Vector7d()<<0,0,0,0,0,0,1).finished()), odom_pose((Eigen::Vector7d()<<0,0,0,0,0,0,1).finished());
problem_origin << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0;
//Create vectors to store data and time
- Eigen::Vector6s data_imu, data_odom;
- Scalar clock;
+ Eigen::Vector6d data_imu, data_odom;
+ double clock;
TimeStamp ts(0), ts_output(0); //will be used to store the data timestamps and set timestamps in captures
// ___Define expected values___
- Eigen::Vector7s expected_KF1_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished()), expected_KF2_pose((Eigen::Vector7s()<<0,-0.06,0,0,0,0,11).finished());
+ Eigen::Vector7d expected_KF1_pose((Eigen::Vector7d()<<0,0,0,0,0,0,1).finished()), expected_KF2_pose((Eigen::Vector7d()<<0,-0.06,0,0,0,0,11).finished());
//#################################################### SETTING PROBLEM
std::string wolf_root = _WOLF_ROOT_DIR;
@@ -121,7 +121,7 @@ int main(int argc, char** argv)
// ___process IMU and odometry___
//Create captures
- CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sensorIMU, data_imu, Matrix6s::Identity(), Vector6s::Zero()); //ts is set at 0
+ CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sensorIMU, data_imu, Matrix6d::Identity(), Vector6d::Zero()); //ts is set at 0
CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(ts, sensorOdom, data_odom, 7, 6, nullptr);
//while we do not reach the end of file, read IMU input (ts, Ax, Ay, Az, Wx, Wy, Wz) and process data through capture
@@ -175,8 +175,8 @@ int main(int argc, char** argv)
// ___Create needed factors___
//Add Fix3D factor on first KeyFrame (with large covariance except for yaw)
- Eigen::MatrixXs featureFix_cov(6,6);
- featureFix_cov = Eigen::MatrixXs::Identity(6,6);
+ Eigen::MatrixXd featureFix_cov(6,6);
+ featureFix_cov = Eigen::MatrixXd::Identity(6,6);
featureFix_cov.topLeftCorner(3,3) *= 1e-8; // position variances (it's fixed anyway)
featureFix_cov(3,3) = pow( .02 , 2); // roll variance
featureFix_cov(4,4) = pow( .02 , 2); // pitch variance
@@ -185,8 +185,8 @@ int main(int argc, char** argv)
FeatureBasePtr featureFix = cap_fix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", problem_origin.head(7), featureFix_cov));
FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(featureFix->addFactor(std::make_shared<FactorPose3D>(featureFix)));
- Eigen::MatrixXs featureFixBias_cov(6,6);
- featureFixBias_cov = Eigen::MatrixXs::Identity(6,6);
+ Eigen::MatrixXd featureFixBias_cov(6,6);
+ featureFixBias_cov = Eigen::MatrixXd::Identity(6,6);
featureFixBias_cov.topLeftCorner(3,3) *= sensorIMU->getAbInitialStdev() * sensorIMU->getAbInitialStdev();
featureFixBias_cov.bottomRightCorner(3,3) *= sensorIMU->getWbInitialStdev() * sensorIMU->getWbInitialStdev();
CaptureBasePtr cap_fixbias = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.tail(6), featureFixBias_cov, 6, 6, nullptr));
@@ -228,7 +228,7 @@ int main(int argc, char** argv)
*/
unsigned int time_iter(0);
- Scalar ms(0.001);
+ double ms(0.001);
ts_output.set(0);
while(ts_output.get() < ts.get() + ms)
{
@@ -248,13 +248,13 @@ int main(int argc, char** argv)
// ___Get standard deviation from covariances___
#ifdef ADD_KF3
- Eigen::MatrixXs cov_KF1(16,16), cov_KF2(16,16), cov_KF3(16,16);
+ Eigen::MatrixXd cov_KF1(16,16), cov_KF2(16,16), cov_KF3(16,16);
problem->getFrameCovariance(KF1, cov_KF1);
problem->getFrameCovariance(KF2, cov_KF2);
problem->getFrameCovariance(KF3, cov_KF3);
- Eigen::Matrix<wolf::Scalar, 16, 1> stdev_KF1, stdev_KF2, stdev_KF3;
+ Eigen::Matrix<double, 16, 1> stdev_KF1, stdev_KF2, stdev_KF3;
stdev_KF1 = cov_KF1.diagonal().array().sqrt();
stdev_KF2 = cov_KF2.diagonal().array().sqrt();
@@ -264,12 +264,12 @@ int main(int argc, char** argv)
WOLF_DEBUG("stdev KF2 : ", stdev_KF2.transpose());
WOLF_DEBUG("stdev KF3 : ", stdev_KF3.transpose());
#else
- Eigen::MatrixXs cov_KF1(16,16), cov_KF2(16,16);
+ Eigen::MatrixXd cov_KF1(16,16), cov_KF2(16,16);
problem->getFrameCovariance(KF1, cov_KF1);
problem->getFrameCovariance(KF2, cov_KF2);
- Eigen::Matrix<wolf::Scalar, 16, 1> stdev_KF1, stdev_KF2;
+ Eigen::Matrix<double, 16, 1> stdev_KF1, stdev_KF2;
stdev_KF1 = cov_KF1.diagonal().array().sqrt();
stdev_KF2 = cov_KF2.diagonal().array().sqrt();
diff --git a/demos/demo_imuDock_autoKFs.cpp b/demos/demo_imuDock_autoKFs.cpp
index 039615445807ab6ef9e1dadab7e273135bc650ef..c22345556e017078596b7f813be05fa9474967d4 100644
--- a/demos/demo_imuDock_autoKFs.cpp
+++ b/demos/demo_imuDock_autoKFs.cpp
@@ -91,20 +91,20 @@ int main(int argc, char** argv)
#endif
// ___initialize variabes that will be used through the code___
- Eigen::VectorXs problem_origin(16);
- Eigen::Vector7s imu_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished()), odom_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished());
+ Eigen::VectorXd problem_origin(16);
+ Eigen::Vector7d imu_pose((Eigen::Vector7d()<<0,0,0,0,0,0,1).finished()), odom_pose((Eigen::Vector7d()<<0,0,0,0,0,0,1).finished());
problem_origin << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0;
//Create vectors to store data and time
- Eigen::Vector6s data_imu, data_odom;
- Scalar clock;
+ Eigen::Vector6d data_imu, data_odom;
+ double clock;
TimeStamp ts(0), ts_output(0); //will be used to store the data timestamps and set timestamps in captures
// ___Define expected values___
- Eigen::Vector7s expected_KF1_pose((Eigen::Vector7s()<<0,0,0,0,0,0,1).finished()), expected_KF2_pose((Eigen::Vector7s()<<0,-0.06,0,0,0,0,11).finished());
+ Eigen::Vector7d expected_KF1_pose((Eigen::Vector7d()<<0,0,0,0,0,0,1).finished()), expected_KF2_pose((Eigen::Vector7d()<<0,-0.06,0,0,0,0,11).finished());
#ifdef AUTO_KFs
- std::array<Scalar, 50> lastms_imuData;
+ std::array<double, 50> lastms_imuData;
#endif
//#################################################### SETTING PROBLEM
std::string wolf_root = _WOLF_ROOT_DIR;
@@ -130,7 +130,7 @@ int main(int argc, char** argv)
// ___process IMU and odometry___
//Create captures
- CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sensorIMU, data_imu, Matrix6s::Identity(), Vector6s::Zero()); //ts is set at 0
+ CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sensorIMU, data_imu, Matrix6d::Identity(), Vector6d::Zero()); //ts is set at 0
CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(ts, sensorOdom, data_odom, 7, 6, nullptr);
//while we do not reach the end of file, read IMU input (ts, Ax, Ay, Az, Wx, Wy, Wz) and process data through capture
@@ -190,8 +190,8 @@ int main(int argc, char** argv)
// ___Create needed factors___
//Add Fix3D factor on first KeyFrame (with large covariance except for yaw)
- Eigen::MatrixXs featureFix_cov(6,6);
- featureFix_cov = Eigen::MatrixXs::Identity(6,6);
+ Eigen::MatrixXd featureFix_cov(6,6);
+ featureFix_cov = Eigen::MatrixXd::Identity(6,6);
featureFix_cov.topLeftCorner(3,3) *= 1e-8; // position variances (it's fixed anyway)
featureFix_cov(3,3) = pow( .01 , 2); // roll variance
featureFix_cov(4,4) = pow( .01 , 2); // pitch variance
@@ -200,8 +200,8 @@ int main(int argc, char** argv)
FeatureBasePtr featureFix = cap_fix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", problem_origin.head(7), featureFix_cov));
FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(featureFix->addFactor(std::make_shared<FactorPose3D>(featureFix)));
- Eigen::MatrixXs featureFixBias_cov(6,6);
- featureFixBias_cov = Eigen::MatrixXs::Identity(6,6);
+ Eigen::MatrixXd featureFixBias_cov(6,6);
+ featureFixBias_cov = Eigen::MatrixXd::Identity(6,6);
featureFixBias_cov.topLeftCorner(3,3) *= sensorIMU->getAbInitialStdev() * sensorIMU->getAbInitialStdev();
featureFixBias_cov.bottomRightCorner(3,3) *= sensorIMU->getWbInitialStdev() * sensorIMU->getWbInitialStdev();
CaptureBasePtr cap_fixbias = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.tail(6), featureFixBias_cov, 6, 6, nullptr));
@@ -220,7 +220,7 @@ int main(int argc, char** argv)
{
frame_imu->getP()->fix();
frame_imu->getO()->unfix();
- frame_imu->getV()->setState((Eigen::Vector3s()<<0,0,0).finished()); //fix all velocties to 0 ()
+ frame_imu->getV()->setState((Eigen::Vector3d()<<0,0,0).finished()); //fix all velocties to 0 ()
frame_imu->getV()->fix();
frame_imu->getAccBias()->unfix();
frame_imu->getGyroBias()->unfix();
@@ -237,7 +237,7 @@ int main(int argc, char** argv)
*/
unsigned int time_iter(0);
- Scalar ms(0.001);
+ double ms(0.001);
ts_output.set(0);
while(ts_output.get() < ts.get() + ms)
{
@@ -256,8 +256,8 @@ int main(int argc, char** argv)
//#################################################### RESULTS PART
// ___Get standard deviation from covariances___ and output this in a file
- Eigen::MatrixXs cov_KF(16,16);
- Eigen::Matrix<wolf::Scalar, 16, 1> stdev_KF;
+ Eigen::MatrixXd cov_KF(16,16);
+ Eigen::Matrix<double, 16, 1> stdev_KF;
for(auto frame : trajectory)
{
if(frame->isKey())
@@ -296,9 +296,9 @@ int main(int argc, char** argv)
return 0;
}
-/*Scalar getIMUStdev(Eigen::VectorXs _data) //input argument : whatever will contain the data in the capture
+/*double getIMUStdev(Eigen::VectorXd _data) //input argument : whatever will contain the data in the capture
{
- Eigen::Vector6s mean(Eigen::Vector6s::Zero()), stdev(Eigen::Vector6s::Zero());
+ Eigen::Vector6d mean(Eigen::Vector6d::Zero()), stdev(Eigen::Vector6d::Zero());
unsigned int _data_size(_data.size());
mean = _data.mean()/_data_size;
diff --git a/demos/demo_imuPlateform_Offline.cpp b/demos/demo_imuPlateform_Offline.cpp
index e0e4e4778e91090221d70121bd89907ad3756823..0b98b3e9675c6c0bf0a3ca1c559d4e4fbcc70f02 100644
--- a/demos/demo_imuPlateform_Offline.cpp
+++ b/demos/demo_imuPlateform_Offline.cpp
@@ -70,7 +70,7 @@ int main(int argc, char** argv)
// WOLF PROBLEM
ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
- Eigen::VectorXs x_origin(16);
+ Eigen::VectorXd x_origin(16);
x_origin << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0; //INITIAL CONDITIONS
TimeStamp t(0);
@@ -82,7 +82,7 @@ int main(int argc, char** argv)
CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
+ SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
ProcessorIMUParamsPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
prc_imu_params->max_time_span = 10;
prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
@@ -94,7 +94,7 @@ int main(int argc, char** argv)
ProcessorIMUPtr processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
// SENSOR + PROCESSOR ODOM 3D
- SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml");
+ SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml");
ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
prc_odom3D_params->max_time_span = 1.9999;
prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
@@ -117,13 +117,13 @@ int main(int argc, char** argv)
//===================================================== PROCESS DATA
// PROCESS DATA
- Eigen::Vector6s data_imu, data_odom3D;
+ Eigen::Vector6d data_imu, data_odom3D;
data_imu << 0,0,-wolf::gravity()(2), 0,0,0;
data_odom3D << 0,-0.06,0, 0,0,0;
- Scalar input_clock;
+ double input_clock;
TimeStamp ts(0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, Matrix6s::Identity(), Vector6s::Zero());
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, Matrix6d::Identity(), Vector6d::Zero());
wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, 6, 6, nullptr);
//when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
@@ -207,10 +207,10 @@ int main(int argc, char** argv)
wolf_problem_ptr_->print(4,1,1,1);
#ifdef DEBUG_RESULTS
- Eigen::VectorXs frm_state(16);
- Eigen::Matrix<wolf::Scalar, 16, 1> cov_stdev;
- Eigen::MatrixXs covX(16,16);
- Eigen::MatrixXs cov3(Eigen::Matrix3s::Zero());
+ Eigen::VectorXd frm_state(16);
+ Eigen::Matrix<double, 16, 1> cov_stdev;
+ Eigen::MatrixXd covX(16,16);
+ Eigen::MatrixXd cov3(Eigen::Matrix3d::Zero());
wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList();
for(FrameBasePtr frm_ptr : frame_list)
diff --git a/demos/demo_imu_constrained0.cpp b/demos/demo_imu_constrained0.cpp
index 817b08a14a4c71caf9f9e4807c71cc14cc43d78e..cff689aaf16c1e4692c79f27f2becf56827acca3 100644
--- a/demos/demo_imu_constrained0.cpp
+++ b/demos/demo_imu_constrained0.cpp
@@ -85,8 +85,8 @@ int main(int argc, char** argv)
// WOLF PROBLEM
ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
- Eigen::VectorXs x_origin(16);
- Eigen::Vector6s origin_bias;
+ Eigen::VectorXd x_origin(16);
+ Eigen::Vector6d origin_bias;
x_origin << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0; //INITIAL CONDITIONS 0.05,0.03,.00, 0.2,-0.05,.00;
TimeStamp t(0);
@@ -103,7 +103,7 @@ int main(int argc, char** argv)
CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
+ SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
ProcessorIMUParamsPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
prc_imu_params->max_time_span = 10;
prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
@@ -115,7 +115,7 @@ int main(int argc, char** argv)
ProcessorIMUPtr processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
// SENSOR + PROCESSOR ODOM 3D
- SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
+ SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
prc_odom3D_params->max_time_span = 20.9999;
prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
@@ -128,7 +128,7 @@ int main(int argc, char** argv)
// reset origin of problem
t.set(0);
- Eigen::Matrix<wolf::Scalar, 10, 1> expected_final_state;
+ Eigen::Matrix<double, 10, 1> expected_final_state;
FrameIMUPtr origin_KF = std::static_pointer_cast<FrameIMU>(processor_ptr_imu->setOrigin(x_origin, t));
processor_ptr_odom3D->setOrigin(origin_KF);
@@ -142,14 +142,14 @@ int main(int argc, char** argv)
//===================================================== PROCESS DATA
// PROCESS DATA
- Eigen::Vector6s data_imu, data_odom3D;
+ Eigen::Vector6d data_imu, data_odom3D;
data_imu << 0,0,-wolf::gravity()(2), 0,0,0;
data_odom3D << 0,0,0, 0,0,0;
- Scalar input_clock;
+ double input_clock;
TimeStamp ts(0);
TimeStamp t_odom(0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, Matrix6s::Identity(), Vector6s::Zero());
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, Matrix6d::Identity(), Vector6d::Zero());
wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, 6, 6, nullptr);
//read first odom data from file
@@ -193,7 +193,7 @@ int main(int argc, char** argv)
t = ts;
//std::string report = ceres_manager_wolf_diff->solve(1); //0: nothing, 1: BriefReport, 2: FullReport
- Eigen::VectorXs frm_state(16);
+ Eigen::VectorXd frm_state(16);
frm_state = origin_KF->getState();
KF0_evolution << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2)
@@ -270,10 +270,10 @@ int main(int argc, char** argv)
wolf_problem_ptr_->print(4,1,1,1);
#ifdef DEBUG_RESULTS
- Eigen::VectorXs frm_state(16);
- Eigen::Matrix<wolf::Scalar, 16, 1> cov_stdev;
- Eigen::MatrixXs covX(16,16);
- Eigen::MatrixXs cov3(Eigen::Matrix3s::Zero());
+ Eigen::VectorXd frm_state(16);
+ Eigen::Matrix<double, 16, 1> cov_stdev;
+ Eigen::MatrixXd covX(16,16);
+ Eigen::MatrixXd cov3(Eigen::Matrix3d::Zero());
wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList();
for(FrameBasePtr frm_ptr : frame_list)
@@ -308,19 +308,19 @@ int main(int argc, char** argv)
}
//trials to print all factorIMUs' residuals
- Eigen::Matrix<wolf::Scalar,15,1> IMU_residuals;
- Eigen::Vector3s p1(Eigen::Vector3s::Zero());
- Eigen::Vector4s q1_vec(Eigen::Vector4s::Zero());
- Eigen::Map<Quaternions> q1(q1_vec.data());
- Eigen::Vector3s v1(Eigen::Vector3s::Zero());
- Eigen::Vector3s ab1(Eigen::Vector3s::Zero());
- Eigen::Vector3s wb1(Eigen::Vector3s::Zero());
- Eigen::Vector3s p2(Eigen::Vector3s::Zero());
- Eigen::Vector4s q2_vec(Eigen::Vector4s::Zero());
- Eigen::Map<Quaternions> q2(q2_vec.data());
- Eigen::Vector3s v2(Eigen::Vector3s::Zero());
- Eigen::Vector3s ab2(Eigen::Vector3s::Zero());
- Eigen::Vector3s wb2(Eigen::Vector3s::Zero());
+ Eigen::Matrix<double,15,1> IMU_residuals;
+ Eigen::Vector3d p1(Eigen::Vector3d::Zero());
+ Eigen::Vector4d q1_vec(Eigen::Vector4d::Zero());
+ Eigen::Map<Quaterniond> q1(q1_vec.data());
+ Eigen::Vector3d v1(Eigen::Vector3d::Zero());
+ Eigen::Vector3d ab1(Eigen::Vector3d::Zero());
+ Eigen::Vector3d wb1(Eigen::Vector3d::Zero());
+ Eigen::Vector3d p2(Eigen::Vector3d::Zero());
+ Eigen::Vector4d q2_vec(Eigen::Vector4d::Zero());
+ Eigen::Map<Quaterniond> q2(q2_vec.data());
+ Eigen::Vector3d v2(Eigen::Vector3d::Zero());
+ Eigen::Vector3d ab2(Eigen::Vector3d::Zero());
+ Eigen::Vector3d wb2(Eigen::Vector3d::Zero());
for(FrameBasePtr frm_ptr : frame_list)
{
@@ -331,8 +331,8 @@ int main(int argc, char** argv)
{
if(fac_ptr->getTypeId() == FAC_IMU)
{
- //Eigen::VectorXs prev_KF_state(fac_ptr->getFrameOther()->getState());
- //Eigen::VectorXs curr_KF_state(fac_ptr->getFeature()->getFrame()->getState());
+ //Eigen::VectorXd prev_KF_state(fac_ptr->getFrameOther()->getState());
+ //Eigen::VectorXd curr_KF_state(fac_ptr->getFeature()->getFrame()->getState());
p1 = fac_ptr->getFrameOther()->getP()->getState();
q1_vec = fac_ptr->getFrameOther()->getO()->getState();
v1 = fac_ptr->getFrameOther()->getV()->getState();
diff --git a/demos/demo_processor_imu.cpp b/demos/demo_processor_imu.cpp
index ec77aedeee3242daffe10d89c1444fa9ea1bf1f9..e6062abbc6310e580e1623b9c01d851e4184a988 100644
--- a/demos/demo_processor_imu.cpp
+++ b/demos/demo_processor_imu.cpp
@@ -71,16 +71,16 @@ int main(int argc, char** argv)
// Wolf problem
ProblemPtr problem_ptr_ = Problem::create("PQVBB 3D");
- Eigen::VectorXs extrinsics(7);
+ Eigen::VectorXd extrinsics(7);
extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
SensorBasePtr sensor_ptr = problem_ptr_->installSensor("IMU", "Main IMU", extrinsics, IntrinsicsBasePtr());
problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
// Time and data variables
TimeStamp t;
- Scalar mti_clock, tmp;
- Eigen::Vector6s data;
- Eigen::Matrix6s data_cov;
+ double mti_clock, tmp;
+ Eigen::Vector6d data;
+ Eigen::Matrix6d data_cov;
data_cov.setIdentity();
data_cov.topLeftCorner(3,3) *= 0.01;
data_cov.bottomRightCorner(3,3) *= 0.01;
@@ -91,12 +91,12 @@ int main(int argc, char** argv)
t.set(mti_clock * 0.0001); // clock in 0,1 ms ticks
// Set the origin
- Eigen::VectorXs x0(16);
+ Eigen::VectorXd x0(16);
x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0; // Try some non-zero biases
problem_ptr_->getProcessorMotion()->setOrigin(x0, t);
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- CaptureIMUPtr imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
+ CaptureIMUPtr imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
// problem_ptr_->print();
@@ -137,7 +137,7 @@ int main(int argc, char** argv)
std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
<< problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
- Eigen::VectorXs x = problem_ptr_->getProcessorMotion()->getCurrentState();
+ Eigen::VectorXd x = problem_ptr_->getProcessorMotion()->getCurrentState();
std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
<< x.head(10).transpose() << std::endl;
@@ -150,9 +150,9 @@ int main(int argc, char** argv)
//#ifdef DEBUG_RESULTS
// ----- dump to file -----
- Eigen::VectorXs delta_debug;
- Eigen::VectorXs delta_integr_debug;
- Eigen::VectorXs x_debug;
+ Eigen::VectorXd delta_debug;
+ Eigen::VectorXd delta_integr_debug;
+ Eigen::VectorXd x_debug;
TimeStamp ts;
delta_debug = problem_ptr_->getProcessorMotion()->getMotion().delta_;
diff --git a/demos/demo_processor_imu_jacobians.cpp b/demos/demo_processor_imu_jacobians.cpp
index 988244d1598d3a8a9a1bdebf0144266b6744044b..3555fa9c8f96c337e7375588684cdbb55bd29383 100644
--- a/demos/demo_processor_imu_jacobians.cpp
+++ b/demos/demo_processor_imu_jacobians.cpp
@@ -23,8 +23,8 @@
using namespace wolf;
-void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq0, Eigen::Map<Eigen::Quaternions>& _dq0);
-void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq, Eigen::Map<Eigen::Quaternions>& _dq, const int& place );
+void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0);
+void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place );
int main(int argc, char** argv)
{
@@ -33,20 +33,20 @@ int main(int argc, char** argv)
std::cout << std::endl << "==================== processor IMU : Checking Jacobians ======================" << std::endl;
TimeStamp t;
- Eigen::Vector6s data_;
- wolf::Scalar deg_to_rad = 3.14159265359/180.0;
+ Eigen::Vector6d data_;
+ double deg_to_rad = 3.14159265359/180.0;
data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad;
// Wolf problem
ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
- Eigen::VectorXs IMU_extrinsics(7);
+ Eigen::VectorXd IMU_extrinsics(7);
IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
//SensorBase* sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, nullptr);
//wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
// Set the origin
t.set(0.0001); // clock in 0,1 ms ticks
- Eigen::VectorXs x0(16);
+ Eigen::VectorXd x0(16);
x0 << 0,1,0, 0,0,0,1, 1,0,0, 0,0,.000, 0,0,.000; // P Q V B B
//wolf_problem_ptr_->getProcessorMotion()->setOrigin(x0, t);
@@ -55,18 +55,18 @@ int main(int argc, char** argv)
ProcessorIMU_UnitTester processor_imu;
//processor_imu.setOrigin(x0, t);
- wolf::Scalar ddelta_bias = 0.00000001;
- wolf::Scalar dt = 0.001;
+ double ddelta_bias = 0.00000001;
+ double 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<double,10,1> Delta0;
+ Delta0 = Eigen::Matrix<double,10,1>::Random();
Delta0.head<3>() = Delta0.head<3>()*100;
Delta0.tail<3>() = Delta0.tail<3>()*10;
- Eigen::Vector3s ang0, ang;
+ Eigen::Vector3d ang0, ang;
ang0 << 120.08*deg_to_rad, 12.36*deg_to_rad, 54.32*deg_to_rad;
//Delta0 << 0,0,0, 0,0,0,1, 0,0,0;
- Eigen::Map<Eigen::Quaternions> Delta0_quat(Delta0.data()+3);
+ Eigen::Map<Eigen::Quaterniond> Delta0_quat(Delta0.data()+3);
Delta0_quat = v2q(ang0);
Delta0_quat.normalize();
ang = q2v(Delta0_quat);
@@ -76,11 +76,11 @@ int main(int argc, char** argv)
struct IMU_jac_bias bias_jac = processor_imu.finite_diff_ab(dt, data_, ddelta_bias, Delta0);
- Eigen::Map<Eigen::Quaternions> Dq0(NULL);
- Eigen::Map<Eigen::Quaternions> dq0(NULL);
- Eigen::Map<Eigen::Quaternions> Dq_noisy(NULL);
- Eigen::Map<Eigen::Quaternions> dq_noisy(NULL);
- Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL);
+ Eigen::Map<Eigen::Quaterniond> Dq0(NULL);
+ Eigen::Map<Eigen::Quaterniond> dq0(NULL);
+ Eigen::Map<Eigen::Quaterniond> Dq_noisy(NULL);
+ Eigen::Map<Eigen::Quaterniond> dq_noisy(NULL);
+ Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
/* IMU_jac_deltas struct form :
contains vectors of size 7 :
@@ -89,7 +89,7 @@ int main(int argc, char** argv)
place 4 : added dw_bx in data place 5 : added dw_by in data place 6 : added dw_bz in data
*/
- Eigen::Matrix3s dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dab, dDq_dwb;
+ Eigen::Matrix3d dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dab, dDq_dwb;
/*
dDp_dab = [dDp_dab_x, dDp_dab_y, dDp_dab_z]
@@ -115,7 +115,7 @@ int main(int argc, char** argv)
std::cout << "\n input data : \n" << data_ << std::endl;
- new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac.Delta0_.data() + 3);
+ new (&q_in_1) Eigen::Map<Eigen::Quaterniond>(bias_jac.Delta0_.data() + 3);
for(int i=0;i<3;i++){
dDp_dab.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i).head(3) - bias_jac.Delta0_.head(3))/ddelta_bias;
dDv_dab.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias;
@@ -123,10 +123,10 @@ int main(int argc, char** argv)
dDp_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).head(3) - bias_jac.Delta0_.head(3))/ddelta_bias;
dDv_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias;
- new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac.Deltas_noisy_vect_(i).data() + 3);
+ new (&q_in_2) Eigen::Map<Eigen::Quaterniond>(bias_jac.Deltas_noisy_vect_(i).data() + 3);
dDq_dab.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
- new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac.Deltas_noisy_vect_(i+3).data() + 3);
+ new (&q_in_2) Eigen::Map<Eigen::Quaterniond>(bias_jac.Deltas_noisy_vect_(i+3).data() + 3);
dDq_dwb.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
//std::cout << "matrix operation result :" << i << "\n" << q_in_1.matrix().transpose() * q_in_2.matrix() << std::endl;
//std::cout << "matrix operation result to vector :" << i << "\n" << R2v( q_in_1.matrix().transpose() * q_in_2.matrix()) << std::endl;
@@ -261,8 +261,8 @@ int main(int argc, char** argv)
*/
//taking care of noise now
- Eigen::Matrix<wolf::Scalar,9,1> Delta_noise;
- Eigen::Matrix<wolf::Scalar,9,1> delta_noise;
+ Eigen::Matrix<double,9,1> Delta_noise;
+ Eigen::Matrix<double,9,1> delta_noise;
Delta_noise << 0.00000001, 0.00000001, 0.00000001, 0.0001, 0.0001, 0.0001, 0.00000001, 0.00000001, 0.00000001;
delta_noise << 0.00000001, 0.00000001, 0.00000001, 0.0001, 0.0001, 0.0001, 0.00000001, 0.00000001, 0.00000001;
@@ -277,8 +277,8 @@ int main(int argc, char** argv)
7: +dVx, 8: +dVy, 9: +dVz 7: + dvx, 8: +dvy, 9: +dvz
*/
- Eigen::Matrix3s dDp_dP, dDp_dV, dDp_dO, dDv_dP, dDv_dV, dDv_dO, dDo_dP, dDo_dV, dDo_dO;
- Eigen::Matrix3s dDp_dp, dDp_dv, dDp_do, dDv_dp, dDv_dv, dDv_do, dDo_dp, dDo_dv, dDo_do;
+ Eigen::Matrix3d dDp_dP, dDp_dV, dDp_dO, dDv_dP, dDv_dV, dDv_dO, dDo_dP, dDo_dV, dDo_dO;
+ Eigen::Matrix3d dDp_dp, dDp_dv, dDp_do, dDv_dp, dDv_dv, dDv_do, dDo_dp, dDo_dv, dDo_do;
remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
@@ -370,7 +370,7 @@ int main(int argc, char** argv)
std::cout << "dDo_dO_a - dDo_dO : \n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) - dDo_dO << "\n" << std::endl;
}
- Eigen::Matrix3s dDp_dp_a, dDv_dv_a, dDo_do_a;
+ Eigen::Matrix3d dDp_dp_a, dDv_dv_a, dDo_do_a;
dDp_dp_a = deltas_jac.jacobian_delta_.block(0,0,3,3);
dDv_dv_a = deltas_jac.jacobian_delta_.block(6,6,3,3);
dDo_do_a = deltas_jac.jacobian_delta_.block(3,3,3,3);
@@ -404,15 +404,15 @@ int main(int argc, char** argv)
using namespace wolf;
-void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq0, Eigen::Map<Eigen::Quaternions>& _dq0){
+void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0){
- new (&_Dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta0_.data() + 3);
- new (&_dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta0_.data() + 3);
+ new (&_Dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta0_.data() + 3);
+ new (&_dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta0_.data() + 3);
}
-void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq, Eigen::Map<Eigen::Quaternions>& _dq, const int& place ){
+void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place ){
assert(place < _jac_delta.Delta_noisy_vect_.size());
- new (&_Dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
- new (&_dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta_noisy_vect_(place).data() + 3);
+ new (&_Dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
+ new (&_dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta_noisy_vect_(place).data() + 3);
}
diff --git a/include/IMU/capture/capture_IMU.h b/include/IMU/capture/capture_IMU.h
index 759b43de0fea48b569328ef9a220c4573d0e8caf..c5c3174f954668ec3aa3ac29e89715f20cc8a7c4 100644
--- a/include/IMU/capture/capture_IMU.h
+++ b/include/IMU/capture/capture_IMU.h
@@ -15,24 +15,24 @@ class CaptureIMU : public CaptureMotion
CaptureIMU(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _data,
- const Eigen::MatrixXs& _data_cov,
+ const Eigen::Vector6d& _data,
+ const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr = nullptr);
CaptureIMU(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _data,
- const Eigen::MatrixXs& _data_cov,
- const Vector6s& _bias,
+ const Eigen::Vector6d& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Vector6d& _bias,
CaptureBasePtr _capture_origin_ptr = nullptr);
virtual ~CaptureIMU();
- virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const override;
+ virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
};
-inline Eigen::VectorXs CaptureIMU::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const
+inline Eigen::VectorXd CaptureIMU::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
{
return imu::plus(_delta, _delta_error);
}
diff --git a/include/IMU/factor/factor_IMU.h b/include/IMU/factor/factor_IMU.h
index 0378abb66e227f4d5de7ac48bb643e0df1ad1f97..d16c866f34a20045ee8be3b045b416992d22a2fe 100644
--- a/include/IMU/factor/factor_IMU.h
+++ b/include/IMU/factor/factor_IMU.h
@@ -45,7 +45,7 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
/** \brief Estimation error given the states in the wolf tree
*
*/
- Eigen::Vector9s error();
+ Eigen::Vector9d error();
/** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
@@ -54,15 +54,15 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
-> compares the corrected measurement with the expected one
-> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
* params :
- * Vector3s _p1 : position in imu frame
- * Vector4s _q1 : orientation quaternion in imu frame
- * Vector3s _v1 : velocity in imu frame
- * Vector3s _ab : accelerometer bias in imu frame
- * Vector3s _wb : gyroscope bias in imu frame
- * Vector3s _p2 : position in current frame
- * Vector4s _q2 : orientation quaternion in current frame
- * Vector3s _v2 : velocity in current frame
- * Matrix<9,1, Scalar> _res : to retrieve residuals (POV) O is rotation vector... NOT A QUATERNION
+ * Vector3d _p1 : position in imu frame
+ * Vector4d _q1 : orientation quaternion in imu frame
+ * Vector3d _v1 : velocity in imu frame
+ * Vector3d _ab : accelerometer bias in imu frame
+ * Vector3d _wb : gyroscope bias in imu frame
+ * Vector3d _p2 : position in current frame
+ * Vector4d _q2 : orientation quaternion in current frame
+ * Vector3d _v2 : velocity in current frame
+ * Matrix<9,1, double> _res : to retrieve residuals (POV) O is rotation vector... NOT A QUATERNION
*/
template<typename D1, typename D2, typename D3>
bool residual(const Eigen::MatrixBase<D1> & _p1,
@@ -79,13 +79,13 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
/** Function expectation(...)
* params :
- * Vector3s _p1 : position in imu frame
- * Vector4s _q1 : orientation quaternion in imu frame
- * Vector3s _v1 : velocity in imu frame
- * Vector3s _p2 : position in current frame
- * Vector4s _q2 : orientation quaternion in current frame
- * Vector3s _v2 : velocity in current frame
- * Scalar _dt : time interval between the two states
+ * Vector3d _p1 : position in imu frame
+ * Vector4d _q1 : orientation quaternion in imu frame
+ * Vector3d _v1 : velocity in imu frame
+ * Vector3d _p2 : position in current frame
+ * Vector4d _q2 : orientation quaternion in current frame
+ * Vector3d _v2 : velocity in current frame
+ * double _dt : time interval between the two states
* _pe : expected position delta
* _qe : expected quaternion delta
* _ve : expected velocity delta
@@ -104,28 +104,28 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
/** \brief : return the expected delta given the state blocks in the wolf tree
*/
- Eigen::VectorXs expectation() const;
+ Eigen::VectorXd expectation() const;
private:
/// Preintegrated delta
- Eigen::Vector3s dp_preint_;
- Eigen::Quaternions dq_preint_;
- Eigen::Vector3s dv_preint_;
+ Eigen::Vector3d dp_preint_;
+ Eigen::Quaterniond dq_preint_;
+ Eigen::Vector3d dv_preint_;
// Biases used during preintegration
- Eigen::Vector3s acc_bias_preint_;
- Eigen::Vector3s gyro_bias_preint_;
+ Eigen::Vector3d acc_bias_preint_;
+ Eigen::Vector3d gyro_bias_preint_;
// Jacobians of preintegrated deltas wrt biases
- Eigen::Matrix3s dDp_dab_;
- Eigen::Matrix3s dDv_dab_;
- Eigen::Matrix3s dDp_dwb_;
- Eigen::Matrix3s dDv_dwb_;
- Eigen::Matrix3s dDq_dwb_;
+ Eigen::Matrix3d dDp_dab_;
+ Eigen::Matrix3d dDv_dab_;
+ Eigen::Matrix3d dDp_dwb_;
+ Eigen::Matrix3d dDv_dwb_;
+ Eigen::Matrix3d dDq_dwb_;
/// Metrics
- const Scalar dt_; ///< delta-time and delta-time-squared between keyframes
- const Scalar ab_rate_stdev_, wb_rate_stdev_; //stdev for standard_deviation (= sqrt(variance))
+ const double dt_; ///< delta-time and delta-time-squared between keyframes
+ const double ab_rate_stdev_, wb_rate_stdev_; //stdev for standard_deviation (= sqrt(variance))
/** bias covariance and bias residuals
*
@@ -139,8 +139,8 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
*
* same logic for gyroscope bias
*/
- const Eigen::Matrix3s sqrt_A_r_dt_inv;
- const Eigen::Matrix3s sqrt_W_r_dt_inv;
+ const Eigen::Matrix3d sqrt_A_r_dt_inv;
+ const Eigen::Matrix3d sqrt_W_r_dt_inv;
};
@@ -181,8 +181,8 @@ inline FactorIMU::FactorIMU(const FeatureIMUPtr& _ftr_ptr,
dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
ab_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapture()->getSensor())->getAbRateStdev()),
wb_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapture()->getSensor())->getWbRateStdev()),
- sqrt_A_r_dt_inv((Eigen::Matrix3s::Identity() * ab_rate_stdev_ * sqrt(dt_)).inverse()),
- sqrt_W_r_dt_inv((Eigen::Matrix3s::Identity() * wb_rate_stdev_ * sqrt(dt_)).inverse())
+ sqrt_A_r_dt_inv((Eigen::Matrix3d::Identity() * ab_rate_stdev_ * sqrt(dt_)).inverse()),
+ sqrt_W_r_dt_inv((Eigen::Matrix3d::Identity() * wb_rate_stdev_ * sqrt(dt_)).inverse())
{
//
}
@@ -220,26 +220,26 @@ inline bool FactorIMU::operator ()(const T* const _p1,
return true;
}
-Eigen::Vector9s FactorIMU::error()
+Eigen::Vector9d FactorIMU::error()
{
- Vector6s bias = capture_other_ptr_.lock()->getCalibration();
+ Vector6d bias = capture_other_ptr_.lock()->getCalibration();
- Map<const Vector3s > acc_bias(bias.data());
- Map<const Vector3s > gyro_bias(bias.data() + 3);
+ Map<const Vector3d > acc_bias(bias.data());
+ Map<const Vector3d > gyro_bias(bias.data() + 3);
- Eigen::Vector9s delta_exp = expectation();
+ Eigen::Vector9d delta_exp = expectation();
- Eigen::Vector9s delta_preint = getMeasurement();
+ Eigen::Vector9d delta_preint = getMeasurement();
- Eigen::Vector9s delta_step;
+ Eigen::Vector9d delta_step;
delta_step.head<3>() = dDp_dab_ * (acc_bias - acc_bias_preint_ ) + dDp_dwb_ * (gyro_bias - gyro_bias_preint_);
delta_step.segment<3>(3) = dDq_dwb_ * (gyro_bias - gyro_bias_preint_);
delta_step.tail<3>() = dDv_dab_ * (acc_bias - acc_bias_preint_ ) + dDv_dwb_ * (gyro_bias - gyro_bias_preint_);
- Eigen::VectorXs delta_corr = imu::plus(delta_preint, delta_step);
+ Eigen::VectorXd delta_corr = imu::plus(delta_preint, delta_step);
- Eigen::Vector9s res = imu::diff(delta_exp, delta_corr);
+ Eigen::Vector9d res = imu::diff(delta_exp, delta_corr);
return res;
}
@@ -421,26 +421,26 @@ inline bool FactorIMU::residual(const Eigen::MatrixBase<D1> & _p1,
return true;
}
-inline Eigen::VectorXs FactorIMU::expectation() const
+inline Eigen::VectorXd FactorIMU::expectation() const
{
FrameBasePtr frm_current = getFeature()->getFrame();
FrameBasePtr frm_previous = getFrameOther();
//get information on current_frame in the FactorIMU
- const Eigen::Vector3s frame_current_pos = (frm_current->getP()->getState());
- const Eigen::Quaternions frame_current_ori (frm_current->getO()->getState().data());
- const Eigen::Vector3s frame_current_vel = (frm_current->getV()->getState());
+ const Eigen::Vector3d frame_current_pos = (frm_current->getP()->getState());
+ const Eigen::Quaterniond frame_current_ori (frm_current->getO()->getState().data());
+ const Eigen::Vector3d frame_current_vel = (frm_current->getV()->getState());
// get info on previous frame in the FactorIMU
- const Eigen::Vector3s frame_previous_pos = (frm_previous->getP()->getState());
- const Eigen::Quaternions frame_previous_ori (frm_previous->getO()->getState().data());
- const Eigen::Vector3s frame_previous_vel = (frm_previous->getV()->getState());
+ const Eigen::Vector3d frame_previous_pos = (frm_previous->getP()->getState());
+ const Eigen::Quaterniond frame_previous_ori (frm_previous->getO()->getState().data());
+ const Eigen::Vector3d frame_previous_vel = (frm_previous->getV()->getState());
// Define results vector and Map bits over it
- Eigen::Matrix<Scalar, 10, 1> exp;
- Eigen::Map<Eigen::Matrix<Scalar, 3, 1> > pe(exp.data() );
- Eigen::Map<Eigen::Quaternion<Scalar> > qe(exp.data() + 3);
- Eigen::Map<Eigen::Matrix<Scalar, 3, 1> > ve(exp.data() + 7);
+ Eigen::Matrix<double, 10, 1> exp;
+ Eigen::Map<Eigen::Matrix<double, 3, 1> > pe(exp.data() );
+ Eigen::Map<Eigen::Quaternion<double> > qe(exp.data() + 3);
+ Eigen::Map<Eigen::Matrix<double, 3, 1> > ve(exp.data() + 7);
expectation(frame_previous_pos, frame_previous_ori, frame_previous_vel,
frame_current_pos, frame_current_ori, frame_current_vel,
diff --git a/include/IMU/factor/factor_fix_bias.h b/include/IMU/factor/factor_fix_bias.h
index 876fa5220cd41f5862574f211cec61de8f677bd3..c6dd1090b6a145a2c4fde782a0d59e39df22275a 100644
--- a/include/IMU/factor/factor_fix_bias.h
+++ b/include/IMU/factor/factor_fix_bias.h
@@ -54,20 +54,20 @@ inline bool FactorFixBias::operator ()(const T* const _ab, const T* const _wb, T
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
// using ceres::Jet;
-// Eigen::MatrixXs J(6,6);
-// J.row(0) = ((Jet<Scalar, 3>)(er(0))).v;
-// J.row(1) = ((Jet<Scalar, 3>)(er(1))).v;
-// J.row(2) = ((Jet<Scalar, 3>)(er(2))).v;
-// J.row(3) = ((Jet<Scalar, 3>)(er(3))).v;
-// J.row(4) = ((Jet<Scalar, 3>)(er(4))).v;
-// J.row(5) = ((Jet<Scalar, 3>)(er(5))).v;
-
-// J.row(0) = ((Jet<Scalar, 3>)(res(0))).v;
-// J.row(1) = ((Jet<Scalar, 3>)(res(1))).v;
-// J.row(2) = ((Jet<Scalar, 3>)(res(2))).v;
-// J.row(3) = ((Jet<Scalar, 3>)(res(3))).v;
-// J.row(4) = ((Jet<Scalar, 3>)(res(4))).v;
-// J.row(5) = ((Jet<Scalar, 3>)(res(5))).v;
+// Eigen::MatrixXd J(6,6);
+// J.row(0) = ((Jet<double, 3>)(er(0))).v;
+// J.row(1) = ((Jet<double, 3>)(er(1))).v;
+// J.row(2) = ((Jet<double, 3>)(er(2))).v;
+// J.row(3) = ((Jet<double, 3>)(er(3))).v;
+// J.row(4) = ((Jet<double, 3>)(er(4))).v;
+// J.row(5) = ((Jet<double, 3>)(er(5))).v;
+
+// J.row(0) = ((Jet<double, 3>)(res(0))).v;
+// J.row(1) = ((Jet<double, 3>)(res(1))).v;
+// J.row(2) = ((Jet<double, 3>)(res(2))).v;
+// J.row(3) = ((Jet<double, 3>)(res(3))).v;
+// J.row(4) = ((Jet<double, 3>)(res(4))).v;
+// J.row(5) = ((Jet<double, 3>)(res(5))).v;
// if (sizeof(er(0)) != sizeof(double))
// {
diff --git a/include/IMU/feature/feature_IMU.h b/include/IMU/feature/feature_IMU.h
index 2413fd551980df7286d9e99a9bf944a1a116d324..6497213dc85c6996930208a48d64b479152c34a8 100644
--- a/include/IMU/feature/feature_IMU.h
+++ b/include/IMU/feature/feature_IMU.h
@@ -26,10 +26,10 @@ class FeatureIMU : public FeatureBase
* \param gyro_bias gyroscope bias of origin frame
* \param _cap_imu_ptr pointer to parent CaptureMotion
*/
- FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
- const Eigen::MatrixXs& _delta_preintegrated_covariance,
- const Eigen::Vector6s& _bias,
- const Eigen::Matrix<Scalar,9,6>& _dD_db_jacobians,
+ FeatureIMU(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::Vector6d& _bias,
+ const Eigen::Matrix<double,9,6>& _dD_db_jacobians,
CaptureMotionPtr _cap_imu_ptr = nullptr);
/** \brief Constructor from capture pointer
@@ -40,33 +40,33 @@ class FeatureIMU : public FeatureBase
virtual ~FeatureIMU();
- const Eigen::Vector3s& getAccBiasPreint() const;
- const Eigen::Vector3s& getGyroBiasPreint() const;
- const Eigen::Matrix<Scalar, 9, 6>& getJacobianBias() const;
+ const Eigen::Vector3d& getAccBiasPreint() const;
+ const Eigen::Vector3d& getGyroBiasPreint() const;
+ const Eigen::Matrix<double, 9, 6>& getJacobianBias() const;
private:
// Used biases
- Eigen::Vector3s acc_bias_preint_; ///< Acceleration bias used for delta preintegration
- Eigen::Vector3s gyro_bias_preint_; ///< Gyrometer bias used for delta preintegration
+ Eigen::Vector3d acc_bias_preint_; ///< Acceleration bias used for delta preintegration
+ Eigen::Vector3d gyro_bias_preint_; ///< Gyrometer bias used for delta preintegration
- Eigen::Matrix<Scalar, 9, 6> jacobian_bias_;
+ Eigen::Matrix<double, 9, 6> jacobian_bias_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
-inline const Eigen::Vector3s& FeatureIMU::getAccBiasPreint() const
+inline const Eigen::Vector3d& FeatureIMU::getAccBiasPreint() const
{
return acc_bias_preint_;
}
-inline const Eigen::Vector3s& FeatureIMU::getGyroBiasPreint() const
+inline const Eigen::Vector3d& FeatureIMU::getGyroBiasPreint() const
{
return gyro_bias_preint_;
}
-inline const Eigen::Matrix<Scalar, 9, 6>& FeatureIMU::getJacobianBias() const
+inline const Eigen::Matrix<double, 9, 6>& FeatureIMU::getJacobianBias() const
{
return jacobian_bias_;
}
diff --git a/include/IMU/math/IMU_tools.h b/include/IMU/math/IMU_tools.h
index 2a599e9477ad9387f93522dd33f73bad2b96f3e1..76a6f5cef00e14e8035204e4f0bd9a4f9e4622a5 100644
--- a/include/IMU/math/IMU_tools.h
+++ b/include/IMU/math/IMU_tools.h
@@ -64,7 +64,7 @@ inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q, MatrixBase<D3>& v)
v << T3(0), T3(0), T3(0);
}
-template<typename T = Scalar>
+template<typename T = double>
inline Matrix<T, 10, 1> identity()
{
Matrix<T, 10, 1> ret;
@@ -188,7 +188,7 @@ inline void compose(const MatrixBase<D1>& d1,
// Jac wrt first delta
J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
J_sum_d1.block(0,3,3,3).noalias() = - dR1 * skew(d2.head(3)) ; // dDp'/dDo
- J_sum_d1.block(0,6,3,3) = Matrix3s::Identity() * dt; // dDp'/dDv = I*dt
+ J_sum_d1.block(0,6,3,3) = Matrix3d::Identity() * dt; // dDp'/dDv = I*dt
J_sum_d1.block(3,3,3,3) = dR2.transpose(); // dDo'/dDo
J_sum_d1.block(6,3,3,3).noalias() = - dR1 * skew(d2.tail(3)) ; // dDv'/dDo
@@ -196,7 +196,7 @@ inline void compose(const MatrixBase<D1>& d1,
J_sum_d2.setIdentity(); //
J_sum_d2.block(0,0,3,3) = dR1; // dDp'/ddp
J_sum_d2.block(6,6,3,3) = dR1; // dDv'/ddv
- // J_sum_d2.block(3,3,3,3) = Matrix3s::Identity(); // dDo'/ddo = I
+ // J_sum_d2.block(3,3,3,3) = Matrix3d::Identity(); // dDo'/ddo = I
// compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
compose(d1, d2, dt, sum);
diff --git a/include/IMU/processor/processor_IMU.h b/include/IMU/processor/processor_IMU.h
index c12188537456673f664ee62d41bfd1520033d083..7a6a7f9a168c9fb4bb11586f4a4e8a39d7b1c6b6 100644
--- a/include/IMU/processor/processor_IMU.h
+++ b/include/IMU/processor/processor_IMU.h
@@ -39,36 +39,36 @@ class ProcessorIMU : public ProcessorMotion{
static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
protected:
- virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
- const Scalar _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) const override;
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _delta_preint_plus_delta) const override;
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _delta_preint_plus_delta,
- Eigen::MatrixXs& _jacobian_delta_preint,
- Eigen::MatrixXs& _jacobian_delta) const override;
- virtual void statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _x_plus_delta ) const override;
- virtual Eigen::VectorXs deltaZero() const override;
+ virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ virtual void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta) const override;
+ virtual void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta,
+ Eigen::MatrixXd& _jacobian_delta_preint,
+ Eigen::MatrixXd& _jacobian_delta) const override;
+ virtual void statePlusDelta(const Eigen::VectorXd& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _x_plus_delta ) const override;
+ virtual Eigen::VectorXd deltaZero() const override;
virtual bool voteForKeyFrame() const override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _calib,
- const VectorXs& _calib_preint,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin) override;
virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
@@ -76,7 +76,7 @@ class ProcessorIMU : public ProcessorMotion{
protected:
ProcessorParamsIMUPtr params_motion_IMU_;
- Eigen::Matrix<Scalar, 9, 9> unmeasured_perturbation_cov_;
+ Eigen::Matrix<double, 9, 9> unmeasured_perturbation_cov_;
};
@@ -88,9 +88,9 @@ class ProcessorIMU : public ProcessorMotion{
namespace wolf{
-inline Eigen::VectorXs ProcessorIMU::deltaZero() const
+inline Eigen::VectorXd ProcessorIMU::deltaZero() const
{
- return (Eigen::VectorXs(10) << 0,0,0, 0,0,0,1, 0,0,0 ).finished(); // p, q, v
+ return (Eigen::VectorXd(10) << 0,0,0, 0,0,0,1, 0,0,0 ).finished(); // p, q, v
}
} // namespace wolf
diff --git a/include/IMU/sensor/sensor_IMU.h b/include/IMU/sensor/sensor_IMU.h
index d592ac88d5480b28d4d82cce187754ec3cc8f0f6..c5da4386a10bc06e40609f16eba9ee9d31fef084 100644
--- a/include/IMU/sensor/sensor_IMU.h
+++ b/include/IMU/sensor/sensor_IMU.h
@@ -14,16 +14,16 @@ WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsIMU);
struct IntrinsicsIMU : public IntrinsicsBase
{
//noise std dev
- Scalar w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
- Scalar a_noise = 0.04; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
+ double w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
+ double a_noise = 0.04; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
//Initial biases std dev
- Scalar ab_initial_stdev = 0.01; //accelerometer micro_g/sec
- Scalar wb_initial_stdev = 0.01; //gyroscope rad/sec
+ double ab_initial_stdev = 0.01; //accelerometer micro_g/sec
+ double wb_initial_stdev = 0.01; //gyroscope rad/sec
// bias rate of change std dev
- Scalar ab_rate_stdev = 0.00001;
- Scalar wb_rate_stdev = 0.00001;
+ double ab_rate_stdev = 0.00001;
+ double wb_rate_stdev = 0.00001;
virtual ~IntrinsicsIMU() = default;
IntrinsicsIMU()
@@ -33,12 +33,12 @@ struct IntrinsicsIMU : public IntrinsicsBase
IntrinsicsIMU(std::string _unique_name, const ParamsServer& _server):
IntrinsicsBase(_unique_name, _server)
{
- w_noise = _server.getParam<Scalar>(_unique_name + "/w_noise");
- a_noise = _server.getParam<Scalar>(_unique_name + "/a_noise");
- ab_initial_stdev = _server.getParam<Scalar>(_unique_name + "/ab_initial_stdev");
- wb_initial_stdev = _server.getParam<Scalar>(_unique_name + "/wb_initial_stdev");
- ab_rate_stdev = _server.getParam<Scalar>(_unique_name + "/ab_rate_stdev");
- wb_rate_stdev = _server.getParam<Scalar>(_unique_name + "/wb_rate_stdev");
+ w_noise = _server.getParam<double>(_unique_name + "/w_noise");
+ a_noise = _server.getParam<double>(_unique_name + "/a_noise");
+ ab_initial_stdev = _server.getParam<double>(_unique_name + "/ab_initial_stdev");
+ wb_initial_stdev = _server.getParam<double>(_unique_name + "/wb_initial_stdev");
+ ab_rate_stdev = _server.getParam<double>(_unique_name + "/ab_rate_stdev");
+ wb_rate_stdev = _server.getParam<double>(_unique_name + "/wb_rate_stdev");
}
std::string print() const
{
@@ -58,60 +58,60 @@ class SensorIMU : public SensorBase
{
protected:
- Scalar a_noise; //Power Spectral Density (same for all the axis) in micro_g/ sqrt(Hz)
- Scalar w_noise; //Rate Noise Spectral Density (same for all the axis) in deg/sec/ sqrt(Hz)
+ double a_noise; //Power Spectral Density (same for all the axis) in micro_g/ sqrt(Hz)
+ double w_noise; //Rate Noise Spectral Density (same for all the axis) in deg/sec/ sqrt(Hz)
//This is a trial to factor how much can the bias change in 1 sec at most
- Scalar ab_initial_stdev; //accelerometer m/sec^s
- Scalar wb_initial_stdev; //gyroscope rad/sec
- Scalar ab_rate_stdev; //accelerometer m/sec^2 / sqrt(sec)
- Scalar wb_rate_stdev; //gyroscope rad/sec / sqrt(sec)
+ double ab_initial_stdev; //accelerometer m/sec^s
+ double wb_initial_stdev; //gyroscope rad/sec
+ double ab_rate_stdev; //accelerometer m/sec^2 / sqrt(sec)
+ double wb_rate_stdev; //gyroscope rad/sec / sqrt(sec)
public:
- SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params);
- SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params);
+ SensorIMU(const Eigen::VectorXd& _extrinsics, const IntrinsicsIMU& _params);
+ SensorIMU(const Eigen::VectorXd& _extrinsics, IntrinsicsIMUPtr _params);
- Scalar getGyroNoise() const;
- Scalar getAccelNoise() const;
- Scalar getWbInitialStdev() const;
- Scalar getAbInitialStdev() const;
- Scalar getWbRateStdev() const;
- Scalar getAbRateStdev() const;
+ double getGyroNoise() const;
+ double getAccelNoise() const;
+ double getWbInitialStdev() const;
+ double getAbInitialStdev() const;
+ double getWbRateStdev() const;
+ double getAbRateStdev() const;
virtual ~SensorIMU();
public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics = nullptr);
+ static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics = nullptr);
};
-inline Scalar SensorIMU::getGyroNoise() const
+inline double SensorIMU::getGyroNoise() const
{
return w_noise;
}
-inline Scalar SensorIMU::getAccelNoise() const
+inline double SensorIMU::getAccelNoise() const
{
return a_noise;
}
-inline Scalar SensorIMU::getWbInitialStdev() const
+inline double SensorIMU::getWbInitialStdev() const
{
return wb_initial_stdev;
}
-inline Scalar SensorIMU::getAbInitialStdev() const
+inline double SensorIMU::getAbInitialStdev() const
{
return ab_initial_stdev;
}
-inline Scalar SensorIMU::getWbRateStdev() const
+inline double SensorIMU::getWbRateStdev() const
{
return wb_rate_stdev;
}
-inline Scalar SensorIMU::getAbRateStdev() const
+inline double SensorIMU::getAbRateStdev() const
{
return ab_rate_stdev;
}
diff --git a/src/capture/capture_IMU.cpp b/src/capture/capture_IMU.cpp
index 23874f1833dac6f1b3c8f92d80751443d684c13a..45ee308845f81dc4e5d0d973085bfb0a0a25217b 100644
--- a/src/capture/capture_IMU.cpp
+++ b/src/capture/capture_IMU.cpp
@@ -6,8 +6,8 @@ namespace wolf {
CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _acc_gyro_data,
- const Eigen::MatrixXs& _data_cov,
+ const Eigen::Vector6d& _acc_gyro_data,
+ const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr) :
CaptureMotion("IMU", _init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 10, 9, _capture_origin_ptr, nullptr, nullptr, std::make_shared<StateBlock>(6, false))
{
@@ -16,9 +16,9 @@ CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _acc_gyro_data,
- const Eigen::MatrixXs& _data_cov,
- const Vector6s& _bias,
+ const Eigen::Vector6d& _acc_gyro_data,
+ const Eigen::MatrixXd& _data_cov,
+ const Vector6d& _bias,
CaptureBasePtr _capture_origin_ptr) :
CaptureMotion("IMU", _init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 10, 9, _capture_origin_ptr, nullptr, nullptr, std::make_shared<StateBlock>(_bias, false))
{
diff --git a/src/feature/feature_IMU.cpp b/src/feature/feature_IMU.cpp
index ee8d115e2f68b3babb7612b6748132d4ac4d6abc..e1a2961348c92abc2ce241ca2fe02d2efbdccec7 100644
--- a/src/feature/feature_IMU.cpp
+++ b/src/feature/feature_IMU.cpp
@@ -2,10 +2,10 @@
namespace wolf {
-FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
- const Eigen::MatrixXs& _delta_preintegrated_covariance,
- const Eigen::Vector6s& _bias,
- const Eigen::Matrix<Scalar,9,6>& _dD_db_jacobians,
+FeatureIMU::FeatureIMU(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::Vector6d& _bias,
+ const Eigen::Matrix<double,9,6>& _dD_db_jacobians,
CaptureMotionPtr _cap_imu_ptr) :
FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance),
acc_bias_preint_(_bias.head<3>()),
diff --git a/src/feature/feature_apriltag.cpp b/src/feature/feature_apriltag.cpp
index 18cb89cdf9813a5112b0aa3b7769834f1318fdaf..8a18da8d07b5fec1d6c231846516dd4057d24bd2 100644
--- a/src/feature/feature_apriltag.cpp
+++ b/src/feature/feature_apriltag.cpp
@@ -3,12 +3,12 @@
namespace wolf {
-FeatureApriltag::FeatureApriltag(const Eigen::Vector7s & _measurement,
- const Eigen::Matrix6s & _meas_uncertainty,
+FeatureApriltag::FeatureApriltag(const Eigen::Vector7d & _measurement,
+ const Eigen::Matrix6d & _meas_uncertainty,
const int _tag_id,
const apriltag_detection_t & _det,
- Scalar _rep_error1,
- Scalar _rep_error2,
+ double _rep_error1,
+ double _rep_error2,
bool _use_rotation,
UncertaintyType _uncertainty_type) :
FeatureBase("APRILTAG", _measurement, _meas_uncertainty, _uncertainty_type),
@@ -33,7 +33,7 @@ FeatureApriltag::~FeatureApriltag()
//
}
-Scalar FeatureApriltag::getTagId() const
+double FeatureApriltag::getTagId() const
{
return tag_id_;
}
@@ -48,12 +48,12 @@ const std::vector<cv::Point2d>& FeatureApriltag::getTagCorners() const
return tag_corners_;
}
-Scalar FeatureApriltag::getRepError1() const
+double FeatureApriltag::getRepError1() const
{
return rep_error1_;
}
-Scalar FeatureApriltag::getRepError2() const
+double FeatureApriltag::getRepError2() const
{
return rep_error2_;
}
diff --git a/src/landmark/landmark_apriltag.cpp b/src/landmark/landmark_apriltag.cpp
index 193c979b275c3e2c4a9202b90347ef376ac1f3d2..dd7112a9da75ea391820cb931ab7c8a94a3143cd 100644
--- a/src/landmark/landmark_apriltag.cpp
+++ b/src/landmark/landmark_apriltag.cpp
@@ -6,12 +6,12 @@
namespace wolf {
-LandmarkApriltag::LandmarkApriltag(Eigen::Vector7s& pose, const int& _tagid, const Scalar& _tag_width) :
+LandmarkApriltag::LandmarkApriltag(Eigen::Vector7d& pose, const int& _tagid, const double& _tag_width) :
LandmarkBase("APRILTAG", std::make_shared<StateBlock>(pose.head(3)), std::make_shared<StateQuaternion>(pose.tail(4))),
tag_id_(_tagid),
tag_width_(_tag_width)
{
- setDescriptor(Eigen::VectorXs::Constant(1,_tagid)); //change tagid to int ? do not use descriptor vector ?
+ setDescriptor(Eigen::VectorXd::Constant(1,_tagid)); //change tagid to int ? do not use descriptor vector ?
setId(_tagid); // overwrite lmk ID to match tag_id.
}
@@ -21,7 +21,7 @@ LandmarkApriltag::~LandmarkApriltag()
}
-Scalar LandmarkApriltag::getTagWidth() const
+double LandmarkApriltag::getTagWidth() const
{
return tag_width_;
}
@@ -39,26 +39,26 @@ LandmarkBasePtr LandmarkApriltag::create(const YAML::Node& _lmk_node)
// Parse YAML node with lmk info and data
unsigned int id = _lmk_node["id"] .as<unsigned int>();
unsigned int tag_id = _lmk_node["tag id"] .as<unsigned int>();
- Scalar tag_width = _lmk_node["tag width"] .as<Scalar>();
- Eigen::Vector3s pos = _lmk_node["position"] .as<Eigen::Vector3s>();
+ double tag_width = _lmk_node["tag width"] .as<double>();
+ Eigen::Vector3d pos = _lmk_node["position"] .as<Eigen::Vector3d>();
bool pos_fixed = _lmk_node["position fixed"] .as<bool>();
- Eigen::Vector4s vquat;
+ Eigen::Vector4d vquat;
if (_lmk_node["orientation"].size() == 3)
{
// we have been given 3 Euler angles in degrees
- Eigen::Vector3s euler = M_TORAD * ( _lmk_node["orientation"] .as<Eigen::Vector3s>() );
- Eigen::Matrix3s R = e2R(euler);
- Eigen::Quaternions quat = R2q(R);
+ Eigen::Vector3d euler = M_TORAD * ( _lmk_node["orientation"] .as<Eigen::Vector3d>() );
+ Eigen::Matrix3d R = e2R(euler);
+ Eigen::Quaterniond quat = R2q(R);
vquat = quat.coeffs();
}
else if (_lmk_node["orientation"].size() == 4)
{
// we have been given a quaternion
- vquat = _lmk_node["orientation"] .as<Eigen::Vector4s>();
+ vquat = _lmk_node["orientation"] .as<Eigen::Vector4d>();
}
bool ori_fixed = _lmk_node["orientation fixed"] .as<bool>();
- Eigen::Vector7s pose; pose << pos, vquat;
+ Eigen::Vector7d pose; pose << pos, vquat;
// Create a new landmark
LandmarkApriltagPtr lmk_ptr = std::make_shared<LandmarkApriltag>(pose, tag_id, tag_width);
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
index a410afaaf843756d75efbc53d073c8bb6fe41de7..46ad0928cb51f068c91a04c849ee5c47f5a5ffe5 100644
--- a/src/processor/processor_IMU.cpp
+++ b/src/processor/processor_IMU.cpp
@@ -13,7 +13,7 @@ ProcessorIMU::ProcessorIMU(ProcessorParamsIMUPtr _params_motion_IMU) :
jacobian_delta_preint_.setIdentity(9,9); // dDp'/dDp, dDv'/dDv, all zeros
jacobian_delta_.setIdentity(9,9); //
jacobian_calib_.setZero(9,6);
- unmeasured_perturbation_cov_ = pow(params_motion_IMU_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix<Scalar, 9, 9>::Identity();
+ unmeasured_perturbation_cov_ = pow(params_motion_IMU_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix<double, 9, 9>::Identity();
}
ProcessorIMU::~ProcessorIMU()
@@ -47,7 +47,7 @@ bool ProcessorIMU::voteForKeyFrame() const
return true;
}
// angle turned
- Scalar angle = 2.0 * asin(delta_integrated_.segment(3,3).norm());
+ double angle = 2.0 * asin(delta_integrated_.segment(3,3).norm());
if (angle > params_motion_IMU_->angle_turned)
{
WOLF_DEBUG( "PM: vote: angle turned" );
@@ -61,10 +61,10 @@ bool ProcessorIMU::voteForKeyFrame() const
CaptureMotionPtr ProcessorIMU::emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _calib,
- const VectorXs& _calib_preint,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin)
{
auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureIMU>(_frame_own,
@@ -99,18 +99,18 @@ FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, Captur
return fac_imu;
}
-void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
- const Scalar _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jac_delta_calib) const
+void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jac_delta_calib) const
{
assert(_data.size() == data_size_ && "Wrong data size!");
using namespace Eigen;
- Matrix<Scalar, 9, 6> jac_delta_data;
+ Matrix<double, 9, 6> jac_delta_data;
/*
* We have the following computing pipeline:
@@ -141,10 +141,10 @@ void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data,
}
-void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _delta_preint_plus_delta) const
+void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta) const
{
/* MATHS according to Sola-16
* Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp = 1/2*(a-a_b)*dt^2
@@ -156,10 +156,10 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
_delta_preint_plus_delta = imu::compose(_delta_preint, _delta, _dt);
}
-void ProcessorIMU::statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _x_plus_delta) const
+void ProcessorIMU::statePlusDelta(const Eigen::VectorXd& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _x_plus_delta) const
{
assert(_x.size() == 10 && "Wrong _x vector size");
assert(_delta.size() == 10 && "Wrong _delta vector size");
@@ -169,12 +169,12 @@ void ProcessorIMU::statePlusDelta(const Eigen::VectorXs& _x,
_x_plus_delta = imu::composeOverState(_x, _delta, _dt);
}
-void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _delta_preint_plus_delta,
- Eigen::MatrixXs& _jacobian_delta_preint,
- Eigen::MatrixXs& _jacobian_delta) const
+void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta,
+ Eigen::MatrixXd& _jacobian_delta_preint,
+ Eigen::MatrixXd& _jacobian_delta) const
{
/*
* Expression of the delta integration step, D' = D (+) d:
diff --git a/src/processor/processor_tracker_landmark_apriltag.cpp b/src/processor/processor_tracker_landmark_apriltag.cpp
index 59e481187f838ee3d3687b44b56c27633b3ca105..148a1bedcf39e695a14b9a48191eeafb21de97c2 100644
--- a/src/processor/processor_tracker_landmark_apriltag.cpp
+++ b/src/processor/processor_tracker_landmark_apriltag.cpp
@@ -143,16 +143,16 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
zarray_get(detections, i, &det);
int tag_id = det->id;
- Scalar tag_width = getTagWidth(tag_id); // tag width in meters
+ double tag_width = getTagWidth(tag_id); // tag width in meters
- Eigen::Isometry3ds c_M_t;
+ Eigen::Isometry3d c_M_t;
bool use_rotation = true;
//////////////////
// IPPE (Infinitesimal Plane-based Pose Estimation)
//////////////////
- Eigen::Isometry3ds M_ippe1, M_ippe2, M_april, M_PnP;
- Scalar rep_error1;
- Scalar rep_error2;
+ Eigen::Isometry3d M_ippe1, M_ippe2, M_april, M_PnP;
+ double rep_error1;
+ double rep_error2;
ippePoseEstimation(det, cv_K_, tag_width, M_ippe1, rep_error1, M_ippe2, rep_error2);
// If not so sure about whether we have the right solution or not, do not create a feature
use_rotation = ((rep_error2 / rep_error1 > ippe_min_ratio_) && rep_error1 < ippe_max_rep_error_);
@@ -160,19 +160,19 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
c_M_t = M_ippe1;
// set the measured pose vector
- Eigen::Vector3s translation ( c_M_t.translation() ); // translation vector in apriltag meters
- Eigen::Vector7s pose;
+ Eigen::Vector3d translation ( c_M_t.translation() ); // translation vector in apriltag meters
+ Eigen::Vector7d pose;
pose << translation, R2q(c_M_t.linear()).coeffs();
// compute the covariance
- // Eigen::Matrix6s cov = getVarVec().asDiagonal() ; // fixed dummy covariance
- Eigen::Matrix6s info = computeInformation(translation, c_M_t.linear(), K_, tag_width, std_pix_); // Lie jacobians covariance
+ // Eigen::Matrix6d cov = getVarVec().asDiagonal() ; // fixed dummy covariance
+ Eigen::Matrix6d info = computeInformation(translation, c_M_t.linear(), K_, tag_width, std_pix_); // Lie jacobians covariance
if (!use_rotation){
// Put a very high covariance on angles measurements (low info matrix)
- info.bottomLeftCorner(3,3) = Eigen::Matrix3s::Zero();
- info.topRightCorner(3,3) = Eigen::Matrix3s::Zero();
- info.bottomRightCorner(3,3) = 0.0001 * Eigen::Matrix3s::Identity();
+ info.bottomLeftCorner(3,3) = Eigen::Matrix3d::Zero();
+ info.topRightCorner(3,3) = Eigen::Matrix3d::Zero();
+ info.bottomRightCorner(3,3) = 0.0001 * Eigen::Matrix3d::Identity();
}
// add to detected features list
@@ -183,7 +183,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
apriltag_detections_destroy(detections);
}
-void ProcessorTrackerLandmarkApriltag::ippePoseEstimation(apriltag_detection_t *_det, cv::Mat_<Scalar> _K, double _tag_width,
+void ProcessorTrackerLandmarkApriltag::ippePoseEstimation(apriltag_detection_t *_det, cv::Mat_<double> _K, double _tag_width,
Eigen::Isometry3d &_M1,
double &_rep_error1,
Eigen::Isometry3d &_M2,
@@ -200,7 +200,7 @@ void ProcessorTrackerLandmarkApriltag::ippePoseEstimation(apriltag_detection_t *
// Same order as the 2D corners (anti clockwise, looking at the tag).
// Looking at the tag, the reference frame is
// X = Right, Y = Down, Z = Inside the plane
- Scalar s = _tag_width/2;
+ double s = _tag_width/2;
obj_pts.emplace_back(-s, s, 0); // bottom left
obj_pts.emplace_back( s, s, 0); // bottom right
obj_pts.emplace_back( s, -s, 0); // top right
@@ -220,15 +220,15 @@ void ProcessorTrackerLandmarkApriltag::ippePoseEstimation(apriltag_detection_t *
// Puts the result in a Eigen affine Transform
cv::Matx33d rmat1;
cv::Rodrigues(rvec1, rmat1);
- Eigen::Matrix3s R_eigen1; cv2eigen(rmat1, R_eigen1);
- Eigen::Vector3s t_eigen1; cv2eigen(tvec1, t_eigen1);
+ Eigen::Matrix3d R_eigen1; cv2eigen(rmat1, R_eigen1);
+ Eigen::Vector3d t_eigen1; cv2eigen(tvec1, t_eigen1);
_M1.matrix().block(0,0,3,3) = R_eigen1;
_M1.matrix().block(0,3,3,1) = t_eigen1;
cv::Matx33d rmat2;
cv::Rodrigues(rvec2, rmat2);
- Eigen::Matrix3s R_eigen2; cv2eigen(rmat2, R_eigen2);
- Eigen::Vector3s t_eigen2; cv2eigen(tvec2, t_eigen2);
+ Eigen::Matrix3d R_eigen2; cv2eigen(rmat2, R_eigen2);
+ Eigen::Vector3d t_eigen2; cv2eigen(tvec2, t_eigen2);
_M2.matrix().block(0,0,3,3) = R_eigen2;
_M2.matrix().block(0,3,3,1) = t_eigen2;
}
@@ -256,27 +256,27 @@ FactorBasePtr ProcessorTrackerLandmarkApriltag::createFactor(FeatureBasePtr _fea
LandmarkBasePtr ProcessorTrackerLandmarkApriltag::createLandmark(FeatureBasePtr _feature_ptr)
{
// world to rob
- Vector3s pos = getLast()->getFrame()->getP()->getState();
- Quaternions quat (getLast()->getFrame()->getO()->getState().data());
- Eigen::Isometry3ds w_M_r = Eigen::Translation<Scalar,3>(pos.head(3)) * quat;
+ Vector3d pos = getLast()->getFrame()->getP()->getState();
+ Quaterniond quat (getLast()->getFrame()->getO()->getState().data());
+ Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos.head(3)) * quat;
// rob to camera
pos = getSensor()->getP()->getState();
quat.coeffs() = getSensor()->getO()->getState();
- Eigen::Isometry3ds r_M_c = Eigen::Translation<Scalar,3>(pos.head(3)) * quat;
+ Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos.head(3)) * quat;
// camera to lmk (tag)
pos = _feature_ptr->getMeasurement().head(3);
quat.coeffs() = _feature_ptr->getMeasurement().tail(4);
- Eigen::Isometry3ds c_M_t = Eigen::Translation<Scalar,3>(pos) * quat;
+ Eigen::Isometry3d c_M_t = Eigen::Translation<double,3>(pos) * quat;
// world to lmk (tag)
- Eigen::Isometry3ds w_M_t = w_M_r * r_M_c * c_M_t;
+ Eigen::Isometry3d w_M_t = w_M_r * r_M_c * c_M_t;
// make 7-vector for lmk (tag) pose
pos = w_M_t.translation();
quat = w_M_t.linear();
- Vector7s w_pose_t;
+ Vector7d w_pose_t;
w_pose_t << pos, quat.coeffs();
FeatureApriltagPtr feat_april = std::static_pointer_cast<FeatureApriltag>(_feature_ptr);
@@ -356,7 +356,7 @@ bool ProcessorTrackerLandmarkApriltag::voteForKeyFrame()
else{
enough_info = true;
}
- Scalar dt_incoming_origin = getIncoming()->getTimeStamp().get() - getOrigin()->getTimeStamp().get();
+ double dt_incoming_origin = getIncoming()->getTimeStamp().get() - getOrigin()->getTimeStamp().get();
bool more_than_min_time_vote = dt_incoming_origin > min_time_vote_;
// Possible decision factors
@@ -383,7 +383,7 @@ unsigned int ProcessorTrackerLandmarkApriltag::findLandmarks(const LandmarkBaseP
if(std::static_pointer_cast<LandmarkApriltag>(landmark_in_ptr)->getTagId() == tag_id)
{
_feature_list_out.push_back(feature_in_image);
- Scalar score(1.0);
+ double score(1.0);
LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(landmark_in_ptr, score); //TODO: smarter score
_feature_landmark_correspondences.emplace ( feature_in_image, matched_landmark );
break;
@@ -394,7 +394,7 @@ unsigned int ProcessorTrackerLandmarkApriltag::findLandmarks(const LandmarkBaseP
return _feature_list_out.size();
}
-wolf::Scalar ProcessorTrackerLandmarkApriltag::getTagWidth(int _id) const
+double ProcessorTrackerLandmarkApriltag::getTagWidth(int _id) const
{
if (tag_widths_.find(_id) != tag_widths_.end())
return tag_widths_.at(_id);
@@ -402,34 +402,34 @@ wolf::Scalar ProcessorTrackerLandmarkApriltag::getTagWidth(int _id) const
return tag_width_default_;
}
-Eigen::Vector6s ProcessorTrackerLandmarkApriltag::getVarVec()
+Eigen::Vector6d ProcessorTrackerLandmarkApriltag::getVarVec()
{
- Eigen::Vector6s var_vec;
+ Eigen::Vector6d var_vec;
var_vec << std_xy_*std_xy_, std_xy_*std_xy_, std_z_*std_z_, std_rpy_*std_rpy_, std_rpy_*std_rpy_, std_rpy_*std_rpy_;
return var_vec;
}
-Eigen::Matrix6s ProcessorTrackerLandmarkApriltag::computeInformation(Eigen::Vector3s const &t, Eigen::Matrix3s const &R, Eigen::Matrix3s const &K, Scalar const &tag_width, double const &sig_q)
+Eigen::Matrix6d ProcessorTrackerLandmarkApriltag::computeInformation(Eigen::Vector3d const &t, Eigen::Matrix3d const &R, Eigen::Matrix3d const &K, double const &tag_width, double const &sig_q)
{
// Same order as the 2D corners (anti clockwise, looking at the tag).
// Looking at the tag, the reference frame is
// X = Right, Y = Down, Z = Inside the plane
- Scalar s = tag_width/2;
- Eigen::Vector3s p1; p1 << -s, s, 0; // bottom left
- Eigen::Vector3s p2; p2 << s, s, 0; // bottom right
- Eigen::Vector3s p3; p3 << s, -s, 0; // top right
- Eigen::Vector3s p4; p4 << -s, -s, 0; // top left
- std::vector<Eigen::Vector3s> pvec = {p1, p2, p3, p4};
- std::vector<Eigen::Vector2s> proj_pix_vec; proj_pix_vec.resize(4);
+ double s = tag_width/2;
+ Eigen::Vector3d p1; p1 << -s, s, 0; // bottom left
+ Eigen::Vector3d p2; p2 << s, s, 0; // bottom right
+ Eigen::Vector3d p3; p3 << s, -s, 0; // top right
+ Eigen::Vector3d p4; p4 << -s, -s, 0; // top left
+ std::vector<Eigen::Vector3d> pvec = {p1, p2, p3, p4};
+ std::vector<Eigen::Vector2d> proj_pix_vec; proj_pix_vec.resize(4);
// Initialize jacobian matrices
- Eigen::Matrix<Scalar, 8, 6> J_u_TR = Eigen::Matrix<Scalar, 8, 6>::Zero(); // 2N x 6 jac
- Eigen::Vector3s h;
- Eigen::Matrix3s J_h_T, J_h_R;
- Eigen::Vector2s eu; // 2D pixel coord, not needed
- Eigen::Matrix<Scalar, 3, 6> J_h_TR;
- Eigen::Matrix<Scalar, 2, 3> J_u_h;
+ Eigen::Matrix<double, 8, 6> J_u_TR = Eigen::Matrix<double, 8, 6>::Zero(); // 2N x 6 jac
+ Eigen::Vector3d h;
+ Eigen::Matrix3d J_h_T, J_h_R;
+ Eigen::Vector2d eu; // 2D pixel coord, not needed
+ Eigen::Matrix<double, 3, 6> J_h_TR;
+ Eigen::Matrix<double, 2, 3> J_u_h;
for (int i=0; i < pvec.size(); i++){
// Pinhole projection to non normalized homogeneous coordinates in pixels (along with jacobians)
pinholeHomogeneous(K, t, R, pvec[i], h, J_h_T, J_h_R);
@@ -443,22 +443,22 @@ Eigen::Matrix6s ProcessorTrackerLandmarkApriltag::computeInformation(Eigen::Vect
}
// Pixel uncertainty covariance matrix
- Eigen::Matrix<Scalar, 8, 8> pixel_cov = pow(sig_q, 2) * Eigen::Matrix<Scalar, 8, 8>::Identity();
+ Eigen::Matrix<double, 8, 8> pixel_cov = pow(sig_q, 2) * Eigen::Matrix<double, 8, 8>::Identity();
// 6 x 6 translation|rotation information matrix computed with covariance propagation formula (inverted)
- Eigen::Matrix6s transformation_info = (J_u_TR.transpose() * pixel_cov.inverse() * J_u_TR); // Wolf jac
+ Eigen::Matrix6d transformation_info = (J_u_TR.transpose() * pixel_cov.inverse() * J_u_TR); // Wolf jac
return transformation_info;
}
-void ProcessorTrackerLandmarkApriltag::pinholeHomogeneous(Eigen::Matrix3s const & K, Eigen::Vector3s const & t,
- Eigen::Matrix3s const & R, Eigen::Vector3s const & p,
- Eigen::Vector3s &h, Eigen::Matrix3s &J_h_T, Eigen::Matrix3s &J_h_R)
+void ProcessorTrackerLandmarkApriltag::pinholeHomogeneous(Eigen::Matrix3d const & K, Eigen::Vector3d const & t,
+ Eigen::Matrix3d const & R, Eigen::Vector3d const & p,
+ Eigen::Vector3d &h, Eigen::Matrix3d &J_h_T, Eigen::Matrix3d &J_h_R)
{
// Pinhole projection + jacobians
h = K * (t + R * p);
J_h_T = K;
- Eigen::Matrix3s p_hat;
+ Eigen::Matrix3d p_hat;
p_hat << 0, -p(2), p(1),
p(2), 0, -p(0),
-p(1), p(0), 0;
@@ -481,7 +481,7 @@ void ProcessorTrackerLandmarkApriltag::configure(SensorBasePtr _sensor)
sen_cam_ptr->useRectifiedImages();
// get camera intrinsic parameters
- Eigen::Vector4s k(_sensor->getIntrinsic()->getState()); //[cx cy fx fy]
+ Eigen::Vector4d k(_sensor->getIntrinsic()->getState()); //[cx cy fx fy]
// Intrinsic matrices for opencv and eigen:
@@ -513,9 +513,9 @@ void ProcessorTrackerLandmarkApriltag::resetDerived()
{
FrameBasePtr ori_frame = getOrigin()->getFrame();
- Eigen::Vector1s dist_meas; dist_meas << 0.0;
+ Eigen::Vector1d dist_meas; dist_meas << 0.0;
double dist_std = 0.5;
- Eigen::Matrix1s cov0(dist_std*dist_std);
+ Eigen::Matrix1d cov0(dist_std*dist_std);
CaptureBasePtr capt3D = std::make_shared<CaptureBase>("Dist", getLast()->getTimeStamp());
getLast()->getFrame()->addCapture(capt3D);
@@ -550,8 +550,8 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
// (getOrigin() != nullptr)
// Retrieve camera extrinsics
- Eigen::Quaternions last_q_cam(getSensor()->getO()->getState().data());
- Eigen::Isometry3ds last_M_cam = Eigen::Translation3ds(getSensor()->getP()->getState()) * last_q_cam;
+ Eigen::Quaterniond last_q_cam(getSensor()->getO()->getState().data());
+ Eigen::Isometry3d last_M_cam = Eigen::Translation3d(getSensor()->getP()->getState()) * last_q_cam;
// get features list of KF linked to origin capture and last capture
FeatureBasePtrList ori_feature_list = getOrigin()->getFeatureList();
@@ -564,7 +564,7 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
}
// Among landmarks detected in origin and last, find the one that has the smallest error ratio (best confidence)
- Scalar lowest_ration = 1; // rep_error1/rep_error2 cannot be higher than 1
+ double lowest_ration = 1; // rep_error1/rep_error2 cannot be higher than 1
FeatureApriltagPtr best_feature;
bool useable_feature = false;
for (auto it_last = last_feature_list.begin(); it_last != last_feature_list.end(); it_last++){
@@ -572,7 +572,7 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
for (auto it_ori = ori_feature_list.begin(); it_ori != ori_feature_list.end(); it_ori++){
FeatureApriltagPtr ori_feat_ptr = std::static_pointer_cast<FeatureApriltag>(*it_ori);
if (ori_feat_ptr->getTagId() == last_feat_ptr->getTagId()){
- Scalar ratio = ori_feat_ptr->getRepError1() / ori_feat_ptr->getRepError2();
+ double ratio = ori_feat_ptr->getRepError1() / ori_feat_ptr->getRepError2();
//if (ratio < lowest_ration){
if (last_feat_ptr->getUserotation() && (ratio < lowest_ration)){
useable_feature = true;
@@ -591,12 +591,12 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
// std::cout << "Best feature id after: " << best_feature->getTagId() << std::endl;
// Retrieve cam to landmark transform
- Eigen::Vector7s cam_pose_lmk = best_feature->getMeasurement();
- Eigen::Quaternions cam_q_lmk(cam_pose_lmk.segment<4>(3).data());
- Eigen::Isometry3ds cam_M_lmk = Eigen::Translation3ds(cam_pose_lmk.head(3)) * cam_q_lmk;
+ Eigen::Vector7d cam_pose_lmk = best_feature->getMeasurement();
+ Eigen::Quaterniond cam_q_lmk(cam_pose_lmk.segment<4>(3).data());
+ Eigen::Isometry3d cam_M_lmk = Eigen::Translation3d(cam_pose_lmk.head(3)) * cam_q_lmk;
// Get corresponding landmarks in origin/last landmark list
- Eigen::Isometry3ds w_M_lmk;
+ Eigen::Isometry3d w_M_lmk;
LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList();
// Iterate in reverse order because landmark detected in last are at the end of the list (while still landmarks to discovers)
for (std::list<LandmarkBasePtr>::reverse_iterator it_lmk = lmk_list.rbegin(); it_lmk != lmk_list.rend(); ++it_lmk){
@@ -607,18 +607,18 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
}
if (lmk_ptr->getTagId() == best_feature->getTagId()){
- Eigen::Vector3s w_t_lmk = lmk_ptr->getP()->getState();
- Eigen::Quaternions w_q_lmk(lmk_ptr->getO()->getState().data());
- w_M_lmk = Eigen::Translation<Scalar,3>(w_t_lmk) * w_q_lmk;
+ Eigen::Vector3d w_t_lmk = lmk_ptr->getP()->getState();
+ Eigen::Quaterniond w_q_lmk(lmk_ptr->getO()->getState().data());
+ w_M_lmk = Eigen::Translation<double,3>(w_t_lmk) * w_q_lmk;
}
}
// Compute last frame estimate
- Eigen::Isometry3ds w_M_last = w_M_lmk * (last_M_cam * cam_M_lmk).inverse();
+ Eigen::Isometry3d w_M_last = w_M_lmk * (last_M_cam * cam_M_lmk).inverse();
// Use the w_M_last to overide last key frame state estimation and keyframe estimation
- Eigen::Vector3s pos_last = w_M_last.translation();
- Eigen::Quaternions quat_last(w_M_last.linear());
+ Eigen::Vector3d pos_last = w_M_last.translation();
+ Eigen::Quaterniond quat_last(w_M_last.linear());
getLast()->getFrame()->getP()->setState(pos_last);
getLast()->getFrame()->getO()->setState(quat_last.coeffs());
@@ -631,10 +631,10 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
for (auto it_feat = new_features_last_.begin(); it_feat != new_features_last_.end(); it_feat++){
FeatureApriltagPtr new_feature_last = std::static_pointer_cast<FeatureApriltag>(*it_feat);
- Eigen::Vector7s cam_pose_lmk = new_feature_last->getMeasurement();
- Eigen::Quaternions cam_q_lmk(cam_pose_lmk.segment<4>(3).data());
- Eigen::Isometry3ds cam_M_lmk_new = Eigen::Translation3ds(cam_pose_lmk.head(3)) * cam_q_lmk;
- Eigen::Isometry3ds w_M_lmk = w_M_last * last_M_cam * cam_M_lmk_new;
+ Eigen::Vector7d cam_pose_lmk = new_feature_last->getMeasurement();
+ Eigen::Quaterniond cam_q_lmk(cam_pose_lmk.segment<4>(3).data());
+ Eigen::Isometry3d cam_M_lmk_new = Eigen::Translation3d(cam_pose_lmk.head(3)) * cam_q_lmk;
+ Eigen::Isometry3d w_M_lmk = w_M_last * last_M_cam * cam_M_lmk_new;
for (auto it_lmk = new_landmarks_.begin(); it_lmk != new_landmarks_.end(); ++it_lmk){
LandmarkApriltagPtr lmk_ptr = std::dynamic_pointer_cast<LandmarkApriltag>(*it_lmk);
@@ -642,8 +642,8 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
continue;
}
if (lmk_ptr->getTagId() == new_feature_last->getTagId()){
- Eigen::Vector3s pos_lmk_last = w_M_lmk.translation();
- Eigen::Quaternions quat_lmk_last(w_M_lmk.linear());
+ Eigen::Vector3d pos_lmk_last = w_M_lmk.translation();
+ Eigen::Quaterniond quat_lmk_last(w_M_lmk.linear());
lmk_ptr->getP()->setState(pos_lmk_last);
lmk_ptr->getO()->setState(quat_lmk_last.coeffs());
break;
diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp
index 33cd3e16ec62bfec0d656f376227405ab43f735c..4418283ea29e2d0be1f1d99083e2e712a7a1f845 100644
--- a/src/sensor/sensor_IMU.cpp
+++ b/src/sensor/sensor_IMU.cpp
@@ -4,14 +4,14 @@
namespace wolf {
-SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params) :
+SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, IntrinsicsIMUPtr _params) :
SensorIMU(_extrinsics, *_params)
{
//
}
-SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params) :
- SensorBase("IMU", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6s()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, true),
+SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, const IntrinsicsIMU& _params) :
+ SensorBase("IMU", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6d()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, true),
a_noise(_params.a_noise),
w_noise(_params.w_noise),
ab_initial_stdev(_params.ab_initial_stdev),
@@ -28,7 +28,7 @@ SensorIMU::~SensorIMU()
}
// Define the factory method
-SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq,
+SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics_pq,
const IntrinsicsBasePtr _intrinsics)
{
// decode extrinsics vector
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index 05d89142a35cc740c69c29712565d132d69fbe93..805307fcd4ba78aee0b7d73ceba8a700c67a59cf 100644
--- a/src/solver_suitesparse/solver_manager.cpp
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -61,7 +61,7 @@ void SolverManager::update(const WolfProblemPtr _problem_ptr)
void SolverManager::addFactor(FactorBasePtr _corr_ptr)
{
- //TODO MatrixXs J; Vector e;
+ //TODO MatrixXd J; Vector e;
// getResidualsAndJacobian(_corr_ptr, J, e);
// solverQR->addFactor(_corr_ptr, J, e);
diff --git a/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp
index a0ed780179c2e4d6bfc1f194be857dcbc9bc3b3e..c077e9e0f1a13cdf2a4a3dee513e5f476e8848cc 100644
--- a/src/yaml/processor_IMU_yaml.cpp
+++ b/src/yaml/processor_IMU_yaml.cpp
@@ -29,13 +29,13 @@ static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _file
YAML::Node kf_vote = config["keyframe_vote"];
ProcessorParamsIMUPtr params = std::make_shared<ProcessorParamsIMU>();
- params->time_tolerance = config["time_tolerance"] .as<Scalar>();
- params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"].as<Scalar>();
+ params->time_tolerance = config["time_tolerance"] .as<double>();
+ params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"].as<double>();
- params->max_time_span = kf_vote["max_time_span"] .as<Scalar>();
+ params->max_time_span = kf_vote["max_time_span"] .as<double>();
params->max_buff_length = kf_vote["max_buff_length"] .as<SizeEigen>();
- params->dist_traveled = kf_vote["dist_traveled"] .as<Scalar>();
- params->angle_turned = kf_vote["angle_turned"] .as<Scalar>();
+ params->dist_traveled = kf_vote["dist_traveled"] .as<double>();
+ params->angle_turned = kf_vote["angle_turned"] .as<double>();
params->voting_active = kf_vote["voting_active"] .as<bool>();
params->voting_aux_active = kf_vote["voting_aux_active"] .as<bool>();
return params;
diff --git a/src/yaml/sensor_IMU_yaml.cpp b/src/yaml/sensor_IMU_yaml.cpp
index 0d6d4c5d49ffa98f7db71aac0a174826a07264c1..af5314497dc7e5ce3b22f066b61cf44411d386d0 100644
--- a/src/yaml/sensor_IMU_yaml.cpp
+++ b/src/yaml/sensor_IMU_yaml.cpp
@@ -32,12 +32,12 @@ static IntrinsicsBasePtr createIntrinsicsIMU(const std::string & _filename_dot_y
IntrinsicsIMUPtr params = std::make_shared<IntrinsicsIMU>();
- params->a_noise = variances["a_noise"] .as<Scalar>();
- params->w_noise = variances["w_noise"] .as<Scalar>();
- params->ab_initial_stdev = variances["ab_initial_stdev"] .as<Scalar>();
- params->wb_initial_stdev = variances["wb_initial_stdev"] .as<Scalar>();
- params->ab_rate_stdev = variances["ab_rate_stdev"] .as<Scalar>();
- params->wb_rate_stdev = variances["wb_rate_stdev"] .as<Scalar>();
+ params->a_noise = variances["a_noise"] .as<double>();
+ params->w_noise = variances["w_noise"] .as<double>();
+ params->ab_initial_stdev = variances["ab_initial_stdev"] .as<double>();
+ params->wb_initial_stdev = variances["wb_initial_stdev"] .as<double>();
+ params->ab_rate_stdev = variances["ab_rate_stdev"] .as<double>();
+ params->wb_rate_stdev = variances["wb_rate_stdev"] .as<double>();
return params;
}
diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp
index 2e06cb01420ae2c8d3aed65d9c7ea47bf1808e1b..8f94c7a247931523f7dd264efbeb64e09148e097 100644
--- a/test/gtest_IMU.cpp
+++ b/test/gtest_IMU.cpp
@@ -42,44 +42,44 @@ class Process_Factor_IMU : public testing::Test
// time
TimeStamp t0, t;
- Scalar dt, DT;
+ double dt, DT;
int num_integrations;
// initial state
- VectorXs x0; // initial state
- Vector3s p0, v0; // initial pos and vel
- Quaternions q0, q; // initial and current orientations
- Matrix<Scalar,9,9> P0; // initial state covariance
+ VectorXd x0; // initial state
+ Vector3d p0, v0; // initial pos and vel
+ Quaterniond q0, q; // initial and current orientations
+ Matrix<double,9,9> P0; // initial state covariance
// bias
- Vector6s bias_real, bias_preint, bias_null; // real, pre-int and zero biases.
- Vector6s bias_0, bias_1; // optimized bias at KF's 0 and 1
+ Vector6d bias_real, bias_preint, bias_null; // real, pre-int and zero biases.
+ Vector6d bias_0, bias_1; // optimized bias at KF's 0 and 1
// input
- Matrix<Scalar, 6, Dynamic> motion; // Motion in IMU frame. Each column is a motion step. If just one column, then the number of steps is defined in num_integrations
- Matrix<Scalar, 3, Dynamic> a, w; // True acc and angvel in IMU frame. Each column is a motion step. Used to create motion with `motion << a,w;`
- Vector6s data; // IMU data. It's the motion with gravity and bias. See imu::motion2data().
+ Matrix<double, 6, Dynamic> motion; // Motion in IMU frame. Each column is a motion step. If just one column, then the number of steps is defined in num_integrations
+ Matrix<double, 3, Dynamic> a, w; // True acc and angvel in IMU frame. Each column is a motion step. Used to create motion with `motion << a,w;`
+ Vector6d data; // IMU data. It's the motion with gravity and bias. See imu::motion2data().
// Deltas and states (exact, integrated, corrected, solved, etc)
- VectorXs D_exact, x1_exact; // exact or ground truth
- VectorXs D_preint_imu, x1_preint_imu; // preintegrated with imu_tools
- VectorXs D_corrected_imu, x1_corrected_imu; // corrected with imu_tools
- VectorXs D_preint, x1_preint; // preintegrated with processor_imu
- VectorXs D_corrected, x1_corrected; // corrected with processor_imu
- VectorXs D_optim, x1_optim; // optimized using factor_imu
- VectorXs D_optim_imu, x1_optim_imu; // corrected with imu_tools using optimized bias
- VectorXs x0_optim; // optimized using factor_imu
+ VectorXd D_exact, x1_exact; // exact or ground truth
+ VectorXd D_preint_imu, x1_preint_imu; // preintegrated with imu_tools
+ VectorXd D_corrected_imu, x1_corrected_imu; // corrected with imu_tools
+ VectorXd D_preint, x1_preint; // preintegrated with processor_imu
+ VectorXd D_corrected, x1_corrected; // corrected with processor_imu
+ VectorXd D_optim, x1_optim; // optimized using factor_imu
+ VectorXd D_optim_imu, x1_optim_imu; // corrected with imu_tools using optimized bias
+ VectorXd x0_optim; // optimized using factor_imu
// Trajectory buffer of correction Jacobians
- std::vector<MatrixXs> Buf_Jac_preint_prc;
+ std::vector<MatrixXd> Buf_Jac_preint_prc;
// Trajectory matrices
- MatrixXs Trj_D_exact, Trj_D_preint_imu, Trj_D_preint_prc, Trj_D_corrected_imu, Trj_D_corrected_prc;
- MatrixXs Trj_x_exact, Trj_x_preint_imu, Trj_x_preint_prc, Trj_x_corrected_imu, Trj_x_corrected_prc;
+ MatrixXd Trj_D_exact, Trj_D_preint_imu, Trj_D_preint_prc, Trj_D_corrected_imu, Trj_D_corrected_prc;
+ MatrixXd Trj_x_exact, Trj_x_preint_imu, Trj_x_preint_prc, Trj_x_corrected_imu, Trj_x_corrected_prc;
// Delta correction Jacobian and step
- Matrix<Scalar,9,6> J_D_bias; // Jacobian of pre-integrated delta w
- Vector9s step;
+ Matrix<double,9,6> J_D_bias; // Jacobian of pre-integrated delta w
+ Vector9d step;
// Flags for fixing/unfixing state blocks
bool p0_fixed, q0_fixed, v0_fixed;
@@ -97,7 +97,7 @@ class Process_Factor_IMU : public testing::Test
ceres_manager = make_shared<CeresManager>(problem, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sensor = problem->installSensor ("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+ SensorBasePtr sensor = problem->installSensor ("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
ProcessorBasePtr processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
sensor_imu = static_pointer_cast<SensorIMU> (sensor);
processor_imu = static_pointer_cast<ProcessorIMU>(processor);
@@ -126,16 +126,16 @@ class Process_Factor_IMU : public testing::Test
* Delta: the preintegrated delta
* J_D_b_ptr: a pointer to the Jacobian of Delta wrt bias. Defaults to nullptr.
*/
- static void integrateOneStep(const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Quaternions& q_real, VectorXs& Delta, Matrix<Scalar, 9, 6>* J_D_b_ptr = nullptr)
+ static void integrateOneStep(const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Quaterniond& q_real, VectorXd& Delta, Matrix<double, 9, 6>* J_D_b_ptr = nullptr)
{
- VectorXs delta(10), data(6);
- VectorXs Delta_plus(10);
- Matrix<Scalar, 9, 6> J_d_d, J_D_b, J_d_b;
- Matrix<Scalar, 9, 9> J_D_D, J_D_d;
+ VectorXd delta(10), data(6);
+ VectorXd Delta_plus(10);
+ Matrix<double, 9, 6> J_d_d, J_D_b, J_d_b;
+ Matrix<double, 9, 9> J_D_D, J_D_d;
data = imu::motion2data(motion, q_real, bias_real);
q_real = q_real*exp_q(motion.tail(3)*dt);
- Vector6s body = data - bias_preint;
+ Vector6d body = data - bias_preint;
if (J_D_b_ptr == nullptr)
{
delta = imu::body2delta(body, dt);
@@ -163,11 +163,11 @@ class Process_Factor_IMU : public testing::Test
* Output:
* return: the preintegrated delta
*/
- static VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt)
+ static VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt)
{
- VectorXs Delta(10);
+ VectorXd Delta(10);
Delta = imu::identity();
- Quaternions q(q0);
+ Quaterniond q(q0);
for (int n = 0; n < N; n++)
{
integrateOneStep(motion, bias_real, bias_preint, dt, q, Delta);
@@ -186,10 +186,10 @@ class Process_Factor_IMU : public testing::Test
* J_D_b: the Jacobian of the preintegrated delta wrt the bias
* return: the preintegrated delta
*/
- static VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
+ static VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Matrix<double, 9, 6>& J_D_b)
{
- VectorXs Delta(10);
- Quaternions q;
+ VectorXd Delta(10);
+ Quaterniond q;
Delta = imu::identity();
J_D_b .setZero();
@@ -211,10 +211,10 @@ class Process_Factor_IMU : public testing::Test
* J_D_b: the Jacobian of the preintegrated delta wrt the bias
* return: the preintegrated delta
*/
- static VectorXs integrateDelta(const Quaternions& q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
+ static VectorXd integrateDelta(const Quaterniond& q0, const MatrixXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Matrix<double, 9, 6>& J_D_b)
{
- VectorXs Delta(10);
- Quaternions q;
+ VectorXd Delta(10);
+ Quaterniond q;
Delta = imu::identity();
J_D_b .setZero();
@@ -236,18 +236,18 @@ class Process_Factor_IMU : public testing::Test
* J_D_b: the Jacobian of the preintegrated delta wrt the bias
* return: the preintegrated delta
*/
- static MotionBuffer integrateDeltaTrajectory(const Quaternions& q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
+ static MotionBuffer integrateDeltaTrajectory(const Quaterniond& q0, const MatrixXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Matrix<double, 9, 6>& J_D_b)
{
MotionBuffer trajectory;
- VectorXs Delta(10);
- MatrixXs M9(9,9), M6(6,6), J9(9,9), J96(9,6), V10(10,1), V6(6,1);
- Quaternions q;
+ VectorXd Delta(10);
+ MatrixXd M9(9,9), M6(6,6), J9(9,9), J96(9,6), V10(10,1), V6(6,1);
+ Quaterniond q;
Delta = imu::identity();
J_D_b .setZero();
q = q0;
TimeStamp t(0);
- trajectory.get().emplace_back(t, Vector6s::Zero(), M6, VectorXs::Zero(10), M9, imu::identity(), M9, J9, J9, MatrixXs::Zero(9,6));
+ trajectory.get().emplace_back(t, Vector6d::Zero(), M6, VectorXd::Zero(10), M9, imu::identity(), M9, J9, J9, MatrixXd::Zero(9,6));
for (int n = 0; n < motion.cols(); n++)
{
t += dt;
@@ -257,9 +257,9 @@ class Process_Factor_IMU : public testing::Test
return trajectory;
}
- MotionBuffer integrateWithProcessor(int N, const TimeStamp& t0, const Quaternions q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, VectorXs& D_preint, VectorXs& D_corrected)
+ MotionBuffer integrateWithProcessor(int N, const TimeStamp& t0, const Quaterniond q0, const MatrixXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, VectorXd& D_preint, VectorXd& D_corrected)
{
- Vector6s motion_now;
+ Vector6d motion_now;
data = imu::motion2data(motion.col(0), q0, bias_real);
capture_imu = make_shared<CaptureIMU>(t0, sensor_imu, data, sensor_imu->getNoiseCov());
q = q0;
@@ -367,7 +367,7 @@ class Process_Factor_IMU : public testing::Test
// Build exact trajectories
int col = 0;
- Scalar Dt = 0;
+ double Dt = 0;
for (auto m : Buf_exact.get())
{
Trj_D_exact.col(col) = m.delta_integr_;
@@ -394,7 +394,7 @@ class Process_Factor_IMU : public testing::Test
Trj_x_preint_imu.col(col) = imu::composeOverState(x0, Trj_D_preint_imu.col(col).eval(), Dt );
// corrected
- VectorXs step = m.jacobian_calib_ * (bias_real - bias_preint);
+ VectorXd step = m.jacobian_calib_ * (bias_real - bias_preint);
Trj_D_corrected_imu.col(col) = imu::plus(m.delta_integr_, step) ;
Trj_x_corrected_imu.col(col) = imu::composeOverState(x0, Trj_D_corrected_imu.col(col).eval(), Dt );
Dt += dt;
@@ -426,7 +426,7 @@ class Process_Factor_IMU : public testing::Test
Trj_x_preint_prc.col(col) = imu::composeOverState(x0, Trj_D_preint_prc.col(col).eval(), Dt );
// corrected
- VectorXs step = m.jacobian_calib_ * (bias_real - bias_preint);
+ VectorXd step = m.jacobian_calib_ * (bias_real - bias_preint);
Trj_D_corrected_prc.col(col) = imu::plus(m.delta_integr_, step) ;
Trj_x_corrected_prc.col(col) = imu::composeOverState(x0, Trj_D_corrected_prc.col(col).eval(), Dt );
@@ -457,26 +457,26 @@ class Process_Factor_IMU : public testing::Test
// This perturbs states to estimate around the exact value, then assigns to the keyframe
// Perturbations are applied only if the state block is unfixed
- VectorXs x_pert(10);
+ VectorXd x_pert(10);
// KF 0
x_pert = x0;
if (!p0_fixed)
- x_pert.head(3) += Vector3s::Random() * 0.01;
+ x_pert.head(3) += Vector3d::Random() * 0.01;
if (!q0_fixed)
- x_pert.segment(3,4) = (Quaternions(x_pert.data() + 3) * exp_q(Vector3s::Random() * 0.01)).coeffs().normalized();
+ x_pert.segment(3,4) = (Quaterniond(x_pert.data() + 3) * exp_q(Vector3d::Random() * 0.01)).coeffs().normalized();
if (!v0_fixed)
- x_pert.tail(3) += Vector3s::Random() * 0.01;
+ x_pert.tail(3) += Vector3d::Random() * 0.01;
KF_0->setState(x_pert);
// KF 1
x_pert = x1_exact;
if (!p1_fixed)
- x_pert.head(3) += Vector3s::Random() * 0.01;
+ x_pert.head(3) += Vector3d::Random() * 0.01;
if (!q1_fixed)
- x_pert.segment(3,4) = (Quaternions(x_pert.data() + 3) * exp_q(Vector3s::Random() * 0.01)).coeffs().normalized();
+ x_pert.segment(3,4) = (Quaterniond(x_pert.data() + 3) * exp_q(Vector3d::Random() * 0.01)).coeffs().normalized();
if (!v1_fixed)
- x_pert.tail(3) += Vector3s::Random() * 0.01;
+ x_pert.tail(3) += Vector3d::Random() * 0.01;
KF_1->setState(x_pert);
}
@@ -489,7 +489,7 @@ class Process_Factor_IMU : public testing::Test
problem->keyFrameCallback(KF, nullptr, dt/2);
// Process IMU for the callback to take effect
- data = Vector6s::Zero();
+ data = Vector6d::Zero();
capture_imu = make_shared<CaptureIMU>(t+dt, sensor_imu, data, sensor_imu->getNoiseCov());
sensor_imu->process(capture_imu);
@@ -603,7 +603,7 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
// ===================================== ODO
string wolf_root = _WOLF_IMU_ROOT_DIR;
- SensorBasePtr sensor = problem->installSensor ("ODOM 3D", "Odometer", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml" );
+ SensorBasePtr sensor = problem->installSensor ("ODOM 3D", "Odometer", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml" );
ProcessorBasePtr processor = problem->installProcessor("ODOM 3D", "Odometer", "Odometer" , wolf_root + "/demos/processor_odom_3D.yaml");
sensor_odo = static_pointer_cast<SensorOdom3D>(sensor);
@@ -619,11 +619,11 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
Process_Factor_IMU::integrateAll();
// ===================================== ODO
- Vector6s data;
- Vector3s p1 = x1_exact.head(3);
- Quaternions q1 (x1_exact.data() + 3);
- Vector3s dp = q0.conjugate() * (p1 - p0);
- Vector3s dth = log_q( q0.conjugate() * q1 );
+ Vector6d data;
+ Vector3d p1 = x1_exact.head(3);
+ Quaterniond q1 (x1_exact.data() + 3);
+ Vector3d dp = q0.conjugate() * (p1 - p0);
+ Vector3d dth = log_q( q0.conjugate() * q1 );
data << dp, dth;
CaptureOdom3DPtr capture_odo = make_shared<CaptureOdom3D>(t, sensor_odo, data, sensor_odo->getNoiseCov());
@@ -637,11 +637,11 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
Process_Factor_IMU::integrateAllTrajectories();
// ===================================== ODO
- Vector6s data;
- Vector3s p1 = x1_exact.head(3);
- Quaternions q1 (x1_exact.data() + 3);
- Vector3s dp = q0.conjugate() * (p1 - p0);
- Vector3s dth = log_q( q0.conjugate() * q1 );
+ Vector6d data;
+ Vector3d p1 = x1_exact.head(3);
+ Quaterniond q1 (x1_exact.data() + 3);
+ Vector3d dp = q0.conjugate() * (p1 - p0);
+ Vector3d dth = log_q( q0.conjugate() * q1 );
data << dp, dth;
CaptureOdom3DPtr capture_odo = make_shared<CaptureOdom3D>(t, sensor_odo, data, sensor_odo->getNoiseCov());
@@ -656,7 +656,7 @@ class Process_Factor_IMU_ODO : public Process_Factor_IMU
// ===================================== ODO
// Process ODO for the callback to take effect
- data = Vector6s::Zero();
+ data = Vector6d::Zero();
capture_odo = make_shared<CaptureOdom3D>(t+dt, sensor_odo, data, sensor_odo->getNoiseCov());
sensor_odo->process(capture_odo);
@@ -685,8 +685,8 @@ TEST_F(Process_Factor_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
bias_preint = -bias_real;
// ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
+ a = Vector3d( 1,2,3 );
+ w = Vector3d( 1,2,3 );
// ---------- fix boundaries
p0_fixed = true;
@@ -729,8 +729,8 @@ TEST_F(Process_Factor_IMU, test_capture) // F_ixed___e_stimated
bias_preint = -bias_real;
// ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
+ a = Vector3d( 1,2,3 );
+ w = Vector3d( 1,2,3 );
// ---------- fix boundaries
p0_fixed = true;
@@ -744,7 +744,7 @@ TEST_F(Process_Factor_IMU, test_capture) // F_ixed___e_stimated
// ================================================================================================================ //
// ===================================== RUN ALL
- Eigen::Vector6s initial_bias((Eigen::Vector6s()<< .002, .0007, -.001, .003, -.002, .001).finished());
+ Eigen::Vector6d initial_bias((Eigen::Vector6d()<< .002, .0007, -.001, .003, -.002, .001).finished());
sensor_imu->getIntrinsic()->setState(initial_bias);
configureAll();
integrateAll();
@@ -775,8 +775,8 @@ TEST_F(Process_Factor_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
bias_preint = -bias_real;
// ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
+ a = Vector3d( 1,2,3 );
+ w = Vector3d( 1,2,3 );
// ---------- fix boundaries
p0_fixed = false;
@@ -819,8 +819,8 @@ TEST_F(Process_Factor_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
bias_preint = -bias_real;
// ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
+ a = Vector3d( 1,2,3 );
+ w = Vector3d( 1,2,3 );
// ---------- fix boundaries
p0_fixed = false;
@@ -863,8 +863,8 @@ TEST_F(Process_Factor_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
bias_preint = -bias_real;
// ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
+ a = Matrix<double, 3, 50>::Random();
+ w = Matrix<double, 3, 50>::Random();
// ---------- fix boundaries
p0_fixed = true;
@@ -907,8 +907,8 @@ TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
bias_preint = -bias_real;
// ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
+ a = Matrix<double, 3, 50>::Random();
+ w = Matrix<double, 3, 50>::Random();
// ---------- fix boundaries
p0_fixed = false;
@@ -951,8 +951,8 @@ TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
bias_preint = -bias_real;
// ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
+ a = Matrix<double, 3, 50>::Random();
+ w = Matrix<double, 3, 50>::Random();
// ---------- fix boundaries
p0_fixed = false;
@@ -995,8 +995,8 @@ TEST_F(Process_Factor_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed__
bias_preint = -bias_real;
// ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
+ a = Vector3d( 1,2,3 );
+ w = Vector3d( 1,2,3 );
// ---------- fix boundaries
p0_fixed = true;
@@ -1039,8 +1039,8 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___
bias_preint = -bias_real;
// ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
+ a = Vector3d( 0,0,0 );
+ w = Vector3d( 1,2,3 );
// ---------- fix boundaries
p0_fixed = true;
@@ -1083,8 +1083,8 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed___
bias_preint = -bias_real;
// ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
+ a = Vector3d( 0,0,0 );
+ w = Vector3d( 1,2,3 );
// ---------- fix boundaries
p0_fixed = true;
@@ -1127,8 +1127,8 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed___
bias_preint = -bias_real;
// ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
+ a = Vector3d( 0,0,0 );
+ w = Vector3d( 1,2,3 );
// ---------- fix boundaries
p0_fixed = true;
@@ -1171,8 +1171,8 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed___
bias_preint = -bias_real;
// ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
+ a = Vector3d( 0,0,0 );
+ w = Vector3d( 1,2,3 );
// ---------- fix boundaries
p0_fixed = true;
@@ -1215,8 +1215,8 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed___
bias_preint = -bias_real;
// ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
+ a = Vector3d( 0,0,0 );
+ w = Vector3d( 1,2,3 );
// ---------- fix boundaries
p0_fixed = true;
@@ -1259,8 +1259,8 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stimat
bias_preint = -bias_real;
// ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
+ a = Vector3d( 1,2,3 );
+ w = Vector3d( 1,2,3 );
// ---------- fix boundaries
p0_fixed = false;
@@ -1303,8 +1303,8 @@ TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stimat
bias_preint = -bias_real;
// ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
+ a = Vector3d( 1,2,3 );
+ w = Vector3d( 1,2,3 );
// ---------- fix boundaries
p0_fixed = false;
@@ -1347,8 +1347,8 @@ TEST_F(Process_Factor_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimated
bias_preint = -bias_real;
// ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
+ a = Matrix<double, 3, 50>::Random();
+ w = Matrix<double, 3, 50>::Random();
// ---------- fix boundaries
p0_fixed = true;
@@ -1391,8 +1391,8 @@ TEST_F(Process_Factor_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
bias_preint = -bias_real;
// ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
+ a = Matrix<double, 3, 50>::Random();
+ w = Matrix<double, 3, 50>::Random();
// ---------- fix boundaries
p0_fixed = true;
@@ -1435,8 +1435,8 @@ TEST_F(Process_Factor_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F
bias_preint = -bias_real;
// ---------- motion params
- a = Matrix<Scalar, 3, 25>::Ones() + 0.1 * Matrix<Scalar, 3, 25>::Random();
- w = Matrix<Scalar, 3, 25>::Ones() + 0.1 * Matrix<Scalar, 3, 25>::Random();
+ a = Matrix<double, 3, 25>::Ones() + 0.1 * Matrix<double, 3, 25>::Random();
+ w = Matrix<double, 3, 25>::Ones() + 0.1 * Matrix<double, 3, 25>::Random();
// ---------- fix boundaries
p0_fixed = true;
@@ -1458,13 +1458,13 @@ TEST_F(Process_Factor_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F
assertAll();
// Build optimal trajectory
- MatrixXs Trj_x_optim(10,Trj_D_preint_prc.cols());
- Scalar Dt = 0;
+ MatrixXd Trj_x_optim(10,Trj_D_preint_prc.cols());
+ double Dt = 0;
SizeEigen i = 0;
for (auto J_D_b : Buf_Jac_preint_prc)
{
- VectorXs step = J_D_b * (bias_0 - bias_preint);
- VectorXs D_opt = imu::plus(Trj_D_preint_prc.col(i).eval(), step);
+ VectorXd step = J_D_b * (bias_0 - bias_preint);
+ VectorXd D_opt = imu::plus(Trj_D_preint_prc.col(i).eval(), step);
Trj_x_optim.col(i) = imu::composeOverState(x0_optim, D_opt, Dt);
Dt += dt;
i ++;
@@ -1473,7 +1473,7 @@ TEST_F(Process_Factor_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F
// Get optimal trajectory via getState()
i = 0;
t = 0;
- MatrixXs Trj_x_optim_prc(10,Trj_D_preint_prc.cols());
+ MatrixXd Trj_x_optim_prc(10,Trj_D_preint_prc.cols());
for (int i = 0; i < Trj_x_optim_prc.cols(); i++)
{
Trj_x_optim_prc.col(i) = problem->getState(t);
diff --git a/test/gtest_IMU_tools.cpp b/test/gtest_IMU_tools.cpp
index c1b438d68d3471f5439730ab50c015cd0260f8fa..16730096e0e31900010b18a13ca11c1ffa21a20a 100644
--- a/test/gtest_IMU_tools.cpp
+++ b/test/gtest_IMU_tools.cpp
@@ -14,37 +14,37 @@ using namespace imu;
TEST(IMU_tools, identity)
{
- VectorXs id_true;
+ VectorXd id_true;
id_true.setZero(10);
id_true(6) = 1.0;
- VectorXs id = identity<Scalar>();
+ VectorXd id = identity<double>();
ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
}
TEST(IMU_tools, identity_split)
{
- VectorXs p(3), qv(4), v(3);
- Quaternions q;
+ VectorXd p(3), qv(4), v(3);
+ Quaterniond q;
identity(p,q,v);
- ASSERT_MATRIX_APPROX(p, Vector3s::Zero(), 1e-10);
- ASSERT_QUATERNION_APPROX(q, Quaternions::Identity(), 1e-10);
- ASSERT_MATRIX_APPROX(v, Vector3s::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(p, Vector3d::Zero(), 1e-10);
+ ASSERT_QUATERNION_APPROX(q, Quaterniond::Identity(), 1e-10);
+ ASSERT_MATRIX_APPROX(v, Vector3d::Zero(), 1e-10);
identity(p,qv,v);
- ASSERT_MATRIX_APPROX(p , Vector3s::Zero(), 1e-10);
- ASSERT_MATRIX_APPROX(qv, (Vector4s()<<0,0,0,1).finished(), 1e-10);
- ASSERT_MATRIX_APPROX(v , Vector3s::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(p , Vector3d::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(qv, (Vector4d()<<0,0,0,1).finished(), 1e-10);
+ ASSERT_MATRIX_APPROX(v , Vector3d::Zero(), 1e-10);
}
TEST(IMU_tools, inverse)
{
- VectorXs d(10), id(10), iid(10), iiid(10), I(10);
- Vector4s qv;
- Scalar dt = 0.1;
+ VectorXd d(10), id(10), iid(10), iiid(10), I(10);
+ Vector4d qv;
+ double dt = 0.1;
- qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ qv = (Vector4d() << 3, 4, 5, 6).finished().normalized();
d << 0, 1, 2, qv, 7, 8, 9;
id = inverse(d, dt);
@@ -62,15 +62,15 @@ TEST(IMU_tools, inverse)
TEST(IMU_tools, compose_between)
{
- VectorXs dx1(10), dx2(10), dx3(10);
- Matrix<Scalar, 10, 1> d1, d2, d3;
- Vector4s qv;
- Scalar dt = 0.1;
+ VectorXd dx1(10), dx2(10), dx3(10);
+ Matrix<double, 10, 1> d1, d2, d3;
+ Vector4d qv;
+ double dt = 0.1;
- qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ qv = (Vector4d() << 3, 4, 5, 6).finished().normalized();
dx1 << 0, 1, 2, qv, 7, 8, 9;
d1 = dx1;
- qv = (Vector4s() << 6, 5, 4, 3).finished().normalized();
+ qv = (Vector4d() << 6, 5, 4, 3).finished().normalized();
dx2 << 9, 8, 7, qv, 2, 1, 0;
d2 = dx2;
@@ -84,8 +84,8 @@ TEST(IMU_tools, compose_between)
ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
// minus(d1, d3) should recover delta_2
- VectorXs diffx(10);
- Matrix<Scalar,10,1> diff;
+ VectorXd diffx(10);
+ Matrix<double,10,1> diff;
between(d1, d3, dt, diff);
ASSERT_MATRIX_APPROX(diff, d2, 1e-10);
@@ -96,13 +96,13 @@ TEST(IMU_tools, compose_between)
TEST(IMU_tools, compose_between_with_state)
{
- VectorXs x(10), d(10), x2(10), x3(10), d2(10), d3(10);
- Vector4s qv;
- Scalar dt = 0.1;
+ VectorXd x(10), d(10), x2(10), x3(10), d2(10), d3(10);
+ Vector4d qv;
+ double dt = 0.1;
- qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ qv = (Vector4d() << 3, 4, 5, 6).finished().normalized();
x << 0, 1, 2, qv, 7, 8, 9;
- qv = (Vector4s() << 6, 5, 4, 3).finished().normalized();
+ qv = (Vector4d() << 6, 5, 4, 3).finished().normalized();
d << 9, 8, 7, qv, 2, 1, 0;
composeOverState(x, d, dt, x2);
@@ -125,59 +125,59 @@ TEST(IMU_tools, compose_between_with_state)
TEST(IMU_tools, lift_retract)
{
- VectorXs d_min(9); d_min << 0, 1, 2, .3, .4, .5, 6, 7, 8; // use angles in the ball theta < pi
+ VectorXd d_min(9); d_min << 0, 1, 2, .3, .4, .5, 6, 7, 8; // use angles in the ball theta < pi
// transform to delta
- VectorXs delta = exp_IMU(d_min);
+ VectorXd delta = exp_IMU(d_min);
// expected delta
- Vector3s dp = d_min.head(3);
- Quaternions dq = v2q(d_min.segment(3,3));
- Vector3s dv = d_min.tail(3);
- VectorXs delta_true(10); delta_true << dp, dq.coeffs(), dv;
+ Vector3d dp = d_min.head(3);
+ Quaterniond dq = v2q(d_min.segment(3,3));
+ Vector3d dv = d_min.tail(3);
+ VectorXd delta_true(10); delta_true << dp, dq.coeffs(), dv;
ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
// transform to a new tangent -- should be the original tangent
- VectorXs d_from_delta = log_IMU(delta);
+ VectorXd d_from_delta = log_IMU(delta);
ASSERT_MATRIX_APPROX(d_from_delta, d_min, 1e-10);
// transform to a new delta -- should be the previous delta
- VectorXs delta_from_d = exp_IMU(d_from_delta);
+ VectorXd delta_from_d = exp_IMU(d_from_delta);
ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
}
TEST(IMU_tools, plus)
{
- VectorXs d1(10), d2(10), d3(10);
- VectorXs err(9);
- Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ VectorXd d1(10), d2(10), d3(10);
+ VectorXd err(9);
+ Vector4d qv = (Vector4d() << 3, 4, 5, 6).finished().normalized();
d1 << 0, 1, 2, qv, 7, 8, 9;
err << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09;
d3.head(3) = d1.head(3) + err.head(3);
- d3.segment(3,4) = (Quaternions(qv.data()) * exp_q(err.segment(3,3))).coeffs();
+ d3.segment(3,4) = (Quaterniond(qv.data()) * exp_q(err.segment(3,3))).coeffs();
d3.tail(3) = d1.tail(3) + err.tail(3);
plus(d1, err, d2);
- ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXs::Zero(9), 1e-10);
+ ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXd::Zero(9), 1e-10);
}
TEST(IMU_tools, diff)
{
- VectorXs d1(10), d2(10);
- Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ VectorXd d1(10), d2(10);
+ Vector4d qv = (Vector4d() << 3, 4, 5, 6).finished().normalized();
d1 << 0, 1, 2, qv, 7, 8, 9;
d2 = d1;
- VectorXs err(9);
+ VectorXd err(9);
diff(d1, d2, err);
- ASSERT_MATRIX_APPROX(err, VectorXs::Zero(9), 1e-10);
- ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXs::Zero(9), 1e-10);
+ ASSERT_MATRIX_APPROX(err, VectorXd::Zero(9), 1e-10);
+ ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXd::Zero(9), 1e-10);
- VectorXs d3(10);
+ VectorXd d3(10);
d3.setRandom(); d3.segment(3,4).normalize();
err.head(3) = d3.head(3) - d1.head(3);
- err.segment(3,3) = log_q(Quaternions(d1.data()+3).conjugate()*Quaternions(d3.data()+3));
+ err.segment(3,3) = log_q(Quaterniond(d1.data()+3).conjugate()*Quaterniond(d3.data()+3));
err.tail(3) = d3.tail(3) - d1.tail(3);
ASSERT_MATRIX_APPROX(err, diff(d1, d3), 1e-10);
@@ -186,15 +186,15 @@ TEST(IMU_tools, diff)
TEST(IMU_tools, compose_jacobians)
{
- VectorXs d1(10), d2(10), d3(10), d1_pert(10), d2_pert(10), d3_pert(10); // deltas
- VectorXs t1(9), t2(9); // tangents
- Matrix<Scalar, 9, 9> J1_a, J2_a, J1_n, J2_n;
- Vector4s qv1, qv2;
- Scalar dt = 0.1;
- Scalar dx = 1e-6;
- qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ VectorXd d1(10), d2(10), d3(10), d1_pert(10), d2_pert(10), d3_pert(10); // deltas
+ VectorXd t1(9), t2(9); // tangents
+ Matrix<double, 9, 9> J1_a, J2_a, J1_n, J2_n;
+ Vector4d qv1, qv2;
+ double dt = 0.1;
+ double dx = 1e-6;
+ qv1 = (Vector4d() << 3, 4, 5, 6).finished().normalized();
d1 << 0, 1, 2, qv1, 7, 8, 9;
- qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
+ qv2 = (Vector4d() << 1, 2, 3, 4).finished().normalized();
d2 << 9, 8, 7, qv2, 2, 1, 0;
// analytical jacobians
@@ -226,14 +226,14 @@ TEST(IMU_tools, compose_jacobians)
TEST(IMU_tools, diff_jacobians)
{
- VectorXs d1(10), d2(10), d3(9), d1_pert(10), d2_pert(10), d3_pert(9); // deltas
- VectorXs t1(9), t2(9); // tangents
- Matrix<Scalar, 9, 9> J1_a, J2_a, J1_n, J2_n;
- Vector4s qv1, qv2;
- Scalar dx = 1e-6;
- qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ VectorXd d1(10), d2(10), d3(9), d1_pert(10), d2_pert(10), d3_pert(9); // deltas
+ VectorXd t1(9), t2(9); // tangents
+ Matrix<double, 9, 9> J1_a, J2_a, J1_n, J2_n;
+ Vector4d qv1, qv2;
+ double dx = 1e-6;
+ qv1 = (Vector4d() << 3, 4, 5, 6).finished().normalized();
d1 << 0, 1, 2, qv1, 7, 8, 9;
- qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
+ qv2 = (Vector4d() << 1, 2, 3, 4).finished().normalized();
d2 << 9, 8, 7, qv2, 2, 1, 0;
// analytical jacobians
@@ -265,14 +265,14 @@ TEST(IMU_tools, diff_jacobians)
TEST(IMU_tools, body2delta_jacobians)
{
- VectorXs delta(10), delta_pert(10); // deltas
- VectorXs body(6), pert(6);
- VectorXs tang(9); // tangents
- Matrix<Scalar, 9, 6> J_a, J_n; // analytic and numerical jacs
- Vector4s qv;;
- Scalar dt = 0.1;
- Scalar dx = 1e-6;
- qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ VectorXd delta(10), delta_pert(10); // deltas
+ VectorXd body(6), pert(6);
+ VectorXd tang(9); // tangents
+ Matrix<double, 9, 6> J_a, J_n; // analytic and numerical jacs
+ Vector4d qv;;
+ double dt = 0.1;
+ double dx = 1e-6;
+ qv = (Vector4d() << 3, 4, 5, 6).finished().normalized();
delta << 0, 1, 2, qv, 7, 8, 9;
body << 1, 2, 3, 3, 2, 1;
@@ -297,93 +297,93 @@ TEST(IMU_tools, body2delta_jacobians)
TEST(motion2data, zero)
{
- Vector6s motion;
- Vector6s bias;
- Quaternions q;
+ Vector6d motion;
+ Vector6d bias;
+ Quaterniond q;
motion .setZero();
bias .setZero();
q .setIdentity();
- Vector6s data = imu::motion2data(motion, q, bias);
+ Vector6d data = imu::motion2data(motion, q, bias);
- Vector6s data_true; data_true << -gravity(), Vector3s::Zero();
+ Vector6d data_true; data_true << -gravity(), Vector3d::Zero();
ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
}
TEST(motion2data, motion)
{
- Vector6s motion, g_extended;
- Vector6s bias;
- Quaternions q;
+ Vector6d motion, g_extended;
+ Vector6d bias;
+ Quaterniond q;
- g_extended << gravity() , Vector3s::Zero();
+ g_extended << gravity() , Vector3d::Zero();
motion << 1,2,3, 4,5,6;
bias .setZero();
q .setIdentity();
- Vector6s data = imu::motion2data(motion, q, bias);
+ Vector6d data = imu::motion2data(motion, q, bias);
- Vector6s data_true; data_true = motion - g_extended;
+ Vector6d data_true; data_true = motion - g_extended;
ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
}
TEST(motion2data, bias)
{
- Vector6s motion, g_extended;
- Vector6s bias;
- Quaternions q;
+ Vector6d motion, g_extended;
+ Vector6d bias;
+ Quaterniond q;
- g_extended << gravity() , Vector3s::Zero();
+ g_extended << gravity() , Vector3d::Zero();
motion .setZero();
bias << 1,2,3, 4,5,6;
q .setIdentity();
- Vector6s data = imu::motion2data(motion, q, bias);
+ Vector6d data = imu::motion2data(motion, q, bias);
- Vector6s data_true; data_true = bias - g_extended;
+ Vector6d data_true; data_true = bias - g_extended;
ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
}
TEST(motion2data, orientation)
{
- Vector6s motion, g_extended;
- Vector6s bias;
- Quaternions q;
+ Vector6d motion, g_extended;
+ Vector6d bias;
+ Quaterniond q;
- g_extended << gravity() , Vector3s::Zero();
+ g_extended << gravity() , Vector3d::Zero();
motion .setZero();
bias .setZero();
- q = v2q(Vector3s(M_PI/2, 0, 0)); // turn 90 deg in X axis
+ q = v2q(Vector3d(M_PI/2, 0, 0)); // turn 90 deg in X axis
- Vector6s data = imu::motion2data(motion, q, bias);
+ Vector6d data = imu::motion2data(motion, q, bias);
- Vector6s data_true; data_true << 0,-gravity()(2),0, 0,0,0;
+ Vector6d data_true; data_true << 0,-gravity()(2),0, 0,0,0;
ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
}
TEST(motion2data, AllRandom)
{
- Vector6s motion, g_extended;
- Vector6s bias;
- Quaternions q;
+ Vector6d motion, g_extended;
+ Vector6d bias;
+ Quaterniond q;
motion .setRandom();
bias .setRandom();
q.coeffs() .setRandom().normalize();
- g_extended << q.conjugate()*gravity() , Vector3s::Zero();
+ g_extended << q.conjugate()*gravity() , Vector3d::Zero();
- Vector6s data = imu::motion2data(motion, q, bias);
+ Vector6d data = imu::motion2data(motion, q, bias);
- Vector6s data_true; data_true = motion + bias - g_extended;
+ Vector6d data_true; data_true = motion + bias - g_extended;
ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
}
@@ -398,14 +398,14 @@ TEST(motion2data, AllRandom)
* Output:
* return: the preintegrated delta
*/
-VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt)
+VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt)
{
- VectorXs data(6);
- VectorXs body(6);
- VectorXs delta(10);
- VectorXs Delta(10), Delta_plus(10);
+ VectorXd data(6);
+ VectorXd body(6);
+ VectorXd delta(10);
+ VectorXd Delta(10), Delta_plus(10);
Delta << 0,0,0, 0,0,0,1, 0,0,0;
- Quaternions q(q0);
+ Quaterniond q(q0);
for (int n = 0; n<N; n++)
{
data = motion2data(motion, q, bias_real);
@@ -429,15 +429,15 @@ VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, co
* J_D_b: the Jacobian of the preintegrated delta wrt the bias
* return: the preintegrated delta
*/
-VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
+VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Matrix<double, 9, 6>& J_D_b)
{
- VectorXs data(6);
- VectorXs body(6);
- VectorXs delta(10);
- Matrix<Scalar, 9, 6> J_d_d, J_d_b;
- Matrix<Scalar, 9, 9> J_D_D, J_D_d;
- VectorXs Delta(10), Delta_plus(10);
- Quaternions q;
+ VectorXd data(6);
+ VectorXd body(6);
+ VectorXd delta(10);
+ Matrix<double, 9, 6> J_d_d, J_d_b;
+ Matrix<double, 9, 9> J_D_D, J_D_d;
+ VectorXd Delta(10), Delta_plus(10);
+ Quaterniond q;
Delta << 0,0,0, 0,0,0,1, 0,0,0;
J_D_b.setZero();
@@ -466,14 +466,14 @@ VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, co
TEST(IMU_tools, integral_jacobian_bias)
{
- VectorXs Delta(10), Delta_pert(10); // deltas
- VectorXs bias_real(6), pert(6), bias_pert(6), bias_preint(6);
- VectorXs body(6), data(6), motion(6);
- VectorXs tang(9); // tangents
- Matrix<Scalar, 9, 6> J_a, J_n; // analytic and numerical jacs
- Scalar dt = 0.001;
- Scalar dx = 1e-4;
- Quaternions q0(3, 4, 5, 6); q0.normalize();
+ VectorXd Delta(10), Delta_pert(10); // deltas
+ VectorXd bias_real(6), pert(6), bias_pert(6), bias_preint(6);
+ VectorXd body(6), data(6), motion(6);
+ VectorXd tang(9); // tangents
+ Matrix<double, 9, 6> J_a, J_n; // analytic and numerical jacs
+ double dt = 0.001;
+ double dx = 1e-4;
+ Quaterniond q0(3, 4, 5, 6); q0.normalize();
motion << .1, .2, .3, .3, .2, .1;
bias_real << .002, .004, .001, .003, .002, .001;
bias_preint << .004, .005, .006, .001, .002, .003;
@@ -501,14 +501,14 @@ TEST(IMU_tools, integral_jacobian_bias)
TEST(IMU_tools, delta_correction)
{
- VectorXs Delta(10), Delta_preint(10), Delta_corr; // deltas
- VectorXs bias(6), pert(6), bias_preint(6);
- VectorXs body(6), motion(6);
- VectorXs tang(9); // tangents
- Matrix<Scalar, 9, 6> J_b; // analytic and numerical jacs
- Vector4s qv;;
- Scalar dt = 0.001;
- Quaternions q0(3, 4, 5, 6); q0.normalize();
+ VectorXd Delta(10), Delta_preint(10), Delta_corr; // deltas
+ VectorXd bias(6), pert(6), bias_preint(6);
+ VectorXd body(6), motion(6);
+ VectorXd tang(9); // tangents
+ Matrix<double, 9, 6> J_b; // analytic and numerical jacs
+ Vector4d qv;;
+ double dt = 0.001;
+ Quaterniond q0(3, 4, 5, 6); q0.normalize();
motion << 1, 2, 3, 3, 2, 1; motion *= .1;
int N = 1000; // # steps of integration
@@ -523,14 +523,14 @@ TEST(IMU_tools, delta_correction)
Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
// correct perturbated
- Vector9s step = J_b*(bias-bias_preint);
+ Vector9d step = J_b*(bias-bias_preint);
Delta_corr = plus(Delta_preint, step);
// Corrected delta should match real delta
ASSERT_MATRIX_APPROX(Delta, Delta_corr, 1e-5);
// diff between real and corrected should be zero
- ASSERT_MATRIX_APPROX(diff(Delta, Delta_corr), Vector9s::Zero(), 1e-5);
+ ASSERT_MATRIX_APPROX(diff(Delta, Delta_corr), Vector9d::Zero(), 1e-5);
// diff between preint and corrected deltas should be the jacobian-computed step
ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-5);
@@ -538,15 +538,15 @@ TEST(IMU_tools, delta_correction)
TEST(IMU_tools, full_delta_residual)
{
- VectorXs x1(10), x2(10);
- VectorXs Delta(10), Delta_preint(10), Delta_corr(10);
- VectorXs Delta_real(9), Delta_err(9), Delta_diff(10), Delta_exp(10);
- VectorXs bias(6), pert(6), bias_preint(6), bias_null(6);
- VectorXs body(6), motion(6);
- VectorXs tang(9); // tangents
- Matrix<Scalar, 9, 6> J_b; // analytic and numerical jacs
- Scalar dt = 0.001;
- Quaternions q0; q0.setIdentity();
+ VectorXd x1(10), x2(10);
+ VectorXd Delta(10), Delta_preint(10), Delta_corr(10);
+ VectorXd Delta_real(9), Delta_err(9), Delta_diff(10), Delta_exp(10);
+ VectorXd bias(6), pert(6), bias_preint(6), bias_null(6);
+ VectorXd body(6), motion(6);
+ VectorXd tang(9); // tangents
+ Matrix<double, 9, 6> J_b; // analytic and numerical jacs
+ double dt = 0.001;
+ Quaterniond q0; q0.setIdentity();
x1 << 0,0,0, 0,0,0,1, 0,0,0;
motion << .3, .2, .1, .1, .2, .3; // acc and gyro
@@ -574,7 +574,7 @@ TEST(IMU_tools, full_delta_residual)
Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
// compute correction step
- Vector9s step = J_b*(bias-bias_preint);
+ Vector9d step = J_b*(bias-bias_preint);
// correct preintegrated delta
Delta_corr = plus(Delta_preint, step);
@@ -583,7 +583,7 @@ TEST(IMU_tools, full_delta_residual)
ASSERT_MATRIX_APPROX(Delta_real, Delta_corr, 1e-3);
// diff between real and corrected should be zero
- ASSERT_MATRIX_APPROX(diff(Delta_real, Delta_corr), Vector9s::Zero(), 1e-3);
+ ASSERT_MATRIX_APPROX(diff(Delta_real, Delta_corr), Vector9d::Zero(), 1e-3);
// expected delta
Delta_exp = betweenStates(x1, x2, N*dt);
@@ -591,10 +591,10 @@ TEST(IMU_tools, full_delta_residual)
ASSERT_MATRIX_APPROX(Delta_exp, Delta_corr, 1e-3);
// compute diff between expected and corrected
-// Matrix<Scalar, 9, 9> J_err_exp, J_err_corr;
+// Matrix<double, 9, 9> J_err_exp, J_err_corr;
diff(Delta_exp, Delta_corr, Delta_err); //, J_err_exp, J_err_corr);
- ASSERT_MATRIX_APPROX(Delta_err, Vector9s::Zero(), 1e-3);
+ ASSERT_MATRIX_APPROX(Delta_err, Vector9d::Zero(), 1e-3);
// compute error between expected and preintegrated
Delta_err = diff(Delta_preint, Delta_exp);
diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_IMU.cpp
index f8fa7b85bcf3060d9665257c0e5121206ae67fbe..4d79d659b46752d93793c48635d840a523aeadc9 100644
--- a/test/gtest_factor_IMU.cpp
+++ b/test/gtest_factor_IMU.cpp
@@ -48,10 +48,10 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
ProcessorIMUPtr processor_imu;
FrameBasePtr KF0;
FrameBasePtr KF1;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
- Eigen::MatrixXs P_origin;
+ Eigen::Vector6d origin_bias;
+ Eigen::VectorXd expected_final_state;
+ Eigen::VectorXd x_origin;
+ Eigen::MatrixXd P_origin;
virtual void SetUp()
{
@@ -73,7 +73,7 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
@@ -100,7 +100,7 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- Eigen::Vector6s data_imu;
+ Eigen::Vector6d data_imu;
data_imu << -wolf::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)
@@ -137,9 +137,9 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
ProcessorIMUPtr processor_imu;
FrameBasePtr KF0;
FrameBasePtr KF1;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
+ Eigen::Vector6d origin_bias;
+ Eigen::VectorXd expected_final_state;
+ Eigen::VectorXd x_origin;
virtual void SetUp()
{
@@ -161,7 +161,7 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
@@ -184,7 +184,7 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- Eigen::Vector6s data_imu;
+ Eigen::Vector6d data_imu;
data_imu << -wolf::gravity(), 0,0,0;
data_imu = data_imu + origin_bias;
@@ -221,9 +221,9 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
ProcessorIMUPtr processor_imu;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
+ Eigen::Vector6d origin_bias;
+ Eigen::VectorXd expected_final_state;
+ Eigen::VectorXd x_origin;
virtual void SetUp()
{
@@ -245,7 +245,7 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
@@ -268,7 +268,7 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- Eigen::Vector6s data_imu;
+ Eigen::Vector6d data_imu;
data_imu << -wolf::gravity(), 0,0,0;
data_imu = data_imu + origin_bias;
@@ -305,9 +305,9 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
ProcessorIMUPtr processor_imu;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
+ Eigen::Vector6d origin_bias;
+ Eigen::VectorXd expected_final_state;
+ Eigen::VectorXd x_origin;
virtual void SetUp()
{
@@ -329,7 +329,7 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
@@ -354,11 +354,11 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- Eigen::Vector6s data_imu;
+ Eigen::Vector6d data_imu;
data_imu << -wolf::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)
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero()); //set data on IMU (measure only gravity here)
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -391,9 +391,9 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
ProcessorIMUPtr processor_imu;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
+ Eigen::Vector6d origin_bias;
+ Eigen::VectorXd expected_final_state;
+ Eigen::VectorXd x_origin;
virtual void SetUp()
{
@@ -415,7 +415,7 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
@@ -429,7 +429,7 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
x_origin << 0,0,0, 0,0,0,1, 0,0,0;
t.set(0);
- Eigen::Vector6s data_imu;
+ Eigen::Vector6d data_imu;
data_imu << 0,10,-wolf::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
@@ -445,7 +445,7 @@ class FactorIMU_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());
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -478,9 +478,9 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
ProcessorIMUPtr processor_imu;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
+ Eigen::Vector6d origin_bias;
+ Eigen::VectorXd expected_final_state;
+ Eigen::VectorXd x_origin;
virtual void SetUp()
{
@@ -502,7 +502,7 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
@@ -515,8 +515,8 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
x_origin << 0,0,0, 0,0,0,1, 0,0,0;
t.set(0);
- Eigen::Vector6s data_imu;
- origin_bias = Eigen::Vector6s::Random() * 0.001;
+ Eigen::Vector6d data_imu;
+ origin_bias = Eigen::Vector6d::Random() * 0.001;
data_imu << 0,10,-wolf::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
@@ -528,7 +528,7 @@ class FactorIMU_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());
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -561,9 +561,9 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
ProcessorIMUPtr processor_imu;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
+ Eigen::Vector6d origin_bias;
+ Eigen::VectorXd expected_final_state;
+ Eigen::VectorXd x_origin;
virtual void SetUp()
{
@@ -585,7 +585,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
@@ -598,14 +598,14 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
x_origin << 0,0,0, 0,0,0,1, 0,0,0;
t.set(0);
- 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
+ Eigen::Vector6d data_imu, data_imu_initial;
+ origin_bias = Eigen::Vector6d::Random() * 0.001;
+ double rate_of_turn = 5 * M_PI/180.0; // rad/s
data_imu << -wolf::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());
+ Eigen::Quaterniond quat_comp(Eigen::Quaterniond::Identity());
quat_comp = quat_comp * wolf::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
@@ -617,7 +617,7 @@ class FactorIMU_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());
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -654,9 +654,9 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
ProcessorIMUPtr processor_imu;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
+ Eigen::Vector6d origin_bias;
+ Eigen::VectorXd expected_final_state;
+ Eigen::VectorXd x_origin;
virtual void SetUp()
{
@@ -678,7 +678,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
@@ -691,15 +691,15 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
x_origin << 0,0,0, 0,0,0,1, 10,-3,4;
t.set(0);
- Eigen::Vector6s data_imu, data_imu_initial;
- origin_bias = Eigen::Vector6s::Random();
+ Eigen::Vector6d data_imu, data_imu_initial;
+ origin_bias = Eigen::Vector6d::Random();
origin_bias << 0,0,0, 0,0,0;
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0; // rad/s
+ double rate_of_turn = 5 * M_PI/180.0; // rad/s
data_imu << -wolf::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());
+ Eigen::Quaterniond quat_comp(Eigen::Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data_imu.tail(3)*1);
// rotated at 5 deg/s for 1s = 5 deg => 5 * M_PI/180
@@ -716,7 +716,7 @@ class FactorIMU_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());
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
{
@@ -755,9 +755,9 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
ProcessorIMUPtr processor_imu;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
+ Eigen::Vector6d origin_bias;
+ Eigen::VectorXd expected_final_state;
+ Eigen::VectorXd x_origin;
virtual void SetUp()
{
@@ -779,7 +779,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
@@ -792,7 +792,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
x_origin << 0,0,0, 0,0,0,1, 0,0,0;
origin_bias << 0.01, 0.02, 0.003, 0.002, 0.005, 0.01;
t.set(0);
- Eigen::Quaternions quat_current(Eigen::Quaternions::Identity());
+ Eigen::Quaterniond quat_current(Eigen::Quaterniond::Identity());
//set origin of the problem
origin_KF = processor_imu->setOrigin(x_origin, t);
@@ -801,12 +801,12 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- Eigen::Vector6s data_imu(Eigen::Vector6s::Zero());
- Eigen::Vector3s rateOfTurn(Eigen::Vector3s::Zero()); //deg/s
+ Eigen::Vector6d data_imu(Eigen::Vector6d::Zero());
+ Eigen::Vector3d rateOfTurn(Eigen::Vector3d::Zero()); //deg/s
- Scalar dt(0.001);
+ double 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());
+ wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
while( ts.get() < 1 )
{
@@ -857,9 +857,9 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
FrameBasePtr last_KF;
CaptureIMUPtr capture_imu;
CaptureOdom3DPtr capture_odo;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
+ Eigen::Vector6d origin_bias;
+ Eigen::VectorXd expected_final_state;
+ Eigen::VectorXd x_origin;
virtual void SetUp()
{
@@ -879,13 +879,13 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
ceres_manager = std::make_shared<CeresManager>(problem, ceres_options);
// SENSOR + PROCESSOR IMU
- sensor = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+ sensor = problem->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
sensor_imu = std::static_pointer_cast<SensorIMU>(sensor);
processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor);
// SENSOR + PROCESSOR ODOM 3D
- sensor = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
+ sensor = problem->installSensor("ODOM 3D", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
sensor_odo = std::static_pointer_cast<SensorOdom3D>(sensor);
sensor_imu->setNoiseStd((sensor_imu->getNoiseStd()/10.0).eval());
@@ -920,12 +920,12 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- Eigen::Vector6s data_imu(Eigen::Vector6s::Zero()),
- data_odo(Eigen::Vector6s::Zero());
- Eigen::Vector3s rateOfTurn(Eigen::Vector3s::Zero()); //deg/s
+ Eigen::Vector6d data_imu(Eigen::Vector6d::Zero()),
+ data_odo(Eigen::Vector6d::Zero());
+ Eigen::Vector3d rateOfTurn(Eigen::Vector3d::Zero()); //deg/s
TimeStamp t_imu(0.0), t_odo(0.0);
- Scalar dt_imu(0.001), dt_odo(.01);
+ double dt_imu(0.001), dt_odo(.01);
capture_imu = std::make_shared<CaptureIMU> (t_imu, sensor_imu, data_imu, sensor_imu->getNoiseCov(), sensor_imu->getCalibration(), nullptr);
@@ -934,8 +934,8 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
t_odo += dt_odo; //first odometry data will be processed at this timestamp
// ground truth for quaternion:
- Eigen::Quaternions quat_odo(Eigen::Quaternions::Identity());
- Eigen::Quaternions quat_imu(Eigen::Quaternions::Identity());
+ Eigen::Quaterniond quat_odo(Eigen::Quaterniond::Identity());
+ Eigen::Quaterniond quat_imu(Eigen::Quaterniond::Identity());
WOLF_TRACE("last delta preint: ", processor_imu->getLast()->getDeltaPreint().transpose());
WOLF_TRACE("last jacoob bias : ", processor_imu->getLast()->getJacobianCalib().row(0));
@@ -947,7 +947,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
// Time and data variables
t_imu += dt_imu;
-// rateOfTurn = Eigen::Vector3s::Random()*10; //to have rate of turn > 0.99 deg/s
+// rateOfTurn = Eigen::Vector3d::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
@@ -990,7 +990,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
// WOLF_TRACE("orig calib preint: ", processor_imu->getOrigin()->getCalibrationPreint().transpose());
//prepare next odometry measurement
- quat_odo = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
+ quat_odo = Eigen::Quaterniond::Identity(); //set to identity to have next odom relative to this last KF
t_odo += dt_odo;
break;
}
@@ -1038,9 +1038,9 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
ProcessorOdom3DPtr processor_odom3D;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
+ Eigen::Vector6d origin_bias;
+ Eigen::VectorXd expected_final_state;
+ Eigen::VectorXd x_origin;
virtual void SetUp()
{
@@ -1062,13 +1062,13 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
// SENSOR + PROCESSOR ODOM 3D
- SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
+ SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3D", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
prc_odom3D_params->max_time_span = 0.9999;
prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
@@ -1088,8 +1088,8 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
origin_bias << 0.0015, 0.004, -0.002, 0.005, -0.0074, -0.003;
origin_bias *= .1;
t.set(0);
- Eigen::Quaternions odom_quat(Eigen::Quaternions::Identity());
- Eigen::Quaternions current_quatState(Eigen::Quaternions::Identity());
+ Eigen::Quaterniond odom_quat(Eigen::Quaterniond::Identity());
+ Eigen::Quaterniond current_quatState(Eigen::Quaterniond::Identity());
//set origin of the problem
origin_KF = processor_imu->setOrigin(x_origin, t);
@@ -1099,14 +1099,14 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- Eigen::Vector6s data_imu(Eigen::Vector6s::Zero()), data_odom3D(Eigen::Vector6s::Zero());
- Eigen::Vector3s rateOfTurn; //deg/s
+ Eigen::Vector6d data_imu(Eigen::Vector6d::Zero()), data_odom3D(Eigen::Vector6d::Zero());
+ Eigen::Vector3d rateOfTurn; //deg/s
rateOfTurn << 0,90,0;
- VectorXs D_cor(10);
+ VectorXd D_cor(10);
- Scalar dt(0.0010), dt_odom(1.0);
+ double 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::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
wolf::CaptureOdom3DPtr mot_ptr = std::make_shared<CaptureOdom3D>(t, sen_odom3D, data_odom3D, sen_odom3D->getNoiseCov(), nullptr);
sen_odom3D->process(mot_ptr);
//first odometry data will be processed at this timestamp
@@ -1146,7 +1146,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
sen_odom3D->process(mot_ptr);
//prepare next odometry measurement
- odom_quat = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
+ odom_quat = Eigen::Quaterniond::Identity(); //set to identity to have next odom relative to this last KF
t_odom.set(t_odom.get() + dt_odom);
}
}
@@ -1179,10 +1179,10 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
ProcessorOdom3DPtr processor_odom3D;
FrameBasePtr origin_KF;
FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
- Eigen::MatrixXs P_origin;
+ Eigen::Vector6d origin_bias;
+ Eigen::VectorXd expected_final_state;
+ Eigen::VectorXd x_origin;
+ Eigen::MatrixXd P_origin;
virtual void SetUp()
{
@@ -1204,13 +1204,13 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
ceres_manager_wolf_diff = std::make_shared<CeresManager>(problem, ceres_options);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+ SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
processor_imu = std::static_pointer_cast<ProcessorIMU>(processor);
// SENSOR + PROCESSOR ODOM 3D
- SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
+ SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3D", "odom", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
prc_odom3D_params->max_time_span = 0.9999;
prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
@@ -1230,8 +1230,8 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
P_origin.setIdentity(9,9); P_origin *= .01;
origin_bias << 0.0015, 0.004, -0.002, 0.005, -0.0074, -0.003;
t.set(0);
- Eigen::Quaternions odom_quat(Eigen::Quaternions::Identity());
- Eigen::Quaternions current_quatState(Eigen::Quaternions::Identity());
+ Eigen::Quaterniond odom_quat(Eigen::Quaterniond::Identity());
+ Eigen::Quaterniond current_quatState(Eigen::Quaterniond::Identity());
//set origin of the problem
origin_KF = problem->setPrior(x_origin, P_origin, t, 0.005);
@@ -1242,19 +1242,19 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
//===================================================== PROCESS DATA
// PROCESS DATA
- Eigen::Vector6s data_imu(Eigen::Vector6s::Zero()), data_odom3D(Eigen::Vector6s::Zero());
- Eigen::Vector3s rateOfTurn; //deg/s
+ Eigen::Vector6d data_imu(Eigen::Vector6d::Zero()), data_odom3D(Eigen::Vector6d::Zero());
+ Eigen::Vector3d rateOfTurn; //deg/s
rateOfTurn << 45,90,0;
- Scalar dt(0.0010), dt_odom(1.0);
+ double 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::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6d::Zero());
wolf::CaptureOdom3DPtr mot_ptr = std::make_shared<CaptureOdom3D>(t, sen_odom3D, data_odom3D, nullptr);
sen_odom3D->process(mot_ptr);
//first odometry data will be processed at this timestamp
//t_odom.set(t_odom.get() + dt_odom);
- Eigen::Vector2s randomPart;
+ Eigen::Vector2d randomPart;
data_imu.tail<3>() = rateOfTurn* M_PI/180.0; //constant rotation =
//when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
@@ -1286,7 +1286,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
sen_odom3D->process(mot_ptr);
//prepare next odometry measurement
- odom_quat = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
+ odom_quat = Eigen::Quaterniond::Identity(); //set to identity to have next odom relative to this last KF
t_odom.set(t_odom.get() + dt_odom);
}
}
@@ -1301,7 +1301,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
ts.set((500 + j)*dt);
/*data_imu.tail<3>() = rateOfTurn* M_PI/180.0;
- randomPart = Eigen::Vector2s::Random(); //to have rate of turn > 0.99 deg/s
+ randomPart = Eigen::Vector2d::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
@@ -1325,7 +1325,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
sen_odom3D->process(mot_ptr);
//prepare next odometry measurement
- odom_quat = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
+ odom_quat = Eigen::Quaterniond::Identity(); //set to identity to have next odom relative to this last KF
t_odom.set(t_odom.get() + dt_odom);
}
}
@@ -1351,7 +1351,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
KF0->getP()->fix();
KF0->getO()->fix();
KF0->getV()->fix();
- KF0->getCaptureOf(sen_imu)->setCalibration((Vector6s()<<1,2,3,1,2,3).finished());
+ KF0->getCaptureOf(sen_imu)->setCalibration((Vector6d()<<1,2,3,1,2,3).finished());
KF1->getP()->setState(expected_final_state.head(3));
KF1->getO()->setState(expected_final_state.segment(3,4));
@@ -1360,7 +1360,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
KF1->getP()->fix();
KF1->getO()->fix();
KF1->getV()->fix();
- KF1->getCaptureOf(sen_imu)->setCalibration((Vector6s()<<-1,-2,-3,-1,-2,-3).finished());
+ KF1->getCaptureOf(sen_imu)->setCalibration((Vector6d()<<-1,-2,-3,-1,-2,-3).finished());
std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
@@ -1382,16 +1382,16 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
KF1->getO()->fix();
KF1->getV()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
+ double epsilon_bias = 0.0000001;
+ Eigen::VectorXd perturbed_bias(origin_bias);
std::string report;
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
epsilon_bias = 0.000001;
- Eigen::Vector6s err;
+ Eigen::Vector6d err;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
@@ -1409,7 +1409,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
//WOLF_INFO("Starting error bias 1e-4")
epsilon_bias = 0.0001;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
@@ -1427,7 +1427,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
//WOLF_INFO("Starting error bias 1e-2")
epsilon_bias = 0.01;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
@@ -1447,7 +1447,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
for(int i = 1; i<10; i++)
{
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
@@ -1498,16 +1498,16 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
KF1->getO()->fix();
KF1->getV()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
+ double epsilon_bias = 0.0000001;
+ Eigen::VectorXd perturbed_bias(origin_bias);
std::string report;
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
epsilon_bias = 0.000001;
- Eigen::Vector6s err;
+ Eigen::Vector6d err;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
@@ -1525,7 +1525,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
//WOLF_INFO("Starting error bias 1e-4")
epsilon_bias = 0.0001;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
@@ -1543,7 +1543,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
//WOLF_INFO("Starting error bias 1e-2")
epsilon_bias = 0.01;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
@@ -1563,7 +1563,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
for(int i = 1; i<10; i++)
{
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
@@ -1611,16 +1611,16 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
last_KF->getO()->fix();
last_KF->getV()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
+ double epsilon_bias = 0.0000001;
+ Eigen::VectorXd perturbed_bias(origin_bias);
std::string report;
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
epsilon_bias = 0.000001;
- Eigen::Vector6s err;
+ Eigen::Vector6d err;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
@@ -1636,7 +1636,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
//WOLF_INFO("Starting error bias 1e-4")
epsilon_bias = 0.0001;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
@@ -1651,7 +1651,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
//WOLF_INFO("Starting error bias 1e-2")
epsilon_bias = 0.01;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
@@ -1668,7 +1668,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
for(int i = 1; i<10; i++)
{
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
@@ -1691,16 +1691,16 @@ TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
last_KF->getO()->fix();
last_KF->getV()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
+ double epsilon_bias = 0.0000001;
+ Eigen::VectorXd perturbed_bias(origin_bias);
std::string report;
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
epsilon_bias = 0.000001;
- Eigen::Vector6s err;
+ Eigen::Vector6d err;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -1716,7 +1716,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
//WOLF_INFO("Starting error bias 1e-4")
epsilon_bias = 0.0001;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -1731,7 +1731,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
//WOLF_INFO("Starting error bias 1e-2")
epsilon_bias = 0.01;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -1748,7 +1748,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
for(int i = 1; i<10; i++)
{
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -1793,15 +1793,15 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
last_KF->getO()->fix();
last_KF->getV()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
+ double epsilon_bias = 0.0000001;
+ Eigen::VectorXd perturbed_bias(origin_bias);
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
epsilon_bias = 0.000001;
- Eigen::Vector6s err;
+ Eigen::Vector6d err;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -1816,7 +1816,7 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
//WOLF_INFO("Starting error bias 1e-4")
epsilon_bias = 0.0001;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -1831,7 +1831,7 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
//WOLF_INFO("Starting error bias 1e-2")
epsilon_bias = 0.01;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -1848,7 +1848,7 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
for(int i = 1; i<10; i++)
{
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -1892,15 +1892,15 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
last_KF->getO()->fix();
last_KF->getV()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
+ double epsilon_bias = 0.0000001;
+ Eigen::VectorXd perturbed_bias(origin_bias);
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
epsilon_bias = 0.000001;
- Eigen::Vector6s err;
+ Eigen::Vector6d err;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -1915,7 +1915,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
//WOLF_INFO("Starting error bias 1e-4")
epsilon_bias = 0.0001;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -1930,7 +1930,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
//WOLF_INFO("Starting error bias 1e-2")
epsilon_bias = 0.01;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -1947,7 +1947,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
for(int i = 1; i<10; i++)
{
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -1992,15 +1992,15 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
last_KF->getO()->fix();
last_KF->getV()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
+ double epsilon_bias = 0.0000001;
+ Eigen::VectorXd perturbed_bias(origin_bias);
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
epsilon_bias = 0.000001;
- Eigen::Vector6s err;
+ Eigen::Vector6d err;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -2015,7 +2015,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
//WOLF_INFO("Starting error bias 1e-4")
epsilon_bias = 0.0001;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -2030,7 +2030,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
//WOLF_INFO("Starting error bias 1e-2")
epsilon_bias = 0.01;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -2047,7 +2047,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
for(int i = 1; i<10; i++)
{
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -2092,15 +2092,15 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
last_KF->getO()->fix();
last_KF->getV()->fix();
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
+ double epsilon_bias = 0.0000001;
+ Eigen::VectorXd perturbed_bias(origin_bias);
//==============================================================
//WOLF_INFO("Starting error bias 1e-6")
epsilon_bias = 0.000001;
- Eigen::Vector6s err;
+ Eigen::Vector6d err;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -2115,7 +2115,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
//WOLF_INFO("Starting error bias 1e-4")
epsilon_bias = 0.0001;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -2130,7 +2130,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
//WOLF_INFO("Starting error bias 1e-2")
epsilon_bias = 0.01;
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -2147,7 +2147,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
for(int i = 1; i<10; i++)
{
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
+ err = Eigen::Vector6d::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
@@ -2217,8 +2217,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_i
last_KF->getV()->fix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2228,8 +2228,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_i
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::MatrixXs covX(10,10);
+ Eigen::Matrix<double, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXd covX(10,10);
//get data from covariance blocks
problem->getFrameCovariance(last_KF, covX);
@@ -2262,8 +2262,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_i
last_KF->getV()->unfix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2274,8 +2274,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_i
ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
+ Eigen::Matrix<double, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXd covX(10,10);
//get data from covariance blocks
problem->getFrameCovariance(last_KF, covX);
@@ -2309,8 +2309,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_i
last_KF->getV()->unfix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.00001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2338,8 +2338,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_i
last_KF->getV()->unfix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.00001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2349,7 +2349,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_i
ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ Eigen::Map<const Eigen::Quaterniond> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
@@ -2368,8 +2368,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_i
last_KF->getV()->unfix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.00001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2396,8 +2396,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_i
last_KF->getV()->unfix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.0001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2407,7 +2407,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_i
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ Eigen::Map<const Eigen::Quaterniond> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
@@ -2425,8 +2425,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_i
last_KF->getV()->unfix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.00001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2438,11 +2438,11 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_i
ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ Eigen::Map<const Eigen::Quaterniond> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
+ Eigen::Matrix<double, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXd covX(10,10);
//get data from covariance blocks
problem->getFrameCovariance(last_KF, covX);
@@ -2464,11 +2464,11 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_i
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
{
//Add fix factor on yaw to make the problem observable
- Eigen::MatrixXs featureFix_cov(6,6);
- featureFix_cov = Eigen::MatrixXs::Identity(6,6);
+ Eigen::MatrixXd featureFix_cov(6,6);
+ featureFix_cov = Eigen::MatrixXd::Identity(6,6);
featureFix_cov(5,5) = 0.1;
- auto capfix = CaptureBase::emplace<CaptureOdom3D>(origin_KF, 0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), nullptr);
- auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov);
+ auto capfix = CaptureBase::emplace<CaptureOdom3D>(origin_KF, 0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr);
+ auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov);
FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix));
//prepare problem for solving
@@ -2483,8 +2483,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i
last_KF->getV()->fix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.00001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2496,11 +2496,11 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i
ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ Eigen::Map<const Eigen::Quaterniond> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
+ Eigen::Matrix<double, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXd covX(10,10);
//get data from covariance blocks
problem->getFrameCovariance(last_KF, covX);
@@ -2522,13 +2522,13 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i
TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
{
//Add fix factor on yaw to make the problem observable
- Eigen::MatrixXs featureFix_cov(6,6);
- featureFix_cov = Eigen::MatrixXs::Identity(6,6);
+ Eigen::MatrixXd featureFix_cov(6,6);
+ featureFix_cov = Eigen::MatrixXd::Identity(6,6);
featureFix_cov(5,5) = 0.1;
- // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureOdom3D>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), nullptr));
- auto capfix = CaptureBase::emplace<CaptureOdom3D>(origin_KF, 0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), nullptr);
- // FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
- auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov);
+ // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureOdom3D>(0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr));
+ auto capfix = CaptureBase::emplace<CaptureOdom3D>(origin_KF, 0, nullptr, (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), nullptr);
+ // FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
+ auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), featureFix_cov);
// FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix));
@@ -2544,8 +2544,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_
last_KF->getV()->fix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.00001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2557,11 +2557,11 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_
ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ Eigen::Map<const Eigen::Quaterniond> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
+ Eigen::Matrix<double, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXd covX(10,10);
//get data from covariance blocks
problem->getFrameCovariance(last_KF, covX);
@@ -2594,8 +2594,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_in
last_KF->getV()->fix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
// WOLF_TRACE("real bias : ", origin_bias.transpose());
@@ -2611,8 +2611,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_in
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::MatrixXs covX(10,10);
+ Eigen::Matrix<double, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXd covX(10,10);
//get data from covariance blocks
problem->getFrameCovariance(last_KF, covX);
@@ -2645,8 +2645,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_in
last_KF->getV()->unfix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2657,8 +2657,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_in
ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
+ Eigen::Matrix<double, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXd covX(10,10);
//get data from covariance blocks
problem->getFrameCovariance(last_KF, covX);
@@ -2691,8 +2691,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_in
last_KF->getV()->unfix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.00001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2718,8 +2718,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_in
last_KF->getV()->unfix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.00001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2729,7 +2729,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_in
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ Eigen::Map<const Eigen::Quaterniond> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
@@ -2747,8 +2747,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_in
last_KF->getV()->unfix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.00001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2774,8 +2774,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_in
last_KF->getV()->unfix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.0001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2785,7 +2785,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_in
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ Eigen::Map<const Eigen::Quaterniond> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
@@ -2803,8 +2803,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_in
last_KF->getV()->unfix();
//perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
+ Eigen::Vector6d random_err(Eigen::Vector6d::Random() * 0.00001);
+ Eigen::Vector6d bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2816,11 +2816,11 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_in
ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ Eigen::Map<const Eigen::Quaterniond> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
+ Eigen::Matrix<double, 10, 1> cov_stdev, actual_state(last_KF->getState());
+ Eigen::MatrixXd covX(10,10);
//get data from covariance blocks
problem->getFrameCovariance(last_KF, covX);
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index f5e9a0c2b4b987cad4373bbb6e48de0c4149b284..f97943db3f4ad03b3a24b415f11673d59403a22c 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -16,13 +16,13 @@ class FeatureIMU_test : public testing::Test
wolf::ProblemPtr problem;
wolf::TimeStamp ts;
wolf::CaptureIMUPtr imu_ptr;
- Eigen::VectorXs state_vec;
- Eigen::VectorXs delta_preint;
- Eigen::Matrix<wolf::Scalar,9,9> delta_preint_cov;
+ Eigen::VectorXd state_vec;
+ Eigen::VectorXd delta_preint;
+ Eigen::Matrix<double,9,9> delta_preint_cov;
std::shared_ptr<wolf::FeatureIMU> feat_imu;
wolf::FrameBasePtr last_frame;
wolf::FrameBasePtr origin_frame;
- Eigen::MatrixXs dD_db_jacobians;
+ Eigen::MatrixXd dD_db_jacobians;
wolf::ProcessorBasePtr processor_ptr_;
//a new of this will be created for each new test
@@ -35,7 +35,7 @@ class FeatureIMU_test : public testing::Test
// Wolf problem
problem = Problem::create("POV", 3);
- Eigen::VectorXs IMU_extrinsics(7);
+ Eigen::VectorXd IMU_extrinsics(7);
IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
IntrinsicsIMUPtr sen_imu_params = std::make_shared<IntrinsicsIMU>();
SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", IMU_extrinsics, sen_imu_params);
@@ -48,14 +48,14 @@ class FeatureIMU_test : public testing::Test
// Time and data variables
TimeStamp t;
- Eigen::Vector6s data_;
+ Eigen::Vector6d data_;
t.set(0);
// Set the origin
- Eigen::VectorXs x0(10);
+ Eigen::VectorXd x0(10);
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0; P0.setIdentity(9,9);
+ MatrixXd P0; P0.setIdentity(9,9);
origin_frame = problem->setPrior(x0, P0, 0.0, 0.01);
// Emplace one capture to store the IMU data arriving from (sensor / callback / file / etc.)
@@ -65,8 +65,8 @@ class FeatureIMU_test : public testing::Test
t,
sensor_ptr,
data_,
- Eigen::Matrix6s::Identity(),
- Eigen::Vector6s::Zero()) );
+ Eigen::Matrix6d::Identity(),
+ Eigen::Vector6d::Zero()) );
//process data
data_ << 2, 0, 9.8, 0, 0, 0;
@@ -86,8 +86,8 @@ class FeatureIMU_test : public testing::Test
//emplace a feature
delta_preint = problem->getProcessorMotion()->getMotion().delta_integr_;
- delta_preint_cov = problem->getProcessorMotion()->getMotion().delta_integr_cov_ + MatrixXs::Identity(9,9)*1e-08;
- VectorXs calib_preint = problem->getProcessorMotion()->getLast()->getCalibrationPreint();
+ delta_preint_cov = problem->getProcessorMotion()->getMotion().delta_integr_cov_ + MatrixXd::Identity(9,9)*1e-08;
+ VectorXd calib_preint = problem->getProcessorMotion()->getLast()->getCalibrationPreint();
dD_db_jacobians = problem->getProcessorMotion()->getMotion().jacobian_calib_;
feat_imu = std::static_pointer_cast<FeatureIMU>(
FeatureBase::emplace<FeatureIMU>(imu_ptr,
@@ -136,7 +136,7 @@ TEST_F(FeatureIMU_test, check_frame)
left_vptr = left_frame->getV();
ASSERT_MATRIX_APPROX(origin_pptr->getState(), left_pptr->getState(), wolf::Constants::EPS_SMALL);
- Eigen::Map<const Eigen::Quaternions> origin_Quat(origin_optr->getState().data()), left_Quat(left_optr->getState().data());
+ Eigen::Map<const Eigen::Quaterniond> 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);
@@ -147,7 +147,7 @@ TEST_F(FeatureIMU_test, access_members)
{
using namespace wolf;
- Eigen::VectorXs delta(10);
+ Eigen::VectorXd 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);
diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp
index 9c51e9c2b327f4af0b86210525ab01f27eee4005..31b1ba84726b575825b116045fcaf4272face078 100644
--- a/test/gtest_processor_IMU.cpp
+++ b/test/gtest_processor_IMU.cpp
@@ -31,11 +31,11 @@ class ProcessorIMUt : public testing::Test
wolf::ProblemPtr problem;
wolf::SensorBasePtr sensor_ptr;
wolf::TimeStamp t;
- wolf::Scalar mti_clock, tmp;
- wolf::Scalar dt;
- Vector6s data;
- Matrix6s data_cov;
- VectorXs x0;
+ double mti_clock, tmp;
+ double dt;
+ Vector6d data;
+ Matrix6d data_cov;
+ VectorXd x0;
std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr;
//a new of this will be created for each new test
@@ -52,19 +52,19 @@ class ProcessorIMUt : public testing::Test
// Wolf problem
problem = Problem::create("POV", 3);
- Vector7s extrinsics = (Vector7s() << 0,0,0, 0,0,0,1).finished();
+ Vector7d extrinsics = (Vector7d() << 0,0,0, 0,0,0,1).finished();
sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/demos/sensor_imu.yaml");
ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
// Time and data variables
- data = Vector6s::Zero();
- data_cov = Matrix6s::Identity();
+ data = Vector6d::Zero();
+ data_cov = Matrix6d::Identity();
// Set the origin
x0.resize(10);
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
+ cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
}
virtual void TearDown()
@@ -101,7 +101,7 @@ TEST(ProcessorIMU_constructors, ALL)
//Factory constructor with pointers
std::string wolf_root = _WOLF_IMU_ROOT_DIR;
ProblemPtr problem = Problem::create("POV", 3);
- Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
+ Vector7d extrinsics = (Vector7d()<<1,0,0, 0,0,0,1).finished();
SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/demos/sensor_imu.yaml");
ProcessorParamsIMUPtr params_default = std::make_shared<ProcessorParamsIMU>();
ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, params_default);
@@ -131,7 +131,7 @@ TEST(ProcessorIMU, voteForKeyFrame)
// Wolf problem
ProblemPtr problem = Problem::create("POV", 3);
- Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
+ Vector7d extrinsics = (Vector7d()<<1,0,0, 0,0,0,1).finished();
SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/demos/sensor_imu.yaml");
ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
prc_imu_params->max_time_span = 1;
@@ -142,24 +142,24 @@ TEST(ProcessorIMU, voteForKeyFrame)
ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
//setting origin
- VectorXs x0(10);
+ VectorXd x0(10);
TimeStamp t(0);
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
//data variable and covariance matrix
// since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
- Vector6s data;
+ Vector6d data;
data << 1,0,0, 0,0,0;
- Matrix6s data_cov(Matrix6s::Identity());
+ Matrix6d data_cov(Matrix6d::Identity());
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<wolf::CaptureIMU> cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
// Time
// we want more than one data to integrate otherwise covariance will be 0
- Scalar dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() - 0.1;
+ double dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() - 0.1;
t.set(dt);
cap_imu_ptr->setTimeStamp(t);
sensor_ptr->process(cap_imu_ptr);
@@ -206,7 +206,7 @@ TEST_F(ProcessorIMUt, acc_x)
{
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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
@@ -216,9 +216,9 @@ TEST_F(ProcessorIMUt, acc_x)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- VectorXs x(10);
+ VectorXd x(10);
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;
+ Vector6d b; b << 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
@@ -232,7 +232,7 @@ TEST_F(ProcessorIMUt, acc_y)
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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
data << 0, 20, 9.806, 0, 0, 0; // only acc_y, but measure gravity!
@@ -242,9 +242,9 @@ TEST_F(ProcessorIMUt, acc_y)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- VectorXs x(10);
+ VectorXd x(10);
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;
+ Vector6d b; b<< 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
@@ -269,7 +269,7 @@ TEST_F(ProcessorIMUt, acc_z)
{
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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
data << 0, 0, 9.806 + 2.0, 0, 0, 0; // only acc_z, but measure gravity!
@@ -279,9 +279,9 @@ TEST_F(ProcessorIMUt, acc_z)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- VectorXs x(10);
+ VectorXd x(10);
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;
+ Vector6d b; b<< 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
@@ -306,7 +306,7 @@ TEST_F(ProcessorIMUt, check_Covariance)
{
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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
@@ -315,22 +315,22 @@ TEST_F(ProcessorIMUt, check_Covariance)
cap_imu_ptr->setTimeStamp(0.1);
sensor_ptr->process(cap_imu_ptr);
- VectorXs delta_preint(problem->getProcessorMotion()->getMotion().delta_integr_);
-// Matrix<wolf::Scalar,9,9> delta_preint_cov = problem->getProcessorMotion()->getCurrentDeltaPreintCov();
+ VectorXd delta_preint(problem->getProcessorMotion()->getMotion().delta_integr_);
+// Matrix<double,9,9> delta_preint_cov = problem->getProcessorMotion()->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_MATRIX_APPROX(delta_preint_cov, MatrixXd::Zero(9,9), wolf::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);
+ double 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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -338,10 +338,10 @@ TEST_F(ProcessorIMUt, gyro_x)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd 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);
@@ -353,7 +353,7 @@ TEST_F(ProcessorIMUt, gyro_x)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity());
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -368,16 +368,16 @@ TEST_F(ProcessorIMUt, gyro_x)
TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
{
// time
- wolf::Scalar dt(0.001);
+ double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
// bias
- wolf::Scalar abx = 0.002;
- Vector6s bias; bias << abx,0,0, 0,0,0;
- Vector3s acc_bias = bias.head(3);
+ double abx = 0.002;
+ Vector6d bias; bias << abx,0,0, 0,0,0;
+ Vector3d acc_bias = bias.head(3);
// state
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
// init things
problem->setPrior(x0, P0, t, 0.01);
@@ -387,7 +387,7 @@ 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;
+ double rate_of_turn = 5 * M_PI/180.0;
data << acc_bias - wolf::gravity(), rate_of_turn, 0, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -395,13 +395,13 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x_true(10);
+ VectorXd 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
- VectorXs x_est(10);
+ VectorXd x_est(10);
x_est = problem->getCurrentState().head(10);
ASSERT_MATRIX_APPROX(x_est.transpose() , x_true.transpose(), wolf::Constants::EPS);
@@ -413,7 +413,7 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -428,20 +428,20 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
{
- wolf::Scalar dt(0.001);
+ double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
- wolf::Scalar abx(0.002), aby(0.01);
+ double abx(0.002), aby(0.01);
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- Vector6s bias; bias << abx,aby,0, 0,0,0;
- Vector3s acc_bias = bias.head(3);
+ Vector6d bias; bias << abx,aby,0, 0,0,0;
+ Vector3d acc_bias = bias.head(3);
problem->getProcessorMotion()->getOrigin()->setCalibration(bias);
problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double 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
@@ -450,10 +450,10 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd 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()
@@ -466,7 +466,7 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -481,13 +481,13 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
TEST_F(ProcessorIMUt, gyro_z)
{
- wolf::Scalar dt(0.001);
+ double 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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
cap_imu_ptr->setData(data);
@@ -495,10 +495,10 @@ TEST_F(ProcessorIMUt, gyro_z)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd 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);
@@ -522,18 +522,18 @@ TEST_F(ProcessorIMUt, gyro_xyz)
{
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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- 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;
+ Vector3d tmp_vec; //will be used to store rate of turn data
+ Quaterniond quat_comp(Quaterniond::Identity());
+ Matrix3d R0(Matrix3d::Identity());
+ double 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;
+ double tmpx, tmpy, tmpz;
/*
ox oy oz evolution in degrees (for understanding) --> converted in rad
@@ -549,7 +549,7 @@ TEST_F(ProcessorIMUt, gyro_xyz)
wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
*/
- const wolf::Scalar dt = 0.001;
+ const double dt = 0.001;
for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++)
{
@@ -564,7 +564,7 @@ TEST_F(ProcessorIMUt, gyro_xyz)
quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
R0 = R0 * wolf::v2R(tmp_vec*dt);
// use processorIMU
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
data.tail(3) = tmp_vec;
@@ -582,16 +582,16 @@ 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)));
- 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() );
+ Quaterniond R2quat(wolf::v2q(wolf::R2v(R0)));
+ Vector4d quat_comp_vec((Vector4d() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
+ Vector4d R2quat_vec((Vector4d() <<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;
- VectorXs x(10);
+ VectorXd 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);
+ Quaterniond result_quat(problem->getCurrentState().data() + 3);
//std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
//check position part
@@ -606,13 +606,13 @@ TEST_F(ProcessorIMUt, gyro_xyz)
TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
{
- wolf::Scalar dt(0.001);
+ double 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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
cap_imu_ptr->setData(data);
@@ -620,10 +620,10 @@ TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd 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);
@@ -645,13 +645,13 @@ TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
{
- wolf::Scalar dt(0.001);
+ double 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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -659,10 +659,10 @@ TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd 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;
@@ -675,7 +675,7 @@ TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity());
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -689,13 +689,13 @@ TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
{
- wolf::Scalar dt(0.001);
+ double 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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -703,10 +703,10 @@ TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd 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;
@@ -719,7 +719,7 @@ TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity());
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -733,13 +733,13 @@ TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
{
- wolf::Scalar dt(0.001);
+ double 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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -747,10 +747,10 @@ TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd 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;
@@ -763,7 +763,7 @@ TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity());
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -779,18 +779,18 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
{
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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- 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;
+ Vector3d tmp_vec; //will be used to store rate of turn data
+ Quaterniond quat_comp(Quaterniond::Identity());
+ Matrix3d R0(Matrix3d::Identity());
+ double 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;
+ double tmpx, tmpy, tmpz;
/*
ox oy oz evolution in degrees (for understanding) --> converted in rad
@@ -806,7 +806,7 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
*/
- const wolf::Scalar dt = 0.001;
+ const double dt = 0.001;
for(unsigned int data_iter = 0; data_iter < 1000; data_iter ++)
{
@@ -821,7 +821,7 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
R0 = R0 * wolf::v2R(tmp_vec*dt);
// use processorIMU
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
data.tail(3) = tmp_vec;
@@ -839,17 +839,17 @@ 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)));
- 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() );
+ Quaterniond R2quat(wolf::v2q(wolf::R2v(R0)));
+ Vector4d quat_comp_vec((Vector4d() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
+ Vector4d R2quat_vec((Vector4d() <<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;
- VectorXs x(10);
+ VectorXd 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);
+ Quaterniond result_quat(problem->getCurrentState().data() + 3);
//std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
//check position part
@@ -865,13 +865,13 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
TEST_F(ProcessorIMUt, gyro_x_acc_x)
{
- wolf::Scalar dt(0.001);
+ double 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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << 1, 0, -wolf::gravity()(2), rate_of_turn, 0, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -879,10 +879,10 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on x axis
// translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on x axis
// v = a*dt = 0.001
@@ -898,8 +898,8 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<1,0,0).finished();
+ Quaterniond rot(problem->getCurrentState().data()+3);
+ data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3d()<<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);
@@ -914,13 +914,13 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
TEST_F(ProcessorIMUt, gyro_y_acc_y)
{
- wolf::Scalar dt(0.001);
+ double 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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << 0, 1, -wolf::gravity()(2), 0, rate_of_turn, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -928,10 +928,10 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on y axis
// translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on y axis
// v = a*dt = 0.001
@@ -947,8 +947,8 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,1,0).finished();
+ Quaterniond rot(problem->getCurrentState().data()+3);
+ data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3d()<<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);
@@ -963,13 +963,13 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
TEST_F(ProcessorIMUt, gyro_z_acc_z)
{
- wolf::Scalar dt(0.001);
+ double 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();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, -wolf::gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity
cap_imu_ptr->setData(data);
@@ -977,10 +977,10 @@ TEST_F(ProcessorIMUt, gyro_z_acc_z)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on z axis
// translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on z axis
// v = a*dt = 0.001
@@ -995,8 +995,8 @@ TEST_F(ProcessorIMUt, gyro_z_acc_z)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,0,1).finished();
+ Quaterniond rot(problem->getCurrentState().data()+3);
+ data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3d()<<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);
diff --git a/test/gtest_processor_IMU_jacobians.cpp b/test/gtest_processor_IMU_jacobians.cpp
index 422972ea078fc1cce80297009e7d2e4edbf9e243..19a7b783fbb6bc99ca5710ccb52253a02185d335 100644
--- a/test/gtest_processor_IMU_jacobians.cpp
+++ b/test/gtest_processor_IMU_jacobians.cpp
@@ -32,37 +32,37 @@ 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::Vector6d data_;
+ Eigen::Matrix<double,10,1> Delta0;
+ double ddelta_bias;
+ double dt;
+ Eigen::Matrix<double,9,1> Delta_noise;
+ Eigen::Matrix<double,9,1> delta_noise;
struct IMU_jac_bias bias_jac;
struct IMU_jac_deltas deltas_jac;
- void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq0, Eigen::Map<Eigen::Quaternions>& _dq0){
+ void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0){
- new (&_Dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta0_.data() + 3);
- new (&_dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta0_.data() + 3);
+ new (&_Dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta0_.data() + 3);
+ new (&_dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta0_.data() + 3);
}
- void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq, Eigen::Map<Eigen::Quaternions>& _dq, const int& place ){
+ void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place ){
assert(place < _jac_delta.Delta_noisy_vect_.size());
- new (&_Dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
- new (&_dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta_noisy_vect_(place).data() + 3);
+ new (&_Dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
+ new (&_dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta_noisy_vect_(place).data() + 3);
}
virtual void SetUp()
{
//SetUp for jacobians
- wolf::Scalar deg_to_rad = M_PI/180.0;
+ double 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
ProblemPtr wolf_problem_ptr_ = Problem::create("POV", 3);
- Eigen::VectorXs IMU_extrinsics(7);
+ Eigen::VectorXd IMU_extrinsics(7);
IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
ProcessorIMU_UnitTester processor_imu;
@@ -70,14 +70,14 @@ 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<double,10,1> Delta0;
+ Delta0 = Eigen::Matrix<double,10,1>::Random();
Delta0.head<3>() = Delta0.head<3>()*100;
Delta0.tail<3>() = Delta0.tail<3>()*10;
- Eigen::Vector3s ang0, ang;
+ Eigen::Vector3d ang0, ang;
ang0 << 120.08*deg_to_rad, 12.36*deg_to_rad, 54.32*deg_to_rad;
- Eigen::Map<Eigen::Quaternions> Delta0_quat(Delta0.data()+3);
+ Eigen::Map<Eigen::Quaterniond> Delta0_quat(Delta0.data()+3);
Delta0_quat = v2q(ang0);
Delta0_quat.normalize();
ang = q2v(Delta0_quat);
@@ -114,34 +114,34 @@ 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::Vector6d data_;
+ Eigen::Matrix<double,10,1> Delta0;
+ double ddelta_bias2;
+ double 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){
+ void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0){
- new (&_Dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta0_.data() + 3);
- new (&_dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta0_.data() + 3);
+ new (&_Dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta0_.data() + 3);
+ new (&_dq0) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta0_.data() + 3);
}
- void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq, Eigen::Map<Eigen::Quaternions>& _dq, const int& place ){
+ void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place ){
assert(place < _jac_delta.Delta_noisy_vect_.size());
- new (&_Dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
- new (&_dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta_noisy_vect_(place).data() + 3);
+ new (&_Dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
+ new (&_dq) Eigen::Map<const Eigen::Quaterniond>(_jac_delta.delta_noisy_vect_(place).data() + 3);
}
virtual void SetUp()
{
//SetUp for jacobians
- wolf::Scalar deg_to_rad = M_PI/180.0;
+ double 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
ProblemPtr wolf_problem_ptr_ = Problem::create("POV", 3);
- Eigen::VectorXs IMU_extrinsics(7);
+ Eigen::VectorXd IMU_extrinsics(7);
IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
ProcessorIMU_UnitTester processor_imu;
@@ -149,14 +149,14 @@ 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<double,10,1> Delta0;
+ Delta0 = Eigen::Matrix<double,10,1>::Random();
Delta0.head<3>() = Delta0.head<3>()*100;
Delta0.tail<3>() = Delta0.tail<3>()*10;
- Eigen::Vector3s ang0, ang;
+ Eigen::Vector3d ang0, ang;
ang0 << 120.08*deg_to_rad, 12.36*deg_to_rad, 54.32*deg_to_rad;
- Eigen::Map<Eigen::Quaternions> Delta0_quat(Delta0.data()+3);
+ Eigen::Map<Eigen::Quaterniond> Delta0_quat(Delta0.data()+3);
Delta0_quat = v2q(ang0);
Delta0_quat.normalize();
ang = q2v(Delta0_quat);
@@ -218,7 +218,7 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
TEST_F(ProcessorIMU_jacobians, dDp_dab)
{
using namespace wolf;
- Eigen::Matrix3s dDp_dab;
+ Eigen::Matrix3d dDp_dab;
for(int i=0;i<3;i++)
dDp_dab.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i).head(3) - bias_jac.Delta0_.head(3))/ddelta_bias;
@@ -230,7 +230,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dab)
TEST_F(ProcessorIMU_jacobians, dDv_dab)
{
using namespace wolf;
- Eigen::Matrix3s dDv_dab;
+ Eigen::Matrix3d dDv_dab;
for(int i=0;i<3;i++)
dDv_dab.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias;
@@ -242,7 +242,7 @@ TEST_F(ProcessorIMU_jacobians, dDv_dab)
TEST_F(ProcessorIMU_jacobians, dDp_dwb)
{
using namespace wolf;
- Eigen::Matrix3s dDp_dwb;
+ Eigen::Matrix3d dDp_dwb;
for(int i=0;i<3;i++)
dDp_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).head(3) - bias_jac.Delta0_.head(3))/ddelta_bias;
@@ -254,7 +254,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dwb)
TEST_F(ProcessorIMU_jacobians, dDv_dwb_)
{
using namespace wolf;
- Eigen::Matrix3s dDv_dwb;
+ Eigen::Matrix3d dDv_dwb;
for(int i=0;i<3;i++)
dDv_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias;
@@ -266,12 +266,12 @@ TEST_F(ProcessorIMU_jacobians, dDv_dwb_)
TEST_F(ProcessorIMU_jacobians, dDq_dab)
{
using namespace wolf;
- Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL);
- Eigen::Matrix3s dDq_dab;
+ Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
+ Eigen::Matrix3d dDq_dab;
- new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac.Delta0_.data() + 3);
+ new (&q_in_1) Eigen::Map<Eigen::Quaterniond>(bias_jac.Delta0_.data() + 3);
for(int i=0;i<3;i++){
- new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac.Deltas_noisy_vect_(i).data() + 3);
+ new (&q_in_2) Eigen::Map<Eigen::Quaterniond>(bias_jac.Deltas_noisy_vect_(i).data() + 3);
dDq_dab.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
}
@@ -281,13 +281,13 @@ TEST_F(ProcessorIMU_jacobians, dDq_dab)
TEST_F(ProcessorIMU_jacobians, dDq_dwb_noise_1Em8_)
{
using namespace wolf;
- Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL);
- Eigen::Matrix3s dDq_dwb;
+ Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
+ Eigen::Matrix3d dDq_dwb;
- new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac.Delta0_.data() + 3);
+ new (&q_in_1) Eigen::Map<Eigen::Quaterniond>(bias_jac.Delta0_.data() + 3);
for(int i=0;i<3;i++){
- new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac.Deltas_noisy_vect_(i+3).data() + 3);
+ new (&q_in_2) Eigen::Map<Eigen::Quaterniond>(bias_jac.Deltas_noisy_vect_(i+3).data() + 3);
dDq_dwb.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
}
@@ -303,12 +303,12 @@ TEST_F(ProcessorIMU_jacobians, dDq_dwb_noise_1Em8_)
TEST_F(ProcessorIMU_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
{
using namespace wolf;
- Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL);
- Eigen::Matrix3s dDq_dwb;
+ Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
+ Eigen::Matrix3d dDq_dwb;
- new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac2.Delta0_.data() + 3);
+ new (&q_in_1) Eigen::Map<Eigen::Quaterniond>(bias_jac2.Delta0_.data() + 3);
for(int i=0;i<3;i++){
- new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac2.Deltas_noisy_vect_(i+3).data() + 3);
+ new (&q_in_2) Eigen::Map<Eigen::Quaterniond>(bias_jac2.Deltas_noisy_vect_(i+3).data() + 3);
dDq_dwb.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias2;
}
@@ -397,7 +397,7 @@ TEST_F(ProcessorIMU_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
TEST_F(ProcessorIMU_jacobians, dDp_dP)
{
using namespace wolf;
- Eigen::Matrix3s dDp_dP;
+ Eigen::Matrix3d dDp_dP;
//dDp_dPx = ((P + dPx) - P)/dPx
for(int i=0;i<3;i++)
@@ -410,7 +410,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dP)
TEST_F(ProcessorIMU_jacobians, dDp_dV)
{
using namespace wolf;
- Eigen::Matrix3s dDp_dV;
+ Eigen::Matrix3d dDp_dV;
//Dp_dVx = ((V + dVx)*dt - V*dt)/dVx
for(int i=0;i<3;i++)
@@ -423,8 +423,8 @@ TEST_F(ProcessorIMU_jacobians, dDp_dV)
TEST_F(ProcessorIMU_jacobians, dDp_dO)
{
using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
- Eigen::Matrix3s dDp_dO;
+ Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
+ Eigen::Matrix3d dDp_dO;
//dDp_dOx = (( dR(Theta) * exp(dThetax,0,0)*dp ) - ( dR(Theta)*dp ))/dThetax
remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
@@ -440,7 +440,7 @@ TEST_F(ProcessorIMU_jacobians, dDp_dO)
TEST_F(ProcessorIMU_jacobians, dDv_dV)
{
using namespace wolf;
- Eigen::Matrix3s dDv_dV;
+ Eigen::Matrix3d dDv_dV;
//dDv_dVx = ((V + dVx) - V)/dVx
for(int i=0;i<3;i++)
@@ -453,8 +453,8 @@ TEST_F(ProcessorIMU_jacobians, dDv_dV)
TEST_F(ProcessorIMU_jacobians, dDv_dO)
{
using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
- Eigen::Matrix3s dDv_dO;
+ Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
+ Eigen::Matrix3d dDv_dO;
//dDv_dOx = (( dR(Theta) * exp(dThetax,0,0)*dv ) - ( dR(Theta)*dv ))/dThetax
remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
@@ -472,8 +472,8 @@ TEST_F(ProcessorIMU_jacobians, dDv_dO)
TEST_F(ProcessorIMU_jacobians, dDo_dO)
{
using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
- Eigen::Matrix3s dDo_dO;
+ Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
+ Eigen::Matrix3d dDo_dO;
//dDo_dOx = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(dThetax,0,0)) * dr(theta) )/dThetax
remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
@@ -489,8 +489,8 @@ TEST_F(ProcessorIMU_jacobians, dDo_dO)
TEST_F(ProcessorIMU_jacobians, dDp_dp)
{
using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL);
- Eigen::Matrix3s dDp_dp, dDp_dp_a;
+ Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL);
+ Eigen::Matrix3d dDp_dp, dDp_dp_a;
dDp_dp_a = deltas_jac.jacobian_delta_.block(0,0,3,3);
remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
@@ -507,8 +507,8 @@ TEST_F(ProcessorIMU_jacobians, dDp_dp)
TEST_F(ProcessorIMU_jacobians, dDv_dv)
{
using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL);
- Eigen::Matrix3s dDv_dv, dDv_dv_a;
+ Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL);
+ Eigen::Matrix3d dDv_dv, dDv_dv_a;
dDv_dv_a = deltas_jac.jacobian_delta_.block(6,6,3,3);
remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
@@ -525,8 +525,8 @@ TEST_F(ProcessorIMU_jacobians, dDv_dv)
TEST_F(ProcessorIMU_jacobians, dDo_do)
{
using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
- Eigen::Matrix3s dDo_do, dDo_do_a;
+ Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
+ Eigen::Matrix3d dDo_do, dDo_do_a;
//dDo_dox = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(dthetax,0,0)) )/dthetax
remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
diff --git a/test/processor_IMU_UnitTester.h b/test/processor_IMU_UnitTester.h
index c352a33648f354ad06118e6be02c50d6c965d104..ccfc60fd4579b33786990d1ca2c888920c12a15a 100644
--- a/test/processor_IMU_UnitTester.h
+++ b/test/processor_IMU_UnitTester.h
@@ -9,13 +9,13 @@
namespace wolf {
struct IMU_jac_bias{ //struct used for checking jacobians by finite difference
- IMU_jac_bias(Eigen::Matrix<Eigen::VectorXs,6,1> _Deltas_noisy_vect,
- Eigen::VectorXs _Delta0 ,
- Eigen::Matrix3s _dDp_dab,
- Eigen::Matrix3s _dDv_dab,
- Eigen::Matrix3s _dDp_dwb,
- Eigen::Matrix3s _dDv_dwb,
- Eigen::Matrix3s _dDq_dwb) :
+ IMU_jac_bias(Eigen::Matrix<Eigen::VectorXd,6,1> _Deltas_noisy_vect,
+ Eigen::VectorXd _Delta0 ,
+ Eigen::Matrix3d _dDp_dab,
+ Eigen::Matrix3d _dDv_dab,
+ Eigen::Matrix3d _dDp_dwb,
+ Eigen::Matrix3d _dDv_dwb,
+ Eigen::Matrix3d _dDq_dwb) :
Deltas_noisy_vect_(_Deltas_noisy_vect),
Delta0_(_Delta0) ,
dDp_dab_(_dDp_dab),
@@ -30,15 +30,15 @@ namespace wolf {
IMU_jac_bias(){
for (int i=0; i<6; i++){
- Deltas_noisy_vect_(i) = Eigen::VectorXs::Zero(1,1);
+ Deltas_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
}
- Delta0_ = Eigen::VectorXs::Zero(1,1);
- dDp_dab_ = Eigen::Matrix3s::Zero();
- dDv_dab_ = Eigen::Matrix3s::Zero();
- dDp_dwb_ = Eigen::Matrix3s::Zero();
- dDv_dwb_ = Eigen::Matrix3s::Zero();
- dDq_dwb_ = Eigen::Matrix3s::Zero();
+ Delta0_ = Eigen::VectorXd::Zero(1,1);
+ dDp_dab_ = Eigen::Matrix3d::Zero();
+ dDv_dab_ = Eigen::Matrix3d::Zero();
+ dDp_dwb_ = Eigen::Matrix3d::Zero();
+ dDv_dwb_ = Eigen::Matrix3d::Zero();
+ dDq_dwb_ = Eigen::Matrix3d::Zero();
}
IMU_jac_bias(IMU_jac_bias const & toCopy){
@@ -57,13 +57,13 @@ namespace wolf {
place 1 : added da_bx in data place 2 : added da_by in data place 3 : added da_bz in data
place 4 : added dw_bx in data place 5 : added dw_by in data place 6 : added dw_bz in data
*/
- Eigen::Matrix<Eigen::VectorXs,6,1> Deltas_noisy_vect_;
- Eigen::VectorXs Delta0_;
- Eigen::Matrix3s dDp_dab_;
- Eigen::Matrix3s dDv_dab_;
- Eigen::Matrix3s dDp_dwb_;
- Eigen::Matrix3s dDv_dwb_;
- Eigen::Matrix3s dDq_dwb_;
+ Eigen::Matrix<Eigen::VectorXd,6,1> Deltas_noisy_vect_;
+ Eigen::VectorXd Delta0_;
+ Eigen::Matrix3d dDp_dab_;
+ Eigen::Matrix3d dDv_dab_;
+ Eigen::Matrix3d dDp_dwb_;
+ Eigen::Matrix3d dDv_dwb_;
+ Eigen::Matrix3d dDq_dwb_;
public:
void copyfrom(IMU_jac_bias const& right){
@@ -80,12 +80,12 @@ namespace wolf {
struct IMU_jac_deltas{
- IMU_jac_deltas(Eigen::VectorXs _Delta0,
- Eigen::VectorXs _delta0,
- Eigen::Matrix<Eigen::VectorXs,9,1> _Delta_noisy_vect,
- Eigen::Matrix<Eigen::VectorXs,9,1> _delta_noisy_vect,
- Eigen::MatrixXs _jacobian_delta_preint,
- Eigen::MatrixXs _jacobian_delta ) :
+ IMU_jac_deltas(Eigen::VectorXd _Delta0,
+ Eigen::VectorXd _delta0,
+ Eigen::Matrix<Eigen::VectorXd,9,1> _Delta_noisy_vect,
+ Eigen::Matrix<Eigen::VectorXd,9,1> _delta_noisy_vect,
+ Eigen::MatrixXd _jacobian_delta_preint,
+ Eigen::MatrixXd _jacobian_delta ) :
Delta0_(_Delta0),
delta0_(_delta0),
Delta_noisy_vect_(_Delta_noisy_vect),
@@ -98,14 +98,14 @@ namespace wolf {
IMU_jac_deltas(){
for (int i=0; i<9; i++){
- Delta_noisy_vect_(i) = Eigen::VectorXs::Zero(1,1);
- delta_noisy_vect_(i) = Eigen::VectorXs::Zero(1,1);
+ Delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
+ delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
}
- Delta0_ = Eigen::VectorXs::Zero(1,1);
- delta0_ = Eigen::VectorXs::Zero(1,1);
- jacobian_delta_preint_ = Eigen::MatrixXs::Zero(9,9);
- jacobian_delta_ = Eigen::MatrixXs::Zero(9,9);
+ Delta0_ = Eigen::VectorXd::Zero(1,1);
+ delta0_ = Eigen::VectorXd::Zero(1,1);
+ jacobian_delta_preint_ = Eigen::MatrixXd::Zero(9,9);
+ jacobian_delta_ = Eigen::MatrixXd::Zero(9,9);
}
IMU_jac_deltas(IMU_jac_deltas const & toCopy){
@@ -128,12 +128,12 @@ namespace wolf {
4: +dOx, 5: +dOy, 6: +dOz 4: + dox, 5: +doy, 6: +doz
7: +dVx, 8: +dVy, 9: +dVz 7: + dvx, 9: +dvy, +: +dvz
*/
- Eigen::VectorXs Delta0_; //this will contain the Delta not affected by noise
- Eigen::VectorXs delta0_; //this will contain the delta not affected by noise
- Eigen::Matrix<Eigen::VectorXs,9,1> Delta_noisy_vect_; //this will contain the Deltas affected by noises
- Eigen::Matrix<Eigen::VectorXs,9,1> delta_noisy_vect_; //this will contain the deltas affected by noises
- Eigen::MatrixXs jacobian_delta_preint_;
- Eigen::MatrixXs jacobian_delta_;
+ Eigen::VectorXd Delta0_; //this will contain the Delta not affected by noise
+ Eigen::VectorXd delta0_; //this will contain the delta not affected by noise
+ Eigen::Matrix<Eigen::VectorXd,9,1> Delta_noisy_vect_; //this will contain the Deltas affected by noises
+ Eigen::Matrix<Eigen::VectorXd,9,1> delta_noisy_vect_; //this will contain the deltas affected by noises
+ Eigen::MatrixXd jacobian_delta_preint_;
+ Eigen::MatrixXd jacobian_delta_;
public:
void copyfrom(IMU_jac_deltas const& right){
@@ -161,10 +161,10 @@ namespace wolf {
_dt : time interval between 2 IMU measurements
da_b : bias noise to add - scalar because adding the same noise to each component of bias (abx, aby, abz, wbx, wby, wbz) one by one.
*/
- IMU_jac_bias finite_diff_ab(const Scalar _dt,
- Eigen::Vector6s& _data,
- const wolf::Scalar& da_b,
- const Eigen::Matrix<wolf::Scalar,10,1>& _delta_preint0);
+ IMU_jac_bias finite_diff_ab(const double _dt,
+ Eigen::Vector6d& _data,
+ const double& da_b,
+ const Eigen::Matrix<double,10,1>& _delta_preint0);
/* params :
_data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
@@ -172,11 +172,11 @@ namespace wolf {
_Delta_noise : noise to add to Delta_preint (D1 in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
_delta_noise : noise to add to instantaneous delta (d in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
*/
- IMU_jac_deltas finite_diff_noise(const Scalar& _dt,
- Eigen::Vector6s& _data,
- const Eigen::Matrix<wolf::Scalar,9,1>& _Delta_noise,
- const Eigen::Matrix<wolf::Scalar,9,1>& _delta_noise,
- const Eigen::Matrix<wolf::Scalar,10,1>& _Delta0);
+ IMU_jac_deltas finite_diff_noise(const double& _dt,
+ Eigen::Vector6d& _data,
+ const Eigen::Matrix<double,9,1>& _Delta_noise,
+ const Eigen::Matrix<double,9,1>& _delta_noise,
+ const Eigen::Matrix<double,10,1>& _Delta0);
public:
static ProcessorBasePtr create(const std::string& _unique_name,
@@ -185,7 +185,7 @@ namespace wolf {
public:
// Maps quat, to be used as temporary
- Eigen::Map<Eigen::Quaternions> Dq_out_;
+ Eigen::Map<Eigen::Quaterniond> Dq_out_;
};
@@ -202,46 +202,46 @@ namespace wolf {
namespace wolf{
//Functions to test jacobians with finite difference method
-inline IMU_jac_bias ProcessorIMU_UnitTester::finite_diff_ab(const Scalar _dt,
- Eigen::Vector6s& _data,
- const wolf::Scalar& da_b,
- const Eigen::Matrix<wolf::Scalar,10,1>& _delta_preint0)
+inline IMU_jac_bias ProcessorIMU_UnitTester::finite_diff_ab(const double _dt,
+ Eigen::Vector6d& _data,
+ const double& da_b,
+ const Eigen::Matrix<double,10,1>& _delta_preint0)
{
//TODO : need to use a reset function here to make sure jacobians have not been used before --> reset everything
///Define all the needed variables
- Eigen::VectorXs Delta0;
- Eigen::Matrix<Eigen::VectorXs,6,1> Deltas_noisy_vect;
- Eigen::Vector6s data0;
+ Eigen::VectorXd Delta0;
+ Eigen::Matrix<Eigen::VectorXd,6,1> Deltas_noisy_vect;
+ Eigen::Vector6d data0;
data0 = _data;
- Eigen::MatrixXs data_cov;
- Eigen::MatrixXs jacobian_delta_preint;
- Eigen::MatrixXs jacobian_delta;
- Eigen::VectorXs delta_preint_plus_delta0;
+ Eigen::MatrixXd data_cov;
+ Eigen::MatrixXd jacobian_delta_preint;
+ Eigen::MatrixXd jacobian_delta;
+ Eigen::VectorXd delta_preint_plus_delta0;
data_cov.resize(6,6);
jacobian_delta_preint.resize(9,9);
jacobian_delta.resize(9,9);
delta_preint_plus_delta0.resize(10);
//set all variables to 0
- data_cov = Eigen::MatrixXs::Zero(6,6);
- jacobian_delta_preint = Eigen::MatrixXs::Zero(9,9);
- jacobian_delta = Eigen::MatrixXs::Zero(9,9);
+ data_cov = Eigen::MatrixXd::Zero(6,6);
+ jacobian_delta_preint = Eigen::MatrixXd::Zero(9,9);
+ jacobian_delta = Eigen::MatrixXd::Zero(9,9);
delta_preint_plus_delta0 << 0,0,0, 0,0,0,1 ,0,0,0; //PQV
- Vector6s bias = Vector6s::Zero();
+ Vector6d bias = Vector6d::Zero();
/*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
place 1 : added da_bx in data place 2 : added da_by in data place 3 : added da_bz in data
place 4 : added dw_bx in data place 5 : added dw_by in data place 6 : added dw_bz in data
*/
- Eigen::Matrix3s dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb;
+ Eigen::Matrix3d dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb;
//Deltas without use of da_b
computeCurrentDelta(_data, data_cov, bias, _dt,delta_,delta_cov_,jacobian_delta_calib_);
deltaPlusDelta(_delta_preint0, delta_, _dt, delta_preint_plus_delta0, jacobian_delta_preint, jacobian_delta);
- MatrixXs jacobian_bias = jacobian_delta * jacobian_delta_calib_;
+ MatrixXd jacobian_bias = jacobian_delta * jacobian_delta_calib_;
Delta0 = delta_preint_plus_delta0; //this is the first preintegrated delta, not affected by any added bias noise
dDp_dab = jacobian_bias.block(0,0,3,3);
dDp_dwb = jacobian_bias.block(0,3,3,3);
@@ -254,7 +254,7 @@ inline IMU_jac_bias ProcessorIMU_UnitTester::finite_diff_ab(const Scalar _dt,
for(int i=0; i<6; i++){
//need to reset stuff here
delta_preint_plus_delta0 << 0,0,0, 0,0,0,1 ,0,0,0; //PQV
- data_cov = Eigen::MatrixXs::Zero(6,6);
+ data_cov = Eigen::MatrixXd::Zero(6,6);
// add da_b
_data = data0;
@@ -269,38 +269,38 @@ inline IMU_jac_bias ProcessorIMU_UnitTester::finite_diff_ab(const Scalar _dt,
return bias_jacobians;
}
-inline IMU_jac_deltas ProcessorIMU_UnitTester::finite_diff_noise(const Scalar& _dt, Eigen::Vector6s& _data, const Eigen::Matrix<wolf::Scalar,9,1>& _Delta_noise, const Eigen::Matrix<wolf::Scalar,9,1>& _delta_noise, const Eigen::Matrix<wolf::Scalar,10,1>& _Delta0)
+inline IMU_jac_deltas ProcessorIMU_UnitTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0)
{
//we do not propagate any noise from data vector
- Eigen::VectorXs Delta_; //will contain the preintegrated Delta affected by added noise
- Eigen::VectorXs delta0; //will contain the delta not affected by added noise
+ Eigen::VectorXd Delta_; //will contain the preintegrated Delta affected by added noise
+ Eigen::VectorXd delta0; //will contain the delta not affected by added noise
// delta_ that /will contain the delta affected by added noise is declared in processor_motion.h
- Eigen::VectorXs delta_preint_plus_delta;
+ Eigen::VectorXd delta_preint_plus_delta;
delta0.resize(10);
delta_preint_plus_delta.resize(10);
delta_preint_plus_delta << 0,0,0 ,0,0,0,1 ,0,0,0;
- Eigen::MatrixXs jacobian_delta_preint; //will be used as input for deltaPlusDelta
- Eigen::MatrixXs jacobian_delta; //will be used as input for deltaPlusDelta
+ Eigen::MatrixXd jacobian_delta_preint; //will be used as input for deltaPlusDelta
+ Eigen::MatrixXd jacobian_delta; //will be used as input for deltaPlusDelta
jacobian_delta_preint.resize(9,9);
jacobian_delta.resize(9,9);
jacobian_delta_preint.setIdentity(9,9);
jacobian_delta.setIdentity(9,9);
- Eigen::MatrixXs jacobian_delta_preint0; //will contain the jacobian we want to check
- Eigen::MatrixXs jacobian_delta0; //will contain the jacobian we want to check
+ Eigen::MatrixXd jacobian_delta_preint0; //will contain the jacobian we want to check
+ Eigen::MatrixXd jacobian_delta0; //will contain the jacobian we want to check
jacobian_delta_preint0.resize(9,9);
jacobian_delta0.resize(9,9);
jacobian_delta_preint0.setIdentity(9,9);
jacobian_delta0.setIdentity(9,9);
- Eigen::MatrixXs data_cov; //will be used filled with Zeros as input for data2delta
+ Eigen::MatrixXd data_cov; //will be used filled with Zeros as input for data2delta
data_cov.resize(6,6);
- data_cov = Eigen::MatrixXs::Zero(6,6);
+ data_cov = Eigen::MatrixXd::Zero(6,6);
- Eigen::Matrix<Eigen::VectorXs,9,1> Delta_noisy_vect; //this will contain the Deltas affected by noises
- Eigen::Matrix<Eigen::VectorXs,9,1> delta_noisy_vect; //this will contain the deltas affected by noises
+ Eigen::Matrix<Eigen::VectorXd,9,1> Delta_noisy_vect; //this will contain the Deltas affected by noises
+ Eigen::Matrix<Eigen::VectorXd,9,1> delta_noisy_vect; //this will contain the deltas affected by noises
- Vector6s bias = Vector6s::Zero();
+ Vector6d bias = Vector6d::Zero();
computeCurrentDelta(_data, data_cov, bias,_dt,delta_,delta_cov_,jacobian_delta_calib_); //Affects dp_out, dv_out and dq_out
delta0 = delta_; //save the delta that is not affected by noise
@@ -330,10 +330,10 @@ inline IMU_jac_deltas ProcessorIMU_UnitTester::finite_diff_noise(const Scalar& _
{
//PQV formulation
//fist we need to reset some stuff
- Eigen::Vector3s dtheta = Eigen::Vector3s::Zero();
+ Eigen::Vector3d dtheta = Eigen::Vector3d::Zero();
delta_ = delta0;
- new (&Dq_out_) Map<Quaternions>(delta_.data() + 3); //not sure that we need this
+ new (&Dq_out_) Map<Quaterniond>(delta_.data() + 3); //not sure that we need this
dtheta(i) += _delta_noise(i+3); //introduce perturbation
Dq_out_ = Dq_out_ * v2q(dtheta);
delta_noisy_vect(i+3) = delta_;
@@ -360,10 +360,10 @@ inline IMU_jac_deltas ProcessorIMU_UnitTester::finite_diff_noise(const Scalar& _
for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions
{
//fist we need to reset some stuff
- Eigen::Vector3s dtheta = Eigen::Vector3s::Zero();
+ Eigen::Vector3d dtheta = Eigen::Vector3d::Zero();
Delta_ = _Delta0;
- new (&Dq_out_) Map<Quaternions>(Delta_.data() + 3);
+ new (&Dq_out_) Map<Quaterniond>(Delta_.data() + 3);
dtheta(i) += _Delta_noise(i+3); //introduce perturbation
Dq_out_ = Dq_out_ * v2q(dtheta);
Delta_noisy_vect(i+3) = Delta_;