diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 0283d709e05c8ee46d06041ddc61a62de32c4950..4e669f9de1fcdf5e428c21037e9424a188f5e4db 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -69,12 +69,16 @@ yaml_templates_generation:
stage: yaml_templates
image: labrobotica/wolf_deps:20.04
cache:
+ - key: wolf-focal
+ paths:
+ - ci_deps/wolf/
- key: yamlschemacpp-focal
paths:
- ci_deps/yaml-schema-cpp/
before_script:
- *preliminaries_definition
- !reference [.install_yamlschemacpp_script]
+ - !reference [.install_wolf_script]
script:
- !reference [.generate_yaml_templates_script]
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7a79ed9a306a55b18844fbfb3fb8e79dc75ceb15..fa4b2502cd1fb86c1481aa2334197b820cdf94b6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -144,8 +144,6 @@ SET(SRCS_PROCESSOR
src/processor/processor_compass.cpp
src/processor/processor_imu_2d.cpp
src/processor/processor_imu_3d.cpp
- test/processor_imu_3d_tester.cpp
- test/processor_imu_2d_tester.cpp
)
SET(SRCS_SENSOR
src/sensor/sensor_compass.cpp
diff --git a/demos/demo_factor_imu_3d.cpp b/demos/demo_factor_imu_3d.cpp
deleted file mode 100644
index 5896ff0066c4407519d1d0abfe938bfb18c68ddb..0000000000000000000000000000000000000000
--- a/demos/demo_factor_imu_3d.cpp
+++ /dev/null
@@ -1,299 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-//Wolf
-#include <core/ceres_wrapper/solver_ceres.h>
-
-#include "core/capture/capture_imu.h"
-#include "core/processor/processor_imu_3d.h"
-#include "core/sensor/sensor_imu.h"
-#include "core/capture/capture_pose.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/factor/factor_odom_3D.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-
-//#define DEBUG_RESULTS
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::cout << std::endl << "==================== test_factor_imu ======================" << std::endl;
-
- bool c0(false), c1(false);// c2(true), c3(true), c4(true);
- // Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
- 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", "");
-
- // Ceres wrappers
- SolverCeres* ceres_manager_wolf_diff = new SolverCeres(wolf_problem_ptr_);
- ceres_manager_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_manager_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3;
- ceres_manager_wolf_diff->getSolverOptions().max_num_iterations = 1e4;
-
- // Time and data variables
- TimeStamp t;
- Eigen::Vector6d data_;
- double mpu_clock = 0;
-
- t.set(mpu_clock);
-
- // Set the origin
- 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_->getMotionProvider()->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::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::Matrix6d::Identity()) );
- imu_ptr->setFrame(wolf_problem_ptr_->getTrajectory()->getFrameList().back());
-
- // set variables
- using namespace std;
- Eigen::VectorXd state_vec;
- Eigen::VectorXd delta_preint;
- //FrameIMUPtr last_frame;
- Eigen::Matrix<double,9,9> delta_preint_cov;
-
- //process data
- mpu_clock = 0.001003;
- //data_ << 0.579595, -0.143701, 9.939331, 0.127445, 0.187814, -0.055003;
- data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
- t.set(mpu_clock);
- // assign data to capture
- imu_ptr->setData(data_);
- imu_ptr->setTimeStamp(t);
- // process data in capture
- sensor_ptr->process(imu_ptr);
-
- if(c0){
- /// ******************************************************************************************** ///
- /// factor creation
- //create FrameIMU
- ts = wolf_problem_ptr_->getMotionProvider()->getBuffer().back().ts_;
- state_vec = wolf_problem_ptr_->getMotionProvider()->getCurrentState();
- FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
- wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
-
- //create a feature
- delta_preint_cov = wolf_problem_ptr_->getMotionProvider()->getCurrentDeltaPreintCov();
- delta_preint = wolf_problem_ptr_->getMotionProvider()->getMotion().delta_integr_;
- std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
- feat_imu->setCapture(imu_ptr);
-
- //create a factorIMU
- FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
- feat_imu->addFactor(factor_imu);
- last_frame->addConstrainedBy(factor_imu);
-
- FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
-
- 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);
- std::cout << "expectation : " << expect.transpose() << std::endl;
-
- factor_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
- std::cout << "residuals : " << residu.transpose() << std::endl;
-
- //reset origin of motion to new frame
- wolf_problem_ptr_->getMotionProvider()->setOrigin(last_frame);
- imu_ptr->setFrame(last_frame);
- }
- /// ******************************************************************************************** ///
-
- mpu_clock = 0.002135;
- //data_ << 0.581990, -0.191602, 10.071057, 0.136836, 0.203912, -0.057686;
- data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
- t.set(mpu_clock);
- imu_ptr->setData(data_);
- imu_ptr->setTimeStamp(t);
- sensor_ptr->process(imu_ptr);
-
- mpu_clock = 0.003040;
- //data_ << 0.596360, -0.225132, 10.205178, 0.154276, 0.174399, -0.036221;
- data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
- t.set(mpu_clock);
- imu_ptr->setData(data_);
- imu_ptr->setTimeStamp(t);
- sensor_ptr->process(imu_ptr);
-
- if(c1){
- /// ******************************************************************************************** ///
- /// factor creation
- //create FrameIMU
- ts = wolf_problem_ptr_->getMotionProvider()->getBuffer().back().ts_;
- state_vec = wolf_problem_ptr_->getMotionProvider()->getCurrentState();
- FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
- wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
-
- //create a feature
- delta_preint_cov = wolf_problem_ptr_->getMotionProvider()->getCurrentDeltaPreintCov();
- delta_preint = wolf_problem_ptr_->getMotionProvider()->getMotion().delta_integr_;
- std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
- feat_imu->setCapture(imu_ptr);
-
- //create a factorIMU
- FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
- feat_imu->addFactor(factor_imu);
- last_frame->addConstrainedBy(factor_imu);
-
- FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
-
- 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);
- std::cout << "expectation : " << expect.transpose() << std::endl;
-
- factor_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
- std::cout << "residuals : " << residu.transpose() << std::endl;
-
- //reset origin of motion to new frame
- wolf_problem_ptr_->getMotionProvider()->setOrigin(last_frame);
- imu_ptr->setFrame(last_frame);
- }
-
- mpu_clock = 0.004046;
- //data_ << 0.553250, -0.203577, 10.324929, 0.128787, 0.156959, -0.044270;
- data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
- t.set(mpu_clock);
- imu_ptr->setData(data_);
- imu_ptr->setTimeStamp(t);
- sensor_ptr->process(imu_ptr);
-
- mpu_clock = 0.005045;
- //data_ << 0.548459, -0.184417, 10.387200, 0.083175, 0.120738, -0.026831;
- data_ << 0.0, 0.0, 9.81, 0.0, 0.0, 0.0;
- t.set(mpu_clock);
- imu_ptr->setData(data_);
- imu_ptr->setTimeStamp(t);
- sensor_ptr->process(imu_ptr);
-
- //create the factor
- //create FrameIMU
- ts = wolf_problem_ptr_->getMotionProvider()->getBuffer().back().ts_;
- state_vec = wolf_problem_ptr_->getMotionProvider()->getCurrentState();
- FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
- wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
-
- //create a feature
- delta_preint_cov = wolf_problem_ptr_->getMotionProvider()->getCurrentDeltaPreintCov();
- delta_preint = wolf_problem_ptr_->getMotionProvider()->getMotion().delta_integr_;
- std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
- feat_imu->setCapture(imu_ptr);
-
- //create a factorIMU
- FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
- feat_imu->addFactor(factor_imu);
- last_frame->addConstrainedBy(factor_imu);
-
- FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
-
- 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);
- std::cout << "expectation : " << expect.transpose() << std::endl;
-
- factor_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
- std::cout << "residuals : " << residu.transpose() << std::endl;
-
- if(wolf_problem_ptr_->check(1)){
- wolf_problem_ptr_->print(4,1,1,1);
- }
-
- ///having a look at covariances
- Eigen::MatrixXd predelta_cov;
- predelta_cov.resize(9,9);
- predelta_cov = wolf_problem_ptr_->getMotionProvider()->getCurrentDeltaPreintCov();
- //std::cout << "predelta_cov : \n" << predelta_cov << std::endl;
-
- ///Optimization
- // PRIOR
- //FrameBasePtr first_frame = wolf_problem_ptr_->getTrajectory()->getFrameList().front();
- wolf_problem_ptr_->getMotionProvider()->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::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;
-
- // COMPUTE COVARIANCES
- std::cout << "computing covariances..." << std::endl;
- ceres_manager_wolf_diff->computeCovariances(ALL_MARGINALS);//ALL_MARGINALS, ALL
- std::cout << "computed!" << std::endl;
-
- /*
- // SOLVING PROBLEMS
- ceres::Solver::Summary summary_diff;
- std::cout << "solving..." << std::endl;
- Eigen::VectorXd prev_wolf_state = wolf_problem_ptr_->getTrajectory()->getFrameList().back()->getState();
- summary_diff = ceres_manager_wolf_diff->solve();
- 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;
- */
-
- /*
- std::cout << "WOLF AUTO DIFF" << std::endl;
- std::cout << "Jacobian evaluation: " << summary_wolf.jacobian_evaluation_time_in_seconds << std::endl;
- std::cout << "Total time: " << summary_wolf.total_time_in_seconds << std::endl;
- */
-
- return 0;
-}
diff --git a/demos/demo_imuDock.cpp b/demos/demo_imuDock.cpp
deleted file mode 100644
index 0a4d9646f87d2e8901aaf2b6db319956ac69ea1f..0000000000000000000000000000000000000000
--- a/demos/demo_imuDock.cpp
+++ /dev/null
@@ -1,332 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#include <core/ceres_wrapper/solver_ceres.h>
-
-#include "core/processor/processor_imu_3d.h"
-#include "core/sensor/sensor_imu.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/processor/processor_odom_3D.h"
-
-//Factors headers
-#include "core/factor/factor_fix_bias.h"
-
-//std
-#include <iostream>
-#include <fstream>
-#include "core/factor/factor_pose_3D.h"
-
-#define OUTPUT_RESULTS
-//#define ADD_KF3
-
-/* OFFLINE VERSION
- In this test, we use the experimental conditions needed for Humanoids 2017.
- IMU data are acquired using the docking station.
-
- Factors are (supposing to KeyFrames, stateblocks or first Keyframe are noted *1 and second Keyframes's are *2) :
- invar : P1, V1, V2
- var : Q1,B1,P2,Q2,B2
- factors : Odometry factor between KeyFrames
- IMU factor
- FixBias factor --> makes the problem observable (adding a big covariance on all part but a smaller one on yaw)
- Fix3D factor
-
- What we expect : Estimate biases (this will strongly depend on the actual trajectory of the IMU)
- Estimate the position and orienttion of the IMU (check with the standard deviation using covariance matrix)
-
- Representation of the application:
-
- Imu
- KF1----------â—¼----------KF2
- /----P1----------\ /----------P2 invar : P1, V1, V2
- Abs|------â—¼ â—¼ var : Q1,B1,P2,Q2,B2
- \----Q1----------/ \----------Q2
- V1 Odom
- Abs|------â—¼-----B1
-*/
-int main(int argc, char** argv)
-{
- //#################################################### INITIALIZATION
- using namespace wolf;
-
- //___get input file for imu data___
- std::ifstream imu_data_input;
- const char * filename_imu;
- if (argc < 02)
- {
- WOLF_ERROR("Missing 1 input argument (path to imu data file).")
- return 1; //return with error
- }
- else
- {
- filename_imu = argv[1];
-
- imu_data_input.open(filename_imu);
- WOLF_INFO("imu file : ", filename_imu)
- }
-
- // ___Check if the file is correctly opened___
- if(!imu_data_input.is_open()){
- WOLF_ERROR("Failed to open data file ! Exiting")
- return 1;
- }
-
- #ifdef OUTPUT_RESULTS
- //define output file
- std::ofstream output_results_before, output_results_after, checking;
- output_results_before.open("imu_dock_beforeOptim.dat");
- output_results_after.open("imu_dock_afterOptim.dat");
- checking.open("KF_pose_stdev.dat");
- #endif
-
- // ___initialize variabes that will be used through the code___
- 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::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::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_CODE_DIR;
-
- // ___Create the WOLF Problem + define origin of the problem___
- ProblemPtr problem = Problem::create("PQVBB 3D");
- SolverCeres* ceres_manager = new SolverCeres(problem);
-
- // ___Configure Ceres if needed___
-
- // ___Create sensors + processors___
- SensorIMUPtr sensorIMU = std::static_pointer_cast<SensorIMU>(problem->installSensor("IMU", "Main IMU", imu_pose, wolf_root + "/src/examples/sensor_imu.yaml"));
- ProcessorIMUPtr processorIMU = std::static_pointer_cast<ProcessorIMU>(problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml"));
-
- SensorOdom3DPtr sensorOdom = std::static_pointer_cast<SensorOdom3D>(problem->installSensor("ODOM 3D", "odom", odom_pose, wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml"));
- ProcessorOdom3DPtr processorOdom = std::static_pointer_cast<ProcessorOdom3D>(problem->installProcessor("ODOM 3D", "odom", "odom", wolf_root + "/src/examples/processor_odom_3D.yaml"));
-
- // ___set origin of processors to the problem's origin___
- FrameIMUPtr KF1 = std::static_pointer_cast<FrameIMU>(processorIMU->setOrigin(problem_origin, ts)); // XXX JS: setting ts to zero, and then reading clock from data, is inconsistent.
- processorOdom->setOrigin(KF1);
-
- //#################################################### PROCESS DATA
- // ___process IMU and odometry___
-
- //Create captures
- 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
- while(!imu_data_input.eof())
- {
- //read
- imu_data_input >> clock >> data_imu[0] >> data_imu[1] >> data_imu[2] >> data_imu[3] >> data_imu[4] >> data_imu[5];
-
- //set capture
- ts.set(clock);
- imu_ptr->setTimeStamp(ts);
- imu_ptr->setData(data_imu);
-
- //process
- sensorIMU->process(imu_ptr);
- }
-
- //All IMU data have been processed, close the file
- imu_data_input.close();
-
- //A KeyFrame should have been created (depending on time_span in processors). get the last KeyFrame
- // XXX JS: in my opinion, we should control the KF creation better, not using time span. Is it possible?
- #ifdef ADD_KF3
- //Add a KeyFrame just before the motion actually starts (we did not move yet)
- data_odom << 0,0,0, 0,0,0;
- TimeStamp t_middle(0.307585);
- mot_ptr->setTimeStamp(t_middle);
- mot_ptr->setData(data_odom);
- sensorOdom->process(mot_ptr);
-
- //Also add a keyframe at the end of the motion
- data_odom << 0,-0.06,0, 0,0,0;
- ts.set(0.981573); //comment this if you want the last KF to be created at last imu's ts
- mot_ptr->setTimeStamp(ts);
- mot_ptr->setData(data_odom);
- sensorOdom->process(mot_ptr);
-
- FrameIMUPtr KF2 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(t_middle));
- FrameIMUPtr KF3 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(ts));
- #else
- //now impose final odometry using last timestamp of imu
- data_odom << 0,-0.06,0, 0,0,0;
- mot_ptr->setTimeStamp(ts);
- mot_ptr->setData(data_odom);
- sensorOdom->process(mot_ptr);
-
- FrameIMUPtr KF2 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(ts));
- #endif
-
- //#################################################### OPTIMIZATION PART
- // ___Create needed factors___
-
- //Add Fix3D factor on first KeyFrame (with large covariance except for yaw)
- 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
- featureFix_cov(5,5) = pow( .01 , 2); // yaw variance
- CaptureBasePtr cap_fix = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.head(7), 7, 6, nullptr));
- 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::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));
- //create a FeatureBase to factor biases
- FeatureBasePtr featureFixBias = cap_fixbias->addFeature(std::make_shared<FeatureBase>("FIX BIAS", problem_origin.tail(6), featureFixBias_cov));
- FactorFixBiasPtr fac_fixBias = std::static_pointer_cast<FactorFixBias>(featureFixBias->addFactor(std::make_shared<FactorFixBias>(featureFixBias)));
-
- // ___Fix/Unfix stateblocks___
- KF1->getP()->fix();
- KF1->getO()->unfix();
- KF1->getV()->fix();
- KF1->getAccBias()->unfix();
- KF1->getGyroBias()->unfix();
-
- #ifdef ADD_KF3
- KF2->getP()->fix();
- KF2->getO()->unfix();
- KF2->getV()->fix();
- KF2->getAccBias()->unfix();
- KF2->getGyroBias()->unfix();
-
- KF3->getP()->unfix();
- KF3->getO()->unfix();
- KF3->getV()->fix();
- KF3->getAccBias()->unfix();
- KF3->getGyroBias()->unfix();
- #else
- KF2->getP()->unfix();
- KF2->getO()->unfix();
- KF2->getV()->fix();
- KF2->getAccBias()->unfix();
- KF2->getGyroBias()->unfix();
- #endif
-
- #ifdef OUTPUT_RESULTS
- // ___OUTPUT___
- /* Produce output file for matlab visualization
- * first output : estimated trajectory BEFORE optimization (getting the states each millisecond)
- */
-
- unsigned int time_iter(0);
- double ms(0.001);
- ts_output.set(0);
- while(ts_output.get() < ts.get() + ms)
- {
- output_results_before << ts_output.get() << "\t" << problem->getState(ts_output).transpose() << std::endl;
- time_iter++;
- ts_output.set(time_iter * ms);
- }
- #endif
-
- // ___Solve + compute covariances___
- problem->print(4,0,1,0);
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- problem->print(1,0,1,0);
-
- //#################################################### RESULTS PART
-
- // ___Get standard deviation from covariances___
- #ifdef ADD_KF3
- Eigen::MatrixXd cov_KF1(16,16), cov_KF2(16,16), cov_KF3(16,16);
-
- problem->getCovariance(KF1, cov_KF1);
- problem->getCovariance(KF2, cov_KF2);
- problem->getCovariance(KF3, cov_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();
- stdev_KF3 = cov_KF3.diagonal().array().sqrt();
-
- WOLF_DEBUG("stdev KF1 : ", stdev_KF1.transpose());
- WOLF_DEBUG("stdev KF2 : ", stdev_KF2.transpose());
- WOLF_DEBUG("stdev KF3 : ", stdev_KF3.transpose());
- #else
- Eigen::MatrixXd cov_KF1(16,16), cov_KF2(16,16);
-
- problem->getCovariance(KF1, cov_KF1);
- problem->getCovariance(KF2, cov_KF2);
-
- Eigen::Matrix<double, 16, 1> stdev_KF1, stdev_KF2;
-
- stdev_KF1 = cov_KF1.diagonal().array().sqrt();
- stdev_KF2 = cov_KF2.diagonal().array().sqrt();
-
- WOLF_DEBUG("stdev KF1 : \n", stdev_KF1.transpose());
- WOLF_DEBUG("stdev KF2 : \n", stdev_KF2.transpose());
- #endif
-
-
- #ifdef OUTPUT_RESULTS
- // ___OUTPUT___
- /* Produce output file for matlab visualization
- * Second output: KF2 position standard deviation computed
- * estimated trajectory AFTER optimization
- * + get KF2 timestamp + state just in case the loop is not working as expected
- */
-
- //estimated trajectort
- time_iter = 0;
- ts_output.set(0);
- while(ts_output.get() < ts.get() + ms)
- {
- output_results_after << ts_output.get() << "\t" << problem->getState(ts_output).transpose() << std::endl;
- time_iter++;
- ts_output.set(time_iter * ms);
- }
-
- //finally, output the timestamp, state and stdev associated to KFs
- #ifdef ADD_KF3
- checking << KF2->getTimeStamp().get() << "\t" << KF2->getState().transpose() << "\t" << stdev_KF2.transpose() << std::endl;
- checking << KF3->getTimeStamp().get() << "\t" << KF3->getState().transpose() << "\t" << stdev_KF3.transpose() << std::endl;
- #else
- checking << KF2->getTimeStamp().get() << "\t" << KF2->getState().transpose() << "\t" << stdev_KF2.transpose() << std::endl;
- #endif
- #endif
-
- // ___Are expected values in the range of estimated +/- 2*stdev ?___
-
- #ifdef OUTPUT_RESULTS
- output_results_before.close();
- output_results_after.close();
- checking.close();
- #endif
-
- return 0;
-}
diff --git a/demos/demo_imuDock_autoKFs.cpp b/demos/demo_imuDock_autoKFs.cpp
deleted file mode 100644
index 6b4a5240cc602a5636f241509cc66d4d478a5f93..0000000000000000000000000000000000000000
--- a/demos/demo_imuDock_autoKFs.cpp
+++ /dev/null
@@ -1,325 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#include <core/ceres_wrapper/solver_ceres.h>
-
-#include "core/processor/processor_imu_3d.h"
-#include "core/sensor/sensor_imu.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/processor/processor_odom_3D.h"
-
-//Factors headers
-#include "core/factor/factor_fix_bias.h"
-
-//std
-#include <iostream>
-#include <fstream>
-#include "core/factor/factor_pose_3D.h"
-
-#define OUTPUT_RESULTS
-//#define AUTO_KFS
-
-/* OFFLINE VERSION
- In this test, we use the experimental conditions needed for Humanoids 2017.
- IMU data are acquired using the docking station.
-
- Factors are (supposing to KeyFrames, stateblocks or first Keyframe are noted *1 and second Keyframes's are *2) :
- invar : P1, V1, V2
- var : Q1,B1,P2,Q2,B2
-
- All Keyframes coming after KF2 are constrained just like KF2
- factors : Odometry factor between KeyFrames
- IMU factor
- FixBias factor --> makes the problem observable (adding a big covariance on all part but a smaller one on yaw)
- Fix3D factor
-
- What we expect : Estimate biases (this will strongly depend on the actual trajectory of the IMU)
- Estimate the position and orienttion of the IMU (check with the standard deviation using covariance matrix)
-
- Representation of the application:
-
- Imu
- KF1----------â—¼----------KF2--..
- /----P1----------\ /----------P2--.. invar : P1, V1, V2
- Abs|------â—¼ â—¼ var : Q1,B1,P2,Q2,B2
- \----Q1----------/ \----------Q2--..
- V1 Odom v2 ..
- Abs|------â—¼-----B1 B2 ..
-*/
-int main(int argc, char** argv)
-{
- //#################################################### INITIALIZATION
- using namespace wolf;
-
- //___get input files for imu and odom data___
- std::ifstream imu_data_input, odom_data_input;
- const char * filename_imu;
- const char * filename_odom;
- if (argc < 03)
- {
- WOLF_ERROR("Missing input argument : path to imu or/and odom data file(s).")
- return 1; //return with error
- }
- else
- {
- filename_imu = argv[1];
- filename_odom = argv[2];
-
- imu_data_input.open(filename_imu);
- WOLF_INFO("imu file : ", filename_imu)
- odom_data_input.open(filename_odom);
- WOLF_INFO("odom file : ", filename_odom)
- }
-
- // ___Check if the file is correctly opened___
- if(!imu_data_input.is_open() || !odom_data_input.is_open()){
- WOLF_ERROR("Failed to open data file ! Exiting")
- return 1;
- }
-
- #ifdef OUTPUT_RESULTS
- //define output file
- std::ofstream output_results_before, output_results_after, checking;
- output_results_before.open("imu_dock_beforeOptim.dat");
- output_results_after.open("imu_dock_afterOptim.dat");
- checking.open("KF_pose_stdev.dat");
- #endif
-
- // ___initialize variabes that will be used through the code___
- 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::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::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<double, 50> lastms_imuData;
- #endif
- //#################################################### SETTING PROBLEM
- std::string wolf_root = _WOLF_CODE_DIR;
-
- // ___Create the WOLF Problem + define origin of the problem___
- ProblemPtr problem = Problem::create("PQVBB 3D");
- SolverCeres* ceres_manager = new SolverCeres(problem);
-
- // ___Configure Ceres if needed___
-
- // ___Create sensors + processors___
- SensorIMUPtr sensorIMU = std::static_pointer_cast<SensorIMU>(problem->installSensor("IMU", "Main IMU", imu_pose, wolf_root + "/src/examples/sensor_imu.yaml"));
- ProcessorIMUPtr processorIMU = std::static_pointer_cast<ProcessorIMU>(problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml"));
-
- SensorOdom3DPtr sensorOdom = std::static_pointer_cast<SensorOdom3D>(problem->installSensor("ODOM 3D", "odom", odom_pose, wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml"));
- ProcessorOdom3DPtr processorOdom = std::static_pointer_cast<ProcessorOdom3D>(problem->installProcessor("ODOM 3D", "odom", "odom", wolf_root + "/src/examples/processor_odom_3D.yaml"));
-
- // ___set origin of processors to the problem's origin___
- FrameIMUPtr KF1 = std::static_pointer_cast<FrameIMU>(processorIMU->setOrigin(problem_origin, ts)); // XXX JS: setting ts to zero, and then reading clock from data, is inconsistent.
- processorOdom->setOrigin(KF1);
-
- //#################################################### PROCESS DATA
- // ___process IMU and odometry___
-
- //Create captures
- 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
- while(!imu_data_input.eof())
- {
- //read
- imu_data_input >> clock >> data_imu[0] >> data_imu[1] >> data_imu[2] >> data_imu[3] >> data_imu[4] >> data_imu[5];
-
- //set capture
- ts.set(clock);
- imu_ptr->setTimeStamp(ts);
- imu_ptr->setData(data_imu);
-
- //process
- sensorIMU->process(imu_ptr);
- }
-
- //Process all the odom data
- // XXX JS: in my opinion, we should control the KF creation better, not using time span. Is it possible?
- while(!odom_data_input.eof())
- {
- //read
- odom_data_input >> clock >> data_odom[0] >> data_odom[1] >> data_odom[2] >> data_odom[3] >> data_odom[4] >> data_odom[5];
-
- //set capture
- ts.set(clock);
- mot_ptr->setTimeStamp(ts);
- mot_ptr->setData(data_odom);
-
- #ifdef AUTO_KFS
- /* We want the KFs to be generated automatically but not using time span as argument of this generation
- * For our application, w want the KFs to be generated when an odometry data is given under condition that the IMU is not moving
- * We check wether the IMU is moving or not by computing the current stdev of the IMU based on data received during 50ms before the odom timestamp
- * We compare this value to the stdev (noise) of the sensor (see sensor_imu.yaml)
- * If the current stdev is below a threshold then we process the odometry data !
- */
-
- // TODO : get data to compute stdev with directly from the capture
- // -> see how these data are stored and change getIMUStdev(..) function defined below main() in this file
- // -> then just use the function to get this stdev of corresponding data
-
- #else
- //process anyway. KFs will be generated based on the configuration given in processor_odom_3D.yaml
- sensorOdom->process(mot_ptr);
- #endif
- }
-
- //All data have been processed, close the files
- imu_data_input.close();
- odom_data_input.close();
-
- //A KeyFrame should have been created (depending on time_span in processors). get all frames
- FrameBasePtrList trajectory = problem->getTrajectory()->getFrameList();
-
-
- //#################################################### OPTIMIZATION PART
- // ___Create needed factors___
-
- //Add Fix3D factor on first KeyFrame (with large covariance except for yaw)
- 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
- featureFix_cov(5,5) = pow( .001 , 2); // yaw variance
- CaptureBasePtr cap_fix = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.head(7), 7, 6, nullptr));
- 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::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));
- //create a FeatureBase to factor biases
- FeatureBasePtr featureFixBias = cap_fixbias->addFeature(std::make_shared<FeatureBase>("FIX BIAS", problem_origin.tail(6), featureFixBias_cov));
- FactorFixBiasPtr fac_fixBias = std::static_pointer_cast<FactorFixBias>(featureFixBias->addFactor(std::make_shared<FactorFixBias>(featureFixBias)));
-
- // ___Fix/Unfix stateblocks___
- // fix all Keyframes here
-
- FrameIMUPtr frame_imu;
- for(auto frame : trajectory)
- {
- frame_imu = std::static_pointer_cast<FrameIMU>(frame);
- if(frame_imu->isKey())
- {
- frame_imu->getP()->fix();
- frame_imu->getO()->unfix();
- 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();
- }
- }
-
- //KF1 (origin) needs to be also fixed in position
- KF1->getP()->fix();
-
- #ifdef OUTPUT_RESULTS
- // ___OUTPUT___
- /* Produce output file for matlab visualization
- * first output : estimated trajectory BEFORE optimization (getting the states each millisecond)
- */
-
- unsigned int time_iter(0);
- double ms(0.001);
- ts_output.set(0);
- while(ts_output.get() < ts.get() + ms)
- {
- output_results_before << ts_output.get() << "\t" << problem->getState(ts_output).transpose() << std::endl;
- time_iter++;
- ts_output.set(time_iter * ms);
- }
- #endif
-
- // ___Solve + compute covariances___
- problem->print(4,0,1,0);
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
- ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- problem->print(1,0,1,0);
-
- //#################################################### RESULTS PART
-
- // ___Get standard deviation from covariances___ and output this in a file
- Eigen::MatrixXd cov_KF(16,16);
- Eigen::Matrix<double, 16, 1> stdev_KF;
- for(auto frame : trajectory)
- {
- if(frame->isKey())
- {
- problem->getCovariance(frame, cov_KF);
- stdev_KF = cov_KF.diagonal().array().sqrt();
- #ifdef OUTPUT_RESULTS
- checking << frame->getTimeStamp().get() << "\t" << frame->getState().transpose() << "\t" << stdev_KF.transpose() << std::endl;
- #endif
- }
- }
-
- #ifdef OUTPUT_RESULTS
- // ___OUTPUT___
- /* Produce output file for matlab visualization
- * Second output: KF position standard deviation computed
- * estimated trajectory AFTER optimization
- */
-
- //estimated trajectort
- time_iter = 0;
- ts_output.set(0);
- while(ts_output.get() < ts.get() + ms)
- {
- output_results_after << ts_output.get() << "\t" << problem->getState(ts_output).transpose() << std::endl;
- time_iter++;
- ts_output.set(time_iter * ms);
- }
-
- //Finished writing in files : close them
- output_results_before.close();
- output_results_after.close();
- checking.close();
- #endif
-
- return 0;
-}
-
-/*double getIMUStdev(Eigen::VectorXd _data) //input argument : whatever will contain the data in the capture
-{
- Eigen::Vector6d mean(Eigen::Vector6d::Zero()), stdev(Eigen::Vector6d::Zero());
- unsigned int _data_size(_data.size());
-
- mean = _data.mean()/_data_size;
-
- for (unsigned int i = 0; i < _data_size; i++)
- {
- stdev += pow(_data()-mean,2); //get the correct data from the container
- }
- return (stdev.array().sqrt()).maxCoeff();
-}*/
diff --git a/demos/demo_imuPlateform_Offline.cpp b/demos/demo_imuPlateform_Offline.cpp
deleted file mode 100644
index fe13bf27c75f0ffcddd282f9e992e95f13fbc3a0..0000000000000000000000000000000000000000
--- a/demos/demo_imuPlateform_Offline.cpp
+++ /dev/null
@@ -1,284 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-//Wolf
-#include <core/ceres_wrapper/solver_ceres.h>
-
-#include "core/capture/capture_imu.h"
-#include "core/processor/processor_imu_3d.h"
-#include "core/sensor/sensor_imu.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/factor/factor_odom_3D.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/processor/processor_odom_3D.h"
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-#define DEBUG_RESULTS
-
-int _kbhit();
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- // LOADING DATA FILE (IMU)
- // FOR IMU, file content is : Timestampt\t Ax\t Ay\t Az\t Wx\t Wy\t Wz
-
- std::ifstream imu_data_input;
- const char * filename_imu;
- if (argc < 02)
- {
-
- WOLF_ERROR("Missing input argument! : needs 1 argument (path to imu data file).")
- return 1;
- }
- else
- {
- filename_imu = argv[1];
-
- imu_data_input.open(filename_imu);
- WOLF_INFO("imu file : ", filename_imu)
-
- //std::string dummy; //this is needed only if first line is headers or useless data
- //getline(imu_data_input, dummy);
- }
-
- if(!imu_data_input.is_open()){
- std::cerr << "Failed to open data files... Exiting" << std::endl;
- return 1;
- }
-
- #ifdef DEBUG_RESULTS
- std::ofstream debug_results;
- debug_results.open("debug_results_imu_constrained0.dat");
- if(debug_results)
- debug_results << "%%TimeStamp\t"
- << "X_x\t" << "X_y\t" << "X_z\t" << "Xq_x\t" << "Xq_y\t" << "Xq_z\t" << "Xq_w\t" << "Xv_x\t" << "Xv_y\t" << "Xv_z\t"
- << "Cov_X\t" << "Cov_Y\t" << "Cov_Z\t" << "Cov_Qx\t" << "Cov_Qy\t" << "Cov_Qz\t" << "Cov_Qw" << "Cov_Vx\t" << "Cov_Vy\t" << "Cov_Vz\t" << std::endl;
- #endif
-
- //===================================================== SETTING PROBLEM
- std::string wolf_root = _WOLF_CODE_DIR;
-
- // WOLF PROBLEM
- ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
- 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);
-
- // CERES WRAPPER
- SolverCeres* ceres_manager_wolf_diff = new SolverCeres(wolf_problem_ptr_);
- ceres_manager_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_manager_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3;
- ceres_manager_wolf_diff->getSolverOptions().max_num_iterations = 1e4;
-
- // SENSOR + PROCESSOR IMU
- 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
- prc_imu_params->dist_traveled = 1000000000;
- prc_imu_params->angle_turned = 1000000000;
-
- ProcessorBasePtr processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", sen0_ptr, prc_imu_params);
- SensorIMUPtr sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- 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", (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
- prc_odom3D_params->dist_traveled = 1000000000;
- prc_odom3D_params->angle_turned = 1000000000;
-
- ProcessorBasePtr processor_ptr_odom = wolf_problem_ptr_->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
- SensorOdom3DPtr sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
- ProcessorOdom3DPtr processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom);
-
- // reset origin of problem
- t.set(0);
-
- FrameIMUPtr origin_KF = std::static_pointer_cast<FrameIMU>(processor_ptr_imu->setOrigin(x_origin, t));
- processor_ptr_odom3D->setOrigin(origin_KF);
-
- //fix parts of the problem if needed
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- 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;
-
- double input_clock;
- TimeStamp ts(0);
- 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
- t = ts;
-
- clock_t begin = clock();
-
- while( !imu_data_input.eof())
- {
- // PROCESS IMU DATA
- // Time and data variables
- imu_data_input >> input_clock >> data_imu[0] >> data_imu[1] >> data_imu[2] >> data_imu[3] >> data_imu[4] >> data_imu[5]; //Ax, Ay, Az, Gx, Gy, Gz
-
- ts.set(input_clock);
- imu_ptr->setTimeStamp(ts);
- imu_ptr->setData(data_imu);
-
- // process data in capture
- imu_ptr->getTimeStamp();
- sen_imu->process(imu_ptr);
- }
-
- TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getMotionProvider()->getBuffer().front().ts_;
- tf = wolf_problem_ptr_->getMotionProvider()->getBuffer().back().ts_;
- int N = wolf_problem_ptr_->getMotionProvider()->getBuffer().size();
-
- //Finally, process the only one odom input
- mot_ptr->setTimeStamp(ts);
- mot_ptr->setData(data_odom3D);
- sen_odom3D->process(mot_ptr);
-
- clock_t end = clock();
- FrameIMUPtr last_KF = std::static_pointer_cast<FrameIMU>(wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts));
-
- //closing file
- imu_data_input.close();
- //===================================================== END{PROCESS DATA}
-
- double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
-
- // Final state
- std::cout << "\nIntegration results ----------------------------------------------------------------------------------------------" << std::endl;
- std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << x_origin.head(16).transpose() << std::endl;
- std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getMotionProvider()->getMotion().delta_integr_.transpose() << std::endl;
- std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getMotionProvider()->getCurrentState().head(16).transpose() << std::endl;
- std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (wolf_problem_ptr_->getMotionProvider()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
-
- // Print statistics
- std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
- std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
-
- /*TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getMotionProvider()->getBuffer().front().ts_;
- tf = wolf_problem_ptr_->getMotionProvider()->getBuffer().back().ts_;
- int N = wolf_problem_ptr_->getMotionProvider()->getBuffer().size();*/
- std::cout << "t0 : " << t0.get() << " s" << std::endl;
- std::cout << "tf : " << tf.get() << " s" << std::endl;
- std::cout << "duration : " << tf-t0 << " s" << std::endl;
- std::cout << "N samples : " << N << std::endl;
- std::cout << "frequency : " << (N-1)/(tf-t0) << " Hz" << std::endl;
- std::cout << "CPU time : " << elapsed_secs << " s" << std::endl;
- std::cout << "s/integr : " << elapsed_secs/(N-1)*1e6 << " us" << std::endl;
- std::cout << "integr/s : " << (N-1)/elapsed_secs << " ips" << std::endl;
-
- //fix parts of the problem if needed
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- std::cout << "\t\t\t ______solving______" << std::endl;
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::FULL);// 0: nothing, 1: BriefReport, 2: FullReport
- std::cout << report << std::endl;
- ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
- std::cout << "\t\t\t ______solved______" << std::endl;
-
- wolf_problem_ptr_->print(4,1,1,1);
-
- #ifdef DEBUG_RESULTS
- 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)
- {
- if(frm_ptr->isKey())
- {
- //prepare needed variables
- FrameIMUPtr frmIMU_ptr = std::static_pointer_cast<FrameIMU>(frm_ptr);
- frm_state = frmIMU_ptr->getState();
- ts = frmIMU_ptr->getTimeStamp();
-
- //get data from covariance blocks
- wolf_problem_ptr_->getCovariance(frmIMU_ptr, covX);
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getV(), frmIMU_ptr->getV(), cov3);
- covX.block(7,7,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getAccBias(), frmIMU_ptr->getAccBias(), cov3);
- covX.block(10,10,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getGyroBias(), frmIMU_ptr->getGyroBias(), cov3);
- covX.block(13,13,3,3) = cov3;
- for(int i = 0; i<16; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- debug_results << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2)
- << "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6)
- << "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9)
- << "\t" << frm_state(10) << "\t" << frm_state(11) << "\t" << frm_state(12) << "\t" << frm_state(13) << "\t" << frm_state(14) << "\t" << frm_state(15)
- << "\t" << cov_stdev(0) << "\t" << cov_stdev(1) << "\t" << cov_stdev(2)
- << "\t" << cov_stdev(3) << "\t" << cov_stdev(4) << "\t" << cov_stdev(5) << "\t" << cov_stdev(6)
- << "\t" << cov_stdev(7) << "\t" << cov_stdev(8) << "\t" << cov_stdev(9)
- << "\t" << cov_stdev(10) << "\t" << cov_stdev(11) << "\t" << cov_stdev(12) << "\t" << cov_stdev(13) << "\t" << cov_stdev(14) << "\t" << cov_stdev(15) << std::endl;
- }
- }
-
- debug_results.close();
- WOLF_WARN("WARNING : DEBUG_RESULTS ACTIVATED - slows the process (writing results to result_debugs.dat file)")
-
- #endif
-
- return 0;
-
-}
-
-int _kbhit()
-{
- struct timeval tv;
- fd_set fds;
- tv.tv_sec = 0;
- tv.tv_usec = 0;
- FD_ZERO(&fds);
- FD_SET(STDIN_FILENO, &fds); //STDIN_FILENO is 0
- select(STDIN_FILENO+1, &fds, NULL, NULL, &tv);
- return FD_ISSET(STDIN_FILENO, &fds);
-}
diff --git a/demos/demo_imu_constrained0.cpp b/demos/demo_imu_constrained0.cpp
deleted file mode 100644
index 3c00b52f40f780301c479e9e051ac8d4b36e7e59..0000000000000000000000000000000000000000
--- a/demos/demo_imu_constrained0.cpp
+++ /dev/null
@@ -1,392 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-//Wolf
-#include <core/ceres_wrapper/solver_ceres.h>
-
-#include "core/capture/capture_imu.h"
-#include "core/processor/processor_imu_3d.h"
-#include "core/sensor/sensor_imu.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/factor/factor_odom_3D.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/processor/processor_odom_3D.h"
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-#define DEBUG_RESULTS
-#define KF0_EVOLUTION
-
-int _kbhit();
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- // LOADING DATA FILES (IMU + ODOM)
- // FOR IMU, file content is : Timestampt\t Ax\t Ay\t Az\t Wx\t Wy\t Wz
- // FOR ODOM, file content is : Timestampt\t Δpx\t Δpy\t Δpz\t Δox\t Δoy\t Δoz
-
- std::ifstream imu_data_input;
- std::ifstream odom_data_input;
- const char * filename_imu;
- const char * filename_odom;
- if (argc < 3)
- {
- std::cout << "Missing input argument! : needs 2 argument (path to imu and odom data files)." << std::endl;
- return 1;
- }
- else
- {
- filename_imu = argv[1];
- filename_odom = argv[2];
-
- imu_data_input.open(filename_imu);
- odom_data_input.open(filename_odom);
-
- std::cout << "file imu : " << filename_imu <<"\t file odom : " << filename_odom << std::endl;
-
- //std::string dummy; //this is needed only if first line is headers or useless data
- //getline(imu_data_input, dummy);
- }
-
- if(!imu_data_input.is_open() || !odom_data_input.is_open()){
- std::cerr << "Failed to open data files... Exiting" << std::endl;
- return 1;
- }
-
- #ifdef DEBUG_RESULTS
- std::ofstream debug_results;
- debug_results.open("debug_results_imu_constrained0.dat");
- if(debug_results)
- debug_results << "%%TimeStamp\t"
- << "X_x\t" << "X_y\t" << "X_z\t" << "Xq_x\t" << "Xq_y\t" << "Xq_z\t" << "Xq_w\t" << "Xv_x\t" << "Xv_y\t" << "Xv_z\t"
- << "Cov_X\t" << "Cov_Y\t" << "Cov_Z\t" << "Cov_Qx\t" << "Cov_Qy\t" << "Cov_Qz\t" << "Cov_Qw" << "Cov_Vx\t" << "Cov_Vy\t" << "Cov_Vz\t" << std::endl;
- #endif
-
- #ifdef KF0_EVOLUTION
- std::ofstream KF0_evolution;
- KF0_evolution.open("KF0_evolution.dat");
- if(KF0_evolution)
- KF0_evolution << "%%TimeStamp\t"
- << "X_x\t" << "X_y\t" << "X_z\t" << "Xq_x\t" << "Xq_y\t" << "Xq_z\t" << "Xq_w\t" << "Xv_x\t" << "Xv_y\t" << "Xv_z\t"
- << "Cov_X\t" << "Cov_Y\t" << "Cov_Z\t" << "Cov_Qx\t" << "Cov_Qy\t" << "Cov_Qz\t" << "Cov_Qw" << "Cov_Vx\t" << "Cov_Vy\t" << "Cov_Vz\t" << std::endl;
- #endif
-
- //===================================================== SETTING PROBLEM
- std::string wolf_root = _WOLF_CODE_DIR;
-
- // WOLF PROBLEM
- ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
- 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);
-
- // initial conditions defined from data file
- // remember that matlab's quaternion is W,X,Y,Z and the one in Eigen has X,Y,Z,W form
- imu_data_input >> x_origin[0] >> x_origin[1] >> x_origin[2] >> x_origin[6] >> x_origin[3] >> x_origin[4] >> x_origin[5] >> x_origin[7] >> x_origin[8] >> x_origin[9];
- imu_data_input >> origin_bias[0] >> origin_bias[1] >> origin_bias[2] >> origin_bias[3] >> origin_bias[4] >> origin_bias[5];
-
- // CERES WRAPPER
- SolverCeres* ceres_manager_wolf_diff = new SolverCeres(wolf_problem_ptr_);
- ceres_manager_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_manager_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3;
- ceres_manager_wolf_diff->getSolverOptions().max_num_iterations = 1e4;
-
- // SENSOR + PROCESSOR IMU
- 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
- prc_imu_params->dist_traveled = 1000000000;
- prc_imu_params->angle_turned = 1000000000;
-
- ProcessorBasePtr processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", sen0_ptr, prc_imu_params);
- SensorIMUPtr sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- 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", (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
- prc_odom3D_params->dist_traveled = 1000000000;
- prc_odom3D_params->angle_turned = 1000000000;
-
- ProcessorBasePtr processor_ptr_odom = wolf_problem_ptr_->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
- SensorOdom3DPtr sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
- ProcessorOdom3DPtr processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom);
-
- // reset origin of problem
- t.set(0);
- 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);
-
- odom_data_input >> expected_final_state[0] >> expected_final_state[1] >> expected_final_state[2] >> expected_final_state[6] >> expected_final_state[3] >>
- expected_final_state[4] >> expected_final_state[5] >> expected_final_state[7] >> expected_final_state[8] >> expected_final_state[9];
-
- //fix parts of the problem if needed
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6d data_imu, data_odom3D;
- data_imu << 0,0,-wolf::gravity()(2), 0,0,0;
- data_odom3D << 0,0,0, 0,0,0;
-
- double input_clock;
- TimeStamp ts(0);
- TimeStamp t_odom(0);
- 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
- odom_data_input >> input_clock >> data_odom3D[0] >> data_odom3D[1] >> data_odom3D[2] >> data_odom3D[3] >> data_odom3D[4] >> data_odom3D[5];
- t_odom.set(input_clock);
- //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
- t = ts;
-
- clock_t begin = clock();
-
- while( !imu_data_input.eof() && !odom_data_input.eof() )
- {
- // PROCESS IMU DATA
- // Time and data variables
- imu_data_input >> input_clock >> data_imu[0] >> data_imu[1] >> data_imu[2] >> data_imu[3] >> data_imu[4] >> data_imu[5]; //Ax, Ay, Az, Gx, Gy, Gz
-
- ts.set(input_clock);
- imu_ptr->setTimeStamp(ts);
- imu_ptr->setData(data_imu);
-
- // process data in capture
- imu_ptr->getTimeStamp();
- sen_imu->process(imu_ptr);
-
- if(ts.get() == t_odom.get())
- {
- // PROCESS ODOM 3D DATA
- mot_ptr->setTimeStamp(t_odom);
- mot_ptr->setData(data_odom3D);
- sen_odom3D->process(mot_ptr);
-
- //prepare next odometry measurement if there is any
- odom_data_input >> input_clock >> data_odom3D[0] >> data_odom3D[1] >> data_odom3D[2] >> data_odom3D[3] >> data_odom3D[4] >> data_odom3D[5];
- t_odom.set(input_clock);
- }
-
- #ifdef KF0_EVOLUTION
-
- if( (ts.get() - t.get()) >= 0.05 )
- {
- t = ts;
- //std::string report = ceres_manager_wolf_diff->solve(1); //0: nothing, 1: BriefReport, 2: FullReport
-
- 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)
- << "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6)
- << "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9)
- << "\t" << frm_state(10) << "\t" << frm_state(11) << "\t" << frm_state(12) << "\t" << frm_state(13) << "\t" << frm_state(14) << "\t" << frm_state(15) << std::endl;
- }
-
- #endif
- }
-
- clock_t end = clock();
- FrameIMUPtr last_KF = std::static_pointer_cast<FrameIMU>(wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts));
-
- //closing file
- imu_data_input.close();
- odom_data_input.close();
-
- #ifdef KF0_EVOLUTION
- KF0_evolution.close();
- #endif
-
- //===================================================== END{PROCESS DATA}
-
- double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
-
- // Final state
- std::cout << "\nIntegration results ----------------------------------------------------------------------------------------------" << std::endl;
- std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << x_origin.head(16).transpose() << std::endl;
- std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getMotionProvider()->getMotion().delta_integr_.transpose() << std::endl;
- std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getMotionProvider()->getCurrentState().head(16).transpose() << std::endl;
- std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (wolf_problem_ptr_->getMotionProvider()->getMotion().delta_integr_cov_.diagonal()).array().sqrt() << std::endl;
-
- // Print statistics
- std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
- std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
-
- TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getMotionProvider()->getBuffer().front().ts_;
- tf = wolf_problem_ptr_->getMotionProvider()->getBuffer().back().ts_;
- int N = wolf_problem_ptr_->getMotionProvider()->getBuffer().size();
- std::cout << "t0 : " << t0.get() << " s" << std::endl;
- std::cout << "tf : " << tf.get() << " s" << std::endl;
- std::cout << "duration : " << tf-t0 << " s" << std::endl;
- std::cout << "N samples : " << N << std::endl;
- std::cout << "frequency : " << (N-1)/(tf-t0) << " Hz" << std::endl;
- std::cout << "CPU time : " << elapsed_secs << " s" << std::endl;
- std::cout << "s/integr : " << elapsed_secs/(N-1)*1e6 << " us" << std::endl;
- std::cout << "integr/s : " << (N-1)/elapsed_secs << " ips" << std::endl;
-
- //fix parts of the problem if needed
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
-
- std::cout << "\t\t\t ______solving______" << std::endl;
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::FULL); //0: nothing, 1: BriefReport, 2: FullReport
- std::cout << report << std::endl;
-
- last_KF->getAccBias()->fix();
- last_KF->getGyroBias()->fix();
-
- std::cout << "\t\t\t solving after fixBias" << std::endl;
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); //0: nothing, 1: BriefReport, 2: FullReport
- std::cout << report << std::endl;
- ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
- std::cout << "\t\t\t ______solved______" << std::endl;
-
- wolf_problem_ptr_->print(4,1,1,1);
-
- #ifdef DEBUG_RESULTS
- 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)
- {
- if(frm_ptr->isKey())
- {
- //prepare needed variables
- FrameIMUPtr frmIMU_ptr = std::static_pointer_cast<FrameIMU>(frm_ptr);
- frm_state = frmIMU_ptr->getState();
- ts = frmIMU_ptr->getTimeStamp();
-
- //get data from covariance blocks
- wolf_problem_ptr_->getCovariance(frmIMU_ptr, covX);
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getV(), frmIMU_ptr->getV(), cov3);
- covX.block(7,7,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getAccBias(), frmIMU_ptr->getAccBias(), cov3);
- covX.block(10,10,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getGyroBias(), frmIMU_ptr->getGyroBias(), cov3);
- covX.block(13,13,3,3) = cov3;
- for(int i = 0; i<16; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- debug_results << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2)
- << "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6)
- << "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9)
- << "\t" << frm_state(10) << "\t" << frm_state(11) << "\t" << frm_state(12) << "\t" << frm_state(13) << "\t" << frm_state(14) << "\t" << frm_state(15)
- << "\t" << cov_stdev(0) << "\t" << cov_stdev(1) << "\t" << cov_stdev(2)
- << "\t" << cov_stdev(3) << "\t" << cov_stdev(4) << "\t" << cov_stdev(5) << "\t" << cov_stdev(6)
- << "\t" << cov_stdev(7) << "\t" << cov_stdev(8) << "\t" << cov_stdev(9)
- << "\t" << cov_stdev(10) << "\t" << cov_stdev(11) << "\t" << cov_stdev(12) << "\t" << cov_stdev(13) << "\t" << cov_stdev(14) << "\t" << cov_stdev(15) << std::endl;
- }
- }
-
- //trials to print all factorIMUs' residuals
- 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)
- {
- if(frm_ptr->isKey())
- {
- FactorBasePtrList fac_list = frm_ptr->getConstrainedByList();
- for(FactorBasePtr fac_ptr : fac_list)
- {
- if(fac_ptr->getTypeId() == FAC_IMU)
- {
- //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();
- ab1 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFrameOther())->getAccBias()->getState();
- wb1 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFrameOther())->getGyroBias()->getState();
-
- p2 = fac_ptr->getFeature()->getFrame()->getP()->getState();
- q2_vec = fac_ptr->getFeature()->getFrame()->getO()->getState();
- v2 = fac_ptr->getFeature()->getFrame()->getV()->getState();
- ab2 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFeature()->getFrame())->getAccBias()->getState();
- wb2 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFeature()->getFrame())->getGyroBias()->getState();
-
- std::static_pointer_cast<FactorIMU>(fac_ptr)->residual(p1, q1, v1, ab1, wb1, p2, q2, v2, ab2, wb2, IMU_residuals);
- std::cout << "IMU residuals : " << IMU_residuals.transpose() << std::endl;
- }
- }
- }
- }
-
- debug_results.close();
- WOLF_WARN("WARNING : DEBUG_RESULTS ACTIVATED - slows the process (writing results to result_debugs.dat file)")
-
- #endif
-
- return 0;
-
-}
-
-int _kbhit()
-{
- struct timeval tv;
- fd_set fds;
- tv.tv_sec = 0;
- tv.tv_usec = 0;
- FD_ZERO(&fds);
- FD_SET(STDIN_FILENO, &fds); //STDIN_FILENO is 0
- select(STDIN_FILENO+1, &fds, NULL, NULL, &tv);
- return FD_ISSET(STDIN_FILENO, &fds);
-}
diff --git a/demos/demo_processor_imu.cpp b/demos/demo_processor_imu.cpp
deleted file mode 100644
index 8f372c0d05ecae859d44670f2153a5ec574fc6e4..0000000000000000000000000000000000000000
--- a/demos/demo_processor_imu.cpp
+++ /dev/null
@@ -1,227 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-//Wolf
-#include "core/capture/capture_imu.h"
-#include "core/processor/processor_imu_3d.h"
-#include "core/sensor/sensor_imu.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-//#define DEBUG_RESULTS
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::cout << std::endl << "==================== processor IMU test ======================" << std::endl;
-
- //load files containing accelerometer and gyroscope data
- std::ifstream data_file_acc;
- std::ifstream data_file_gyro;
- const char * filename_acc;
- const char * filename_gyro;
-
- //prepare creation of file if DEBUG_RESULTS activated
-#ifdef DEBUG_RESULTS
- std::ofstream debug_results;
- debug_results.open("debug_results.dat");
- if(debug_results)
- debug_results << "%%TimeStamp\t"
- << "dp_x\t" << "dp_y\t" << "dp_z\t" << "dq_x\t" << "dq_y\t" << "dq_z\t" << "dq_w\t" << "dv_x\t" << "dv_y\t" << "dv_z\t"
- << "Dp_x\t" << "Dp_y\t" << "Dp_z\t" << "Dq_x\t" << "Dq_y\t" << "Dq_z\t" << "Dq_w\t" << "Dv_x\t" << "Dv_y\t" << "Dv_z\t"
- << "X_x\t" << "X_y\t" << "X_z\t" << "Xq_x\t" << "Xq_y\t" << "Xq_z\t" << "Xq_w\t" << "Xv_x\t" << "Xv_y\t" << "Xv_z\t" << std::endl;
-#endif
-
- if (argc < 3)
- {
- std::cout << "Missing input argument! : needs 2 arguments (path to accelerometer file and path to gyroscope data)." << std::endl;
- }
- else
- {
- filename_acc = argv[1];
- filename_gyro = argv[2];
- data_file_acc.open(filename_acc);
- data_file_gyro.open(filename_gyro);
- std::cout << "Acc file: " << filename_acc << std::endl;
- std::cout << "Gyro file: " << filename_gyro << std::endl;
-
- std::string dummy;
- getline(data_file_acc, dummy); getline(data_file_gyro, dummy);
-
- if(!data_file_acc.is_open() || !data_file_gyro.is_open()){
- std::cerr << "Failed to open data files... Exiting" << std::endl;
- return 1;
- }
- }
-
- // Wolf problem
- ProblemPtr problem_ptr_ = Problem::create("PQVBB 3D");
- 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;
- 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;
-
- // Get initial data
- data_file_acc >> mti_clock >> data[0] >> data[1] >> data[2];
- data_file_gyro >> tmp >> data[3] >> data[4] >> data[5];
- t.set(mti_clock * 0.0001); // clock in 0,1 ms ticks
-
- // Set the origin
- 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_->getMotionProvider()->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, Vector6d::Zero());
-
-// problem_ptr_->print();
-
- std::cout << "Main loop -----------" << std::endl;
-
- // main loop
- using namespace std;
- clock_t begin = clock();
- int n = 1;
- while(!data_file_acc.eof() && n < 5000){
- n++;
-
- // read new data
- data_file_acc >> mti_clock >> data[0] >> data[1] >> data[2];
- data_file_gyro >> tmp >> data[3] >> data[4] >> data[5];
- t.set(mti_clock * 0.0001); // clock in 0,1 ms ticks
-
-// data.setZero();
-// data(2) = 9.8;
-
- // assign data to capture
- imu_ptr->setData(data);
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sensor_ptr->process(imu_ptr);
-
-#ifdef DEBUG_RESULTS
-
- // --- print to screen ----
-
- std::cout << "Current data : " << std::fixed << std::setprecision(3) << std::setw(8) << std::right
- << data.transpose() << std::endl;
-
- std::cout << "Current delta: " << std::fixed << std::setprecision(3) << std::setw(8) << std::right
- << problem_ptr_->getMotionProvider()->getMotion().delta_.transpose() << std::endl;
-
- std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << problem_ptr_->getMotionProvider()->getMotion().delta_integr_.transpose() << std::endl;
-
- Eigen::VectorXd x = problem_ptr_->getMotionProvider()->getCurrentState();
-
- std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << x.head(10).transpose() << std::endl;
-
- std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (problem_ptr_->getMotionProvider()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
-
- std::cout << std::endl;
-
-//#ifdef DEBUG_RESULTS
- // ----- dump to file -----
-
- Eigen::VectorXd delta_debug;
- Eigen::VectorXd delta_integr_debug;
- Eigen::VectorXd x_debug;
- TimeStamp ts;
-
- delta_debug = problem_ptr_->getMotionProvider()->getMotion().delta_;
- delta_integr_debug = problem_ptr_->getMotionProvider()->getMotion().delta_integr_;
- x_debug = problem_ptr_->getMotionProvider()->getCurrentState();
- ts = problem_ptr_->getMotionProvider()->getBuffer().back().ts_;
-
- if(debug_results)
- debug_results << ts.get() << "\t" << delta_debug(0) << "\t" << delta_debug(1) << "\t" << delta_debug(2) << "\t" << delta_debug(3) << "\t" << delta_debug(4) << "\t"
- << delta_debug(5) << "\t" << delta_debug(6) << "\t" << delta_debug(7) << "\t" << delta_debug(8) << "\t" << delta_debug(9) << "\t"
- << delta_integr_debug(0) << "\t" << delta_integr_debug(1) << "\t" << delta_integr_debug(2) << "\t" << delta_integr_debug(3) << "\t" << delta_integr_debug(4) << "\t"
- << delta_integr_debug(5) << "\t" << delta_integr_debug(6) << "\t" << delta_integr_debug(7) << "\t" << delta_integr_debug(8) << "\t" << delta_integr_debug(9) << "\t"
- << x_debug(0) << "\t" << x_debug(1) << "\t" << x_debug(2) << "\t" << x_debug(3) << "\t" << x_debug(4) << "\t"
- << x_debug(5) << "\t" << x_debug(6) << "\t" << x_debug(7) << "\t" << x_debug(8) << "\t" << x_debug(9) << "\n";
-#endif
-
- }
- clock_t end = clock();
- double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
-
- // Final state
- std::cout << "\nIntegration results ----------------------------------------------------------------------------------------------" << std::endl;
- std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << x0.head(16).transpose() << std::endl;
- std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << problem_ptr_->getMotionProvider()->getMotion().delta_integr_.transpose() << std::endl;
- std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << problem_ptr_->getMotionProvider()->getCurrentState().head(16).transpose() << std::endl;
-// std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
-// << (problem_ptr_->getMotionProvider()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
-
- // Print statistics
- std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
- std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
-
-#ifdef DEBUG_RESULTS
- std::cout << "\t\tWARNING : DEBUG_RESULTS ACTIVATED - slows the process (writing results to result_debugs.dat file)" << std::endl;
- debug_results.close();
-#endif
-
- TimeStamp t0, tf;
- t0 = problem_ptr_->getMotionProvider()->getBuffer().front().ts_;
- tf = problem_ptr_->getMotionProvider()->getBuffer().back().ts_;
- int N = problem_ptr_->getMotionProvider()->getBuffer().size();
- std::cout << "t0 : " << t0.get() << " s" << std::endl;
- std::cout << "tf : " << tf.get() << " s" << std::endl;
- std::cout << "duration : " << tf-t0 << " s" << std::endl;
- std::cout << "N samples : " << N << std::endl;
- std::cout << "frequency : " << (N-1)/(tf-t0) << " Hz" << std::endl;
- std::cout << "CPU time : " << elapsed_secs << " s" << std::endl;
- std::cout << "s/integr : " << elapsed_secs/(N-1)*1e6 << " us" << std::endl;
- std::cout << "integr/s : " << (N-1)/elapsed_secs << " ips" << std::endl;
-
- // close data files
- data_file_acc.close(); // no impact on leaks
- data_file_gyro.close();
-
- return 0;
-}
diff --git a/demos/demo_processor_imu_jacobians.cpp b/demos/demo_processor_imu_jacobians.cpp
deleted file mode 100644
index a23a4cbdc87eaf57d3c3ff12fcceb1eb0efd54e9..0000000000000000000000000000000000000000
--- a/demos/demo_processor_imu_jacobians.cpp
+++ /dev/null
@@ -1,431 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-//Wolf
-#include "core/capture/capture_imu.h"
-#include "core/sensor/sensor_imu.h"
-#include <test/processor_imu_tester.h>
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-//#define DEBUG_RESULTS
-
-using namespace wolf;
-
-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)
-{
- using namespace wolf;
-
- std::cout << std::endl << "==================== processor IMU : Checking Jacobians ======================" << std::endl;
-
- TimeStamp t;
- 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::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::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_->getMotionProvider()->setOrigin(x0, t);
-
- //CaptureIMU* imu_ptr;
-
- ProcessorImu3dTester processor_imu;
- //processor_imu.setOrigin(x0, t);
- double ddelta_bias = 0.00000001;
- double dt = 0.001;
-
- //defining a random Delta to begin with (not to use Origin point)
- 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::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::Quaterniond> Delta0_quat(Delta0.data()+3);
- Delta0_quat = v2q(ang0);
- Delta0_quat.normalize();
- ang = q2v(Delta0_quat);
-
- std::cout << "\ninput Delta0 : " << Delta0 << std::endl;
- std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
-
- struct IMU_jac_bias bias_jac = processor_imu.finite_diff_ab(dt, data_, ddelta_bias, Delta0);
-
- 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 :
- Elements at place 0 are those not affected by the bias noise that we add (da_bx,..., dw_bx,... ).
- 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::Matrix3d dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dab, dDq_dwb;
-
- /*
- dDp_dab = [dDp_dab_x, dDp_dab_y, dDp_dab_z]
- dDp_dab_x = (dDp(ab_x + dab_x, ab_y, ab_z) - dDp(ab_x,ab_y,ab_z)) / dab_x
- dDp_dab_x = (dDp(ab_x, ab_y + dab_y, ab_z) - dDp(ab_x,ab_y,ab_z)) / dab_y
- dDp_dab_x = (dDp(ab_x, ab_y, ab_z + dab_z) - dDp(ab_x,ab_y,ab_z)) / dab_z
-
- similar for dDv_dab
- note dDp_dab_x, dDp_dab_y, dDp_dab_z, dDv_dab_x, dDv_dab_y, dDv_dab_z are 3x1 vectors !
-
- dDq_dab = 0_{3x3}
- dDq_dwb = [dDq_dwb_x, dDq_dwb_y, dDq_dwb_z]
- dDq_dwb_x = log( dR(wb).transpose() * dR(wb - dwb_x))/dwb_x
- = log( dR(wb).transpose() * exp((wx - wbx - dwb_x)dt, (wy - wby)dt, (wy - wby)dt))/dwb_x
- dDq_dwb_y = log( dR(wb).transpose() * dR(wb - dwb_y))/dwb_y
- dDq_dwb_z = log( dR(wb).transpose() * dR(wb + dwb_z))/dwb_z
-
- Note : dDq_dwb must be computed recursively ! So comparing the one returned by processor_imu and the numerical
- one will have no sense if we aredoing this from a random Delta. The Delta here should be the origin.
- dDq_dwb_ = dR.tr()*dDq_dwb - Jr(wdt)*dt
- Then at first step, dR.tr() = Id, dDq_dwb = 0_{3x3}, which boils down to dDq_dwb_ = Jr(wdt)*dt
- */
-
- std::cout << "\n input data : \n" << data_ << std::endl;
-
- 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;
-
- 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::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::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;
- }
-/* Delta1 = bias_jac.Deltas_noisy_vect_(3);
- Delta2 = bias_jac.Deltas_noisy_vect_(4);
- Delta3 = bias_jac.Deltas_noisy_vect_(5);
-
- if( (Delta1 == Delta2 || Delta1 == Delta3 ))
- std::cout << "\n problem" << std::endl;
- else
- std::cout << "\n no problem" << std::endl;
- if(Delta2 == Delta3)
- std::cout << "\n problem" << std::endl;
- else
- std::cout << "\n no problem" << std::endl;*/
-
- //Check the jacobians wrt to bias using finite difference
-
- if(dDp_dab.isApprox(bias_jac.dDp_dab_, wolf::Constants::EPS) )
- std::cout<< "dDp_dab_ jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDp_dab_ jacobian is not correct ..." << std::endl;
- std::cout << "dDp_dab : \n" << dDp_dab << "\n bias_jac.dDp_dab_ :\n" << bias_jac.dDp_dab_ << "\n" << std::endl;
- std::cout << "dDp_dab_a - dDp_dab_ : \n" << bias_jac.dDp_dab_ - dDp_dab << "\n" << std::endl;
- }
-
- if(dDv_dab.isApprox(bias_jac.dDv_dab_, wolf::Constants::EPS) )
- std::cout<< "dDv_dab_ jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDv_dab_ jacobian is not correct ..." << std::endl;
- std::cout << "dDv_dab_ : \n" << dDv_dab << "\n bias_jac.dDv_dab_ :\n" << bias_jac.dDv_dab_ << "\n" << std::endl;
- std::cout << "dDv_dab_a - dDv_dab_ : \n" << bias_jac.dDv_dab_ - dDv_dab << "\n" << std::endl;
- }
-
- if(dDp_dwb.isApprox(bias_jac.dDp_dwb_, wolf::Constants::EPS) )
- std::cout<< "dDp_dwb_ jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDp_dwb_ jacobian is not correct ..." << std::endl;
- std::cout << "dDp_dwb_ : \n" << dDp_dwb << "\n bias_jac.dDp_dwb_ :\n" << bias_jac.dDp_dwb_ << "\n" << std::endl;
- std::cout << "dDp_dwb_a - dDp_dwb_ : \n" << bias_jac.dDp_dwb_ - dDp_dwb << "\n" << std::endl;
- }
-
- if(dDv_dwb.isApprox(bias_jac.dDv_dwb_, wolf::Constants::EPS) )
- std::cout<< "dDv_dwb_ jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDv_dwb_ jacobian is not correct ..." << std::endl;
- std::cout << "dDv_dwb_ : \n" << dDv_dwb << "\n bias_jac.dDv_dwb_ :\n" << bias_jac.dDv_dwb_ << "\n" << std::endl;
- std::cout << "dDv_dwb_a - dDv_dwb_ : \n" << bias_jac.dDv_dwb_ - dDv_dwb << "\n" << std::endl;
- }
-
- if(dDq_dwb.isApprox(bias_jac.dDq_dwb_, wolf::Constants::EPS) )
- std::cout<< "dDq_dwb_ jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDq_dwb_ jacobian is not correct ..." << std::endl;
- std::cout << "dDq_dwb_ : \n" << dDq_dwb << "\n bias_jac.dDq_dwb_ :\n" << bias_jac.dDq_dwb_ << "\n" << std::endl;
- std::cout << "dDq_dwb_a - dDq_dwb_ : \n" << bias_jac.dDq_dwb_ - dDq_dwb << "\n" << std::endl;
- }
-
- //Check jacobians that are supposed to be Zeros just in case
- if(dDq_dab.isZero(wolf::Constants::EPS))
- std::cout<< "dDq_dab_ jacobian is correct (Zero) !" << std::endl;
- else
- std::cout<< "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
-
- /* Numerical method to check jacobians wrt noise
-
- dDp_dP = [dDp_dPx, dDp_dPy, dDp_dPz]
- dDp_dPx = ((P + dPx) - P)/dPx = Id
- dDp_dPy = ((P + dPy) - P)/dPy = Id
- dDp_dPz = ((P + dPz) - P)/dPz = Id
-
- dDp_dV = [dDp_dVx, dDp_dVy, dDp_dVz]
- dDp_dVx = ((V + dVx)*dt - V*dt)/dVx = Id*dt
- dDp_dVy = ((V + dVy)*dt - V*dt)/dVy = Id*dt
- dDp_dVz = ((V + dVz)*dt - V*dt)/dVz = Id*dt
-
- dDp_dO = [dDp_dOx, dDp_dOy, dDp_dOz]
- dDp_dOx = (( dR(Theta + dThetax)*dp ) - ( dR(Theta)*dp ))/dThetax
- = (( dR(Theta) * exp(dThetax,0,0)*dp ) - ( dR(Theta)*dp ))/dThetax
- dDp_dOy = (( dR(Theta) * exp(0,dThetay,0)*dp ) - ( dR(Theta)*dp ))/dThetay
- dDp_dOz = (( dR(Theta) * exp(0,0,dThetaz)*dp ) - ( dR(Theta)*dp ))/dThetaz
-
- dDv_dP = [dDv_dPx, dDv_dPy, dDv_dPz] = [0, 0, 0]
-
- dDv_dV = [dDv_dVx, dDv_dVy, dDv_dVz]
- dDv_dVx = ((V + dVx) - V)/dVx = Id
- dDv_dVy = ((V + dVy) - V)/dVy = Id
- dDv_dVz = ((V + dVz) - V)/dVz = Id
-
- dDv_dO = [dDv_dOx, dDv_dOy, dDv_dOz]
- dDv_dOx = (( dR(Theta + dThetax)*dv ) - ( dR(Theta)*dv ))/dThetax
- = (( dR(Theta) * exp(dThetax,0,0)*dv ) - ( dR(Theta)*dv ))/dThetax
- dDv_dOx = (( dR(Theta) * exp(0,dThetay,0)*dv ) - ( dR(Theta)*dv ))/dThetay
- dDv_dOz = (( dR(Theta) * exp(0,0,dThetaz)*dv ) - ( dR(Theta)*dv ))/dThetaz
-
- dDp_dp = [dDp_dpx, dDp_dpy, dDp_dpz]
- dDp_dpx = ( dR*(p + dpx) - dR*(p))/dpx
- dDp_dpy = ( dR*(p + dpy) - dR*(p))/dpy
- dDp_dpz = ( dR*(p + dpz) - dR*(p))/dpy
-
- dDp_dv = [dDp_dvx, dDp_dvy, dDp_dvz] = [0, 0, 0]
-
- dDp_do = [dDp_dox, dDp_doy, dDp_doz] = [0, 0, 0]
-
- dDv_dp = [dDv_dpx, dDv_dpy, dDv_dpz] = [0, 0, 0]
-
- dDv_dv = [dDv_dvx, dDv_dvy, dDv_dvz]
- dDv_dvx = ( dR*(v + dvx) - dR*(v))/dvx
- dDv_dvy = ( dR*(v + dvy) - dR*(v))/dvy
- dDv_dvz = ( dR*(v + dvz) - dR*(v))/dvz
-
- dDv_do = [dDv_dox, dDv_doy, dDv_doz] = [0, 0, 0]
-
- dDo_dp = dDo_dv = dDo_dP = dDo_dV = [0, 0, 0]
-
- dDo_dO = [dDo_dOx, dDo_dOy, dDo_dOz]
-
- dDo_dOx = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta+dThetax) * dr(theta) )/dThetax
- = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(dThetax,0,0)) * dr(theta) )/dThetax
- = log( (_Delta * _delta).transpose() * (_Delta_noisy * _delta))
- dDo_dOy = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(0,dThetay,0)) * dr(theta) )/dThetay
- dDo_dOz = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(0,0,dThetaz)) * dr(theta) )/dThetaz
-
- dDo_do = [dDo_dox, dDo_doy, dDo_doz]
-
- dDo_dox = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * dr(theta+dthetax) )/dthetax
- = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(dthetax,0,0)) )/dthetax
- = log( (_Delta * _delta).transpose() * (_Delta * _delta_noisy))
- dDo_doy = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,dthetay,0)) )/dthetay
- dDo_doz = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,0,dthetaz)) )/dthetaz
- */
-
- //taking care of noise now
- 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;
-
- struct IMU_jac_deltas deltas_jac = processor_imu.finite_diff_noise(dt, data_, Delta_noise, delta_noise, Delta0);
-
- /* reminder :
- jacobian_delta_preint_vect_ jacobian_delta_vect_
- 0: + 0, 0: + 0
- 1: +dPx, 2: +dPy, 3: +dPz 1: + dpx, 2: +dpy, 3: +dpz
- 4: +dOx, 5: +dOy, 6: +dOz 4: + dox, 5: +doy, 6: +doz
- 7: +dVx, 8: +dVy, 9: +dVz 7: + dvx, 8: +dvy, 9: +dvz
- */
-
- 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);
-
- for(int i = 0; i < 3; i++){
-
- //dDp_dPx = ((P + dPx) - P)/dPx
- dDp_dP.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i).head(3) - deltas_jac.Delta0_.head(3))/Delta_noise(i);
- //Dp_dVx = ((V + dVx)*dt - V*dt)/dVx
- dDp_dV.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i+6).tail(3)*dt - deltas_jac.Delta0_.tail(3)*dt)/Delta_noise(i+6);
- //dDp_dOx = (( dR(Theta) * exp(dThetax,0,0)*dp ) - ( dR(Theta)*dp ))/dThetax
- remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
- dDp_dO.block<3,1>(0,i) = ((Dq_noisy.matrix() * deltas_jac.delta0_.head(3)) - (Dq0.matrix()* deltas_jac.delta0_.head(3)))/Delta_noise(i+3);
-
- //dDv_dP = [0, 0, 0]
- //dDv_dVx = ((V + dVx) - V)/dVx
- dDv_dV.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i+6).tail(3) - deltas_jac.Delta0_.tail(3))/Delta_noise(i+6);
- //dDv_dOx = (( dR(Theta) * exp(dThetax,0,0)*dv ) - ( dR(Theta)*dv ))/dThetax
- dDv_dO.block<3,1>(0,i) = ((Dq_noisy.matrix() * deltas_jac.delta0_.tail(3)) - (Dq0.matrix()* deltas_jac.delta0_.tail(3)))/Delta_noise(i+3);
-
- //dDo_dP = dDo_dV = [0, 0, 0]
- //dDo_dOx = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(dThetax,0,0)) * dr(theta) )/dThetax
- dDo_dO.block<3,1>(0,i) = R2v( (Dq0.matrix() * dq0.matrix()).transpose() * (Dq_noisy.matrix() * dq0.matrix()) )/Delta_noise(i+3);
-
- //dDp_dpx = ( dR*(p + dpx) - dR*(p))/dpx
- dDp_dp.block<3,1>(0,i) = ( (Dq0.matrix() * deltas_jac.delta_noisy_vect_(i).head(3)) - (Dq0.matrix() * deltas_jac.delta0_.head(3)) )/delta_noise(i);
- //dDp_dv = dDp_do = [0, 0, 0]
-
- //dDv_dp = [0, 0, 0]
- //dDv_dvx = ( dR*(v + dvx) - dR*(v))/dvx
- dDv_dv.block<3,1>(0,i) = ( (Dq0 * deltas_jac.delta_noisy_vect_(i+6).tail(3)) - (Dq0 * deltas_jac.delta0_.tail(3)) )/delta_noise(i+6);
- //dDv_do = [0, 0, 0]
-
- //dDo_dp = dDo_dv = [0, 0, 0]
- //dDo_dox = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(dthetax,0,0)) )/dthetax
- dDo_do.block<3,1>(0,i) = R2v( (Dq0.matrix() * dq0.matrix()).transpose() * (Dq0.matrix() * dq_noisy.matrix()) )/Delta_noise(i+3);
- }
-
- /* jacobians wrt deltas have PVQ form :
- dDp_dP = deltas_jac.jacobian_delta_preint_.block(0,0,3,3); dDp_dV = deltas_jac.jacobian_delta_preint_.block(0,3,3,3); dDp_dO = deltas_jac.jacobian_delta_preint_.block(0,6,3,3);
- dDv_dP = deltas_jac.jacobian_delta_preint_.block(3,0,3,3); dDv_dV = deltas_jac.jacobian_delta_preint_.block(3,3,3,3); dDv_dO = deltas_jac.jacobian_delta_preint_.block(3,6,3,3);
- dDo_dP = deltas_jac.jacobian_delta_preint_.block(6,0,3,3); dDo_dV = deltas_jac.jacobian_delta_preint_.block(6,3,3,3); dDo_dO = deltas_jac.jacobian_delta_preint_.block(6,6,3,3);
- */
-
- if(dDp_dP.isApprox(deltas_jac.jacobian_delta_preint_.block(0,0,3,3), wolf::Constants::EPS) )
- std::cout<< "dDp_dP jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDp_dP jacobian is not correct ..." << std::endl;
- std::cout << "dDp_dP : \n" << dDp_dP << "\n deltas_jac.jacobian_delta_preint_.block(0,0,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) << "\n" << std::endl;
- std::cout << "dDp_dP_a - dDp_dP : \n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) - dDp_dP << "\n" << std::endl;
- }
-
- if(dDp_dV.isApprox(deltas_jac.jacobian_delta_preint_.block(0,6,3,3), wolf::Constants::EPS) )
- std::cout<< "dDp_dV jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDp_dV jacobian is not correct ..." << std::endl;
- std::cout << "dDp_dV : \n" << dDp_dV << "\n deltas_jac.jacobian_delta_preint_.block(0,6,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) << "\n" << std::endl;
- std::cout << "dDp_dV_a - dDp_dV : \n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) - dDp_dV << "\n" << std::endl;
- }
-
- if(dDp_dO.isApprox(deltas_jac.jacobian_delta_preint_.block(0,3,3,3), wolf::Constants::EPS) )
- std::cout<< "dDp_dO jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDp_dO jacobian is not correct ..." << std::endl;
- std::cout << "dDp_dO : \n" << dDp_dO << "\n deltas_jac.jacobian_delta_preint_.block(0,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) << "\n" << std::endl;
- std::cout << "dDp_dO_a - dDp_dO : \n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) - dDp_dO << "\n" << std::endl;
- }
-
- if(dDv_dV.isApprox(deltas_jac.jacobian_delta_preint_.block(6,6,3,3), wolf::Constants::EPS) )
- std::cout<< "dDv_dV jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDv_dV jacobian is not correct ..." << std::endl;
- std::cout << "dDv_dV : \n" << dDv_dV << "\n deltas_jac.jacobian_delta_preint_.block(6,6,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) << "\n" << std::endl;
- std::cout << "dDv_dV_a - dDv_dV : \n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) - dDv_dV << "\n" << std::endl;
- }
-
- if(dDv_dO.isApprox(deltas_jac.jacobian_delta_preint_.block(6,3,3,3), wolf::Constants::EPS) )
- std::cout<< "dDv_dO jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDv_dO jacobian is not correct ..." << std::endl;
- std::cout << "dDv_dO : \n" << dDv_dO << "\n deltas_jac.jacobian_delta_preint_.block(6,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(6,3,3,3) << "\n" << std::endl;
- std::cout << "dDv_dO_a - dDv_dO : \n" << deltas_jac.jacobian_delta_preint_.block(6,3,3,3) - dDv_dO << "\n" << std::endl;
- }
-
- if(dDo_dO.isApprox(deltas_jac.jacobian_delta_preint_.block(3,3,3,3), wolf::Constants::EPS) )
- std::cout<< "dDo_dO jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDo_dO jacobian is not correct ..." << std::endl;
- std::cout << "dDo_dO : \n" << dDo_dO << "\n deltas_jac.jacobian_delta_preint_.block(3,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) << "\n" << std::endl;
- std::cout << "dDo_dO_a - dDo_dO : \n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) - dDo_dO << "\n" << std::endl;
- }
-
- 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);
-
- if(dDp_dp.isApprox(dDp_dp_a, wolf::Constants::EPS) )
- std::cout<< "dDp_dp jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDp_dp jacobian is not correct ..." << std::endl;
- std::cout << "dDp_dp : \n" << dDv_dp << "\n dDp_dp_a :\n" << dDp_dp_a << "\n" << std::endl;
- std::cout << "dDp_dp_a - dDp_dp : \n" << dDp_dp_a - dDv_dp << "\n" << std::endl;
- }
-
- if(dDv_dv.isApprox(dDv_dv_a, wolf::Constants::EPS) )
- std::cout<< "dDv_dv jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDv_dv jacobian is not correct ..." << std::endl;
- std::cout << "dDv_dv : \n" << dDv_dv << "\n dDv_dv_a :\n" << dDv_dv_a << "\n" << std::endl;
- std::cout << "dDv_dv_a - dDv_dv : \n" << dDv_dv_a - dDv_dv << "\n" << std::endl;
- }
-
- if(dDo_do.isApprox(dDo_do_a, wolf::Constants::EPS) )
- std::cout<< "dDo_do jacobian is correct !" << std::endl;
- else{
- std::cout<< "\t\tdDo_do jacobian is not correct ..." << std::endl;
- std::cout << "dDo_do : \n" << dDo_do << "\n dDo_do_a :\n" << dDo_do_a << "\n" << std::endl;
- std::cout << "dDo_do_a - dDo_do : \n" << dDo_do_a - dDo_do << "\n" << std::endl;
- }
-
- return 0;
-}
-
-using namespace wolf;
-
-void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaterniond>& _Dq0, Eigen::Map<Eigen::Quaterniond>& _dq0){
-
- 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::Quaterniond>& _Dq, Eigen::Map<Eigen::Quaterniond>& _dq, const int& place ){
-
- assert(place < _jac_delta.Delta_noisy_vect_.size());
- 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/demos/processor_imu.yaml b/demos/processor_imu.yaml
deleted file mode 100644
index 1afbb00a6ab5112cbad1555098985ae1f0687f8b..0000000000000000000000000000000000000000
--- a/demos/processor_imu.yaml
+++ /dev/null
@@ -1,16 +0,0 @@
-type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-
-time_tolerance: 0.0025 # Time tolerance for joining KFs
-unmeasured_perturbation_std: 0.00001
-
-keyframe_vote:
- voting_active: false
- max_time_span: 2.0 # seconds
- max_buff_length: 20000 # motion deltas
- dist_traveled: 2.0 # meters
- angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
-
-bootstrap:
- enable: false
- method: "G"
- averaging_length: 0.10 # seconds
\ No newline at end of file
diff --git a/demos/processor_imu2d.yaml b/demos/processor_imu2d.yaml
deleted file mode 100644
index 6d8d756c415d5d9db4b32ab80cf1ea06f72736fd..0000000000000000000000000000000000000000
--- a/demos/processor_imu2d.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-type: "ProcessorImu2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-
-time_tolerance: 0.0025 # Time tolerance for joining KFs
-unmeasured_perturbation_std: 0.00001
-
-keyframe_vote:
- voting_active: false
- voting_aux_active: false
- max_time_span: 2.0 # seconds
- max_buff_length: 20000 # motion deltas
- dist_traveled: 2.0 # meters
- angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
diff --git a/demos/processor_imu_bootstrap.yaml b/demos/processor_imu_bootstrap.yaml
deleted file mode 100644
index 3a6c6c9587ab23aeb66ae95495c8054be46e0c96..0000000000000000000000000000000000000000
--- a/demos/processor_imu_bootstrap.yaml
+++ /dev/null
@@ -1,17 +0,0 @@
-type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-
-time_tolerance: 0.0025 # Time tolerance for joining KFs
-unmeasured_perturbation_std: 0.00001
-
-keyframe_vote:
- voting_active: false
- max_time_span: 2.0 # seconds
- max_buff_length: 20000 # motion deltas
- dist_traveled: 2.0 # meters
- angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
-
-bootstrap:
- enable: true
- method: "G" # methods: "G", "STATIC" or "V0_G"
- averaging_length: 0.10 # seconds
- keyframe_provider_processor_name: "processor_other_name" # Not yet implemented
\ No newline at end of file
diff --git a/demos/processor_odom_3d.yaml b/demos/processor_odom_3d.yaml
deleted file mode 100644
index 9ee3321a500e229837b29fb1d03f366f71223f19..0000000000000000000000000000000000000000
--- a/demos/processor_odom_3d.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-type: "ProcessorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-
-time_tolerance: 0.01 # seconds
-
-keyframe_vote:
- voting_active: false
- max_time_span: 0.2 # seconds
- max_buff_length: 10 # motion deltas
- dist_traveled: 0.5 # meters
- angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
-
-unmeasured_perturbation_std: 0.001
diff --git a/demos/sensor_imu.yaml b/demos/sensor_imu.yaml
deleted file mode 100644
index 38a3df944cc7e8a7021e0bfe418fb5c79b324b2f..0000000000000000000000000000000000000000
--- a/demos/sensor_imu.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-type: "SensorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-
-motion_variances:
- a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2
- w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
- ab_initial_stdev: 0.800 # m/s2 - initial bias
- wb_initial_stdev: 0.350 # rad/sec - initial bias
- ab_rate_stdev: 0.1 # m/s2/sqrt(s)
- wb_rate_stdev: 0.0400 # rad/s/sqrt(s)
-
-dynamic_imu_bias: true
\ No newline at end of file
diff --git a/demos/sensor_imu2d.yaml b/demos/sensor_imu2d.yaml
deleted file mode 100644
index 2db3ed90f61e62e596fab85d5d9f1d6c85d89a65..0000000000000000000000000000000000000000
--- a/demos/sensor_imu2d.yaml
+++ /dev/null
@@ -1,18 +0,0 @@
-states:
- P:
- state: [0, 0]
- mode: fix
- dynamic: false
- O:
- state: [0]
- mode: fix
- dynamic: false
- I:
- state: [0, 0, 0]
- mode: factor
- dynamic: true
- noise_std: [0.800, 0.800, 0.350] # Previously named ab_initial_stdev [m/s²] and wb_initial_stdev [rad/sec]
- drift_std: [0.1, 0.1, 0.04] # Previously named ab_rate_stdev [m/s²/sqrt(s)] and wb_rate_stdev [rad/s/sqrt(s)]
-
-a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s²
-w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
\ No newline at end of file
diff --git a/demos/sensor_imu2d_with_gravity.yaml b/demos/sensor_imu2d_with_gravity.yaml
deleted file mode 100644
index eb30116dd303e9fc0d497ff62e2102d66adeca7d..0000000000000000000000000000000000000000
--- a/demos/sensor_imu2d_with_gravity.yaml
+++ /dev/null
@@ -1,10 +0,0 @@
-type: "SensorImu2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-
-motion_variances:
- a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2
- w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
- ab_initial_stdev: 0.800 # m/s2 - initial bias
- wb_initial_stdev: 0.350 # rad/sec - initial bias
- ab_rate_stdev: 0.1 # m/s2/sqrt(s)
- wb_rate_stdev: 0.0400 # rad/s/sqrt(s)
- orthogonal_gravity: false
diff --git a/demos/sensor_odom_3d.yaml b/demos/sensor_odom_3d.yaml
deleted file mode 100644
index 58db1c088fbc80339a78ba815fddbaf69674d3b6..0000000000000000000000000000000000000000
--- a/demos/sensor_odom_3d.yaml
+++ /dev/null
@@ -1,7 +0,0 @@
-type: "SensorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-
-k_disp_to_disp: 0.02 # m^2 / m
-k_disp_to_rot: 0.02 # rad^2 / m
-k_rot_to_rot: 0.01 # rad^2 / rad
-min_disp_var: 0.01 # m^2
-min_rot_var: 0.01 # rad^2
diff --git a/include/imu/processor/processor_imu_3d.h b/include/imu/processor/processor_imu_3d.h
index 74a06972d50ec7b70c3559387fdfdbafbc08cf05..e28c98c92e1e395f1291182c1e37439a3416c620 100644
--- a/include/imu/processor/processor_imu_3d.h
+++ b/include/imu/processor/processor_imu_3d.h
@@ -91,9 +91,13 @@ class ProcessorImu3d : public ProcessorMotion
virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
public:
+ /** \brief Disable bootstrapping process
+ */
+ void bootstrapDisable();
+
/** \brief Enable bootstrapping process
*/
- void bootstrapEnable(bool _bootstrap_enable = true);
+ void bootstrapEnable(BootstrapMethod _method, const double& _averaging_length);
protected:
/** \brief Bootstrap the IMU initial conditions
diff --git a/src/processor/processor_imu_3d.cpp b/src/processor/processor_imu_3d.cpp
index 8fdd36ffd4d0f64d727a37bda48fc04ea149fdca..0461f1d4f513ae76a447c8ef7f47bab605b5ed8e 100644
--- a/src/processor/processor_imu_3d.cpp
+++ b/src/processor/processor_imu_3d.cpp
@@ -41,7 +41,6 @@ ProcessorImu3d::ProcessorImu3d(const YAML::Node& _params)
6,
_params)
{
- WOLF_DEBUG("in ProcessorImu3d");
bootstrapping_ = _params["bootstrap"]["enabled"].as<bool>();
bootstrap_factor_list_.clear();
@@ -134,8 +133,7 @@ void ProcessorImu3d::setCalibration(const CaptureBasePtr _capture, const VectorX
void ProcessorImu3d::configure(SensorBasePtr _sensor)
{
- if (_sensor->isStateBlockDynamic('I'))
- imu_drift_cov_ = _sensor->getDriftCov('I');
+ if (_sensor->isStateBlockDynamic('I')) imu_drift_cov_ = _sensor->getDriftCov('I');
}
void ProcessorImu3d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
@@ -152,7 +150,8 @@ void ProcessorImu3d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, C
CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
- auto fac_imu = FactorBase::emplace<FactorImu3d>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), applyLossFunction());
+ auto fac_imu =
+ FactorBase::emplace<FactorImu3d>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), applyLossFunction());
if (bootstrapping_)
{
@@ -346,14 +345,14 @@ void ProcessorImu3d::bootstrap()
VectorXd delta_int = bootstrapDelta();
// compute local g and transformation to global g
- double dt = t_current - first_capture->getTimeStamp(); //
- const auto& dv = delta_int.segment(7, 3); //
- Vector3d g_l = -(q_l_s * dv / dt); // See eq. (20)
- const auto& g_w = gravity(); //
- const auto& p_w_l = Vector3d::Zero(); // will pivot around the origin
- Quaterniond q_w_l = Quaterniond::FromTwoVectors(g_l, g_w); //
- transfo_w_l.at('P') = p_w_l; //
- transfo_w_l.at('O') = q_w_l.coeffs(); //
+ double dt = t_current - first_capture->getTimeStamp(); //
+ const auto& dv = delta_int.segment(7, 3); //
+ Vector3d g_l = -(q_l_s * dv / dt); // See eq. (20)
+ const auto& g_w = gravity(); //
+ const auto& p_w_l = Vector3d::Zero(); // will pivot around the origin
+ Quaterniond q_w_l = Quaterniond::FromTwoVectors(g_l, g_w); //
+ transfo_w_l['P'] = p_w_l; //
+ transfo_w_l['O'] = q_w_l.coeffs(); //
// Transform problem to new reference
getProblem()->transform(transfo_w_l);
@@ -386,6 +385,8 @@ void ProcessorImu3d::bootstrap()
// Implementation of G strategy.
if (t_current - first_capture->getTimeStamp() >= bootstrap_averaging_length_)
{
+ WOLF_DEBUG("Computing bootstrap method G, after averaging length ", bootstrap_averaging_length_);
+
// get initial IMU frame 's' expressed in local world frame 'l'
Quaterniond q_l_r;
q_l_r.coeffs() = first_capture->getFrame()->getO()->getState();
@@ -397,14 +398,14 @@ void ProcessorImu3d::bootstrap()
VectorXd delta_int = bootstrapDelta();
// compute local g and transformation to global g
- double dt = t_current - first_capture->getTimeStamp(); //
- const auto& dv_l = delta_int.segment(7, 3); //
- Vector3d g_l = -(q_l_s * dv_l / dt); // See eq. (20)
- const auto& g_w = gravity(); //
- const auto& p_w_l = Vector3d::Zero(); // will pivot around the origin
- Quaterniond q_w_l = Quaterniond::FromTwoVectors(g_l, g_w); //
- transfo_w_l.at('P') = p_w_l; //
- transfo_w_l.at('O') = q_w_l.coeffs(); //
+ double dt = t_current - first_capture->getTimeStamp(); //
+ const auto& dv_l = delta_int.segment(7, 3); //
+ Vector3d g_l = -(q_l_s * dv_l / dt); // See eq. (20)
+ const auto& g_w = gravity(); //
+ const auto& p_w_l = Vector3d::Zero(); // will pivot around the origin
+ Quaterniond q_w_l = Quaterniond::FromTwoVectors(g_l, g_w); //
+ transfo_w_l['P'] = p_w_l; //
+ transfo_w_l['O'] = q_w_l.coeffs(); //
// Transform problem to new reference
getProblem()->transform(transfo_w_l);
@@ -444,10 +445,18 @@ void ProcessorImu3d::bootstrap()
}
}
-void ProcessorImu3d::bootstrapEnable(bool _bootstrap_enable)
+void ProcessorImu3d::bootstrapDisable()
+{
+ bootstrapping_ = false;
+}
+
+void ProcessorImu3d::bootstrapEnable(BootstrapMethod _method, const double& _averaging_length)
{
- bootstrapping_ = _bootstrap_enable;
-};
+ bootstrapping_ = true;
+ bootstrap_method_ = _method;
+ bootstrap_averaging_length_ = _averaging_length;
+ bootstrap_factor_list_.clear();
+}
CaptureBasePtr ProcessorImu3d::bootstrapOrigin() const
{
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 0ab211adef0f8fd3ebddf0af853b3dc851f3e920..019c2209000a152d666b730b529fd5ea78b8bde8 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,6 +1,14 @@
# Retrieve googletest from github & compile
add_subdirectory(gtest)
+# Add here the source (.cpp) files in dummy folder
+set(SRC_DUMMY
+ dummy/load_dummy.cpp
+ dummy/processor_imu_2d_tester.cpp
+ dummy/processor_imu_3d_tester.cpp
+ )
+add_library(dummy SHARED ${SRC_DUMMY})
+target_link_libraries(dummy ${PLUGIN_NAME})
############# USE THIS TEST AS AN EXAMPLE #################
# #
@@ -25,6 +33,8 @@ wolf_add_gtest(gtest_imu_2d_static_init gtest_imu_2d_static_init.cpp)
wolf_add_gtest(gtest_imu_2d_tools gtest_imu_2d_tools.cpp)
+wolf_add_gtest(gtest_imu_3d_bootstrap gtest_imu_3d_bootstrap.cpp)
+
wolf_add_gtest(gtest_imu_3d_mocap_fusion gtest_imu_3d_mocap_fusion.cpp)
wolf_add_gtest(gtest_imu_3d_static_init gtest_imu_3d_static_init.cpp)
@@ -34,6 +44,7 @@ wolf_add_gtest(gtest_imu_3d_tools gtest_imu_3d_tools.cpp)
wolf_add_gtest(gtest_imu_3d gtest_imu_3d.cpp)
wolf_add_gtest(gtest_processor_imu_3d_jacobians gtest_processor_imu_3d_jacobians.cpp)
+target_link_libraries(gtest_processor_imu_3d_jacobians PUBLIC dummy)
wolf_add_gtest(gtest_processor_imu_3d gtest_processor_imu_3d.cpp)
@@ -44,5 +55,6 @@ wolf_add_gtest(gtest_processor_imu_2d gtest_processor_imu_2d.cpp)
wolf_add_gtest(gtest_processor_motion_intrinsics_update gtest_processor_motion_intrinsics_update.cpp)
wolf_add_gtest(gtest_schema gtest_schema.cpp)
+target_link_libraries(gtest_schema PUBLIC dummy)
wolf_add_gtest(gtest_sensor_compass gtest_sensor_compass.cpp)
diff --git a/test/dummy/load_dummy.cpp b/test/dummy/load_dummy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3aac5f6120338772769e7e21a7dc0bd1d4923cec
--- /dev/null
+++ b/test/dummy/load_dummy.cpp
@@ -0,0 +1,35 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include "load_dummy.h"
+
+namespace wolf
+{
+bool LoadDummy::aux_var = true;
+
+// LoadDummy::LoadDummy()
+// {
+// }
+
+// LoadDummy::~LoadDummy()
+// {
+// }
+
+} // namespace wolf
diff --git a/test/dummy/load_dummy.h b/test/dummy/load_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..c64b2e3a0969de3c8be533a41bea56ccd572b97f
--- /dev/null
+++ b/test/dummy/load_dummy.h
@@ -0,0 +1,52 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include "core/common/factory.h"
+
+namespace wolf
+{
+// This class is just to easily force loading of the .so in tests
+struct LoadDummy
+{
+ static bool aux_var;
+};
+
+#ifdef __GNUC__
+#define WOLF_UNUSED __attribute__((used))
+#elif defined _MSC_VER
+#pragma warning(disable : Cxxxxx)
+#define WOLF_UNUSED
+#elif defined(__LCLINT__)
+#define WOLF_UNUSED /*@unused@*/
+#elif defined(__cplusplus)
+#define WOLF_UNUSED
+#else
+#define UNUSED(x) x
+#endif
+
+#define WOLF_LOAD_DUMMY \
+ namespace \
+ { \
+ const bool WOLF_UNUSED aux_var_dummy = wolf::LoadDummy::aux_var; \
+ }
+
+} // namespace wolf
diff --git a/test/processor_imu_2d_tester.cpp b/test/dummy/processor_imu_2d_tester.cpp
similarity index 100%
rename from test/processor_imu_2d_tester.cpp
rename to test/dummy/processor_imu_2d_tester.cpp
diff --git a/test/processor_imu_2d_tester.h b/test/dummy/processor_imu_2d_tester.h
similarity index 100%
rename from test/processor_imu_2d_tester.h
rename to test/dummy/processor_imu_2d_tester.h
diff --git a/test/processor_imu_3d_tester.cpp b/test/dummy/processor_imu_3d_tester.cpp
similarity index 100%
rename from test/processor_imu_3d_tester.cpp
rename to test/dummy/processor_imu_3d_tester.cpp
diff --git a/test/processor_imu_3d_tester.h b/test/dummy/processor_imu_3d_tester.h
similarity index 100%
rename from test/processor_imu_3d_tester.h
rename to test/dummy/processor_imu_3d_tester.h
diff --git a/test/gtest_imu_3d.cpp b/test/gtest_imu_3d.cpp
index 4212b159f07f563795abba33e469b8872269a9a2..3208626ec85d87c83a8209d1d79cd358a6984033 100644
--- a/test/gtest_imu_3d.cpp
+++ b/test/gtest_imu_3d.cpp
@@ -120,6 +120,8 @@ class Process_Factor_Imu : public testing::Test
solver = FactorySolverFile::create(
"SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
+ // Time
+ t0 = 0;
dt = 0.01;
// Some initializations
@@ -569,9 +571,13 @@ class Process_Factor_Imu : public testing::Test
std::string runAll(SolverManager::ReportVerbosity verbose)
{
configureAll();
+ WOLF_DEBUG("configured")
integrateAll();
+ WOLF_DEBUG("integrated all")
buildProblem();
+ WOLF_DEBUG("built problem")
std::string report = solveProblem(verbose);
+ WOLF_DEBUG("solved")
return report;
}
@@ -659,6 +665,7 @@ class Process_Factor_Imu_ODO : public Process_Factor_Imu
{
// ===================================== Imu
Process_Factor_Imu::configureAll();
+ WOLF_DEBUG("Process_Factor_Imu configured");
// ===================================== ODO
processor_odo->setOrigin(KF_0);
@@ -720,14 +727,8 @@ class Process_Factor_Imu_ODO : public Process_Factor_Imu
TEST_F(Process_Factor_Imu, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -750,11 +751,9 @@ TEST_F(Process_Factor_Imu, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
p1_fixed = true;
q1_fixed = true;
v1_fixed = true;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -767,14 +766,8 @@ TEST_F(Process_Factor_Imu, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
TEST_F(Process_Factor_Imu, test_capture) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -797,11 +790,9 @@ TEST_F(Process_Factor_Imu, test_capture) // F_ixed___e_stimated
p1_fixed = true;
q1_fixed = true;
v1_fixed = true;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
Eigen::Vector6d initial_bias((Eigen::Vector6d() << .002, .0007, -.001, .003, -.002, .001).finished());
@@ -819,14 +810,8 @@ TEST_F(Process_Factor_Imu, test_capture) // F_ixed___e_stimated
TEST_F(Process_Factor_Imu, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -849,11 +834,9 @@ TEST_F(Process_Factor_Imu, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
p1_fixed = true;
q1_fixed = true;
v1_fixed = true;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -866,14 +849,8 @@ TEST_F(Process_Factor_Imu, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
TEST_F(Process_Factor_Imu, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -896,11 +873,9 @@ TEST_F(Process_Factor_Imu, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
p1_fixed = true;
q1_fixed = true;
v1_fixed = false;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -913,14 +888,8 @@ TEST_F(Process_Factor_Imu, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
TEST_F(Process_Factor_Imu, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -943,11 +912,9 @@ TEST_F(Process_Factor_Imu, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
p1_fixed = true;
q1_fixed = true;
v1_fixed = true;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -960,14 +927,8 @@ TEST_F(Process_Factor_Imu, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -990,11 +951,9 @@ TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
p1_fixed = true;
q1_fixed = true;
v1_fixed = false;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1007,14 +966,8 @@ TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -1037,11 +990,9 @@ TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
p1_fixed = false;
q1_fixed = true;
v1_fixed = true;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1054,14 +1005,8 @@ TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
TEST_F(Process_Factor_Imu, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -1084,11 +1029,9 @@ TEST_F(Process_Factor_Imu, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed_
p1_fixed = true;
q1_fixed = true;
v1_fixed = true;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1101,14 +1044,8 @@ TEST_F(Process_Factor_Imu, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed_
TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -1131,11 +1068,9 @@ TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed__
p1_fixed = true;
q1_fixed = true;
v1_fixed = true;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1148,14 +1083,8 @@ TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed__
TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -1178,11 +1107,9 @@ TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed__
p1_fixed = true;
q1_fixed = true;
v1_fixed = false;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1195,14 +1122,8 @@ TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed__
TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -1225,11 +1146,9 @@ TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed__
p1_fixed = true;
q1_fixed = false;
v1_fixed = false;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1242,14 +1161,8 @@ TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed__
TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -1272,11 +1185,9 @@ TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed__
p1_fixed = false;
q1_fixed = true;
v1_fixed = false;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1289,14 +1200,8 @@ TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed__
TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -1319,11 +1224,9 @@ TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed__
p1_fixed = false;
q1_fixed = false;
v1_fixed = false;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1336,14 +1239,8 @@ TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed__
TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -1366,11 +1263,9 @@ TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stima
p1_fixed = false;
q1_fixed = false;
v1_fixed = true;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1383,14 +1278,8 @@ TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stima
TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -1413,11 +1302,9 @@ TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stima
p1_fixed = false;
q1_fixed = false;
v1_fixed = false;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1430,14 +1317,8 @@ TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stima
TEST_F(Process_Factor_Imu_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -1460,11 +1341,9 @@ TEST_F(Process_Factor_Imu_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimate
p1_fixed = false;
q1_fixed = false;
v1_fixed = false;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1477,14 +1356,8 @@ TEST_F(Process_Factor_Imu_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimate
TEST_F(Process_Factor_Imu_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 50;
// ---------- initial pose
@@ -1507,11 +1380,9 @@ TEST_F(Process_Factor_Imu_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimate
p1_fixed = false;
q1_fixed = false;
v1_fixed = true;
- //
+
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
std::string report = runAll(SolverManager::ReportVerbosity::BRIEF);
@@ -1524,14 +1395,8 @@ TEST_F(Process_Factor_Imu_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimate
TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
{
// ================================================================================================================
- // //
// ==================================== INITIAL CONDITIONS -- USER OPTIONS ========================================
- // //
// ================================================================================================================
- // //
- //
- // ---------- time
- t0 = 0;
num_integrations = 25;
// ---------- initial pose
@@ -1554,11 +1419,10 @@ TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) //
p1_fixed = false;
q1_fixed = false;
v1_fixed = true;
- //
+
+ WOLF_INFO("TEST initial conditions set");
// ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE ===============================
- // //
// ================================================================================================================
- // //
// ===================================== RUN ALL
configureAll();
@@ -1616,48 +1480,11 @@ TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) //
ASSERT_MATRIX_APPROX(Trj_x_optim_prc.rightCols(1), x1_exact, 1e-6);
}
-TEST_F(Process_Factor_Imu, bootstrap)
-{
- processor_imu->setVotingActive(true);
- processor_imu->setMaxTimeSpan(0.04);
- processor_imu->bootstrapEnable(true);
-
- // FIXME: error maybe due to enable bootstrap before problem->applyPriorOptions()
-
- // auto KF0 = problem->emplaceFrame(0.0);
- // problem->keyFrameCallback(KF0, nullptr);
- problem->print(4, 0, 1, 0);
-
- // Vector6d data(0,0,9.806, 0,0,0); // gravity on Z
- // Vector6d data(0.0, 9.806, 0.0, 0.0, 0.0, 0.0); // gravity on Y
- Vector6d data;
- data << 0.0, 9.806, 0.0, 0.0, 0.0, 0.0; // gravity on Y
-
- capture_imu = std::make_shared<CaptureImu>(0.0, sensor_imu, data, Matrix6d::Identity());
-
- for (t = 0; t < 0.14; t += dt)
- {
- capture_imu->setTimeStamp(t);
- capture_imu->process();
- }
-
- problem->print(4, 0, 1, 1);
-
- Quaterniond qref(AngleAxisd(M_PI / 2, Vector3d::UnitX())); // turn of +90deg over X
- for (auto pair_ts_frame : problem->getTrajectory()->getFrameMap())
- {
- ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("P"), Vector3d::Zero(), 1e-10);
- ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("O"), qref.coeffs(), 1e-10);
- ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("V"), Vector3d::Zero(), 1e-10);
- }
-}
-
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
- testing::GTEST_FLAG(filter) = "Process_Factor_Imu.bootstrap";
// testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.*";
- // testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
+ testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_imu_3d_bootstrap.cpp b/test/gtest_imu_3d_bootstrap.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..95de61f08899c090e6bcb1e9ff1b8b6b1456f8ae
--- /dev/null
+++ b/test/gtest_imu_3d_bootstrap.cpp
@@ -0,0 +1,121 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include "imu/common/imu.h"
+#include "imu/processor/processor_imu_3d.h"
+#include "imu/sensor/sensor_imu.h"
+#include "imu/math/imu_3d_tools.h"
+
+#include <core/utils/utils_gtest.h>
+
+using namespace Eigen;
+using namespace wolf;
+
+std::string imu_dir = _WOLF_IMU_CODE_DIR;
+
+class Imu3dBootstrap : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SensorImu3dPtr sensor_imu;
+ ProcessorImu3dPtr processor_imu;
+
+ TimeStamp t0;
+ double dt;
+
+ void SetUp() override
+ {
+ // problem
+ problem = Problem::autoSetup(imu_dir + "/test/yaml/problem_imu_3d_bootstrap.yaml", {imu_dir});
+
+ // sensor imu
+ sensor_imu = std::static_pointer_cast<SensorImu3d>(problem->findSensor("cool sensor imu"));
+
+ // processor imu
+ processor_imu = std::static_pointer_cast<ProcessorImu3d>(sensor_imu->getProcessorList().front());
+
+ // Time
+ t0 = 0;
+ dt = 0.01;
+ }
+};
+
+TEST_F(Imu3dBootstrap, bootstrap_static)
+{
+ // set bootstrap static
+ processor_imu->bootstrapEnable(BootstrapMethod::BOOTSTRAP_STATIC, 0.9);
+
+ // capture
+ Vector6d data;
+ data << 0.0, 9.806, 0.0, 0.0, 0.0, 0.0; // gravity on Y
+ // data << 0.0, 0.0, 9.806, 0.0, 0.0, 0.0; // gravity on Z
+ auto capture_imu = std::make_shared<CaptureImu>(t0, sensor_imu, data, Matrix6d::Identity());
+
+ for (auto t = t0; t < 1; t += dt)
+ {
+ capture_imu->setTimeStamp(t);
+ capture_imu->process();
+ }
+ problem->print(4, 1, 1, 1);
+
+ // check correct frames states
+ Quaterniond qref(AngleAxisd(M_PI / 2, Vector3d::UnitX())); // turn of +90deg over X
+ for (auto pair_ts_frame : problem->getTrajectory()->getFrameMap())
+ {
+ ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("P"), Vector3d::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("O"), qref.coeffs(), 1e-10);
+ ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("V"), Vector3d::Zero(), 1e-10);
+ }
+}
+
+TEST_F(Imu3dBootstrap, bootstrap_G)
+{
+ // set bootstrap G
+ processor_imu->bootstrapEnable(BootstrapMethod::BOOTSTRAP_G, 0.9);
+
+ // capture
+ Vector6d data;
+ data << 0.0, 9.806, 0.0, 0.0, 0.0, 0.0; // gravity on Y
+ // data << 0.0, 0.0, 9.806, 0.0, 0.0, 0.0; // gravity on Z
+ auto capture_imu = std::make_shared<CaptureImu>(t0, sensor_imu, data, Matrix6d::Identity());
+
+ for (auto t = t0; t < 1; t += dt)
+ {
+ capture_imu->setTimeStamp(t);
+ capture_imu->process();
+ }
+ problem->print(4, 1, 1, 1);
+
+ // check correct frames states
+ Quaterniond qref(AngleAxisd(M_PI / 2, Vector3d::UnitX())); // turn of +90deg over X
+ for (auto pair_ts_frame : problem->getTrajectory()->getFrameMap())
+ {
+ ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("P"), Vector3d::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("O"), qref.coeffs(), 1e-10);
+ ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("V"), Vector3d::Zero(), 1e-10);
+ }
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ // testing::GTEST_FLAG(filter) = "Imu3dBootstrap";
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/gtest_processor_imu_3d_jacobians.cpp b/test/gtest_processor_imu_3d_jacobians.cpp
index e4575c131cde13c7ab111c5aad6e401e059fcdbd..ec946665194c2d4d6c175cf5eee128adbfcd0f35 100644
--- a/test/gtest_processor_imu_3d_jacobians.cpp
+++ b/test/gtest_processor_imu_3d_jacobians.cpp
@@ -21,7 +21,7 @@
// Wolf
#include "imu/capture/capture_imu.h"
#include "imu/sensor/sensor_imu.h"
-#include "processor_imu_3d_tester.h"
+#include "dummy/processor_imu_3d_tester.h"
#include "core/common/wolf.h"
#include "core/problem/problem.h"
#include "core/state_block/state_block.h"
diff --git a/test/gtest_schema.cpp b/test/gtest_schema.cpp
index d125b6a4a2b2eb299e70c82e8f32741819a4fa87..44501c36c59a79fc396a774d38a8fd2968cc5627 100644
--- a/test/gtest_schema.cpp
+++ b/test/gtest_schema.cpp
@@ -39,6 +39,10 @@ std::string imu_dir = _WOLF_IMU_CODE_DIR;
WOLF_LOAD_CORE;
WOLF_LOAD_IMU;
+// Force load .so of dummy library
+#include "dummy/load_dummy.h"
+WOLF_LOAD_DUMMY;
+
bool existsSchema(std::string name_schema)
{
// Check extension
diff --git a/test/yaml/imu_2d_static_init/processor_imu_2d.yaml b/test/yaml/imu_2d_static_init/processor_imu_2d.yaml
index 0bc741fcab19bed2f140d0bc8f32a211c651d12f..68b453002af649674b7643d01e98396488f54312 100644
--- a/test/yaml/imu_2d_static_init/processor_imu_2d.yaml
+++ b/test/yaml/imu_2d_static_init/processor_imu_2d.yaml
@@ -13,11 +13,6 @@ keyframe_vote:
max_buff_length: 1000000000 # motion deltas
max_dist_traveled: 100000000000 # meters
max_angle_turned: 10000000000 # radians (1 rad approx 57 deg, approx 60 deg)
-
-bootstrap:
- enable: false
- method: "G"
- averaging_length: 10
state_provider: true
state_provider_order: 1
diff --git a/test/yaml/imu_3d/processor_imu_3d.yaml b/test/yaml/imu_3d/processor_imu_3d.yaml
index cd9cfab8c29bd983842841d242b647b1c67b093d..c9dcfe815e125ab2e7e8d297932f24d1392cd153 100644
--- a/test/yaml/imu_3d/processor_imu_3d.yaml
+++ b/test/yaml/imu_3d/processor_imu_3d.yaml
@@ -18,4 +18,4 @@ state_provider: true
state_provider_order: 1
bootstrap:
- enabled: false
\ No newline at end of file
+ enabled: false
\ No newline at end of file
diff --git a/test/yaml/problem_imu_3d_bootstrap.yaml b/test/yaml/problem_imu_3d_bootstrap.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..351e63cdc4fbb166f1ebcfffa65b913a52eed25e
--- /dev/null
+++ b/test/yaml/problem_imu_3d_bootstrap.yaml
@@ -0,0 +1,20 @@
+problem:
+ dimension: 3
+ first_frame:
+ P:
+ state: [0,0,0]
+ mode: "initial_guess"
+ O:
+ state: [0,0,0,1]
+ mode: "initial_guess"
+ V:
+ state: [0,0,0]
+ mode: "initial_guess"
+ tree_manager:
+ type: "None"
+
+sensors:
+ - follow: sensor_imu_3d.yaml
+
+processors:
+ - follow: processor_imu_3d_bootstrap.yaml
\ No newline at end of file
diff --git a/test/yaml/processor_imu_3d_bootstrap.yaml b/test/yaml/processor_imu_3d_bootstrap.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..10c38d512068b216b5387d146d6a1c4c2ba4c348
--- /dev/null
+++ b/test/yaml/processor_imu_3d_bootstrap.yaml
@@ -0,0 +1,23 @@
+name: "cool processor imu"
+plugin: imu
+type: ProcessorImu3d
+sensor_name: "cool sensor imu"
+
+time_tolerance: 0.0025 # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.00001
+apply_loss_function: false
+
+keyframe_vote:
+ voting_active: true
+ max_time_span: 0.04 # seconds
+ max_buff_length: 20000 # motion deltas
+ max_dist_traveled: 2.0 # meters
+ max_angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
+
+state_provider: true
+state_provider_order: 1
+
+bootstrap:
+ enabled: true
+ method: "G"
+ averaging_length: 0.95
\ No newline at end of file