diff --git a/demos/demo_factor_imu.cpp b/demos/demo_factor_imu.cpp
index 913932a305c57780ac2a5499d0b0d2c0e2b2c83b..581d7429331d13a0bf67f847f7d9924c0814358e 100644
--- a/demos/demo_factor_imu.cpp
+++ b/demos/demo_factor_imu.cpp
@@ -1,4 +1,6 @@
//Wolf
+#include <core/ceres_wrapper/solver_ceres.h>
+
#include "core/capture/capture_imu.h"
#include "core/processor/processor_imu.h"
#include "core/sensor/sensor_imu.h"
@@ -8,7 +10,6 @@
#include "core/factor/factor_odom_3D.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
-#include "core/ceres_wrapper/ceres_manager.h"
//#define DEBUG_RESULTS
@@ -30,11 +31,10 @@ int main(int argc, char** argv)
wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
// Ceres wrappers
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true);
+ 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;
diff --git a/demos/demo_imuDock.cpp b/demos/demo_imuDock.cpp
index 73f693e491acf81eb4020cfc99099f66f09ed94e..e218a88cbf8f48b7537954f52d02ce7c7ee957f1 100644
--- a/demos/demo_imuDock.cpp
+++ b/demos/demo_imuDock.cpp
@@ -5,11 +5,12 @@
* \author: Dinesh Atchuthan
*/
+#include <core/ceres_wrapper/solver_ceres.h>
+
#include "core/processor/processor_imu.h"
#include "core/sensor/sensor_imu.h"
#include "core/common/wolf.h"
#include "core/problem/problem.h"
-#include "core/ceres_wrapper/ceres_manager.h"
#include "core/sensor/sensor_odom_3D.h"
#include "core/processor/processor_odom_3D.h"
@@ -102,7 +103,7 @@ int main(int argc, char** argv)
// ___Create the WOLF Problem + define origin of the problem___
ProblemPtr problem = Problem::create("PQVBB 3D");
- CeresManager* ceres_manager = new CeresManager(problem);
+ SolverCeres* ceres_manager = new SolverCeres(problem);
// ___Configure Ceres if needed___
diff --git a/demos/demo_imuDock_autoKFs.cpp b/demos/demo_imuDock_autoKFs.cpp
index cfd482b6faea95d6bd606c04fdc1e9370ba9a837..d5bd9aa8bc357f6be1c5135d8c1ad7730c4858da 100644
--- a/demos/demo_imuDock_autoKFs.cpp
+++ b/demos/demo_imuDock_autoKFs.cpp
@@ -5,11 +5,12 @@
* \author: Dinesh Atchuthan
*/
+#include <core/ceres_wrapper/solver_ceres.h>
+
#include "core/processor/processor_imu.h"
#include "core/sensor/sensor_imu.h"
#include "core/common/wolf.h"
#include "core/problem/problem.h"
-#include "core/ceres_wrapper/ceres_manager.h"
#include "core/sensor/sensor_odom_3D.h"
#include "core/processor/processor_odom_3D.h"
@@ -111,7 +112,7 @@ int main(int argc, char** argv)
// ___Create the WOLF Problem + define origin of the problem___
ProblemPtr problem = Problem::create("PQVBB 3D");
- CeresManager* ceres_manager = new CeresManager(problem);
+ SolverCeres* ceres_manager = new SolverCeres(problem);
// ___Configure Ceres if needed___
diff --git a/demos/demo_imuPlateform_Offline.cpp b/demos/demo_imuPlateform_Offline.cpp
index a3565850e74ecc27f58b0e47248813d75fe98bb3..628c4b65ed2e8b88a3cdfbedc8f07ef80f906001 100644
--- a/demos/demo_imuPlateform_Offline.cpp
+++ b/demos/demo_imuPlateform_Offline.cpp
@@ -1,4 +1,6 @@
//Wolf
+#include <core/ceres_wrapper/solver_ceres.h>
+
#include "core/capture/capture_imu.h"
#include "core/processor/processor_imu.h"
#include "core/sensor/sensor_imu.h"
@@ -9,9 +11,6 @@
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/processor/processor_odom_3D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-//std
#include <iostream>
#include <fstream>
#include <iomanip>
@@ -75,11 +74,10 @@ int main(int argc, char** argv)
TimeStamp t(0);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
+ 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");
diff --git a/demos/demo_imu_constrained0.cpp b/demos/demo_imu_constrained0.cpp
index 3f6a245ddec51b5f9f5f7ae043099c2e49021d1d..66db5c1c71e81035aa6ea70e000a8a92d29b4af6 100644
--- a/demos/demo_imu_constrained0.cpp
+++ b/demos/demo_imu_constrained0.cpp
@@ -1,4 +1,6 @@
//Wolf
+#include <core/ceres_wrapper/solver_ceres.h>
+
#include "core/capture/capture_imu.h"
#include "core/processor/processor_imu.h"
#include "core/sensor/sensor_imu.h"
@@ -9,9 +11,6 @@
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/processor/processor_odom_3D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-//std
#include <iostream>
#include <fstream>
#include <iomanip>
@@ -96,11 +95,10 @@ int main(int argc, char** argv)
imu_data_input >> origin_bias[0] >> origin_bias[1] >> origin_bias[2] >> origin_bias[3] >> origin_bias[4] >> origin_bias[5];
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
+ 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");
diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index cf15fb37a5d8c1130211126835f675ca0afa599b..2be43b54d15773093796950d7bb89a9e1581d43d 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -398,6 +398,31 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
//////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////// PRINT VALUES ///////////////////////////////////
#if 0
+ if (sizeof(T) == sizeof(double))
+ {
+ std::cout << "Frames " << getFrame()->id() << " - " << getFrameOther()->id() << std::endl;
+ std::cout << "\tpreint: " << acc_bias_preint_.transpose()
+ << " " << gyro_bias_preint_.transpose() << std::endl;
+ std::cout << "\tb1: " << _ab1(0) << " "
+ << _ab1(1) << " "
+ << _ab1(2) << " "
+ << _wb1(0) << " "
+ << _wb1(1) << " "
+ << _wb1(2) << "\n";
+ std::cout << "\tb2: " << _ab2(0) << " "
+ << _ab2(1) << " "
+ << _ab2(2) << " "
+ << _wb2(0) << " "
+ << _wb2(1) << " "
+ << _wb2(2) << "\n";
+ std::cout << "\tb2-b1 " << _ab2(0)-_ab1(0) << " "
+ << _ab2(1)-_ab1(1) << " "
+ << _ab2(2)-_ab1(2) << " "
+ << _wb2(0)-_wb1(0) << " "
+ << _wb2(1)-_wb1(1) << " "
+ << _wb2(2)-_wb1(2) << "\n";
+ }
+
// print values -----------------------
Matrix<T, 10, 1> x1; x1 << _p1 , _q1.coeffs(), _v1;
Matrix<T, 10, 1> x2; x2 << _p2 , _q2.coeffs(), _v2;
diff --git a/include/imu/math/imu_tools.h b/include/imu/math/imu_tools.h
index 5be0bbd2b3ff0ace7264b83db249f382740ba362..d9b0b58454132c8abaa11d50a5847d18b829337f 100644
--- a/include/imu/math/imu_tools.h
+++ b/include/imu/math/imu_tools.h
@@ -80,9 +80,9 @@ inline Matrix<T, 10, 1> identity()
inline VectorComposite identityComposite()
{
VectorComposite D;
- D.emplace("P", Vector3d::Zero());
- D.emplace("O", Quaterniond::Identity().coeffs());
- D.emplace("V", Vector3d::Zero());
+ D.emplace('P', Vector3d::Zero());
+ D.emplace('O', Quaterniond::Identity().coeffs());
+ D.emplace('V', Vector3d::Zero());
return D;
}
@@ -194,13 +194,13 @@ inline void compose(const MatrixBase<D1>& d1,
inline void compose(const VectorComposite& d1, const VectorComposite& d2, double dt, VectorComposite& dc)
{
- compose(d1.at("P"), d1.at("O"), d1.at("V"), d2.at("P"), d2.at("O"), d2.at("V"), dt, dc["P"], dc["O"], dc["V"]);
+ compose(d1.at('P'), d1.at('O'), d1.at('V'), d2.at('P'), d2.at('O'), d2.at('V'), dt, dc['P'], dc['O'], dc['V']);
}
inline VectorComposite compose(const VectorComposite& d1, const VectorComposite& d2, double dt)
{
VectorComposite dc("POV", {3,4,3});
- compose(d1.at("P"), d1.at("O"), d1.at("V"), d2.at("P"), d2.at("O"), d2.at("V"), dt, dc["P"], dc["O"], dc["V"]);
+ compose(d1.at('P'), d1.at('O'), d1.at('V'), d2.at('P'), d2.at('O'), d2.at('V'), dt, dc['P'], dc['O'], dc['V']);
return dc;
}
@@ -258,33 +258,33 @@ inline void compose(const VectorComposite& d1,
{
// Some useful temporaries
- Matrix3d dR1 = q2R(d1.at("O")); //dq1.matrix(); // First Delta, DR
- Matrix3d dR2 = q2R(d2.at("O")); //dq2.matrix(); // Second delta, dR
+ Matrix3d dR1 = q2R(d1.at('O')); //dq1.matrix(); // First Delta, DR
+ Matrix3d dR2 = q2R(d2.at('O')); //dq2.matrix(); // Second delta, dR
// Jac wrt first delta // TODO find optimal way to re-use memory allocation!!!
J_sum_d1.clear();
- J_sum_d1.emplace("P","P", Matrix3d::Identity()); // dDp'/dDp = I
- J_sum_d1.emplace("P","O", - dR1 * skew(d2.at("P"))) ; // dDp'/dDo
- J_sum_d1.emplace("P","V", Matrix3d::Identity() * dt); // dDp'/dDv = I*dt
- J_sum_d1.emplace("O","P", Matrix3d::Zero());
- J_sum_d1.emplace("O","O", dR2.transpose()); // dDo'/dDo
- J_sum_d1.emplace("O","V", Matrix3d::Zero());
- J_sum_d1.emplace("V","P", Matrix3d::Zero());
- J_sum_d1.emplace("V","O", - dR1 * skew(d2.at("V"))) ; // dDv'/dDo
- J_sum_d1.emplace("V","V", Matrix3d::Identity()); // dDv'/dDv = I
+ J_sum_d1.emplace('P','P', Matrix3d::Identity()); // dDp'/dDp = I
+ J_sum_d1.emplace('P','O', - dR1 * skew(d2.at('P'))) ; // dDp'/dDo
+ J_sum_d1.emplace('P','V', Matrix3d::Identity() * dt); // dDp'/dDv = I*dt
+ J_sum_d1.emplace('O','P', Matrix3d::Zero());
+ J_sum_d1.emplace('O','O', dR2.transpose()); // dDo'/dDo
+ J_sum_d1.emplace('O','V', Matrix3d::Zero());
+ J_sum_d1.emplace('V','P', Matrix3d::Zero());
+ J_sum_d1.emplace('V','O', - dR1 * skew(d2.at('V'))) ; // dDv'/dDo
+ J_sum_d1.emplace('V','V', Matrix3d::Identity()); // dDv'/dDv = I
// Jac wrt second delta
J_sum_d2.clear();
- J_sum_d2.emplace("P","P", dR1); // dDp'/ddp
- J_sum_d2.emplace("P","O", Matrix3d::Zero()) ; // dDp'/ddo
- J_sum_d2.emplace("P","V", Matrix3d::Zero()); // dDp'/ddv
- J_sum_d2.emplace("O","P", Matrix3d::Zero());
- J_sum_d2.emplace("O","O", Matrix3d::Identity());// dDo'/ddo
- J_sum_d2.emplace("O","V", Matrix3d::Zero());
- J_sum_d2.emplace("V","P", Matrix3d::Zero());
- J_sum_d2.emplace("V","O", Matrix3d::Zero()) ; // dDv'/ddo
- J_sum_d2.emplace("V","V", dR1); // dDv'/ddv
+ J_sum_d2.emplace('P','P', dR1); // dDp'/ddp
+ J_sum_d2.emplace('P','O', Matrix3d::Zero()) ; // dDp'/ddo
+ J_sum_d2.emplace('P','V', Matrix3d::Zero()); // dDp'/ddv
+ J_sum_d2.emplace('O','P', Matrix3d::Zero());
+ J_sum_d2.emplace('O','O', Matrix3d::Identity());// dDo'/ddo
+ J_sum_d2.emplace('O','V', Matrix3d::Zero());
+ J_sum_d2.emplace('V','P', Matrix3d::Zero());
+ J_sum_d2.emplace('V','O', Matrix3d::Zero()) ; // dDv'/ddo
+ J_sum_d2.emplace('V','V', dR1); // dDv'/ddv
// compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
compose(d1, d2, dt, sum);
@@ -428,14 +428,14 @@ inline void composeOverState(const VectorComposite& x,
T dt,
VectorComposite& x_plus_d)
{
- assert(x_plus_d.count("P") && "provided reference does not have key 'P'");
- assert(x_plus_d.count("O") && "provided reference does not have key 'O'");
- assert(x_plus_d.count("V") && "provided reference does not have key 'V'");
+ assert(x_plus_d.count('P') && "provided reference does not have key 'P'");
+ assert(x_plus_d.count('O') && "provided reference does not have key 'O'");
+ assert(x_plus_d.count('V') && "provided reference does not have key 'V'");
- composeOverState(x.at("P"), x.at("O"), x.at("V"),
- d.at("P"), d.at("O"), d.at("V"),
+ composeOverState(x.at('P'), x.at('O'), x.at('V'),
+ d.at('P'), d.at('O'), d.at('V'),
dt,
- x_plus_d["P"], x_plus_d["O"], x_plus_d["V"]);
+ x_plus_d['P'], x_plus_d['O'], x_plus_d['V']);
}
template<class T>
@@ -592,7 +592,7 @@ inline Matrix<typename D1::Scalar, 10, 1> plus(const MatrixBase<D1>& d1,
inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
{
- plus(x.at("P"), x.at("O"), x.at("V"), tau.at("P"), tau.at("O"), tau.at("V"), res.at("P"), res.at("O"), res.at("V"));
+ plus(x.at('P'), x.at('O'), x.at('V'), tau.at('P'), tau.at('O'), tau.at('V'), res.at('P'), res.at('O'), res.at('V'));
}
inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
@@ -755,22 +755,12 @@ inline void body2delta(const MatrixBase<D1>& body,
{
MatrixSizeCheck<6,1>::check(body);
-// _delta["P"] = Vector3d();
-// _delta["O"] = Vector4d();
-// _delta["V"] = Vector3d();
-
body2delta(body.block(0,0,3,1),
body.block(3,0,3,1),
dt,
- _delta["P"],
- _delta["O"],
- _delta["V"]);
-// body2delta(body.block(0,0,3,1),
-// body.block(3,0,3,1),
-// dt,
-// _delta.at("P"),
-// _delta.at("O"),
-// _delta.at("V"));
+ _delta['P'],
+ _delta['O'],
+ _delta['V']);
}
template<typename D1, typename D2, typename D3>
@@ -808,29 +798,12 @@ inline void body2delta(const MatrixBase<D1>& body,
Matrix<T, 3, 1> w = body.block(3,0,3,1);
-// jac_body.setZero();
-// jac_body.block(0,0,3,3) = 0.5 * dt * dt * Matrix<T, 3, 3>::Identity();
-// jac_body.block(3,3,3,3) = dt * jac_SO3_right(w * dt);
-// jac_body.block(6,0,3,3) = dt * Matrix<T, 3, 3>::Identity();
-
-// Matrix<double, 3, 6> J_dp_data;
-// Matrix<double, 3, 6> J_do_data;
-// Matrix<double, 3, 6> J_dv_data;
-
-// jac_body.emplace("P","I", MatrixXd(3,6));
-// jac_body.emplace("O","I", MatrixXd(3,6));
-// jac_body.emplace("V","I", MatrixXd(3,6));
-//
-// jac_body.at("P","I") << 0.5 * dt * dt * Matrix3d::Identity() , Matrix3d::Zero() ;
-// jac_body.at("O","I") << Matrix3d::Zero() , dt * jac_SO3_right(w * dt) ;
-// jac_body.at("V","I") << dt * Matrix3d::Identity() , Matrix3d::Zero() ;
-
- jac_body.emplace("P","a", 0.5 * dt * dt * Matrix3d::Identity()); // 0,0
- jac_body.emplace("P","w", Matrix3d::Zero()); // 0,3
- jac_body.emplace("O","a", Matrix3d::Zero()); // 3,0
- jac_body.emplace("O","w", dt * jac_SO3_right(w * dt)); // 3,3
- jac_body.emplace("V","a", dt * Matrix3d::Identity()); // 6,0
- jac_body.emplace("V","w", Matrix3d::Zero()); // 6,6
+ jac_body.emplace('P','a', 0.5 * dt * dt * Matrix3d::Identity()); // 0,0
+ jac_body.emplace('P','w', Matrix3d::Zero()); // 0,3
+ jac_body.emplace('O','a', Matrix3d::Zero()); // 3,0
+ jac_body.emplace('O','w', dt * jac_SO3_right(w * dt)); // 3,3
+ jac_body.emplace('V','a', dt * Matrix3d::Identity()); // 6,0
+ jac_body.emplace('V','w', Matrix3d::Zero()); // 6,6
}
template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index 42361d86e834f8572fb08d6d6071d667e5f8d6a6..bf1fe72b6241c7b66dca58738d4fe168903c8773 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -87,6 +87,8 @@ FeatureBasePtr ProcessorImu::emplaceFeature(CaptureMotionPtr _capture_motion)
VectorXd ProcessorImu::getCalibration (const CaptureBasePtr _capture) const
{
+ assert(_capture && "called with a null capture");
+ assert(_capture->getSensorIntrinsic() && "null sensor intrinsics state block");
return _capture->getSensorIntrinsic()->getState();
}
diff --git a/test/gtest_factor_imu.cpp b/test/gtest_factor_imu.cpp
index 04f4ef9fc8f02bc2a4006acdaf7e757d72523507..f53f9af38ff6da00ba0f57d815d4b3a336267e9e 100644
--- a/test/gtest_factor_imu.cpp
+++ b/test/gtest_factor_imu.cpp
@@ -5,6 +5,7 @@
* \author: Dinesh Atchuthan
*/
+#include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h>
#include <core/utils/logging.h>
@@ -17,9 +18,6 @@
//Wolf
#include <core/factor/factor_pose_3d.h>
#include <core/processor/processor_odom_3d.h>
-#include <core/ceres_wrapper/ceres_manager.h>
-
-// debug
#include <iostream>
#include <fstream>
@@ -43,7 +41,7 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test
wolf::TimeStamp t;
SensorImuPtr sen_imu;
ProblemPtr problem;
- CeresManager* ceres_manager_wolf_diff;
+ SolverCeresPtr solver_wolf_diff;
ProcessorBasePtr processor_;
ProcessorImuPtr processor_imu;
FrameBasePtr KF0;
@@ -66,11 +64,10 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test
problem = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
+ solver_wolf_diff = std::make_shared<SolverCeres>(problem);
+ solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
@@ -132,7 +129,7 @@ class FactorImu_biasTest_Static_NonNullAccBias : public testing::Test
wolf::TimeStamp t;
SensorImuPtr sen_imu;
ProblemPtr problem;
- CeresManager* ceres_manager_wolf_diff;
+ SolverCeresPtr solver_wolf_diff;
ProcessorBasePtr processor_;
ProcessorImuPtr processor_imu;
FrameBasePtr KF0;
@@ -154,11 +151,10 @@ class FactorImu_biasTest_Static_NonNullAccBias : public testing::Test
problem = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
+ solver_wolf_diff = std::make_shared<SolverCeres>(problem);
+ solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
@@ -216,7 +212,7 @@ class FactorImu_biasTest_Static_NonNullGyroBias : public testing::Test
wolf::TimeStamp t;
SensorImuPtr sen_imu;
ProblemPtr problem;
- CeresManager* ceres_manager_wolf_diff;
+ SolverCeresPtr solver_wolf_diff;
ProcessorBasePtr processor_;
ProcessorImuPtr processor_imu;
FrameBasePtr origin_KF;
@@ -238,11 +234,10 @@ class FactorImu_biasTest_Static_NonNullGyroBias : public testing::Test
problem = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
-// ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-// ceres_options.max_line_search_step_contraction = 1e-3;
-// ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
+ solver_wolf_diff = std::make_shared<SolverCeres>(problem);
+ //solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ //solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ //solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
@@ -300,7 +295,7 @@ class FactorImu_biasTest_Static_NonNullBias : public testing::Test
wolf::TimeStamp t;
SensorImuPtr sen_imu;
ProblemPtr problem;
- CeresManager* ceres_manager_wolf_diff;
+ SolverCeresPtr solver_wolf_diff;
ProcessorBasePtr processor_;
ProcessorImuPtr processor_imu;
FrameBasePtr origin_KF;
@@ -322,11 +317,10 @@ class FactorImu_biasTest_Static_NonNullBias : public testing::Test
problem = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
+ solver_wolf_diff = std::make_shared<SolverCeres>(problem);
+ solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
@@ -386,7 +380,7 @@ class FactorImu_biasTest_Move_NullBias : public testing::Test
wolf::TimeStamp t;
SensorImuPtr sen_imu;
ProblemPtr problem;
- CeresManager* ceres_manager_wolf_diff;
+ SolverCeresPtr solver_wolf_diff;
ProcessorBasePtr processor_;
ProcessorImuPtr processor_imu;
FrameBasePtr origin_KF;
@@ -408,11 +402,10 @@ class FactorImu_biasTest_Move_NullBias : public testing::Test
problem = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
+ solver_wolf_diff = std::make_shared<SolverCeres>(problem);
+ solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
@@ -473,7 +466,7 @@ class FactorImu_biasTest_Move_NonNullBias : public testing::Test
wolf::TimeStamp t;
SensorImuPtr sen_imu;
ProblemPtr problem;
- CeresManager* ceres_manager_wolf_diff;
+ SolverCeresPtr solver_wolf_diff;
ProcessorBasePtr processor_;
ProcessorImuPtr processor_imu;
FrameBasePtr origin_KF;
@@ -495,11 +488,10 @@ class FactorImu_biasTest_Move_NonNullBias : public testing::Test
problem = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
+ solver_wolf_diff = std::make_shared<SolverCeres>(problem);
+ solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
@@ -556,7 +548,7 @@ class FactorImu_biasTest_Move_NonNullBiasRotCst : public testing::Test
wolf::TimeStamp t;
SensorImuPtr sen_imu;
ProblemPtr problem;
- CeresManager* ceres_manager_wolf_diff;
+ SolverCeresPtr solver_wolf_diff;
ProcessorBasePtr processor_;
ProcessorImuPtr processor_imu;
FrameBasePtr origin_KF;
@@ -578,11 +570,10 @@ class FactorImu_biasTest_Move_NonNullBiasRotCst : public testing::Test
problem = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
+ solver_wolf_diff = std::make_shared<SolverCeres>(problem);
+ solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
@@ -649,7 +640,7 @@ class FactorImu_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
wolf::TimeStamp t;
SensorImuPtr sen_imu;
ProblemPtr problem;
- CeresManager* ceres_manager_wolf_diff;
+ SolverCeresPtr solver_wolf_diff;
ProcessorBasePtr processor_;
ProcessorImuPtr processor_imu;
FrameBasePtr origin_KF;
@@ -671,11 +662,10 @@ class FactorImu_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
problem = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
+ solver_wolf_diff = std::make_shared<SolverCeres>(problem);
+ solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
@@ -750,7 +740,7 @@ class FactorImu_biasTest_Move_NonNullBiasRot : public testing::Test
wolf::TimeStamp t;
SensorImuPtr sen_imu;
ProblemPtr problem;
- CeresManager* ceres_manager_wolf_diff;
+ SolverCeresPtr solver_wolf_diff;
ProcessorBasePtr processor_;
ProcessorImuPtr processor_imu;
FrameBasePtr origin_KF;
@@ -772,11 +762,10 @@ class FactorImu_biasTest_Move_NonNullBiasRot : public testing::Test
problem = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION;
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
+ solver_wolf_diff = std::make_shared<SolverCeres>(problem);
+ solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
@@ -846,7 +835,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
public:
wolf::TimeStamp t;
ProblemPtr problem;
- CeresManagerPtr ceres_manager;
+ SolverCeresPtr ceres_manager;
SensorBasePtr sensor;
SensorImuPtr sensor_imu;
SensorOdom3dPtr sensor_odo;
@@ -874,9 +863,8 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
problem = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION;
- ceres_manager = std::make_shared<CeresManager>(problem, ceres_options);
+ ceres_manager = std::make_shared<SolverCeres>(problem);
+ ceres_manager->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
// SENSOR + PROCESSOR Imu
sensor = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
@@ -1032,7 +1020,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
SensorImuPtr sen_imu;
SensorOdom3dPtr sen_odom3d;
ProblemPtr problem;
- CeresManager* ceres_manager_wolf_diff;
+ SolverCeresPtr solver_wolf_diff;
ProcessorBasePtr processor_;
ProcessorImuPtr processor_imu;
ProcessorOdom3dPtr processor_odom3d;
@@ -1055,11 +1043,10 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
problem = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION;
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
+ solver_wolf_diff = std::make_shared<SolverCeres>(problem);
+ solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
@@ -1173,7 +1160,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
SensorImuPtr sen_imu;
SensorOdom3dPtr sen_odom3d;
ProblemPtr problem;
- CeresManagerPtr ceres_manager_wolf_diff;
+ SolverCeresPtr solver_wolf_diff;
ProcessorBasePtr processor;
ProcessorImuPtr processor_imu;
ProcessorOdom3dPtr processor_odom3d;
@@ -1197,11 +1184,10 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
problem = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION;
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = std::make_shared<CeresManager>(problem, ceres_options);
+ solver_wolf_diff = std::make_shared<SolverCeres>(problem);
+ solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
+ solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4;
// SENSOR + PROCESSOR Imu
SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
@@ -1362,7 +1348,7 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
KF1->getV()->fix();
KF1->getCaptureOf(sen_imu)->setCalibration((Vector6d()<<-1,-2,-3,-1,-2,-3).finished());
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1396,7 +1382,7 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1414,7 +1400,7 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1432,7 +1418,7 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1452,7 +1438,7 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1478,7 +1464,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK
KF1->getO()->fix();
KF1->getV()->fix();
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1512,7 +1498,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1530,7 +1516,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1548,7 +1534,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1568,7 +1554,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1594,7 +1580,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initO
last_KF->getO()->fix();
last_KF->getV()->fix();
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1626,7 +1612,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
last_KF ->getCaptureOf(sen_imu)->setCalibration(origin_bias);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1641,7 +1627,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1656,7 +1642,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1673,7 +1659,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1705,7 +1691,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
@@ -1721,7 +1707,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1736,7 +1722,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1753,7 +1739,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1775,7 +1761,7 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
last_KF->getO()->fix();
last_KF->getV()->fix();
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1806,7 +1792,7 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1821,7 +1807,7 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1836,7 +1822,7 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1853,7 +1839,7 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1874,7 +1860,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
last_KF->getO()->fix();
last_KF->getV()->fix();
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1905,7 +1891,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1920,7 +1906,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1935,7 +1921,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1952,7 +1938,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1974,7 +1960,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initO
last_KF->getO()->fix();
last_KF->getV()->fix();
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2005,7 +1991,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2020,7 +2006,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2035,7 +2021,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2052,7 +2038,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2074,7 +2060,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_i
last_KF->getO()->fix();
last_KF->getV()->fix();
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2105,7 +2091,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2120,7 +2106,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2135,7 +2121,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2152,7 +2138,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
- report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2174,7 +2160,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_
last_KF->setState(expected_final_state);
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2195,7 +2181,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_
//
// last_KF->setState(expected_final_state);
//
-// std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+// std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
//
// //Only biases are unfixed
// ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3)
@@ -2221,8 +2207,8 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_i
Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ solver_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2266,8 +2252,8 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_i
Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ solver_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2313,8 +2299,8 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_i
Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
- //ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ //solver_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2342,7 +2328,7 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_i
Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2372,7 +2358,7 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_i
Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2400,7 +2386,7 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_i
Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2429,8 +2415,8 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_i
Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ solver_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2487,8 +2473,8 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i
Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ solver_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2548,8 +2534,8 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_
Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
- std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
+ std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+ solver_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
diff --git a/test/gtest_feature_imu.cpp b/test/gtest_feature_imu.cpp
index bc87d0be0abc0c5118f772b106c7676c87f52642..916dc5c53d7064d1f4ca26bf941bb206fccd3c7a 100644
--- a/test/gtest_feature_imu.cpp
+++ b/test/gtest_feature_imu.cpp
@@ -50,21 +50,15 @@ class FeatureImu_test : public testing::Test
// Time and data variables
TimeStamp t;
- Eigen::Vector6d data_;
+ Eigen::Vector6d data_=Eigen::Vector6d::Zero();
t.set(0);
// Set the origin
VectorComposite x0("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
VectorComposite s0("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
- problem->setPriorOptions("factor", 0.01, x0, s0);
- // initial dummy capture process for seting prior at t=0
- std::make_shared<CaptureImu>(t,
- sensor_ptr,
- data_,
- Eigen::Matrix6d::Identity()*0.01,
- Eigen::Vector6d::Zero())->process();
- origin_frame = problem->getTrajectory()->getFirstFrame();
+ origin_frame = problem->setPriorFactor(x0, s0, t, 0.01);
+ processor_motion_ptr_->setOrigin(origin_frame);
// Emplace one capture to store the Imu data arriving from (sensor / callback / file / etc.)
// give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp
index 98695a245cd761d49733a4af2dd3658ee522fb9e..60d54ca54c8a6da097ae9b91381f47b2422b5b3d 100644
--- a/test/gtest_imu.cpp
+++ b/test/gtest_imu.cpp
@@ -6,6 +6,8 @@
*/
//Wolf
+#include <core/ceres_wrapper/solver_ceres.h>
+
#include "imu/processor/processor_imu.h"
#include "imu/sensor/sensor_imu.h"
@@ -14,8 +16,6 @@
#include <core/common/wolf.h>
#include <core/sensor/sensor_odom_3d.h>
#include <core/processor/processor_odom_3d.h>
-#include <core/ceres_wrapper/ceres_manager.h>
-
#include <core/utils/utils_gtest.h>
//#include <core/utils/logging.h>
@@ -34,7 +34,7 @@ class Process_Factor_Imu : public testing::Test
public:
// Wolf objects
ProblemPtr problem;
- CeresManagerPtr ceres_manager;
+ SolverCeresPtr solver;
SensorImuPtr sensor_imu;
ProcessorImuPtr processor_imu;
CaptureImuPtr capture_imu;
@@ -105,8 +105,7 @@ class Process_Factor_Imu : public testing::Test
problem = Problem::create("POV", 3);
// CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_manager = make_shared<CeresManager>(problem, ceres_options);
+ solver = make_shared<SolverCeres>(problem);
// SENSOR + PROCESSOR Imu
SensorBasePtr sensor = problem->installSensor ("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
@@ -524,7 +523,7 @@ class Process_Factor_Imu : public testing::Test
string solveProblem(SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::BRIEF)
{
- string report = ceres_manager->solve(verbose);
+ string report = solver->solve(verbose);
bias_0 = C_0->getSensorIntrinsic()->getState();
bias_1 = C_1->getSensorIntrinsic()->getState();
diff --git a/test/gtest_processor_imu.cpp b/test/gtest_processor_imu.cpp
index d04e18cbc06050de3be8259d49b09f8f3bf933a1..9ba48e91aee18e61ab938eba4179ad5de2ea7104 100644
--- a/test/gtest_processor_imu.cpp
+++ b/test/gtest_processor_imu.cpp
@@ -16,9 +16,6 @@
#include "core/utils/logging.h"
#include "core/math/rotations.h"
-#include "core/ceres_wrapper/ceres_manager.h"
-
-
#include <cmath>
#include <iostream>
@@ -61,6 +58,7 @@ class ProcessorImut : public testing::Test
processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
// Time and data variables
+ t.set(0);
data = Vector6d::Zero();
data_cov = Matrix6d::Identity();