//    $Revision: 1.1 $

using System;
using System.Collections.Generic;
using System.Text;
using fvalgcli;

namespace TC
{
    public partial class FIE
    {
        /// <summary>
        /// 点列を歪み補正画像座標へ変換.
        /// </summary>
        public DPNT_T_PTR fnFIE_calib_undistort_points()
        {
            int status = (int)f_err.F_ERR_NONE;

            // ハンドルオープン用変数.
            FHANDLE hcalib = FHANDLE.Zero;                                // キャリブレーションハンドル.
            int type = (int)fie_calib_model_type.F_CALIB_MODEL_CIRCLE;    // 円マーカ.
            int n = 8;                                                    // 縦、横の円の数.
            double unit = 4;                                            // モデルの単位長さ.ワールド座標系の単位(ミリメートル等)で与えます.
            DPNT_T offset = DPNT_T.init(-130, -170);                    // モデル原点とワールド座標系原点のオフセット.ワールド座標系の単位(ミリメートル等)で与えます.

            FHANDLE image = FHANDLE.Zero;                                // 特徴点を検出する画像.
            DPNT_T_PTR src_points = DPNT_T_PTR.Zero;                    // 検出点(カメラ座標系).
            DPNT_T_PTR dst_points = DPNT_T_PTR.Zero;                    // 理想画像座標系へ変換された検出点.
            int num_points = 0;                                            // 検出点数.

            // 内部パラメータ.
            F_CAMERA_PARAM camera = F_CAMERA_PARAM.init(916.396023110107, 910.774803134449,
                                                        1.12644931840078, 331.36766487411, 
                                                        227.057842377842, -0.215845914024188, 
                                                        0.0707690551914399);

            try
            {
                // ハンドルのオープン.
                hcalib = api.fnFIE_calib_open2(type, n, unit, offset, ref status);
                Assert.IsTrue(status == (int)f_err.F_ERR_NONE, "エラーが発生しました。({0})", (f_err)status);

                // 画像のロード.
                status = api.fnFIE_load_img_file(TestImageDir + "/testdata/calib_rectify.bmp", ref image, f_color_img_type.F_COLOR_IMG_TYPE_UC8);
                Assert.IsTrue(status == (int)f_err.F_ERR_NONE, "エラーが発生しました。({0})", (f_err)status);

                // 特徴点検出.
                status = api.fnFIE_calib_detect(hcalib, image, ref src_points, ref num_points);
                Assert.IsTrue(status == (int)f_err.F_ERR_NONE, "エラーが発生しました。({0})", (f_err)status);

                //理想画像座標系へ検出点を変換.
                dst_points = DPNT_T_PTR.alloc(num_points);
                status = api.fnFIE_calib_undistort_points(camera, src_points, dst_points, num_points);
                Assert.IsTrue(status == (int)f_err.F_ERR_NONE, "エラーが発生しました。({0})", (f_err)status);

                return dst_points;
            }
            finally
            {
                image.Dispose();
                hcalib.Dispose();
                src_points.Dispose();
            }
        }
    }
}

'    $Revision: 1.1 $

Imports System.Collections.Generic
Imports System.Text
Imports fvalgcli

Public Partial Class FIE
    ''' <summary>
    ''' 点列を歪み補正画像座標へ変換.
    ''' </summary>
    Public Function fnFIE_calib_undistort_points() As DPNT_T_PTR
        Dim status As Integer = CInt(f_err.F_ERR_NONE)

        ' ハンドルオープン用変数.
        Dim hcalib As FHANDLE = FHANDLE.Zero
        ' キャリブレーションハンドル.
        Dim type As Integer = CInt(fie_calib_model_type.F_CALIB_MODEL_CIRCLE)
        ' 円マーカ.
        Dim n As Integer = 8
        ' 縦、横の円の数.
        Dim unit As Double = 4
        ' モデルの単位長さ.ワールド座標系の単位(ミリメートル等)で与えます.
        Dim offset As DPNT_T = DPNT_T.init(-130, -170)
        ' モデル原点とワールド座標系原点のオフセット.ワールド座標系の単位(ミリメートル等)で与えます.
        Dim image As FHANDLE = FHANDLE.Zero
        ' 特徴点を検出する画像.
        Dim src_points As DPNT_T_PTR = DPNT_T_PTR.Zero
        ' 検出点(カメラ座標系).
        Dim dst_points As DPNT_T_PTR = DPNT_T_PTR.Zero
        ' 理想画像座標系へ変換された検出点.
        Dim num_points As Integer = 0
        ' 検出点数.
        ' 内部パラメータ.
        Dim camera As F_CAMERA_PARAM = F_CAMERA_PARAM.init(916.396023110107, 910.774803134449, 1.12644931840078, 331.36766487411, 227.057842377842, -0.215845914024188, _
            0.0707690551914399)

        Try
            ' ハンドルのオープン.
            hcalib = api.fnFIE_calib_open2(type, n, unit, offset, status)
            Assert.IsTrue(status = CInt(f_err.F_ERR_NONE), "エラーが発生しました。({0})", CType(status, f_err))

            ' 画像のロード.
            status = api.fnFIE_load_img_file(TestImageDir & "/testdata/calib_rectify.bmp", image, f_color_img_type.F_COLOR_IMG_TYPE_UC8)
            Assert.IsTrue(status = CInt(f_err.F_ERR_NONE), "エラーが発生しました。({0})", CType(status, f_err))

            ' 特徴点検出.
            status = api.fnFIE_calib_detect(hcalib, image, src_points, num_points)
            Assert.IsTrue(status = CInt(f_err.F_ERR_NONE), "エラーが発生しました。({0})", CType(status, f_err))

            '理想画像座標系へ検出点を変換.
            dst_points = DPNT_T_PTR.alloc(num_points)
            status = api.fnFIE_calib_undistort_points(camera, src_points, dst_points, num_points)
            Assert.IsTrue(status = CInt(f_err.F_ERR_NONE), "エラーが発生しました。({0})", CType(status, f_err))

            Return dst_points
        Finally
            image.Dispose()
            hcalib.Dispose()
            src_points.Dispose()
        End Try
    End Function
End Class