diff --git a/include/gnss/factor/factor_gnss_fix_2D.h b/include/gnss/factor/factor_gnss_fix_2d.h
similarity index 89%
rename from include/gnss/factor/factor_gnss_fix_2D.h
rename to include/gnss/factor/factor_gnss_fix_2d.h
index 7e8a2497735d4ae9643a420d3353abd00811eedb..4965cb14bf5fd4ffd933204821bcdc6c0d678a9c 100644
--- a/include/gnss/factor/factor_gnss_fix_2D.h
+++ b/include/gnss/factor/factor_gnss_fix_2d.h
@@ -1,6 +1,6 @@
-#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);
diff --git a/include/gnss/factor/factor_gnss_fix_3D.h b/include/gnss/factor/factor_gnss_fix_3d.h
similarity index 88%
rename from include/gnss/factor/factor_gnss_fix_3D.h
rename to include/gnss/factor/factor_gnss_fix_3d.h
index 4041ca790a8b592c854b9e9d93f9007995dbdbd3..beaf7808ba5410f6bedb54e2f9c0d782f0b03d39 100644
--- a/include/gnss/factor/factor_gnss_fix_3D.h
+++ b/include/gnss/factor/factor_gnss_fix_3d.h
@@ -1,6 +1,6 @@
-#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
diff --git a/include/gnss/factor/factor_gnss_single_diff_2D.h b/include/gnss/factor/factor_gnss_single_diff_2d.h
similarity index 80%
rename from include/gnss/factor/factor_gnss_single_diff_2D.h
rename to include/gnss/factor/factor_gnss_single_diff_2d.h
index e8f139c430fa689d451bc0189a147d3b83a1c309..44a6dbdff015ad7b9049d0d115c48899ddf35cf2 100644
--- a/include/gnss/factor/factor_gnss_single_diff_2D.h
+++ b/include/gnss/factor/factor_gnss_single_diff_2d.h
@@ -1,6 +1,6 @@
-#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;
}
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 6ae3ebdebecae4cfdc1ce098c074dae0c102afbb..7477b0711f94b6c4782090f92ef404b5a8f43179 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -1,5 +1,5 @@
-#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)
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index 96fdddde675f6f1685dc7e9b001f5a4a973886fd..b69c833bdd06af917a1134772c1ba5a4b15656f8 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -1,4 +1,4 @@
-#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;
}
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index 52ded0f3d3aa05b37568242681d5cb4e57c8b9e9..d7dd217023f93289789abc378a763570f383ff82 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -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
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 4686b740fca68fc811028129737b3640fb9eab6d..4938b4e67a3dbe1add04c0026420ec581b50ca77 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -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
diff --git a/test/gtest_factor_gnss_fix_2D.cpp b/test/gtest_factor_gnss_fix_2d.cpp
similarity index 97%
rename from test/gtest_factor_gnss_fix_2D.cpp
rename to test/gtest_factor_gnss_fix_2d.cpp
index be550e395fcfa0b1a6d279185a2b4e25951ed97f..aa04c5dfe3a215b1ea4b7d919b2870c1dfb3575d 100644
--- a/test/gtest_factor_gnss_fix_2D.cpp
+++ b/test/gtest_factor_gnss_fix_2d.cpp
@@ -1,11 +1,11 @@
/**
- * \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);
diff --git a/test/gtest_factor_gnss_single_diff_2D.cpp b/test/gtest_factor_gnss_single_diff_2d.cpp
similarity index 91%
rename from test/gtest_factor_gnss_single_diff_2D.cpp
rename to test/gtest_factor_gnss_single_diff_2d.cpp
index 65f93e5bd69e5100d842917cf7b4afed67d1d648..9250d3b0ee9561cfbea842b2b55472bb37e20b35 100644
--- a/test/gtest_factor_gnss_single_diff_2D.cpp
+++ b/test/gtest_factor_gnss_single_diff_2d.cpp
@@ -1,19 +1,19 @@
/**
- * \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);