module OpenCV.Extra.ArUco
(
Dictionary
, PredefinedDictionaryName(..)
, getPredefinedDictionary
, detectMarkers
, ArUcoMarkers
, drawDetectedMarkers
, ChArUcoBoard
, createChArUcoBoard
, drawChArUcoBoard
, interpolateChArUcoMarkers
, estimatePoseChArUcoBoard
, calibrateCameraFromFrames
, drawDetectedCornersCharuco
, drawEstimatedPose
) where
import "opencv" OpenCV.Internal.Exception
import "base" Control.Monad (guard)
import "primitive" Control.Monad.Primitive
import "base" Data.Monoid ((<>))
import "base" Data.Word ( Word8 )
import qualified "vector" Data.Vector.Storable as SV
import "base" Foreign.C
import "base" Foreign.ForeignPtr (ForeignPtr, withForeignPtr)
import "base" Foreign.Marshal.Alloc
import "base" Foreign.Ptr
import "base" Foreign.Storable (peek)
import qualified "inline-c" Language.C.Inline as C
import qualified "inline-c-cpp" Language.C.Inline.Cpp as C
import qualified "inline-c" Language.C.Inline.Unsafe as CU
import "linear" Linear
import "opencv" OpenCV
import "opencv" OpenCV.Core.Types.Vec (Vec3d)
import "this" OpenCV.Extra.Internal.C.Inline ( openCvExtraCtx )
import "this" OpenCV.Extra.Internal.C.Types
import "opencv" OpenCV.Internal
import "opencv" OpenCV.Internal.C.Types
import "opencv" OpenCV.Internal.Core.Types.Mat
import "base" System.IO.Unsafe
C.context openCvExtraCtx
C.include "opencv2/aruco.hpp"
C.include "opencv2/aruco/charuco.hpp"
C.include "opencv2/core.hpp"
C.include "iostream"
C.include "aruco.hpp"
C.using "namespace cv"
C.using "namespace cv::aruco"
C.using "namespace std"
newtype Dictionary = Dictionary
{ unDictionary :: ForeignPtr C'Ptr'Dictionary
}
type instance C Dictionary = C'Ptr'Dictionary
instance FromPtr Dictionary where
fromPtr =
objFromPtr Dictionary $ \ptr ->
[CU.block| void { delete $(Ptr_Dictionary * ptr); }|]
instance WithPtr Dictionary where
withPtr = withForeignPtr . unDictionary
newtype ChArUcoBoard = ChArUcoBoard
{ unChArUcoBoard :: ForeignPtr C'Ptr'CharucoBoard
}
type instance C ChArUcoBoard = C'Ptr'CharucoBoard
instance FromPtr ChArUcoBoard where
fromPtr =
objFromPtr ChArUcoBoard $ \ptr ->
[CU.block| void { delete $(Ptr_CharucoBoard * ptr); }|]
instance WithPtr ChArUcoBoard where
withPtr = withForeignPtr . unChArUcoBoard
newtype Vector'Int = Vector'Int
{ unVectorInt :: ForeignPtr C'Vector'Int
}
type instance C Vector'Int = C'Vector'Int
instance FromPtr Vector'Int where
fromPtr =
objFromPtr Vector'Int $ \ptr ->
[CU.block| void { delete $(VectorInt * ptr); }|]
instance WithPtr Vector'Int where
withPtr = withForeignPtr . unVectorInt
newtype Vector'Vector'Point2f = Vector'Vector'Point2f
{ unVectorVectorPoint2f :: ForeignPtr C'Vector'Vector'Point2f
}
type instance C Vector'Vector'Point2f = C'Vector'Vector'Point2f
instance FromPtr Vector'Vector'Point2f where
fromPtr =
objFromPtr Vector'Vector'Point2f $ \ptr ->
[CU.block| void { delete $(VectorVectorPoint2f * ptr); }|]
instance WithPtr Vector'Vector'Point2f where
withPtr = withForeignPtr . unVectorVectorPoint2f
data ChArUcoMarkers = ChArUcoMarkers
{ charucoIds :: Mat 'D 'D 'D
, charucoCorners :: Mat 'D 'D 'D
}
interpolateChArUcoMarkers
:: ChArUcoBoard
-> Mat ('S '[ h, w]) channels depth
-> ArUcoMarkers
-> Maybe ChArUcoMarkers
interpolateChArUcoMarkers charucoBoard image ArUcoMarkers {..} =
unsafePerformIO $
alloca $ \charucoCornersPtr ->
alloca $ \charucoIdsPtr ->
withPtr arucoCorners $ \c'arucoCorners ->
withPtr arucoIds $ \c'arucoIds ->
withPtr image $ \imagePtr ->
withPtr charucoBoard $ \c'charucoBoard -> do
success <-
[C.block| bool {
auto & corners = *$(VectorVectorPoint2f * c'arucoCorners);
auto & ids = *$(VectorInt * c'arucoIds);
auto & frame = *$(Mat * imagePtr);
auto charucoCorners = new Mat();
auto charucoIds = new Mat();
interpolateCornersCharuco(corners,
ids,
frame,
*$(Ptr_CharucoBoard * c'charucoBoard),
*charucoCorners,
*charucoIds);
*$(Mat * * charucoIdsPtr) = charucoIds;
*$(Mat * * charucoCornersPtr) = charucoCorners;
return !charucoIds->empty();
}|]
ids <- fromPtr (peek charucoIdsPtr)
corners <- fromPtr (peek charucoCornersPtr)
return (ChArUcoMarkers ids corners <$ guard (success /= 0))
estimatePoseChArUcoBoard
:: ChArUcoBoard
-> ChArUcoMarkers
-> (Matx33d, Matx51d)
-> Maybe (Vec3d, Vec3d)
estimatePoseChArUcoBoard charucoBoard ChArUcoMarkers {..} (cameraMatrix, distCoeffs) =
unsafePerformIO $ do
rvec <- toVecIO (V3 0.0 0.0 0.0)
tvec <- toVecIO (V3 0.0 0.0 0.0)
withPtr cameraMatrix $ \c'cameraMatrix ->
withPtr distCoeffs $ \c'distCoeffs ->
withPtr charucoIds $ \c'idsPtr ->
withPtr charucoBoard $ \c'charucoBoard ->
withPtr rvec $ \rvecPtr ->
withPtr tvec $ \tvecPtr ->
withPtr charucoCorners $ \c'cornersPtr -> do
success <- [C.block| bool {
return estimatePoseCharucoBoard(*$(Mat * c'cornersPtr),
*$(Mat * c'idsPtr),
*$(Ptr_CharucoBoard * c'charucoBoard),
*$(Matx33d * c'cameraMatrix),
*$(Matx51d * c'distCoeffs),
*$(Vec3d * rvecPtr),
*$(Vec3d * tvecPtr));
}|]
return (( fromVec rvec , fromVec tvec) <$ guard (success /= 0))
drawEstimatedPose
:: PrimMonad m
=> Matx33d
-> Matx51d
-> (Vec3d, Vec3d)
-> Mut (Mat ('S '[ h, w]) channels depth) (PrimState m)
-> m ()
drawEstimatedPose cameraMatrix distCoeffs (rvec, tvec) image =
unsafePrimToPrim $ do
withPtr image $ \imagePtr ->
withPtr cameraMatrix $ \c'cameraMatrix ->
withPtr distCoeffs $ \c'distCoeffs ->
withPtr rvec $ \rvecPtr ->
withPtr tvec $ \tvecPtr ->
[C.block| void {
drawAxis(*$(Mat * imagePtr),
*$(Matx33d * c'cameraMatrix),
*$(Matx51d * c'distCoeffs),
*$(Vec3d * rvecPtr),
*$(Vec3d * tvecPtr),
1);
}|]
calibrateCameraFromFrames
:: ChArUcoBoard
-> Int
-> Int
-> [(ArUcoMarkers, ChArUcoMarkers)]
-> CvExcept (Matx33d, Matx51d)
calibrateCameraFromFrames board width height frames =
unsafeWrapException $ do
cameraMatrix <- newMatx33d 0 0 0 0 0 0 0 0 0
distCoeffs <- newMatx51d 0 0 0 0 0
handleCvException (pure (cameraMatrix, distCoeffs)) $
withPtr cameraMatrix $ \cameraMatrixPtr ->
withPtr distCoeffs $ \distCoeffsPtr ->
withPtr board $ \c'board ->
withPtrs (map (arucoIds . fst) frames) $ \c'allIds ->
withPtrs (map (arucoCorners . fst) frames) $ \c'allCorners ->
withPtrs (fmap (charucoCorners . snd) frames) $ \c'allCharucoCorners ->
withPtrs (fmap (charucoIds . snd) frames) $ \c'allCharucoIds -> do
[cvExcept|
vector< vector<Point2f> > allCorners;
for(auto i = 0; i < $veclen:c'allCorners; i++) {
auto & corners =
*$vecptr:(VectorVectorPoint2f * * c'allCorners)[i];
allCorners.insert(allCorners.end(), corners.begin(), corners.end());
}
vector<int> allIds;
vector<int> counter;
for(auto i = 0; i < $veclen:c'allIds; i++) {
auto & ids = *$vecptr:(VectorInt * * c'allIds)[i];
allIds.insert(allIds.end(), ids.begin(), ids.end());
counter.push_back(ids.size());
}
Size frameSize($(int c'width), $(int c'height));
Ptr<CharucoBoard> charucoBoard = *$(Ptr_CharucoBoard * c'board);
Ptr<cv::aruco::Board> board = charucoBoard.staticCast<cv::aruco::Board>();
calibrateCameraAruco(allCorners,
allIds,
counter,
board,
frameSize,
*$(Matx33d * cameraMatrixPtr),
*$(Matx51d * distCoeffsPtr));
vector<Mat> allCharucoCorners;
for(auto i = 0; i < $veclen:c'allCharucoCorners; i++) {
auto & corners = *$vecptr:(Mat * * c'allCharucoCorners)[i];
allCharucoCorners.push_back(corners);
}
vector<Mat> allCharucoIds;
for(auto i = 0; i < $veclen:c'allCharucoIds; i++) {
auto & ids = *$vecptr:(Mat * * c'allCharucoIds)[i];
allCharucoIds.push_back(ids);
}
Mat perViewErrors;
calibrateCameraCharuco(allCharucoCorners,
allCharucoIds,
charucoBoard,
frameSize,
*$(Matx33d * cameraMatrixPtr),
*$(Matx51d * distCoeffsPtr),
noArray(),
noArray(),
noArray(),
noArray(),
perViewErrors);
|]
where
c'width = fromIntegral width
c'height = fromIntegral height
data ArUcoMarkers = ArUcoMarkers
{ arucoCorners :: Vector'Vector'Point2f
, arucoIds :: Vector'Int
}
detectMarkers
:: Dictionary
-> Mat ('S '[ h, w]) channels depth
-> Maybe ArUcoMarkers
detectMarkers dictionary image =
unsafePerformIO $
withPtr image $ \imagePtr ->
withPtr dictionary $ \c'dictionary ->
alloca $ \cornersOutPtr ->
alloca $ \idsOutPtr -> do
success <- fmap (/= 0) $
[C.block| bool {
auto * corners = new vector< vector<Point2f> >();
auto * ids = new vector<int>();
detectMarkers(*$(Mat * imagePtr),
*$(Ptr_Dictionary * c'dictionary),
*corners,
*ids);
*$(VectorVectorPoint2f * * cornersOutPtr) = corners;
*$(VectorInt * * idsOutPtr) = ids;
return ids->size() > 0;
}|]
corners <- fromPtr (peek cornersOutPtr)
ids <- fromPtr (peek idsOutPtr)
return (ArUcoMarkers corners ids <$ guard success)
drawDetectedMarkers
:: PrimMonad m
=> Mut (Mat ('S [h, w]) channels depth) (PrimState m)
-> ArUcoMarkers
-> m ()
drawDetectedMarkers image ArUcoMarkers{..} =
unsafePrimToPrim $
withPtr image $ \imagePtr ->
withPtr arucoCorners $ \c'cornersPtr ->
withPtr arucoIds $ \c'idsPtr ->
[C.block| void {
drawDetectedMarkers(*$(Mat * imagePtr),
*$(VectorVectorPoint2f * c'cornersPtr),
*$(VectorInt * c'idsPtr));
}|]
drawDetectedCornersCharuco
:: PrimMonad m
=> Mut (Mat ('S '[ h, w]) channels depth) (PrimState m)
-> ChArUcoMarkers
-> m ()
drawDetectedCornersCharuco image ChArUcoMarkers{..} =
unsafePrimToPrim $
withPtr image $ \imagePtr ->
withPtr charucoIds $ \c'idsPtr ->
withPtr charucoCorners $ \c'cornersPtr ->
[C.block| void {
drawDetectedCornersCharuco(*$(Mat * imagePtr),
*$(Mat * c'cornersPtr),
*$(Mat * c'idsPtr));
}|]
createChArUcoBoard
:: Int
-> Int
-> Double
-> Double
-> Dictionary
-> ChArUcoBoard
createChArUcoBoard squaresX squaresY squareLength markerLength dictionary =
unsafePerformIO $
withPtr dictionary $ \c'dictionary ->
fromPtr $
[C.block| Ptr_CharucoBoard * {
return
new Ptr<CharucoBoard>(CharucoBoard::create($(int c'squaresX),
$(int c'squaresY),
$(double c'squareLength),
$(double c'markerLength),
*$(Ptr_Dictionary * c'dictionary)));
}|]
where c'squaresX = fromIntegral squaresX
c'squaresY = fromIntegral squaresY
c'squareLength = realToFrac squareLength
c'markerLength = realToFrac markerLength
data PredefinedDictionaryName = DICT_7X7_1000
getPredefinedDictionary :: PredefinedDictionaryName -> Dictionary
getPredefinedDictionary name =
unsafePerformIO $
fromPtr $
[C.block| Ptr_Dictionary * {
return
new Ptr<Dictionary>(getPredefinedDictionary($(int c'name)));
}|]
where
c'name =
case name of
DICT_7X7_1000 -> [C.pure| int { DICT_7X7_1000 } |]
drawChArUcoBoard
:: (ToInt32 w, ToInt32 h)
=> ChArUcoBoard
-> w
-> h
-> Mat ('S '[DSNat h, DSNat w]) ('S 1) ('S Word8)
drawChArUcoBoard charucoBoard width height = unsafePerformIO $ do
dst <- newEmptyMat
withPtr charucoBoard $ \c'board ->
withPtr dst $ \dstPtr ->
[C.block| void {
Mat & board = * $(Mat * dstPtr);
Ptr<CharucoBoard> & charucoBoard = *$(Ptr_CharucoBoard * c'board);
charucoBoard->draw(cv::Size($(int32_t w), $(int32_t h)), board);
}|]
pure (unsafeCoerceMat dst)
where
w = toInt32 width
h = toInt32 height
withPtrs
:: WithPtr a
=> [a] -> (SV.Vector (Ptr (C a)) -> IO b) -> IO b
withPtrs [] io = io mempty
withPtrs (x:xs) io =
withPtr x $ \ptr -> withPtrs xs $ \sv -> io (SV.singleton ptr <> sv)