$search
00001 //================================================================================================= 00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Flight Systems and Automatic Control group, 00013 // TU Darmstadt, nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 //================================================================================================= 00028 00029 #include <hector_pose_estimation/global_reference.h> 00030 #include <cmath> 00031 00032 using namespace std; 00033 00034 namespace hector_pose_estimation { 00035 00036 GlobalReference::GlobalReference() 00037 { 00038 parameters().add("reference_latitude", position_.latitude); 00039 parameters().add("reference_longitude", position_.longitude); 00040 parameters().add("reference_altitude", position_.altitude); 00041 parameters().add("reference_heading", heading_.value); 00042 00043 reset(); 00044 } 00045 00046 void GlobalReference::reset() 00047 { 00048 position_ = Position(); 00049 heading_ = Heading(); 00050 00051 has_position_ = false; 00052 has_heading_ = false; 00053 has_altitude_ = false; 00054 00055 updated(); 00056 } 00057 00058 ParameterList& GlobalReference::parameters() { 00059 return parameters_; 00060 } 00061 00062 void GlobalReference::updated() { 00063 // check if a non-default reference has been set 00064 if (position_.latitude != Position().latitude) has_position_ = true; 00065 if (position_.longitude != Position().longitude) has_position_ = true; 00066 if (position_.altitude != Position().altitude) has_altitude_ = true; 00067 if (heading_.value != Heading().value) has_heading_ = true; 00068 00069 // WGS84 constants 00070 static const double equatorial_radius = 6378137.0; 00071 static const double flattening = 1.0/298.257223563; 00072 static const double excentrity2 = 2*flattening - flattening*flattening; 00073 00074 // calculate earth radii 00075 double temp = 1.0 / (1.0 - excentrity2 * sin(position_.latitude) * sin(position_.latitude)); 00076 double prime_vertical_radius = equatorial_radius * sqrt(temp); 00077 radius_.north = prime_vertical_radius * (1 - excentrity2) * temp; 00078 radius_.east = prime_vertical_radius * cos(position_.latitude); 00079 00080 // calculate sin and cos of the heading reference 00081 sincos(heading_.value, &heading_.sin, &heading_.cos); 00082 } 00083 00084 void GlobalReference::fromWGS84(double latitude, double longitude, double &x, double &y) { 00085 double north = radius_.north * (latitude - position_.latitude); 00086 double east = radius_.east * (longitude - position_.longitude); 00087 fromNorthEast(north, east, x, y); 00088 } 00089 00090 void GlobalReference::toWGS84(double x, double y, double &latitude, double &longitude) { 00091 if (radius_.north == 0.0 || radius_.east == 0.0) { 00092 latitude = 0.0; 00093 longitude = 0.0; 00094 return; 00095 } 00096 00097 double north, east; 00098 toNorthEast(x, y, north, east); 00099 latitude = position_.latitude + north / radius_.north; 00100 longitude = position_.longitude + east / radius_.east; 00101 } 00102 00103 void GlobalReference::fromNorthEast(double north, double east, double &x, double &y) { 00104 x = north * heading_.cos + east * heading_.sin; 00105 y = north * heading_.sin - east * heading_.cos; 00106 } 00107 00108 void GlobalReference::toNorthEast(double x, double y, double &north, double &east) { 00109 north = x * heading_.cos + y * heading_.sin; 00110 east = x * heading_.sin - y * heading_.cos; 00111 } 00112 00113 GlobalReference& GlobalReference::setPosition(double latitude, double longitude) { 00114 position_.latitude = latitude; 00115 position_.longitude = longitude; 00116 has_position_ = true; 00117 updated(); 00118 return *this; 00119 } 00120 00121 GlobalReference& GlobalReference::setHeading(double heading) { 00122 heading_.value = heading; 00123 has_heading_ = true; 00124 updated(); 00125 return *this; 00126 } 00127 00128 GlobalReference& GlobalReference::setAltitude(double altitude) { 00129 position_.altitude = altitude; 00130 has_altitude_ = true; 00131 updated(); 00132 return *this; 00133 } 00134 00135 } // namespace hector_pose_estimation