pcd-loader (empty) → 0.1.1.0
raw patch · 8 files changed
+419/−0 lines, 8 filesdep +attoparsecdep +basedep +binarysetup-changed
Dependencies added: attoparsec, base, binary, bytestring, deepseq, lens, linear, mtl, pcd-loader, text, vector
Files
- LICENSE +30/−0
- Setup.hs +2/−0
- pcd-loader.cabal +35/−0
- src/PCD/Data.hs +136/−0
- src/PCD/Header.hs +149/−0
- src/PCD/Internal/SmallLens.hs +45/−0
- src/PCD/Internal/Types.hs +12/−0
- src/executable/Main.hs +10/−0
+ LICENSE view
@@ -0,0 +1,30 @@+Copyright (c) 2012, Anthony Cowley++All rights reserved.++Redistribution and use in source and binary forms, with or without+modification, are permitted provided that the following conditions are met:++ * Redistributions of source code must retain the above copyright+ notice, this list of conditions and the following disclaimer.++ * Redistributions in binary form must reproduce the above+ copyright notice, this list of conditions and the following+ disclaimer in the documentation and/or other materials provided+ with the distribution.++ * Neither the name of Anthony Cowley nor the names of other+ contributors may be used to endorse or promote products derived+ from this software without specific prior written permission.++THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ Setup.hs view
@@ -0,0 +1,2 @@+import Distribution.Simple+main = defaultMain
+ pcd-loader.cabal view
@@ -0,0 +1,35 @@+name: pcd-loader+version: 0.1.1.0+synopsis: PCD file loader.+description: Parser for PCD (point cloud data) formats. See+ <http://pointclouds.org/documentation/tutorials/pcd_file_format.php>+ for more information.+license: BSD3+license-file: LICENSE+author: Anthony Cowley+maintainer: acowley@gmail.com+copyright: (c) Anthony Cowley 2012+category: Robotics Graphics+build-type: Simple+cabal-version: >=1.10++source-repository head+ type: git+ location: git://github.com/acowley/pcd-loader.git++library+ exposed-modules: PCD.Data, PCD.Header, PCD.Internal.Types+ other-modules: PCD.Internal.SmallLens+ ghc-options: -O2 -Wall+ hs-source-dirs: src+ build-depends: base >= 4.6 && < 5, text, mtl, lens, vector, bytestring, + attoparsec, binary, deepseq, linear+ default-language: Haskell2010++executable pcd2bin+ hs-source-dirs: src/executable+ main-is: Main.hs + ghc-options: -O2 -Wall+ build-depends: base >= 4.6 && < 5, pcd-loader + default-language: Haskell2010+
+ src/PCD/Data.hs view
@@ -0,0 +1,136 @@+{-# LANGUAGE ScopedTypeVariables #-}+-- |Parser for PCD (point cloud data) files. Also provides a facility+-- for converting from ASCII to binary formatted point data.+module PCD.Data where+import Control.Applicative+import Control.DeepSeq+import Control.Monad (when)+import Data.Attoparsec.Text+import qualified Data.Attoparsec.Text.Lazy as ATL+import qualified Data.Text.Lazy.IO as TL+import qualified Data.Text.IO as T+import qualified Data.Vector as B+import qualified Data.Vector.Generic as G+import qualified Data.Vector.Generic.Mutable as GM+import qualified Data.Vector.Storable as V+import qualified Data.Vector.Storable.Mutable as VM+import Foreign.Storable (Storable, sizeOf)+import System.IO (Handle, openFile, hClose, + IOMode(..), withBinaryFile, hPutBuf, hGetBuf)+import PCD.Header+import PCD.Internal.SmallLens+import PCD.Internal.Types++-- |Read point data using a user-supplied ASCII point parser.+readAsciiPoints :: (G.Vector v a) => + Header -> Handle -> ATL.Parser a -> IO (v a)+readAsciiPoints pcd h p = aux <$> TL.hGetContents h+ where n = fromIntegral $ pcd^.points+ aux t0 = G.create $+ do v <- GM.new n+ let write = GM.write v+ go i t+ | i == n = return v+ | otherwise = case ATL.parse p t of+ ATL.Done t' pt -> write i pt >> + go (i+1) t'+ ATL.Fail _ _ msg -> error msg+ go 0 t0++-- |Load points of unknown dimension into a boxed vector with a list+-- of 'FieldType' as the point representation.+readAsciiPointsDefault :: Header -> Handle -> IO (B.Vector [FieldType])+readAsciiPointsDefault pcd h = readAsciiPoints pcd h $ pointParser pcd++-- |Read back 'Storable' points saved as binary data.+readBinPoints :: forall a. Storable a => Header -> Handle -> IO (Either String (Vector a))+readBinPoints pcd h+ | ptSize /= sz = return . Left $ + "Deserialization type is not the same size as the points "+++ "described by this file. The PCD file dicates "+++ show ptSize++" bytes per point; destination type takes up "+++ show sz++" bytes."+ | otherwise = do vm <- VM.new (fromIntegral $ pcd^.points)+ _ <- VM.unsafeWith vm (flip (hGetBuf h) numBytes)+ Right <$> V.freeze vm+ where sz = sizeOf (undefined::a)+ numBytes = fromIntegral (pcd^.points) * sz+ ptSize = sum (_sizes pcd)++-- |Reads point data in either ASCII or binary formats given an ASCII+-- parser for the point data type and a 'Storable' instance. If you+-- know that your points are binary or ASCII, consider using+-- 'readBinPoints' or 'readAsciiPoints'.+readPointData :: Storable a => + Header -> Handle -> ATL.Parser a -> + IO (Either String (Vector a))+readPointData hd h pa + | hd^.format == ASCII = readAsciiPoints hd h pa >>= return . Right+ | otherwise = readBinPoints hd h++-- |Parse 3D points serialized in ASCII.+readXYZ_ascii :: Fractional a => ATL.Parser (V3 a)+readXYZ_ascii = (\[x,y,z] -> V3 x y z) <$> + count 3 ((realToFrac <$> double) <* skipSpace)++-- |Parse 4D points serialized to ASCII. This is useful for points+-- with X,Y,Z, and RGB fields each represented by a single float.+readXYZW_ascii :: Fractional a => ATL.Parser (V4 a)+readXYZW_ascii = (\[x,y,z,w] -> V4 x y z w) <$>+ count 4 ((realToFrac <$> double) <* skipSpace)++-- |Use an existing PCD header to save binary point data to a+-- file. The supplied header is used as-is, except that its format is+-- set to 'Binary'.+saveBinaryPcd :: forall a. Storable a => + FilePath -> Header -> V.Vector a -> IO ()+saveBinaryPcd outputFile pcd pts = + do putStrLn $ "Converting "++show (V.length pts)++" points"+ let pcd' = format .~ Binary $ pcd+ sz = sizeOf (undefined::a) * V.length pts+ T.writeFile outputFile (writeHeader pcd')+ withBinaryFile outputFile AppendMode $ \h ->+ V.unsafeWith pts (flip (hPutBuf h) sz)++-- |Convert the single-precision floating point XYZ or XYZW (where+-- \"W\" may be an RGB triple encoded as a single float) points in an+-- ASCII PCD to a binary PCD file.+asciiToBinary :: FilePath -> FilePath -> IO ()+asciiToBinary i o = do h <- openFile i ReadMode+ (pcdh,_) <- readHeader h+ pcdh `deepseq` print pcdh+ when ((pcdh^.format) /= ASCII)+ (error "Input PCD is already binary!")+ case pcdh ^. sizes of+ [4,4,4] -> readAsciiPoints pcdh h readXYZ_ascii >>=+ (saveBinaryPcd o pcdh+ :: V.Vector (V3 Float) -> IO ())+ [4,4,4,4] -> readAsciiPoints pcdh h readXYZW_ascii >>=+ (saveBinaryPcd o pcdh+ :: V.Vector (V4 Float) -> IO ())+ _ -> error $ "Only 32-bit floating point 3D or 4D "+++ "points are supported."+ hClose h++-- |Load points stored in a PCD file into a 'Vector'.+loadPoints :: Storable a => ATL.Parser a -> FilePath -> IO (Vector a)+loadPoints parser pcdFile = do h <- openFile pcdFile ReadMode+ (pcdh,_) <- readHeader h+ r <- pcdh `deepseq` readPointData pcdh h parser+ hClose h+ return $ either (const V.empty) id r++-- |Read a PCD file consisting of floating point XYZ coordinates for+-- each point.+loadXyz :: (Fractional a, Storable a) => FilePath -> IO (Vector (V3 a))+loadXyz = loadPoints readXYZ_ascii+{-# SPECIALIZE loadXyz :: FilePath -> IO (Vector (V3 Float)) #-}+{-# SPECIALIZE loadXyz :: FilePath -> IO (Vector (V3 Double)) #-}++-- |Read a PCD file consisting of floating point XYZW coordinates for+-- each point (where the final \"W\" field may be an RGB triple+-- encoded as a float).+loadXyzw :: (Fractional a, Storable a) => FilePath -> IO (Vector (V4 a))+loadXyzw = loadPoints readXYZW_ascii+{-# SPECIALIZE loadXyzw :: FilePath -> IO (Vector (V4 Float)) #-}+{-# SPECIALIZE loadXyzw :: FilePath -> IO (Vector (V4 Double)) #-}
+ src/PCD/Header.hs view
@@ -0,0 +1,149 @@+{-# LANGUAGE TemplateHaskell, OverloadedStrings, FlexibleContexts, + BangPatterns #-}+-- |Define a data structure for a PCD file header and an associated+-- parser.+module PCD.Header where+import Control.Applicative+import Control.Arrow ((***))+import Control.Monad.State+import Data.Foldable (Foldable, foldMap)+import Data.Int+import Data.List (intersperse)+import Data.Monoid (mconcat, (<>))+import Data.Text (Text)+import qualified Data.Text as T+import Data.Text.IO+import Data.Word+import Data.Attoparsec.Text hiding (I)+import System.IO (Handle)+import Control.DeepSeq+import PCD.Internal.SmallLens+import PCD.Internal.Types++-- |Fields attached to a point may be signed integers (I), unsigned+-- integers (U), or floating point (F).+data DimType = I | U | F deriving (Eq,Show,Ord)++-- |The PCD format has both ASCII and binary variants.+data DataFormat = ASCII | Binary deriving (Eq,Show,Ord)++data FieldType = TUchar Word8+ | TChar Int8+ | TUshort Word16+ | TShort Int16+ | TUint Word32+ | TInt Int32+ | TFloat Float+ | TDouble Double++-- |Construct a parser for a field based on its type and size.+fieldParser :: DimType -> Int -> Parser FieldType+fieldParser I 1 = TChar <$> (decimal :: Parser Int8)+fieldParser I 2 = TShort <$> (decimal :: Parser Int16)+fieldParser I 4 = TInt <$> (decimal :: Parser Int32)+fieldParser U 1 = TUchar <$> (decimal :: Parser Word8)+fieldParser U 2 = TUshort <$> (decimal :: Parser Word16)+fieldParser U 4 = TUint <$> (decimal :: Parser Word32)+fieldParser F 4 = TFloat . realToFrac <$> double+fieldParser F 8 = TDouble <$> double+fieldParser t s = error $ "Unknown field description: "++show t++" "++show s++-- |Assemble a parser for points by sequencing together all necessary+-- field parsers.+pointParser :: Header -> Parser [FieldType]+pointParser h = sequence $ zipWith fieldParser (_dimTypes h) (_sizes h)++data Header = Header { _version :: Text + , _fields :: [Text]+ , _sizes :: [Int]+ , _dimTypes :: [DimType]+ , _counts :: [Int]+ , _width :: Integer+ , _height :: Int+ , _viewpoint :: (V3 Double, Quaternion Double)+ , _points :: Integer+ , _format :: DataFormat } deriving Show+makeLenses ''Header++instance NFData Header where+ rnf (Header !_v !_f !_s !_d !_c !_w !_h !(!_t,!_r) !_p !_fmt) = ()++defaultHeader :: Header+defaultHeader = Header "" [] [] [] [] 0 0 (0, Quaternion 1 0) 0 ASCII++readVersion :: Parser Text+readVersion = "VERSION" .*> space *> takeText++readFields :: Parser [Text]+readFields = "FIELDS" .*> space *> fmap T.words takeText++readTypes :: Parser [DimType]+readTypes = "TYPE" .*> space *> sepBy t space+ where t = "I" .*> return I <|> "U" .*> return U <|> "F" .*> return F++namedIntegral :: Integral a => Text -> Parser a+namedIntegral n = n .*> space *> decimal++namedIntegrals :: Integral a => Text -> Parser [a]+namedIntegrals n = n .*> space *> sepBy decimal space++readViewpoint :: Parser (V3 Double, Quaternion Double)+readViewpoint = "VIEWPOINT" .*> space *> ((,) <$> v <*> q)+ where v = fmap (\[x,y,z] -> V3 x y z) $ count 3 (double <* skipSpace)+ q = fmap (\[w,i,j,k] -> Quaternion w (V3 i j k)) $ + count 4 (double <* skipSpace)++readFormat :: Parser DataFormat+readFormat = "DATA" .*> space *> + (("ascii" .*> pure ASCII) <|> ("binary" .*> pure Binary))++-- |Get the next non-comment line.+nextLine :: Handle -> IO Text+nextLine h = do t <- hGetLine h + either (const $ return t) (const $ nextLine h) $ + parseOnly isComment t+ where isComment = string "#"++-- |Parse a PCD header. Returns the 'Header' and the rest of the file+-- contents.+readHeader :: Handle -> IO (Header, Maybe Text)+readHeader h = flip execStateT (defaultHeader, Nothing) $ + sequence_ [ entry readVersion version+ , entry readFields fields+ , entry (namedIntegrals "SIZE") sizes+ , entry readTypes dimTypes+ , entry (namedIntegrals "COUNT") counts+ , entry (namedIntegral "WIDTH") width+ , entry (namedIntegral "HEIGHT") height+ , entry readViewpoint viewpoint + , entry (namedIntegral "POINTS") points+ , entry readFormat format ]+ where nxt :: (MonadState (a,Maybe Text) m, MonadIO m) => m ()+ nxt = liftIO (nextLine h) >>= (_2.=) . Just+ entry :: (MonadState (s,Maybe Text) m, MonadIO m, Functor m) => + Parser a -> Setting s s a a -> m ()+ entry parser field = do use _2 >>= maybe nxt (const (return ()))+ Just ln <- use _2+ case parseOnly parser ln of+ Left _ -> return ()+ Right v -> _1 . field .= v >> _2.=Nothing++-- |Format a 'Header' to be compatible with the PCD specification.+writeHeader :: Header -> Text+writeHeader h = (<> "\n") . mconcat . intersperse "\n" $+ [ "VERSION " <> (h^.version)+ , "FIELDS " <> joinFields (h^.fields)+ , "SIZE " <> joinFields (map tshow (h^.sizes))+ , "TYPE " <> joinFields (map tshow (h^.dimTypes))+ , "COUNT " <> joinFields (map tshow (h^.counts))+ , "WIDTH " <> tshow (h^.width)+ , "HEIGHT " <> tshow (h^.height)+ , "VIEWPOINT " <> (uncurry (<>) . (ftshow***((" "<>).ftshow)) $+ (h^.viewpoint))+ , "POINTS " <> tshow (h^.points)+ , "DATA " <> T.toLower (tshow (h^.format)) ]+ where joinFields = mconcat . intersperse " "+ tshow :: Show a => a -> Text+ tshow = T.pack . show+ ftshow :: (Foldable t, Show a) => t a -> Text+ ftshow = mconcat . intersperse " " . foldMap ((:[]) . tshow)
+ src/PCD/Internal/SmallLens.hs view
@@ -0,0 +1,45 @@+-- |Re-export the lens package without the Zipper module whose names+-- tend to clash.+module PCD.Internal.SmallLens ( module Control.Lens.Type+ , module Control.Lens.Traversal+ , module Control.Lens.Getter+ , module Control.Lens.Setter+ , module Control.Lens.Action+ , module Control.Lens.Combinators+ , module Control.Lens.Fold+ , module Control.Lens.Iso+ , module Control.Lens.Indexed+ , module Control.Lens.IndexedFold+ , module Control.Lens.IndexedGetter+ , module Control.Lens.IndexedLens+ , module Control.Lens.IndexedTraversal+ , module Control.Lens.IndexedSetter+ , module Control.Lens.Plated+ , module Control.Lens.Projection+ , module Control.Lens.Representable+ , module Control.Lens.TH+ , module Control.Lens.Tuple+ , module Control.Lens.WithIndex+ , module Control.Lens.Zoom ) where+import Control.Lens.Type+import Control.Lens.Traversal+import Control.Lens.Getter+import Control.Lens.Setter+import Control.Lens.Action+import Control.Lens.Combinators+import Control.Lens.Fold+import Control.Lens.Iso+import Control.Lens.Indexed+import Control.Lens.IndexedFold+import Control.Lens.IndexedGetter+import Control.Lens.IndexedLens+import Control.Lens.IndexedTraversal+import Control.Lens.IndexedSetter+import Control.Lens.Plated+import Control.Lens.Projection+import Control.Lens.Representable+import Control.Lens.TH+import Control.Lens.Tuple+import Control.Lens.WithIndex+import Control.Lens.Zoom+
+ src/PCD/Internal/Types.hs view
@@ -0,0 +1,12 @@+-- |Common types for dealing with PCD files.+module PCD.Internal.Types (V2(..), module Linear.Vector, + V3(..), V4(..), M44, Quaternion(..),+ Vector, Word8) where+import Data.Vector.Storable (Vector)+import Data.Word (Word8)+import Linear.V2 (V2(..))+import Linear.V3 (V3(..))+import Linear.V4 (V4(..))+import Linear.Matrix (M44)+import Linear.Vector+import Linear.Quaternion (Quaternion(..))
+ src/executable/Main.hs view
@@ -0,0 +1,10 @@+module Main (main) where+import Control.Monad (when)+import System.Environment (getArgs)+import PCD.Data (asciiToBinary)++main :: IO ()+main = do args@(~[inputFile, outputFile]) <- getArgs+ when (length args /= 2)+ (error "Usage: pcd2bin asciiPcd outputBinaryFile")+ asciiToBinary inputFile outputFile