diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index 92950ca3f6c16ceedb8a352aa51a3d2f3aaaafd0..707bb45b02d2880cf461bd161d3f3137f9897737 100644
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
@@ -147,8 +147,8 @@ int main(int argc, char** argv)
bNum.clear();
// add frame to problem
- FrameBasePtr vertex_frame_ptr_autodiff = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- FrameBasePtr vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_autodiff = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
wolf_problem_autodiff->getTrajectory()->addFrame(vertex_frame_ptr_autodiff);
wolf_problem_analytic->getTrajectory()->addFrame(vertex_frame_ptr_analytic);
// store
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index bc8982ca2ce696719aab35105f059639d6c72f23..ed612ffcad7f3e1299f8e02d434c61969705b718 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -84,7 +84,7 @@ int main(int argc, char** argv)
// Wolf problem
ProblemPtr wolf_problem_full = new Problem(FRM_PO_2d);
ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2d);
- SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
+ SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)), std::make_shared<StateAngle>(Eigen::VectorXd::Zero(1)), std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2);
Eigen::SparseMatrix<double> Lambda(0,0);
@@ -151,8 +151,8 @@ int main(int argc, char** argv)
bNum.clear();
// add frame to problem
- FrameBasePtr vertex_frame_ptr_full = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- FrameBasePtr vertex_frame_ptr_prun = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_full = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_prun = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
// store
diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index e634dac6f2de62d79296525dd6ca2631b4041108..d85e2cdebeacd0bbc1d39220cacd562f018e291b 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.cpp
@@ -181,10 +181,10 @@ int main(int argc, char *argv[])
Eigen::Vector4d laser_1_pose, laser_2_pose; //xyz + theta
laser_1_pose << 1.2, 0, 0, 0; //laser 1
laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
- SensorOdom2D odom_sensor(std::make_shared<StateBlock>(odom_pose.head(2)), std::make_shared<StateBlock>(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
- SensorGPSFix gps_sensor(std::make_shared<StateBlock>(gps_pose.head(2)), std::make_shared<StateBlock>(gps_pose.tail(1)), gps_std);
- SensorLaser2D laser_1_sensor(std::make_shared<StateBlock>(laser_1_pose.head(2)), std::make_shared<StateBlock>(laser_1_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
- SensorLaser2D laser_2_sensor(std::make_shared<StateBlock>(laser_2_pose.head(2)), std::make_shared<StateBlock>(laser_2_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
+ SensorOdom2D odom_sensor(std::make_shared<StatePoint2d>(odom_pose.head(2)), std::make_shared<StateAngle>(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
+ SensorGPSFix gps_sensor(std::make_shared<StatePoint2d>(gps_pose.head(2)), std::make_shared<StateAngle>(gps_pose.tail(1)), gps_std);
+ SensorLaser2D laser_1_sensor(std::make_shared<StatePoint2d>(laser_1_pose.head(2)), std::make_shared<StateAngle>(laser_1_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
+ SensorLaser2D laser_2_sensor(std::make_shared<StatePoint2d>(laser_2_pose.head(2)), std::make_shared<StateAngle>(laser_2_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
// Initial pose
pose_odom << 2, 8, 0;
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index 54525c8682b4c80d95e3e38b2ef34784e4a84cfe..b72b6a3e677d29889cf80a14a81fc32d163944b2 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -29,6 +29,8 @@
#include "processor_tracker_landmark_dummy.h"
#include "core/landmark/landmark_base.h"
#include "factor_landmark_dummy.h"
+#include "core/state_block/state_angle.h"
+#include "core/state_block/state_block_derived.h"
namespace wolf
{
@@ -124,8 +126,8 @@ LandmarkBasePtr ProcessorTrackerLandmarkDummy::emplaceLandmark(FeatureBasePtr _f
//std::cout << "ProcessorTrackerLandmarkDummy::emplaceLandmark" << std::endl;
return LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
"LandmarkBase",
- std::make_shared<StateBlock>(2),
- std::make_shared<StateBlock>(1));
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(0));
}
FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _feature_ptr,
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index aa36078108a936109484bd4a8d761c258105d236..aaf3aa4fbaca881a24b520ecb85795d5fb0f9b7d 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -29,6 +29,7 @@
#include "core/utils/utils_gtest.h"
#include "core/capture/capture_base.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
using namespace wolf;
@@ -58,9 +59,9 @@ TEST(CaptureBase, ConstructorWithSensor)
TEST(CaptureBase, Static_sensor_params)
{
- StateBlockPtr p(std::make_shared<StateBlock>(2));
- StateBlockPtr o(std::make_shared<StateAngle>() );
- StateBlockPtr i(std::make_shared<StateBlock>(2));
+ StateBlockPtr p(std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ StateBlockPtr o(std::make_shared<StateAngle>(0) );
+ StateBlockPtr i(std::make_shared<StateParams2>(Vector2d::Zero()));
SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", p, o, i, 2));
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
@@ -77,10 +78,10 @@ TEST(CaptureBase, Static_sensor_params)
TEST(CaptureBase, Dynamic_sensor_params)
{
- StateBlockPtr p(std::make_shared<StateBlock>(2));
- StateBlockPtr o(std::make_shared<StateAngle>() );
- StateBlockPtr i(std::make_shared<StateBlock>(2));
- SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", std::make_shared<StateBlock>(2), std::make_shared<StateAngle>(), std::make_shared<StateBlock>(2), 2, true, true, true)); // last 3 'true' mark dynamic
+ StateBlockPtr p(std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ StateBlockPtr o(std::make_shared<StateAngle>(0) );
+ StateBlockPtr i(std::make_shared<StateParams2>(Vector2d::Zero()));
+ SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", std::make_shared<StatePoint2d>(Vector2d::Zero()), std::make_shared<StateAngle>(0), std::make_shared<StateParams2>(Vector2d::Zero()), 2, true, true, true)); // last 3 'true' mark dynamic
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5
// query sensor blocks -- need KFs to find some Capture with the params
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 0aa294035d9f712b706883636e46cd1e4ae62593..956f6eb4e76f44b333ea5253a0023c0ec4fadac5 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -28,7 +28,8 @@
#include "core/utils/utils_gtest.h"
-
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
#include "core/sensor/sensor_odom_2d.h"
@@ -64,7 +65,7 @@ TEST(Emplace, Frame)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
}
@@ -89,7 +90,7 @@ TEST(Emplace, Capture)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(0,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -103,7 +104,7 @@ TEST(Emplace, Feature)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -121,7 +122,7 @@ TEST(Emplace, Factor)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -141,7 +142,7 @@ TEST(Emplace, EmplaceDerived)
{
ProblemPtr P = Problem::create("POV", 3);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
auto sen = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Eigen::VectorXd(3), ParamsSensorOdom2d());
auto cov = Eigen::MatrixXd::Identity(2,2);
auto cpt = CaptureBase::emplace<CaptureOdom2d>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
@@ -163,7 +164,7 @@ TEST(Emplace, ReturnDerived)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
auto cov = Eigen::MatrixXd::Identity(2,2);
auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 7a1a330916e5ba4a5e7c28f784bd6bfd21278da7..a2f8dfaf9f6bfe3a08ae392937b186377476e28e 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -30,6 +30,7 @@
#include "dummy/factor_dummy_zero_1.h"
#include "dummy/factor_dummy_zero_12.h"
+#include "core/state_block/state_block_derived.h"
#include "core/factor/factor_relative_pose_2d.h"
#include "core/utils/utils_gtest.h"
#include "core/sensor/sensor_odom_2d.h"
@@ -44,7 +45,7 @@ using namespace Eigen;
TEST(FactorAutodiff, AutodiffDummy1)
{
- StateBlockPtr sb = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
+ StateBlockPtr sb = std::make_shared<StateParams1>(Eigen::Vector1d::Random());
FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
auto fac = std::make_shared<FactorDummyZero1>(ftr, sb);
@@ -61,18 +62,18 @@ TEST(FactorAutodiff, AutodiffDummy1)
TEST(FactorAutodiff, AutodiffDummy12)
{
- StateBlockPtr sb1 = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
- StateBlockPtr sb2 = std::make_shared<StateBlock>(Eigen::Vector2d::Random());
- StateBlockPtr sb3 = std::make_shared<StateBlock>(Eigen::Vector3d::Random());
- StateBlockPtr sb4 = std::make_shared<StateBlock>(Eigen::Vector4d::Random());
- StateBlockPtr sb5 = std::make_shared<StateBlock>(Eigen::Vector5d::Random());
- StateBlockPtr sb6 = std::make_shared<StateBlock>(Eigen::Vector6d::Random());
- StateBlockPtr sb7 = std::make_shared<StateBlock>(Eigen::Vector7d::Random());
- StateBlockPtr sb8 = std::make_shared<StateBlock>(Eigen::Vector8d::Random());
- StateBlockPtr sb9 = std::make_shared<StateBlock>(Eigen::Vector9d::Random());
- StateBlockPtr sb10 = std::make_shared<StateBlock>(Eigen::Vector10d::Random());
- StateBlockPtr sb11 = std::make_shared<StateBlock>(Eigen::VectorXd::Random(11));
- StateBlockPtr sb12 = std::make_shared<StateBlock>(Eigen::VectorXd::Random(12));
+ StateBlockPtr sb1 = std::make_shared<StateParams1>(Eigen::Vector1d::Random());
+ StateBlockPtr sb2 = std::make_shared<StateParams2>(Eigen::Vector2d::Random());
+ StateBlockPtr sb3 = std::make_shared<StateParams3>(Eigen::Vector3d::Random());
+ StateBlockPtr sb4 = std::make_shared<StateParams4>(Eigen::Vector4d::Random());
+ StateBlockPtr sb5 = std::make_shared<StateParams5>(Eigen::Vector5d::Random());
+ StateBlockPtr sb6 = std::make_shared<StateParams6>(Eigen::Vector6d::Random());
+ StateBlockPtr sb7 = std::make_shared<StateParams7>(Eigen::Vector7d::Random());
+ StateBlockPtr sb8 = std::make_shared<StateParams8>(Eigen::Vector8d::Random());
+ StateBlockPtr sb9 = std::make_shared<StateParams9>(Eigen::Vector9d::Random());
+ StateBlockPtr sb10 = std::make_shared<StateParams10>(Eigen::Vector10d::Random());
+ StateBlockPtr sb11 = std::make_shared<StateParams<11>>(Eigen::VectorXd::Random(11));
+ StateBlockPtr sb12 = std::make_shared<StateParams<12>>(Eigen::VectorXd::Random(12));
std::vector<const double*> states_ptr({sb1->getStateData(),sb2->getStateData(),sb3->getStateData(),sb4->getStateData(),sb5->getStateData(),sb6->getStateData(),sb7->getStateData(),sb8->getStateData(),sb9->getStateData(),sb10->getStateData(),sb11->getStateData(),sb12->getStateData()});
std::vector<Eigen::MatrixXd> J;
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index 0f89a11003bd0e297ed6b3df283b6aba536ca5ea..f4755e11139f07ba42d537e0fc4e070c300cf9a3 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -25,6 +25,7 @@
#include "core/common/wolf.h"
#include "core/utils/logging.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/capture/capture_pose.h"
@@ -117,9 +118,9 @@ class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
// Create sensor to be able to initialize (a camera for instance)
S = SensorBase::emplace<SensorBase>(problem->getHardware(), "SensorPose",
- std::make_shared<StateBlock>(pos_camera, true),
+ std::make_shared<StatePoint3d>(pos_camera, true),
std::make_shared<StateQuaternion>(vquat_camera, true),
- std::make_shared<StateBlock>(Vector4d::Zero(), false), // fixed
+ std::make_shared<StateParams4>(Vector4d::Zero(), false), // fixed
Vector1d::Zero());
// ParamsSensorCameraPtr params_camera = std::make_shared<ParamsSensorCamera>();
@@ -142,7 +143,7 @@ class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
c1 = std::static_pointer_cast<CapturePose>(CaptureBase::emplace<CapturePose>(F1, 0, S, pose_landmark, meas_cov));
f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, pose_landmark, meas_cov));
lmk1 = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkPose",
- std::make_shared<StateBlock>(pos_landmark),
+ std::make_shared<StatePoint3d>(pos_landmark),
std::make_shared<StateQuaternion>(Quaterniond(vquat_landmark)));
}
};
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index eb61f798d41b2aec792b864dffb39cb1cdb7d098..64dbd6d85484e2ac6ed270b470f674107d146cb6 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -33,6 +33,8 @@
#include "core/sensor/sensor_odom_2d.h"
#include "core/processor/processor_odom_2d.h"
#include "core/capture/capture_motion.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include <iostream>
@@ -42,7 +44,7 @@ using namespace wolf;
TEST(FrameBase, GettersAndSetters)
{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
// getters
ASSERT_EQ(F->id(), (unsigned int) 1);
@@ -55,7 +57,7 @@ TEST(FrameBase, GettersAndSetters)
TEST(FrameBase, StateBlocks)
{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
ASSERT_EQ(F->getStateBlockMap().size(),(unsigned int) 2);
ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2);
@@ -65,7 +67,7 @@ TEST(FrameBase, StateBlocks)
TEST(FrameBase, LinksBasic)
{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
ASSERT_FALSE(F->getTrajectory());
ASSERT_FALSE(F->getProblem());
@@ -85,8 +87,8 @@ TEST(FrameBase, LinksToTree)
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
- auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>());
@@ -139,16 +141,16 @@ TEST(FrameBase, Frames)
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
- auto F0 = FrameBase::emplace<FrameBase>(T, 0, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F3 = FrameBase::emplace<FrameBase>(T, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F4 = FrameBase::emplace<FrameBase>(T, 4, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F5 = FrameBase::emplace<FrameBase>(T, 5, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F6 = FrameBase::emplace<FrameBase>(T, 6, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F7 = FrameBase::emplace<FrameBase>(T, 7, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F8 = FrameBase::emplace<FrameBase>(T, 8, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F9 = FrameBase::emplace<FrameBase>(T, 9, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F0 = FrameBase::emplace<FrameBase>(T, 0, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F3 = FrameBase::emplace<FrameBase>(T, 3, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F4 = FrameBase::emplace<FrameBase>(T, 4, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F5 = FrameBase::emplace<FrameBase>(T, 5, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F6 = FrameBase::emplace<FrameBase>(T, 6, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F7 = FrameBase::emplace<FrameBase>(T, 7, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F8 = FrameBase::emplace<FrameBase>(T, 8, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F9 = FrameBase::emplace<FrameBase>(T, 9, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
// tree is now consistent
ASSERT_TRUE(P->check(0));
@@ -396,8 +398,8 @@ TEST(FrameBase, Frames)
TEST(FrameBase, GetSetState)
{
// Create PQV_3d state blocks
- StateBlockPtr sbp = make_shared<StateBlock>(3);
- StateBlockPtr sbv = make_shared<StateBlock>(3);
+ StateBlockPtr sbp = make_shared<StatePoint3d>(Vector3d::Zero());
+ StateBlockPtr sbv = make_shared<StateVector3d>(Vector3d::Zero());
StateQuaternionPtr sbq = make_shared<StateQuaternion>();
// Create frame
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index e60bc875310148a2fc0a127884b4fb1b3b77ab6a..2b02be288217cca57703ab98b5b9c7a40f32d505 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -34,6 +34,8 @@
#include "core/sensor/sensor_base.h"
#include "core/landmark/landmark_base.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
using namespace wolf;
@@ -55,12 +57,12 @@ class HasStateBlocksTest : public testing::Test
{
problem = Problem::create("POV", 3);
- sbp0 = std::make_shared<StateBlock>(3); // size 3
+ sbp0 = std::make_shared<StatePoint3d>(Vector3d::Zero()); // size 3
sbo0 = std::make_shared<StateQuaternion>();
- sbv0 = std::make_shared<StateBlock>(3); // size 3
- sbp1 = std::make_shared<StateBlock>(3); // size 3
+ sbv0 = std::make_shared<StateVector3d>(Vector3d::Zero()); // size 3
+ sbp1 = std::make_shared<StatePoint3d>(Vector3d::Zero()); // size 3
sbo1 = std::make_shared<StateQuaternion>();
- sbv1 = std::make_shared<StateBlock>(3); // size 3
+ sbv1 = std::make_shared<StateVector3d>(Vector3d::Zero()); // size 3
F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF
F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF
diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp
index 5c4b2b7893837316736ab7a69a49f1b08c169b9b..9ca7544ee6c9f28013f4037642de45504eca0b92 100644
--- a/test/gtest_map_yaml.cpp
+++ b/test/gtest_map_yaml.cpp
@@ -31,7 +31,8 @@
#include "core/problem/problem.h"
#include "core/map/map_base.h"
#include "core/landmark/landmark_base.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/local_parametrization_quaternion.h"
#include "core/yaml/yaml_conversion.h"
@@ -51,11 +52,11 @@ TEST(MapYaml, save_2d)
o2 << 2;
o3 << 3;
- StateBlockPtr p1_sb = std::make_shared<StateBlock>(p1);
- StateBlockPtr p2_sb = std::make_shared<StateBlock>(p2);
- StateBlockPtr o2_sb = std::make_shared<StateBlock>(o2);
- StateBlockPtr p3_sb = std::make_shared<StateBlock>(p3, true);
- StateBlockPtr o3_sb = std::make_shared<StateBlock>(o3, true);
+ StateBlockPtr p1_sb = std::make_shared<StatePoint2d>(p1);
+ StateBlockPtr p2_sb = std::make_shared<StatePoint2d>(p2);
+ StateBlockPtr o2_sb = std::make_shared<StateAngle>(o2(0));
+ StateBlockPtr p3_sb = std::make_shared<StatePoint2d>(p3, true);
+ StateBlockPtr o3_sb = std::make_shared<StateAngle>(o3(0), true);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p1_sb);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, o2_sb);
@@ -174,10 +175,10 @@ TEST(MapYaml, save_3d)
q2 << 0, 1, 0, 0;
q3 << 0, 0, 1, 0;
- auto p1_sb = std::make_shared<StateBlock>(p1);
- auto p2_sb = std::make_shared<StateBlock>(p2);
+ auto p1_sb = std::make_shared<StatePoint2d>(p1);
+ auto p2_sb = std::make_shared<StatePoint2d>(p2);
auto q2_sb = std::make_shared<StateQuaternion>(q2);
- auto p3_sb = std::make_shared<StateBlock>(p3, true);
+ auto p3_sb = std::make_shared<StatePoint2d>(p3, true);
auto q3_sb = std::make_shared<StateQuaternion>(q3, true);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p1_sb);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 522246910cc6bd3944f6924a832fbf028cb2b75c..88f76ed653605423a85eedd7ac2cba8ab4815605 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -43,6 +43,8 @@
#include "core/capture/capture_diff_drive.h"
#include "core/factor/factor_diff_drive.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include <iostream>
@@ -388,7 +390,7 @@ TEST(Problem, perturb)
for (int j = 0; j< 2 ; j++)
{
- auto sb = std::make_shared<StateBlock>(Vector3d(1,1,1));
+ auto sb = std::make_shared<StatePoint3d>(Vector3d(1,1,1));
auto cap = CaptureBase::emplace<CaptureBase>(F, "CaptureBase", t, sensor, nullptr, nullptr, sb);
}
i++;
@@ -396,8 +398,8 @@ TEST(Problem, perturb)
for (int l = 0; l < 2; l++)
{
- auto sb1 = std::make_shared<StateBlock>(Vector2d(1,2));
- auto sb2 = std::make_shared<StateBlock>(Vector1d(3));
+ auto sb1 = std::make_shared<StatePoint2d>(Vector2d(1,2));
+ auto sb2 = std::make_shared<StateAngle>(3);
auto L = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", sb1, sb2);
if (l==0) L->fix();
}
@@ -479,7 +481,7 @@ TEST(Problem, check)
for (int j = 0; j< 2 ; j++)
{
- auto sb = std::make_shared<StateBlock>(Vector3d(1,1,1));
+ auto sb = std::make_shared<StatePoint3d>(Vector3d(1,1,1));
auto cap = CaptureBase::emplace<CaptureDiffDrive>(F, t, sensor, Vector2d(1,2), Matrix2d::Identity(), nullptr, nullptr, nullptr, sb);
for (int k = 0; k<2; k++)
@@ -495,8 +497,8 @@ TEST(Problem, check)
for (int l = 0; l < 2; l++)
{
- auto sb1 = std::make_shared<StateBlock>(Vector2d(1,2));
- auto sb2 = std::make_shared<StateBlock>(Vector1d(3));
+ auto sb1 = std::make_shared<StatePoint2d>(Vector2d(1,2));
+ auto sb2 = std::make_shared<StateAngle>(3);
auto L = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", sb1, sb2);
if (l==0) L->fix();
}
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 89341815653b03ed0d189b7d91c8e1c759f9bd9f..5840118ff7c4b4ee12ed7413f2ae3e64e896ffd0 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -36,6 +36,8 @@
#include "core/capture/capture_void.h"
#include "core/problem/problem.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
// STL
#include <iterator>
@@ -104,9 +106,9 @@ TEST(ProcessorBase, KeyFrameCallback)
// Install tracker (sensor and processor)
auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SensorTrackerDummy",
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)),
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)),
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
+ std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)),
+ std::make_shared<StateAngle>(0),
+ std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2);
auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
proc_trk_params->time_tolerance = dt/2;
auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk, proc_trk_params);
diff --git a/test/gtest_processor_fixed_wing_model.cpp b/test/gtest_processor_fixed_wing_model.cpp
index b2bcb1990ccd266756c7f106478b79aadff3cc88..4f7500423edb371503bafc23169b12775ff3d5c5 100644
--- a/test/gtest_processor_fixed_wing_model.cpp
+++ b/test/gtest_processor_fixed_wing_model.cpp
@@ -20,15 +20,17 @@
//
//--------LICENSE_END--------
+#include "core/processor/processor_fixed_wing_model.h"
+
#include "core/utils/utils_gtest.h"
#include "core/problem/problem.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_block_derived.h"
// STL
#include <iterator>
#include <iostream>
-#include "../include/core/processor/processor_fixed_wing_model.h"
using namespace wolf;
using namespace Eigen;
@@ -52,7 +54,7 @@ class ProcessorFixWingModelTest : public testing::Test
// Emplace sensor
sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SensorBase",
- std::make_shared<StateBlock>(Vector3d::Zero()),
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
std::make_shared<StateQuaternion>((Vector4d() << 0,0,0,1).finished()),
nullptr,
2);
diff --git a/test/gtest_processor_loop_closure.cpp b/test/gtest_processor_loop_closure.cpp
index 9623ecbaa1dd1560650c8cb9e96c044c87d18e9b..57a0d4cbdb0dc23509fb4ef9f7f691d14c36cbea 100644
--- a/test/gtest_processor_loop_closure.cpp
+++ b/test/gtest_processor_loop_closure.cpp
@@ -24,6 +24,8 @@
#include "core/problem/problem.h"
#include "core/capture/capture_base.h"
#include "core/factor/factor_relative_pose_2d.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include "dummy/processor_loop_closure_dummy.h"
@@ -47,8 +49,8 @@ class ProcessorLoopClosureTest : public testing::Test
// Emplace sensor
sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SensorBase",
- std::make_shared<StateBlock>(Vector2d::Zero()),
- std::make_shared<StateBlock>(Vector1d::Zero()),
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(0),
nullptr,
2);
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 4ed3c0846017e15fe5882607ff1807fbbbbc93f5..69ed41d60e75af3346156192b145dff16c15bf31 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -25,6 +25,8 @@
#include "core/sensor/factory_sensor.h"
#include "dummy/processor_tracker_landmark_dummy.h"
#include "core/capture/capture_void.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
using namespace wolf;
@@ -285,8 +287,8 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceFactor)
Eigen::Vector1d::Ones(),
Eigen::MatrixXd::Ones(1, 1)));
LandmarkBasePtr lmk(std::make_shared<LandmarkBase>("LandmarkBase",
- std::make_shared<StateBlock>(1),
- std::make_shared<StateBlock>(1)));
+ std::make_shared<StateParams1>(Vector1d(0)),
+ std::make_shared<StateAngle>(0)));
FactorBasePtr fac = processor->callEmplaceFactor(ftr, lmk);
ASSERT_EQ(fac->getFeature(),ftr);
diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
index 89bf93042391ae90c0f12560922795029ff6d704..3fb6f4abadd3528878978b95764fc1359aaf9148 100644
--- a/test/gtest_solver_ceres.cpp
+++ b/test/gtest_solver_ceres.cpp
@@ -30,7 +30,7 @@
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
#include "core/capture/capture_void.h"
#include "core/factor/factor_pose_2d.h"
#include "core/factor/factor_quaternion_absolute.h"
@@ -57,7 +57,7 @@ using namespace Eigen;
StateBlockPtr createStateBlock()
{
Vector4d state; state << 1, 0, 0, 0;
- return std::make_shared<StateBlock>(state);
+ return std::make_shared<StateParams4>(state);
}
FactorBasePtr createFactor(StateBlockPtr sb_ptr)
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index e66342ba48d3ba7e2c6248e249e364540c1c9537..65056864243e2104071e4a5349961efb56d6b7aa 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -37,6 +37,8 @@
#include "core/factor/factor_block_absolute.h"
#include "core/state_block/local_parametrization_quaternion.h"
#include "dummy/solver_manager_dummy.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include <iostream>
#include <thread>
@@ -52,7 +54,7 @@ using namespace Eigen;
StateBlockPtr createStateBlock()
{
Vector4d state; state << 1, 0, 0, 0;
- return std::make_shared<StateBlock>(state);
+ return std::make_shared<StateParams4>(state);
}
FactorBasePtr createFactor(StateBlockPtr sb_ptr)
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
index e1696bcc92f15f5f87a1aeed24fbed554e8cdbab..17ad54a8ce833aecb23ac1735b2d4c7c93439d59 100644
--- a/test/gtest_state_block.cpp
+++ b/test/gtest_state_block.cpp
@@ -40,10 +40,10 @@ using namespace Eigen;
using namespace std;
using namespace wolf;
-TEST(StateBlock, block_perturb)
+TEST(StateBlock, point_perturb)
{
- Vector3d x(10, 20, 30);
- StateBlock sb(x);
+ Vector3d x(10, 20, 30);
+ StatePoint3d sb(x);
sb.perturb(0.5);
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
index 74dfe643ab18b1658a774fa782fe22e0a9284a66..5631d5fddeb26014baeb2698817481a602239a5e 100644
--- a/test/gtest_state_composite.cpp
+++ b/test/gtest_state_composite.cpp
@@ -27,6 +27,7 @@
*/
#include "core/state_block/state_composite.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
#include "core/utils/utils_gtest.h"
@@ -46,11 +47,11 @@ class StateBlockCompositeInit : public testing::Test
void SetUp() override
{
- sbp = states.emplace('P', Vector3d(1,2,3));
- sbv = states.emplace('V', Vector3d(4,5,6));
+ sbp = states.emplace<StatePoint3d>('P', Vector3d(1,2,3));
+ sbv = states.emplace<StateVector3d>('V', Vector3d(4,5,6));
sbq = states.emplace<StateQuaternion>('Q', Vector4d(.5,.5,.5,.5));
- sbx = std::make_shared<StateBlock>(Vector3d(7,8,9));
+ sbx = std::make_shared<StateVector3d>(Vector3d(7,8,9));
}
};