Skip to content
Snippets Groups Projects
Commit 7a5e03a3 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

gtests added and working

parent b2ccb263
No related branches found
No related tags found
1 merge request!417Resolve "Graph search"
Pipeline #6642 passed
Hide whitespace changes
Inline Side-by-side
src/utils/graph_search.cpp
+ 32
− 11
View file @ 7a5e03a3
......@@ -17,25 +17,40 @@ FactorBasePtrList GraphSearch::computeShortestPath(FrameBasePtr frm1,
FrameBasePtr frm2,
const unsigned int max_graph_dist)
{
//WOLF_INFO("GraphSearch::computeShortestPath: from frame ", frm1->id(), " to frame ", frm2->id());
std::set<FrameBasePtr> frm_neigs({frm1});
parents_[frm1] = std::pair<FactorBasePtr,FrameBasePtr>(nullptr, nullptr);
unsigned int depth = 0;
//WOLF_INFO(frm1->id());
while (not frm_neigs.empty())
{
frm_neigs = getNeighborFrames(frm_neigs);
depth++;
//if (not frm_neigs.empty())
//{
// std::string frm_neigs_str(depth, '.');
// for (auto frm : frm_neigs)
// frm_neigs_str += std::to_string(frm->id()) + std::string(" ");
// WOLF_INFO(frm_neigs_str);
//}
// finish
if (frm_neigs.count(frm2) != 0)
{
//WOLF_INFO("Frame ", frm2->id(), " found!");
assert(parents_.count(frm2) != 0);
FactorBasePtrList factor_path;
auto prev_frm = frm1;
auto frm_it = frm2;
while (parents_.at(prev_frm).second != frm2)
while (frm_it != frm1)
{
factor_path.push_back(parents_.at(prev_frm).first);
prev_frm = parents_.at(prev_frm).second;
factor_path.push_back(parents_.at(frm_it).first);
frm_it = parents_.at(frm_it).second;
}
return factor_path;
......@@ -45,6 +60,7 @@ FactorBasePtrList GraphSearch::computeShortestPath(FrameBasePtr frm1,
if (max_graph_dist > 0 and depth == max_graph_dist)
break;
}
//WOLF_INFO("Path to frame ", frm2->id(), " NOT found!");
return FactorBasePtrList();
}
......@@ -61,10 +77,13 @@ std::set<FrameBasePtr> GraphSearch::getNeighborFrames(const std::set<FrameBasePt
// Iterate over all factors_by
for (auto && fac_by : facs_by)
{
//WOLF_INFO_COND(fac_by, "fac_by: ", fac_by->id());
//WOLF_INFO_COND(fac_by->getFrame(), "fac_by->getFrame(): ", fac_by->getFrame()->id());
if (fac_by and
fac_by->getFrame() and
parents_.count(fac_by->getFrame()) != 0)
parents_.count(fac_by->getFrame()) == 0)
{
//WOLF_INFO("registering");
frm_neigs.insert(fac_by->getFrame());
parents_[fac_by->getFrame()] = std::pair<FactorBasePtr,FrameBasePtr>(fac_by, frm);
}
......@@ -76,22 +95,24 @@ std::set<FrameBasePtr> GraphSearch::getNeighborFrames(const std::set<FrameBasePt
// Iterate over all factors_own
for (auto && fac_own : facs_own)
{
//WOLF_INFO_COND(fac_own, "fac_own: ", fac_own->id());
//WOLF_INFO_COND(fac_own->getFrameOtherList().empty(), "fac_own->getFrameOtherList() is empty");
if (fac_own and not fac_own->getFrameOtherList().empty())
for (auto frm_other_w : fac_own->getFrameOtherList())
{
auto frm_other = frm_other_w.lock();
if (frm_other and parents_.count(frm_other))
//WOLF_INFO_COND(frm_other, "frm_other ", frm_other->id());
if (frm_other and
parents_.count(frm_other) == 0)
{
//WOLF_INFO("registering");
frm_neigs.insert(frm_other);
parents_[frm_other] = std::pair<FactorBasePtr,FrameBasePtr>(fac_own, frm);
}
}
}
}
// TODO
// get list of factors and "constrained by" factor of each frame
// check that these factors are not in factor_parents_
// check that frames are not in frames_parents_ ant not in frm_neigs
// store in factor_parents_ and frames_parents and frm_neigs
return frm_neigs;
}
......
test/CMakeLists.txt
+ 4
− 0
View file @ 7a5e03a3
......@@ -80,6 +80,10 @@ target_link_libraries(gtest_feature_base ${PLUGIN_NAME})
wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
target_link_libraries(gtest_frame_base ${PLUGIN_NAME})
# GraphSearch class test
wolf_add_gtest(gtest_graph_search gtest_graph_search.cpp)
target_link_libraries(gtest_graph_search ${PLUGIN_NAME})
# HasStateBlocks classes test
wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME})
......
test/gtest_graph_search.cpp 0 → 100644
+ 225
− 0
View file @ 7a5e03a3
/*
* gtest_graph_search.cpp
*
* Created on: Jul, 2021
* Author: jvallve
*/
#include "core/utils/utils_gtest.h"
#include "core/problem/problem.h"
#include "core/capture/capture_void.h"
#include "core/factor/factor_relative_pose_2d.h"
#include "core/utils/graph_search.h"
#include <iostream>
#include <thread>
using namespace wolf;
using namespace Eigen;
class GraphSearchTest : public testing::Test
{
public:
ProblemPtr problem;
void SetUp() override
{
problem = Problem::create("PO", 2);
}
FrameBasePtr emplaceFrame(const TimeStamp& ts)
{
return problem->emplaceFrame(ts, Vector3d::Zero());
}
FactorBasePtr createFactor(FrameBasePtr frm1, FrameBasePtr frm2)
{
auto C12 = CaptureBase::emplace<CaptureVoid>(frm2, frm2->getTimeStamp(), nullptr);
auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", Vector3d::Zero(), Matrix3d::Identity());
return FactorBase::emplace<FactorRelativePose2d>(f12, f12, frm1, nullptr, false, TOP_MOTION);
}
};
TEST_F(GraphSearchTest, setup)
{
ASSERT_TRUE(problem->check());
}
TEST_F(GraphSearchTest, nonsense11)
{
auto F1 = emplaceFrame(1);
auto fac_list = GraphSearch::shortestPath(F1, F1, 10);
ASSERT_TRUE(fac_list.empty());
}
TEST_F(GraphSearchTest, single12)
{
auto F1 = emplaceFrame(1);
auto F2 = emplaceFrame(2);
auto fac12 = createFactor(F1,F2);
auto fac_list = GraphSearch::shortestPath(F1, F2, 10);
ASSERT_EQ(fac_list.size(), 1);
ASSERT_EQ(fac_list.front(), fac12);
}
TEST_F(GraphSearchTest, single21)
{
auto F1 = emplaceFrame(1);
auto F2 = emplaceFrame(2);
auto fac12 = createFactor(F1,F2);
auto fac_list = GraphSearch::shortestPath(F2, F1, 10);
ASSERT_EQ(fac_list.size(), 1);
ASSERT_EQ(fac_list.front(), fac12);
}
TEST_F(GraphSearchTest, double12)
{
auto F1 = emplaceFrame(1);
auto F2 = emplaceFrame(2);
auto fac12 = createFactor(F1,F2);
auto fac12b = createFactor(F1,F2);
auto fac_list = GraphSearch::shortestPath(F1, F2, 10);
ASSERT_EQ(fac_list.size(), 1);
ASSERT_TRUE(fac_list.front() == fac12 or fac_list.front() == fac12b);
}
TEST_F(GraphSearchTest, no_solution12)
{
auto F1 = emplaceFrame(1);
auto F2 = emplaceFrame(2);
auto F3 = emplaceFrame(3);
auto fac23 = createFactor(F2,F3);
auto fac_list = GraphSearch::shortestPath(F1, F2, 10);
ASSERT_TRUE(fac_list.empty());
}
TEST_F(GraphSearchTest, no_solution21)
{
auto F1 = emplaceFrame(1);
auto F2 = emplaceFrame(2);
auto F3 = emplaceFrame(3);
auto fac23 = createFactor(F2,F3);
auto fac_list = GraphSearch::shortestPath(F2, F1, 10);
ASSERT_TRUE(fac_list.empty());
}
TEST_F(GraphSearchTest, large)
{
auto F1 = emplaceFrame(1);
auto F2 = emplaceFrame(2);
auto F3 = emplaceFrame(3);
auto F4 = emplaceFrame(4);
auto F5 = emplaceFrame(5);
auto F6 = emplaceFrame(6);
auto F7 = emplaceFrame(7);
auto F8 = emplaceFrame(8);
auto F9 = emplaceFrame(9);
auto fac12 = createFactor(F1,F2);
auto fac23 = createFactor(F2,F3);
auto fac34 = createFactor(F3,F4);
auto fac45 = createFactor(F4,F5);
auto fac56 = createFactor(F5,F6);
auto fac67 = createFactor(F6,F7);
auto fac78 = createFactor(F7,F8);
auto fac89 = createFactor(F8,F9);
auto fac_list = GraphSearch::shortestPath(F1, F9, 8);
ASSERT_EQ(fac_list.size(), 8);
auto fac_list_2 = GraphSearch::shortestPath(F1, F9, 7);
ASSERT_EQ(fac_list_2.size(), 0);
}
TEST_F(GraphSearchTest, large_no_solution)
{
auto F1 = emplaceFrame(1);
auto F2 = emplaceFrame(2);
auto F3 = emplaceFrame(3);
auto F4 = emplaceFrame(4);
auto F5 = emplaceFrame(5);
auto F6 = emplaceFrame(6);
auto F7 = emplaceFrame(7);
auto F8 = emplaceFrame(8);
auto F9 = emplaceFrame(9);
auto fac12 = createFactor(F1,F2);
auto fac23 = createFactor(F2,F3);
auto fac34 = createFactor(F3,F4);
auto fac45 = createFactor(F4,F5);
auto fac67 = createFactor(F6,F7);
auto fac78 = createFactor(F7,F8);
auto fac89 = createFactor(F8,F9);
auto fac_list = GraphSearch::shortestPath(F1, F9, 10);
ASSERT_EQ(fac_list.size(), 0);
auto fac_list_2 = GraphSearch::shortestPath(F9, F1, 10);
ASSERT_EQ(fac_list_2.size(), 0);
}
TEST_F(GraphSearchTest, large_dense)
{
/* ------- ---------------
* | | | |
* 1---2---3---4---5---6---7---8---9
* | |
* -----------
*/
auto F1 = emplaceFrame(1);
auto F2 = emplaceFrame(2);
auto F3 = emplaceFrame(3);
auto F4 = emplaceFrame(4);
auto F5 = emplaceFrame(5);
auto F6 = emplaceFrame(6);
auto F7 = emplaceFrame(7);
auto F8 = emplaceFrame(8);
auto F9 = emplaceFrame(9);
auto fac12 = createFactor(F1,F2);
auto fac13 = createFactor(F1,F3);
auto fac23 = createFactor(F2,F3);
auto fac34 = createFactor(F3,F4);
auto fac36 = createFactor(F3,F6);
auto fac45 = createFactor(F4,F5);
auto fac56 = createFactor(F5,F6);
auto fac59 = createFactor(F5,F9);
auto fac67 = createFactor(F6,F7);
auto fac78 = createFactor(F7,F8);
auto fac89 = createFactor(F8,F9);
auto fac_list = GraphSearch::shortestPath(F1, F9, 10);
ASSERT_EQ(fac_list.size(), 4);
auto fac_list_2 = GraphSearch::shortestPath(F9, F1, 10);
ASSERT_EQ(fac_list_2.size(), 4);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment