diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index da6c8cbd654e557a6bec6c7a8e5d5ffd53947c3f..ccee9e6bf80f67e897db1ec84225457b96a9eff2 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1159,10 +1159,10 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
if (_verbose) _stream << std::endl;
if (_verbose) _stream << "Wolf tree integrity ---------------------" << std::endl;
- auto P_raw = this;
+ auto P = shared_from_this();
if (_verbose)
{
- _stream << "Prb @ " << P_raw << std::endl;
+ _stream << "Prb @ " << P.get() << std::endl;
}
// ------------------------
// HARDWARE branch
@@ -1176,9 +1176,9 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check pointer to Problem
std::stringstream inconsistency_explanation;
inconsistency_explanation << "Hardware problem pointer is " << H->getProblem().get()
- << " but problem pointer is " << P_raw << "\n";
- log.addAssertion((H->getProblem().get() == P_raw), inconsistency_explanation);
- //Clear inconsistency_explanation
+ << " but problem pointer is " << P.get() << "\n";
+ log.assertTrue((H->getProblem() == P), inconsistency_explanation);
+
// Sensors =======================================================================================
for (auto S : H->getSensorList())
@@ -1204,14 +1204,14 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check problem and hardware pointers
inconsistency_explanation << "Sensor problem pointer is " << S->getProblem().get()
- << " but problem pointer is " << P_raw << "\n";
- log.addAssertion((S->getProblem().get() == P_raw), inconsistency_explanation);
- //Clear inconsistency_explanation
+ << " but problem pointer is " << P.get() << "\n";
+ log.assertTrue((S->getProblem() == P), inconsistency_explanation);
+
inconsistency_explanation << "Sensor hardware pointer is " << S->getHardware()
<< " but hardware pointer is " << H << "\n";
- log.addAssertion((S->getHardware() == H), inconsistency_explanation);
- //Clear inconsistency_explanation
+ log.assertTrue((S->getHardware() == H), inconsistency_explanation);
+
// Processors =======================================================================================
for (auto p : S->getProcessorList())
@@ -1225,14 +1225,14 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check problem and sensor pointers
inconsistency_explanation << "Processor problem pointer is " << p->getProblem().get()
- << " but problem pointer is " << P_raw << "\n";
- log.addAssertion((p->getProblem().get() == P_raw), inconsistency_explanation);
- //Clear inconsistency_explanation
+ << " but problem pointer is " << P.get() << "\n";
+ log.assertTrue((p->getProblem() == P), inconsistency_explanation);
+
inconsistency_explanation << "Processor sensor pointer is " << p->getSensor()
- << " but sensor pointer is " << P_raw << "\n";
- log.addAssertion((p->getProblem().get() == P_raw), inconsistency_explanation);
- //Clear inconsistency_explanation
+ << " but sensor pointer is " << P.get() << "\n";
+ log.assertTrue((p->getProblem() == P), inconsistency_explanation);
+
}
}
// ------------------------
@@ -1246,9 +1246,9 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check pointer to Problem
inconsistency_explanation << "Trajectory problem pointer is " << T->getProblem().get()
- << " but problem pointer is" << P_raw << "\n";
- log.addAssertion((T->getProblem().get() == P_raw), inconsistency_explanation);
- //Clear inconsistency_explanation
+ << " but problem pointer is" << P.get() << "\n";
+ log.assertTrue((T->getProblem() == P), inconsistency_explanation);
+
// Frames =======================================================================================
for (auto F : T->getFrameList())
@@ -1267,7 +1267,7 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
inconsistency_explanation << "Frame " << F->id() << " @ "<< F.get()
<< " has State block pointer " << sb.get()
<< " = 0 \n";
- log.addAssertion((sb.get() != 0), inconsistency_explanation);
+ log.assertTrue((sb.get() != 0), inconsistency_explanation);
if (_verbose) {
_stream << " "<< pair.first << " sb @ " << sb.get();
@@ -1282,14 +1282,14 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check problem pointer
inconsistency_explanation << "Frame problem pointer is " << F->getProblem().get()
- << " but problem pointer is" << P_raw << "\n";
- log.addAssertion((F->getProblem().get() == P_raw), inconsistency_explanation);
+ << " but problem pointer is" << P.get() << "\n";
+ log.assertTrue((F->getProblem() == P), inconsistency_explanation);
// check trajectory pointer
inconsistency_explanation << "Frame trajectory pointer is " << F->getTrajectory()
<< " but trajectory pointer is" << T << "\n";
- log.addAssertion((F->getTrajectory() == T), inconsistency_explanation);
- //Clear inconsistency_explanation
+ log.assertTrue((F->getTrajectory() == T), inconsistency_explanation);
+
// check constrained_by
for (auto cby : F->getConstrainedByList())
@@ -1304,7 +1304,7 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check constrained_by pointer to this frame
inconsistency_explanation << "constrained-by frame " << F->id() << " @ " << F
<< " not found among constrained-by factors\n";
- log.addAssertion((cby->hasFrameOther(F)), inconsistency_explanation);
+ log.assertTrue((cby->hasFrameOther(F)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector())
{
@@ -1340,7 +1340,7 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check for valid state block
inconsistency_explanation << "Capture " << C->id() << " @ " << C.get() << " has State block pointer "
<< sb.get() << " = 0 \n";
- log.addAssertion((sb.get() != 0), inconsistency_explanation);
+ log.assertTrue((sb.get() != 0), inconsistency_explanation);
if (_verbose)
{
@@ -1356,21 +1356,21 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check problem and frame pointers
inconsistency_explanation << "Capture problem pointer is " << C->getProblem().get()
- << " but problem pointer is" << P_raw << "\n";
- log.addAssertion((C->getProblem().get() == P_raw), inconsistency_explanation);
- //Clear inconsistency_explanation
+ << " but problem pointer is" << P.get() << "\n";
+ log.assertTrue((C->getProblem() == P), inconsistency_explanation);
+
inconsistency_explanation << "Capture frame pointer is " << C->getFrame()
<< " but frame pointer is" << F << "\n";
- log.addAssertion((C->getFrame() == F), inconsistency_explanation);
- //Clear inconsistency_explanation
+ log.assertTrue((C->getFrame() == F), inconsistency_explanation);
+
// check contrained_by
for (const auto& cby : C->getConstrainedByList())
{
if (_verbose)
{
- _stream << " <- c" << cby->id() << " -> ";
+ _stream << " <- c" << cby->id() << " -> ";
for (const auto& Cow : cby->getCaptureOtherList())
_stream << " C" << Cow.lock()->id();
_stream << std::endl;
@@ -1379,7 +1379,7 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check constrained_by pointer to this capture
inconsistency_explanation << "constrained by capture " << C->id() << " @ " << C
<< " not found among constrained-by factors\n";
- log.addAssertion((cby->hasCaptureOther(C)), inconsistency_explanation);
+ log.assertTrue((cby->hasCaptureOther(C)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector())
{
@@ -1411,21 +1411,21 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check problem and capture pointers
inconsistency_explanation << "Feature frame pointer is " << f->getProblem().get()
- << " but problem pointer is" << P_raw << "\n";
- log.addAssertion((f->getProblem().get() == P_raw), inconsistency_explanation);
- //Clear inconsistency_explanation
+ << " but problem pointer is" << P.get() << "\n";
+ log.assertTrue((f->getProblem() == P), inconsistency_explanation);
+
inconsistency_explanation << "Feature capture pointer is " << f->getCapture()
<< " but capture pointer is" << C << "\n";
- log.addAssertion((f->getCapture() == C), inconsistency_explanation);
- //Clear inconsistency_explanation
+ log.assertTrue((f->getCapture() == C), inconsistency_explanation);
+
// check contrained_by
for (auto cby : f->getConstrainedByList())
{
if (_verbose)
{
- _stream << " <- c" << cby->id() << " -> ";
+ _stream << " <- c" << cby->id() << " -> ";
for (const auto& fow : cby->getFeatureOtherList())
_stream << " f" << fow.lock()->id();
_stream << std::endl;
@@ -1434,8 +1434,8 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check constrained_by pointer to this feature
inconsistency_explanation << "constrained by Feature " << f->id() << " @ " << f
<< " not found among constrained-by factors\n";
- log.addAssertion((cby->hasFeatureOther(f)), inconsistency_explanation);
- //Clear inconsistency_explanation
+ log.assertTrue((cby->hasFeatureOther(f)), inconsistency_explanation);
+
}
// Factors =======================================================================================
@@ -1470,8 +1470,8 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
bool found = Fo->isConstrainedBy(c);
inconsistency_explanation << "The constrained Feature " << Fo->id() << " @ " << Fo
<< " not found among constrained-by factors\n";
- log.addAssertion((found), inconsistency_explanation);
- //Clear inconsistency_explanation
+ log.assertTrue((found), inconsistency_explanation);
+
}
}
if (_verbose && !c->getFrameOtherList().empty())
@@ -1495,8 +1495,8 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
bool found = Co->isConstrainedBy(c);
inconsistency_explanation << "The constrained capture " << Co->id() << " @ " << Co
<< " not found among constrained-by factors\n";
- log.addAssertion((found), inconsistency_explanation);
- //Clear inconsistency_explanation
+ log.assertTrue((found), inconsistency_explanation);
+
}
}
if (_verbose && !c->getCaptureOtherList().empty())
@@ -1519,7 +1519,7 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
bool found = fo->isConstrainedBy(c);
inconsistency_explanation << "The constrained feature " << fo->id() << " @ " << fo
<< " not found among constrained-by factors\n";
- log.addAssertion((found), inconsistency_explanation);
+ log.assertTrue((found), inconsistency_explanation);
}
}
if (_verbose && !c->getFeatureOtherList().empty())
@@ -1543,8 +1543,8 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
bool found = Lo->isConstrainedBy(c);
inconsistency_explanation << "The constrained landmark " << Lo->id() << " @ " << Lo
<< " not found among constrained-by factors\n";
- log.addAssertion((found), inconsistency_explanation);
- //Clear inconsistency_explanation
+ log.assertTrue((found), inconsistency_explanation);
+
}
}
if (_verbose && !c->getLandmarkOtherList().empty())
@@ -1561,14 +1561,14 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check problem and feature pointers
inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has problem ptr " << c->getProblem().get()
- << " but problem ptr is " << P_raw << "\n";
- log.addAssertion((c->getProblem().get() == P_raw), inconsistency_explanation);
- //Clear inconsistency_explanation
+ << " but problem ptr is " << P.get() << "\n";
+ log.assertTrue((c->getProblem() == P), inconsistency_explanation);
+
inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has feature ptr " << c->getFeature()
<< " but it should have " << f << "\n";
- log.addAssertion((c->getProblem().get() == P_raw), inconsistency_explanation);
- //Clear inconsistency_explanation
+ log.assertTrue((c->getProblem() == P), inconsistency_explanation);
+
// find state block pointers in all constrained nodes
SensorBasePtr S = c->getFeature()->getCapture()->getSensor(); // get own sensor to check sb
@@ -1697,10 +1697,10 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check that the state block has been found somewhere
inconsistency_explanation << "The stateblock " << sb << " has not been found (is floating!)";
- log.addAssertion((found), inconsistency_explanation);
+ log.assertTrue((found), inconsistency_explanation);
inconsistency_explanation << "The stateblock " << sb << " of factor " << c->id() << " @ " << c << " is null\n";
- log.addAssertion((sb.get() != nullptr), inconsistency_explanation);
+ log.assertTrue((sb.get() != nullptr), inconsistency_explanation);
}
}
}
@@ -1714,8 +1714,8 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
_stream << "Map @ " << M.get() << std::endl;
// check pointer to Problem
- inconsistency_explanation << "The map problem ptr is " << M->getProblem().get() << " but problem is " << P_raw << "\n";
- log.addAssertion((M->getProblem().get() == P_raw), inconsistency_explanation);
+ inconsistency_explanation << "The map problem ptr is " << M->getProblem().get() << " but problem is " << P.get() << "\n";
+ log.assertTrue((M->getProblem() == P), inconsistency_explanation);
// Landmarks =======================================================================================
for (auto L : M->getLandmarkList())
@@ -1742,14 +1742,14 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check problem and map pointers
inconsistency_explanation << "Landmarks' problem ptr is "
<< L->getProblem().get() << " but problem is "
- << P_raw << "\n";
+ << P.get() << "\n";
- log.addAssertion((L->getProblem().get() == P_raw), inconsistency_explanation);
+ log.assertTrue((L->getProblem() == P), inconsistency_explanation);
inconsistency_explanation << "The Landmarks' map ptr is "
<< L->getMap() << " but map is "
<< M << "\n";
- log.addAssertion((M->getProblem().get() == P_raw), inconsistency_explanation);
+ log.assertTrue((M->getProblem() == P), inconsistency_explanation);
for (auto cby : L->getConstrainedByList())
{
@@ -1771,7 +1771,7 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// check constrained-by factors
inconsistency_explanation << "constrained-by landmark " << L->id() << " @ " << L
<< " not found among constrained-by factors\n";
- log.addAssertion((cby->hasLandmarkOther(L)), inconsistency_explanation);
+ log.assertTrue((cby->hasLandmarkOther(L)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector()) {
if (_verbose) {
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index e0ba9a42a17936b62ec329c1fb022052d5ae6c30..d25f1451492f90d90613b5a08882ee02ae9c3f4d 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -19,8 +19,12 @@
#include "core/sensor/sensor_diff_drive.h"
#include "core/processor/processor_diff_drive.h"
#include "core/capture/capture_diff_drive.h"
+//#include "core/feature/feature_diff_drive.h"
+#include "core/factor/factor_diff_drive.h"
+
#include "core/state_block/state_quaternion.h"
+
#include <iostream>
using namespace wolf;
@@ -413,6 +417,70 @@ TEST(Problem, perturb)
}
}
+TEST(Problem, check)
+{
+ auto problem = Problem::create("PO", 2);
+
+ // make a sensor first
+ auto intr = std::make_shared<ParamsSensorDiffDrive>();
+ intr->radius_left = 1.0;
+ intr->radius_right = 1.0;
+ intr->wheel_separation = 1.0;
+ intr->ticks_per_wheel_revolution = 100;
+ Vector3d extr(0,0,0);
+ auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
+ sensor->setStateBlockDynamic("I", true);
+
+ Vector3d pose; pose << 0,0,0;
+
+ int i = 0;
+ for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
+ {
+ auto F = problem->emplaceFrame(KEY, pose, t);
+ if (i==0) F->fix();
+
+ for (int j = 0; j< 2 ; j++)
+ {
+ auto sb = std::make_shared<StateBlock>(Vector3d(1,1,1));
+ auto cap = CaptureBase::emplace<CaptureDiffDrive>(F, t, sensor, Vector2d(1,2), Matrix2d::Identity(), nullptr, nullptr, nullptr, sb);
+
+ for (int k = 0; k<2; k++)
+ {
+ Vector3d d(1,2,3), c(1,1,1);
+ Matrix3d D = Matrix3d::Identity(), J=Matrix3d::Zero();
+ auto fea = FeatureBase::emplace<FeatureMotion>(cap, "FeatureDiffDrive", d, D, c, J);
+ auto fac = FactorBase::emplace<FactorDiffDrive>(fea, fea, cap, nullptr, false);
+ }
+ }
+ i++;
+ }
+
+ for (int l = 0; l < 2; l++)
+ {
+ auto sb1 = std::make_shared<StateBlock>(Vector2d(1,2));
+ auto sb2 = std::make_shared<StateBlock>(Vector1d(3));
+ auto L = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", sb1, sb2);
+ if (l==0) L->fix();
+ }
+
+ ASSERT_TRUE(problem->check(true, std::cout));
+
+ // remove stuff from problem, and re-check
+
+ auto F = problem->getLastKeyFrame();
+
+ auto cby = F->getConstrainedByList().front();
+
+ cby->remove(true);
+
+ ASSERT_TRUE(problem->check(true, std::cout));
+
+ F->remove();
+
+ ASSERT_TRUE(problem->check(true, std::cout));
+}
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);