diff --git a/include/open_karto/Karto.h b/include/open_karto/Karto.h index 73dc233..8f3b049 100644 --- a/include/open_karto/Karto.h +++ b/include/open_karto/Karto.h @@ -15,8 +15,8 @@ * along with this program. If not, see . */ -#ifndef __KARTO_KARTO__ -#define __KARTO_KARTO__ +#ifndef OPEN_KARTO_KARTO_H +#define OPEN_KARTO_KARTO_H #include #include @@ -40,8 +40,8 @@ #include #endif -#include "Math.h" -#include "Macros.h" +#include +#include #define KARTO_Object(name) \ virtual const char* GetClassName() const { return #name; } \ @@ -49,24 +49,24 @@ typedef kt_int32u kt_objecttype; -const kt_objecttype ObjectType_None = 0x00000000; -const kt_objecttype ObjectType_Sensor = 0x00001000; -const kt_objecttype ObjectType_SensorData = 0x00002000; -const kt_objecttype ObjectType_CustomData = 0x00004000; -const kt_objecttype ObjectType_Misc = 0x10000000; +const kt_objecttype ObjectType_None = 0x00000000; +const kt_objecttype ObjectType_Sensor = 0x00001000; +const kt_objecttype ObjectType_SensorData = 0x00002000; +const kt_objecttype ObjectType_CustomData = 0x00004000; +const kt_objecttype ObjectType_Misc = 0x10000000; -const kt_objecttype ObjectType_Drive = ObjectType_Sensor | 0x01; -const kt_objecttype ObjectType_LaserRangeFinder = ObjectType_Sensor | 0x02; -const kt_objecttype ObjectType_Camera = ObjectType_Sensor | 0x04; +const kt_objecttype ObjectType_Drive = ObjectType_Sensor | 0x01; +const kt_objecttype ObjectType_LaserRangeFinder = ObjectType_Sensor | 0x02; +const kt_objecttype ObjectType_Camera = ObjectType_Sensor | 0x04; -const kt_objecttype ObjectType_DrivePose = ObjectType_SensorData | 0x01; -const kt_objecttype ObjectType_LaserRangeScan = ObjectType_SensorData | 0x02; -const kt_objecttype ObjectType_LocalizedRangeScan = ObjectType_SensorData | 0x04; -const kt_objecttype ObjectType_CameraImage = ObjectType_SensorData | 0x08; +const kt_objecttype ObjectType_DrivePose = ObjectType_SensorData | 0x01; +const kt_objecttype ObjectType_LaserRangeScan = ObjectType_SensorData | 0x02; +const kt_objecttype ObjectType_LocalizedRangeScan = ObjectType_SensorData | 0x04; +const kt_objecttype ObjectType_CameraImage = ObjectType_SensorData | 0x08; -const kt_objecttype ObjectType_Header = ObjectType_Misc | 0x01; -const kt_objecttype ObjectType_Parameters = ObjectType_Misc | 0x02; -const kt_objecttype ObjectType_DatasetInfo = ObjectType_Misc | 0x04; +const kt_objecttype ObjectType_Header = ObjectType_Misc | 0x01; +const kt_objecttype ObjectType_Parameters = ObjectType_Misc | 0x02; +const kt_objecttype ObjectType_DatasetInfo = ObjectType_Misc | 0x04; const kt_objecttype ObjectType_Module = ObjectType_Misc | 0x08; namespace karto @@ -152,7 +152,7 @@ namespace karto private: std::string m_Message; kt_int32s m_ErrorCode; - }; // class Exception + }; // class Exception //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -176,7 +176,7 @@ namespace karto virtual ~NonCopyable() { } - }; // class NonCopyable + }; // class NonCopyable //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -248,7 +248,7 @@ namespace karto * Functor function */ virtual void operator() (kt_int32u) {}; - }; // Functor + }; // Functor //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -338,7 +338,7 @@ namespace karto private: ParameterVector m_Parameters; std::map m_ParameterLookup; - }; // ParameterManager + }; // ParameterManager //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -401,7 +401,7 @@ namespace karto inline void SetName(const std::string& rName) { std::string::size_type pos = rName.find_last_of('/'); - if(pos != 0 && pos != std::string::npos) + if (pos != 0 && pos != std::string::npos) { throw Exception("Name can't contain a scope!"); } @@ -433,7 +433,7 @@ namespace karto */ inline std::string ToString() const { - if(m_Scope == "") + if (m_Scope == "") { return m_Name; } @@ -508,7 +508,7 @@ namespace karto { std::string::size_type pos = rName.find_last_of('/'); - if(pos == std::string::npos) + if (pos == std::string::npos) { m_Name = rName; } @@ -518,7 +518,7 @@ namespace karto m_Name = rName.substr(pos+1, rName.size()); // remove '/' from m_Scope if first!! - if(m_Scope.size() > 0 && m_Scope[0] == '/') + if (m_Scope.size() > 0 && m_Scope[0] == '/') { m_Scope = m_Scope.substr(1, m_Scope.size()); } @@ -537,7 +537,7 @@ namespace karto } char c = rName[0]; - if(IsValidFirst(c)) + if (IsValidFirst(c)) { for (size_t i = 1; i < rName.size(); ++i) { @@ -751,9 +751,9 @@ namespace karto class KARTO_EXPORT Module : public Object { public: - //@cond EXCLUDE + // @cond EXCLUDE KARTO_Object(Module) - //@endcond + // @endcond public: /** @@ -878,16 +878,16 @@ namespace karto /** * Equality operator */ - inline kt_bool operator == (const Size2& rOther) const - { - return (m_Width == rOther.m_Width && m_Height == rOther.m_Height); - } + inline kt_bool operator == (const Size2& rOther) const + { + return (m_Width == rOther.m_Width && m_Height == rOther.m_Height); + } /** * Inequality operator */ - inline kt_bool operator != (const Size2& rOther) const - { + inline kt_bool operator != (const Size2& rOther) const + { return (m_Width != rOther.m_Width || m_Height != rOther.m_Height); } @@ -905,7 +905,7 @@ namespace karto private: T m_Width; T m_Height; - }; // Size2 + }; // Size2 //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -1179,13 +1179,13 @@ namespace karto */ friend inline std::istream& operator >> (std::istream& rStream, const Vector2& /*rVector*/) { - // Implement me!! + // Implement me!! TODO(lucbettaieb): What the what? Do I need to implement this? return rStream; } private: T m_Values[2]; - }; // Vector2 + }; // Vector2 /** * Type declaration of Vector2 vector @@ -1366,7 +1366,9 @@ namespace karto */ inline const Vector3 operator + (const Vector3& rOther) const { - return Vector3(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1], m_Values[2] + rOther.m_Values[2]); + return Vector3(m_Values[0] + rOther.m_Values[0], + m_Values[1] + rOther.m_Values[1], + m_Values[2] + rOther.m_Values[2]); } /** @@ -1376,7 +1378,9 @@ namespace karto */ inline const Vector3 operator + (kt_double scalar) const { - return Vector3(m_Values[0] + scalar, m_Values[1] + scalar, m_Values[2] + scalar); + return Vector3(m_Values[0] + scalar, + m_Values[1] + scalar, + m_Values[2] + scalar); } /** @@ -1386,7 +1390,9 @@ namespace karto */ inline const Vector3 operator - (const Vector3& rOther) const { - return Vector3(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1], m_Values[2] - rOther.m_Values[2]); + return Vector3(m_Values[0] - rOther.m_Values[0], + m_Values[1] - rOther.m_Values[1], + m_Values[2] - rOther.m_Values[2]); } /** @@ -1414,7 +1420,9 @@ namespace karto */ inline kt_bool operator == (const Vector3& rOther) const { - return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1] && m_Values[2] == rOther.m_Values[2]); + return (m_Values[0] == rOther.m_Values[0] && + m_Values[1] == rOther.m_Values[1] && + m_Values[2] == rOther.m_Values[2]); } /** @@ -1423,7 +1431,9 @@ namespace karto */ inline kt_bool operator != (const Vector3& rOther) const { - return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1] || m_Values[2] != rOther.m_Values[2]); + return (m_Values[0] != rOther.m_Values[0] || + m_Values[1] != rOther.m_Values[1] || + m_Values[2] != rOther.m_Values[2]); } /** @@ -1449,7 +1459,7 @@ namespace karto private: T m_Values[3]; - }; // Vector3 + }; // Vector3 //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -1675,7 +1685,10 @@ namespace karto */ inline kt_bool operator == (const Quaternion& rOther) const { - return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1] && m_Values[2] == rOther.m_Values[2] && m_Values[3] == rOther.m_Values[3]); + return (m_Values[0] == rOther.m_Values[0] && + m_Values[1] == rOther.m_Values[1] && + m_Values[2] == rOther.m_Values[2] && + m_Values[3] == rOther.m_Values[3]); } /** @@ -1684,7 +1697,10 @@ namespace karto */ inline kt_bool operator != (const Quaternion& rOther) const { - return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1] || m_Values[2] != rOther.m_Values[2] || m_Values[3] != rOther.m_Values[3]); + return (m_Values[0] != rOther.m_Values[0] || + m_Values[1] != rOther.m_Values[1] || + m_Values[2] != rOther.m_Values[2] || + m_Values[3] != rOther.m_Values[3]); } /** @@ -1694,7 +1710,10 @@ namespace karto */ friend inline std::ostream& operator << (std::ostream& rStream, const Quaternion& rQuaternion) { - rStream << rQuaternion.m_Values[0] << " " << rQuaternion.m_Values[1] << " " << rQuaternion.m_Values[2] << " " << rQuaternion.m_Values[3]; + rStream << rQuaternion.m_Values[0] << " " + << rQuaternion.m_Values[1] << " " + << rQuaternion.m_Values[2] << " " + << rQuaternion.m_Values[3]; return rStream; } @@ -1897,23 +1916,23 @@ namespace karto /** * Equality operator */ - inline kt_bool operator == (const Rectangle2& rOther) const - { - return (m_Position == rOther.m_Position && m_Size == rOther.m_Size); - } + inline kt_bool operator == (const Rectangle2& rOther) const + { + return (m_Position == rOther.m_Position && m_Size == rOther.m_Size); + } /** * Inequality operator */ - inline kt_bool operator != (const Rectangle2& rOther) const - { + inline kt_bool operator != (const Rectangle2& rOther) const + { return (m_Position != rOther.m_Position || m_Size != rOther.m_Size); } private: Vector2 m_Position; Size2 m_Size; - }; // Rectangle2 + }; // Rectangle2 //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -2069,16 +2088,16 @@ namespace karto /** * Equality operator */ - inline kt_bool operator == (const Pose2& rOther) const - { - return (m_Position == rOther.m_Position && m_Heading == rOther.m_Heading); - } + inline kt_bool operator == (const Pose2& rOther) const + { + return (m_Position == rOther.m_Position && m_Heading == rOther.m_Heading); + } /** * Inequality operator */ - inline kt_bool operator != (const Pose2& rOther) const - { + inline kt_bool operator != (const Pose2& rOther) const + { return (m_Position != rOther.m_Position || m_Heading != rOther.m_Heading); } @@ -2136,7 +2155,7 @@ namespace karto Vector2 m_Position; kt_double m_Heading; - }; // Pose2 + }; // Pose2 /** * Type declaration of Pose2 vector @@ -2268,16 +2287,16 @@ namespace karto /** * Equality operator */ - inline kt_bool operator == (const Pose3& rOther) const - { - return (m_Position == rOther.m_Position && m_Orientation == rOther.m_Orientation); - } + inline kt_bool operator == (const Pose3& rOther) const + { + return (m_Position == rOther.m_Position && m_Orientation == rOther.m_Orientation); + } /** * Inequality operator */ - inline kt_bool operator != (const Pose3& rOther) const - { + inline kt_bool operator != (const Pose3& rOther) const + { return (m_Position != rOther.m_Position || m_Orientation != rOther.m_Orientation); } @@ -2305,15 +2324,15 @@ namespace karto private: Vector3 m_Position; Quaternion m_Orientation; - }; // Pose3 + }; // Pose3 //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// - /** - * Defines a Matrix 3 x 3 class. - */ + /** + * Defines a Matrix 3 x 3 class. + */ class Matrix3 { public: @@ -2415,7 +2434,7 @@ namespace karto /** * Returns the inverse of the matrix */ - Matrix3 Inverse() const + Matrix3 Inverse() const { Matrix3 kInverse = *this; kt_bool haveInverse = InverseFast(kInverse, 1e-14); @@ -2444,7 +2463,9 @@ namespace karto rkInverse.m_Matrix[2][1] = m_Matrix[0][1]*m_Matrix[2][0] - m_Matrix[0][0]*m_Matrix[2][1]; rkInverse.m_Matrix[2][2] = m_Matrix[0][0]*m_Matrix[1][1] - m_Matrix[0][1]*m_Matrix[1][0]; - kt_double fDet = m_Matrix[0][0]*rkInverse.m_Matrix[0][0] + m_Matrix[0][1]*rkInverse.m_Matrix[1][0]+ m_Matrix[0][2]*rkInverse.m_Matrix[2][0]; + kt_double fDet = m_Matrix[0][0]*rkInverse.m_Matrix[0][0] + + m_Matrix[0][1]*rkInverse.m_Matrix[1][0] + + m_Matrix[0][2]*rkInverse.m_Matrix[2][0]; if (fabs(fDet) <= fTolerance) { @@ -2500,9 +2521,9 @@ namespace karto * @return reference to mat(r,c) */ inline kt_double& operator()(kt_int32u row, kt_int32u column) - { - return m_Matrix[row][column]; - } + { + return m_Matrix[row][column]; + } /** * Read-only matrix element access, allows use of construct mat(r, c) @@ -2528,7 +2549,9 @@ namespace karto { for (size_t col = 0; col < 3; col++) { - product.m_Matrix[row][col] = m_Matrix[row][0]*rOther.m_Matrix[0][col] + m_Matrix[row][1]*rOther.m_Matrix[1][col] + m_Matrix[row][2]*rOther.m_Matrix[2][col]; + product.m_Matrix[row][col] = m_Matrix[row][0]*rOther.m_Matrix[0][col] + + m_Matrix[row][1]*rOther.m_Matrix[1][col] + + m_Matrix[row][2]*rOther.m_Matrix[2][col]; } } @@ -2544,9 +2567,12 @@ namespace karto { Pose2 pose2; - pose2.SetX(m_Matrix[0][0] * rPose2.GetX() + m_Matrix[0][1] * rPose2.GetY() + m_Matrix[0][2] * rPose2.GetHeading()); - pose2.SetY(m_Matrix[1][0] * rPose2.GetX() + m_Matrix[1][1] * rPose2.GetY() + m_Matrix[1][2] * rPose2.GetHeading()); - pose2.SetHeading(m_Matrix[2][0] * rPose2.GetX() + m_Matrix[2][1] * rPose2.GetY() + m_Matrix[2][2] * rPose2.GetHeading()); + pose2.SetX(m_Matrix[0][0] * rPose2.GetX() + m_Matrix[0][1] * + rPose2.GetY() + m_Matrix[0][2] * rPose2.GetHeading()); + pose2.SetY(m_Matrix[1][0] * rPose2.GetX() + m_Matrix[1][1] * + rPose2.GetY() + m_Matrix[1][2] * rPose2.GetHeading()); + pose2.SetHeading(m_Matrix[2][0] * rPose2.GetX() + m_Matrix[2][1] * + rPose2.GetY() + m_Matrix[2][2] * rPose2.GetHeading()); return pose2; } @@ -2579,15 +2605,15 @@ namespace karto private: kt_double m_Matrix[3][3]; - }; // Matrix3 + }; // Matrix3 //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// /** - * Defines a general Matrix class. - */ + * Defines a general Matrix class. + */ class Matrix { public: @@ -2717,7 +2743,7 @@ namespace karto kt_int32u m_Columns; kt_double* m_pData; - }; // Matrix + }; // Matrix //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -2813,7 +2839,7 @@ namespace karto private: Vector2 m_Minimum; Vector2 m_Maximum; - }; // BoundingBox2 + }; // BoundingBox2 //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -2824,7 +2850,7 @@ namespace karto */ class Transform { - public: + public: /** * Constructs a transformation from the origin to the given pose * @param rPose pose @@ -2844,15 +2870,15 @@ namespace karto SetTransform(rPose1, rPose2); } - public: - /** + public: + /** * Transforms the pose according to this transform * @param rSourcePose pose to transform from * @return transformed pose */ inline Pose2 TransformPose(const Pose2& rSourcePose) { - Pose2 newPosition = m_Transform + m_Rotation * rSourcePose; + Pose2 newPosition = m_Transform + m_Rotation * rSourcePose; kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() + m_Transform.GetHeading()); return Pose2(newPosition.GetPosition(), angle); @@ -2865,45 +2891,45 @@ namespace karto */ inline Pose2 InverseTransformPose(const Pose2& rSourcePose) { - Pose2 newPosition = m_InverseRotation * (rSourcePose - m_Transform); + Pose2 newPosition = m_InverseRotation * (rSourcePose - m_Transform); kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() - m_Transform.GetHeading()); // components of transform - return Pose2(newPosition.GetPosition(), angle); + return Pose2(newPosition.GetPosition(), angle); } private: - /** - * Sets this to be the transformation from the first pose to the second pose - * @param rPose1 first pose - * @param rPose2 second pose - */ + /** + * Sets this to be the transformation from the first pose to the second pose + * @param rPose1 first pose + * @param rPose2 second pose + */ void SetTransform(const Pose2& rPose1, const Pose2& rPose2) { - if (rPose1 == rPose2) - { - m_Rotation.SetToIdentity(); - m_InverseRotation.SetToIdentity(); + if (rPose1 == rPose2) + { + m_Rotation.SetToIdentity(); + m_InverseRotation.SetToIdentity(); m_Transform = Pose2(); - return; - } + return; + } - // heading transformation - m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading()); - m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading()); + // heading transformation + m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading()); + m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading()); - // position transformation - Pose2 newPosition; - if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0) - { - newPosition = rPose2 - m_Rotation * rPose1; - } - else - { - newPosition = rPose2; - } + // position transformation + Pose2 newPosition; + if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0) + { + newPosition = rPose2 - m_Rotation * rPose1; + } + else + { + newPosition = rPose2; + } - m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading()); + m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading()); } private: @@ -2912,7 +2938,7 @@ namespace karto Matrix3 m_Rotation; Matrix3 m_InverseRotation; - }; // Transform + }; // Transform //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -2964,7 +2990,9 @@ namespace karto * @param rDescription * @param pParameterManger */ - AbstractParameter(const std::string& rName, const std::string& rDescription, ParameterManager* pParameterManger = NULL) + AbstractParameter(const std::string& rName, + const std::string& rDescription, + ParameterManager* pParameterManger = NULL) : m_Name(rName) , m_Description(rDescription) { @@ -3037,7 +3065,7 @@ namespace karto private: std::string m_Name; std::string m_Description; - }; // AbstractParameter + }; // AbstractParameter //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -3069,7 +3097,10 @@ namespace karto * @param value * @param pParameterManger */ - Parameter(const std::string& rName, const std::string& rDescription, T value, ParameterManager* pParameterManger = NULL) + Parameter(const std::string& rName, + const std::string& rDescription, + T value, + ParameterManager* pParameterManger = NULL) : AbstractParameter(rName, rDescription, pParameterManger) , m_Value(value) { @@ -3158,7 +3189,7 @@ namespace karto * Parameter value */ T m_Value; - }; // Parameter + }; // Parameter template<> inline void Parameter::SetValueFromString(const std::string& rStringValue) @@ -3334,7 +3365,7 @@ namespace karto private: EnumMap m_EnumDefines; - }; // ParameterEnum + }; // ParameterEnum //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -3346,9 +3377,9 @@ namespace karto class Parameters : public Object { public: - //@cond EXCLUDE + // @cond EXCLUDE KARTO_Object(Parameters) - //@endcond + // @endcond public: /** @@ -3370,7 +3401,7 @@ namespace karto private: Parameters(const Parameters&); const Parameters& operator=(const Parameters&); - }; // Parameters + }; // Parameters //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -3384,9 +3415,9 @@ namespace karto class KARTO_EXPORT Sensor : public Object { public: - //@cond EXCLUDE + // @cond EXCLUDE KARTO_Object(Sensor) - //@endcond + // @endcond protected: /** @@ -3449,7 +3480,7 @@ namespace karto * Sensor offset pose */ Parameter* m_pOffsetPose; - }; // Sensor + }; // Sensor /** * Type declaration of Sensor vector @@ -3505,7 +3536,9 @@ namespace karto if ((m_Sensors.find(pSensor->GetName()) != m_Sensors.end()) && !override) { - throw Exception("Cannot register sensor: already registered: [" + pSensor->GetName().ToString() + "] (Consider setting 'override' to true)"); + throw Exception("Cannot register sensor: already registered: [" + + pSensor->GetName().ToString() + + "] (Consider setting 'override' to true)"); } std::cout << "Registering sensor: [" << pSensor->GetName().ToString() << "]" << std::endl; @@ -3521,7 +3554,7 @@ namespace karto { Validate(pSensor); - if(m_Sensors.find(pSensor->GetName()) != m_Sensors.end()) + if (m_Sensors.find(pSensor->GetName()) != m_Sensors.end()) { std::cout << "Unregistering sensor: " << pSensor->GetName().ToString() << std::endl; @@ -3540,7 +3573,7 @@ namespace karto */ Sensor* GetSensorByName(const Name& rName) { - if(m_Sensors.find(rName) != m_Sensors.end()) + if (m_Sensors.find(rName) != m_Sensors.end()) { return m_Sensors[rName]; } @@ -3614,9 +3647,9 @@ namespace karto class Drive : public Sensor { public: - //@cond EXCLUDE + // @cond EXCLUDE KARTO_Object(Drive) - //@endcond + // @endcond public: /** @@ -3642,7 +3675,7 @@ namespace karto virtual kt_bool Validate(SensorData* pSensorData) { - if(pSensorData == NULL) + if (pSensorData == NULL) { return false; } @@ -3653,7 +3686,7 @@ namespace karto private: Drive(const Drive&); const Drive& operator=(const Drive&); - }; // class Drive + }; // class Drive //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -3677,9 +3710,9 @@ namespace karto class KARTO_EXPORT LaserRangeFinder : public Sensor { public: - //@cond EXCLUDE + // @cond EXCLUDE KARTO_Object(LaserRangeFinder) - //@endcond + // @endcond public: /** @@ -3831,7 +3864,8 @@ namespace karto throw Exception(stream.str()); } } - else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS200 || m_pType->GetValue() == LaserRangeFinder_Sick_LMS291) + else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS200 || + m_pType->GetValue() == LaserRangeFinder_Sick_LMS291) { if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25))) { @@ -3884,7 +3918,8 @@ namespace karto if (math::InRange(GetRangeThreshold(), GetMinimumRange(), GetMaximumRange()) == false) { - std::cout << "Please set range threshold to a value between [" << GetMinimumRange() << ";" << GetMaximumRange() << "]" << std::endl; + std::cout << "Please set range threshold to a value between [" + << GetMinimumRange() << ";" << GetMaximumRange() << "]" << std::endl; return false; } @@ -3900,7 +3935,10 @@ namespace karto * @param ignoreThresholdPoints * @param flipY */ - const PointVectorDouble GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan, CoordinateConverter* pCoordinateConverter, kt_bool ignoreThresholdPoints = true, kt_bool flipY = false) const; + const PointVectorDouble GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan, + CoordinateConverter* pCoordinateConverter, + kt_bool ignoreThresholdPoints = true, + kt_bool flipY = false) const; public: /** @@ -3913,7 +3951,7 @@ namespace karto { LaserRangeFinder* pLrf = NULL; - switch(type) + switch (type) { // see http://www.hizook.com/files/publications/SICK_LMS100.pdf // set range threshold to 18m @@ -4005,7 +4043,8 @@ namespace karto { pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo URG-04LX")); - // Sensing range is 4 meters. It has detection problems when scanning absorptive surfaces (such as black trimming). + // Sensing range is 4 meters. It has detection problems when + // scanning absorptive surfaces (such as black trimming). pLrf->m_pMinimumRange->SetValue(0.02); pLrf->m_pMaximumRange->SetValue(4.0); @@ -4065,7 +4104,9 @@ namespace karto m_pMinimumAngle = new Parameter("MinimumAngle", -KT_PI_2, GetParameterManager()); m_pMaximumAngle = new Parameter("MaximumAngle", KT_PI_2, GetParameterManager()); - m_pAngularResolution = new Parameter("AngularResolution", math::DegreesToRadians(1), GetParameterManager()); + m_pAngularResolution = new Parameter("AngularResolution", + math::DegreesToRadians(1), + GetParameterManager()); m_pRangeThreshold = new Parameter("RangeThreshold", 12.0, GetParameterManager()); @@ -4079,11 +4120,14 @@ namespace karto } /** - * Set the number of range readings based on the minimum and maximum angles of the sensor and the angular resolution + * Set the number of range readings based on the minimum and + * maximum angles of the sensor and the angular resolution */ void Update() { - m_NumberOfRangeReadings = static_cast(math::Round((GetMaximumAngle() - GetMinimumAngle()) / GetAngularResolution()) + 1); + m_NumberOfRangeReadings = static_cast(math::Round((GetMaximumAngle() - + GetMinimumAngle()) + / GetAngularResolution()) + 1); } private: @@ -4106,8 +4150,8 @@ namespace karto kt_int32u m_NumberOfRangeReadings; - //static std::string LaserRangeFinderTypeNames[6]; - }; // LaserRangeFinder + // static std::string LaserRangeFinderTypeNames[6]; + }; // LaserRangeFinder //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -4295,7 +4339,7 @@ namespace karto kt_double m_Scale; Vector2 m_Offset; - }; // CoordinateConverter + }; // CoordinateConverter //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -4418,7 +4462,8 @@ namespace karto if (IsValidGridIndex(rGrid) == false) { std::stringstream error; - error << "Index " << rGrid << " out of range. Index must be between [0; " << m_Width << ") and [0; " << m_Height << ")"; + error << "Index " << rGrid << " out of range. Index must be between [0; " + << m_Width << ") and [0; " << m_Height << ")"; throw Exception(error.str()); } } @@ -4688,8 +4733,9 @@ namespace karto kt_int32s m_WidthStep; // 8 bit aligned width of grid T* m_pData; // grid data - CoordinateConverter* m_pCoordinateConverter; // coordinate converter to convert between world coordinates and grid coordinates - }; // Grid + // coordinate converter to convert between world coordinates and grid coordinates + CoordinateConverter* m_pCoordinateConverter; + }; // Grid //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -4701,9 +4747,9 @@ namespace karto class CustomData : public Object { public: - //@cond EXCLUDE + // @cond EXCLUDE KARTO_Object(CustomData) - //@endcond + // @endcond public: /** @@ -4754,9 +4800,9 @@ namespace karto class KARTO_EXPORT SensorData : public Object { public: - //@cond EXCLUDE + // @cond EXCLUDE KARTO_Object(SensorData) - //@endcond + // @endcond public: /** @@ -4902,9 +4948,9 @@ namespace karto class LaserRangeScan : public SensorData { public: - //@cond EXCLUDE + // @cond EXCLUDE KARTO_Object(LaserRangeScan) - //@endcond + // @endcond public: /** @@ -4962,13 +5008,14 @@ namespace karto */ inline void SetRangeReadings(const RangeReadingsVector& rRangeReadings) { - // ignore for now! XXXMAE BUGUBUG 05/21/2010 - //if (rRangeReadings.size() != GetNumberOfRangeReadings()) - //{ - // std::stringstream error; - // error << "Given number of readings (" << rRangeReadings.size() << ") does not match expected number of range finder (" << GetNumberOfRangeReadings() << ")"; - // throw Exception(error.str()); - //} + // ignore for now! XXXMAE BUGUBUG 05/21/2010 << TODO(lucbettaieb): What the heck is this?? + // if (rRangeReadings.size() != GetNumberOfRangeReadings()) + // { + // std::stringstream error; + // error << "Given number of readings (" << rRangeReadings.size() + // << ") does not match expected number of range finder (" << GetNumberOfRangeReadings() << ")"; + // throw Exception(error.str()); + // } if (!rRangeReadings.empty()) { @@ -5023,7 +5070,7 @@ namespace karto private: kt_double* m_pRangeReadings; kt_int32u m_NumberOfRangeReadings; - }; // LaserRangeScan + }; // LaserRangeScan //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -5035,9 +5082,9 @@ namespace karto class DrivePose : public SensorData { public: - //@cond EXCLUDE + // @cond EXCLUDE KARTO_Object(DrivePose) - //@endcond + // @endcond public: /** @@ -5084,7 +5131,7 @@ namespace karto * Odometric pose of robot */ Pose3 m_OdometricPose; - }; // class DrivePose + }; // class DrivePose //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -5099,9 +5146,9 @@ namespace karto class LocalizedRangeScan : public LaserRangeScan { public: - //@cond EXCLUDE + // @cond EXCLUDE KARTO_Object(LocalizedRangeScan) - //@endcond + // @endcond public: /** @@ -5395,7 +5442,7 @@ namespace karto * Internal flag used to update point readings, barycenter and bounding box */ kt_bool m_IsDirty; - }; // LocalizedRangeScan + }; // LocalizedRangeScan /** * Type declaration of LocalizedRangeScan vector @@ -5424,7 +5471,7 @@ namespace karto private: OccupancyGrid* m_pOccupancyGrid; - }; // CellUpdater + }; // CellUpdater /** * Occupancy grid definition. See GridStates for possible grid values. @@ -5504,7 +5551,10 @@ namespace karto */ OccupancyGrid* Clone() const { - OccupancyGrid* pOccupancyGrid = new OccupancyGrid(GetWidth(), GetHeight(), GetCoordinateConverter()->GetOffset(), 1.0 / GetCoordinateConverter()->GetScale()); + OccupancyGrid* pOccupancyGrid = new OccupancyGrid(GetWidth(), + GetHeight(), + GetCoordinateConverter()->GetOffset(), + 1.0 / GetCoordinateConverter()->GetScale()); memcpy(pOccupancyGrid->GetDataPointer(), GetDataPointer(), GetDataSize()); pOccupancyGrid->GetCoordinateConverter()->SetSize(GetCoordinateConverter()->GetSize()); @@ -5521,7 +5571,7 @@ namespace karto */ virtual kt_bool IsFree(const Vector2& rPose) const { - kt_int8u* pOffsets = (kt_int8u*)GetDataPointer(rPose); + kt_int8u* pOffsets = reinterpret_cast(GetDataPointer(rPose)); if (*pOffsets == GridStates_Free) { return true; @@ -5624,7 +5674,11 @@ namespace karto * @param rHeight * @param rOffset */ - static void ComputeDimensions(const LocalizedRangeScanVector& rScans, kt_double resolution, kt_int32s& rWidth, kt_int32s& rHeight, Vector2& rOffset) + static void ComputeDimensions(const LocalizedRangeScanVector& rScans, + kt_double resolution, + kt_int32s& rWidth, + kt_int32s& rHeight, + Vector2& rOffset) { BoundingBox2 boundingBox; const_forEach(LocalizedRangeScanVector, &rScans) @@ -5725,7 +5779,10 @@ namespace karto * @param doUpdate whether to update the cells' occupancy status immediately * @return returns false if an endpoint fell off the grid, otherwise true */ - virtual kt_bool RayTrace(const Vector2& rWorldFrom, const Vector2& rWorldTo, kt_bool isEndPointValid, kt_bool doUpdate = false) + virtual kt_bool RayTrace(const Vector2& rWorldFrom, + const Vector2& rWorldTo, + kt_bool isEndPointValid, + kt_bool doUpdate = false) { assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL); @@ -5851,7 +5908,7 @@ namespace karto // Minimum ratio of beams hitting cell to beams passing through cell for cell to be marked as occupied Parameter* m_pOccupancyThreshold; - }; // OccupancyGrid + }; // OccupancyGrid //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -5864,9 +5921,9 @@ namespace karto class DatasetInfo : public Object { public: - //@cond EXCLUDE + // @cond EXCLUDE KARTO_Object(DatasetInfo) - //@endcond + // @endcond public: DatasetInfo() @@ -5924,7 +5981,7 @@ namespace karto Parameter* m_pAuthor; Parameter* m_pDescription; Parameter* m_pCopyright; - }; // class DatasetInfo + }; // class DatasetInfo //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -5965,7 +6022,7 @@ namespace karto if (dynamic_cast(pObject)) { Sensor* pSensor = dynamic_cast(pObject); - if(pSensor != NULL) + if (pSensor != NULL) { m_SensorNameLookup[pSensor->GetName()] = pSensor; @@ -6036,7 +6093,7 @@ namespace karto std::map m_SensorNameLookup; ObjectVector m_Objects; DatasetInfo* m_pDatasetInfo; - }; // Dataset + }; // Dataset //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -6155,7 +6212,7 @@ namespace karto kt_int32s* m_pArray; kt_int32u m_Capacity; kt_int32u m_Size; - }; // LookupArray + }; // LookupArray //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -6227,7 +6284,10 @@ namespace karto * @param angleOffset computes lookup arrays for the angles within this offset around angleStart * @param angleResolution how fine a granularity to compute lookup arrays in the angular space */ - void ComputeOffsets(LocalizedRangeScan* pScan, kt_double angleCenter, kt_double angleOffset, kt_double angleResolution) + void ComputeOffsets(LocalizedRangeScan* pScan, + kt_double angleCenter, + kt_double angleOffset, + kt_double angleResolution) { assert(angleOffset != 0.0); assert(angleResolution != 0.0); @@ -6260,7 +6320,7 @@ namespace karto angle = startAngle + angleIndex * angleResolution; ComputeOffsets(angleIndex, angle, localPoints); } - //assert(math::DoubleEqual(angle, angleCenter + angleOffset)); + // assert(math::DoubleEqual(angle, angleCenter + angleOffset)); } private: @@ -6293,8 +6353,8 @@ namespace karto // counterclockwise rotation and that rotation is about the origin (0, 0). Vector2 offset; - offset.SetX(cosine * rPosition.GetX() - sine * rPosition.GetY()); - offset.SetY( sine * rPosition.GetX() + cosine * rPosition.GetY()); + offset.SetX(cosine * rPosition.GetX() - sine * rPosition.GetY()); + offset.SetY(sine * rPosition.GetX() + cosine * rPosition.GetY()); // have to compensate for the grid offset when getting the grid index Vector2 gridPoint = m_pGrid->WorldToGrid(offset + rGridOffset); @@ -6361,7 +6421,7 @@ namespace karto // for sanity check std::vector m_Angles; - }; // class GridIndexLookup + }; // class GridIndexLookup //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -6380,7 +6440,7 @@ namespace karto //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// - //@cond EXCLUDE + // @cond EXCLUDE template inline void Object::SetParameter(const std::string& rName, T value) @@ -6412,9 +6472,9 @@ namespace karto } } - //@endcond + // @endcond /*@}*/ -} +} // namespace karto -#endif // __KARTO_KARTO__ +#endif // OPEN_KARTO_KARTO_H diff --git a/include/open_karto/Macros.h b/include/open_karto/Macros.h index f339695..d257581 100644 --- a/include/open_karto/Macros.h +++ b/include/open_karto/Macros.h @@ -15,8 +15,8 @@ * along with this program. If not, see . */ -#ifndef __KARTO_MACROS__ -#define __KARTO_MACROS__ +#ifndef OPEN_KARTO_MACROS_H +#define OPEN_KARTO_MACROS_H //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -26,7 +26,7 @@ * Karto defines for handling deprecated code */ #ifndef KARTO_DEPRECATED -# if defined(__GNUC__) && (__GNUC__ >= 4 || (__GNUC__==3 && __GNUC_MINOR__>=1)) +# if defined(__GNUC__) && (__GNUC__ >= 4 || (__GNUC__ == 3 && __GNUC_MINOR__>=1)) # define KARTO_DEPRECATED __attribute__((deprecated)) # elif defined(__INTEL) || defined(_MSC_VER) # define KARTO_DEPRECATED __declspec(deprecated) @@ -50,7 +50,7 @@ # define KARTO_EXPORT __declspec(dllexport) # else # define KARTO_EXPORT __declspec(dllimport) -# endif // KARTO_DYNAMIC +# endif // KARTO_DYNAMIC # endif #else # define KARTO_EXPORT @@ -122,6 +122,6 @@ // remark #10121: #pragma warning(disable:10121) -#endif // __INTEL_COMPILER +#endif // __INTEL_COMPILER -#endif // __KARTO_MACROS__ +#endif // OPEN_KARTO_MACROS_H diff --git a/include/open_karto/Mapper.h b/include/open_karto/Mapper.h index 128991e..6959068 100644 --- a/include/open_karto/Mapper.h +++ b/include/open_karto/Mapper.h @@ -15,13 +15,13 @@ * along with this program. If not, see . */ -#ifndef __KARTO_MAPPER__ -#define __KARTO_MAPPER__ +#ifndef OPEN_KARTO_MAPPER_H +#define OPEN_KARTO_MAPPER_H #include #include -#include "Karto.h" +#include namespace karto { @@ -72,7 +72,7 @@ namespace karto * Called when loop closure is over */ virtual void EndLoopClosure(const std::string& /*rInfo*/) {}; - }; // MapperListener + }; // MapperListener //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -97,7 +97,7 @@ namespace karto virtual ~EdgeLabel() { } - }; // EdgeLabel + }; // EdgeLabel //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -191,7 +191,7 @@ namespace karto Pose2 m_Pose2; Pose2 m_PoseDifference; Matrix3 m_Covariance; - }; // LinkInfo + }; // LinkInfo //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -282,7 +282,7 @@ namespace karto T* m_pObject; std::vector*> m_Edges; - }; // Vertex + }; // Vertex //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -365,7 +365,7 @@ namespace karto Vertex* m_pSource; Vertex* m_pTarget; EdgeLabel* m_pLabel; - }; // class Edge + }; // class Edge //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -384,7 +384,7 @@ namespace karto * @return true if the visitor accepted the vertex, false otherwise */ virtual kt_bool Visit(Vertex* pVertex) = 0; - }; // Visitor + }; // Visitor //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -429,7 +429,7 @@ namespace karto * Graph being traversed */ Graph* m_pGraph; - }; // GraphTraversal + }; // GraphTraversal //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -533,7 +533,7 @@ namespace karto * Edges of this graph */ std::vector*> m_Edges; - }; // Graph + }; // Graph //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -575,7 +575,9 @@ namespace karto * @param rIsNewEdge set to true if the edge is new * @return edge between source and target scan vertices */ - Edge* AddEdge(LocalizedRangeScan* pSourceScan, LocalizedRangeScan* pTargetScan, kt_bool& rIsNewEdge); + Edge* AddEdge(LocalizedRangeScan* pSourceScan, + LocalizedRangeScan* pTargetScan, + kt_bool& rIsNewEdge); /** * Link scan to last scan and nearby chains of scans @@ -632,7 +634,10 @@ namespace karto * @param rMean * @param rCovariance */ - void LinkScans(LocalizedRangeScan* pFromScan, LocalizedRangeScan* pToScan, const Pose2& rMean, const Matrix3& rCovariance); + void LinkScans(LocalizedRangeScan* pFromScan, + LocalizedRangeScan* pToScan, + const Pose2& rMean, + const Matrix3& rCovariance); /** * Find nearby chains of scans and link them to scan if response is high enough @@ -649,7 +654,10 @@ namespace karto * @param rMean * @param rCovariance */ - void LinkChainToScan(const LocalizedRangeScanVector& rChain, LocalizedRangeScan* pScan, const Pose2& rMean, const Matrix3& rCovariance); + void LinkChainToScan(const LocalizedRangeScanVector& rChain, + LocalizedRangeScan* pScan, + const Pose2& rMean, + const Matrix3& rCovariance); /** * Find chains of scans that are close to given scan @@ -674,7 +682,9 @@ namespace karto * @param rStartNum * @return chain that can possibly close a loop with given scan */ - LocalizedRangeScanVector FindPossibleLoopClosure(LocalizedRangeScan* pScan, const Name& rSensorName, kt_int32u& rStartNum); + LocalizedRangeScanVector FindPossibleLoopClosure(LocalizedRangeScan* pScan, + const Name& rSensorName, + kt_int32u& rStartNum); /** * Optimizes scan poses @@ -696,7 +706,7 @@ namespace karto * Traversal algorithm to find near linked scans */ GraphTraversal* m_pTraversal; - }; // MapperGraph + }; // MapperGraph //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -771,7 +781,7 @@ namespace karto * Resets the solver */ virtual void Clear() {}; - }; // ScanSolver + }; // ScanSolver //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -800,7 +810,10 @@ namespace karto * @param smearDeviation * @return correlation grid */ - static CorrelationGrid* CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution, kt_double smearDeviation) + static CorrelationGrid* CreateGrid(kt_int32s width, + kt_int32s height, + kt_double resolution, + kt_double smearDeviation) { assert(resolution != 0.0); @@ -893,7 +906,8 @@ namespace karto * @param resolution * @param smearDeviation */ - CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize, kt_double resolution, kt_double smearDeviation) + CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize, + kt_double resolution, kt_double smearDeviation) : Grid(width + borderSize * 2, height + borderSize * 2) , m_SmearDeviation(smearDeviation) , m_pKernel(NULL) @@ -926,7 +940,10 @@ namespace karto if (!math::InRange(m_SmearDeviation, MIN_SMEAR_DISTANCE_DEVIATION, MAX_SMEAR_DISTANCE_DEVIATION)) { std::stringstream error; - error << "Mapper Error: Smear deviation too small: Must be between " << MIN_SMEAR_DISTANCE_DEVIATION << " and " << MAX_SMEAR_DISTANCE_DEVIATION; + error << "Mapper Error: Smear deviation too small: Must be between " + << MIN_SMEAR_DISTANCE_DEVIATION + << " and " + << MAX_SMEAR_DISTANCE_DEVIATION; throw std::runtime_error(error.str()); } @@ -993,7 +1010,7 @@ namespace karto // region of interest Rectangle2 m_Roi; - }; // CorrelationGrid + }; // CorrelationGrid //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -1014,7 +1031,11 @@ namespace karto /** * Create a scan matcher with the given parameters */ - static ScanMatcher* Create(Mapper* pMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold); + static ScanMatcher* Create(Mapper* pMapper, + kt_double searchSize, + kt_double resolution, + kt_double smearDeviation, + kt_double rangeThreshold); /** * Match given scan against set of scans @@ -1026,8 +1047,11 @@ namespace karto * @param doRefineMatch whether to do finer-grained matching if coarse match is good (default is true) * @return strength of response */ - kt_double MatchScan(LocalizedRangeScan* pScan, const LocalizedRangeScanVector& rBaseScans, Pose2& rMean, Matrix3& rCovariance, - kt_bool doPenalize = true, kt_bool doRefineMatch = true); + kt_double MatchScan(LocalizedRangeScan* pScan, + const LocalizedRangeScanVector& rBaseScans, + Pose2& rMean, Matrix3& rCovariance, + kt_bool doPenalize = true, + kt_bool doRefineMatch = true); /** * Finds the best pose for the scan centering the search in the correlation grid @@ -1045,8 +1069,16 @@ namespace karto * @param doingFineMatch whether to do a finer search after coarse search * @return strength of response */ - kt_double CorrelateScan(LocalizedRangeScan* pScan, const Pose2& rSearchCenter, const Vector2& rSearchSpaceOffset, const Vector2& rSearchSpaceResolution, - kt_double searchAngleOffset, kt_double searchAngleResolution, kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, kt_bool doingFineMatch); + kt_double CorrelateScan(LocalizedRangeScan* pScan, + const Pose2& rSearchCenter, + const Vector2& rSearchSpaceOffset, + const Vector2& rSearchSpaceResolution, + kt_double searchAngleOffset, + kt_double searchAngleResolution, + kt_bool doPenalize, + Pose2& rMean, + Matrix3& rCovariance, + kt_bool doingFineMatch); /** * Computes the positional covariance of the best pose @@ -1058,9 +1090,13 @@ namespace karto * @param searchAngleResolution * @param rCovariance */ - void ComputePositionalCovariance(const Pose2& rBestPose, kt_double bestResponse, const Pose2& rSearchCenter, - const Vector2& rSearchSpaceOffset, const Vector2& rSearchSpaceResolution, - kt_double searchAngleResolution, Matrix3& rCovariance); + void ComputePositionalCovariance(const Pose2& rBestPose, + kt_double bestResponse, + const Pose2& rSearchCenter, + const Vector2& rSearchSpaceOffset, + const Vector2& rSearchSpaceResolution, + kt_double searchAngleResolution, + Matrix3& rCovariance); /** * Computes the angular covariance of the best pose @@ -1071,8 +1107,12 @@ namespace karto * @param searchAngleResolution * @param rCovariance */ - void ComputeAngularCovariance(const Pose2& rBestPose, kt_double bestResponse, const Pose2& rSearchCenter, - kt_double searchAngleOffset, kt_double searchAngleResolution, Matrix3& rCovariance); + void ComputeAngularCovariance(const Pose2& rBestPose, + kt_double bestResponse, + const Pose2& rSearchCenter, + kt_double searchAngleOffset, + kt_double searchAngleResolution, + Matrix3& rCovariance); /** * Gets the correlation grid data (for debugging) @@ -1134,8 +1174,7 @@ namespace karto Grid* m_pSearchSpaceProbs; GridIndexLookup* m_pGridLookup; - - }; // ScanMatcher + }; // ScanMatcher //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -1146,7 +1185,7 @@ namespace karto /** * Manages the devices for the mapper */ - class KARTO_EXPORT MapperSensorManager// : public SensorManager + class KARTO_EXPORT MapperSensorManager // : public SensorManager { typedef std::map ScanManagerMap; @@ -1279,7 +1318,7 @@ namespace karto */ inline ScanManager* GetScanManager(const Name& rSensorName) { - if(m_ScanManagers.find(rSensorName) != m_ScanManagers.end()) + if (m_ScanManagers.find(rSensorName) != m_ScanManagers.end()) { return m_ScanManagers[rSensorName]; } @@ -1297,7 +1336,7 @@ namespace karto kt_int32s m_NextScanId; std::vector m_Scans; - }; // MapperSensorManager + }; // MapperSensorManager //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -1826,12 +1865,12 @@ namespace karto // whether to increase the search space if no good matches are initially found Parameter* m_pUseResponseExpansion; - + public: /* Abstract methods for parameter setters and getters */ /* Getters */ - //General Parameters + // General Parameters bool getParamUseScanMatching(); bool getParamUseScanBarycenter(); double getParamMinimumTravelDistance(); @@ -1847,17 +1886,17 @@ namespace karto double getParamLoopMatchMinimumResponseCoarse(); double getParamLoopMatchMinimumResponseFine(); - //Correlation Parameters - Correlation Parameters + // Correlation Parameters - Correlation Parameters double getParamCorrelationSearchSpaceDimension(); double getParamCorrelationSearchSpaceResolution(); double getParamCorrelationSearchSpaceSmearDeviation(); - //Correlation Parameters - Loop Closure Parameters + // Correlation Parameters - Loop Closure Parameters double getParamLoopSearchSpaceDimension(); double getParamLoopSearchSpaceResolution(); double getParamLoopSearchSpaceSmearDeviation(); - //Scan Matcher Parameters + // Scan Matcher Parameters double getParamDistanceVariancePenalty(); double getParamAngleVariancePenalty(); double getParamFineSearchAngleOffset(); @@ -1868,7 +1907,7 @@ namespace karto bool getParamUseResponseExpansion(); /* Setters */ - //General Parameters + // General Parameters void setParamUseScanMatching(bool b); void setParamUseScanBarycenter(bool b); void setParamMinimumTravelDistance(double d); @@ -1884,17 +1923,17 @@ namespace karto void setParamLoopMatchMinimumResponseCoarse(double d); void setParamLoopMatchMinimumResponseFine(double d); - //Correlation Parameters - Correlation Parameters + // Correlation Parameters - Correlation Parameters void setParamCorrelationSearchSpaceDimension(double d); void setParamCorrelationSearchSpaceResolution(double d); void setParamCorrelationSearchSpaceSmearDeviation(double d); - //Correlation Parameters - Loop Closure Parameters + // Correlation Parameters - Loop Closure Parameters void setParamLoopSearchSpaceDimension(double d); void setParamLoopSearchSpaceResolution(double d); void setParamLoopSearchSpaceSmearDeviation(double d); - //Scan Matcher Parameters + // Scan Matcher Parameters void setParamDistanceVariancePenalty(double d); void setParamAngleVariancePenalty(double d); void setParamFineSearchAngleOffset(double d); @@ -1903,12 +1942,7 @@ namespace karto void setParamMinimumAnglePenalty(double d); void setParamMinimumDistancePenalty(double d); void setParamUseResponseExpansion(bool b); - - - - }; +} // namespace karto -} - -#endif // __KARTO_MAPPER__ +#endif // OPEN_KARTO_MAPPER_H diff --git a/include/open_karto/Math.h b/include/open_karto/Math.h index 4d18b9a..a595133 100644 --- a/include/open_karto/Math.h +++ b/include/open_karto/Math.h @@ -15,13 +15,13 @@ * along with this program. If not, see . */ -#ifndef __KARTO_MATH__ -#define __KARTO_MATH__ +#ifndef OPEN_KARTO_MATH_H +#define OPEN_KARTO_MATH_H #include #include -#include "Types.h" +#include namespace karto { @@ -238,8 +238,8 @@ namespace karto { return static_cast ((value + (alignValue - 1)) & ~(alignValue - 1)); } - } // Math + } // namespace math -} +} // namespace karto -#endif // __KARTO_MATH__ +#endif // OPEN_KARTO_MATH_H diff --git a/include/open_karto/Types.h b/include/open_karto/Types.h index 287ee03..b4b6fc5 100644 --- a/include/open_karto/Types.h +++ b/include/open_karto/Types.h @@ -15,8 +15,8 @@ * along with this program. If not, see . */ -#ifndef __KARTO_TYPES__ -#define __KARTO_TYPES__ +#ifndef OPEN_KARTO_TYPES_H +#define OPEN_KARTO_TYPES_H #include @@ -66,4 +66,4 @@ typedef char kt_char; typedef float kt_float; typedef double kt_double; -#endif // __KARTO_TYPES__ +#endif // OPEN_KARTO_TYPES_H diff --git a/src/Karto.cpp b/src/Karto.cpp index ab35939..58eb361 100644 --- a/src/Karto.cpp +++ b/src/Karto.cpp @@ -139,7 +139,10 @@ namespace karto //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// - const PointVectorDouble LaserRangeFinder::GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan, CoordinateConverter* pCoordinateConverter, kt_bool ignoreThresholdPoints, kt_bool flipY) const + const PointVectorDouble LaserRangeFinder::GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan, + CoordinateConverter* pCoordinateConverter, + kt_bool ignoreThresholdPoints, + kt_bool flipY) const { PointVectorDouble pointReadings; @@ -189,9 +192,10 @@ namespace karto LaserRangeScan* pLaserRangeScan = dynamic_cast(pSensorData); // verify number of range readings in LaserRangeScan matches the number of expected range readings - if(pLaserRangeScan->GetNumberOfRangeReadings() != GetNumberOfRangeReadings()) + if (pLaserRangeScan->GetNumberOfRangeReadings() != GetNumberOfRangeReadings()) { - std::cout << "LaserRangeScan contains " << pLaserRangeScan->GetNumberOfRangeReadings() << " range readings, expected " << GetNumberOfRangeReadings() << std::endl; + std::cout << "LaserRangeScan contains " << pLaserRangeScan->GetNumberOfRangeReadings() + << " range readings, expected " << GetNumberOfRangeReadings() << std::endl; return false; } @@ -250,5 +254,4 @@ namespace karto "Hokuyo_URG_04LX" }; */ -} - +} // namespace karto diff --git a/src/Mapper.cpp b/src/Mapper.cpp index 6c3d481..1069382 100644 --- a/src/Mapper.cpp +++ b/src/Mapper.cpp @@ -32,7 +32,7 @@ namespace karto { // enable this for verbose debug information - //#define KARTO_DEBUG + // #define KARTO_DEBUG #define MAX_VARIANCE 500.0 #define DISTANCE_PENALTY_GAIN 0.2 @@ -134,7 +134,8 @@ namespace karto // cap vector size and remove all scans from front of vector that are too far from end of vector kt_double squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition()); - while (m_RunningScans.size() > m_RunningBufferMaximumSize || squaredDistance > math::Square(m_RunningBufferMaximumDistance) - KT_TOLERANCE) + while (m_RunningScans.size() > m_RunningBufferMaximumSize || + squaredDistance > math::Square(m_RunningBufferMaximumDistance) - KT_TOLERANCE) { // remove front of running scans m_RunningScans.erase(m_RunningScans.begin()); @@ -162,7 +163,7 @@ namespace karto kt_int32u m_RunningBufferMaximumSize; kt_double m_RunningBufferMaximumDistance; - }; // ScanManager + }; // ScanManager //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -170,7 +171,7 @@ namespace karto void MapperSensorManager::RegisterSensor(const Name& rSensorName) { - if(GetScanManager(rSensorName) == NULL) + if (GetScanManager(rSensorName) == NULL) { m_ScanManagers[rSensorName] = new ScanManager(m_RunningBufferMaximumSize, m_RunningBufferMaximumDistance); } @@ -186,7 +187,7 @@ namespace karto LocalizedRangeScan* MapperSensorManager::GetScan(const Name& rSensorName, kt_int32s scanIndex) { ScanManager* pScanManager = GetScanManager(rSensorName); - if(pScanManager != NULL) + if (pScanManager != NULL) { return pScanManager->GetScans().at(scanIndex); } @@ -300,7 +301,8 @@ namespace karto delete m_pGridLookup; } - ScanMatcher* ScanMatcher::Create(Mapper* pMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold) + ScanMatcher* ScanMatcher::Create(Mapper* pMapper, kt_double searchSize, kt_double resolution, + kt_double smearDeviation, kt_double rangeThreshold) { // invalid parameters if (resolution <= 0) @@ -336,7 +338,8 @@ namespace karto CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(gridSize, gridSize, resolution, smearDeviation); // create search space probabilities - Grid* pSearchSpaceProbs = Grid::CreateGrid(searchSpaceSideSize, searchSpaceSideSize, resolution); + Grid* pSearchSpaceProbs = Grid::CreateGrid(searchSpaceSideSize, + searchSpaceSideSize, resolution); ScanMatcher* pScanMatcher = new ScanMatcher(pMapper); pScanMatcher->m_pCorrelationGrid = pCorrelationGrid; @@ -356,8 +359,8 @@ namespace karto * @param doRefineMatch whether to do finer-grained matching if coarse match is good (default is true) * @return strength of response */ - kt_double ScanMatcher::MatchScan(LocalizedRangeScan* pScan, const LocalizedRangeScanVector& rBaseScans, Pose2& rMean, Matrix3& rCovariance, - kt_bool doPenalize, kt_bool doRefineMatch) + kt_double ScanMatcher::MatchScan(LocalizedRangeScan* pScan, const LocalizedRangeScanVector& rBaseScans, Pose2& rMean, + Matrix3& rCovariance, kt_bool doPenalize, kt_bool doRefineMatch) { /////////////////////////////////////// // set scan pose to be center of grid @@ -372,9 +375,9 @@ namespace karto rMean = scanPose; // maximum covariance - rCovariance(0, 0) = MAX_VARIANCE; // XX - rCovariance(1, 1) = MAX_VARIANCE; // YY - rCovariance(2, 2) = 4 * math::Square(m_pMapper->m_pCoarseAngleResolution->GetValue()); // TH*TH + rCovariance(0, 0) = MAX_VARIANCE; // XX + rCovariance(1, 1) = MAX_VARIANCE; // YY + rCovariance(2, 2) = 4 * math::Square(m_pMapper->m_pCoarseAngleResolution->GetValue()); // TH*TH return 0.0; } @@ -397,13 +400,18 @@ namespace karto // compute how far to search in each direction Vector2 searchDimensions(m_pSearchSpaceProbs->GetWidth(), m_pSearchSpaceProbs->GetHeight()); - Vector2 coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * m_pCorrelationGrid->GetResolution(), 0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution()); + Vector2 coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * m_pCorrelationGrid->GetResolution(), + 0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution()); // a coarse search only checks half the cells in each dimension - Vector2 coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(), 2 * m_pCorrelationGrid->GetResolution()); + Vector2 coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(), + 2 * m_pCorrelationGrid->GetResolution()); // actual scan-matching - kt_double bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution, m_pMapper->m_pCoarseSearchAngleOffset->GetValue(), m_pMapper->m_pCoarseAngleResolution->GetValue(), doPenalize, rMean, rCovariance, false); + kt_double bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution, + m_pMapper->m_pCoarseSearchAngleOffset->GetValue(), + m_pMapper->m_pCoarseAngleResolution->GetValue(), + doPenalize, rMean, rCovariance, false); if (m_pMapper->m_pUseResponseExpansion->GetValue() == true) { @@ -418,7 +426,9 @@ namespace karto { newSearchAngleOffset += math::DegreesToRadians(20); - bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution, newSearchAngleOffset, m_pMapper->m_pCoarseAngleResolution->GetValue(), doPenalize, rMean, rCovariance, false); + bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution, + newSearchAngleOffset, m_pMapper->m_pCoarseAngleResolution->GetValue(), + doPenalize, rMean, rCovariance, false); if (math::DoubleEqual(bestResponse, 0.0) == false) { @@ -439,13 +449,16 @@ namespace karto { Vector2 fineSearchOffset(coarseSearchResolution * 0.5); Vector2 fineSearchResolution(m_pCorrelationGrid->GetResolution(), m_pCorrelationGrid->GetResolution()); - bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset, fineSearchResolution, 0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(), m_pMapper->m_pFineSearchAngleOffset->GetValue(), doPenalize, rMean, rCovariance, true); + bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset, fineSearchResolution, + 0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(), + m_pMapper->m_pFineSearchAngleOffset->GetValue(), + doPenalize, rMean, rCovariance, true); } #ifdef KARTO_DEBUG - std::cout << " BEST POSE = " << rMean << " BEST RESPONSE = " << bestResponse << ", VARIANCE = " << rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl; + std::cout << " BEST POSE = " << rMean << " BEST RESPONSE = " << bestResponse << ", VARIANCE = " + << rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl; #endif - assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI)); return bestResponse; @@ -467,8 +480,11 @@ namespace karto * @param doingFineMatch whether to do a finer search after coarse search * @return strength of response */ - kt_double ScanMatcher::CorrelateScan(LocalizedRangeScan* pScan, const Pose2& rSearchCenter, const Vector2& rSearchSpaceOffset, const Vector2& rSearchSpaceResolution, - kt_double searchAngleOffset, kt_double searchAngleResolution, kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, kt_bool doingFineMatch) + kt_double ScanMatcher::CorrelateScan(LocalizedRangeScan* pScan, const Pose2& rSearchCenter, + const Vector2& rSearchSpaceOffset, + const Vector2& rSearchSpaceResolution, + kt_double searchAngleOffset, kt_double searchAngleResolution, + kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, kt_bool doingFineMatch) { assert(searchAngleResolution != 0.0); @@ -488,7 +504,8 @@ namespace karto // calculate position arrays std::vector xPoses; - kt_int32u nX = static_cast(math::Round(rSearchSpaceOffset.GetX() * 2.0 / rSearchSpaceResolution.GetX()) + 1); + kt_int32u nX = static_cast(math::Round(rSearchSpaceOffset.GetX() * + 2.0 / rSearchSpaceResolution.GetX()) + 1); kt_double startX = -rSearchSpaceOffset.GetX(); for (kt_int32u xIndex = 0; xIndex < nX; xIndex++) { @@ -497,7 +514,8 @@ namespace karto assert(math::DoubleEqual(xPoses.back(), -startX)); std::vector yPoses; - kt_int32u nY = static_cast(math::Round(rSearchSpaceOffset.GetY() * 2.0 / rSearchSpaceResolution.GetY()) + 1); + kt_int32u nY = static_cast(math::Round(rSearchSpaceOffset.GetY() * + 2.0 / rSearchSpaceResolution.GetY()) + 1); kt_double startY = -rSearchSpaceOffset.GetY(); for (kt_int32u yIndex = 0; yIndex < nY; yIndex++) { @@ -513,7 +531,8 @@ namespace karto // allocate array std::pair* pPoseResponse = new std::pair[poseResponseSize]; - Vector2 startGridPoint = m_pCorrelationGrid->WorldToGrid(Vector2(rSearchCenter.GetX() + startX, rSearchCenter.GetY() + startY)); + Vector2 startGridPoint = m_pCorrelationGrid->WorldToGrid(Vector2(rSearchCenter.GetX() + + startX, rSearchCenter.GetY() + startY)); kt_int32u poseResponseCounter = 0; forEachAs(std::vector, &yPoses, yIter) @@ -544,18 +563,21 @@ namespace karto // simple model (approximate Gaussian) to take odometry into account kt_double squaredDistance = squareX + squareY; - kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN * squaredDistance / m_pMapper->m_pDistanceVariancePenalty->GetValue()); + kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN * + squaredDistance / m_pMapper->m_pDistanceVariancePenalty->GetValue()); distancePenalty = math::Maximum(distancePenalty, m_pMapper->m_pMinimumDistancePenalty->GetValue()); kt_double squaredAngleDistance = math::Square(angle - rSearchCenter.GetHeading()); - kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN * squaredAngleDistance / m_pMapper->m_pAngleVariancePenalty->GetValue()); + kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN * + squaredAngleDistance / m_pMapper->m_pAngleVariancePenalty->GetValue()); anglePenalty = math::Maximum(anglePenalty, m_pMapper->m_pMinimumAnglePenalty->GetValue()); response *= (distancePenalty * anglePenalty); } // store response and pose - pPoseResponse[poseResponseCounter] = std::pair(response, Pose2(newPositionX, newPositionY, math::NormalizeAngle(angle))); + pPoseResponse[poseResponseCounter] = std::pair(response, Pose2(newPositionX, newPositionY, + math::NormalizeAngle(angle))); poseResponseCounter++; } @@ -577,7 +599,8 @@ namespace karto const Pose2& rPose = pPoseResponse[i].second; Vector2 grid = m_pSearchSpaceProbs->WorldToGrid(rPose.GetPosition()); - kt_double* ptr = (kt_double*)m_pSearchSpaceProbs->GetDataPointer(grid); + // Changed (kt_double*) to the reinterpret_cast - Luc + kt_double* ptr = reinterpret_cast(m_pSearchSpaceProbs->GetDataPointer(grid)); if (ptr == NULL) { throw std::runtime_error("Mapper FATAL ERROR - Index out of range in probability search!"); @@ -631,11 +654,13 @@ namespace karto if (!doingFineMatch) { - ComputePositionalCovariance(averagePose, bestResponse, rSearchCenter, rSearchSpaceOffset, rSearchSpaceResolution, searchAngleResolution, rCovariance); + ComputePositionalCovariance(averagePose, bestResponse, rSearchCenter, rSearchSpaceOffset, + rSearchSpaceResolution, searchAngleResolution, rCovariance); } else { - ComputeAngularCovariance(averagePose, bestResponse, rSearchCenter, searchAngleOffset, searchAngleResolution, rCovariance); + ComputeAngularCovariance(averagePose, bestResponse, rSearchCenter, + searchAngleOffset, searchAngleResolution, rCovariance); } rMean = averagePose; @@ -665,8 +690,10 @@ namespace karto * @param searchAngleResolution * @param rCovariance */ - void ScanMatcher::ComputePositionalCovariance(const Pose2& rBestPose, kt_double bestResponse, const Pose2& rSearchCenter, - const Vector2& rSearchSpaceOffset, const Vector2& rSearchSpaceResolution, + void ScanMatcher::ComputePositionalCovariance(const Pose2& rBestPose, kt_double bestResponse, + const Pose2& rSearchCenter, + const Vector2& rSearchSpaceOffset, + const Vector2& rSearchSpaceResolution, kt_double searchAngleResolution, Matrix3& rCovariance) { // reset covariance to identity matrix @@ -675,9 +702,9 @@ namespace karto // if best response is vary small return max variance if (bestResponse < KT_TOLERANCE) { - rCovariance(0, 0) = MAX_VARIANCE; // XX - rCovariance(1, 1) = MAX_VARIANCE; // YY - rCovariance(2, 2) = 4 * math::Square(searchAngleResolution); // TH*TH + rCovariance(0, 0) = MAX_VARIANCE; // XX + rCovariance(1, 1) = MAX_VARIANCE; // YY + rCovariance(2, 2) = 4 * math::Square(searchAngleResolution); // TH*TH return; } @@ -709,7 +736,8 @@ namespace karto { kt_double x = startX + xIndex * rSearchSpaceResolution.GetX(); - Vector2 gridPoint = m_pSearchSpaceProbs->WorldToGrid(Vector2(rSearchCenter.GetX() + x, rSearchCenter.GetY() + y)); + Vector2 gridPoint = m_pSearchSpaceProbs->WorldToGrid(Vector2(rSearchCenter.GetX() + x, + rSearchCenter.GetY() + y)); kt_double response = *(m_pSearchSpaceProbs->GetDataPointer(gridPoint)); // response is not a low response @@ -743,7 +771,7 @@ namespace karto rCovariance(0, 1) = varianceXY * multiplier; rCovariance(1, 0) = varianceXY * multiplier; rCovariance(1, 1) = varianceYY * multiplier; - rCovariance(2, 2) = varianceTHTH; // this value will be set in ComputeAngularCovariance + rCovariance(2, 2) = varianceTHTH; // this value will be set in ComputeAngularCovariance } // if values are 0, set to MAX_VARIANCE @@ -768,8 +796,12 @@ namespace karto * @param searchAngleResolution * @param rCovariance */ - void ScanMatcher::ComputeAngularCovariance(const Pose2& rBestPose, kt_double bestResponse, const Pose2& rSearchCenter, - kt_double searchAngleOffset, kt_double searchAngleResolution, Matrix3& rCovariance) + void ScanMatcher::ComputeAngularCovariance(const Pose2& rBestPose, + kt_double bestResponse, + const Pose2& rSearchCenter, + kt_double searchAngleOffset, + kt_double searchAngleResolution, + Matrix3& rCovariance) { // NOTE: do not reset covariance matrix @@ -847,7 +879,8 @@ namespace karto const_forEach(PointVectorDouble, &validPoints) { Vector2 gridPoint = m_pCorrelationGrid->WorldToGrid(*iter); - if (!math::IsUpTo(gridPoint.GetX(), m_pCorrelationGrid->GetROI().GetWidth()) || !math::IsUpTo(gridPoint.GetY(), m_pCorrelationGrid->GetROI().GetHeight())) + if (!math::IsUpTo(gridPoint.GetX(), m_pCorrelationGrid->GetROI().GetWidth()) || + !math::IsUpTo(gridPoint.GetY(), m_pCorrelationGrid->GetROI().GetHeight())) { // point not in grid continue; @@ -883,7 +916,7 @@ namespace karto const PointVectorDouble& rPointReadings = pScan->GetPointReadings(); // points must be at least 10 cm away when making comparisons of inside/outside of viewpoint - const kt_double minSquareDistance = math::Square(0.1); // in m^2 + const kt_double minSquareDistance = math::Square(0.1); // in m^2 // this iterator lags from the main iterator adding points only when the points are on // the same side as the viewpoint @@ -918,7 +951,7 @@ namespace karto // reset beginning point firstPoint = currentPoint; - if (ss < 0.0) // wrong side, skip and keep going + if (ss < 0.0) // wrong side, skip and keep going { trailingPointIter = iter; } @@ -1053,8 +1086,7 @@ namespace karto return objects; } - - }; // class BreadthFirstTraversal + }; // class BreadthFirstTraversal //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -1084,8 +1116,7 @@ namespace karto Pose2 m_CenterPose; kt_double m_MaxDistanceSquared; kt_bool m_UseScanBarycenter; - - }; // NearScanVisitor + }; // NearScanVisitor //////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////// @@ -1094,7 +1125,9 @@ namespace karto MapperGraph::MapperGraph(Mapper* pMapper, kt_double rangeThreshold) : m_pMapper(pMapper) { - m_pLoopScanMatcher = ScanMatcher::Create(pMapper, m_pMapper->m_pLoopSearchSpaceDimension->GetValue(), m_pMapper->m_pLoopSearchSpaceResolution->GetValue(), m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue(), rangeThreshold); + m_pLoopScanMatcher = ScanMatcher::Create(pMapper, m_pMapper->m_pLoopSearchSpaceDimension->GetValue(), + m_pMapper->m_pLoopSearchSpaceResolution->GetValue(), + m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue(), rangeThreshold); assert(m_pLoopScanMatcher); m_pTraversal = new BreadthFirstTraversal(this); @@ -1113,7 +1146,7 @@ namespace karto { assert(pScan); - if(pScan != NULL) + if (pScan != NULL) { Vertex* pVertex = new Vertex(pScan); Graph::AddVertex(pScan->GetSensorName(), pVertex); @@ -1159,7 +1192,9 @@ namespace karto Pose2 bestPose; Matrix3 covariance; - kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, pSensorManager->GetScans(rCandidateSensorName), bestPose, covariance); + kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, + pSensorManager->GetScans(rCandidateSensorName), + bestPose, covariance); LinkScans(pScan, pSensorManager->GetScan(rCandidateSensorName, 0), bestPose, covariance); // only add to means and covariances if response was high "enough" @@ -1200,11 +1235,16 @@ namespace karto { Pose2 bestPose; Matrix3 covariance; - kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain, bestPose, covariance, false, false); + kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain, + bestPose, covariance, false, false); std::stringstream stream; - stream << "COARSE RESPONSE: " << coarseResponse << " (> " << m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")" << std::endl; - stream << " var: " << covariance(0, 0) << ", " << covariance(1, 1) << " (< " << m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")"; + stream << "COARSE RESPONSE: " << coarseResponse + << " (> " << m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")" + << std::endl; + stream << " var: " << covariance(0, 0) << ", " << covariance(1, 1) + << " (< " << m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")"; + m_pMapper->FireLoopClosureCheck(stream.str()); if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) && @@ -1217,10 +1257,12 @@ namespace karto tmpScan.SetStateId(pScan->GetStateId()); tmpScan.SetCorrectedPose(pScan->GetCorrectedPose()); tmpScan.SetSensorPose(bestPose); // This also updates OdometricPose. - kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan(&tmpScan, candidateChain, bestPose, covariance, false); + kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan(&tmpScan, candidateChain, + bestPose, covariance, false); std::stringstream stream1; - stream1 << "FINE RESPONSE: " << fineResponse << " (>" << m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl; + stream1 << "FINE RESPONSE: " << fineResponse << " (>" + << m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl; m_pMapper->FireLoopClosureCheck(stream1.str()); if (fineResponse < m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue()) @@ -1247,7 +1289,8 @@ namespace karto return loopClosed; } - LocalizedRangeScan* MapperGraph::GetClosestScanToPose(const LocalizedRangeScanVector& rScans, const Pose2& rPose) const + LocalizedRangeScan* MapperGraph::GetClosestScanToPose(const LocalizedRangeScanVector& rScans, + const Pose2& rPose) const { LocalizedRangeScan* pClosestScan = NULL; kt_double bestSquaredDistance = DBL_MAX; @@ -1267,7 +1310,8 @@ namespace karto return pClosestScan; } - Edge* MapperGraph::AddEdge(LocalizedRangeScan* pSourceScan, LocalizedRangeScan* pTargetScan, kt_bool& rIsNewEdge) + Edge* MapperGraph::AddEdge(LocalizedRangeScan* pSourceScan, + LocalizedRangeScan* pTargetScan, kt_bool& rIsNewEdge) { // check that vertex exists assert(pSourceScan->GetStateId() < (kt_int32s)m_Vertices[pSourceScan->GetSensorName()].size()); @@ -1294,7 +1338,8 @@ namespace karto return pEdge; } - void MapperGraph::LinkScans(LocalizedRangeScan* pFromScan, LocalizedRangeScan* pToScan, const Pose2& rMean, const Matrix3& rCovariance) + void MapperGraph::LinkScans(LocalizedRangeScan* pFromScan, LocalizedRangeScan* pToScan, + const Pose2& rMean, const Matrix3& rCovariance) { kt_bool isNewEdge = true; Edge* pEdge = AddEdge(pFromScan, pToScan, isNewEdge); @@ -1333,7 +1378,8 @@ namespace karto } } - void MapperGraph::LinkChainToScan(const LocalizedRangeScanVector& rChain, LocalizedRangeScan* pScan, const Pose2& rMean, const Matrix3& rCovariance) + void MapperGraph::LinkChainToScan(const LocalizedRangeScanVector& rChain, LocalizedRangeScan* pScan, + const Pose2& rMean, const Matrix3& rCovariance) { Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); @@ -1358,7 +1404,8 @@ namespace karto // to keep track of which scans have been added to a chain LocalizedRangeScanVector processed; - const LocalizedRangeScanVector nearLinkedScans = FindNearLinkedScans(pScan, m_pMapper->m_pLinkScanMaximumDistance->GetValue()); + const LocalizedRangeScanVector nearLinkedScans = FindNearLinkedScans(pScan, + m_pMapper->m_pLinkScanMaximumDistance->GetValue()); const_forEach(LocalizedRangeScanVector, &nearLinkedScans) { LocalizedRangeScan* pNearScan = *iter; @@ -1383,7 +1430,8 @@ namespace karto // add scans before current scan being processed for (kt_int32s candidateScanNum = pNearScan->GetStateId() - 1; candidateScanNum >= 0; candidateScanNum--) { - LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(), candidateScanNum); + LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(), + candidateScanNum); // chain is invalid--contains scan being added if (pCandidateScan == pScan) @@ -1411,7 +1459,8 @@ namespace karto kt_int32u end = static_cast(m_pMapper->m_pMapperSensorManager->GetScans(pNearScan->GetSensorName()).size()); for (kt_int32u candidateScanNum = pNearScan->GetStateId() + 1; candidateScanNum < end; candidateScanNum++) { - LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(), candidateScanNum); + LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(), + candidateScanNum); if (pCandidateScan == pScan) { @@ -1498,15 +1547,18 @@ namespace karto return accumulatedPose; } - LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure(LocalizedRangeScan* pScan, const Name& rSensorName, kt_int32u& rStartNum) + LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure(LocalizedRangeScan* pScan, + const Name& rSensorName, + kt_int32u& rStartNum) { - LocalizedRangeScanVector chain; // return value + LocalizedRangeScanVector chain; // return value Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); // possible loop closure chain should not include close scans that have a // path of links to the scan of interest - const LocalizedRangeScanVector nearLinkedScans = FindNearLinkedScans(pScan, m_pMapper->m_pLoopSearchMaximumDistance->GetValue()); + const LocalizedRangeScanVector nearLinkedScans = + FindNearLinkedScans(pScan, m_pMapper->m_pLoopSearchMaximumDistance->GetValue()); kt_int32u nScans = static_cast(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size()); for (; rStartNum < nScans; rStartNum++) @@ -1800,257 +1852,318 @@ namespace karto false, GetParameterManager()); } /* Adding in getters and setters here for easy parameter access */ - + // General Parameters - bool Mapper::getParamUseScanMatching(){ - return (bool)m_pUseScanMatching->GetValue(); + bool Mapper::getParamUseScanMatching() + { + return static_cast(m_pUseScanMatching->GetValue()); } - bool Mapper::getParamUseScanBarycenter(){ - return (bool)m_pUseScanBarycenter->GetValue(); + bool Mapper::getParamUseScanBarycenter() + { + return static_cast(m_pUseScanBarycenter->GetValue()); } - double Mapper::getParamMinimumTravelDistance(){ - return (double)m_pMinimumTravelDistance->GetValue(); + double Mapper::getParamMinimumTravelDistance() + { + return static_cast(m_pMinimumTravelDistance->GetValue()); } - double Mapper::getParamMinimumTravelHeading(){ - return math::RadiansToDegrees((double)m_pMinimumTravelHeading->GetValue()); + double Mapper::getParamMinimumTravelHeading() + { + return math::RadiansToDegrees(static_cast(m_pMinimumTravelHeading->GetValue())); } - int Mapper::getParamScanBufferSize(){ - return (int)m_pScanBufferSize->GetValue(); + int Mapper::getParamScanBufferSize() + { + return static_cast(m_pScanBufferSize->GetValue()); } - double Mapper::getParamScanBufferMaximumScanDistance(){ - return (double)m_pScanBufferMaximumScanDistance->GetValue(); + double Mapper::getParamScanBufferMaximumScanDistance() + { + return static_cast(m_pScanBufferMaximumScanDistance->GetValue()); } - double Mapper::getParamLinkMatchMinimumResponseFine(){ - return (double)m_pLinkMatchMinimumResponseFine->GetValue(); + double Mapper::getParamLinkMatchMinimumResponseFine() + { + return static_cast(m_pLinkMatchMinimumResponseFine->GetValue()); } - double Mapper::getParamLinkScanMaximumDistance(){ - return (double)m_pLinkScanMaximumDistance->GetValue(); + double Mapper::getParamLinkScanMaximumDistance() + { + return static_cast(m_pLinkScanMaximumDistance->GetValue()); } - double Mapper::getParamLoopSearchMaximumDistance(){ - return (double)m_pLoopSearchMaximumDistance->GetValue(); + double Mapper::getParamLoopSearchMaximumDistance() + { + return static_cast(m_pLoopSearchMaximumDistance->GetValue()); } - bool Mapper::getParamDoLoopClosing(){ - return (bool)m_pDoLoopClosing->GetValue(); + bool Mapper::getParamDoLoopClosing() + { + return static_cast(m_pDoLoopClosing->GetValue()); } - int Mapper::getParamLoopMatchMinimumChainSize(){ - return (int)m_pLoopMatchMinimumChainSize->GetValue(); + int Mapper::getParamLoopMatchMinimumChainSize() + { + return static_cast(m_pLoopMatchMinimumChainSize->GetValue()); } - double Mapper::getParamLoopMatchMaximumVarianceCoarse(){ - return (double)std::sqrt(m_pLoopMatchMaximumVarianceCoarse->GetValue()); + double Mapper::getParamLoopMatchMaximumVarianceCoarse() + { + return static_cast(std::sqrt(m_pLoopMatchMaximumVarianceCoarse->GetValue())); } - double Mapper::getParamLoopMatchMinimumResponseCoarse(){ - return (double)m_pLoopMatchMinimumResponseCoarse->GetValue(); + double Mapper::getParamLoopMatchMinimumResponseCoarse() + { + return static_cast(m_pLoopMatchMinimumResponseCoarse->GetValue()); } - - double Mapper::getParamLoopMatchMinimumResponseFine(){ - return (double)m_pLoopMatchMinimumResponseFine->GetValue(); + + double Mapper::getParamLoopMatchMinimumResponseFine() + { + return static_cast(m_pLoopMatchMinimumResponseFine->GetValue()); } // Correlation Parameters - Correlation Parameters - double Mapper::getParamCorrelationSearchSpaceDimension(){ - return (double)m_pCorrelationSearchSpaceDimension->GetValue(); + double Mapper::getParamCorrelationSearchSpaceDimension() + { + return static_cast(m_pCorrelationSearchSpaceDimension->GetValue()); } - double Mapper::getParamCorrelationSearchSpaceResolution(){ - return (double)m_pCorrelationSearchSpaceResolution->GetValue(); + double Mapper::getParamCorrelationSearchSpaceResolution() + { + return static_cast(m_pCorrelationSearchSpaceResolution->GetValue()); } - double Mapper::getParamCorrelationSearchSpaceSmearDeviation(){ - return (double)m_pCorrelationSearchSpaceSmearDeviation->GetValue(); + double Mapper::getParamCorrelationSearchSpaceSmearDeviation() + { + return static_cast(m_pCorrelationSearchSpaceSmearDeviation->GetValue()); } - // Correlation Parameters - Loop Correlation Parameters - - double Mapper::getParamLoopSearchSpaceDimension(){ - return (double)m_pLoopSearchSpaceDimension->GetValue(); + // Correlation Parameters - Loop Correlation Parameters + + double Mapper::getParamLoopSearchSpaceDimension() + { + return static_cast(m_pLoopSearchSpaceDimension->GetValue()); } - double Mapper::getParamLoopSearchSpaceResolution(){ - return (double)m_pLoopSearchSpaceResolution->GetValue(); + double Mapper::getParamLoopSearchSpaceResolution() + { + return static_cast(m_pLoopSearchSpaceResolution->GetValue()); } - double Mapper::getParamLoopSearchSpaceSmearDeviation(){ - return (double)m_pLoopSearchSpaceSmearDeviation->GetValue(); + double Mapper::getParamLoopSearchSpaceSmearDeviation() + { + return static_cast(m_pLoopSearchSpaceSmearDeviation->GetValue()); } // ScanMatcher Parameters - double Mapper::getParamDistanceVariancePenalty(){ - return std::sqrt((double)m_pDistanceVariancePenalty->GetValue()); + double Mapper::getParamDistanceVariancePenalty() + { + return std::sqrt(static_cast(m_pDistanceVariancePenalty->GetValue())); } - double Mapper::getParamAngleVariancePenalty(){ - return std::sqrt((double)m_pAngleVariancePenalty->GetValue()); + double Mapper::getParamAngleVariancePenalty() + { + return std::sqrt(static_cast(m_pAngleVariancePenalty->GetValue())); } - double Mapper::getParamFineSearchAngleOffset(){ - return (double)m_pFineSearchAngleOffset->GetValue(); + double Mapper::getParamFineSearchAngleOffset() + { + return static_cast(m_pFineSearchAngleOffset->GetValue()); } - double Mapper::getParamCoarseSearchAngleOffset(){ - return (double)m_pCoarseSearchAngleOffset->GetValue(); + double Mapper::getParamCoarseSearchAngleOffset() + { + return static_cast(m_pCoarseSearchAngleOffset->GetValue()); } - double Mapper::getParamCoarseAngleResolution(){ - return (double)m_pCoarseAngleResolution->GetValue(); + double Mapper::getParamCoarseAngleResolution() + { + return static_cast(m_pCoarseAngleResolution->GetValue()); } - double Mapper::getParamMinimumAnglePenalty(){ - return (double)m_pMinimumAnglePenalty->GetValue(); + double Mapper::getParamMinimumAnglePenalty() + { + return static_cast(m_pMinimumAnglePenalty->GetValue()); } - double Mapper::getParamMinimumDistancePenalty(){ - return (double)m_pMinimumDistancePenalty->GetValue(); + double Mapper::getParamMinimumDistancePenalty() + { + return static_cast(m_pMinimumDistancePenalty->GetValue()); } - bool Mapper::getParamUseResponseExpansion(){ - return (bool)m_pUseResponseExpansion->GetValue(); + bool Mapper::getParamUseResponseExpansion() + { + return static_cast(m_pUseResponseExpansion->GetValue()); } /* Setters for parameters */ - //General Parameters - void Mapper::setParamUseScanMatching(bool b){ + // General Parameters + void Mapper::setParamUseScanMatching(bool b) + { m_pUseScanMatching->SetValue((kt_bool)b); } - void Mapper::setParamUseScanBarycenter(bool b){ + void Mapper::setParamUseScanBarycenter(bool b) + { m_pUseScanBarycenter->SetValue((kt_bool)b); } - void Mapper::setParamMinimumTravelDistance(double d){ + void Mapper::setParamMinimumTravelDistance(double d) + { m_pMinimumTravelDistance->SetValue((kt_double)d); } - void Mapper::setParamMinimumTravelHeading(double d){ + void Mapper::setParamMinimumTravelHeading(double d) + { m_pMinimumTravelHeading->SetValue((kt_double)d); } - void Mapper::setParamScanBufferSize(int i){ + void Mapper::setParamScanBufferSize(int i) + { m_pScanBufferSize->SetValue((kt_int32u)i); } - void Mapper::setParamScanBufferMaximumScanDistance(double d){ + void Mapper::setParamScanBufferMaximumScanDistance(double d) + { m_pScanBufferMaximumScanDistance->SetValue((kt_double)d); } - void Mapper::setParamLinkMatchMinimumResponseFine(double d){ + void Mapper::setParamLinkMatchMinimumResponseFine(double d) + { m_pLinkMatchMinimumResponseFine->SetValue((kt_double)d); } - void Mapper::setParamLinkScanMaximumDistance(double d){ + void Mapper::setParamLinkScanMaximumDistance(double d) + { m_pLinkScanMaximumDistance->SetValue((kt_double)d); } - void Mapper::setParamLoopSearchMaximumDistance(double d){ + void Mapper::setParamLoopSearchMaximumDistance(double d) + { m_pLoopSearchMaximumDistance->SetValue((kt_double)d); } - void Mapper::setParamDoLoopClosing(bool b){ + void Mapper::setParamDoLoopClosing(bool b) + { m_pDoLoopClosing->SetValue((kt_bool)b); } - void Mapper::setParamLoopMatchMinimumChainSize(int i){ + void Mapper::setParamLoopMatchMinimumChainSize(int i) + { m_pLoopMatchMinimumChainSize->SetValue((kt_int32u)i); } - void Mapper::setParamLoopMatchMaximumVarianceCoarse(double d){ + void Mapper::setParamLoopMatchMaximumVarianceCoarse(double d) + { m_pLoopMatchMaximumVarianceCoarse->SetValue((kt_double)math::Square(d)); } - void Mapper::setParamLoopMatchMinimumResponseCoarse(double d){ + void Mapper::setParamLoopMatchMinimumResponseCoarse(double d) + { m_pLoopMatchMinimumResponseCoarse->SetValue((kt_double)d); } - void Mapper::setParamLoopMatchMinimumResponseFine(double d){ + void Mapper::setParamLoopMatchMinimumResponseFine(double d) + { m_pLoopMatchMinimumResponseFine->SetValue((kt_double)d); } - //Correlation Parameters - Correlation Parameters - void Mapper::setParamCorrelationSearchSpaceDimension(double d){ + // Correlation Parameters - Correlation Parameters + void Mapper::setParamCorrelationSearchSpaceDimension(double d) + { m_pCorrelationSearchSpaceDimension->SetValue((kt_double)d); } - void Mapper::setParamCorrelationSearchSpaceResolution(double d){ + void Mapper::setParamCorrelationSearchSpaceResolution(double d) + { m_pCorrelationSearchSpaceResolution->SetValue((kt_double)d); } - void Mapper::setParamCorrelationSearchSpaceSmearDeviation(double d){ + void Mapper::setParamCorrelationSearchSpaceSmearDeviation(double d) + { m_pCorrelationSearchSpaceSmearDeviation->SetValue((kt_double)d); } - //Correlation Parameters - Loop Closure Parameters - void Mapper::setParamLoopSearchSpaceDimension(double d){ + // Correlation Parameters - Loop Closure Parameters + void Mapper::setParamLoopSearchSpaceDimension(double d) + { m_pLoopSearchSpaceDimension->SetValue((kt_double)d); } - void Mapper::setParamLoopSearchSpaceResolution(double d){ + void Mapper::setParamLoopSearchSpaceResolution(double d) + { m_pLoopSearchSpaceResolution->SetValue((kt_double)d); } - void Mapper::setParamLoopSearchSpaceSmearDeviation(double d){ + void Mapper::setParamLoopSearchSpaceSmearDeviation(double d) + { m_pLoopSearchSpaceSmearDeviation->SetValue((kt_double)d); } - //Scan Matcher Parameters - void Mapper::setParamDistanceVariancePenalty(double d){ + // Scan Matcher Parameters + void Mapper::setParamDistanceVariancePenalty(double d) + { m_pDistanceVariancePenalty->SetValue((kt_double)math::Square(d)); } - void Mapper::setParamAngleVariancePenalty(double d){ + void Mapper::setParamAngleVariancePenalty(double d) + { m_pAngleVariancePenalty->SetValue((kt_double)math::Square(d)); } - void Mapper::setParamFineSearchAngleOffset(double d){ + void Mapper::setParamFineSearchAngleOffset(double d) + { m_pFineSearchAngleOffset->SetValue((kt_double)d); } - void Mapper::setParamCoarseSearchAngleOffset(double d){ + void Mapper::setParamCoarseSearchAngleOffset(double d) + { m_pCoarseSearchAngleOffset->SetValue((kt_double)d); } - void Mapper::setParamCoarseAngleResolution(double d){ + void Mapper::setParamCoarseAngleResolution(double d) + { m_pCoarseAngleResolution->SetValue((kt_double)d); } - void Mapper::setParamMinimumAnglePenalty(double d){ + void Mapper::setParamMinimumAnglePenalty(double d) + { m_pMinimumAnglePenalty->SetValue((kt_double)d); } - void Mapper::setParamMinimumDistancePenalty(double d){ + void Mapper::setParamMinimumDistancePenalty(double d) + { m_pMinimumDistancePenalty->SetValue((kt_double)d); } - void Mapper::setParamUseResponseExpansion(bool b){ + void Mapper::setParamUseResponseExpansion(bool b) + { m_pUseResponseExpansion->SetValue((kt_bool)b); } - + void Mapper::Initialize(kt_double rangeThreshold) { if (m_Initialized == false) { // create sequential scan and loop matcher - m_pSequentialScanMatcher = ScanMatcher::Create(this, m_pCorrelationSearchSpaceDimension->GetValue(), m_pCorrelationSearchSpaceResolution->GetValue(), m_pCorrelationSearchSpaceSmearDeviation->GetValue(), rangeThreshold); + m_pSequentialScanMatcher = ScanMatcher::Create(this, + m_pCorrelationSearchSpaceDimension->GetValue(), + m_pCorrelationSearchSpaceResolution->GetValue(), + m_pCorrelationSearchSpaceSmearDeviation->GetValue(), + rangeThreshold); assert(m_pSequentialScanMatcher); - m_pMapperSensorManager = new MapperSensorManager(m_pScanBufferSize->GetValue(), m_pScanBufferMaximumScanDistance->GetValue()); + m_pMapperSensorManager = new MapperSensorManager(m_pScanBufferSize->GetValue(), + m_pScanBufferMaximumScanDistance->GetValue()); m_pGraph = new MapperGraph(this, rangeThreshold); @@ -2072,14 +2185,14 @@ namespace karto m_Initialized = false; } - kt_bool Mapper::Process(Object* /*pObject*/) + kt_bool Mapper::Process(Object* /*pObject*/) { return true; } kt_bool Mapper::Process(LocalizedRangeScan* pScan) { - if(pScan != NULL) + if (pScan != NULL) { karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder(); @@ -2118,7 +2231,10 @@ namespace karto if (m_pUseScanMatching->GetValue() && pLastScan != NULL) { Pose2 bestPose; - m_pSequentialScanMatcher->MatchScan(pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), bestPose, covariance); + m_pSequentialScanMatcher->MatchScan(pScan, + m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), + bestPose, + covariance); pScan->SetSensorPose(bestPose); } @@ -2302,4 +2418,4 @@ namespace karto { return m_pGraph->GetLoopScanMatcher(); } -} +} // namespace karto