diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index e20da100d5f3dbd7f78df097eaa2634bae51b3e7..402c590326567b5f4c0c564404c09b331f8ba674 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -134,14 +134,14 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
{
public:
typedef enum {
- FIRST_TIME_WITHOUT_PACK, // NOT POSSIBLE
- FIRST_TIME_WITH_PACK_BEFORE_INCOMING,
- FIRST_TIME_WITH_PACK_ON_INCOMING,
- FIRST_TIME_WITH_PACK_AFTER_INCOMING, // NOT POSSIBLE
- RUNNING_WITHOUT_PACK,
- RUNNING_WITH_PACK_BEFORE_ORIGIN,
- RUNNING_WITH_PACK_ON_ORIGIN,
- RUNNING_WITH_PACK_AFTER_ORIGIN
+ FIRST_TIME_WITHOUT_KF,
+ FIRST_TIME_WITH_KF_BEFORE_INCOMING,
+ FIRST_TIME_WITH_KF_ON_INCOMING,
+ FIRST_TIME_WITH_KF_AFTER_INCOMING,
+ RUNNING_WITHOUT_KF,
+ RUNNING_WITH_KF_BEFORE_ORIGIN,
+ RUNNING_WITH_KF_ON_ORIGIN,
+ RUNNING_WITH_KF_AFTER_ORIGIN
} ProcessingStep ;
protected:
@@ -204,6 +204,11 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*/
CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
+ MotionBuffer& getBuffer();
+ const MotionBuffer& getBuffer() const;
+
+ // Helper functions:
+ protected:
/** Set the origin of all motion for this processor
* \param _origin_frame the keyframe to be the origin
*/
@@ -215,13 +220,6 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*/
FrameBasePtr setOrigin(const Eigen::VectorXd& _x_origin, const TimeStamp& _ts_origin);
-
- MotionBuffer& getBuffer();
- const MotionBuffer& getBuffer() const;
-
- // Helper functions:
- protected:
-
/** \brief process an incoming capture
*
* Each derived processor should implement this function. It will be called if:
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 189e0f99bc51a765893c8c52e55ec2ac0971ccf0..263f205c1724fef3569116ce31b07e63214b166e 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -493,6 +493,7 @@ Eigen::VectorXd Problem::zeroState() const
// Set the quaternion identity for 3d states. Set only the real part to 1:
if(this->getDim() == 3)
state(6) = 1.0;
+
return state;
}
@@ -908,7 +909,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
FrameBasePtr prior_keyframe(nullptr);
- if (prior_options_->mode != "nothing")
+ if (prior_options_->mode != "nothing" and prior_options_->mode != "")
{
prior_keyframe = emplaceFrame(this->getFrameStructure(),
this->getDim(),
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index b3389b3bfdbc2574bf57c25509b4228eca1feb63..b9c5182f9bbe1abd2f00ed4340b603f8233931bc 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -17,7 +17,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
ProcessorParamsMotionPtr _params_motion) :
ProcessorBase(_type, _dim, _params_motion),
params_motion_(_params_motion),
- processing_step_(RUNNING_WITHOUT_PACK),
+ processing_step_(RUNNING_WITHOUT_KF),
x_size_(_state_size),
data_size_(_data_size),
delta_size_(_delta_size),
@@ -93,23 +93,35 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
switch(processing_step_)
{
- case FIRST_TIME_WITHOUT_PACK :
- // TODO: create keyframe with zero state? asking problem emplaceFrame?
+ case FIRST_TIME_WITHOUT_KF :
+ {
+ // There is no KF: create own origin (before this capture: 2 x time_tolerance before)
+ setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp()-2*params_->time_tolerance);
break;
- case FIRST_TIME_WITH_PACK_BEFORE_INCOMING :
- // TODO: create keyframe asking problem emplaceFrame and setOrigin()
+ }
+ case FIRST_TIME_WITH_KF_BEFORE_INCOMING :
+ {
+ // cannot joint to the KF: create own origin (before this capture: 2 x time_tolerance before)
+ setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp()-2*params_->time_tolerance);
break;
- case FIRST_TIME_WITH_PACK_ON_INCOMING :
- // TODO: joint to pack and setOrigin()
+ }
+ case FIRST_TIME_WITH_KF_ON_INCOMING :
+ {
+ // can joint to the KF
+ setOrigin(pack->key_frame);
break;
- case FIRST_TIME_WITH_PACK_AFTER_INCOMING :
- // TODO: create keyframe asking problem emplaceFrame and setOrigin()
+ }
+ case FIRST_TIME_WITH_KF_AFTER_INCOMING :
+ {
+ // cannot joint to the KF: create own origin (before this capture: 2 x time_tolerance before)
+ setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp()-2*params_->time_tolerance);
break;
- case RUNNING_WITHOUT_PACK :
- case RUNNING_WITH_PACK_ON_ORIGIN :
+ }
+ case RUNNING_WITHOUT_KF :
+ case RUNNING_WITH_KF_ON_ORIGIN :
break;
- case RUNNING_WITH_PACK_BEFORE_ORIGIN :
+ case RUNNING_WITH_KF_BEFORE_ORIGIN :
{
/*
@@ -196,7 +208,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
break;
}
- case RUNNING_WITH_PACK_AFTER_ORIGIN :
+ case RUNNING_WITH_KF_AFTER_ORIGIN :
{
/*
* Legend:
@@ -277,7 +289,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// NOW on with the received data
// integrate data
+ WOLF_INFO("before integrating, getCurrentState = ", getCurrentState().transpose());
integrateOneStep();
+ WOLF_INFO("after integrating, getCurrentState = ", getCurrentState().transpose());
// Update state and time stamps
last_ptr_->setTimeStamp(getCurrentTimeStamp());
@@ -330,8 +344,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// create a new frame
auto frame_new = getProblem()->emplaceFrame(NON_ESTIMATED,
- getProblem()->getCurrentState(),
- getCurrentTimeStamp());
+ getProblem()->getCurrentState(),
+ getCurrentTimeStamp());
// create a new capture
auto capture_new = emplaceCapture(frame_new,
getSensor(),
@@ -433,7 +447,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// Make non-key-frame for last Capture
TimeStamp origin_ts = _origin_frame->getTimeStamp();
auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
- _origin_frame->getState(),
+ _origin_frame->getState(),
origin_ts);
// emplace (emtpy) last Capture
@@ -617,23 +631,25 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
if (pack)
{
if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, incoming_ptr_->getTimeStamp(), params_motion_->time_tolerance))
- processing_step_ = FIRST_TIME_WITH_PACK_ON_INCOMING;
+ processing_step_ = FIRST_TIME_WITH_KF_ON_INCOMING;
else if (pack->key_frame->getTimeStamp() < incoming_ptr_->getTimeStamp())
{
WOLF_INFO("First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.")
- processing_step_ = FIRST_TIME_WITH_PACK_BEFORE_INCOMING;
+ processing_step_ = FIRST_TIME_WITH_KF_BEFORE_INCOMING;
}
else
{
WOLF_INFO("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
- processing_step_ = FIRST_TIME_WITH_PACK_AFTER_INCOMING;
+ processing_step_ = FIRST_TIME_WITH_KF_AFTER_INCOMING;
}
}
else
{
WOLF_INFO("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
- processing_step_ = FIRST_TIME_WITHOUT_PACK;
+ processing_step_ = FIRST_TIME_WITHOUT_KF;
}
+
+ return pack;
}
else
{
@@ -651,20 +667,23 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
WOLF_INFO(" ... It seems you missed something!");
WOLF_ERROR("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
// throw std::runtime_error("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
- processing_step_ = RUNNING_WITH_PACK_ON_ORIGIN;
+ processing_step_ = RUNNING_WITH_KF_ON_ORIGIN;
}
else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp())
- processing_step_ = RUNNING_WITH_PACK_BEFORE_ORIGIN;
+ processing_step_ = RUNNING_WITH_KF_BEFORE_ORIGIN;
else
- processing_step_ = RUNNING_WITH_PACK_AFTER_ORIGIN;
+ processing_step_ = RUNNING_WITH_KF_AFTER_ORIGIN;
}
else
- processing_step_ = RUNNING_WITHOUT_PACK;
+ processing_step_ = RUNNING_WITHOUT_KF;
+
+ return pack;
}
- return pack;
+ // not reached
+ return nullptr;
}
void ProcessorMotion::setProblem(ProblemPtr _problem)
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 2f6ebb53e7f315ebd82db9f172f8b43d00d8470a..0c88c19c7a719754f82abb964a2f8818aaeacb35 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -449,7 +449,8 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
Vector3d x2(3.0, -3.0, 0.0);
Matrix3d P0; P0.setIdentity();
- auto F0 = problem->setPriorFactor(x0, P0, t, 0.1);
+ //auto F0 = problem->setPriorFactor(x0, P0, t+dt, 0.1);
+ problem->setPriorOptions("factor", 0.1, x0, P0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
@@ -459,7 +460,10 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
- auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
+ // initial dummy capture process for seting prior at t=0
+ std::make_shared<CaptureDiffDrive>(t, sensor, Eigen::Vector2d::Zero(), Eigen::Matrix2d::Identity()*0.01, nullptr)->process();
+
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
for (int n = 0; n < N; n++)
{
@@ -480,15 +484,15 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
}
auto F2 = problem->getLastKeyFrame();
-
+ auto F0 = problem->getTrajectory()->getFrameList().front();
// Fix boundaries
F0->fix();
F2->fix();
// Perturb S
- Vector3d calib_pert = calib_gt + Vector3d::Random()*0.2;
- sensor->getIntrinsic()->setState(calib_pert);
+ sensor->getIntrinsic()->perturb(0.2);
+ Vector3d calib_pert = sensor->getIntrinsic()->getState();
WOLF_TRACE("\n ========== SOLVE =========");
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -573,7 +577,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
-
TimeStamp t(0.0);
double dt = 1.0;
Vector3d x0(0,0,0);
@@ -581,7 +584,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
Vector3d x2(3.0, -3.0, 0.0);
Matrix3d P0; P0.setIdentity();
- auto F0 = problem->setPriorFactor(x0, P0, t, 0.1);
+ problem->setPriorOptions("factor", 0.1, x0, P0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
@@ -591,7 +594,10 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
- auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
+ // initial dummy capture process for seting prior at t=0
+ std::make_shared<CaptureDiffDrive>(t, sensor, Eigen::Vector2d::Zero(), Eigen::Matrix2d::Identity()*0.01, nullptr)->process();
+
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
for (int n = 0; n < N; n++)
{
@@ -613,6 +619,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
C->process();
}
+ auto F0 = problem->getTrajectory()->getFrameList().front();
auto F2 = problem->getLastKeyFrame();
F2->setState(x2); // Impose known final state regardless of integrated value.
@@ -621,7 +628,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
F0->fix();
F2->fix();
-
WOLF_TRACE("\n ========== SOLVE =========");
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE("\n", report);
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 0929163a71e7ea0e1a2d31ca1958b39e7a2cb902..a17ea2d4dc50b36dcab596fcb5a970f56e4b5c1d 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -200,9 +200,10 @@ TEST(Odom2d, VoteForKfAndSolve)
ProcessorParamsOdom2dPtr params(std::make_shared<ProcessorParamsOdom2d>());
params->voting_active = true;
params->dist_traveled = 100;
- params->angle_turned = 6.28;
- params->max_time_span = 2.5*dt; // force KF at every third process()
+ params->angle_turned = data(1)*2.5; // force KF at every third process()
+ params->max_time_span = 1e4; // since prior is automatically set at first capture, first KF is not in t=0
params->cov_det = 100;
+ params->time_tolerance = dt/2;
params->unmeasured_perturbation_std = 0.00;
Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, params);
@@ -211,13 +212,13 @@ TEST(Odom2d, VoteForKfAndSolve)
// NOTE: We integrate and create KFs as follows:
// i= 0 1 2 3 4 5 6
- // KF -- * -- * -- KF - * -- * -- KF - *
+ // KF - * -- * -- KF - * -- * -- KF - *
// Ceres wrapper
CeresManager ceres_manager(problem);
// Origin Key Frame
- FrameBasePtr origin_frame = problem->setPriorFactor(x0, P0, t0, dt/2);
+ problem->setPriorOptions("factor", dt/2, x0, P0);
ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
@@ -246,7 +247,7 @@ TEST(Odom2d, VoteForKfAndSolve)
for (int n=1; n<=N; n++)
{
- // std::cout << "-------------------\nStep " << i << " at time " << t << std::endl;
+ std::cout << "-------------------\nStep " << n << " at time " << t << std::endl;
// re-use capture with updated timestamp
capture->setTimeStamp(t);
@@ -269,9 +270,9 @@ TEST(Odom2d, VoteForKfAndSolve)
integrated_delta_cov = Jx * integrated_delta_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
}
- WOLF_DEBUG("n: ", n, " t:", t);
- WOLF_DEBUG("wolf delta: ", processor_odom2d->getMotion().delta_integr_.transpose());
- WOLF_DEBUG("test delta: ", integrated_delta .transpose());
+ WOLF_INFO("n: ", n, " t:", t);
+ WOLF_INFO("wolf delta: ", processor_odom2d->getMotion().delta_integr_.transpose());
+ WOLF_INFO("test delta: ", integrated_delta .transpose());
ASSERT_POSE2d_APPROX(processor_odom2d->getMotion().delta_integr_, integrated_delta, 1e-6);
ASSERT_MATRIX_APPROX(odom2d_delta_cov, integrated_delta_cov, 1e-6);
@@ -352,7 +353,9 @@ TEST(Odom2d, KF_callback)
CeresManager ceres_manager(problem);
// Origin Key Frame
- FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0, dt/2);
+ //FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0, dt/2);
+ problem->setPriorOptions("factor", dt/2, x0, x0_cov);
+
// Check covariance values
Eigen::Vector3d integrated_pose = x0;
@@ -465,6 +468,7 @@ TEST(Odom2d, KF_callback)
// solve
// cout << "===== full ====" << endl;
+ FrameBasePtr keyframe_0 = problem->getTrajectory()->getFrameList().front();
keyframe_0->setState(Vector3d(1,2,3));
keyframe_1->setState(Vector3d(2,3,1));
keyframe_2->setState(Vector3d(3,1,2));
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index f9800e3f3ee2495ad445de725d284d4ef943ceed..794d2aebaabdbe02214f2dde17bdf8118437985c 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -139,10 +139,12 @@ TEST(Problem, SetOrigin_PO_2d)
ProblemPtr P = Problem::create("PO", 2);
TimeStamp t0(0);
Eigen::VectorXd x0(3); x0 << 1,2,3;
- Eigen::MatrixXd P0(3,3); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
+ Eigen::MatrixXd P0(Eigen::MatrixXd::Identity(3,3) * 0.1); // P0 is 0.1*Id
P->setPriorFactor(x0, P0, t0, 1.0);
+ P->print(4,1,1,1);
+
// check that no sensor has been added
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
@@ -153,7 +155,8 @@ TEST(Problem, SetOrigin_PO_2d)
TrajectoryBasePtr T = P->getTrajectory();
FrameBasePtr F = P->getLastFrame();
CaptureBasePtr C = F->getCaptureList().front();
- FeatureBasePtr f = C->getFeatureList().front();
+ FeatureBasePtr fp = C->getFeatureList().front();
+ FeatureBasePtr fo = C->getFeatureList().back();
FactorBasePtrList fac_list;
F->getFactorList(fac_list);
@@ -163,7 +166,7 @@ TEST(Problem, SetOrigin_PO_2d)
ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
// check that we have two features (prior: one per state block)
ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 2);
- // check that we have one frame, one capture, one feature, one factor
+ // check that we have two factors (prior: one per state block)
ASSERT_EQ(fac_list.size(), (SizeStd) 2);
// check that the factors are absolute (no pointers to other F, f, or L)
@@ -176,8 +179,10 @@ TEST(Problem, SetOrigin_PO_2d)
}
// check that the Feature measurement and covariance are the ones provided
- ASSERT_MATRIX_APPROX(x0, f->getMeasurement(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0, f->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0.head<2>(), fp->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0.tail<1>(), fo->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0.topLeftCorner(2,2), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0.bottomRightCorner(1,1), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
@@ -186,7 +191,7 @@ TEST(Problem, SetOrigin_PO_3d)
{
ProblemPtr P = Problem::create("PO", 3);
TimeStamp t0(0);
- Eigen::VectorXd x0(7); x0 << 1,2,3,4,5,6,7;
+ Eigen::VectorXd x0(7); x0 << 1,2,3,4,5,6,7; // not normalized quaternion, this is not what's tested
Eigen::MatrixXd P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
P->setPriorFactor(x0, P0, t0, 1.0);
@@ -199,22 +204,36 @@ TEST(Problem, SetOrigin_PO_3d)
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
- ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
FrameBasePtr F = P->getLastFrame();
- ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
CaptureBasePtr C = F->getCaptureList().front();
- ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 1);
- FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getFactorList().size(), (SizeStd) 1);
+ FeatureBasePtr fp = C->getFeatureList().front();
+ FeatureBasePtr fo = C->getFeatureList().back();
+ FactorBasePtrList fac_list;
+ F->getFactorList(fac_list);
+
+ // check that we have one frame (prior)
+ ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
+ // check that we have one capture (prior)
+ ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
+ // check that we have two features (prior: one per state block)
+ ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 2);
+ // check that we have two factors (prior: one per state block)
+ ASSERT_EQ(fac_list.size(), (SizeStd) 2);
- // check that the factor is absolute (no pointers to other F, f, or L)
- FactorBasePtr c = f->getFactorList().front();
- ASSERT_FALSE(c->getFrameOther());
- ASSERT_FALSE(c->getLandmarkOther());
+ // check that the factors are absolute (no pointers to other F, f, or L)
+ for (auto fac : fac_list)
+ {
+ ASSERT_FALSE(fac->getFrameOther());
+ ASSERT_FALSE(fac->getLandmarkOther());
+ ASSERT_FALSE(fac->getCaptureOther());
+ ASSERT_FALSE(fac->getFeatureOther());
+ }
// check that the Feature measurement and covariance are the ones provided
- ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- ASSERT_TRUE((P0 - f->getMeasurementCovariance()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_MATRIX_APPROX(x0.head<3>(), fp->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0.tail<4>(), fo->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0.topLeftCorner(3,3), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0.bottomRightCorner(3,3), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index d68ace3de5e37428d21d829ef3632008983a436f..bcabf20437590cda0472777cd0e0b4e21bb381a3 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -320,14 +320,13 @@ TEST_F(ProcessorDiffDriveTest, process)
auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
- WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
for (int n = 1; n <= N; n++)
{
+ C->setTimeStamp(t);
C->process();
WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
t += dt;
- C->setTimeStamp(t);
}
problem->print(4,1,1,1);
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 6170c83c7efbc2f6bbe91b71b289874294ddce40..4e449f781ba1c8091806fa7634d8b3c87bd4ff1e 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -72,18 +72,43 @@ class ProcessorMotion_test : public testing::Test{
params->unmeasured_perturbation_std = 0.001;
processor = ProcessorBase::emplace<ProcessorOdom2dPublic>(sensor, params);
capture = std::make_shared<CaptureMotion>("CaptureOdom2d", 0.0, sensor, data, data_cov, 3, 3, nullptr);
-
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
- problem->setPriorFactor(x0, P0, 0.0, 0.01);
}
virtual void TearDown(){}
};
-TEST_F(ProcessorMotion_test, IntegrateStraight)
+TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ problem->setPriorOptions("factor", 0.01, x0, P0);
+
+ data << 1, 0; // advance straight
+ data_cov.setIdentity();
+ TimeStamp t(0.0);
+
+ for (int i = 0; i<9; i++)
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<9,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
+{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ problem->setPriorOptions("fix", 0.01, x0);
+
data << 1, 0; // advance straight
data_cov.setIdentity();
TimeStamp t(0.0);
@@ -101,8 +126,37 @@ TEST_F(ProcessorMotion_test, IntegrateStraight)
ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<9,0,0).finished(), 1e-8);
}
-TEST_F(ProcessorMotion_test, IntegrateCircle)
+TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ problem->setPriorOptions("factor", 0.01, x0, P0);
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
+ TimeStamp t(0.0);
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<0,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
+{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ problem->setPriorOptions("fix", 0.01, x0);
+
data << 1, 2*M_PI/10; // advance in circle
data_cov.setIdentity();
TimeStamp t(0.0);
@@ -120,8 +174,57 @@ TEST_F(ProcessorMotion_test, IntegrateCircle)
ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<0,0,0).finished(), 1e-8);
}
-TEST_F(ProcessorMotion_test, splitBuffer)
+TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ problem->setPriorOptions("factor", 0.01, x0, P0);
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
+ TimeStamp t(0.0);
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ }
+
+ SensorBasePtr S = processor->getSensor();
+
+ TimeStamp t_target = 8.5;
+ FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target);
+ CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
+ CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+ "ODOM 2d",
+ t_target,
+ sensor,
+ data,
+ 3,
+ 3,
+ nullptr);
+
+ processor->splitBuffer(C_source,
+ t_target,
+ F_target,
+ C_target);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
+}
+
+TEST_F(ProcessorMotion_test, splitBufferFixPrior)
+{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+ problem->setPriorOptions("fix", 0.01, x0);
+
data << 1, 2*M_PI/10; // advance in circle
data_cov.setIdentity();
TimeStamp t(0.0);
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index c8bf458bbcdc5058862accd76beedcb13b1d011f..e42943aeca2ef0f7359c226f20a16200da6cfdc6 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -283,7 +283,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
{
//1ST TIME -> KF (origin)
- WOLF_DEBUG("First time...");
+ WOLF_INFO("First time...");
CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor);
cap1->process();
@@ -292,7 +292,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
//2ND TIME
- WOLF_DEBUG("Second time...");
+ WOLF_INFO("Second time...");
CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(1, sensor);
cap2->process();
@@ -301,7 +301,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
//3RD TIME
- WOLF_DEBUG("Third time...");
+ WOLF_INFO("Third time...");
CaptureBasePtr cap3 = std::make_shared<CaptureVoid>(2, sensor);
cap3->process();
@@ -309,7 +309,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
//4TH TIME
- WOLF_DEBUG("Forth time...");
+ WOLF_INFO("Forth time...");
CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(3, sensor);
cap4->process();
@@ -317,7 +317,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
//5TH TIME -> KF in cap4 (tracked features < 7 (params->min_features_for_keyframe))
- WOLF_DEBUG("Fifth time...");
+ WOLF_INFO("Fifth time...");
CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
cap5->process();
@@ -326,7 +326,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
// check factors
- WOLF_DEBUG("checking factors...");
+ WOLF_INFO("checking factors...");
TrackMatrix track_matrix = processor->getTrackMatrix();
ASSERT_EQ(cap4->getFeatureList().size(), params->min_features_for_keyframe + params->max_new_features);
ASSERT_TRUE(problem->check(0));