Transform.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 #ifndef TRANSFORM_H_
00029 #define TRANSFORM_H_
00030 
00031 #include <rtabmap/core/RtabmapExp.h>
00032 #include <vector>
00033 #include <string>
00034 #include <Eigen/Core>
00035 #include <Eigen/Geometry>
00036 #include <opencv2/core/core.hpp>
00037 
00038 namespace rtabmap {
00039 
00040 class RTABMAP_EXP Transform
00041 {
00042 public:
00043 
00044         // Zero by default
00045         Transform();
00046         // rotation matrix r## and origin o##
00047         Transform(float r11, float r12, float r13, float o14,
00048                       float r21, float r22, float r23, float o24,
00049                           float r31, float r32, float r33, float o34);
00050         // should have 3 rows, 4 cols and type CV_32FC1
00051         Transform(const cv::Mat & transformationMatrix);
00052         // x,y,z, roll,pitch,yaw
00053         Transform(float x, float y, float z, float roll, float pitch, float yaw);
00054         // x,y,z, qx,qy,qz,qw
00055         Transform(float x, float y, float z, float qx, float qy, float qz, float qw);
00056         // x,y, theta
00057         Transform(float x, float y, float theta);
00058 
00059         Transform clone() const;
00060 
00061         float r11() const {return data()[0];}
00062         float r12() const {return data()[1];}
00063         float r13() const {return data()[2];}
00064         float r21() const {return data()[4];}
00065         float r22() const {return data()[5];}
00066         float r23() const {return data()[6];}
00067         float r31() const {return data()[8];}
00068         float r32() const {return data()[9];}
00069         float r33() const {return data()[10];}
00070 
00071         float o14() const {return data()[3];}
00072         float o24() const {return data()[7];}
00073         float o34() const {return data()[11];}
00074 
00075         float & operator[](int index) {return data()[index];}
00076         const float & operator[](int index) const {return data()[index];}
00077         float & operator()(int row, int col) {return data()[row*4 + col];}
00078         const float & operator()(int row, int col) const {return data()[row*4 + col];}
00079 
00080         bool isNull() const;
00081         bool isIdentity() const;
00082 
00083         void setNull();
00084         void setIdentity();
00085 
00086         const cv::Mat & dataMatrix() const {return data_;}
00087         const float * data() const {return (const float *)data_.data;}
00088         float * data() {return (float *)data_.data;}
00089         int size() const {return 12;}
00090 
00091         float & x() {return data()[3];}
00092         float & y() {return data()[7];}
00093         float & z() {return data()[11];}
00094         const float & x() const {return data()[3];}
00095         const float & y() const {return data()[7];}
00096         const float & z() const {return data()[11];}
00097 
00098         float theta() const;
00099 
00100         Transform inverse() const;
00101         Transform rotation() const;
00102         Transform translation() const;
00103         Transform to3DoF() const;
00104 
00105         cv::Mat rotationMatrix() const;
00106         cv::Mat translationMatrix() const;
00107 
00108         void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
00109         void getEulerAngles(float & roll, float & pitch, float & yaw) const;
00110         void getTranslation(float & x, float & y, float & z) const;
00111         float getAngle(float x=1.0f, float y=0.0f, float z=0.0f) const;
00112         float getNorm() const;
00113         float getNormSquared() const;
00114         float getDistance(const Transform & t) const;
00115         float getDistanceSquared(const Transform & t) const;
00116         Transform interpolate(float t, const Transform & other) const;
00117         void normalizeRotation();
00118         std::string prettyPrint() const;
00119 
00120         Transform operator*(const Transform & t) const;
00121         Transform & operator*=(const Transform & t);
00122         bool operator==(const Transform & t) const;
00123         bool operator!=(const Transform & t) const;
00124 
00125         Eigen::Matrix4f toEigen4f() const;
00126         Eigen::Matrix4d toEigen4d() const;
00127         Eigen::Affine3f toEigen3f() const;
00128         Eigen::Affine3d toEigen3d() const;
00129 
00130         Eigen::Quaternionf getQuaternionf() const;
00131         Eigen::Quaterniond getQuaterniond() const;
00132 
00133 public:
00134         static Transform getIdentity();
00135         static Transform fromEigen4f(const Eigen::Matrix4f & matrix);
00136         static Transform fromEigen4d(const Eigen::Matrix4d & matrix);
00137         static Transform fromEigen3f(const Eigen::Affine3f & matrix);
00138         static Transform fromEigen3d(const Eigen::Affine3d & matrix);
00139         static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
00140         static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
00141 
00149         static Transform fromString(const std::string & string);
00150         static bool canParseString(const std::string & string);
00151 
00152 private:
00153         cv::Mat data_;
00154 };
00155 
00156 RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const Transform& s);
00157 
00158 }
00159 
00160 #endif /* TRANSFORM_H_ */


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