Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef ODOMETRYEVENT_H_
00029 #define ODOMETRYEVENT_H_
00030
00031 #include "rtabmap/utilite/UEvent.h"
00032 #include "rtabmap/utilite/ULogger.h"
00033 #include "rtabmap/utilite/UMath.h"
00034 #include "rtabmap/core/SensorData.h"
00035 #include "rtabmap/core/OdometryInfo.h"
00036
00037 namespace rtabmap {
00038
00039 class OdometryEvent : public UEvent
00040 {
00041 public:
00042 OdometryEvent()
00043 {
00044 _info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
00045 }
00046 OdometryEvent(
00047 const SensorData & data,
00048 const Transform & pose,
00049 const OdometryInfo & info = OdometryInfo()) :
00050 _data(data),
00051 _pose(pose),
00052 _info(info)
00053 {
00054 if(_info.reg.covariance.empty())
00055 {
00056 _info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
00057 }
00058 UASSERT(_info.reg.covariance.cols == 6 && _info.reg.covariance.rows == 6 && _info.reg.covariance.type() == CV_64FC1);
00059 UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(0,0)) && _info.reg.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
00060 UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(1,1)) && _info.reg.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
00061 UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(2,2)) && _info.reg.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
00062 UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(3,3)) && _info.reg.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
00063 UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(4,4)) && _info.reg.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
00064 UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(5,5)) && _info.reg.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
00065 }
00066 virtual ~OdometryEvent() {}
00067 virtual std::string getClassName() const {return "OdometryEvent";}
00068
00069 SensorData & data() {return _data;}
00070 const SensorData & data() const {return _data;}
00071 const Transform & pose() const {return _pose;}
00072 const cv::Mat & covariance() const {return _info.reg.covariance;}
00073 std::vector<float> velocity() const {
00074 if(_info.interval>0.0)
00075 {
00076 std::vector<float> velocity(6,0);
00077 float x,y,z,roll,pitch,yaw;
00078 _info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
00079 velocity[0] = x/_info.interval;
00080 velocity[1] = y/_info.interval;
00081 velocity[2] = z/_info.interval;
00082 velocity[3] = roll/_info.interval;
00083 velocity[4] = pitch/_info.interval;
00084 velocity[5] = yaw/_info.interval;
00085 return velocity;
00086 }
00087 return std::vector<float>();
00088 }
00089 const OdometryInfo & info() const {return _info;}
00090
00091 private:
00092 SensorData _data;
00093 Transform _pose;
00094 OdometryInfo _info;
00095 };
00096
00097 class OdometryResetEvent : public UEvent
00098 {
00099 public:
00100 OdometryResetEvent(const Transform & pose = Transform::getIdentity()){_pose = pose;}
00101 virtual ~OdometryResetEvent() {}
00102 virtual std::string getClassName() const {return "OdometryResetEvent";}
00103 const Transform & getPose() const {return _pose;}
00104 private:
00105 Transform _pose;
00106 };
00107
00108 }
00109
00110
00111 #endif