Skip to content
Snippets Groups Projects
Commit c4b88111 authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Rename

parent d45fe35f
No related branches found
No related tags found
2 merge requests!28release after RAL,!27After 2nd RAL submission
Hide whitespace changes
Inline Side-by-side
Showing
with 84 additions and 84 deletions
include/gnss/factor/factor_gnss_fix_2D.h → include/gnss/factor/factor_gnss_fix_2d.h
+ 12
− 12
View file @ c4b88111
#ifndef FACTOR_GNSS_FIX_2D_H_
#define FACTOR_GNSS_FIX_2D_H_
#ifndef FACTOR_GNSS_FIX_2d_H_
#define FACTOR_GNSS_FIX_2d_H_
//Wolf includes
#include "core/common/wolf.h"
......@@ -10,17 +10,17 @@
namespace wolf {
WOLF_PTR_TYPEDEFS(FactorGnssFix2D);
WOLF_PTR_TYPEDEFS(FactorGnssFix2d);
class FactorGnssFix2D : public FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1, 1, 1>
class FactorGnssFix2d : public FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1, 1, 1>
{
protected:
SensorGnssPtr sensor_gnss_ptr_;
public:
FactorGnssFix2D(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1, 1, 1>("FactorGnssFix2D",
FactorGnssFix2d(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1, 1, 1>("FactorGnssFix2d",
nullptr,
nullptr,
nullptr,
......@@ -37,10 +37,10 @@ class FactorGnssFix2D : public FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1,
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an active GNSS Fix 2D factor without initializing ENU");
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an active GNSS Fix 2d factor without initializing ENU");
}
virtual ~FactorGnssFix2D() = default;
virtual ~FactorGnssFix2d() = default;
virtual std::string getTopology() const override
{
......@@ -60,7 +60,7 @@ class FactorGnssFix2D : public FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1,
};
template<typename T>
inline bool FactorGnssFix2D::operator ()(const T* const _x,
inline bool FactorGnssFix2d::operator ()(const T* const _x,
const T* const _o,
const T* const _x_antena,
const T* const _t_ENU_map,
......@@ -84,19 +84,19 @@ inline bool FactorGnssFix2D::operator ()(const T* const _x,
//std::cout << "getREnuEcef():\n" << sensor_gnss_ptr_->gettEnuEcef() << std::endl;
//std::cout << "gettEnuEcef(): " << sensor_gnss_ptr_->gettEnuEcef().transpose() << std::endl;
// Transform ECEF 3D Fix Feature to 2D Fix in Map coordinates (removing z)
// Transform ECEF 3d Fix Feature to 2d Fix in Map coordinates (removing z)
//std::cout << "R_map_ENU:\n\t" << R_map_ENU(0,0) << "\t" << R_map_ENU(0,1) << "\t" << R_map_ENU(0,2) << "\n\t" << R_map_ENU(1,0) << "\t" << R_map_ENU(1,1) << "\t" << R_map_ENU(1,2) << std::endl;
//std::cout << "R_ENU_ECEF:\n\t" << R_ENU_ECEF(0,0) << "\t" << R_ENU_ECEF(0,1) << "\t" << R_ENU_ECEF(0,2) << "\n\t" << R_ENU_ECEF(1,0) << "\t" << R_ENU_ECEF(1,1) << "\t" << R_ENU_ECEF(1,2) << "\n\t" << R_ENU_ECEF(2,0) << "\t" << R_ENU_ECEF(2,1) << "\t" << R_ENU_ECEF(2,2) << std::endl;
//std::cout << "t_ENU_ECEF:\n\t" << t_ENU_ECEF(0) << "\n\t" << t_ENU_ECEF(1) << "\n\t" << t_ENU_ECEF(2) << std::endl;
Eigen::Matrix<T,2,1> fix_map = R_map_ENU * (R_ENU_ECEF * fix_ECEF + t_ENU_ECEF - t_ENU_map);
//std::cout << "fix 3D:\n\t" << getMeasurement()(0) << "\n\t" << getMeasurement()(1) << "\n\t" << getMeasurement()(2) << std::endl;
//std::cout << "fix 3d:\n\t" << getMeasurement()(0) << "\n\t" << getMeasurement()(1) << "\n\t" << getMeasurement()(2) << std::endl;
//std::cout << "fix_map:\n\t" << fix_map[0] << "\n\t" << fix_map[1] << std::endl;
// Antena position in Map coordinates
Eigen::Matrix<T,2,1> antena_map = R_map_base * t_base_antena.head(2) + t_map_base;
//std::cout << "antena_map:\n\t" << antena_map[0] << "\n\t" << antena_map[1] << std::endl;
// Compute residual rotating information matrix to 2D Map coordinates
// Compute residual rotating information matrix to 2d Map coordinates
// Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R'
// In this case R = R_map_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_map_ENU'
residuals_ECEF = (getMeasurementSquareRootInformationUpper().cast<T>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map);
......
include/gnss/factor/factor_gnss_fix_3D.h → include/gnss/factor/factor_gnss_fix_3d.h
+ 10
− 10
View file @ c4b88111
#ifndef FACTOR_GNSS_FIX_3D_H_
#define FACTOR_GNSS_FIX_3D_H_
#ifndef FACTOR_GNSS_FIX_3d_H_
#define FACTOR_GNSS_FIX_3d_H_
//Wolf includes
#include "core/common/wolf.h"
......@@ -9,17 +9,17 @@
namespace wolf {
WOLF_PTR_TYPEDEFS(FactorGnssFix3D);
WOLF_PTR_TYPEDEFS(FactorGnssFix3d);
class FactorGnssFix3D : public FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1, 1, 1>
class FactorGnssFix3d : public FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1, 1, 1>
{
protected:
SensorGnssPtr sensor_gnss_ptr_;
public:
FactorGnssFix3D(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1, 1, 1>("FactorGnssFix3D",
FactorGnssFix3d(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1, 1, 1>("FactorGnssFix3d",
nullptr,
nullptr,
nullptr,
......@@ -36,10 +36,10 @@ class FactorGnssFix3D : public FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1,
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3D factor without initializing ENU");
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3d factor without initializing ENU");
}
virtual ~FactorGnssFix3D() = default;
virtual ~FactorGnssFix3d() = default;
virtual std::string getTopology() const override
{
......@@ -59,7 +59,7 @@ class FactorGnssFix3D : public FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1,
};
template<typename T>
inline bool FactorGnssFix3D::operator ()(const T* const _x,
inline bool FactorGnssFix3d::operator ()(const T* const _x,
const T* const _o,
const T* const _x_antena,
const T* const _t_ENU_map,
......@@ -79,7 +79,7 @@ inline bool FactorGnssFix3D::operator ()(const T* const _x,
Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
Eigen::Matrix<T,3,1> fix_ECEF = getMeasurement().cast<T>();
// Transform ECEF 3D Fix Feature to Map coordinates
// Transform ECEF 3d Fix Feature to Map coordinates
Eigen::Matrix<T,3,1> fix_map = R_map_ENU * (R_ENU_ECEF * fix_ECEF + t_ENU_ECEF - t_ENU_map);
// Antena position in Map coordinates
......
include/gnss/factor/factor_gnss_single_diff_2D.h → include/gnss/factor/factor_gnss_single_diff_2d.h
+ 17
− 17
View file @ c4b88111
#ifndef FACTOR_GNSS_SINGLE_DIFF_2D_H_
#define FACTOR_GNSS_SINGLE_DIFF_2D_H_
#ifndef FACTOR_GNSS_SINGLE_DIFF_2d_H_
#define FACTOR_GNSS_SINGLE_DIFF_2d_H_
//Wolf includes
#include "core/common/wolf.h"
......@@ -10,17 +10,17 @@
namespace wolf {
WOLF_PTR_TYPEDEFS(FactorGnssSingleDiff2D);
WOLF_PTR_TYPEDEFS(FactorGnssSingleDiff2d);
class FactorGnssSingleDiff2D : public FactorAutodiff<FactorGnssSingleDiff2D, 3, 2, 1, 2, 1, 3, 1, 1, 1>
class FactorGnssSingleDiff2d : public FactorAutodiff<FactorGnssSingleDiff2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>
{
protected:
SensorGnssPtr sensor_gnss_ptr_;
public:
FactorGnssSingleDiff2D(FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssSingleDiff2D, 3, 2, 1, 2, 1, 3, 1, 1, 1>("GNSS SINGLE DIFFERENCES 2D",
FactorGnssSingleDiff2d(FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssSingleDiff2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("GNSS SINGLE DIFFERENCES 2d",
_frame_other_ptr,
nullptr,
nullptr,
......@@ -38,10 +38,10 @@ class FactorGnssSingleDiff2D : public FactorAutodiff<FactorGnssSingleDiff2D, 3,
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 2D factor without initializing ENU");
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 2d factor without initializing ENU");
}
virtual ~FactorGnssSingleDiff2D() = default;
virtual ~FactorGnssSingleDiff2d() = default;
virtual std::string getTopology() const override
{
......@@ -62,7 +62,7 @@ class FactorGnssSingleDiff2D : public FactorAutodiff<FactorGnssSingleDiff2D, 3,
};
template<typename T>
inline bool FactorGnssSingleDiff2D::operator ()(const T* const _x1,
inline bool FactorGnssSingleDiff2d::operator ()(const T* const _x1,
const T* const _o1,
const T* const _x2,
const T* const _o2,
......@@ -82,23 +82,23 @@ inline bool FactorGnssSingleDiff2D::operator ()(const T* const _x1,
Eigen::Matrix<T,3,3> R_ENU_ECEF = sensor_gnss_ptr_->getREnuEcef().cast<T>();
Eigen::Matrix<T,2,3> R_MAP_ENU = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_MAP[0], _pitch_ENU_MAP[0], _yaw_ENU_MAP[0]).transpose().topRows(2);
// Transform ECEF 3D SingleDiff Feature to 2D SingleDiff in Map coordinates (removing z)
Eigen::Matrix<T,2,1> measured_diff_2D_MAP = R_MAP_ENU * (R_ENU_ECEF * getMeasurement().cast<T>());
// Transform ECEF 3d SingleDiff Feature to 2d SingleDiff in Map coordinates (removing z)
Eigen::Matrix<T,2,1> measured_diff_2d_MAP = R_MAP_ENU * (R_ENU_ECEF * getMeasurement().cast<T>());
// Substraction of expected antena positions in Map coordinates
Eigen::Matrix<T,2,1> expected_diff_2D_MAP = (R_MAP_BASE2 * t_BASE_ANTENA.head(2) + t_MAP_BASE2) - (R_MAP_BASE1 * t_BASE_ANTENA.head(2) + t_MAP_BASE1);
Eigen::Matrix<T,2,1> expected_diff_2d_MAP = (R_MAP_BASE2 * t_BASE_ANTENA.head(2) + t_MAP_BASE2) - (R_MAP_BASE1 * t_BASE_ANTENA.head(2) + t_MAP_BASE1);
// Compute residual rotating information matrix to 2D Map coordinates
// Compute residual rotating information matrix to 2d Map coordinates
// Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R'
// In this case R = R_2DMAP_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_2DMAP_ENU'
residuals_ECEF = (getMeasurementSquareRootInformationUpper().cast<T>() * R_ENU_ECEF.transpose() * R_MAP_ENU.transpose()) * (expected_diff_2D_MAP - measured_diff_2D_MAP);
// In this case R = R_2dMAP_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_2dMAP_ENU'
residuals_ECEF = (getMeasurementSquareRootInformationUpper().cast<T>() * R_ENU_ECEF.transpose() * R_MAP_ENU.transpose()) * (expected_diff_2d_MAP - measured_diff_2d_MAP);
//std::cout << "frame1: " << _x1[0] << " " << _x1[1] << " " << _o1[0] << std::endl;
//std::cout << "frame2: " << _x2[0] << " " << _x2[1] << " " << _o2[0] << std::endl;
//std::cout << "antena: " << _x_antena[0] << " " << _x_antena[1] << " " << _x_antena[2] << std::endl;
//std::cout << "RPY: " << _roll[0] << " " << _pitch[1] << " " << _yaw[2] << std::endl;
//std::cout << "measured_diff_2D: " << measured_diff_2D[0] << " " << measured_diff_2D[1] << std::endl;
//std::cout << "expected_diff_2D: " << expected_diff_2D[0] << " " << expected_diff_2D[1] << std::endl;
//std::cout << "measured_diff_2d: " << measured_diff_2d[0] << " " << measured_diff_2d[1] << std::endl;
//std::cout << "expected_diff_2d: " << expected_diff_2d[0] << " " << expected_diff_2d[1] << std::endl;
return true;
}
......
src/processor/processor_gnss_fix.cpp
+ 6
− 6
View file @ c4b88111
#include "gnss/factor/factor_gnss_fix_2D.h"
#include "gnss/factor/factor_gnss_fix_3D.h"
#include "gnss/factor/factor_gnss_fix_2d.h"
#include "gnss/factor/factor_gnss_fix_3d.h"
#include "gnss/feature/feature_gnss_fix.h"
#include "gnss/processor/processor_gnss_fix.h"
#include "gnss/capture/capture_gnss_fix.h"
......@@ -107,12 +107,12 @@ FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr _ftr)
{
// CREATE CONSTRAINT --------------------
//WOLF_DEBUG("creating the factor...");
// 2D
// 2d
if (getProblem()->getDim() == 2)
return FactorBase::emplace<FactorGnssFix2D>(_ftr, _ftr, sensor_gnss_, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
// 3D
return FactorBase::emplace<FactorGnssFix2d>(_ftr, _ftr, sensor_gnss_, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
// 3d
else
return FactorBase::emplace<FactorGnssFix3D>(_ftr, _ftr, sensor_gnss_, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
return FactorBase::emplace<FactorGnssFix3d>(_ftr, _ftr, sensor_gnss_, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
}
bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac)
......
src/processor/processor_gnss_single_diff.cpp
+ 5
− 5
View file @ c4b88111
#include "gnss/factor/factor_gnss_single_diff_2D.h"
#include "gnss/factor/factor_gnss_single_diff_2d.h"
#include "gnss/feature/feature_gnss_single_diff.h"
#include "gnss/processor/processor_gnss_single_diff.h"
#include "gnss/capture/capture_gnss_single_diff.h"
......@@ -90,12 +90,12 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture)
FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr _ftr)
{
//WOLF_DEBUG("creating the factor...");
// 2D
// 2d
if (getProblem()->getDim() == 2)
return FactorBase::emplace<FactorGnssSingleDiff2D>(_ftr, _ftr, incoming_capture_->getOriginFrame(), sensor_gnss_, shared_from_this(), params_->apply_loss_function);
// 3D TODO
return FactorBase::emplace<FactorGnssSingleDiff2d>(_ftr, _ftr, incoming_capture_->getOriginFrame(), sensor_gnss_, shared_from_this(), params_->apply_loss_function);
// 3d TODO
else
std::runtime_error("Single Differences in 3D not implemented yet.");
std::runtime_error("Single Differences in 3d not implemented yet.");
return nullptr;
}
......
src/sensor/sensor_gnss.cpp
+ 3
− 3
View file @ c4b88111
......@@ -11,7 +11,7 @@ static double kFirstEccentricitySquared = 6.69437999014 * 0.001;
static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
//static double kFlattening = 1 / 298.257223563;
SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D position with respect to the car frame (base frame)
SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3d position with respect to the car frame (base frame)
StateBlockPtr _bias_ptr, //GNSS sensor time bias
ParamsSensorGnssPtr params) //sensor params
:
......@@ -129,7 +129,7 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXd& _pose_MAP_frameENU, con
// compute fix vector (from 1 to 2) in ENU coordinates
Eigen::Vector3d v_ENU = R_ENU_ECEF * (_t_ECEF_antena2 - _t_ECEF_antenaENU);
// 2D
// 2d
if (getProblem()->getDim() == 2)
{
// compute antena vector (from 1 to 2) in MAP
......@@ -178,7 +178,7 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXd& _pose_MAP_frameENU, con
WOLF_INFO("SensorGnss: ENU-MAP initialized.")
}
// 3D
// 3d
else
{
//TODO
......
test/CMakeLists.txt
+ 6
− 6
View file @ c4b88111
......@@ -13,10 +13,10 @@ include_directories(${GTEST_INCLUDE_DIRS})
# #
###########################################################
# FactorGnssFix2D test
wolf_add_gtest(gtest_factor_gnss_fix_2D gtest_factor_gnss_fix_2D.cpp)
target_link_libraries(gtest_factor_gnss_fix_2D ${PLUGIN_NAME} ${wolf_LIBRARY})
# FactorGnssFix2d test
wolf_add_gtest(gtest_factor_gnss_fix_2d gtest_factor_gnss_fix_2d.cpp)
target_link_libraries(gtest_factor_gnss_fix_2d ${PLUGIN_NAME} ${wolf_LIBRARY})
# FactorGnssSingleDiff2D test
wolf_add_gtest(gtest_factor_gnss_single_diff_2D gtest_factor_gnss_single_diff_2D.cpp)
target_link_libraries(gtest_factor_gnss_single_diff_2D ${PLUGIN_NAME} ${wolf_LIBRARY})
\ No newline at end of file
# FactorGnssSingleDiff2d test
wolf_add_gtest(gtest_factor_gnss_single_diff_2d gtest_factor_gnss_single_diff_2d.cpp)
target_link_libraries(gtest_factor_gnss_single_diff_2d ${PLUGIN_NAME} ${wolf_LIBRARY})
\ No newline at end of file
test/gtest_factor_gnss_fix_2D.cpp → test/gtest_factor_gnss_fix_2d.cpp
+ 8
− 8
View file @ c4b88111
/**
* \file gtest_factor_gnss_fix_2D.cpp
* \file gtest_factor_gnss_fix_2d.cpp
*
* Created on: Aug 1, 2018
* \author: jvallve
*/
#include "gnss/factor/factor_gnss_fix_2D.h"
#include "gnss/factor/factor_gnss_fix_2d.h"
#include <core/utils/utils_gtest.h>
#include "core/problem/problem.h"
......@@ -93,7 +93,7 @@ FrameBasePtr frame_ptr;
////////////////////////////////////////////////////////
TEST(FactorGnssFix2DTest, configure_tree)
TEST(FactorGnssFix2dTest, configure_tree)
{
ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100;
......@@ -130,7 +130,7 @@ TEST(FactorGnssFix2DTest, configure_tree)
* Estimating: MAP_BASE position.
* Fixed: ENU_MAP, MAP_BASE orientation, BASE_ANTENA.
*/
TEST(FactorGnssFix2DTest, gnss_1_map_base_position)
TEST(FactorGnssFix2dTest, gnss_1_map_base_position)
{
// --------------------------- Reset states
resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
......@@ -176,7 +176,7 @@ TEST(FactorGnssFix2DTest, gnss_1_map_base_position)
* Estimating: MAP_BASE orientation.
* Fixed: ENU_MAP, MAP_BASE position, BASE_ANTENA.
*/
TEST(FactorGnssFix2DTest, gnss_1_map_base_orientation)
TEST(FactorGnssFix2dTest, gnss_1_map_base_orientation)
{
// --------------------------- Reset states
resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
......@@ -219,7 +219,7 @@ TEST(FactorGnssFix2DTest, gnss_1_map_base_orientation)
* Estimating: ENU_MAP yaw.
* Fixed: ENU_MAP position, MAP_BASE and BASE_ANTENA.
*/
TEST(FactorGnssFix2DTest, gnss_1_enu_map_yaw)
TEST(FactorGnssFix2dTest, gnss_1_enu_map_yaw)
{
// --------------------------- Reset states
resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
......@@ -262,7 +262,7 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_yaw)
* Estimating: ENU_MAP position.
* Fixed: ENU_MAP yaw, MAP_BASE and BASE_ANTENA.
*/
TEST(FactorGnssFix2DTest, gnss_1_enu_map_position)
TEST(FactorGnssFix2dTest, gnss_1_enu_map_position)
{
// --------------------------- Reset states
resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
......@@ -306,7 +306,7 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_position)
* Estimating: BASE_ANTENA (2 first components that are observable).
* Fixed: ENU_MAP, MAP_BASE.
*/
TEST(FactorGnssFix2DTest, gnss_1_base_antena)
TEST(FactorGnssFix2dTest, gnss_1_base_antena)
{
// --------------------------- Reset states
resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
......
test/gtest_factor_gnss_single_diff_2D.cpp → test/gtest_factor_gnss_single_diff_2d.cpp
+ 17
− 17
View file @ c4b88111
/**
* \file gtest_factor_gnss_single_diff_2D.cpp
* \file gtest_factor_gnss_single_diff_2d.cpp
*
* Created on: Aug 28, 2018
* \author: jvallve
*/
#include "gnss/factor/factor_gnss_single_diff_2D.h"
#include "gnss/factor/factor_gnss_single_diff_2d.h"
#include <core/utils/utils_gtest.h>
#include "core/capture/capture_motion.h"
#include "core/problem/problem.h"
#include "gnss/sensor/sensor_gnss.h"
#include "core/sensor/sensor_odom_2D.h"
#include "core/processor/processor_odom_2D.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/processor/processor_odom_2d.h"
#include "gnss/processor/processor_gnss_single_diff.h"
#include "core/ceres_wrapper/ceres_manager.h"
......@@ -25,7 +25,7 @@ using std::cout;
using std::endl;
class FactorGnssSingleDiff2DTest : public testing::Test
class FactorGnssSingleDiff2dTest : public testing::Test
{
public:
......@@ -41,10 +41,10 @@ class FactorGnssSingleDiff2DTest : public testing::Test
ProblemPtr problem_ptr;
CeresManagerPtr ceres_mgr_ptr;
SensorGnssPtr gnss_sensor_ptr;
SensorOdom2DPtr odom_sensor_ptr;
SensorOdom2dPtr odom_sensor_ptr;
FrameBasePtr prior_frame_ptr;
FactorGnssSingleDiff2DTest()
FactorGnssSingleDiff2dTest()
{
o_enu_map << 2.6;
t_map_base1 << 32, -34, 0; // z=0
......@@ -80,21 +80,21 @@ class FactorGnssSingleDiff2DTest : public testing::Test
problem_ptr->installProcessor("ProcessorGnssSingleDiff", "gnss single difference", gnss_sensor_ptr, gnss_params_ptr);
// odom sensor & processor
ParamsSensorOdom2DPtr odom_intrinsics_ptr = std::make_shared<ParamsSensorOdom2D>();
ParamsSensorOdom2dPtr odom_intrinsics_ptr = std::make_shared<ParamsSensorOdom2d>();
odom_intrinsics_ptr->k_disp_to_disp = 0.1;
odom_intrinsics_ptr->k_rot_to_rot = 0.1;
//<<<<<<< HEAD
odom_sensor_ptr = std::static_pointer_cast<SensorOdom2D>(problem_ptr->installSensor("SensorOdom2D", "odometer", VectorXd::Zero(3), odom_intrinsics_ptr));
odom_sensor_ptr = std::static_pointer_cast<SensorOdom2d>(problem_ptr->installSensor("SensorOdom2d", "odometer", VectorXd::Zero(3), odom_intrinsics_ptr));
//=======
// odom_sensor_ptr = std::static_pointer_cast<SensorOdom2D>(problem_ptr->installSensor("ODOM 2D", "odometer", VectorXd::Zero(3), odom_intrinsics_ptr));
// odom_sensor_ptr = std::static_pointer_cast<SensorOdom2d>(problem_ptr->installSensor("ODOM 2d", "odometer", VectorXd::Zero(3), odom_intrinsics_ptr));
//>>>>>>> 8-replace-scalar-to-double
ProcessorParamsOdom2DPtr odom_params_ptr = std::make_shared<ProcessorParamsOdom2D>();
ProcessorParamsOdom2dPtr odom_params_ptr = std::make_shared<ProcessorParamsOdom2d>();
odom_params_ptr->voting_active = true;
odom_params_ptr->dist_traveled = 1;
odom_params_ptr->angle_turned = 2.0;
odom_params_ptr->max_time_span = 1.0;
odom_params_ptr->time_tolerance = 1.0;
problem_ptr->installProcessor("ProcessorOdom2D", "main odometry", odom_sensor_ptr, odom_params_ptr);
problem_ptr->installProcessor("ProcessorOdom2d", "main odometry", odom_sensor_ptr, odom_params_ptr);
//problem_ptr->setProcessorMotion("main odometry");
// set prior (FIXED)
......@@ -126,7 +126,7 @@ class FactorGnssSingleDiff2DTest : public testing::Test
////////////////////////////////////////////////////////
TEST_F(FactorGnssSingleDiff2DTest, check_tree)
TEST_F(FactorGnssSingleDiff2dTest, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
ASSERT_TRUE(prior_frame_ptr != nullptr);
......@@ -138,7 +138,7 @@ TEST_F(FactorGnssSingleDiff2DTest, check_tree)
* Estimating: MAP_BASE2 position.
* Fixed: MAP_BASE1, ENU_MAP fixed, MAP_BASE2 orientation, BASE_ANTENA.
*/
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_position)
TEST_F(FactorGnssSingleDiff2dTest, gnss_1_map_base_position)
{
// Create GNSS Fix capture
CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3d::Identity(), prior_frame_ptr);
......@@ -171,7 +171,7 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_position)
* Estimating: MAP_BASE2 orientation.
* Fixed: MAP_BASE1, ENU_MAP fixed, MAP_BASE2 position, BASE_ANTENA.
*/
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_orientation)
TEST_F(FactorGnssSingleDiff2dTest, gnss_1_map_base_orientation)
{
ASSERT_TRUE(prior_frame_ptr != nullptr);
// Create GNSS Fix capture
......@@ -206,7 +206,7 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_orientation)
* Estimating: ENU_MAP yaw.
* Fixed: MAP_BASE1, MAP_BASE2 and BASE_ANTENA.
*/
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_enu_map_yaw)
TEST_F(FactorGnssSingleDiff2dTest, gnss_1_enu_map_yaw)
{
// Create GNSS Fix capture
CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3d::Identity(), prior_frame_ptr);
......@@ -238,7 +238,7 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_enu_map_yaw)
* Estimating: BASE_ANTENA (2 first components obsevable).
* Fixed: MAP_BASE1, ENU_MAP, MAP_BASE2.
*/
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_base_antena)
TEST_F(FactorGnssSingleDiff2dTest, gnss_1_base_antena)
{
// Create GNSS Fix capture
CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3d::Identity(), prior_frame_ptr);
......
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