diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index df397f679192a6910cdbb9ff0f000db7d2c40500..5e1250ff066611a40af092e937324e840e58ae0e 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -301,15 +301,6 @@ inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _structure)
inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure) const
{
-//<<<<<<< HEAD
-// StateStructure structure = (_structure == "" ? structure_ : _structure);
-//
-// VectorComposite state;
-// for (const auto& ckey : structure)
-// {
-// const auto& key = string(1,ckey); // ckey is char
-// state.emplace(key, state_block_map_.at(key)->getState());
-//=======
const StateStructure& structure = (_structure == "" ? structure_ : _structure);
VectorComposite state;
@@ -321,7 +312,6 @@ inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure
if (state_it != state_block_map_.end())
state.emplace(key, state_it->second->getState());
-//>>>>>>> devel
}
return state;
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index fec8d0d7b4c0d59c9eb741997b931f91e1f71aa5..e50283b2643e7310e92b58befa22d7e0ded36a31 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -330,9 +330,6 @@ FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp, //
const SizeEigen _dim, //
const Eigen::VectorXd& _frame_state)
{
-//<<<<<<< HEAD
-// auto frm = FrameBase::emplaceKeyFrame<FrameBase>(trajectory_ptr_,
-//=======
VectorComposite state;
SizeEigen index = 0;
SizeEigen size = 0;
@@ -356,11 +353,9 @@ FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp, //
assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large.");
auto frm = FrameBase::emplaceKeyFrame<FrameBase>(trajectory_ptr_,
-// _frame_key_type,
-//>>>>>>> devel
- _time_stamp,
- _frame_structure,
- state);
+ _time_stamp,
+ _frame_structure,
+ state);
return frm;
}
@@ -410,17 +405,6 @@ FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp)
TimeStamp Problem::getTimeStamp ( ) const
{
-//<<<<<<< HEAD
-// if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
-// {
-// auto last_kf = trajectory_ptr_->getLastKeyFrame();
-//
-// assert(last_kf != nullptr && "Problem has no Keyframe so no timestamp can be obtained!");
-//
-// return last_kf->getTimeStamp();
-// }
-// else
-//=======
TimeStamp ts = TimeStamp::Invalid();
for (const auto& prc : processor_is_motion_list_)
@@ -430,7 +414,6 @@ TimeStamp Problem::getTimeStamp ( ) const
if (not ts.ok())
-//>>>>>>> devel
{
const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyFrame();
@@ -451,20 +434,8 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
VectorComposite state;
-//<<<<<<< HEAD
-// if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
-// {
-// auto last_kf = trajectory_ptr_->getLastKeyFrame();
-// if (last_kf)
-// state = last_kf->getState(structure);
-// else
-// state = stateZero(structure);
-// }
-// else // Compose from different processor motion
-//=======
// compose the states of all processor motions into one only state
for (const auto& prc : processor_is_motion_list_)
-//>>>>>>> devel
{
const auto& prc_state = prc->getState(structure);
for (const auto& pair_key_vec : prc_state)
@@ -507,22 +478,6 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
{
StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
-//<<<<<<< HEAD
-// if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
-// {
-// const auto& last_kf = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
-// if (last_kf)
-// {
-// return last_kf->getState(structure);
-// }
-// else
-// {
-// return stateZero(structure);
-// }
-// }
-//
-//=======
-//>>>>>>> devel
VectorComposite state;
for (const auto& prc : processor_is_motion_list_)
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 15279127f85d620c0dd44381314bb8acd8265476..c595c2bbb26b049a39f0dd6b53685a7609db581d 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -656,16 +656,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- LAST ----------
// Make non-key-frame for last Capture
-//<<<<<<< HEAD
last_frame_ptr_ = FrameBase::createNonKeyFrame<FrameBase>(origin_ts,
getStateStructure(),
_origin_frame->getState());
-//=======
-// auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
-// origin_ts,
-// getProblem()->getFrameStructure(),
-// _origin_frame->getState());
-//>>>>>>> devel
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(last_frame_ptr_,
@@ -801,20 +794,17 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
{
-//<<<<<<< HEAD
+ assert(_ts.ok());
+
// First check if last_ptr is the one we are looking for
if (last_ptr_->containsTimeStamp(_ts, this->params_->time_tolerance))
-// if (fabs(last_ptr_->getTimeStamp() - _ts) <= this->params_->time_tolerance)
return last_ptr_;
// Then look in the Wolf tree...
// -----------------------------
//
-//=======
- assert(_ts.ok());
-//>>>>>>> devel
// We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
// Note: since the buffer goes from a KF in the past until the next KF, we need to:
// 1. See that the KF contains a CaptureMotion
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index a1f158ad0916c6a16ace4f1a15f3cc8e3eb6a8e3..91867f39528b54cc7335fcca91e7fcc108757576 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -81,17 +81,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_PACK" );
-//<<<<<<< HEAD
FrameBasePtr kfrm = FrameBase::emplaceKeyFrame<FrameBase>(getProblem()->getTrajectory(),
incoming_ptr_->getTimeStamp(),
getStateStructure(),
getProblem()->getState(getStateStructure()));
-//=======
-// FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY,
-// incoming_ptr_->getTimeStamp());//,
-// //getStateStructure(),
-// //getProblem()->getState(getStateStructure()));
-//>>>>>>> devel
incoming_ptr_->link(kfrm);
// Process info
@@ -210,16 +203,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
resetDerived();
// make F; append incoming to new F
-//<<<<<<< HEAD
FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
-// getProblem()->getDim(),
last_ptr_->getFrame()->getState());
-//=======
-// FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
-// incoming_ptr_->getTimeStamp(),
-// last_ptr_->getFrame()->getState());
-//>>>>>>> devel
incoming_ptr_->link(frm);
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
@@ -266,16 +252,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
advanceDerived();
// Replace last frame for a new one in incoming
-//<<<<<<< HEAD
FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
-// getProblem()->getDim(),
last_ptr_->getFrame()->getState());
-//=======
-// FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
-// incoming_ptr_->getTimeStamp(),
-// last_ptr_->getFrame()->getState());
-//>>>>>>> devel
incoming_ptr_->link(frm);
last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove();
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index ab797470d4315eba2b7f94782e880356251146f3..0a914d715ee66edfdcb51eed377cb10cca209b88 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -159,29 +159,14 @@ class FactorDiffDriveTest : public testing::Test
processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
// frames
-//<<<<<<< HEAD
-// F0 = FrameBase::emplaceKeyFrame<FrameBase>(trajectory,
-// 0.0,
-// "PO",
-// 2,
-// Vector3d(0,0,0));
-// F1 = FrameBase::emplaceKeyFrame<FrameBase>(trajectory,
-// 1.0,
-// "PO",
-// 2,
-// Vector3d(1,0,0));
-//=======
F0 = FrameBase::emplaceKeyFrame<FrameBase>(trajectory,
-// KEY,
0.0,
"PO",
std::list<VectorXd>{Vector2d(0,0), Vector1d(0)});
F1 = FrameBase::emplaceKeyFrame<FrameBase>(trajectory,
-// KEY,
1.0,
"PO",
std::list<VectorXd>{Vector2d(1,0), Vector1d(0)});
-//>>>>>>> devel
// captures
C0 = CaptureBase::emplace<CaptureDiffDrive>(F0,
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index fbb6addbe717d2eee4ab00853ededff0ce857f83..646dd602e619b0c2d2fadd70eb6c6f94e7f79a5f 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -130,15 +130,8 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
-//<<<<<<< HEAD
-// // F0->link(problem->getTrajectory());
-// F0->addStateBlock("V", sbv0);
-// F0->setKey(problem);
-//=======
-// F0->link(problem->getTrajectory());
F0->addStateBlock('V', sbv0);
F0->setKey(problem);
-//>>>>>>> devel
ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
ASSERT_EQ(n, ADD);
@@ -219,9 +212,6 @@ TEST_F(HasStateBlocksTest, getState_structure)
ASSERT_FALSE(state0.count('V'));
ASSERT_FALSE(state0.count('W'));
-
-
-
}
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 3a36425cc91766164b3554d3dcf2f1e436a81935..743b0fcc56a4088ceb87073a6941200fd74ccd33 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -50,11 +50,6 @@ class ProcessorMotion_test : public testing::Test{
Vector2d data;
Matrix2d data_cov;
-// ProcessorMotion_test() :
-// ProcessorOdom2d(std::make_shared<ProcessorParamsOdom2d>()),
-// dt(0)
-// { }
-
void SetUp() override
{
std::string wolf_root = _WOLF_ROOT_DIR;
@@ -146,8 +141,7 @@ TEST_F(ProcessorMotion_test, getState_time_structure)
capture->setTimeStamp(t);
capture->setData(data);
capture->setDataCovariance(data_cov);
-// capture->process();
- processor->captureCallback(capture);
+ capture->process();
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}