diff --git a/demos/demo_factor_imu.cpp b/demos/demo_factor_imu.cpp index f99b44448fb07d337a0319b3bb451560daf19c76..c5488ed1f739e5a955dfbc6949437261e1c57e67 100644 --- a/demos/demo_factor_imu.cpp +++ b/demos/demo_factor_imu.cpp @@ -46,7 +46,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_->getProcessorMotion()->setOrigin(x0, t); //this also creates a keyframe at origin + wolf_problem_ptr_->getProcessorIsMotion()->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); @@ -78,14 +78,14 @@ int main(int argc, char** argv) /// ******************************************************************************************** /// /// factor creation //create FrameIMU - ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; - state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState(); + ts = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().get().back().ts_; + state_vec = wolf_problem_ptr_->getProcessorIsMotion()->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_->getProcessorMotion()->getCurrentDeltaPreintCov(); - delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_; + delta_preint_cov = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentDeltaPreintCov(); + delta_preint = wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_; std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov); feat_imu->setCapture(imu_ptr); @@ -114,7 +114,7 @@ int main(int argc, char** argv) std::cout << "residuals : " << residu.transpose() << std::endl; //reset origin of motion to new frame - wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame); + wolf_problem_ptr_->getProcessorIsMotion()->setOrigin(last_frame); imu_ptr->setFrame(last_frame); } /// ******************************************************************************************** /// @@ -139,14 +139,14 @@ int main(int argc, char** argv) /// ******************************************************************************************** /// /// factor creation //create FrameIMU - ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; - state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState(); + ts = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().get().back().ts_; + state_vec = wolf_problem_ptr_->getProcessorIsMotion()->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_->getProcessorMotion()->getCurrentDeltaPreintCov(); - delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_; + delta_preint_cov = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentDeltaPreintCov(); + delta_preint = wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_; std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov); feat_imu->setCapture(imu_ptr); @@ -175,7 +175,7 @@ int main(int argc, char** argv) std::cout << "residuals : " << residu.transpose() << std::endl; //reset origin of motion to new frame - wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame); + wolf_problem_ptr_->getProcessorIsMotion()->setOrigin(last_frame); imu_ptr->setFrame(last_frame); } @@ -197,14 +197,14 @@ int main(int argc, char** argv) //create the factor //create FrameIMU - ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; - state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState(); + ts = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().get().back().ts_; + state_vec = wolf_problem_ptr_->getProcessorIsMotion()->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_->getProcessorMotion()->getCurrentDeltaPreintCov(); - delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_; + delta_preint_cov = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentDeltaPreintCov(); + delta_preint = wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_; std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov); feat_imu->setCapture(imu_ptr); @@ -239,13 +239,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_->getProcessorMotion()->getCurrentDeltaPreintCov(); + predelta_cov = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentDeltaPreintCov(); //std::cout << "predelta_cov : \n" << predelta_cov << std::endl; ///Optimization // PRIOR //FrameBasePtr first_frame = wolf_problem_ptr_->getTrajectory()->getFrameList().front(); - wolf_problem_ptr_->getProcessorMotion()->setOrigin(wolf_problem_ptr_->getTrajectory()->getFrameList().front()); + wolf_problem_ptr_->getProcessorIsMotion()->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 0b98b3e9675c6c0bf0a3ca1c559d4e4fbcc70f02..019e921f81d1cbde012aae7187fc65ad3e0cdffe 100644 --- a/demos/demo_imuPlateform_Offline.cpp +++ b/demos/demo_imuPlateform_Offline.cpp @@ -147,9 +147,9 @@ int main(int argc, char** argv) } TimeStamp t0, tf; - t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_; - tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; - int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size(); + t0 = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().get().front().ts_; + tf = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().get().back().ts_; + int N = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().get().size(); //Finally, process the only one odom input mot_ptr->setTimeStamp(ts); @@ -170,20 +170,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_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl; + << wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_.transpose() << std::endl; std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8) - << wolf_problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl; + << wolf_problem_ptr_->getProcessorIsMotion()->getCurrentState().head(16).transpose() << std::endl; std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8) - << (wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; + << (wolf_problem_ptr_->getProcessorIsMotion()->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_->getProcessorMotion()->getBuffer().get().front().ts_; - tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; - int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();*/ + t0 = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().get().front().ts_; + tf = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().get().back().ts_; + int N = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().get().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 cff689aaf16c1e4692c79f27f2becf56827acca3..242fb18ac80b8c6c97b9d9180c57e47dc29b3f64 100644 --- a/demos/demo_imu_constrained0.cpp +++ b/demos/demo_imu_constrained0.cpp @@ -225,20 +225,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_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl; + << wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_.transpose() << std::endl; std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8) - << wolf_problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl; + << wolf_problem_ptr_->getProcessorIsMotion()->getCurrentState().head(16).transpose() << std::endl; std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8) - << (wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal()).array().sqrt() << std::endl; + << (wolf_problem_ptr_->getProcessorIsMotion()->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_->getProcessorMotion()->getBuffer().get().front().ts_; - tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; - int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size(); + t0 = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().get().front().ts_; + tf = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().get().back().ts_; + int N = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().get().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 e6062abbc6310e580e1623b9c01d851e4184a988..93bfa93dbb6ddc6e53b52fae5bc0fcf372bb94cf 100644 --- a/demos/demo_processor_imu.cpp +++ b/demos/demo_processor_imu.cpp @@ -93,7 +93,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_->getProcessorMotion()->setOrigin(x0, t); + problem_ptr_->getProcessorIsMotion()->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()); @@ -132,18 +132,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_->getProcessorMotion()->getMotion().delta_.transpose() << std::endl; + << problem_ptr_->getProcessorIsMotion()->getMotion().delta_.transpose() << std::endl; std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8) - << problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl; + << problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_.transpose() << std::endl; - Eigen::VectorXd x = problem_ptr_->getProcessorMotion()->getCurrentState(); + Eigen::VectorXd x = problem_ptr_->getProcessorIsMotion()->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_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; + << (problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; std::cout << std::endl; @@ -155,10 +155,10 @@ int main(int argc, char** argv) Eigen::VectorXd x_debug; TimeStamp ts; - delta_debug = problem_ptr_->getProcessorMotion()->getMotion().delta_; - delta_integr_debug = problem_ptr_->getProcessorMotion()->getMotion().delta_integr_; - x_debug = problem_ptr_->getProcessorMotion()->getCurrentState(); - ts = problem_ptr_->getProcessorMotion()->getBuffer().get().back().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().get().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" @@ -178,11 +178,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_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl; + << problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_.transpose() << std::endl; std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8) - << problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl; + << problem_ptr_->getProcessorIsMotion()->getCurrentState().head(16).transpose() << std::endl; // std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8) -// << (problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; +// << (problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; // Print statistics std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl; @@ -194,9 +194,9 @@ int main(int argc, char** argv) #endif TimeStamp t0, tf; - t0 = problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_; - tf = problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; - int N = problem_ptr_->getProcessorMotion()->getBuffer().get().size(); + t0 = problem_ptr_->getProcessorIsMotion()->getBuffer().get().front().ts_; + tf = problem_ptr_->getProcessorIsMotion()->getBuffer().get().back().ts_; + int N = problem_ptr_->getProcessorIsMotion()->getBuffer().get().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 3555fa9c8f96c337e7375588684cdbb55bd29383..0029a8bdddc020c9ef109cb1c57c6fae508384ba 100644 --- a/demos/demo_processor_imu_jacobians.cpp +++ b/demos/demo_processor_imu_jacobians.cpp @@ -49,7 +49,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_->getProcessorMotion()->setOrigin(x0, t); + //wolf_problem_ptr_->getProcessorIsMotion()->setOrigin(x0, t); //CaptureIMU* imu_ptr; diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp index fdb2cecef0f134882fdd6b45232c6c03479f495b..2aa73c0330dcfc4aa47008da96f0d6109f0831ce 100644 --- a/src/processor/processor_IMU.cpp +++ b/src/processor/processor_IMU.cpp @@ -6,7 +6,7 @@ namespace wolf { ProcessorIMU::ProcessorIMU(ProcessorParamsIMUPtr _params_motion_IMU) : - ProcessorMotion("ProcessorImu", 10, 10, 9, 6, 6, _params_motion_IMU), + ProcessorMotion("ProcessorImu", "POV", 10, 10, 9, 6, 6, _params_motion_IMU), params_motion_IMU_(std::make_shared<ProcessorParamsIMU>(*_params_motion_IMU)) { // Set constant parts of Jacobians @@ -202,6 +202,7 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXd& _delta_preint, imu::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in imu_tools } + } // namespace wolf // Register in the SensorFactory