diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 3708f51d3aa1141756dc54c27f76039671bfa42e..e0d6187408d8c621e2fc8f2a4f94b0202d6b1546 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -104,7 +104,7 @@ int main()
using namespace wolf;
// Wolf problem and solver
- ProblemPtr problem = Problem::create("PO 2D");
+ ProblemPtr problem = Problem::create("PO", 2);
ceres::Solver::Options options;
options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options);
diff --git a/include/base/frame/frame_base.h b/include/base/frame/frame_base.h
index fa6f1c29ecb8c19ffa3d27af8681215b7db57f59..0dadab694990a8d684d66c5b2c068e9315d3c482 100644
--- a/include/base/frame/frame_base.h
+++ b/include/base/frame/frame_base.h
@@ -64,6 +64,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
**/
FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr);
+ FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x);
+
virtual ~FrameBase();
virtual void remove();
diff --git a/include/base/problem/problem.h b/include/base/problem/problem.h
index 938074f78020ebff96905714d70d7f1259a168e1..c964a4d846cec1e028907e5ff3755360df6558be 100644
--- a/include/base/problem/problem.h
+++ b/include/base/problem/problem.h
@@ -47,19 +47,22 @@ class Problem : public std::enable_shared_from_this<Problem>
mutable std::mutex mut_state_block_notifications_;
mutable std::mutex mut_covariances_;
bool prior_is_set_;
+ std::string frame_structure_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
Problem(const std::string& _frame_structure); // USE create() below !!
+ Problem(const std::string& _frame_structure, SizeEigen _dim); // USE create() below !!
void setup();
public:
- static ProblemPtr create(const std::string& _frame_structure); // USE THIS AS A CONSTRUCTOR!
+ static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim); // USE THIS AS A CONSTRUCTOR!
virtual ~Problem();
// Properties -----------------------------------------
SizeEigen getFrameStructureSize() const;
void getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const;
SizeEigen getDim() const;
+ std::string getFrameStructure() const;
// Hardware branch ------------------------------------
HardwareBasePtr getHardware();
@@ -138,6 +141,7 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief Emplace frame from string frame_structure
* \param _frame_structure String indicating the frame structure.
+ * \param _dim variable indicating the dimension of the problem
* \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
@@ -148,12 +152,14 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const std::string& _frame_structure, //
+ const SizeEigen _dim, //
FrameType _frame_key_type, //
const Eigen::VectorXs& _frame_state, //
const TimeStamp& _time_stamp);
/** \brief Emplace frame from string frame_structure without state
* \param _frame_structure String indicating the frame structure.
+ * \param _dim variable indicating the dimension of the problem
* \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
* \param _time_stamp Time stamp of the frame
*
@@ -163,6 +169,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const std::string& _frame_structure, //
+ const SizeEigen _dim, //
FrameType _frame_key_type, //
const TimeStamp& _time_stamp);
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
index fc45eea814bc120ab3d7ec44867fcda407c3d4e5..5f8fc95429cca302d4e80293325c66c4e74854ed 100644
--- a/src/capture/capture_pose.cpp
+++ b/src/capture/capture_pose.cpp
@@ -22,6 +22,7 @@ void CapturePose::emplaceFeatureAndFactor()
// addFeature(feature_pose);
auto feature_pose = FeatureBase::emplace<FeaturePose>(shared_from_this(), data_, data_covariance_);
+ std::cout << data_.size() << " ~~ " << data_covariance_.rows() << "x" << data_covariance_.cols() << std::endl;
// Emplace factor
if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3 )
FactorBase::emplace<FactorPose2D>(feature_pose, feature_pose);
diff --git a/src/examples/test_diff_drive.cpp b/src/examples/test_diff_drive.cpp
index 23980071f58e0e30bc3eeebef8c6d51106e2adf2..561cb26764601f50108f292ae2a4bd6bb3aae645 100644
--- a/src/examples/test_diff_drive.cpp
+++ b/src/examples/test_diff_drive.cpp
@@ -163,7 +163,7 @@ int main(int argc, char** argv)
}
// Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("PO 2D");
+ ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2);
const std::string sensor_name("Main Odometer");
Eigen::VectorXs extrinsics(3);
diff --git a/src/examples/test_factor_AHP.cpp b/src/examples/test_factor_AHP.cpp
index 75031dff808230fb6ff00b8633699960a6e361e6..bd1ad546de08195e53d2608e2032ac1229f6142f 100644
--- a/src/examples/test_factor_AHP.cpp
+++ b/src/examples/test_factor_AHP.cpp
@@ -21,7 +21,7 @@ int main()
//=====================================================
// Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("PO 3D");
+ ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 3);
/* 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 b2b61f5207d29021982729d795978ba73a42bed4..15bdab59916abd1d9a29a8c897f8345d0eafc950 100644
--- a/src/examples/test_image.cpp
+++ b/src/examples/test_image.cpp
@@ -52,7 +52,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("PO 3D");
+ ProblemPtr wolf_problem_ = Problem::create("PO", 3);
//=====================================================
// 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 05bd165977ac79ac6b6b4f61d6a72cb83d6e2eff..9fbc4991b1c27711d5fb62eaa33445269d03919f 100644
--- a/src/examples/test_kf_callback.cpp
+++ b/src/examples/test_kf_callback.cpp
@@ -16,7 +16,7 @@ int main()
using namespace Eigen;
using namespace std;
- ProblemPtr problem = Problem::create("PO 2D");
+ ProblemPtr problem = Problem::create("PO", 2);
SensorBasePtr sen_odo = problem->installSensor ("ODOM 2D", "main odometer", (Vector3s()<<0,0,0).finished(),"");
ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
diff --git a/src/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp
index 2dd5ab64d964e3621eb671d03ef5e8cf179256cc..159ce8532349020a8f04107719f5f810db6da651 100644
--- a/src/examples/test_map_yaml.cpp
+++ b/src/examples/test_map_yaml.cpp
@@ -75,7 +75,7 @@ int main()
std::string wolf_config = wolf_root + "/src/examples";
std::cout << "\nWolf directory for configuration files: " << wolf_config << std::endl;
- ProblemPtr problem = Problem::create("PO 2D");
+ ProblemPtr problem = Problem::create("PO", 2);
filename = wolf_config + "/map_polyline_example.yaml";
std::cout << "Reading map from file: " << filename << std::endl;
problem->loadMap(filename);
diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index 79798150de9d17ece60fbcec7020e5693080d6b6..2427dc24b0b55da36640f9175bcdc7a6eea0b4bc 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -40,7 +40,7 @@ int main (int argc, char** argv)
}
cout << "Final timestamp tf = " << tf.get() << " s" << endl;
- ProblemPtr problem = Problem::create("PO 3D");
+ ProblemPtr problem = Problem::create("PO", 3);
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 77e2b35e9956b01622d81f8ca1e7cdae0b60c25e..eccd7b49e4d920ed03db8fff1a2963fbac5b93fa 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("PO 2D");
+ ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2);
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 433bd153be7061e2cbfe55f2e10dba658d8d191d..b3ce8be59ead808976a1909982ea307e2ab675ef 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("PO 2D");
+ ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 2);
// 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_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp
index be7df536e971273559252aeff8dc51bd808bb14b..bbcc96a732610cca9116e0dcb16040020f133a38 100644
--- a/src/examples/test_processor_tracker_landmark_image.cpp
+++ b/src/examples/test_processor_tracker_landmark_image.cpp
@@ -80,7 +80,7 @@ int main(int argc, char** argv)
//=====================================================
// Wolf problem
- ProblemPtr problem = Problem::create("PO 3D");
+ ProblemPtr problem = Problem::create("PO", 3);
// 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_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index 6bd1805dc78a0c65bd27ca022fbb46bd272cf6d4..094156deacf68d88a49c7c150c6e4380638292d6 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -95,7 +95,7 @@ int main(int argc, char** argv)
// ============================================================================================================
/* 1 */
- ProblemPtr problem = Problem::create("PO 3D");
+ ProblemPtr problem = Problem::create("PO", 3);
// One anchor frame to define the lmk, and a copy to make a factor
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/frame/frame_base.cpp b/src/frame/frame_base.cpp
index c01462c1ce98f9a72c5247faec316ecbb5cce39a..544aa9c9e64f79388a6d812c4e037917c4cd7962 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -37,6 +37,51 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
state_block_vec_[2] = _v_ptr;
}
+FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) :
+ NodeBase("FRAME", "Base"),
+ trajectory_ptr_(),
+ state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
+ frame_id_(++frame_id_count_),
+ type_(_tp),
+ time_stamp_(_ts)
+{
+ if(_frame_structure.compare("PO") == 0 and _dim == 2){
+ // auto _x = Eigen::VectorXs::Zero(3);
+ assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2D!");
+ StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) );
+ StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((Scalar) _x(2) ) );
+ StateBlockPtr v_ptr ( nullptr );
+ state_block_vec_[0] = p_ptr;
+ state_block_vec_[1] = o_ptr;
+ state_block_vec_[2] = v_ptr;
+ this->setType("PO 2D");
+ }else if(_frame_structure.compare("PO") == 0 and _dim == 3){
+ // auto _x = Eigen::VectorXs::Zero(7);
+ assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3D!");
+ StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
+ StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) );
+ StateBlockPtr v_ptr ( nullptr );
+ state_block_vec_[0] = p_ptr;
+ state_block_vec_[1] = o_ptr;
+ state_block_vec_[2] = v_ptr;
+ this->setType("PO 3D");
+ }else if(_frame_structure.compare("POV") == 0 and _dim == 3){
+ // auto _x = Eigen::VectorXs::Zero(10);
+ assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for 3D!");
+ StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
+ StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) );
+ StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.tail <3> ( ) ) );
+ state_block_vec_[0] = p_ptr;
+ state_block_vec_[1] = o_ptr;
+ state_block_vec_[2] = v_ptr;
+ this->setType("POV 3D");
+ }else{
+ std::cout << _frame_structure << " ^^ " << _dim << std::endl;
+ throw std::runtime_error("Unknown frame structure");
+ }
+
+}
+
FrameBase::~FrameBase()
{
if ( isKey() )
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 04da40061aac22df86701fcebf6a84f5b46b38a8..d6e66ed571435a1c11d8faf6954f9441fdb836c3 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -27,28 +27,27 @@ namespace
std::string uppercase(std::string s) {for (auto & c: s) c = std::toupper(c); return s;}
}
-Problem::Problem(const std::string& _frame_structure) :
+Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
map_ptr_(std::make_shared<MapBase>()),
processor_motion_ptr_(),
- prior_is_set_(false)
+ prior_is_set_(false),
+ frame_structure_(_frame_structure)
{
- if (_frame_structure == "PO 2D")
+ if (_frame_structure == "PO" and _dim == 2)
{
state_size_ = 3;
state_cov_size_ = 3;
dim_ = 2;
- }
-
- else if (_frame_structure == "PO 3D")
+ }else if (_frame_structure == "PO" and _dim == 3)
{
state_size_ = 7;
state_cov_size_ = 6;
dim_ = 3;
- }
- else if (_frame_structure == "POV 3D")
+ } else if (_frame_structure == "POV" and _dim == 3)
{
+ std::cout << "HOLA" << std::endl;
state_size_ = 10;
state_cov_size_ = 9;
dim_ = 3;
@@ -65,9 +64,9 @@ void Problem::setup()
map_ptr_ -> setProblem(shared_from_this());
}
-ProblemPtr Problem::create(const std::string& _frame_structure)
+ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
{
- ProblemPtr p(new Problem(_frame_structure)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
+ ProblemPtr p(new Problem(_frame_structure, _dim)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
p->setup();
return p->shared_from_this();
}
@@ -213,34 +212,40 @@ void Problem::clearProcessorMotion()
}
FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
+ const SizeEigen _dim, //
FrameType _frame_key_type, //
const Eigen::VectorXs& _frame_state, //
const TimeStamp& _time_stamp)
{
- FrameBasePtr frm = FrameFactory::get().create(_frame_structure, _frame_key_type, _time_stamp, _frame_state);
- // trajectory_ptr_->addFrame(frm);
- frm->link(trajectory_ptr_);
+ // FrameBasePtr frm = FrameFactory::get().create(_frame_structure, _frame_key_type, _time_stamp, _frame_state);
+ // FrameBasePtr frm = std::make_shared<FrameBase>(_frame_structure, _dim, _frame_key_type, _time_stamp, _frame_state);
+ // // trajectory_ptr_->addFrame(frm);
+ // frm->link(trajectory_ptr_);
+ auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_, _frame_structure, _dim, _frame_key_type, _time_stamp, _frame_state);
return frm;
}
FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
+ const SizeEigen _dim, //
FrameType _frame_key_type, //
const TimeStamp& _time_stamp)
{
- return emplaceFrame(_frame_structure, _frame_key_type, getState(_time_stamp), _time_stamp);
+ return emplaceFrame(_frame_structure, _dim, _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);
+ // return emplaceFrame(trajectory_ptr_->getFrameStructure(), _frame_key_type, _frame_state, _time_stamp);
+ return emplaceFrame(this->getFrameStructure(), this->getDim(), _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);
+ // return emplaceFrame(trajectory_ptr_->getFrameStructure(), _frame_key_type, _time_stamp);
+ return emplaceFrame(this->getFrameStructure(), this->getDim(), _frame_key_type, _time_stamp);
}
Eigen::VectorXs Problem::getCurrentState()
@@ -311,6 +316,10 @@ SizeEigen Problem::getDim() const
{
return dim_;
}
+std::string Problem::getFrameStructure() const
+{
+ return frame_structure_;
+}
Eigen::VectorXs Problem::zeroState()
{
@@ -665,7 +674,7 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen:
CaptureBasePtr init_capture_base;
// init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
// init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state, _prior_cov);
- if (trajectory_ptr_->getFrameStructure() == "POV 3D")
+ if (this->getFrameStructure() == "POV" and this->getDim() == 3)
init_capture_base = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
else
init_capture_base = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp
index d2657e4156cf6991d3342505cb558712837cb344..28a0875a7e02275226f379702fbd15a9e5280506 100644
--- a/test/gtest_IMU.cpp
+++ b/test/gtest_IMU.cpp
@@ -87,7 +87,7 @@ class Process_Factor_IMU : public testing::Test
string wolf_root = _WOLF_ROOT_DIR;
//===================================== SETTING PROBLEM
- problem = Problem::create("POV 3D");
+ problem = Problem::create("POV", 3);
// CERES WRAPPER
ceres::Solver::Options ceres_options;
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index a4bfd4edb5f0a4090fb9262d343931dfec9b6c08..3dc9a388081af74adf52bb2088fb1d04aeca4940 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -81,7 +81,7 @@ class CeresManagerWrapper : public CeresManager
TEST(CeresManager, Create)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// check double ointers to branches
@@ -93,7 +93,7 @@ TEST(CeresManager, Create)
TEST(CeresManager, AddStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
@@ -116,7 +116,7 @@ TEST(CeresManager, AddStateBlock)
TEST(CeresManager, DoubleAddStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
@@ -145,7 +145,7 @@ TEST(CeresManager, DoubleAddStateBlock)
TEST(CeresManager, UpdateStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
@@ -176,7 +176,7 @@ TEST(CeresManager, UpdateStateBlock)
TEST(CeresManager, AddUpdateStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
@@ -203,7 +203,7 @@ TEST(CeresManager, AddUpdateStateBlock)
TEST(CeresManager, RemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
@@ -235,7 +235,7 @@ TEST(CeresManager, RemoveStateBlock)
TEST(CeresManager, AddRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
@@ -261,7 +261,7 @@ TEST(CeresManager, AddRemoveStateBlock)
TEST(CeresManager, RemoveUpdateStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
@@ -286,7 +286,7 @@ TEST(CeresManager, RemoveUpdateStateBlock)
TEST(CeresManager, DoubleRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
@@ -314,7 +314,7 @@ TEST(CeresManager, DoubleRemoveStateBlock)
TEST(CeresManager, AddFactor)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
@@ -339,7 +339,7 @@ TEST(CeresManager, AddFactor)
TEST(CeresManager, DoubleAddFactor)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
@@ -367,7 +367,7 @@ TEST(CeresManager, DoubleAddFactor)
TEST(CeresManager, RemoveFactor)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
@@ -398,7 +398,7 @@ TEST(CeresManager, RemoveFactor)
TEST(CeresManager, AddRemoveFactor)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
@@ -428,7 +428,7 @@ TEST(CeresManager, AddRemoveFactor)
TEST(CeresManager, DoubleRemoveFactor)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
@@ -466,7 +466,7 @@ TEST(CeresManager, DoubleRemoveFactor)
TEST(CeresManager, AddStateBlockLocalParam)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
@@ -493,7 +493,7 @@ TEST(CeresManager, AddStateBlockLocalParam)
TEST(CeresManager, RemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
@@ -525,7 +525,7 @@ TEST(CeresManager, RemoveLocalParam)
TEST(CeresManager, AddLocalParam)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
@@ -558,7 +558,7 @@ TEST(CeresManager, AddLocalParam)
TEST(CeresManager, FactorsRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO 3D");
+ ProblemPtr P = Problem::create("PO", 3);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) 2 factors quaternion
@@ -604,7 +604,7 @@ TEST(CeresManager, FactorsRemoveLocalParam)
TEST(CeresManager, FactorsUpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO 3D");
+ ProblemPtr P = Problem::create("PO", 3);
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) 2 factors quaternion
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 68d4ad11574d3ffab8bca44b0cb092c673dd0e92..46c90ab9a96fadde729ddf690748c2afe699c8e2 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -26,7 +26,7 @@ using namespace Eigen;
TEST(Emplace, Landmark)
{
- ProblemPtr P = Problem::create("POV 3D");
+ ProblemPtr P = Problem::create("POV", 3);
LandmarkBase::emplace<LandmarkBase>(P->getMap(),"Dummy", nullptr, nullptr);
ASSERT_EQ(P, P->getMap()->getLandmarkList().front()->getMap()->getProblem());
@@ -34,14 +34,14 @@ TEST(Emplace, Landmark)
TEST(Emplace, Sensor)
{
- ProblemPtr P = Problem::create("POV 3D");
+ ProblemPtr P = Problem::create("POV", 3);
SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getHardware()->getProblem());
}
TEST(Emplace, Frame)
{
- ProblemPtr P = Problem::create("POV 3D");
+ ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
@@ -50,7 +50,7 @@ TEST(Emplace, Frame)
TEST(Emplace, Processor)
{
- ProblemPtr P = Problem::create("POV 3D");
+ ProblemPtr P = Problem::create("POV", 3);
auto sen = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
auto prc = ProcessorOdom2D::emplace<ProcessorOdom2D>(sen, std::make_shared<ProcessorParamsOdom2D>());
@@ -61,7 +61,7 @@ TEST(Emplace, Processor)
TEST(Emplace, Capture)
{
- ProblemPtr P = Problem::create("POV 3D");
+ ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
@@ -75,7 +75,7 @@ TEST(Emplace, Capture)
TEST(Emplace, Feature)
{
- ProblemPtr P = Problem::create("POV 3D");
+ ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
@@ -95,7 +95,7 @@ TEST(Emplace, Feature)
}
TEST(Emplace, Factor)
{
- ProblemPtr P = Problem::create("POV 3D");
+ ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
@@ -118,7 +118,7 @@ TEST(Emplace, Factor)
TEST(Emplace, EmplaceDerived)
{
- ProblemPtr P = Problem::create("POV 3D");
+ ProblemPtr P = Problem::create("POV", 3);
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
// LandmarkBase::emplace<LandmarkBase>(MapBaseWPtr(P->getMap()),"Dummy", nullptr, nullptr);
diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_IMU.cpp
index 18e3d8b966f57c5c06f3e1066b1d735c9d3e3ce9..27628e97e5c1f79492fcee368ebe3ee8492eef63 100644
--- a/test/gtest_factor_IMU.cpp
+++ b/test/gtest_factor_IMU.cpp
@@ -58,7 +58,7 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
+ wolf_problem_ptr_ = Problem::create("POV", 3);
// CERES WRAPPER
ceres::Solver::Options ceres_options;
@@ -144,7 +144,7 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
+ wolf_problem_ptr_ = Problem::create("POV", 3);
// CERES WRAPPER
ceres::Solver::Options ceres_options;
@@ -228,7 +228,7 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
+ wolf_problem_ptr_ = Problem::create("POV", 3);
// CERES WRAPPER
ceres::Solver::Options ceres_options;
@@ -312,7 +312,7 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
+ wolf_problem_ptr_ = Problem::create("POV", 3);
// CERES WRAPPER
ceres::Solver::Options ceres_options;
@@ -398,7 +398,7 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
+ wolf_problem_ptr_ = Problem::create("POV", 3);
// CERES WRAPPER
ceres::Solver::Options ceres_options;
@@ -485,7 +485,7 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
+ wolf_problem_ptr_ = Problem::create("POV", 3);
// CERES WRAPPER
ceres::Solver::Options ceres_options;
@@ -568,7 +568,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
+ wolf_problem_ptr_ = Problem::create("POV", 3);
// CERES WRAPPER
ceres::Solver::Options ceres_options;
@@ -661,7 +661,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
+ wolf_problem_ptr_ = Problem::create("POV", 3);
// CERES WRAPPER
ceres::Solver::Options ceres_options;
@@ -762,7 +762,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
+ wolf_problem_ptr_ = Problem::create("POV", 3);
// CERES WRAPPER
ceres::Solver::Options ceres_options;
@@ -864,7 +864,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
- problem = Problem::create("POV 3D");
+ problem = Problem::create("POV", 3);
// CERES WRAPPER
ceres::Solver::Options ceres_options;
@@ -1045,7 +1045,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
+ wolf_problem_ptr_ = Problem::create("POV", 3);
// CERES WRAPPER
ceres::Solver::Options ceres_options;
@@ -1186,7 +1186,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
+ wolf_problem_ptr_ = Problem::create("POV", 3);
// CERES WRAPPER
ceres::Solver::Options ceres_options;
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index a651aae785de3864892d4f62652ebf5fcfc2c105..008e35ee294a41c56688292fc70ecff15a939116 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -31,7 +31,7 @@ Eigen::Matrix<wolf::Scalar, 9, 9> data_cov = 0.2 * Eigen::Matrix<Scalar,9,9>::Id
Vector10s x0 = pose9toPose10(pose + Vector9s::Random()*0.25);
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("POV 3D");
+ProblemPtr problem_ptr = Problem::create("POV", 3);
CeresManager ceres_mgr(problem_ptr);
// Two frames
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
index 7b3c813201ab9196fdf321e6c76c5948b9900ee4..ed0816020210f5c3e5068b214f51a3e1ddea34cb 100644
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3D.cpp
@@ -52,7 +52,7 @@ class FactorAutodiffDistance3D_Test : public testing::Test
dist = Vector1s(sqrt(2.0));
dist_cov = Matrix1s(0.01);
- problem = Problem::create("PO 3D");
+ problem = Problem::create("PO", 3);
ceres_manager = std::make_shared<CeresManager>(problem);
F1 = problem->emplaceFrame (KEY_FRAME, pose1, 1.0);
diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp
index 50b1988d84ee617bac7de900f1ab393b3f60b5a7..fb6362c0c32febbd463c9db214a4b69816f33d09 100644
--- a/test/gtest_factor_autodiff_trifocal.cpp
+++ b/test/gtest_factor_autodiff_trifocal.cpp
@@ -112,7 +112,7 @@ class FactorAutodiffTrifocalTest : public testing::Test{
pose_cam << pos_cam, vquat_cam;
// Build problem
- problem = Problem::create("PO 3D");
+ problem = Problem::create("PO", 3);
ceres::Solver::Options options;
ceres_manager = std::make_shared<CeresManager>(problem, options);
diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp
index e4c754ddadacc39a5bf81086052ff0426d8d3dfa..2eb802638a88163a2557d863d3c1f5dc15bb512a 100644
--- a/test/gtest_factor_odom_3D.cpp
+++ b/test/gtest_factor_odom_3D.cpp
@@ -33,7 +33,7 @@ Vector7s x0 = data2delta(Vector6s::Random()*0.25);
Vector7s x1 = data2delta(data + Vector6s::Random()*0.25);
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO 3D");
+ProblemPtr problem_ptr = Problem::create("PO", 3);
CeresManager ceres_mgr(problem_ptr);
// Two frames
diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp
index fdcd2d624ce5841a10351cb1ff2b7d2e6b6fdf24..60aee68f3e8e5957fa4e1a51446a0860fd27c3d9 100644
--- a/test/gtest_factor_pose_2D.cpp
+++ b/test/gtest_factor_pose_2D.cpp
@@ -26,7 +26,7 @@ Matrix3s data_cov = data_var.asDiagonal();
Vector3s x0 = pose + Vector3s::Random()*0.25;
// Problem and solver
-ProblemPtr problem = Problem::create("PO 2D");
+ProblemPtr problem = Problem::create("PO", 2);
CeresManager ceres_mgr(problem);
// Two frames
diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp
index ab7f30ecf774254f715ee64a034949ec9dfb7fa7..208bf076d404e79e8dad0fb241761fd4927dab35 100644
--- a/test/gtest_factor_pose_3D.cpp
+++ b/test/gtest_factor_pose_3D.cpp
@@ -32,7 +32,7 @@ Matrix6s data_cov = data_var.asDiagonal();
Vector7s x0 = pose6toPose7(pose + Vector6s::Random()*0.25);
// Problem and solver
-ProblemPtr problem = Problem::create("PO 3D");
+ProblemPtr problem = Problem::create("PO", 3);
CeresManager ceres_mgr(problem);
// Two frames
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 715be59f53a14c3e814168286762b58f642cb6f2..b601c13b58a12d5cd6b0c6ca67cfef693bcac948 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -34,7 +34,7 @@ class FeatureIMU_test : public testing::Test
using std::static_pointer_cast;
// Wolf problem
- problem = Problem::create("POV 3D");
+ problem = Problem::create("POV", 3);
Eigen::VectorXs IMU_extrinsics(7);
IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
IntrinsicsIMUPtr sen_imu_params = std::make_shared<IntrinsicsIMU>();
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 85f3d873d1dccba7c87ba5409a2f29ddf583d25c..b7b4e0ba44af39e2b9b9dcb0ad3b263b43ff415a 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -62,7 +62,7 @@ TEST(FrameBase, LinksBasic)
TEST(FrameBase, LinksToTree)
{
// Problem with 2 frames and one motion factor between them
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
IntrinsicsOdom2D intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 1;
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 0259ebeb2ef16978934fa2b5e469405351bd9362..9d40059c92ccb04b87c2a22dbee9ca555cb24195 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -123,7 +123,7 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
Vector3s delta (2,0,0);
Matrix3s delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
- ProblemPtr Pr = Problem::create("PO 2D");
+ ProblemPtr Pr = Problem::create("PO", 2);
CeresManager ceres_manager(Pr);
// KF0 and absolute prior
@@ -200,7 +200,7 @@ TEST(Odom2D, VoteForKfAndSolve)
int N = 7; // number of process() steps
// Create Wolf tree nodes
- ProblemPtr problem = Problem::create("PO 2D");
+ ProblemPtr problem = Problem::create("PO", 2);
SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0));
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
params->voting_active = true;
@@ -328,7 +328,7 @@ TEST(Odom2D, KF_callback)
// KF11
// Create Wolf tree nodes
- ProblemPtr problem = Problem::create("PO 2D");
+ ProblemPtr problem = Problem::create("PO", 2);
SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0));
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
params->dist_traveled = 100;
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index 79f7aff496e524368e17ee087fed609efb2dd721..72335f11f97c1701723a77e8c1d3c6655f588c90 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -16,7 +16,7 @@
using namespace wolf;
-ProblemPtr problem_ptr = Problem::create("PO 3D");
+ProblemPtr problem_ptr = Problem::create("PO", 3);
CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
Eigen::Vector7s initial_extrinsics((Eigen::Vector7s() << 1, 2, 3, 1, 0, 0, 0).finished());
Eigen::Vector7s prior_extrinsics((Eigen::Vector7s() << 10, 20, 30, 0, 0, 0, 1).finished());
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index b4ccf0efb1fc87e43594f332a6bd0f9fd1a1baf3..aad0210c90f851b7f44e0ea0bba3898829d6153c 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -21,7 +21,7 @@ using namespace Eigen;
TEST(Problem, create)
{
- ProblemPtr P = Problem::create("POV 3D");
+ ProblemPtr P = Problem::create("POV", 3);
// check double ointers to branches
ASSERT_EQ(P, P->getHardware()->getProblem());
@@ -34,7 +34,7 @@ TEST(Problem, create)
TEST(Problem, Sensors)
{
- ProblemPtr P = Problem::create("POV 3D");
+ ProblemPtr P = Problem::create("POV", 3);
// add a dummy sensor
// SensorBasePtr S = std::make_shared<SensorBase>("Dummy", nullptr, nullptr, nullptr, 2, false);
@@ -49,7 +49,7 @@ TEST(Problem, Sensors)
TEST(Problem, Processor)
{
- ProblemPtr P = Problem::create("PO 3D");
+ ProblemPtr P = Problem::create("PO", 3);
// check motion processor is null
ASSERT_FALSE(P->getProcessorMotion());
@@ -76,7 +76,7 @@ TEST(Problem, Processor)
TEST(Problem, Installers)
{
std::string wolf_root = _WOLF_ROOT_DIR;
- ProblemPtr P = Problem::create("PO 3D");
+ ProblemPtr P = Problem::create("PO", 3);
Eigen::Vector7s xs;
SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
@@ -105,7 +105,7 @@ TEST(Problem, Installers)
TEST(Problem, SetOrigin_PO_2D)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
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
@@ -142,7 +142,7 @@ TEST(Problem, SetOrigin_PO_2D)
TEST(Problem, SetOrigin_PO_3D)
{
- ProblemPtr P = Problem::create("PO 3D");
+ ProblemPtr P = Problem::create("PO", 3);
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
@@ -179,11 +179,11 @@ TEST(Problem, SetOrigin_PO_3D)
TEST(Problem, emplaceFrame_factory)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
- 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));
- FrameBasePtr f2 = P->emplaceFrame("POV 3D", KEY_FRAME, VectorXs(10), TimeStamp(2.0));
+ FrameBasePtr f0 = P->emplaceFrame("PO", 2, KEY_FRAME, VectorXs(3), TimeStamp(0.0));
+ FrameBasePtr f1 = P->emplaceFrame("PO", 3, KEY_FRAME, VectorXs(7), TimeStamp(1.0));
+ FrameBasePtr f2 = P->emplaceFrame("POV", 3, KEY_FRAME, VectorXs(10), TimeStamp(2.0));
// std::cout << "f0: type PO 2D? " << f0->getType() << std::endl;
// std::cout << "f1: type PO 3D? " << f1->getType() << std::endl;
@@ -206,7 +206,7 @@ TEST(Problem, StateBlocks)
{
std::string wolf_root = _WOLF_ROOT_DIR;
- ProblemPtr P = Problem::create("PO 3D");
+ ProblemPtr P = Problem::create("PO", 3);
Eigen::Vector7s xs;
// 2 state blocks, fixed
@@ -227,7 +227,7 @@ TEST(Problem, StateBlocks)
ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
// 2 state blocks, estimated
- P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
+ P->emplaceFrame("PO", 3, KEY_FRAME, xs, 0);
ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int)(/*2 + 3*/ + 2)); // consume empties the notification map, so only should contain notification since last call
// P->print(4,1,1,1);
@@ -243,7 +243,7 @@ TEST(Problem, Covariances)
{
std::string wolf_root = _WOLF_ROOT_DIR;
- ProblemPtr P = Problem::create("PO 3D");
+ ProblemPtr P = Problem::create("PO", 3);
Eigen::Vector7s xs;
SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
@@ -261,7 +261,7 @@ TEST(Problem, Covariances)
// 4 state blocks, estimated
St->unfixExtrinsics();
- FrameBasePtr F = P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
+ FrameBasePtr F = P->emplaceFrame("PO", 3, KEY_FRAME, xs, 0);
// set covariance (they are not computed without a solver)
P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity());
diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp
index 822fc5b816eda32d729067f0476f8dd4b98b6ac5..5d15b01a5e3444d2e8280062ee6865fb268391f8 100644
--- a/test/gtest_processor_IMU.cpp
+++ b/test/gtest_processor_IMU.cpp
@@ -48,7 +48,7 @@ class ProcessorIMUt : public testing::Test
std::string wolf_root = _WOLF_ROOT_DIR;
// Wolf problem
- problem = Problem::create("POV 3D");
+ problem = Problem::create("POV", 3);
Vector7s extrinsics = (Vector7s() << 0,0,0, 0,0,0,1).finished();
sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
@@ -97,7 +97,7 @@ TEST(ProcessorIMU_constructors, ALL)
//Factory constructor without yaml
std::string wolf_root = _WOLF_ROOT_DIR;
- ProblemPtr problem = Problem::create("POV 3D");
+ ProblemPtr problem = Problem::create("POV", 3);
Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
@@ -127,7 +127,7 @@ TEST(ProcessorIMU, voteForKeyFrame)
std::string wolf_root = _WOLF_ROOT_DIR;
// Wolf problem
- ProblemPtr problem = Problem::create("POV 3D");
+ ProblemPtr problem = Problem::create("POV", 3);
Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
@@ -144,7 +144,6 @@ TEST(ProcessorIMU, voteForKeyFrame)
x0 << 0,0,0, 0,0,0,1, 0,0,0;
MatrixXs P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
-
//data variable and covariance matrix
// since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
Vector6s data;
diff --git a/test/gtest_processor_IMU_jacobians.cpp b/test/gtest_processor_IMU_jacobians.cpp
index 627549043e314e3861763eb4ba1253a6631e38ba..df316061a061236b8bbb7005def716dbf7be8199 100644
--- a/test/gtest_processor_IMU_jacobians.cpp
+++ b/test/gtest_processor_IMU_jacobians.cpp
@@ -61,7 +61,7 @@ class ProcessorIMU_jacobians : public testing::Test
data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad;
// Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("POV 3D");
+ ProblemPtr wolf_problem_ptr_ = Problem::create("POV", 3);
Eigen::VectorXs IMU_extrinsics(7);
IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
@@ -140,7 +140,7 @@ class ProcessorIMU_jacobians_Dq : public testing::Test
data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad;
// Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("POV 3D");
+ ProblemPtr wolf_problem_ptr_ = Problem::create("POV", 3);
Eigen::VectorXs IMU_extrinsics(7);
IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 096824ee97d758c29ec83e5ef25c91a398bb125e..b19ea72daa2236c17c5fa99210dc7fbc72c04712 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -35,7 +35,7 @@ TEST(ProcessorBase, KeyFrameCallback)
Scalar dt = 0.01;
// Wolf problem
- ProblemPtr problem = Problem::create("PO 2D");
+ ProblemPtr problem = Problem::create("PO", 2);
// Install tracker (sensor and processor)
// SensorBasePtr sens_trk = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index 4c40933db6faa786a7c8dcf203c094d65e99ec10..b2b313fee19ae6c651e19191fafc59f2efe07ae0 100644
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -25,7 +25,7 @@ struct DummyLoopCloser : public wolf::ProcessorFrameNearestNeighborFilter
};
// Declare Wolf problem
-wolf::ProblemPtr problem = wolf::Problem::create("PO 2D");
+wolf::ProblemPtr problem = wolf::Problem::create("PO", 2);
// Declare Sensor
Eigen::Vector3s odom_extrinsics = Eigen::Vector3s(0,0,0);
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 0a02d13a13dbd9d25871cbc718aaaf3bb3902568..634a446acc61338c410e662211a6c4f0f8becd60 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -36,7 +36,7 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
virtual void SetUp()
{
dt = 1.0;
- problem = Problem::create("PO 2D");
+ problem = Problem::create("PO", 2);
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
params->time_tolerance = 0.25;
params->dist_traveled = 100;
diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp
index b94ef87e7b80fe18827dee79d814a7550ee9f32f..95f811afa3a534cb8e63010c30d89dc68482b4e1 100644
--- a/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -71,7 +71,7 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
Scalar dt = 0.01;
// Wolf problem
- ProblemPtr problem = Problem::create("PO 3D");
+ ProblemPtr problem = Problem::create("PO", 3);
// Install tracker (sensor and processor)
IntrinsicsCameraPtr intr = make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 56d3aa2ec37ec58753c922966c818412145ce635..71a4880da9de749679637f04790aae01dc2f7c2b 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -107,7 +107,7 @@ class SolverManagerWrapper : public SolverManager
TEST(SolverManager, Create)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// check double pointers to branches
@@ -116,7 +116,7 @@ TEST(SolverManager, Create)
TEST(SolverManager, AddStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
@@ -135,7 +135,7 @@ TEST(SolverManager, AddStateBlock)
TEST(SolverManager, DoubleAddStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
@@ -160,7 +160,7 @@ TEST(SolverManager, DoubleAddStateBlock)
TEST(SolverManager, UpdateStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
@@ -208,7 +208,7 @@ TEST(SolverManager, UpdateStateBlock)
TEST(SolverManager, AddUpdateStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
@@ -246,7 +246,7 @@ TEST(SolverManager, AddUpdateStateBlock)
TEST(SolverManager, AddUpdateLocalParamStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
@@ -289,7 +289,7 @@ TEST(SolverManager, AddUpdateLocalParamStateBlock)
TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
@@ -336,7 +336,7 @@ TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
TEST(SolverManager, RemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
@@ -361,7 +361,7 @@ TEST(SolverManager, RemoveStateBlock)
TEST(SolverManager, AddRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
@@ -383,7 +383,7 @@ TEST(SolverManager, AddRemoveStateBlock)
TEST(SolverManager, RemoveUpdateStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
@@ -402,7 +402,7 @@ TEST(SolverManager, RemoveUpdateStateBlock)
TEST(SolverManager, DoubleRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
@@ -427,7 +427,7 @@ TEST(SolverManager, DoubleRemoveStateBlock)
TEST(SolverManager, AddUpdatedStateBlock)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
@@ -491,7 +491,7 @@ TEST(SolverManager, AddUpdatedStateBlock)
TEST(SolverManager, AddFactor)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
@@ -516,7 +516,7 @@ TEST(SolverManager, AddFactor)
TEST(SolverManager, RemoveFactor)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
@@ -547,7 +547,7 @@ TEST(SolverManager, RemoveFactor)
TEST(SolverManager, AddRemoveFactor)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
@@ -578,7 +578,7 @@ TEST(SolverManager, AddRemoveFactor)
TEST(SolverManager, DoubleRemoveFactor)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 3366ae781f4784b483984a6f951705ba17240326..68a08078b5c937f05c15555b3385cadf81875e00 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -63,7 +63,7 @@ bool debug = false;
TEST(TrajectoryBase, ClosestKeyFrame)
{
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
// Trajectory status:
@@ -103,7 +103,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
{
using std::make_shared;
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
DummyNotificationProcessor N(P);
@@ -185,7 +185,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
{
using std::make_shared;
- ProblemPtr P = Problem::create("PO 2D");
+ ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
// Trajectory status: