diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index dce04f5eff7d55bf2726075b07064a666af51f5f..e51f7a33658bf1c3afb9f2d20e8118825886ebf6 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -139,8 +139,10 @@ int main()
// initialize
TimeStamp t(0.0); // t : 0.0
- Vector3d x(0,0,0);
- Matrix3d P = Matrix3d::Identity() * 0.1;
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P = Matrix3d::Identity() * 0.1;
+ VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
FrameBasePtr KF1 = problem->setPriorFactor(x, P, t, 0.5); // KF1 : (0,0,0)
std::static_pointer_cast<ProcessorMotion>(processor)->setOrigin(KF1);
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index fa1e25a62820e0d7979de789b9dfcde17f3a07a6..985f05da3a869bc1d4b9c122643dc41b78ec65f5 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -48,8 +48,10 @@ class FixtureFactorBlockDifference : public testing::Test
TimeStamp t0(0);
TimeStamp t1(1);
- Vector10d x_origin = problem_->stateZero().vector("POV");
- Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
+ // Vector10d x_origin = problem_->stateZero().vector("POV");
+ VectorComposite x_origin(problem_->stateZero().vector("POV"), "POV", {3, 4, 3});
+ // Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
+ VectorComposite cov_prior(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "POV", {1,1,1});
KF0_ = problem_->setPriorFactor(x_origin, cov_prior, t0, 0.1);
//CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 7aab9e5cef754bd1b9942ba5ee37c0d76e684634..8e97b7f177ef034f94e18c881c94630fcff96668 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -440,10 +440,11 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
TimeStamp t(0.0);
double dt = 1.0;
- Vector3d x0(0,0,0);
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
Vector3d x1(1.5, -1.5, -M_PI/2.0);
Vector3d x2(3.0, -3.0, 0.0);
- Matrix3d P0; P0.setIdentity();
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite P0(Vector3d(0,0,0), "PO", {2,1});
// set prior at t=0 and processor origin
auto F0 = problem->setPriorFactor(x0, P0, t, 0.1);
@@ -572,10 +573,12 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
TimeStamp t(0.0);
double dt = 1.0;
- Vector3d x0(0,0,0);
+ // Vector3d x0(0,0,0);
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
Vector3d x1(1.5, -1.5, -M_PI/2.0);
Vector3d x2(3.0, -3.0, 0.0);
- Matrix3d P0; P0.setIdentity();
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite P0(Vector3d(1,1,1), "PO", {2,1});
// set prior at t=0 and processor origin
auto F0 = problem->setPriorFactor(x0, P0, t, 0.1);
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index cdf8a749bbd57846a9c200bb81c771ef3fca9d19..1afb7c474602426e4ba1a3ead594d7c1d7e73468 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -119,8 +119,8 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
TimeStamp t0(0.0), t = t0;
double dt = .01;
- Vector3d x0 (0,0,0);
- Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite P0(Vector3d(0.1,0.1,0.1), "PO", {2,1});
Vector3d delta (2,0,0);
Matrix3d delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
@@ -187,8 +187,10 @@ TEST(Odom2d, VoteForKfAndSolve)
TimeStamp t0(0.0), t = t0;
double dt = .01;
// Origin frame:
- Vector3d x0(0,0,0);
- Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1;
+ // Vector3d x0(0,0,0);
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1;
+ VectorComposite P0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
// motion data
VectorXd data(Vector2d(1, M_PI_4) ); // advance 1m turn pi/4 rad (45 deg). Need 8 steps for a complete turn
Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
@@ -229,8 +231,8 @@ TEST(Odom2d, VoteForKfAndSolve)
// std::cout << "Motion data : " << data.transpose() << std::endl;
// Check covariance values
- Eigen::Vector3d integrated_pose = x0;
- Eigen::Matrix3d integrated_cov = P0;
+ Eigen::Vector3d integrated_pose = Vector3d(0,0,0);
+ Eigen::Matrix3d integrated_cov = Eigen::Matrix3d::Identity() * 0.1;
Eigen::Vector3d integrated_delta = Eigen::Vector3d::Zero();
Eigen::Matrix3d integrated_delta_cov = Eigen::Matrix3d::Zero();
Eigen::MatrixXd Ju(3, 2);
@@ -325,8 +327,8 @@ TEST(Odom2d, KF_callback)
// time
TimeStamp t0(0.0), t = t0;
double dt = .01;
- Vector3d x0(0,0,0);
- Eigen::Matrix3d x0_cov = Eigen::Matrix3d::Identity() * 0.1;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite x0_cov(Vector3d(0.1,0.1,0.1), "PO", {2,1});
VectorXd data(Vector2d(1, M_PI/4) ); // advance 1m
Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
int N = 8; // number of process() steps
@@ -362,8 +364,8 @@ TEST(Odom2d, KF_callback)
processor_odom2d->setOrigin(keyframe_0);
// Check covariance values
- Eigen::Vector3d integrated_pose = x0;
- Eigen::Matrix3d integrated_cov = x0_cov;
+ Eigen::Vector3d integrated_pose = Vector3d(0,0,0);
+ Eigen::Matrix3d integrated_cov = Eigen::Matrix3d::Identity() * 0.1;
Eigen::Vector3d integrated_delta = Eigen::Vector3d::Zero();
Eigen::Matrix3d integrated_delta_cov = Eigen::Matrix3d::Zero();
Eigen::MatrixXd Ju(3, 2);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 26219e7a0f6e034983de50ac8ec53d8cb075823a..f2eef1fbe81a6599c5a373d2b17de7cd1b5158ef 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -106,8 +106,10 @@ TEST(Problem, SetOrigin_PO_2d)
{
ProblemPtr P = Problem::create("PO", 2);
TimeStamp t0(0);
- Eigen::VectorXd x0(3); x0 << 1,2,3;
- Eigen::MatrixXd P0(Eigen::MatrixXd::Identity(3,3) * 0.1); // P0 is 0.1*Id
+ // Eigen::VectorXd x0(3); x0 << 1,2,3;
+ VectorComposite x0(Vector3d(1,2,3), "PO", {2,1});
+ // Eigen::MatrixXd P0(Eigen::MatrixXd::Identity(3,3) * 0.1); // P0 is 0.1*Id
+ VectorComposite P0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
P->setPriorFactor(x0, P0, t0, 1.0);
@@ -159,8 +161,11 @@ TEST(Problem, SetOrigin_PO_3d)
{
ProblemPtr P = Problem::create("PO", 3);
TimeStamp t0(0);
- Eigen::VectorXd x0(7); x0 << 1,2,3,4,5,6,7; // not normalized quaternion, this is not what's tested
- Eigen::MatrixXd P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
+ Eigen::VectorXd vec7(7); vec7 << 1,2,3,4,5,6,7; // not normalized quaternion, this is not what's tested
+ VectorComposite x0(vec7, "PO", {3,4});
+ // Eigen::MatrixXd P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
+ Eigen::VectorXd vec6(6); vec6 << sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1);
+ VectorComposite P0(vec6, "PO", {3,3});
P->setPriorFactor(x0, P0, t0, 1.0);
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 8e71af5a7729f061f656e2a9d76ed1c3dc31589e..ed01245b29c903a7584e932196fb93a2c01c5c20 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -96,8 +96,10 @@ TEST(ProcessorBase, KeyFrameCallback)
// initialize
TimeStamp t(0.0);
- Vector3d x(0,0,0);
- Matrix3d P = Matrix3d::Identity() * 0.1;
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P = Matrix3d::Identity() * 0.1;
+ VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
problem->setPriorFactor(x, P, t, dt/2); // KF1
CaptureOdom2dPtr capt_odo = make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5,0));
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 60990ad83928a235d76606208d91de35f5b9c385..ee908013b9eadd216363d528d7f60757ea331f29 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -309,8 +309,10 @@ TEST_F(ProcessorDiffDriveTest, process)
Matrix2d data_cov; data_cov . setIdentity();
TimeStamp t = 0.0;
double dt = 1.0;
- Vector3d x(0,0,0);
- Matrix3d P; P.setIdentity();
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P; P.setIdentity();
+ VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
auto F0 = problem->setPriorFactor(x, P, t, 0.1);
processor->setOrigin(F0);
@@ -338,8 +340,10 @@ TEST_F(ProcessorDiffDriveTest, linear)
Vector2d data;
Matrix2d data_cov; data_cov . setIdentity();
TimeStamp t = 0.0;
- Vector3d x(0,0,0);
- Matrix3d P; P.setIdentity();
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P; P.setIdentity();
+ VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
auto F0 = problem->setPriorFactor(x, P, t, 0.1);
processor->setOrigin(F0);
@@ -364,8 +368,10 @@ TEST_F(ProcessorDiffDriveTest, angular)
Vector2d data;
Matrix2d data_cov; data_cov . setIdentity();
TimeStamp t = 0.0;
- Vector3d x(0,0,0);
- Matrix3d P; P.setIdentity();
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P; P.setIdentity();
+ VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
auto F0 = problem->setPriorFactor(x, P, t, 0.1);
processor->setOrigin(F0);
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index 23d2f612eecfbf56f68cb6c39688a812d0d4898e..9b8683f8b74ee566370d4630a248d039257ebb17 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -78,8 +78,10 @@ TEST(ProcessorLoopClosure, installProcessor)
// initialize
TimeStamp t(0.0);
- Vector3d x(0,0,0);
- Matrix3d P = Matrix3d::Identity() * 0.1;
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P = Matrix3d::Identity() * 0.1;
+ VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
problem->setPriorFactor(x, P, t, dt/2); // KF1
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index aca6e7f5f3942d836da33e3321eb11c2fb9a1326..4ed782610c80de2e4e5dadd8c41b07acfe16515f 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -106,8 +106,10 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
{
// Prior
TimeStamp t(0.0);
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ // Vector3d x0; x0 << 0, 0, 0;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite P0(Vector3d(1,1,1), "PO", {2,1});
auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01);
processor->setOrigin(KF_0);
@@ -179,8 +181,10 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
{
// Prior
TimeStamp t(0.0);
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ // Vector3d x0; x0 << 0, 0, 0;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite P0(Vector3d(1,1,1), "PO", {2,1});
auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01);
processor->setOrigin(KF_0);
@@ -270,8 +274,10 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
{
// Prior
TimeStamp t(0.0);
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
+ // Vector3d x0; x0 << 0, 0, 0;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite P0(Vector3d(1,1,1), "PO", {2,1});
auto KF_0 = problem->setPriorFactor(x0, P0, t, 0.01);
processor->setOrigin(KF_0);