' *****************************************************************************
' 
' File name: M3dreg.vb
' Location: See Matrox Example Launcher in the MIL Control Center
' 
'
' Synopsis:  To fill.
'
' Copyright (C) Matrox Electronic Systems Ltd., 1992-2020.
' All Rights Reserved
'
Imports Microsoft.VisualBasic
Imports System
Imports System.Threading
Imports Matrox.MatroxImagingLibrary


Module M3dreg

    ' ----------------------------------------------------------------------------
    ' Example description.
    ' ----------------------------------------------------------------------------
    Sub PrintHeader()
        Console.WriteLine("[EXAMPLE NAME]")
        Console.WriteLine("M3dreg")
        Console.WriteLine()
        Console.WriteLine("[SYNOPSIS]")
        Console.WriteLine("This example demonstrates how to use the 3D Registration module ")
        Console.WriteLine("to stitch several partial point clouds of a 3D object together ")
        Console.WriteLine("into a single complete point cloud.")
        Console.WriteLine()
        Console.WriteLine("[MODULES USED]")
        Console.WriteLine("Modules used: 3D Registration, 3D Display, 3D Graphics, and 3D Image" + vbNewLine + "Processing.")
        Console.WriteLine()
    End Sub

    ' Input scanned ply files
    Private ReadOnly NUM_SCANS As Integer = 6
    Private ReadOnly FILE_POINT_CLOUD As String() = New String() {MIL.M_IMAGE_PATH & "Cloud1.ply", MIL.M_IMAGE_PATH & "Cloud2.ply", MIL.M_IMAGE_PATH & "Cloud3.ply",
                                                                  MIL.M_IMAGE_PATH & "Cloud4.ply", MIL.M_IMAGE_PATH & "Cloud5.ply", MIL.M_IMAGE_PATH & "Cloud6.ply"}

    ' The colors assigned for each cloud
    Private ReadOnly Color As MIL_INT() = New MIL_INT() {MIL.M_RGB888(0, 159, 255), MIL.M_RGB888(154, 77, 66), MIL.M_RGB888(0, 255, 190), MIL.M_RGB888(120, 63, 193),
                                                         MIL.M_RGB888(31, 150, 152), MIL.M_RGB888(255, 172, 253), MIL.M_RGB888(177, 204, 113)}


    ' --------------------------------------------------------------
    Sub Main()
        ' Print example information in console.
        PrintHeader()

        Dim MilApplication As MIL_ID = MIL.M_NULL ' MIL application identifier
        Dim MilSystem As MIL_ID = MIL.M_NULL ' MIL system identifier
        Dim MilDisplay As MIL_ID = MIL.M_NULL ' MIL display identifier

        ' Allocate MIL objects.
        MIL.MappAlloc(MIL.M_NULL, MIL.M_DEFAULT, MilApplication)
        MIL.MsysAlloc(MIL.M_DEFAULT, MIL.M_SYSTEM_DEFAULT, MIL.M_DEFAULT, MIL.M_DEFAULT, MilSystem)

        Dim MilContainerIds As MIL_ID() = New MIL_ID(NUM_SCANS - 1) {}
        Console.Write("Reading the PLY files of 6 partial point clouds")

        For i As Integer = 0 To NUM_SCANS - 1
            Console.Write(".")
            MilContainerIds(i) = MIL.MbufImport(FILE_POINT_CLOUD(i), MIL.M_DEFAULT, MIL.M_RESTORE, MilSystem, CType(MIL.M_NULL, IntPtr))
            ColorCloud(MilContainerIds(i), Color(i))
        Next
        Console.WriteLine()
        Console.WriteLine()

        MilDisplay = Alloc3dDisplayId(MilSystem)

        ' Used for 2D display if needed.
        Dim MilDisplayImage As MIL_ID = MIL.M_NULL
        Dim MilDepthMap As MIL_ID = MIL.M_NULL

        ' Display the first point cloud container.
        DisplayContainer(MilSystem, MilDisplay, MilContainerIds(0), MilDepthMap, MilDisplayImage)
        AutoRotateDisplay(MilSystem, MilDisplay)

        Console.WriteLine("Showing the first partial point cloud of the object.")
        Console.WriteLine("Press <Enter> to start.")
        Console.WriteLine()
        Console.ReadKey()

      ' Allocate context And result for 3D registration (stitching).
      Dim MilContext As MIL_ID = MIL.M3dregAlloc(MilSystem, MIL.M_PAIRWISE_REGISTRATION_CONTEXT, MIL.M_DEFAULT, CType(MIL.M_NULL, IntPtr))
        Dim MilResult As MIL_ID = MIL.M3dregAllocResult(MilSystem, MIL.M_PAIRWISE_REGISTRATION_RESULT, MIL.M_DEFAULT, CType(MIL.M_NULL, IntPtr))

        MIL.M3dregControl(MilContext, MIL.M_DEFAULT, MIL.M_NUMBER_OF_REGISTRATION_ELEMENTS, NUM_SCANS)
        MIL.M3dregControl(MilContext, MIL.M_DEFAULT, MIL.M_MAX_ITERATIONS, 40)

        ' Pairwise registration context controls.
        ' Use normal subsampling to preserve edges And yield faster registration.
        Dim MilSubsampleContext As MIL_ID = MIL.M_NULL
        MIL.M3dregInquire(MilContext, MIL.M_DEFAULT, MIL.M_SUBSAMPLE_CONTEXT_ID, MilSubsampleContext)
        MIL.M3dregControl(MilContext, MIL.M_DEFAULT, MIL.M_SUBSAMPLE, MIL.M_ENABLE)

        ' Keep edge points.
        MIL.M3dimControl(MilSubsampleContext, MIL.M_SUBSAMPLE_MODE, MIL.M_SUBSAMPLE_NORMAL)
        MIL.M3dimControl(MilSubsampleContext, MIL.M_NEIGHBORHOOD_DISTANCE, 10)

        ' Chain of set location, i==0 Is referencing to the GLOBAL.
        For i As Integer = 1 To NUM_SCANS - 1
            MIL.M3dregSetLocation(MilContext, i, i - 1, MIL.M_IDENTITY_MATRIX, MIL.M_DEFAULT, MIL.M_DEFAULT, MIL.M_DEFAULT)
        Next

        Console.WriteLine("The 3D stitching between partial point clouds has been performed with")
        Console.WriteLine("the help of the points within the expected common overlap regions.")
        Console.WriteLine()

        ' Calculate the time to perform the registration.
        Dim ComputationTimeMS As Double
        MIL.MappTimer(MIL.M_DEFAULT, MIL.M_TIMER_RESET, CType(MIL.M_NULL, IntPtr))

        ' Perform the registration (stitching).
        MIL.M3dregCalculate(MilContext, MilContainerIds, NUM_SCANS, MilResult, MIL.M_DEFAULT)
        ComputationTimeMS = MIL.MappTimer(MIL.M_DEFAULT, MIL.M_TIMER_READ, CType(MIL.M_NULL, IntPtr)) * 1000.0

        Console.Write("The registration of the 6 partial point clouds ")
        Console.WriteLine("succeeded in {0,0:f2} ms .", ComputationTimeMS)
        Console.WriteLine()

        ' Merging overlapping point clouds will result into unneeded large number of points at
        ' the overlapping regions.                                                            
        ' During merging, subsampling Is used to ensure that the density of points
        ' Is fairly dense, but without replications.
        Dim GridSize As Double = 0.0
        Dim StatResultId As MIL_ID = MIL.M3dimAllocResult(MilSystem, MIL.M_STATISTICS_RESULT, MIL.M_DEFAULT, CType(MIL.M_NULL, IntPtr))
        MIL.M3dimStat(MIL.M_STAT_CONTEXT_DISTANCE_TO_NEAREST_NEIGHBOR, MilContainerIds(0), StatResultId, MIL.M_DEFAULT)

        ' Nearest neighbour distances gives a measure of the point cloud density.
        MIL.M3dimGetResult(StatResultId, MIL.M_MIN_DISTANCE_TO_NEAREST_NEIGHBOR, GridSize)

        ' Use the measured point cloud density as guide for the subsampling.
        Dim MilMergeSubsampleContext As MIL_ID = MIL.M3dimAlloc(MilSystem, MIL.M_SUBSAMPLE_CONTEXT, MIL.M_DEFAULT, CType(MIL.M_NULL, IntPtr))
        MIL.M3dimControl(MilMergeSubsampleContext, MIL.M_SUBSAMPLE_MODE, MIL.M_SUBSAMPLE_GRID)
        MIL.M3dimControl(MilMergeSubsampleContext, MIL.M_GRID_SIZE_X, GridSize)
        MIL.M3dimControl(MilMergeSubsampleContext, MIL.M_GRID_SIZE_Y, GridSize)
        MIL.M3dimControl(MilMergeSubsampleContext, MIL.M_GRID_SIZE_Z, GridSize)

        ' Allocate the point cloud for the final stitched clouds.
        Dim MilStitchedId As MIL_ID = MIL.MbufAllocContainer(MilSystem, MIL.M_PROC + MIL.M_DISP, MIL.M_DEFAULT, CType(MIL.M_NULL, IntPtr))

        Console.WriteLine("The merging of point clouds will be shown incrementally.")
        Console.WriteLine("Press <Enter> to show 2 merged point clouds of 6.")
        Console.WriteLine()
        Console.ReadKey()

        ' Merge can merge all clouds at once, but here it Is done incrementally for the display.
        For i As Integer = 2 To NUM_SCANS
            MIL.M3dregMerge(MilResult, MilContainerIds, i, MilStitchedId, MilMergeSubsampleContext, MIL.M_DEFAULT)

            If i = 2 Then
                DisplayContainer(MilSystem, MilDisplay, MilStitchedId, MilDepthMap, MilDisplayImage)
            Else
                UpdateDisplay(MilSystem, MilStitchedId, MilDepthMap, MilDisplayImage)
            End If

            If i < NUM_SCANS Then
                Console.WriteLine("Press <Enter> to show {0} merged point clouds of 6.", i + 1)
                Console.WriteLine()
            Else
                Console.WriteLine("Press <Enter> to end.")
                Console.WriteLine()
            End If

            AutoRotateDisplay(MilSystem, MilDisplay)
            Console.ReadKey()
        Next

        ' Free Objects
        For i As Integer = 0 To NUM_SCANS - 1
            MIL.MbufFree(MilContainerIds(i))
        Next

        MIL.MbufFree(MilStitchedId)
        MIL.M3dimFree(StatResultId)
        MIL.M3dimFree(MilMergeSubsampleContext)
        MIL.M3dregFree(MilContext)
        MIL.M3dregFree(MilResult)
        FreeDisplay(MilDisplay)

        If MilDisplayImage <> MIL.M_NULL Then
            MIL.MbufFree(MilDisplayImage)
        End If

        If MilDepthMap <> MIL.M_NULL Then
            MIL.MbufFree(MilDepthMap)
        End If

        MIL.MsysFree(MilSystem)
        MIL.MappFree(MilApplication)
    End Sub

    ' --------------------------------------------------------------
    ' Color the container  
    ' --------------------------------------------------------------
    Sub ColorCloud(ByVal MilPointCloud As MIL_ID, ByVal Col As MIL_INT)
        Dim SizeX As MIL_INT = MIL.MbufInquireContainer(MilPointCloud, MIL.M_COMPONENT_RANGE, MIL.M_SIZE_X, CType(MIL.M_NULL, IntPtr))
        Dim SizeY As MIL_INT = MIL.MbufInquireContainer(MilPointCloud, MIL.M_COMPONENT_RANGE, MIL.M_SIZE_Y, CType(MIL.M_NULL, IntPtr))
        Dim ReflectanceId As MIL_ID = MIL.MbufAllocComponent(MilPointCloud, 3, SizeX, SizeY, MIL.M_UNSIGNED + 8, MIL.M_IMAGE, MIL.M_COMPONENT_REFLECTANCE, CType(MIL.M_NULL, IntPtr))
        MIL.MbufClear(ReflectanceId, Col)
    End Sub

   ' --------------------------------------------------------------
   ' Auto rotate the 3D object
   ' --------------------------------------------------------------
   Sub AutoRotateDisplay(ByVal MilSystem As MIL_ID, ByVal MilDisplay As MIL_ID)
        Dim DisplayType As Long = 0
        MIL.MobjInquire(MilDisplay, MIL.M_OBJECT_TYPE, DisplayType)

      ' AutoRotate available only for the 3D display.
      If DisplayType = MIL.M_DISPLAY Then
            Return
        End If

        ' By default the display rotates around the Z axis, but the robot Is oriented along the Y axis. 
        Dim Geometry As MIL_ID = MIL.M3dgeoAlloc(MilSystem, MIL.M_GEOMETRY, MIL.M_DEFAULT, MIL.M_NULL)
        MIL.M3ddispCopy(MilDisplay, Geometry, MIL.M_ROTATION_AXIS, MIL.M_DEFAULT)
        Dim CenterX As Double = MIL.M3dgeoInquire(Geometry, MIL.M_START_POINT_X, CType(MIL.M_NULL, IntPtr))
        Dim CenterY As Double = MIL.M3dgeoInquire(Geometry, MIL.M_START_POINT_Y, CType(MIL.M_NULL, IntPtr))
        Dim CenterZ As Double = MIL.M3dgeoInquire(Geometry, MIL.M_START_POINT_Z, CType(MIL.M_NULL, IntPtr))
        MIL.M3dgeoLine(Geometry, MIL.M_POINT_AND_VECTOR, MIL.M_UNCHANGED, MIL.M_UNCHANGED, MIL.M_UNCHANGED, 0, 1, 0, MIL.M_UNCHANGED, MIL.M_DEFAULT)
        MIL.M3ddispCopy(Geometry, MilDisplay, MIL.M_ROTATION_AXIS, MIL.M_DEFAULT)
        MIL.M3ddispControl(MilDisplay, MIL.M_AUTO_ROTATE, MIL.M_ENABLE)
        MIL.M3dgeoFree(Geometry)

    End Sub


   ' *****************************************************************************
   ' Allocates a 3D display and returns its MIL identifier.
   ' *****************************************************************************
   Function Alloc3dDisplayId(ByVal MilSystem As MIL_ID) As MIL_ID
      MIL.MappControl(MIL.M_DEFAULT, MIL.M_ERROR, MIL.M_PRINT_DISABLE)
      Dim MilDisplay As MIL_ID = MIL.M3ddispAlloc(MilSystem, MIL.M_DEFAULT, "M_DEFAULT", MIL.M_DEFAULT, CType(MIL.M_NULL, IntPtr))
      MIL.MappControl(MIL.M_DEFAULT, MIL.M_ERROR, MIL.M_PRINT_ENABLE)

      If MilDisplay = MIL.M_NULL Then
         Console.WriteLine()
         Console.WriteLine("The current system does not support the 3D display.")
         Console.WriteLine("A 2D display will be used instead.")
         Console.WriteLine("Press any key to continue.")
         Console.ReadKey()

         ' Allocate a 2D Display instead.
         MilDisplay = MIL.MdispAlloc(MilSystem, MIL.M_DEFAULT, "M_DEFAULT", MIL.M_WINDOWED, CType(MIL.M_NULL, IntPtr))
      Else

         ' Adjust the viewpoint of the 3D display.
         MIL.M3ddispSetView(MilDisplay, MIL.M_AUTO, MIL.M_BOTTOM_VIEW, MIL.M_DEFAULT, MIL.M_DEFAULT, MIL.M_DEFAULT)
         Console.WriteLine("Press <R> on the display window to stop/start the rotation.")
         Console.WriteLine()
      End If

      Return MilDisplay
   End Function

   ' --------------------------------------------------------------
   ' Display the received container.
   ' --------------------------------------------------------------
   Sub DisplayContainer(ByVal MilSystem As MIL_ID, ByVal MilDisplay As MIL_ID, ByVal MilContainer As MIL_ID, ByRef MilDepthMap As MIL_ID, ByRef MilIntensityMap As MIL_ID)
        Dim DisplayType As Long = 0
        MIL.MobjInquire(MilDisplay, MIL.M_OBJECT_TYPE, DisplayType)
        Dim Use3D As Boolean = DisplayType = MIL.M_3D_DISPLAY

        If Use3D Then
            MIL.M3ddispSelect(MilDisplay, MilContainer, MIL.M_ADD, MIL.M_DEFAULT)
            MIL.M3ddispSelect(MilDisplay, MIL.M_NULL, MIL.M_OPEN, MIL.M_DEFAULT)
        Else

            If MilDepthMap = MIL.M_NULL Then
                Dim SizeX As MIL_INT = 0 ' Image size X For 2D display
                Dim SizeY As MIL_INT = 0 ' Image size Y For 2D display

                MIL.M3dimCalculateMapSize(MIL.M_DEFAULT, MilContainer, MIL.M_NULL, MIL.M_DEFAULT, SizeX, SizeY)

                MilIntensityMap = MIL.MbufAllocColor(MilSystem, 3, SizeX, SizeY, MIL.M_UNSIGNED + 8, MIL.M_IMAGE Or MIL.M_PROC Or MIL.M_DISP, CType(MIL.M_NULL, IntPtr))
                MilDepthMap = MIL.MbufAlloc2d(MilSystem, SizeX, SizeY, MIL.M_UNSIGNED + 8, MIL.M_IMAGE Or MIL.M_PROC Or MIL.M_DISP, CType(MIL.M_NULL, IntPtr))

                MIL.M3dimCalibrateDepthMap(MilContainer, MilDepthMap, MilIntensityMap, MIL.M_NULL, MIL.M_DEFAULT, MIL.M_DEFAULT, MIL.M_CENTER)
            End If

            ' Rotate the point cloud container to be in the xy plane before projecting.
            Dim RotatedContainer As MIL_ID = MIL.MbufAllocContainer(MilSystem, MIL.M_PROC, MIL.M_DEFAULT, CType(MIL.M_NULL, IntPtr))

            MIL.M3dimRotate(MilContainer, RotatedContainer, MIL.M_ROTATION_XYZ, 90, 60, 0, MIL.M_DEFAULT, MIL.M_DEFAULT, MIL.M_DEFAULT, MIL.M_DEFAULT, MIL.M_DEFAULT)
            MIL.M3dimProject(MilContainer, MilDepthMap, MilIntensityMap, MIL.M_DEFAULT, MIL.M_MIN_Z, MIL.M_DEFAULT, MIL.M_DEFAULT)

            ' Display the projected point cloud container.
            MIL.MdispSelect(MilDisplay, MilIntensityMap)
            MIL.MbufFree(RotatedContainer)
        End If
    End Sub

    ' --------------------------------------------------------------
    ' Updated the displayed image.
    ' --------------------------------------------------------------
    Sub UpdateDisplay(ByVal MilSystem As MIL_ID, ByVal MilContainer As MIL_ID, ByVal MilDepthMap As MIL_ID, ByVal MilIntensityMap As MIL_ID)
        If MilDepthMap = MIL.M_NULL Then
            Return
        End If

        Dim RotatedContainer As MIL_ID = MIL.MbufAllocContainer(MilSystem, MIL.M_PROC, MIL.M_DEFAULT, CType(MIL.M_NULL, IntPtr))

        MIL.M3dimRotate(MilContainer, RotatedContainer, MIL.M_ROTATION_XYZ, 90, 70, 0, MIL.M_DEFAULT, MIL.M_DEFAULT, MIL.M_DEFAULT, MIL.M_DEFAULT, MIL.M_DEFAULT)
        MIL.M3dimProject(MilContainer, MilDepthMap, MilIntensityMap, MIL.M_DEFAULT, MIL.M_MIN_Z, MIL.M_DEFAULT, MIL.M_DEFAULT)

        MIL.MbufFree(RotatedContainer)
    End Sub

    ' --------------------------------------------------------------
    ' Free the display.
    ' --------------------------------------------------------------
    Sub FreeDisplay(ByVal MilDisplay As MIL_ID)
        Dim DisplayType As Long = 0
        MIL.MobjInquire(MilDisplay, MIL.M_OBJECT_TYPE, DisplayType)

        If DisplayType = MIL.M_DISPLAY Then
            MIL.MdispFree(MilDisplay)
        Else
            MIL.M3ddispFree(MilDisplay)
        End If
    End Sub
End Module