Skip to content
Snippets Groups Projects
Commit 821ca5a9 authored by Jaime Tarraso's avatar Jaime Tarraso
Browse files

Merge branch 'master' of https://github.com/IRI-MobileRobotics/Wolf

parents 9740a1ce 3f651b3d
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
src/constraint_gps_pseudorange_2D.h
+ 62
− 57
View file @ 821ca5a9
...@@ -26,8 +26,6 @@ ...@@ -26,8 +26,6 @@
class ConstraintGPSPseudorange2D : public ConstraintSparse<1, 2, 1, 3, 1, 3, 1> class ConstraintGPSPseudorange2D : public ConstraintSparse<1, 2, 1, 3, 1, 3, 1>
{ {
public: public:
int verbose_level_ = 1; // 0=nothing printed. only for debug purpose
ConstraintGPSPseudorange2D(FeatureBase* _ftr_ptr, ConstraintStatus _status = CTR_ACTIVE) : ConstraintGPSPseudorange2D(FeatureBase* _ftr_ptr, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintSparse<1, 2, 1, 3, 1, 3, 1>(_ftr_ptr, ConstraintSparse<1, 2, 1, 3, 1, 3, 1>(_ftr_ptr,
CTR_GPS_PR_2D, CTR_GPS_PR_2D,
...@@ -84,40 +82,45 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c ...@@ -84,40 +82,45 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c
const T* const _map_p, const T* const _map_o, const T* const _map_p, const T* const _map_o,
T* _residual) const T* _residual) const
{ {
int verbose_level_ = 1; // 0=nothing printed. only for debug purpose
std::stringstream aux; std::stringstream aux;
aux << std::setprecision(12); aux << std::setprecision(12);
std::cout << std::setprecision(12);
std::cout << "+OPERATOR()+" << nodeId() << std::endl;
if (verbose_level_ >= 2) if (verbose_level_ >= 2)
{ {
std::cout << std::setprecision(12); std::cout << "+OPERATOR()+" << nodeId() << std::endl;
std::cout << "\n++++++OPERATOR()++++++\n"; std::cout << "_sensor_p(_base): " << _sensor_p[0] << ", " << _sensor_p[1] << ", " << _sensor_p[2] << std::endl;
std::cout << "_map_p: " << _map_p[0] << ", " << _map_p[1] << ", " << _map_p[2] << std::endl;
std::cout << "_map_o: " << _map_o[0] << std::endl;
std::cout << "_vehicle_p(_map): " << _vehicle_p[0] << ", " << _vehicle_p[1] << std::endl; std::cout << "_vehicle_p(_map): " << _vehicle_p[0] << ", " << _vehicle_p[1] << std::endl;
std::cout << "_vehicle_o(_map): " << _vehicle_o[0] << std::endl; std::cout << "_vehicle_o(_map): " << _vehicle_o[0] << std::endl;
std::cout << "_sensor_p(_base): " << _sensor_p[0] << ", " << _sensor_p[1] << ", " << _sensor_p[2] << std::endl; std::cout << "_map_p: " << _map_p[0] << ", " << _map_p[1] << ", " << _map_p[2] << std::endl;
std::cout << "_map_o: " << _map_o[0] << std::endl;
} }
Eigen::Matrix<T, 3, 1> sensor_p_base(_sensor_p[0], _sensor_p[1], _sensor_p[2]); //sensor position with respect to the base (the vehicle) //Filling Eigen vectors
Eigen::Matrix<T, 3, 1> vehicle_p_map(_vehicle_p[0], _vehicle_p[1], T(0)); Eigen::Matrix<T, 4, 1> sensor_p_base(_sensor_p[0], _sensor_p[1], _sensor_p[2], T(1)); //sensor position with respect to the base (the vehicle)
Eigen::Matrix<T, 3, 1> map_p(_map_p[0], _map_p[1], _map_p[2]);
/* /*
* Base-to-map transform matrix * Base-to-map transform matrix
*/ */
Eigen::Matrix<T, 3, 3> T_base2map = Eigen::Matrix<T, 3, 3>::Identity(); Eigen::Matrix<T, 4, 4> T_base2map = Eigen::Matrix<T, 4, 4>::Identity();
T_base2map(0, 0) = T(cos(_vehicle_o[0])); T_base2map(0, 0) = T(cos(_vehicle_o[0]));
T_base2map(0, 1) = T(sin(_vehicle_o[0])); T_base2map(0, 1) = T(-sin(_vehicle_o[0]));
T_base2map(1, 0) = T(-sin(_vehicle_o[0])); T_base2map(1, 0) = T(sin(_vehicle_o[0]));
T_base2map(1, 1) = T(cos(_vehicle_o[0])); T_base2map(1, 1) = T(cos(_vehicle_o[0]));
Eigen::Matrix<T, 3, 1> sensor_p_map; // sensor position with respect to map frame (initial frame of the experiment) T_base2map(0, 3) = T(_vehicle_p[0]);
sensor_p_map = T_base2map * sensor_p_base + vehicle_p_map; T_base2map(1, 3) = T(_vehicle_p[1]);
if (verbose_level_ >= 1)
// sensor position with respect to map frame
Eigen::Matrix<T, 4, 1> sensor_p_map = T_base2map * sensor_p_base;
if (verbose_level_ >= 2)
{ {
aux.str(std::string()); aux.str(std::string());
aux << sensor_p_map(0); aux << sensor_p_map(0);
if (aux.str().substr(0, 1) != "[") if (aux.str().substr(0, 1) != "[")
std::cout << "!!! sensor_p_map: " << sensor_p_map[0] << ", " << sensor_p_map[1] << ", " << std::cout << "!!! sensor_p_map: " << sensor_p_map[0] << ", " << sensor_p_map[1] << ", " << sensor_p_map[2] << std::endl;
sensor_p_map[2] << std::endl;
else { else {
std::cout << "!!! sensor_p_map: " << aux.str().substr(1, aux.str().find(" ") - 1) << ", "; std::cout << "!!! sensor_p_map: " << aux.str().substr(1, aux.str().find(" ") - 1) << ", ";
aux.str(std::string()); aux.str(std::string());
...@@ -129,9 +132,9 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c ...@@ -129,9 +132,9 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c
aux.str(std::string()); aux.str(std::string());
} }
} }
/* /*
* _map_p from ecef to lla * _map_p from ECEF to LLA (math from https://microem.ru/files/2012/08/GPS.G1-X-00006.pdf )
* https://microem.ru/files/2012/08/GPS.G1-X-00006.pdf
*/ */
// WGS84 ellipsoid constants // WGS84 ellipsoid constants
T a = T(6378137); // earth's radius T a = T(6378137); // earth's radius
...@@ -145,51 +148,56 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c ...@@ -145,51 +148,56 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c
T th = T(atan2(a * _map_p[2], b * p)); T th = T(atan2(a * _map_p[2], b * p));
T lon = T(atan2(_map_p[1], _map_p[0])); T lon = T(atan2(_map_p[1], _map_p[0]));
T lat = T(atan2((_map_p[2] + ep * ep * b * pow(sin(th), 3)), (p - esq * a * pow(cos(th), 3)))); T lat = T(atan2((_map_p[2] + ep * ep * b * pow(sin(th), 3)), (p - esq * a * pow(cos(th), 3))));
// T N = T(a/( sqrt(T(1)-esq*pow(sin(lat),2)) ));
// T alt = T(p / cos(lat) - N); if (verbose_level_ >= 3)
// mod lat to 0-2pi
while (lon <= T(M_PI)) lon += T(2 * M_PI);
while (lon > T(M_PI)) lon -= T(2 * M_PI);
// correction for altitude near poles left out.
if (verbose_level_ >= 2)
{ {
std::cout << "_map_p: " << _map_p[0] << ", " << _map_p[1] << ", " << _map_p[2] << std::endl; std::cout << "_map_p: " << _map_p[0] << ", " << _map_p[1] << ", " << _map_p[2] << std::endl;
std::cout << "_map_p LLA: " << lat << ", " << lon /*<< ", " << alt*/ << std::endl; std::cout << "_map_p LLA: " << lat << ", " << lon /*<< ", " << alt*/ << std::endl;
std::cout << "_map_p LLA degrees: " << lat * T(180 / M_PI) << ", " << lon * T(180 / M_PI) /*<< ", " << alt*/ << std::endl; std::cout << "_map_p LLA degrees: " << lat * T(180 / M_PI) << ", " << lon * T(180 / M_PI) /*<< ", " << alt*/ << std::endl;
} }
/* /*
* map-to-ECEF transform matrix * map-to-ECEF transform matrix
* made by the product of the next 4 matrixes
*/ */
Eigen::Matrix<T, 3, 3> R1 = Eigen::Matrix<T, 3, 3>::Identity(); Eigen::Matrix<T, 4, 4> T_tran = Eigen::Matrix<T, 4, 4>::Identity();
R1(0, 0) = T(cos(lon)); T_tran(0, 3) = T(_map_p[0]);
R1(0, 1) = T(sin(lon)); T_tran(1, 3) = T(_map_p[1]);
R1(1, 0) = T(-sin(lon)); T_tran(2, 3) = T(_map_p[2]);
R1(1, 1) = T(cos(lon));
Eigen::Matrix<T, 3, 3> R2 = Eigen::Matrix<T, 3, 3>::Identity(); Eigen::Matrix<T, 4, 4> T_lon2ecef = Eigen::Matrix<T, 4, 4>::Identity();
R2(0, 0) = T(cos(lat)); T_lon2ecef(0, 0) = T(cos(lon));
R2(0, 2) = T(sin(lat)); T_lon2ecef(0, 1) = T(-sin(lon));
R2(2, 0) = T(-sin(lat)); T_lon2ecef(1, 0) = T(sin(lon));
R2(2, 2) = T(cos(lat)); T_lon2ecef(1, 1) = T(cos(lon));
Eigen::Matrix<T, 3, 3> R3 = Eigen::Matrix<T, 3, 3>::Zero();
R3(0, 1) = R3(1, 2) = R3(2, 0) = T(1); Eigen::Matrix<T, 4, 4> T_lat2lon = Eigen::Matrix<T, 4, 4>::Identity();
Eigen::Matrix<T, 3, 3> R4 = Eigen::Matrix<T, 3, 3>::Identity(); T_lat2lon(0, 0) = T(cos(lat));
R4(0, 0) = T(cos(_map_o[0])); T_lat2lon(0, 2) = T(-sin(lat));
R4(0, 1) = T(sin(_map_o[0])); T_lat2lon(2, 0) = T(sin(lat));
R4(1, 0) = T(-sin(_map_o[0])); T_lat2lon(2, 2) = T(cos(lat));
R4(1, 1) = T(cos(_map_o[0]));
Eigen::Matrix<T, 3, 3> T_map2ecef = (R4 * R3 * R2 * R1).inverse();
/* Eigen::Matrix<T, 4, 4> T_enu2aux = Eigen::Matrix<T, 4, 4>::Zero();
* result I want to find: sensor position with respect to ecef T_enu2aux(0, 2) = T_enu2aux(1, 0) = T_enu2aux(2, 1) = T_enu2aux(3, 3) = T(1);
*/
Eigen::Matrix<T, 3, 1> sensor_p_ecef; //sensor position with respect to ecef coordinate system Eigen::Matrix<T, 4, 4> T_map2eno = Eigen::Matrix<T, 4, 4>::Identity();
sensor_p_ecef = T_map2ecef * sensor_p_map + map_p; T_map2eno(0, 0) = T(cos(_map_o[0]));
T_map2eno(0, 1) = T(-sin(_map_o[0]));
T_map2eno(1, 0) = T(sin(_map_o[0]));
T_map2eno(1, 1) = T(cos(_map_o[0]));
Eigen::Matrix<T, 4, 4> T_map2ecef = T_tran * T_map2eno * T_enu2aux * T_lat2lon * T_lon2ecef ;
//sensor position with respect to ecef coordinate system
Eigen::Matrix<T, 4, 1> sensor_p_ecef = T_map2ecef * sensor_p_map;
if (verbose_level_ >= 1) if (verbose_level_ >= 1)
{ {
aux.str(std::string()); aux.str(std::string());
aux << sensor_p_ecef(0); aux << sensor_p_ecef(0);
if (aux.str().substr(0, 1) != "[") if (aux.str().substr(0, 1) != "[")
std::cout << "!!! sensor_p_ecef: " << sensor_p_ecef[0] << ", " << sensor_p_ecef[1] << ", " << std::cout << "!!! sensor_p_ecef: " << sensor_p_ecef[0] << ", " << sensor_p_ecef[1] << ", " << sensor_p_ecef[2] << std::endl;
sensor_p_ecef[2] << std::endl;
else { else {
std::cout << "!!! sensor_p_ecef: " << aux.str().substr(1, aux.str().find(" ") - 1) << ", "; std::cout << "!!! sensor_p_ecef: " << aux.str().substr(1, aux.str().find(" ") - 1) << ", ";
aux.str(std::string()); aux.str(std::string());
...@@ -206,18 +214,15 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c ...@@ -206,18 +214,15 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c
*/ */
T square_sum = T(0); T square_sum = T(0);
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
{
square_sum += (sensor_p_ecef[i] - T(sat_position_[i]))*(sensor_p_ecef[i] - T(sat_position_[i])); square_sum += (sensor_p_ecef[i] - T(sat_position_[i]))*(sensor_p_ecef[i] - T(sat_position_[i]));
}
T distance = (square_sum != T(0)) ? sqrt(square_sum) : T(0); T distance = (square_sum != T(0)) ? sqrt(square_sum) : T(0);
// error = (expected measurement) - (actual measurement) // error = (expected measurement) - (actual measurement)
_residual[0] = (distance + _bias[0] * T(LIGHT_SPEED)) - (pseudorange_); _residual[0] = (distance + _bias[0] * T(LIGHT_SPEED)) - (pseudorange_);
if (verbose_level_ >= 2) if (verbose_level_ >= 2)
{
std::cout << "!!! Residual: " << _residual[0] << "\n"; std::cout << "!!! Residual: " << _residual[0] << "\n";
}
// normalizing by the covariance // normalizing by the covariance
_residual[0] = _residual[0] / T(getMeasurementCovariance()(0, 0));//T(sqrt(getMeasurementCovariance()(0, 0))); _residual[0] = _residual[0] / T(getMeasurementCovariance()(0, 0));//T(sqrt(getMeasurementCovariance()(0, 0)));
......
src/examples/CMakeLists.txt
+ 0
− 7
View file @ 821ca5a9
...@@ -96,10 +96,3 @@ IF(faramotics_FOUND) ...@@ -96,10 +96,3 @@ IF(faramotics_FOUND)
ENDIF (laser_scan_utils_FOUND) ENDIF (laser_scan_utils_FOUND)
ENDIF(faramotics_FOUND) ENDIF(faramotics_FOUND)
# Testing GPS stuff
IF(raw_gps_utils_FOUND)
ADD_EXECUTABLE(test_gps_wolf test_gps_wolf.cpp)
TARGET_LINK_LIBRARIES(test_gps_wolf ${raw_gps_utils_LIBRARY} ${PROJECT_NAME})
ENDIF(raw_gps_utils_FOUND)
src/examples/test_gps_wolf.cpp deleted 100644 → 0
+ 0
− 597
View file @ 9740a1ce
This diff is collapsed.
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