//    $Revision: 1.1 $

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

namespace TC
{
    public partial class FIE
    {
        /// <summary>
        /// カメラ外部パラメータとカメラ混合パラメータの算出.
        /// </summary>
        public FMATRIX_PTR fnFIE_calib_calc_extrinsic_parameters()
        {
            int status = (int)f_err.F_ERR_NONE;

            DPNT_T_PTR model_points = DPNT_T_PTR.Zero;        // モデルデータの配列.
            DPNT_T_PTR marker_points = DPNT_T_PTR.Zero;        // 画像から検出して歪み補正を適用済みの特徴点座標の配列.
            int num_points = 63;                            // 点数.
            FMATRIX_PTR rotation = FMATRIX_PTR.Zero;        // カメラ外部パラメータ(回転行列).
            FVECTOR_PTR translation = FVECTOR_PTR.Zero;        // カメラ外部パラメータ(並進ベクトル).
            FMATRIX_PTR homography = FMATRIX_PTR.Zero;        // カメラ混合パラメータ.

            // 内部パラメータ.
            F_CAMERA_PARAM camera = F_CAMERA_PARAM.init(916.396023110107, 910.774803134449,
                                                        1.12644931840078, 331.36766487411,
                                                        227.057842377842, -0.215845914024188,
                                                        0.0707690551914399);
            try
            {
                // 画像から検出して歪み補正を適用済みの特徴点座標の配列を取得.
                marker_points = fnFIE_calib_undistort_points();

                // モデルデータの配列を取得.
                model_points = fnFIE_calib_get_model_points();

                // カメラ混合パラメータの取得.
                rotation = FMATRIX_PTR.alloc(3, 3);
                translation = FVECTOR_PTR.alloc(3);
                homography = FMATRIX_PTR.alloc(3, 3);
                status = api.fnFIE_calib_calc_extrinsic_parameters(camera, model_points, marker_points, 
                                                                    num_points, rotation, translation, homography);
                Assert.IsTrue(status == (int)f_err.F_ERR_NONE, "エラーが発生しました。({0})", (f_err)status);

                Console.WriteLine("rotation");
                ConsoleOut.WriteFMATRIX(rotation, rotation.row, rotation.col);
                Console.Write("\n");

                Console.WriteLine("translation\n");
                Console.WriteLine("{0}, {1}, {2}\n", translation[0], translation[1], translation[2]);

                Console.WriteLine("homography");
                ConsoleOut.WriteFMATRIX(homography, homography.row, homography.col);
                Console.Write("\n");

                return homography;
            }
            finally
            {
                model_points.Dispose();
                marker_points.Dispose();
                rotation.Dispose();
                translation.Dispose();
            }
        }
    }
}

'    $Revision: 1.1 $

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

Public Partial Class FIE
    ''' <summary>
    ''' カメラ外部パラメータとカメラ混合パラメータの算出.
    ''' </summary>
    Public Function fnFIE_calib_calc_extrinsic_parameters() As FMATRIX_PTR
        Dim status As Integer = CInt(f_err.F_ERR_NONE)

        Dim model_points As DPNT_T_PTR = DPNT_T_PTR.Zero
        ' モデルデータの配列.
        Dim marker_points As DPNT_T_PTR = DPNT_T_PTR.Zero
        ' 画像から検出して歪み補正を適用済みの特徴点座標の配列.
        Dim num_points As Integer = 63
        ' 点数.
        Dim rotation As FMATRIX_PTR = FMATRIX_PTR.Zero
        ' カメラ外部パラメータ(回転行列).
        Dim translation As FVECTOR_PTR = FVECTOR_PTR.Zero
        ' カメラ外部パラメータ(並進ベクトル).
        Dim homography As FMATRIX_PTR = FMATRIX_PTR.Zero
        ' カメラ混合パラメータ.
        ' 内部パラメータ.
        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
            ' 画像から検出して歪み補正を適用済みの特徴点座標の配列を取得.
            marker_points = fnFIE_calib_undistort_points()

            ' モデルデータの配列を取得.
            model_points = fnFIE_calib_get_model_points()

            ' カメラ混合パラメータの取得.
            rotation = FMATRIX_PTR.alloc(3, 3)
            translation = FVECTOR_PTR.alloc(3)
            homography = FMATRIX_PTR.alloc(3, 3)
            status = api.fnFIE_calib_calc_extrinsic_parameters(camera, model_points, marker_points, num_points, rotation, translation, _
                homography)
            Assert.IsTrue(status = CInt(f_err.F_ERR_NONE), "エラーが発生しました。({0})", CType(status, f_err))

            Console.WriteLine("rotation")
            ConsoleOut.WriteFMATRIX(rotation, rotation.row, rotation.col)
            Console.Write(vbLf)

            Console.WriteLine("translation" & vbLf)
            Console.WriteLine("{0}, {1}, {2}" & vbLf, translation(0), translation(1), translation(2))

            Console.WriteLine("homography")
            ConsoleOut.WriteFMATRIX(homography, homography.row, homography.col)
            Console.Write(vbLf)

            Return homography
        Finally
            model_points.Dispose()
            marker_points.Dispose()
            rotation.Dispose()
            translation.Dispose()
        End Try
    End Function
End Class