18 #ifndef karto_sdk_KARTO_H
19 #define karto_sdk_KARTO_H
32 #include <shared_mutex>
39 #include <boost/thread.hpp>
41 #include <boost/serialization/base_object.hpp>
42 #include <boost/serialization/nvp.hpp>
43 #include <boost/serialization/utility.hpp>
44 #include <boost/serialization/export.hpp>
45 #include <boost/type_traits/is_abstract.hpp>
46 #include <boost/archive/binary_iarchive.hpp>
47 #include <boost/archive/binary_oarchive.hpp>
48 #include <boost/serialization/map.hpp>
49 #include <boost/serialization/vector.hpp>
50 #include <boost/serialization/string.hpp>
51 #include <boost/serialization/array.hpp>
52 #include <boost/version.hpp>
55 #include <Poco/Mutex.h>
61 #define KARTO_Object(name) \
62 virtual const char* GetClassName() const { return #name; } \
63 virtual kt_objecttype GetObjectType() const { return ObjectType_##name; }
108 : m_Message(rMessage)
109 , m_ErrorCode(errorCode)
117 : m_Message(rException.m_Message)
118 , m_ErrorCode(rException.m_ErrorCode)
196 friend class boost::serialization::access;
197 template<
class Archive>
237 Poco::FastMutex::ScopedLock lock(m_Mutex);
251 Poco::FastMutex m_Mutex;
321 if (m_ParameterLookup.find(rName) != m_ParameterLookup.end())
323 return m_ParameterLookup[rName];
326 std::cout <<
"Unknown parameter: " << rName << std::endl;
368 friend class boost::serialization::access;
369 template<
class Archive>
372 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
NonCopyable);
373 ar & BOOST_SERIALIZATION_NVP(m_Parameters);
374 ar & BOOST_SERIALIZATION_NVP(m_ParameterLookup);
438 std::string::size_type pos = rName.find_last_of(
'/');
439 if (pos != 0 && pos != std::string::npos)
441 throw Exception(
"Name can't contain a scope!");
515 return !(*
this == rOther);
542 void Parse(
const std::string& rName)
544 std::string::size_type pos = rName.find_last_of(
'/');
546 if (pos == std::string::npos)
552 m_Scope = rName.substr(0, pos);
553 m_Name = rName.substr(pos+1, rName.size());
577 for (
size_t i = 1; i < rName.size(); ++i)
582 throw Exception(
"Invalid character in name. Valid characters must be within the ranges A-Z, a-z, 0-9, '/', '_' and '-'.");
588 throw Exception(
"Invalid first character in name. Valid characters must be within the ranges A-Z, a-z, and '/'.");
599 return (isalpha(c) || c ==
'/');
609 return (isalnum(c) || c ==
'/' || c ==
'_' || c ==
'-');
619 template<
class Archive>
622 ar & BOOST_SERIALIZATION_NVP(
m_Name);
623 ar & BOOST_SERIALIZATION_NVP(
m_Scope);
667 virtual const char* GetClassName()
const = 0;
681 return m_pParameterManager;
691 return m_pParameterManager->Get(rName);
700 inline void SetParameter(
const std::string& rName, T value);
708 return m_pParameterManager->GetParameterVector();
720 friend class boost::serialization::access;
721 template<
class Archive>
724 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
NonCopyable);
725 ar & BOOST_SERIALIZATION_NVP(m_pParameterManager);
726 ar & BOOST_SERIALIZATION_NVP(m_Name);
729 BOOST_SERIALIZATION_ASSUME_ABSTRACT(Object)
830 Module(
const std::string& rName);
841 virtual void Reset() = 0;
855 friend class boost::serialization::access;
856 template<
class Archive>
859 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Object);
862 BOOST_SERIALIZATION_ASSUME_ABSTRACT(Module)
983 friend class boost::serialization::access;
984 template<
class Archive>
987 ar & BOOST_SERIALIZATION_NVP(
m_Width);
988 ar & BOOST_SERIALIZATION_NVP(
m_Height);
1104 return (*
this - rOther).SquaredLength();
1254 rStream << rVector.
GetX() <<
" " << rVector.
GetY();
1268 friend class boost::serialization::access;
1269 template<
class Archive>
1272 ar & boost::serialization::make_nvp(
"m_Values_0",
m_Values[0]);
1273 ar & boost::serialization::make_nvp(
"m_Values_1",
m_Values[1]);
1292 template<
typename T>
1431 std::stringstream converter;
1432 converter.precision(std::numeric_limits<double>::digits10);
1436 return converter.str();
1710 else if (
test < -0.499)
1724 rPitch = asin(2 *
test);
1744 angle = pitch * 0.5;
1752 m_Values[0] = sYaw * sPitch * cRoll + cYaw * cPitch * sRoll;
1753 m_Values[1] = sYaw * cPitch * cRoll + cYaw * sPitch * sRoll;
1754 m_Values[2] = cYaw * sPitch * cRoll - sYaw * cPitch * sRoll;
1755 m_Values[3] = cYaw * cPitch * cRoll - sYaw * sPitch * sRoll;
1803 rStream << rQuaternion.
m_Values[0] <<
" "
1822 template<
typename T>
1909 return m_Size.GetWidth();
1927 return m_Size.GetHeight();
1936 m_Size.SetHeight(height);
2028 friend class boost::serialization::access;
2029 template<
class Archive>
2033 ar & BOOST_SERIALIZATION_NVP(
m_Size);
2255 template<
class Archive>
2259 ar & BOOST_SERIALIZATION_NVP(
m_Heading);
2375 std::stringstream converter;
2376 converter.precision(std::numeric_limits<double>::digits10);
2380 return converter.str();
2496 kt_double oneMinusCos = 1.0 - cosRadians;
2510 m_Matrix[0][0] = xx * oneMinusCos + cosRadians;
2515 m_Matrix[1][1] = yy * oneMinusCos + cosRadians;
2520 m_Matrix[2][2] = zz * oneMinusCos + cosRadians;
2549 if (haveInverse ==
false)
2578 if (fabs(fDet) <= fTolerance)
2584 for (
size_t row = 0; row < 3; row++)
2586 for (
size_t col = 0; col < 3; col++)
2588 rkInverse.
m_Matrix[row][col] *= fInvDet;
2601 std::stringstream converter;
2602 converter.precision(std::numeric_limits<double>::digits10);
2604 for (
int row = 0; row < 3; row++)
2606 for (
int col = 0; col < 3; col++)
2608 converter <<
m_Matrix[row][col] <<
" ";
2612 return converter.str();
2656 for (
size_t row = 0; row < 3; row++)
2658 for (
size_t col = 0; col < 3; col++)
2717 template<
class Archive>
2720 ar & BOOST_SERIALIZATION_NVP(
m_Matrix);
2826 catch (
const std::bad_alloc& ex)
2828 throw Exception(
"Matrix allocation error");
2833 throw Exception(
"Matrix allocation error");
2846 throw Exception(
"Matrix - RangeCheck ERROR!!!!");
2851 throw Exception(
"Matrix - RangeCheck ERROR!!!!");
2876 :
m_Minimum(999999999999999999.99999, 999999999999999999.99999)
2877 ,
m_Maximum(-999999999999999999.99999, -999999999999999999.99999)
2957 template<
class Archive>
2960 ar & BOOST_SERIALIZATION_NVP(
m_Minimum);
2961 ar & BOOST_SERIALIZATION_NVP(
m_Maximum);
3034 if (rPose1 == rPose2)
3048 if (rPose1.
GetX() != 0.0 || rPose1.
GetY() != 0.0)
3054 newPosition = rPose2;
3068 template<
class Archive>
3119 if (pParameterManger != NULL)
3121 pParameterManger->Add(
this);
3132 const std::string& rDescription,
3138 if (pParameterManger != NULL)
3140 pParameterManger->Add(
this);
3196 rStream.precision(6);
3197 rStream.flags(std::ios::fixed);
3210 template<
class Archive>
3213 ar & BOOST_SERIALIZATION_NVP(
m_Name);
3217 BOOST_SERIALIZATION_ASSUME_ABSTRACT(AbstractParameter)
3226 template<
typename T>
3253 const std::string& rDescription,
3293 std::stringstream converter;
3295 return converter.str();
3304 std::stringstream converter;
3305 converter.str(rStringValue);
3342 friend class boost::serialization::access;
3343 template<
class Archive>
3347 ar & BOOST_SERIALIZATION_NVP(
m_Value);
3356 BOOST_SERIALIZATION_ASSUME_ABSTRACT(
Parameter)
3361 int precision = std::numeric_limits<double>::digits10;
3362 std::stringstream converter;
3363 converter.precision(precision);
3365 converter.str(rStringValue);
3368 converter >> m_Value;
3374 std::stringstream converter;
3375 converter.precision(std::numeric_limits<double>::digits10);
3376 converter << m_Value;
3377 return converter.str();
3383 if (rStringValue ==
"true" || rStringValue ==
"TRUE")
3396 if (m_Value ==
true)
3457 if (m_EnumDefines.find(rStringValue) != m_EnumDefines.end())
3459 m_Value = m_EnumDefines[rStringValue];
3463 std::string validValues;
3467 validValues += iter->first +
", ";
3470 throw Exception(
"Unable to set enum: " + rStringValue +
". Valid values are: " + validValues);
3482 if (iter->second == m_Value)
3488 throw Exception(
"Unable to lookup enum");
3498 if (m_EnumDefines.find(rName) == m_EnumDefines.end())
3500 m_EnumDefines[rName] = value;
3504 std::cerr <<
"Overriding enum value: " << m_EnumDefines[rName] <<
" with " << value << std::endl;
3506 m_EnumDefines[rName] = value;
3536 friend class boost::serialization::access;
3537 template<
class Archive>
3541 ar & BOOST_SERIALIZATION_NVP(m_EnumDefines);
3544 BOOST_SERIALIZATION_ASSUME_ABSTRACT(ParameterEnum)
3579 friend class boost::serialization::access;
3580 template<
class Archive>
3583 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Object);
3634 return m_pOffsetPose->GetValue();
3643 m_pOffsetPose->SetValue(rPose);
3650 virtual kt_bool Validate() = 0;
3675 friend class boost::serialization::access;
3676 template<
class Archive>
3679 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Object);
3680 ar & BOOST_SERIALIZATION_NVP(m_pOffsetPose);
3683 BOOST_SERIALIZATION_ASSUME_ABSTRACT(Sensor)
3736 if ((m_Sensors.find(pSensor->
GetName()) != m_Sensors.end()) && !
override)
3738 throw Exception(
"Cannot register sensor: already registered: [" +
3740 "] (Consider setting 'override' to true)");
3743 std::cout <<
"Registering sensor: [" << pSensor->
GetName().
ToString() <<
"]" << std::endl;
3745 m_Sensors[pSensor->
GetName()] = pSensor;
3756 if (m_Sensors.find(pSensor->
GetName()) != m_Sensors.end())
3758 std::cout <<
"Unregistering sensor: " << pSensor->
GetName().
ToString() << std::endl;
3760 m_Sensors.erase(pSensor->
GetName());
3775 if (m_Sensors.find(rName) != m_Sensors.end())
3777 return m_Sensors[rName];
3780 throw Exception(
"Sensor not registered: [" + rName.
ToString() +
"] (Did you add the sensor to the Dataset?)");
3791 Sensor* pSensor = GetSensorByName(rName);
3793 return dynamic_cast<T*
>(pSensor);
3806 sensors.push_back(iter->second);
3819 if (pSensor == NULL)
3821 throw Exception(
"Invalid sensor: NULL");
3825 throw Exception(
"Invalid sensor: nameless");
3835 friend class boost::serialization::access;
3836 template<
class Archive>
3839 ar & BOOST_SERIALIZATION_NVP(m_Sensors);
3883 if (pSensorData == NULL)
3894 friend class boost::serialization::access;
3895 template<
class Archive>
3898 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Sensor);
3902 BOOST_SERIALIZATION_ASSUME_ABSTRACT(Drive)
3907 class LocalizedRangeScan;
3908 class CoordinateConverter;
3947 return m_pMinimumRange->GetValue();
3956 m_pMinimumRange->SetValue(minimumRange);
3958 SetRangeThreshold(GetRangeThreshold());
3967 return m_pMaximumRange->GetValue();
3976 m_pMaximumRange->SetValue(maximumRange);
3978 SetRangeThreshold(GetRangeThreshold());
3987 return m_pRangeThreshold->GetValue();
3997 m_pRangeThreshold->SetValue(
math::Clip(rangeThreshold, GetMinimumRange(), GetMaximumRange()));
4001 std::cout <<
"Info: clipped range threshold to be within minimum and maximum range!" << std::endl;
4011 return m_pMinimumAngle->GetValue();
4020 m_pMinimumAngle->SetValue(minimumAngle);
4031 return m_pMaximumAngle->GetValue();
4040 m_pMaximumAngle->SetValue(maximumAngle);
4051 return m_pAngularResolution->GetValue();
4062 m_pAngularResolution->SetValue(angularResolution);
4076 std::stringstream stream;
4077 stream <<
"Invalid resolution for Sick LMS100: ";
4078 stream << angularResolution;
4099 std::stringstream stream;
4100 stream <<
"Invalid resolution for Sick LMS291: ";
4101 stream << angularResolution;
4107 throw Exception(
"Can't set angular resolution, please create a LaserRangeFinder of type Custom");
4118 return m_pType->GetValue();
4127 return m_NumberOfRangeReadings;
4137 return m_pIs360Laser->GetValue();
4146 m_pIs360Laser->SetValue(is_360_laser);
4156 if (
math::InRange(GetRangeThreshold(), GetMinimumRange(), GetMaximumRange()) ==
false)
4158 std::cout <<
"Please set range threshold to a value between ["
4159 << GetMinimumRange() <<
";" << GetMaximumRange() <<
"]" << std::endl;
4177 kt_bool ignoreThresholdPoints =
true,
4323 Pose2 defaultOffset;
4336 , m_NumberOfRangeReadings(0)
4368 if (GetIs360Laser())
4375 / GetAngularResolution()) + residual);
4401 friend class boost::serialization::access;
4402 template<
class Archive>
4405 if (Archive::is_loading::value)
4424 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Sensor);
4425 ar & BOOST_SERIALIZATION_NVP(m_pMinimumAngle);
4426 ar & BOOST_SERIALIZATION_NVP(m_pMaximumAngle);
4427 ar & BOOST_SERIALIZATION_NVP(m_pAngularResolution);
4428 ar & BOOST_SERIALIZATION_NVP(m_pMinimumRange);
4429 ar & BOOST_SERIALIZATION_NVP(m_pMaximumRange);
4430 ar & BOOST_SERIALIZATION_NVP(m_pRangeThreshold);
4431 ar & BOOST_SERIALIZATION_NVP(m_pIs360Laser);
4432 ar & BOOST_SERIALIZATION_NVP(m_pType);
4433 ar & BOOST_SERIALIZATION_NVP(m_NumberOfRangeReadings);
4436 BOOST_SERIALIZATION_ASSUME_ABSTRACT(LaserRangeFinder)
4479 return value * m_Scale;
4490 kt_double gridX = (rWorld.
GetX() - m_Offset.GetX()) * m_Scale;
4495 gridY = (rWorld.
GetY() - m_Offset.GetY()) * m_Scale;
4499 gridY = (m_Size.GetHeight() / m_Scale - rWorld.
GetY() + m_Offset.GetY()) * m_Scale;
4518 worldY = m_Offset.GetY() + rGrid.
GetY() / m_Scale;
4522 worldY = m_Offset.GetY() + (m_Size.GetHeight() - rGrid.
GetY()) / m_Scale;
4588 return 1.0 / m_Scale;
4597 m_Scale = 1.0 / resolution;
4610 kt_double maxX = minX + GetSize().GetWidth() * GetResolution();
4611 kt_double maxY = minY + GetSize().GetHeight() * GetResolution();
4623 friend class boost::serialization::access;
4624 template<
class Archive>
4627 ar & BOOST_SERIALIZATION_NVP(m_Size);
4628 ar & BOOST_SERIALIZATION_NVP(m_Scale);
4629 ar & BOOST_SERIALIZATION_NVP(m_Offset);
4641 template<
typename T>
4657 Grid* pGrid =
new Grid(width, height);
4673 if (m_pCoordinateConverter)
4675 delete m_pCoordinateConverter;
4685 memset(m_pData, 0, GetDataSize() *
sizeof(T));
4694 Grid* pGrid = CreateGrid(GetWidth(), GetHeight(), GetResolution());
4697 memcpy(pGrid->
GetDataPointer(), GetDataPointer(), GetDataSize());
4711 m_WidthStep = math::AlignValue<kt_int32s>(width, 8);
4713 if (m_pData != NULL)
4721 m_pData =
new T[GetDataSize()];
4723 if (m_pCoordinateConverter == NULL)
4759 if (boundaryCheck ==
true)
4761 if (IsValidGridIndex(rGrid) ==
false)
4763 std::stringstream error;
4764 error <<
"Index " << rGrid <<
" out of range. Index must be between [0; "
4765 << m_Width <<
") and [0; " << m_Height <<
")";
4772 if (boundaryCheck ==
true)
4789 grid.
SetY(index / m_WidthStep);
4790 grid.
SetX(index - grid.
GetY() * m_WidthStep);
4803 return GetCoordinateConverter()->WorldToGrid(rWorld, flipY);
4814 return GetCoordinateConverter()->GridToWorld(rGrid, flipY);
4824 kt_int32s index = GridIndex(rGrid,
true);
4825 return m_pData + index;
4835 kt_int32s index = GridIndex(rGrid,
true);
4836 return m_pData + index;
4899 return m_WidthStep * m_Height;
4910 return m_pData[index];
4919 return m_pCoordinateConverter;
4928 return GetCoordinateConverter()->GetResolution();
4937 return GetCoordinateConverter()->GetBoundingBox();
4951 kt_bool steep = abs(y1 - y0) > abs(x1 - x0);
4995 if (2 * error >= deltaX)
5002 if (IsValidGridIndex(gridIndex))
5004 kt_int32s index = GridIndex(gridIndex,
false);
5005 T* pGridPointer = GetDataPointer();
5006 pGridPointer[index]++;
5024 , m_pCoordinateConverter(NULL)
5026 Resize(width, height);
5037 friend class boost::serialization::access;
5038 template<
class Archive>
5041 ar & BOOST_SERIALIZATION_NVP(m_Width);
5042 ar & BOOST_SERIALIZATION_NVP(m_Height);
5043 ar & BOOST_SERIALIZATION_NVP(m_WidthStep);
5044 ar & BOOST_SERIALIZATION_NVP(m_pCoordinateConverter);
5047 if (Archive::is_loading::value)
5049 m_pData =
new T[m_WidthStep * m_Height];
5051 ar & boost::serialization::make_array<T>(m_pData, m_WidthStep * m_Height);
5055 BOOST_SERIALIZATION_ASSUME_ABSTRACT(Grid)
5092 virtual const std::string
Write()
const = 0;
5098 virtual void Read(
const std::string& rValue) = 0;
5104 friend class boost::serialization::access;
5105 template<
class Archive>
5108 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Object);
5111 BOOST_SERIALIZATION_ASSUME_ABSTRACT(CustomData)
5156 m_StateId = stateId;
5174 m_UniqueId = uniqueId;
5201 return m_SensorName;
5210 m_CustomData.push_back(pCustomData);
5219 return m_CustomData;
5262 friend class boost::serialization::access;
5263 template<
class Archive>
5266 ar & BOOST_SERIALIZATION_NVP(m_StateId);
5267 ar & BOOST_SERIALIZATION_NVP(m_UniqueId);
5268 ar & BOOST_SERIALIZATION_NVP(m_SensorName);
5269 ar & BOOST_SERIALIZATION_NVP(m_Time);
5270 ar & BOOST_SERIALIZATION_NVP(m_CustomData);
5271 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Object);
5302 , m_pRangeReadings(NULL)
5303 , m_NumberOfRangeReadings(0)
5318 , m_pRangeReadings(NULL)
5319 , m_NumberOfRangeReadings(0)
5321 assert(rSensorName.
ToString() !=
"");
5323 SetRangeReadings(rRangeReadings);
5331 delete [] m_pRangeReadings;
5332 m_pRangeReadings =
nullptr;
5342 return m_pRangeReadings;
5365 if (!rRangeReadings.empty())
5367 if (rRangeReadings.size() != m_NumberOfRangeReadings)
5370 delete [] m_pRangeReadings;
5373 m_NumberOfRangeReadings =
static_cast<kt_int32u>(rRangeReadings.size());
5376 m_pRangeReadings =
new kt_double[m_NumberOfRangeReadings];
5383 m_pRangeReadings[index++] = *iter;
5388 delete [] m_pRangeReadings;
5389 m_pRangeReadings = NULL;
5399 return SensorManager::GetInstance()->GetSensorByName<
LaserRangeFinder>(GetSensorName());
5408 return m_NumberOfRangeReadings;
5419 friend class boost::serialization::access;
5420 template<
class Archive>
5423 ar & BOOST_SERIALIZATION_NVP(m_NumberOfRangeReadings);
5424 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
SensorData);
5426 if (Archive::is_loading::value)
5428 m_pRangeReadings =
new kt_double[m_NumberOfRangeReadings];
5430 ar & boost::serialization::make_array<kt_double>(m_pRangeReadings, m_NumberOfRangeReadings);
5472 return m_OdometricPose;
5481 m_OdometricPose = rPose;
5542 return m_OdometricPose;
5551 m_OdometricPose = rPose;
5564 return m_CorrectedPose;
5573 m_CorrectedPose = rPose;
5584 SetCorrectedPose(rPose);
5594 std::shared_lock<std::shared_mutex> lock(m_Lock);
5599 std::unique_lock<std::shared_mutex> uniqueLock(m_Lock);
5603 return m_BarycenterPose;
5608 m_BarycenterPose = bcenter;
5618 std::shared_lock<std::shared_mutex> lock(m_Lock);
5623 std::unique_lock<std::shared_mutex> uniqueLock(m_Lock);
5627 return useBarycenter ? GetBarycenterPose() : GetSensorPose();
5636 return GetSensorAt(m_CorrectedPose);
5641 m_IsDirty = rIsDirty;
5650 m_CorrectedPose = GetCorrectedAt(rScanPose);
5672 Pose2 deviceOffsetPose2 = GetLaserRangeFinder()->GetOffsetPose();
5675 kt_double angleoffset = atan2(deviceOffsetPose2.
GetY(), deviceOffsetPose2.
GetX());
5677 Pose2 worldSensorOffset =
Pose2(offsetLength * cos(correctedHeading + angleoffset - offsetHeading),
5678 offsetLength * sin(correctedHeading + angleoffset - offsetHeading),
5681 return sPose - worldSensorOffset;
5690 std::shared_lock<std::shared_mutex> lock(m_Lock);
5695 std::unique_lock<std::shared_mutex> uniqueLock(m_Lock);
5699 return m_BoundingBox;
5704 m_BoundingBox = bbox;
5712 std::shared_lock<std::shared_mutex> lock(m_Lock);
5717 std::unique_lock<std::shared_mutex> uniqueLock(m_Lock);
5721 if (wantFiltered ==
true)
5723 return m_PointReadings;
5727 return m_UnfilteredPointReadings;
5735 m_PointReadings = points;
5739 m_UnfilteredPointReadings = points;
5752 if (pLaserRangeFinder != NULL)
5754 m_PointReadings.clear();
5755 m_UnfilteredPointReadings.clear();
5760 Pose2 scanPose = GetSensorPose();
5767 kt_double rangeReading = GetRangeReadings()[i];
5776 m_UnfilteredPointReadings.push_back(point);
5786 m_PointReadings.push_back(point);
5787 m_UnfilteredPointReadings.push_back(point);
5789 rangePointsSum += point;
5797 m_BarycenterPose =
Pose2(averagePosition, 0.0);
5801 m_BarycenterPose = scanPose;
5809 m_BoundingBox.Add(*iter);
5819 friend class boost::serialization::access;
5820 template<
class Archive>
5823 ar & BOOST_SERIALIZATION_NVP(m_OdometricPose);
5824 ar & BOOST_SERIALIZATION_NVP(m_CorrectedPose);
5825 ar & BOOST_SERIALIZATION_NVP(m_BarycenterPose);
5826 ar & BOOST_SERIALIZATION_NVP(m_PointReadings);
5827 ar & BOOST_SERIALIZATION_NVP(m_UnfilteredPointReadings);
5828 ar & BOOST_SERIALIZATION_NVP(m_BoundingBox);
5829 ar & BOOST_SERIALIZATION_NVP(m_IsDirty);
5902 : m_Points(rPoints),
5920 m_PointReadings.clear();
5921 m_UnfilteredPointReadings.clear();
5923 Pose2 scanPose = GetSensorPose();
5924 Pose2 robotPose = GetCorrectedPose();
5928 for (
kt_int32u i = 0; i < m_Points.size(); i++)
5931 if (!std::isfinite(m_Points[i].GetX()) || !std::isfinite(m_Points[i].GetY()))
5934 m_UnfilteredPointReadings.push_back(point);
5940 Pose2 pointPose(m_Points[i].GetX(), m_Points[i].GetY(), 0);
5944 m_PointReadings.push_back(point);
5945 m_UnfilteredPointReadings.push_back(point);
5947 rangePointsSum += point;
5955 m_BarycenterPose =
Pose2(averagePosition, 0.0);
5959 m_BarycenterPose = scanPose;
5967 m_BoundingBox.Add(*iter);
5991 : m_pOccupancyGrid(pGrid)
6011 friend class IncrementalOccupancyGrid;
6023 , m_pCellPassCnt(
Grid<
kt_int32u>::CreateGrid(0, 0, resolution))
6024 , m_pCellHitsCnt(
Grid<
kt_int32u>::CreateGrid(0, 0, resolution))
6025 , m_pCellUpdater(NULL)
6031 throw Exception(
"Resolution cannot be 0");
6037 GetCoordinateConverter()->SetScale(1.0 / resolution);
6038 GetCoordinateConverter()->SetOffset(rOffset);
6046 delete m_pCellUpdater;
6048 delete m_pCellPassCnt;
6049 delete m_pCellHitsCnt;
6051 delete m_pMinPassThrough;
6052 delete m_pOccupancyThreshold;
6070 ComputeDimensions(rScans, resolution, width, height, offset);
6074 return pOccupancyGrid;
6085 GetCoordinateConverter()->GetOffset(),
6086 1.0 / GetCoordinateConverter()->GetScale());
6087 memcpy(pOccupancyGrid->
GetDataPointer(), GetDataPointer(), GetDataSize());
6093 return pOccupancyGrid;
6121 double scale = GetCoordinateConverter()->GetScale();
6130 kt_double xStop = x + maxRange * cosTheta;
6131 kt_double xSteps = 1 + fabs(xStop - x) * scale;
6133 kt_double yStop = y + maxRange * sinTheta;
6134 kt_double ySteps = 1 + fabs(yStop - y) * scale;
6146 if (IsValidGridIndex(gridIndex) && IsFree(gridIndex))
6148 distance = (i + 1) * delta;
6156 return (distance < maxRange)? distance : maxRange;
6166 m_pMinPassThrough->SetValue(count);
6175 m_pOccupancyThreshold->SetValue(thresh);
6185 return m_pCellHitsCnt;
6194 return m_pCellPassCnt;
6216 if (*iter ==
nullptr)
6221 boundingBox.
Add((*iter)->GetBoundingBox());
6238 m_pCellPassCnt->Resize(GetWidth(), GetHeight());
6239 m_pCellPassCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
6241 m_pCellHitsCnt->Resize(GetWidth(), GetHeight());
6242 m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
6246 if (*iter ==
nullptr)
6286 if (rangeReading <= minRange || rangeReading >= maxRange || std::isnan(rangeReading))
6292 else if (rangeReading >= rangeThreshold)
6295 kt_double ratio = rangeThreshold / rangeReading;
6298 point.
SetX(scanPosition.
GetX() + ratio * dx);
6299 point.
SetY(scanPosition.
GetY() + ratio * dy);
6302 kt_bool isInMap = RayTrace(scanPosition, point, isEndPointValid, doUpdate);
6328 assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
6333 CellUpdater* pCellUpdater = doUpdate ? m_pCellUpdater : NULL;
6334 m_pCellPassCnt->TraceLine(gridFrom.
GetX(), gridFrom.
GetY(), gridTo.
GetX(), gridTo.
GetY(), pCellUpdater);
6337 if (isEndPointValid)
6339 if (m_pCellPassCnt->IsValidGridIndex(gridTo))
6341 kt_int32s index = m_pCellPassCnt->GridIndex(gridTo,
false);
6343 kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
6344 kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
6347 pCellPassCntPtr[index]++;
6348 pCellHitCntPtr[index]++;
6352 (*m_pCellUpdater)(index);
6357 return m_pCellPassCnt->IsValidGridIndex(gridTo);
6368 if (cellPassCnt > m_pMinPassThrough->GetValue())
6372 if (hitRatio > m_pOccupancyThreshold->GetValue())
6388 assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
6394 kt_int8u* pDataPtr = GetDataPointer();
6395 kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
6396 kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
6399 for (
kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++)
6401 UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr);
6413 m_pCellPassCnt->Resize(width, height);
6414 m_pCellHitsCnt->Resize(width, height);
6489 return m_pTitle->GetValue();
6497 return m_pAuthor->GetValue();
6505 return m_pDescription->GetValue();
6513 return m_pCopyright->GetValue();
6529 friend class boost::serialization::access;
6530 template<
class Archive>
6533 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Object);
6534 ar & BOOST_SERIALIZATION_NVP(*m_pTitle);
6535 ar & BOOST_SERIALIZATION_NVP(*m_pAuthor);
6536 ar & BOOST_SERIALIZATION_NVP(*m_pDescription);
6537 ar & BOOST_SERIALIZATION_NVP(*m_pCopyright);
6556 : m_pDatasetInfo(NULL)
6574 printf(
"Save To File\n");
6575 std::ofstream ofs(
filename.c_str());
6576 boost::archive::binary_oarchive oa(ofs, boost::archive::no_codecvt);
6577 oa << BOOST_SERIALIZATION_NVP(*
this);
6586 printf(
"Load From File\n");
6587 std::ifstream ifs(
filename.c_str());
6588 boost::archive::binary_iarchive ia(ifs, boost::archive::no_codecvt);
6589 ia >> BOOST_SERIALIZATION_NVP(*
this);
6599 if (pObject != NULL)
6601 if (
dynamic_cast<Sensor*
>(pObject))
6604 if (pSensor != NULL)
6606 m_SensorNameLookup[pSensor->
GetName()] = pSensor;
6610 m_Lasers.push_back(pObject);
6615 m_Data.insert({pSensorData->
GetUniqueId(), pSensorData});
6619 m_pDatasetInfo =
dynamic_cast<DatasetInfo*
>(pObject);
6623 std::cout <<
"Did not save object of non-data and non-sensor type" << std::endl;
6653 if (iterator != m_Data.end())
6655 delete iterator->second;
6656 iterator->second =
nullptr;
6657 m_Data.erase(iterator);
6661 std::cout <<
"Failed to remove data. Pointer to LocalizedRangeScan could not be found in dataset. "
6662 <<
"Most likely different pointer address but same object TODO STEVE." << std::endl;
6672 return m_pDatasetInfo;
6680 for (std::map<Name, Sensor*>::iterator iter = m_SensorNameLookup.begin(); iter != m_SensorNameLookup.end(); ++iter)
6694 for (
auto iter = m_Data.begin(); iter != m_Data.end(); ++iter)
6698 delete iter->second;
6699 iter->second = NULL;
6706 if (m_pDatasetInfo != NULL)
6708 delete m_pDatasetInfo;
6709 m_pDatasetInfo = NULL;
6721 friend class boost::serialization::access;
6722 template<
class Archive>
6725 std::cout<<
"**Serializing Dataset**\n";
6726 std::cout<<
"Dataset <- m_SensorNameLookup\n";
6727 ar & BOOST_SERIALIZATION_NVP(m_SensorNameLookup);
6728 std::cout<<
"Dataset <- m_Data\n";
6729 ar & BOOST_SERIALIZATION_NVP(m_Data);
6730 std::cout<<
"Dataset <- m_Lasers\n";
6731 ar & BOOST_SERIALIZATION_NVP(m_Lasers);
6732 std::cout<<
"Dataset <- m_pDatasetInfo\n";
6733 ar & BOOST_SERIALIZATION_NVP(m_pDatasetInfo);
6734 std::cout<<
"**Finished serializing Dataset**\n";
6738 BOOST_SERIALIZATION_ASSUME_ABSTRACT(Dataset)
6765 assert(m_pArray != NULL);
6777 memset(m_pArray, 0,
sizeof(
kt_int32s) * m_Capacity);
6797 if (size > m_Capacity)
6799 if (m_pArray != NULL)
6817 assert(index < m_Size);
6819 return m_pArray[index];
6829 assert(index < m_Size);
6831 return m_pArray[index];
6856 friend class boost::serialization::access;
6857 template<
class Archive>
6860 ar & BOOST_SERIALIZATION_NVP(m_Capacity);
6861 ar & BOOST_SERIALIZATION_NVP(m_Size);
6862 if (Archive::is_loading::value)
6866 ar & boost::serialization::make_array<kt_int32s >(m_pArray, m_Capacity);
6889 template<
typename T>
6904 , m_ppLookupArray(NULL)
6926 return m_ppLookupArray[index];
6950 assert(angleOffset != 0.0);
6951 assert(angleResolution != 0.0);
6969 localPoints.push_back(vec);
6975 kt_double startAngle = angleCenter - angleOffset;
6976 for (
kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
6978 angle = startAngle + angleIndex * angleResolution;
6979 ComputeOffsets(angleIndex,
angle, localPoints, pScan);
6993 m_ppLookupArray[angleIndex]->SetSize(
static_cast<kt_int32u>(rLocalPoints.size()));
6994 m_Angles.at(angleIndex) =
angle;
6999 const Vector2<kt_double>& rGridOffset = m_pGrid->GetCoordinateConverter()->GetOffset();
7006 kt_int32s* pAngleIndexPointer = m_ppLookupArray[angleIndex]->GetArrayPointer();
7024 offset.
SetX(cosine * rPosition.
GetX() - sine * rPosition.
GetY());
7025 offset.
SetY(sine * rPosition.
GetX() + cosine * rPosition.
GetY());
7031 kt_int32s lookupIndex = m_pGrid->Grid<T>::GridIndex(gridPoint,
false);
7033 pAngleIndexPointer[readingIndex] = lookupIndex;
7037 assert(readingIndex == rLocalPoints.size());
7048 if (size > m_Capacity)
7050 if (m_ppLookupArray != NULL)
7057 for (
kt_int32u i = 0; i < m_Capacity; i++)
7065 m_Angles.resize(size);
7073 if (m_ppLookupArray)
7075 for (
kt_int32u i = 0; i < m_Capacity; i++)
7077 delete m_ppLookupArray[i];
7080 if (m_ppLookupArray)
7082 delete[] m_ppLookupArray;
7083 m_ppLookupArray = NULL;
7097 friend class boost::serialization::access;
7098 template<
class Archive>
7101 ar & BOOST_SERIALIZATION_NVP(m_pGrid);
7102 ar & BOOST_SERIALIZATION_NVP(m_Capacity);
7103 ar & BOOST_SERIALIZATION_NVP(m_Size);
7104 ar & BOOST_SERIALIZATION_NVP(m_Angles);
7105 if (Archive::is_loading::value)
7108 for (
kt_int32u i = 0; i < m_Capacity; i++)
7113 ar & boost::serialization::make_array<LookupArray*>(m_ppLookupArray, m_Capacity);
7122 : m_Position(rPose.GetPosition().GetX(), rPose.GetPosition().GetY())
7136 template<
typename T>
7140 if (pParameter != NULL)
7142 std::stringstream stream;
7148 throw Exception(
"Parameter does not exist: " + rName);
7156 if (pParameter != NULL)
7158 pParameter->SetValueFromString(value ?
"true" :
"false");
7162 throw Exception(
"Parameter does not exist: " + rName);
7199 #endif // karto_sdk_KARTO_H