diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 04ef3a42e9569220b298cd2bb25cbfb929774675..0f64dcfb417db430ee99311e7747d444d1e25ffa 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -83,7 +83,7 @@ IF(Suitesparse_INCLUDE_DIRS)
MESSAGE("Suitesparse FOUND: wolf_solver will be built.")
ELSE (Suitesparse_INCLUDE_DIRS)
SET(Suitesparse_FOUND FALSE)
- MESSAGE("Suitesparse NOT FOUND: wolf_solver won't be built.")
+ MESSAGE(FATAL_ERROR "Suitesparse NOT FOUND")
ENDIF (Suitesparse_INCLUDE_DIRS)
# Define the directory where will be the configured config.h
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 2c1205d928342f4200257f6b692567f294c97c14..6eb1c89b6592109d2a1c5ee65aef0845e6c3d0fd 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -11,8 +11,12 @@ CeresManager::CeresManager(ProblemPtr _wolf_problem, const ceres::Solver::Option
ceres_options_(_ceres_options)
{
ceres::Covariance::Options covariance_options;
+ #if CERES_VERSION_MINOR >= 10
covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
- covariance_options.num_threads = 8;//ceres::DENSE_SVD;
+ #else
+ covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD;
+ #endif
+ covariance_options.num_threads = 8;
covariance_options.apply_loss_function = false;
//covariance_options.null_space_rank = -1;
covariance_ = new ceres::Covariance(covariance_options);
@@ -40,23 +44,23 @@ CeresManager::~CeresManager()
std::string CeresManager::solve(const unsigned int& _report_level)
{
- //std::cout << "Residual blocks: " << ceres_problem_->NumResidualBlocks() << " Parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
+ //std::cout << "Residual blocks: " << ceres_problem_->NumResidualBlocks() << " Parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
// update problem
- update();
+ update();
//std::cout << "After Update: Residual blocks: " << ceres_problem_->NumResidualBlocks() << " Parameter blocks: " << ceres_problem_->NumParameterBlocks() << std::endl;
- // run Ceres Solver
- ceres::Solve(ceres_options_, ceres_problem_, &summary_);
- //std::cout << "solved" << std::endl;
+ // run Ceres Solver
+ ceres::Solve(ceres_options_, ceres_problem_, &summary_);
+ //std::cout << "solved" << std::endl;
//return report
if (_report_level == 0)
return std::string();
else if (_report_level == 1)
return summary_.BriefReport();
- else if (_report_level == 1)
+ else if (_report_level == 2)
return summary_.FullReport();
else
throw std::invalid_argument( "Report level should be 0, 1 or 2!" );
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index d5b8f6b92c499bb96244316713af60d157e74389..c36144c4ed60cb8f63e840475f3e90be138efd1e 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -39,7 +39,7 @@ class CeresManager : public SolverManager
~CeresManager();
- virtual std::string solve(const unsigned int& _report_level = 0);
+ virtual std::string solve(const unsigned int& _report_level);
ceres::Solver::Summary getSummary();
diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
index b2be568b0bd2fca63d110a178dbed35660e3d8f0..9c5f592ec5728d999f66cc688f73739d080eef86 100644
--- a/src/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -328,8 +328,8 @@ int main(int argc, char** argv)
// SOLVE OPTIMIZATION ---------------------------
std::cout << "SOLVING..." << std::endl;
t1 = clock();
- ceres::Solver::Summary summary = ceres_manager.solve();
- std::cout << summary.FullReport() << std::endl;
+ std::string summary = ceres_manager.solve(2);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::cout << summary << std::endl;
mean_times(3) += ((double) clock() - t1) / CLOCKS_PER_SEC;
// COMPUTE COVARIANCES ---------------------------
diff --git a/src/examples/test_constraint_AHP.cpp b/src/examples/test_constraint_AHP.cpp
index 62264c58b442042ba2027eb6214af36638f7ce31..dc2d89fb9aa871693d5e618259b74615571cb656 100644
--- a/src/examples/test_constraint_AHP.cpp
+++ b/src/examples/test_constraint_AHP.cpp
@@ -21,7 +21,7 @@ int main()
//=====================================================
// Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create(FRM_PO_3D);
+ ProblemPtr wolf_problem_ptr_ = Problem::create("PO 3D");
/* Do this while there aren't extrinsic parameters on the yaml */
Eigen::Vector7s extrinsic_cam;
diff --git a/src/examples/test_image.cpp b/src/examples/test_image.cpp
index 62bb7dbb036c4cb997729f03d8be6a5af6c7bae8..27e893067928f833488e80a4edf917912e274038 100644
--- a/src/examples/test_image.cpp
+++ b/src/examples/test_image.cpp
@@ -70,7 +70,7 @@ int main(int argc, char** argv)
std::string wolf_root = _WOLF_ROOT_DIR;
std::cout << "Wolf root: " << wolf_root << std::endl;
- ProblemPtr wolf_problem_ = Problem::create(FRM_PO_3D);
+ ProblemPtr wolf_problem_ = Problem::create("PO 3D");
//=====================================================
// Method 1: Use data generated here for sensor and processor
diff --git a/src/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp
index b58feb8c86ced07d0cccff0ec64d2cb85d0c3b31..a116b314b9a595e855764874a58c7bf263ad4f30 100644
--- a/src/examples/test_kf_callback.cpp
+++ b/src/examples/test_kf_callback.cpp
@@ -18,7 +18,7 @@ int main()
using namespace Eigen;
using namespace std;
- ProblemPtr problem = Problem::create(FRM_PO_2D);
+ ProblemPtr problem = Problem::create("PO 2D");
SensorBasePtr sen_odo = problem->installSensor ("ODOM 2D", "main odometer", (Vector3s()<<0,0,0).finished(),"");
ProcessorBasePtr prc_odo = problem->installProcessor("ODOM 2D", "odometry integrator", "main odometer", "");
diff --git a/src/examples/test_processor_image_landmark.cpp b/src/examples/test_processor_image_landmark.cpp
index e34747818b141c9bc3ae7b1fa1f37d923337befd..e92a7f8783ec08e75669d7bcca748990115bd39a 100644
--- a/src/examples/test_processor_image_landmark.cpp
+++ b/src/examples/test_processor_image_landmark.cpp
@@ -105,7 +105,7 @@ int main(int argc, char** argv)
//=====================================================
// Wolf problem
- ProblemPtr problem = Problem::create(FRM_PO_3D);
+ ProblemPtr problem = Problem::create("PO 3D");
// ODOM SENSOR AND PROCESSOR
SensorBasePtr sensor_base = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml");
@@ -264,8 +264,8 @@ int main(int argc, char** argv)
if (problem->getTrajectoryPtr()->getFrameList().size() > number_of_KFs)
{
number_of_KFs = problem->getTrajectoryPtr()->getFrameList().size();
- ceres::Solver::Summary summary = ceres_manager.solve();
- std::cout << summary.BriefReport() << std::endl;
+ std::string summary = ceres_manager.solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::cout << summary << std::endl;
}
diff --git a/src/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp
index 14f479c54fdc133519c4289bde35ffdbc0910b37..8117e00c0746dbb6dff8be4d60e99a68abb80e1d 100644
--- a/src/examples/test_processor_imu.cpp
+++ b/src/examples/test_processor_imu.cpp
@@ -73,7 +73,7 @@ int main(int argc, char** argv)
}
// Wolf problem
- ProblemPtr problem_ptr_ = Problem::create(FRM_PQVBB_3D);
+ ProblemPtr problem_ptr_ = Problem::create("PQVBB 3D");
Eigen::VectorXs extrinsics(7);
extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
SensorBasePtr sensor_ptr = problem_ptr_->installSensor("IMU", "Main IMU", extrinsics, IntrinsicsBasePtr());
diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index bec86b8d217b48b524e1ae2085f41cef12fcac72..a495f11d83e23d539eb7547fa5eab746001dec99 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -43,7 +43,7 @@ int main (int argc, char** argv)
}
cout << "Final timestamp tf = " << tf.get() << " s" << endl;
- ProblemPtr problem = Problem::create(FRM_PO_3D);
+ ProblemPtr problem = Problem::create("PO 3D");
ceres::Solver::Options ceres_options;
// ceres_options.max_num_iterations = 1000;
// ceres_options.function_tolerance = 1e-10;
diff --git a/src/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp
index 93934ba708626801c2781e2f8828fbd5770c3f33..15655bce2e5f2553974f5278127117169dcd65dd 100644
--- a/src/examples/test_processor_tracker_feature.cpp
+++ b/src/examples/test_processor_tracker_feature.cpp
@@ -26,7 +26,7 @@ int main()
std::cout << std::endl << "==================== processor tracker feature test ======================" << std::endl;
// Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create(FRM_PO_2D);
+ ProblemPtr wolf_problem_ptr_ = Problem::create("PO 2D");
SensorBasePtr sensor_ptr_ = make_shared<SensorBase>("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
diff --git a/src/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp
index b852416777729f3880d1394ebb3a0535d9c51863..bc5955a56a0cc50f0daffc85b455a9bfa9179898 100644
--- a/src/examples/test_processor_tracker_landmark.cpp
+++ b/src/examples/test_processor_tracker_landmark.cpp
@@ -61,7 +61,7 @@ int main()
std::cout << std::endl << "==================== processor tracker landmark test ======================" << std::endl;
// Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create(FRM_PO_2D);
+ ProblemPtr wolf_problem_ptr_ = Problem::create("PO 2D");
SensorBasePtr sensor_ptr_ = std::make_shared< SensorBase>("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index 9187695cb6b3b72f8bf367bb3975adb68e8643fb..08b76d7ee0343f426f4226cf6d161ff201f078ba 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -94,7 +94,7 @@ int main(int argc, char** argv)
// ============================================================================================================
/* 1 */
- ProblemPtr problem = Problem::create(FRM_PO_3D);
+ ProblemPtr problem = Problem::create("PO 3D");
// One anchor frame to define the lmk, and a copy to make a constraint
FrameBasePtr kf_1 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
FrameBasePtr kf_2 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
@@ -244,8 +244,8 @@ int main(int argc, char** argv)
CeresManager ceres_manager(problem, ceres_options);
- ceres::Solver::Summary summary = ceres_manager.solve();
- std::cout << summary.FullReport() << std::endl;
+ std::string summary = ceres_manager.solve(2);// 0: nothing, 1: BriefReport, 2: FullReport
+ std::cout << summary << std::endl;
// Test of convergence over the lmk params
bool pass = (lmk_2->point() - lmk_1->point()).isMuchSmallerThan(1,Constants::EPS);
diff --git a/src/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp
index 7883a0764d0bbbc2f7b2265fd54ebd4b9f84fa3e..bf797869098964ffe95683a7155e8657d6622b4f 100644
--- a/src/examples/test_sparsification.cpp
+++ b/src/examples/test_sparsification.cpp
@@ -186,7 +186,7 @@ int main(int argc, char** argv)
// Wolf problem
FrameBasePtr last_frame_ptr, frame_from_ptr, frame_to_ptr;
- ProblemPtr bl_problem_ptr = Problem::create(FRM_PO_2D);
+ ProblemPtr bl_problem_ptr = Problem::create("PO_2D");
SensorBasePtr sensor_ptr = bl_problem_ptr->installSensor("ODOM 2D", "Odometry", Eigen::VectorXs::Zero(3), IntrinsicsBasePtr());
// Ceres wrapper
diff --git a/src/problem.cpp b/src/problem.cpp
index 27fd134a3c10802c4dd97792e571e81b46ed583b..c1bc34fa8b0b541dfb8fed4388553051f8b3adce 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -27,7 +27,7 @@ std::string uppercase(std::string s) {for (auto & c: s) c = std::toupper(c); ret
}
-Problem::Problem(FrameStructure _frame_structure) :
+Problem::Problem(const std::string& _frame_structure) :
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
map_ptr_(std::make_shared<MapBase>()),
@@ -44,7 +44,7 @@ void Problem::setup()
map_ptr_->setProblem(shared_from_this());
}
-ProblemPtr Problem::create(FrameStructure _frame_structure)
+ProblemPtr Problem::create(const std::string& _frame_structure)
{
ProblemPtr p(new Problem(_frame_structure)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passes to `make_shared`.
p->setup();
@@ -178,53 +178,6 @@ void Problem::clearProcessorMotion()
processor_motion_ptr_.reset();
}
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_type, const TimeStamp& _time_stamp)
-{
- return emplaceFrame(_frame_type, getState(_time_stamp), _time_stamp);
-}
-
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, const Eigen::VectorXs& _frame_state,
- const TimeStamp& _time_stamp)
-{
- assert(_frame_state.size() == getFrameStructureSize() && "Wrong state vector size");
-
- //std::cout << "Problem::createFrame" << std::endl;
- // ---------------------- CREATE NEW FRAME ---------------------
- // Create frame
- switch (trajectory_ptr_->getFrameStructure())
- {
- case FRM_PO_2D:
- {
- assert(_frame_state.size() == 3 && "Wrong state vector size. Use 3 for 2D pose.");
- return trajectory_ptr_->addFrame(FrameBase::create_PO_2D(_frame_key_type, _time_stamp, _frame_state));
-// return trajectory_ptr_->addFrame(std::make_shared<FrameBase>(_frame_key_type, _time_stamp, std::make_shared<StateBlock>(_frame_state.head(2)),
-// std::make_shared<StateBlock>(_frame_state.tail(1))));
- }
- case FRM_PO_3D:
- {
- assert(_frame_state.size() == 7 && "Wrong state vector size. Use 7 for 3D pose.");
- return trajectory_ptr_->addFrame(FrameBase::create_PO_3D(_frame_key_type, _time_stamp, _frame_state));
-// return trajectory_ptr_->addFrame(std::make_shared<FrameBase>(_frame_key_type, _time_stamp, std::make_shared<StateBlock>(_frame_state.head(3)),
-// std::make_shared<StateQuaternion>(_frame_state.tail(4))));
- }
- case FRM_POV_3D:
- {
- assert(_frame_state.size() == 10 && "Wrong state vector size. Use 10 for 3D pose and velocity.");
- return trajectory_ptr_->addFrame(FrameBase::create_POV_3D(_frame_key_type, _time_stamp, _frame_state));
-// return trajectory_ptr_->addFrame(std::make_shared<FrameBase>(_frame_key_type, _time_stamp, std::make_shared<StateBlock>(_frame_state.head(3)),
-// std::make_shared<StateQuaternion>(_frame_state.segment<4>(3)),
-// std::make_shared<StateBlock>(_frame_state.tail(3))));
- }
- case FRM_PQVBB_3D:
- {
- assert(_frame_state.size() == 16 && "Wrong state vector size. Use 16 for 3D pose, velocity, and IMU biases.");
- return trajectory_ptr_->addFrame(std::make_shared<FrameIMU>(_frame_key_type, _time_stamp, _frame_state));
- }
- default:
- throw std::runtime_error(
- "Unknown frame structure. Add appropriate frame structure to the switch statement.");
- }
-}
FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, FrameType _frame_key_type,
const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp)
@@ -234,6 +187,20 @@ FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, FrameTyp
return frm;
}
+FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, FrameType _frame_key_type, const TimeStamp& _time_stamp)
+{
+ return emplaceFrame(_frame_structure, _frame_key_type, getState(_time_stamp), _time_stamp);
+}
+
+FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp)
+{
+ return emplaceFrame(trajectory_ptr_->getFrameStructure(), _frame_key_type, _frame_state, _time_stamp);
+}
+
+FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, const TimeStamp& _time_stamp)
+{
+ return emplaceFrame(trajectory_ptr_->getFrameStructure(), _frame_key_type, _time_stamp);
+}
Eigen::VectorXs Problem::getCurrentState()
{
@@ -306,42 +273,43 @@ Eigen::VectorXs Problem::getState(const TimeStamp& _ts)
Size Problem::getFrameStructureSize() const
{
- switch (trajectory_ptr_->getFrameStructure())
- {
- case FRM_PO_2D:
- return 3;
- case FRM_PO_3D:
- return 7;
- case FRM_POV_3D:
- return 10;
- case FRM_PQVBB_3D:
- return 16;
- default:
- throw std::runtime_error(
- "Problem::getFrameStructureSize(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
- }
+ if (trajectory_ptr_->getFrameStructure() == "PO 2D")
+ return 3;
+ if (trajectory_ptr_->getFrameStructure() == "PO 3D")
+ return 7;
+ if (trajectory_ptr_->getFrameStructure() == "POV 3D")
+ return 10;
+ if (trajectory_ptr_->getFrameStructure() == "PQVBB 3D")
+ return 16;
+ throw std::runtime_error(
+ "Problem::getFrameStructureSize(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
}
void Problem::getFrameStructureSize(Size& _x_size, Size& _cov_size) const
{
- switch (trajectory_ptr_->getFrameStructure())
+ if (trajectory_ptr_->getFrameStructure() == "PO 2D")
{
- case FRM_PO_2D:
- _x_size = 3; _cov_size = 3;
- break;
- case FRM_PO_3D:
- _x_size = 7; _cov_size = 6;
- break;
- case FRM_POV_3D:
- _x_size = 10; _cov_size = 10;
- break;
- case FRM_PQVBB_3D:
- _x_size = 16; _cov_size = 15;
- break;
- default:
- throw std::runtime_error(
- "Problem::getFrameStructureSize(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
+ _x_size = 3;
+ _cov_size = 3;
+ }
+ else if (trajectory_ptr_->getFrameStructure() == "PO 3D")
+ {
+ _x_size = 7;
+ _cov_size = 6;
}
+ else if (trajectory_ptr_->getFrameStructure() == "POV 3D")
+ {
+ _x_size = 10;
+ _cov_size = 10;
+ }
+ else if (trajectory_ptr_->getFrameStructure() == "PQVBB 3D")
+ {
+ _x_size = 16;
+ _cov_size = 15;
+ }
+ else
+ throw std::runtime_error(
+ "Problem::getFrameStructureSize(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
}
Eigen::VectorXs Problem::zeroState()
@@ -349,18 +317,10 @@ Eigen::VectorXs Problem::zeroState()
Eigen::VectorXs state = Eigen::VectorXs::Zero(getFrameStructureSize());
// Set the quaternion identity for 3D states. Set only the real part to 1:
- switch (trajectory_ptr_->getFrameStructure())
- {
- case FRM_PO_2D:
- break;
- case FRM_PO_3D:
- case FRM_POV_3D:
- case FRM_PQVBB_3D:
- state(6) = 1.0;
- break;
- default:
- break;
- }
+ if (trajectory_ptr_->getFrameStructure() == "PO 3D" ||
+ trajectory_ptr_->getFrameStructure() == "POV 3D"||
+ trajectory_ptr_->getFrameStructure() == "PQVBB 3D")
+ state(6) = 1.0;
return state;
}
diff --git a/src/problem.h b/src/problem.h
index 2ee0c14295d27fc7223f8def2e6aa996cb2f812b..796a37dca8087c10618070cd1538300c5ca827f8 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -57,11 +57,11 @@ class Problem : public std::enable_shared_from_this<Problem>
bool origin_is_set_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
- Problem(FrameStructure _frame_structure); // USE create() below !!
+ Problem(const std::string& _frame_structure); // USE create() below !!
void setup();
public:
- static ProblemPtr create(FrameStructure _frame_structure); // USE THIS AS A CONSTRUCTOR!
+ static ProblemPtr create(const std::string& _frame_structure); // USE THIS AS A CONSTRUCTOR!
virtual ~Problem();
// Properties -----------------------------------------
@@ -143,8 +143,10 @@ class Problem : public std::enable_shared_from_this<Problem>
virtual FrameBasePtr setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov,
const TimeStamp& _ts);
- /** \brief Emplace Frame of the correct size
+ /** \brief Emplace frame from string frame_structure
+ * \param _frame_structure String indicating the frame structure.
* \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
+ * \param _frame_state State vector; must match the size and format of the chosen frame structure
* \param _time_stamp Time stamp of the frame
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
@@ -152,23 +154,26 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(FrameType _frame_key_type, const TimeStamp& _time_stamp);
+ FrameBasePtr emplaceFrame(const std::string& _frame_structure,
+ FrameType _frame_key_type,
+ const Eigen::VectorXs& _frame_state,
+ const TimeStamp& _time_stamp);
- /** \brief Emplace Frame from vector
+ /** \brief Emplace frame from string frame_structure without state
+ * \param _frame_structure String indicating the frame structure.
* \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
- * \param _frame_state State vector; must match the size and format of the chosen frame structure
* \param _time_stamp Time stamp of the frame
*
- * This acts as a Frame factory, but also takes care to update related lists in WolfProblem
+ * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(FrameType _frame_key_type, const Eigen::VectorXs& _frame_state,
- const TimeStamp& _time_stamp);
+ FrameBasePtr emplaceFrame(const std::string& _frame_structure,
+ FrameType _frame_key_type,
+ const TimeStamp& _time_stamp);
- /** \brief Emplace frame fron string frame_structure
- * \param _frame_structure String indicating the frame structure.
+ /** \brief Emplace frame from string frame_structure without structure
* \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
* \param _frame_state State vector; must match the size and format of the chosen frame structure
* \param _time_stamp Time stamp of the frame
@@ -178,8 +183,21 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(const std::string& _frame_structure, FrameType _frame_key_type, const Eigen::VectorXs& _frame_state,
- const TimeStamp& _time_stamp);
+ FrameBasePtr emplaceFrame(FrameType _frame_key_type,
+ const Eigen::VectorXs& _frame_state,
+ const TimeStamp& _time_stamp);
+
+ /** \brief Emplace frame from string frame_structure without structure nor state
+ * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
+ * \param _time_stamp Time stamp of the frame
+ *
+ * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
+ * - Create a Frame
+ * - Add it to Trajectory
+ * - If it is key-frame, update state-block lists in Problem
+ */
+ FrameBasePtr emplaceFrame(FrameType _frame_key_type,
+ const TimeStamp& _time_stamp);
// State getters
Eigen::VectorXs getCurrentState();
diff --git a/src/test/gtest_constraint_fix.cpp b/src/test/gtest_constraint_fix.cpp
index a9543d6acf31988744c6c86bb3335ef54bf121cf..770bf678db6d15e759bf5de7157d05276da838b3 100644
--- a/src/test/gtest_constraint_fix.cpp
+++ b/src/test/gtest_constraint_fix.cpp
@@ -28,7 +28,7 @@ Matrix3s data_cov = data_var.asDiagonal();
Vector3s x0 = pose6 + Vector3s::Random()*0.25;
// Problem and solver
-ProblemPtr problem = Problem::create(FRM_PO_2D);
+ProblemPtr problem = Problem::create("PO 2D");
CeresManager ceres_mgr(problem);
// Two frames
@@ -60,7 +60,7 @@ TEST(ConstraintFix, solve)
frm0->setState(x0);
// solve for frm0
- std::string report = ceres_mgr.solve();
+ std::string report = ceres_mgr.solve(0); // 0: nothing, 1: BriefReport, 2: FullReport
ASSERT_MATRIX_APPROX(frm0->getState(), pose6, 1e-6);
diff --git a/src/test/gtest_constraint_fix_3D.cpp b/src/test/gtest_constraint_fix_3D.cpp
index ef0fcc13eb8c1c95bf5d4fe316bde214ad2261ea..16c3498244d336fb3dfcf797d88e5777f02ab083 100644
--- a/src/test/gtest_constraint_fix_3D.cpp
+++ b/src/test/gtest_constraint_fix_3D.cpp
@@ -35,7 +35,7 @@ Matrix6s data_cov = data_var.asDiagonal();
Vector7s x0 = pose6toPose7(pose6 + Vector6s::Random()*0.25);
// Problem and solver
-ProblemPtr problem = Problem::create(FRM_PO_3D);
+ProblemPtr problem = Problem::create("PO 3D");
CeresManager ceres_mgr(problem);
// Two frames
diff --git a/src/test/gtest_constraint_odom_3D.cpp b/src/test/gtest_constraint_odom_3D.cpp
index f5010d1ebe450684b2700eb86d6bdbdac2489e70..1dea78dc865e10d3da688655af0908538aab6e0b 100644
--- a/src/test/gtest_constraint_odom_3D.cpp
+++ b/src/test/gtest_constraint_odom_3D.cpp
@@ -35,7 +35,7 @@ Vector7s x0 = data2delta(Vector6s::Random()*0.25);
Vector7s x1 = data2delta(data + Vector6s::Random()*0.25);
// Problem and solver
-ProblemPtr problem = Problem::create(FRM_PO_3D);
+ProblemPtr problem = Problem::create("PO 3D");
CeresManager ceres_mgr(problem);
// Two frames
diff --git a/src/test/gtest_frame_base.cpp b/src/test/gtest_frame_base.cpp
index 9f60d5c17fa14a1ba9bb431f9ddf8de07ef6c8c4..06d6cb6d453fcb6be4e4f44ecc240187977f45c4 100644
--- a/src/test/gtest_frame_base.cpp
+++ b/src/test/gtest_frame_base.cpp
@@ -65,7 +65,7 @@ TEST(FrameBase, LinksBasic)
TEST(FrameBase, LinksToTree)
{
// Problem with 2 frames and one motion constraint between them
- ProblemPtr P = Problem::create(FRM_PO_2D);
+ ProblemPtr P = Problem::create("PO 2D");
TrajectoryBasePtr T = P->getTrajectoryPtr();
SensorOdom2DPtr S = make_shared<SensorOdom2D>(make_shared<StateBlock>(2), make_shared<StateBlock>(1), 1,1);
P->getHardwarePtr()->addSensor(S);
diff --git a/src/test/gtest_odom_2D.cpp b/src/test/gtest_odom_2D.cpp
index 7780a73f20d8e8bc18dbf9e5107b16ec0623ed64..19b0e1c8b96447e442b3f983e9d345da239c047f 100644
--- a/src/test/gtest_odom_2D.cpp
+++ b/src/test/gtest_odom_2D.cpp
@@ -123,7 +123,7 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
Vector3s delta (2,0,0);
Matrix3s delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
- ProblemPtr Pr = Problem::create(FRM_PO_2D);
+ ProblemPtr Pr = Problem::create("PO 2D");
CeresManager ceres_manager(Pr);
// KF0 and absolute prior
@@ -190,7 +190,7 @@ TEST(Odom2D, VoteForKfAndSolve)
int N = 7; // number of process() steps
// Create Wolf tree nodes
- ProblemPtr problem = Problem::create(FRM_PO_2D);
+ ProblemPtr problem = Problem::create("PO 2D");
SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0));
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
params->dist_traveled_th_ = 100;
@@ -308,7 +308,7 @@ TEST(Odom2D, KF_callback)
// Create Wolf tree nodes
- ProblemPtr problem = Problem::create(FRM_PO_2D);
+ ProblemPtr problem = Problem::create("PO 2D");
SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0));
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
params->dist_traveled_th_ = 100; // don't make keyframes
diff --git a/src/test/gtest_problem.cpp b/src/test/gtest_problem.cpp
index c4da0e270da6e8669b4aa84e66144a9559e57b29..314c406c5f5ee92a1fab3853e6177b872954d878 100644
--- a/src/test/gtest_problem.cpp
+++ b/src/test/gtest_problem.cpp
@@ -20,7 +20,7 @@ using namespace Eigen;
TEST(Problem, create)
{
- ProblemPtr P = Problem::create(FRM_PQVBB_3D);
+ ProblemPtr P = Problem::create("PQVBB 3D");
// check double ointers to branches
ASSERT_EQ(P, P->getHardwarePtr()->getProblem());
@@ -33,7 +33,7 @@ TEST(Problem, create)
TEST(Problem, Sensors)
{
- ProblemPtr P = Problem::create(FRM_PQVBB_3D);
+ ProblemPtr P = Problem::create("PQVBB 3D");
// add a dummy sensor
SensorBasePtr S = std::make_shared<SensorBase>("Dummy", nullptr, nullptr, nullptr, 2, false);
@@ -47,7 +47,7 @@ TEST(Problem, Sensors)
TEST(Problem, Processor)
{
- ProblemPtr P = Problem::create(FRM_PO_3D);
+ ProblemPtr P = Problem::create("PO 3D");
// check motion processor is null
ASSERT_FALSE(P->getProcessorMotionPtr());
@@ -72,7 +72,7 @@ TEST(Problem, Processor)
TEST(Problem, Installers)
{
std::string wolf_root = _WOLF_ROOT_DIR;
- ProblemPtr P = Problem::create(FRM_PO_3D);
+ ProblemPtr P = Problem::create("PO 3D");
Eigen::Vector7s xs;
SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
@@ -95,7 +95,7 @@ TEST(Problem, Installers)
TEST(Problem, SetOrigin_PO_2D)
{
- ProblemPtr P = Problem::create(FRM_PO_2D);
+ ProblemPtr P = Problem::create("PO 2D");
TimeStamp t0(0);
Eigen::VectorXs x0(3); x0 << 1,2,3;
Eigen::MatrixXs P0(3,3); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
@@ -134,7 +134,7 @@ TEST(Problem, SetOrigin_PO_2D)
TEST(Problem, SetOrigin_PO_3D)
{
- ProblemPtr P = Problem::create(FRM_PO_3D);
+ ProblemPtr P = Problem::create("PO 3D");
TimeStamp t0(0);
Eigen::VectorXs x0(7); x0 << 1,2,3,4,5,6,7;
Eigen::MatrixXs P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
@@ -174,7 +174,7 @@ TEST(Problem, SetOrigin_PO_3D)
TEST(Problem, emplaceFrame_factory)
{
- ProblemPtr P = Problem::create(FRM_PO_2D);
+ ProblemPtr P = Problem::create("PO 2D");
FrameBasePtr f0 = P->emplaceFrame("PO 2D", KEY_FRAME, VectorXs(3), TimeStamp(0.0));
FrameBasePtr f1 = P->emplaceFrame("PO 3D", KEY_FRAME, VectorXs(7), TimeStamp(1.0));
@@ -210,7 +210,7 @@ TEST(Problem, StateBlocks)
{
std::string wolf_root = _WOLF_ROOT_DIR;
- ProblemPtr P = Problem::create(FRM_PO_3D);
+ ProblemPtr P = Problem::create("PO 3D");
Eigen::Vector7s xs;
// 2 state blocks, fixed
@@ -246,7 +246,7 @@ TEST(Problem, Covariances)
{
std::string wolf_root = _WOLF_ROOT_DIR;
- ProblemPtr P = Problem::create(FRM_PO_3D);
+ ProblemPtr P = Problem::create("PO 3D");
Eigen::Vector7s xs;
SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
diff --git a/src/test/gtest_processor_imu.cpp b/src/test/gtest_processor_imu.cpp
index 7d26d96ac590bf1a85018ac574b72d0f3d597815..d7cc239338c7ba230c5624ef10584eb26a50eb09 100644
--- a/src/test/gtest_processor_imu.cpp
+++ b/src/test/gtest_processor_imu.cpp
@@ -27,7 +27,7 @@ using namespace wolf::Constants;
// Wolf problem
-ProblemPtr problem = Problem::create(FRM_PQVBB_3D);
+ProblemPtr problem = Problem::create("PQVBB 3D");
Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, IntrinsicsBasePtr());
ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
diff --git a/src/test/gtest_trajectory.cpp b/src/test/gtest_trajectory.cpp
index 1ca871af231c0b3f7fbe7c0cbb42011c9ed217df..f6835809e5074b2bcf30c789c8e38aaff0e268bb 100644
--- a/src/test/gtest_trajectory.cpp
+++ b/src/test/gtest_trajectory.cpp
@@ -23,7 +23,7 @@ bool debug = false;
TEST(TrajectoryBase, ClosestKeyFrame)
{
- ProblemPtr P = Problem::create(FRM_PO_2D);
+ ProblemPtr P = Problem::create("PO 2D");
TrajectoryBasePtr T = P->getTrajectoryPtr();
// Trajectory status:
@@ -60,7 +60,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
{
using std::make_shared;
- ProblemPtr P = Problem::create(FRM_PO_2D);
+ ProblemPtr P = Problem::create("PO 2D");
TrajectoryBasePtr T = P->getTrajectoryPtr();
// Trajectory status:
@@ -124,7 +124,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
{
using std::make_shared;
- ProblemPtr P = Problem::create(FRM_PO_2D);
+ ProblemPtr P = Problem::create("PO 2D");
TrajectoryBasePtr T = P->getTrajectoryPtr();
// Trajectory status:
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index dade698f2aab162109a2449c1b134979d475b198..afade396af94c8e18eac6bcf329972fdd1c2bd8f 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -4,9 +4,10 @@
namespace wolf {
-TrajectoryBase::TrajectoryBase(FrameStructure _frame_structure) :
+TrajectoryBase::TrajectoryBase(const std::string& _frame_structure) :
NodeBase("TRAJECTORY"),
- frame_structure_(_frame_structure), last_key_frame_ptr_(nullptr)
+ frame_structure_(_frame_structure),
+ last_key_frame_ptr_(nullptr)
{
// WOLF_DEBUG("constructed T");
}
diff --git a/src/trajectory_base.h b/src/trajectory_base.h
index bd7553b100948082c4da7e2ad752303f9b59d56e..18c63e39e77f4f7d647dd4b03980e895ea6cc861 100644
--- a/src/trajectory_base.h
+++ b/src/trajectory_base.h
@@ -15,7 +15,6 @@ class TimeStamp;
//std includes
-
namespace wolf {
//class TrajectoryBase
@@ -25,15 +24,15 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
std::list<FrameBasePtr> frame_list_;
protected:
- FrameStructure frame_structure_; // Defines the structure of the Frames in the Trajectory.
+ std::string frame_structure_; // Defines the structure of the Frames in the Trajectory.
FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame
public:
- TrajectoryBase(FrameStructure _frame_sturcture);
+ TrajectoryBase(const std::string& _frame_sturcture);
virtual ~TrajectoryBase();
// Properties
- FrameStructure getFrameStructure() const;
+ std::string getFrameStructure() const;
// Frames
FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
@@ -84,7 +83,7 @@ inline void TrajectoryBase::setLastKeyFramePtr(FrameBasePtr _key_frame_ptr)
}
-inline FrameStructure TrajectoryBase::getFrameStructure() const
+inline std::string TrajectoryBase::getFrameStructure() const
{
return frame_structure_;
}
diff --git a/src/wolf.h b/src/wolf.h
index 555b1f7bd61e606d27cb1abe57f775063e15fd66..257f71c5eb2a338d5106d761942dd20b8255d3c3 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -192,18 +192,6 @@ typedef enum
KEY_FRAME = 1 ///< key frame. It will stay in the frames window and play at optimizations.
} FrameType;
-/** \brief Enumeration of all possible frames
- *
- * You may add items to this list as needed. Be concise with names, and document your entries.
- */
-typedef enum
-{
- FRM_PO_2D = 1, ///< 2D frame containing position (x,y) and orientation angle.
- FRM_PO_3D, ///< 3D frame containing position (x,y,z) and orientation quaternion (qx,qy,qz,qw).
- FRM_POV_3D, ///< 3D frame with position, orientation quaternion, and linear velocity (vx,vy,vz)
- FRM_PQVBB_3D ///< 3D frame with pos, orient quat, velocity, acc bias (abx,aby,abz), and gyro bias (wbx,wby,wbz).
-} FrameStructure;
-
/** \brief Enumeration of all possible constraints
*
* You may add items to this list as needed. Be concise with names, and document your entries.