ekf_slam_solver_test.cpp

Go to the documentation of this file.

#include "slam_solver/ekf_slam_solver.hpp"
 
#include <gtest/gtest.h>
 
#include "motion_lib/v2p_models/map.hpp"
#include "perception_sensor_lib/data_association/map.hpp"
#include "perception_sensor_lib/landmark_filter/map.hpp"
 
class EKFSLAMSolverTest : public ::testing::Test {
protected:
  void SetUp() override {
    SLAMParameters params;
    params.load_config();
    data_association = data_association_models_map.at(params.data_association_model_name_)(
        DataAssociationParameters(params.data_association_limit_distance_,
                                  params.data_association_gate_,
                                  params.new_landmark_confidence_gate_, params.observation_x_noise_,
                                  params.observation_y_noise_));
    motion_model = v2p_models_map.at(params.motion_model_name_)();
 
    ekf_slam_solver = std::make_shared<EKFSLAMSolver>(params, data_association, motion_model,
                                                      landmark_filter, execution_time, nullptr);
  }
 
  std::shared_ptr<DataAssociationModel> data_association;
  std::shared_ptr<V2PMotionModel> motion_model;
  std::shared_ptr<LandmarkFilter> landmark_filter;
  std::shared_ptr<EKFSLAMSolver> ekf_slam_solver;
  std::shared_ptr<std::vector<double>> execution_time;
  std::weak_ptr<rclcpp::Node> node;
};
 
TEST_F(EKFSLAMSolverTest, stateAugmentation) {
  Eigen::VectorXd state = Eigen::VectorXd::Zero(3);
  Eigen::MatrixXd covariance = Eigen::MatrixXd::Identity(3, 3);
  Eigen::VectorXd new_landmarks_coordinates(10);
  new_landmarks_coordinates << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
  ekf_slam_solver->state_augmentation(state, covariance, new_landmarks_coordinates);
  std::vector<double> expected_state = {0, 0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
  EXPECT_EQ(state.size(), expected_state.size());
  for (int i = 0; i < state.size(); i++) {
    EXPECT_NEAR(state(i), expected_state[i], 0.0001);
  }
}
 
TEST_F(EKFSLAMSolverTest, stateAugmentation2) {
  Eigen::VectorXd state = Eigen::VectorXd::Zero(3);
  state << 0, 0, M_PI / 2;
  Eigen::MatrixXd covariance = Eigen::MatrixXd::Identity(3, 3);
  Eigen::VectorXd new_landmarks_coordinates(10);
  Eigen::VectorXd new_landmarks_confidences(5);
  new_landmarks_coordinates << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
  ekf_slam_solver->state_augmentation(state, covariance, new_landmarks_coordinates);
  std::vector<double> expected_state = {0, 0, M_PI / 2, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
  EXPECT_EQ(state.size(), expected_state.size());
  for (int i = 0; i < state.size(); i++) {
    EXPECT_NEAR(state(i), expected_state[i], 0.0001);
  }
}