diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index 54bbb3aec498cacc736321f77abc127a24414106..dbd86a30e2a5c3499c4aca126f12f875d98b0a48 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -75,7 +75,7 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
{
// new landmark:
// - create landmark
- lmk = std::static_pointer_cast<LandmarkPoint2D>(LandmarkBase::emplace<LandmarkPoint2D>(getProblem()->getMap(), id, invObserve(range, bearing)));
+ lmk = LandmarkBase::emplace<LandmarkPoint2D>(getProblem()->getMap(), id, invObserve(range, bearing));
WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose());
// - add to known landmarks
known_lmks.emplace(id, lmk);
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index eab55cc02cf4d12045fddf7813c4d7cdf87f35b4..60b395cf8719bb2f10ce1e481888afe1c8c240f9 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -759,16 +759,10 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen:
// create origin capture with the given state as data
// Capture fix only takes 3D position and Quaternion orientation
CapturePosePtr init_capture;
- CaptureBasePtr init_capture_base;
- // init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
- // init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state, _prior_cov);
if (this->getFrameStructure() == "POV" and this->getDim() == 3)
- init_capture_base = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
+ init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
else
- init_capture_base = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
-
- init_capture = std::static_pointer_cast<CapturePose>(init_capture_base);
- // origin_keyframe->addCapture(init_capture);
+ init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
// emplace feature and factor
init_capture->emplaceFeatureAndFactor();
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 395a2520b00ddbb85ecabae25d4c03bc6e09dda2..a1abf31c8acfb5d58006b6c3ad170893bdfb6b1e 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -207,15 +207,15 @@ CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_o
StateBlockPtr i_ptr = _sensor->isIntrinsicDynamic()?
std::make_shared<StateBlock>(3, false) : nullptr;
- auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureWheelJointPosition>(_frame_own,
- _ts,
- _sensor,
- _data,
- _data_cov,
- _frame_origin,
- nullptr,
- nullptr,
- i_ptr));
+ auto cap_motion = CaptureBase::emplace<CaptureWheelJointPosition>(_frame_own,
+ _ts,
+ _sensor,
+ _data,
+ _data_cov,
+ _frame_origin,
+ nullptr,
+ nullptr,
+ i_ptr);
cap_motion->setCalibration(_calib);
cap_motion->setCalibrationPreint(_calib_preint);
@@ -225,8 +225,10 @@ CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_o
FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin)
{
- auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature, _feature, _capture_origin->getFrame(),
- shared_from_this());
+ auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature,
+ _feature,
+ _capture_origin->getFrame(),
+ shared_from_this());
return fac_odom;
}
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 48a2747789e943fc76a171c894c5802635bc0093..16253a32fa230da41fb6f123f151304640cdef8f 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -153,7 +153,7 @@ CaptureMotionPtr ProcessorOdom2D::emplaceCapture(const FrameBasePtr& _frame_own,
const VectorXs& _calib_preint,
const FrameBasePtr& _frame_origin)
{
- auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureOdom2D>(_frame_own, _ts, _sensor, _data, _data_cov, _frame_origin));
+ auto cap_motion = CaptureBase::emplace<CaptureOdom2D>(_frame_own, _ts, _sensor, _data, _data_cov, _frame_origin);
cap_motion->setCalibration(_calib);
cap_motion->setCalibrationPreint(_calib_preint);
@@ -162,7 +162,9 @@ CaptureMotionPtr ProcessorOdom2D::emplaceCapture(const FrameBasePtr& _frame_own,
FactorBasePtr ProcessorOdom2D::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
{
- auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature, _feature, _capture_origin->getFrame(),
+ auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature,
+ _feature,
+ _capture_origin->getFrame(),
shared_from_this());
return fac_odom;
}
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 76fc35d3bc166eb30e878ab2e61b4f4fbe2950e3..3168276f93aa8da7c5220aaba098503863ff0e68 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -404,7 +404,7 @@ CaptureMotionPtr ProcessorOdom3D::emplaceCapture(const FrameBasePtr& _frame_own,
const VectorXs& _calib_preint,
const FrameBasePtr& _frame_origin)
{
- auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureOdom3D>(_frame_own, _ts, _sensor, _data, _data_cov, _frame_origin));
+ auto cap_motion = CaptureBase::emplace<CaptureOdom3D>(_frame_own, _ts, _sensor, _data, _data_cov, _frame_origin);
cap_motion->setCalibration(_calib);
cap_motion->setCalibrationPreint(_calib_preint);
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 1f508dbe5cb760e6c5360d4cab16d5f4eb378ac7..da5a01b80e98cf776957422d5bca8729b89ff038 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -321,7 +321,7 @@ TEST(CeresManager, AddFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f));
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// update solver
ceres_manager_ptr->update();
@@ -343,7 +343,7 @@ TEST(CeresManager, DoubleAddFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f));
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// add factor again
P->notifyFactor(c,ADD);
@@ -368,7 +368,7 @@ TEST(CeresManager, RemoveFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f));
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// update solver
ceres_manager_ptr->update();
@@ -396,7 +396,7 @@ TEST(CeresManager, AddRemoveFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f));
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// remove factor
P->notifyFactor(c,REMOVE);
@@ -423,7 +423,7 @@ TEST(CeresManager, DoubleRemoveFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f));
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// update solver
ceres_manager_ptr->update();
@@ -550,8 +550,8 @@ TEST(CeresManager, FactorsRemoveLocalParam)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO()));
- FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO()));
+ FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
+ FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
// update solver
ceres_manager_ptr->update();
@@ -592,8 +592,8 @@ TEST(CeresManager, FactorsUpdateLocalParam)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO()));
- FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO()));
+ FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
+ FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
// update solver
ceres_manager_ptr->update();
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index af452608f8a602ad2128ea5c44b81cf832b6004b..7213c1084cf13077a0413cbbea48af0676b330d8 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -38,7 +38,6 @@ CeresManager ceres_mgr(problem_ptr);
FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
// Capture, feature and factor
-// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS", 0, nullptr, pose10, 10, 9, nullptr));
auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr);
////////////////////////////////////////////////////////
@@ -49,9 +48,7 @@ auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pos
TEST(FactorBlockAbs, fac_block_abs_p)
{
- // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>());
- // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP()));
FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP());
ASSERT_TRUE(problem_ptr->check(0));
@@ -69,9 +66,7 @@ TEST(FactorBlockAbs, fac_block_abs_p)
TEST(FactorBlockAbs, fac_block_abs_p_tail2)
{
- // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>()));
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>());
- // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP(),1,2));
FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP(),1,2);
// Unfix frame 0, perturb frm0
@@ -87,9 +82,7 @@ TEST(FactorBlockAbs, fac_block_abs_p_tail2)
TEST(FactorBlockAbs, fac_block_abs_v)
{
- // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>());
- // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getV()));
FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getV());
ASSERT_TRUE(problem_ptr->check(0));
@@ -107,9 +100,7 @@ TEST(FactorBlockAbs, fac_block_abs_v)
TEST(FactorQuatAbs, fac_block_abs_o)
{
- // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3));
- // fea0->addFactor(std::make_shared<FactorQuaternionAbsolute>(fea0->getFrame()->getO()));
FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0->getFrame()->getO());
ASSERT_TRUE(problem_ptr->check(0));
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 108ae75cdb54fa50a3caf5eaa9733a096b893578..1ad1635a2a980b73771171e4b238e3db222f1d19 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -29,19 +29,12 @@ TEST(CaptureAutodiff, ConstructorOdom2d)
SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
// CAPTURE
- // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
- // fr2_ptr->addCapture(capture_ptr);
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity());
- // capture_ptr->addFeature(feature_ptr);
auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity());
// FACTOR
- // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
- // feature_ptr->addFactor(factor_ptr);
- // fr1_ptr->addConstrainedBy(factor_ptr);
auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
ASSERT_TRUE(factor_ptr->getFeature());
@@ -67,23 +60,15 @@ TEST(CaptureAutodiff, ResidualOdom2d)
SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
// CAPTURE
- // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
- // fr2_ptr->addCapture(capture_ptr);
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
Eigen::Vector3s d;
d << -1, -4, M_PI/2;
- // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
- // capture_ptr->addFeature(feature_ptr);
auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity());
// FACTOR
- // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
- // feature_ptr->addFactor(factor_ptr);
- // fr1_ptr->addConstrainedBy(factor_ptr);
-
auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
// EVALUATE
@@ -119,21 +104,14 @@ TEST(CaptureAutodiff, JacobianOdom2d)
SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
// CAPTURE
- // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
- // fr2_ptr->addCapture(capture_ptr);
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
Eigen::Vector3s d;
d << -1, -4, M_PI/2;
- // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
- // capture_ptr->addFeature(feature_ptr);
auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity());
// FACTOR
- // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
- // feature_ptr->addFactor(factor_ptr);
- // fr1_ptr->addConstrainedBy(factor_ptr);
auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
// COMPUTE JACOBIANS
@@ -204,25 +182,15 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
// CAPTURE
- // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
- // fr2_ptr->addCapture(capture_ptr);
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
Eigen::Vector3s d;
d << -1, -4, M_PI/2;
- // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
- // capture_ptr->addFeature(feature_ptr);
auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity());
// FACTOR
- // FactorOdom2DPtr fac_autodiff_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
- // feature_ptr->addFactor(fac_autodiff_ptr);
- // fr1_ptr->addConstrainedBy(fac_autodiff_ptr);
auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
- // FactorOdom2DAnalyticPtr fac_analytic_ptr = std::make_shared<FactorOdom2DAnalytic>(feature_ptr, fr1_ptr);
- // feature_ptr->addFactor(fac_analytic_ptr);
- // fr1_ptr->addConstrainedBy(fac_analytic_ptr);
auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2DAnalytic>(feature_ptr, feature_ptr, fr1_ptr);
// COMPUTE JACOBIANS
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
index 6fb4c644e4abc8045d1c842d533e5a73286cb431..bb5e6092e414b5520ff399b40803856f0f91e6cd 100644
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3D.cpp
@@ -60,7 +60,7 @@ class FactorAutodiffDistance3D_Test : public testing::Test
F2 = problem->emplaceFrame (KEY, pose2, 2.0);
C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0);
f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov);
- c2 = std::static_pointer_cast<FactorAutodiffDistance3D>(FactorBase::emplace<FactorAutodiffDistance3D>(f2, f2, F1, nullptr, false, FAC_ACTIVE));
+ c2 = FactorBase::emplace<FactorAutodiffDistance3D>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
}
diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp
index 7c239323a93e009a7e8f4c71eca650022ee2cd94..d6eb4ddf7ea7b07cdb675b96cb7af81e3adc3c8d 100644
--- a/test/gtest_factor_odom_3D.cpp
+++ b/test/gtest_factor_odom_3D.cpp
@@ -41,13 +41,9 @@ FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), Tim
FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, delta, TimeStamp(1));
// Capture, feature and factor from frm1 to frm0
-// CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 1, nullptr, delta, 7, 6, nullptr));
auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "ODOM 3D", 1, nullptr, delta, 7, 6, nullptr);
-// FeatureBasePtr fea1 = cap1->addFeature(std::make_shared<FeatureBase>("ODOM 3D", delta, data_cov));
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "ODOM 3D", delta, data_cov);
-// FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(fea1->addFactor(std::make_shared<FactorOdom3D>(fea1, frm0, nullptr))); // create and add
-FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(FactorBase::emplace<FactorOdom3D>(fea1, fea1, frm0, nullptr)); // create and add
-// FactorBasePtr dummy = frm0->addConstrainedBy(ctr1);
+FactorOdom3DPtr ctr1 = FactorBase::emplace<FactorOdom3D>(fea1, fea1, frm0, nullptr); // create and add
////////////////////////////////////////////////////////
diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp
index c28a8946ef393b2e41de96d424108dd44691d84a..3c88521c95ab2966b839cd6ab551ef6224b27390 100644
--- a/test/gtest_factor_pose_2D.cpp
+++ b/test/gtest_factor_pose_2D.cpp
@@ -38,7 +38,7 @@ auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "ODOM 2D", 0, nullptr, pos
// FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 2D", pose, data_cov));
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "ODOM 2D", pose, data_cov);
// FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(fea0->addFactor(std::make_shared<FactorPose2D>(fea0)));
-FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(fea0, fea0));
+FactorPose2DPtr ctr0 = FactorBase::emplace<FactorPose2D>(fea0, fea0);
////////////////////////////////////////////////////////
diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp
index eda34b179b9d579cfc535dc3d488c5fef74cbc7b..0b5032a27978d172432b7f68a04fdea28afc64ff 100644
--- a/test/gtest_factor_pose_3D.cpp
+++ b/test/gtest_factor_pose_3D.cpp
@@ -39,12 +39,9 @@ CeresManager ceres_mgr(problem);
FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
// Capture, feature and factor
-// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr));
auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr);
-// FEATUREBASEPTR fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov));
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "ODOM 3D", pose7, data_cov);
-// FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(fea0->addFactor(std::make_shared<FactorPose3D>(fea0)));
-FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(fea0, fea0));
+FactorPose3DPtr ctr0 = FactorBase::emplace<FactorPose3D>(fea0, fea0);
////////////////////////////////////////////////////////
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index f02418eb99ce6d49eeaaee923755314c85c8fcc4..03f991c07c4a144f560a3759c4c081cdd2410ed1 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -68,25 +68,13 @@ TEST(FrameBase, LinksToTree)
IntrinsicsOdom2D intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
- // SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
- // P->getHardware()->addSensor(S);
auto S = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Vector3s::Zero(), intrinsics_odo);
- // FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- // T->addFrame(F1);
auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- // FrameBasePtr F2 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- // T->addFrame(F2);
auto F2 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- // CaptureMotionPtr C = make_shared<CaptureMotion>("MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr);
- // F1->addCapture(C);
auto C = CaptureBase::emplace<CaptureMotion>(F1, "MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr);
/// @todo link sensor & proccessor
ProcessorBasePtr p = std::make_shared<ProcessorOdom2D>(make_shared<ProcessorParamsOdom2D>());
- // FeatureBasePtr f = make_shared<FeatureBase>("f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01);
- // C->addFeature(f);
auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01);
- // FactorOdom2DPtr c = make_shared<FactorOdom2D>(f, F2, p);
- // f->addFactor(c);
auto c = FactorBase::emplace<FactorOdom2D>(f, f, F2, p);
//TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic.
@@ -97,9 +85,6 @@ TEST(FrameBase, LinksToTree)
// tree is inconsistent since we are missing the constrained_by link
// ASSERT_FALSE(P->check(0));
- // establish constrained_by link F2 -> c
- // F2->addConstrainedBy(c);
-
// tree is now consistent
ASSERT_TRUE(P->check(0));
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 9a4204d158505cb8647358567bfb8bbe3eaa2678..5c64c90b5dd35b0efa6bc85cc77d39b383b4940f 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -76,8 +76,6 @@ TEST(Problem, Sensors)
ProblemPtr P = Problem::create("POV", 3);
// add a dummy sensor
- // SensorBasePtr S = std::make_shared<SensorBase>("Dummy", nullptr, nullptr, nullptr, 2, false);
- // P->addSensor(S);
auto S = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
// check pointers
@@ -94,14 +92,10 @@ TEST(Problem, Processor)
ASSERT_FALSE(P->getProcessorMotion());
// add a motion sensor and processor
- // SensorBasePtr Sm = std::make_shared<SensorOdom3D>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D()); // with dummy intrinsics
- // P->addSensor(Sm);
auto Sm = SensorBase::emplace<SensorOdom3D>(P->getHardware(), (Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D());
// add motion processor
- // ProcessorMotionPtr Pm = std::make_shared<ProcessorOdom3D>(std::make_shared<ProcessorParamsOdom3D>());
- // Sm->addProcessor(Pm);
- auto Pm = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom3D>(Sm, std::make_shared<ProcessorParamsOdom3D>()));
+ auto Pm = ProcessorBase::emplace<ProcessorOdom3D>(Sm, std::make_shared<ProcessorParamsOdom3D>());
// check motion processor IS NOT by emplace
ASSERT_EQ(P->getProcessorMotion(), Pm);
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index de653851d9de3aa625421088b4fb076e9b586afb..2616d61040d4f224cfd65020faec182d93e02376 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -518,7 +518,7 @@ TEST(SolverManager, AddFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f));
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
// notification
Notification notif;
@@ -545,7 +545,7 @@ TEST(SolverManager, RemoveFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f));
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
// update solver
solver_manager_ptr->update();
@@ -579,7 +579,7 @@ TEST(SolverManager, AddRemoveFactor)
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f));
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
// notification
Notification notif;
@@ -614,7 +614,7 @@ TEST(SolverManager, DoubleRemoveFactor)
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
- FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f));
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
// update solver
solver_manager_ptr->update();