Pose6D.h
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License: New BSD
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
00032  */
00033 
00034 #ifndef OCTOMATH_POSE6D_H
00035 #define OCTOMATH_POSE6D_H
00036 
00037 #include "Vector3.h"
00038 #include "Quaternion.h"
00039 
00040 namespace octomath {
00041 
00049   class Pose6D {
00050   public:
00051 
00052     Pose6D();
00053     ~Pose6D();
00054 
00060     Pose6D(const Vector3& trans, const Quaternion& rot);
00061 
00069     Pose6D(float x, float y, float z, double roll, double pitch, double yaw);
00070 
00071     Pose6D& operator= (const Pose6D& other);
00072     bool operator==(const Pose6D& other) const;
00073     bool operator!=(const Pose6D& other) const;
00074 
00080     inline Vector3& trans() { return translation; }
00086     inline Quaternion& rot() { return rotation; }
00092     const Vector3& trans() const { return translation; }
00097     const Quaternion& rot() const { return rotation; }
00098 
00099 
00100     inline float& x() { return translation(0); }
00101     inline float& y() { return translation(1); }
00102     inline float& z() { return translation(2); }
00103     inline const float& x() const { return translation(0); }
00104     inline const float& y() const { return translation(1); }
00105     inline const float& z() const { return translation(2); }
00106 
00107     inline double roll()  const {return (rotation.toEuler())(0); }
00108     inline double pitch() const {return (rotation.toEuler())(1); }
00109     inline double yaw()   const {return (rotation.toEuler())(2); }
00110 
00119     Vector3 transform (const Vector3 &v) const;
00120 
00127     Pose6D inv() const;
00128 
00135     Pose6D& inv_IP();
00136 
00144     Pose6D operator* (const Pose6D &p) const;
00152     const Pose6D& operator*= (const Pose6D &p);
00153 
00159     double distance(const Pose6D &other) const;
00160 
00167     double transLength() const;
00168 
00174     std::ostream& write(std::ostream &s) const;
00180     std::istream& read(std::istream &s);
00186     std::ostream& writeBinary(std::ostream &s) const;
00192     std::istream& readBinary(std::istream &s);
00193 
00194   protected:
00195     Vector3 translation;
00196     Quaternion rotation;
00197   };
00198 
00200   std::ostream& operator<<(std::ostream& s, const Pose6D& p);
00201 
00202 }
00203 
00204 #endif


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Feb 11 2016 23:50:59