Adapting to changes in core

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);

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;

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;

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;

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;
 

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