Skip to content
Snippets Groups Projects
Commit 9b8d0286 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Update gtest_odom_2D.cpp

parent 333c2044
No related branches found
No related tags found
No related merge requests found
Pipeline #4273 passed
...@@ -8,14 +8,16 @@ ...@@ -8,14 +8,16 @@
#include "core/utils/utils_gtest.h" #include "core/utils/utils_gtest.h"
// Classes under test // Classes under test
#include "core/sensor/sensor_odom_2D.h"
#include "core/processor/processor_odom_2D.h" #include "core/processor/processor_odom_2D.h"
#include "core/factor/factor_odom_2D.h" #include "core/factor/factor_odom_2D.h"
#include "core/capture/capture_odom_2D.h"
// Wolf includes // Wolf includes
#include "core/sensor/sensor_odom_2D.h"
#include "core/state_block/state_block.h" #include "core/state_block/state_block.h"
#include "core/common/wolf.h" #include "core/common/wolf.h"
#include "core/ceres_wrapper/ceres_manager.h" #include "core/ceres_wrapper/ceres_manager.h"
#include "core/capture/capture_pose.h"
// STL includes // STL includes
#include <map> #include <map>
...@@ -26,7 +28,6 @@ ...@@ -26,7 +28,6 @@
// General includes // General includes
#include <iostream> #include <iostream>
#include <iomanip> // std::setprecision #include <iomanip> // std::setprecision
#include "core/capture/capture_pose.h"
using namespace wolf; using namespace wolf;
using namespace Eigen; using namespace Eigen;
...@@ -234,6 +235,9 @@ TEST(Odom2D, VoteForKfAndSolve) ...@@ -234,6 +235,9 @@ TEST(Odom2D, VoteForKfAndSolve)
std::vector<Eigen::VectorXs> integrated_pose_vector; std::vector<Eigen::VectorXs> integrated_pose_vector;
std::vector<Eigen::MatrixXs> integrated_cov_vector; std::vector<Eigen::MatrixXs> integrated_cov_vector;
integrated_pose_vector.push_back(integrated_pose);
integrated_cov_vector.push_back(integrated_cov);
// std::cout << "\nIntegrating data..." << std::endl; // std::cout << "\nIntegrating data..." << std::endl;
t += dt; t += dt;
...@@ -249,10 +253,7 @@ TEST(Odom2D, VoteForKfAndSolve) ...@@ -249,10 +253,7 @@ TEST(Odom2D, VoteForKfAndSolve)
// Processor // Processor
capture->process(); capture->process();
ASSERT_TRUE(problem->check(0)); ASSERT_TRUE(problem->check(0));
// Matrix3s odom2d_delta_cov = processor_odom2d->integrateBufferCovariance(processor_odom2d->getBuffer());
Matrix3s odom2d_delta_cov = processor_odom2d->getMotion().delta_integr_cov_; Matrix3s odom2d_delta_cov = processor_odom2d->getMotion().delta_integr_cov_;
// std::cout << "State(" << (t - t0) << ") : " << processor_odom2d->getCurrentState().transpose() << std::endl;
// std::cout << "PRC cov: \n" << odom2d_delta_cov << std::endl;
// Integrate Delta // Integrate Delta
if (n==3 || n==6) // keyframes if (n==3 || n==6) // keyframes
...@@ -291,12 +292,21 @@ TEST(Odom2D, VoteForKfAndSolve) ...@@ -291,12 +292,21 @@ TEST(Odom2D, VoteForKfAndSolve)
// Solve // Solve
std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
// std::cout << report << std::endl;
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
// Check the 3 KFs' states and covariances
MatrixXs computed_cov; MatrixXs computed_cov;
ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov)); int n = 0;
ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[5], 1e-6); for (auto F : problem->getTrajectory()->getFrameList())
{
if (!F->isKey()) break;
ASSERT_POSE2D_APPROX(F->getState(), integrated_pose_vector[n] , 1e-6);
ASSERT_TRUE (F->getCovariance(computed_cov));
ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[n] , 1e-6);
n += 3;
}
} }
TEST(Odom2D, KF_callback) TEST(Odom2D, KF_callback)
...@@ -314,7 +324,7 @@ TEST(Odom2D, KF_callback) ...@@ -314,7 +324,7 @@ TEST(Odom2D, KF_callback)
Eigen::Matrix3s x0_cov = Eigen::Matrix3s::Identity() * 0.1; Eigen::Matrix3s x0_cov = Eigen::Matrix3s::Identity() * 0.1;
VectorXs data(Vector2s(1, M_PI/4) ); // advance 1m VectorXs data(Vector2s(1, M_PI/4) ); // advance 1m
Eigen::MatrixXs data_cov = Eigen::MatrixXs::Identity(2, 2) * 0.01; Eigen::MatrixXs data_cov = Eigen::MatrixXs::Identity(2, 2) * 0.01;
int N = 16; // number of process() steps int N = 8; // number of process() steps
// NOTE: We integrate and create KFs as follows: // NOTE: We integrate and create KFs as follows:
// n = 0 1 2 3 4 5 6 7 8 // n = 0 1 2 3 4 5 6 7 8
...@@ -361,7 +371,7 @@ TEST(Odom2D, KF_callback) ...@@ -361,7 +371,7 @@ TEST(Odom2D, KF_callback)
// std::cout << "\nIntegrating data..." << std::endl; // std::cout << "\nIntegrating data..." << std::endl;
// Capture to use as container for all incoming data // Capture to use as container for all incoming data
CaptureMotionPtr capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr); auto capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr);
std::cout << "t: " << t << std::endl; std::cout << "t: " << t << std::endl;
for (int n=1; n<=N; n++) for (int n=1; n<=N; n++)
...@@ -405,11 +415,16 @@ TEST(Odom2D, KF_callback) ...@@ -405,11 +415,16 @@ TEST(Odom2D, KF_callback)
Vector3s x_split = processor_odom2d->getState(t_split); Vector3s x_split = processor_odom2d->getState(t_split);
FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, x_split, t_split); FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, x_split, t_split);
// there must be 2KF and one F
ASSERT_EQ(problem->getTrajectory()->getFrameList().size(), 3);
// The last KF must have TS = 0.08
ASSERT_EQ(problem->getLastKeyFrame()->getTimeStamp().getNanoSeconds(), 80000000);
ASSERT_TRUE(problem->check(0)); ASSERT_TRUE(problem->check(0));
processor_odom2d->keyFrameCallback(keyframe_2, dt/2); processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
ASSERT_TRUE(problem->check(0)); ASSERT_TRUE(problem->check(0));
t += dt; t += dt;
capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr); capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr);
capture->process(); capture->process();
ASSERT_TRUE(problem->check(0)); ASSERT_TRUE(problem->check(0));
...@@ -441,7 +456,7 @@ TEST(Odom2D, KF_callback) ...@@ -441,7 +456,7 @@ TEST(Odom2D, KF_callback)
processor_odom2d->keyFrameCallback(keyframe_1, dt/2); processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
ASSERT_TRUE(problem->check(0)); ASSERT_TRUE(problem->check(0));
t += dt; t += dt;
capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr); capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr);
capture->process(); capture->process();
ASSERT_TRUE(problem->check(0)); ASSERT_TRUE(problem->check(0));
...@@ -485,7 +500,7 @@ TEST(Odom2D, KF_callback) ...@@ -485,7 +500,7 @@ TEST(Odom2D, KF_callback)
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
// testing::GTEST_FLAG(filter) = "Odom2D.KF_callback"; // testing::GTEST_FLAG(filter) = "Odom2D.VoteForKfAndSolve";
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment