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..1103e11bb83cc21fceb7b6f685e3f07b92aa84dd 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");
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 e97b23cf496f0fff56fd25cbcece11ecae9393a0..9f938032bb71d4cf39b83eebdcfeb2617c156b63 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..78ab782bac8b1b14cb8d2615a6abca2446219ea4 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));
diff --git a/src/problem.cpp b/src/problem.cpp
index 5d98f8287fa54cf4914d52b03c6a9b63b7c39ccb..9305ae083559f90c991ca0436778030b543ebe88 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 3cfcfa01e86a6beafa8d3423bb4227290398a15c..13e9af4d22855026f0cffac66dc5f58c033b1505 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -59,11 +59,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 -----------------------------------------
@@ -145,8 +145,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:
@@ -154,23 +156,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
@@ -180,8 +185,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 c2cdd85a370563b5a50254f1e947f76842e3bc33..b7ca8ed8938048147ff6d801f1238c810c4460b0 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
diff --git a/src/test/gtest_constraint_fix_3D.cpp b/src/test/gtest_constraint_fix_3D.cpp
index 68d98b63ec5498703624f1d4ffdc6069768b96d5..6a1636148fb7a3350d875befd2e491b55a2db0ed 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 0b8bde1dcdf2aa6c15a67148ac1a4247dfe74dc3..ccdfeba3a2d2f1c5af46608d5634a6500fa7e385 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 9f1793598ebc9527656bf4af4d7aea16aa467915..896538ae05f857992ac0b862c54ae22ec1bbef80 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);
ceres::Solver::Summary summary;
@@ -191,7 +191,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 d77789c3087779fde462a67c9edc0927df924050..490ce112f781fa678bb3af5133b8563cf74dcf61 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -189,18 +189,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.