Scan.cpp
Go to the documentation of this file.
00001 //
00002 // Scan.cpp
00003 //
00004 // Scan data container.
00005 //
00006 // Version history
00007 // 17.11.2011, willhvo: First version
00008 //
00009 
00010 #include "Scan.hpp"
00011 #include <algorithm>
00012 #include <cmath>
00013 #include <limits>
00014 #include "ScannerInfo.hpp"
00015 #include "../tools/errorhandler.hpp"
00016 #include <set>
00017 #include "../tools/toolbox.hpp"
00018 
00019 namespace datatypes
00020 {
00021         
00022 //
00023 // maxPoints The maximum number of scan points that are allocated by the
00024 //                               constructor. This can be increased later by calling reserve().
00025 //
00026 Scan::Scan (size_type maxPoints)
00027         : m_flags(0)
00028         , m_scanNumber(0x0000)
00029         , m_points()
00030 {
00031         m_datatype = Datatype_Scan;
00032         m_points.reserve(maxPoints); // allocate memory for the point array
00033 
00034         m_beVerbose = false;
00035 }
00036 
00037 Scan::Scan (const Scan& other)
00038         : m_points()
00039 {
00040         copy(other);
00041 }
00042 
00043 // Estimate the memory usage of this object
00044 const UINT32 Scan::getUsedMemory() const
00045 {
00046         return (sizeof(*this) +
00047                         (getNumPoints() * sizeof(ScanPoint)) +
00048                         (m_scannerInfos.size() * sizeof(ScannerInfo)));
00049 }
00050 
00051 
00052 Scan& Scan::copy (const Scan& other)
00053 {
00054         // These are the trivial by-value assignments
00055 //      m_scanStartTime  = other.m_scanStartTime;
00056 //      m_scanEndTime    = other.m_scanEndTime;
00057 //      m_scanMidTime    = other.m_scanMidTime;
00058 //      m_syncTimestamp  = other.m_syncTimestamp;
00059 //      m_startAngle     = other.m_startAngle;
00060 //      m_endAngle       = other.m_endAngle;
00061 //      m_scannerStatus  = other.m_scannerStatus;
00062         m_flags          = other.m_flags;
00063         m_scanNumber     = other.m_scanNumber;
00064 //      m_deviceID       = other.m_deviceID;
00065 //      m_scannerType    = other.m_scannerType;
00066         m_sourceId                      = other.m_sourceId; 
00067         m_datatype                      = other.m_datatype;
00068 
00069         // The PointList vector itself is copied element-by-element.
00070         m_points         = other.m_points;
00071         m_points.reserve (other.capacity());
00072 
00073         // This vector is copied element-by-element, by value.
00074         m_scannerInfos   = other.m_scannerInfos;
00075 
00076         // Verbose-Flag
00077         m_beVerbose = other.m_beVerbose;
00078         
00079         return *this;
00080 }
00081 
00082 //
00083 // Tries its best to estimate the current memory usage of this object.
00084 // This function is required to estimate the required storage space in buffers.
00085 //
00086 UINT32 Scan::getTotalObjectSize()
00087 {
00088         UINT32 size;
00089 
00090         size = sizeof(*this);                                                                           // Dieses Objekt selber
00091         size += getNumPoints() * sizeof(ScanPoint);                     // Seine Scanpunkte
00092         size += m_scannerInfos.size() * sizeof(ScannerInfo);    // Die Scanner-Infos
00093 
00094         return size;
00095 }
00096 
00097 Scan& Scan::operator= (const Scan& other)
00098 {
00099         return copy(other);
00100 }
00101 
00102 
00103 /*
00104 bool Scan::operator==(const Scan& other) const
00105 {
00106         return
00107                 m_scanNumber     == other.m_scanNumber
00108                 && m_flags          == other.m_flags
00109                 && m_scanStartTime  == other.m_scanStartTime
00110                 && m_scanEndTime    == other.m_scanEndTime
00111                 && m_scanMidTime    == other.m_scanMidTime
00112                 && m_syncTimestamp  == other.m_syncTimestamp
00113                 && m_points         == other.m_points
00114                 && m_scannerInfos   == other.m_scannerInfos
00115                 // Either scannerInfos is not empty (in which case the
00116                 // deprecated variables are ignored anyway), or the old
00117                 // variables must be equal
00118                 && (!m_scannerInfos.empty()
00119                         || ((m_startAngle        == other.m_startAngle
00120                                  || (isNaN(m_startAngle) && isNaN(other.m_startAngle)))
00121                                 && (m_endAngle       == other.m_endAngle
00122                                         || (isNaN(m_endAngle) && isNaN(other.m_endAngle)))
00123                                 && m_scannerStatus  == other.m_scannerStatus
00124                                 && m_deviceID       == other.m_deviceID
00125                                 && m_scannerType    == other.m_scannerType
00126                            ));
00127 }
00128 */
00129 
00130 //
00131 // After clear(), the scan will not contain any scanpoints at all.
00132 //
00133 void Scan::clear ()
00134 {
00135         // Clear member variables
00136         m_flags                 = 0;
00137         m_scanNumber    = 0x0000;
00138         m_sourceId              = 0;
00139 
00140         m_points.clear();
00141         m_scannerInfos.clear();
00142 }
00143 
00144 // Default destructor
00145 Scan::~Scan()
00146 {
00147 }
00148 
00149 void Scan::resize(size_type new_size, const ScanPoint& default_point)
00150 {
00151         if (size_type((UINT16)new_size) != new_size)
00152         {
00153                 throw std::out_of_range ("Scan::resize was called with new size larger than what fits into UINT16: " + toString(new_size) + " but only 65536 is allowed!");
00154         }
00155 
00156         m_points.resize(new_size, default_point);
00157 }
00158 
00159 void Scan::reserve(size_type new_capacity)
00160 {
00161         if (size_type((UINT16)new_capacity) != new_capacity)
00162         {
00163                 throw std::out_of_range ("Scan::reserve was called with new capacity larger than what fits into UINT16: " + toString(new_capacity) + " but only 65536 is allowed!");
00164         }
00165 
00166         m_points.reserve(new_capacity);
00167 }
00168 
00173 ScanPoint& Scan::addNewPoint()
00174 {
00175         m_points.push_back(ScanPoint());
00176         return m_points.back();
00177 }
00178 
00179 void Scan::setVehicleCoordinates(bool inVehicleCoordinates)
00180 {
00181         if (inVehicleCoordinates)
00182                 m_flags |= FlagVehicleCoordinates;
00183         else
00184                 m_flags &= ~FlagVehicleCoordinates;
00185 }
00186 
00187 
00188 // Sort criterion. This method is passed to std::sort() to sort scan
00189 // points by descending azimuth angles.
00190 static bool isDescendingAngle (const ScanPoint& P1, const ScanPoint& P2)
00191 {
00192         return (P1.getHAngle() > P2.getHAngle());
00193 }
00194 
00195 void Scan::sort()
00196 {
00197         std::sort(getPointListBegin(), getPointListEnd(), isDescendingAngle);
00198 }
00199 
00200 void Scan::addCartesianOffset(double offsetX, double offsetY, double offsetZ)
00201 {
00202         for (PointList::iterator point = getPointListBegin();  point != getPointListEnd();  point++)
00203                 point->addCartesianOffset(offsetX, offsetY, offsetZ);
00204 }
00205 
00206 void Scan::addPolarOffset(double distOffset, double hAngleOffset, double vAngleOffset)
00207 {
00208         for (PointList::iterator point = getPointListBegin();  point != getPointListEnd();  point++)
00209                 point->addPolarOffset(distOffset, hAngleOffset, vAngleOffset);
00210 }
00211 
00212 void Scan::setScannerInfos(const ScannerInfoVector& v)
00213 {
00214         // Sanity check: Only one ScannerInfo per deviceID allowed, except
00215         // for 8-Layer scanners.
00216         std::set<UINT8> availableDevicesOnce;
00217         std::set<UINT8> availableDevicesTwice;
00218         for (ScannerInfoVector::const_iterator it = v.begin();
00219                  it != v.end(); it++)
00220         {
00221                 const ScannerInfo& sinfo = *it;
00222                 const UINT8 deviceID = sinfo.getDeviceID();
00223 
00224                 // Did we already see this deviceID previously?
00225                 if (availableDevicesOnce.count(deviceID) == 0)
00226                 {
00227                         // No, so record this deviceID for later.
00228                         availableDevicesOnce.insert(deviceID);
00229                 }
00230                 else
00231                 {
00232                         // Error: 4L-deviceID is here a second time!
00233                         throw std::logic_error("Scan::setScannerInfos called with a list (size=" +
00234                                                                         ::toString(v.size()) + ") that contains two ScannerInfos with device ID " +
00235                                                                         ::toString(int(deviceID)) + ", type " + ScannerInfo::scannerTypeToString(sinfo.getScannerType()) +
00236                                                                         ". This is not allowed - for each scanner at most one ScannerInfo may be stored in the ScannerInfoVector.");
00237                 }
00238         }
00239 
00240         m_scannerInfos = v;
00241 }
00242 
00243 
00244 
00245 // Flags
00246 /*
00247 void Scan::setGroundLabeled(bool isGroundLabeled)
00248 {
00249         if (isGroundLabeled)
00250                 m_flags |=  FlagGroundLabeled;
00251         else
00252                 m_flags &= ~FlagGroundLabeled;
00253 }
00254 
00255 void Scan::setRainLabeled(bool isRainLabeled)
00256 {
00257         if (isRainLabeled)
00258                 m_flags |=  FlagRainLabeled;
00259         else
00260                 m_flags &= ~FlagRainLabeled;
00261 }
00262 
00263 void Scan::setDirtLabeled(bool isDirtLabeled)
00264 {
00265         if (isDirtLabeled)
00266                 m_flags |=  FlagDirtLabeled;
00267         else
00268                 m_flags &= ~FlagDirtLabeled;
00269 }
00270 
00271 void Scan::setCoverageLabeled(bool isCoverageLabeled)
00272 {
00273         if (isCoverageLabeled)
00274                 m_flags |=  FlagCoverageLabeled;
00275         else
00276                 m_flags &= ~FlagCoverageLabeled;
00277 }
00278 
00279 void Scan::setBackgroundLabeled(bool isBackgroundLabeled)
00280 {
00281         if (isBackgroundLabeled)
00282                 m_flags |=  FlagBackgroundLabeled;
00283         else
00284                 m_flags &= ~FlagBackgroundLabeled;
00285 }
00286 
00287 void Scan::setReflectorLabeled(bool isReflectorLabeled)
00288 {
00289         if (isReflectorLabeled)
00290                 m_flags |=  FlagReflectorLabeled;
00291         else
00292                 m_flags &= ~FlagReflectorLabeled;
00293 }
00294 */
00295 
00296 //
00297 // Transform the scan to vehicle coordinates, but do not sort the resulting scanpoints by angles.
00298 //
00299 bool Scan::transformToVehicleCoordinatesUnsorted()
00300 {
00301         // Check if Scan might already be converted
00302         if ((getFlags() & FlagVehicleCoordinates) != 0)
00303         {
00304                 // Ist schon in Fahrzeugkoordinaten.
00305                 // traceDebug(SCAN_VERSION) << "convertToVehicleCoordinates: Scan (#" << getScanNumber() << ") already in vehicle coordinates. Nothing to do." << std::endl;
00306                 return true; // it's ok, job is already done
00307         }
00308 
00309         // If there are no scanner informations available -> no transformation
00310         if (getScannerInfos().empty())
00311         {
00312                 printWarning("Scan::convertToVehicleCoordinates: Scan (#" + ::toString(getScanNumber()) +
00313                                                 ") has empty ScannerInfos - no conversion possible, aborting!");
00314                 return false;
00315         }
00316 
00317         // Hole die Scanner-Position
00318         Position3D mountPos = m_scannerInfos.at(0).getMountingPosition();
00319 //      printInfoMessage("Scan::transformToVehicleCoordinatesUnsorted: Mounting position is " + mountPos.toString() + ".", true);
00320         
00321         // Transformiere alle Punkte
00322         for (Scan::iterator sptIt = begin(); sptIt != end(); sptIt++)
00323         {
00324                 Point3D pt = sptIt->toPoint3D();
00325                 mountPos.transformToVehicle(&pt);
00326                 sptIt->setPoint3D(pt);
00327         }
00328 
00329 
00330         // Set transformation flag (if everything went well)
00331         setFlags(getFlags() | FlagVehicleCoordinates );
00332 
00333         // Also set the transformation flag in all the ScannerInfo entries
00334         for (ScannerInfoVector::iterator siIt = getScannerInfos().begin(); siIt != getScannerInfos().end(); siIt++)
00335         {
00336                 ScannerInfo& sinfo = *siIt;
00337                 sinfo.setScanFlags(sinfo.getScanFlags() | FlagVehicleCoordinates);
00338         }
00339 
00340         return true;
00341 }
00342 
00343 bool Scan::transformToVehicleCoordinates()
00344 {
00345         const bool r = transformToVehicleCoordinatesUnsorted();
00346 
00347         // We also sort this because this got
00348         // forgotten all too often.
00349         sort();
00350 
00351         return r;
00352 }
00353 
00354 
00355 Scan::ScannerInfoVector& Scan::getScannerInfos()
00356 {
00357         return m_scannerInfos;
00358 }
00359 
00360 const ScannerInfo* Scan::getScannerInfoByDeviceId(UINT8 id) const
00361 {
00362         for (ScannerInfoVector::const_iterator it = m_scannerInfos.begin();
00363                  it != m_scannerInfos.end();
00364                  it++)
00365         {
00366                 if ((*it).getDeviceID() == id)
00367                         return &(*it);
00368         }
00369 
00370         return NULL;
00371 }
00372 
00373 void Scan::clearLabelFlag(Scan::ScanFlags scanFlag)
00374 {
00375         UINT32 test = ~(scanFlag);
00376         m_flags &= test;
00377 }
00378 
00379 }       // namespace datatypes


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Thu Jun 6 2019 21:02:36