From 1c4e2efcd59445a806b3bbe51332682ab9fa1026 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Thu, 11 Jun 2020 12:19:34 +0200
Subject: [PATCH] [WIP] fix trajectory iterators & emplaceKeyFrame
One test fails
---
demos/demo_simple_AHP.cpp | 8 ++++----
demos/demo_sort_keyframes.cpp | 4 ++--
demos/demo_sparsification.cpp | 4 ++--
test/gtest_factor_epipolar.cpp | 4 ++--
test/gtest_factor_pixel_hp.cpp | 10 +++++-----
test/gtest_factor_trifocal.cpp | 6 +++---
test/gtest_processor_bundle_adjustment.cpp | 2 +-
7 files changed, 19 insertions(+), 19 deletions(-)
diff --git a/demos/demo_simple_AHP.cpp b/demos/demo_simple_AHP.cpp
index a6af99e96..b99d765e3 100644
--- a/demos/demo_simple_AHP.cpp
+++ b/demos/demo_simple_AHP.cpp
@@ -97,11 +97,11 @@ int main(int argc, char** argv)
/* 1 */
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,(Vector7d()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
- FrameBasePtr kf_2 = problem->emplaceFrame(KEY,(Vector7d()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
+ FrameBasePtr kf_1 = problem->emplaceKeyFrame((Vector7d()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
+ FrameBasePtr kf_2 = problem->emplaceKeyFrame((Vector7d()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
// and two other frames to observe the lmk
- FrameBasePtr kf_3 = problem->emplaceFrame(KEY,(Vector7d()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0));
- FrameBasePtr kf_4 = problem->emplaceFrame(KEY,(Vector7d()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0));
+ FrameBasePtr kf_3 = problem->emplaceKeyFrame((Vector7d()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0));
+ FrameBasePtr kf_4 = problem->emplaceKeyFrame((Vector7d()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0));
kf_1->fix();
kf_2->fix();
diff --git a/demos/demo_sort_keyframes.cpp b/demos/demo_sort_keyframes.cpp
index ae3dfb9aa..4ee482fb8 100644
--- a/demos/demo_sort_keyframes.cpp
+++ b/demos/demo_sort_keyframes.cpp
@@ -65,12 +65,12 @@ int main()
printFrames(problem_ptr);
- FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXd::Zero(3), TimeStamp(0.7));
+ FrameBasePtr frm7 = problem_ptr->emplaceKeyFrame(Eigen::VectorXd::Zero(3), TimeStamp(0.7));
std::cout << std::endl << "created Key Frame " << frm7->id() << " TS: " << 0.7 << std::endl;
printFrames(problem_ptr);
- FrameBasePtr frm8 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXd::Zero(3), TimeStamp(0.35));
+ FrameBasePtr frm8 = problem_ptr->emplaceKeyFrame(Eigen::VectorXd::Zero(3), TimeStamp(0.35));
std::cout << std::endl << "created Key Frame " << frm8->id() << " TS: " << 0.35 << std::endl;
printFrames(problem_ptr);
diff --git a/demos/demo_sparsification.cpp b/demos/demo_sparsification.cpp
index eaa840df5..4cdde86f6 100644
--- a/demos/demo_sparsification.cpp
+++ b/demos/demo_sparsification.cpp
@@ -216,7 +216,7 @@ int main(int argc, char** argv)
// ------------------------ START EXPERIMENT ------------------------
// First frame FIXED
- last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, Eigen::Vector3d::Zero(),TimeStamp(0));
+ last_frame_ptr = bl_problem_ptr->emplaceKeyFrame(Eigen::Vector3d::Zero(),TimeStamp(0));
last_frame_ptr->fix();
bl_problem_ptr->print(4, true, false, true);
@@ -238,7 +238,7 @@ int main(int argc, char** argv)
Eigen::Vector3d from_pose = frame_from_ptr->getState();
R.topLeftCorner(2,2) = Eigen::Rotation2Dd(from_pose(2)).matrix();
Eigen::Vector3d new_frame_pose = from_pose + R*meas;
- last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, new_frame_pose, TimeStamp(double(edge_to)));
+ last_frame_ptr = bl_problem_ptr->emplaceKeyFrame(new_frame_pose, TimeStamp(double(edge_to)));
frame_to_ptr = last_frame_ptr;
diff --git a/test/gtest_factor_epipolar.cpp b/test/gtest_factor_epipolar.cpp
index 0bf3c2677..ddb6d5db4 100644
--- a/test/gtest_factor_epipolar.cpp
+++ b/test/gtest_factor_epipolar.cpp
@@ -41,8 +41,8 @@ TEST(FactorEpipolar, exemple)
auto S = P->installSensor("SensorCamera", "camera", posecam, intr);
auto camera = std::static_pointer_cast<SensorCamera>(S);
- auto F0 = P ->emplaceFrame(KEY, 0.0, pose0);
- auto F1 = P ->emplaceFrame(KEY, 1.0, pose1);
+ auto F0 = P ->emplaceKeyFrame(0.0, pose0);
+ auto F1 = P ->emplaceKeyFrame(1.0, pose1);
auto C0 = CaptureBase ::emplace<CaptureImage>(F0, F0->getTimeStamp(), camera, cv::Mat());
auto C1 = CaptureBase ::emplace<CaptureImage>(F1, F1->getTimeStamp(), camera, cv::Mat());
auto f0 = FeatureBase ::emplace<FeaturePointImage>(C0, pix0, 0, cv::Mat(), Matrix2d::Identity());
diff --git a/test/gtest_factor_pixel_hp.cpp b/test/gtest_factor_pixel_hp.cpp
index 6246b8683..966ed9165 100644
--- a/test/gtest_factor_pixel_hp.cpp
+++ b/test/gtest_factor_pixel_hp.cpp
@@ -172,15 +172,15 @@ class FactorPixelHpTest : public testing::Test{
cv::KeyPoint kp = cv::KeyPoint(p, 32.0f);
cv::Mat des = cv::Mat::zeros(1,8, CV_8UC1);
- F1 = problem->emplaceFrame(KEY, 1.0, pose1);
+ F1 = problem->emplaceKeyFrame(1.0, pose1);
I1 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F1, 1.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1)));
f11 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp, 0, des, pix_cov)); // pixel at origin
- F2 = problem->emplaceFrame(KEY, 2.0, pose2);
+ F2 = problem->emplaceKeyFrame(2.0, pose2);
I2 = std::static_pointer_cast<CaptureImage>((CaptureBase::emplace<CaptureImage>(F2, 2.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1))));
f21 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I2, kp, 0, des, pix_cov)); // pixel at origin
- F3 = problem->emplaceFrame(KEY, 3.0, pose3);
+ F3 = problem->emplaceKeyFrame(3.0, pose3);
I3 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F3, 3.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1)));
f31 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I3, kp, 0, des, pix_cov)); // pixel at origin
@@ -221,7 +221,7 @@ TEST(ProcessorFactorPixelHp, testZeroResidual)
ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
// Frame
- FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0.0, problem_ptr->stateZero());
+ FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0.0, problem_ptr->stateZero());
// Capture
auto cap0 = std::static_pointer_cast<CaptureImage>(CaptureImage::emplace<CaptureImage>(frm0, TimeStamp(0), camera, cv::Mat::zeros(480,640, 1)));
@@ -590,7 +590,7 @@ TEST_F(FactorPixelHpTest, testSolveBundleAdjustment)
// perturb states
// kfs
- for (auto kf : problem->getTrajectory()->getFrameList())
+ for (auto kf : *problem->getTrajectory())
{
if (kf == F1) continue;
diff --git a/test/gtest_factor_trifocal.cpp b/test/gtest_factor_trifocal.cpp
index 418fba909..4600a202f 100644
--- a/test/gtest_factor_trifocal.cpp
+++ b/test/gtest_factor_trifocal.cpp
@@ -142,15 +142,15 @@ class FactorTrifocalTest : public testing::Test{
Vector2d pix(0,0);
Matrix2d pix_cov(Matrix2d::Identity() * pow(pixel_noise_std, 2));
- F1 = problem->emplaceFrame(KEY, 1.0, pose1);
+ F1 = problem->emplaceKeyFrame(1.0, pose1);
I1 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F1, 1.0, camera, cv::Mat(2,2,CV_8UC1)));
f1 = FeatureBase::emplace<FeatureBase>(I1, "PIXEL", pix, pix_cov); // pixel at origin
- F2 = problem->emplaceFrame(KEY, 2.0, pose2);
+ F2 = problem->emplaceKeyFrame(2.0, pose2);
I2 = std::static_pointer_cast<CaptureImage>((CaptureBase::emplace<CaptureImage>(F2, 2.0, camera, cv::Mat(2,2,CV_8UC1))));
f2 = FeatureBase::emplace<FeatureBase>(I2, "PIXEL", pix, pix_cov); // pixel at origin
- F3 = problem->emplaceFrame(KEY, 3.0, pose3);
+ F3 = problem->emplaceKeyFrame(3.0, pose3);
I3 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F3, 3.0, camera, cv::Mat(2,2,CV_8UC1)));
f3 = FeatureBase::emplace<FeatureBase>(I3, "PIXEL", pix, pix_cov); // pixel at origin
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index ca5670248..d37e1911e 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -255,7 +255,7 @@ TEST(ProcessorBundleAdjustment, emplaceLandmark)
ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
//Frame
- FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0.0, problem_ptr->stateZero());
+ FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0.0, problem_ptr->stateZero());
// Capture, feature and factor
auto cap0 = std::static_pointer_cast<CaptureImage>(CaptureImage::emplace<CaptureImage>(frm0, TimeStamp(0), camera, cv::Mat::zeros(480,640, 1)));
--
GitLab