diff --git a/demos/demo_factor_imu.cpp b/demos/demo_factor_imu.cpp index 3f054bf9dd9e7075e652910f454e0bdd4a4762c5..5b40987af672c5de1230555a02ef30feb35acfe3 100644 --- a/demos/demo_factor_imu.cpp +++ b/demos/demo_factor_imu.cpp @@ -67,7 +67,7 @@ int main(int argc, char** argv) // 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_->getProcessorIsMotion()->setOrigin(x0, t); //this also creates a keyframe at origin + 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); @@ -99,14 +99,14 @@ int main(int argc, char** argv) /// ******************************************************************************************** /// /// factor creation //create FrameIMU - ts = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; - state_vec = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentState(); + 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_->getProcessorIsMotion()->getCurrentDeltaPreintCov(); - delta_preint = wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_; + 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); @@ -135,7 +135,7 @@ int main(int argc, char** argv) std::cout << "residuals : " << residu.transpose() << std::endl; //reset origin of motion to new frame - wolf_problem_ptr_->getProcessorIsMotion()->setOrigin(last_frame); + wolf_problem_ptr_->getMotionProvider()->setOrigin(last_frame); imu_ptr->setFrame(last_frame); } /// ******************************************************************************************** /// @@ -160,14 +160,14 @@ int main(int argc, char** argv) /// ******************************************************************************************** /// /// factor creation //create FrameIMU - ts = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; - state_vec = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentState(); + 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_->getProcessorIsMotion()->getCurrentDeltaPreintCov(); - delta_preint = wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_; + 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); @@ -196,7 +196,7 @@ int main(int argc, char** argv) std::cout << "residuals : " << residu.transpose() << std::endl; //reset origin of motion to new frame - wolf_problem_ptr_->getProcessorIsMotion()->setOrigin(last_frame); + wolf_problem_ptr_->getMotionProvider()->setOrigin(last_frame); imu_ptr->setFrame(last_frame); } @@ -218,14 +218,14 @@ int main(int argc, char** argv) //create the factor //create FrameIMU - ts = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; - state_vec = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentState(); + 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_->getProcessorIsMotion()->getCurrentDeltaPreintCov(); - delta_preint = wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_; + 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); @@ -260,13 +260,13 @@ int main(int argc, char** argv) ///having a look at covariances Eigen::MatrixXd predelta_cov; predelta_cov.resize(9,9); - predelta_cov = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentDeltaPreintCov(); + 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_->getProcessorIsMotion()->setOrigin(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); diff --git a/demos/demo_imuPlateform_Offline.cpp b/demos/demo_imuPlateform_Offline.cpp index bed5e236f78c1c04c9541476c14ea51f4c78d757..73ea4a40ba97f4f9d787c1118d7f26676175f127 100644 --- a/demos/demo_imuPlateform_Offline.cpp +++ b/demos/demo_imuPlateform_Offline.cpp @@ -166,9 +166,9 @@ int main(int argc, char** argv) } TimeStamp t0, tf; - t0 = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().front().ts_; - tf = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; - int N = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().size(); + 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); @@ -189,20 +189,20 @@ int main(int argc, char** argv) 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_->getProcessorIsMotion()->getMotion().delta_integr_.transpose() << std::endl; + << 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_->getProcessorIsMotion()->getCurrentState().head(16).transpose() << std::endl; + << 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_->getProcessorIsMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; + << (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_->getProcessorIsMotion()->getBuffer().front().ts_; - tf = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; - int N = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().size();*/ + 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; diff --git a/demos/demo_imu_constrained0.cpp b/demos/demo_imu_constrained0.cpp index 19dbf56c1b1ff393701a16604a2e6d32f7f84196..8f2e868cff688ea987d9147097299e4ee063d8d2 100644 --- a/demos/demo_imu_constrained0.cpp +++ b/demos/demo_imu_constrained0.cpp @@ -244,20 +244,20 @@ int main(int argc, char** argv) 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_->getProcessorIsMotion()->getMotion().delta_integr_.transpose() << std::endl; + << 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_->getProcessorIsMotion()->getCurrentState().head(16).transpose() << std::endl; + << 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_->getProcessorIsMotion()->getMotion().delta_integr_cov_.diagonal()).array().sqrt() << std::endl; + << (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_->getProcessorIsMotion()->getBuffer().front().ts_; - tf = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; - int N = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().size(); + 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; diff --git a/demos/demo_processor_imu.cpp b/demos/demo_processor_imu.cpp index c9b46d01d993ff08901322e8d1b85075111944ee..43213af551d160dd1e69b284d5692b15eefcd93f 100644 --- a/demos/demo_processor_imu.cpp +++ b/demos/demo_processor_imu.cpp @@ -114,7 +114,7 @@ int main(int argc, char** argv) // 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_->getProcessorIsMotion()->setOrigin(x0, t); + 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()); @@ -153,18 +153,18 @@ int main(int argc, char** argv) << data.transpose() << std::endl; std::cout << "Current delta: " << std::fixed << std::setprecision(3) << std::setw(8) << std::right - << problem_ptr_->getProcessorIsMotion()->getMotion().delta_.transpose() << std::endl; + << problem_ptr_->getMotionProvider()->getMotion().delta_.transpose() << std::endl; std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8) - << problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_.transpose() << std::endl; + << problem_ptr_->getMotionProvider()->getMotion().delta_integr_.transpose() << std::endl; - Eigen::VectorXd x = problem_ptr_->getProcessorIsMotion()->getCurrentState(); + 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_->getProcessorIsMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; + << (problem_ptr_->getMotionProvider()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; std::cout << std::endl; @@ -176,10 +176,10 @@ int main(int argc, char** argv) Eigen::VectorXd x_debug; TimeStamp ts; - delta_debug = problem_ptr_->getProcessorIsMotion()->getMotion().delta_; - delta_integr_debug = problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_; - x_debug = problem_ptr_->getProcessorIsMotion()->getCurrentState(); - ts = problem_ptr_->getProcessorIsMotion()->getBuffer().back().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" @@ -199,11 +199,11 @@ int main(int argc, char** argv) 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_->getProcessorIsMotion()->getMotion().delta_integr_.transpose() << std::endl; + << problem_ptr_->getMotionProvider()->getMotion().delta_integr_.transpose() << std::endl; std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8) - << problem_ptr_->getProcessorIsMotion()->getCurrentState().head(16).transpose() << std::endl; + << problem_ptr_->getMotionProvider()->getCurrentState().head(16).transpose() << std::endl; // std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8) -// << (problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; +// << (problem_ptr_->getMotionProvider()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; // Print statistics std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl; @@ -215,9 +215,9 @@ int main(int argc, char** argv) #endif TimeStamp t0, tf; - t0 = problem_ptr_->getProcessorIsMotion()->getBuffer().front().ts_; - tf = problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; - int N = problem_ptr_->getProcessorIsMotion()->getBuffer().size(); + 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; diff --git a/demos/demo_processor_imu_jacobians.cpp b/demos/demo_processor_imu_jacobians.cpp index 0a784bf040389bb227309a124fc7f1cb51e43d17..efc781562a7b8aa6570a08ac975744a8f4628314 100644 --- a/demos/demo_processor_imu_jacobians.cpp +++ b/demos/demo_processor_imu_jacobians.cpp @@ -70,7 +70,7 @@ int main(int argc, char** argv) 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_->getProcessorIsMotion()->setOrigin(x0, t); + //wolf_problem_ptr_->getMotionProvider()->setOrigin(x0, t); //CaptureIMU* imu_ptr;