本記事ではARToolKitPlusを対象としますが，その本家であるARToolKitでも通用する話ですので，タイトルはARToolKitにしました．

ARToolKitPlusでの歪み補正

ARToolKitPlusのレンズ歪みの式は次の通り．（ARToolKitの公式チュートリアルより引用）

f:id:Mzawa2:20151220221100p:plain f:id:Mzawa2:20151220220658p:plain

このように歪みベクトルとして，歪みの中心座標(x0, y0)，歪み係数f，スケールファクターs（センサの物理的大きさの逆数に対応？）を持ちます．基本的に半径方向の歪みのみを考慮しています．

OpenCVでの歪み補正

一方OpenCVでは，レンズ歪みの式を次のように規定しています．（OpenCV2.2 C++リファレンスより引用）

f:id:Mzawa2:20151220221505p:plain

ARToolKitPlusの方とは式が全く異なる上に，OpenCVの方では半径方向だけでなく円周方向の歪みも考慮されています．ちなみに歪み係数ベクトルdist_coeffs[]の並びは(k1, k2, p1, p2, k3(, k4, k5, k6))です．k4以降はデフォルトでは考慮されません．

ここで言いたいのは，「OpenCVのdist_coeffs[]をARToolKitのdist_factor[]にそのまま入れると破綻する」という事です．OpenCVで得たレンズ歪みパラメータはARToolKit(Plus)上ではそのまま使えないのです．（ただし内部パラメータは使える）

解決策

ではどうするか．簡単です．ARToolKitPlusに渡す前にOpenCV上で歪み補正してやればいいのです．つまり，

OpenCVの内部パラメータをもとに，歪みベクトルが全て0のARToolKitPlus用のパラメータを用意する．
ARToolKitPlus初期化の際に外部ファイルを使用せず，上記を渡す．
ARToolKitPlusの歪み補正を無効化する．
画像を得る．
OpenCVの機能で歪み補正する．
ARToolKitPlusに渡す

という方針でやります．以下が必要な部分だけ取り出したコードです．

class OpenCVCamera : ARToolKitPlus::Camera
{
public:
    /**
   * Takes the OpenCV camera matrix and distortion coefficients, and generates
   * ARToolKitPlus compatible Camera.
   */
    static Camera * fromOpenCV(const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs, cv::Size size)
    {
        Camera *cam = new ARToolKitPlus::CameraImpl;

        cam->xsize = size.width;
        cam->ysize = size.height;

        for (int i = 0; i < 3; i++)
        for (int j = 0; j < 4; j++)
            cam->mat[i][j] = 0;

        float fx = (float)cameraMatrix.at<double>(0, 0);
        float fy = (float)cameraMatrix.at<double>(1, 1);
        float cx = (float)cameraMatrix.at<double>(0, 2);
        float cy = (float)cameraMatrix.at<double>(1, 2);

        cam->mat[0][0] = fx;
        cam->mat[1][1] = fy;
        cam->mat[0][2] = cx;
        cam->mat[1][2] = cy;
        cam->mat[2][2] = 1.0;

        // OpenCVの歪み補正とARToolKitの歪み補正は全く別の計算式を使っているため，単純に値が使えない
        // ここではARToolKitの歪みベクトルをゼロとし，OpenCV側で補正してやることにする
        for (int i = 0; i < 4; i++) {
            cam->dist_factor[i] = 0;
        }
        return cam;
    }
}

int main()
{
    // ARToolKitPlusの初期化
    ARToolKitPlus::Camera *param = OpenCVCamera::fromOpenCV(cameraMatrix, distCoeffs, cameraSize);

    ARToolKitPlus::Logger *logger = nullptr;
    tracker = new ARToolKitPlus::TrackerSingleMarkerImpl<6, 6, 6, 1, 10>(cameraSize.width, cameraSize.height);
    tracker->init(NULL, 0.1f, 5000.0f);   // ファイルは使用しない
    tracker->setCamera(param);
    tracker->activateAutoThreshold(true);
    tracker->setNumAutoThresholdRetries(5);
    tracker->setBorderWidth(0.125f);            // BCH boader width = 12.5%
    tracker->setPatternWidth(60.0f);            // marker physical width = 60.0mm
    tracker->setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_BGR);        // With OpenCV
    tracker->setUndistortionMode(ARToolKitPlus::UNDIST_NONE);        // UndistortionはOpenCV側で行う
    tracker->setMarkerMode(ARToolKitPlus::MARKER_ID_BCH);
    tracker->setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_RPP);

    // Undistort Map
    initUndistortRectifyMap(
        cameraMatrix, distCoeffs,
        Mat(), cameraMatrix, cameraSize, CV_32FC1,
        mapC1, mapC2);

    // メインループ
    while (1)                            
    {
        colorImg = flycap.readImage();     // 適当なカメラ画像読込関数
        Mat temp;
        remap(colorImg, temp, mapC1, mapC2, INTER_LINEAR);
        ARToolKitPlus::ARMarkerInfo *markers;
        int markerID = tracker->calc(temp.data, -1, true, &markers);
        float conf = (float)tracker->getConfidence();      // 信頼度
        if (markerID == 4)
        {
                    // 適当なマーカー位置検出処理
                }
                // 適当な描画処理
    }
}