Skip to content
Snippets Groups Projects
Commit f5cd992b authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Fix covariances and vector sizes

parent f593031f
No related branches found
No related tags found
1 merge request!276Several fixes for Mac
This commit is part of merge request !276. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
Showing
with 8 additions and 14 deletions
test/gtest_emplace.cpp
+ 8
− 14
View file @ f5cd992b
...@@ -83,10 +83,8 @@ TEST(Emplace, Feature) ...@@ -83,10 +83,8 @@ TEST(Emplace, Feature)
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem()); ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem()); ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame()); ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
auto cov = Eigen::MatrixXs(2,2); auto cov = Eigen::MatrixXs::Identity(2,2);
cov(0,0) = 1; FeatureBase::emplace<FeatureBase>(cpt, "Dummy", Eigen::VectorXs(2), cov);
cov(1,1) = 1;
FeatureBase::emplace<FeatureBase>(cpt, "Dummy", Eigen::VectorXs(5), cov);
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem()); ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem()); ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture()); ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
...@@ -103,10 +101,8 @@ TEST(Emplace, Factor) ...@@ -103,10 +101,8 @@ TEST(Emplace, Factor)
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem()); ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem()); ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame()); ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
auto cov = Eigen::MatrixXs(2,2); auto cov = Eigen::MatrixXs::Identity(2,2);
cov(0,0) = 1; auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(2), cov);
cov(1,1) = 1;
auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(5), cov);
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem()); ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem()); ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture()); ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
...@@ -121,17 +117,15 @@ TEST(Emplace, EmplaceDerived) ...@@ -121,17 +117,15 @@ TEST(Emplace, EmplaceDerived)
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
// LandmarkBase::emplace<LandmarkBase>(MapBaseWPtr(P->getMap()),"Dummy", nullptr, nullptr); // LandmarkBase::emplace<LandmarkBase>(MapBaseWPtr(P->getMap()),"Dummy", nullptr, nullptr);
auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXs(3), IntrinsicsOdom2D()); auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXs(3), IntrinsicsOdom2D());
auto cov = Eigen::MatrixXs(2,2); auto cov = Eigen::MatrixXs::Identity(2,2);
cov(0,0) = 1; auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::VectorXs(2), cov, frm);
cov(1,1) = 1;
auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::Vector6s(), cov, frm);
auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt); auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt);
auto m = Eigen::Matrix<Scalar,9,6>(); auto m = Eigen::Matrix<Scalar,9,6>();
for(int i = 0; i < 9; i++) for(int i = 0; i < 9; i++)
for(int j = 0; j < 6; j++) for(int j = 0; j < 6; j++)
m(i,j) = 1; m(i,j) = 1;
auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(5), cov); auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(2), cov);
ASSERT_EQ(sen, P->getHardware()->getSensorList().front()); ASSERT_EQ(sen, P->getHardware()->getSensorList().front());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem()); ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
} }
...@@ -143,4 +137,4 @@ int main(int argc, char **argv) ...@@ -143,4 +137,4 @@ int main(int argc, char **argv)
{ {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment