diff --git a/CMakeLists.txt b/CMakeLists.txt
index 41368240328963d68fd2a020ce1376e74b9fa5e9..681ff424bdb66621b50c123fe37ed00ce06d880f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,6 @@
# Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+MESSAGE("Starting ${PLUGIN_NAME} CMakeLists ...")
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
if(COMMAND cmake_policy)
cmake_policy(SET CMP0005 NEW)
@@ -72,8 +73,6 @@ if(BUILD_TESTS)
enable_testing()
endif()
-MESSAGE("Starting ${PLUGIN_NAME} CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
#CMAKE modules
diff --git a/test/gtest_factor_epipolar.cpp b/test/gtest_factor_epipolar.cpp
index ddb6d5db49918ecd233369121cb56776bbe9a996..f468fc3e7ca158dde67a7f7f0a77d0501e15bc78 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 ->emplaceKeyFrame(0.0, pose0);
- auto F1 = P ->emplaceKeyFrame(1.0, pose1);
+ auto F0 = P ->emplaceFrame(0.0, pose0);
+ auto F1 = P ->emplaceFrame(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 a6e9bd316c6753e7d09ff2fc81849627b0679db6..70875a7c0ff2c70c0408f1749b3ae5627505cedf 100644
--- a/test/gtest_factor_pixel_hp.cpp
+++ b/test/gtest_factor_pixel_hp.cpp
@@ -169,15 +169,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->emplaceKeyFrame(1.0, pose1);
+ F1 = problem->emplaceFrame(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->emplaceKeyFrame(2.0, pose2);
+ F2 = problem->emplaceFrame(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->emplaceKeyFrame(3.0, pose3);
+ F3 = problem->emplaceFrame(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
@@ -217,7 +217,7 @@ TEST(ProcessorFactorPixelHp, testZeroResidual)
ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
// Frame
- FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0.0, problem_ptr->stateZero());
+ FrameBasePtr frm0 = problem_ptr->emplaceFrame(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)));
@@ -279,7 +279,8 @@ TEST_F(FactorPixelHpTest, testSolveLandmarkAltered)
L1->unfix();
auto orig = L1->point();
- L1->getP()->setState(L1->getState().vector("P") + Vector4d::Random());
+ L1->getP()->perturb(1.0);
+
std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
std::cout << report << std::endl;
@@ -341,11 +342,11 @@ TEST_F(FactorPixelHpTest, testSolveFramePosition)
//landmarks to camera coordinates
Transform<double,3,Isometry> T_w_r
- = Translation<double,3>(F1->getP()->getState())
- * Quaterniond(F1->getO()->getState().data());
+ = Translation<double,3>(F1->getP()->getState())
+ * Quaterniond(F1->getO()->getState().data());
Transform<double,3,Isometry> T_r_c
- = Translation<double,3>(I1->getSensorP()->getState())
- * Quaterniond(I1->getSensorO()->getState().data());
+ = Translation<double,3>(I1->getSensorP()->getState())
+ * Quaterniond(I1->getSensorO()->getState().data());
Eigen::Matrix<double, 4, 1> lmkHP2_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP2;
Eigen::Matrix<double, 4, 1> lmkHP3_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP3;
Eigen::Matrix<double, 4, 1> lmkHP4_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP4;
@@ -395,15 +396,10 @@ TEST_F(FactorPixelHpTest, testSolveFramePosition)
auto orig = F1->getP()->getState();
//change state
- Vector3d position;
- position << Vector3d::Random()*100;//2.0, 2.0, 2.0;
- auto ori = F1->getO()->getState();
-// Vector7d state;
-// state << position, ori;
- F1->setState("PO", {position, ori});
+ F1->getP()->unfix();
+ F1->getP()->perturb(0.1);
F1->getO()->fix();
- F1->getP()->unfix();
std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
@@ -415,9 +411,7 @@ TEST_F(FactorPixelHpTest, testSolveFramePosition)
// This test checks 3 DoF (3doF are observable).
ASSERT_MATRIX_APPROX(F1->getP()->getState(), orig, 1e-6);
- Eigen::VectorXd expect = c11->expectation();
- ASSERT_FLOAT_EQ(expect(0,0),f11->getMeasurement()(0,0));
- ASSERT_FLOAT_EQ(expect(1,0),f11->getMeasurement()(1,0));
+ ASSERT_MATRIX_APPROX(c11->expectation(), f11->getMeasurement(), 1e-3);
}
@@ -584,26 +578,7 @@ TEST_F(FactorPixelHpTest, testSolveBundleAdjustment)
ASSERT_MATRIX_APPROX(L4->getP()->getState(), l4, 1e-6);
// perturb states
-
- // kfs
- for (auto kf : *problem->getTrajectory())
- {
- if (kf == F1) continue;
-
- if (!kf->getP()->isFixed())
- kf->getP()->setState(kf->getP()->getState() + 0.2*Vector3d::Random());
- if (!kf->getO()->isFixed())
- kf->getO()->setState((Quaterniond(kf->getO()->getState().data()) * exp_q(0.2*Vector3d::Random())).coeffs());
- }
-
- // lmks
- for (auto lmk : problem->getMap()->getLandmarkList())
- {
- if (lmk == L1) continue;
-
- if (!lmk->isFixed())
- lmk->getP()->setState(lmk->getP()->getState() + 0.2*Vector4d::Random());
- }
+ problem->perturb(0.1);
// solve again
problem->print(1,0,1,1);
diff --git a/test/gtest_factor_trifocal.cpp b/test/gtest_factor_trifocal.cpp
index 4d67f4ecfc625b275f26348cfd9a257853186969..0d15c6bb3d4e17d8e79f159c36d6904276caf533 100644
--- a/test/gtest_factor_trifocal.cpp
+++ b/test/gtest_factor_trifocal.cpp
@@ -141,15 +141,15 @@ class FactorTrifocalTest : public testing::Test{
Vector2d pix(0,0);
Matrix2d pix_cov(Matrix2d::Identity() * pow(pixel_noise_std, 2));
- F1 = problem->emplaceKeyFrame(1.0, pose1);
+ F1 = problem->emplaceFrame(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->emplaceKeyFrame(2.0, pose2);
+ F2 = problem->emplaceFrame(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->emplaceKeyFrame(3.0, pose3);
+ F3 = problem->emplaceFrame(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
@@ -182,7 +182,7 @@ TEST_F(FactorTrifocalTest, InfoMatrix)
*/
Matrix3d sqrt_info_gt = Matrix3d::Identity() / pixel_noise_std / sqrt(2.0);
- ASSERT_MATRIX_APPROX(c123->getSqrtInformationUpper(), sqrt_info_gt, 1e-8);
+ ASSERT_MATRIX_APPROX(c123->getSqrtInformationUpper(), sqrt_info_gt, 1e-6);
}
@@ -228,10 +228,10 @@ TEST_F(FactorTrifocalTest, expectation)
l2 = c2Ec1 * _m1;
// check epipolar lines (check only director vectors for equal direction)
- ASSERT_MATRIX_APPROX(l2/l2(1), _l2/_l2(1), 1e-8);
+ ASSERT_MATRIX_APPROX(l2/l2(1), _l2/_l2(1), 1e-6);
// check perpendicular lines (check only director vectors for orthogonal direction)
- ASSERT_NEAR(l2(0)*_p2(0) + l2(1)*_p2(1), 0, 1e-8);
+ ASSERT_NEAR(l2(0)*_p2(0) + l2(1)*_p2(1), 0, 1e-6);
// Verify trilinearities
@@ -241,18 +241,18 @@ TEST_F(FactorTrifocalTest, expectation)
// Point-line-point
Vector3d plp = wolf::skew(_m3) * m1Tt * _p2;
- ASSERT_MATRIX_APPROX(plp, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(plp, Vector3d::Zero(), 1e-6);
// Point-point-line
Vector3d ppl = _p3.transpose() * m1Tt * wolf::skew(_m2);
- ASSERT_MATRIX_APPROX(ppl, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(ppl, Vector3d::Zero(), 1e-6);
// Point-point-point
Matrix3d ppp = wolf::skew(_m3) * m1Tt * wolf::skew(_m2);
- ASSERT_MATRIX_APPROX(ppp, Matrix3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(ppp, Matrix3d::Zero(), 1e-6);
// check epipolars
- ASSERT_MATRIX_APPROX(c2Ec1/c2Ec1(0,1), _c2Ec1/_c2Ec1(0,1), 1e-8);
+ ASSERT_MATRIX_APPROX(c2Ec1/c2Ec1(0,1), _c2Ec1/_c2Ec1(0,1), 1e-6);
}
TEST_F(FactorTrifocalTest, residual)
@@ -265,7 +265,7 @@ TEST_F(FactorTrifocalTest, residual)
c123->expectation(pos1, quat1, pos2, quat2, pos3, quat3, pos_cam, quat_cam, tensor, c2Ec1);
residual = c123->residual(tensor, c2Ec1);
- ASSERT_MATRIX_APPROX(residual, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(residual, Vector3d::Zero(), 1e-6);
}
TEST_F(FactorTrifocalTest, error_jacobians)
@@ -350,7 +350,7 @@ TEST_F(FactorTrifocalTest, operator_parenthesis)
pos_cam.data(), vquat_cam.data(),
res.data());
- ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
}
TEST_F(FactorTrifocalTest, solve_F1)
@@ -381,15 +381,10 @@ TEST_F(FactorTrifocalTest, solve_F1)
WOLF_DEBUG("Initial state: ", F1->getState().transpose());
WOLF_DEBUG("residual before perturbing: ", res.transpose());
- ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
// Residual with perturbated state
-
-// Vector7d pose_perturbated = F1->getState() + 0.1 * Vector7d::Random();
-// pose_perturbated.segment(3,4).normalize();
-// F1->setState(pose_perturbated);
F1->perturb(0.1);
-// VectorXd pose_perturbated = F1->getState().vector("PO");
F1_p = F1->getP()->getState();
F1_o = F1->getO()->getState();
@@ -432,7 +427,7 @@ TEST_F(FactorTrifocalTest, solve_F1)
WOLF_DEBUG(report, " AND UNION");
- ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
}
@@ -465,7 +460,7 @@ TEST_F(FactorTrifocalTest, solve_F2)
WOLF_DEBUG("Initial state: ", F2->getState().vector("PO").transpose());
WOLF_DEBUG("residual before perturbing: ", res.transpose());
- ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
// Residual with perturbated state
@@ -512,7 +507,7 @@ TEST_F(FactorTrifocalTest, solve_F2)
WOLF_DEBUG(report, " AND UNION");
- ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
}
@@ -545,7 +540,7 @@ TEST_F(FactorTrifocalTest, solve_F3)
WOLF_DEBUG("Initial state: ", F3->getState().vector("PO").transpose());
WOLF_DEBUG("residual before perturbing: ", res.transpose());
- ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
// Residual with perturbated state
@@ -562,7 +557,7 @@ TEST_F(FactorTrifocalTest, solve_F3)
WOLF_DEBUG("perturbed state: ", F3->getState().vector("PO").transpose());
WOLF_DEBUG("residual before solve: ", res.transpose());
- ASSERT_NEAR(res(2), 0, 1e-8); // Epipolar c2-c1 should be respected when perturbing F3
+ ASSERT_NEAR(res(2), 0, 1e-6); // Epipolar c2-c1 should be respected when perturbing F3
// Residual with solved state
@@ -593,7 +588,7 @@ TEST_F(FactorTrifocalTest, solve_F3)
WOLF_DEBUG(report, " AND UNION");
- ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
}
@@ -626,7 +621,7 @@ TEST_F(FactorTrifocalTest, solve_S)
WOLF_DEBUG("Initial state: ", S->getP()->getState().transpose(), " ", S->getO()->getState().transpose());
WOLF_DEBUG("residual before perturbing: ", res.transpose());
- ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
// Residual with perturbated state
@@ -673,7 +668,7 @@ TEST_F(FactorTrifocalTest, solve_S)
WOLF_DEBUG(report, " AND UNION");
- ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
}
@@ -769,12 +764,7 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point)
F3->getO()->unfix(); // Estimate Cam 3 ori
// Perturbate states, keep scale
- F1->getP()->setState( pos1 );
- F1->getO()->setState( vquat1 );
- F2->getP()->setState( pos2 ); // this fixes the scale
- F2->getO()->setState((vquat2 + 0.2*Vector4d::Random()).normalized());
- F3->getP()->setState( pos3 + 0.2*Vector3d::Random());
- F3->getO()->setState((vquat3 + 0.2*Vector4d::Random()).normalized());
+ problem->perturb(0.1);
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
@@ -807,7 +797,7 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point)
S_p. data(), S_o. data(),
res.data());
- ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
}
}
@@ -834,12 +824,13 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_scale)
F3->getO()->unfix(); // Estimate Cam 3 ori
// Perturbate states, change scale
+// problem->perturb(0.1);
F1->getP()->setState( 2 * pos1 );
F1->getO()->setState( vquat1 );
F2->getP()->setState( 2 * pos2 );
- F2->getO()->setState(( vquat2 + 0.2*Vector4d::Random()).normalized());
- F3->getP()->setState( 2 * pos3 + 0.2*Vector3d::Random());
- F3->getO()->setState(( vquat3 + 0.2*Vector4d::Random()).normalized());
+ F2->getO()->setState(( vquat2 + 0.1*Vector4d::Random()).normalized());
+ F3->getP()->setState( 2 * pos3 + 0.1*Vector3d::Random());
+ F3->getO()->setState(( vquat3 + 0.1*Vector4d::Random()).normalized());
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -848,10 +839,10 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_scale)
problem->print(1,0,1,0);
// Evaluate final states
- ASSERT_MATRIX_APPROX(F2->getP()->getState(), 2 * pos2, 1e-8);
- ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-8);
- ASSERT_MATRIX_APPROX(F3->getP()->getState(), 2 * pos3, 1e-8);
- ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-8);
+ ASSERT_MATRIX_APPROX(F2->getP()->getState(), 2 * pos2, 1e-6);
+ ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-6);
+ ASSERT_MATRIX_APPROX(F3->getP()->getState(), 2 * pos3, 1e-6);
+ ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-6);
Eigen::VectorXd F1_p = F1->getP()->getState();
Eigen::VectorXd F1_o = F1->getO()->getState();
@@ -872,7 +863,7 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_scale)
S_p. data(), S_o. data(),
res.data());
- ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
}
}
@@ -900,6 +891,7 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_distance)
F3->getO()->unfix(); // Estimate Cam 3 ori
// Perturbate states, change scale
+// problem->perturb(0.1);
F1->getP()->setState( pos1 );
F1->getO()->setState( vquat1 );
F2->getP()->setState( pos2 + 0.2*Vector3d::Random() );
@@ -936,10 +928,10 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_distance)
problem->print(1,0,1,0);
// Evaluate final states
- ASSERT_MATRIX_APPROX(F2->getP()->getState(), pos2 , 1e-8);
- ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-8);
- ASSERT_MATRIX_APPROX(F3->getP()->getState(), pos3 , 1e-8);
- ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-8);
+ ASSERT_MATRIX_APPROX(F2->getP()->getState(), pos2 , 1e-6);
+ ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-6);
+ ASSERT_MATRIX_APPROX(F3->getP()->getState(), pos3 , 1e-6);
+ ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-6);
Eigen::VectorXd F1_p = F1->getP()->getState();
Eigen::VectorXd F1_o = F1->getO()->getState();
@@ -960,7 +952,7 @@ TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_distance)
S_p. data(), S_o. data(),
res.data());
- ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-6);
}
}
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
index d37e1911e21902c1e7d98e666c531cd6ee4b5e7b..a5a8042adcc92605e3b6cf28228f649059a426ae 100644
--- a/test/gtest_processor_bundle_adjustment.cpp
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -26,7 +26,11 @@ class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
{
public:
- ProcessorBundleAdjustmentDummy(ParamsProcessorBundleAdjustmentPtr& _params_bundle_adjustment):ProcessorBundleAdjustment(_params_bundle_adjustment){}
+ ProcessorBundleAdjustmentDummy(ParamsProcessorBundleAdjustmentPtr& _params_bundle_adjustment):
+ ProcessorBundleAdjustment(_params_bundle_adjustment)
+ {
+
+ }
void setLast(CaptureImagePtr _last_ptr)
{
@@ -68,11 +72,9 @@ class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
return matches_last_from_incoming_;
}
- CaptureImagePtr createCaptureImage(std::string _path, SensorCameraPtr _sensor, bool detectAndDescript = false)
+ CaptureImagePtr createCaptureImage(TimeStamp _ts, std::string _path, SensorCameraPtr _sensor, bool detectAndDescript = false)
{
- const double time = 0.0;
- TimeStamp ts(time);
- CaptureImagePtr im = std::make_shared<CaptureImage>(ts, _sensor, cv::imread(_path));
+ CaptureImagePtr im = std::make_shared<CaptureImage>(_ts, _sensor, cv::imread(_path));
if (detectAndDescript){
// Detect KeyPoints
im->keypoints_ = det_ptr_->detect(im->getImage());
@@ -138,10 +140,10 @@ TEST(ProcessorBundleAdjustment, preProcess)
auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
// Put an image on incoming_ptr_
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(0, wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
proc_dummy->setInc(image_inc_ptr);
// Put an image on last_ptr_
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(1, wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
proc_dummy->setLast(image_last_ptr);
// demo dpreProcess
proc_dummy->preProcess();
@@ -169,7 +171,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
// Put an image on last_ptr_
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(0, wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
ASSERT_NE(image_last_ptr->keypoints_.size(), 0);
proc_dummy->setLast(image_last_ptr);
@@ -178,7 +180,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
ASSERT_EQ(num, 0);
// Put an image on incoming_ptr_
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(1, wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
proc_dummy->setInc(image_inc_ptr);
// demo detectNewFeatures
@@ -212,9 +214,9 @@ TEST(ProcessorBundleAdjustment, trackFeatures)
//fill feat_last list
FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
- CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(0, wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
proc_dummy->setLast(image_last_ptr);
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(1, wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
proc_dummy->setInc(image_inc_ptr);
proc_dummy->preProcess();
CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
@@ -255,7 +257,7 @@ TEST(ProcessorBundleAdjustment, emplaceLandmark)
ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
//Frame
- FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0.0, problem_ptr->stateZero());
+ FrameBasePtr frm0 = problem_ptr->emplaceFrame(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)));
@@ -282,10 +284,10 @@ TEST(ProcessorBundleAdjustment, process)
ProblemPtr problem = Problem::create("PO", 3);
// Install camera
- ParamsSensorCameraPtr intr = std::make_shared<ParamsSensorCamera>(); // TODO init params or read from YAML
- intr->pinhole_model_raw = Eigen::Vector4d(0,0,1,1); //TODO: initialize
- intr->width = 640;
- intr->height = 480;
+ ParamsSensorCameraPtr intr = std::make_shared<ParamsSensorCamera>();
+ intr->pinhole_model_raw = Eigen::Vector4d(640, 480, 640, 640);
+ intr->width = 1280;
+ intr->height = 960;
SensorCameraPtr sens_cam = std::static_pointer_cast<SensorCamera>(problem->installSensor("SensorCamera", "camera", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), intr));
// Install processor
@@ -303,38 +305,42 @@ TEST(ProcessorBundleAdjustment, process)
auto proc_dummy = std::static_pointer_cast<ProcessorBundleAdjustmentDummy>(proc);
//1ST TIME
- CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(1, wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
image_inc_ptr->process();
-// proc_dummy->process(image_inc_ptr);
+
+ problem->print(4,0,1,0);
+
ASSERT_EQ(proc_dummy->getTrackMatrix().numTracks(), 0);
ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 0);
//2ND TIME
- CaptureImagePtr image_inc_ptr2 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
-// proc_dummy->setInc(image_inc_ptr2);
-// proc_dummy->preProcess();
-// proc_dummy->getProcessKnown();
-// proc_dummy->getProcessNew(params->max_new_features);
-// proc_dummy->establishFactors();
+ CaptureImagePtr image_inc_ptr2 = proc_dummy->createCaptureImage(2, wolf_vision_root + "/demos/demo_gazebo_x00cm_y-10cm.jpg", sens_cam);
image_inc_ptr2->process();
-// proc_dummy->process(image_inc_ptr2);
+
+ problem->print(4,0,1,0);
+
ASSERT_LE(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
ASSERT_NE(problem->getMap(), nullptr);
- ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), true);
//3RD TIME -- RUNNING
- CaptureImagePtr image_inc_ptr3 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+ CaptureImagePtr image_inc_ptr3 = proc_dummy->createCaptureImage(3, wolf_vision_root + "/demos/demo_gazebo_x00cm_y-20cm.jpg", sens_cam);
assert(proc_dummy->getOrigin()!=nullptr);
assert(proc_dummy->getLast()!= nullptr && proc_dummy->getLast()!=proc_dummy->getOrigin());
image_inc_ptr3->process();
-// proc_dummy->process(image_inc_ptr3);
+
+ problem->print(4,0,1,0);
+
+
ASSERT_LE(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
//4TH TIME -- RUNNING
- CaptureImagePtr image_inc_ptr4 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+ CaptureImagePtr image_inc_ptr4 = proc_dummy->createCaptureImage(4, wolf_vision_root + "/demos/demo_gazebo_x-10cm_y-20cm.jpg", sens_cam);
image_inc_ptr4->process();
-// proc_dummy->process(image_inc_ptr4);
+
+ problem->print(4,0,1,0);
+
+
ASSERT_LE(image_inc_ptr4->getFeatureList().size(), params->max_new_features);
}
@@ -353,6 +359,7 @@ TEST(ProcessorBundleAdjustment, processNew)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
+// ::testing::GTEST_FLAG(filter) = "ProcessorBundleAdjustment.process";
return RUN_ALL_TESTS();
}