GeodeticCoords.h
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
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 Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 /*
00029  * The methods in this file were modified from the originals of the MRPT toolkit (see notice below):
00030  * https://github.com/MRPT/mrpt/blob/master/libs/topography/src/conversions.cpp
00031  */
00032 
00033 /* +---------------------------------------------------------------------------+
00034    |                     Mobile Robot Programming Toolkit (MRPT)               |
00035    |                          http://www.mrpt.org/                             |
00036    |                                                                           |
00037    | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
00038    | See: http://www.mrpt.org/Authors - All rights reserved.                   |
00039    | Released under BSD License. See details in http://www.mrpt.org/License    |
00040    +---------------------------------------------------------------------------+ */
00041 
00042 
00043 #ifndef GEODETICCOORDS_H_
00044 #define GEODETICCOORDS_H_
00045 
00046 #include <rtabmap/core/RtabmapExp.h>
00047 
00048 #include <opencv2/core/core.hpp>
00049 
00050 namespace rtabmap {
00051 
00052 class RTABMAP_EXP GeodeticCoords
00053 {
00054 public:
00055         GeodeticCoords();
00056         GeodeticCoords(double latitude, double longitude, double altitude);
00057 
00058         const double & latitude() const {return latitude_;}
00059         const double & longitude() const {return longitude_;}
00060         const double & altitude() const {return altitude_;}
00061 
00062         void setLatitude(const double & value) {latitude_ = value;}
00063         void setLongitude(const double & value) {longitude_ = value;}
00064         void setAltitude(const double & value) {altitude_ = value;}
00065 
00066         cv::Point3d toGeocentric_WGS84() const;
00067         cv::Point3d toENU_WGS84(const GeodeticCoords & origin) const; // East=X, North=Y
00068 
00069         void fromGeocentric_WGS84(const cv::Point3d& geocentric);
00070         void fromENU_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
00071 
00072         static cv::Point3d ENU_WGS84ToGeocentric_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
00073 
00074 private:
00075         double latitude_;  // deg
00076         double longitude_; // deg
00077         double altitude_;  // m
00078 };
00079 
00080 class GPS
00081 {
00082 public:
00083         GPS():
00084                 stamp_(0.0),
00085                 longitude_(0.0),
00086                 latitude_(0.0),
00087                 altitude_(0.0),
00088                 error_(0.0),
00089                 bearing_(0.0)
00090         {}
00091         GPS(const double & stamp,
00092                 const double & longitude,
00093                 const double & latitude,
00094                 const double & altitude,
00095                 const double & error,
00096                 const double & bearing):
00097                         stamp_(stamp),
00098                         longitude_(longitude),
00099                         latitude_(latitude),
00100                         altitude_(altitude),
00101                         error_(error),
00102                         bearing_(bearing)
00103         {}
00104         const double & stamp() const {return stamp_;}
00105         const double & longitude() const {return longitude_;}
00106         const double & latitude() const {return latitude_;}
00107         const double & altitude() const {return altitude_;}
00108         const double & error() const {return error_;}
00109         const double & bearing() const {return bearing_;}
00110 
00111         GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);}
00112 private:
00113         double stamp_;     // in sec
00114         double longitude_; // DD
00115         double latitude_;  // DD
00116         double altitude_;  // m
00117         double error_;     // m
00118         double bearing_;   // deg (North 0->360 clockwise)
00119 };
00120 
00121 }
00122 
00123 #endif /* GEODETICCOORDS_H_ */


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:20