/
Main.hs
194 lines (180 loc) · 8.21 KB
/
Main.hs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
{-# LANGUAGE RecordWildCards, TemplateHaskell #-}
module Main where
import Control.Applicative
import SmallLens
import Control.Monad (when)
import Data.Foldable (toList)
import Data.IORef (newIORef, writeIORef, readIORef)
import qualified Data.Set as S
import qualified Data.Vector.Storable as V
import qualified Renderer as R
import Graphics.Rendering.OpenGL
import Graphics.GLUtil
import Camera
import Linear.Matrix ((!*!), M44)
import Linear.V2
import Linear.V3
import Linear.V4
import Linear.Vector
import qualified PCD.Data as PCD
import PointsGL
import VizMarkers
import MyPaths
import HeatPalette
import FrameGrabber
import PointLoader
import System.Directory (canonicalizePath, createDirectory, doesDirectoryExist)
import System.Environment (getArgs)
import System.FilePath ((</>), takeDirectory, takeExtension)
data AppState = AppState { _cam :: Camera
, _prevMouse :: Maybe (V2 Int)
, _saveDepthmap :: AppState -> IO () }
makeLenses ''AppState
keyActions :: AppState -> [(R.Key, Bool)] -> IO AppState
keyActions s keys
| (R.CharKey 'F', True) `elem` keys = (s^.saveDepthmap) s >> return s
| (R.CharKey 'C', True) `elem` keys = print (s^.cam) >> return s
| otherwise = return s
cameraControl :: Float -> Double -> R.UIEvents -> AppState -> (Bool, AppState)
cameraControl scale dt (R.UIEvents{..}) st =
(stop, ((cam.~c').(prevMouse.~prev')) $ st)
where c = st^.cam
prev = st^.prevMouse
stop = R.KeyEsc `elem` map fst (fst keys)
c' = auxKey (go (inc*^forward c)) R.KeyUp
. auxKey (go ((-inc)*^forward c)) R.KeyDown
. auxKey (go ((-inc)*^right c)) R.KeyLeft
. auxKey (go (inc*^right c)) R.KeyRight
-- . auxKeyOnce (roll (pi*0.5)) R.KeyPageup
-- . auxKeyOnce (roll (-pi * 0.5)) R.KeyPagedown
. auxKey (roll 0.01) R.KeyPageup
. auxKey (roll (-0.01)) R.KeyPagedown
. maybe id (pan . (^._x)) dMouse
. maybe id (tilt . negate . (^._y)) dMouse
. slow 0.9
$ update dt c
s = 15.0 / scale -- max speed
inc = 1.0 / scale -- 0.1
go = (clampSpeed s .) . deltaV
auxKey f k = if S.member k (snd keys) then f else id
auxKeyOnce f k = if (k, True) `elem` fst keys then f else id
dMouse = (\old -> (fromIntegral <$> mousePos ^-^ old) ^* 0.01) <$> prev
prev' = maybe (const mousePos <$> prev)
(bool (Just mousePos) Nothing)
(lookup R.MouseButton0 mouseButtons)
handler :: Float -> AppState -> Double -> R.UIEvents -> IO (Bool, AppState)
handler scale s dt ui = keyActions s (fst (R.keys ui)) >>=
return . cameraControl scale dt ui
bool :: a -> a -> Bool -> a
bool t _ True = t
bool _ f False = f
data ShaderArgs = ShaderArgs { camMat :: UniformLocation
, heatTex :: UniformLocation
, vertexPos :: AttribLocation
, cloudProg :: Program }
initShader :: IO ShaderArgs
initShader = do vs <- loadShader =<< getDataFileName "etc/cloud.vert"
fs <- loadShader =<< getDataFileName "etc/cloud.frag"
p <- linkShaderProgram [vs] [fs]
currentProgram $= Just p
ShaderArgs <$> get (uniformLocation p "cam")
<*> get (uniformLocation p "heat")
<*> get (attribLocation p "vertexCoord")
<*> pure p
buildMat :: Float -> Float -> Float -> M44 Float
buildMat s near far = V4 (set _x s 0)
(set _y s 0)
(V4 0 0 (-2/(far-near)) ((near-far)/(far-near)))
(set _z (-s) 0)
-- Configures OpenGL and returns a drawing function.
setup :: Float -> FilePath -> IO (FilePath -> IO (), Camera -> IO ())
setup scale ptFile = do clearColor $= Color4 1 1 1 0
depthFunc $= Just Lequal
vertexProgramPointSize $= Enabled
pointSmooth $= Enabled
--textureFunction $= Decal
lighting $= Disabled
s <- initShader
activeTexture $= TextureUnit 0
uniform (heatTex s) $= Index1 (0::GLint)
(heatVec, t) <- heatTexture 1024
let ext = takeExtension ptFile
gp <- groundPlane 5 0.1
v <- case () of
_ | ext == ".pcd" ->
PCD.loadXyz ptFile >>= \v' ->
if V.null v'
then V.map (view _xyz) <$>
PCD.loadXyzw ptFile
else return v'
_ | ext == ".conf" -> loadConf ptFile
_ | ext == ".ply" ->
V.map (\(SurfacePoint p _ _) -> p) <$>
loadPLYfancy ptFile
_ | otherwise -> load3DVerts ptFile
let m = uniformMat (camMat s)
proj = buildMat scale 0.01 100.0
drawPoints <- prepPoints v (vertexPos s)
let draw c = do gp Z (V3 1 0 0) (fmap (fmap realToFrac) $
proj !*! toMatrix c)
currentProgram $= Just (cloudProg s)
m $= (toList . fmap (toList . fmap realToFrac) $
proj !*! toMatrix c)
activeTexture $= TextureUnit 0
uniform (heatTex s) $= Index1 (0::GLuint)
textureBinding Texture1D $= Just t
drawPoints
return (saveFloatFrame heatVec, draw)
preDraw :: IO ()
preDraw = clear [ColorBuffer, DepthBuffer]
makeFrameSaver :: FilePath -> (FilePath -> IO ()) -> IO (AppState -> IO ())
makeFrameSaver pcdRoot dump =
do cnt <- newIORef (1::Int)
let dir = pcdRoot </> "depthmaps"
baseName = dir </> "depths"
dirExists <- doesDirectoryExist dir
when (not dirExists)
(createDirectory dir)
let f s = do n <- readIORef cnt
writeIORef cnt (n+1)
dump $ baseName++show n++".bin"
writeFile (baseName++show n++"pose.txt")
(writePose (_cam s))
return f
runDisplay :: Float -> FilePath -> IO ()
runDisplay scale pcdFile =
do loop <- R.setup
(dumpDepth, drawCloud) <- setup scale pcdFile
dumper <- makeFrameSaver (takeDirectory pcdFile) dumpDepth
occasionally <- R.onlyEvery 3
rate <- R.rateLimitHz 60
(incFrame,getFPS) <- R.fps
let renderLoop = loop (handler scale)
(\s -> preDraw >> drawCloud (s^.cam))
go frame c =
do incFrame
(shouldExit,c') <- renderLoop c
occasionally $ putStr "FPS: " >> getFPS >>= print
if shouldExit
then R.shutdown
else rate >> go (frame+1) c'
startCam = defaultCamera
-- startCam = (translation._z .~ 2)
-- . tilt ((-pi)*0.5)
-- . (cameraUp.~(V3 0 0 1))
-- $ defaultCamera
go (0::Int) $ AppState startCam Nothing dumper
pairs :: [a] -> [(a,a)]
pairs [] = []
pairs [x] = []
pairs (x:y:xs) = (x,y) : pairs xs
main :: IO ()
main = getArgs >>= aux
where aux [pcdFile] = canonicalizePath pcdFile >>= runDisplay 1
aux (pcdFile:args@(_:_)) = canonicalizePath pcdFile >>=
runDisplay (maybe 1 read (lookup "-s" (pairs args)))
aux _ = do putStrLn "Usage: PcdViewer PointCloudFile [-s X]"
putStrLn $ "- To view a PCD, PLY, or .conf file, supply "++
"the file name as a parameter. The \"-s\""++
" option may be used to apply a scale factor "++
"to the geometry."