|
| 1 | +/* +---------------------------------------------------------------------------+ |
| 2 | + | Mobile Robot Programming Toolkit (MRPT) | |
| 3 | + | http://www.mrpt.org/ | |
| 4 | + | | |
| 5 | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | |
| 6 | + | See: http://www.mrpt.org/Authors - All rights reserved. | |
| 7 | + | Released under BSD License. See details in http://www.mrpt.org/License | |
| 8 | + +---------------------------------------------------------------------------+ */ |
| 9 | +#ifndef CPOINTSMAP_LIBLAS_H |
| 10 | +#define CPOINTSMAP_LIBLAS_H |
| 11 | + |
| 12 | +/** \file Include this file in your user application only if you have libLAS installed in your system */ |
| 13 | +#include <liblas/liblas.hpp> |
| 14 | +#include <liblas/reader.hpp> |
| 15 | +#include <liblas/writer.hpp> |
| 16 | + |
| 17 | +#include <mrpt/maps/CPointsMap.h> |
| 18 | +#include <string> |
| 19 | +#include <iostream> |
| 20 | +#include <fstream> |
| 21 | + |
| 22 | +namespace mrpt |
| 23 | +{ |
| 24 | +/** \ingroup mrpt_maps_grp */ |
| 25 | +namespace maps |
| 26 | +{ |
| 27 | + /** \addtogroup mrpt_maps_liblas_grp libLAS interface for CPointsMap (in #include <mrpt/maps/CPointsMaps_liblas.h>) |
| 28 | + * \ingroup mrpt_maps_grp |
| 29 | + * @{ */ |
| 30 | + |
| 31 | + /** Optional settings for saveLASFile() */ |
| 32 | + struct LAS_WriteParams |
| 33 | + { |
| 34 | + // None. |
| 35 | + }; |
| 36 | + |
| 37 | + /** Optional settings for loadLASFile() */ |
| 38 | + struct LAS_LoadParams |
| 39 | + { |
| 40 | + // None. |
| 41 | + }; |
| 42 | + |
| 43 | + /** Extra information gathered from the LAS file header */ |
| 44 | + struct LAS_HeaderInfo |
| 45 | + { |
| 46 | + std::string FileSignature; |
| 47 | + std::string SystemIdentifier; |
| 48 | + std::string SoftwareIdentifier; |
| 49 | + std::string project_guid; |
| 50 | + std::string spatial_reference_proj4; //!< Proj.4 string describing the Spatial Reference System. |
| 51 | + uint16_t creation_year;//!< Creation date (Year number) |
| 52 | + uint16_t creation_DOY; //!< Creation day of year |
| 53 | + |
| 54 | + LAS_HeaderInfo() : creation_year(0),creation_DOY(0) |
| 55 | + {} |
| 56 | + }; |
| 57 | + |
| 58 | + /** Save the point cloud as an ASPRS LAS binary file (requires MRPT built against liblas). Refer to http://www.liblas.org/ |
| 59 | + * \return false on any error */ |
| 60 | + inline bool saveLASFile( |
| 61 | + const mrpt::maps::CPointsMaps &ptmap, |
| 62 | + const std::string &filename, |
| 63 | + const LAS_WriteParams & params = LAS_WriteParams() ) |
| 64 | + { |
| 65 | + std::ofstream ofs; |
| 66 | + ofs.open(filename.c_str(), ios::out | ios::binary); |
| 67 | + |
| 68 | + if (!ofs.is_open()) |
| 69 | + { |
| 70 | + cerr << "[saveLASFile] Couldn't write to file: " << filename << endl; |
| 71 | + return false; |
| 72 | + } |
| 73 | + |
| 74 | + // Fill-in header: |
| 75 | + // --------------------------------- |
| 76 | + liblas::Header header; |
| 77 | + const size_t nPts = ptmap.size(); |
| 78 | + |
| 79 | + header.SetPointRecordsCount(nPts); |
| 80 | + |
| 81 | + // Create writer: |
| 82 | + // --------------------------------- |
| 83 | + liblas::Writer writer(ofs,header); |
| 84 | + |
| 85 | + const bool has_color = ptmap.hasColorPoints(); |
| 86 | + const float col_fract = 255.0f; |
| 87 | + |
| 88 | + liblas::Point pt(&header); |
| 89 | + liblas::Color col; |
| 90 | + for (size_t i=0;i<nPts;i++) |
| 91 | + { |
| 92 | + float x,y,z,R,G,B; |
| 93 | + ptmap.getPoint(i, x,y,z, R,G,B); |
| 94 | + |
| 95 | + pt.SetX(x); |
| 96 | + pt.SetY(y); |
| 97 | + pt.SetZ(z); |
| 98 | + |
| 99 | + if (has_color) |
| 100 | + { |
| 101 | + col.SetRed( static_cast<uint16_t>(R*col_fract) ); |
| 102 | + col.SetGreen( static_cast<uint16_t>(G*col_fract) ); |
| 103 | + col.SetBlue( static_cast<uint16_t>(B*col_fract) ); |
| 104 | + pt.SetColor(col); |
| 105 | + } |
| 106 | + |
| 107 | + if (!writer.WritePoint(pt)) |
| 108 | + { |
| 109 | + std::cerr << "[saveLASFile] liblas returned error writing point #" << i << " to file.\n"; |
| 110 | + return false; |
| 111 | + } |
| 112 | + } |
| 113 | + return true; // All ok. |
| 114 | + } |
| 115 | + |
| 116 | + /** Load the point cloud from an ASPRS LAS binary file (requires MRPT built against liblas). Refer to http://www.liblas.org/ |
| 117 | + * \note Color (RGB) information will be taken into account if using the derived class mrpt::maps::CColouredPointsMap |
| 118 | + * \return false on any error */ |
| 119 | + inline bool loadLASFile( |
| 120 | + mrpt::maps::CPointsMaps &ptmap, |
| 121 | + const std::string &filename, |
| 122 | + LAS_HeaderInfo &out_headerInfo, |
| 123 | + const LAS_LoadParams ¶ms = LAS_LoadParams() ) |
| 124 | + { |
| 125 | + using namespace std; |
| 126 | + ptmap.clear(); |
| 127 | + |
| 128 | + ifstream ifs; |
| 129 | + ifs.open(filename.c_str(), ios::in | ios::binary); |
| 130 | + |
| 131 | + if (!ifs.is_open()) |
| 132 | + { |
| 133 | + cerr << "[loadLASFile] Couldn't open file: " << filename << endl; |
| 134 | + return false; |
| 135 | + } |
| 136 | + |
| 137 | + // Create LAS reader: |
| 138 | + // --------------------- |
| 139 | + liblas::Reader reader(ifs); |
| 140 | + |
| 141 | + // Parse header info: |
| 142 | + // --------------------- |
| 143 | + liblas::Header const& header = reader.GetHeader(); |
| 144 | + const size_t nPts = header.GetPointRecordsCount(); |
| 145 | + ptmap.reserve(nPts); |
| 146 | + |
| 147 | + out_headerInfo.FileSignature = header.GetFileSignature(); |
| 148 | + out_headerInfo.SystemIdentifier = header.GetSystemId(); |
| 149 | + out_headerInfo.SoftwareIdentifier = header.GetSoftwareId(); |
| 150 | +#if LIBLAS_VERSION_NUM < 1800 |
| 151 | + out_headerInfo.project_guid = header.GetProjectId().to_string(); |
| 152 | +#else |
| 153 | + out_headerInfo.project_guid = boost::lexical_cast<std::string>(header.GetProjectId()); |
| 154 | +#endif |
| 155 | + out_headerInfo.spatial_reference_proj4 = header.GetSRS().GetProj4(); |
| 156 | + out_headerInfo.creation_year = header.GetCreationYear(); |
| 157 | + out_headerInfo.creation_DOY = header.GetCreationDOY(); |
| 158 | + |
| 159 | + // Load points: |
| 160 | + // --------------------- |
| 161 | + const bool has_color = ptmap.hasColorPoints(); |
| 162 | + const float col_fract = 1.0f/255.0f; |
| 163 | + while (reader.ReadNextPoint()) |
| 164 | + { |
| 165 | + liblas::Point const& p = reader.GetPoint(); |
| 166 | + |
| 167 | + if (has_color) |
| 168 | + { |
| 169 | + liblas::Color const& col = p.GetColor(); |
| 170 | + ptmap.insertPoint( p.GetX(),p.GetY(),p.GetZ(), col.GetRed()*col_fract,col.GetGreen()*col_fract,col.GetBlue()*col_fract ); |
| 171 | + } |
| 172 | + else |
| 173 | + { |
| 174 | + ptmap.insertPoint( p.GetX(),p.GetY(),p.GetZ() ); |
| 175 | + } |
| 176 | + } |
| 177 | + |
| 178 | + if (ptmap.size()!=nPts) |
| 179 | + cerr << "[loadLASFile] Header says point count is " << nPts << " but only " << ptmap.size() << " were really parsed in.\n"; |
| 180 | + |
| 181 | + ptmap.mark_as_modified(); |
| 182 | + |
| 183 | + return true; // All ok. |
| 184 | + } |
| 185 | + /** @} */ |
| 186 | +} |
| 187 | +} // End of namespaces |
| 188 | + |
| 189 | +#endif |
0 commit comments