From 977ebed8d12b15c29628102b8e4ebbed9cceb05d Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Thu, 14 Jul 2016 13:33:56 +0200 Subject: [PATCH 01/50] Split Angle.hpp into implementation and header --- src/Angle.cpp | 171 ++++++++++++++++++++++++++++++++++++ {base => src}/Angle.hpp | 190 +++------------------------------------- src/CMakeLists.txt | 2 +- 3 files changed, 185 insertions(+), 178 deletions(-) create mode 100644 src/Angle.cpp rename {base => src}/Angle.hpp (58%) diff --git a/src/Angle.cpp b/src/Angle.cpp new file mode 100644 index 00000000..947cd5cc --- /dev/null +++ b/src/Angle.cpp @@ -0,0 +1,171 @@ +#include "Angle.hpp" +#include +#include + +base::Angle base::Angle::vectorToVector(const base::Vector3d& a, const base::Vector3d& b) +{ + double dot = a.dot(b); + double norm = a.norm() * b.norm(); + return fromRad(acos(dot / norm)); +} + +base::Angle base::Angle::vectorToVector(const base::Vector3d& a, const base::Vector3d& b, const base::Vector3d& positive) +{ + double cos = a.dot(b) / (a.norm() * b.norm()); + + bool is_positive = (a.cross(b).dot(positive) > 0); + if (is_positive) + return fromRad(acos(cos)); + else + return fromRad(-acos(cos)); +} + +std::ostream& operator << (std::ostream& os, base::Angle angle) +{ + os << angle.getRad() << boost::format("[%3.1fdeg]") % angle.getDeg(); + return os; +} + +base::AngleSegment::AngleSegment(): width(0), startRad(0), endRad(0) +{ +} + +base::AngleSegment::AngleSegment(const Angle &start, double _width): width(_width), startRad(start.getRad()), endRad(startRad + width) +{ + if(width < 0) + throw std::runtime_error("Error got segment with negative width"); +} + +bool base::AngleSegment::isInside(const base::Angle& angle) const +{ + double angleRad = angle.getRad(); + if(angleRad < startRad) + angleRad += 2*M_PI; + + if(angleRad <= endRad) //startRad <= angleRad && + return true; + + return false; +} + +bool base::AngleSegment::isInside(const base::AngleSegment& segment) const +{ + double otherStart = segment.startRad; + if(otherStart < startRad) + otherStart += 2*M_PI; + + double otherEnd = otherStart + segment.width; + + if(otherEnd <= endRad) + return true; + + return false; +} + +std::vector< base::AngleSegment > base::AngleSegment::getIntersections(const base::AngleSegment& b) const +{ + std::vector ret; + //special case, this segment is a whole circle + if(width >= 2*M_PI) + { + ret.push_back(b); + return ret; + } + + //special case, other segment is a whole circle + if(b.width >= 2*M_PI) + { + ret.push_back(*this); + return ret; + } + + double startA = startRad; + double startB = b.startRad; + double widthA = width; + double widthB = b.width; + + //make A the smaller angle + if(startA > startB) + { + std::swap(startA, startB); + std::swap(widthA, widthB); + } + double endA = startA + widthA; + double endB = startB + widthB; + + //test if segemnts do not intersect at all + if(endA < startB) + { + //wrap case + if(endB > M_PI) + { + //check if segments intersect after wrap correction + if(startA < endB - 2*M_PI) + { + //this means the start of A is inside of B + //drop first part of B and realign it to -M_PI + //also switch A and B as B is now the 'lower' one + double newWidthA = widthB - (M_PI - startB); + startB = startA; + widthB = widthA; + startA = - M_PI; + widthA = newWidthA; + endA = startA + widthA; + endB = startB + widthB; + //no return, still need + //to check for intersection + } + else + //no intersection + return ret; + } else + //no intersection + return ret; + } + + //normal case, no wrap around + double newStart = startB; + double newEnd = 0; + + if(endA < endB) + { + newEnd = endA; + } + else + { + newEnd = endB; + } + + double newWidth = newEnd - newStart; + + //filter invalid segments + if(newWidth > 1e-10) + ret.push_back(AngleSegment(Angle::fromRad(newStart), newWidth)); + + newStart = endB - 2*M_PI; + if(newStart > startA) + { + newWidth = newStart - startA; + //filter invalid segments + if(newWidth > 1e-10) + ret.push_back(AngleSegment(Angle::fromRad(startA), newWidth)); + } + + return ret; +} + +base::Angle base::AngleSegment::getStart() const +{ + return base::Angle::fromRad(startRad); +} + +base::Angle base::AngleSegment::getEnd() const +{ + return base::Angle::fromRad(endRad); +} + +std::ostream& operator << (std::ostream& os, base::AngleSegment seg) +{ + os << " Segmend start " << seg.startRad/M_PI *180.0 << " end " << seg.endRad/M_PI * 180.0 << " width " << seg.width /M_PI * 180.0; + return os; +} diff --git a/base/Angle.hpp b/src/Angle.hpp similarity index 58% rename from base/Angle.hpp rename to src/Angle.hpp index 158aef5e..1edbef8b 100644 --- a/base/Angle.hpp +++ b/src/Angle.hpp @@ -1,12 +1,10 @@ #ifndef __BASE_ANGLE_HH__ #define __BASE_ANGLE_HH__ -#include #include -#include #include -#include #include +#include namespace base { @@ -203,23 +201,6 @@ class Angle return Angle::fromRad( getRad() * val ); } -BASE_TYPES_DEPRECATED_SUPPRESS_START - /** - * @deprecated this function does not work is right_limit == left_limit. - * From the API one can't differenciate between small interval - * or a full cycle. Use base::AngleSegment instead - * @return true if the angle is insight the given interval - */ - bool isInRange(const Angle& left_limit, const Angle& right_limit) const BASE_TYPES_DEPRECATED - { - if((right_limit-left_limit).rad < 0) - return !isInRange(right_limit,left_limit); - if((*this -left_limit).getRad() >= 0 && (right_limit -*this).getRad() >= 0) - return true; - return false; - } -BASE_TYPES_DEPRECATED_SUPPRESS_STOP - /** * Returns a new angle which is the inverse of tis object. * */ @@ -242,12 +223,8 @@ BASE_TYPES_DEPRECATED_SUPPRESS_STOP /** Computes the unsigned angle of the rotation that makes +a+ colinear with +b+ */ - static Angle vectorToVector(const base::Vector3d& a, const base::Vector3d& b) - { - double dot = a.dot(b); - double norm = a.norm() * b.norm(); - return fromRad(acos(dot / norm)); - } + static Angle vectorToVector(const base::Vector3d& a, const base::Vector3d& b); + /** Computes the angle of the rotation that makes +a+ colinear with +b+ * @@ -255,16 +232,7 @@ BASE_TYPES_DEPRECATED_SUPPRESS_STOP * @c positive vector defining the positive rotation direction. @c positive * is required to be of unit length. */ - static Angle vectorToVector(const base::Vector3d& a, const base::Vector3d& b, const base::Vector3d& positive) - { - double cos = a.dot(b) / (a.norm() * b.norm()); - - bool is_positive = (a.cross(b).dot(positive) > 0); - if (is_positive) - return fromRad(acos(cos)); - else - return fromRad(-acos(cos)); - } + static Angle vectorToVector(const base::Vector3d& a, const base::Vector3d& b, const base::Vector3d& positive); }; static inline Angle operator*( double a, Angle b ) @@ -272,11 +240,7 @@ static inline Angle operator*( double a, Angle b ) return Angle::fromRad( a * b.getRad() ); } -static inline std::ostream& operator << (std::ostream& os, Angle angle) -{ - os << angle.getRad() << boost::format("[%3.1fdeg]") % angle.getDeg(); - return os; -} +std::ostream& operator << (std::ostream& os, Angle angle); /** * This class represents a Segment of a Circle. @@ -286,51 +250,23 @@ static inline std::ostream& operator << (std::ostream& os, Angle angle) class AngleSegment { public: - AngleSegment(): width(0), startRad(0), endRad(0) - { - } + AngleSegment(); - AngleSegment(const Angle &start, double _width): width(_width), startRad(start.getRad()), endRad(startRad + width) - { - if(width < 0) - throw std::runtime_error("Error got segment with negative width"); - } + AngleSegment(const Angle &start, double _width); /** * Tests if the given angle is inside of the segment. * @param angle - angle to be tested * @return true if angle is inside the segment * */ - bool isInside(const Angle &angle) const - { - double angleRad = angle.getRad(); - if(angleRad < startRad) - angleRad += 2*M_PI; - - if(angleRad <= endRad) //startRad <= angleRad && - return true; - - return false; - }; + bool isInside(const Angle &angle) const; /** * Tests if the given segment is inside of this segment. * @param segment - segment to be tested * @return true if the given segment is inside of this segment * */ - bool isInside(const AngleSegment &segment) const - { - double otherStart = segment.startRad; - if(otherStart < startRad) - otherStart += 2*M_PI; - - double otherEnd = otherStart + segment.width; - - if(otherEnd <= endRad) - return true; - - return false; - }; + bool isInside(const AngleSegment &segment) const; bool split(const Angle &angle, AngleSegment &rest) { @@ -347,97 +283,7 @@ class AngleSegment * @param b The segment wich schould be tested * @return A vector, containing a segment for each intersecting part of the segments. * */ - std::vector getIntersections(const AngleSegment &b) const - { - std::vector ret; - //special case, this segment is a whole circle - if(width >= 2*M_PI) - { - ret.push_back(b); - return ret; - } - - //special case, other segment is a whole circle - if(b.width >= 2*M_PI) - { - ret.push_back(*this); - return ret; - } - - double startA = startRad; - double startB = b.startRad; - double widthA = width; - double widthB = b.width; - - //make A the smaller angle - if(startA > startB) - { - std::swap(startA, startB); - std::swap(widthA, widthB); - } - double endA = startA + widthA; - double endB = startB + widthB; - - //test if segemnts do not intersect at all - if(endA < startB) - { - //wrap case - if(endB > M_PI) - { - //check if segments intersect after wrap correction - if(startA < endB - 2*M_PI) - { - //this means the start of A is inside of B - //drop first part of B and realign it to -M_PI - //also switch A and B as B is now the 'lower' one - double newWidthA = widthB - (M_PI - startB); - startB = startA; - widthB = widthA; - startA = - M_PI; - widthA = newWidthA; - endA = startA + widthA; - endB = startB + widthB; - //no return, still need - //to check for intersection - } - else - //no intersection - return ret; - } else - //no intersection - return ret; - } - - //normal case, no wrap around - double newStart = startB; - double newEnd = 0; - - if(endA < endB) - { - newEnd = endA; - } - else - { - newEnd = endB; - } - - double newWidth = newEnd - newStart; - - //filter invalid segments - if(newWidth > 1e-10) - ret.push_back(AngleSegment(Angle::fromRad(newStart), newWidth)); - - newStart = endB - 2*M_PI; - if(newStart > startA) - { - newWidth = newStart - startA; - //filter invalid segments - if(newWidth > 1e-10) - ret.push_back(AngleSegment(Angle::fromRad(startA), newWidth)); - } - - return ret; - } + std::vector getIntersections(const AngleSegment &b) const; /** * Returns the width of the segment in radians @@ -452,10 +298,7 @@ class AngleSegment * Returns the start angle of the segement * @return the start angle of the segement * */ - base::Angle getStart() const - { - return base::Angle::fromRad(startRad); - } + base::Angle getStart() const; /** * Returns the end angle of the segement @@ -465,10 +308,7 @@ class AngleSegment * getStart + getWidth; * @return the end angle of the segement * */ - base::Angle getEnd() const - { - return base::Angle::fromRad(endRad); - } + base::Angle getEnd() const; /** * Widht of the segment in radians @@ -486,11 +326,7 @@ class AngleSegment double endRad; }; -static inline std::ostream& operator << (std::ostream& os, AngleSegment seg) -{ - os << " Segmend start " << seg.startRad/M_PI *180.0 << " end " << seg.endRad/M_PI * 180.0 << " width " << seg.width /M_PI * 180.0; - return os; -} +std::ostream& operator << (std::ostream& os, AngleSegment seg); } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 408508cf..2f2031a9 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,4 +1,4 @@ -set(SOURCES ) +set(SOURCES Angle.cpp) set(HEADERS Logging.hpp Singleton.hpp logging/logging_iostream_style.h logging/logging_printf_style.h) From 23b22174c10e690f948473bd13878ce1f53283aa Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Thu, 14 Jul 2016 13:34:12 +0200 Subject: [PATCH 02/50] Split Pose.hpp into implementation and header --- src/CMakeLists.txt | 2 +- src/Pose.cpp | 165 +++++++++++++++++++++++++++++++++++++++++ {base => src}/Pose.hpp | 153 ++++++-------------------------------- 3 files changed, 190 insertions(+), 130 deletions(-) create mode 100644 src/Pose.cpp rename {base => src}/Pose.hpp (53%) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 2f2031a9..c3311254 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,4 +1,4 @@ -set(SOURCES Angle.cpp) +set(SOURCES Angle.cpp Pose.cpp) set(HEADERS Logging.hpp Singleton.hpp logging/logging_iostream_style.h logging/logging_printf_style.h) diff --git a/src/Pose.cpp b/src/Pose.cpp new file mode 100644 index 00000000..f8fca1db --- /dev/null +++ b/src/Pose.cpp @@ -0,0 +1,165 @@ +#include "Pose.hpp" + +namespace base +{ + +base::Vector3d getEuler(const base::Orientation& orientation) +{ + const Eigen::Matrix3d m = orientation.toRotationMatrix(); + double x = base::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm(); + base::Vector3d res(0,::atan2(-m.coeff(2,0), x),0); + if (x > Eigen::NumTraits::dummy_precision()){ + res[0] = ::atan2(m.coeff(1,0), m.coeff(0,0)); + res[2] = ::atan2(m.coeff(2,1), m.coeff(2,2)); + }else{ + res[0] = 0; + res[2] = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1)); + } + return res; +} + +base::Vector3d getEuler(const base::AngleAxisd& orientation) +{ + const Eigen::Matrix3d m = orientation.toRotationMatrix(); + double x = base::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm(); + base::Vector3d res(0,::atan2(-m.coeff(2,0), x),0); + if (x > Eigen::NumTraits::dummy_precision()){ + res[0] = ::atan2(m.coeff(1,0), m.coeff(0,0)); + res[2] = ::atan2(m.coeff(2,1), m.coeff(2,2)); + }else{ + res[0] = 0; + res[2] = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1)); + } + return res; +} + +double getYaw(const base::Orientation& orientation) +{ + return base::getEuler(orientation)[0]; +} + +double getYaw(const base::AngleAxisd& orientation) +{ + return base::getEuler(orientation)[0]; +} + +double getPitch(const base::Orientation& orientation) +{ + return base::getEuler(orientation)[1]; +} + +double getPitch(const base::AngleAxisd& orientation) +{ + return base::getEuler(orientation)[1]; +} + +double getRoll(const base::Orientation& orientation) +{ + return base::getEuler(orientation)[2]; +} + +double getRoll(const base::AngleAxisd& orientation) +{ + return base::getEuler(orientation)[2]; +} + +base::Orientation removeYaw(const base::Orientation& orientation) +{ + return Eigen::AngleAxisd( -getYaw(orientation), Eigen::Vector3d::UnitZ()) * orientation; +} + +base::Orientation removeYaw(const base::AngleAxisd& orientation) +{ + return Eigen::AngleAxisd( -getYaw(orientation), Eigen::Vector3d::UnitZ()) * orientation; +} + +base::Orientation removePitch(const base::Orientation& orientation) +{ + return Eigen::AngleAxisd( -getPitch(orientation), Eigen::Vector3d::UnitY()) * orientation; +} + +base::Orientation removePitch(const base::AngleAxisd& orientation) +{ + return Eigen::AngleAxisd( -getPitch(orientation), Eigen::Vector3d::UnitY()) * orientation; +} + +base::Orientation removeRoll(const base::Orientation& orientation) +{ + return Eigen::AngleAxisd( -getRoll(orientation), Eigen::Vector3d::UnitX()) * orientation; +} + +base::Orientation removeRoll(const base::AngleAxisd& orientation) +{ + return Eigen::AngleAxisd( -getRoll(orientation), Eigen::Vector3d::UnitX()) * orientation; +} + + +PoseUpdateThreshold::PoseUpdateThreshold() +{ +} + +PoseUpdateThreshold::PoseUpdateThreshold(double _distance, double _angle): distance( _distance ), angle( _angle ) +{ +} + +bool PoseUpdateThreshold::test( double other_distance, double other_angle ) +{ + return other_distance > distance || other_angle > angle; +} + +bool PoseUpdateThreshold::test( const Eigen::Affine3d& pdelta ) +{ + return test( pdelta.translation().norm(), Eigen::AngleAxisd( pdelta.linear() ).angle() ); +} + +bool PoseUpdateThreshold::test(const Eigen::Affine3d& a2b, const Eigen::Affine3d& aprime2b) +{ + return test( a2b.inverse() * aprime2b ); +} + + +void Pose::fromVector6d(const Vector6d& v) +{ + const Eigen::Vector3d saxis = v.head<3>(); + if( saxis.norm() > 1e-9 ) + orientation = Eigen::AngleAxisd( saxis.norm(), saxis.normalized() ); + else + orientation = Eigen::Quaterniond::Identity(); + + position = v.tail<3>(); +} + +Vector6d Pose::toVector6d() const +{ + Vector6d res; + Eigen::AngleAxisd aa(orientation); + res.head<3>() = aa.axis() * aa.angle(); + res.tail<3>() = position; + + return res; +} + +std::ostream& operator << (std::ostream& io, base::Pose const& pose) +{ + io << "Position " + << pose.position.transpose() + << " Orientation (RPY)" + << getRoll(pose.orientation) << " " + << getPitch(pose.orientation) << " " + << getYaw(pose.orientation); + return io; +} + +std::ostream& operator << (std::ostream& io, base::Pose2D const& pose) +{ + + io << "Position " + << pose.position.transpose() + << " Orientation (Theta) " + << pose.orientation ; + return io; +} + + + +} \ No newline at end of file diff --git a/base/Pose.hpp b/src/Pose.hpp similarity index 53% rename from base/Pose.hpp rename to src/Pose.hpp index 0551e357..0fd425a2 100644 --- a/base/Pose.hpp +++ b/src/Pose.hpp @@ -25,19 +25,7 @@ namespace base * assuming angles in range of: a0:(-pi,pi), a1:(-pi/2,pi/2), a2:(-pi/2,pi/2) * */ - static base::Vector3d getEuler(const base::Orientation &orientation){ - const Eigen::Matrix3d m = orientation.toRotationMatrix(); - double x = base::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm(); - base::Vector3d res(0,::atan2(-m.coeff(2,0), x),0); - if (x > Eigen::NumTraits::dummy_precision()){ - res[0] = ::atan2(m.coeff(1,0), m.coeff(0,0)); - res[2] = ::atan2(m.coeff(2,1), m.coeff(2,2)); - }else{ - res[0] = 0; - res[2] = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1)); - } - return res; - } + base::Vector3d getEuler(const base::Orientation &orientation); /* * Decomposes the orientation in euler angles so that this can be @@ -50,79 +38,32 @@ namespace base * assuming angles in range of: a0:(-pi,pi), a1:(-pi/2,pi/2), a2:(-pi/2,pi/2) * */ - static base::Vector3d getEuler(const base::AngleAxisd &orientation){ - const Eigen::Matrix3d m = orientation.toRotationMatrix(); - double x = base::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm(); - base::Vector3d res(0,::atan2(-m.coeff(2,0), x),0); - if (x > Eigen::NumTraits::dummy_precision()){ - res[0] = ::atan2(m.coeff(1,0), m.coeff(0,0)); - res[2] = ::atan2(m.coeff(2,1), m.coeff(2,2)); - }else{ - res[0] = 0; - res[2] = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1)); - } - return res; - } + base::Vector3d getEuler(const base::AngleAxisd &orientation); - static double getYaw(const base::Orientation& orientation) - { - return base::getEuler(orientation)[0]; - } + double getYaw(const base::Orientation& orientation); + - static double getYaw(const base::AngleAxisd& orientation) - { - return base::getEuler(orientation)[0]; - } + double getYaw(const base::AngleAxisd& orientation); - static double getPitch(const base::Orientation& orientation) - { - return base::getEuler(orientation)[1]; - } + double getPitch(const base::Orientation& orientation); - static double getPitch(const base::AngleAxisd& orientation) - { - return base::getEuler(orientation)[1]; - } + double getPitch(const base::AngleAxisd& orientation); - static double getRoll(const base::Orientation& orientation) - { - return base::getEuler(orientation)[2]; - } + double getRoll(const base::Orientation& orientation); - static double getRoll(const base::AngleAxisd& orientation) - { - return base::getEuler(orientation)[2]; - } + double getRoll(const base::AngleAxisd& orientation); - static inline base::Orientation removeYaw(const base::Orientation& orientation) - { - return Eigen::AngleAxisd( -getYaw(orientation), Eigen::Vector3d::UnitZ()) * orientation; - } + base::Orientation removeYaw(const base::Orientation& orientation); - static inline base::Orientation removeYaw(const base::AngleAxisd& orientation) - { - return Eigen::AngleAxisd( -getYaw(orientation), Eigen::Vector3d::UnitZ()) * orientation; - } + base::Orientation removeYaw(const base::AngleAxisd& orientation); - static inline base::Orientation removePitch(const base::Orientation& orientation) - { - return Eigen::AngleAxisd( -getPitch(orientation), Eigen::Vector3d::UnitY()) * orientation; - } + base::Orientation removePitch(const base::Orientation& orientation); - static inline base::Orientation removePitch(const base::AngleAxisd& orientation) - { - return Eigen::AngleAxisd( -getPitch(orientation), Eigen::Vector3d::UnitY()) * orientation; - } + base::Orientation removePitch(const base::AngleAxisd& orientation); - static inline base::Orientation removeRoll(const base::Orientation& orientation) - { - return Eigen::AngleAxisd( -getRoll(orientation), Eigen::Vector3d::UnitX()) * orientation; - } + base::Orientation removeRoll(const base::Orientation& orientation); - static inline base::Orientation removeRoll(const base::AngleAxisd& orientation) - { - return Eigen::AngleAxisd( -getRoll(orientation), Eigen::Vector3d::UnitX()) * orientation; - } + base::Orientation removeRoll(const base::AngleAxisd& orientation); /** * Represents a pose update threshold, with a number of test methods to see @@ -130,31 +71,24 @@ namespace base */ struct PoseUpdateThreshold { - PoseUpdateThreshold() {}; + PoseUpdateThreshold(); /** * Constructor with distance and angle thresholds */ - PoseUpdateThreshold( double _distance, double _angle ) - : distance( _distance ), angle( _angle ) {}; + PoseUpdateThreshold( double _distance, double _angle ); /** * Test if distance or angle is greater than the * stored threshold. */ - bool test( double other_distance, double other_angle ) - { - return other_distance > distance || other_angle > angle; - } + bool test( double other_distance, double other_angle ); /** * Test if the provided delta transformation is greater in * either distance or angle than the threshold */ - bool test( const Eigen::Affine3d& pdelta ) - { - return test( pdelta.translation().norm(), Eigen::AngleAxisd( pdelta.linear() ).angle() ); - } + bool test( const Eigen::Affine3d& pdelta ); /** * Test if the delta of the provided transformations is greater in @@ -165,10 +99,7 @@ namespace base * * @result true if the transformation A' to A is greater than the stored thresholds */ - bool test( const Eigen::Affine3d& a2b, const Eigen::Affine3d& aprime2b ) - { - return test( a2b.inverse() * aprime2b ); - } + bool test( const Eigen::Affine3d& a2b, const Eigen::Affine3d& aprime2b ); double distance; double angle; @@ -243,16 +174,7 @@ namespace base * @param v compact 6 vector [r t], where r is a 3 vector representing * the rotation in scaled axis form, and t is the translation 3 vector. */ - void fromVector6d( const Vector6d &v ) - { - const Eigen::Vector3d saxis = v.head<3>(); - if( saxis.norm() > 1e-9 ) - orientation = Eigen::AngleAxisd( saxis.norm(), saxis.normalized() ); - else - orientation = Eigen::Quaterniond::Identity(); - - position = v.tail<3>(); - } + void fromVector6d( const Vector6d &v ); /** * @brief get compact scaled-axis representation of pose @@ -260,15 +182,7 @@ namespace base * @result compact 6 vector [r t], where r is a 3 vector representing * the rotation in scaled axis form, and t is the translation 3 vector. */ - Vector6d toVector6d() const - { - Vector6d res; - Eigen::AngleAxisd aa(orientation); - res.head<3>() = aa.axis() * aa.angle(); - res.tail<3>() = position; - - return res; - } + Vector6d toVector6d() const; /** * @result yaw (rotation around z-axis) part of the rotational part of the pose @@ -279,17 +193,7 @@ namespace base } }; - inline std::ostream& operator << (std::ostream& io, base::Pose const& pose) - { - io << "Position " - << pose.position.transpose() - << " Orientation (RPY)" - << getRoll(pose.orientation) << " " - << getPitch(pose.orientation) << " " - << getYaw(pose.orientation); -; - return io; - } + std::ostream& operator << (std::ostream& io, base::Pose const& pose); /** * Representation for a pose in 2D @@ -319,16 +223,7 @@ namespace base }; - inline std::ostream& operator << (std::ostream& io, base::Pose2D const& pose) - { - - io << "Position " - << pose.position.transpose() - << " Orientation (Theta) " - << pose.orientation ; -; - return io; - } + std::ostream& operator << (std::ostream& io, base::Pose2D const& pose); } #endif From 204bb8de60c91a253a027eff013b3f4d9a98a4c0 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Thu, 14 Jul 2016 13:39:09 +0200 Subject: [PATCH 03/50] Float.hpp: Use std function, not boost ones --- {base => src}/Float.hpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) rename {base => src}/Float.hpp (50%) diff --git a/base/Float.hpp b/src/Float.hpp similarity index 50% rename from base/Float.hpp rename to src/Float.hpp index fe657902..61ef6b25 100644 --- a/base/Float.hpp +++ b/src/Float.hpp @@ -1,17 +1,18 @@ #ifndef BASE_FLOAT_H #define BASE_FLOAT_H -#include +#include +#include namespace base { template T NaN() { return std::numeric_limits::quiet_NaN(); } - template bool isNaN(T value) { return boost::math::isnan(value); } + template bool isNaN(T value) { return std::isnan(value); } template T unset() { return std::numeric_limits::quiet_NaN(); } - template bool isUnset(T value) { return boost::math::isnan(value); } + template bool isUnset(T value) { return std::isnan(value); } template T unknown() { return std::numeric_limits::quiet_NaN(); } - template bool isUnknown(T value) { return boost::math::isnan(value); } + template bool isUnknown(T value) { return std::isnan(value); } template T infinity() { return std::numeric_limits::infinity(); } - template bool isInfinity(T value) { return boost::math::isinf(value); } + template bool isInfinity(T value) { return std::isinf(value); } } #endif From 340d78e77c8567a3b90279fd9aef8d379985fc9b Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Thu, 14 Jul 2016 13:49:09 +0200 Subject: [PATCH 04/50] Eigen: remove isnotnan and isfinite This is a duplicate of the buildin allFinite() --- base/TwistWithCovariance.hpp | 4 ++-- {base => src}/Eigen.hpp | 0 2 files changed, 2 insertions(+), 2 deletions(-) rename {base => src}/Eigen.hpp (100%) diff --git a/base/TwistWithCovariance.hpp b/base/TwistWithCovariance.hpp index c1c1aba3..4fe618f7 100644 --- a/base/TwistWithCovariance.hpp +++ b/base/TwistWithCovariance.hpp @@ -86,7 +86,7 @@ namespace base { /** Check Methods **/ bool hasValidVelocity() const { - return base::isnotnan(this->vel) && base::isnotnan(this->rot); + return this->vel.allFinite() && this->rot.allFinite(); } void invalidateVelocity() @@ -95,7 +95,7 @@ namespace base { this->rot = base::Vector3d::Ones() * base::unknown(); } - bool hasValidCovariance() const { return base::isnotnan(this->cov); } + bool hasValidCovariance() const { return this->cov.allFinite(); } void invalidateCovariance() { this->cov = Covariance::Ones() * base::unknown(); diff --git a/base/Eigen.hpp b/src/Eigen.hpp similarity index 100% rename from base/Eigen.hpp rename to src/Eigen.hpp From c628b933cb48e9de7db7aa4846b315c4824744c2 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Thu, 14 Jul 2016 14:11:07 +0200 Subject: [PATCH 05/50] move guaranteeSPD into TwistWithCovariance. This one includes SVD, which is a big include in the base eigen. --- base/TwistWithCovariance.hpp | 26 ++++++++++++++++++++++++++ src/Eigen.hpp | 2 ++ 2 files changed, 28 insertions(+) diff --git a/base/TwistWithCovariance.hpp b/base/TwistWithCovariance.hpp index 4fe618f7..9bb1679f 100644 --- a/base/TwistWithCovariance.hpp +++ b/base/TwistWithCovariance.hpp @@ -13,6 +13,32 @@ namespace base { + // Guarantee Semi-Positive Definite (SPD) matrix. + template + static _MatrixType guaranteeSPD (const _MatrixType &A) + { + _MatrixType spdA; + Eigen::VectorXd s; + s.resize(A.rows(), 1); + + /** + * Single Value Decomposition + */ + Eigen::JacobiSVD svdOfA (A, Eigen::ComputeThinU | Eigen::ComputeThinV); + + s = svdOfA.singularValues(); //!eigenvalues + + for (register int i=0; i Date: Thu, 14 Jul 2016 14:23:46 +0200 Subject: [PATCH 06/50] moved Wrench.hpp to new src --- {base => src}/TwistWithCovariance.hpp | 0 {base => src}/Wrench.hpp | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename {base => src}/TwistWithCovariance.hpp (100%) rename {base => src}/Wrench.hpp (100%) diff --git a/base/TwistWithCovariance.hpp b/src/TwistWithCovariance.hpp similarity index 100% rename from base/TwistWithCovariance.hpp rename to src/TwistWithCovariance.hpp diff --git a/base/Wrench.hpp b/src/Wrench.hpp similarity index 100% rename from base/Wrench.hpp rename to src/Wrench.hpp From 1623fe513fb7b32672ffce6ccc85f0dbe79d5760 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 14 Jul 2016 14:24:11 +0200 Subject: [PATCH 07/50] moved TwistWithCovariance.hpp to new lib/src --- src/TwistWithCovariance.cpp | 333 ++++++++++++++++++++++++++++++++++++ src/TwistWithCovariance.hpp | 253 +++++---------------------- 2 files changed, 377 insertions(+), 209 deletions(-) create mode 100644 src/TwistWithCovariance.cpp diff --git a/src/TwistWithCovariance.cpp b/src/TwistWithCovariance.cpp new file mode 100644 index 00000000..ba2b2b64 --- /dev/null +++ b/src/TwistWithCovariance.cpp @@ -0,0 +1,333 @@ +#include "TwistWithCovariance.hpp" + +typedef base::TwistWithCovariance::Covariance Covariance; + +base::TwistWithCovariance::TwistWithCovariance(const base::Vector3d& vel, const base::Vector3d& rot) + : vel(vel), rot(rot) +{ + this->invalidateCovariance(); +} + +base::TwistWithCovariance::TwistWithCovariance(const base::Vector3d& vel, const base::Vector3d& rot, const base::TwistWithCovariance::Covariance& cov) + : vel(vel), rot(rot), cov(cov) +{ + +} + +base::TwistWithCovariance::TwistWithCovariance(const base::Vector6d& velocity, const base::TwistWithCovariance::Covariance& cov) +{ + this->setVelocity(velocity); + this->cov = cov; +} + +const base::Vector3d& base::TwistWithCovariance::getTranslation() const +{ + return this->vel; +} + +void base::TwistWithCovariance::setTranslation(const base::Vector3d& vel) +{ + this->vel = vel; +} + +const base::Vector3d& base::TwistWithCovariance::getRotation() const +{ + return this->rot; +} + +void base::TwistWithCovariance::setRotation(const base::Vector3d& rot) +{ + this->rot = rot; +} + +const base::TwistWithCovariance::Covariance& base::TwistWithCovariance::getCovariance() const +{ + return this->cov; +} + +void base::TwistWithCovariance::setCovariance(const base::TwistWithCovariance::Covariance& cov) +{ + this->cov = cov; +} + +const base::Matrix3d base::TwistWithCovariance::getLinearVelocityCov() const +{ + return this->cov.block<3,3>(0,0); +} + +void base::TwistWithCovariance::setLinearVelocityCov(const base::Matrix3d& cov) +{ + this->cov.block<3,3>(0,0) = cov; +} + +const base::Matrix3d base::TwistWithCovariance::getAngularVelocityCov() const +{ + return this->cov.block<3,3>(3,3); +} + +void base::TwistWithCovariance::setAngularVelocityCov(const base::Matrix3d& cov) +{ + this->cov.block<3,3>(3,3) = cov; +} + +const base::Vector3d& base::TwistWithCovariance::getLinearVelocity() const +{ + return this->getTranslation(); +} + +void base::TwistWithCovariance::setLinearVelocity(const base::Vector3d& vel) +{ + return this->setTranslation(vel); +} + +const base::Vector3d& base::TwistWithCovariance::getAngularVelocity() const +{ + return this->getRotation(); +} + +void base::TwistWithCovariance::setAngularVelocity(const base::Vector3d& rot) +{ + return this->setRotation(rot); +} + +const base::Vector3d& base::TwistWithCovariance::translation() const +{ + return this->getTranslation(); +} + +const base::Vector3d& base::TwistWithCovariance::rotation() const +{ + return this->getRotation(); +} + +const base::Vector6d base::TwistWithCovariance::getVelocity() const +{ + base::Vector6d all_velocities; + all_velocities.block<3,1>(0,0) = this->vel; + all_velocities.block<3,1>(3,0) = this->rot; + return all_velocities; +} + +void base::TwistWithCovariance::setVelocity(const base::Vector6d& velocity) +{ + /** Linear velocity at first place, Angular velocity at second place **/ + this->vel = velocity.block<3,1>(0,0); + this->rot = velocity.block<3,1>(3,0); +} + +bool base::TwistWithCovariance::hasValidVelocity() const +{ + return base::isnotnan(this->vel) && base::isnotnan(this->rot); +} + +void base::TwistWithCovariance::invalidateVelocity() +{ + this->vel = base::Vector3d::Ones() * base::unknown(); + this->rot = base::Vector3d::Ones() * base::unknown(); +} + +bool base::TwistWithCovariance::hasValidCovariance() const +{ + return base::isnotnan(this->cov); +} + +void base::TwistWithCovariance::invalidateCovariance() +{ + this->cov = Covariance::Ones() * base::unknown(); +} + +void base::TwistWithCovariance::invalidate() +{ + this->invalidateVelocity(); + this->invalidateCovariance(); +} + +base::TwistWithCovariance base::TwistWithCovariance::Zero() +{ + return TwistWithCovariance(static_cast(base::Vector3d::Zero()), static_cast(base::Vector3d::Zero())); +} + +double& base::TwistWithCovariance::operator[](int i) +{ + if (i<3) + return this->vel(i); + else + return this->rot(i-3); +} + +double base::TwistWithCovariance::operator[](int i) const +{ + if (i<3) + return this->vel(i); + else + return this->rot(i-3); +} + +base::TwistWithCovariance& base::TwistWithCovariance::operator+=(const base::TwistWithCovariance& arg) +{ + this->vel += arg.vel; + this->rot += arg.rot; + if (this->hasValidCovariance() && arg.hasValidCovariance()) + { + this->cov += arg.cov; + base::guaranteeSPD(this->cov); + } + return *this; +} + +base::TwistWithCovariance operator+(base::TwistWithCovariance lhs, const base::TwistWithCovariance& rhs) +{ + return lhs += rhs; +} + +base::TwistWithCovariance& base::TwistWithCovariance::operator-=(const base::TwistWithCovariance& arg) +{ + this->vel -= arg.vel; + this->rot -= arg.rot; + if (this->hasValidCovariance() && arg.hasValidCovariance()) + { + this->cov += arg.cov; + base::guaranteeSPD(this->cov); + } + + return *this; +} + +base::TwistWithCovariance operator-(base::TwistWithCovariance lhs, const base::TwistWithCovariance& rhs) +{ + return lhs -= rhs; +} + +base::TwistWithCovariance operator*(const base::TwistWithCovariance& lhs, double rhs) +{ + if (!lhs.hasValidCovariance()) + { + return base::TwistWithCovariance(static_cast(lhs.vel*rhs), static_cast(lhs.rot*rhs)); + } + else + { + return base::TwistWithCovariance(static_cast(lhs.vel*rhs), static_cast(lhs.rot*rhs), static_cast((rhs*rhs)*lhs.cov)); + } +} + +base::TwistWithCovariance operator*(double lhs, const base::TwistWithCovariance& rhs) +{ + if (!rhs.hasValidCovariance()) + { + return base::TwistWithCovariance(static_cast(lhs*rhs.vel), static_cast(lhs*rhs.rot)); + } + else + { + return base::TwistWithCovariance(static_cast(lhs*rhs.vel), static_cast(lhs*rhs.rot), static_cast((lhs*lhs)*rhs.cov)); + } +} + +base::TwistWithCovariance operator*(const base::TwistWithCovariance& lhs, const base::TwistWithCovariance& rhs) +{ + base::TwistWithCovariance tmp; + tmp.vel = lhs.rot.cross(rhs.vel)+lhs.vel.cross(rhs.rot); + tmp.rot = lhs.rot.cross(rhs.rot); + + /** In case the two twist have uncertainty **/ + if (lhs.hasValidCovariance() && rhs.hasValidCovariance()) + { + Eigen::Matrix cross_jacob; + Eigen::Matrix cross_cov; + + /** Initialize covariance **/ + tmp.cov.setZero(); + + cross_jacob = base::TwistWithCovariance::crossJacobian(lhs.rot, rhs.vel); + cross_cov << lhs.cov.block<3,3>(3,3), base::Matrix3d::Zero(), + base::Matrix3d::Zero(), rhs.cov.block<3,3>(0,0); + + /** Linear velocity is at the first covariance block **/ + tmp.cov.block<3,3>(0,0) = cross_jacob * cross_cov * cross_jacob.transpose(); + + cross_jacob = base::TwistWithCovariance::crossJacobian(lhs.vel, rhs.rot); + cross_cov << lhs.cov.block<3,3>(0,0), base::Matrix3d::Zero(), + base::Matrix3d::Zero(),rhs.cov.block<3,3>(3,3); + + /** Linear velocity is at the first covariance block **/ + tmp.cov.block<3,3>(0,0) += cross_jacob * cross_cov * cross_jacob.transpose(); + + cross_jacob = base::TwistWithCovariance::crossJacobian(lhs.rot, rhs.rot); + cross_cov << lhs.cov.block<3,3>(3,3), base::Matrix3d::Zero(), + base::Matrix3d::Zero(),rhs.cov.block<3,3>(3,3); + + /** Angular velocity is at the first covariance block **/ + tmp.cov.block<3,3>(3,3) = cross_jacob * cross_cov * cross_jacob.transpose(); + + base::guaranteeSPD(tmp.cov); + } + + return tmp; +} + +base::TwistWithCovariance operator/(const base::TwistWithCovariance& lhs, double rhs) +{ + return base::TwistWithCovariance(static_cast(lhs.vel/rhs), static_cast(lhs.rot/rhs), static_cast((1.0/(rhs *rhs))*lhs.cov)); +} + +base::TwistWithCovariance operator-(const base::TwistWithCovariance& arg) +{ + return base::TwistWithCovariance(static_cast(-arg.vel), static_cast(-arg.rot), arg.cov); +} + +Eigen::Matrix< double, int(3), int(6) > base::TwistWithCovariance::crossJacobian(const base::Vector3d& u, const base::Vector3d& v) +{ + Eigen::Matrix cross_jacob; + base::Matrix3d cross_u, cross_v; + cross_u << 0.0, -u[2], u[1], u[2], 0.0, -u[0], -u[1], u[0], 0.0; + cross_v << 0.0, -v[2], v[1], v[2], 0.0, -v[0], -v[1], v[0], 0.0; + cross_jacob << cross_u, cross_v; + return cross_jacob; +} + +std::ostream& base::operator<<(std::ostream& out, const base::TwistWithCovariance& twist) +{ + /** cout the 6D twist vector (rotational first and linear second) with its associated covariance matrix **/ + for (register unsigned short i=0; iinvalidateCovariance(); }; + explicit TwistWithCovariance (const base::Vector3d& vel = base::Vector3d::Zero(), const base::Vector3d& rot = base::Vector3d::Zero() ); - TwistWithCovariance(const base::Vector3d& vel, const base::Vector3d& rot, const Covariance& cov): - vel(vel), rot(rot), cov(cov) {}; + TwistWithCovariance(const base::Vector3d& vel, const base::Vector3d& rot, const Covariance& cov); - TwistWithCovariance(const base::Vector6d& velocity, const Covariance& cov) - { - this->setVelocity(velocity); - this->cov = cov; - } + TwistWithCovariance(const base::Vector6d& velocity, const Covariance& cov); /** Default std::cout function **/ friend std::ostream & operator<<(std::ostream &out, const base::Vector6d& vel); /** Get and Set Methods **/ - const base::Vector3d& getTranslation() const { return this->vel; } - void setTranslation(const base::Vector3d& vel) { this->vel = vel; } + const base::Vector3d& getTranslation() const; + void setTranslation(const base::Vector3d& vel); - const base::Vector3d& getRotation() const { return this->rot; } - void setRotation(const base::Vector3d& rot) { this->rot = rot; } + const base::Vector3d& getRotation() const; + void setRotation(const base::Vector3d& rot); - const Covariance& getCovariance() const { return this->cov; } - void setCovariance(const Covariance& cov) { this->cov = cov; } + const Covariance& getCovariance() const; + void setCovariance(const Covariance& cov); - const base::Matrix3d getLinearVelocityCov() const { return this->cov.block<3,3>(0,0); } - void setLinearVelocityCov(const base::Matrix3d& cov) { this->cov.block<3,3>(0,0) = cov; } - const base::Matrix3d getAngularVelocityCov() const { return this->cov.block<3,3>(3,3); } - void setAngularVelocityCov(const base::Matrix3d& cov) { this->cov.block<3,3>(3,3) = cov; } + const base::Matrix3d getLinearVelocityCov() const; + void setLinearVelocityCov(const base::Matrix3d& cov); + const base::Matrix3d getAngularVelocityCov() const; + void setAngularVelocityCov(const base::Matrix3d& cov); - const base::Vector3d& getLinearVelocity() const {return this->getTranslation(); } - void setLinearVelocity(const base::Vector3d& vel) { return this->setTranslation(vel); } - const base::Vector3d& getAngularVelocity() const {return this->getRotation(); } - void setAngularVelocity(const base::Vector3d& rot) { return this->setRotation(rot); } + const base::Vector3d& getLinearVelocity() const; + void setLinearVelocity(const base::Vector3d& vel); + const base::Vector3d& getAngularVelocity() const; + void setAngularVelocity(const base::Vector3d& rot); - const base::Vector3d& translation() const {return this->getTranslation(); } - const base::Vector3d& rotation() const {return this->getRotation(); } + const base::Vector3d& translation() const; + const base::Vector3d& rotation() const; - const base::Vector6d getVelocity() const - { - base::Vector6d all_velocities; - all_velocities.block<3,1>(0,0) = this->vel; - all_velocities.block<3,1>(3,0) = this->rot; - return all_velocities; - } + const base::Vector6d getVelocity() const; + - void setVelocity(const base::Vector6d& velocity) - { - /** Linear velocity at first place, Angular velocity at second place **/ - this->vel = velocity.block<3,1>(0,0); - this->rot = velocity.block<3,1>(3,0); - } + void setVelocity(const base::Vector6d& velocity); /** Check Methods **/ - bool hasValidVelocity() const - { - return this->vel.allFinite() && this->rot.allFinite(); - } - void invalidateVelocity() - { - this->vel = base::Vector3d::Ones() * base::unknown(); - this->rot = base::Vector3d::Ones() * base::unknown(); - } + bool hasValidVelocity() const; - bool hasValidCovariance() const { return this->cov.allFinite(); } - void invalidateCovariance() - { - this->cov = Covariance::Ones() * base::unknown(); - } + void invalidateVelocity(); - void invalidate() - { - this->invalidateVelocity(); - this->invalidateCovariance(); - } + bool hasValidCovariance() const; + void invalidateCovariance(); - static TwistWithCovariance Zero() - { - return TwistWithCovariance(static_cast(base::Vector3d::Zero()), static_cast(base::Vector3d::Zero())); - } - double& operator[](int i) - { - if (i<3) - return this->vel(i); - else - return this->rot(i-3); - } + void invalidate(); - double operator[](int i) const - { - if (i<3) - return this->vel(i); - else - return this->rot(i-3); - } + static TwistWithCovariance Zero(); + + double& operator[](int i); + double operator[](int i) const; - inline TwistWithCovariance& operator+=(const TwistWithCovariance& arg) - { - this->vel += arg.vel; - this->rot += arg.rot; - if (this->hasValidCovariance() && arg.hasValidCovariance()) - { - this->cov += arg.cov; - base::guaranteeSPD(this->cov); - } - return *this; - } - inline friend TwistWithCovariance operator+(TwistWithCovariance lhs,const TwistWithCovariance& rhs) - { - return lhs += rhs; - } + inline TwistWithCovariance& operator+=(const TwistWithCovariance& arg); - inline TwistWithCovariance& operator-=(const TwistWithCovariance& arg) - { - this->vel -= arg.vel; - this->rot -= arg.rot; - if (this->hasValidCovariance() && arg.hasValidCovariance()) - { - this->cov += arg.cov; - base::guaranteeSPD(this->cov); - } - - return *this; - } - inline friend TwistWithCovariance operator-(TwistWithCovariance lhs, const TwistWithCovariance& rhs) - { - return lhs -= rhs; - } + inline friend TwistWithCovariance operator+(TwistWithCovariance lhs,const TwistWithCovariance& rhs); - inline friend TwistWithCovariance operator*(const TwistWithCovariance& lhs,double rhs) - { - if (!lhs.hasValidCovariance()) - { - return TwistWithCovariance(static_cast(lhs.vel*rhs), static_cast(lhs.rot*rhs)); - } - else - { - return TwistWithCovariance(static_cast(lhs.vel*rhs), static_cast(lhs.rot*rhs), static_cast((rhs*rhs)*lhs.cov)); - } - } - - inline friend TwistWithCovariance operator*(double lhs, const TwistWithCovariance& rhs) - { - if (!rhs.hasValidCovariance()) - { - return TwistWithCovariance(static_cast(lhs*rhs.vel), static_cast(lhs*rhs.rot)); - } - else - { - return TwistWithCovariance(static_cast(lhs*rhs.vel), static_cast(lhs*rhs.rot), static_cast((lhs*lhs)*rhs.cov)); - } - } - - - //Spatial Cross product - inline friend TwistWithCovariance operator*(const TwistWithCovariance& lhs, const TwistWithCovariance& rhs) - { - TwistWithCovariance tmp; - tmp.vel = lhs.rot.cross(rhs.vel)+lhs.vel.cross(rhs.rot); - tmp.rot = lhs.rot.cross(rhs.rot); - - /** In case the two twist have uncertainty **/ - if (lhs.hasValidCovariance() && rhs.hasValidCovariance()) - { - Eigen::Matrix cross_jacob; - Eigen::Matrix cross_cov; - - /** Initialize covariance **/ - tmp.cov.setZero(); - - cross_jacob = TwistWithCovariance::crossJacobian(lhs.rot, rhs.vel); - cross_cov << lhs.cov.block<3,3>(3,3), base::Matrix3d::Zero(), - base::Matrix3d::Zero(), rhs.cov.block<3,3>(0,0); - - /** Linear velocity is at the first covariance block **/ - tmp.cov.block<3,3>(0,0) = cross_jacob * cross_cov * cross_jacob.transpose(); - - cross_jacob = TwistWithCovariance::crossJacobian(lhs.vel, rhs.rot); - cross_cov << lhs.cov.block<3,3>(0,0), base::Matrix3d::Zero(), - base::Matrix3d::Zero(),rhs.cov.block<3,3>(3,3); + inline TwistWithCovariance& operator-=(const TwistWithCovariance& arg); - /** Linear velocity is at the first covariance block **/ - tmp.cov.block<3,3>(0,0) += cross_jacob * cross_cov * cross_jacob.transpose(); + inline friend TwistWithCovariance operator-(TwistWithCovariance lhs, const TwistWithCovariance& rhs); - cross_jacob = TwistWithCovariance::crossJacobian(lhs.rot, rhs.rot); - cross_cov << lhs.cov.block<3,3>(3,3), base::Matrix3d::Zero(), - base::Matrix3d::Zero(),rhs.cov.block<3,3>(3,3); + inline friend TwistWithCovariance operator*(const TwistWithCovariance& lhs,double rhs); - /** Angular velocity is at the first covariance block **/ - tmp.cov.block<3,3>(3,3) = cross_jacob * cross_cov * cross_jacob.transpose(); + inline friend TwistWithCovariance operator*(double lhs, const TwistWithCovariance& rhs); - base::guaranteeSPD(tmp.cov); - } + //Spatial Cross product + inline friend TwistWithCovariance operator*(const TwistWithCovariance& lhs, const TwistWithCovariance& rhs); - return tmp; - } - - inline friend TwistWithCovariance operator/(const TwistWithCovariance& lhs,double rhs) - { - return TwistWithCovariance(static_cast(lhs.vel/rhs), static_cast(lhs.rot/rhs), static_cast((1.0/(rhs *rhs))*lhs.cov)); - } + inline friend TwistWithCovariance operator/(const TwistWithCovariance& lhs,double rhs); /** unary - **/ - inline friend TwistWithCovariance operator-(const TwistWithCovariance& arg) - { - return TwistWithCovariance(static_cast(-arg.vel), static_cast(-arg.rot), arg.cov); - } + inline friend TwistWithCovariance operator-(const TwistWithCovariance& arg); - static Eigen::Matrix crossJacobian(const base::Vector3d& u, const base::Vector3d& v) - { - Eigen::Matrix cross_jacob; - base::Matrix3d cross_u, cross_v; - cross_u << 0.0, -u[2], u[1], u[2], 0.0, -u[0], -u[1], u[0], 0.0; - cross_v << 0.0, -v[2], v[1], v[2], 0.0, -v[0], -v[1], v[0], 0.0; - cross_jacob << cross_u, cross_v; - return cross_jacob; - } + static Eigen::Matrix crossJacobian(const base::Vector3d& u, const base::Vector3d& v); }; /** Default std::cout function */ - inline std::ostream & operator<<(std::ostream &out, const base::TwistWithCovariance& twist) - { - /** cout the 6D twist vector (rotational first and linear second) with its associated covariance matrix **/ - for (register unsigned short i=0; i Date: Thu, 14 Jul 2016 14:24:32 +0200 Subject: [PATCH 08/50] moved Waypoint to new src folder --- src/CMakeLists.txt | 5 +++-- src/Waypoint.cpp | 18 ++++++++++++++++++ {base => src}/Waypoint.hpp | 13 ++++--------- 3 files changed, 25 insertions(+), 11 deletions(-) create mode 100644 src/Waypoint.cpp rename {base => src}/Waypoint.hpp (59%) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c3311254..a4e82419 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,6 +1,7 @@ -set(SOURCES Angle.cpp Pose.cpp) +set(SOURCES Angle.cpp Pose.cpp TwistWithCovariance.cpp Waypoint.cpp) -set(HEADERS Logging.hpp Singleton.hpp logging/logging_iostream_style.h logging/logging_printf_style.h) +set(HEADERS Logging.hpp Singleton.hpp logging/logging_iostream_style.h logging/logging_printf_style.h + Wrench.hpp) # Using SISL as optional dependency diff --git a/src/Waypoint.cpp b/src/Waypoint.cpp new file mode 100644 index 00000000..c74d6fa6 --- /dev/null +++ b/src/Waypoint.cpp @@ -0,0 +1,18 @@ +#include "Waypoint.hpp" + +base::Waypoint::Waypoint() : position(Position::Identity()), heading(0), tol_position(0), tol_heading(0) +{ + +} + +base::Waypoint::Waypoint(const base::Vector3d& _position, double _heading, double _tol_position, double _tol_heading) + : position(_position), heading(_heading), tol_position(_tol_position), tol_heading(_tol_heading) +{ + +} + +base::Waypoint::Waypoint(const Eigen::Vector3d& _position, double _heading, double _tol_position, double _tol_heading) + : position(_position), heading(_heading), tol_position(_tol_position), tol_heading(_tol_heading) +{ + +} diff --git a/base/Waypoint.hpp b/src/Waypoint.hpp similarity index 59% rename from base/Waypoint.hpp rename to src/Waypoint.hpp index cdc65bc6..97d4eedd 100644 --- a/base/Waypoint.hpp +++ b/src/Waypoint.hpp @@ -21,19 +21,14 @@ namespace base double tol_heading; // default: initializing with identity and zero - Waypoint() - : position(Position::Identity()), heading(0), tol_position(0), - tol_heading(0){} + Waypoint(); + // use base::Vector3d Waypoint(base::Vector3d const &_position, double _heading, - double _tol_position, double _tol_heading) - : position(_position), heading(_heading), - tol_position(_tol_position), tol_heading(_tol_heading){} + double _tol_position, double _tol_heading); // convenience: same for Eigen::Vector3d Waypoint(Eigen::Vector3d const &_position, double _heading, - double _tol_position, double _tol_heading) - : position(_position), heading(_heading), - tol_position(_tol_position), tol_heading(_tol_heading){} + double _tol_position, double _tol_heading); }; } From 5c03871d9694902d0eb09b51950a993acc23c649 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 14 Jul 2016 14:41:04 +0200 Subject: [PATCH 09/50] re-patched new isnan interface --- src/TwistWithCovariance.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/TwistWithCovariance.cpp b/src/TwistWithCovariance.cpp index ba2b2b64..a4b6843e 100644 --- a/src/TwistWithCovariance.cpp +++ b/src/TwistWithCovariance.cpp @@ -117,7 +117,7 @@ void base::TwistWithCovariance::setVelocity(const base::Vector6d& velocity) bool base::TwistWithCovariance::hasValidVelocity() const { - return base::isnotnan(this->vel) && base::isnotnan(this->rot); + return this->vel.allFinite() && this->rot.allFinite(); } void base::TwistWithCovariance::invalidateVelocity() @@ -128,7 +128,7 @@ void base::TwistWithCovariance::invalidateVelocity() bool base::TwistWithCovariance::hasValidCovariance() const { - return base::isnotnan(this->cov); + return this->cov.allFinite(); } void base::TwistWithCovariance::invalidateCovariance() From bb7e8f09beac0372bc5121e9752748c1ee0e0adb Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 14 Jul 2016 15:09:03 +0200 Subject: [PATCH 10/50] splitted TransformWithCovariance --- base/TransformWithCovariance.hpp | 413 ------------------------------- src/CMakeLists.txt | 17 +- src/TransformWithCovariance.cpp | 399 +++++++++++++++++++++++++++++ src/TransformWithCovariance.hpp | 150 +++++++++++ 4 files changed, 563 insertions(+), 416 deletions(-) delete mode 100644 base/TransformWithCovariance.hpp create mode 100644 src/TransformWithCovariance.cpp create mode 100644 src/TransformWithCovariance.hpp diff --git a/base/TransformWithCovariance.hpp b/base/TransformWithCovariance.hpp deleted file mode 100644 index 2a0ce417..00000000 --- a/base/TransformWithCovariance.hpp +++ /dev/null @@ -1,413 +0,0 @@ -#ifndef __BASE_TRANSFORM_WITH_COVARIANCE_HPP__ -#define __BASE_TRANSFORM_WITH_COVARIANCE_HPP__ - -#include // std::setprecision - -#include -#include -#include -#include - -#include -#include -#include - -namespace base { - - /** - * Class which represents a 3D Transformation with associated uncertainty information. - * - * The uncertainty is represented as a 6x6 matrix, which is the covariance - * matrix of the [r t] representation of the error. Here r is the rotation orientation - * part expressed as a scaled axis of orientation, and t the translational - * component. - * - * The uncertainty information is optional. The hasValidCovariance() method can - * be used to see if uncertainty information is associated with the class. - */ - class TransformWithCovariance - { - - public: - typedef base::Matrix6d Covariance; - - public: - /** The transformation is represented 6D vector [translation orientation] - * Here orientation is the rotational part expressed as a quaternion - * orientation, and t the translational component. - */ - base::Position translation; - - base::Quaterniond orientation; - - /** The uncertainty is represented as a 6x6 matrix, which is the covariance - * matrix of the [translation orientation] representation of the error. - */ - Covariance cov; - - public: - - TransformWithCovariance() : translation(base::Position::Zero()), - orientation(base::Quaterniond::Identity()) {this->invalidateCovariance();} - - explicit TransformWithCovariance( const base::Affine3d& trans) - {this->setTransform(trans); this->invalidateCovariance();}; - - TransformWithCovariance( const base::Affine3d& trans, const Covariance& cov ) - {this->setTransform(trans); this->cov = cov;}; - - TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation) : - translation(translation), orientation(orientation) {this->invalidateCovariance();}; - - TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation, const Covariance& cov ) : - translation(translation), orientation(orientation), cov(cov){}; - - static TransformWithCovariance Identity() - { - return TransformWithCovariance(); - }; - - - /** Default std::cout function - */ - friend std::ostream & operator<<(std::ostream &out, const TransformWithCovariance& trans); - - /** performs a composition of this transform with the transform given. - * The result is another transform with result = this * trans - */ - TransformWithCovariance composition( const TransformWithCovariance& trans ) const - { - return this->operator*( trans ); - }; - - /** performs an inverse composition of two transformations. - * The result is such that result * trans = this. Note that this is different from - * calling result = this * inv(trans), in the way the uncertainties are handled. - */ - TransformWithCovariance compositionInv( const TransformWithCovariance& trans ) const - { - const TransformWithCovariance &tf(*this); - const TransformWithCovariance &t1(trans); - base::Position p2(tf.translation + (tf.orientation * t1.inverse().translation)); - Eigen::Quaterniond q2( tf.orientation * t1.orientation.inverse()); - - // short path if there is no uncertainty - if( !t1.hasValidCovariance() && !tf.hasValidCovariance() ) - return TransformWithCovariance(p2, q2); - - // convert the orientations of the respective transforms into quaternions - // in order to inverse the covariances, we need to get both the t1 and t2=[r2 p2] transformations - // based on the composition tf = t2 * t1 - Eigen::Quaterniond q1( t1.orientation ); - Eigen::Quaterniond q( q2 * q1 ); - - // initialize resulting covariance - Eigen::Matrix cov = Eigen::Matrix::Zero(); - - Eigen::Matrix J1; - J1 << q2.toRotationMatrix(), Eigen::Matrix3d::Zero(), - Eigen::Matrix3d::Zero(), dr2r1_by_r1(q, q1, q2); - - Eigen::Matrix J2; - J2 << Eigen::Matrix3d::Identity(), drx_by_dr(q2, t1.translation), - Eigen::Matrix3d::Zero(), dr2r1_by_r2(q, q1, q2); - - cov = J2.inverse() * ( tf.getCovariance() - J1 * t1.getCovariance() * J1.transpose() ) * J2.transpose().inverse(); - - // and return the resulting uncertainty transform - return TransformWithCovariance( p2, q2, cov ); - }; - - /** Same as compositionInv, just that the result is such that trans * result = this. - * Note that this is different from calling result = inv(trans) * this, - * in the way the uncertainties are handled. - */ - TransformWithCovariance preCompositionInv( const TransformWithCovariance& trans ) const - { - const TransformWithCovariance &tf(*this); - const TransformWithCovariance &t2(trans); - base::Position p1(t2.inverse().translation + (t2.orientation.inverse() * tf.translation)); - Eigen::Quaterniond q1(t2.orientation.inverse() * tf.orientation); - - // short path if there is no uncertainty - if( !t2.hasValidCovariance() && !tf.hasValidCovariance() ) - return TransformWithCovariance( p1, q1 ); - - // convert the orientations of the respective transforms into quaternions - // in order to inverse the covariances, we need to get both the t1=[p1 q1] and t2 transformations - // based on the composition tf = t2 * t1 - Eigen::Quaterniond q2(t2.orientation); - Eigen::Quaterniond q( q2 * q1 ); - - // initialize resulting covariance - Eigen::Matrix cov = Eigen::Matrix::Zero(); - - Eigen::Matrix J1; - J1 << t2.getTransform().linear(), Eigen::Matrix3d::Zero(), - Eigen::Matrix3d::Zero(), dr2r1_by_r1(q, q1, q2); - - Eigen::Matrix J2; - J2 << Eigen::Matrix3d::Identity(), drx_by_dr(q2, p1), - Eigen::Matrix3d::Zero(), dr2r1_by_r2(q, q1, q2); - - cov = J1.inverse() * ( tf.getCovariance() - J2 * t2.getCovariance() * J2.transpose() ) * J1.transpose().inverse(); - - // and return the resulting uncertainty transform - return TransformWithCovariance( p1, q1, cov ); - }; - - /** alias for the composition of two transforms - */ - TransformWithCovariance operator*( const TransformWithCovariance& trans ) const - { - const TransformWithCovariance &t2(*this); - const TransformWithCovariance &t1(trans); - - const base::Quaterniond t(t2.orientation * t1.orientation); - const base::Position p(t2.translation + (t2.orientation * t1.translation)); - - // short path if there is no uncertainty - if( !t1.hasValidCovariance() && !t2.hasValidCovariance() ) - { - return TransformWithCovariance(p, t); - } - - // convert the orientations of the respective transforms into quaternions - const Eigen::Quaterniond q1( t1.orientation ), q2( t2.orientation ); - const Eigen::Quaterniond q( t2.orientation * t1.orientation ); - - // initialize resulting covariance - Eigen::Matrix cov = Eigen::Matrix::Zero(); - - // calculate the Jacobians (this is what all the above functions are for) - // and add to the resulting covariance - if( t1.hasValidCovariance() ) - { - Eigen::Matrix J1; - J1 << t2.getTransform().linear(), Eigen::Matrix3d::Zero(), - Eigen::Matrix3d::Zero(), dr2r1_by_r1(q, q1, q2); - - cov += J1*t1.getCovariance()*J1.transpose(); - } - - if( t2.hasValidCovariance() ) - { - Eigen::Matrix J2; - J2 << Eigen::Matrix3d::Identity(), drx_by_dr(q2, t1.translation), - Eigen::Matrix3d::Zero(), dr2r1_by_r2(q, q1, q2); - - cov += J2*t2.getCovariance()*J2.transpose(); - } - - // and return the resulting uncertainty transform - return TransformWithCovariance(p, t, cov); - }; - - TransformWithCovariance inverse() const - { - // short path if there is no uncertainty - if( !hasValidCovariance() ) - return TransformWithCovariance(static_cast(-(this->orientation.inverse() * this->translation)), this->orientation.inverse()); - - Eigen::Quaterniond q(this->orientation); - Eigen::Vector3d t(this->translation); - Eigen::Matrix J; - J << q.toRotationMatrix().transpose(), drx_by_dr( q.inverse(), t ), - Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Identity(); - - return TransformWithCovariance(static_cast(-(this->orientation.inverse() * this->translation)), - this->orientation.inverse(), - J*this->getCovariance()*J.transpose()); - }; - - const Covariance& getCovariance() const { return this->cov; } - void setCovariance( const Covariance& cov ) { this->cov = cov; } - - const base::Matrix3d getTranslationCov() const { return this->cov.topLeftCorner<3,3>(); } - void setTranslationCov(const base::Matrix3d& cov) { this->cov.topLeftCorner<3,3>() = cov; } - - const base::Matrix3d getOrientationCov() const { return this->cov.bottomRightCorner<3,3>(); } - void setOrientationCov(const base::Matrix3d& cov) { this->cov.bottomRightCorner<3,3>() = cov; } - - const base::Affine3d getTransform() const - { - base::Affine3d trans (this->orientation); - trans.translation() = this->translation; - return trans; - } - void setTransform( const base::Affine3d& trans ) - { - this->translation = trans.translation(); - this->orientation = base::Quaterniond(trans.rotation()); - } - - const base::Orientation getOrientation() const - { - return base::Orientation(this->orientation); - } - - void setOrientation(const base::Orientation & q) - { - this->orientation = base::Quaterniond(q); - } - - bool hasValidTransform() const - { - return !translation.hasNaN() && !orientation.coeffs().hasNaN(); - } - - void invalidateTransform() - { - translation = base::Position::Ones() * base::NaN(); - orientation.coeffs() = base::Vector4d::Ones() * base::NaN(); - } - - /** @warning This method is computationally expensive. Use with care! */ - bool hasValidCovariance() const { return !cov.hasNaN(); } - void invalidateCovariance() - { - cov = Covariance::Ones() * base::NaN(); - } - - protected: - // The uncertainty transformations are implemented according to: - // Pennec X, Thirion JP. A framework for uncertainty and validation of 3-D - // registration methods based on points and frames. International Journal of - // Computer Vion. 1997;25(3):203–229. Available at: - // http://www.springerlink.com/index/JJ25N2Q23T402682.pdf. - - static Eigen::Quaterniond r_to_q( const Eigen::Vector3d& r ) - { - double theta = r.norm(); - if( fabs(theta) > 1e-5 ) - return Eigen::Quaterniond( base::AngleAxisd( theta, r/theta ) ); - else - return Eigen::Quaterniond::Identity(); - } - - static Eigen::Vector3d q_to_r( const Eigen::Quaterniond& q ) - { - base::AngleAxisd aa( q ); - return aa.axis() * aa.angle(); - } - - static inline double sign( double v ) - { - return v > 0.0 ? 1.0 : -1.0; - } - - static Eigen::Matrix skew_symmetric( const Eigen::Vector3d& r ) - { - Eigen::Matrix3d res; - res << 0, -r.z(), r.y(), - r.z(), 0, -r.x(), - -r.y(), r.x(), 0; - return res; - } - - static Eigen::Matrix dq_by_dr( const Eigen::Quaterniond& q ) - { - const Eigen::Vector3d r( q_to_r( q ) ); - - const double theta = r.norm(); - const double kappa = 0.5 - theta*theta / 48.0; // approx. see Paper - const double lambda = 1.0/24.0*(1.0-theta*theta/40.0); // approx. - Eigen::Matrix res; - res << - q.vec().transpose()/2.0, - kappa * Eigen::Matrix3d::Identity() - lambda * r * r.transpose(); - - return res; - } - - static Eigen::Matrix dr_by_dq( const Eigen::Quaterniond& q ) - { - const Eigen::Vector3d r( q_to_r( q ) ); - const double mu = q.vec().norm(); - const double tau = 2.0 * sign( q.w() ) * ( 1.0 + mu*mu/6.0 ); // approx - const double nu = -2.0 * sign( q.w() ) * ( 2.0/3.0 + mu*mu/5.0 ); // approx - - Eigen::Matrix res; - res << -2*q.vec(), tau * Eigen::Matrix3d::Identity() + nu * q.vec() * q.vec().transpose(); - - return res; - } - - static Eigen::Matrix dq2q1_by_dq1( const Eigen::Quaterniond& q2 ) - { - Eigen::Matrix res; - res << 0, -q2.vec().transpose(), - q2.vec(), skew_symmetric( q2.vec() ); - return Eigen::Matrix::Identity() * q2.w() + res; - } - - static Eigen::Matrix dq2q1_by_dq2( const Eigen::Quaterniond& q1 ) - { - Eigen::Matrix res; - res << 0, -q1.vec().transpose(), - q1.vec(), -skew_symmetric( q1.vec() ); - return Eigen::Matrix::Identity() * q1.w() + res; - } - - static Eigen::Matrix dr2r1_by_r1( const Eigen::Quaterniond& q, const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2 ) - { - return Eigen::Matrix3d( - dr_by_dq( q ) - * dq2q1_by_dq1( q2 ) - * dq_by_dr( q1 ) ); - } - - static Eigen::Matrix dr2r1_by_r2( const Eigen::Quaterniond& q, const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2 ) - { - return Eigen::Matrix3d( - dr_by_dq( q ) - * dq2q1_by_dq2( q1 ) - * dq_by_dr( q2 ) ); - } - - static Eigen::Matrix drx_by_dr( const Eigen::Quaterniond& q, const Eigen::Vector3d& x ) - { - const Eigen::Vector3d r( q_to_r( q ) ); - const double theta = r.norm(); - const double alpha = 1.0 - theta*theta/6.0; - const double beta = 0.5 - theta*theta/24.0; - const double gamma = 1.0 / 3.0 - theta*theta/30.0; - const double delta = -1.0 / 12.0 + theta*theta/180.0; - - return Eigen::Matrix3d( - -skew_symmetric(x)*(gamma*r*r.transpose() - - beta*skew_symmetric(r)+alpha*Eigen::Matrix3d::Identity()) - -skew_symmetric(r)*skew_symmetric(x)*(delta*r*r.transpose() - + 2.0*beta*Eigen::Matrix3d::Identity()) ); - } - }; - - /** Default std::cout function - */ - inline std::ostream & operator<<(std::ostream &out, const TransformWithCovariance& trans) - { - /** cout the 6D pose vector (translation and scaled axis orientation) with its associated covariance matrix **/ - base::Vector3d scaled_axis; - base::AngleAxisd angle_axis (trans.orientation); - scaled_axis = angle_axis.axis() * angle_axis.angle(); - for (register unsigned short i=0; iinvalidateCovariance(); +} + +base::TransformWithCovariance::TransformWithCovariance(const base::Affine3d& trans) +{ + this->setTransform(trans); + this->invalidateCovariance(); +} + +base::TransformWithCovariance::TransformWithCovariance(const base::Affine3d& trans, const base::TransformWithCovariance::Covariance& cov) +{ + this->setTransform(trans); + this->cov = cov; +} + +base::TransformWithCovariance::TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation) + : translation(translation), orientation(orientation) +{ + this->invalidateCovariance(); +} + +base::TransformWithCovariance::TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation, const base::TransformWithCovariance::Covariance& cov) + : translation(translation), orientation(orientation), cov(cov) +{ + +} + +base::TransformWithCovariance base::TransformWithCovariance::Identity() +{ + return TransformWithCovariance(); +} + +base::TransformWithCovariance base::TransformWithCovariance::composition(const base::TransformWithCovariance& trans) const +{ + return this->operator*( trans ); +} + +base::TransformWithCovariance base::TransformWithCovariance::compositionInv(const base::TransformWithCovariance& trans) const +{ + const TransformWithCovariance &tf(*this); + const TransformWithCovariance &t1(trans); + base::Position p2(tf.translation + (tf.orientation * t1.inverse().translation)); + Eigen::Quaterniond q2( tf.orientation * t1.orientation.inverse()); + + // short path if there is no uncertainty + if( !t1.hasValidCovariance() && !tf.hasValidCovariance() ) + return TransformWithCovariance(p2, q2); + + // convert the orientations of the respective transforms into quaternions + // in order to inverse the covariances, we need to get both the t1 and t2=[r2 p2] transformations + // based on the composition tf = t2 * t1 + Eigen::Quaterniond q1( t1.orientation ); + Eigen::Quaterniond q( q2 * q1 ); + + // initialize resulting covariance + Eigen::Matrix cov = Eigen::Matrix::Zero(); + + Eigen::Matrix J1; + J1 << q2.toRotationMatrix(), Eigen::Matrix3d::Zero(), + Eigen::Matrix3d::Zero(), dr2r1_by_r1(q, q1, q2); + + Eigen::Matrix J2; + J2 << Eigen::Matrix3d::Identity(), drx_by_dr(q2, t1.translation), + Eigen::Matrix3d::Zero(), dr2r1_by_r2(q, q1, q2); + + cov = J2.inverse() * ( tf.getCovariance() - J1 * t1.getCovariance() * J1.transpose() ) * J2.transpose().inverse(); + + // and return the resulting uncertainty transform + return TransformWithCovariance( p2, q2, cov ); +} + +base::TransformWithCovariance base::TransformWithCovariance::preCompositionInv(const base::TransformWithCovariance& trans) const +{ + const TransformWithCovariance &tf(*this); + const TransformWithCovariance &t2(trans); + base::Position p1(t2.inverse().translation + (t2.orientation.inverse() * tf.translation)); + Eigen::Quaterniond q1(t2.orientation.inverse() * tf.orientation); + + // short path if there is no uncertainty + if( !t2.hasValidCovariance() && !tf.hasValidCovariance() ) + return TransformWithCovariance( p1, q1 ); + + // convert the orientations of the respective transforms into quaternions + // in order to inverse the covariances, we need to get both the t1=[p1 q1] and t2 transformations + // based on the composition tf = t2 * t1 + Eigen::Quaterniond q2(t2.orientation); + Eigen::Quaterniond q( q2 * q1 ); + + // initialize resulting covariance + Eigen::Matrix cov = Eigen::Matrix::Zero(); + + Eigen::Matrix J1; + J1 << t2.getTransform().linear(), Eigen::Matrix3d::Zero(), + Eigen::Matrix3d::Zero(), dr2r1_by_r1(q, q1, q2); + + Eigen::Matrix J2; + J2 << Eigen::Matrix3d::Identity(), drx_by_dr(q2, p1), + Eigen::Matrix3d::Zero(), dr2r1_by_r2(q, q1, q2); + + cov = J1.inverse() * ( tf.getCovariance() - J2 * t2.getCovariance() * J2.transpose() ) * J1.transpose().inverse(); + + // and return the resulting uncertainty transform + return TransformWithCovariance( p1, q1, cov ); +} + + +base::TransformWithCovariance base::TransformWithCovariance::operator*(const base::TransformWithCovariance& trans) const +{ + const TransformWithCovariance &t2(*this); + const TransformWithCovariance &t1(trans); + + const base::Quaterniond t(t2.orientation * t1.orientation); + const base::Position p(t2.translation + (t2.orientation * t1.translation)); + + // short path if there is no uncertainty + if( !t1.hasValidCovariance() && !t2.hasValidCovariance() ) + { + return TransformWithCovariance(p, t); + } + + // convert the orientations of the respective transforms into quaternions + const Eigen::Quaterniond q1( t1.orientation ), q2( t2.orientation ); + const Eigen::Quaterniond q( t2.orientation * t1.orientation ); + + // initialize resulting covariance + Eigen::Matrix cov = Eigen::Matrix::Zero(); + + // calculate the Jacobians (this is what all the above functions are for) + // and add to the resulting covariance + if( t1.hasValidCovariance() ) + { + Eigen::Matrix J1; + J1 << t2.getTransform().linear(), Eigen::Matrix3d::Zero(), + Eigen::Matrix3d::Zero(), dr2r1_by_r1(q, q1, q2); + + cov += J1*t1.getCovariance()*J1.transpose(); + } + + if( t2.hasValidCovariance() ) + { + Eigen::Matrix J2; + J2 << Eigen::Matrix3d::Identity(), drx_by_dr(q2, t1.translation), + Eigen::Matrix3d::Zero(), dr2r1_by_r2(q, q1, q2); + + cov += J2*t2.getCovariance()*J2.transpose(); + } + + // and return the resulting uncertainty transform + return TransformWithCovariance(p, t, cov); +} + +base::TransformWithCovariance base::TransformWithCovariance::inverse() const +{ + // short path if there is no uncertainty + if( !hasValidCovariance() ) + return TransformWithCovariance(static_cast(-(this->orientation.inverse() * this->translation)), this->orientation.inverse()); + + Eigen::Quaterniond q(this->orientation); + Eigen::Vector3d t(this->translation); + Eigen::Matrix J; + J << q.toRotationMatrix().transpose(), drx_by_dr( q.inverse(), t ), + Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Identity(); + + return TransformWithCovariance(static_cast(-(this->orientation.inverse() * this->translation)), + this->orientation.inverse(), + J*this->getCovariance()*J.transpose()); +} + +const base::TransformWithCovariance::Covariance& base::TransformWithCovariance::getCovariance() const +{ + return this->cov; +} + +void base::TransformWithCovariance::setCovariance(const base::TransformWithCovariance::Covariance& cov) +{ + this->cov = cov; +} + +const base::Matrix3d base::TransformWithCovariance::getTranslationCov() const +{ + return this->cov.topLeftCorner<3,3>(); +} + +void base::TransformWithCovariance::setTranslationCov(const base::Matrix3d& cov) +{ + this->cov.topLeftCorner<3,3>() = cov; +} + +const base::Matrix3d base::TransformWithCovariance::getOrientationCov() const +{ + return this->cov.bottomRightCorner<3,3>(); +} + +void base::TransformWithCovariance::setOrientationCov(const base::Matrix3d& cov) +{ + this->cov.bottomRightCorner<3,3>() = cov; +} + +const base::Affine3d base::TransformWithCovariance::getTransform() const +{ + base::Affine3d trans (this->orientation); + trans.translation() = this->translation; + return trans; +} + +void base::TransformWithCovariance::setTransform(const base::Affine3d& trans) +{ + this->translation = trans.translation(); + this->orientation = base::Quaterniond(trans.rotation()); +} + +const base::Orientation base::TransformWithCovariance::getOrientation() const +{ + return base::Orientation(this->orientation); +} + +void base::TransformWithCovariance::setOrientation(const base::Orientation& q) +{ + this->orientation = base::Quaterniond(q); +} + +bool base::TransformWithCovariance::hasValidTransform() const +{ + return !translation.hasNaN() && !orientation.coeffs().hasNaN(); +} + +void base::TransformWithCovariance::invalidateTransform() +{ + translation = base::Position::Ones() * base::NaN(); + orientation.coeffs() = base::Vector4d::Ones() * base::NaN(); +} + +bool base::TransformWithCovariance::hasValidCovariance() const +{ + return !cov.hasNaN(); +} + +void base::TransformWithCovariance::invalidateCovariance() +{ + cov = Covariance::Ones() * base::NaN(); +} + +Eigen::Quaterniond base::TransformWithCovariance::r_to_q(const Eigen::Vector3d& r) +{ + double theta = r.norm(); + if( fabs(theta) > 1e-5 ) + return Eigen::Quaterniond( base::AngleAxisd( theta, r/theta ) ); + else + return Eigen::Quaterniond::Identity(); +} + +Eigen::Vector3d base::TransformWithCovariance::q_to_r(const Eigen::Quaterniond& q) +{ + base::AngleAxisd aa( q ); + return aa.axis() * aa.angle(); +} + +double base::TransformWithCovariance::sign(double v) +{ + return v > 0.0 ? 1.0 : -1.0; +} + +Eigen::Matrix< double, int(3), int(3) > base::TransformWithCovariance::skew_symmetric(const Eigen::Vector3d& r) +{ + Eigen::Matrix3d res; + res << 0, -r.z(), r.y(), + r.z(), 0, -r.x(), + -r.y(), r.x(), 0; + return res; +} + +Eigen::Matrix< double, int(4), int(3) > base::TransformWithCovariance::dq_by_dr(const Eigen::Quaterniond& q) +{ + const Eigen::Vector3d r( q_to_r( q ) ); + + const double theta = r.norm(); + const double kappa = 0.5 - theta*theta / 48.0; // approx. see Paper + const double lambda = 1.0/24.0*(1.0-theta*theta/40.0); // approx. + Eigen::Matrix res; + res << - q.vec().transpose()/2.0, + kappa * Eigen::Matrix3d::Identity() - lambda * r * r.transpose(); + + return res; +} + +Eigen::Matrix< double, int(3), int(4) > base::TransformWithCovariance::dr_by_dq(const Eigen::Quaterniond& q) +{ + const Eigen::Vector3d r( q_to_r( q ) ); + const double mu = q.vec().norm(); + const double tau = 2.0 * sign( q.w() ) * ( 1.0 + mu*mu/6.0 ); // approx + const double nu = -2.0 * sign( q.w() ) * ( 2.0/3.0 + mu*mu/5.0 ); // approx + + Eigen::Matrix res; + res << -2*q.vec(), tau * Eigen::Matrix3d::Identity() + nu * q.vec() * q.vec().transpose(); + + return res; +} + +Eigen::Matrix< double, int(4), int(4) > base::TransformWithCovariance::dq2q1_by_dq1(const Eigen::Quaterniond& q2) +{ + Eigen::Matrix res; + res << 0, -q2.vec().transpose(), + q2.vec(), skew_symmetric( q2.vec() ); + return Eigen::Matrix::Identity() * q2.w() + res; +} + +Eigen::Matrix< double, int(4), int(4) > base::TransformWithCovariance::dq2q1_by_dq2(const Eigen::Quaterniond& q1) +{ + Eigen::Matrix res; + res << 0, -q1.vec().transpose(), + q1.vec(), -skew_symmetric( q1.vec() ); + return Eigen::Matrix::Identity() * q1.w() + res; +} + +Eigen::Matrix< double, int(3), int(3) > base::TransformWithCovariance::dr2r1_by_r1(const Eigen::Quaterniond& q, const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2) +{ + return Eigen::Matrix3d( + dr_by_dq( q ) + * dq2q1_by_dq1( q2 ) + * dq_by_dr( q1 ) ); +} + +Eigen::Matrix< double, int(3), int(3) > base::TransformWithCovariance::dr2r1_by_r2(const Eigen::Quaterniond& q, const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2) +{ + return Eigen::Matrix3d( + dr_by_dq( q ) + * dq2q1_by_dq2( q1 ) + * dq_by_dr( q2 ) ); +} + +Eigen::Matrix< double, int(3), int(3) > base::TransformWithCovariance::drx_by_dr(const Eigen::Quaterniond& q, const Eigen::Vector3d& x) +{ + const Eigen::Vector3d r( q_to_r( q ) ); + const double theta = r.norm(); + const double alpha = 1.0 - theta*theta/6.0; + const double beta = 0.5 - theta*theta/24.0; + const double gamma = 1.0 / 3.0 - theta*theta/30.0; + const double delta = -1.0 / 12.0 + theta*theta/180.0; + + return Eigen::Matrix3d( + -skew_symmetric(x)*(gamma*r*r.transpose() + - beta*skew_symmetric(r)+alpha*Eigen::Matrix3d::Identity()) + -skew_symmetric(r)*skew_symmetric(x)*(delta*r*r.transpose() + + 2.0*beta*Eigen::Matrix3d::Identity()) ); +} + +std::ostream& base::operator<<(std::ostream& out, const base::TransformWithCovariance& trans) +{ + /** cout the 6D pose vector (translation and scaled axis orientation) with its associated covariance matrix **/ + base::Vector3d scaled_axis; + base::AngleAxisd angle_axis (trans.orientation); + scaled_axis = angle_axis.axis() * angle_axis.angle(); + for (register unsigned short i=0; i // std::setprecision + +#include +#include +#include +#include + +#include +#include +#include + +namespace base { + + /** + * Class which represents a 3D Transformation with associated uncertainty information. + * + * The uncertainty is represented as a 6x6 matrix, which is the covariance + * matrix of the [r t] representation of the error. Here r is the rotation orientation + * part expressed as a scaled axis of orientation, and t the translational + * component. + * + * The uncertainty information is optional. The hasValidCovariance() method can + * be used to see if uncertainty information is associated with the class. + */ + class TransformWithCovariance + { + + public: + typedef base::Matrix6d Covariance; + + public: + /** The transformation is represented 6D vector [translation orientation] + * Here orientation is the rotational part expressed as a quaternion + * orientation, and t the translational component. + */ + base::Position translation; + + base::Quaterniond orientation; + + /** The uncertainty is represented as a 6x6 matrix, which is the covariance + * matrix of the [translation orientation] representation of the error. + */ + Covariance cov; + + public: + + TransformWithCovariance(); + + explicit TransformWithCovariance( const base::Affine3d& trans); + + TransformWithCovariance( const base::Affine3d& trans, const Covariance& cov ); + + TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation); + + TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation, const Covariance& cov ); + + static TransformWithCovariance Identity(); + + + /** Default std::cout function + */ + friend std::ostream & operator<<(std::ostream &out, const TransformWithCovariance& trans); + + /** performs a composition of this transform with the transform given. + * The result is another transform with result = this * trans + */ + TransformWithCovariance composition( const TransformWithCovariance& trans ) const; + + /** performs an inverse composition of two transformations. + * The result is such that result * trans = this. Note that this is different from + * calling result = this * inv(trans), in the way the uncertainties are handled. + */ + TransformWithCovariance compositionInv( const TransformWithCovariance& trans ) const; + + /** Same as compositionInv, just that the result is such that trans * result = this. + * Note that this is different from calling result = inv(trans) * this, + * in the way the uncertainties are handled. + */ + TransformWithCovariance preCompositionInv( const TransformWithCovariance& trans ) const; + + /** alias for the composition of two transforms + */ + TransformWithCovariance operator*( const TransformWithCovariance& trans ) const; + + TransformWithCovariance inverse() const; + + const Covariance& getCovariance() const; + void setCovariance( const Covariance& cov ); + + const base::Matrix3d getTranslationCov() const; + void setTranslationCov(const base::Matrix3d& cov); + + const base::Matrix3d getOrientationCov() const; + void setOrientationCov(const base::Matrix3d& cov); + + const base::Affine3d getTransform() const; + + void setTransform( const base::Affine3d& trans ); + + const base::Orientation getOrientation() const; + + void setOrientation(const base::Orientation & q); + + bool hasValidTransform() const; + + void invalidateTransform(); + + /** @warning This method is computationally expensive. Use with care! */ + bool hasValidCovariance() const; + void invalidateCovariance(); + + protected: + // The uncertainty transformations are implemented according to: + // Pennec X, Thirion JP. A framework for uncertainty and validation of 3-D + // registration methods based on points and frames. International Journal of + // Computer Vion. 1997;25(3):203–229. Available at: + // http://www.springerlink.com/index/JJ25N2Q23T402682.pdf. + + static Eigen::Quaterniond r_to_q( const Eigen::Vector3d& r ); + + static Eigen::Vector3d q_to_r( const Eigen::Quaterniond& q ); + + static inline double sign( double v ); + + static Eigen::Matrix skew_symmetric( const Eigen::Vector3d& r ); + + static Eigen::Matrix dq_by_dr( const Eigen::Quaterniond& q ); + + static Eigen::Matrix dr_by_dq( const Eigen::Quaterniond& q ); + + static Eigen::Matrix dq2q1_by_dq1( const Eigen::Quaterniond& q2 ); + + static Eigen::Matrix dq2q1_by_dq2( const Eigen::Quaterniond& q1 ); + + static Eigen::Matrix dr2r1_by_r1( const Eigen::Quaterniond& q, const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2 ); + + static Eigen::Matrix dr2r1_by_r2( const Eigen::Quaterniond& q, const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2 ); + + static Eigen::Matrix drx_by_dr( const Eigen::Quaterniond& q, const Eigen::Vector3d& x ); + }; + + /** Default std::cout function + */ + inline std::ostream & operator<<(std::ostream &out, const TransformWithCovariance& trans); +} // namespaces + +#endif From 416b212371cd1ef46e1f1c1bcd9668aaa6afde2a Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 14 Jul 2016 15:34:00 +0200 Subject: [PATCH 11/50] splitted Timueout --- src/Timeout.cpp | 50 +++++++++++++++++++++++++++++++++++++++ {base => src}/Timeout.hpp | 0 2 files changed, 50 insertions(+) create mode 100644 src/Timeout.cpp rename {base => src}/Timeout.hpp (100%) diff --git a/src/Timeout.cpp b/src/Timeout.cpp new file mode 100644 index 00000000..9ad8b39b --- /dev/null +++ b/src/Timeout.cpp @@ -0,0 +1,50 @@ +#include "Timeout.hpp" + +base::Timeout::Timeout(base::Time timeout) : timeout(timeout) +{ + start_time = base::Time::now(); +} + +void base::Timeout::restart() +{ + start_time = base::Time::now(); +} + +bool base::Timeout::elapsed() const +{ + return elapsed(timeout); +} + +bool base::Timeout::elapsed(const base::Time& timeout) const +{ + if(!timeout.isNull()) + { + return start_time + timeout < base::Time::now(); + } + else + { + return false; + } +} + +base::Time base::Timeout::timeLeft() const +{ + return timeLeft(timeout); +} + +base::Time base::Timeout::timeLeft(const base::Time& timeout) const +{ + if(!timeout.isNull()) + { + return start_time + timeout - base::Time::now(); + } + else + { + return base::Time::fromSeconds(0); + } +} + + + + + diff --git a/base/Timeout.hpp b/src/Timeout.hpp similarity index 100% rename from base/Timeout.hpp rename to src/Timeout.hpp From faadc05373c6e850dbb06560d03aa9369213735e Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 14 Jul 2016 15:34:23 +0200 Subject: [PATCH 12/50] moved headers to cpp files --- base/samples/DepthMap.hpp | 2 ++ src/Angle.hpp | 2 ++ src/CMakeLists.txt | 13 +++++++- src/Pose.hpp | 2 +- src/Timeout.hpp | 34 ++++----------------- src/Trajectory.cpp | 12 ++++++++ {base => src}/Trajectory.hpp | 8 ++--- src/TransformWithCovariance.cpp | 9 ++++++ src/TransformWithCovariance.hpp | 8 ----- src/TwistWithCovariance.cpp | 41 +++++++++++++++++++++++-- src/TwistWithCovariance.hpp | 53 +++++++-------------------------- 11 files changed, 94 insertions(+), 90 deletions(-) create mode 100644 src/Trajectory.cpp rename {base => src}/Trajectory.hpp (85%) diff --git a/base/samples/DepthMap.hpp b/base/samples/DepthMap.hpp index 58382d95..bfca4fe8 100644 --- a/base/samples/DepthMap.hpp +++ b/base/samples/DepthMap.hpp @@ -9,6 +9,8 @@ #include #include +#include + namespace base { namespace samples { /** diff --git a/src/Angle.hpp b/src/Angle.hpp index 1edbef8b..2fa3e798 100644 --- a/src/Angle.hpp +++ b/src/Angle.hpp @@ -6,6 +6,8 @@ #include #include +#include + namespace base { diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index bfaee811..3bb6e906 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,16 +1,27 @@ set(SOURCES Angle.cpp Pose.cpp + Timeout.cpp + Trajectory.cpp TransformWithCovariance.cpp TwistWithCovariance.cpp Waypoint.cpp ) set(HEADERS - Logging.hpp + Angle.hpp + Eigen.hpp + Float.hpp + Logging.hpp + Pose.hpp Singleton.hpp + Timeout.hpp + Trajectory.hpp + TransformWithCovariance.hpp + TwistWithCovariance.hpp logging/logging_iostream_style.h logging/logging_printf_style.h + Waypoint.hpp Wrench.hpp ) diff --git a/src/Pose.hpp b/src/Pose.hpp index 0fd425a2..08667e87 100644 --- a/src/Pose.hpp +++ b/src/Pose.hpp @@ -1,7 +1,7 @@ #ifndef __BASE_POSE_HH__ #define __BASE_POSE_HH__ -#include +#include "Eigen.hpp" #include "Angle.hpp" namespace base diff --git a/src/Timeout.hpp b/src/Timeout.hpp index df494a60..8618d55e 100644 --- a/src/Timeout.hpp +++ b/src/Timeout.hpp @@ -17,27 +17,19 @@ class Timeout { * Initializes and starts a timeout * @param timeout, if zero is given the timeout is inactive */ - Timeout(base::Time timeout = base::Time::fromSeconds(0)) - : timeout(timeout) - { - start_time = base::Time::now(); - } + Timeout(base::Time timeout = base::Time::fromSeconds(0)); /** * Restarts the timeout */ - void restart(){ - start_time = base::Time::now(); - } + void restart(); /** * Checks if the timeout is already elapsed. * This uses a syscall, so use sparingly and cache results * @returns true if the timeout is elapsed */ - bool elapsed() const{ - return elapsed(timeout); - } + bool elapsed() const; /** * Checks if the timeout is already elapsed. @@ -45,22 +37,14 @@ class Timeout { * @param timeout a custom timeout * @returns true if the timeout is elapsed */ - bool elapsed(const base::Time &timeout) const{ - if(!timeout.isNull()){ - return start_time + timeout < base::Time::now(); - }else{ - return false; - } - } + bool elapsed(const base::Time &timeout) const; /** * Calculates the time left for this timeout * This uses a syscall, so use sparingly and cache results * @returns number of milliseconds this timeout as left */ - base::Time timeLeft() const{ - return timeLeft(timeout); - } + base::Time timeLeft() const; /** * Calculates the time left for this timeout @@ -68,13 +52,7 @@ class Timeout { * @param timeout a custom timeout * @returns number of milliseconds this timeout as left */ - base::Time timeLeft(const base::Time &timeout) const{ - if(!timeout.isNull()){ - return start_time + timeout - base::Time::now(); - }else{ - return base::Time::fromSeconds(0); - } - } + base::Time timeLeft(const base::Time &timeout) const; }; diff --git a/src/Trajectory.cpp b/src/Trajectory.cpp new file mode 100644 index 00000000..e91f7d21 --- /dev/null +++ b/src/Trajectory.cpp @@ -0,0 +1,12 @@ +#include "Trajectory.hpp" + +base::Trajectory::Trajectory() : speed(0) +{ + +} + +bool base::Trajectory::driveForward() const +{ + return speed >= 0; +} + diff --git a/base/Trajectory.hpp b/src/Trajectory.hpp similarity index 85% rename from base/Trajectory.hpp rename to src/Trajectory.hpp index 9aec6d1b..d11c3476 100644 --- a/base/Trajectory.hpp +++ b/src/Trajectory.hpp @@ -8,9 +8,7 @@ namespace base struct Trajectory { - Trajectory(): speed(0) - { - } + Trajectory(); /** * The speed in which the trajectory should be @@ -23,9 +21,7 @@ struct Trajectory * on the trajectory. * False if the robot should drive backwards. * */ - bool driveForward() const{ - return speed >= 0; - } + bool driveForward() const; /** * A spline representing the trajectory that should diff --git a/src/TransformWithCovariance.cpp b/src/TransformWithCovariance.cpp index 910bd770..69c73311 100644 --- a/src/TransformWithCovariance.cpp +++ b/src/TransformWithCovariance.cpp @@ -1,5 +1,14 @@ #include "TransformWithCovariance.hpp" +#include // std::setprecision + +#include +#include +#include +#include + +#include + base::TransformWithCovariance::TransformWithCovariance() : translation(base::Position::Zero()) , orientation(base::Quaterniond::Identity()) { diff --git a/src/TransformWithCovariance.hpp b/src/TransformWithCovariance.hpp index bdc3c01a..23b55f03 100644 --- a/src/TransformWithCovariance.hpp +++ b/src/TransformWithCovariance.hpp @@ -1,14 +1,6 @@ #ifndef __BASE_TRANSFORM_WITH_COVARIANCE_HPP__ #define __BASE_TRANSFORM_WITH_COVARIANCE_HPP__ -#include // std::setprecision - -#include -#include -#include -#include - -#include #include #include diff --git a/src/TwistWithCovariance.cpp b/src/TwistWithCovariance.cpp index a4b6843e..646d219d 100644 --- a/src/TwistWithCovariance.cpp +++ b/src/TwistWithCovariance.cpp @@ -1,5 +1,40 @@ #include "TwistWithCovariance.hpp" +#include // std::setprecision + +#include +#include +#include +#include + +#include + +// Guarantee Semi-Positive Definite (SPD) matrix. +template +static _MatrixType guaranteeSPD (const _MatrixType &A) +{ + _MatrixType spdA; + Eigen::VectorXd s; + s.resize(A.rows(), 1); + + /** + * Single Value Decomposition + */ + Eigen::JacobiSVD svdOfA (A, Eigen::ComputeThinU | Eigen::ComputeThinV); + + s = svdOfA.singularValues(); //!eigenvalues + + for (register int i=0; ihasValidCovariance() && arg.hasValidCovariance()) { this->cov += arg.cov; - base::guaranteeSPD(this->cov); + guaranteeSPD(this->cov); } return *this; } @@ -187,7 +222,7 @@ base::TwistWithCovariance& base::TwistWithCovariance::operator-=(const base::Twi if (this->hasValidCovariance() && arg.hasValidCovariance()) { this->cov += arg.cov; - base::guaranteeSPD(this->cov); + guaranteeSPD(this->cov); } return *this; @@ -258,7 +293,7 @@ base::TwistWithCovariance operator*(const base::TwistWithCovariance& lhs, const /** Angular velocity is at the first covariance block **/ tmp.cov.block<3,3>(3,3) = cross_jacob * cross_cov * cross_jacob.transpose(); - base::guaranteeSPD(tmp.cov); + guaranteeSPD(tmp.cov); } return tmp; diff --git a/src/TwistWithCovariance.hpp b/src/TwistWithCovariance.hpp index a1998ef4..85cc27de 100644 --- a/src/TwistWithCovariance.hpp +++ b/src/TwistWithCovariance.hpp @@ -1,43 +1,10 @@ #ifndef __BASE_TWIST_WITH_COVARIANCE_HPP__ #define __BASE_TWIST_WITH_COVARIANCE_HPP__ -#include // std::setprecision - -#include -#include -#include -#include - -#include #include namespace base { - // Guarantee Semi-Positive Definite (SPD) matrix. - template - static _MatrixType guaranteeSPD (const _MatrixType &A) - { - _MatrixType spdA; - Eigen::VectorXd s; - s.resize(A.rows(), 1); - - /** - * Single Value Decomposition - */ - Eigen::JacobiSVD svdOfA (A, Eigen::ComputeThinU | Eigen::ComputeThinV); - - s = svdOfA.singularValues(); //!eigenvalues - - for (register int i=0; i crossJacobian(const base::Vector3d& u, const base::Vector3d& v); @@ -140,7 +107,7 @@ namespace base { /** Default std::cout function */ - inline std::ostream & operator<<(std::ostream &out, const base::TwistWithCovariance& twist); + std::ostream & operator<<(std::ostream &out, const base::TwistWithCovariance& twist); } // namespaces From 284c78cb9a9f305ab760df484e3b66195628021c Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 14 Jul 2016 15:50:39 +0200 Subject: [PATCH 13/50] splitted TimeMark --- base/TimeMark.hpp | 39 --------------------------------------- src/CMakeLists.txt | 2 ++ src/TimeMark.cpp | 25 +++++++++++++++++++++++++ src/TimeMark.hpp | 29 +++++++++++++++++++++++++++++ 4 files changed, 56 insertions(+), 39 deletions(-) delete mode 100644 base/TimeMark.hpp create mode 100644 src/TimeMark.cpp create mode 100644 src/TimeMark.hpp diff --git a/base/TimeMark.hpp b/base/TimeMark.hpp deleted file mode 100644 index ae05987b..00000000 --- a/base/TimeMark.hpp +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef BASE_TIMEMARK_H__ -#define BASE_TIMEMARK_H__ - -#include -#include - -namespace base { - -struct TimeMark -{ - std::string label; - Time mark; - clock_t clock; - - TimeMark(const std::string& label) : label(label), mark( Time::now() ), clock( ::clock() ) {}; - - /** Return the time that has passed since the recorded time and now */ - Time passed() const - { - return (Time::now() - mark); - } - - clock_t cycles() const - { - return ::clock() - clock; - } -}; - - -inline std::ostream &operator<<(std::ostream &stream, const TimeMark &ob) -{ - stream << ob.cycles() << "cyc (" << ob.passed() << "s) since " << ob.label; - return stream; -} - -} // namespace base - - -#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3bb6e906..afad300f 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,6 +1,7 @@ set(SOURCES Angle.cpp Pose.cpp + TimeMark.cpp Timeout.cpp Trajectory.cpp TransformWithCovariance.cpp @@ -15,6 +16,7 @@ set(HEADERS Logging.hpp Pose.hpp Singleton.hpp + TimeMark.hpp Timeout.hpp Trajectory.hpp TransformWithCovariance.hpp diff --git a/src/TimeMark.cpp b/src/TimeMark.cpp new file mode 100644 index 00000000..02ba8070 --- /dev/null +++ b/src/TimeMark.cpp @@ -0,0 +1,25 @@ +#include "TimeMark.hpp" + +base::TimeMark::TimeMark(const std::__cxx11::string& label) : label(label), mark( Time::now() ), clock( ::clock() ) +{ + +} + +base::Time base::TimeMark::passed() const +{ + return (Time::now() - mark); +} + +clock_t base::TimeMark::cycles() const +{ + return ::clock() - clock; +} + +std::ostream& base::operator<<(std::ostream& stream, const base::TimeMark& ob) +{ + stream << ob.cycles() << "cyc (" << ob.passed() << "s) since " << ob.label; + return stream; +} + + + diff --git a/src/TimeMark.hpp b/src/TimeMark.hpp new file mode 100644 index 00000000..85426198 --- /dev/null +++ b/src/TimeMark.hpp @@ -0,0 +1,29 @@ +#ifndef BASE_TIMEMARK_H__ +#define BASE_TIMEMARK_H__ + +#include +#include + +namespace base { + +struct TimeMark +{ + std::string label; + Time mark; + clock_t clock; + + TimeMark(const std::string& label); + + /** Return the time that has passed since the recorded time and now */ + Time passed() const; + + clock_t cycles() const; +}; + + +std::ostream &operator<<(std::ostream &stream, const TimeMark &ob); + +} // namespace base + + +#endif From cdddb408d94068b7444323e08280b57adf946d4a Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 14 Jul 2016 16:10:16 +0200 Subject: [PATCH 14/50] removed inlining --- src/Time.hpp | 252 ++++++++++++++++++++++++++++++++ src/TransformWithCovariance.hpp | 2 +- 2 files changed, 253 insertions(+), 1 deletion(-) create mode 100644 src/Time.hpp diff --git a/src/Time.hpp b/src/Time.hpp new file mode 100644 index 00000000..7e984181 --- /dev/null +++ b/src/Time.hpp @@ -0,0 +1,252 @@ +#ifndef BASE_TIME_H__ +#define BASE_TIME_H__ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace base +{ + struct Time + { + private: + explicit Time(int64_t _microseconds) + : microseconds(_microseconds) { } + public: + int64_t microseconds; + + static const int UsecPerSec = 1000000LL; + + enum Resolution { Seconds = 1, Milliseconds = 1000, Microseconds = 1000000 }; + + Time() + : microseconds(0) {} + + + public: + /** Returns the current time */ + static Time now() { + timeval t; + gettimeofday(&t, 0); + return Time(static_cast(t.tv_sec) * UsecPerSec + t.tv_usec); + } + + bool operator < (Time const& ts) const + { return microseconds < ts.microseconds; } + bool operator > (Time const& ts) const + { return microseconds > ts.microseconds; } + bool operator == (Time const& ts) const + { return microseconds == ts.microseconds; } + bool operator != (Time const& ts) const + { return !(*this == ts); } + bool operator >= (Time const& ts) const + { return !(*this < ts); } + bool operator <= (Time const& ts) const + { return !(*this > ts); } + Time operator - (Time const& ts) const + { return Time(microseconds - ts.microseconds); } + Time operator + (Time const& ts) const + { return Time(microseconds + ts.microseconds); } + Time operator / (int divider) const + { return Time(microseconds / divider); } + Time operator * (double factor) const + { return Time(microseconds * factor); } + + /** True if this time is zero */ + bool isNull() const { return microseconds == 0; } + + /** Converts this time as a timeval object */ + timeval toTimeval() const + { + timeval tv = { static_cast(microseconds / UsecPerSec), static_cast(microseconds % UsecPerSec) }; + return tv; + } + + /** Convert time into a string + * \param resolution Resolution which the string should present + * \param mainFormat Main format to use -- this is passed to strftime and appended by ':' plus the + * below seconds resolution if requested by the resolution argument + **/ + std::string toString(base::Time::Resolution resolution = Microseconds, const std::string& mainFormat = "%Y%m%d-%H:%M:%S") const + { + struct timeval tv = toTimeval(); + int uSecs = tv.tv_usec; + + time_t when = tv.tv_sec; + struct tm *tm = localtime(&when); + + char time[50]; + strftime(time,50, mainFormat.c_str(), tm); + + char buffer[57]; + switch(resolution) + { + case Seconds: + return std::string(time); + case Milliseconds: + sprintf(buffer,"%s:%03d", time, (int) (uSecs/1000.0)); + break; + case Microseconds: + sprintf(buffer,"%s:%06d", time, uSecs); + break; + default: + throw std::invalid_argument( + "base::Time::toString(): invalid " + "value in switch-statement"); + } + + return std::string(buffer); + } + + /** Returns this time as a fractional number of seconds */ + double toSeconds() const + { return static_cast(microseconds) / UsecPerSec; } + /** Returns this time as an integer number of milliseconds (thus dropping the microseconds) */ + int64_t toMilliseconds() const + { return microseconds / 1000; } + /** Returns this time as an integer number of microseconds */ + int64_t toMicroseconds() const + { return microseconds; } + static Time fromMicroseconds(uint64_t value) + { return Time(value); } + static Time fromMilliseconds(uint64_t value) + { return Time(value * 1000); } + static Time fromSeconds(int64_t value) + { return Time(value * UsecPerSec); } + static Time fromSeconds(int value) + { return Time(static_cast(value) * UsecPerSec); } + static Time fromSeconds(int64_t value, int microseconds) + { return Time(value * UsecPerSec + static_cast(microseconds)); } + static Time fromSeconds(double value) + { + int64_t seconds = value; + return Time(seconds * UsecPerSec + static_cast(round((value - seconds) * UsecPerSec))); + } + + + /** + * \brief Create time from int Time Values. + * Creates a time object from the time values (year, month, day ...) given as integer values. This function can be used + * when the time values are only available as seperated values in numerical form. + * \param year The year as integer value. (should be 4 digits) + * \param month The month of the year (1..12). + * \param day Day of the month (1..31). + * \param hour The hour of the day (since midnight 0..23). + * \param minute The minutes after the hour (0..59). + * \param seconds The seconds after the minute (0..59) + * \param millis Milliseconds after the last second (0..999) + * \param micros Microseconds additional to the milliseconds (0..999) + * \returns a Time object generated from the parameters. + * + */ + static Time fromTimeValues(int year, int month, int day, int hour, int minute, int seconds, int millis, int micros) + { + + struct tm timeobj; + timeobj.tm_year = year - 1900; + timeobj.tm_mon = month - 1; + timeobj.tm_mday = day; + timeobj.tm_hour = hour; + timeobj.tm_min = minute; + timeobj.tm_sec = seconds; + timeobj.tm_isdst = -1; + + time_t tTime; + tTime = mktime(&timeobj); + + int64_t timeVal = static_cast(tTime); + + timeVal = timeVal * UsecPerSec; + timeVal += millis * 1000 + micros; + + + return Time(timeVal); + + } + + + /** + * Create a time object from an input string, by default all parameters are set to convert the string returned + * by toString back to a Time object. + * \param stringTime String describing the time + * \param resolution Set to a resolution higher than Secs if a (non-standard) msec or usec field is present, i.e. the non standard field is separated by ':' + * \param mainFormat valid format for strptime, e.g."%Y%m%d-%H:%M:%S" which the given time string has + * \throws std::runtime_error on failure such as a mismatching format + */ + static Time fromString(const std::string& stringTime, Resolution resolution = Microseconds, const std::string& mainFormat = "%Y%m%d-%H:%M:%S") + { + std::string mainTime = stringTime; + int32_t usecs = 0; + if(resolution > Seconds) + { + size_t pos = stringTime.find_last_of(':'); + std::string usecsString = stringTime.substr(pos+1); + size_t usecsStringLength = usecsString.size(); + if( (usecsStringLength == 3 || usecsStringLength == 6) && !(usecsStringLength == 3 && resolution > Milliseconds)) + { + // string matches resolutions + } else + { + throw std::runtime_error("base::Time::fromString failed - resolution does not match provided Time-String"); + } + + switch(resolution) + { + case Milliseconds: + sscanf(usecsString.c_str(), "%03d", &usecs); + usecs = usecs*1000; + break; + case Microseconds: + sscanf(usecsString.c_str(), "%06d", &usecs); + break; + case Seconds: + throw std::invalid_argument( + "base::Time::fromString(); " + "'Seconds' is an invalid case " + "here"); + default: + throw std::invalid_argument("base::Time::fromString(): " + "invalid value in " + "switch-statement"); + } + } + + struct tm tm; + if(NULL == strptime(mainTime.c_str(), mainFormat.c_str(), &tm)) + { + throw std::runtime_error("base::Time::fromString failed- Time-String '" + mainTime + "' did not match the given format '" + mainFormat +"'"); + } + // " ... not set by strptime(); tells mktime() to determine + // whether daylight saving time is in effect ..." + // (http://pubs.opengroup.org/onlinepubs/007904975/functions/strptime.html) + + tm.tm_isdst = -1; + time_t time = mktime(&tm); + + return Time(static_cast(time)*UsecPerSec + static_cast(usecs)); + } + + }; + + inline std::ostream& operator << (std::ostream& io, base::Time const& time) + { + const int64_t microsecs = time.toMicroseconds(); + + io << (microsecs / 1000000) + << std::setfill('0') + << "." << std::setw(3) << (std::llabs(microsecs) / 1000) % 1000 + << "." << std::setw(3) << (std::llabs(microsecs) % 1000) + << std::setfill(' '); + + return io; + } +} + + +#endif diff --git a/src/TransformWithCovariance.hpp b/src/TransformWithCovariance.hpp index 23b55f03..00fcacf9 100644 --- a/src/TransformWithCovariance.hpp +++ b/src/TransformWithCovariance.hpp @@ -136,7 +136,7 @@ namespace base { /** Default std::cout function */ - inline std::ostream & operator<<(std::ostream &out, const TransformWithCovariance& trans); + std::ostream & operator<<(std::ostream &out, const TransformWithCovariance& trans); } // namespaces #endif From f4988cae3ba4364370bafcfee7c6df86a6f8e28d Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 14 Jul 2016 16:10:24 +0200 Subject: [PATCH 15/50] splitted Time --- base/Time.hpp | 252 ------------------------------------------ src/CMakeLists.txt | 2 + src/Time.cpp | 270 +++++++++++++++++++++++++++++++++++++++++++++ src/Time.hpp | 224 +++++++------------------------------ 4 files changed, 312 insertions(+), 436 deletions(-) delete mode 100644 base/Time.hpp create mode 100644 src/Time.cpp diff --git a/base/Time.hpp b/base/Time.hpp deleted file mode 100644 index 7e984181..00000000 --- a/base/Time.hpp +++ /dev/null @@ -1,252 +0,0 @@ -#ifndef BASE_TIME_H__ -#define BASE_TIME_H__ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace base -{ - struct Time - { - private: - explicit Time(int64_t _microseconds) - : microseconds(_microseconds) { } - public: - int64_t microseconds; - - static const int UsecPerSec = 1000000LL; - - enum Resolution { Seconds = 1, Milliseconds = 1000, Microseconds = 1000000 }; - - Time() - : microseconds(0) {} - - - public: - /** Returns the current time */ - static Time now() { - timeval t; - gettimeofday(&t, 0); - return Time(static_cast(t.tv_sec) * UsecPerSec + t.tv_usec); - } - - bool operator < (Time const& ts) const - { return microseconds < ts.microseconds; } - bool operator > (Time const& ts) const - { return microseconds > ts.microseconds; } - bool operator == (Time const& ts) const - { return microseconds == ts.microseconds; } - bool operator != (Time const& ts) const - { return !(*this == ts); } - bool operator >= (Time const& ts) const - { return !(*this < ts); } - bool operator <= (Time const& ts) const - { return !(*this > ts); } - Time operator - (Time const& ts) const - { return Time(microseconds - ts.microseconds); } - Time operator + (Time const& ts) const - { return Time(microseconds + ts.microseconds); } - Time operator / (int divider) const - { return Time(microseconds / divider); } - Time operator * (double factor) const - { return Time(microseconds * factor); } - - /** True if this time is zero */ - bool isNull() const { return microseconds == 0; } - - /** Converts this time as a timeval object */ - timeval toTimeval() const - { - timeval tv = { static_cast(microseconds / UsecPerSec), static_cast(microseconds % UsecPerSec) }; - return tv; - } - - /** Convert time into a string - * \param resolution Resolution which the string should present - * \param mainFormat Main format to use -- this is passed to strftime and appended by ':' plus the - * below seconds resolution if requested by the resolution argument - **/ - std::string toString(base::Time::Resolution resolution = Microseconds, const std::string& mainFormat = "%Y%m%d-%H:%M:%S") const - { - struct timeval tv = toTimeval(); - int uSecs = tv.tv_usec; - - time_t when = tv.tv_sec; - struct tm *tm = localtime(&when); - - char time[50]; - strftime(time,50, mainFormat.c_str(), tm); - - char buffer[57]; - switch(resolution) - { - case Seconds: - return std::string(time); - case Milliseconds: - sprintf(buffer,"%s:%03d", time, (int) (uSecs/1000.0)); - break; - case Microseconds: - sprintf(buffer,"%s:%06d", time, uSecs); - break; - default: - throw std::invalid_argument( - "base::Time::toString(): invalid " - "value in switch-statement"); - } - - return std::string(buffer); - } - - /** Returns this time as a fractional number of seconds */ - double toSeconds() const - { return static_cast(microseconds) / UsecPerSec; } - /** Returns this time as an integer number of milliseconds (thus dropping the microseconds) */ - int64_t toMilliseconds() const - { return microseconds / 1000; } - /** Returns this time as an integer number of microseconds */ - int64_t toMicroseconds() const - { return microseconds; } - static Time fromMicroseconds(uint64_t value) - { return Time(value); } - static Time fromMilliseconds(uint64_t value) - { return Time(value * 1000); } - static Time fromSeconds(int64_t value) - { return Time(value * UsecPerSec); } - static Time fromSeconds(int value) - { return Time(static_cast(value) * UsecPerSec); } - static Time fromSeconds(int64_t value, int microseconds) - { return Time(value * UsecPerSec + static_cast(microseconds)); } - static Time fromSeconds(double value) - { - int64_t seconds = value; - return Time(seconds * UsecPerSec + static_cast(round((value - seconds) * UsecPerSec))); - } - - - /** - * \brief Create time from int Time Values. - * Creates a time object from the time values (year, month, day ...) given as integer values. This function can be used - * when the time values are only available as seperated values in numerical form. - * \param year The year as integer value. (should be 4 digits) - * \param month The month of the year (1..12). - * \param day Day of the month (1..31). - * \param hour The hour of the day (since midnight 0..23). - * \param minute The minutes after the hour (0..59). - * \param seconds The seconds after the minute (0..59) - * \param millis Milliseconds after the last second (0..999) - * \param micros Microseconds additional to the milliseconds (0..999) - * \returns a Time object generated from the parameters. - * - */ - static Time fromTimeValues(int year, int month, int day, int hour, int minute, int seconds, int millis, int micros) - { - - struct tm timeobj; - timeobj.tm_year = year - 1900; - timeobj.tm_mon = month - 1; - timeobj.tm_mday = day; - timeobj.tm_hour = hour; - timeobj.tm_min = minute; - timeobj.tm_sec = seconds; - timeobj.tm_isdst = -1; - - time_t tTime; - tTime = mktime(&timeobj); - - int64_t timeVal = static_cast(tTime); - - timeVal = timeVal * UsecPerSec; - timeVal += millis * 1000 + micros; - - - return Time(timeVal); - - } - - - /** - * Create a time object from an input string, by default all parameters are set to convert the string returned - * by toString back to a Time object. - * \param stringTime String describing the time - * \param resolution Set to a resolution higher than Secs if a (non-standard) msec or usec field is present, i.e. the non standard field is separated by ':' - * \param mainFormat valid format for strptime, e.g."%Y%m%d-%H:%M:%S" which the given time string has - * \throws std::runtime_error on failure such as a mismatching format - */ - static Time fromString(const std::string& stringTime, Resolution resolution = Microseconds, const std::string& mainFormat = "%Y%m%d-%H:%M:%S") - { - std::string mainTime = stringTime; - int32_t usecs = 0; - if(resolution > Seconds) - { - size_t pos = stringTime.find_last_of(':'); - std::string usecsString = stringTime.substr(pos+1); - size_t usecsStringLength = usecsString.size(); - if( (usecsStringLength == 3 || usecsStringLength == 6) && !(usecsStringLength == 3 && resolution > Milliseconds)) - { - // string matches resolutions - } else - { - throw std::runtime_error("base::Time::fromString failed - resolution does not match provided Time-String"); - } - - switch(resolution) - { - case Milliseconds: - sscanf(usecsString.c_str(), "%03d", &usecs); - usecs = usecs*1000; - break; - case Microseconds: - sscanf(usecsString.c_str(), "%06d", &usecs); - break; - case Seconds: - throw std::invalid_argument( - "base::Time::fromString(); " - "'Seconds' is an invalid case " - "here"); - default: - throw std::invalid_argument("base::Time::fromString(): " - "invalid value in " - "switch-statement"); - } - } - - struct tm tm; - if(NULL == strptime(mainTime.c_str(), mainFormat.c_str(), &tm)) - { - throw std::runtime_error("base::Time::fromString failed- Time-String '" + mainTime + "' did not match the given format '" + mainFormat +"'"); - } - // " ... not set by strptime(); tells mktime() to determine - // whether daylight saving time is in effect ..." - // (http://pubs.opengroup.org/onlinepubs/007904975/functions/strptime.html) - - tm.tm_isdst = -1; - time_t time = mktime(&tm); - - return Time(static_cast(time)*UsecPerSec + static_cast(usecs)); - } - - }; - - inline std::ostream& operator << (std::ostream& io, base::Time const& time) - { - const int64_t microsecs = time.toMicroseconds(); - - io << (microsecs / 1000000) - << std::setfill('0') - << "." << std::setw(3) << (std::llabs(microsecs) / 1000) % 1000 - << "." << std::setw(3) << (std::llabs(microsecs) % 1000) - << std::setfill(' '); - - return io; - } -} - - -#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index afad300f..e9f5efb4 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,6 +1,7 @@ set(SOURCES Angle.cpp Pose.cpp + Time.cpp TimeMark.cpp Timeout.cpp Trajectory.cpp @@ -16,6 +17,7 @@ set(HEADERS Logging.hpp Pose.hpp Singleton.hpp + Time.hpp TimeMark.hpp Timeout.hpp Trajectory.hpp diff --git a/src/Time.cpp b/src/Time.cpp new file mode 100644 index 00000000..c28a0ccf --- /dev/null +++ b/src/Time.cpp @@ -0,0 +1,270 @@ +#include "Time.hpp" + +#include +#include +#include +#include +#include +#include + +base::Time::Time(int64_t _microseconds) : microseconds(_microseconds) +{ + +} + +base::Time::Time() : microseconds(0) +{ + +} + +base::Time base::Time::now() +{ + timeval t; + gettimeofday(&t, 0); + return Time(static_cast(t.tv_sec) * UsecPerSec + t.tv_usec); +} + +bool base::Time::operator<(const base::Time& ts) const +{ + return microseconds < ts.microseconds; +} + +bool base::Time::operator>(const base::Time& ts) const +{ + return microseconds > ts.microseconds; +} + +bool base::Time::operator==(const base::Time& ts) const +{ + return microseconds == ts.microseconds; +} + +bool base::Time::operator!=(const base::Time& ts) const +{ + return !(*this == ts); +} + +bool base::Time::operator>=(const base::Time& ts) const +{ + return !(*this < ts); +} + +bool base::Time::operator<=(const base::Time& ts) const +{ + return !(*this > ts); +} + +base::Time base::Time::operator-(const base::Time& ts) const +{ + return Time(microseconds - ts.microseconds); +} + +base::Time base::Time::operator+(const base::Time& ts) const +{ + return Time(microseconds + ts.microseconds); +} + +base::Time base::Time::operator/(int divider) const +{ + return Time(microseconds / divider); +} + +base::Time base::Time::operator*(double factor) const +{ + return Time(microseconds * factor); +} + +bool base::Time::isNull() const +{ + return microseconds == 0; +} + +timeval base::Time::toTimeval() const +{ + timeval tv = { static_cast(microseconds / UsecPerSec), static_cast(microseconds % UsecPerSec) }; + return tv; +} + +std::__cxx11::string base::Time::toString(base::Time::Resolution resolution, const std::__cxx11::string& mainFormat) const +{ + struct timeval tv = toTimeval(); + int uSecs = tv.tv_usec; + + time_t when = tv.tv_sec; + struct tm *tm = localtime(&when); + + char time[50]; + strftime(time,50, mainFormat.c_str(), tm); + + char buffer[57]; + switch(resolution) + { + case Seconds: + return std::string(time); + case Milliseconds: + sprintf(buffer,"%s:%03d", time, (int) (uSecs/1000.0)); + break; + case Microseconds: + sprintf(buffer,"%s:%06d", time, uSecs); + break; + default: + throw std::invalid_argument( + "base::Time::toString(): invalid " + "value in switch-statement"); + } + + return std::string(buffer); +} + +double base::Time::toSeconds() const +{ + return static_cast(microseconds) / UsecPerSec; +} + +int64_t base::Time::toMilliseconds() const +{ + return microseconds / 1000; +} + +int64_t base::Time::toMicroseconds() const +{ + return microseconds; +} + +base::Time base::Time::fromMicroseconds(uint64_t value) +{ + return Time(value); +} + +base::Time base::Time::fromMilliseconds(uint64_t value) +{ + return Time(value * 1000); +} + +base::Time base::Time::fromSeconds(int64_t value) +{ + return Time(value * UsecPerSec); +} + +base::Time base::Time::fromSeconds(int value) +{ + return Time(static_cast(value) * UsecPerSec); +} + +base::Time base::Time::fromSeconds(int64_t value, int microseconds) +{ + return Time(value * UsecPerSec + static_cast(microseconds)); +} + +base::Time base::Time::fromSeconds(double value) +{ + int64_t seconds = value; + return Time(seconds * UsecPerSec + static_cast(round((value - seconds) * UsecPerSec))); +} + +base::Time base::Time::fromTimeValues(int year, int month, int day, int hour, int minute, int seconds, int millis, int micros) +{ + struct tm timeobj; + timeobj.tm_year = year - 1900; + timeobj.tm_mon = month - 1; + timeobj.tm_mday = day; + timeobj.tm_hour = hour; + timeobj.tm_min = minute; + timeobj.tm_sec = seconds; + timeobj.tm_isdst = -1; + + time_t tTime; + tTime = mktime(&timeobj); + + int64_t timeVal = static_cast(tTime); + + timeVal = timeVal * UsecPerSec; + timeVal += millis * 1000 + micros; + + + return Time(timeVal); +} + +base::Time base::Time::fromString(const std::__cxx11::string& stringTime, base::Time::Resolution resolution, const std::__cxx11::string& mainFormat) +{ + std::string mainTime = stringTime; + int32_t usecs = 0; + if(resolution > Seconds) + { + size_t pos = stringTime.find_last_of(':'); + std::string usecsString = stringTime.substr(pos+1); + size_t usecsStringLength = usecsString.size(); + if( (usecsStringLength == 3 || usecsStringLength == 6) && !(usecsStringLength == 3 && resolution > Milliseconds)) + { + // string matches resolutions + } else + { + throw std::runtime_error("base::Time::fromString failed - resolution does not match provided Time-String"); + } + + switch(resolution) + { + case Milliseconds: + sscanf(usecsString.c_str(), "%03d", &usecs); + usecs = usecs*1000; + break; + case Microseconds: + sscanf(usecsString.c_str(), "%06d", &usecs); + break; + case Seconds: + throw std::invalid_argument( + "base::Time::fromString(); " + "'Seconds' is an invalid case " + "here"); + default: + throw std::invalid_argument("base::Time::fromString(): " + "invalid value in " + "switch-statement"); + } + } + + struct tm tm; + if(NULL == strptime(mainTime.c_str(), mainFormat.c_str(), &tm)) + { + throw std::runtime_error("base::Time::fromString failed- Time-String '" + mainTime + "' did not match the given format '" + mainFormat +"'"); + } + // " ... not set by strptime(); tells mktime() to determine + // whether daylight saving time is in effect ..." + // (http://pubs.opengroup.org/onlinepubs/007904975/functions/strptime.html) + + tm.tm_isdst = -1; + time_t time = mktime(&tm); + + return Time(static_cast(time)*UsecPerSec + static_cast(usecs)); +} + + +std::ostream& base::operator<<(std::ostream& io, const base::Time& time) +{ + const int64_t microsecs = time.toMicroseconds(); + + io << (microsecs / 1000000) + << std::setfill('0') + << "." << std::setw(3) << (std::llabs(microsecs) / 1000) % 1000 + << "." << std::setw(3) << (std::llabs(microsecs) % 1000) + << std::setfill(' '); + + return io; +} + + + + + + + + + + + + + + + + + diff --git a/src/Time.hpp b/src/Time.hpp index 7e984181..e1178325 100644 --- a/src/Time.hpp +++ b/src/Time.hpp @@ -1,23 +1,17 @@ #ifndef BASE_TIME_H__ #define BASE_TIME_H__ -#include -#include -#include -#include #include -#include #include -#include -#include +#include namespace base { struct Time { private: - explicit Time(int64_t _microseconds) - : microseconds(_microseconds) { } + explicit Time(int64_t _microseconds); + public: int64_t microseconds; @@ -25,109 +19,57 @@ namespace base enum Resolution { Seconds = 1, Milliseconds = 1000, Microseconds = 1000000 }; - Time() - : microseconds(0) {} + Time(); public: /** Returns the current time */ - static Time now() { - timeval t; - gettimeofday(&t, 0); - return Time(static_cast(t.tv_sec) * UsecPerSec + t.tv_usec); - } - - bool operator < (Time const& ts) const - { return microseconds < ts.microseconds; } - bool operator > (Time const& ts) const - { return microseconds > ts.microseconds; } - bool operator == (Time const& ts) const - { return microseconds == ts.microseconds; } - bool operator != (Time const& ts) const - { return !(*this == ts); } - bool operator >= (Time const& ts) const - { return !(*this < ts); } - bool operator <= (Time const& ts) const - { return !(*this > ts); } - Time operator - (Time const& ts) const - { return Time(microseconds - ts.microseconds); } - Time operator + (Time const& ts) const - { return Time(microseconds + ts.microseconds); } - Time operator / (int divider) const - { return Time(microseconds / divider); } - Time operator * (double factor) const - { return Time(microseconds * factor); } + static Time now(); + + bool operator < (Time const& ts) const; + bool operator > (Time const& ts) const; + bool operator == (Time const& ts) const; + bool operator != (Time const& ts) const; + bool operator >= (Time const& ts) const; + bool operator <= (Time const& ts) const; + Time operator - (Time const& ts) const; + Time operator + (Time const& ts) const; + Time operator / (int divider) const; + Time operator * (double factor) const; /** True if this time is zero */ - bool isNull() const { return microseconds == 0; } + bool isNull() const; /** Converts this time as a timeval object */ - timeval toTimeval() const - { - timeval tv = { static_cast(microseconds / UsecPerSec), static_cast(microseconds % UsecPerSec) }; - return tv; - } + timeval toTimeval() const; /** Convert time into a string * \param resolution Resolution which the string should present * \param mainFormat Main format to use -- this is passed to strftime and appended by ':' plus the * below seconds resolution if requested by the resolution argument **/ - std::string toString(base::Time::Resolution resolution = Microseconds, const std::string& mainFormat = "%Y%m%d-%H:%M:%S") const - { - struct timeval tv = toTimeval(); - int uSecs = tv.tv_usec; - - time_t when = tv.tv_sec; - struct tm *tm = localtime(&when); - - char time[50]; - strftime(time,50, mainFormat.c_str(), tm); - - char buffer[57]; - switch(resolution) - { - case Seconds: - return std::string(time); - case Milliseconds: - sprintf(buffer,"%s:%03d", time, (int) (uSecs/1000.0)); - break; - case Microseconds: - sprintf(buffer,"%s:%06d", time, uSecs); - break; - default: - throw std::invalid_argument( - "base::Time::toString(): invalid " - "value in switch-statement"); - } - - return std::string(buffer); - } - + std::string toString(base::Time::Resolution resolution = Microseconds, const std::string& mainFormat = "%Y%m%d-%H:%M:%S") const; + /** Returns this time as a fractional number of seconds */ - double toSeconds() const - { return static_cast(microseconds) / UsecPerSec; } + double toSeconds() const; + /** Returns this time as an integer number of milliseconds (thus dropping the microseconds) */ - int64_t toMilliseconds() const - { return microseconds / 1000; } + int64_t toMilliseconds() const; + /** Returns this time as an integer number of microseconds */ - int64_t toMicroseconds() const - { return microseconds; } - static Time fromMicroseconds(uint64_t value) - { return Time(value); } - static Time fromMilliseconds(uint64_t value) - { return Time(value * 1000); } - static Time fromSeconds(int64_t value) - { return Time(value * UsecPerSec); } - static Time fromSeconds(int value) - { return Time(static_cast(value) * UsecPerSec); } - static Time fromSeconds(int64_t value, int microseconds) - { return Time(value * UsecPerSec + static_cast(microseconds)); } - static Time fromSeconds(double value) - { - int64_t seconds = value; - return Time(seconds * UsecPerSec + static_cast(round((value - seconds) * UsecPerSec))); - } + int64_t toMicroseconds() const; + + static Time fromMicroseconds(uint64_t value); + + static Time fromMilliseconds(uint64_t value); + + static Time fromSeconds(int64_t value); + + static Time fromSeconds(int value); + + static Time fromSeconds(int64_t value, int microseconds); + + static Time fromSeconds(double value); /** @@ -145,30 +87,7 @@ namespace base * \returns a Time object generated from the parameters. * */ - static Time fromTimeValues(int year, int month, int day, int hour, int minute, int seconds, int millis, int micros) - { - - struct tm timeobj; - timeobj.tm_year = year - 1900; - timeobj.tm_mon = month - 1; - timeobj.tm_mday = day; - timeobj.tm_hour = hour; - timeobj.tm_min = minute; - timeobj.tm_sec = seconds; - timeobj.tm_isdst = -1; - - time_t tTime; - tTime = mktime(&timeobj); - - int64_t timeVal = static_cast(tTime); - - timeVal = timeVal * UsecPerSec; - timeVal += millis * 1000 + micros; - - - return Time(timeVal); - - } + static Time fromTimeValues(int year, int month, int day, int hour, int minute, int seconds, int millis, int micros); /** @@ -179,73 +98,10 @@ namespace base * \param mainFormat valid format for strptime, e.g."%Y%m%d-%H:%M:%S" which the given time string has * \throws std::runtime_error on failure such as a mismatching format */ - static Time fromString(const std::string& stringTime, Resolution resolution = Microseconds, const std::string& mainFormat = "%Y%m%d-%H:%M:%S") - { - std::string mainTime = stringTime; - int32_t usecs = 0; - if(resolution > Seconds) - { - size_t pos = stringTime.find_last_of(':'); - std::string usecsString = stringTime.substr(pos+1); - size_t usecsStringLength = usecsString.size(); - if( (usecsStringLength == 3 || usecsStringLength == 6) && !(usecsStringLength == 3 && resolution > Milliseconds)) - { - // string matches resolutions - } else - { - throw std::runtime_error("base::Time::fromString failed - resolution does not match provided Time-String"); - } - - switch(resolution) - { - case Milliseconds: - sscanf(usecsString.c_str(), "%03d", &usecs); - usecs = usecs*1000; - break; - case Microseconds: - sscanf(usecsString.c_str(), "%06d", &usecs); - break; - case Seconds: - throw std::invalid_argument( - "base::Time::fromString(); " - "'Seconds' is an invalid case " - "here"); - default: - throw std::invalid_argument("base::Time::fromString(): " - "invalid value in " - "switch-statement"); - } - } - - struct tm tm; - if(NULL == strptime(mainTime.c_str(), mainFormat.c_str(), &tm)) - { - throw std::runtime_error("base::Time::fromString failed- Time-String '" + mainTime + "' did not match the given format '" + mainFormat +"'"); - } - // " ... not set by strptime(); tells mktime() to determine - // whether daylight saving time is in effect ..." - // (http://pubs.opengroup.org/onlinepubs/007904975/functions/strptime.html) - - tm.tm_isdst = -1; - time_t time = mktime(&tm); - - return Time(static_cast(time)*UsecPerSec + static_cast(usecs)); - } - + static Time fromString(const std::string& stringTime, Resolution resolution = Microseconds, const std::string& mainFormat = "%Y%m%d-%H:%M:%S"); }; - inline std::ostream& operator << (std::ostream& io, base::Time const& time) - { - const int64_t microsecs = time.toMicroseconds(); - - io << (microsecs / 1000000) - << std::setfill('0') - << "." << std::setw(3) << (std::llabs(microsecs) / 1000) % 1000 - << "." << std::setw(3) << (std::llabs(microsecs) % 1000) - << std::setfill(' '); - - return io; - } + std::ostream& operator << (std::ostream& io, base::Time const& time); } From cf396ed1d511af2d718a898d2f79213a0392a802 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 14 Jul 2016 16:30:07 +0200 Subject: [PATCH 16/50] splitted Temperature --- base/Temperature.hpp | 175 ------------------------------------------- src/CMakeLists.txt | 2 + src/Temperature.cpp | 120 +++++++++++++++++++++++++++++ src/Temperature.hpp | 113 ++++++++++++++++++++++++++++ 4 files changed, 235 insertions(+), 175 deletions(-) delete mode 100644 base/Temperature.hpp create mode 100644 src/Temperature.cpp create mode 100644 src/Temperature.hpp diff --git a/base/Temperature.hpp b/base/Temperature.hpp deleted file mode 100644 index 37c580e9..00000000 --- a/base/Temperature.hpp +++ /dev/null @@ -1,175 +0,0 @@ -#ifndef __BASE_TEMPERATURE_HH__ -#define __BASE_TEMPERATURE_HH__ - -#include -#include // std::abs -#include - -namespace base -{ - -/** - * This class represents a temperature, and can be used instead of double for - * convenience. The class has a canonical representation of the temperature in - * kelvin. - */ -class Temperature -{ -public: - - /** - * temperature in kelvins - * - * - * @note don't use this value directly. It's only public to allow this class - * to be used as an interface type. - */ - double kelvin; - - /** - * default constructor, which will leave the temperature uninitialized. - */ - Temperature() : kelvin(base::unknown()) {} - -protected: - explicit Temperature( double kelvin ) : kelvin(kelvin) - { - - } - - -public: - /** - * static conversion from kelvin to celsius - * @param kelvin temperature in kelvin - * @result temperature in celsius - */ - static inline double kelvin2Celsius( double kelvin ) - { - return kelvin - 273.15; - } - - /** - * static conversion from celsius to kelvin - * @param celsius temperature in celsius - * @result temperature in kelvin - */ - static inline double celsius2Kelvin( double celsius ) - { - return celsius + 273.15; - } - - /** - * use this method to get temperature from Kelvin. - * @return representation of the given temperature. - * @param kelvin - temperature in Kelvin. - */ - static inline Temperature fromKelvin( double kelvin ) - { - return Temperature( kelvin ); - } - - /** - * use this method to get temperature from Celsius - * @return representation of the given temperature. - * @param celsius - temperature in celsius. - */ - static inline Temperature fromCelsius( double celsius ) - { - return Temperature( celsius + 273.15); - } - - /** - * @return canonical value of the temperature in kelvin - */ - double inline getKelvin() const - { - return kelvin; - } - - /** - * @return canonical value of the temperature in celsius - */ - double inline getCelsius() const - { - return kelvin - 273.15; - } - - /** - * @return true if the temperature is insight the given interval - */ - bool inline isInRange(const Temperature &left_limit,const Temperature &right_limit) const; - - /** - * compare two temperatures for approximate equality - * @param other - temperature to compare - * @param prec - precision interval in kelvin - * @return true if temperature is approximately equal - */ - bool inline isApprox( Temperature other, double prec = 1e-5 ) const - { - return std::abs( other.kelvin - kelvin ) < prec; - } - - void operator=(const Temperature &other) - { - kelvin = other.kelvin; - } - - inline bool operator==(const Temperature &other ) const - { - return this->kelvin == other.kelvin; - } - - inline bool operator<(const Temperature &other ) const - { - return this->kelvin < other.kelvin; - } - - inline bool operator>(const Temperature &other ) const - { - return this->kelvin > other.kelvin; - } - -}; - -static inline Temperature operator+( Temperature a, Temperature b ) -{ - return Temperature::fromKelvin( a.getKelvin() + b.getKelvin() ); -} - -static inline Temperature operator-( Temperature a, Temperature b ) -{ - return Temperature::fromKelvin( a.getKelvin() - b.getKelvin() ); -} - -static inline Temperature operator*( Temperature a, double b ) -{ - return Temperature::fromKelvin( a.getKelvin() * b ); -} - -static inline Temperature operator*( double a, Temperature b ) -{ - return Temperature::fromKelvin( a * b.getKelvin() ); -} - - - -static inline std::ostream& operator << (std::ostream& os, Temperature temperature) -{ - os << temperature.getCelsius() << boost::format("[%3.1f celsius]"); - return os; -} - -bool Temperature::isInRange(const Temperature &left_limit, const Temperature &right_limit) const -{ - if((right_limit-left_limit).kelvin < 0) - return !isInRange(right_limit,left_limit); - if((*this -left_limit).getKelvin() >= 0 && (right_limit -*this).getKelvin() >= 0) - return true; - return false; -} - -} - -#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e9f5efb4..f92687aa 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,6 +1,7 @@ set(SOURCES Angle.cpp Pose.cpp + Temperature.cpp Time.cpp TimeMark.cpp Timeout.cpp @@ -17,6 +18,7 @@ set(HEADERS Logging.hpp Pose.hpp Singleton.hpp + Temperature.hpp Time.hpp TimeMark.hpp Timeout.hpp diff --git a/src/Temperature.cpp b/src/Temperature.cpp new file mode 100644 index 00000000..e7747cb8 --- /dev/null +++ b/src/Temperature.cpp @@ -0,0 +1,120 @@ +#include "Temperature.hpp" + +#include +#include + +base::Temperature::Temperature() : kelvin(base::unknown()) +{ + +} + +base::Temperature::Temperature(double kelvin) : kelvin(kelvin) +{ + +} + +double base::Temperature::kelvin2Celsius(double kelvin) +{ + return kelvin - 273.15; +} + +double base::Temperature::celsius2Kelvin(double celsius) +{ + return celsius + 273.15; +} + +base::Temperature base::Temperature::fromKelvin(double kelvin) +{ + return Temperature( kelvin ); +} + +base::Temperature base::Temperature::fromCelsius(double celsius) +{ + return Temperature( celsius + 273.15); +} + +double base::Temperature::getKelvin() const +{ + return kelvin; +} + +double base::Temperature::getCelsius() const +{ + return kelvin - 273.15; +} + +bool base::Temperature::isApprox(base::Temperature other, double prec) const +{ + return std::abs( other.kelvin - kelvin ) < prec; +} + +void base::Temperature::operator=(const base::Temperature& other) +{ + kelvin = other.kelvin; +} + +bool base::Temperature::operator==(const base::Temperature& other) const +{ + return this->kelvin == other.kelvin; +} + +bool base::Temperature::operator<(const base::Temperature& other) const +{ + return this->kelvin < other.kelvin; +} + +bool base::Temperature::operator>(const base::Temperature& other) const +{ + return this->kelvin > other.kelvin; +} + +base::Temperature base::operator+(base::Temperature a, base::Temperature b) +{ + return Temperature::fromKelvin( a.getKelvin() + b.getKelvin() ); +} + +base::Temperature base::operator-(base::Temperature a, base::Temperature b) +{ + return Temperature::fromKelvin( a.getKelvin() - b.getKelvin() ); +} + +base::Temperature base::operator*(base::Temperature a, double b) +{ + return Temperature::fromKelvin( a.getKelvin() * b ); +} + +base::Temperature base::operator*(double a, base::Temperature b) +{ + return Temperature::fromKelvin( a * b.getKelvin() ); +} + +std::ostream& base::operator<<(std::ostream& os, base::Temperature temperature) +{ + os << temperature.getCelsius() << boost::format("[%3.1f celsius]"); + return os; +} + +bool base::Temperature::isInRange(const base::Temperature &left_limit, const base::Temperature &right_limit) const +{ + if((right_limit-left_limit).kelvin < 0) + return !isInRange(right_limit,left_limit); + if((*this -left_limit).getKelvin() >= 0 && (right_limit -*this).getKelvin() >= 0) + return true; + return false; +} + + + + + + + + + + + + + + + + diff --git a/src/Temperature.hpp b/src/Temperature.hpp new file mode 100644 index 00000000..008cdca4 --- /dev/null +++ b/src/Temperature.hpp @@ -0,0 +1,113 @@ +#ifndef __BASE_TEMPERATURE_HH__ +#define __BASE_TEMPERATURE_HH__ + +#include // std::abs + +namespace base +{ + +/** + * This class represents a temperature, and can be used instead of double for + * convenience. The class has a canonical representation of the temperature in + * kelvin. + */ +class Temperature +{ +public: + + /** + * temperature in kelvins + * + * + * @note don't use this value directly. It's only public to allow this class + * to be used as an interface type. + */ + double kelvin; + + /** + * default constructor, which will leave the temperature uninitialized. + */ + Temperature(); + +protected: + explicit Temperature( double kelvin ); + + +public: + /** + * static conversion from kelvin to celsius + * @param kelvin temperature in kelvin + * @result temperature in celsius + */ + static double kelvin2Celsius( double kelvin ); + + /** + * static conversion from celsius to kelvin + * @param celsius temperature in celsius + * @result temperature in kelvin + */ + static double celsius2Kelvin( double celsius ); + + + /** + * use this method to get temperature from Kelvin. + * @return representation of the given temperature. + * @param kelvin - temperature in Kelvin. + */ + static Temperature fromKelvin( double kelvin ); + + + /** + * use this method to get temperature from Celsius + * @return representation of the given temperature. + * @param celsius - temperature in celsius. + */ + static Temperature fromCelsius( double celsius ); + + /** + * @return canonical value of the temperature in kelvin + */ + double getKelvin() const ; + + /** + * @return canonical value of the temperature in celsius + */ + double getCelsius() const; + + /** + * @return true if the temperature is insight the given interval + */ + bool isInRange(const Temperature &left_limit,const Temperature &right_limit) const; + + /** + * compare two temperatures for approximate equality + * @param other - temperature to compare + * @param prec - precision interval in kelvin + * @return true if temperature is approximately equal + */ + bool isApprox( Temperature other, double prec = 1e-5 ) const; + + void operator=(const Temperature &other); + + bool operator==(const Temperature &other ) const; + + bool operator<(const Temperature &other ) const; + + bool operator>(const Temperature &other ) const; + +}; + +static Temperature operator+( Temperature a, Temperature b ); + +static Temperature operator-( Temperature a, Temperature b ); + +static Temperature operator*( Temperature a, double b ); + +static Temperature operator*( double a, Temperature b ); + +static std::ostream& operator << (std::ostream& os, Temperature temperature); + + +} + +#endif From 7485c1612dd7bb8dacc69178d073d9bd3334c535 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 14 Jul 2016 16:31:18 +0200 Subject: [PATCH 17/50] moved Point --- src/CMakeLists.txt | 1 + {base => src}/Point.hpp | 0 2 files changed, 1 insertion(+) rename {base => src}/Point.hpp (100%) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index f92687aa..c6791738 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -16,6 +16,7 @@ set(HEADERS Eigen.hpp Float.hpp Logging.hpp + Point.hpp Pose.hpp Singleton.hpp Temperature.hpp diff --git a/base/Point.hpp b/src/Point.hpp similarity index 100% rename from base/Point.hpp rename to src/Point.hpp From d1e5d419c7ad10045daa08651a4783550bff3def Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 14 Jul 2016 16:39:59 +0200 Subject: [PATCH 18/50] splitted Pressure --- src/CMakeLists.txt | 2 ++ src/Pressure.cpp | 46 ++++++++++++++++++++++++++++++++++++++ {base => src}/Pressure.hpp | 36 ++++++----------------------- 3 files changed, 55 insertions(+), 29 deletions(-) create mode 100644 src/Pressure.cpp rename {base => src}/Pressure.hpp (61%) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c6791738..4a765933 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,6 +1,7 @@ set(SOURCES Angle.cpp Pose.cpp + Pressure.cpp Temperature.cpp Time.cpp TimeMark.cpp @@ -18,6 +19,7 @@ set(HEADERS Logging.hpp Point.hpp Pose.hpp + Pressure.hpp Singleton.hpp Temperature.hpp Time.hpp diff --git a/src/Pressure.cpp b/src/Pressure.cpp new file mode 100644 index 00000000..fabef3b1 --- /dev/null +++ b/src/Pressure.cpp @@ -0,0 +1,46 @@ +#include "Pressure.hpp" + +#include "Float.hpp" + +base::Pressure::Pressure() : pascal(base::unknown()) +{ + +} + +base::Pressure base::Pressure::fromPascal(float pascal) +{ + Pressure result; + result.pascal = pascal; + return result; +} + +base::Pressure base::Pressure::fromBar(float bar) +{ + return fromPascal(bar * 100000); +} + +base::Pressure base::Pressure::fromPSI(float psi) +{ + return fromPascal(psi * 6894.75729); +} + +float base::Pressure::toPa() const +{ + return pascal; +} + +float base::Pressure::toBar() const +{ + return pascal / 100000; +} + +float base::Pressure::toPSI() const +{ + return pascal / 6894.75729; +} + + + + + + diff --git a/base/Pressure.hpp b/src/Pressure.hpp similarity index 61% rename from base/Pressure.hpp rename to src/Pressure.hpp index 4fdcd8cf..95762188 100644 --- a/base/Pressure.hpp +++ b/src/Pressure.hpp @@ -1,8 +1,6 @@ #ifndef __BASE_PRESSURE_HPP__ #define __BASE_PRESSURE_HPP__ -#include - namespace base { /** Representation of a pressure value @@ -25,46 +23,26 @@ namespace base /** * default constructor, which will initialize the value to unknown (NaN) */ - Pressure() : pascal(base::unknown()) {} + Pressure(); public: /** Create a pressure object using a value in pascals */ - static Pressure fromPascal(float pascal) - { - Pressure result; - result.pascal = pascal; - return result; - } + static Pressure fromPascal(float pascal); /** Create a pressure object using a value in bars */ - static Pressure fromBar(float bar) - { - return fromPascal(bar * 100000); - } + static Pressure fromBar(float bar); /** Create a pressure object using a value in PSI */ - static Pressure fromPSI(float psi) - { - return fromPascal(psi * 6894.75729); - } + static Pressure fromPSI(float psi); /** Returns the raw pressure value in pascals */ - float toPa() const - { - return pascal; - } + float toPa() const; /** Returns the raw pressure value in bar */ - float toBar() const - { - return pascal / 100000; - } + float toBar() const; /** Returns the raw pressure value in psi */ - float toPSI() const - { - return pascal / 6894.75729; - } + float toPSI() const; }; } From 91ef2c785152a74255dec36b6d836e9ea6787b20 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Fri, 15 Jul 2016 14:43:22 +0200 Subject: [PATCH 19/50] fixed compile errors --- src/Time.cpp | 4 ++-- src/TimeMark.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/Time.cpp b/src/Time.cpp index c28a0ccf..9c937616 100644 --- a/src/Time.cpp +++ b/src/Time.cpp @@ -85,7 +85,7 @@ timeval base::Time::toTimeval() const return tv; } -std::__cxx11::string base::Time::toString(base::Time::Resolution resolution, const std::__cxx11::string& mainFormat) const +std::string base::Time::toString(base::Time::Resolution resolution, const std::string& mainFormat) const { struct timeval tv = toTimeval(); int uSecs = tv.tv_usec; @@ -185,7 +185,7 @@ base::Time base::Time::fromTimeValues(int year, int month, int day, int hour, in return Time(timeVal); } -base::Time base::Time::fromString(const std::__cxx11::string& stringTime, base::Time::Resolution resolution, const std::__cxx11::string& mainFormat) +base::Time base::Time::fromString(const std::string& stringTime, base::Time::Resolution resolution, const std::string& mainFormat) { std::string mainTime = stringTime; int32_t usecs = 0; diff --git a/src/TimeMark.cpp b/src/TimeMark.cpp index 02ba8070..f37c4b00 100644 --- a/src/TimeMark.cpp +++ b/src/TimeMark.cpp @@ -1,6 +1,6 @@ #include "TimeMark.hpp" -base::TimeMark::TimeMark(const std::__cxx11::string& label) : label(label), mark( Time::now() ), clock( ::clock() ) +base::TimeMark::TimeMark(const std::string& label) : label(label), mark( Time::now() ), clock( ::clock() ) { } From f06110cb48a23fdb2558e9961436e1825b7c7c10 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Fri, 15 Jul 2016 14:45:36 +0200 Subject: [PATCH 20/50] Added correct PC file for base-types, made base-lib a dummy --- CMakeLists.txt | 4 +-- bindings/ruby/CMakeLists.txt | 2 +- src/CMakeLists.txt | 38 ++++++++++------------------ base-lib.pc.in => src/base-lib.pc.in | 4 +-- src/base-types.pc.in | 13 ++++++++++ viz/CMakeLists.txt | 2 +- 6 files changed, 31 insertions(+), 32 deletions(-) rename base-lib.pc.in => src/base-lib.pc.in (63%) create mode 100644 src/base-types.pc.in diff --git a/CMakeLists.txt b/CMakeLists.txt index cb6ac147..18ad23ad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.6) find_package(Rock) rock_init(base 1.0) -#rock_export_includedir(${PROJECT_SOURCE_DIR}/base ${PROJECT_NAME}) +# rock_export_includedir(${PROJECT_SOURCE_DIR}/base ${PROJECT_NAME}) rock_export_includedir(${PROJECT_SOURCE_DIR}/src base/geometry) include_directories(BEFORE ${PROJECT_SOURCE_DIR}) @@ -34,7 +34,5 @@ endif() rock_standard_layout() -configure_file(base-types.pc.in base-types.pc @ONLY) install(DIRECTORY ${CMAKE_SOURCE_DIR}/base DESTINATION include) -install(FILES ${CMAKE_BINARY_DIR}/base-types.pc DESTINATION lib/pkgconfig) diff --git a/bindings/ruby/CMakeLists.txt b/bindings/ruby/CMakeLists.txt index 84eeed0f..354bf24d 100644 --- a/bindings/ruby/CMakeLists.txt +++ b/bindings/ruby/CMakeLists.txt @@ -17,7 +17,7 @@ if (base_AVAILABLE) endif(SISL_FOUND) install(FILES lib/eigen.rb DESTINATION ${RUBY_LIBRARY_INSTALL_DIR}) - target_link_libraries(base_types_ruby base) + target_link_libraries(base_types_ruby base-types) rock_typelib_ruby_plugin(lib/base/typelib_plugin.rb RENAME base_types.rb) endif() diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 4a765933..60e29723 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,7 +1,9 @@ -set(SOURCES +rock_library( + base-types Angle.cpp Pose.cpp Pressure.cpp + Spline.cpp Temperature.cpp Time.cpp TimeMark.cpp @@ -10,9 +12,7 @@ set(SOURCES TransformWithCovariance.cpp TwistWithCovariance.cpp Waypoint.cpp -) - -set(HEADERS + HEADERS Angle.hpp Eigen.hpp Float.hpp @@ -21,6 +21,7 @@ set(HEADERS Pose.hpp Pressure.hpp Singleton.hpp + Spline.hpp Temperature.hpp Time.hpp TimeMark.hpp @@ -28,30 +29,19 @@ set(HEADERS Trajectory.hpp TransformWithCovariance.hpp TwistWithCovariance.hpp - logging/logging_iostream_style.h - logging/logging_printf_style.h Waypoint.hpp Wrench.hpp + logging/logging_iostream_style.h + logging/logging_printf_style.h + DEPS_CMAKE + SISL + DEPS_PKGCONFIG + base-logging + eigen3 ) - -# Using SISL as optional dependency -find_package(SISL) -if(SISL_FOUND) - rock_library(base ${SOURCES} Spline.cpp - DEPS_CMAKE SISL - HEADERS ${HEADERS} Spline.hpp - DEPS_PKGCONFIG base-logging) - add_definitions(-DSISL_FOUND) -else(SISL_FOUND) - message(STATUS "Compiling ${PROJECT_NAME} without 'spline' support") - rock_library(base ${SOURCES} - HEADERS ${HEADERS} - DEPS_PKGCONFIG base-logging) -endif(SISL_FOUND) - -configure_file(${CMAKE_SOURCE_DIR}/base-lib.pc.in ${CMAKE_BINARY_DIR}/base-lib.pc @ONLY) -install(FILES ${CMAKE_BINARY_DIR}/base-lib.pc DESTINATION lib/pkgconfig) install(FILES ${CMAKE_SOURCE_DIR}/src/Spline.hpp DESTINATION include/base/geometry) +configure_file(base-lib.pc.in ${CMAKE_BINARY_DIR}/base-lib.pc @ONLY) +install(FILES ${CMAKE_BINARY_DIR}/base-lib.pc DESTINATION lib/pkgconfig) diff --git a/base-lib.pc.in b/src/base-lib.pc.in similarity index 63% rename from base-lib.pc.in rename to src/base-lib.pc.in index d77770c0..4d046849 100644 --- a/base-lib.pc.in +++ b/src/base-lib.pc.in @@ -5,7 +5,5 @@ includedir=${prefix}/include Name: baseLib Version: 0.1 Description: Common types for robotics modules, part that require a library -Cflags: -I${includedir} -Requires: eigen3 base-types base-logging -Libs: -L@CMAKE_INSTALL_PREFIX@/lib -lbase +Requires: base-types diff --git a/src/base-types.pc.in b/src/base-types.pc.in new file mode 100644 index 00000000..574e9661 --- /dev/null +++ b/src/base-types.pc.in @@ -0,0 +1,13 @@ +prefix=@CMAKE_INSTALL_PREFIX@ +exec_prefix=@CMAKE_INSTALL_PREFIX@ +libdir=${prefix}/lib +includedir=${prefix}/include + +Name: @TARGET_NAME@ +Description: @PROJECT_DESCRIPTION@ +Version: @PROJECT_VERSION@ +Requires: @DEPS_PKGCONFIG@ +Libs: -L${libdir} -l@TARGET_NAME@ +Cflags: -I${includedir} + + diff --git a/viz/CMakeLists.txt b/viz/CMakeLists.txt index 772365f6..7b0656dc 100644 --- a/viz/CMakeLists.txt +++ b/viz/CMakeLists.txt @@ -26,7 +26,7 @@ rock_vizkit_plugin(base-viz SonarBeamVisualization.hpp PointcloudVisualization.hpp DepthMapVisualization.hpp - DEPS base + DEPS base-types LIBS ${Boost_SYSTEM_LIBRARY} DEPS_PKGCONFIG base-logging ) From f878d357a93ea583fff600b72cd0d9242c1178bf Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Mon, 8 Aug 2016 11:24:25 +0200 Subject: [PATCH 21/50] moved CircularBuffer --- base/CircularBuffer.hpp | 72 ----------------------------------------- 1 file changed, 72 deletions(-) delete mode 100644 base/CircularBuffer.hpp diff --git a/base/CircularBuffer.hpp b/base/CircularBuffer.hpp deleted file mode 100644 index 2fd5bce7..00000000 --- a/base/CircularBuffer.hpp +++ /dev/null @@ -1,72 +0,0 @@ -// Circular buffer library header file. - -// Copyright (c) 2003-2008 Jan Gaspar - -// Use, modification, and distribution is subject to the Boost Software -// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at -// http://www.boost.org/LICENSE_1_0.txt) - -// See www.boost.org/libs/circular_buffer for documentation. - -#if !defined(BOOST_CIRCULAR_BUFFER_HPP) -#define BOOST_CIRCULAR_BUFFER_HPP - -#if defined(_MSC_VER) && _MSC_VER >= 1200 - #pragma once -#endif - -#include -#include - -// BOOST_CB_ENABLE_DEBUG: Debug support control. -#ifndef BOOST_CB_ENABLE_DEBUG - #define BOOST_CB_ENABLE_DEBUG 0 -#endif - -// BOOST_CB_ASSERT: Runtime assertion. -#if BOOST_CB_ENABLE_DEBUG - #include - #define BOOST_CB_ASSERT(Expr) BOOST_ASSERT(Expr) -#else - #define BOOST_CB_ASSERT(Expr) ((void)0) -#endif - -// BOOST_CB_STATIC_ASSERT: Compile time assertion. -#if BOOST_WORKAROUND(BOOST_MSVC, < 1300) - #define BOOST_CB_STATIC_ASSERT(Expr) ((void)0) -#else - #include - #define BOOST_CB_STATIC_ASSERT(Expr) BOOST_STATIC_ASSERT(Expr) -#endif - -// BOOST_CB_IS_CONVERTIBLE: Check if Iterator::value_type is convertible to Type. -#if BOOST_WORKAROUND(__BORLANDC__, <= 0x0550) || BOOST_WORKAROUND(__MWERKS__, <= 0x2407) || \ - BOOST_WORKAROUND(BOOST_MSVC, < 1300) - #define BOOST_CB_IS_CONVERTIBLE(Iterator, Type) ((void)0) -#else - #include - #include - #define BOOST_CB_IS_CONVERTIBLE(Iterator, Type) \ - BOOST_CB_STATIC_ASSERT((is_convertible::value_type, Type>::value)) -#endif - -// BOOST_CB_ASSERT_TEMPLATED_ITERATOR_CONSTRUCTORS: -// Check if the STL provides templated iterator constructors for its containers. -#if defined(BOOST_NO_TEMPLATED_ITERATOR_CONSTRUCTORS) - #define BOOST_CB_ASSERT_TEMPLATED_ITERATOR_CONSTRUCTORS BOOST_CB_STATIC_ASSERT(false); -#else - #define BOOST_CB_ASSERT_TEMPLATED_ITERATOR_CONSTRUCTORS ((void)0); -#endif - -#include -#include -#include -#include - -#undef BOOST_CB_ASSERT_TEMPLATED_ITERATOR_CONSTRUCTORS -#undef BOOST_CB_IS_CONVERTIBLE -#undef BOOST_CB_STATIC_ASSERT -#undef BOOST_CB_ASSERT -#undef BOOST_CB_ENABLE_DEBUG - -#endif // #if !defined(BOOST_CIRCULAR_BUFFER_HPP) From 9a787e45dd3b20245e3f5dd92def1908bbfd0793 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Mon, 8 Aug 2016 11:24:39 +0200 Subject: [PATCH 22/50] moved Deprecated --- base/Deprecated.hpp | 24 ------------------------ 1 file changed, 24 deletions(-) delete mode 100644 base/Deprecated.hpp diff --git a/base/Deprecated.hpp b/base/Deprecated.hpp deleted file mode 100644 index 1b497b15..00000000 --- a/base/Deprecated.hpp +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef BASE_TYPES_DEPRECATED -#ifdef __GNUC__ - #define BASE_TYPES_DEPRECATED __attribute__ ((deprecated)) -#else - #define BASE_TYPES_DEPRECATED -#endif -#endif - -#ifndef BASE_TYPES_DEPRECATED_SUPPRESS_START -#ifdef __GNUC__ - #define BASE_TYPES_DEPRECATED_SUPPRESS_START _Pragma("GCC diagnostic push") \ - _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"") -#else - #define BASE_TYPES_DEPRECATED_SUPPRESS_START -#endif -#endif - -#ifndef BASE_TYPES_DEPRECATED_SUPPRESS_STOP -#ifdef __GNUC__ - #define BASE_TYPES_DEPRECATED_SUPPRESS_STOP _Pragma("GCC diagnostic pop") -#else - #define BASE_TYPES_DEPRECATED_SUPPRESS_STOP -#endif -#endif \ No newline at end of file From e7fd071957a51c2b4248c208a395031079d66fee Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Mon, 8 Aug 2016 11:25:22 +0200 Subject: [PATCH 23/50] moved JointLimitRange --- base/JointLimitRange.hpp | 166 --------------------------------------- src/CMakeLists.txt | 4 + src/CircularBuffer.hpp | 72 +++++++++++++++++ src/Deprecated.hpp | 24 ++++++ src/JointLimitRange.cpp | 118 ++++++++++++++++++++++++++++ src/JointLimitRange.hpp | 79 +++++++++++++++++++ 6 files changed, 297 insertions(+), 166 deletions(-) delete mode 100644 base/JointLimitRange.hpp create mode 100644 src/CircularBuffer.hpp create mode 100644 src/Deprecated.hpp create mode 100644 src/JointLimitRange.cpp create mode 100644 src/JointLimitRange.hpp diff --git a/base/JointLimitRange.hpp b/base/JointLimitRange.hpp deleted file mode 100644 index 54ae5f2f..00000000 --- a/base/JointLimitRange.hpp +++ /dev/null @@ -1,166 +0,0 @@ -#ifndef BASE_JOINT_LIMIT_RANGE_HPP -#define BASE_JOINT_LIMIT_RANGE_HPP - -#include -#include - -namespace base -{ - -struct JointLimitRange -{ - JointState min; - JointState max; - - struct OutOfBounds : public std::runtime_error - { - static std::string errorString( std::string name, double min, double max, double value ) - { - std::ostringstream ss; - ss << "The " << name << " value " << value - << " was outside the allowed bounds [" << min << ":" << max << "]."; - return ss.str(); - } - - std::string name; - double min, max, value; - OutOfBounds() - : std::runtime_error( std::string() ), min(0), max(0), value(0) {} - OutOfBounds( std::string name, double min, double max, double value ) - : std::runtime_error( errorString( name, min, max, value ) ) - , name( name ) - , min( min ), max( max ), value( value ) - { - } - ~OutOfBounds() throw() {} - }; - - /** - * Checks that the provided JointState is within limits for the values that are valid - * - * @throw OutOfBoundsException if limits are exceeded. - */ - void validate( const JointState &state ) const - { - std::pair check = verifyValidity(state); - if (check.first) - throw check.second; - } - - /** - * Checks that the provided JointState is within limits for the values that are valid - * - * @return true if the check passes, false otherwise - */ - bool isValid( const JointState &state ) const - { - std::pair check = verifyValidity(state); - return check.first; - } - - /** Creates a JointLimitRange structure with the position range set to \c - * min, \c max - */ - static JointLimitRange Position(double min, double max) - { - JointLimitRange result; - result.min.position = min; - result.max.position = max; - return result; - } - - /** Creates a JointLimitRange structure with the speed range set to \c - * min, \c max - */ - static JointLimitRange Speed(double min, double max) - { - JointLimitRange result; - result.min.speed = min; - result.max.speed = max; - return result; - } - - /** Creates a JointLimitRange structure with the effort range set to \c - * min, \c max - */ - static JointLimitRange Effort(double min, double max) - { - JointLimitRange result; - result.min.effort = min; - result.max.effort = max; - return result; - } - - /** Creates a JointLimitRange structure with the raw range set to \c - * min, \c max - */ - static JointLimitRange Raw(double min, double max) - { - JointLimitRange result; - result.min.raw = min; - result.max.raw = max; - return result; - } - - /** Creates a JointLimitRange structure with the acceleration range set to \c - * min, \c max - */ - static JointLimitRange Acceleration(double min, double max) - { - JointLimitRange result; - result.min.acceleration = min; - result.max.acceleration = max; - return result; - } - -private: - /** Internal helper method for validate and isValid */ - std::pair verifyValidity( const JointState& state ) const - { - using std::make_pair; - if( state.hasPosition() ) - { - if( min.hasPosition() && min.position > state.position ) - return make_pair(false, OutOfBounds( "position", min.position, max.position, state.position )); - if( max.hasPosition() && max.position < state.position ) - return make_pair(false, OutOfBounds( "position", min.position, max.position, state.position )); - } - - if( state.hasSpeed() ) - { - if( min.hasSpeed() && min.speed > state.speed ) - return make_pair(false, OutOfBounds( "speed", min.speed, max.speed, state.speed )); - if( max.hasSpeed() && max.speed < state.speed ) - return make_pair(false, OutOfBounds( "speed", min.speed, max.speed, state.speed )); - } - - if( state.hasEffort() ) - { - if( min.hasEffort() && min.effort > state.effort ) - return make_pair(false, OutOfBounds( "effort", min.effort, max.effort, state.effort )); - if( max.hasEffort() && max.effort < state.effort ) - return make_pair(false, OutOfBounds( "effort", min.effort, max.effort, state.effort )); - } - - if( state.hasRaw() ) - { - if( min.hasRaw() && min.raw > state.raw ) - return make_pair(false, OutOfBounds( "raw", min.raw, max.raw, state.raw )); - if( max.hasRaw() && max.raw < state.raw ) - return make_pair(false, OutOfBounds( "raw", min.raw, max.raw, state.raw )); - } - - if( state.hasAcceleration() ) - { - if( min.hasAcceleration() && min.acceleration > state.acceleration ) - return make_pair(false, OutOfBounds( "acceleration", min.acceleration, max.acceleration, state.acceleration )); - if( max.hasAcceleration() && max.acceleration < state.acceleration ) - return make_pair(false, OutOfBounds( "acceleration", min.acceleration, max.acceleration, state.acceleration )); - } - return make_pair(true, OutOfBounds()); - } -}; - -} - -#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 60e29723..9a4274af 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,6 +1,7 @@ rock_library( base-types Angle.cpp + JointLimitRange.cpp Pose.cpp Pressure.cpp Spline.cpp @@ -14,8 +15,11 @@ rock_library( Waypoint.cpp HEADERS Angle.hpp + CircularBuffer.hpp + Deprecated.hpp Eigen.hpp Float.hpp + JointLimitRange.cpp Logging.hpp Point.hpp Pose.hpp diff --git a/src/CircularBuffer.hpp b/src/CircularBuffer.hpp new file mode 100644 index 00000000..2fd5bce7 --- /dev/null +++ b/src/CircularBuffer.hpp @@ -0,0 +1,72 @@ +// Circular buffer library header file. + +// Copyright (c) 2003-2008 Jan Gaspar + +// Use, modification, and distribution is subject to the Boost Software +// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at +// http://www.boost.org/LICENSE_1_0.txt) + +// See www.boost.org/libs/circular_buffer for documentation. + +#if !defined(BOOST_CIRCULAR_BUFFER_HPP) +#define BOOST_CIRCULAR_BUFFER_HPP + +#if defined(_MSC_VER) && _MSC_VER >= 1200 + #pragma once +#endif + +#include +#include + +// BOOST_CB_ENABLE_DEBUG: Debug support control. +#ifndef BOOST_CB_ENABLE_DEBUG + #define BOOST_CB_ENABLE_DEBUG 0 +#endif + +// BOOST_CB_ASSERT: Runtime assertion. +#if BOOST_CB_ENABLE_DEBUG + #include + #define BOOST_CB_ASSERT(Expr) BOOST_ASSERT(Expr) +#else + #define BOOST_CB_ASSERT(Expr) ((void)0) +#endif + +// BOOST_CB_STATIC_ASSERT: Compile time assertion. +#if BOOST_WORKAROUND(BOOST_MSVC, < 1300) + #define BOOST_CB_STATIC_ASSERT(Expr) ((void)0) +#else + #include + #define BOOST_CB_STATIC_ASSERT(Expr) BOOST_STATIC_ASSERT(Expr) +#endif + +// BOOST_CB_IS_CONVERTIBLE: Check if Iterator::value_type is convertible to Type. +#if BOOST_WORKAROUND(__BORLANDC__, <= 0x0550) || BOOST_WORKAROUND(__MWERKS__, <= 0x2407) || \ + BOOST_WORKAROUND(BOOST_MSVC, < 1300) + #define BOOST_CB_IS_CONVERTIBLE(Iterator, Type) ((void)0) +#else + #include + #include + #define BOOST_CB_IS_CONVERTIBLE(Iterator, Type) \ + BOOST_CB_STATIC_ASSERT((is_convertible::value_type, Type>::value)) +#endif + +// BOOST_CB_ASSERT_TEMPLATED_ITERATOR_CONSTRUCTORS: +// Check if the STL provides templated iterator constructors for its containers. +#if defined(BOOST_NO_TEMPLATED_ITERATOR_CONSTRUCTORS) + #define BOOST_CB_ASSERT_TEMPLATED_ITERATOR_CONSTRUCTORS BOOST_CB_STATIC_ASSERT(false); +#else + #define BOOST_CB_ASSERT_TEMPLATED_ITERATOR_CONSTRUCTORS ((void)0); +#endif + +#include +#include +#include +#include + +#undef BOOST_CB_ASSERT_TEMPLATED_ITERATOR_CONSTRUCTORS +#undef BOOST_CB_IS_CONVERTIBLE +#undef BOOST_CB_STATIC_ASSERT +#undef BOOST_CB_ASSERT +#undef BOOST_CB_ENABLE_DEBUG + +#endif // #if !defined(BOOST_CIRCULAR_BUFFER_HPP) diff --git a/src/Deprecated.hpp b/src/Deprecated.hpp new file mode 100644 index 00000000..1b497b15 --- /dev/null +++ b/src/Deprecated.hpp @@ -0,0 +1,24 @@ +#ifndef BASE_TYPES_DEPRECATED +#ifdef __GNUC__ + #define BASE_TYPES_DEPRECATED __attribute__ ((deprecated)) +#else + #define BASE_TYPES_DEPRECATED +#endif +#endif + +#ifndef BASE_TYPES_DEPRECATED_SUPPRESS_START +#ifdef __GNUC__ + #define BASE_TYPES_DEPRECATED_SUPPRESS_START _Pragma("GCC diagnostic push") \ + _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"") +#else + #define BASE_TYPES_DEPRECATED_SUPPRESS_START +#endif +#endif + +#ifndef BASE_TYPES_DEPRECATED_SUPPRESS_STOP +#ifdef __GNUC__ + #define BASE_TYPES_DEPRECATED_SUPPRESS_STOP _Pragma("GCC diagnostic pop") +#else + #define BASE_TYPES_DEPRECATED_SUPPRESS_STOP +#endif +#endif \ No newline at end of file diff --git a/src/JointLimitRange.cpp b/src/JointLimitRange.cpp new file mode 100644 index 00000000..d3ee9210 --- /dev/null +++ b/src/JointLimitRange.cpp @@ -0,0 +1,118 @@ +#include "JointLimitRange.hpp" +#include + +namespace base +{ + + +std::string JointLimitRange::OutOfBounds::errorString(std::string name, double min, double max, double value) +{ + std::ostringstream ss; + ss << "The " << name << " value " << value + << " was outside the allowed bounds [" << min << ":" << max << "]."; + return ss.str(); +} + +void JointLimitRange::validate(const JointState& state) const +{ + std::pair check = verifyValidity(state); + if (check.first) + throw check.second; +} + +bool JointLimitRange::isValid(const JointState& state) const +{ + std::pair check = verifyValidity(state); + return check.first; +} + +JointLimitRange JointLimitRange::Position(double min, double max) +{ + JointLimitRange result; + result.min.position = min; + result.max.position = max; + return result; +} + + +JointLimitRange JointLimitRange::Speed(double min, double max) +{ + JointLimitRange result; + result.min.speed = min; + result.max.speed = max; + return result; +} + + +JointLimitRange JointLimitRange::Effort(double min, double max) +{ + JointLimitRange result; + result.min.effort = min; + result.max.effort = max; + return result; +} + +JointLimitRange JointLimitRange::Raw(double min, double max) +{ + JointLimitRange result; + result.min.raw = min; + result.max.raw = max; + return result; +} + +JointLimitRange JointLimitRange::Acceleration(double min, double max) +{ + JointLimitRange result; + result.min.acceleration = min; + result.max.acceleration = max; + return result; +} + +std::pair< bool, JointLimitRange::OutOfBounds > JointLimitRange::verifyValidity(const JointState& state) const +{ + using std::make_pair; + if( state.hasPosition() ) + { + if( min.hasPosition() && min.position > state.position ) + return make_pair(false, OutOfBounds( "position", min.position, max.position, state.position )); + if( max.hasPosition() && max.position < state.position ) + return make_pair(false, OutOfBounds( "position", min.position, max.position, state.position )); + } + + if( state.hasSpeed() ) + { + if( min.hasSpeed() && min.speed > state.speed ) + return make_pair(false, OutOfBounds( "speed", min.speed, max.speed, state.speed )); + if( max.hasSpeed() && max.speed < state.speed ) + return make_pair(false, OutOfBounds( "speed", min.speed, max.speed, state.speed )); + } + + if( state.hasEffort() ) + { + if( min.hasEffort() && min.effort > state.effort ) + return make_pair(false, OutOfBounds( "effort", min.effort, max.effort, state.effort )); + if( max.hasEffort() && max.effort < state.effort ) + return make_pair(false, OutOfBounds( "effort", min.effort, max.effort, state.effort )); + } + + if( state.hasRaw() ) + { + if( min.hasRaw() && min.raw > state.raw ) + return make_pair(false, OutOfBounds( "raw", min.raw, max.raw, state.raw )); + if( max.hasRaw() && max.raw < state.raw ) + return make_pair(false, OutOfBounds( "raw", min.raw, max.raw, state.raw )); + } + + if( state.hasAcceleration() ) + { + if( min.hasAcceleration() && min.acceleration > state.acceleration ) + return make_pair(false, OutOfBounds( "acceleration", min.acceleration, max.acceleration, state.acceleration )); + if( max.hasAcceleration() && max.acceleration < state.acceleration ) + return make_pair(false, OutOfBounds( "acceleration", min.acceleration, max.acceleration, state.acceleration )); + } + return make_pair(true, OutOfBounds()); +} + + + +} \ No newline at end of file diff --git a/src/JointLimitRange.hpp b/src/JointLimitRange.hpp new file mode 100644 index 00000000..63ce05af --- /dev/null +++ b/src/JointLimitRange.hpp @@ -0,0 +1,79 @@ +#ifndef BASE_JOINT_LIMIT_RANGE_HPP +#define BASE_JOINT_LIMIT_RANGE_HPP + +#include + +namespace base +{ + +struct JointLimitRange +{ + JointState min; + JointState max; + + struct OutOfBounds : public std::runtime_error + { + static std::string errorString( std::string name, double min, double max, double value ); + + std::string name; + double min, max, value; + OutOfBounds() + : std::runtime_error( std::string() ), min(0), max(0), value(0) {} + OutOfBounds( std::string name, double min, double max, double value ) + : std::runtime_error( errorString( name, min, max, value ) ) + , name( name ) + , min( min ), max( max ), value( value ) + { + } + ~OutOfBounds() throw() {} + }; + + /** + * Checks that the provided JointState is within limits for the values that are valid + * + * @throw OutOfBoundsException if limits are exceeded. + */ + void validate( const JointState &state ) const; + + /** + * Checks that the provided JointState is within limits for the values that are valid + * + * @return true if the check passes, false otherwise + */ + bool isValid( const JointState &state ) const; + + /** Creates a JointLimitRange structure with the position range set to \c + * min, \c max + */ + static JointLimitRange Position(double min, double max); + + /** Creates a JointLimitRange structure with the speed range set to \c + * min, \c max + */ + static JointLimitRange Speed(double min, double max); + + /** Creates a JointLimitRange structure with the effort range set to \c + * min, \c max + */ + static JointLimitRange Effort(double min, double max); + + /** Creates a JointLimitRange structure with the raw range set to \c + * min, \c max + */ + static JointLimitRange Raw(double min, double max); + + /** Creates a JointLimitRange structure with the acceleration range set to \c + * min, \c max + */ + static JointLimitRange Acceleration(double min, double max); + + +private: + /** Internal helper method for validate and isValid */ + std::pair verifyValidity( const JointState& state ) const; + +}; + +} + +#endif From f04df26fc060825ae977d4ec228e0bd0a6684432 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Mon, 8 Aug 2016 13:32:43 +0200 Subject: [PATCH 24/50] moved JointLimits --- base/JointLimits.hpp | 51 -------------------------------------------- src/JointLimits.cpp | 33 ++++++++++++++++++++++++++++ src/JointLimits.hpp | 23 ++++++++++++++++++++ 3 files changed, 56 insertions(+), 51 deletions(-) delete mode 100644 base/JointLimits.hpp create mode 100644 src/JointLimits.cpp create mode 100644 src/JointLimits.hpp diff --git a/base/JointLimits.hpp b/base/JointLimits.hpp deleted file mode 100644 index ee1d4c40..00000000 --- a/base/JointLimits.hpp +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef BASE_JOINT_LIMITS_HPP -#define BASE_JOINT_LIMITS_HPP - -#include -#include -#include - -namespace base -{ - struct JointLimits : public NamedVector - { - bool isValid( const base::samples::Joints& joints ) const - { - if (joints.hasNames()) - { - for( size_t i=0; i +#include + +namespace base +{ + struct JointLimits : public NamedVector + { + bool isValid( const base::samples::Joints& joints ) const; + + /** + * Makes sure that all jointstate are within their respective limits + * + * Will throw if this is not the case. Will also throw if there are no + * limits for a particular joint. + */ + void validate( const base::samples::Joints& joints ) const; + }; +} + +#endif From 7d1f032a79ae22a6db0b7bd0fb2a7bdbd407f875 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Mon, 8 Aug 2016 13:32:53 +0200 Subject: [PATCH 25/50] moved JointState --- src/JointState.cpp | 149 +++++++++++++++++++++++++++++++++++ {base => src}/JointState.hpp | 104 +++++------------------- 2 files changed, 167 insertions(+), 86 deletions(-) create mode 100644 src/JointState.cpp rename {base => src}/JointState.hpp (61%) diff --git a/src/JointState.cpp b/src/JointState.cpp new file mode 100644 index 00000000..622fd231 --- /dev/null +++ b/src/JointState.cpp @@ -0,0 +1,149 @@ +#include "JointState.hpp" + +base::JointState base::JointState::Position(double value) +{ + JointState ret; + ret.position = value; + return ret; +} + +base::JointState base::JointState::Speed(float value) +{ + JointState ret; + ret.speed = value; + return ret; +} + +base::JointState base::JointState::Effort(float value) +{ + JointState ret; + ret.effort = value; + return ret; +} + +base::JointState base::JointState::Raw(float value) +{ + JointState ret; + ret.raw = value; + return ret; +} + +base::JointState base::JointState::Acceleration(float value) +{ + JointState ret; + ret.acceleration = value; + return ret; +} + +bool base::JointState::hasPosition() const +{ + return !base::isUnset(position); +} + +bool base::JointState::hasSpeed() const +{ + return !base::isUnset(speed); +} + +bool base::JointState::hasEffort() const +{ + return !base::isUnset(effort); +} + +bool base::JointState::hasRaw() const +{ + return !base::isUnset(raw); +} + +bool base::JointState::hasAcceleration() const +{ + return !base::isUnset(acceleration); +} + +bool base::JointState::isPosition() const +{ + return hasPosition() && !hasSpeed() && !hasEffort() && !hasRaw() && !hasAcceleration(); +} + +bool base::JointState::isSpeed() const +{ + return !hasPosition() && hasSpeed() && !hasEffort() && !hasRaw() && !hasAcceleration(); +} + +bool base::JointState::isEffort() const +{ + return !hasPosition() && !hasSpeed() && hasEffort() && !hasRaw() && !hasAcceleration(); +} + +bool base::JointState::isRaw() const +{ + return !hasPosition() && !hasSpeed() && !hasEffort() && hasRaw() && !hasAcceleration(); +} + +bool base::JointState::isAcceleration() const +{ + return !hasPosition() && !hasSpeed() && !hasEffort() && !hasRaw() && hasAcceleration(); +} + +double base::JointState::getField(int mode) const +{ + switch(mode) + { + case POSITION: return position; + case SPEED: return speed; + case EFFORT: return effort; + case RAW: return raw; + case ACCELERATION: return acceleration; + default: throw std::runtime_error("invalid mode given to getField"); + } +} + +void base::JointState::setField(int mode, double value) +{ + switch(mode) + { + case POSITION: + position = value; + return; + case SPEED: + speed = value; + return; + case EFFORT: + effort = value; + return; + case RAW: + raw = value; + return; + case ACCELERATION: + acceleration = value; + return; + default: throw std::runtime_error("invalid mode given to getField"); + } +} + +base::JointState::MODE base::JointState::getMode() const +{ + if (isPosition()) return POSITION; + else if (isSpeed()) return SPEED; + else if (isEffort()) return EFFORT; + else if (isRaw()) return RAW; + else if (isAcceleration()) return ACCELERATION; + else if (hasPosition() || hasSpeed() || hasEffort() || hasRaw() || hasAcceleration()) + throw std::runtime_error("getMode() called on a JointState that has more than one field set"); + else + return UNSET; +} + + + + + + + + + + + + + + diff --git a/base/JointState.hpp b/src/JointState.hpp similarity index 61% rename from base/JointState.hpp rename to src/JointState.hpp index d18e0dfd..8a5759c3 100644 --- a/base/JointState.hpp +++ b/src/JointState.hpp @@ -69,63 +69,38 @@ namespace base /** Returns a JointState object with the position field set to the given * value */ - static JointState Position(double value) - { - JointState ret; - ret.position = value; - return ret; - } + static JointState Position(double value); /** Returns a JointState object with the speed field set to the given * value */ - static JointState Speed(float value) - { - JointState ret; - ret.speed = value; - return ret; - } + static JointState Speed(float value); /** Returns a JointState object with the effort field set to the given * value */ - static JointState Effort(float value) - { - JointState ret; - ret.effort = value; - return ret; - } + static JointState Effort(float value); /** Returns a JointState object with the raw field set to the given * value */ - static JointState Raw(float value) - { - JointState ret; - ret.raw = value; - return ret; - } + static JointState Raw(float value); /** Returns a JointState object with the acceleration field set to the given * value */ - static JointState Acceleration(float value) - { - JointState ret; - ret.acceleration = value; - return ret; - } + static JointState Acceleration(float value); /** Tests whether the position field is set */ - bool hasPosition() const { return !base::isUnset(position); } + bool hasPosition() const; /** Tests whether the speed field is set */ - bool hasSpeed() const { return !base::isUnset(speed); } + bool hasSpeed() const; /** Tests whether the effort field is set */ - bool hasEffort() const { return !base::isUnset(effort); } + bool hasEffort() const; /** Tests whether the raw field is set */ - bool hasRaw() const { return !base::isUnset(raw); } + bool hasRaw() const; /** Tests whether the acceleration field is set */ - bool hasAcceleration() const { return !base::isUnset(acceleration); } + bool hasAcceleration() const; /** Tests whether the position field is the only field set * @@ -133,21 +108,21 @@ namespace base * structure is a valid command for said controller (e.g. a position * controller would throw if isPosition() returns false) */ - bool isPosition() const { return hasPosition() && !hasSpeed() && !hasEffort() && !hasRaw() && !hasAcceleration(); } + bool isPosition() const; /** Tests whether the speed field is the only field set * * This is commonly used in controllers to test whether this data * structure is a valid command for said controller (e.g. a speed * controller would throw if isSpeed() returns false) */ - bool isSpeed() const { return !hasPosition() && hasSpeed() && !hasEffort() && !hasRaw() && !hasAcceleration(); } + bool isSpeed() const; /** Tests whether the effort field is the only field set * * This is commonly used in controllers to test whether this data * structure is a valid command for said controller (e.g. a torque * controller would throw if isEffort() returns false) */ - bool isEffort() const { return !hasPosition() && !hasSpeed() && hasEffort() && !hasRaw() && !hasAcceleration(); } + bool isEffort() const; /** Tests whether the raw field is the only field set * * This is commonly used in controllers to test whether this data @@ -155,7 +130,7 @@ namespace base * driver that is controlled in PWM would throw if isRaw() returns * false) */ - bool isRaw() const { return !hasPosition() && !hasSpeed() && !hasEffort() && hasRaw() && !hasAcceleration(); } + bool isRaw() const; /** Tests whether the acceleration field is the only field set * * This is commonly used in controllers to test whether this data @@ -163,51 +138,19 @@ namespace base * driver that is controlled in acceleration would throw if isAcceleration() returns * false) */ - bool isAcceleration() const { return !hasPosition() && !hasSpeed() && !hasEffort() && !hasRaw() && hasAcceleration(); } + bool isAcceleration() const; /** Returns the field that matches the given mode * * @see MODE */ - double getField(int mode) const - { - switch(mode) - { - case POSITION: return position; - case SPEED: return speed; - case EFFORT: return effort; - case RAW: return raw; - case ACCELERATION: return acceleration; - default: throw std::runtime_error("invalid mode given to getField"); - } - } + double getField(int mode) const; /** Sets the given field to the given value * * @see MODE */ - void setField(int mode, double value) - { - switch(mode) - { - case POSITION: - position = value; - return; - case SPEED: - speed = value; - return; - case EFFORT: - effort = value; - return; - case RAW: - raw = value; - return; - case ACCELERATION: - acceleration = value; - return; - default: throw std::runtime_error("invalid mode given to getField"); - } - } + void setField(int mode, double value); /** Returns the mode of this JointState * @@ -217,18 +160,7 @@ namespace base * * @see MODE */ - MODE getMode() const - { - if (isPosition()) return POSITION; - else if (isSpeed()) return SPEED; - else if (isEffort()) return EFFORT; - else if (isRaw()) return RAW; - else if (isAcceleration()) return ACCELERATION; - else if (hasPosition() || hasSpeed() || hasEffort() || hasRaw() || hasAcceleration()) - throw std::runtime_error("getMode() called on a JointState that has more than one field set"); - else - return UNSET; - } + MODE getMode() const; }; } From 9a78c4e938610990b1e9730531588674cf3eaa20 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Mon, 8 Aug 2016 13:33:06 +0200 Subject: [PATCH 26/50] moved JointTransform --- base/JointTransform.hpp | 64 ----------------------------------------- src/JointTransform.cpp | 28 ++++++++++++++++++ src/JointTransform.hpp | 39 +++++++++++++++++++++++++ 3 files changed, 67 insertions(+), 64 deletions(-) delete mode 100644 base/JointTransform.hpp create mode 100644 src/JointTransform.cpp create mode 100644 src/JointTransform.hpp diff --git a/base/JointTransform.hpp b/base/JointTransform.hpp deleted file mode 100644 index 6ebec893..00000000 --- a/base/JointTransform.hpp +++ /dev/null @@ -1,64 +0,0 @@ -#ifndef BASE_JOINT_TRANSFORM_HPP -#define BASE_JOINT_TRANSFORM_HPP - -#include -#include -#include -#include -#include - -namespace base -{ - -/** - * defines the frame transformation for a joint, - * and the rotation axis - */ -struct JointTransform -{ - std::string sourceFrame; - std::string targetFrame; - base::Vector3d rotationAxis; -}; - -/** - * Vector of JointTranformations with added functionality - * to fill a vector of RigidBodyStates given a jointsState sample - */ -struct JointTransformVector : public base::NamedVector -{ - /** - * will fill the rbs structure with the information from the joints and - * the JointTransform configuration. - */ - void setRigidBodyStates( const base::samples::Joints& joints, std::vector& rbs ) const - { - if (joints.names.empty()) { - throw std::runtime_error("base::JointTransformVector::" - "setRigidBodyStates(): the vector " - "'joints.names()' is empty"); - } - - rbs.resize( joints.size() ); - for( size_t i=0; i& rbs) const +{ + if (joints.names.empty()) { + throw std::runtime_error("base::JointTransformVector::" + "setRigidBodyStates(): the vector " + "'joints.names()' is empty"); + } + + rbs.resize( joints.size() ); + for( size_t i=0; i +#include +#include +#include +#include + +namespace base +{ + +/** + * defines the frame transformation for a joint, + * and the rotation axis + */ +struct JointTransform +{ + std::string sourceFrame; + std::string targetFrame; + base::Vector3d rotationAxis; +}; + +/** + * Vector of JointTranformations with added functionality + * to fill a vector of RigidBodyStates given a jointsState sample + */ +struct JointTransformVector : public base::NamedVector +{ + /** + * will fill the rbs structure with the information from the joints and + * the JointTransform configuration. + */ + void setRigidBodyStates( const base::samples::Joints& joints, std::vector& rbs ) const; +}; + +} + +#endif From 72efe114dd8da3fc2134817ae7375372519ce161 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Mon, 8 Aug 2016 13:33:16 +0200 Subject: [PATCH 27/50] moved JointsTrajectory --- src/JointsTrajectory.cpp | 78 ++++++++++++++++++++++++++++++ {base => src}/JointsTrajectory.hpp | 67 +++---------------------- 2 files changed, 86 insertions(+), 59 deletions(-) create mode 100644 src/JointsTrajectory.cpp rename {base => src}/JointsTrajectory.hpp (65%) diff --git a/src/JointsTrajectory.cpp b/src/JointsTrajectory.cpp new file mode 100644 index 00000000..62be1025 --- /dev/null +++ b/src/JointsTrajectory.cpp @@ -0,0 +1,78 @@ +#include "JointsTrajectory.hpp" + +bool base::JointsTrajectory::isValid() const +{ + size_t samples = getTimeSteps(); + + for(size_t i=0; iresize(num_joints); + for(size_t i=0; i getTimeSteps()) + throw(InvalidTimeStep(time_step)); + + joints.resize(getNumberOfJoints()); + joints.names = names; + for(size_t i=0; iresize(num_joints); - for(size_t i=0; i getTimeSteps()) - throw(InvalidTimeStep(time_step)); - - joints.resize(getNumberOfJoints()); - joints.names = names; - for(size_t i=0; i Date: Mon, 8 Aug 2016 13:33:33 +0200 Subject: [PATCH 28/50] moved NamedVector --- {base => src}/NamedVector.hpp | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename {base => src}/NamedVector.hpp (100%) diff --git a/base/NamedVector.hpp b/src/NamedVector.hpp similarity index 100% rename from base/NamedVector.hpp rename to src/NamedVector.hpp From 232ee5b62207b1da51ce250963df0644abca7e89 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Mon, 8 Aug 2016 13:33:42 +0200 Subject: [PATCH 29/50] moved Odometry --- base/Odometry.hpp | 16 ---------------- src/CMakeLists.txt | 10 ++++++++++ 2 files changed, 10 insertions(+), 16 deletions(-) delete mode 100644 base/Odometry.hpp diff --git a/base/Odometry.hpp b/base/Odometry.hpp deleted file mode 100644 index 9f879c16..00000000 --- a/base/Odometry.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef __BASE_ODOMETRY_HPP__ -#define __BASE_ODOMETRY_HPP__ - -#warning "the headers base/odometry.h and base/Odometry.hpp are deprecated. Include the corresponding headers from the slam/odometry package (e.g. odometry/State.hpp for the odometry::State<> template)" -#include -#include -#include -#include -#include - -namespace base -{ - namespace odometry = ::odometry; -} - -#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 9a4274af..9acab668 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -2,6 +2,10 @@ rock_library( base-types Angle.cpp JointLimitRange.cpp + JointLimits.cpp + JointState.cpp + JointsTrajectory.cpp + JointTransform.cpp Pose.cpp Pressure.cpp Spline.cpp @@ -20,7 +24,13 @@ rock_library( Eigen.hpp Float.hpp JointLimitRange.cpp + JointLimits.hpp + JointState.hpp + JointsTrajectory.hpp + JointTransform.hpp Logging.hpp + NamedVector.hpp + Odometry.hpp Point.hpp Pose.hpp Pressure.hpp From 61aa16c0e2ee5496ec59f8500f4551eefea1d33c Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Mon, 8 Aug 2016 13:33:55 +0200 Subject: [PATCH 30/50] added odometry --- src/Odometry.hpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 src/Odometry.hpp diff --git a/src/Odometry.hpp b/src/Odometry.hpp new file mode 100644 index 00000000..9f879c16 --- /dev/null +++ b/src/Odometry.hpp @@ -0,0 +1,16 @@ +#ifndef __BASE_ODOMETRY_HPP__ +#define __BASE_ODOMETRY_HPP__ + +#warning "the headers base/odometry.h and base/Odometry.hpp are deprecated. Include the corresponding headers from the slam/odometry package (e.g. odometry/State.hpp for the odometry::State<> template)" +#include +#include +#include +#include +#include + +namespace base +{ + namespace odometry = ::odometry; +} + +#endif From a30659daba3b0b4a0763b98b34fd40308b700981 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Mon, 8 Aug 2016 16:59:51 +0200 Subject: [PATCH 31/50] ported rest of types --- CMakeLists.txt | 2 - base/commands/AUVMotion.hpp | 26 - base/commands/AUVPosition.hpp | 24 - base/samples/BodyState.hpp | 337 ----------- base/samples/Frame.hpp | 562 ------------------ base/samples/Joints.hpp | 116 ---- base/samples/RigidBodyState.hpp | 293 --------- base/samples/SonarBeam.hpp | 103 ---- base/samples/SonarScan.hpp | 398 ------------- src/CMakeLists.txt | 37 +- src/Odometry.hpp | 16 - {base => src}/commands/Joints.hpp | 0 src/commands/Motion2D.cpp | 12 + {base => src}/commands/Motion2D.hpp | 4 +- {base => src}/commands/Speed6D.hpp | 0 src/samples/BodyState.cpp | 325 ++++++++++ src/samples/BodyState.hpp | 167 ++++++ {base => src}/samples/CommandSamples.hpp | 0 src/samples/CompressedFrame.cpp | 9 + {base => src}/samples/CompressedFrame.hpp | 12 +- src/samples/DepthMap.cpp | 91 +++ {base => src}/samples/DepthMap.hpp | 87 +-- src/samples/DistanceImage.cpp | 38 ++ {base => src}/samples/DistanceImage.hpp | 36 +- src/samples/Frame.cpp | 419 +++++++++++++ src/samples/Frame.hpp | 283 +++++++++ {base => src}/samples/IMUSensors.hpp | 0 src/samples/Joints.cpp | 94 +++ src/samples/Joints.hpp | 47 ++ src/samples/LaserScan.cpp | 70 +++ {base => src}/samples/LaserScan.hpp | 72 +-- {base => src}/samples/Pointcloud.hpp | 0 src/samples/Pressure.cpp | 16 + {base => src}/samples/Pressure.hpp | 19 +- src/samples/RigidBodyAcceleration.cpp | 7 + .../samples/RigidBodyAcceleration.hpp | 5 +- src/samples/RigidBodyState.cpp | 306 ++++++++++ src/samples/RigidBodyState.hpp | 180 ++++++ src/samples/Sonar.cpp | 268 +++++++++ {base => src}/samples/Sonar.hpp | 252 +------- src/samples/SonarBeam.cpp | 63 ++ src/samples/SonarBeam.hpp | 56 ++ src/samples/SonarScan.cpp | 310 ++++++++++ src/samples/SonarScan.hpp | 164 +++++ {base => src}/samples/Wrench.hpp | 0 {base => src}/samples/Wrenches.hpp | 0 {base => src}/templates/TimeStamped.hpp | 0 47 files changed, 3024 insertions(+), 2302 deletions(-) delete mode 100644 base/commands/AUVMotion.hpp delete mode 100644 base/commands/AUVPosition.hpp delete mode 100644 base/samples/BodyState.hpp delete mode 100644 base/samples/Frame.hpp delete mode 100644 base/samples/Joints.hpp delete mode 100644 base/samples/RigidBodyState.hpp delete mode 100644 base/samples/SonarBeam.hpp delete mode 100644 base/samples/SonarScan.hpp delete mode 100644 src/Odometry.hpp rename {base => src}/commands/Joints.hpp (100%) create mode 100644 src/commands/Motion2D.cpp rename {base => src}/commands/Motion2D.hpp (76%) rename {base => src}/commands/Speed6D.hpp (100%) create mode 100644 src/samples/BodyState.cpp create mode 100644 src/samples/BodyState.hpp rename {base => src}/samples/CommandSamples.hpp (100%) create mode 100644 src/samples/CompressedFrame.cpp rename {base => src}/samples/CompressedFrame.hpp (75%) create mode 100644 src/samples/DepthMap.cpp rename {base => src}/samples/DepthMap.hpp (91%) create mode 100644 src/samples/DistanceImage.cpp rename {base => src}/samples/DistanceImage.hpp (85%) create mode 100644 src/samples/Frame.cpp create mode 100644 src/samples/Frame.hpp rename {base => src}/samples/IMUSensors.hpp (100%) create mode 100644 src/samples/Joints.cpp create mode 100644 src/samples/Joints.hpp create mode 100644 src/samples/LaserScan.cpp rename {base => src}/samples/LaserScan.hpp (71%) rename {base => src}/samples/Pointcloud.hpp (100%) create mode 100644 src/samples/Pressure.cpp rename {base => src}/samples/Pressure.hpp (72%) create mode 100644 src/samples/RigidBodyAcceleration.cpp rename {base => src}/samples/RigidBodyAcceleration.hpp (79%) create mode 100644 src/samples/RigidBodyState.cpp create mode 100644 src/samples/RigidBodyState.hpp create mode 100644 src/samples/Sonar.cpp rename {base => src}/samples/Sonar.hpp (52%) create mode 100644 src/samples/SonarBeam.cpp create mode 100644 src/samples/SonarBeam.hpp create mode 100644 src/samples/SonarScan.cpp create mode 100644 src/samples/SonarScan.hpp rename {base => src}/samples/Wrench.hpp (100%) rename {base => src}/samples/Wrenches.hpp (100%) rename {base => src}/templates/TimeStamped.hpp (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 18ad23ad..b16389ac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,5 +34,3 @@ endif() rock_standard_layout() -install(DIRECTORY ${CMAKE_SOURCE_DIR}/base DESTINATION include) - diff --git a/base/commands/AUVMotion.hpp b/base/commands/AUVMotion.hpp deleted file mode 100644 index 407e47fd..00000000 --- a/base/commands/AUVMotion.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef BASE_COMMANDS_AUVMOTION -#define BASE_COMMANDS_AUVMOTION - -namespace base -{ - namespace commands - { - - /** @deprecated */ - struct AUVMotion - { - double heading; //! absolute heading, in radians (positive - //! counter-clockwise, has to be in -PI/PI) - double z; //! absolute altitude, in meters (goes positive upwards) - double x_speed; //! desired forward speed, in m/s - double y_speed; //! desired left speed, in m/s - - AUVMotion() - : heading(0), z(0), x_speed(0), y_speed(0) {} - }; - - } -} - -#endif - diff --git a/base/commands/AUVPosition.hpp b/base/commands/AUVPosition.hpp deleted file mode 100644 index 6f07f74c..00000000 --- a/base/commands/AUVPosition.hpp +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef BASE_COMMANDS_AUVPOSITION -#define BASE_COMMANDS_AUVPOSITION - -namespace base -{ - namespace commands - { - - /** @deprecated */ - struct AUVPosition - { - double heading; //! absolute heading, in radians (positive - //! counter-clockwise, has to be in -PI/PI) - double z; //! absolute altitude, in meters (goes positive upwards) - double x; //! X Position in Pool 0,0 means middle of the Pool - double y; //! Y Position - - AUVPosition() - : heading(0), z(0), x(0), y(0) {} - }; - } -} -#endif - diff --git a/base/samples/BodyState.hpp b/base/samples/BodyState.hpp deleted file mode 100644 index d85debe0..00000000 --- a/base/samples/BodyState.hpp +++ /dev/null @@ -1,337 +0,0 @@ -#ifndef __BASE_SAMPLES_BODY_STATE_HH -#define __BASE_SAMPLES_BODY_STATE_HH - -#include -#include -#include -#include -#include -#include /** For backward compatibility with RBS **/ - -#include -#include - -namespace base { namespace samples { - - /** Representation of the state of a rigid body - * - * This is among other things used to express frame transformations by - * Rock's transformer - * - * Given a source and target frame, this structure expresses the _frame - * change_ between these two frames. In effect, it represents the state of - * the source frame expressed in the target frame. - * - * Per [Rock's conventions](http://rock.opendfki.de/wiki/WikiStart/Standards), you - * should use a X-forward, right handed coordinate system when assigning - * frames to bodies (i.e. X=forward, Y=left, Z=up). In addition, - * world-fixed frames should be aligned to North (North-East-Up) - * - * For instance, if sourceFrame is "body" and targetFrame is "world", then - * the BodyState object is the state of body in the world frame - * (usually, the world frame has an arbitrary origin and a North-East-Up - * orientation). - */ - struct BodyState - { - - BodyState(bool doInvalidation=true) - { - if(doInvalidation) - invalidate(); - }; - - BodyState(const base::TransformWithCovariance& pose, const base::TwistWithCovariance& velocity): - pose(pose), velocity(velocity) {}; - - /** Time-stamp **/ - base::Time time; - - /** Robot pose: rotation in radians and translation in meters */ - base::TransformWithCovariance pose; - - /** TwistWithCovariance: Linear[m/s] and Angular[rad/s] Velocity of the Body */ - /** It is assumed here that velocity is the derivative of a delta pose for - * a given interval. When such interval tends to zero, one could talk - * of instantaneous velocity **/ - base::TwistWithCovariance velocity; - - void setPose(const base::Affine3d& pose) - { - this->pose.setTransform(pose); - } - - const base::Affine3d getPose() const - { - return this->pose.getTransform(); - } - - double getYaw() const - { - return base::getYaw(this->pose.orientation); - } - - double getPitch() const - { - return base::getPitch(this->pose.orientation); - } - - double getRoll() const - { - return base::getRoll(this->pose.orientation); - } - - inline const base::Position& position() const - { - return this->pose.translation; - } - - inline base::Position& position() - { - return this->pose.translation; - } - - /** A read-only expression of the rotation **/ - inline const base::Quaterniond& orientation() const - { - return this->pose.orientation; - } - - inline base::Quaterniond& orientation() - { - return this->pose.orientation; - } - - inline const base::Vector3d& linear_velocity() const - { - return this->velocity.vel; - } - - inline base::Position& linear_velocity() - { - return this->velocity.vel; - } - - inline const base::Vector3d& angular_velocity() const - { - return this->velocity.rot; - } - - inline base::Position& angular_velocity() - { - return this->velocity.rot; - } - - - /** A read-only expression of the pose covariance **/ - inline const base::Matrix6d& cov_pose() const - { - return this->pose.cov; - } - - inline base::Matrix6d& cov_pose() - { - return this->pose.cov; - } - - /** A read-only expression of the rotation covariance **/ - inline const base::Matrix3d cov_orientation() const - { - return this->pose.getOrientationCov(); - } - - inline void cov_orientation(const base::Matrix3d& cov) - { - return this->pose.setOrientationCov(cov); - } - - /** A read-only expression of the position covariance **/ - inline const base::Matrix3d cov_position() const - { - return this->pose.getTranslationCov(); - } - - inline void cov_position(const base::Matrix3d& cov) - { - return this->pose.setTranslationCov(cov); - } - - /** A read-only expression of the velocity covariance **/ - inline const base::Matrix6d& cov_velocity() const - { - return this->velocity.cov; - } - - inline base::Matrix6d& cov_velocity() - { - return this->velocity.cov; - } - - /** A read-only expression of the linear velocity covariance **/ - inline const base::Matrix3d cov_linear_velocity() const - { - return this->velocity.getLinearVelocityCov(); - } - - inline void cov_linear_velocity(const base::Matrix3d& cov) - { - return this->velocity.setLinearVelocityCov(cov); - } - - /** A read-only expression of the angular velocity covariance **/ - inline const base::Matrix3d cov_angular_velocity() const - { - return this->velocity.getAngularVelocityCov(); - } - - inline void cov_angular_velocity(const base::Matrix3d& cov) - { - return this->velocity.setAngularVelocityCov(cov); - } - - static BodyState Unknown() - { - BodyState result(false); - result.initUnknown(); - return result; - }; - - static BodyState Invalid() - { - BodyState result(true); - return result; - }; - - /** For backward compatibility only. Use invalidate() */ - void initSane() - { - invalidate(); - } - - /** Initializes the rigid body state with NaN for the - * position, velocity, orientation and angular velocity. - */ - void invalidate() - { - invalidatePose(); - invalidatePoseCovariance(); - invalidateVelocity(); - invalidateVelocityCovariance(); - } - - /** - * Initializes the rigid body state unknown with Zero for the - * position, velocity and angular velocity, Identity for the orientation - * and infinity for all covariances. - */ - void initUnknown() - { - this->pose.setTransform(base::Affine3d::Identity()); - this->pose.invalidateCovariance(); - this->velocity.setVelocity(base::Vector6d::Zero()); - this->velocity.invalidateCovariance(); - } - - bool hasValidPose() const { return this->pose.hasValidTransform(); } - bool hasValidPoseCovariance() const { return this->pose.hasValidCovariance(); } - void invalidatePose() { this->pose.invalidateTransform(); } - void invalidatePoseCovariance() { this->pose.invalidateCovariance(); } - - bool hasValidVelocity() const { return this->velocity.hasValidVelocity(); } - bool hasValidVelocityCovariance() const { return this->velocity.hasValidCovariance(); } - void invalidateVelocity() { this->velocity.invalidateVelocity(); } - void invalidateVelocityCovariance() { this->velocity.invalidateCovariance(); } - - void invalidateValues ( bool pose = true, bool velocity = true) - { - if (pose) this->invalidatePose(); - if (velocity) this->invalidateVelocity(); - } - - void invalidateCovariances ( bool pose = true, bool velocity = true) - { - if (pose) this->invalidatePoseCovariance(); - if (velocity) this->invalidateVelocityCovariance(); - } - - /** For backward compatibility with RBS **/ - BodyState& operator=( const base::samples::RigidBodyState& rbs ) - { - /** extract the transformation **/ - this->pose.setTransform(rbs.getTransform()); - - /** and the transformation covariance **/ - this->pose.cov << rbs.cov_orientation, Eigen::Matrix3d::Zero(), - Eigen::Matrix3d::Zero(), rbs.cov_position; - - /** Extract the velocity **/ - this->velocity.vel = rbs.velocity; - this->velocity.rot = rbs.angular_velocity; - - /** Extract the velocity covariance **/ - this->velocity.cov << rbs.cov_angular_velocity, Eigen::Matrix3d::Zero(), - Eigen::Matrix3d::Zero(), rbs.cov_velocity; - - return *this; - }; - - - /** Default std::cout function - */ - friend std::ostream & operator<<(std::ostream &out, const TransformWithCovariance& trans); - - /** performs a composition of this Body State with the Body State given. - * The result is another Body State with result = this * trans - */ - BodyState composition( const BodyState& bs ) const - { - return this->operator*( bs ); - }; - - /** alias for the composition of two body states - */ - BodyState operator*( const BodyState& bs ) const - { - const BodyState &bs2(*this); - const BodyState &bs1(bs); - - /** The composition of two body states is here defined as the - * composition of the pose transformation as it is defined in the - * TransformWithCovariance. The velocity held by the last body - * state (here bs1) is assumed to be the current "instantaneous" - * body state velocity and of the resulting body state. - **/ - base::TransformWithCovariance result_pose (static_cast(bs2.pose * bs1.pose)); - base::TwistWithCovariance result_velocity (bs1.velocity); - - /** Resulting velocity and covariance with respect to the pose base frame **/ - result_velocity.vel = result_pose.orientation * result_velocity.vel; - result_velocity.rot = result_pose.orientation * result_velocity.rot; - if (result_velocity.hasValidCovariance()) - { - /** Uncertainty propagation through assuming linear transformation. From R. Astudillo and R.Kolossa Chapter 3 Uncertainty Propagation **/ - /** Improvement at this point(TO-DO): change to Jacobian derivation since the propagation is not linear **/ - Eigen::Matrix3d rot_matrix(result_pose.orientation.toRotationMatrix()); - result_velocity.cov.block<3,3>(0,0) = (rot_matrix * result_velocity.cov.block<3,3>(0,0) * rot_matrix.transpose()); - result_velocity.cov.block<3,3>(3,3) = (rot_matrix * result_velocity.cov.block<3,3>(3,3) * rot_matrix.transpose()); - result_velocity.cov.block<3,3>(3,0) = (rot_matrix * result_velocity.cov.block<3,3>(3,0) * rot_matrix.transpose()); - result_velocity.cov.block<3,3>(0,3) = (rot_matrix * result_velocity.cov.block<3,3>(0,3) * rot_matrix.transpose()); - } - - /* Result Body State **/ - return BodyState(result_pose, result_velocity); - } - }; - - /** Default std::cout function - */ - inline std::ostream & operator<<(std::ostream &out, const base::samples::BodyState& bs) - { - out << bs.pose << "\n"; - out << bs.velocity << "\n"; - return out; - }; - -}}//end namespace base::samples -#endif - diff --git a/base/samples/Frame.hpp b/base/samples/Frame.hpp deleted file mode 100644 index fb9fae60..00000000 --- a/base/samples/Frame.hpp +++ /dev/null @@ -1,562 +0,0 @@ -/*! \file frame.h - \brief container for imaging data -*/ - -#ifndef BASE_SAMPLES_FRAME_H__ -#define BASE_SAMPLES_FRAME_H__ - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - - -namespace base { namespace samples { namespace frame { - struct frame_attrib_t - { - std::string data_; - std::string name_; - inline void set(const std::string &name,const std::string &data) - { - name_ = name; - data_ = data; - } - }; - - struct frame_size_t { - frame_size_t() : width(0), height(0) {} - frame_size_t(uint16_t w, uint16_t h) : width(w), height(h) {} - - bool operator==(const frame_size_t &other) const - { - if(width == other.width && height==other.height) - return true; - return false; - }; - - bool operator!=(const frame_size_t &other) const - { - return !(*this == other); - }; - uint16_t width; - uint16_t height; - }; - - enum frame_mode_t { - MODE_UNDEFINED = 0, - MODE_GRAYSCALE = 1, - MODE_RGB = 2, - MODE_UYVY = 3, - MODE_BGR = 4, - MODE_RGB32 = 5, - RAW_MODES = 128, - MODE_BAYER = RAW_MODES + 0, - MODE_BAYER_RGGB = RAW_MODES + 1, - MODE_BAYER_GRBG = RAW_MODES + 2, - MODE_BAYER_BGGR = RAW_MODES + 3, - MODE_BAYER_GBRG = RAW_MODES + 4, - COMPRESSED_MODES = 256, //if an image is compressed it has no relationship - //between number of pixels and number of bytes - MODE_PJPG = COMPRESSED_MODES + 1, - MODE_JPEG = COMPRESSED_MODES + 2, - MODE_PNG = COMPRESSED_MODES + 3 - }; - - enum frame_status_t { - STATUS_EMPTY, - STATUS_VALID, - STATUS_INVALID - }; - - /* A single image frame */ - struct Frame - { - public: - /** - * Initialize the frame - * @param width the image width, in pixels - * @param height the image height, in pixels - * @param data_depth the number of effective bits per pixels - * @param mode the frame mode (raw, hdr, greyscale, colour) - */ - Frame() : - image(), size(), data_depth(0), pixel_size(0), - frame_mode() - { - setDataDepth(0); - reset(); - } - - //@depth number of bits per pixel and channel - Frame(uint16_t width, uint16_t height, uint8_t depth=8, frame_mode_t mode=MODE_GRAYSCALE, uint8_t const val = 0,size_t sizeInBytes=0) - { - init(width,height,depth,mode,val,sizeInBytes); - } - - //makes a copy of other - Frame(const Frame &other,bool bcopy = true) - { - init(other,bcopy); - } - - //copies all attributes which are independent from size and mode - //if an attribute already exists the old value is over written - void copyImageIndependantAttributes(const Frame &other) - { - std::vector::const_iterator iter = other.attributes.begin(); - for(;iter!= other.attributes.end();++iter) - setAttribute(iter->name_,iter->data_); - time = other.time; - received_time = other.received_time; - frame_status = other.frame_status; - } - - //makes a copy of other - void init(const Frame &other,bool bcopy = true) - { - //hdr is copied by attributes = other.attributes; - init(other.getWidth(),other.getHeight(),other.getDataDepth(), other.getFrameMode(),-1,other.getNumberOfBytes()); - if(bcopy) - setImage(other.getImage()); - copyImageIndependantAttributes(other); - } - - void init(uint16_t width, uint16_t height, uint8_t depth=8, frame_mode_t mode=MODE_GRAYSCALE, const uint8_t val = 0, size_t sizeInBytes=0) - { - //change size if the frame does not fit - if(this->size.height != height || this->size.width != width || this->frame_mode != mode || - this->data_depth != depth || (sizeInBytes != 0 && sizeInBytes != image.size())) - { - //check if depth = 0 - //this might be a programmer error - if(depth==0 && (height != 0 || width != 0 )) - throw std::runtime_error("Frame::init: Cannot initialize frame with depth = 0."); - - this->frame_mode = mode; - this->size = frame_size_t(width, height); - setDataDepth(depth); - } - //calculate size if not given - if(!sizeInBytes) - sizeInBytes = getPixelSize() * getPixelCount(); - - validateImageSize(sizeInBytes); - image.resize(sizeInBytes); - reset(val); - } - - // if val is negative the image will not be initialized - void reset(int const val = 0) - { - this->time = base::Time(); - if (this->image.size() > 0 && val >= 0) { - memset(&this->image[0], val%256, this->image.size()); - } - setStatus(STATUS_EMPTY); - attributes.clear(); - } - - void swap(Frame &frame) - { - //swap vector - image.swap(frame.image); - attributes.swap(frame.attributes); - - //copy values to temp - base::Time temp_time = frame.time; - base::Time temp_received_time = frame.received_time; - frame_size_t temp_size = frame.size; - uint32_t temp_data_depth = frame.data_depth; - uint32_t temp_pixel_size = frame.pixel_size; - uint32_t temp_row_size = frame.row_size; - frame_mode_t temp_frame_mode = frame.frame_mode; - frame_status_t temp_frame_status = frame.frame_status; - - //copy values - frame.time = time; - frame.received_time = received_time; - frame.size = size; - frame.data_depth = data_depth; - frame.pixel_size = pixel_size; - frame.row_size = row_size; - frame.frame_mode = frame_mode; - frame.frame_status = frame_status; - - time = temp_time; - received_time = temp_received_time; - size = temp_size; - data_depth = temp_data_depth; - pixel_size = temp_pixel_size; - row_size = temp_row_size; - frame_mode = temp_frame_mode; - frame_status = temp_frame_status; - } - - inline bool isHDR()const - { - return (hasAttribute("hdr")&&getAttribute("hdr")); - } - - inline void setHDR(bool value) - { - setAttribute("hdr",true); - } - - inline bool isCompressed()const - { - return frame_mode >= COMPRESSED_MODES; - } - - inline bool isGrayscale()const { - return this->frame_mode == MODE_GRAYSCALE; - } - inline bool isRGB()const { - return this->frame_mode == MODE_RGB; - } - - inline bool isBayer()const { - return (this->frame_mode == MODE_BAYER || this->frame_mode == MODE_BAYER_RGGB || this->frame_mode == MODE_BAYER_GRBG || this->frame_mode == MODE_BAYER_BGGR || this->frame_mode == MODE_BAYER_GBRG); - } - - inline void setStatus(const frame_status_t value){ - frame_status = value; - } - - inline frame_status_t getStatus()const{ - return frame_status; - } - - inline uint32_t getChannelCount() const { - return getChannelCount(this->frame_mode); - } - static uint32_t getChannelCount(frame_mode_t mode) - { - switch (mode) - { - case MODE_UNDEFINED: - return 0; - case MODE_BAYER: - case MODE_BAYER_RGGB: - case MODE_BAYER_BGGR: - case MODE_BAYER_GBRG: - case MODE_BAYER_GRBG: - case MODE_GRAYSCALE: - case MODE_UYVY: - return 1; - case MODE_RGB: - case MODE_BGR: - return 3; - case MODE_RGB32: - return 4; - case MODE_PJPG: - case MODE_JPEG: - case MODE_PNG: - return 1; - default: - throw std::runtime_error("Frame::getChannelCount: Unknown frame_mode"); - return 0; - } - } - - //qt ruby does not support enums as slot parameters - //therefore frame_mode_t is passed as string - static frame_mode_t toFrameMode(const std::string &str) - { - if(str == "MODE_UNDEFINED") - return MODE_UNDEFINED; - else if (str == "MODE_GRAYSCALE") - return MODE_GRAYSCALE; - else if (str == "MODE_RGB") - return MODE_RGB; - else if (str == "MODE_BGR") - return MODE_BGR; - else if (str == "MODE_UYVY") - return MODE_UYVY; - else if (str == "RAW_MODES") - return RAW_MODES; - else if (str == "MODE_BAYER") - return MODE_BAYER; - else if (str == "MODE_BAYER_RGGB") - return MODE_BAYER_RGGB; - else if (str == "MODE_BAYER_GRBG") - return MODE_BAYER_GRBG; - else if (str == "MODE_BAYER_BGGR") - return MODE_BAYER_BGGR; - else if (str == "MODE_BAYER_GBRG") - return MODE_BAYER_GBRG; - else if (str == "MODE_RGB32") - return MODE_RGB32; - else if (str == "COMPRESSED_MODES") - return COMPRESSED_MODES; - else if (str == "MODE_PJPG") - return MODE_PJPG; - else if (str == "MODE_JPEG") - return MODE_JPEG; - else if (str == "MODE_PNG") - return MODE_PNG; - else - return MODE_UNDEFINED; - }; - - inline frame_mode_t getFrameMode() const { - return this->frame_mode; - } - - /** - * Returns the size of a pixel (in bytes). This takes into account the image - * mode as well as the data depth. - * @return Number of channels * bytes used to represent one colour - */ - inline uint32_t getPixelSize() const { - return this->pixel_size; - } - - /** - * Returns the size of a row (in bytes). This takes into account the image - * mode as well as the data depth. - * @return Number of channels * width * bytes used to represent one colour - * @return 0 if the image is compressed - */ - inline uint32_t getRowSize() const { - if(isCompressed()) - throw std::runtime_error("Frame::getRowSize: There is no raw size for an compressed image!"); - return this->row_size; - } - - /** - * Returns the total number of bytes for the image - */ - inline uint32_t getNumberOfBytes() const { - return image.size(); - } - - /** - * Returns the total count of pixels in this frame - * @return Returns the overall number of pixels (width * height) - */ - inline uint32_t getPixelCount() const { - return size.width * size.height; - } - - inline uint32_t getDataDepth() const { - return this->data_depth; - } - void setDataDepth(uint32_t value) - { - this->data_depth = value; - - // Update pixel size - uint32_t comp_size = ((this->data_depth + 7) / 8); - this->pixel_size = getChannelCount(this->frame_mode) * comp_size; - - //update row size - if(isCompressed()) - this->row_size = 0; //disable row size - else - this->row_size = this->pixel_size * getWidth(); - - } - - void setFrameMode(frame_mode_t mode) - { - this->frame_mode = mode; - - // Update pixel size - uint32_t comp_size = ((this->data_depth + 7) / 8); - this->pixel_size = getChannelCount(this->frame_mode) * comp_size; - - //update row size - if(isCompressed()) - this->row_size = 0; //disable row size - else - this->row_size = this->pixel_size * getWidth(); - } - - inline frame_size_t getSize() const { - return this->size; - } - inline uint16_t getWidth() const { - return this->size.width; - } - inline uint16_t getHeight() const { - return this->size.height; - } - - inline const std::vector &getImage() const { - return this->image; - } - - void validateImageSize(size_t sizeToValidate) const { - size_t expected_size = getPixelSize()*getPixelCount(); - if (!isCompressed() && sizeToValidate != expected_size){ - std::cerr << "Frame: " - << __FUNCTION__ << " (" << __FILE__ << ", line " - << __LINE__ << "): " << "image size mismatch in setImage() (" - << "getting " << sizeToValidate << " bytes but I was expecting " << expected_size << " bytes)" - << std::endl; - throw std::runtime_error("Frame::validateImageSize: wrong image size!"); - } - } - inline void setImage(const std::vector &newImage) { - // calling the overloading function wich uses the "char*" interface - return setImage(newImage.data(), newImage.size()); - } - /** This is for backward compatibility for the people that were - * using the 'char' signature */ - inline void setImage(const char *data, size_t newImageSize) { - return setImage(reinterpret_cast(data), newImageSize); - } - inline void setImage(const uint8_t *data, size_t newImageSize) { - validateImageSize(newImageSize); - image.resize(newImageSize); - memcpy(&this->image[0], data, newImageSize); - } - - inline uint8_t *getImagePtr() { - return static_cast(image.data()); - } - inline const uint8_t *getImageConstPtr() const { - return static_cast(image.data()); - } - - inline uint8_t* getLastByte(){ - return static_cast(&this->image.back()); - } - - inline const uint8_t* getLastConstByte()const{ - return static_cast(&this->image.back()); - } - - inline bool hasAttribute(const std::string &name)const - { - std::vector::const_iterator _iter = attributes.begin(); - for (;_iter != attributes.end();_iter++) - { - if (_iter->name_ == name) - return true; - } - return false; - } - template - inline T getAttribute(const std::string &name)const - { - static T default_value; - std::stringstream strstr; - - std::vector::const_iterator _iter = attributes.begin(); - for (;_iter != attributes.end();_iter++) - { - if (_iter->name_ == name) - { - T data; - strstr << _iter->data_; - strstr >> data; - return data; - } - } - return default_value; - } - - inline bool deleteAttribute(const std::string &name) - { - std::vector::iterator _iter = attributes.begin(); - for (;_iter != attributes.end();_iter++) - { - if (_iter->name_ == name) - { - attributes.erase(_iter); - return true; - } - } - return false; - } - - - template - inline void setAttribute(const std::string &name,const T &data) - { - //if attribute exists - std::stringstream strstr; - strstr << data; - std::vector::iterator _iter = attributes.begin(); - for (;_iter != attributes.end();_iter++) - { - if (_iter->name_ == name) - { - _iter->set(name,strstr.str()); - return; - } - } - //if attribute does not exist - attributes.push_back(frame_attrib_t()); - attributes.back().set(name,strstr.str()); - return ; - } - - template Tp& at(unsigned int column,unsigned int row) - { - if(column >= size.width || row >= size.height ) - throw std::runtime_error("out of index"); - return *((Tp*)(getImagePtr()+row*getRowSize()+column*getPixelSize())); - } - - /** The time at which this frame has been captured - * - * This is obviously an estimate - */ - base::Time time; - /** The time at which this frame has been received on the system */ - base::Time received_time; - - /** The raw data */ - std::vector image; - /** Additional metadata */ - std::vector attributes; - - /** The image size in pixels */ - frame_size_t size; - - /** The number of effective bits per channel. The number - * of actual bits per channel is always a multiple of - * height (i.e. a 12-bit effective depth is represented - * using 16-bits per channels). The number of greyscale - * levels is 2^(this_number) - */ - uint32_t data_depth; - /** The size of one pixel, in bytes - * - * For instance, for a RGB image with a 8 bit data depth, it would - * be 3. For a 12 bit non-packed image (i.e with each channel - * encoded on 2 bytes), it would be 6. - */ - uint32_t pixel_size; - - /** The size of a complete row in bytes - */ - uint32_t row_size; - - /** The colorspace of the image - */ - frame_mode_t frame_mode; - - /** Status flag */ - frame_status_t frame_status; - }; - - struct FramePair - { - base::Time time; - Frame first; - Frame second; - uint32_t id; - }; -}}} - -#endif diff --git a/base/samples/Joints.hpp b/base/samples/Joints.hpp deleted file mode 100644 index 5a13109a..00000000 --- a/base/samples/Joints.hpp +++ /dev/null @@ -1,116 +0,0 @@ -#ifndef BASE_SAMPLES_JOINTS_HPP -#define BASE_SAMPLES_JOINTS_HPP - -#include -#include - -#include -#include -#include - -namespace base -{ - namespace samples - { - /** Data structure that gives out state readings for a set of joints - */ - struct Joints : public base::NamedVector - { - /** The sample timestamp */ - base::Time time; - - static Joints Positions(std::vector const& positions) - { - Joints result; - result.elements.resize(positions.size()); - for (std::size_t i = 0; i != positions.size(); ++i) - result[i].position = positions[i]; - return result; - } - - static Joints Positions(std::vector const& positions, std::vector const& names) - { - Joints result = Positions(positions); - if (result.elements.size() != names.size()) - throw std::runtime_error("the position and names vectors differ"); - result.names = names; - return result; - } - - static Joints Speeds(std::vector const& speeds) - { - Joints result; - result.elements.resize(speeds.size()); - for (std::size_t i = 0; i != speeds.size(); ++i) - result[i].speed= speeds[i]; - return result; - } - - static Joints Speeds(std::vector const& speeds, std::vector const& names) - { - Joints result = Speeds(speeds); - if (result.elements.size() != names.size()) - throw std::runtime_error("the speeds and names vectors differ"); - result.names = names; - return result; - } - - static Joints Efforts(std::vector const& efforts) - { - Joints result; - result.elements.resize(efforts.size()); - for (std::size_t i = 0; i != efforts.size(); ++i) - result[i].effort = efforts[i]; - return result; - } - - static Joints Efforts(std::vector const& efforts, std::vector const& names) - { - Joints result = Efforts(efforts); - if (result.elements.size() != names.size()) - throw std::runtime_error("the effort and names vectors differ"); - result.names = names; - return result; - } - - static Joints Raw(std::vector const& raw) - { - Joints result; - result.elements.resize(raw.size()); - for (std::size_t i = 0; i != raw.size(); ++i) - result[i].raw = raw[i]; - return result; - } - - static Joints Raw(std::vector const& raw, std::vector const& names) - { - Joints result = Raw(raw); - if (result.elements.size() != names.size()) - throw std::runtime_error("the raw and names vectors differ"); - result.names = names; - return result; - } - - static Joints Accelerations(std::vector const& acceleration) - { - Joints result; - result.elements.resize(acceleration.size()); - for (std::size_t i = 0; i != acceleration.size(); ++i) - result[i].acceleration = acceleration[i]; - return result; - } - - static Joints Accelerations(std::vector const& acceleration, std::vector const& names) - { - Joints result = Accelerations(acceleration); - if (result.elements.size() != names.size()) - throw std::runtime_error("the acceleration and names vectors differ"); - result.names = names; - return result; - } - }; - } -} - -#endif - diff --git a/base/samples/RigidBodyState.hpp b/base/samples/RigidBodyState.hpp deleted file mode 100644 index 06de3209..00000000 --- a/base/samples/RigidBodyState.hpp +++ /dev/null @@ -1,293 +0,0 @@ -#ifndef __BASE_SAMPLES_RIGID_BODY_STATE_HH -#define __BASE_SAMPLES_RIGID_BODY_STATE_HH - -#include -#include -#include - -#include -#include - -namespace base { namespace samples { - /** Representation of the state of a rigid body - * - * This is among other things used to express frame transformations by - * Rock's transformer - * - * Given a source and target frame, this structure expresses the _frame - * change_ between these two frames. In effect, it represents the state of - * the source frame expressed in the target frame. - * - * Per [Rock's conventions](http://rock.opendfki.de/wiki/WikiStart/Standards), you - * should use a X-forward, right handed coordinate system when assigning - * frames to bodies (i.e. X=forward, Y=left, Z=up). In addition, - * world-fixed frames should be aligned to North (North-West-Up, aka NWU) - * - * For instance, if sourceFrame is "body" and targetFrame is "world", then - * the RigidBodyState object is the state of body in the world frame - * (usually, the world frame has an arbitrary origin and a North-West-Up - * orientation). - */ - struct RigidBodyState - { - - RigidBodyState(bool doInvalidation=true){ - if(doInvalidation) - invalidate(); - }; - - base::Time time; - - /** Name of the source reference frame */ - std::string sourceFrame; - - /** Name of the target reference frame */ - std::string targetFrame; - - /** Position in m of sourceFrame's origin expressed in targetFrame - */ - Position position; - /** Covariance matrix of the position - */ - base::Matrix3d cov_position; - - /** Orientation of targetFrame expressed in sourceFrame */ - Orientation orientation; - /** Covariance matrix of the orientation as an axis/angle manifold in - * body coordinates - */ - base::Matrix3d cov_orientation; - - /** Velocity in m/s of sourceFrame expressed in targetFrame */ - base::Vector3d velocity; - /** Covariance of the velocity - */ - base::Matrix3d cov_velocity; - - /** Angular Velocity as an axis-angle representation in body fixed frame - * - * The direction of the vector is the axis, its length the speed */ - base::Vector3d angular_velocity; - /** Covariance of the angular velocity - */ - base::Matrix3d cov_angular_velocity; - - void setTransform(const Eigen::Affine3d& transform) - { - position = transform.translation(); - orientation = Eigen::Quaterniond( transform.linear() ); - } - - Eigen::Affine3d getTransform() const - { - Eigen::Affine3d ret; - ret.setIdentity(); - ret.rotate(this->orientation); - ret.translation() = this->position; - return ret; - } - - void setPose(const base::Pose& pose) - { - orientation = pose.orientation; - position = pose.position; - } - - base::Pose getPose() const - { - return base::Pose( position, orientation ); - } - - double getYaw() const - { - return base::getYaw(orientation); - } - - double getPitch() const - { - return base::getPitch(orientation); - } - - double getRoll() const - { - return base::getRoll(orientation); - } - - template - operator Eigen::Transform() const - { - Eigen::Transform ret; - ret.setIdentity(); - ret.rotate(this->orientation); - ret.translation() = this->position; - return ret; - } - - static RigidBodyState unknown(){ - RigidBodyState result(false); - result.initUnknown(); - return result; - }; - - static RigidBodyState invalid() { - RigidBodyState result(true); - return result; - }; - - /** For backward compatibility only. Use invalidate() */ - void initSane() { - invalidate(); - } - - /** Initializes the rigid body state with NaN for the - * position, velocity, orientation and angular velocity. - */ - void invalidate() { - invalidateOrientation(); - invalidateOrientationCovariance(); - invalidatePosition(); - invalidatePositionCovariance(); - invalidateVelocity(); - invalidateVelocityCovariance(); - invalidateAngularVelocity(); - invalidateAngularVelocityCovariance(); - } - - /** - * Initializes the rigid body state unknown with Zero for the - * position, velocity and angular velocity, Identity for the orientation - * and infinity for all covariances. - */ - void initUnknown() - { - position.setZero(); - velocity.setZero(); - orientation = Eigen::Quaterniond::Identity(); - angular_velocity.setZero(); - cov_position = setValueUnknown(); - cov_orientation = setValueUnknown(); - cov_velocity = setValueUnknown(); - cov_angular_velocity = setValueUnknown(); - } - - /** Helper method that checks if a value is valid (not NaN anywhere). */ - static bool isValidValue(base::Vector3d const& vec) - { - return !base::isNaN(vec(0)) && - !base::isNaN(vec(1)) && - !base::isNaN(vec(2)); - } - - /** Helper method that checks if an orientation is valid (not NaN anywhere) - * and that the orientation is an unit quaternion. */ - static bool isValidValue(base::Orientation const& ori) - { - return !base::isNaN(ori.w()) && - !base::isNaN(ori.x()) && - !base::isNaN(ori.y()) && - !base::isNaN(ori.z()) && - fabs(ori.squaredNorm()-1.0) < 1e-6; //assuming at least single precision - } - - /** Helper method that checks if the value whose covariance is - * represented by the given matrix is a known value (not Inf in the diagonal). - */ - static bool isKnownValue(base::Matrix3d const& cov) - { - return !base::isInfinity(cov(0,0)) && - !base::isInfinity(cov(1,1)) && - !base::isInfinity(cov(2,2)); - } - - /** Helper method that checks if the covariance represented by the given - * matrix is valid - */ - static bool isValidCovariance(base::Matrix3d const& cov) - { - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - if (base::isNaN(cov(i, j))) - return false; - return true; - } - - /** Checks if a given dimension of a vector is valid. */ - static bool isValidValue(base::Vector3d const& vec, int dim) - { - return !base::isNaN(vec(dim)); - } - - /** Checks if a given dimension of a covariance matrix is valid. */ - static bool isValidCovariance(base::Matrix3d const& cov, int dim) - { - return !base::isNaN(cov(dim,dim)); - } - - /** Checks if the dimension of a vector given by a covariance is known. */ - static bool isKnownValue(base::Matrix3d const& cov, int dim) - { - return !base::isInfinity(cov(dim,dim)); - } - - static base::Vector3d invalidValue() - { - return base::Vector3d::Ones() * base::NaN(); - } - - static base::Orientation invalidOrientation() - { - return base::Orientation(Eigen::Vector4d::Ones() * base::NaN()); - } - - static base::Matrix3d setValueUnknown() - { - return base::Matrix3d::Ones() * base::infinity(); - } - - static base::Matrix3d invalidCovariance() - { - return base::Matrix3d::Ones() * base::NaN(); - } - - bool hasValidPosition() const { return isValidValue(position); } - bool hasValidPosition(int idx) const { return isValidValue(position, idx); } - bool hasValidPositionCovariance() const { return isValidCovariance(cov_position); } - void invalidatePosition() { position = invalidValue(); } - void invalidatePositionCovariance() { cov_position = invalidCovariance(); } - - bool hasValidOrientation() const { return isValidValue(orientation); } - bool hasValidOrientationCovariance() const { return isValidCovariance(cov_orientation); } - void invalidateOrientation() { orientation = invalidOrientation(); } - void invalidateOrientationCovariance() { cov_orientation = invalidCovariance(); } - - bool hasValidVelocity() const { return isValidValue(velocity); } - bool hasValidVelocity(int idx) const { return isValidValue(velocity, idx); } - bool hasValidVelocityCovariance() const { return isValidCovariance(cov_velocity); } - void invalidateVelocity() { velocity = invalidValue(); } - void invalidateVelocityCovariance() { cov_velocity = invalidCovariance(); } - - bool hasValidAngularVelocity() const { return isValidValue(angular_velocity); } - bool hasValidAngularVelocity(int idx) const { return isValidValue(angular_velocity, idx); } - bool hasValidAngularVelocityCovariance() const { return isValidCovariance(cov_angular_velocity); } - void invalidateAngularVelocity() { angular_velocity = invalidValue(); } - void invalidateAngularVelocityCovariance() { cov_angular_velocity = invalidCovariance(); } - - void invalidateValues(bool invPos, bool invOri, bool invVel = true, - bool invAngVel = true) { - if (invPos) invalidatePosition(); - if (invOri) invalidateOrientation(); - if (invVel) invalidateVelocity(); - if (invAngVel) invalidateAngularVelocity(); - } - - void invalidateCovariances(bool invPos = true, bool invOri = true, - bool invVel = true, bool invAngVel = true) { - if (invPos) invalidatePositionCovariance(); - if (invOri) invalidateOrientationCovariance(); - if (invVel) invalidateVelocityCovariance(); - if (invAngVel) invalidateAngularVelocityCovariance(); - } - }; -}} - -#endif diff --git a/base/samples/SonarBeam.hpp b/base/samples/SonarBeam.hpp deleted file mode 100644 index 1ca76a8f..00000000 --- a/base/samples/SonarBeam.hpp +++ /dev/null @@ -1,103 +0,0 @@ -#ifndef BASE_SAMPLES_SONARBEAM_H__ -#define BASE_SAMPLES_SONARBEAM_H__ - -#include -#include -#include -#include -#include - -namespace base { namespace samples { - - struct SonarBeam { - typedef boost::uint8_t uint8_t; - - //timestamp of the sonar beam - Time time; - - //direction of the sonar beam in radians [-pi,+pi] - //zero is at the front - Angle bearing; - - //sampling interval of each range bin in secs - double sampling_interval; - - //speed of sound - //this takes the medium into account - float speed_of_sound; - - //horizontal beamwidth of the sonar beam in radians - float beamwidth_horizontal; - - //vertical beamwidth of the sonar beam in radians - float beamwidth_vertical; - - //received echoes (bins) along the beam - std::vector beam; - - SonarBeam(): - sampling_interval(std::numeric_limits::signaling_NaN()), - speed_of_sound(std::numeric_limits::signaling_NaN()), - beamwidth_horizontal(std::numeric_limits::signaling_NaN()), - beamwidth_vertical(std::numeric_limits::signaling_NaN()){} - - SonarBeam(const SonarBeam &other) - { - init(other); - } - - //calculates the spatial resolution of the sonar beam in meter - //this takes the sampling_interval and the speed of sound into account - double getSpatialResolution()const - { - //the sampling interval includes the time for - //the sound traveling from the transiter to the target an back - //to receiver - return sampling_interval*0.5*speed_of_sound; - } - - SonarBeam &operator=(const SonarBeam &other) - { - init(other); - return *this; - } - - void init(const SonarBeam &other) - { - time = other.time; - bearing = other.bearing; - sampling_interval = other.sampling_interval; - speed_of_sound = other.speed_of_sound; - beamwidth_horizontal = other.beamwidth_horizontal; - beamwidth_vertical = other.beamwidth_vertical; - beam = other.beam; - } - - void swap(SonarBeam &other) - { - Time temp_time = time; - Angle temp_bearing = bearing; - double temp_sampling_interval = sampling_interval; - float temp_speed_of_sound = speed_of_sound; - float temp_beamwidth_horizontal = beamwidth_horizontal; - float temp_beamwidth_vertical= beamwidth_vertical; - - time = other.time; - bearing = other.bearing; - sampling_interval = other.sampling_interval; - speed_of_sound = other.speed_of_sound; - beamwidth_horizontal = other.beamwidth_horizontal; - beamwidth_vertical = other.beamwidth_vertical; - beam.swap(other.beam); - - other.time = temp_time; - other.bearing = temp_bearing; - other.sampling_interval = temp_sampling_interval; - other.speed_of_sound = temp_speed_of_sound; - other.beamwidth_horizontal = temp_beamwidth_horizontal; - other.beamwidth_vertical = temp_beamwidth_vertical; - } - }; -}} - -#endif diff --git a/base/samples/SonarScan.hpp b/base/samples/SonarScan.hpp deleted file mode 100644 index 5de61a8f..00000000 --- a/base/samples/SonarScan.hpp +++ /dev/null @@ -1,398 +0,0 @@ -/*! \file sonar_scan.h - \brief container for sonar scan data - */ - -#ifndef BASE_SAMPLES_SONARSCAN_H__ -#define BASE_SAMPLES_SONARSCAN_H__ - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace base { namespace samples { - - struct SonarScan - { - public: - /** - * Initialize the sonar scan - */ - SonarScan() - : number_of_beams(0) - , number_of_bins(0) - , sampling_interval(0) - , speed_of_sound(0) - , beamwidth_horizontal(base::Angle::fromRad(0)) - , beamwidth_vertical(base::Angle::fromRad(0)) - , memory_layout_column(true) - , polar_coordinates(true) - { - reset(); - } - - SonarScan(uint16_t number_of_beams,uint16_t number_of_bins,Angle start_bearing,Angle angular_resolution,bool memory_layout_column=true) - { - init(number_of_beams,number_of_bins,start_bearing,angular_resolution,memory_layout_column); - } - - //makes a copy of other - SonarScan(const SonarScan &other,bool bcopy = true) - { - init(other,bcopy); - } - - SonarScan &operator=(const SonarScan &other) - { - init(other,true); - return *this; - } - - //makes a copy of other - void init(const SonarScan &other,bool bcopy = true) - { - init(other.number_of_beams,other.number_of_bins,other.start_bearing,other.angular_resolution,other.memory_layout_column); - time = other.time; - - beamwidth_vertical = other.beamwidth_vertical; - beamwidth_horizontal = other.beamwidth_horizontal; - sampling_interval = other.sampling_interval; - speed_of_sound = other.speed_of_sound; - polar_coordinates = other.polar_coordinates; - if(bcopy){ - setData(other.getData()); - time_beams = other.time_beams; - } - } - - void init(uint16_t number_of_beams, uint16_t number_of_bins, Angle start_bearing, Angle angular_resolution,bool memory_layout_column = true, int val=-1) - { - //change size if the sonar scan does not fit - if(this->number_of_beams != number_of_beams || this->number_of_bins != number_of_bins) - { - this->number_of_beams = number_of_beams; - this->number_of_bins = number_of_bins; - data.resize(number_of_beams*number_of_bins); - } - this->start_bearing = start_bearing; - this->angular_resolution = angular_resolution; - this->memory_layout_column = memory_layout_column; - speed_of_sound = 0; - beamwidth_horizontal = base::Angle::fromRad(0); - beamwidth_vertical = base::Angle::fromRad(0); - reset(val); - } - - // if val is negative the sonar scan data will not be initialized - void reset(int const val = 0) - { - this->time = base::Time(); - if (this->data.size() > 0 && val >= 0) - { - memset(&this->data[0], val%256, this->data.size()); - } - time_beams.clear(); - } - - //returns the index/column of the sonar beam which - //holds information about the given bearing - //returns -1 if the sonar scan has no sonar beam for the given bearing - int beamIndexForBearing(const Angle bearing,bool range_check=true)const - { - double temp_rad = (start_bearing-bearing).rad; - int index = round(temp_rad/angular_resolution.rad); - if(range_check && (index < 0 || index >= number_of_beams)) - return -1; - return index; - } - - //returns true if a sonar beam was already added for the given - //bearing - //internally time_beams is used for checking - bool hasSonarBeam(const base::samples::SonarBeam &sonar_beam)const - { - return hasSonarBeam(sonar_beam.bearing); - } - - bool hasSonarBeam(const Angle bearing)const - { - int index = beamIndexForBearing(bearing); - if(index < 0) - return false; - - //it is assumed that all data are set at once (imaging sonar) - if(time_beams.empty()) - return true; - - //it is assumed that the data are set beam by beam (scanning sonar) - if(time_beams[index].microseconds != 0) - return true; - return false; - } - - - //adds a sonar beam to the sonar scan - //throws an exception if the sonar scan cannot hold the sonar beam and resize is set to false - //otherwise the sonar scan will be resized (the start angle is not changed!) - // - //the memory layout must be one sonar beam per row otherwise the function will throw a std::runtime_error - void addSonarBeam(const base::samples::SonarBeam &sonar_beam,bool resize=true) - { - if(memory_layout_column) - throw std::runtime_error("addSonarBeam: cannot add sonar beam because the memory layout is not supported. Call toggleMemoryLayout()"); - - if(number_of_bins < sonar_beam.beam.size()) - throw std::runtime_error("addSonarBeam: cannot add sonar beam: too many bins"); - - int index = beamIndexForBearing(sonar_beam.bearing,false); - if(index < 0) - throw std::runtime_error("addSonarBeam: negative index!"); - if(index >= number_of_beams) - { - if(!resize) - throw std::runtime_error("addSonarBeam: bearing is out of range"); - number_of_beams = index+1; - data.resize(number_of_beams*number_of_bins); - } - if(time_beams.size() != number_of_beams) - time_beams.resize(number_of_beams); - - time_beams[index] = sonar_beam.time; - sampling_interval = sonar_beam.sampling_interval; - beamwidth_vertical = Angle::fromRad(sonar_beam.beamwidth_vertical); - beamwidth_horizontal = Angle::fromRad(sonar_beam.beamwidth_horizontal); - speed_of_sound = sonar_beam.speed_of_sound; - memcpy(&data[index*number_of_bins],&sonar_beam.beam[0],sonar_beam.beam.size()); - } - - //returns a sonar beam for a given bearing - //throws an exception if the sonar scans holds no information for the given bearing - //the memory layout must be one beam per row - // - //an exception is thrown if the sonar beam holds no information about the given bearing - void getSonarBeam(const Angle bearing,SonarBeam &sonar_beam)const - { - if(memory_layout_column) - throw std::runtime_error("getSonarBeam: Wrong memory layout!"); - int index = beamIndexForBearing(bearing); - if(index<0) - throw std::runtime_error("getSonarBeam: No Data for the given bearing!"); - - sonar_beam.beam.resize(number_of_bins); - memcpy(&sonar_beam.beam[0],&data[number_of_bins*index],number_of_bins); - if((int)time_beams.size() > index) - sonar_beam.time = time_beams[index]; - else - sonar_beam.time = time; - sonar_beam.speed_of_sound = speed_of_sound; - sonar_beam.beamwidth_horizontal = beamwidth_horizontal.rad; - sonar_beam.beamwidth_vertical = beamwidth_vertical.rad; - sonar_beam.sampling_interval = sampling_interval; - sonar_beam.bearing = bearing; - } - - //this toggles the memory layout between one sonar beam per row and one sonar beam per column - //to add sonar beams the memory layout must be one sonar beam per row - void toggleMemoryLayout() - { - std::vector temp; - temp.resize(data.size()); - - if(memory_layout_column) - { - for(int row=0;row < number_of_beams;++row) - for(int column=0;column < number_of_bins;++column) - temp[row*number_of_bins+column] =data[column*number_of_beams+row]; - } - else - { - for(int row=0;row < number_of_beams;++row) - for(int column=0;column < number_of_bins;++column) - temp[column*number_of_beams+row] =data[row*number_of_bins+column]; - } - memory_layout_column = !memory_layout_column; - data.swap(temp); - } - - void swap(SonarScan &sonar_scan) - { - //swap vector - data.swap(sonar_scan.data); - - //copy values to temp - base::Time temp_time = sonar_scan.time; - Angle temp_beamwidth_vertical = sonar_scan.beamwidth_vertical; - Angle temp_beamwidth_horizontal = sonar_scan.beamwidth_horizontal; - double temp_sampling_interval = sonar_scan.sampling_interval; - uint16_t temp_number_of_beams = sonar_scan.number_of_beams; - uint16_t temp_number_of_bins = sonar_scan.number_of_bins; - Angle temp_start_bearing = sonar_scan.start_bearing; - Angle temp_angular_resolution = sonar_scan.angular_resolution; - bool temp_memory_layout_column = sonar_scan.memory_layout_column; - bool temp_polar_coordinates = sonar_scan.polar_coordinates; - float temp_speed_of_sound = sonar_scan.speed_of_sound; - - //copy values - sonar_scan.time = time; - sonar_scan.beamwidth_vertical = beamwidth_vertical; - sonar_scan.beamwidth_horizontal = beamwidth_horizontal; - sonar_scan.sampling_interval = sampling_interval; - sonar_scan.number_of_beams = number_of_beams; - sonar_scan.number_of_bins = number_of_bins; - sonar_scan.start_bearing = start_bearing; - sonar_scan.angular_resolution = angular_resolution; - sonar_scan.memory_layout_column = memory_layout_column; - sonar_scan.polar_coordinates = polar_coordinates; - sonar_scan.speed_of_sound = speed_of_sound; - - time = temp_time; - beamwidth_vertical = temp_beamwidth_vertical; - beamwidth_horizontal = temp_beamwidth_horizontal; - sampling_interval = temp_sampling_interval; - number_of_beams = temp_number_of_beams; - number_of_bins = temp_number_of_bins; - start_bearing = temp_start_bearing; - angular_resolution = temp_angular_resolution; - memory_layout_column = temp_memory_layout_column; - polar_coordinates = temp_polar_coordinates; - speed_of_sound = temp_speed_of_sound; - } - - inline uint32_t getNumberOfBytes() const { - return data.size(); - } - - /** - * Returns the total count of bins in this sonar scan - * @return Returns the overall number of bins (number_of_beams * number_of_bins) - */ - inline uint32_t getBinCount() const { - return number_of_beams * number_of_bins; - } - - inline const std::vector &getData() const { - return this->data; - } - - - Angle getEndBearing()const - { - return start_bearing-angular_resolution*(number_of_beams-1); - } - - Angle getStartBearing()const - { - return start_bearing; - } - - Angle getAngularResolution()const - { - return angular_resolution; - } - - //calculates the spatial resolution of the sonar scan in meter - //this takes the sampling_interval and the speed of sound into account - double getSpatialResolution()const - { - //the sampling interval includes the time for - //the sound traveling from the transmitter to the target an back - //to the receiver - return sampling_interval*0.5*speed_of_sound; - } - - inline void setData(const std::vector &data) { - this->data = data; - } - inline void setData(const char *data, uint32_t size) { - if (size != this->data.size()) - { - std::cerr << "SonarScan: " - << __FUNCTION__ << " (" << __FILE__ << ", line " - << __LINE__ << "): " << "size mismatch in setData() (" - << size << " != " << this->data.size() << ")" - << std::endl; - return; - } - memcpy(&this->data[0], data, size); - } - - inline uint8_t *getDataPtr() { - return static_cast(&this->data[0]); - } - inline const uint8_t *getDataConstPtr() const { - return static_cast(&this->data[0]); - } - - //The time at which this sonar scan has been captured - base::Time time; - - //The raw data of the sonar scan - std::vector data; - - //Time stamp for each sonar beam - //if the time stamp is 1 January 1970 - //the beam is regarded as not be set - //vector can be empty in this case time - //is used for all beams - std::vector time_beams; - - //number of beams - //this can be interpreted as image width - uint16_t number_of_beams; - - //number of bins - //this can be interpreted as image height - uint16_t number_of_bins; - - /** The angle at which the reading starts. Zero is at the front of - * the device and turns counter-clockwise. - * This value is in radians - * - * All beams are stored from left to right to match the memory - * layout of an image !!! Therefore the end bearing is usually - * smaller than the start bearing - */ - Angle start_bearing; - - // Angle difference between two beams in radians - // The beams are stored from left to right - // to match the memory layout of an image - // - // This value must be always positive ! - Angle angular_resolution; - - //sampling interval of each range bin in secs - double sampling_interval; - - //speed of sound - //this takes the medium into account - float speed_of_sound; - - //horizontal beam width of the sonar beam in radians - Angle beamwidth_horizontal; - - //vertical beam width of the sonar beam in radians - Angle beamwidth_vertical; - - //if set to true one sonar beam is stored per column - //otherwise per row - bool memory_layout_column; - - //if set to true the bins are interpreted in polar coordinates - //otherwise in Cartesian coordinates - // - //Some imaging sonars store their data in Cartesian rather than - //polar coordinates (BlueView) - bool polar_coordinates; - }; -}} - -#endif - diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 9acab668..ab3ca28e 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -17,6 +17,20 @@ rock_library( TransformWithCovariance.cpp TwistWithCovariance.cpp Waypoint.cpp + commands/Motion2D.cpp + samples/BodyState.cpp + samples/CompressedFrame.cpp + samples/DepthMap.cpp + samples/DistanceImage.cpp + samples/Frame.cpp + samples/Joints.cpp + samples/LaserScan.cpp + samples/Pressure.cpp + samples/RigidBodyAcceleration.cpp + samples/RigidBodyState.cpp + samples/Sonar.cpp + samples/SonarBeam.cpp + samples/SonarScan.cpp HEADERS Angle.hpp CircularBuffer.hpp @@ -30,7 +44,6 @@ rock_library( JointTransform.hpp Logging.hpp NamedVector.hpp - Odometry.hpp Point.hpp Pose.hpp Pressure.hpp @@ -45,8 +58,30 @@ rock_library( TwistWithCovariance.hpp Waypoint.hpp Wrench.hpp + commands/Joints.hpp + commands/Motion2D.hpp + commands/Speed6D.hpp logging/logging_iostream_style.h logging/logging_printf_style.h + samples/BodyState.hpp + samples/CommandSamples.hpp + samples/CompressedFrame.hpp + samples/DepthMap.hpp + samples/DistanceImage.hpp + samples/Frame.hpp + samples/IMUSensors.hpp + samples/Joints.hpp + samples/LaserScan.hpp + samples/Pointcloud.hpp + samples/Pressure.hpp + samples/RigidBodyAcceleration.hpp + samples/RigidBodyState.hpp + samples/Sonar.hpp + samples/SonarBeam.hpp + samples/SonarScan.hpp + samples/Wrench.hpp + samples/Wrenches.hpp + templates/TimeStamped.hpp DEPS_CMAKE SISL DEPS_PKGCONFIG diff --git a/src/Odometry.hpp b/src/Odometry.hpp deleted file mode 100644 index 9f879c16..00000000 --- a/src/Odometry.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef __BASE_ODOMETRY_HPP__ -#define __BASE_ODOMETRY_HPP__ - -#warning "the headers base/odometry.h and base/Odometry.hpp are deprecated. Include the corresponding headers from the slam/odometry package (e.g. odometry/State.hpp for the odometry::State<> template)" -#include -#include -#include -#include -#include - -namespace base -{ - namespace odometry = ::odometry; -} - -#endif diff --git a/base/commands/Joints.hpp b/src/commands/Joints.hpp similarity index 100% rename from base/commands/Joints.hpp rename to src/commands/Joints.hpp diff --git a/src/commands/Motion2D.cpp b/src/commands/Motion2D.cpp new file mode 100644 index 00000000..ebf2d7f0 --- /dev/null +++ b/src/commands/Motion2D.cpp @@ -0,0 +1,12 @@ +#include "Motion2D.hpp" + +bool base::commands::operator==(const base::commands::Motion2D& lhs, const base::commands::Motion2D& rhs) +{ + return lhs.translation == rhs.translation && lhs.rotation == rhs.rotation && lhs.heading == rhs.heading; +} + +bool base::commands::operator!=(const base::commands::Motion2D& lhs, const base::commands::Motion2D& rhs) +{ + return !(lhs == rhs); +} + diff --git a/base/commands/Motion2D.hpp b/src/commands/Motion2D.hpp similarity index 76% rename from base/commands/Motion2D.hpp rename to src/commands/Motion2D.hpp index 0bf904a5..5ce32b6f 100644 --- a/base/commands/Motion2D.hpp +++ b/src/commands/Motion2D.hpp @@ -21,8 +21,8 @@ namespace base Motion2D(double translation, double rotation):translation(translation), rotation(rotation), heading(base::Angle::fromRad(0)){}; Motion2D(double translation, double rotation, base::Angle heading):translation(translation), rotation(rotation), heading(heading){}; }; - inline bool operator==(const Motion2D& lhs, const Motion2D& rhs){ return lhs.translation == rhs.translation && lhs.rotation == rhs.rotation && lhs.heading == rhs.heading;} - inline bool operator!=(const Motion2D& lhs, const Motion2D& rhs){ return !(lhs == rhs); } + bool operator==(const Motion2D& lhs, const Motion2D& rhs); + bool operator!=(const Motion2D& lhs, const Motion2D& rhs); } } diff --git a/base/commands/Speed6D.hpp b/src/commands/Speed6D.hpp similarity index 100% rename from base/commands/Speed6D.hpp rename to src/commands/Speed6D.hpp diff --git a/src/samples/BodyState.cpp b/src/samples/BodyState.cpp new file mode 100644 index 00000000..7ba8e728 --- /dev/null +++ b/src/samples/BodyState.cpp @@ -0,0 +1,325 @@ +#include "BodyState.hpp" + +#include +#include + +base::samples::BodyState::BodyState(bool doInvalidation) +{ + if(doInvalidation) + invalidate(); +} + +void base::samples::BodyState::setPose(const base::Affine3d& pose) +{ + this->pose.setTransform(pose); +} + +const base::Affine3d base::samples::BodyState::getPose() const +{ + return this->pose.getTransform(); +} + +double base::samples::BodyState::getYaw() const +{ + return base::getYaw(this->pose.orientation); +} + +double base::samples::BodyState::getPitch() const +{ + return base::getPitch(this->pose.orientation); +} + +double base::samples::BodyState::getRoll() const +{ + return base::getRoll(this->pose.orientation); +} + +const base::Position& base::samples::BodyState::position() const +{ + return this->pose.translation; +} + +base::Position& base::samples::BodyState::position() +{ + return this->pose.translation; +} + +const base::Quaterniond& base::samples::BodyState::orientation() const +{ + return this->pose.orientation; +} + +base::Quaterniond& base::samples::BodyState::orientation() +{ + return this->pose.orientation; +} + +const base::Vector3d& base::samples::BodyState::linear_velocity() const +{ + return this->velocity.vel; +} + +const base::Vector3d& base::samples::BodyState::angular_velocity() const +{ + return this->velocity.rot; +} + +base::Position& base::samples::BodyState::linear_velocity() +{ + return this->velocity.vel; +} + +base::Position& base::samples::BodyState::angular_velocity() +{ + return this->velocity.rot; +} + +const base::Matrix6d& base::samples::BodyState::cov_pose() const +{ + return this->pose.cov; +} + +base::Matrix6d& base::samples::BodyState::cov_pose() +{ + return this->pose.cov; +} + +const base::Matrix3d base::samples::BodyState::cov_orientation() const +{ + return this->pose.getOrientationCov(); +} + +void base::samples::BodyState::cov_orientation(const base::Matrix3d& cov) +{ + return this->pose.setOrientationCov(cov); +} + +const base::Matrix3d base::samples::BodyState::cov_position() const +{ + return this->pose.getTranslationCov(); +} + +void base::samples::BodyState::cov_position(const base::Matrix3d& cov) +{ + return this->pose.setTranslationCov(cov); +} + +const base::Matrix6d& base::samples::BodyState::cov_velocity() const +{ + return this->velocity.cov; +} + +base::Matrix6d& base::samples::BodyState::cov_velocity() +{ + return this->velocity.cov; +} + +const base::Matrix3d base::samples::BodyState::cov_linear_velocity() const +{ + return this->velocity.getLinearVelocityCov(); +} + +void base::samples::BodyState::cov_linear_velocity(const base::Matrix3d& cov) +{ + return this->velocity.setLinearVelocityCov(cov); +} + +const base::Matrix3d base::samples::BodyState::cov_angular_velocity() const +{ + return this->velocity.getAngularVelocityCov(); +} + +void base::samples::BodyState::cov_angular_velocity(const base::Matrix3d& cov) +{ + return this->velocity.setAngularVelocityCov(cov); +} + +base::samples::BodyState base::samples::BodyState::Unknown() +{ + BodyState result(false); + result.initUnknown(); + return result; +} + +base::samples::BodyState base::samples::BodyState::Invalid() +{ + BodyState result(true); + return result; +} + +void base::samples::BodyState::initSane() +{ + invalidate(); +} + +void base::samples::BodyState::invalidate() +{ + invalidatePose(); + invalidatePoseCovariance(); + invalidateVelocity(); + invalidateVelocityCovariance(); +} + +void base::samples::BodyState::initUnknown() +{ + this->pose.setTransform(base::Affine3d::Identity()); + this->pose.invalidateCovariance(); + this->velocity.setVelocity(base::Vector6d::Zero()); + this->velocity.invalidateCovariance(); +} + +bool base::samples::BodyState::hasValidPose() const +{ + return this->pose.hasValidTransform(); +} + +bool base::samples::BodyState::hasValidPoseCovariance() const +{ + return this->pose.hasValidCovariance(); +} + +void base::samples::BodyState::invalidatePose() +{ + this->pose.invalidateTransform(); +} + +void base::samples::BodyState::invalidatePoseCovariance() +{ + this->pose.invalidateCovariance(); +} + +bool base::samples::BodyState::hasValidVelocity() const +{ + return this->velocity.hasValidVelocity(); +} + +bool base::samples::BodyState::hasValidVelocityCovariance() const +{ + return this->velocity.hasValidCovariance(); +} + +void base::samples::BodyState::invalidateVelocity() +{ + this->velocity.invalidateVelocity(); +} + +void base::samples::BodyState::invalidateVelocityCovariance() +{ + this->velocity.invalidateCovariance(); +} + +void base::samples::BodyState::invalidateValues(bool pose, bool velocity) +{ + if (pose) this->invalidatePose(); + if (velocity) this->invalidateVelocity(); +} + +void base::samples::BodyState::invalidateCovariances(bool pose, bool velocity) +{ + if (pose) this->invalidatePoseCovariance(); + if (velocity) this->invalidateVelocityCovariance(); +} + +base::samples::BodyState& base::samples::BodyState::operator=(const base::samples::RigidBodyState& rbs) +{ + /** extract the transformation **/ + this->pose.setTransform(rbs.getTransform()); + + /** and the transformation covariance **/ + this->pose.cov << rbs.cov_orientation, Eigen::Matrix3d::Zero(), + Eigen::Matrix3d::Zero(), rbs.cov_position; + + /** Extract the velocity **/ + this->velocity.vel = rbs.velocity; + this->velocity.rot = rbs.angular_velocity; + + /** Extract the velocity covariance **/ + this->velocity.cov << rbs.cov_angular_velocity, Eigen::Matrix3d::Zero(), + Eigen::Matrix3d::Zero(), rbs.cov_velocity; + + return *this; +} + +base::samples::BodyState base::samples::BodyState::composition(const base::samples::BodyState& bs) const +{ + return this->operator*( bs ); +} + +base::samples::BodyState base::samples::BodyState::operator*(const base::samples::BodyState& bs) const +{ + const BodyState &bs2(*this); + const BodyState &bs1(bs); + + /** The composition of two body states is here defined as the + * composition of the pose transformation as it is defined in the + * TransformWithCovariance. The velocity held by the last body + * state (here bs1) is assumed to be the current "instantaneous" + * body state velocity and of the resulting body state. + **/ + base::TransformWithCovariance result_pose (static_cast(bs2.pose * bs1.pose)); + base::TwistWithCovariance result_velocity (bs1.velocity); + + /** Resulting velocity and covariance with respect to the pose base frame **/ + result_velocity.vel = result_pose.orientation * result_velocity.vel; + result_velocity.rot = result_pose.orientation * result_velocity.rot; + if (result_velocity.hasValidCovariance()) + { + /** Uncertainty propagation through assuming linear transformation. From R. Astudillo and R.Kolossa Chapter 3 Uncertainty Propagation **/ + /** Improvement at this point(TO-DO): change to Jacobian derivation since the propagation is not linear **/ + Eigen::Matrix3d rot_matrix(result_pose.orientation.toRotationMatrix()); + result_velocity.cov.block<3,3>(0,0) = (rot_matrix * result_velocity.cov.block<3,3>(0,0) * rot_matrix.transpose()); + result_velocity.cov.block<3,3>(3,3) = (rot_matrix * result_velocity.cov.block<3,3>(3,3) * rot_matrix.transpose()); + result_velocity.cov.block<3,3>(3,0) = (rot_matrix * result_velocity.cov.block<3,3>(3,0) * rot_matrix.transpose()); + result_velocity.cov.block<3,3>(0,3) = (rot_matrix * result_velocity.cov.block<3,3>(0,3) * rot_matrix.transpose()); + } + + /* Result Body State **/ + return BodyState(result_pose, result_velocity); +} + +std::ostream& base::samples::operator<<(std::ostream& out, const base::samples::BodyState& bs) +{ + out << bs.pose << "\n"; + out << bs.velocity << "\n"; + return out; +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/samples/BodyState.hpp b/src/samples/BodyState.hpp new file mode 100644 index 00000000..1f36b52d --- /dev/null +++ b/src/samples/BodyState.hpp @@ -0,0 +1,167 @@ +#ifndef __BASE_SAMPLES_BODY_STATE_HH +#define __BASE_SAMPLES_BODY_STATE_HH + +#include +#include +#include +#include +#include /** For backward compatibility with RBS **/ + +namespace base { namespace samples { + + /** Representation of the state of a rigid body + * + * This is among other things used to express frame transformations by + * Rock's transformer + * + * Given a source and target frame, this structure expresses the _frame + * change_ between these two frames. In effect, it represents the state of + * the source frame expressed in the target frame. + * + * Per [Rock's conventions](http://rock.opendfki.de/wiki/WikiStart/Standards), you + * should use a X-forward, right handed coordinate system when assigning + * frames to bodies (i.e. X=forward, Y=left, Z=up). In addition, + * world-fixed frames should be aligned to North (North-East-Up) + * + * For instance, if sourceFrame is "body" and targetFrame is "world", then + * the BodyState object is the state of body in the world frame + * (usually, the world frame has an arbitrary origin and a North-East-Up + * orientation). + */ + struct BodyState + { + + BodyState(bool doInvalidation=true); + + BodyState(const base::TransformWithCovariance& pose, const base::TwistWithCovariance& velocity): + pose(pose), velocity(velocity) {}; + + /** Time-stamp **/ + base::Time time; + + /** Robot pose: rotation in radians and translation in meters */ + base::TransformWithCovariance pose; + + /** TwistWithCovariance: Linear[m/s] and Angular[rad/s] Velocity of the Body */ + /** It is assumed here that velocity is the derivative of a delta pose for + * a given interval. When such interval tends to zero, one could talk + * of instantaneous velocity **/ + base::TwistWithCovariance velocity; + + void setPose(const base::Affine3d& pose); + + const base::Affine3d getPose() const; + + double getYaw() const; + + double getPitch() const; + + double getRoll() const; + + const base::Position& position() const; + + base::Position& position(); + + /** A read-only expression of the rotation **/ + const base::Quaterniond& orientation() const; + + base::Quaterniond& orientation(); + + const base::Vector3d& linear_velocity() const; + + base::Position& linear_velocity(); + + const base::Vector3d& angular_velocity() const; + + base::Position& angular_velocity(); + + + /** A read-only expression of the pose covariance **/ + const base::Matrix6d& cov_pose() const; + + base::Matrix6d& cov_pose(); + + /** A read-only expression of the rotation covariance **/ + const base::Matrix3d cov_orientation() const; + + void cov_orientation(const base::Matrix3d& cov); + + /** A read-only expression of the position covariance **/ + const base::Matrix3d cov_position() const; + + void cov_position(const base::Matrix3d& cov); + + /** A read-only expression of the velocity covariance **/ + const base::Matrix6d& cov_velocity() const; + + base::Matrix6d& cov_velocity(); + + /** A read-only expression of the linear velocity covariance **/ + const base::Matrix3d cov_linear_velocity() const; + + void cov_linear_velocity(const base::Matrix3d& cov); + + /** A read-only expression of the angular velocity covariance **/ + const base::Matrix3d cov_angular_velocity() const; + + void cov_angular_velocity(const base::Matrix3d& cov); + + static BodyState Unknown(); + + static BodyState Invalid(); + + /** For backward compatibility only. Use invalidate() */ + void initSane(); + + /** Initializes the rigid body state with NaN for the + * position, velocity, orientation and angular velocity. + */ + void invalidate(); + + /** + * Initializes the rigid body state unknown with Zero for the + * position, velocity and angular velocity, Identity for the orientation + * and infinity for all covariances. + */ + void initUnknown(); + + bool hasValidPose() const; + bool hasValidPoseCovariance() const; + void invalidatePose(); + void invalidatePoseCovariance(); + + bool hasValidVelocity() const; + bool hasValidVelocityCovariance() const; + void invalidateVelocity(); + void invalidateVelocityCovariance(); + + void invalidateValues ( bool pose = true, bool velocity = true); + + void invalidateCovariances ( bool pose = true, bool velocity = true); + + /** For backward compatibility with RBS **/ + BodyState& operator=( const base::samples::RigidBodyState& rbs ); + + + /** Default std::cout function + */ + friend std::ostream & operator<<(std::ostream &out, const TransformWithCovariance& trans); + + /** performs a composition of this Body State with the Body State given. + * The result is another Body State with result = this * trans + */ + BodyState composition( const BodyState& bs ) const; + + /** alias for the composition of two body states + */ + BodyState operator*( const BodyState& bs ) const; + }; + + /** Default std::cout function + */ + std::ostream & operator<<(std::ostream &out, const base::samples::BodyState& bs); + + +}}//end namespace base::samples +#endif + diff --git a/base/samples/CommandSamples.hpp b/src/samples/CommandSamples.hpp similarity index 100% rename from base/samples/CommandSamples.hpp rename to src/samples/CommandSamples.hpp diff --git a/src/samples/CompressedFrame.cpp b/src/samples/CompressedFrame.cpp new file mode 100644 index 00000000..15d48594 --- /dev/null +++ b/src/samples/CompressedFrame.cpp @@ -0,0 +1,9 @@ +#include "CompressedFrame.hpp" + +base::samples::frame::frame_compressed_mode_t base::samples::frame::CompressedFrame::toFrameMode(const std::__cxx11::string& str) +{ + if(str == "MODE_COMPRESSED_UNDEFINED") + return frame_compressed_mode_t::MODE_COMPRESSED_UNDEFINED; + else if (str == "MODE_PJPG") + return frame_compressed_mode_t::MODE_PJPG; +} diff --git a/base/samples/CompressedFrame.hpp b/src/samples/CompressedFrame.hpp similarity index 75% rename from base/samples/CompressedFrame.hpp rename to src/samples/CompressedFrame.hpp index 4ff830c5..719a299d 100644 --- a/base/samples/CompressedFrame.hpp +++ b/src/samples/CompressedFrame.hpp @@ -4,7 +4,7 @@ #include namespace base { namespace samples { namespace frame { - enum frame_compressed_mode_t { + enum class frame_compressed_mode_t { MODE_COMPRESSED_UNDEFINED = 0, MODE_PJPG = 1 }; @@ -27,18 +27,12 @@ namespace base { namespace samples { namespace frame { /** The image size in pixels */ frame_size_t size; - frame_compressed_mode_t frame_mode; + frame_compressed_mode_t frame_mode; /** Status flag */ frame_status_t frame_status; - static frame_compressed_mode_t toFrameMode(const std::string &str) - { - if(str == "MODE_COMPRESSED_UNDEFINED") - return MODE_COMPRESSED_UNDEFINED; - else if (str == "MODE_PJPG") - return MODE_PJPG; - } + static frame_compressed_mode_t toFrameMode(const std::string &str); }; diff --git a/src/samples/DepthMap.cpp b/src/samples/DepthMap.cpp new file mode 100644 index 00000000..4294ada6 --- /dev/null +++ b/src/samples/DepthMap.cpp @@ -0,0 +1,91 @@ +#include "DepthMap.hpp" + +#include + +void base::samples::DepthMap::reset() +{ + timestamps.clear(); + vertical_projection = POLAR; + horizontal_projection = POLAR; + vertical_interval.clear(); + horizontal_interval.clear(); + vertical_size = 0; + horizontal_size = 0; + distances.clear(); + remissions.clear(); +} + +base::samples::DepthMap::DepthMatrixMap base::samples::DepthMap::getDistanceMatrixMap() +{ + return (DepthMatrixMap(distances.data(), vertical_size, horizontal_size)); +} + +base::samples::DepthMap::DepthMatrixMapConst base::samples::DepthMap::getDistanceMatrixMapConst() const +{ + return (DepthMatrixMapConst(distances.data(), vertical_size, horizontal_size)); +} + +base::samples::DepthMap::DEPTH_MEASUREMENT_STATE base::samples::DepthMap::getIndexState(std::size_t index) const +{ + if(index >= distances.size()) + throw std::out_of_range("Invalid measurement index given"); + + return getMeasurementState(distances[index]); +} + +base::samples::DepthMap::DEPTH_MEASUREMENT_STATE base::samples::DepthMap::getMeasurementState(base::samples::DepthMap::uint32_t v_index, base::samples::DepthMap::uint32_t h_index) const +{ + if(!checkSizeConfig()) + throw std::out_of_range("Vertical and horizontal size does not match the distance array size."); + if(v_index >= vertical_size || h_index >= horizontal_size) + throw std::out_of_range("Invalid vertical or horizontal index given."); + + return getMeasurementState(distances[getIndex(v_index,h_index)]); +} + +base::samples::DepthMap::DEPTH_MEASUREMENT_STATE base::samples::DepthMap::getMeasurementState(base::samples::DepthMap::scalar distance) const +{ + if(base::isNaN(distance)) + return MEASUREMENT_ERROR; + else if(base::isInfinity(distance)) + return TOO_FAR; + else if(distance <= 0.0) + return TOO_NEAR; + else + return VALID_MEASUREMENT; +} + +bool base::samples::DepthMap::isIndexValid(std::size_t index) const +{ + if(index >= distances.size()) + throw std::out_of_range("Invalid measurement index given"); + + return isMeasurementValid(distances[index]); +} + +bool base::samples::DepthMap::isMeasurementValid(base::samples::DepthMap::uint32_t v_index, base::samples::DepthMap::uint32_t h_index) const +{ + if(!checkSizeConfig()) + throw std::out_of_range("Vertical and horizontal size does not match the distance array size."); + if(v_index >= vertical_size || h_index >= horizontal_size) + throw std::out_of_range("Invalid vertical or horizontal index given."); + + return isMeasurementValid(distances[getIndex(v_index,h_index)]); +} + +bool base::samples::DepthMap::isMeasurementValid(base::samples::DepthMap::scalar distance) const +{ + return getMeasurementState(distance) == VALID_MEASUREMENT; +} + +std::size_t base::samples::DepthMap::getIndex(base::samples::DepthMap::uint32_t v_index, base::samples::DepthMap::uint32_t h_index) const +{ + return ((size_t)v_index * (size_t)horizontal_size + (size_t)h_index); +} + +bool base::samples::DepthMap::checkSizeConfig() const +{ + return ((((size_t)vertical_size * (size_t)horizontal_size) == distances.size()) ? true : false); +} + + diff --git a/base/samples/DepthMap.hpp b/src/samples/DepthMap.hpp similarity index 91% rename from base/samples/DepthMap.hpp rename to src/samples/DepthMap.hpp index bfca4fe8..a22292c3 100644 --- a/base/samples/DepthMap.hpp +++ b/src/samples/DepthMap.hpp @@ -4,9 +4,8 @@ #include #include -#include -#include #include +#include #include #include @@ -129,82 +128,38 @@ struct DepthMap vertical_size(0), horizontal_size(0) {} /** Reset the sample */ - void reset() - { - timestamps.clear(); - vertical_projection = POLAR; - horizontal_projection = POLAR; - vertical_interval.clear(); - horizontal_interval.clear(); - vertical_size = 0; - horizontal_size = 0; - distances.clear(); - remissions.clear(); - } + void reset(); /** Creates a mapping to a eigen matrix with dynamic sizes */ - inline DepthMatrixMap getDistanceMatrixMap() - { return (DepthMatrixMap(distances.data(), vertical_size, horizontal_size)); } + DepthMatrixMap getDistanceMatrixMap(); /** Creates a mapping to a const eigen matrix with dynamic sizes */ - inline DepthMatrixMapConst getDistanceMatrixMapConst() const - { return (DepthMatrixMapConst(distances.data(), vertical_size, horizontal_size)); } + DepthMatrixMapConst getDistanceMatrixMapConst() const; /** Returns the measurement state of a given index. * * @param index of the measurement */ - DEPTH_MEASUREMENT_STATE getIndexState(size_t index) const - { - if(index >= distances.size()) - throw std::out_of_range("Invalid measurement index given"); - - return getMeasurementState(distances[index]); - } + DEPTH_MEASUREMENT_STATE getIndexState(size_t index) const; /** Returns the measurement state of a given vertical and horizontal index. * * @param v_index * @param h_index */ - DEPTH_MEASUREMENT_STATE getMeasurementState(uint32_t v_index, uint32_t h_index) const - { - if(!checkSizeConfig()) - throw std::out_of_range("Vertical and horizontal size does not match the distance array size."); - if(v_index >= vertical_size || h_index >= horizontal_size) - throw std::out_of_range("Invalid vertical or horizontal index given."); - - return getMeasurementState(distances[getIndex(v_index,h_index)]); - } + DEPTH_MEASUREMENT_STATE getMeasurementState(uint32_t v_index, uint32_t h_index) const; /** Returns the measurement state of a given measurement * * @param distance measurement */ - DEPTH_MEASUREMENT_STATE getMeasurementState(scalar distance) const - { - if(base::isNaN(distance)) - return MEASUREMENT_ERROR; - else if(base::isInfinity(distance)) - return TOO_FAR; - else if(distance <= 0.0) - return TOO_NEAR; - else - return VALID_MEASUREMENT; - - } + DEPTH_MEASUREMENT_STATE getMeasurementState(scalar distance) const; /** Returns true if the measurement at the given index is valid. * * @param index of the measurement */ - bool isIndexValid(size_t index) const - { - if(index >= distances.size()) - throw std::out_of_range("Invalid measurement index given"); - - return isMeasurementValid(distances[index]); - } + bool isIndexValid(size_t index) const; /** Returns true if the measurement at the given vertical and * horizontal index is valid. @@ -212,33 +167,19 @@ struct DepthMap * @param v_index * @param h_index */ - bool isMeasurementValid(uint32_t v_index, uint32_t h_index) const - { - if(!checkSizeConfig()) - throw std::out_of_range("Vertical and horizontal size does not match the distance array size."); - if(v_index >= vertical_size || h_index >= horizontal_size) - throw std::out_of_range("Invalid vertical or horizontal index given."); - - return isMeasurementValid(distances[getIndex(v_index,h_index)]); - } + bool isMeasurementValid(uint32_t v_index, uint32_t h_index) const; /** Returns true if the measurement is valid. * * @param distance measurement */ - bool isMeasurementValid(scalar distance) const - { - return getMeasurementState(distance) == VALID_MEASUREMENT; - } + bool isMeasurementValid(scalar distance) const; /** Computes the index in the distance and remission vector * of a given vertical and horizontal index. * Note that the data is stored in row major form. */ - inline size_t getIndex(uint32_t v_index, uint32_t h_index) const - { - return ((size_t)v_index * (size_t)horizontal_size + (size_t)h_index); - } + size_t getIndex(uint32_t v_index, uint32_t h_index) const; /** Converts the depth map to a pointcloud in the coordinate system of the sensor * (x-axis = forward, y-axis = to the left, z-axis = upwards). @@ -666,10 +607,8 @@ struct DepthMap }; /** Checks if the vertical and horizontal sizes match the size of distance vector. */ - inline bool checkSizeConfig() const - { - return ((((size_t)vertical_size * (size_t)horizontal_size) == distances.size()) ? true : false); - } + bool checkSizeConfig() const; + private: /** Lookup table for rotations around one unit axis */ diff --git a/src/samples/DistanceImage.cpp b/src/samples/DistanceImage.cpp new file mode 100644 index 00000000..e534adca --- /dev/null +++ b/src/samples/DistanceImage.cpp @@ -0,0 +1,38 @@ +#include "DistanceImage.hpp" + +#include +#include + +void base::samples::DistanceImage::clear() +{ + std::fill( data.begin(), data.end(), std::numeric_limits::quiet_NaN() ); +} + +base::samples::Pointcloud base::samples::DistanceImage::getPointCloud() const +{ + base::samples::Pointcloud pointCloud; + Eigen::Vector3d point; + for(size_t y = 0; y < height ; y++) + { + for(size_t x = 0; x < width ; x++) + { + pointCloud.points.push_back(point); + } + } + return pointCloud; +} + +void base::samples::DistanceImage::setIntrinsic(double f_x, double f_y, double c_x, double c_y) +{ + scale_x = 1.0 / f_x; + scale_y = 1.0 / f_y; + center_x = -c_x / f_x; + center_y = -c_y / f_y; +} + +void base::samples::DistanceImage::setSize(double width, double height) +{ + this->width = width; + this->height = height; + data.resize( width * height ); +} diff --git a/base/samples/DistanceImage.hpp b/src/samples/DistanceImage.hpp similarity index 85% rename from base/samples/DistanceImage.hpp rename to src/samples/DistanceImage.hpp index 16fec541..a1f31ac9 100644 --- a/base/samples/DistanceImage.hpp +++ b/src/samples/DistanceImage.hpp @@ -5,9 +5,7 @@ #include #include "Pointcloud.hpp" #include -#include #include -#include namespace base { @@ -67,10 +65,7 @@ namespace samples /** * resets all values in the distance image to nan */ - void clear() - { - std::fill( data.begin(), data.end(), std::numeric_limits::quiet_NaN() ); - } + void clear(); /** * Transforms a x, y pixel coordinates into a 3d scene point, using the distance @@ -122,19 +117,7 @@ namespace samples return true; } - base::samples::Pointcloud getPointCloud() const - { - base::samples::Pointcloud pointCloud; - Eigen::Vector3d point; - for(size_t y = 0; y < height ; y++) - { - for(size_t x = 0; x < width ; x++) - { - pointCloud.points.push_back(point); - } - } - return pointCloud; - } + base::samples::Pointcloud getPointCloud() const; /** * The intrinsic matrix has the following form @@ -177,13 +160,7 @@ namespace samples * @param c_x center point in x * @param c_y center point in y */ - void setIntrinsic( double f_x, double f_y, double c_x, double c_y ) - { - scale_x = 1.0 / f_x; - scale_y = 1.0 / f_y; - center_x = -c_x / f_x; - center_y = -c_y / f_y; - } + void setIntrinsic( double f_x, double f_y, double c_x, double c_y ); /** @brief set the size of the distance image * @@ -193,12 +170,7 @@ namespace samples * @param width width of the image * @param height height of the image */ - void setSize( double width, double height ) - { - this->width = width; - this->height = height; - data.resize( width * height ); - } + void setSize( double width, double height ); }; } } diff --git a/src/samples/Frame.cpp b/src/samples/Frame.cpp new file mode 100644 index 00000000..57a8aa65 --- /dev/null +++ b/src/samples/Frame.cpp @@ -0,0 +1,419 @@ +#include "Frame.hpp" + +#include +#include +#include +#include +#include + +void base::samples::frame::frame_attrib_t::set(const std::__cxx11::string& name, const std::string& data) +{ + name_ = name; + data_ = data; +} + +bool base::samples::frame::frame_size_t::operator==(const base::samples::frame::frame_size_t& other) const +{ + if(width == other.width && height==other.height) + return true; + return false; +} + +bool base::samples::frame::frame_size_t::operator!=(const base::samples::frame::frame_size_t& other) const +{ + return !(*this == other); +} + +base::samples::frame::Frame::Frame() : image(), size(), data_depth(0), pixel_size(0), frame_mode() +{ + setDataDepth(0); + reset(); +} + +base::samples::frame::Frame::Frame(uint16_t width, uint16_t height, uint8_t depth, base::samples::frame::frame_mode_t mode, const uint8_t val, size_t sizeInBytes) +{ + init(width,height,depth,mode,val,sizeInBytes); +} + +base::samples::frame::Frame::Frame(const base::samples::frame::Frame& other, bool bcopy) +{ + init(other,bcopy); +} + +void base::samples::frame::Frame::copyImageIndependantAttributes(const base::samples::frame::Frame& other) +{ + std::vector::const_iterator iter = other.attributes.begin(); + for(;iter!= other.attributes.end();++iter) + setAttribute(iter->name_,iter->data_); + time = other.time; + received_time = other.received_time; + frame_status = other.frame_status; +} + +void base::samples::frame::Frame::init(const base::samples::frame::Frame& other, bool bcopy) +{ + //hdr is copied by attributes = other.attributes; + init(other.getWidth(),other.getHeight(),other.getDataDepth(), other.getFrameMode(),-1,other.getNumberOfBytes()); + if(bcopy) + setImage(other.getImage()); + copyImageIndependantAttributes(other); +} + +void base::samples::frame::Frame::init(uint16_t width, uint16_t height, uint8_t depth, base::samples::frame::frame_mode_t mode, const uint8_t val, size_t sizeInBytes) +{ + //change size if the frame does not fit + if(this->size.height != height || this->size.width != width || this->frame_mode != mode || + this->data_depth != depth || (sizeInBytes != 0 && sizeInBytes != image.size())) + { + //check if depth = 0 + //this might be a programmer error + if(depth==0 && (height != 0 || width != 0 )) + throw std::runtime_error("Frame::init: Cannot initialize frame with depth = 0."); + + this->frame_mode = mode; + this->size = frame_size_t(width, height); + setDataDepth(depth); + } + //calculate size if not given + if(!sizeInBytes) + sizeInBytes = getPixelSize() * getPixelCount(); + + validateImageSize(sizeInBytes); + image.resize(sizeInBytes); + reset(val); +} + +void base::samples::frame::Frame::reset(const int val) +{ + this->time = base::Time(); + if (this->image.size() > 0 && val >= 0) { + memset(&this->image[0], val%256, this->image.size()); + } + setStatus(STATUS_EMPTY); + attributes.clear(); +} + +void base::samples::frame::Frame::swap(base::samples::frame::Frame& frame) +{ + //swap vector + image.swap(frame.image); + attributes.swap(frame.attributes); + + //copy values to temp + base::Time temp_time = frame.time; + base::Time temp_received_time = frame.received_time; + frame_size_t temp_size = frame.size; + uint32_t temp_data_depth = frame.data_depth; + uint32_t temp_pixel_size = frame.pixel_size; + uint32_t temp_row_size = frame.row_size; + frame_mode_t temp_frame_mode = frame.frame_mode; + frame_status_t temp_frame_status = frame.frame_status; + + //copy values + frame.time = time; + frame.received_time = received_time; + frame.size = size; + frame.data_depth = data_depth; + frame.pixel_size = pixel_size; + frame.row_size = row_size; + frame.frame_mode = frame_mode; + frame.frame_status = frame_status; + + time = temp_time; + received_time = temp_received_time; + size = temp_size; + data_depth = temp_data_depth; + pixel_size = temp_pixel_size; + row_size = temp_row_size; + frame_mode = temp_frame_mode; + frame_status = temp_frame_status; +} + +bool base::samples::frame::Frame::isHDR() const +{ + return (hasAttribute("hdr")&&getAttribute("hdr")); +} + +void base::samples::frame::Frame::setHDR(bool value) +{ + setAttribute("hdr",true); +} + +bool base::samples::frame::Frame::isCompressed() const +{ + return frame_mode >= COMPRESSED_MODES; +} + +bool base::samples::frame::Frame::isGrayscale() const +{ + return this->frame_mode == MODE_GRAYSCALE; +} + +bool base::samples::frame::Frame::isRGB() const +{ + return this->frame_mode == MODE_RGB; +} + +bool base::samples::frame::Frame::isBayer() const +{ + return (this->frame_mode == MODE_BAYER || this->frame_mode == MODE_BAYER_RGGB || this->frame_mode == MODE_BAYER_GRBG || this->frame_mode == MODE_BAYER_BGGR || this->frame_mode == MODE_BAYER_GBRG); +} + +void base::samples::frame::Frame::setStatus(const base::samples::frame::frame_status_t value) +{ + frame_status = value; +} + +base::samples::frame::frame_status_t base::samples::frame::Frame::getStatus() const +{ + return frame_status; +} + +uint32_t base::samples::frame::Frame::getChannelCount() const +{ + return getChannelCount(this->frame_mode); +} + +uint32_t base::samples::frame::Frame::getChannelCount(base::samples::frame::frame_mode_t mode) +{ + switch (mode) + { + case MODE_UNDEFINED: + return 0; + case MODE_BAYER: + case MODE_BAYER_RGGB: + case MODE_BAYER_BGGR: + case MODE_BAYER_GBRG: + case MODE_BAYER_GRBG: + case MODE_GRAYSCALE: + case MODE_UYVY: + return 1; + case MODE_RGB: + case MODE_BGR: + return 3; + case MODE_RGB32: + return 4; + case MODE_PJPG: + case MODE_JPEG: + case MODE_PNG: + return 1; + default: + throw std::runtime_error("Frame::getChannelCount: Unknown frame_mode"); + return 0; + } +} + +base::samples::frame::frame_mode_t base::samples::frame::Frame::toFrameMode(const std::__cxx11::string& str) +{ + if(str == "MODE_UNDEFINED") + return MODE_UNDEFINED; + else if (str == "MODE_GRAYSCALE") + return MODE_GRAYSCALE; + else if (str == "MODE_RGB") + return MODE_RGB; + else if (str == "MODE_BGR") + return MODE_BGR; + else if (str == "MODE_UYVY") + return MODE_UYVY; + else if (str == "RAW_MODES") + return RAW_MODES; + else if (str == "MODE_BAYER") + return MODE_BAYER; + else if (str == "MODE_BAYER_RGGB") + return MODE_BAYER_RGGB; + else if (str == "MODE_BAYER_GRBG") + return MODE_BAYER_GRBG; + else if (str == "MODE_BAYER_BGGR") + return MODE_BAYER_BGGR; + else if (str == "MODE_BAYER_GBRG") + return MODE_BAYER_GBRG; + else if (str == "MODE_RGB32") + return MODE_RGB32; + else if (str == "COMPRESSED_MODES") + return COMPRESSED_MODES; + else if (str == "MODE_PJPG") + return MODE_PJPG; + else if (str == "MODE_JPEG") + return MODE_JPEG; + else if (str == "MODE_PNG") + return MODE_PNG; + else + return MODE_UNDEFINED; +} + +base::samples::frame::frame_mode_t base::samples::frame::Frame::getFrameMode() const +{ + return this->frame_mode; +} + +uint32_t base::samples::frame::Frame::getPixelSize() const +{ + return this->pixel_size; +} + +uint32_t base::samples::frame::Frame::getRowSize() const +{ + if(isCompressed()) + throw std::runtime_error("Frame::getRowSize: There is no raw size for an compressed image!"); + return this->row_size; +} + +uint32_t base::samples::frame::Frame::getNumberOfBytes() const +{ + return image.size(); +} + +uint32_t base::samples::frame::Frame::getPixelCount() const +{ + return size.width * size.height; +} + +uint32_t base::samples::frame::Frame::getDataDepth() const +{ + return this->data_depth; +} + +void base::samples::frame::Frame::setDataDepth(uint32_t value) +{ + this->data_depth = value; + + // Update pixel size + uint32_t comp_size = ((this->data_depth + 7) / 8); + this->pixel_size = getChannelCount(this->frame_mode) * comp_size; + + //update row size + if(isCompressed()) + this->row_size = 0; //disable row size + else + this->row_size = this->pixel_size * getWidth(); +} + +void base::samples::frame::Frame::setFrameMode(base::samples::frame::frame_mode_t mode) +{ + this->frame_mode = mode; + + // Update pixel size + uint32_t comp_size = ((this->data_depth + 7) / 8); + this->pixel_size = getChannelCount(this->frame_mode) * comp_size; + + //update row size + if(isCompressed()) + this->row_size = 0; //disable row size + else + this->row_size = this->pixel_size * getWidth(); +} + +base::samples::frame::frame_size_t base::samples::frame::Frame::getSize() const +{ + return this->size; +} + +uint16_t base::samples::frame::Frame::getWidth() const +{ + return this->size.width; +} + +uint16_t base::samples::frame::Frame::getHeight() const +{ + return this->size.height; +} + +const std::vector< uint8_t >& base::samples::frame::Frame::getImage() const +{ + return this->image; +} + +void base::samples::frame::Frame::validateImageSize(size_t sizeToValidate) const +{ + size_t expected_size = getPixelSize()*getPixelCount(); + if (!isCompressed() && sizeToValidate != expected_size){ + std::cerr << "Frame: " + << __FUNCTION__ << " (" << __FILE__ << ", line " + << __LINE__ << "): " << "image size mismatch in setImage() (" + << "getting " << sizeToValidate << " bytes but I was expecting " << expected_size << " bytes)" + << std::endl; + throw std::runtime_error("Frame::validateImageSize: wrong image size!"); + } +} + +void base::samples::frame::Frame::setImage(const std::vector< uint8_t >& newImage) +{ + // calling the overloading function wich uses the "char*" interface + return setImage(newImage.data(), newImage.size()); +} + +void base::samples::frame::Frame::setImage(const char* data, size_t newImageSize) +{ + return setImage(reinterpret_cast(data), newImageSize); +} + +void base::samples::frame::Frame::setImage(const uint8_t* data, size_t newImageSize) +{ + validateImageSize(newImageSize); + image.resize(newImageSize); + memcpy(&this->image[0], data, newImageSize); +} + +uint8_t* base::samples::frame::Frame::getImagePtr() +{ + return static_cast(image.data()); +} + +const uint8_t* base::samples::frame::Frame::getImageConstPtr() const +{ + return static_cast(image.data()); +} + +uint8_t* base::samples::frame::Frame::getLastByte() +{ + return static_cast(&this->image.back()); +} + +const uint8_t* base::samples::frame::Frame::getLastConstByte() const +{ + return static_cast(&this->image.back()); +} + +bool base::samples::frame::Frame::hasAttribute(const std::__cxx11::string& name) const +{ + std::vector::const_iterator _iter = attributes.begin(); + for (;_iter != attributes.end();_iter++) + { + if (_iter->name_ == name) + return true; + } + return false; +} + +bool base::samples::frame::Frame::deleteAttribute(const std::__cxx11::string& name) +{ + std::vector::iterator _iter = attributes.begin(); + for (;_iter != attributes.end();_iter++) + { + if (_iter->name_ == name) + { + attributes.erase(_iter); + return true; + } + } + return false; +} + + + + + + + + + + + + + + + + + + + diff --git a/src/samples/Frame.hpp b/src/samples/Frame.hpp new file mode 100644 index 00000000..b49b0b1d --- /dev/null +++ b/src/samples/Frame.hpp @@ -0,0 +1,283 @@ +/*! \file frame.h + \brief container for imaging data +*/ + +#ifndef BASE_SAMPLES_FRAME_H__ +#define BASE_SAMPLES_FRAME_H__ + +#include +#include +#include + +#include + + +namespace base { namespace samples { namespace frame { + struct frame_attrib_t + { + std::string data_; + std::string name_; + void set(const std::string &name,const std::string &data); + }; + + struct frame_size_t { + frame_size_t() : width(0), height(0) {} + frame_size_t(uint16_t w, uint16_t h) : width(w), height(h) {} + + bool operator==(const frame_size_t &other) const; + + + bool operator!=(const frame_size_t &other) const; + + uint16_t width; + uint16_t height; + }; + + enum frame_mode_t { + MODE_UNDEFINED = 0, + MODE_GRAYSCALE = 1, + MODE_RGB = 2, + MODE_UYVY = 3, + MODE_BGR = 4, + MODE_RGB32 = 5, + RAW_MODES = 128, + MODE_BAYER = RAW_MODES + 0, + MODE_BAYER_RGGB = RAW_MODES + 1, + MODE_BAYER_GRBG = RAW_MODES + 2, + MODE_BAYER_BGGR = RAW_MODES + 3, + MODE_BAYER_GBRG = RAW_MODES + 4, + COMPRESSED_MODES = 256, //if an image is compressed it has no relationship + //between number of pixels and number of bytes + MODE_PJPG = COMPRESSED_MODES + 1, + MODE_JPEG = COMPRESSED_MODES + 2, + MODE_PNG = COMPRESSED_MODES + 3 + }; + + enum frame_status_t { + STATUS_EMPTY, + STATUS_VALID, + STATUS_INVALID + }; + + /* A single image frame */ + struct Frame + { + public: + /** + * Initialize the frame + * @param width the image width, in pixels + * @param height the image height, in pixels + * @param data_depth the number of effective bits per pixels + * @param mode the frame mode (raw, hdr, greyscale, colour) + */ + Frame(); + + //@depth number of bits per pixel and channel + Frame(uint16_t width, uint16_t height, uint8_t depth=8, frame_mode_t mode=MODE_GRAYSCALE, uint8_t const val = 0,size_t sizeInBytes=0); + + //makes a copy of other + Frame(const Frame &other,bool bcopy = true); + + //copies all attributes which are independent from size and mode + //if an attribute already exists the old value is over written + void copyImageIndependantAttributes(const Frame &other); + + //makes a copy of other + void init(const Frame &other,bool bcopy = true); + + void init(uint16_t width, uint16_t height, uint8_t depth=8, frame_mode_t mode=MODE_GRAYSCALE, const uint8_t val = 0, size_t sizeInBytes=0); + + // if val is negative the image will not be initialized + void reset(int const val = 0); + + void swap(Frame &frame); + + bool isHDR()const; + + void setHDR(bool value); + + bool isCompressed()const; + + bool isGrayscale()const; + + bool isRGB()const; + + bool isBayer()const; + + void setStatus(const frame_status_t value); + + frame_status_t getStatus()const; + + uint32_t getChannelCount() const; + + static uint32_t getChannelCount(frame_mode_t mode); + + //qt ruby does not support enums as slot parameters + //therefore frame_mode_t is passed as string + static frame_mode_t toFrameMode(const std::string &str); + + frame_mode_t getFrameMode() const; + + /** + * Returns the size of a pixel (in bytes). This takes into account the image + * mode as well as the data depth. + * @return Number of channels * bytes used to represent one colour + */ + uint32_t getPixelSize() const; + + /** + * Returns the size of a row (in bytes). This takes into account the image + * mode as well as the data depth. + * @return Number of channels * width * bytes used to represent one colour + * @return 0 if the image is compressed + */ + uint32_t getRowSize() const; + + /** + * Returns the total number of bytes for the image + */ + uint32_t getNumberOfBytes() const; + + /** + * Returns the total count of pixels in this frame + * @return Returns the overall number of pixels (width * height) + */ + uint32_t getPixelCount() const; + + uint32_t getDataDepth() const; + + void setDataDepth(uint32_t value); + + void setFrameMode(frame_mode_t mode); + + frame_size_t getSize() const; + + uint16_t getWidth() const; + + uint16_t getHeight() const; + + const std::vector &getImage() const; + + void validateImageSize(size_t sizeToValidate) const; + + void setImage(const std::vector &newImage); + + /** This is for backward compatibility for the people that were + * using the 'char' signature */ + void setImage(const char *data, size_t newImageSize); + + void setImage(const uint8_t *data, size_t newImageSize); + + uint8_t *getImagePtr(); + + const uint8_t *getImageConstPtr() const; + + uint8_t* getLastByte(); + + const uint8_t* getLastConstByte()const; + + bool hasAttribute(const std::string &name)const; + + template + inline T getAttribute(const std::string &name)const + { + static T default_value; + std::stringstream strstr; + + std::vector::const_iterator _iter = attributes.begin(); + for (;_iter != attributes.end();_iter++) + { + if (_iter->name_ == name) + { + T data; + strstr << _iter->data_; + strstr >> data; + return data; + } + } + return default_value; + } + + bool deleteAttribute(const std::string &name); + + template + inline void setAttribute(const std::string &name,const T &data) + { + //if attribute exists + std::stringstream strstr; + strstr << data; + std::vector::iterator _iter = attributes.begin(); + for (;_iter != attributes.end();_iter++) + { + if (_iter->name_ == name) + { + _iter->set(name,strstr.str()); + return; + } + } + //if attribute does not exist + attributes.push_back(frame_attrib_t()); + attributes.back().set(name,strstr.str()); + return ; + } + + template Tp& at(unsigned int column,unsigned int row) + { + if(column >= size.width || row >= size.height ) + throw std::runtime_error("out of index"); + return *((Tp*)(getImagePtr()+row*getRowSize()+column*getPixelSize())); + } + + /** The time at which this frame has been captured + * + * This is obviously an estimate + */ + base::Time time; + /** The time at which this frame has been received on the system */ + base::Time received_time; + + /** The raw data */ + std::vector image; + /** Additional metadata */ + std::vector attributes; + + /** The image size in pixels */ + frame_size_t size; + + /** The number of effective bits per channel. The number + * of actual bits per channel is always a multiple of + * height (i.e. a 12-bit effective depth is represented + * using 16-bits per channels). The number of greyscale + * levels is 2^(this_number) + */ + uint32_t data_depth; + /** The size of one pixel, in bytes + * + * For instance, for a RGB image with a 8 bit data depth, it would + * be 3. For a 12 bit non-packed image (i.e with each channel + * encoded on 2 bytes), it would be 6. + */ + uint32_t pixel_size; + + /** The size of a complete row in bytes + */ + uint32_t row_size; + + /** The colorspace of the image + */ + frame_mode_t frame_mode; + + /** Status flag */ + frame_status_t frame_status; + }; + + struct FramePair + { + base::Time time; + Frame first; + Frame second; + uint32_t id; + }; +}}} + +#endif diff --git a/base/samples/IMUSensors.hpp b/src/samples/IMUSensors.hpp similarity index 100% rename from base/samples/IMUSensors.hpp rename to src/samples/IMUSensors.hpp diff --git a/src/samples/Joints.cpp b/src/samples/Joints.cpp new file mode 100644 index 00000000..23cbf226 --- /dev/null +++ b/src/samples/Joints.cpp @@ -0,0 +1,94 @@ +#include "Joints.hpp" + +base::samples::Joints base::samples::Joints::Positions(const std::vector< double >& positions) +{ + Joints result; + result.elements.resize(positions.size()); + for (std::size_t i = 0; i != positions.size(); ++i) + result[i].position = positions[i]; + return result; +} + +base::samples::Joints base::samples::Joints::Positions(const std::vector< double >& positions, const std::vector< std::__cxx11::string >& names) +{ + Joints result = Positions(positions); + if (result.elements.size() != names.size()) + throw std::runtime_error("the position and names vectors differ"); + result.names = names; + return result; +} + +base::samples::Joints base::samples::Joints::Speeds(const std::vector< float >& speeds) +{ + Joints result; + result.elements.resize(speeds.size()); + for (std::size_t i = 0; i != speeds.size(); ++i) + result[i].speed= speeds[i]; + return result; +} + +base::samples::Joints base::samples::Joints::Speeds(const std::vector< float >& speeds, const std::vector< std::__cxx11::string >& names) +{ + Joints result = Speeds(speeds); + if (result.elements.size() != names.size()) + throw std::runtime_error("the speeds and names vectors differ"); + result.names = names; + return result; +} + +base::samples::Joints base::samples::Joints::Efforts(const std::vector< float >& efforts) +{ + Joints result; + result.elements.resize(efforts.size()); + for (std::size_t i = 0; i != efforts.size(); ++i) + result[i].effort = efforts[i]; + return result; +} + +base::samples::Joints base::samples::Joints::Efforts(const std::vector< float >& efforts, const std::vector< std::__cxx11::string >& names) +{ + Joints result = Efforts(efforts); + if (result.elements.size() != names.size()) + throw std::runtime_error("the effort and names vectors differ"); + result.names = names; + return result; +} + +base::samples::Joints base::samples::Joints::Raw(const std::vector< float >& raw) +{ + Joints result; + result.elements.resize(raw.size()); + for (std::size_t i = 0; i != raw.size(); ++i) + result[i].raw = raw[i]; + return result; +} + +base::samples::Joints base::samples::Joints::Raw(const std::vector< float >& raw, const std::vector< std::__cxx11::string >& names) +{ + Joints result = Raw(raw); + if (result.elements.size() != names.size()) + throw std::runtime_error("the raw and names vectors differ"); + result.names = names; + return result; +} + +base::samples::Joints base::samples::Joints::Accelerations(const std::vector< float >& acceleration) +{ + Joints result; + result.elements.resize(acceleration.size()); + for (std::size_t i = 0; i != acceleration.size(); ++i) + result[i].acceleration = acceleration[i]; + return result; +} + +base::samples::Joints base::samples::Joints::Accelerations(const std::vector< float >& acceleration, const std::vector< std::__cxx11::string >& names) +{ + Joints result = Accelerations(acceleration); + if (result.elements.size() != names.size()) + throw std::runtime_error("the acceleration and names vectors differ"); + result.names = names; + return result; +} + + + diff --git a/src/samples/Joints.hpp b/src/samples/Joints.hpp new file mode 100644 index 00000000..ec67f190 --- /dev/null +++ b/src/samples/Joints.hpp @@ -0,0 +1,47 @@ +#ifndef BASE_SAMPLES_JOINTS_HPP +#define BASE_SAMPLES_JOINTS_HPP + +#include +#include + +#include +#include +#include + +namespace base +{ + namespace samples + { + /** Data structure that gives out state readings for a set of joints + */ + struct Joints : public base::NamedVector + { + /** The sample timestamp */ + base::Time time; + + static Joints Positions(std::vector const& positions); + + static Joints Positions(std::vector const& positions, std::vector const& names); + + static Joints Speeds(std::vector const& speeds); + + + static Joints Speeds(std::vector const& speeds, std::vector const& names); + + static Joints Efforts(std::vector const& efforts); + + static Joints Efforts(std::vector const& efforts, std::vector const& names); + + static Joints Raw(std::vector const& raw); + + static Joints Raw(std::vector const& raw, std::vector const& names); + + static Joints Accelerations(std::vector const& acceleration); + + static Joints Accelerations(std::vector const& acceleration, std::vector const& names); + }; + } +} + +#endif + diff --git a/src/samples/LaserScan.cpp b/src/samples/LaserScan.cpp new file mode 100644 index 00000000..95b9082b --- /dev/null +++ b/src/samples/LaserScan.cpp @@ -0,0 +1,70 @@ +#include "LaserScan.hpp" + +#include +#include + +bool base::samples::LaserScan::isValidBeam(const unsigned int i) const +{ + if(i > ranges.size()) + throw std::out_of_range("Invalid beam index given"); + return isRangeValid(ranges[i]); +} + +void base::samples::LaserScan::reset() +{ + speed = 0.0; + start_angle = 0.0; + minRange = 0; + maxRange = 0; + ranges.clear(); + remission.clear(); +} + +bool base::samples::LaserScan::isRangeValid(base::samples::LaserScan::uint32_t range) const +{ + if(range >= minRange && range <= maxRange && range >= END_LASER_RANGE_ERRORS) + return true; + return false; +} + +bool base::samples::LaserScan::getPointFromScanBeamXForward(const unsigned int i, Eigen::Vector3d& point) const +{ + if(!isValidBeam(i)) + return false; + + //get a vector with the right length + point = Eigen::Vector3d(ranges[i] / 1000.0, 0.0, 0.0); + //rotate + point = Eigen::Quaterniond(Eigen::AngleAxisd(start_angle + i * angular_resolution, Eigen::Vector3d::UnitZ())) * point; + + return true; +} + +bool base::samples::LaserScan::getPointFromScanBeam(const unsigned int i, Eigen::Vector3d& point) const +{ + if(!isValidBeam(i)) + return false; + + //get a vector with the right length + point = Eigen::Vector3d(0.0 , ranges[i] / 1000.0, 0.0); + //rotate + point = Eigen::Quaterniond(Eigen::AngleAxisd(start_angle + i * angular_resolution, Eigen::Vector3d::UnitZ())) * point; + + return true; +} + +std::vector< Eigen::Vector3d > base::samples::LaserScan::convertScanToPointCloud(const Eigen::Affine3d& transform) const +{ + std::vector pointCloud; + pointCloud.reserve(ranges.size()); + + for(unsigned int i = 0; i < ranges.size(); i++) { + Eigen::Vector3d point; + if(getPointFromScanBeam(i, point)) { + point = transform * point; + pointCloud.push_back(point); + } + } + + return pointCloud; +} diff --git a/base/samples/LaserScan.hpp b/src/samples/LaserScan.hpp similarity index 71% rename from base/samples/LaserScan.hpp rename to src/samples/LaserScan.hpp index 05fc280e..d9ea01c8 100644 --- a/base/samples/LaserScan.hpp +++ b/src/samples/LaserScan.hpp @@ -1,12 +1,9 @@ #ifndef BASE_SAMPLES_LASER_H__ #define BASE_SAMPLES_LASER_H__ -#include -#include #include -#include - #include +#include #include #include @@ -64,29 +61,12 @@ namespace base { namespace samples { LaserScan() : start_angle(0), angular_resolution(0), speed(0), minRange(0), maxRange(0) {} - bool isValidBeam(const unsigned int i) const { - if(i > ranges.size()) - throw std::out_of_range("Invalid beam index given"); - return isRangeValid(ranges[i]); - } + bool isValidBeam(const unsigned int i) const; //resets the sample - void reset() - { - speed = 0.0; - start_angle = 0.0; - minRange = 0; - maxRange = 0; - ranges.clear(); - remission.clear(); - } - - inline bool isRangeValid(uint32_t range) const - { - if(range >= minRange && range <= maxRange && range >= END_LASER_RANGE_ERRORS) - return true; - return false; - } + void reset(); + + bool isRangeValid(uint32_t range) const; /** converts the laser scan into a point cloud according to the given transformation matrix, * the start_angle and the angular_resolution. If the transformation matrix is set to @@ -135,53 +115,17 @@ namespace base { namespace samples { * Helper function that converts range 'i' to a point. * The origin ot the point will be the laserScanner */ - bool getPointFromScanBeamXForward(const unsigned int i, Eigen::Vector3d &point) const - { - if(!isValidBeam(i)) - return false; - - //get a vector with the right length - point = Eigen::Vector3d(ranges[i] / 1000.0, 0.0, 0.0); - //rotate - point = Eigen::Quaterniond(Eigen::AngleAxisd(start_angle + i * angular_resolution, Eigen::Vector3d::UnitZ())) * point; - - return true; - } + bool getPointFromScanBeamXForward(const unsigned int i, Eigen::Vector3d &point) const; /** \deprecated - * returns the points in a wrong coordinate system */ - bool getPointFromScanBeam(const unsigned int i, Eigen::Vector3d &point) const - { - if(!isValidBeam(i)) - return false; - - //get a vector with the right length - point = Eigen::Vector3d(0.0 , ranges[i] / 1000.0, 0.0); - //rotate - point = Eigen::Quaterniond(Eigen::AngleAxisd(start_angle + i * angular_resolution, Eigen::Vector3d::UnitZ())) * point; - - return true; - } + bool getPointFromScanBeam(const unsigned int i, Eigen::Vector3d &point) const; /** \deprecated - * returns the points in a wrong coordinate system */ - std::vector convertScanToPointCloud(const Eigen::Affine3d& transform) const BASE_TYPES_DEPRECATED - { - std::vector pointCloud; - pointCloud.reserve(ranges.size()); - - for(unsigned int i = 0; i < ranges.size(); i++) { - Eigen::Vector3d point; - if(getPointFromScanBeam(i, point)) { - point = transform * point; - pointCloud.push_back(point); - } - } - - return pointCloud; - } + std::vector convertScanToPointCloud(const Eigen::Affine3d& transform) const BASE_TYPES_DEPRECATED; }; }} // namespaces diff --git a/base/samples/Pointcloud.hpp b/src/samples/Pointcloud.hpp similarity index 100% rename from base/samples/Pointcloud.hpp rename to src/samples/Pointcloud.hpp diff --git a/src/samples/Pressure.cpp b/src/samples/Pressure.cpp new file mode 100644 index 00000000..f01b5221 --- /dev/null +++ b/src/samples/Pressure.cpp @@ -0,0 +1,16 @@ +#include "Pressure.hpp" + +base::samples::Pressure base::samples::Pressure::fromPascal(const base::Time& time, float pascal) +{ + return Pressure(time, base::Pressure::fromPascal(pascal)); +} + +base::samples::Pressure base::samples::Pressure::fromBar(const base::Time& time, float bar) +{ + return Pressure(time, base::Pressure::fromBar(bar)); +} + +base::samples::Pressure base::samples::Pressure::fromPSI(const base::Time& time, float psi) +{ + return Pressure(time, base::Pressure::fromPSI(psi)); +} diff --git a/base/samples/Pressure.hpp b/src/samples/Pressure.hpp similarity index 72% rename from base/samples/Pressure.hpp rename to src/samples/Pressure.hpp index eb882b42..f0c1e817 100644 --- a/base/samples/Pressure.hpp +++ b/src/samples/Pressure.hpp @@ -22,20 +22,11 @@ namespace base : base::Pressure(pressure) , time(time) {} - static Pressure fromPascal(base::Time const& time, float pascal) - { - return Pressure(time, base::Pressure::fromPascal(pascal)); - } - - static Pressure fromBar(base::Time const& time, float bar) - { - return Pressure(time, base::Pressure::fromBar(bar)); - } - - static Pressure fromPSI(base::Time const& time, float psi) - { - return Pressure(time, base::Pressure::fromPSI(psi)); - } + static Pressure fromPascal(base::Time const& time, float pascal); + + static Pressure fromBar(base::Time const& time, float bar); + + static Pressure fromPSI(base::Time const& time, float psi); }; } } diff --git a/src/samples/RigidBodyAcceleration.cpp b/src/samples/RigidBodyAcceleration.cpp new file mode 100644 index 00000000..49de4f3f --- /dev/null +++ b/src/samples/RigidBodyAcceleration.cpp @@ -0,0 +1,7 @@ +#include "RigidBodyAcceleration.hpp" + +void base::samples::RigidBodyAcceleration::invalidateOrientation() +{ + cov_acceleration = Eigen::Matrix3d::Identity(); + cov_acceleration *= INFINITY; +} diff --git a/base/samples/RigidBodyAcceleration.hpp b/src/samples/RigidBodyAcceleration.hpp similarity index 79% rename from base/samples/RigidBodyAcceleration.hpp rename to src/samples/RigidBodyAcceleration.hpp index 23425196..f272ec8c 100644 --- a/base/samples/RigidBodyAcceleration.hpp +++ b/src/samples/RigidBodyAcceleration.hpp @@ -15,10 +15,7 @@ namespace base { namespace samples { */ base::Matrix3d cov_acceleration; - void invalidateOrientation() { - cov_acceleration = Eigen::Matrix3d::Identity(); - cov_acceleration *= INFINITY; - } + void invalidateOrientation(); }; }} diff --git a/src/samples/RigidBodyState.cpp b/src/samples/RigidBodyState.cpp new file mode 100644 index 00000000..59524a1e --- /dev/null +++ b/src/samples/RigidBodyState.cpp @@ -0,0 +1,306 @@ +#include "RigidBodyState.hpp" + +base::samples::RigidBodyState::RigidBodyState(bool doInvalidation) +{ + if(doInvalidation) + invalidate(); +} + +void base::samples::RigidBodyState::setTransform(const Eigen::Affine3d& transform) +{ + position = transform.translation(); + orientation = Eigen::Quaterniond( transform.linear() ); +} + +Eigen::Affine3d base::samples::RigidBodyState::getTransform() const +{ + Eigen::Affine3d ret; + ret.setIdentity(); + ret.rotate(this->orientation); + ret.translation() = this->position; + return ret; +} + +void base::samples::RigidBodyState::setPose(const base::Pose& pose) +{ + orientation = pose.orientation; + position = pose.position; +} + +base::Pose base::samples::RigidBodyState::getPose() const +{ + return base::Pose( position, orientation ); +} + +double base::samples::RigidBodyState::getYaw() const +{ + return base::getYaw(orientation); +} + +double base::samples::RigidBodyState::getPitch() const +{ + return base::getPitch(orientation); +} + +double base::samples::RigidBodyState::getRoll() const +{ + return base::getRoll(orientation); +} + +base::samples::RigidBodyState base::samples::RigidBodyState::unknown() +{ + RigidBodyState result(false); + result.initUnknown(); + return result; +} + +base::samples::RigidBodyState base::samples::RigidBodyState::invalid() +{ + RigidBodyState result(true); + return result; +} + +void base::samples::RigidBodyState::initSane() +{ + invalidate(); +} + +void base::samples::RigidBodyState::invalidate() +{ + invalidateOrientation(); + invalidateOrientationCovariance(); + invalidatePosition(); + invalidatePositionCovariance(); + invalidateVelocity(); + invalidateVelocityCovariance(); + invalidateAngularVelocity(); + invalidateAngularVelocityCovariance(); +} + +void base::samples::RigidBodyState::initUnknown() +{ + position.setZero(); + velocity.setZero(); + orientation = Eigen::Quaterniond::Identity(); + angular_velocity.setZero(); + cov_position = setValueUnknown(); + cov_orientation = setValueUnknown(); + cov_velocity = setValueUnknown(); + cov_angular_velocity = setValueUnknown(); +} + +bool base::samples::RigidBodyState::isValidValue(const base::Vector3d& vec) +{ + return !base::isNaN(vec(0)) && + !base::isNaN(vec(1)) && + !base::isNaN(vec(2)); +} + +bool base::samples::RigidBodyState::isValidValue(const base::Orientation& ori) +{ + return !base::isNaN(ori.w()) && + !base::isNaN(ori.x()) && + !base::isNaN(ori.y()) && + !base::isNaN(ori.z()) && + fabs(ori.squaredNorm()-1.0) < 1e-6; //assuming at least single precision +} + +bool base::samples::RigidBodyState::isKnownValue(const base::Matrix3d& cov) +{ + return !base::isInfinity(cov(0,0)) && + !base::isInfinity(cov(1,1)) && + !base::isInfinity(cov(2,2)); +} + +bool base::samples::RigidBodyState::isValidCovariance(const base::Matrix3d& cov) +{ + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + if (base::isNaN(cov(i, j))) + return false; + return true; +} + +bool base::samples::RigidBodyState::isValidValue(const base::Vector3d& vec, int dim) +{ + return !base::isNaN(vec(dim)); +} + +bool base::samples::RigidBodyState::isValidCovariance(const base::Matrix3d& cov, int dim) +{ + return !base::isNaN(cov(dim,dim)); +} + +bool base::samples::RigidBodyState::isKnownValue(const base::Matrix3d& cov, int dim) +{ + return !base::isInfinity(cov(dim,dim)); +} + +base::Vector3d base::samples::RigidBodyState::invalidValue() +{ + return base::Vector3d::Ones() * base::NaN(); +} + +base::Orientation base::samples::RigidBodyState::invalidOrientation() +{ + return base::Orientation(Eigen::Vector4d::Ones() * base::NaN()); +} + +base::Matrix3d base::samples::RigidBodyState::setValueUnknown() +{ + return base::Matrix3d::Ones() * base::infinity(); +} + +base::Matrix3d base::samples::RigidBodyState::invalidCovariance() +{ + return base::Matrix3d::Ones() * base::NaN(); +} + +bool base::samples::RigidBodyState::hasValidPosition() const +{ + return isValidValue(position); +} + +bool base::samples::RigidBodyState::hasValidPosition(int idx) const +{ + return isValidValue(position, idx); +} + +bool base::samples::RigidBodyState::hasValidPositionCovariance() const +{ + return isValidCovariance(cov_position); +} + +void base::samples::RigidBodyState::invalidatePosition() +{ + position = invalidValue(); +} + +void base::samples::RigidBodyState::invalidatePositionCovariance() +{ + cov_position = invalidCovariance(); +} + +bool base::samples::RigidBodyState::hasValidOrientation() const +{ + return isValidValue(orientation); +} + +bool base::samples::RigidBodyState::hasValidOrientationCovariance() const +{ + return isValidCovariance(cov_orientation); +} + +void base::samples::RigidBodyState::invalidateOrientation() +{ + orientation = invalidOrientation(); +} + +void base::samples::RigidBodyState::invalidateOrientationCovariance() +{ + cov_orientation = invalidCovariance(); +} + +bool base::samples::RigidBodyState::hasValidVelocity() const +{ + return isValidValue(velocity); +} + +bool base::samples::RigidBodyState::hasValidVelocity(int idx) const +{ + return isValidValue(velocity, idx); +} + +bool base::samples::RigidBodyState::hasValidVelocityCovariance() const +{ + return isValidCovariance(cov_velocity); +} + +void base::samples::RigidBodyState::invalidateVelocity() +{ + velocity = invalidValue(); +} + +void base::samples::RigidBodyState::invalidateVelocityCovariance() +{ + cov_velocity = invalidCovariance(); +} + +bool base::samples::RigidBodyState::hasValidAngularVelocity() const +{ + return isValidValue(angular_velocity); +} + +bool base::samples::RigidBodyState::hasValidAngularVelocity(int idx) const +{ + return isValidValue(angular_velocity, idx); +} + +bool base::samples::RigidBodyState::hasValidAngularVelocityCovariance() const +{ + return isValidCovariance(cov_angular_velocity); +} + +void base::samples::RigidBodyState::invalidateAngularVelocity() +{ + angular_velocity = invalidValue(); +} + +void base::samples::RigidBodyState::invalidateAngularVelocityCovariance() +{ + cov_angular_velocity = invalidCovariance(); +} + +void base::samples::RigidBodyState::invalidateValues(bool invPos, bool invOri, bool invVel, bool invAngVel) +{ + if (invPos) invalidatePosition(); + if (invOri) invalidateOrientation(); + if (invVel) invalidateVelocity(); + if (invAngVel) invalidateAngularVelocity(); +} + +void base::samples::RigidBodyState::invalidateCovariances(bool invPos, bool invOri, bool invVel, bool invAngVel) +{ + if (invPos) invalidatePositionCovariance(); + if (invOri) invalidateOrientationCovariance(); + if (invVel) invalidateVelocityCovariance(); + if (invAngVel) invalidateAngularVelocityCovariance(); +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/samples/RigidBodyState.hpp b/src/samples/RigidBodyState.hpp new file mode 100644 index 00000000..cef8edae --- /dev/null +++ b/src/samples/RigidBodyState.hpp @@ -0,0 +1,180 @@ +#ifndef __BASE_SAMPLES_RIGID_BODY_STATE_HH +#define __BASE_SAMPLES_RIGID_BODY_STATE_HH + +#include +#include +#include + +#include +#include + +namespace base { namespace samples { + /** Representation of the state of a rigid body + * + * This is among other things used to express frame transformations by + * Rock's transformer + * + * Given a source and target frame, this structure expresses the _frame + * change_ between these two frames. In effect, it represents the state of + * the source frame expressed in the target frame. + * + * Per [Rock's conventions](http://rock.opendfki.de/wiki/WikiStart/Standards), you + * should use a X-forward, right handed coordinate system when assigning + * frames to bodies (i.e. X=forward, Y=left, Z=up). In addition, + * world-fixed frames should be aligned to North (North-West-Up, aka NWU) + * + * For instance, if sourceFrame is "body" and targetFrame is "world", then + * the RigidBodyState object is the state of body in the world frame + * (usually, the world frame has an arbitrary origin and a North-West-Up + * orientation). + */ + struct RigidBodyState + { + + RigidBodyState(bool doInvalidation=true); + + base::Time time; + + /** Name of the source reference frame */ + std::string sourceFrame; + + /** Name of the target reference frame */ + std::string targetFrame; + + /** Position in m of sourceFrame's origin expressed in targetFrame + */ + Position position; + /** Covariance matrix of the position + */ + base::Matrix3d cov_position; + + /** Orientation of targetFrame expressed in sourceFrame */ + Orientation orientation; + /** Covariance matrix of the orientation as an axis/angle manifold in + * body coordinates + */ + base::Matrix3d cov_orientation; + + /** Velocity in m/s of sourceFrame expressed in targetFrame */ + base::Vector3d velocity; + /** Covariance of the velocity + */ + base::Matrix3d cov_velocity; + + /** Angular Velocity as an axis-angle representation in body fixed frame + * + * The direction of the vector is the axis, its length the speed */ + base::Vector3d angular_velocity; + /** Covariance of the angular velocity + */ + base::Matrix3d cov_angular_velocity; + + void setTransform(const Eigen::Affine3d& transform); + + Eigen::Affine3d getTransform() const; + + void setPose(const base::Pose& pose); + + base::Pose getPose() const; + + double getYaw() const; + + double getPitch() const; + + double getRoll() const; + + template + operator Eigen::Transform() const + { + Eigen::Transform ret; + ret.setIdentity(); + ret.rotate(this->orientation); + ret.translation() = this->position; + return ret; + } + + static RigidBodyState unknown(); + + static RigidBodyState invalid(); + + /** For backward compatibility only. Use invalidate() */ + void initSane(); + + /** Initializes the rigid body state with NaN for the + * position, velocity, orientation and angular velocity. + */ + void invalidate(); + + /** + * Initializes the rigid body state unknown with Zero for the + * position, velocity and angular velocity, Identity for the orientation + * and infinity for all covariances. + */ + void initUnknown(); + + /** Helper method that checks if a value is valid (not NaN anywhere). */ + static bool isValidValue(base::Vector3d const& vec); + + /** Helper method that checks if an orientation is valid (not NaN anywhere) + * and that the orientation is an unit quaternion. */ + static bool isValidValue(base::Orientation const& ori); + + /** Helper method that checks if the value whose covariance is + * represented by the given matrix is a known value (not Inf in the diagonal). + */ + static bool isKnownValue(base::Matrix3d const& cov); + + /** Helper method that checks if the covariance represented by the given + * matrix is valid + */ + static bool isValidCovariance(base::Matrix3d const& cov); + + /** Checks if a given dimension of a vector is valid. */ + static bool isValidValue(base::Vector3d const& vec, int dim); + + /** Checks if a given dimension of a covariance matrix is valid. */ + static bool isValidCovariance(base::Matrix3d const& cov, int dim); + + /** Checks if the dimension of a vector given by a covariance is known. */ + static bool isKnownValue(base::Matrix3d const& cov, int dim); + + static base::Vector3d invalidValue(); + + static base::Orientation invalidOrientation(); + + static base::Matrix3d setValueUnknown(); + + static base::Matrix3d invalidCovariance(); + + bool hasValidPosition() const; + bool hasValidPosition(int idx) const; + bool hasValidPositionCovariance() const; + void invalidatePosition(); + void invalidatePositionCovariance(); + + bool hasValidOrientation() const; + bool hasValidOrientationCovariance() const; + void invalidateOrientation(); + void invalidateOrientationCovariance(); + + bool hasValidVelocity() const; + bool hasValidVelocity(int idx) const; + bool hasValidVelocityCovariance() const; + void invalidateVelocity(); + void invalidateVelocityCovariance(); + + bool hasValidAngularVelocity() const; + bool hasValidAngularVelocity(int idx) const; + bool hasValidAngularVelocityCovariance() const; + void invalidateAngularVelocity(); + void invalidateAngularVelocityCovariance(); + + void invalidateValues(bool invPos, bool invOri, bool invVel = true, + bool invAngVel = true); + + void invalidateCovariances(bool invPos = true, bool invOri = true, + bool invVel = true, bool invAngVel = true); + }; +}} + +#endif diff --git a/src/samples/Sonar.cpp b/src/samples/Sonar.cpp new file mode 100644 index 00000000..20c6cb59 --- /dev/null +++ b/src/samples/Sonar.cpp @@ -0,0 +1,268 @@ +#include "Sonar.hpp" + +void base::samples::Sonar::resize(int bin_count, int beam_count, bool per_beam_timestamps) +{ + if (per_beam_timestamps) + timestamps.resize(beam_count); + else + timestamps.clear(); + + bearings.resize(beam_count, base::Angle::unknown()); + bins.resize(beam_count * bin_count, base::unknown()); + this->bin_count = bin_count; + this->beam_count = beam_count; +} + +base::samples::Sonar base::samples::Sonar::fromSingleBeam(base::Time time, base::Time bin_duration, base::Angle beam_width, base::Angle beam_height, const std::vector< float >& bins, base::Angle bearing, float speed_of_sound) +{ + Sonar sample(time, bin_duration, bins.size(), beam_width, beam_height); + sample.speed_of_sound = speed_of_sound; + sample.pushBeam(bins, bearing); + return sample; +} + +base::Time base::samples::Sonar::getBinRelativeStartTime(unsigned int bin_idx) const +{ + return bin_duration * bin_idx; +} + +base::Time base::samples::Sonar::getBeamAcquisitionStartTime(unsigned int beam) const +{ + if (timestamps.empty()) + return time; + else + return timestamps[beam]; +} + +base::Time base::samples::Sonar::getBinTime(unsigned int bin, unsigned int beam) const +{ + return getBeamAcquisitionStartTime(beam) + getBinRelativeStartTime(bin); +} + +float base::samples::Sonar::getBinStartDistance(unsigned int bin) const +{ + return getBinRelativeStartTime(bin).toSeconds() * speed_of_sound; +} + +void base::samples::Sonar::setRegularBeamBearings(base::Angle start, base::Angle interval) +{ + base::Angle angle(start); + bearings.resize(beam_count); + for (uint32_t i = 0; i < beam_count; ++i, angle += interval) + bearings[i] = angle; +} + +void base::samples::Sonar::pushBeam(const std::vector< float >& bins) +{ + if (!timestamps.empty()) + throw std::invalid_argument("cannot call pushBeam(bins): the structure uses per-beam timestamps, use pushBeams(time, bins) instead"); + + pushBeamBins(bins); +} + +void base::samples::Sonar::pushBeam(const std::vector< float >& bins, base::Angle bearing) +{ + pushBeam(bins); + bearings.push_back(bearing); +} + +void base::samples::Sonar::pushBeam(const base::Time& beam_time, const std::vector< float >& beam_bins) +{ + pushBeamBins(beam_bins); + timestamps.push_back(beam_time); +} + +void base::samples::Sonar::pushBeam(const base::Time& beam_time, const std::vector< float >& beam_bins, base::Angle bearing) +{ + pushBeam(beam_time, beam_bins); + bearings.push_back(bearing); +} + +void base::samples::Sonar::pushBeamBins(const std::vector< float >& beam_bins) +{ + if (beam_bins.size() != bin_count) + throw std::invalid_argument("pushBeam: the provided beam does not match the expected bin_count"); + bins.insert(bins.end(), beam_bins.begin(), beam_bins.end()); + beam_count++; +} + +void base::samples::Sonar::setBeam(unsigned int beam, const std::vector< float >& bins) +{ + if (!timestamps.empty()) + throw std::invalid_argument("cannot call setBeam(bins): the structure uses per-beam timestamps, use setBeams(time, bins) instead"); + + setBeamBins(beam, bins); +} + +void base::samples::Sonar::setBeam(unsigned int beam, const std::vector< float >& bins, base::Angle bearing) +{ + setBeam(beam, bins); + bearings[beam] = bearing; +} + +void base::samples::Sonar::setBeam(unsigned int beam, const base::Time& beam_time, const std::vector< float >& beam_bins) +{ + setBeamBins(beam, beam_bins); + timestamps[beam] = beam_time; +} + +void base::samples::Sonar::setBeam(unsigned int beam, const base::Time& beam_time, const std::vector< float >& beam_bins, base::Angle bearing) +{ + setBeam(beam, beam_time, beam_bins); + bearings[beam] = bearing; +} + +void base::samples::Sonar::setBeamBins(int beam, const std::vector< float >& beam_bins) +{ + if (beam_bins.size() != bin_count) + throw std::invalid_argument("pushBeam: the provided beam does not match the expected bin_count"); + std::copy(beam_bins.begin(), beam_bins.end(), bins.begin() + beam * bin_count); +} + +base::Angle base::samples::Sonar::getBeamBearing(unsigned int beam) const +{ + return bearings[beam]; +} + +std::vector< float > base::samples::Sonar::getBeamBins(unsigned int beam) const +{ + std::vector bins; + getBeamBins(beam, bins); + return bins; +} + +void base::samples::Sonar::getBeamBins(unsigned int beam, std::vector< float >& beam_bins) const +{ + beam_bins.resize(bin_count); + std::vector::const_iterator ptr = bins.begin() + beam * bin_count; + std::copy(ptr, ptr + bin_count, beam_bins.begin()); +} + +base::samples::Sonar base::samples::Sonar::getBeam(unsigned int beam) const +{ + return fromSingleBeam( + getBeamAcquisitionStartTime(beam), + bin_duration, + beam_width, + beam_height, + getBeamBins(beam), + getBeamBearing(beam), + speed_of_sound); +} + +void base::samples::Sonar::validate() +{ + if (bin_count * beam_count != bins.size()) + throw std::logic_error("the number of elements in 'bins' does not match the bin and beam counts"); + if (!timestamps.empty() && timestamps.size() != beam_count) + throw std::logic_error("the number of elements in 'timestamps' does not match the beam count"); + if (bearings.size() != beam_count) + throw std::logic_error("the number of elements in 'bearings' does not match the beam count"); +} + +base::samples::Sonar::Sonar(SonarScan const& old, float gain) + : time(old.time) + , timestamps(old.time_beams) + , bin_duration(base::Time::fromSeconds(old.getSpatialResolution() / old.speed_of_sound)) + , beam_width(old.beamwidth_horizontal) + , beam_height(old.beamwidth_vertical) + , speed_of_sound(old.speed_of_sound) + , bin_count(old.number_of_bins) + , beam_count(old.number_of_beams) +{ + if (!old.polar_coordinates) + throw std::invalid_argument("there's not such thing as a non-polar sonar device, fix your driver"); + + bins.resize(bin_count * beam_count); + + SonarScan scan(old); + if (old.memory_layout_column) + scan.toggleMemoryLayout(); + for (unsigned int i = 0; i < bins.size(); ++i) + bins[i] = static_cast(scan.data[i] * 1.0 / 255) * gain; + + setRegularBeamBearings(old.getStartBearing(), old.getAngularResolution()); + validate(); +} + + +base::samples::Sonar::Sonar(SonarBeam const& old, float gain) + : time(old.time) + , timestamps() + , bin_duration(base::Time::fromSeconds(old.sampling_interval / 2.0)) + , beam_width(base::Angle::fromRad(old.beamwidth_horizontal)) + , beam_height(base::Angle::fromRad(old.beamwidth_vertical)) + , speed_of_sound(old.speed_of_sound) + , bin_count(old.beam.size()) + , beam_count(0) +{ + std::vector bins; + bins.resize(bin_count); + for (unsigned int i = 0; i < bin_count; ++i) + bins[i] = static_cast(old.beam[i] * 1.0 / 255) * gain; + pushBeam(bins, old.bearing); +} + +base::samples::SonarBeam base::samples::Sonar::toSonarBeam(float gain) +{ + base::samples::SonarBeam sonar_beam; + sonar_beam.time = time; + sonar_beam.speed_of_sound = speed_of_sound; + sonar_beam.beamwidth_horizontal = beam_width.rad; + sonar_beam.beamwidth_vertical = beam_height.rad; + sonar_beam.bearing = bearings[0]; + sonar_beam.sampling_interval = bin_duration.toSeconds() * 2.0; + + // if any value of sonar data is higher than 1, normalize it + std::vector raw_data(bins.begin(), bins.end()); + std::vector::iterator max = std::max_element(raw_data.begin(), raw_data.end()); + if (*max > 1) + std::transform(raw_data.begin(), raw_data.end(), raw_data.begin(), std::bind2nd(std::divides(), *max)); + + std::transform(raw_data.begin(), raw_data.end(), raw_data.begin(), std::bind2nd(std::multiplies(), 255 * gain)); + + std::vector data(raw_data.begin(), raw_data.end()); + sonar_beam.beam = data; + return sonar_beam; +} + +base::samples::SonarScan base::samples::Sonar::toSonarScan(float gain) +{ + base::samples::SonarScan sonar_scan; + sonar_scan.time = time; + sonar_scan.time_beams = timestamps; + sonar_scan.speed_of_sound = speed_of_sound; + sonar_scan.number_of_bins = bin_count; + sonar_scan.number_of_beams = beam_count; + sonar_scan.beamwidth_horizontal = beam_width; + sonar_scan.beamwidth_vertical = beam_height; + sonar_scan.start_bearing = bearings[0]; + sonar_scan.angular_resolution = base::Angle::fromRad(beam_width.rad / beam_count); + sonar_scan.memory_layout_column = false; + sonar_scan.polar_coordinates = true; + + // if any value of sonar data is higher than 1, normalize it + std::vector raw_data(bins.begin(), bins.end()); + std::vector::iterator max = std::max_element(raw_data.begin(), raw_data.end()); + if (*max > 1) + std::transform(raw_data.begin(), raw_data.end(), raw_data.begin(), std::bind2nd(std::divides(), *max)); + + std::transform(raw_data.begin(), raw_data.end(), raw_data.begin(), std::bind2nd(std::multiplies(), 255 * gain)); + + std::vector data(raw_data.begin(), raw_data.end()); + sonar_scan.data = data; + return sonar_scan; +} + + + + + + + + + + + + + diff --git a/base/samples/Sonar.hpp b/src/samples/Sonar.hpp similarity index 52% rename from base/samples/Sonar.hpp rename to src/samples/Sonar.hpp index d8552e3f..7c5ff34c 100644 --- a/base/samples/Sonar.hpp +++ b/src/samples/Sonar.hpp @@ -162,62 +162,30 @@ struct Sonar resize(bin_count, beam_count, per_beam_timestamps); } - void resize(int bin_count, int beam_count, bool per_beam_timestamps) - { - if (per_beam_timestamps) - timestamps.resize(beam_count); - else - timestamps.clear(); - - bearings.resize(beam_count, base::Angle::unknown()); - bins.resize(beam_count * bin_count, base::unknown()); - this->bin_count = bin_count; - this->beam_count = beam_count; - } + void resize(int bin_count, int beam_count, bool per_beam_timestamps); /** Initializes a Sonar structure to represent a single beam */ static Sonar fromSingleBeam(base::Time time, base::Time bin_duration, base::Angle beam_width, base::Angle beam_height, std::vector const& bins, base::Angle bearing = base::Angle(), - float speed_of_sound = getSpeedOfSoundInWater()) - { - Sonar sample(time, bin_duration, bins.size(), beam_width, beam_height); - sample.speed_of_sound = speed_of_sound; - sample.pushBeam(bins, bearing); - return sample; - } + float speed_of_sound = getSpeedOfSoundInWater()); /** The start of a bin in the time domain, relative to the beam's * acquisition time */ - base::Time getBinRelativeStartTime(unsigned int bin_idx) const - { - return bin_duration * bin_idx; - } + base::Time getBinRelativeStartTime(unsigned int bin_idx) const; /** The acquisition start of a beam */ - base::Time getBeamAcquisitionStartTime(unsigned int beam) const - { - if (timestamps.empty()) - return time; - else - return timestamps[beam]; - } + base::Time getBeamAcquisitionStartTime(unsigned int beam) const; /** The start of a bin in the time domain, absolute */ - base::Time getBinTime(unsigned int bin, unsigned int beam) const - { - return getBeamAcquisitionStartTime(beam) + getBinRelativeStartTime(bin); - } + base::Time getBinTime(unsigned int bin, unsigned int beam) const; /** Returns the distance of the start of one bin relative to the sonar's * emission point */ - float getBinStartDistance(unsigned int bin) const - { - return getBinRelativeStartTime(bin).toSeconds() * speed_of_sound; - } + float getBinStartDistance(unsigned int bin) const; /** Sets the bearing field for a regular sampling * @@ -228,13 +196,7 @@ struct Sonar * @arg interval the interval between two beams * */ - void setRegularBeamBearings(base::Angle start, base::Angle interval) - { - base::Angle angle(start); - bearings.resize(beam_count); - for (uint32_t i = 0; i < beam_count; ++i, angle += interval) - bearings[i] = angle; - } + void setRegularBeamBearings(base::Angle start, base::Angle interval); /** Add data for one beam * @@ -242,37 +204,20 @@ struct Sonar * timestamps. In this case, use the overload that sets the beam time as * well */ - void pushBeam(std::vector const& bins) - { - if (!timestamps.empty()) - throw std::invalid_argument("cannot call pushBeam(bins): the structure uses per-beam timestamps, use pushBeams(time, bins) instead"); - - pushBeamBins(bins); - } + void pushBeam(std::vector const& bins); /** Add data for one beam */ - void pushBeam(std::vector const& bins, base::Angle bearing) - { - pushBeam(bins); - bearings.push_back(bearing); - } + void pushBeam(std::vector const& bins, base::Angle bearing); /** Add data for one beam */ - void pushBeam(base::Time const& beam_time, std::vector const& beam_bins) - { - pushBeamBins(beam_bins); - timestamps.push_back(beam_time); - } + void pushBeam(base::Time const& beam_time, std::vector const& beam_bins); + /** Add data for one beam */ - void pushBeam(base::Time const& beam_time, std::vector const& beam_bins, base::Angle bearing) - { - pushBeam(beam_time, beam_bins); - bearings.push_back(bearing); - } + void pushBeam(base::Time const& beam_time, std::vector const& beam_bins, base::Angle bearing); /** Adds a set of bins to the bin set, updating beam_bins * @@ -282,13 +227,7 @@ struct Sonar * @raise std::invalid_argument if the number of bins in the argument does * not match bin_count */ - void pushBeamBins(std::vector const& beam_bins) - { - if (beam_bins.size() != bin_count) - throw std::invalid_argument("pushBeam: the provided beam does not match the expected bin_count"); - bins.insert(bins.end(), beam_bins.begin(), beam_bins.end()); - beam_count++; - } + void pushBeamBins(std::vector const& beam_bins); /** Set data for one beam * @@ -296,37 +235,19 @@ struct Sonar * timestamps. In this case, use the overload that sets the beam time as * well */ - void setBeam(unsigned int beam, std::vector const& bins) - { - if (!timestamps.empty()) - throw std::invalid_argument("cannot call setBeam(bins): the structure uses per-beam timestamps, use setBeams(time, bins) instead"); - - setBeamBins(beam, bins); - } + void setBeam(unsigned int beam, std::vector const& bins); /** Add data for one beam */ - void setBeam(unsigned int beam, std::vector const& bins, base::Angle bearing) - { - setBeam(beam, bins); - bearings[beam] = bearing; - } + void setBeam(unsigned int beam, std::vector const& bins, base::Angle bearing); /** Add data for one beam */ - void setBeam(unsigned int beam, base::Time const& beam_time, std::vector const& beam_bins) - { - setBeamBins(beam, beam_bins); - timestamps[beam] = beam_time; - } + void setBeam(unsigned int beam, base::Time const& beam_time, std::vector const& beam_bins); /** Add data for one beam */ - void setBeam(unsigned int beam, base::Time const& beam_time, std::vector const& beam_bins, base::Angle bearing) - { - setBeam(beam, beam_time, beam_bins); - bearings[beam] = bearing; - } + void setBeam(unsigned int beam, base::Time const& beam_time, std::vector const& beam_bins, base::Angle bearing); /** Adds a set of bins to the bin set, updating beam_bins * @@ -336,155 +257,36 @@ struct Sonar * @raise std::invalid_argument if the number of bins in the argument does * not match bin_count */ - void setBeamBins(int beam, std::vector const& beam_bins) - { - if (beam_bins.size() != bin_count) - throw std::invalid_argument("pushBeam: the provided beam does not match the expected bin_count"); - std::copy(beam_bins.begin(), beam_bins.end(), bins.begin() + beam * bin_count); - } + void setBeamBins(int beam, std::vector const& beam_bins); /** Returns the bearing of a given beam * * This is the bearing of the center of the beam. A zero bearing means the * front of the device */ - base::Angle getBeamBearing(unsigned int beam) const - { - return bearings[beam]; - } + base::Angle getBeamBearing(unsigned int beam) const; /** Returns the bins of a given beam */ - std::vector getBeamBins(unsigned int beam) const - { - std::vector bins; - getBeamBins(beam, bins); - return bins; - } + std::vector getBeamBins(unsigned int beam) const; /** Copies the bins of a given beam */ - void getBeamBins(unsigned int beam, std::vector& beam_bins) const - { - beam_bins.resize(bin_count); - std::vector::const_iterator ptr = bins.begin() + beam * bin_count; - std::copy(ptr, ptr + bin_count, beam_bins.begin()); - } + void getBeamBins(unsigned int beam, std::vector& beam_bins) const; /** Returns the data structure that represents a single beam */ - Sonar getBeam(unsigned int beam) const - { - return fromSingleBeam( - getBeamAcquisitionStartTime(beam), - bin_duration, - beam_width, - beam_height, - getBeamBins(beam), - getBeamBearing(beam), - speed_of_sound); - } + Sonar getBeam(unsigned int beam) const; /** Verify this structure's consistency */ - void validate() - { - if (bin_count * beam_count != bins.size()) - throw std::logic_error("the number of elements in 'bins' does not match the bin and beam counts"); - if (!timestamps.empty() && timestamps.size() != beam_count) - throw std::logic_error("the number of elements in 'timestamps' does not match the beam count"); - if (bearings.size() != beam_count) - throw std::logic_error("the number of elements in 'bearings' does not match the beam count"); - } + void validate(); BASE_TYPES_DEPRECATED_SUPPRESS_START - explicit Sonar(SonarScan const& old, float gain = 1) - : time(old.time) - , timestamps(old.time_beams) - , bin_duration(base::Time::fromSeconds(old.getSpatialResolution() / old.speed_of_sound)) - , beam_width(old.beamwidth_horizontal) - , beam_height(old.beamwidth_vertical) - , speed_of_sound(old.speed_of_sound) - , bin_count(old.number_of_bins) - , beam_count(old.number_of_beams) - { - if (!old.polar_coordinates) - throw std::invalid_argument("there's not such thing as a non-polar sonar device, fix your driver"); - - bins.resize(bin_count * beam_count); - - SonarScan scan(old); - if (old.memory_layout_column) - scan.toggleMemoryLayout(); - for (unsigned int i = 0; i < bins.size(); ++i) - bins[i] = static_cast(scan.data[i] * 1.0 / 255) * gain; + explicit Sonar(SonarScan const& old, float gain = 1); - setRegularBeamBearings(old.getStartBearing(), old.getAngularResolution()); - validate(); - } + explicit Sonar(SonarBeam const& old, float gain = 1); - explicit Sonar(SonarBeam const& old, float gain = 1) - : time(old.time) - , timestamps() - , bin_duration(base::Time::fromSeconds(old.sampling_interval / 2.0)) - , beam_width(base::Angle::fromRad(old.beamwidth_horizontal)) - , beam_height(base::Angle::fromRad(old.beamwidth_vertical)) - , speed_of_sound(old.speed_of_sound) - , bin_count(old.beam.size()) - , beam_count(0) - { - std::vector bins; - bins.resize(bin_count); - for (unsigned int i = 0; i < bin_count; ++i) - bins[i] = static_cast(old.beam[i] * 1.0 / 255) * gain; - pushBeam(bins, old.bearing); - } + base::samples::SonarBeam toSonarBeam(float gain = 1); - base::samples::SonarBeam toSonarBeam(float gain = 1) { - base::samples::SonarBeam sonar_beam; - sonar_beam.time = time; - sonar_beam.speed_of_sound = speed_of_sound; - sonar_beam.beamwidth_horizontal = beam_width.rad; - sonar_beam.beamwidth_vertical = beam_height.rad; - sonar_beam.bearing = bearings[0]; - sonar_beam.sampling_interval = bin_duration.toSeconds() * 2.0; - - // if any value of sonar data is higher than 1, normalize it - std::vector raw_data(bins.begin(), bins.end()); - std::vector::iterator max = std::max_element(raw_data.begin(), raw_data.end()); - if (*max > 1) - std::transform(raw_data.begin(), raw_data.end(), raw_data.begin(), std::bind2nd(std::divides(), *max)); - - std::transform(raw_data.begin(), raw_data.end(), raw_data.begin(), std::bind2nd(std::multiplies(), 255 * gain)); - - std::vector data(raw_data.begin(), raw_data.end()); - sonar_beam.beam = data; - return sonar_beam; - } - - base::samples::SonarScan toSonarScan(float gain = 1) { - base::samples::SonarScan sonar_scan; - sonar_scan.time = time; - sonar_scan.time_beams = timestamps; - sonar_scan.speed_of_sound = speed_of_sound; - sonar_scan.number_of_bins = bin_count; - sonar_scan.number_of_beams = beam_count; - sonar_scan.beamwidth_horizontal = beam_width; - sonar_scan.beamwidth_vertical = beam_height; - sonar_scan.start_bearing = bearings[0]; - sonar_scan.angular_resolution = base::Angle::fromRad(beam_width.rad / beam_count); - sonar_scan.memory_layout_column = false; - sonar_scan.polar_coordinates = true; - - // if any value of sonar data is higher than 1, normalize it - std::vector raw_data(bins.begin(), bins.end()); - std::vector::iterator max = std::max_element(raw_data.begin(), raw_data.end()); - if (*max > 1) - std::transform(raw_data.begin(), raw_data.end(), raw_data.begin(), std::bind2nd(std::divides(), *max)); - - std::transform(raw_data.begin(), raw_data.end(), raw_data.begin(), std::bind2nd(std::multiplies(), 255 * gain)); - - std::vector data(raw_data.begin(), raw_data.end()); - sonar_scan.data = data; - return sonar_scan; - } + base::samples::SonarScan toSonarScan(float gain = 1); BASE_TYPES_DEPRECATED_SUPPRESS_STOP }; diff --git a/src/samples/SonarBeam.cpp b/src/samples/SonarBeam.cpp new file mode 100644 index 00000000..c416993c --- /dev/null +++ b/src/samples/SonarBeam.cpp @@ -0,0 +1,63 @@ +#include "SonarBeam.hpp" + +#include +#include + +base::samples::SonarBeam::SonarBeam(const base::samples::SonarBeam& other) +{ + init(other); +} + +double base::samples::SonarBeam::getSpatialResolution() const +{ + //the sampling interval includes the time for + //the sound traveling from the transiter to the target an back + //to receiver + return sampling_interval*0.5*speed_of_sound; +} + +base::samples::SonarBeam& base::samples::SonarBeam::operator=(const base::samples::SonarBeam& other) +{ + init(other); + return *this; +} + +void base::samples::SonarBeam::init(const base::samples::SonarBeam& other) +{ + time = other.time; + bearing = other.bearing; + sampling_interval = other.sampling_interval; + speed_of_sound = other.speed_of_sound; + beamwidth_horizontal = other.beamwidth_horizontal; + beamwidth_vertical = other.beamwidth_vertical; + beam = other.beam; +} + +void base::samples::SonarBeam::swap(base::samples::SonarBeam& other) +{ + Time temp_time = time; + Angle temp_bearing = bearing; + double temp_sampling_interval = sampling_interval; + float temp_speed_of_sound = speed_of_sound; + float temp_beamwidth_horizontal = beamwidth_horizontal; + float temp_beamwidth_vertical= beamwidth_vertical; + + time = other.time; + bearing = other.bearing; + sampling_interval = other.sampling_interval; + speed_of_sound = other.speed_of_sound; + beamwidth_horizontal = other.beamwidth_horizontal; + beamwidth_vertical = other.beamwidth_vertical; + beam.swap(other.beam); + + other.time = temp_time; + other.bearing = temp_bearing; + other.sampling_interval = temp_sampling_interval; + other.speed_of_sound = temp_speed_of_sound; + other.beamwidth_horizontal = temp_beamwidth_horizontal; + other.beamwidth_vertical = temp_beamwidth_vertical; +} + + + + diff --git a/src/samples/SonarBeam.hpp b/src/samples/SonarBeam.hpp new file mode 100644 index 00000000..7a1009cb --- /dev/null +++ b/src/samples/SonarBeam.hpp @@ -0,0 +1,56 @@ +#ifndef BASE_SAMPLES_SONARBEAM_H__ +#define BASE_SAMPLES_SONARBEAM_H__ + +#include +#include +#include + +namespace base { namespace samples { + + struct SonarBeam { + typedef boost::uint8_t uint8_t; + + //timestamp of the sonar beam + Time time; + + //direction of the sonar beam in radians [-pi,+pi] + //zero is at the front + Angle bearing; + + //sampling interval of each range bin in secs + double sampling_interval; + + //speed of sound + //this takes the medium into account + float speed_of_sound; + + //horizontal beamwidth of the sonar beam in radians + float beamwidth_horizontal; + + //vertical beamwidth of the sonar beam in radians + float beamwidth_vertical; + + //received echoes (bins) along the beam + std::vector beam; + + SonarBeam(): + sampling_interval(std::numeric_limits::signaling_NaN()), + speed_of_sound(std::numeric_limits::signaling_NaN()), + beamwidth_horizontal(std::numeric_limits::signaling_NaN()), + beamwidth_vertical(std::numeric_limits::signaling_NaN()){} + + SonarBeam(const SonarBeam &other); + + //calculates the spatial resolution of the sonar beam in meter + //this takes the sampling_interval and the speed of sound into account + double getSpatialResolution()const; + + SonarBeam &operator=(const SonarBeam &other); + + void init(const SonarBeam &other); + + void swap(SonarBeam &other); + }; +}} + +#endif diff --git a/src/samples/SonarScan.cpp b/src/samples/SonarScan.cpp new file mode 100644 index 00000000..425fe406 --- /dev/null +++ b/src/samples/SonarScan.cpp @@ -0,0 +1,310 @@ +#include "SonarScan.hpp" + +#include +#include +#include +#include +#include +#include +#include + +base::samples::SonarScan::SonarScan() + : number_of_beams(0) + , number_of_bins(0) + , sampling_interval(0) + , speed_of_sound(0) + , beamwidth_horizontal(base::Angle::fromRad(0)) + , beamwidth_vertical(base::Angle::fromRad(0)) + , memory_layout_column(true) + , polar_coordinates(true) +{ + reset(); +} + +base::samples::SonarScan::SonarScan(uint16_t number_of_beams, uint16_t number_of_bins, base::Angle start_bearing, base::Angle angular_resolution, bool memory_layout_column) +{ + init(number_of_beams,number_of_bins,start_bearing,angular_resolution,memory_layout_column); +} + +base::samples::SonarScan::SonarScan(const base::samples::SonarScan& other, bool bcopy) +{ + init(other,bcopy); +} + + +base::samples::SonarScan& base::samples::SonarScan::operator=(const base::samples::SonarScan& other) +{ + init(other,true); + return *this; +} + +void base::samples::SonarScan::init(const base::samples::SonarScan& other, bool bcopy) +{ + init(other.number_of_beams,other.number_of_bins,other.start_bearing,other.angular_resolution,other.memory_layout_column); + time = other.time; + + beamwidth_vertical = other.beamwidth_vertical; + beamwidth_horizontal = other.beamwidth_horizontal; + sampling_interval = other.sampling_interval; + speed_of_sound = other.speed_of_sound; + polar_coordinates = other.polar_coordinates; + if(bcopy){ + setData(other.getData()); + time_beams = other.time_beams; + } +} + +void base::samples::SonarScan::init(uint16_t number_of_beams, uint16_t number_of_bins, base::Angle start_bearing, base::Angle angular_resolution, bool memory_layout_column, int val) +{ + //change size if the sonar scan does not fit + if(this->number_of_beams != number_of_beams || this->number_of_bins != number_of_bins) + { + this->number_of_beams = number_of_beams; + this->number_of_bins = number_of_bins; + data.resize(number_of_beams*number_of_bins); + } + this->start_bearing = start_bearing; + this->angular_resolution = angular_resolution; + this->memory_layout_column = memory_layout_column; + speed_of_sound = 0; + beamwidth_horizontal = base::Angle::fromRad(0); + beamwidth_vertical = base::Angle::fromRad(0); + reset(val); +} + +void base::samples::SonarScan::reset(const int val) +{ + this->time = base::Time(); + if (this->data.size() > 0 && val >= 0) + { + memset(&this->data[0], val%256, this->data.size()); + } + time_beams.clear(); +} + +int base::samples::SonarScan::beamIndexForBearing(const base::Angle bearing, bool range_check) const +{ + double temp_rad = (start_bearing-bearing).rad; + int index = round(temp_rad/angular_resolution.rad); + if(range_check && (index < 0 || index >= number_of_beams)) + return -1; + return index; +} + +bool base::samples::SonarScan::hasSonarBeam(const base::samples::SonarBeam& sonar_beam) const +{ + return hasSonarBeam(sonar_beam.bearing); +} + +bool base::samples::SonarScan::hasSonarBeam(const base::Angle bearing) const +{ + int index = beamIndexForBearing(bearing); + if(index < 0) + return false; + + //it is assumed that all data are set at once (imaging sonar) + if(time_beams.empty()) + return true; + + //it is assumed that the data are set beam by beam (scanning sonar) + if(time_beams[index].microseconds != 0) + return true; + return false; +} + +void base::samples::SonarScan::addSonarBeam(const base::samples::SonarBeam& sonar_beam, bool resize) +{ + if(memory_layout_column) + throw std::runtime_error("addSonarBeam: cannot add sonar beam because the memory layout is not supported. Call toggleMemoryLayout()"); + + if(number_of_bins < sonar_beam.beam.size()) + throw std::runtime_error("addSonarBeam: cannot add sonar beam: too many bins"); + + int index = beamIndexForBearing(sonar_beam.bearing,false); + if(index < 0) + throw std::runtime_error("addSonarBeam: negative index!"); + if(index >= number_of_beams) + { + if(!resize) + throw std::runtime_error("addSonarBeam: bearing is out of range"); + number_of_beams = index+1; + data.resize(number_of_beams*number_of_bins); + } + if(time_beams.size() != number_of_beams) + time_beams.resize(number_of_beams); + + time_beams[index] = sonar_beam.time; + sampling_interval = sonar_beam.sampling_interval; + beamwidth_vertical = Angle::fromRad(sonar_beam.beamwidth_vertical); + beamwidth_horizontal = Angle::fromRad(sonar_beam.beamwidth_horizontal); + speed_of_sound = sonar_beam.speed_of_sound; + memcpy(&data[index*number_of_bins],&sonar_beam.beam[0],sonar_beam.beam.size()); +} + +void base::samples::SonarScan::getSonarBeam(const base::Angle bearing, base::samples::SonarBeam& sonar_beam) const +{ + if(memory_layout_column) + throw std::runtime_error("getSonarBeam: Wrong memory layout!"); + int index = beamIndexForBearing(bearing); + if(index<0) + throw std::runtime_error("getSonarBeam: No Data for the given bearing!"); + + sonar_beam.beam.resize(number_of_bins); + memcpy(&sonar_beam.beam[0],&data[number_of_bins*index],number_of_bins); + if((int)time_beams.size() > index) + sonar_beam.time = time_beams[index]; + else + sonar_beam.time = time; + sonar_beam.speed_of_sound = speed_of_sound; + sonar_beam.beamwidth_horizontal = beamwidth_horizontal.rad; + sonar_beam.beamwidth_vertical = beamwidth_vertical.rad; + sonar_beam.sampling_interval = sampling_interval; + sonar_beam.bearing = bearing; +} + +void base::samples::SonarScan::toggleMemoryLayout() +{ + std::vector temp; + temp.resize(data.size()); + + if(memory_layout_column) + { + for(int row=0;row < number_of_beams;++row) + for(int column=0;column < number_of_bins;++column) + temp[row*number_of_bins+column] =data[column*number_of_beams+row]; + } + else + { + for(int row=0;row < number_of_beams;++row) + for(int column=0;column < number_of_bins;++column) + temp[column*number_of_beams+row] =data[row*number_of_bins+column]; + } + memory_layout_column = !memory_layout_column; + data.swap(temp); +} + +void base::samples::SonarScan::swap(base::samples::SonarScan& sonar_scan) +{ + //swap vector + data.swap(sonar_scan.data); + + //copy values to temp + base::Time temp_time = sonar_scan.time; + Angle temp_beamwidth_vertical = sonar_scan.beamwidth_vertical; + Angle temp_beamwidth_horizontal = sonar_scan.beamwidth_horizontal; + double temp_sampling_interval = sonar_scan.sampling_interval; + uint16_t temp_number_of_beams = sonar_scan.number_of_beams; + uint16_t temp_number_of_bins = sonar_scan.number_of_bins; + Angle temp_start_bearing = sonar_scan.start_bearing; + Angle temp_angular_resolution = sonar_scan.angular_resolution; + bool temp_memory_layout_column = sonar_scan.memory_layout_column; + bool temp_polar_coordinates = sonar_scan.polar_coordinates; + float temp_speed_of_sound = sonar_scan.speed_of_sound; + + //copy values + sonar_scan.time = time; + sonar_scan.beamwidth_vertical = beamwidth_vertical; + sonar_scan.beamwidth_horizontal = beamwidth_horizontal; + sonar_scan.sampling_interval = sampling_interval; + sonar_scan.number_of_beams = number_of_beams; + sonar_scan.number_of_bins = number_of_bins; + sonar_scan.start_bearing = start_bearing; + sonar_scan.angular_resolution = angular_resolution; + sonar_scan.memory_layout_column = memory_layout_column; + sonar_scan.polar_coordinates = polar_coordinates; + sonar_scan.speed_of_sound = speed_of_sound; + + time = temp_time; + beamwidth_vertical = temp_beamwidth_vertical; + beamwidth_horizontal = temp_beamwidth_horizontal; + sampling_interval = temp_sampling_interval; + number_of_beams = temp_number_of_beams; + number_of_bins = temp_number_of_bins; + start_bearing = temp_start_bearing; + angular_resolution = temp_angular_resolution; + memory_layout_column = temp_memory_layout_column; + polar_coordinates = temp_polar_coordinates; + speed_of_sound = temp_speed_of_sound; +} + +uint32_t base::samples::SonarScan::getNumberOfBytes() const +{ + return data.size(); +} + +uint32_t base::samples::SonarScan::getBinCount() const +{ + return number_of_beams * number_of_bins; +} + +const std::vector< uint8_t >& base::samples::SonarScan::getData() const +{ + return this->data; +} + +base::Angle base::samples::SonarScan::getEndBearing() const +{ + return start_bearing-angular_resolution*(number_of_beams-1); +} + +base::Angle base::samples::SonarScan::getStartBearing() const +{ + return start_bearing; +} + +base::Angle base::samples::SonarScan::getAngularResolution() const +{ + return angular_resolution; +} + +double base::samples::SonarScan::getSpatialResolution() const +{ + //the sampling interval includes the time for + //the sound traveling from the transmitter to the target an back + //to the receiver + return sampling_interval*0.5*speed_of_sound; +} + +void base::samples::SonarScan::setData(const std::vector< uint8_t >& data) +{ + this->data = data; +} + +void base::samples::SonarScan::setData(const char* data, uint32_t size) +{ + if (size != this->data.size()) + { + std::cerr << "SonarScan: " + << __FUNCTION__ << " (" << __FILE__ << ", line " + << __LINE__ << "): " << "size mismatch in setData() (" + << size << " != " << this->data.size() << ")" + << std::endl; + return; + } + memcpy(&this->data[0], data, size); +} + +uint8_t* base::samples::SonarScan::getDataPtr() +{ + return static_cast(&this->data[0]); +} + +const uint8_t* base::samples::SonarScan::getDataConstPtr() const +{ + return static_cast(&this->data[0]); +} + + + + + + + + + + + + + + + diff --git a/src/samples/SonarScan.hpp b/src/samples/SonarScan.hpp new file mode 100644 index 00000000..7443a2a5 --- /dev/null +++ b/src/samples/SonarScan.hpp @@ -0,0 +1,164 @@ +/*! \file sonar_scan.h + \brief container for sonar scan data + */ + +#ifndef BASE_SAMPLES_SONARSCAN_H__ +#define BASE_SAMPLES_SONARSCAN_H__ + +#include +#include +#include + +namespace base { namespace samples { + + struct SonarScan + { + public: + /** + * Initialize the sonar scan + */ + SonarScan(); + + SonarScan(uint16_t number_of_beams,uint16_t number_of_bins,Angle start_bearing,Angle angular_resolution,bool memory_layout_column=true); + + //makes a copy of other + SonarScan(const SonarScan &other,bool bcopy = true); + + SonarScan &operator=(const SonarScan &other); + + //makes a copy of other + void init(const SonarScan &other,bool bcopy = true); + + void init(uint16_t number_of_beams, uint16_t number_of_bins, Angle start_bearing, Angle angular_resolution,bool memory_layout_column = true, int val=-1); + + // if val is negative the sonar scan data will not be initialized + void reset(int const val = 0); + + //returns the index/column of the sonar beam which + //holds information about the given bearing + //returns -1 if the sonar scan has no sonar beam for the given bearing + int beamIndexForBearing(const Angle bearing,bool range_check=true)const; + + //returns true if a sonar beam was already added for the given + //bearing + //internally time_beams is used for checking + bool hasSonarBeam(const base::samples::SonarBeam &sonar_beam)const; + + bool hasSonarBeam(const Angle bearing)const; + + + //adds a sonar beam to the sonar scan + //throws an exception if the sonar scan cannot hold the sonar beam and resize is set to false + //otherwise the sonar scan will be resized (the start angle is not changed!) + // + //the memory layout must be one sonar beam per row otherwise the function will throw a std::runtime_error + void addSonarBeam(const base::samples::SonarBeam &sonar_beam,bool resize=true); + + //returns a sonar beam for a given bearing + //throws an exception if the sonar scans holds no information for the given bearing + //the memory layout must be one beam per row + // + //an exception is thrown if the sonar beam holds no information about the given bearing + void getSonarBeam(const Angle bearing,SonarBeam &sonar_beam)const; + + //this toggles the memory layout between one sonar beam per row and one sonar beam per column + //to add sonar beams the memory layout must be one sonar beam per row + void toggleMemoryLayout(); + + void swap(SonarScan &sonar_scan); + + uint32_t getNumberOfBytes() const; + + /** + * Returns the total count of bins in this sonar scan + * @return Returns the overall number of bins (number_of_beams * number_of_bins) + */ + uint32_t getBinCount() const; + + const std::vector &getData() const; + + + Angle getEndBearing()const; + + Angle getStartBearing()const; + + Angle getAngularResolution()const; + + //calculates the spatial resolution of the sonar scan in meter + //this takes the sampling_interval and the speed of sound into account + double getSpatialResolution()const; + + void setData(const std::vector &data); + + void setData(const char *data, uint32_t size); + + uint8_t *getDataPtr(); + + const uint8_t *getDataConstPtr() const; + + //The time at which this sonar scan has been captured + base::Time time; + + //The raw data of the sonar scan + std::vector data; + + //Time stamp for each sonar beam + //if the time stamp is 1 January 1970 + //the beam is regarded as not be set + //vector can be empty in this case time + //is used for all beams + std::vector time_beams; + + //number of beams + //this can be interpreted as image width + uint16_t number_of_beams; + + //number of bins + //this can be interpreted as image height + uint16_t number_of_bins; + + /** The angle at which the reading starts. Zero is at the front of + * the device and turns counter-clockwise. + * This value is in radians + * + * All beams are stored from left to right to match the memory + * layout of an image !!! Therefore the end bearing is usually + * smaller than the start bearing + */ + Angle start_bearing; + + // Angle difference between two beams in radians + // The beams are stored from left to right + // to match the memory layout of an image + // + // This value must be always positive ! + Angle angular_resolution; + + //sampling interval of each range bin in secs + double sampling_interval; + + //speed of sound + //this takes the medium into account + float speed_of_sound; + + //horizontal beam width of the sonar beam in radians + Angle beamwidth_horizontal; + + //vertical beam width of the sonar beam in radians + Angle beamwidth_vertical; + + //if set to true one sonar beam is stored per column + //otherwise per row + bool memory_layout_column; + + //if set to true the bins are interpreted in polar coordinates + //otherwise in Cartesian coordinates + // + //Some imaging sonars store their data in Cartesian rather than + //polar coordinates (BlueView) + bool polar_coordinates; + }; +}} + +#endif + diff --git a/base/samples/Wrench.hpp b/src/samples/Wrench.hpp similarity index 100% rename from base/samples/Wrench.hpp rename to src/samples/Wrench.hpp diff --git a/base/samples/Wrenches.hpp b/src/samples/Wrenches.hpp similarity index 100% rename from base/samples/Wrenches.hpp rename to src/samples/Wrenches.hpp diff --git a/base/templates/TimeStamped.hpp b/src/templates/TimeStamped.hpp similarity index 100% rename from base/templates/TimeStamped.hpp rename to src/templates/TimeStamped.hpp From 183eeb37cb06ca76475e8f2a0f36c041eb6c2b94 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Mon, 8 Aug 2016 17:17:28 +0200 Subject: [PATCH 32/50] re-removed vector included --- src/samples/LaserScan.cpp | 1 - src/samples/LaserScan.hpp | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/src/samples/LaserScan.cpp b/src/samples/LaserScan.cpp index 95b9082b..badfa8d2 100644 --- a/src/samples/LaserScan.cpp +++ b/src/samples/LaserScan.cpp @@ -1,6 +1,5 @@ #include "LaserScan.hpp" -#include #include bool base::samples::LaserScan::isValidBeam(const unsigned int i) const diff --git a/src/samples/LaserScan.hpp b/src/samples/LaserScan.hpp index d9ea01c8..07a4c145 100644 --- a/src/samples/LaserScan.hpp +++ b/src/samples/LaserScan.hpp @@ -1,6 +1,7 @@ #ifndef BASE_SAMPLES_LASER_H__ #define BASE_SAMPLES_LASER_H__ +#include #include #include #include From 5aa82b751d7a37e9868a3eb5358b063116badb81 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Tue, 9 Aug 2016 14:11:12 +0200 Subject: [PATCH 33/50] re deleted pc file --- base-types.pc.in | 11 ----------- 1 file changed, 11 deletions(-) delete mode 100644 base-types.pc.in diff --git a/base-types.pc.in b/base-types.pc.in deleted file mode 100644 index dc18a185..00000000 --- a/base-types.pc.in +++ /dev/null @@ -1,11 +0,0 @@ -prefix=@CMAKE_INSTALL_PREFIX@ -exec_prefix=${prefix} -includedir=${prefix}/include - -Name: baseTypes -Version: 0.1 -Description: Common types for robotics modules -Libs: -Requires: eigen3 base-logging -Cflags: -I${includedir} -I${includedir}/base/backward - From 5e1d5dff91cff2723d7d6ceaaee9415ab167007e Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Tue, 9 Aug 2016 14:11:21 +0200 Subject: [PATCH 34/50] corrected std::string type --- src/samples/CompressedFrame.cpp | 2 +- src/samples/Frame.cpp | 8 ++++---- src/samples/Joints.cpp | 10 +++++----- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/samples/CompressedFrame.cpp b/src/samples/CompressedFrame.cpp index 15d48594..6a3ce30d 100644 --- a/src/samples/CompressedFrame.cpp +++ b/src/samples/CompressedFrame.cpp @@ -1,6 +1,6 @@ #include "CompressedFrame.hpp" -base::samples::frame::frame_compressed_mode_t base::samples::frame::CompressedFrame::toFrameMode(const std::__cxx11::string& str) +base::samples::frame::frame_compressed_mode_t base::samples::frame::CompressedFrame::toFrameMode(const std::string& str) { if(str == "MODE_COMPRESSED_UNDEFINED") return frame_compressed_mode_t::MODE_COMPRESSED_UNDEFINED; diff --git a/src/samples/Frame.cpp b/src/samples/Frame.cpp index 57a8aa65..f61bf347 100644 --- a/src/samples/Frame.cpp +++ b/src/samples/Frame.cpp @@ -6,7 +6,7 @@ #include #include -void base::samples::frame::frame_attrib_t::set(const std::__cxx11::string& name, const std::string& data) +void base::samples::frame::frame_attrib_t::set(const std::string& name, const std::string& data) { name_ = name; data_ = data; @@ -203,7 +203,7 @@ uint32_t base::samples::frame::Frame::getChannelCount(base::samples::frame::fram } } -base::samples::frame::frame_mode_t base::samples::frame::Frame::toFrameMode(const std::__cxx11::string& str) +base::samples::frame::frame_mode_t base::samples::frame::Frame::toFrameMode(const std::string& str) { if(str == "MODE_UNDEFINED") return MODE_UNDEFINED; @@ -374,7 +374,7 @@ const uint8_t* base::samples::frame::Frame::getLastConstByte() const return static_cast(&this->image.back()); } -bool base::samples::frame::Frame::hasAttribute(const std::__cxx11::string& name) const +bool base::samples::frame::Frame::hasAttribute(const std::string& name) const { std::vector::const_iterator _iter = attributes.begin(); for (;_iter != attributes.end();_iter++) @@ -385,7 +385,7 @@ bool base::samples::frame::Frame::hasAttribute(const std::__cxx11::string& name) return false; } -bool base::samples::frame::Frame::deleteAttribute(const std::__cxx11::string& name) +bool base::samples::frame::Frame::deleteAttribute(const std::string& name) { std::vector::iterator _iter = attributes.begin(); for (;_iter != attributes.end();_iter++) diff --git a/src/samples/Joints.cpp b/src/samples/Joints.cpp index 23cbf226..5989c023 100644 --- a/src/samples/Joints.cpp +++ b/src/samples/Joints.cpp @@ -9,7 +9,7 @@ base::samples::Joints base::samples::Joints::Positions(const std::vector< double return result; } -base::samples::Joints base::samples::Joints::Positions(const std::vector< double >& positions, const std::vector< std::__cxx11::string >& names) +base::samples::Joints base::samples::Joints::Positions(const std::vector< double >& positions, const std::vector< std::string >& names) { Joints result = Positions(positions); if (result.elements.size() != names.size()) @@ -27,7 +27,7 @@ base::samples::Joints base::samples::Joints::Speeds(const std::vector< float >& return result; } -base::samples::Joints base::samples::Joints::Speeds(const std::vector< float >& speeds, const std::vector< std::__cxx11::string >& names) +base::samples::Joints base::samples::Joints::Speeds(const std::vector< float >& speeds, const std::vector< std::string >& names) { Joints result = Speeds(speeds); if (result.elements.size() != names.size()) @@ -45,7 +45,7 @@ base::samples::Joints base::samples::Joints::Efforts(const std::vector< float >& return result; } -base::samples::Joints base::samples::Joints::Efforts(const std::vector< float >& efforts, const std::vector< std::__cxx11::string >& names) +base::samples::Joints base::samples::Joints::Efforts(const std::vector< float >& efforts, const std::vector< std::string >& names) { Joints result = Efforts(efforts); if (result.elements.size() != names.size()) @@ -63,7 +63,7 @@ base::samples::Joints base::samples::Joints::Raw(const std::vector< float >& raw return result; } -base::samples::Joints base::samples::Joints::Raw(const std::vector< float >& raw, const std::vector< std::__cxx11::string >& names) +base::samples::Joints base::samples::Joints::Raw(const std::vector< float >& raw, const std::vector< std::string >& names) { Joints result = Raw(raw); if (result.elements.size() != names.size()) @@ -81,7 +81,7 @@ base::samples::Joints base::samples::Joints::Accelerations(const std::vector< fl return result; } -base::samples::Joints base::samples::Joints::Accelerations(const std::vector< float >& acceleration, const std::vector< std::__cxx11::string >& names) +base::samples::Joints base::samples::Joints::Accelerations(const std::vector< float >& acceleration, const std::vector< std::string >& names) { Joints result = Accelerations(acceleration); if (result.elements.size() != names.size()) From 8f6f83e3e1c2a5073bdc08f1cdb3ae5673c52f52 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Tue, 9 Aug 2016 14:41:39 +0200 Subject: [PATCH 35/50] moved guaranteeSPD to TwistWithCovariance only --- src/Eigen.hpp | 26 -------------------------- src/TwistWithCovariance.cpp | 33 ++++++++++++++++----------------- 2 files changed, 16 insertions(+), 43 deletions(-) diff --git a/src/Eigen.hpp b/src/Eigen.hpp index 6953fec6..246676cd 100644 --- a/src/Eigen.hpp +++ b/src/Eigen.hpp @@ -4,7 +4,6 @@ #include #include -#include namespace base { @@ -54,31 +53,6 @@ namespace base return isnotnan(x - x); }; - /** - * Guarantee Symmetric (semi-) Positive Definite (SPD) matrix. - * The input matrix must be symmetric already (only the lower triangular part will be considered) - */ - template - static typename _MatrixType::PlainObject guaranteeSPD (const _MatrixType &A, const typename _MatrixType::RealScalar& minEig = 0.0) - { - typedef typename _MatrixType::PlainObject Matrix; - typedef Eigen::SelfAdjointEigenSolver EigenDecomp; - typedef typename EigenDecomp::RealVectorType EigenValues; - - // Eigenvalue decomposition - Eigen::SelfAdjointEigenSolver eigOfA (A, Eigen::ComputeEigenvectors); - - // truncate Eigenvalues: - EigenValues s = eigOfA.eigenvalues(); - s = s.cwiseMax(minEig); - - // re-compose matrix: - Matrix spdA = eigOfA.eigenvectors() * s.asDiagonal() * eigOfA.eigenvectors().adjoint(); - - return spdA; - }; - - } #endif diff --git a/src/TwistWithCovariance.cpp b/src/TwistWithCovariance.cpp index 646d219d..8ceb934f 100644 --- a/src/TwistWithCovariance.cpp +++ b/src/TwistWithCovariance.cpp @@ -5,31 +5,30 @@ #include #include #include -#include +#include #include -// Guarantee Semi-Positive Definite (SPD) matrix. +/** + * Guarantee Symmetric (semi-) Positive Definite (SPD) matrix. + * The input matrix must be symmetric already (only the lower triangular part will be considered) + */ template -static _MatrixType guaranteeSPD (const _MatrixType &A) +static typename _MatrixType::PlainObject guaranteeSPD (const _MatrixType &A, const typename _MatrixType::RealScalar& minEig = 0.0) { - _MatrixType spdA; - Eigen::VectorXd s; - s.resize(A.rows(), 1); + typedef typename _MatrixType::PlainObject Matrix; + typedef Eigen::SelfAdjointEigenSolver EigenDecomp; + typedef typename EigenDecomp::RealVectorType EigenValues; - /** - * Single Value Decomposition - */ - Eigen::JacobiSVD svdOfA (A, Eigen::ComputeThinU | Eigen::ComputeThinV); + // Eigenvalue decomposition + Eigen::SelfAdjointEigenSolver eigOfA (A, Eigen::ComputeEigenvectors); - s = svdOfA.singularValues(); //!eigenvalues + // truncate Eigenvalues: + EigenValues s = eigOfA.eigenvalues(); + s = s.cwiseMax(minEig); - for (register int i=0; i Date: Wed, 10 Aug 2016 14:08:50 +0200 Subject: [PATCH 36/50] added missing include --- src/samples/Frame.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/samples/Frame.hpp b/src/samples/Frame.hpp index b49b0b1d..94f2ab74 100644 --- a/src/samples/Frame.hpp +++ b/src/samples/Frame.hpp @@ -8,7 +8,7 @@ #include #include #include - +#include #include From d5d5d11095e79ccb1b41edc4fe59cb5bcf22e6a0 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 18 Aug 2016 12:06:10 +0200 Subject: [PATCH 37/50] removed deprecated CompressedFrame --- src/CMakeLists.txt | 2 -- src/samples/CompressedFrame.cpp | 9 -------- src/samples/CompressedFrame.hpp | 41 --------------------------------- 3 files changed, 52 deletions(-) delete mode 100644 src/samples/CompressedFrame.cpp delete mode 100644 src/samples/CompressedFrame.hpp diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ab3ca28e..c70fb8b8 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -19,7 +19,6 @@ rock_library( Waypoint.cpp commands/Motion2D.cpp samples/BodyState.cpp - samples/CompressedFrame.cpp samples/DepthMap.cpp samples/DistanceImage.cpp samples/Frame.cpp @@ -65,7 +64,6 @@ rock_library( logging/logging_printf_style.h samples/BodyState.hpp samples/CommandSamples.hpp - samples/CompressedFrame.hpp samples/DepthMap.hpp samples/DistanceImage.hpp samples/Frame.hpp diff --git a/src/samples/CompressedFrame.cpp b/src/samples/CompressedFrame.cpp deleted file mode 100644 index 6a3ce30d..00000000 --- a/src/samples/CompressedFrame.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include "CompressedFrame.hpp" - -base::samples::frame::frame_compressed_mode_t base::samples::frame::CompressedFrame::toFrameMode(const std::string& str) -{ - if(str == "MODE_COMPRESSED_UNDEFINED") - return frame_compressed_mode_t::MODE_COMPRESSED_UNDEFINED; - else if (str == "MODE_PJPG") - return frame_compressed_mode_t::MODE_PJPG; -} diff --git a/src/samples/CompressedFrame.hpp b/src/samples/CompressedFrame.hpp deleted file mode 100644 index 719a299d..00000000 --- a/src/samples/CompressedFrame.hpp +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef _COMPRESSED_FRAME_H_ -#define _COMPRESSED_FRAME_H_ - -#include - -namespace base { namespace samples { namespace frame { - enum class frame_compressed_mode_t { - MODE_COMPRESSED_UNDEFINED = 0, - MODE_PJPG = 1 - }; - - struct CompressedFrame{ - - /** The time at which this frame has been captured - * - * This is obviously an estimate - */ - base::Time time; - /** The time at which this frame has been received on the system */ - base::Time received_time; - - /** The raw data */ - std::vector image; - /** Additional metadata */ - std::vector attributes; - - /** The image size in pixels */ - frame_size_t size; - - frame_compressed_mode_t frame_mode; - - /** Status flag */ - frame_status_t frame_status; - - static frame_compressed_mode_t toFrameMode(const std::string &str); - }; - - -}}}; - -#endif From 4dda96d93ce3e94fd7e3501e8c682c5175287093 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 18 Aug 2016 14:25:34 +0200 Subject: [PATCH 38/50] moved namespaces to namespace { style --- src/Angle.cpp | 30 ++--- src/JointLimitRange.cpp | 2 +- src/JointLimits.cpp | 7 +- src/JointState.cpp | 60 +++++----- src/JointTransform.cpp | 10 +- src/JointsTrajectory.cpp | 22 ++-- src/Pose.cpp | 54 ++++----- src/Pressure.cpp | 18 +-- src/Spline.cpp | 27 +++-- src/Temperature.cpp | 53 ++++----- src/Time.cpp | 84 ++++++-------- src/TimeMark.cpp | 12 +- src/Timeout.cpp | 26 +++-- src/Trajectory.cpp | 7 +- src/TransformWithCovariance.cpp | 115 +++++++++---------- src/TwistWithCovariance.cpp | 153 +++++++++++--------------- src/Waypoint.cpp | 10 +- src/commands/Motion2D.cpp | 8 +- src/samples/BodyState.cpp | 101 ++++++++--------- src/samples/DepthMap.cpp | 29 ++--- src/samples/DistanceImage.cpp | 14 ++- src/samples/Frame.cpp | 98 +++++++++-------- src/samples/Joints.cpp | 23 ++-- src/samples/LaserScan.cpp | 16 ++- src/samples/Pressure.cpp | 10 +- src/samples/RigidBodyAcceleration.cpp | 6 +- src/samples/RigidBodyState.cpp | 132 +++++++++++----------- src/samples/Sonar.cpp | 87 +++++++-------- src/samples/SonarBeam.cpp | 14 ++- src/samples/SonarScan.cpp | 78 ++++++------- 30 files changed, 654 insertions(+), 652 deletions(-) diff --git a/src/Angle.cpp b/src/Angle.cpp index 947cd5cc..6f0a4f4d 100644 --- a/src/Angle.cpp +++ b/src/Angle.cpp @@ -2,14 +2,16 @@ #include #include -base::Angle base::Angle::vectorToVector(const base::Vector3d& a, const base::Vector3d& b) +namespace base { + +Angle Angle::vectorToVector(const Vector3d& a, const Vector3d& b) { double dot = a.dot(b); double norm = a.norm() * b.norm(); return fromRad(acos(dot / norm)); } -base::Angle base::Angle::vectorToVector(const base::Vector3d& a, const base::Vector3d& b, const base::Vector3d& positive) +Angle Angle::vectorToVector(const Vector3d& a, const Vector3d& b, const Vector3d& positive) { double cos = a.dot(b) / (a.norm() * b.norm()); @@ -20,23 +22,23 @@ base::Angle base::Angle::vectorToVector(const base::Vector3d& a, const base::Vec return fromRad(-acos(cos)); } -std::ostream& operator << (std::ostream& os, base::Angle angle) +std::ostream& operator << (std::ostream& os, Angle angle) { os << angle.getRad() << boost::format("[%3.1fdeg]") % angle.getDeg(); return os; } -base::AngleSegment::AngleSegment(): width(0), startRad(0), endRad(0) +AngleSegment::AngleSegment(): width(0), startRad(0), endRad(0) { } -base::AngleSegment::AngleSegment(const Angle &start, double _width): width(_width), startRad(start.getRad()), endRad(startRad + width) +AngleSegment::AngleSegment(const Angle &start, double _width): width(_width), startRad(start.getRad()), endRad(startRad + width) { if(width < 0) throw std::runtime_error("Error got segment with negative width"); } -bool base::AngleSegment::isInside(const base::Angle& angle) const +bool AngleSegment::isInside(const Angle& angle) const { double angleRad = angle.getRad(); if(angleRad < startRad) @@ -48,7 +50,7 @@ bool base::AngleSegment::isInside(const base::Angle& angle) const return false; } -bool base::AngleSegment::isInside(const base::AngleSegment& segment) const +bool AngleSegment::isInside(const AngleSegment& segment) const { double otherStart = segment.startRad; if(otherStart < startRad) @@ -62,7 +64,7 @@ bool base::AngleSegment::isInside(const base::AngleSegment& segment) const return false; } -std::vector< base::AngleSegment > base::AngleSegment::getIntersections(const base::AngleSegment& b) const +std::vector< AngleSegment > AngleSegment::getIntersections(const AngleSegment& b) const { std::vector ret; //special case, this segment is a whole circle @@ -154,18 +156,20 @@ std::vector< base::AngleSegment > base::AngleSegment::getIntersections(const bas return ret; } -base::Angle base::AngleSegment::getStart() const +Angle AngleSegment::getStart() const { - return base::Angle::fromRad(startRad); + return Angle::fromRad(startRad); } -base::Angle base::AngleSegment::getEnd() const +Angle AngleSegment::getEnd() const { - return base::Angle::fromRad(endRad); + return Angle::fromRad(endRad); } -std::ostream& operator << (std::ostream& os, base::AngleSegment seg) +std::ostream& operator << (std::ostream& os, AngleSegment seg) { os << " Segmend start " << seg.startRad/M_PI *180.0 << " end " << seg.endRad/M_PI * 180.0 << " width " << seg.width /M_PI * 180.0; return os; } + +} //end namespace base diff --git a/src/JointLimitRange.cpp b/src/JointLimitRange.cpp index d3ee9210..338d94a9 100644 --- a/src/JointLimitRange.cpp +++ b/src/JointLimitRange.cpp @@ -115,4 +115,4 @@ std::pair< bool, JointLimitRange::OutOfBounds > JointLimitRange::verifyValidity( -} \ No newline at end of file +} //end namespace base \ No newline at end of file diff --git a/src/JointLimits.cpp b/src/JointLimits.cpp index 7d052301..67dff650 100644 --- a/src/JointLimits.cpp +++ b/src/JointLimits.cpp @@ -1,6 +1,8 @@ #include "JointLimits.hpp" -bool base::JointLimits::isValid(const base::samples::Joints& joints) const +namespace base { + +bool JointLimits::isValid(const samples::Joints& joints) const { if (joints.hasNames()) { @@ -17,7 +19,7 @@ bool base::JointLimits::isValid(const base::samples::Joints& joints) const return true; } -void base::JointLimits::validate(const base::samples::Joints& joints) const +void JointLimits::validate(const samples::Joints& joints) const { if (joints.hasNames()) { @@ -31,3 +33,4 @@ void base::JointLimits::validate(const base::samples::Joints& joints) const } } +} //end namespace base \ No newline at end of file diff --git a/src/JointState.cpp b/src/JointState.cpp index 622fd231..0efa255b 100644 --- a/src/JointState.cpp +++ b/src/JointState.cpp @@ -1,91 +1,93 @@ #include "JointState.hpp" -base::JointState base::JointState::Position(double value) +namespace base { + +JointState JointState::Position(double value) { JointState ret; ret.position = value; return ret; } -base::JointState base::JointState::Speed(float value) +JointState JointState::Speed(float value) { JointState ret; ret.speed = value; return ret; } -base::JointState base::JointState::Effort(float value) +JointState JointState::Effort(float value) { JointState ret; ret.effort = value; return ret; } -base::JointState base::JointState::Raw(float value) +JointState JointState::Raw(float value) { JointState ret; ret.raw = value; return ret; } -base::JointState base::JointState::Acceleration(float value) +JointState JointState::Acceleration(float value) { JointState ret; ret.acceleration = value; return ret; } -bool base::JointState::hasPosition() const +bool JointState::hasPosition() const { - return !base::isUnset(position); + return !isUnset(position); } -bool base::JointState::hasSpeed() const +bool JointState::hasSpeed() const { - return !base::isUnset(speed); + return !isUnset(speed); } -bool base::JointState::hasEffort() const +bool JointState::hasEffort() const { - return !base::isUnset(effort); + return !isUnset(effort); } -bool base::JointState::hasRaw() const +bool JointState::hasRaw() const { - return !base::isUnset(raw); + return !isUnset(raw); } -bool base::JointState::hasAcceleration() const +bool JointState::hasAcceleration() const { - return !base::isUnset(acceleration); + return !isUnset(acceleration); } -bool base::JointState::isPosition() const +bool JointState::isPosition() const { return hasPosition() && !hasSpeed() && !hasEffort() && !hasRaw() && !hasAcceleration(); } -bool base::JointState::isSpeed() const +bool JointState::isSpeed() const { return !hasPosition() && hasSpeed() && !hasEffort() && !hasRaw() && !hasAcceleration(); } -bool base::JointState::isEffort() const +bool JointState::isEffort() const { return !hasPosition() && !hasSpeed() && hasEffort() && !hasRaw() && !hasAcceleration(); } -bool base::JointState::isRaw() const +bool JointState::isRaw() const { return !hasPosition() && !hasSpeed() && !hasEffort() && hasRaw() && !hasAcceleration(); } -bool base::JointState::isAcceleration() const +bool JointState::isAcceleration() const { return !hasPosition() && !hasSpeed() && !hasEffort() && !hasRaw() && hasAcceleration(); } -double base::JointState::getField(int mode) const +double JointState::getField(int mode) const { switch(mode) { @@ -98,7 +100,7 @@ double base::JointState::getField(int mode) const } } -void base::JointState::setField(int mode, double value) +void JointState::setField(int mode, double value) { switch(mode) { @@ -121,7 +123,7 @@ void base::JointState::setField(int mode, double value) } } -base::JointState::MODE base::JointState::getMode() const +JointState::MODE JointState::getMode() const { if (isPosition()) return POSITION; else if (isSpeed()) return SPEED; @@ -135,15 +137,5 @@ base::JointState::MODE base::JointState::getMode() const } - - - - - - - - - - - +} //end namespace base diff --git a/src/JointTransform.cpp b/src/JointTransform.cpp index 64064ce2..308144b5 100644 --- a/src/JointTransform.cpp +++ b/src/JointTransform.cpp @@ -1,9 +1,11 @@ #include "JointTransform.hpp" -void base::JointTransformVector::setRigidBodyStates(const base::samples::Joints& joints, std::vector< base::samples::RigidBodyState >& rbs) const +namespace base { + +void JointTransformVector::setRigidBodyStates(const samples::Joints& joints, std::vector< samples::RigidBodyState >& rbs) const { if (joints.names.empty()) { - throw std::runtime_error("base::JointTransformVector::" + throw std::runtime_error("JointTransformVector::" "setRigidBodyStates(): the vector " "'joints.names()' is empty"); } @@ -13,7 +15,7 @@ void base::JointTransformVector::setRigidBodyStates(const base::samples::Joints& { if (!joints[i].hasPosition()) { std::stringstream ss; - ss << "base::JointTransformVector::setRigidBodyStates(): " + ss << "JointTransformVector::setRigidBodyStates(): " "the joint 'joints[" << i << "]' has no position"; throw std::runtime_error(ss.str()); } @@ -26,3 +28,5 @@ void base::JointTransformVector::setRigidBodyStates(const base::samples::Joints& rbs[i].setTransform( Eigen::Isometry3d( Eigen::AngleAxisd( joints[i].position, jt.rotationAxis ) ) ); } } + +} //end namespace base \ No newline at end of file diff --git a/src/JointsTrajectory.cpp b/src/JointsTrajectory.cpp index 62be1025..863c34ef 100644 --- a/src/JointsTrajectory.cpp +++ b/src/JointsTrajectory.cpp @@ -1,6 +1,8 @@ #include "JointsTrajectory.hpp" -bool base::JointsTrajectory::isValid() const +namespace base { + +bool JointsTrajectory::isValid() const { size_t samples = getTimeSteps(); @@ -16,7 +18,7 @@ bool base::JointsTrajectory::isValid() const return true; } -void base::JointsTrajectory::resize(int num_joints, int num_samples) +void JointsTrajectory::resize(int num_joints, int num_samples) { this->resize(num_joints); for(size_t i=0; i getTimeSteps()) throw(InvalidTimeStep(time_step)); @@ -42,12 +44,12 @@ void base::JointsTrajectory::getJointsAtTimeStep(size_t time_step, base::samples } } -bool base::JointsTrajectory::isTimed() const +bool JointsTrajectory::isTimed() const { return !times.empty(); } -size_t base::JointsTrajectory::getTimeSteps() const +size_t JointsTrajectory::getTimeSteps() const { size_t steps = 0; if( !elements.empty() ) @@ -55,14 +57,14 @@ size_t base::JointsTrajectory::getTimeSteps() const return steps; } -size_t base::JointsTrajectory::getNumberOfJoints() const +size_t JointsTrajectory::getNumberOfJoints() const { return elements.size(); } -base::Time base::JointsTrajectory::getDuration() const +Time JointsTrajectory::getDuration() const { - base::Time summed; + Time summed; for(size_t i=0; i Eigen::NumTraits::dummy_precision()){ res[0] = ::atan2(m.coeff(1,0), m.coeff(0,0)); res[2] = ::atan2(m.coeff(2,1), m.coeff(2,2)); @@ -18,11 +18,11 @@ base::Vector3d getEuler(const base::Orientation& orientation) return res; } -base::Vector3d getEuler(const base::AngleAxisd& orientation) +Vector3d getEuler(const AngleAxisd& orientation) { const Eigen::Matrix3d m = orientation.toRotationMatrix(); - double x = base::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm(); - base::Vector3d res(0,::atan2(-m.coeff(2,0), x),0); + double x = Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm(); + Vector3d res(0,::atan2(-m.coeff(2,0), x),0); if (x > Eigen::NumTraits::dummy_precision()){ res[0] = ::atan2(m.coeff(1,0), m.coeff(0,0)); res[2] = ::atan2(m.coeff(2,1), m.coeff(2,2)); @@ -33,62 +33,62 @@ base::Vector3d getEuler(const base::AngleAxisd& orientation) return res; } -double getYaw(const base::Orientation& orientation) +double getYaw(const Orientation& orientation) { - return base::getEuler(orientation)[0]; + return getEuler(orientation)[0]; } -double getYaw(const base::AngleAxisd& orientation) +double getYaw(const AngleAxisd& orientation) { - return base::getEuler(orientation)[0]; + return getEuler(orientation)[0]; } -double getPitch(const base::Orientation& orientation) +double getPitch(const Orientation& orientation) { - return base::getEuler(orientation)[1]; + return getEuler(orientation)[1]; } -double getPitch(const base::AngleAxisd& orientation) +double getPitch(const AngleAxisd& orientation) { - return base::getEuler(orientation)[1]; + return getEuler(orientation)[1]; } -double getRoll(const base::Orientation& orientation) +double getRoll(const Orientation& orientation) { - return base::getEuler(orientation)[2]; + return getEuler(orientation)[2]; } -double getRoll(const base::AngleAxisd& orientation) +double getRoll(const AngleAxisd& orientation) { - return base::getEuler(orientation)[2]; + return getEuler(orientation)[2]; } -base::Orientation removeYaw(const base::Orientation& orientation) +Orientation removeYaw(const Orientation& orientation) { return Eigen::AngleAxisd( -getYaw(orientation), Eigen::Vector3d::UnitZ()) * orientation; } -base::Orientation removeYaw(const base::AngleAxisd& orientation) +Orientation removeYaw(const AngleAxisd& orientation) { return Eigen::AngleAxisd( -getYaw(orientation), Eigen::Vector3d::UnitZ()) * orientation; } -base::Orientation removePitch(const base::Orientation& orientation) +Orientation removePitch(const Orientation& orientation) { return Eigen::AngleAxisd( -getPitch(orientation), Eigen::Vector3d::UnitY()) * orientation; } -base::Orientation removePitch(const base::AngleAxisd& orientation) +Orientation removePitch(const AngleAxisd& orientation) { return Eigen::AngleAxisd( -getPitch(orientation), Eigen::Vector3d::UnitY()) * orientation; } -base::Orientation removeRoll(const base::Orientation& orientation) +Orientation removeRoll(const Orientation& orientation) { return Eigen::AngleAxisd( -getRoll(orientation), Eigen::Vector3d::UnitX()) * orientation; } -base::Orientation removeRoll(const base::AngleAxisd& orientation) +Orientation removeRoll(const AngleAxisd& orientation) { return Eigen::AngleAxisd( -getRoll(orientation), Eigen::Vector3d::UnitX()) * orientation; } @@ -139,7 +139,7 @@ Vector6d Pose::toVector6d() const return res; } -std::ostream& operator << (std::ostream& io, base::Pose const& pose) +std::ostream& operator << (std::ostream& io, Pose const& pose) { io << "Position " << pose.position.transpose() @@ -150,7 +150,7 @@ std::ostream& operator << (std::ostream& io, base::Pose const& pose) return io; } -std::ostream& operator << (std::ostream& io, base::Pose2D const& pose) +std::ostream& operator << (std::ostream& io, Pose2D const& pose) { io << "Position " @@ -162,4 +162,4 @@ std::ostream& operator << (std::ostream& io, base::Pose2D const& pose) -} \ No newline at end of file +} //end namespace base \ No newline at end of file diff --git a/src/Pressure.cpp b/src/Pressure.cpp index fabef3b1..2c0b8df6 100644 --- a/src/Pressure.cpp +++ b/src/Pressure.cpp @@ -2,44 +2,46 @@ #include "Float.hpp" -base::Pressure::Pressure() : pascal(base::unknown()) +namespace base { + +Pressure::Pressure() : pascal(unknown()) { } -base::Pressure base::Pressure::fromPascal(float pascal) +Pressure Pressure::fromPascal(float pascal) { Pressure result; result.pascal = pascal; return result; } -base::Pressure base::Pressure::fromBar(float bar) +Pressure Pressure::fromBar(float bar) { return fromPascal(bar * 100000); } -base::Pressure base::Pressure::fromPSI(float psi) +Pressure Pressure::fromPSI(float psi) { return fromPascal(psi * 6894.75729); } -float base::Pressure::toPa() const +float Pressure::toPa() const { return pascal; } -float base::Pressure::toBar() const +float Pressure::toBar() const { return pascal / 100000; } -float base::Pressure::toPSI() const +float Pressure::toPSI() const { return pascal / 6894.75729; } - +} //end namespace base diff --git a/src/Spline.cpp b/src/Spline.cpp index 4772638c..346d7958 100644 --- a/src/Spline.cpp +++ b/src/Spline.cpp @@ -8,10 +8,11 @@ #include using namespace std; -using namespace base::geometry; using boost::lexical_cast; using namespace Eigen; + + static double angleLimit(double angle) { if(angle > M_PI) @@ -22,6 +23,8 @@ static double angleLimit(double angle) return angle; } +namespace base { namespace geometry { + SplineBase::SplineBase (int dim, double _geometric_resolution, int _curve_order) : dimension(dim), curve(0), geometric_resolution(_geometric_resolution) , curve_order(_curve_order) @@ -99,7 +102,7 @@ void SplineBase::getPoint(double* result, double _param) const void SplineBase::getPointAndTangent(double* result, double _param) const { return getPointAndTangentHelper(result, _param, true); } -bool base::geometry::SplineBase::checkAndNormalizeParam(double& _param, double equalDistance) const +bool SplineBase::checkAndNormalizeParam(double& _param, double equalDistance) const { if(_param < start_param && start_param - _param < equalDistance) _param = start_param; @@ -184,7 +187,7 @@ double SplineBase::getVariationOfCurvature(double _param) // Variation of Curva return VoC; } -double base::geometry::SplineBase::getCurveLength(double relative_resolution) const +double SplineBase::getCurveLength(double relative_resolution) const { if (isSingleton()) return 0; @@ -288,7 +291,7 @@ void SplineBase::interpolate(const vector< double >& points, else { if (coord_types.size() * dimension != points.size()) { - throw std::runtime_error("base::types::Spline::interpolate(): " + throw std::runtime_error("types::Spline::interpolate(): " "'points.size()' does not match " "expectation"); } @@ -852,7 +855,7 @@ double SplineBase::join(SplineBase const& other, double tolerance, bool with_tan //be sure joining_Types has the right size if (joining_points.size() / dim > 4) { - throw std::runtime_error("base::SplineBase::join(): the size of " + throw std::runtime_error("SplineBase::join(): the size of " "'joining_points' is unexpectedly big"); } @@ -969,7 +972,7 @@ SISLCurve* SplineBase::getSISLCurve() return curve; } -base::Matrix3d SplineBase::getFrenetFrame(double _param) +Matrix3d SplineBase::getFrenetFrame(double _param) { if (!curve) throw std::runtime_error("getFrenetFrame() called on an empty or degenerate curve"); @@ -1006,7 +1009,7 @@ double SplineBase::headingError(double _actHeading, double _param) return angleLimit( _actHeading - getHeading(_param)); } -double SplineBase::distanceError(base::Vector3d _pt, double _param) +double SplineBase::distanceError(Vector3d _pt, double _param) { // Error vector Vector3d curve_point; @@ -1024,7 +1027,7 @@ double SplineBase::distanceError(base::Vector3d _pt, double _param) return (angle >= 0.0)?(error.norm()):(-error.norm()); } -base::Vector3d SplineBase::poseError(base::Vector3d _position, double _heading, double _guess, double minParam) +Vector3d SplineBase::poseError(Vector3d _position, double _heading, double _guess, double minParam) { double param = findOneClosestPoint(_position.data(), _guess, getGeometricResolution()); @@ -1032,15 +1035,15 @@ base::Vector3d SplineBase::poseError(base::Vector3d _position, double _heading, param = minParam; // Returns the error [distance error, orientation error, parameter] - return base::Vector3d(distanceError(_position, param), headingError(_heading, param), param); + return Vector3d(distanceError(_position, param), headingError(_heading, param), param); } -base::Vector3d SplineBase::poseError(base::Vector3d _position, double _heading, double _guess) +Vector3d SplineBase::poseError(Vector3d _position, double _heading, double _guess) { double param = findOneClosestPoint(_position.data(), _guess, getGeometricResolution()); // Returns the error [distance error, orientation error, parameter] - return base::Vector3d(distanceError(_position, param), headingError(_heading, param), param); + return Vector3d(distanceError(_position, param), headingError(_heading, param), param); } void SplineBase::crop(double start_t, double end_t) @@ -1143,3 +1146,5 @@ SplineBase *SplineBase::getSubSpline(double start_t, double end_t) const return new SplineBase(getGeometricResolution(), newCurve); } + +}} //end namespace base::geometry diff --git a/src/Temperature.cpp b/src/Temperature.cpp index e7747cb8..ac3b0b36 100644 --- a/src/Temperature.cpp +++ b/src/Temperature.cpp @@ -3,98 +3,100 @@ #include #include -base::Temperature::Temperature() : kelvin(base::unknown()) +namespace base { + +Temperature::Temperature() : kelvin(unknown()) { } -base::Temperature::Temperature(double kelvin) : kelvin(kelvin) +Temperature::Temperature(double kelvin) : kelvin(kelvin) { } -double base::Temperature::kelvin2Celsius(double kelvin) +double Temperature::kelvin2Celsius(double kelvin) { return kelvin - 273.15; } -double base::Temperature::celsius2Kelvin(double celsius) +double Temperature::celsius2Kelvin(double celsius) { return celsius + 273.15; } -base::Temperature base::Temperature::fromKelvin(double kelvin) +Temperature Temperature::fromKelvin(double kelvin) { return Temperature( kelvin ); } -base::Temperature base::Temperature::fromCelsius(double celsius) +Temperature Temperature::fromCelsius(double celsius) { return Temperature( celsius + 273.15); } -double base::Temperature::getKelvin() const +double Temperature::getKelvin() const { return kelvin; } -double base::Temperature::getCelsius() const +double Temperature::getCelsius() const { return kelvin - 273.15; } -bool base::Temperature::isApprox(base::Temperature other, double prec) const +bool Temperature::isApprox(Temperature other, double prec) const { return std::abs( other.kelvin - kelvin ) < prec; } -void base::Temperature::operator=(const base::Temperature& other) +void Temperature::operator=(const Temperature& other) { kelvin = other.kelvin; } -bool base::Temperature::operator==(const base::Temperature& other) const +bool Temperature::operator==(const Temperature& other) const { return this->kelvin == other.kelvin; } -bool base::Temperature::operator<(const base::Temperature& other) const +bool Temperature::operator<(const Temperature& other) const { return this->kelvin < other.kelvin; } -bool base::Temperature::operator>(const base::Temperature& other) const +bool Temperature::operator>(const Temperature& other) const { return this->kelvin > other.kelvin; } -base::Temperature base::operator+(base::Temperature a, base::Temperature b) +Temperature operator+(Temperature a, Temperature b) { return Temperature::fromKelvin( a.getKelvin() + b.getKelvin() ); } -base::Temperature base::operator-(base::Temperature a, base::Temperature b) +Temperature operator-(Temperature a, Temperature b) { return Temperature::fromKelvin( a.getKelvin() - b.getKelvin() ); } -base::Temperature base::operator*(base::Temperature a, double b) +Temperature operator*(Temperature a, double b) { return Temperature::fromKelvin( a.getKelvin() * b ); } -base::Temperature base::operator*(double a, base::Temperature b) +Temperature operator*(double a, Temperature b) { return Temperature::fromKelvin( a * b.getKelvin() ); } -std::ostream& base::operator<<(std::ostream& os, base::Temperature temperature) +std::ostream& operator<<(std::ostream& os, Temperature temperature) { os << temperature.getCelsius() << boost::format("[%3.1f celsius]"); return os; } -bool base::Temperature::isInRange(const base::Temperature &left_limit, const base::Temperature &right_limit) const +bool Temperature::isInRange(const Temperature &left_limit, const Temperature &right_limit) const { if((right_limit-left_limit).kelvin < 0) return !isInRange(right_limit,left_limit); @@ -104,17 +106,6 @@ bool base::Temperature::isInRange(const base::Temperature &left_limit, const bas } - - - - - - - - - - - - +} //end namespace base diff --git a/src/Time.cpp b/src/Time.cpp index 9c937616..91a0f3aa 100644 --- a/src/Time.cpp +++ b/src/Time.cpp @@ -7,85 +7,87 @@ #include #include -base::Time::Time(int64_t _microseconds) : microseconds(_microseconds) +namespace base { + +Time::Time(int64_t _microseconds) : microseconds(_microseconds) { } -base::Time::Time() : microseconds(0) +Time::Time() : microseconds(0) { } -base::Time base::Time::now() +Time Time::now() { timeval t; gettimeofday(&t, 0); return Time(static_cast(t.tv_sec) * UsecPerSec + t.tv_usec); } -bool base::Time::operator<(const base::Time& ts) const +bool Time::operator<(const Time& ts) const { return microseconds < ts.microseconds; } -bool base::Time::operator>(const base::Time& ts) const +bool Time::operator>(const Time& ts) const { return microseconds > ts.microseconds; } -bool base::Time::operator==(const base::Time& ts) const +bool Time::operator==(const Time& ts) const { return microseconds == ts.microseconds; } -bool base::Time::operator!=(const base::Time& ts) const +bool Time::operator!=(const Time& ts) const { return !(*this == ts); } -bool base::Time::operator>=(const base::Time& ts) const +bool Time::operator>=(const Time& ts) const { return !(*this < ts); } -bool base::Time::operator<=(const base::Time& ts) const +bool Time::operator<=(const Time& ts) const { return !(*this > ts); } -base::Time base::Time::operator-(const base::Time& ts) const +Time Time::operator-(const Time& ts) const { return Time(microseconds - ts.microseconds); } -base::Time base::Time::operator+(const base::Time& ts) const +Time Time::operator+(const Time& ts) const { return Time(microseconds + ts.microseconds); } -base::Time base::Time::operator/(int divider) const +Time Time::operator/(int divider) const { return Time(microseconds / divider); } -base::Time base::Time::operator*(double factor) const +Time Time::operator*(double factor) const { return Time(microseconds * factor); } -bool base::Time::isNull() const +bool Time::isNull() const { return microseconds == 0; } -timeval base::Time::toTimeval() const +timeval Time::toTimeval() const { timeval tv = { static_cast(microseconds / UsecPerSec), static_cast(microseconds % UsecPerSec) }; return tv; } -std::string base::Time::toString(base::Time::Resolution resolution, const std::string& mainFormat) const +std::string Time::toString(Time::Resolution resolution, const std::string& mainFormat) const { struct timeval tv = toTimeval(); int uSecs = tv.tv_usec; @@ -109,60 +111,60 @@ std::string base::Time::toString(base::Time::Resolution resolution, const std::s break; default: throw std::invalid_argument( - "base::Time::toString(): invalid " + "Time::toString(): invalid " "value in switch-statement"); } return std::string(buffer); } -double base::Time::toSeconds() const +double Time::toSeconds() const { return static_cast(microseconds) / UsecPerSec; } -int64_t base::Time::toMilliseconds() const +int64_t Time::toMilliseconds() const { return microseconds / 1000; } -int64_t base::Time::toMicroseconds() const +int64_t Time::toMicroseconds() const { return microseconds; } -base::Time base::Time::fromMicroseconds(uint64_t value) +Time Time::fromMicroseconds(uint64_t value) { return Time(value); } -base::Time base::Time::fromMilliseconds(uint64_t value) +Time Time::fromMilliseconds(uint64_t value) { return Time(value * 1000); } -base::Time base::Time::fromSeconds(int64_t value) +Time Time::fromSeconds(int64_t value) { return Time(value * UsecPerSec); } -base::Time base::Time::fromSeconds(int value) +Time Time::fromSeconds(int value) { return Time(static_cast(value) * UsecPerSec); } -base::Time base::Time::fromSeconds(int64_t value, int microseconds) +Time Time::fromSeconds(int64_t value, int microseconds) { return Time(value * UsecPerSec + static_cast(microseconds)); } -base::Time base::Time::fromSeconds(double value) +Time Time::fromSeconds(double value) { int64_t seconds = value; return Time(seconds * UsecPerSec + static_cast(round((value - seconds) * UsecPerSec))); } -base::Time base::Time::fromTimeValues(int year, int month, int day, int hour, int minute, int seconds, int millis, int micros) +Time Time::fromTimeValues(int year, int month, int day, int hour, int minute, int seconds, int millis, int micros) { struct tm timeobj; timeobj.tm_year = year - 1900; @@ -185,7 +187,7 @@ base::Time base::Time::fromTimeValues(int year, int month, int day, int hour, in return Time(timeVal); } -base::Time base::Time::fromString(const std::string& stringTime, base::Time::Resolution resolution, const std::string& mainFormat) +Time Time::fromString(const std::string& stringTime, Time::Resolution resolution, const std::string& mainFormat) { std::string mainTime = stringTime; int32_t usecs = 0; @@ -199,7 +201,7 @@ base::Time base::Time::fromString(const std::string& stringTime, base::Time::Res // string matches resolutions } else { - throw std::runtime_error("base::Time::fromString failed - resolution does not match provided Time-String"); + throw std::runtime_error("Time::fromString failed - resolution does not match provided Time-String"); } switch(resolution) @@ -213,11 +215,11 @@ base::Time base::Time::fromString(const std::string& stringTime, base::Time::Res break; case Seconds: throw std::invalid_argument( - "base::Time::fromString(); " + "Time::fromString(); " "'Seconds' is an invalid case " "here"); default: - throw std::invalid_argument("base::Time::fromString(): " + throw std::invalid_argument("Time::fromString(): " "invalid value in " "switch-statement"); } @@ -226,7 +228,7 @@ base::Time base::Time::fromString(const std::string& stringTime, base::Time::Res struct tm tm; if(NULL == strptime(mainTime.c_str(), mainFormat.c_str(), &tm)) { - throw std::runtime_error("base::Time::fromString failed- Time-String '" + mainTime + "' did not match the given format '" + mainFormat +"'"); + throw std::runtime_error("Time::fromString failed- Time-String '" + mainTime + "' did not match the given format '" + mainFormat +"'"); } // " ... not set by strptime(); tells mktime() to determine // whether daylight saving time is in effect ..." @@ -239,7 +241,7 @@ base::Time base::Time::fromString(const std::string& stringTime, base::Time::Res } -std::ostream& base::operator<<(std::ostream& io, const base::Time& time) +std::ostream& operator<<(std::ostream& io, const Time& time) { const int64_t microsecs = time.toMicroseconds(); @@ -253,18 +255,4 @@ std::ostream& base::operator<<(std::ostream& io, const base::Time& time) } - - - - - - - - - - - - - - - +} //end namespace base diff --git a/src/TimeMark.cpp b/src/TimeMark.cpp index f37c4b00..f83b4677 100644 --- a/src/TimeMark.cpp +++ b/src/TimeMark.cpp @@ -1,25 +1,27 @@ #include "TimeMark.hpp" -base::TimeMark::TimeMark(const std::string& label) : label(label), mark( Time::now() ), clock( ::clock() ) +namespace base { + +TimeMark::TimeMark(const std::string& label) : label(label), mark( Time::now() ), clock( ::clock() ) { } -base::Time base::TimeMark::passed() const +Time TimeMark::passed() const { return (Time::now() - mark); } -clock_t base::TimeMark::cycles() const +clock_t TimeMark::cycles() const { return ::clock() - clock; } -std::ostream& base::operator<<(std::ostream& stream, const base::TimeMark& ob) +std::ostream& operator<<(std::ostream& stream, const TimeMark& ob) { stream << ob.cycles() << "cyc (" << ob.passed() << "s) since " << ob.label; return stream; } - +} //end namespace base diff --git a/src/Timeout.cpp b/src/Timeout.cpp index 9ad8b39b..30f2747a 100644 --- a/src/Timeout.cpp +++ b/src/Timeout.cpp @@ -1,25 +1,27 @@ #include "Timeout.hpp" -base::Timeout::Timeout(base::Time timeout) : timeout(timeout) +namespace base { + +Timeout::Timeout(Time timeout) : timeout(timeout) { - start_time = base::Time::now(); + start_time = Time::now(); } -void base::Timeout::restart() +void Timeout::restart() { - start_time = base::Time::now(); + start_time = Time::now(); } -bool base::Timeout::elapsed() const +bool Timeout::elapsed() const { return elapsed(timeout); } -bool base::Timeout::elapsed(const base::Time& timeout) const +bool Timeout::elapsed(const Time& timeout) const { if(!timeout.isNull()) { - return start_time + timeout < base::Time::now(); + return start_time + timeout < Time::now(); } else { @@ -27,24 +29,24 @@ bool base::Timeout::elapsed(const base::Time& timeout) const } } -base::Time base::Timeout::timeLeft() const +Time Timeout::timeLeft() const { return timeLeft(timeout); } -base::Time base::Timeout::timeLeft(const base::Time& timeout) const +Time Timeout::timeLeft(const Time& timeout) const { if(!timeout.isNull()) { - return start_time + timeout - base::Time::now(); + return start_time + timeout - Time::now(); } else { - return base::Time::fromSeconds(0); + return Time::fromSeconds(0); } } - +} //end namespace base diff --git a/src/Trajectory.cpp b/src/Trajectory.cpp index e91f7d21..dabb907c 100644 --- a/src/Trajectory.cpp +++ b/src/Trajectory.cpp @@ -1,12 +1,15 @@ #include "Trajectory.hpp" -base::Trajectory::Trajectory() : speed(0) +namespace base { + +Trajectory::Trajectory() : speed(0) { } -bool base::Trajectory::driveForward() const +bool Trajectory::driveForward() const { return speed >= 0; } +} //end namespace base \ No newline at end of file diff --git a/src/TransformWithCovariance.cpp b/src/TransformWithCovariance.cpp index 69c73311..0691deca 100644 --- a/src/TransformWithCovariance.cpp +++ b/src/TransformWithCovariance.cpp @@ -9,51 +9,53 @@ #include -base::TransformWithCovariance::TransformWithCovariance() - : translation(base::Position::Zero()) , orientation(base::Quaterniond::Identity()) +namespace base { + +TransformWithCovariance::TransformWithCovariance() + : translation(Position::Zero()) , orientation(Quaterniond::Identity()) { this->invalidateCovariance(); } -base::TransformWithCovariance::TransformWithCovariance(const base::Affine3d& trans) +TransformWithCovariance::TransformWithCovariance(const Affine3d& trans) { this->setTransform(trans); this->invalidateCovariance(); } -base::TransformWithCovariance::TransformWithCovariance(const base::Affine3d& trans, const base::TransformWithCovariance::Covariance& cov) +TransformWithCovariance::TransformWithCovariance(const Affine3d& trans, const TransformWithCovariance::Covariance& cov) { this->setTransform(trans); this->cov = cov; } -base::TransformWithCovariance::TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation) +TransformWithCovariance::TransformWithCovariance(const Position& translation, const Quaterniond& orientation) : translation(translation), orientation(orientation) { this->invalidateCovariance(); } -base::TransformWithCovariance::TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation, const base::TransformWithCovariance::Covariance& cov) +TransformWithCovariance::TransformWithCovariance(const Position& translation, const Quaterniond& orientation, const TransformWithCovariance::Covariance& cov) : translation(translation), orientation(orientation), cov(cov) { } -base::TransformWithCovariance base::TransformWithCovariance::Identity() +TransformWithCovariance TransformWithCovariance::Identity() { return TransformWithCovariance(); } -base::TransformWithCovariance base::TransformWithCovariance::composition(const base::TransformWithCovariance& trans) const +TransformWithCovariance TransformWithCovariance::composition(const TransformWithCovariance& trans) const { return this->operator*( trans ); } -base::TransformWithCovariance base::TransformWithCovariance::compositionInv(const base::TransformWithCovariance& trans) const +TransformWithCovariance TransformWithCovariance::compositionInv(const TransformWithCovariance& trans) const { const TransformWithCovariance &tf(*this); const TransformWithCovariance &t1(trans); - base::Position p2(tf.translation + (tf.orientation * t1.inverse().translation)); + Position p2(tf.translation + (tf.orientation * t1.inverse().translation)); Eigen::Quaterniond q2( tf.orientation * t1.orientation.inverse()); // short path if there is no uncertainty @@ -83,11 +85,11 @@ base::TransformWithCovariance base::TransformWithCovariance::compositionInv(cons return TransformWithCovariance( p2, q2, cov ); } -base::TransformWithCovariance base::TransformWithCovariance::preCompositionInv(const base::TransformWithCovariance& trans) const +TransformWithCovariance TransformWithCovariance::preCompositionInv(const TransformWithCovariance& trans) const { const TransformWithCovariance &tf(*this); const TransformWithCovariance &t2(trans); - base::Position p1(t2.inverse().translation + (t2.orientation.inverse() * tf.translation)); + Position p1(t2.inverse().translation + (t2.orientation.inverse() * tf.translation)); Eigen::Quaterniond q1(t2.orientation.inverse() * tf.orientation); // short path if there is no uncertainty @@ -118,13 +120,13 @@ base::TransformWithCovariance base::TransformWithCovariance::preCompositionInv(c } -base::TransformWithCovariance base::TransformWithCovariance::operator*(const base::TransformWithCovariance& trans) const +TransformWithCovariance TransformWithCovariance::operator*(const TransformWithCovariance& trans) const { const TransformWithCovariance &t2(*this); const TransformWithCovariance &t1(trans); - const base::Quaterniond t(t2.orientation * t1.orientation); - const base::Position p(t2.translation + (t2.orientation * t1.translation)); + const Quaterniond t(t2.orientation * t1.orientation); + const Position p(t2.translation + (t2.orientation * t1.translation)); // short path if there is no uncertainty if( !t1.hasValidCovariance() && !t2.hasValidCovariance() ) @@ -163,11 +165,11 @@ base::TransformWithCovariance base::TransformWithCovariance::operator*(const bas return TransformWithCovariance(p, t, cov); } -base::TransformWithCovariance base::TransformWithCovariance::inverse() const +TransformWithCovariance TransformWithCovariance::inverse() const { // short path if there is no uncertainty if( !hasValidCovariance() ) - return TransformWithCovariance(static_cast(-(this->orientation.inverse() * this->translation)), this->orientation.inverse()); + return TransformWithCovariance(static_cast(-(this->orientation.inverse() * this->translation)), this->orientation.inverse()); Eigen::Quaterniond q(this->orientation); Eigen::Vector3d t(this->translation); @@ -175,106 +177,106 @@ base::TransformWithCovariance base::TransformWithCovariance::inverse() const J << q.toRotationMatrix().transpose(), drx_by_dr( q.inverse(), t ), Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Identity(); - return TransformWithCovariance(static_cast(-(this->orientation.inverse() * this->translation)), + return TransformWithCovariance(static_cast(-(this->orientation.inverse() * this->translation)), this->orientation.inverse(), J*this->getCovariance()*J.transpose()); } -const base::TransformWithCovariance::Covariance& base::TransformWithCovariance::getCovariance() const +const TransformWithCovariance::Covariance& TransformWithCovariance::getCovariance() const { return this->cov; } -void base::TransformWithCovariance::setCovariance(const base::TransformWithCovariance::Covariance& cov) +void TransformWithCovariance::setCovariance(const TransformWithCovariance::Covariance& cov) { this->cov = cov; } -const base::Matrix3d base::TransformWithCovariance::getTranslationCov() const +const Matrix3d TransformWithCovariance::getTranslationCov() const { return this->cov.topLeftCorner<3,3>(); } -void base::TransformWithCovariance::setTranslationCov(const base::Matrix3d& cov) +void TransformWithCovariance::setTranslationCov(const Matrix3d& cov) { this->cov.topLeftCorner<3,3>() = cov; } -const base::Matrix3d base::TransformWithCovariance::getOrientationCov() const +const Matrix3d TransformWithCovariance::getOrientationCov() const { return this->cov.bottomRightCorner<3,3>(); } -void base::TransformWithCovariance::setOrientationCov(const base::Matrix3d& cov) +void TransformWithCovariance::setOrientationCov(const Matrix3d& cov) { this->cov.bottomRightCorner<3,3>() = cov; } -const base::Affine3d base::TransformWithCovariance::getTransform() const +const Affine3d TransformWithCovariance::getTransform() const { - base::Affine3d trans (this->orientation); + Affine3d trans (this->orientation); trans.translation() = this->translation; return trans; } -void base::TransformWithCovariance::setTransform(const base::Affine3d& trans) +void TransformWithCovariance::setTransform(const Affine3d& trans) { this->translation = trans.translation(); - this->orientation = base::Quaterniond(trans.rotation()); + this->orientation = Quaterniond(trans.rotation()); } -const base::Orientation base::TransformWithCovariance::getOrientation() const +const Orientation TransformWithCovariance::getOrientation() const { - return base::Orientation(this->orientation); + return Orientation(this->orientation); } -void base::TransformWithCovariance::setOrientation(const base::Orientation& q) +void TransformWithCovariance::setOrientation(const Orientation& q) { - this->orientation = base::Quaterniond(q); + this->orientation = Quaterniond(q); } -bool base::TransformWithCovariance::hasValidTransform() const +bool TransformWithCovariance::hasValidTransform() const { return !translation.hasNaN() && !orientation.coeffs().hasNaN(); } -void base::TransformWithCovariance::invalidateTransform() +void TransformWithCovariance::invalidateTransform() { - translation = base::Position::Ones() * base::NaN(); - orientation.coeffs() = base::Vector4d::Ones() * base::NaN(); + translation = Position::Ones() * NaN(); + orientation.coeffs() = Vector4d::Ones() * NaN(); } -bool base::TransformWithCovariance::hasValidCovariance() const +bool TransformWithCovariance::hasValidCovariance() const { return !cov.hasNaN(); } -void base::TransformWithCovariance::invalidateCovariance() +void TransformWithCovariance::invalidateCovariance() { - cov = Covariance::Ones() * base::NaN(); + cov = Covariance::Ones() * NaN(); } -Eigen::Quaterniond base::TransformWithCovariance::r_to_q(const Eigen::Vector3d& r) +Eigen::Quaterniond TransformWithCovariance::r_to_q(const Eigen::Vector3d& r) { double theta = r.norm(); if( fabs(theta) > 1e-5 ) - return Eigen::Quaterniond( base::AngleAxisd( theta, r/theta ) ); + return Eigen::Quaterniond( AngleAxisd( theta, r/theta ) ); else return Eigen::Quaterniond::Identity(); } -Eigen::Vector3d base::TransformWithCovariance::q_to_r(const Eigen::Quaterniond& q) +Eigen::Vector3d TransformWithCovariance::q_to_r(const Eigen::Quaterniond& q) { - base::AngleAxisd aa( q ); + AngleAxisd aa( q ); return aa.axis() * aa.angle(); } -double base::TransformWithCovariance::sign(double v) +double TransformWithCovariance::sign(double v) { return v > 0.0 ? 1.0 : -1.0; } -Eigen::Matrix< double, int(3), int(3) > base::TransformWithCovariance::skew_symmetric(const Eigen::Vector3d& r) +Eigen::Matrix< double, int(3), int(3) > TransformWithCovariance::skew_symmetric(const Eigen::Vector3d& r) { Eigen::Matrix3d res; res << 0, -r.z(), r.y(), @@ -283,7 +285,7 @@ Eigen::Matrix< double, int(3), int(3) > base::TransformWithCovariance::skew_symm return res; } -Eigen::Matrix< double, int(4), int(3) > base::TransformWithCovariance::dq_by_dr(const Eigen::Quaterniond& q) +Eigen::Matrix< double, int(4), int(3) > TransformWithCovariance::dq_by_dr(const Eigen::Quaterniond& q) { const Eigen::Vector3d r( q_to_r( q ) ); @@ -297,7 +299,7 @@ Eigen::Matrix< double, int(4), int(3) > base::TransformWithCovariance::dq_by_dr( return res; } -Eigen::Matrix< double, int(3), int(4) > base::TransformWithCovariance::dr_by_dq(const Eigen::Quaterniond& q) +Eigen::Matrix< double, int(3), int(4) > TransformWithCovariance::dr_by_dq(const Eigen::Quaterniond& q) { const Eigen::Vector3d r( q_to_r( q ) ); const double mu = q.vec().norm(); @@ -310,7 +312,7 @@ Eigen::Matrix< double, int(3), int(4) > base::TransformWithCovariance::dr_by_dq( return res; } -Eigen::Matrix< double, int(4), int(4) > base::TransformWithCovariance::dq2q1_by_dq1(const Eigen::Quaterniond& q2) +Eigen::Matrix< double, int(4), int(4) > TransformWithCovariance::dq2q1_by_dq1(const Eigen::Quaterniond& q2) { Eigen::Matrix res; res << 0, -q2.vec().transpose(), @@ -318,7 +320,7 @@ Eigen::Matrix< double, int(4), int(4) > base::TransformWithCovariance::dq2q1_by_ return Eigen::Matrix::Identity() * q2.w() + res; } -Eigen::Matrix< double, int(4), int(4) > base::TransformWithCovariance::dq2q1_by_dq2(const Eigen::Quaterniond& q1) +Eigen::Matrix< double, int(4), int(4) > TransformWithCovariance::dq2q1_by_dq2(const Eigen::Quaterniond& q1) { Eigen::Matrix res; res << 0, -q1.vec().transpose(), @@ -326,7 +328,7 @@ Eigen::Matrix< double, int(4), int(4) > base::TransformWithCovariance::dq2q1_by_ return Eigen::Matrix::Identity() * q1.w() + res; } -Eigen::Matrix< double, int(3), int(3) > base::TransformWithCovariance::dr2r1_by_r1(const Eigen::Quaterniond& q, const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2) +Eigen::Matrix< double, int(3), int(3) > TransformWithCovariance::dr2r1_by_r1(const Eigen::Quaterniond& q, const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2) { return Eigen::Matrix3d( dr_by_dq( q ) @@ -334,7 +336,7 @@ Eigen::Matrix< double, int(3), int(3) > base::TransformWithCovariance::dr2r1_by_ * dq_by_dr( q1 ) ); } -Eigen::Matrix< double, int(3), int(3) > base::TransformWithCovariance::dr2r1_by_r2(const Eigen::Quaterniond& q, const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2) +Eigen::Matrix< double, int(3), int(3) > TransformWithCovariance::dr2r1_by_r2(const Eigen::Quaterniond& q, const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2) { return Eigen::Matrix3d( dr_by_dq( q ) @@ -342,7 +344,7 @@ Eigen::Matrix< double, int(3), int(3) > base::TransformWithCovariance::dr2r1_by_ * dq_by_dr( q2 ) ); } -Eigen::Matrix< double, int(3), int(3) > base::TransformWithCovariance::drx_by_dr(const Eigen::Quaterniond& q, const Eigen::Vector3d& x) +Eigen::Matrix< double, int(3), int(3) > TransformWithCovariance::drx_by_dr(const Eigen::Quaterniond& q, const Eigen::Vector3d& x) { const Eigen::Vector3d r( q_to_r( q ) ); const double theta = r.norm(); @@ -358,11 +360,11 @@ Eigen::Matrix< double, int(3), int(3) > base::TransformWithCovariance::drx_by_dr + 2.0*beta*Eigen::Matrix3d::Identity()) ); } -std::ostream& base::operator<<(std::ostream& out, const base::TransformWithCovariance& trans) +std::ostream& operator<<(std::ostream& out, const TransformWithCovariance& trans) { /** cout the 6D pose vector (translation and scaled axis orientation) with its associated covariance matrix **/ - base::Vector3d scaled_axis; - base::AngleAxisd angle_axis (trans.orientation); + Vector3d scaled_axis; + AngleAxisd angle_axis (trans.orientation); scaled_axis = angle_axis.axis() * angle_axis.angle(); for (register unsigned short i=0; i +namespace base { + /** * Guarantee Symmetric (semi-) Positive Definite (SPD) matrix. * The input matrix must be symmetric already (only the lower triangular part will be considered) @@ -34,154 +36,154 @@ static typename _MatrixType::PlainObject guaranteeSPD (const _MatrixType &A, con }; -typedef base::TwistWithCovariance::Covariance Covariance; +typedef TwistWithCovariance::Covariance Covariance; -base::TwistWithCovariance::TwistWithCovariance(const base::Vector3d& vel, const base::Vector3d& rot) +TwistWithCovariance::TwistWithCovariance(const Vector3d& vel, const Vector3d& rot) : vel(vel), rot(rot) { this->invalidateCovariance(); } -base::TwistWithCovariance::TwistWithCovariance(const base::Vector3d& vel, const base::Vector3d& rot, const base::TwistWithCovariance::Covariance& cov) +TwistWithCovariance::TwistWithCovariance(const Vector3d& vel, const Vector3d& rot, const TwistWithCovariance::Covariance& cov) : vel(vel), rot(rot), cov(cov) { } -base::TwistWithCovariance::TwistWithCovariance(const base::Vector6d& velocity, const base::TwistWithCovariance::Covariance& cov) +TwistWithCovariance::TwistWithCovariance(const Vector6d& velocity, const TwistWithCovariance::Covariance& cov) { this->setVelocity(velocity); this->cov = cov; } -const base::Vector3d& base::TwistWithCovariance::getTranslation() const +const Vector3d& TwistWithCovariance::getTranslation() const { return this->vel; } -void base::TwistWithCovariance::setTranslation(const base::Vector3d& vel) +void TwistWithCovariance::setTranslation(const Vector3d& vel) { this->vel = vel; } -const base::Vector3d& base::TwistWithCovariance::getRotation() const +const Vector3d& TwistWithCovariance::getRotation() const { return this->rot; } -void base::TwistWithCovariance::setRotation(const base::Vector3d& rot) +void TwistWithCovariance::setRotation(const Vector3d& rot) { this->rot = rot; } -const base::TwistWithCovariance::Covariance& base::TwistWithCovariance::getCovariance() const +const TwistWithCovariance::Covariance& TwistWithCovariance::getCovariance() const { return this->cov; } -void base::TwistWithCovariance::setCovariance(const base::TwistWithCovariance::Covariance& cov) +void TwistWithCovariance::setCovariance(const TwistWithCovariance::Covariance& cov) { this->cov = cov; } -const base::Matrix3d base::TwistWithCovariance::getLinearVelocityCov() const +const Matrix3d TwistWithCovariance::getLinearVelocityCov() const { return this->cov.block<3,3>(0,0); } -void base::TwistWithCovariance::setLinearVelocityCov(const base::Matrix3d& cov) +void TwistWithCovariance::setLinearVelocityCov(const Matrix3d& cov) { this->cov.block<3,3>(0,0) = cov; } -const base::Matrix3d base::TwistWithCovariance::getAngularVelocityCov() const +const Matrix3d TwistWithCovariance::getAngularVelocityCov() const { return this->cov.block<3,3>(3,3); } -void base::TwistWithCovariance::setAngularVelocityCov(const base::Matrix3d& cov) +void TwistWithCovariance::setAngularVelocityCov(const Matrix3d& cov) { this->cov.block<3,3>(3,3) = cov; } -const base::Vector3d& base::TwistWithCovariance::getLinearVelocity() const +const Vector3d& TwistWithCovariance::getLinearVelocity() const { return this->getTranslation(); } -void base::TwistWithCovariance::setLinearVelocity(const base::Vector3d& vel) +void TwistWithCovariance::setLinearVelocity(const Vector3d& vel) { return this->setTranslation(vel); } -const base::Vector3d& base::TwistWithCovariance::getAngularVelocity() const +const Vector3d& TwistWithCovariance::getAngularVelocity() const { return this->getRotation(); } -void base::TwistWithCovariance::setAngularVelocity(const base::Vector3d& rot) +void TwistWithCovariance::setAngularVelocity(const Vector3d& rot) { return this->setRotation(rot); } -const base::Vector3d& base::TwistWithCovariance::translation() const +const Vector3d& TwistWithCovariance::translation() const { return this->getTranslation(); } -const base::Vector3d& base::TwistWithCovariance::rotation() const +const Vector3d& TwistWithCovariance::rotation() const { return this->getRotation(); } -const base::Vector6d base::TwistWithCovariance::getVelocity() const +const Vector6d TwistWithCovariance::getVelocity() const { - base::Vector6d all_velocities; + Vector6d all_velocities; all_velocities.block<3,1>(0,0) = this->vel; all_velocities.block<3,1>(3,0) = this->rot; return all_velocities; } -void base::TwistWithCovariance::setVelocity(const base::Vector6d& velocity) +void TwistWithCovariance::setVelocity(const Vector6d& velocity) { /** Linear velocity at first place, Angular velocity at second place **/ this->vel = velocity.block<3,1>(0,0); this->rot = velocity.block<3,1>(3,0); } -bool base::TwistWithCovariance::hasValidVelocity() const +bool TwistWithCovariance::hasValidVelocity() const { return this->vel.allFinite() && this->rot.allFinite(); } -void base::TwistWithCovariance::invalidateVelocity() +void TwistWithCovariance::invalidateVelocity() { - this->vel = base::Vector3d::Ones() * base::unknown(); - this->rot = base::Vector3d::Ones() * base::unknown(); + this->vel = Vector3d::Ones() * unknown(); + this->rot = Vector3d::Ones() * unknown(); } -bool base::TwistWithCovariance::hasValidCovariance() const +bool TwistWithCovariance::hasValidCovariance() const { return this->cov.allFinite(); } -void base::TwistWithCovariance::invalidateCovariance() +void TwistWithCovariance::invalidateCovariance() { - this->cov = Covariance::Ones() * base::unknown(); + this->cov = Covariance::Ones() * unknown(); } -void base::TwistWithCovariance::invalidate() +void TwistWithCovariance::invalidate() { this->invalidateVelocity(); this->invalidateCovariance(); } -base::TwistWithCovariance base::TwistWithCovariance::Zero() +TwistWithCovariance TwistWithCovariance::Zero() { - return TwistWithCovariance(static_cast(base::Vector3d::Zero()), static_cast(base::Vector3d::Zero())); + return TwistWithCovariance(static_cast(Vector3d::Zero()), static_cast(Vector3d::Zero())); } -double& base::TwistWithCovariance::operator[](int i) +double& TwistWithCovariance::operator[](int i) { if (i<3) return this->vel(i); @@ -189,7 +191,7 @@ double& base::TwistWithCovariance::operator[](int i) return this->rot(i-3); } -double base::TwistWithCovariance::operator[](int i) const +double TwistWithCovariance::operator[](int i) const { if (i<3) return this->vel(i); @@ -197,7 +199,7 @@ double base::TwistWithCovariance::operator[](int i) const return this->rot(i-3); } -base::TwistWithCovariance& base::TwistWithCovariance::operator+=(const base::TwistWithCovariance& arg) +TwistWithCovariance& TwistWithCovariance::operator+=(const TwistWithCovariance& arg) { this->vel += arg.vel; this->rot += arg.rot; @@ -209,12 +211,12 @@ base::TwistWithCovariance& base::TwistWithCovariance::operator+=(const base::Twi return *this; } -base::TwistWithCovariance operator+(base::TwistWithCovariance lhs, const base::TwistWithCovariance& rhs) +TwistWithCovariance operator+(TwistWithCovariance lhs, const TwistWithCovariance& rhs) { return lhs += rhs; } -base::TwistWithCovariance& base::TwistWithCovariance::operator-=(const base::TwistWithCovariance& arg) +TwistWithCovariance& TwistWithCovariance::operator-=(const TwistWithCovariance& arg) { this->vel -= arg.vel; this->rot -= arg.rot; @@ -227,38 +229,38 @@ base::TwistWithCovariance& base::TwistWithCovariance::operator-=(const base::Twi return *this; } -base::TwistWithCovariance operator-(base::TwistWithCovariance lhs, const base::TwistWithCovariance& rhs) +TwistWithCovariance operator-(TwistWithCovariance lhs, const TwistWithCovariance& rhs) { return lhs -= rhs; } -base::TwistWithCovariance operator*(const base::TwistWithCovariance& lhs, double rhs) +TwistWithCovariance operator*(const TwistWithCovariance& lhs, double rhs) { if (!lhs.hasValidCovariance()) { - return base::TwistWithCovariance(static_cast(lhs.vel*rhs), static_cast(lhs.rot*rhs)); + return TwistWithCovariance(static_cast(lhs.vel*rhs), static_cast(lhs.rot*rhs)); } else { - return base::TwistWithCovariance(static_cast(lhs.vel*rhs), static_cast(lhs.rot*rhs), static_cast((rhs*rhs)*lhs.cov)); + return TwistWithCovariance(static_cast(lhs.vel*rhs), static_cast(lhs.rot*rhs), static_cast((rhs*rhs)*lhs.cov)); } } -base::TwistWithCovariance operator*(double lhs, const base::TwistWithCovariance& rhs) +TwistWithCovariance operator*(double lhs, const TwistWithCovariance& rhs) { if (!rhs.hasValidCovariance()) { - return base::TwistWithCovariance(static_cast(lhs*rhs.vel), static_cast(lhs*rhs.rot)); + return TwistWithCovariance(static_cast(lhs*rhs.vel), static_cast(lhs*rhs.rot)); } else { - return base::TwistWithCovariance(static_cast(lhs*rhs.vel), static_cast(lhs*rhs.rot), static_cast((lhs*lhs)*rhs.cov)); + return TwistWithCovariance(static_cast(lhs*rhs.vel), static_cast(lhs*rhs.rot), static_cast((lhs*lhs)*rhs.cov)); } } -base::TwistWithCovariance operator*(const base::TwistWithCovariance& lhs, const base::TwistWithCovariance& rhs) +TwistWithCovariance operator*(const TwistWithCovariance& lhs, const TwistWithCovariance& rhs) { - base::TwistWithCovariance tmp; + TwistWithCovariance tmp; tmp.vel = lhs.rot.cross(rhs.vel)+lhs.vel.cross(rhs.rot); tmp.rot = lhs.rot.cross(rhs.rot); @@ -271,23 +273,23 @@ base::TwistWithCovariance operator*(const base::TwistWithCovariance& lhs, const /** Initialize covariance **/ tmp.cov.setZero(); - cross_jacob = base::TwistWithCovariance::crossJacobian(lhs.rot, rhs.vel); - cross_cov << lhs.cov.block<3,3>(3,3), base::Matrix3d::Zero(), - base::Matrix3d::Zero(), rhs.cov.block<3,3>(0,0); + cross_jacob = TwistWithCovariance::crossJacobian(lhs.rot, rhs.vel); + cross_cov << lhs.cov.block<3,3>(3,3), Matrix3d::Zero(), + Matrix3d::Zero(), rhs.cov.block<3,3>(0,0); /** Linear velocity is at the first covariance block **/ tmp.cov.block<3,3>(0,0) = cross_jacob * cross_cov * cross_jacob.transpose(); - cross_jacob = base::TwistWithCovariance::crossJacobian(lhs.vel, rhs.rot); - cross_cov << lhs.cov.block<3,3>(0,0), base::Matrix3d::Zero(), - base::Matrix3d::Zero(),rhs.cov.block<3,3>(3,3); + cross_jacob = TwistWithCovariance::crossJacobian(lhs.vel, rhs.rot); + cross_cov << lhs.cov.block<3,3>(0,0), Matrix3d::Zero(), + Matrix3d::Zero(),rhs.cov.block<3,3>(3,3); /** Linear velocity is at the first covariance block **/ tmp.cov.block<3,3>(0,0) += cross_jacob * cross_cov * cross_jacob.transpose(); - cross_jacob = base::TwistWithCovariance::crossJacobian(lhs.rot, rhs.rot); - cross_cov << lhs.cov.block<3,3>(3,3), base::Matrix3d::Zero(), - base::Matrix3d::Zero(),rhs.cov.block<3,3>(3,3); + cross_jacob = TwistWithCovariance::crossJacobian(lhs.rot, rhs.rot); + cross_cov << lhs.cov.block<3,3>(3,3), Matrix3d::Zero(), + Matrix3d::Zero(),rhs.cov.block<3,3>(3,3); /** Angular velocity is at the first covariance block **/ tmp.cov.block<3,3>(3,3) = cross_jacob * cross_cov * cross_jacob.transpose(); @@ -298,27 +300,27 @@ base::TwistWithCovariance operator*(const base::TwistWithCovariance& lhs, const return tmp; } -base::TwistWithCovariance operator/(const base::TwistWithCovariance& lhs, double rhs) +TwistWithCovariance operator/(const TwistWithCovariance& lhs, double rhs) { - return base::TwistWithCovariance(static_cast(lhs.vel/rhs), static_cast(lhs.rot/rhs), static_cast((1.0/(rhs *rhs))*lhs.cov)); + return TwistWithCovariance(static_cast(lhs.vel/rhs), static_cast(lhs.rot/rhs), static_cast((1.0/(rhs *rhs))*lhs.cov)); } -base::TwistWithCovariance operator-(const base::TwistWithCovariance& arg) +TwistWithCovariance operator-(const TwistWithCovariance& arg) { - return base::TwistWithCovariance(static_cast(-arg.vel), static_cast(-arg.rot), arg.cov); + return TwistWithCovariance(static_cast(-arg.vel), static_cast(-arg.rot), arg.cov); } -Eigen::Matrix< double, int(3), int(6) > base::TwistWithCovariance::crossJacobian(const base::Vector3d& u, const base::Vector3d& v) +Eigen::Matrix< double, int(3), int(6) > TwistWithCovariance::crossJacobian(const Vector3d& u, const Vector3d& v) { Eigen::Matrix cross_jacob; - base::Matrix3d cross_u, cross_v; + Matrix3d cross_u, cross_v; cross_u << 0.0, -u[2], u[1], u[2], 0.0, -u[0], -u[1], u[0], 0.0; cross_v << 0.0, -v[2], v[1], v[2], 0.0, -v[0], -v[1], v[0], 0.0; cross_jacob << cross_u, cross_v; return cross_jacob; } -std::ostream& base::operator<<(std::ostream& out, const base::TwistWithCovariance& twist) +std::ostream& operator<<(std::ostream& out, const TwistWithCovariance& twist) { /** cout the 6D twist vector (rotational first and linear second) with its associated covariance matrix **/ for (register unsigned short i=0; i #include -base::samples::BodyState::BodyState(bool doInvalidation) +namespace base { namespace samples { + +BodyState::BodyState(bool doInvalidation) { if(doInvalidation) invalidate(); } -void base::samples::BodyState::setPose(const base::Affine3d& pose) +void BodyState::setPose(const Affine3d& pose) { this->pose.setTransform(pose); } -const base::Affine3d base::samples::BodyState::getPose() const +const Affine3d BodyState::getPose() const { return this->pose.getTransform(); } -double base::samples::BodyState::getYaw() const +double BodyState::getYaw() const { return base::getYaw(this->pose.orientation); } -double base::samples::BodyState::getPitch() const +double BodyState::getPitch() const { return base::getPitch(this->pose.orientation); } -double base::samples::BodyState::getRoll() const +double BodyState::getRoll() const { return base::getRoll(this->pose.orientation); } -const base::Position& base::samples::BodyState::position() const +const Position& BodyState::position() const { return this->pose.translation; } -base::Position& base::samples::BodyState::position() +Position& BodyState::position() { return this->pose.translation; } -const base::Quaterniond& base::samples::BodyState::orientation() const +const Quaterniond& BodyState::orientation() const { return this->pose.orientation; } -base::Quaterniond& base::samples::BodyState::orientation() +Quaterniond& BodyState::orientation() { return this->pose.orientation; } -const base::Vector3d& base::samples::BodyState::linear_velocity() const +const Vector3d& BodyState::linear_velocity() const { return this->velocity.vel; } -const base::Vector3d& base::samples::BodyState::angular_velocity() const +const Vector3d& BodyState::angular_velocity() const { return this->velocity.rot; } -base::Position& base::samples::BodyState::linear_velocity() +Position& BodyState::linear_velocity() { return this->velocity.vel; } -base::Position& base::samples::BodyState::angular_velocity() +Position& BodyState::angular_velocity() { return this->velocity.rot; } -const base::Matrix6d& base::samples::BodyState::cov_pose() const +const Matrix6d& BodyState::cov_pose() const { return this->pose.cov; } -base::Matrix6d& base::samples::BodyState::cov_pose() +Matrix6d& BodyState::cov_pose() { return this->pose.cov; } -const base::Matrix3d base::samples::BodyState::cov_orientation() const +const Matrix3d BodyState::cov_orientation() const { return this->pose.getOrientationCov(); } -void base::samples::BodyState::cov_orientation(const base::Matrix3d& cov) +void BodyState::cov_orientation(const Matrix3d& cov) { return this->pose.setOrientationCov(cov); } -const base::Matrix3d base::samples::BodyState::cov_position() const +const Matrix3d BodyState::cov_position() const { return this->pose.getTranslationCov(); } -void base::samples::BodyState::cov_position(const base::Matrix3d& cov) +void BodyState::cov_position(const Matrix3d& cov) { return this->pose.setTranslationCov(cov); } -const base::Matrix6d& base::samples::BodyState::cov_velocity() const +const Matrix6d& BodyState::cov_velocity() const { return this->velocity.cov; } -base::Matrix6d& base::samples::BodyState::cov_velocity() +Matrix6d& BodyState::cov_velocity() { return this->velocity.cov; } -const base::Matrix3d base::samples::BodyState::cov_linear_velocity() const +const Matrix3d BodyState::cov_linear_velocity() const { return this->velocity.getLinearVelocityCov(); } -void base::samples::BodyState::cov_linear_velocity(const base::Matrix3d& cov) +void BodyState::cov_linear_velocity(const Matrix3d& cov) { return this->velocity.setLinearVelocityCov(cov); } -const base::Matrix3d base::samples::BodyState::cov_angular_velocity() const +const Matrix3d BodyState::cov_angular_velocity() const { return this->velocity.getAngularVelocityCov(); } -void base::samples::BodyState::cov_angular_velocity(const base::Matrix3d& cov) +void BodyState::cov_angular_velocity(const Matrix3d& cov) { return this->velocity.setAngularVelocityCov(cov); } -base::samples::BodyState base::samples::BodyState::Unknown() +BodyState BodyState::Unknown() { BodyState result(false); result.initUnknown(); return result; } -base::samples::BodyState base::samples::BodyState::Invalid() +BodyState BodyState::Invalid() { BodyState result(true); return result; } -void base::samples::BodyState::initSane() +void BodyState::initSane() { invalidate(); } -void base::samples::BodyState::invalidate() +void BodyState::invalidate() { invalidatePose(); invalidatePoseCovariance(); @@ -160,67 +162,67 @@ void base::samples::BodyState::invalidate() invalidateVelocityCovariance(); } -void base::samples::BodyState::initUnknown() +void BodyState::initUnknown() { - this->pose.setTransform(base::Affine3d::Identity()); + this->pose.setTransform(Affine3d::Identity()); this->pose.invalidateCovariance(); - this->velocity.setVelocity(base::Vector6d::Zero()); + this->velocity.setVelocity(Vector6d::Zero()); this->velocity.invalidateCovariance(); } -bool base::samples::BodyState::hasValidPose() const +bool BodyState::hasValidPose() const { return this->pose.hasValidTransform(); } -bool base::samples::BodyState::hasValidPoseCovariance() const +bool BodyState::hasValidPoseCovariance() const { return this->pose.hasValidCovariance(); } -void base::samples::BodyState::invalidatePose() +void BodyState::invalidatePose() { this->pose.invalidateTransform(); } -void base::samples::BodyState::invalidatePoseCovariance() +void BodyState::invalidatePoseCovariance() { this->pose.invalidateCovariance(); } -bool base::samples::BodyState::hasValidVelocity() const +bool BodyState::hasValidVelocity() const { return this->velocity.hasValidVelocity(); } -bool base::samples::BodyState::hasValidVelocityCovariance() const +bool BodyState::hasValidVelocityCovariance() const { return this->velocity.hasValidCovariance(); } -void base::samples::BodyState::invalidateVelocity() +void BodyState::invalidateVelocity() { this->velocity.invalidateVelocity(); } -void base::samples::BodyState::invalidateVelocityCovariance() +void BodyState::invalidateVelocityCovariance() { this->velocity.invalidateCovariance(); } -void base::samples::BodyState::invalidateValues(bool pose, bool velocity) +void BodyState::invalidateValues(bool pose, bool velocity) { if (pose) this->invalidatePose(); if (velocity) this->invalidateVelocity(); } -void base::samples::BodyState::invalidateCovariances(bool pose, bool velocity) +void BodyState::invalidateCovariances(bool pose, bool velocity) { if (pose) this->invalidatePoseCovariance(); if (velocity) this->invalidateVelocityCovariance(); } -base::samples::BodyState& base::samples::BodyState::operator=(const base::samples::RigidBodyState& rbs) +BodyState& BodyState::operator=(const RigidBodyState& rbs) { /** extract the transformation **/ this->pose.setTransform(rbs.getTransform()); @@ -240,12 +242,12 @@ base::samples::BodyState& base::samples::BodyState::operator=(const base::sample return *this; } -base::samples::BodyState base::samples::BodyState::composition(const base::samples::BodyState& bs) const +BodyState BodyState::composition(const BodyState& bs) const { return this->operator*( bs ); } -base::samples::BodyState base::samples::BodyState::operator*(const base::samples::BodyState& bs) const +BodyState BodyState::operator*(const BodyState& bs) const { const BodyState &bs2(*this); const BodyState &bs1(bs); @@ -256,8 +258,8 @@ base::samples::BodyState base::samples::BodyState::operator*(const base::samples * state (here bs1) is assumed to be the current "instantaneous" * body state velocity and of the resulting body state. **/ - base::TransformWithCovariance result_pose (static_cast(bs2.pose * bs1.pose)); - base::TwistWithCovariance result_velocity (bs1.velocity); + TransformWithCovariance result_pose (static_cast(bs2.pose * bs1.pose)); + TwistWithCovariance result_velocity (bs1.velocity); /** Resulting velocity and covariance with respect to the pose base frame **/ result_velocity.vel = result_pose.orientation * result_velocity.vel; @@ -277,13 +279,14 @@ base::samples::BodyState base::samples::BodyState::operator*(const base::samples return BodyState(result_pose, result_velocity); } -std::ostream& base::samples::operator<<(std::ostream& out, const base::samples::BodyState& bs) +std::ostream& operator<<(std::ostream& out, const BodyState& bs) { out << bs.pose << "\n"; out << bs.velocity << "\n"; return out; } +}} //end namespace base::samples diff --git a/src/samples/DepthMap.cpp b/src/samples/DepthMap.cpp index 4294ada6..82409bf0 100644 --- a/src/samples/DepthMap.cpp +++ b/src/samples/DepthMap.cpp @@ -2,7 +2,9 @@ #include -void base::samples::DepthMap::reset() +namespace base { namespace samples { + +void DepthMap::reset() { timestamps.clear(); vertical_projection = POLAR; @@ -15,17 +17,17 @@ void base::samples::DepthMap::reset() remissions.clear(); } -base::samples::DepthMap::DepthMatrixMap base::samples::DepthMap::getDistanceMatrixMap() +DepthMap::DepthMatrixMap DepthMap::getDistanceMatrixMap() { return (DepthMatrixMap(distances.data(), vertical_size, horizontal_size)); } -base::samples::DepthMap::DepthMatrixMapConst base::samples::DepthMap::getDistanceMatrixMapConst() const +DepthMap::DepthMatrixMapConst DepthMap::getDistanceMatrixMapConst() const { return (DepthMatrixMapConst(distances.data(), vertical_size, horizontal_size)); } -base::samples::DepthMap::DEPTH_MEASUREMENT_STATE base::samples::DepthMap::getIndexState(std::size_t index) const +DepthMap::DEPTH_MEASUREMENT_STATE DepthMap::getIndexState(std::size_t index) const { if(index >= distances.size()) throw std::out_of_range("Invalid measurement index given"); @@ -33,7 +35,7 @@ base::samples::DepthMap::DEPTH_MEASUREMENT_STATE base::samples::DepthMap::getInd return getMeasurementState(distances[index]); } -base::samples::DepthMap::DEPTH_MEASUREMENT_STATE base::samples::DepthMap::getMeasurementState(base::samples::DepthMap::uint32_t v_index, base::samples::DepthMap::uint32_t h_index) const +DepthMap::DEPTH_MEASUREMENT_STATE DepthMap::getMeasurementState(DepthMap::uint32_t v_index, DepthMap::uint32_t h_index) const { if(!checkSizeConfig()) throw std::out_of_range("Vertical and horizontal size does not match the distance array size."); @@ -43,11 +45,11 @@ base::samples::DepthMap::DEPTH_MEASUREMENT_STATE base::samples::DepthMap::getMea return getMeasurementState(distances[getIndex(v_index,h_index)]); } -base::samples::DepthMap::DEPTH_MEASUREMENT_STATE base::samples::DepthMap::getMeasurementState(base::samples::DepthMap::scalar distance) const +DepthMap::DEPTH_MEASUREMENT_STATE DepthMap::getMeasurementState(DepthMap::scalar distance) const { - if(base::isNaN(distance)) + if(isNaN(distance)) return MEASUREMENT_ERROR; - else if(base::isInfinity(distance)) + else if(isInfinity(distance)) return TOO_FAR; else if(distance <= 0.0) return TOO_NEAR; @@ -55,7 +57,7 @@ base::samples::DepthMap::DEPTH_MEASUREMENT_STATE base::samples::DepthMap::getMea return VALID_MEASUREMENT; } -bool base::samples::DepthMap::isIndexValid(std::size_t index) const +bool DepthMap::isIndexValid(std::size_t index) const { if(index >= distances.size()) throw std::out_of_range("Invalid measurement index given"); @@ -63,7 +65,7 @@ bool base::samples::DepthMap::isIndexValid(std::size_t index) const return isMeasurementValid(distances[index]); } -bool base::samples::DepthMap::isMeasurementValid(base::samples::DepthMap::uint32_t v_index, base::samples::DepthMap::uint32_t h_index) const +bool DepthMap::isMeasurementValid(DepthMap::uint32_t v_index, DepthMap::uint32_t h_index) const { if(!checkSizeConfig()) throw std::out_of_range("Vertical and horizontal size does not match the distance array size."); @@ -73,19 +75,20 @@ bool base::samples::DepthMap::isMeasurementValid(base::samples::DepthMap::uint32 return isMeasurementValid(distances[getIndex(v_index,h_index)]); } -bool base::samples::DepthMap::isMeasurementValid(base::samples::DepthMap::scalar distance) const +bool DepthMap::isMeasurementValid(DepthMap::scalar distance) const { return getMeasurementState(distance) == VALID_MEASUREMENT; } -std::size_t base::samples::DepthMap::getIndex(base::samples::DepthMap::uint32_t v_index, base::samples::DepthMap::uint32_t h_index) const +std::size_t DepthMap::getIndex(DepthMap::uint32_t v_index, DepthMap::uint32_t h_index) const { return ((size_t)v_index * (size_t)horizontal_size + (size_t)h_index); } -bool base::samples::DepthMap::checkSizeConfig() const +bool DepthMap::checkSizeConfig() const { return ((((size_t)vertical_size * (size_t)horizontal_size) == distances.size()) ? true : false); } +}} //end namespace base::samples diff --git a/src/samples/DistanceImage.cpp b/src/samples/DistanceImage.cpp index e534adca..4bf7f8dd 100644 --- a/src/samples/DistanceImage.cpp +++ b/src/samples/DistanceImage.cpp @@ -3,14 +3,16 @@ #include #include -void base::samples::DistanceImage::clear() +namespace base { namespace samples { + +void DistanceImage::clear() { std::fill( data.begin(), data.end(), std::numeric_limits::quiet_NaN() ); } -base::samples::Pointcloud base::samples::DistanceImage::getPointCloud() const +Pointcloud DistanceImage::getPointCloud() const { - base::samples::Pointcloud pointCloud; + Pointcloud pointCloud; Eigen::Vector3d point; for(size_t y = 0; y < height ; y++) { @@ -22,7 +24,7 @@ base::samples::Pointcloud base::samples::DistanceImage::getPointCloud() const return pointCloud; } -void base::samples::DistanceImage::setIntrinsic(double f_x, double f_y, double c_x, double c_y) +void DistanceImage::setIntrinsic(double f_x, double f_y, double c_x, double c_y) { scale_x = 1.0 / f_x; scale_y = 1.0 / f_y; @@ -30,9 +32,11 @@ void base::samples::DistanceImage::setIntrinsic(double f_x, double f_y, double c center_y = -c_y / f_y; } -void base::samples::DistanceImage::setSize(double width, double height) +void DistanceImage::setSize(double width, double height) { this->width = width; this->height = height; data.resize( width * height ); } + +}} //end namespace base::samples \ No newline at end of file diff --git a/src/samples/Frame.cpp b/src/samples/Frame.cpp index f61bf347..b946591d 100644 --- a/src/samples/Frame.cpp +++ b/src/samples/Frame.cpp @@ -6,41 +6,43 @@ #include #include -void base::samples::frame::frame_attrib_t::set(const std::string& name, const std::string& data) +namespace base { namespace samples { + +void frame::frame_attrib_t::set(const std::string& name, const std::string& data) { name_ = name; data_ = data; } -bool base::samples::frame::frame_size_t::operator==(const base::samples::frame::frame_size_t& other) const +bool frame::frame_size_t::operator==(const frame::frame_size_t& other) const { if(width == other.width && height==other.height) return true; return false; } -bool base::samples::frame::frame_size_t::operator!=(const base::samples::frame::frame_size_t& other) const +bool frame::frame_size_t::operator!=(const frame::frame_size_t& other) const { return !(*this == other); } -base::samples::frame::Frame::Frame() : image(), size(), data_depth(0), pixel_size(0), frame_mode() +frame::Frame::Frame() : image(), size(), data_depth(0), pixel_size(0), frame_mode() { setDataDepth(0); reset(); } -base::samples::frame::Frame::Frame(uint16_t width, uint16_t height, uint8_t depth, base::samples::frame::frame_mode_t mode, const uint8_t val, size_t sizeInBytes) +frame::Frame::Frame(uint16_t width, uint16_t height, uint8_t depth, frame::frame_mode_t mode, const uint8_t val, size_t sizeInBytes) { init(width,height,depth,mode,val,sizeInBytes); } -base::samples::frame::Frame::Frame(const base::samples::frame::Frame& other, bool bcopy) +frame::Frame::Frame(const frame::Frame& other, bool bcopy) { init(other,bcopy); } -void base::samples::frame::Frame::copyImageIndependantAttributes(const base::samples::frame::Frame& other) +void frame::Frame::copyImageIndependantAttributes(const frame::Frame& other) { std::vector::const_iterator iter = other.attributes.begin(); for(;iter!= other.attributes.end();++iter) @@ -50,7 +52,7 @@ void base::samples::frame::Frame::copyImageIndependantAttributes(const base::sam frame_status = other.frame_status; } -void base::samples::frame::Frame::init(const base::samples::frame::Frame& other, bool bcopy) +void frame::Frame::init(const frame::Frame& other, bool bcopy) { //hdr is copied by attributes = other.attributes; init(other.getWidth(),other.getHeight(),other.getDataDepth(), other.getFrameMode(),-1,other.getNumberOfBytes()); @@ -59,7 +61,7 @@ void base::samples::frame::Frame::init(const base::samples::frame::Frame& other, copyImageIndependantAttributes(other); } -void base::samples::frame::Frame::init(uint16_t width, uint16_t height, uint8_t depth, base::samples::frame::frame_mode_t mode, const uint8_t val, size_t sizeInBytes) +void frame::Frame::init(uint16_t width, uint16_t height, uint8_t depth, frame::frame_mode_t mode, const uint8_t val, size_t sizeInBytes) { //change size if the frame does not fit if(this->size.height != height || this->size.width != width || this->frame_mode != mode || @@ -83,9 +85,9 @@ void base::samples::frame::Frame::init(uint16_t width, uint16_t height, uint8_t reset(val); } -void base::samples::frame::Frame::reset(const int val) +void frame::Frame::reset(const int val) { - this->time = base::Time(); + this->time = Time(); if (this->image.size() > 0 && val >= 0) { memset(&this->image[0], val%256, this->image.size()); } @@ -93,15 +95,15 @@ void base::samples::frame::Frame::reset(const int val) attributes.clear(); } -void base::samples::frame::Frame::swap(base::samples::frame::Frame& frame) +void frame::Frame::swap(frame::Frame& frame) { //swap vector image.swap(frame.image); attributes.swap(frame.attributes); //copy values to temp - base::Time temp_time = frame.time; - base::Time temp_received_time = frame.received_time; + Time temp_time = frame.time; + Time temp_received_time = frame.received_time; frame_size_t temp_size = frame.size; uint32_t temp_data_depth = frame.data_depth; uint32_t temp_pixel_size = frame.pixel_size; @@ -129,52 +131,52 @@ void base::samples::frame::Frame::swap(base::samples::frame::Frame& frame) frame_status = temp_frame_status; } -bool base::samples::frame::Frame::isHDR() const +bool frame::Frame::isHDR() const { return (hasAttribute("hdr")&&getAttribute("hdr")); } -void base::samples::frame::Frame::setHDR(bool value) +void frame::Frame::setHDR(bool value) { setAttribute("hdr",true); } -bool base::samples::frame::Frame::isCompressed() const +bool frame::Frame::isCompressed() const { return frame_mode >= COMPRESSED_MODES; } -bool base::samples::frame::Frame::isGrayscale() const +bool frame::Frame::isGrayscale() const { return this->frame_mode == MODE_GRAYSCALE; } -bool base::samples::frame::Frame::isRGB() const +bool frame::Frame::isRGB() const { return this->frame_mode == MODE_RGB; } -bool base::samples::frame::Frame::isBayer() const +bool frame::Frame::isBayer() const { return (this->frame_mode == MODE_BAYER || this->frame_mode == MODE_BAYER_RGGB || this->frame_mode == MODE_BAYER_GRBG || this->frame_mode == MODE_BAYER_BGGR || this->frame_mode == MODE_BAYER_GBRG); } -void base::samples::frame::Frame::setStatus(const base::samples::frame::frame_status_t value) +void frame::Frame::setStatus(const frame::frame_status_t value) { frame_status = value; } -base::samples::frame::frame_status_t base::samples::frame::Frame::getStatus() const +frame::frame_status_t frame::Frame::getStatus() const { return frame_status; } -uint32_t base::samples::frame::Frame::getChannelCount() const +uint32_t frame::Frame::getChannelCount() const { return getChannelCount(this->frame_mode); } -uint32_t base::samples::frame::Frame::getChannelCount(base::samples::frame::frame_mode_t mode) +uint32_t frame::Frame::getChannelCount(frame::frame_mode_t mode) { switch (mode) { @@ -203,7 +205,7 @@ uint32_t base::samples::frame::Frame::getChannelCount(base::samples::frame::fram } } -base::samples::frame::frame_mode_t base::samples::frame::Frame::toFrameMode(const std::string& str) +frame::frame_mode_t frame::Frame::toFrameMode(const std::string& str) { if(str == "MODE_UNDEFINED") return MODE_UNDEFINED; @@ -241,39 +243,39 @@ base::samples::frame::frame_mode_t base::samples::frame::Frame::toFrameMode(cons return MODE_UNDEFINED; } -base::samples::frame::frame_mode_t base::samples::frame::Frame::getFrameMode() const +frame::frame_mode_t frame::Frame::getFrameMode() const { return this->frame_mode; } -uint32_t base::samples::frame::Frame::getPixelSize() const +uint32_t frame::Frame::getPixelSize() const { return this->pixel_size; } -uint32_t base::samples::frame::Frame::getRowSize() const +uint32_t frame::Frame::getRowSize() const { if(isCompressed()) throw std::runtime_error("Frame::getRowSize: There is no raw size for an compressed image!"); return this->row_size; } -uint32_t base::samples::frame::Frame::getNumberOfBytes() const +uint32_t frame::Frame::getNumberOfBytes() const { return image.size(); } -uint32_t base::samples::frame::Frame::getPixelCount() const +uint32_t frame::Frame::getPixelCount() const { return size.width * size.height; } -uint32_t base::samples::frame::Frame::getDataDepth() const +uint32_t frame::Frame::getDataDepth() const { return this->data_depth; } -void base::samples::frame::Frame::setDataDepth(uint32_t value) +void frame::Frame::setDataDepth(uint32_t value) { this->data_depth = value; @@ -288,7 +290,7 @@ void base::samples::frame::Frame::setDataDepth(uint32_t value) this->row_size = this->pixel_size * getWidth(); } -void base::samples::frame::Frame::setFrameMode(base::samples::frame::frame_mode_t mode) +void frame::Frame::setFrameMode(frame::frame_mode_t mode) { this->frame_mode = mode; @@ -303,27 +305,27 @@ void base::samples::frame::Frame::setFrameMode(base::samples::frame::frame_mode_ this->row_size = this->pixel_size * getWidth(); } -base::samples::frame::frame_size_t base::samples::frame::Frame::getSize() const +frame::frame_size_t frame::Frame::getSize() const { return this->size; } -uint16_t base::samples::frame::Frame::getWidth() const +uint16_t frame::Frame::getWidth() const { return this->size.width; } -uint16_t base::samples::frame::Frame::getHeight() const +uint16_t frame::Frame::getHeight() const { return this->size.height; } -const std::vector< uint8_t >& base::samples::frame::Frame::getImage() const +const std::vector< uint8_t >& frame::Frame::getImage() const { return this->image; } -void base::samples::frame::Frame::validateImageSize(size_t sizeToValidate) const +void frame::Frame::validateImageSize(size_t sizeToValidate) const { size_t expected_size = getPixelSize()*getPixelCount(); if (!isCompressed() && sizeToValidate != expected_size){ @@ -336,45 +338,45 @@ void base::samples::frame::Frame::validateImageSize(size_t sizeToValidate) const } } -void base::samples::frame::Frame::setImage(const std::vector< uint8_t >& newImage) +void frame::Frame::setImage(const std::vector< uint8_t >& newImage) { // calling the overloading function wich uses the "char*" interface return setImage(newImage.data(), newImage.size()); } -void base::samples::frame::Frame::setImage(const char* data, size_t newImageSize) +void frame::Frame::setImage(const char* data, size_t newImageSize) { return setImage(reinterpret_cast(data), newImageSize); } -void base::samples::frame::Frame::setImage(const uint8_t* data, size_t newImageSize) +void frame::Frame::setImage(const uint8_t* data, size_t newImageSize) { validateImageSize(newImageSize); image.resize(newImageSize); memcpy(&this->image[0], data, newImageSize); } -uint8_t* base::samples::frame::Frame::getImagePtr() +uint8_t* frame::Frame::getImagePtr() { return static_cast(image.data()); } -const uint8_t* base::samples::frame::Frame::getImageConstPtr() const +const uint8_t* frame::Frame::getImageConstPtr() const { return static_cast(image.data()); } -uint8_t* base::samples::frame::Frame::getLastByte() +uint8_t* frame::Frame::getLastByte() { return static_cast(&this->image.back()); } -const uint8_t* base::samples::frame::Frame::getLastConstByte() const +const uint8_t* frame::Frame::getLastConstByte() const { return static_cast(&this->image.back()); } -bool base::samples::frame::Frame::hasAttribute(const std::string& name) const +bool frame::Frame::hasAttribute(const std::string& name) const { std::vector::const_iterator _iter = attributes.begin(); for (;_iter != attributes.end();_iter++) @@ -385,7 +387,7 @@ bool base::samples::frame::Frame::hasAttribute(const std::string& name) const return false; } -bool base::samples::frame::Frame::deleteAttribute(const std::string& name) +bool frame::Frame::deleteAttribute(const std::string& name) { std::vector::iterator _iter = attributes.begin(); for (;_iter != attributes.end();_iter++) @@ -399,7 +401,7 @@ bool base::samples::frame::Frame::deleteAttribute(const std::string& name) return false; } - +}} //end namespace base::samples diff --git a/src/samples/Joints.cpp b/src/samples/Joints.cpp index 5989c023..53cf1697 100644 --- a/src/samples/Joints.cpp +++ b/src/samples/Joints.cpp @@ -1,6 +1,8 @@ #include "Joints.hpp" -base::samples::Joints base::samples::Joints::Positions(const std::vector< double >& positions) +namespace base { namespace samples { + +Joints Joints::Positions(const std::vector< double >& positions) { Joints result; result.elements.resize(positions.size()); @@ -9,7 +11,7 @@ base::samples::Joints base::samples::Joints::Positions(const std::vector< double return result; } -base::samples::Joints base::samples::Joints::Positions(const std::vector< double >& positions, const std::vector< std::string >& names) +Joints Joints::Positions(const std::vector< double >& positions, const std::vector< std::string >& names) { Joints result = Positions(positions); if (result.elements.size() != names.size()) @@ -18,7 +20,7 @@ base::samples::Joints base::samples::Joints::Positions(const std::vector< double return result; } -base::samples::Joints base::samples::Joints::Speeds(const std::vector< float >& speeds) +Joints Joints::Speeds(const std::vector< float >& speeds) { Joints result; result.elements.resize(speeds.size()); @@ -27,7 +29,7 @@ base::samples::Joints base::samples::Joints::Speeds(const std::vector< float >& return result; } -base::samples::Joints base::samples::Joints::Speeds(const std::vector< float >& speeds, const std::vector< std::string >& names) +Joints Joints::Speeds(const std::vector< float >& speeds, const std::vector< std::string >& names) { Joints result = Speeds(speeds); if (result.elements.size() != names.size()) @@ -36,7 +38,7 @@ base::samples::Joints base::samples::Joints::Speeds(const std::vector< float >& return result; } -base::samples::Joints base::samples::Joints::Efforts(const std::vector< float >& efforts) +Joints Joints::Efforts(const std::vector< float >& efforts) { Joints result; result.elements.resize(efforts.size()); @@ -45,7 +47,7 @@ base::samples::Joints base::samples::Joints::Efforts(const std::vector< float >& return result; } -base::samples::Joints base::samples::Joints::Efforts(const std::vector< float >& efforts, const std::vector< std::string >& names) +Joints Joints::Efforts(const std::vector< float >& efforts, const std::vector< std::string >& names) { Joints result = Efforts(efforts); if (result.elements.size() != names.size()) @@ -54,7 +56,7 @@ base::samples::Joints base::samples::Joints::Efforts(const std::vector< float >& return result; } -base::samples::Joints base::samples::Joints::Raw(const std::vector< float >& raw) +Joints Joints::Raw(const std::vector< float >& raw) { Joints result; result.elements.resize(raw.size()); @@ -63,7 +65,7 @@ base::samples::Joints base::samples::Joints::Raw(const std::vector< float >& raw return result; } -base::samples::Joints base::samples::Joints::Raw(const std::vector< float >& raw, const std::vector< std::string >& names) +Joints Joints::Raw(const std::vector< float >& raw, const std::vector< std::string >& names) { Joints result = Raw(raw); if (result.elements.size() != names.size()) @@ -72,7 +74,7 @@ base::samples::Joints base::samples::Joints::Raw(const std::vector< float >& raw return result; } -base::samples::Joints base::samples::Joints::Accelerations(const std::vector< float >& acceleration) +Joints Joints::Accelerations(const std::vector< float >& acceleration) { Joints result; result.elements.resize(acceleration.size()); @@ -81,7 +83,7 @@ base::samples::Joints base::samples::Joints::Accelerations(const std::vector< fl return result; } -base::samples::Joints base::samples::Joints::Accelerations(const std::vector< float >& acceleration, const std::vector< std::string >& names) +Joints Joints::Accelerations(const std::vector< float >& acceleration, const std::vector< std::string >& names) { Joints result = Accelerations(acceleration); if (result.elements.size() != names.size()) @@ -90,5 +92,6 @@ base::samples::Joints base::samples::Joints::Accelerations(const std::vector< fl return result; } +}} //end namespace base::samples diff --git a/src/samples/LaserScan.cpp b/src/samples/LaserScan.cpp index badfa8d2..d98d6df4 100644 --- a/src/samples/LaserScan.cpp +++ b/src/samples/LaserScan.cpp @@ -2,14 +2,16 @@ #include -bool base::samples::LaserScan::isValidBeam(const unsigned int i) const +namespace base { namespace samples { + +bool LaserScan::isValidBeam(const unsigned int i) const { if(i > ranges.size()) throw std::out_of_range("Invalid beam index given"); return isRangeValid(ranges[i]); } -void base::samples::LaserScan::reset() +void LaserScan::reset() { speed = 0.0; start_angle = 0.0; @@ -19,14 +21,14 @@ void base::samples::LaserScan::reset() remission.clear(); } -bool base::samples::LaserScan::isRangeValid(base::samples::LaserScan::uint32_t range) const +bool LaserScan::isRangeValid(LaserScan::uint32_t range) const { if(range >= minRange && range <= maxRange && range >= END_LASER_RANGE_ERRORS) return true; return false; } -bool base::samples::LaserScan::getPointFromScanBeamXForward(const unsigned int i, Eigen::Vector3d& point) const +bool LaserScan::getPointFromScanBeamXForward(const unsigned int i, Eigen::Vector3d& point) const { if(!isValidBeam(i)) return false; @@ -39,7 +41,7 @@ bool base::samples::LaserScan::getPointFromScanBeamXForward(const unsigned int i return true; } -bool base::samples::LaserScan::getPointFromScanBeam(const unsigned int i, Eigen::Vector3d& point) const +bool LaserScan::getPointFromScanBeam(const unsigned int i, Eigen::Vector3d& point) const { if(!isValidBeam(i)) return false; @@ -52,7 +54,7 @@ bool base::samples::LaserScan::getPointFromScanBeam(const unsigned int i, Eigen: return true; } -std::vector< Eigen::Vector3d > base::samples::LaserScan::convertScanToPointCloud(const Eigen::Affine3d& transform) const +std::vector< Eigen::Vector3d > LaserScan::convertScanToPointCloud(const Eigen::Affine3d& transform) const { std::vector pointCloud; pointCloud.reserve(ranges.size()); @@ -67,3 +69,5 @@ std::vector< Eigen::Vector3d > base::samples::LaserScan::convertScanToPointCloud return pointCloud; } + +}} //end namespace base::samples \ No newline at end of file diff --git a/src/samples/Pressure.cpp b/src/samples/Pressure.cpp index f01b5221..22ecf300 100644 --- a/src/samples/Pressure.cpp +++ b/src/samples/Pressure.cpp @@ -1,16 +1,20 @@ #include "Pressure.hpp" -base::samples::Pressure base::samples::Pressure::fromPascal(const base::Time& time, float pascal) +namespace base { namespace samples { + +Pressure Pressure::fromPascal(const Time& time, float pascal) { return Pressure(time, base::Pressure::fromPascal(pascal)); } -base::samples::Pressure base::samples::Pressure::fromBar(const base::Time& time, float bar) +Pressure Pressure::fromBar(const Time& time, float bar) { return Pressure(time, base::Pressure::fromBar(bar)); } -base::samples::Pressure base::samples::Pressure::fromPSI(const base::Time& time, float psi) +Pressure Pressure::fromPSI(const Time& time, float psi) { return Pressure(time, base::Pressure::fromPSI(psi)); } + +}} //end namespace base::samples \ No newline at end of file diff --git a/src/samples/RigidBodyAcceleration.cpp b/src/samples/RigidBodyAcceleration.cpp index 49de4f3f..08164281 100644 --- a/src/samples/RigidBodyAcceleration.cpp +++ b/src/samples/RigidBodyAcceleration.cpp @@ -1,7 +1,11 @@ #include "RigidBodyAcceleration.hpp" -void base::samples::RigidBodyAcceleration::invalidateOrientation() +namespace base { namespace samples { + +void RigidBodyAcceleration::invalidateOrientation() { cov_acceleration = Eigen::Matrix3d::Identity(); cov_acceleration *= INFINITY; } + +}} //end namespace base::samples \ No newline at end of file diff --git a/src/samples/RigidBodyState.cpp b/src/samples/RigidBodyState.cpp index 59524a1e..1668b6a4 100644 --- a/src/samples/RigidBodyState.cpp +++ b/src/samples/RigidBodyState.cpp @@ -1,18 +1,20 @@ #include "RigidBodyState.hpp" -base::samples::RigidBodyState::RigidBodyState(bool doInvalidation) +namespace base { namespace samples { + +RigidBodyState::RigidBodyState(bool doInvalidation) { if(doInvalidation) invalidate(); } -void base::samples::RigidBodyState::setTransform(const Eigen::Affine3d& transform) +void RigidBodyState::setTransform(const Eigen::Affine3d& transform) { position = transform.translation(); orientation = Eigen::Quaterniond( transform.linear() ); } -Eigen::Affine3d base::samples::RigidBodyState::getTransform() const +Eigen::Affine3d RigidBodyState::getTransform() const { Eigen::Affine3d ret; ret.setIdentity(); @@ -21,51 +23,51 @@ Eigen::Affine3d base::samples::RigidBodyState::getTransform() const return ret; } -void base::samples::RigidBodyState::setPose(const base::Pose& pose) +void RigidBodyState::setPose(const Pose& pose) { orientation = pose.orientation; position = pose.position; } -base::Pose base::samples::RigidBodyState::getPose() const +Pose RigidBodyState::getPose() const { - return base::Pose( position, orientation ); + return Pose( position, orientation ); } -double base::samples::RigidBodyState::getYaw() const +double RigidBodyState::getYaw() const { return base::getYaw(orientation); } -double base::samples::RigidBodyState::getPitch() const +double RigidBodyState::getPitch() const { return base::getPitch(orientation); } -double base::samples::RigidBodyState::getRoll() const +double RigidBodyState::getRoll() const { return base::getRoll(orientation); } -base::samples::RigidBodyState base::samples::RigidBodyState::unknown() +RigidBodyState RigidBodyState::unknown() { RigidBodyState result(false); result.initUnknown(); return result; } -base::samples::RigidBodyState base::samples::RigidBodyState::invalid() +RigidBodyState RigidBodyState::invalid() { RigidBodyState result(true); return result; } -void base::samples::RigidBodyState::initSane() +void RigidBodyState::initSane() { invalidate(); } -void base::samples::RigidBodyState::invalidate() +void RigidBodyState::invalidate() { invalidateOrientation(); invalidateOrientationCovariance(); @@ -77,7 +79,7 @@ void base::samples::RigidBodyState::invalidate() invalidateAngularVelocityCovariance(); } -void base::samples::RigidBodyState::initUnknown() +void RigidBodyState::initUnknown() { position.setZero(); velocity.setZero(); @@ -89,169 +91,169 @@ void base::samples::RigidBodyState::initUnknown() cov_angular_velocity = setValueUnknown(); } -bool base::samples::RigidBodyState::isValidValue(const base::Vector3d& vec) +bool RigidBodyState::isValidValue(const Vector3d& vec) { - return !base::isNaN(vec(0)) && - !base::isNaN(vec(1)) && - !base::isNaN(vec(2)); + return !isNaN(vec(0)) && + !isNaN(vec(1)) && + !isNaN(vec(2)); } -bool base::samples::RigidBodyState::isValidValue(const base::Orientation& ori) +bool RigidBodyState::isValidValue(const Orientation& ori) { - return !base::isNaN(ori.w()) && - !base::isNaN(ori.x()) && - !base::isNaN(ori.y()) && - !base::isNaN(ori.z()) && + return !isNaN(ori.w()) && + !isNaN(ori.x()) && + !isNaN(ori.y()) && + !isNaN(ori.z()) && fabs(ori.squaredNorm()-1.0) < 1e-6; //assuming at least single precision } -bool base::samples::RigidBodyState::isKnownValue(const base::Matrix3d& cov) +bool RigidBodyState::isKnownValue(const Matrix3d& cov) { - return !base::isInfinity(cov(0,0)) && - !base::isInfinity(cov(1,1)) && - !base::isInfinity(cov(2,2)); + return !isInfinity(cov(0,0)) && + !isInfinity(cov(1,1)) && + !isInfinity(cov(2,2)); } -bool base::samples::RigidBodyState::isValidCovariance(const base::Matrix3d& cov) +bool RigidBodyState::isValidCovariance(const Matrix3d& cov) { for (int i = 0; i < 3; ++i) for (int j = 0; j < 3; ++j) - if (base::isNaN(cov(i, j))) + if (isNaN(cov(i, j))) return false; return true; } -bool base::samples::RigidBodyState::isValidValue(const base::Vector3d& vec, int dim) +bool RigidBodyState::isValidValue(const Vector3d& vec, int dim) { - return !base::isNaN(vec(dim)); + return !isNaN(vec(dim)); } -bool base::samples::RigidBodyState::isValidCovariance(const base::Matrix3d& cov, int dim) +bool RigidBodyState::isValidCovariance(const Matrix3d& cov, int dim) { - return !base::isNaN(cov(dim,dim)); + return !isNaN(cov(dim,dim)); } -bool base::samples::RigidBodyState::isKnownValue(const base::Matrix3d& cov, int dim) +bool RigidBodyState::isKnownValue(const Matrix3d& cov, int dim) { - return !base::isInfinity(cov(dim,dim)); + return !isInfinity(cov(dim,dim)); } -base::Vector3d base::samples::RigidBodyState::invalidValue() +Vector3d RigidBodyState::invalidValue() { - return base::Vector3d::Ones() * base::NaN(); + return Vector3d::Ones() * NaN(); } -base::Orientation base::samples::RigidBodyState::invalidOrientation() +Orientation RigidBodyState::invalidOrientation() { - return base::Orientation(Eigen::Vector4d::Ones() * base::NaN()); + return Orientation(Eigen::Vector4d::Ones() * NaN()); } -base::Matrix3d base::samples::RigidBodyState::setValueUnknown() +Matrix3d RigidBodyState::setValueUnknown() { - return base::Matrix3d::Ones() * base::infinity(); + return Matrix3d::Ones() * infinity(); } -base::Matrix3d base::samples::RigidBodyState::invalidCovariance() +Matrix3d RigidBodyState::invalidCovariance() { - return base::Matrix3d::Ones() * base::NaN(); + return Matrix3d::Ones() * NaN(); } -bool base::samples::RigidBodyState::hasValidPosition() const +bool RigidBodyState::hasValidPosition() const { return isValidValue(position); } -bool base::samples::RigidBodyState::hasValidPosition(int idx) const +bool RigidBodyState::hasValidPosition(int idx) const { return isValidValue(position, idx); } -bool base::samples::RigidBodyState::hasValidPositionCovariance() const +bool RigidBodyState::hasValidPositionCovariance() const { return isValidCovariance(cov_position); } -void base::samples::RigidBodyState::invalidatePosition() +void RigidBodyState::invalidatePosition() { position = invalidValue(); } -void base::samples::RigidBodyState::invalidatePositionCovariance() +void RigidBodyState::invalidatePositionCovariance() { cov_position = invalidCovariance(); } -bool base::samples::RigidBodyState::hasValidOrientation() const +bool RigidBodyState::hasValidOrientation() const { return isValidValue(orientation); } -bool base::samples::RigidBodyState::hasValidOrientationCovariance() const +bool RigidBodyState::hasValidOrientationCovariance() const { return isValidCovariance(cov_orientation); } -void base::samples::RigidBodyState::invalidateOrientation() +void RigidBodyState::invalidateOrientation() { orientation = invalidOrientation(); } -void base::samples::RigidBodyState::invalidateOrientationCovariance() +void RigidBodyState::invalidateOrientationCovariance() { cov_orientation = invalidCovariance(); } -bool base::samples::RigidBodyState::hasValidVelocity() const +bool RigidBodyState::hasValidVelocity() const { return isValidValue(velocity); } -bool base::samples::RigidBodyState::hasValidVelocity(int idx) const +bool RigidBodyState::hasValidVelocity(int idx) const { return isValidValue(velocity, idx); } -bool base::samples::RigidBodyState::hasValidVelocityCovariance() const +bool RigidBodyState::hasValidVelocityCovariance() const { return isValidCovariance(cov_velocity); } -void base::samples::RigidBodyState::invalidateVelocity() +void RigidBodyState::invalidateVelocity() { velocity = invalidValue(); } -void base::samples::RigidBodyState::invalidateVelocityCovariance() +void RigidBodyState::invalidateVelocityCovariance() { cov_velocity = invalidCovariance(); } -bool base::samples::RigidBodyState::hasValidAngularVelocity() const +bool RigidBodyState::hasValidAngularVelocity() const { return isValidValue(angular_velocity); } -bool base::samples::RigidBodyState::hasValidAngularVelocity(int idx) const +bool RigidBodyState::hasValidAngularVelocity(int idx) const { return isValidValue(angular_velocity, idx); } -bool base::samples::RigidBodyState::hasValidAngularVelocityCovariance() const +bool RigidBodyState::hasValidAngularVelocityCovariance() const { return isValidCovariance(cov_angular_velocity); } -void base::samples::RigidBodyState::invalidateAngularVelocity() +void RigidBodyState::invalidateAngularVelocity() { angular_velocity = invalidValue(); } -void base::samples::RigidBodyState::invalidateAngularVelocityCovariance() +void RigidBodyState::invalidateAngularVelocityCovariance() { cov_angular_velocity = invalidCovariance(); } -void base::samples::RigidBodyState::invalidateValues(bool invPos, bool invOri, bool invVel, bool invAngVel) +void RigidBodyState::invalidateValues(bool invPos, bool invOri, bool invVel, bool invAngVel) { if (invPos) invalidatePosition(); if (invOri) invalidateOrientation(); @@ -259,7 +261,7 @@ void base::samples::RigidBodyState::invalidateValues(bool invPos, bool invOri, b if (invAngVel) invalidateAngularVelocity(); } -void base::samples::RigidBodyState::invalidateCovariances(bool invPos, bool invOri, bool invVel, bool invAngVel) +void RigidBodyState::invalidateCovariances(bool invPos, bool invOri, bool invVel, bool invAngVel) { if (invPos) invalidatePositionCovariance(); if (invOri) invalidateOrientationCovariance(); @@ -267,7 +269,7 @@ void base::samples::RigidBodyState::invalidateCovariances(bool invPos, bool invO if (invAngVel) invalidateAngularVelocityCovariance(); } - +}} //end namespace base::samples diff --git a/src/samples/Sonar.cpp b/src/samples/Sonar.cpp index 20c6cb59..91c320a1 100644 --- a/src/samples/Sonar.cpp +++ b/src/samples/Sonar.cpp @@ -1,19 +1,21 @@ #include "Sonar.hpp" -void base::samples::Sonar::resize(int bin_count, int beam_count, bool per_beam_timestamps) +namespace base { namespace samples { + +void Sonar::resize(int bin_count, int beam_count, bool per_beam_timestamps) { if (per_beam_timestamps) timestamps.resize(beam_count); else timestamps.clear(); - bearings.resize(beam_count, base::Angle::unknown()); - bins.resize(beam_count * bin_count, base::unknown()); + bearings.resize(beam_count, Angle::unknown()); + bins.resize(beam_count * bin_count, unknown()); this->bin_count = bin_count; this->beam_count = beam_count; } -base::samples::Sonar base::samples::Sonar::fromSingleBeam(base::Time time, base::Time bin_duration, base::Angle beam_width, base::Angle beam_height, const std::vector< float >& bins, base::Angle bearing, float speed_of_sound) +Sonar Sonar::fromSingleBeam(Time time, Time bin_duration, Angle beam_width, Angle beam_height, const std::vector< float >& bins, Angle bearing, float speed_of_sound) { Sonar sample(time, bin_duration, bins.size(), beam_width, beam_height); sample.speed_of_sound = speed_of_sound; @@ -21,12 +23,12 @@ base::samples::Sonar base::samples::Sonar::fromSingleBeam(base::Time time, base: return sample; } -base::Time base::samples::Sonar::getBinRelativeStartTime(unsigned int bin_idx) const +Time Sonar::getBinRelativeStartTime(unsigned int bin_idx) const { return bin_duration * bin_idx; } -base::Time base::samples::Sonar::getBeamAcquisitionStartTime(unsigned int beam) const +Time Sonar::getBeamAcquisitionStartTime(unsigned int beam) const { if (timestamps.empty()) return time; @@ -34,25 +36,25 @@ base::Time base::samples::Sonar::getBeamAcquisitionStartTime(unsigned int beam) return timestamps[beam]; } -base::Time base::samples::Sonar::getBinTime(unsigned int bin, unsigned int beam) const +Time Sonar::getBinTime(unsigned int bin, unsigned int beam) const { return getBeamAcquisitionStartTime(beam) + getBinRelativeStartTime(bin); } -float base::samples::Sonar::getBinStartDistance(unsigned int bin) const +float Sonar::getBinStartDistance(unsigned int bin) const { return getBinRelativeStartTime(bin).toSeconds() * speed_of_sound; } -void base::samples::Sonar::setRegularBeamBearings(base::Angle start, base::Angle interval) +void Sonar::setRegularBeamBearings(Angle start, Angle interval) { - base::Angle angle(start); + Angle angle(start); bearings.resize(beam_count); for (uint32_t i = 0; i < beam_count; ++i, angle += interval) bearings[i] = angle; } -void base::samples::Sonar::pushBeam(const std::vector< float >& bins) +void Sonar::pushBeam(const std::vector< float >& bins) { if (!timestamps.empty()) throw std::invalid_argument("cannot call pushBeam(bins): the structure uses per-beam timestamps, use pushBeams(time, bins) instead"); @@ -60,25 +62,25 @@ void base::samples::Sonar::pushBeam(const std::vector< float >& bins) pushBeamBins(bins); } -void base::samples::Sonar::pushBeam(const std::vector< float >& bins, base::Angle bearing) +void Sonar::pushBeam(const std::vector< float >& bins, Angle bearing) { pushBeam(bins); bearings.push_back(bearing); } -void base::samples::Sonar::pushBeam(const base::Time& beam_time, const std::vector< float >& beam_bins) +void Sonar::pushBeam(const Time& beam_time, const std::vector< float >& beam_bins) { pushBeamBins(beam_bins); timestamps.push_back(beam_time); } -void base::samples::Sonar::pushBeam(const base::Time& beam_time, const std::vector< float >& beam_bins, base::Angle bearing) +void Sonar::pushBeam(const Time& beam_time, const std::vector< float >& beam_bins, Angle bearing) { pushBeam(beam_time, beam_bins); bearings.push_back(bearing); } -void base::samples::Sonar::pushBeamBins(const std::vector< float >& beam_bins) +void Sonar::pushBeamBins(const std::vector< float >& beam_bins) { if (beam_bins.size() != bin_count) throw std::invalid_argument("pushBeam: the provided beam does not match the expected bin_count"); @@ -86,7 +88,7 @@ void base::samples::Sonar::pushBeamBins(const std::vector< float >& beam_bins) beam_count++; } -void base::samples::Sonar::setBeam(unsigned int beam, const std::vector< float >& bins) +void Sonar::setBeam(unsigned int beam, const std::vector< float >& bins) { if (!timestamps.empty()) throw std::invalid_argument("cannot call setBeam(bins): the structure uses per-beam timestamps, use setBeams(time, bins) instead"); @@ -94,51 +96,51 @@ void base::samples::Sonar::setBeam(unsigned int beam, const std::vector< float > setBeamBins(beam, bins); } -void base::samples::Sonar::setBeam(unsigned int beam, const std::vector< float >& bins, base::Angle bearing) +void Sonar::setBeam(unsigned int beam, const std::vector< float >& bins, Angle bearing) { setBeam(beam, bins); bearings[beam] = bearing; } -void base::samples::Sonar::setBeam(unsigned int beam, const base::Time& beam_time, const std::vector< float >& beam_bins) +void Sonar::setBeam(unsigned int beam, const Time& beam_time, const std::vector< float >& beam_bins) { setBeamBins(beam, beam_bins); timestamps[beam] = beam_time; } -void base::samples::Sonar::setBeam(unsigned int beam, const base::Time& beam_time, const std::vector< float >& beam_bins, base::Angle bearing) +void Sonar::setBeam(unsigned int beam, const Time& beam_time, const std::vector< float >& beam_bins, Angle bearing) { setBeam(beam, beam_time, beam_bins); bearings[beam] = bearing; } -void base::samples::Sonar::setBeamBins(int beam, const std::vector< float >& beam_bins) +void Sonar::setBeamBins(int beam, const std::vector< float >& beam_bins) { if (beam_bins.size() != bin_count) throw std::invalid_argument("pushBeam: the provided beam does not match the expected bin_count"); std::copy(beam_bins.begin(), beam_bins.end(), bins.begin() + beam * bin_count); } -base::Angle base::samples::Sonar::getBeamBearing(unsigned int beam) const +Angle Sonar::getBeamBearing(unsigned int beam) const { return bearings[beam]; } -std::vector< float > base::samples::Sonar::getBeamBins(unsigned int beam) const +std::vector< float > Sonar::getBeamBins(unsigned int beam) const { std::vector bins; getBeamBins(beam, bins); return bins; } -void base::samples::Sonar::getBeamBins(unsigned int beam, std::vector< float >& beam_bins) const +void Sonar::getBeamBins(unsigned int beam, std::vector< float >& beam_bins) const { beam_bins.resize(bin_count); std::vector::const_iterator ptr = bins.begin() + beam * bin_count; std::copy(ptr, ptr + bin_count, beam_bins.begin()); } -base::samples::Sonar base::samples::Sonar::getBeam(unsigned int beam) const +Sonar Sonar::getBeam(unsigned int beam) const { return fromSingleBeam( getBeamAcquisitionStartTime(beam), @@ -150,7 +152,7 @@ base::samples::Sonar base::samples::Sonar::getBeam(unsigned int beam) const speed_of_sound); } -void base::samples::Sonar::validate() +void Sonar::validate() { if (bin_count * beam_count != bins.size()) throw std::logic_error("the number of elements in 'bins' does not match the bin and beam counts"); @@ -160,10 +162,10 @@ void base::samples::Sonar::validate() throw std::logic_error("the number of elements in 'bearings' does not match the beam count"); } -base::samples::Sonar::Sonar(SonarScan const& old, float gain) +Sonar::Sonar(SonarScan const& old, float gain) : time(old.time) , timestamps(old.time_beams) - , bin_duration(base::Time::fromSeconds(old.getSpatialResolution() / old.speed_of_sound)) + , bin_duration(Time::fromSeconds(old.getSpatialResolution() / old.speed_of_sound)) , beam_width(old.beamwidth_horizontal) , beam_height(old.beamwidth_vertical) , speed_of_sound(old.speed_of_sound) @@ -186,12 +188,12 @@ base::samples::Sonar::Sonar(SonarScan const& old, float gain) } -base::samples::Sonar::Sonar(SonarBeam const& old, float gain) +Sonar::Sonar(SonarBeam const& old, float gain) : time(old.time) , timestamps() - , bin_duration(base::Time::fromSeconds(old.sampling_interval / 2.0)) - , beam_width(base::Angle::fromRad(old.beamwidth_horizontal)) - , beam_height(base::Angle::fromRad(old.beamwidth_vertical)) + , bin_duration(Time::fromSeconds(old.sampling_interval / 2.0)) + , beam_width(Angle::fromRad(old.beamwidth_horizontal)) + , beam_height(Angle::fromRad(old.beamwidth_vertical)) , speed_of_sound(old.speed_of_sound) , bin_count(old.beam.size()) , beam_count(0) @@ -203,9 +205,9 @@ base::samples::Sonar::Sonar(SonarBeam const& old, float gain) pushBeam(bins, old.bearing); } -base::samples::SonarBeam base::samples::Sonar::toSonarBeam(float gain) +SonarBeam Sonar::toSonarBeam(float gain) { - base::samples::SonarBeam sonar_beam; + SonarBeam sonar_beam; sonar_beam.time = time; sonar_beam.speed_of_sound = speed_of_sound; sonar_beam.beamwidth_horizontal = beam_width.rad; @@ -226,9 +228,9 @@ base::samples::SonarBeam base::samples::Sonar::toSonarBeam(float gain) return sonar_beam; } -base::samples::SonarScan base::samples::Sonar::toSonarScan(float gain) +SonarScan Sonar::toSonarScan(float gain) { - base::samples::SonarScan sonar_scan; + SonarScan sonar_scan; sonar_scan.time = time; sonar_scan.time_beams = timestamps; sonar_scan.speed_of_sound = speed_of_sound; @@ -237,7 +239,7 @@ base::samples::SonarScan base::samples::Sonar::toSonarScan(float gain) sonar_scan.beamwidth_horizontal = beam_width; sonar_scan.beamwidth_vertical = beam_height; sonar_scan.start_bearing = bearings[0]; - sonar_scan.angular_resolution = base::Angle::fromRad(beam_width.rad / beam_count); + sonar_scan.angular_resolution = Angle::fromRad(beam_width.rad / beam_count); sonar_scan.memory_layout_column = false; sonar_scan.polar_coordinates = true; @@ -254,15 +256,4 @@ base::samples::SonarScan base::samples::Sonar::toSonarScan(float gain) return sonar_scan; } - - - - - - - - - - - - +}} //end namespace base::samples diff --git a/src/samples/SonarBeam.cpp b/src/samples/SonarBeam.cpp index c416993c..f8f5bf9f 100644 --- a/src/samples/SonarBeam.cpp +++ b/src/samples/SonarBeam.cpp @@ -3,12 +3,14 @@ #include #include -base::samples::SonarBeam::SonarBeam(const base::samples::SonarBeam& other) +namespace base { namespace samples { + +SonarBeam::SonarBeam(const SonarBeam& other) { init(other); } -double base::samples::SonarBeam::getSpatialResolution() const +double SonarBeam::getSpatialResolution() const { //the sampling interval includes the time for //the sound traveling from the transiter to the target an back @@ -16,13 +18,13 @@ double base::samples::SonarBeam::getSpatialResolution() const return sampling_interval*0.5*speed_of_sound; } -base::samples::SonarBeam& base::samples::SonarBeam::operator=(const base::samples::SonarBeam& other) +SonarBeam& SonarBeam::operator=(const SonarBeam& other) { init(other); return *this; } -void base::samples::SonarBeam::init(const base::samples::SonarBeam& other) +void SonarBeam::init(const SonarBeam& other) { time = other.time; bearing = other.bearing; @@ -33,7 +35,7 @@ void base::samples::SonarBeam::init(const base::samples::SonarBeam& other) beam = other.beam; } -void base::samples::SonarBeam::swap(base::samples::SonarBeam& other) +void SonarBeam::swap(SonarBeam& other) { Time temp_time = time; Angle temp_bearing = bearing; @@ -59,5 +61,5 @@ void base::samples::SonarBeam::swap(base::samples::SonarBeam& other) } - +}} //end namespace base::samples diff --git a/src/samples/SonarScan.cpp b/src/samples/SonarScan.cpp index 425fe406..b191be31 100644 --- a/src/samples/SonarScan.cpp +++ b/src/samples/SonarScan.cpp @@ -8,37 +8,39 @@ #include #include -base::samples::SonarScan::SonarScan() +namespace base { namespace samples { + +SonarScan::SonarScan() : number_of_beams(0) , number_of_bins(0) , sampling_interval(0) , speed_of_sound(0) - , beamwidth_horizontal(base::Angle::fromRad(0)) - , beamwidth_vertical(base::Angle::fromRad(0)) + , beamwidth_horizontal(Angle::fromRad(0)) + , beamwidth_vertical(Angle::fromRad(0)) , memory_layout_column(true) , polar_coordinates(true) { reset(); } -base::samples::SonarScan::SonarScan(uint16_t number_of_beams, uint16_t number_of_bins, base::Angle start_bearing, base::Angle angular_resolution, bool memory_layout_column) +SonarScan::SonarScan(uint16_t number_of_beams, uint16_t number_of_bins, Angle start_bearing, Angle angular_resolution, bool memory_layout_column) { init(number_of_beams,number_of_bins,start_bearing,angular_resolution,memory_layout_column); } -base::samples::SonarScan::SonarScan(const base::samples::SonarScan& other, bool bcopy) +SonarScan::SonarScan(const SonarScan& other, bool bcopy) { init(other,bcopy); } -base::samples::SonarScan& base::samples::SonarScan::operator=(const base::samples::SonarScan& other) +SonarScan& SonarScan::operator=(const SonarScan& other) { init(other,true); return *this; } -void base::samples::SonarScan::init(const base::samples::SonarScan& other, bool bcopy) +void SonarScan::init(const SonarScan& other, bool bcopy) { init(other.number_of_beams,other.number_of_bins,other.start_bearing,other.angular_resolution,other.memory_layout_column); time = other.time; @@ -54,7 +56,7 @@ void base::samples::SonarScan::init(const base::samples::SonarScan& other, bool } } -void base::samples::SonarScan::init(uint16_t number_of_beams, uint16_t number_of_bins, base::Angle start_bearing, base::Angle angular_resolution, bool memory_layout_column, int val) +void SonarScan::init(uint16_t number_of_beams, uint16_t number_of_bins, Angle start_bearing, Angle angular_resolution, bool memory_layout_column, int val) { //change size if the sonar scan does not fit if(this->number_of_beams != number_of_beams || this->number_of_bins != number_of_bins) @@ -67,14 +69,14 @@ void base::samples::SonarScan::init(uint16_t number_of_beams, uint16_t number_of this->angular_resolution = angular_resolution; this->memory_layout_column = memory_layout_column; speed_of_sound = 0; - beamwidth_horizontal = base::Angle::fromRad(0); - beamwidth_vertical = base::Angle::fromRad(0); + beamwidth_horizontal = Angle::fromRad(0); + beamwidth_vertical = Angle::fromRad(0); reset(val); } -void base::samples::SonarScan::reset(const int val) +void SonarScan::reset(const int val) { - this->time = base::Time(); + this->time = Time(); if (this->data.size() > 0 && val >= 0) { memset(&this->data[0], val%256, this->data.size()); @@ -82,7 +84,7 @@ void base::samples::SonarScan::reset(const int val) time_beams.clear(); } -int base::samples::SonarScan::beamIndexForBearing(const base::Angle bearing, bool range_check) const +int SonarScan::beamIndexForBearing(const Angle bearing, bool range_check) const { double temp_rad = (start_bearing-bearing).rad; int index = round(temp_rad/angular_resolution.rad); @@ -91,12 +93,12 @@ int base::samples::SonarScan::beamIndexForBearing(const base::Angle bearing, boo return index; } -bool base::samples::SonarScan::hasSonarBeam(const base::samples::SonarBeam& sonar_beam) const +bool SonarScan::hasSonarBeam(const SonarBeam& sonar_beam) const { return hasSonarBeam(sonar_beam.bearing); } -bool base::samples::SonarScan::hasSonarBeam(const base::Angle bearing) const +bool SonarScan::hasSonarBeam(const Angle bearing) const { int index = beamIndexForBearing(bearing); if(index < 0) @@ -112,7 +114,7 @@ bool base::samples::SonarScan::hasSonarBeam(const base::Angle bearing) const return false; } -void base::samples::SonarScan::addSonarBeam(const base::samples::SonarBeam& sonar_beam, bool resize) +void SonarScan::addSonarBeam(const SonarBeam& sonar_beam, bool resize) { if(memory_layout_column) throw std::runtime_error("addSonarBeam: cannot add sonar beam because the memory layout is not supported. Call toggleMemoryLayout()"); @@ -141,7 +143,7 @@ void base::samples::SonarScan::addSonarBeam(const base::samples::SonarBeam& sona memcpy(&data[index*number_of_bins],&sonar_beam.beam[0],sonar_beam.beam.size()); } -void base::samples::SonarScan::getSonarBeam(const base::Angle bearing, base::samples::SonarBeam& sonar_beam) const +void SonarScan::getSonarBeam(const Angle bearing, SonarBeam& sonar_beam) const { if(memory_layout_column) throw std::runtime_error("getSonarBeam: Wrong memory layout!"); @@ -162,7 +164,7 @@ void base::samples::SonarScan::getSonarBeam(const base::Angle bearing, base::sam sonar_beam.bearing = bearing; } -void base::samples::SonarScan::toggleMemoryLayout() +void SonarScan::toggleMemoryLayout() { std::vector temp; temp.resize(data.size()); @@ -183,13 +185,13 @@ void base::samples::SonarScan::toggleMemoryLayout() data.swap(temp); } -void base::samples::SonarScan::swap(base::samples::SonarScan& sonar_scan) +void SonarScan::swap(SonarScan& sonar_scan) { //swap vector data.swap(sonar_scan.data); //copy values to temp - base::Time temp_time = sonar_scan.time; + Time temp_time = sonar_scan.time; Angle temp_beamwidth_vertical = sonar_scan.beamwidth_vertical; Angle temp_beamwidth_horizontal = sonar_scan.beamwidth_horizontal; double temp_sampling_interval = sonar_scan.sampling_interval; @@ -227,37 +229,37 @@ void base::samples::SonarScan::swap(base::samples::SonarScan& sonar_scan) speed_of_sound = temp_speed_of_sound; } -uint32_t base::samples::SonarScan::getNumberOfBytes() const +uint32_t SonarScan::getNumberOfBytes() const { return data.size(); } -uint32_t base::samples::SonarScan::getBinCount() const +uint32_t SonarScan::getBinCount() const { return number_of_beams * number_of_bins; } -const std::vector< uint8_t >& base::samples::SonarScan::getData() const +const std::vector< uint8_t >& SonarScan::getData() const { return this->data; } -base::Angle base::samples::SonarScan::getEndBearing() const +Angle SonarScan::getEndBearing() const { return start_bearing-angular_resolution*(number_of_beams-1); } -base::Angle base::samples::SonarScan::getStartBearing() const +Angle SonarScan::getStartBearing() const { return start_bearing; } -base::Angle base::samples::SonarScan::getAngularResolution() const +Angle SonarScan::getAngularResolution() const { return angular_resolution; } -double base::samples::SonarScan::getSpatialResolution() const +double SonarScan::getSpatialResolution() const { //the sampling interval includes the time for //the sound traveling from the transmitter to the target an back @@ -265,12 +267,12 @@ double base::samples::SonarScan::getSpatialResolution() const return sampling_interval*0.5*speed_of_sound; } -void base::samples::SonarScan::setData(const std::vector< uint8_t >& data) +void SonarScan::setData(const std::vector< uint8_t >& data) { this->data = data; } -void base::samples::SonarScan::setData(const char* data, uint32_t size) +void SonarScan::setData(const char* data, uint32_t size) { if (size != this->data.size()) { @@ -284,27 +286,15 @@ void base::samples::SonarScan::setData(const char* data, uint32_t size) memcpy(&this->data[0], data, size); } -uint8_t* base::samples::SonarScan::getDataPtr() +uint8_t* SonarScan::getDataPtr() { return static_cast(&this->data[0]); } -const uint8_t* base::samples::SonarScan::getDataConstPtr() const +const uint8_t* SonarScan::getDataConstPtr() const { return static_cast(&this->data[0]); } - - - - - - - - - - - - - +}} //end namespace base::samples \ No newline at end of file From 3ed5a0256a5cffb09aae850a77e5673331faf5bc Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 18 Aug 2016 14:26:18 +0200 Subject: [PATCH 39/50] fixed namespaces for older cpp standard --- src/TransformWithCovariance.cpp | 2 +- src/TwistWithCovariance.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/TransformWithCovariance.cpp b/src/TransformWithCovariance.cpp index 0691deca..499b6293 100644 --- a/src/TransformWithCovariance.cpp +++ b/src/TransformWithCovariance.cpp @@ -382,7 +382,7 @@ std::ostream& operator<<(std::ostream& out, const TransformWithCovariance& trans } out<<"\n"; } - out.unsetf(std::ios_floatfield); + out.unsetf(std::ios::floatfield); return out; } diff --git a/src/TwistWithCovariance.cpp b/src/TwistWithCovariance.cpp index 35e53b57..505df547 100644 --- a/src/TwistWithCovariance.cpp +++ b/src/TwistWithCovariance.cpp @@ -339,7 +339,7 @@ std::ostream& operator<<(std::ostream& out, const TwistWithCovariance& twist) } out<<"\n"; } - out.unsetf(std::ios_floatfield); + out.unsetf(std::ios::floatfield); return out; } From 79731851f497f66308733950f6d132056320b29f Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Thu, 18 Aug 2016 19:15:24 +0200 Subject: [PATCH 40/50] Added missing includes --- src/samples/DepthMap.hpp | 2 ++ src/samples/Sonar.cpp | 1 + 2 files changed, 3 insertions(+) diff --git a/src/samples/DepthMap.hpp b/src/samples/DepthMap.hpp index a22292c3..013b5861 100644 --- a/src/samples/DepthMap.hpp +++ b/src/samples/DepthMap.hpp @@ -8,6 +8,8 @@ #include #include +#include + #include namespace base { namespace samples { diff --git a/src/samples/Sonar.cpp b/src/samples/Sonar.cpp index 91c320a1..ae90e836 100644 --- a/src/samples/Sonar.cpp +++ b/src/samples/Sonar.cpp @@ -1,4 +1,5 @@ #include "Sonar.hpp" +#include namespace base { namespace samples { From f8a79462db7b396d76276bf039148ea56b66cb88 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Thu, 18 Aug 2016 19:58:34 +0200 Subject: [PATCH 41/50] fixed installation of JointLimitRange.hpp --- src/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c70fb8b8..319109d5 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -36,7 +36,7 @@ rock_library( Deprecated.hpp Eigen.hpp Float.hpp - JointLimitRange.cpp + JointLimitRange.hpp JointLimits.hpp JointState.hpp JointsTrajectory.hpp From 2ece2b8b21ab1b1a6be01644d0351cf79ec24665 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Fri, 19 Aug 2016 13:01:18 +0200 Subject: [PATCH 42/50] removed deprecated includes --- test/test.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/test/test.cpp b/test/test.cpp index 09cf103b..9e2da71c 100644 --- a/test/test.cpp +++ b/test/test.cpp @@ -2,8 +2,6 @@ #include #include -#include -#include #include #include #include From bf648ac4a51f5706fee9ec41d1f26598a385a0d2 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Fri, 19 Aug 2016 13:01:29 +0200 Subject: [PATCH 43/50] adapted CMakeLists to new lib name --- test/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index a9c836d4..e857f44d 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,9 +1,9 @@ rock_testsuite(test_base_types test.cpp test_samples_Sonar.cpp test_Eigen.cpp - DEPS base + DEPS base-types DEPS_PKGCONFIG base-logging) rock_executable(benchmark benchmark.cpp bench_func.cpp - DEPS base + DEPS base-types DEPS_PKGCONFIG base-logging NOINSTALL) From c777ae03601896dcaf174af89e8ee03d508e1b4d Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Fri, 19 Aug 2016 13:01:38 +0200 Subject: [PATCH 44/50] Fixed compile error --- test/test_Eigen.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/test/test_Eigen.cpp b/test/test_Eigen.cpp index 6c0a2f83..82c5af3d 100644 --- a/test/test_Eigen.cpp +++ b/test/test_Eigen.cpp @@ -1,6 +1,7 @@ #include #include #include +#include "../src/TwistWithCovariance.cpp" BOOST_AUTO_TEST_SUITE(Eigen) From b647e0317c5ee0c6c6c041feef3acdd621358278 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Thu, 18 Aug 2016 22:47:43 +0200 Subject: [PATCH 45/50] optimize TranformWithCovariance --- src/TransformWithCovariance.cpp | 108 -------------------------- src/TransformWithCovariance.hpp | 132 ++++++++++++++++++++++++-------- 2 files changed, 102 insertions(+), 138 deletions(-) diff --git a/src/TransformWithCovariance.cpp b/src/TransformWithCovariance.cpp index 499b6293..caa95313 100644 --- a/src/TransformWithCovariance.cpp +++ b/src/TransformWithCovariance.cpp @@ -11,40 +11,6 @@ namespace base { -TransformWithCovariance::TransformWithCovariance() - : translation(Position::Zero()) , orientation(Quaterniond::Identity()) -{ - this->invalidateCovariance(); -} - -TransformWithCovariance::TransformWithCovariance(const Affine3d& trans) -{ - this->setTransform(trans); - this->invalidateCovariance(); -} - -TransformWithCovariance::TransformWithCovariance(const Affine3d& trans, const TransformWithCovariance::Covariance& cov) -{ - this->setTransform(trans); - this->cov = cov; -} - -TransformWithCovariance::TransformWithCovariance(const Position& translation, const Quaterniond& orientation) - : translation(translation), orientation(orientation) -{ - this->invalidateCovariance(); -} - -TransformWithCovariance::TransformWithCovariance(const Position& translation, const Quaterniond& orientation, const TransformWithCovariance::Covariance& cov) - : translation(translation), orientation(orientation), cov(cov) -{ - -} - -TransformWithCovariance TransformWithCovariance::Identity() -{ - return TransformWithCovariance(); -} TransformWithCovariance TransformWithCovariance::composition(const TransformWithCovariance& trans) const { @@ -182,80 +148,6 @@ TransformWithCovariance TransformWithCovariance::inverse() const J*this->getCovariance()*J.transpose()); } -const TransformWithCovariance::Covariance& TransformWithCovariance::getCovariance() const -{ - return this->cov; -} - -void TransformWithCovariance::setCovariance(const TransformWithCovariance::Covariance& cov) -{ - this->cov = cov; -} - -const Matrix3d TransformWithCovariance::getTranslationCov() const -{ - return this->cov.topLeftCorner<3,3>(); -} - -void TransformWithCovariance::setTranslationCov(const Matrix3d& cov) -{ - this->cov.topLeftCorner<3,3>() = cov; -} - -const Matrix3d TransformWithCovariance::getOrientationCov() const -{ - return this->cov.bottomRightCorner<3,3>(); -} - -void TransformWithCovariance::setOrientationCov(const Matrix3d& cov) -{ - this->cov.bottomRightCorner<3,3>() = cov; -} - -const Affine3d TransformWithCovariance::getTransform() const -{ - Affine3d trans (this->orientation); - trans.translation() = this->translation; - return trans; -} - -void TransformWithCovariance::setTransform(const Affine3d& trans) -{ - this->translation = trans.translation(); - this->orientation = Quaterniond(trans.rotation()); -} - -const Orientation TransformWithCovariance::getOrientation() const -{ - return Orientation(this->orientation); -} - -void TransformWithCovariance::setOrientation(const Orientation& q) -{ - this->orientation = Quaterniond(q); -} - -bool TransformWithCovariance::hasValidTransform() const -{ - return !translation.hasNaN() && !orientation.coeffs().hasNaN(); -} - -void TransformWithCovariance::invalidateTransform() -{ - translation = Position::Ones() * NaN(); - orientation.coeffs() = Vector4d::Ones() * NaN(); -} - -bool TransformWithCovariance::hasValidCovariance() const -{ - return !cov.hasNaN(); -} - -void TransformWithCovariance::invalidateCovariance() -{ - cov = Covariance::Ones() * NaN(); -} - Eigen::Quaterniond TransformWithCovariance::r_to_q(const Eigen::Vector3d& r) { double theta = r.norm(); diff --git a/src/TransformWithCovariance.hpp b/src/TransformWithCovariance.hpp index 00fcacf9..6d99f80c 100644 --- a/src/TransformWithCovariance.hpp +++ b/src/TransformWithCovariance.hpp @@ -38,18 +38,40 @@ namespace base { Covariance cov; public: + TransformWithCovariance() + : translation(Position::Zero()) , orientation(Quaterniond::Identity()) + { + this->invalidateCovariance(); + } - TransformWithCovariance(); - - explicit TransformWithCovariance( const base::Affine3d& trans); - - TransformWithCovariance( const base::Affine3d& trans, const Covariance& cov ); - - TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation); - - TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation, const Covariance& cov ); - - static TransformWithCovariance Identity(); + explicit TransformWithCovariance( const base::Affine3d& trans) + { + this->setTransform(trans); + this->invalidateCovariance(); + } + + TransformWithCovariance( const base::Affine3d& trans, const Covariance& cov ) + { + this->setTransform(trans); + this->cov = cov; + } + + TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation) + : translation(translation), orientation(orientation) + { + this->invalidateCovariance(); + } + + TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation, const Covariance& cov ) + : translation(translation), orientation(orientation), cov(cov) + { + + } + + static TransformWithCovariance Identity() + { + return TransformWithCovariance(); + } /** Default std::cout function @@ -79,30 +101,80 @@ namespace base { TransformWithCovariance inverse() const; - const Covariance& getCovariance() const; - void setCovariance( const Covariance& cov ); - - const base::Matrix3d getTranslationCov() const; - void setTranslationCov(const base::Matrix3d& cov); + const Covariance& getCovariance() const + { + return this->cov; + } - const base::Matrix3d getOrientationCov() const; - void setOrientationCov(const base::Matrix3d& cov); + void setCovariance( const Covariance& cov ) + { + this->cov = cov; + } - const base::Affine3d getTransform() const; + const base::Matrix3d getTranslationCov() const + { + return this->cov.topLeftCorner<3,3>(); + } - void setTransform( const base::Affine3d& trans ); - - const base::Orientation getOrientation() const; - - void setOrientation(const base::Orientation & q); - - bool hasValidTransform() const; - - void invalidateTransform(); + void setTranslationCov(const base::Matrix3d& cov) + { + this->cov.topLeftCorner<3,3>() = cov; + } + + const base::Matrix3d getOrientationCov() const + { + return this->cov.bottomRightCorner<3,3>(); + } + + void setOrientationCov(const base::Matrix3d& cov) + { + this->cov.bottomRightCorner<3,3>() = cov; + } + + const base::Affine3d getTransform() const + { + Affine3d trans (this->orientation); + trans.translation() = this->translation; + return trans; + } + + void setTransform( const base::Affine3d& trans ) + { + this->translation = trans.translation(); + this->orientation = Quaterniond(trans.rotation()); + } + + const base::Orientation &getOrientation() const + { + return this->orientation; + } + + void setOrientation(const base::Orientation & q) + { + this->orientation = q; + } + + bool hasValidTransform() const + { + return !translation.hasNaN() && !orientation.coeffs().hasNaN(); + } + + void invalidateTransform() + { + translation = Position::Ones() * NaN(); + orientation.coeffs() = Vector4d::Ones() * NaN(); + } /** @warning This method is computationally expensive. Use with care! */ - bool hasValidCovariance() const; - void invalidateCovariance(); + bool hasValidCovariance() const + { + return !cov.hasNaN(); + } + + void invalidateCovariance() + { + cov = Covariance::Ones() * NaN(); + } protected: // The uncertainty transformations are implemented according to: From c712629d84caed099d97b3d31e35f272e1cd3c53 Mon Sep 17 00:00:00 2001 From: Rafael Saback Date: Wed, 17 Aug 2016 08:44:20 -0300 Subject: [PATCH 46/50] Move LinearAngular6DCommand from auv_control --- base/commands/LinearAngular6DCommand.hpp | 49 ++++++++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 base/commands/LinearAngular6DCommand.hpp diff --git a/base/commands/LinearAngular6DCommand.hpp b/base/commands/LinearAngular6DCommand.hpp new file mode 100644 index 00000000..544fd839 --- /dev/null +++ b/base/commands/LinearAngular6DCommand.hpp @@ -0,0 +1,49 @@ +#ifndef BASE_COMMANDS_LINEAR_ANGULAR_6D_COMMAND_HPP_ +#define BASE_COMMANDS_LINEAR_ANGULAR_6D_COMMAND_HPP_ + +#include +#include +#include + +namespace base +{ + namespace commands + { + /** Common command structure for all controller types, in all control frames */ + struct LinearAngular6DCommand + { + /** The command timestamp */ + base::Time time; + /** The linear part of the command, as (x,y,z) */ + base::Vector3d linear; + /** The angular part of the command, as (r,p,y) */ + base::Vector3d angular; + + LinearAngular6DCommand(){ + for(int i = 0; i < 3; i++){ + linear(i) = base::unset(); + angular(i) = base::unset(); + } + } + + double& x() { return linear(0); } + double& y() { return linear(1); } + double& z() { return linear(2); } + double& roll() { return angular(0); } + double& pitch() { return angular(1); } + double& yaw() { return angular(2); } + + double x() const { return linear(0); } + double y() const { return linear(1); } + double z() const { return linear(2); } + double roll() const { return angular(0); } + double pitch() const { return angular(1); } + double yaw() const { return angular(2); } + }; + } + + // For barckwards compatibility only + typedef base::commands::LinearAngular6DCommand LinearAngular6DCommand; +} + +#endif From 04736ed6c625feddbd7511d1491ac1564b981300 Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Tue, 6 Sep 2016 16:26:57 +0200 Subject: [PATCH 47/50] moved guaranteeSPD to own header --- src/CMakeLists.txt | 1 + src/Matrix.hpp | 33 +++++++++++++++++++++++++++++++++ src/TwistWithCovariance.cpp | 33 +++------------------------------ 3 files changed, 37 insertions(+), 30 deletions(-) create mode 100644 src/Matrix.hpp diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 319109d5..ced23534 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -42,6 +42,7 @@ rock_library( JointsTrajectory.hpp JointTransform.hpp Logging.hpp + Matrix.hpp NamedVector.hpp Point.hpp Pose.hpp diff --git a/src/Matrix.hpp b/src/Matrix.hpp new file mode 100644 index 00000000..22f76e3b --- /dev/null +++ b/src/Matrix.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include + +namespace base { + +/** +* Guarantee Symmetric (semi-) Positive Definite (SPD) matrix. +* The input matrix must be symmetric already (only the lower triangular part will be considered) +*/ +template +static typename _MatrixType::PlainObject guaranteeSPD (const _MatrixType &A, const typename _MatrixType::RealScalar& minEig = 0.0) +{ + typedef typename _MatrixType::PlainObject Matrix; + typedef Eigen::SelfAdjointEigenSolver EigenDecomp; + typedef typename EigenDecomp::RealVectorType EigenValues; + + // Eigenvalue decomposition + Eigen::SelfAdjointEigenSolver eigOfA (A, Eigen::ComputeEigenvectors); + + // truncate Eigenvalues: + EigenValues s = eigOfA.eigenvalues(); + s = s.cwiseMax(minEig); + + // re-compose matrix: + Matrix spdA = eigOfA.eigenvectors() * s.asDiagonal() * eigOfA.eigenvectors().adjoint(); + + return spdA; +}; + + + +} // end namespace base \ No newline at end of file diff --git a/src/TwistWithCovariance.cpp b/src/TwistWithCovariance.cpp index 505df547..08eb5a34 100644 --- a/src/TwistWithCovariance.cpp +++ b/src/TwistWithCovariance.cpp @@ -1,39 +1,12 @@ #include "TwistWithCovariance.hpp" #include // std::setprecision - -#include -#include -#include -#include - +#include #include -namespace base { - -/** - * Guarantee Symmetric (semi-) Positive Definite (SPD) matrix. - * The input matrix must be symmetric already (only the lower triangular part will be considered) - */ -template -static typename _MatrixType::PlainObject guaranteeSPD (const _MatrixType &A, const typename _MatrixType::RealScalar& minEig = 0.0) -{ - typedef typename _MatrixType::PlainObject Matrix; - typedef Eigen::SelfAdjointEigenSolver EigenDecomp; - typedef typename EigenDecomp::RealVectorType EigenValues; +#include "Matrix.hpp" - // Eigenvalue decomposition - Eigen::SelfAdjointEigenSolver eigOfA (A, Eigen::ComputeEigenvectors); - - // truncate Eigenvalues: - EigenValues s = eigOfA.eigenvalues(); - s = s.cwiseMax(minEig); - - // re-compose matrix: - Matrix spdA = eigOfA.eigenvectors() * s.asDiagonal() * eigOfA.eigenvectors().adjoint(); - - return spdA; -}; +namespace base { typedef TwistWithCovariance::Covariance Covariance; From 16d0f299252a4c3add2db0d8caf64b89d790bf4b Mon Sep 17 00:00:00 2001 From: Dennis Hemker Date: Thu, 8 Sep 2016 16:17:49 +0200 Subject: [PATCH 48/50] removed static declarations --- src/Temperature.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/Temperature.hpp b/src/Temperature.hpp index 008cdca4..bdfebf7c 100644 --- a/src/Temperature.hpp +++ b/src/Temperature.hpp @@ -97,13 +97,13 @@ class Temperature }; -static Temperature operator+( Temperature a, Temperature b ); +Temperature operator+( Temperature a, Temperature b ); -static Temperature operator-( Temperature a, Temperature b ); +Temperature operator-( Temperature a, Temperature b ); -static Temperature operator*( Temperature a, double b ); +Temperature operator*( Temperature a, double b ); -static Temperature operator*( double a, Temperature b ); +Temperature operator*( double a, Temperature b ); static std::ostream& operator << (std::ostream& os, Temperature temperature); From e8450f9ba7c9faac3770aa166360649936f6e563 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Fri, 9 Sep 2016 09:09:07 +0200 Subject: [PATCH 49/50] removed wrong static --- src/Temperature.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Temperature.hpp b/src/Temperature.hpp index bdfebf7c..6d0862ca 100644 --- a/src/Temperature.hpp +++ b/src/Temperature.hpp @@ -105,7 +105,7 @@ Temperature operator*( Temperature a, double b ); Temperature operator*( double a, Temperature b ); -static std::ostream& operator << (std::ostream& os, Temperature temperature); +std::ostream& operator << (std::ostream& os, Temperature temperature); } From 62264e666146ec702c51937a61c7dd9657e4c79a Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Fri, 9 Sep 2016 09:19:12 +0200 Subject: [PATCH 50/50] move LinearAngular6DCommand.hpp into src dir and install header --- src/CMakeLists.txt | 1 + {base => src}/commands/LinearAngular6DCommand.hpp | 0 2 files changed, 1 insertion(+) rename {base => src}/commands/LinearAngular6DCommand.hpp (100%) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ced23534..934dbaaf 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -61,6 +61,7 @@ rock_library( commands/Joints.hpp commands/Motion2D.hpp commands/Speed6D.hpp + commands/LinearAngular6DCommand.hpp logging/logging_iostream_style.h logging/logging_printf_style.h samples/BodyState.hpp diff --git a/base/commands/LinearAngular6DCommand.hpp b/src/commands/LinearAngular6DCommand.hpp similarity index 100% rename from base/commands/LinearAngular6DCommand.hpp rename to src/commands/LinearAngular6DCommand.hpp