Ceres-Solver学习日志：扰动导数使用样例与广义导数及曲空间和切空间

         Ceres中的扰动导数主要通过是继承ceres::LocalParameterization来实现。当然，LocalParameterization本身具有更远大的使命或者说具有更广义的功能，扰动导数只是其特定的应用，也是其典型应用。

         谈到扰动求导自然避不开李代数的知识，但这不属于本文的范围，可参见相关理论或我之前的文章，本文假设你已具备了李代数的知识。如果实在没有，也不是不行，正如前面所说，扰动导数只是LocalParameterization的特定应用，本文虽然题为扰动导数巴拉巴拉，但大部分内容是在阐述链式求导、广义导数及如何使用LocalParameterization。

1.链式求导

         以二维为例，以x为自变量，y为自变量，Y为两者之间的映射关系，则世间二维关系都可以建模为Y(x,y)=0。

         如果映射关系很简单，则根据多元微分理论，可得y对x的导数为dy/dx=∂Y(x,y)/∂y   /   ∂Y(x,y)/∂x。

         然而，实际中的二维关系或者说几何物理模型通常并不这么简单，而是存在多个中间关系，或者说通过多个中间关系可以更好地表达实际意义。

         于是，实际的二维关系可能是这样的：A(x,a)=0, B(a, b)=0, C(b, c)=0, Y(c, y)=0。式中，小写字母是中间变量，大写字母是映射关系。根据链式链式求导方法，可得dy/dx=dy/dc*dc/db*db/da*da/dx。这里是四个链式结点。

         Ceres提供了一种实现，可以将所有链式结点分成前后两部分实现，前一部分由残差类(对自动求导而言)或CostFunction::Evaluate(对手动求导而言)实现，后一分部分由LocalParameterization::ComuputeJacobian实现，前一部分导数命名为GlobalJacobian，后一部分命名为Jacobian，整个导数命名为LocalJacobian，即LocalJacobian=GlobaJacobian*Jacobian。

         当然，你也可以在前一部分实现整个导数，则Jacobian为单位矩阵。或在后一部分实现全部导数，则GloabalJacobian为单位矩阵。

2.广义导数

         以上链式环节中，默认都是常规求导，即因变量增量与自变量增量的比值的极限，我们暂且称之为加法导数，以与后面的乘法导数(即扰动导数)呼应。

         自近代物理后，加法导数以不能满足物理学求解的需求，于是出了乘法导数(即扰动导数)，具体怎么回事，参见李代数相关理论。这里，主要是想说以上链式结点中，每个结点都可以是不同的求导方式，尽管当前数学主是加法导数和乘法导数，但我们可以根据实际需求自定义某种导数(这是要成为数学家的节奏啦)，我这里就称之为广义导数。

         Ceres将链式求导分为两段的目的就是为了能使用两种导数，从而减小计算量。因为不同的结点使用不同的求导方式可以减小计算，如旋转矩阵对旋转向量的乘法导数的计算量远小于加法导数。

         此外，利用不同的求导方式所得的雅可比矩阵来计算函数的下降方向的增量(相关理论参见梯度下降法、牛顿法、高斯牛顿法及LM法)，此增量与初值的合并方式也因求导方式而异，默认是加法。Ceres提供了LocalParameterization::Plus来实现非加法的合并方式。所以，无论前一部分是什么求导方式，只要最终求导方式(即因变量对自变量的导数不是加法导数)不是加法导数，则一定要重载LocalParameterization::Plus。

3.曲切空间

         有没有发现，上面所举例的中间关系，也是二维关系，而实际中通常是更高维的关系或更低维的关系。如自变量是10，因变量是5维，中间变量可能有3维、7维、30维、700维，这都取决于实际物理几何模型。如针孔成像模型中，世界坐标作为自变量是3维，图像坐标作为因变量是2维的，中间变量归一化物理坐标是2维的，中间变量相机坐标是3维的。或者将世界坐标当作已知量，将旋转当作未知量，则旋转向量作为自变量是3维，中间变量旋转矩阵是9维的，中间变量相机坐标是3维的，中间变量归一化物理坐标是2维的。

         可见，自变量和因变量的维度与中间变量的维度没有太大关系，关键还是取决于如何建模型。在不影响几何物理意义的情况下，当然是越少越好，这可以极大的减少计算量，也符合大道至简的规则。

        于是，根据变量的维度，我们相对地可以定义曲空间和切空间。在表示能表示同一物理几何意义的情况下，将最低维度的空间或更低维度的空间称为切空间，将最高维度的空间或更高维度的空间称为曲空间。

        典型地，对旋转向量和旋转四元数，旋转向量位于切空间，旋转四元数位于曲空间；对旋转向量和旋转矩阵，旋转向量位于切空间，旋转矩阵位于曲空间；对旋转四元数和旋转矩阵，旋转四元数位于切空间，旋转矩阵位于曲空间。

4.使用样例

         提供三个使用样例，封装为两个类：

         OptimizeRt：基于单次观察，优化此次观察的外参，定义了ProjectionModelRt、ProjectionCostRt、LocalParamRWithGeneralJ、LocalParamRWithIdentJ四个类。

         OptimizeKDRt：基于多次观察，优化相机内参和所有观察的外参，定义了ProjectionModelKDRt和ProjectionCostKDRt两个类并使用了OptimizeRt:: LocalParamRWithGeneralJ和OptimizeRt::LocalParamRWithIdentJ两个类。

　　  OptimzedRtWithNormazelized: 正如其名，基归一化坐标来优化外参，比较这两个类可发现它们的联系和区别，但归一化坐标是用cv::projectPoints生成而没用cv::undistortPoints(用之不能收敛)，以排除数据本身仿真且cv::undistortPoints非闭环实现的干扰。

         其中类MotionSim用于生成仿真数据，使用说明参见《CV学习日志：CV开发之三维仿真器》。

         存在可能不收敛的测试结果，属于正常现象，可修改初值精度来增加收敛性。

         以下是详细代码，依赖于C++14、OpenCV4.x、Ceres和Spdlog。

#include <opencv2/opencv.hpp>
#include <opencv2/viz.hpp>
#include <spdlog/spdlog.h>
#include <ceres/ceres.h>
using namespace std;
using namespace cv;

class MotionSim
{
public:
    static void TestMe(int argc, char** argv)
    {
        MotionSim motionSim(false);
        motionSim.camFovX = 45;
        motionSim.camFovY = 30;
        motionSim.camRand = 10;
        motionSim.enableVerbose = false;
        motionSim.runMotion(false, false, 7);
        motionSim.visMotion();
    }

public:
    struct MotionView
    {
        Mat_<double> r = Mat_<double>(3, 1);
        Mat_<double> t = Mat_<double>(3, 1);
        Mat_<double> q = Mat_<double>(4, 1);
        Mat_<double> rt = Mat_<double>(6, 1);
        Mat_<double> radian = Mat_<double>(3, 1);
        Mat_<double> degree = Mat_<double>(3, 1);
        Mat_<double> R = Mat_<double>(3, 3);
        Mat_<double> T = Mat_<double>(3, 4);
        Mat_<double> K;
        Mat_<double> D;
        Mat_<Vec3d> point3D;
        Mat_<Vec2d> point2D;
        Mat_<int> point3DIds;
        string print(string savePath = "")
        {
            string str;
            str += fmt::format("r: {}
", cvarr2str(r.t()));
            str += fmt::format("t: {}
", cvarr2str(t.t()));
            str += fmt::format("q: {}
", cvarr2str(q.t()));
            str += fmt::format("rt: {}
", cvarr2str(rt.t()));
            str += fmt::format("radian: {}
", cvarr2str(radian.t()));
            str += fmt::format("degree: {}
", cvarr2str(degree.t()));
            str += fmt::format("R: {}
", cvarr2str(R));
            str += fmt::format("T: {}
", cvarr2str(T));
            str += fmt::format("K: {}
", cvarr2str(K));
            str += fmt::format("D: {}
", cvarr2str(D.t()));
            if (savePath.empty() == false) { FILE* out = fopen(savePath.c_str(), "w"); fprintf(out, str.c_str()); fclose(out); }
            return str;
        }
    };
    static string cvarr2str(InputArray v)
    {
        Ptr<Formatted> fmtd = cv::format(v, Formatter::FMT_DEFAULT);
        string dst; fmtd->reset();
        for (const char* str = fmtd->next(); str; str = fmtd->next()) dst += string(str);
        return dst;
    }
    static void euler2matrix(double e[3], double R[9], bool forward = true, int argc = 0, char** argv = 0)
    {
        if (argc > 0)
        {
            int N = 999;
            for (int k = 0; k < N; ++k)//OpenCV not better than DIY
            {
                //1.GenerateData
                Matx31d radian0 = radian0.randu(-3.14159265358979323846, 3.14159265358979323846);
                Matx33d R; euler2matrix(radian0.val, R.val, true);
                const double deg2rad = 3.14159265358979323846 * 0.0055555555555555556;
                const double rad2deg = 180 * 0.3183098861837906715;

                //2.CalcByOpenCV
                Matx31d radian1 = cv::RQDecomp3x3(R, Matx33d(), Matx33d()) * deg2rad;

                //3.CalcByDIY
                Matx31d radian2; euler2matrix(R.val, radian2.val, false);

                //4.AnalyzeError
                double infRadian0Radian1 = norm(radian0, radian1, NORM_INF);
                double infRadian1Radian2 = norm(radian1, radian2, NORM_INF);

                //5.PrintError
                cout << endl << "LoopCount: " << k << endl;
                if (infRadian0Radian1 > 0 || infRadian1Radian2 > 0)
                {
                    cout << endl << "5.1PrintError" << endl;
                    cout << endl << "infRadian0Radian1: " << infRadian0Radian1 << endl;
                    cout << endl << "infRadian1Radian2: " << infRadian1Radian2 << endl;
                    if (0)
                    {
                        cout << endl << "5.2PrintDiff" << endl;
                        cout << endl << "radian0-degree0:" << endl << radian0.t() << endl << radian0.t() * rad2deg << endl;
                        cout << endl << "radian1-degree1:" << endl << radian1.t() << endl << radian1.t() * rad2deg << endl;
                        cout << endl << "radian2-degree2:" << endl << radian2.t() << endl << radian2.t() * rad2deg << endl;
                        cout << endl << "5.3PrintOthers" << endl;
                        cout << endl << "R:" << endl << R << endl;
                    }
                    cout << endl << "Press any key to continue" << endl; std::getchar();
                }
            }
            return;
        }
        if (forward)//check with 3D Rotation Converter
        {
            double sinR = std::sin(e[0]);
            double sinP = std::sin(e[1]);
            double sinY = std::sin(e[2]);
            double cosR = std::cos(e[0]);
            double cosP = std::cos(e[1]);
            double cosY = std::cos(e[2]);

            //RPY indicates: first Yaw aroundZ, second Pitch aroundY, third Roll aroundX
            R[0] = cosY * cosP; R[1] = cosY * sinP * sinR - sinY * cosR; R[2] = cosY * sinP * cosR + sinY * sinR;
            R[3] = sinY * cosP; R[4] = sinY * sinP * sinR + cosY * cosR; R[5] = sinY * sinP * cosR - cosY * sinR;
            R[6] = -sinP;       R[7] = cosP * sinR;                      R[8] = cosP * cosR;
        }
        else
        {
            double vs1 = std::abs(R[6] - 1.);
            double vs_1 = std::abs(R[6] + 1.);
            if (vs1 > 1E-9 && vs_1 > 1E-9)
            {
                e[2] = std::atan2(R[3], R[0]); //Yaw aroundZ
                e[1] = std::asin(-R[6]);//Pitch aroundY
                e[0] = std::atan2(R[7], R[8]); //Roll aroundX
            }
            else if (vs_1 <= 1E-9)
            {
                e[2] = 0; //Yaw aroundZ
                e[1] = 3.14159265358979323846 * 0.5;//Pitch aroundY
                e[0] = e[2] + atan2(R[1], R[2]); //Roll aroundX
            }
            else
            {
                e[2] = 0; //Yaw aroundZ
                e[1] = -3.14159265358979323846 * 0.5;//Pitch aroundY
                e[0] = -e[2] + atan2(-R[1], -R[2]); //Roll aroundX
            }
        }
    };
    static void quat2matrix(double q[4], double R[9], bool forward = true)
    {
        if (forward)//refer to qglviwer
        {
            double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
            if (std::abs(L1 - 1) > 1E-9) { std::printf("Not uint quaternion: NormQ=%.9f
", L1); abort(); }

            double xx = 2.0 * q[1] * q[1];
            double yy = 2.0 * q[2] * q[2];
            double zz = 2.0 * q[3] * q[3];

            double xy = 2.0 * q[1] * q[2];
            double xz = 2.0 * q[1] * q[3];
            double wx = 2.0 * q[1] * q[0];

            double yz = 2.0 * q[2] * q[3];
            double wy = 2.0 * q[2] * q[0];

            double wz = 2.0 * q[3] * q[0];

            R[0] = 1.0 - yy - zz;
            R[4] = 1.0 - xx - zz;
            R[8] = 1.0 - xx - yy;

            R[1] = xy - wz;
            R[3] = xy + wz;

            R[2] = xz + wy;
            R[6] = xz - wy;

            R[5] = yz - wx;
            R[7] = yz + wx;
        }
        else
        {
            double onePlusTrace = 1.0 + R[0] + R[4] + R[8];// Compute one plus the trace of the matrix
            if (onePlusTrace > 1E-9)
            {
                double s = sqrt(onePlusTrace) * 2.0;
                double is = 1 / s;
                q[0] = 0.25 * s;
                q[1] = (R[7] - R[5]) * is;
                q[2] = (R[2] - R[6]) * is;
                q[3] = (R[3] - R[1]) * is;
            }
            else
            {
                std::printf("1+trace(R)=%.9f is too small and (R11,R22,R33)=(%.9f,%.9f,%.9f)
", onePlusTrace, R[0], R[4], R[8]);
                if ((R[0] > R[4]) && (R[0] > R[8]))//max(R00, R11, R22)=R00
                {
                    double s = sqrt(1.0 + R[0] - R[4] - R[8]) * 2.0;
                    double is = 1 / s;
                    q[0] = (R[5] - R[7]) * is;
                    q[1] = 0.25 * s;
                    q[2] = (R[1] + R[3]) * is;
                    q[3] = (R[2] + R[6]) * is;
                }
                else if (R[4] > R[8])//max(R00, R11, R22)=R11
                {
                    double s = sqrt(1.0 - R[0] + R[4] - R[8]) * 2.0;
                    double is = 1 / s;
                    q[0] = (R[2] - R[6]) * is;
                    q[1] = (R[1] + R[3]) * is;
                    q[2] = 0.25 * s;
                    q[3] = (R[5] + R[7]) * is;
                }
                else//max(R00, R11, R22)=R22
                {
                    double s = sqrt(1.0 - R[0] - R[4] + R[8]) * 2.0;
                    double is = 1 / s;
                    q[0] = (R[1] - R[3]) * is;
                    q[1] = (R[2] + R[6]) * is;
                    q[2] = (R[5] + R[7]) * is;
                    q[3] = 0.25 * s;
                }
            }
            double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
            if (L1 < 1e-9) { std::printf("Wrong rotation matrix: NormQ=%.9f
", L1); abort(); }
            else { L1 = 1 / L1; q[0] *= L1; q[1] *= L1; q[2] *= L1; q[3] *= L1; }
        }
    }
    static void vec2quat(double r[3], double q[4], bool forward = true)
    {
        if (forward)//refer to qglviwer
        {
            double theta = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]);
            if (std::abs(theta) < 1E-9)
            {
                q[0] = 1; q[1] = q[2] = q[3] = 0;
                std::printf("Rotation approximates zero: Theta=%.9f
", theta);
            };

            q[0] = std::cos(theta * 0.5);
            double ss = std::sin(theta * 0.5) / theta;
            q[1] = r[0] * ss;
            q[2] = r[1] * ss;
            q[3] = r[2] * ss;
        }
        else
        {
            double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
            if (std::abs(L1 - 1) > 1E-9) { std::printf("Not uint quaternion: NormQ=%.9f
", L1); abort(); }

            double theta = 2 * acos(q[0]);
            if (theta > 3.14159265358979323846) theta = 2 * 3.14159265358979323846 - theta;
            double thetaEx = theta / std::sin(theta * 0.5);
            r[0] = q[1] * thetaEx;
            r[1] = q[2] * thetaEx;
            r[2] = q[3] * thetaEx;
        }
    }
    static void vec2matrix(double r[3], double R[9], bool forward = true, int argc = 0, char** argv = 0)
    {
        if (argc > 0)
        {
            int N = 999;
            for (int k = 0; k < N; ++k) //refer to the subsequent article for more details
            {
                //1.GenerateData
                Matx31d r0 = r0.randu(-999, 999);
                Matx33d R0; cv::Rodrigues(r0, R0);

                //2.CalcByOpenCV
                Matx33d R1;
                Matx31d r1;
                cv::Rodrigues(r0, R1);
                cv::Rodrigues(R0, r1);

                //3.CalcByDIY
                Matx33d R2;
                Matx31d r2;
                vec2matrix(r0.val, R2.val, true);
                vec2matrix(r2.val, R0.val, false);

                //4.AnalyzeError
                double infR1R2 = norm(R1, R2, NORM_INF);
                double infr1r2 = norm(r1, r2, NORM_INF);

                //5.PrintError
                cout << endl << "LoopCount: " << k << endl;
                if (infR1R2 > 1E-12 || infr1r2 > 1E-12)
                {
                    cout << endl << "5.1PrintError" << endl;
                    cout << endl << "infR1R2: " << infR1R2 << endl;
                    cout << endl << "infr1r2: " << infr1r2 << endl;
                    if (0)
                    {
                        cout << endl << "5.2PrintDiff" << endl;
                        cout << endl << "R1: " << endl << R1 << endl;
                        cout << endl << "R2: " << endl << R2 << endl;
                        cout << endl;
                        cout << endl << "r1: " << endl << r1.t() << endl;
                        cout << endl << "r2: " << endl << r2.t() << endl;
                        cout << endl << "5.3PrintOthers" << endl;
                    }
                    cout << endl << "Press any key to continue" << endl; std::getchar();
                }
            }
            return;
        }

        if (forward)
        {
            double theta = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]);
            if (theta < 1E-9)
            {
                R[0] = R[4] = R[8] = 1.0;
                R[1] = R[2] = R[3] = R[5] = R[6] = R[7] = 0.0;
                std::printf("Rotation approximates zero: Theta=%.9f
", theta);
                return;
            }
            double cs = cos(theta);
            double sn = sin(theta);
            double itheta = 1. / theta;
            double cs1 = 1 - cs;
            double nx = r[0] * itheta;
            double ny = r[1] * itheta;
            double nz = r[2] * itheta;

            double nxnx = nx * nx, nyny = ny * ny, nznz = nz * nz;
            double nxny = nx * ny, nxnz = nx * nz, nynz = ny * nz;
            double nxsn = nx * sn, nysn = ny * sn, nzsn = nz * sn;

            R[0] = nxnx * cs1 + cs;
            R[3] = nxny * cs1 + nzsn;
            R[6] = nxnz * cs1 - nysn;

            R[1] = nxny * cs1 - nzsn;
            R[4] = nyny * cs1 + cs;
            R[7] = nynz * cs1 + nxsn;

            R[2] = nxnz * cs1 + nysn;
            R[5] = nynz * cs1 - nxsn;
            R[8] = nznz * cs1 + cs;

            if (0)
            {
                Mat_<double> dRdu({ 9, 4 }, {
                    2 * nx * cs1, 0, 0, (nxnx - 1) * sn,
                    ny * cs1, nx * cs1, -sn, nxny * sn - nz * cs,
                    nz * cs1, sn, nx * cs1, nxnz * sn + ny * cs,
                    ny * cs1, nx * cs1, sn, nxny * sn + nz * cs,
                    0, 2 * ny * cs1, 0, (nyny - 1) * sn,
                    -sn, nz * cs1, ny * cs1, nynz * sn - nx * cs,
                    nz * cs1, -sn, nx * cs1, nxnz * sn - ny * cs,
                    sn, nz * cs1, ny * cs1, nynz * sn + nx * cs,
                    0, 0, 2 * nz * cs1, (nznz - 1) * sn });

                Mat_<double> dudv({ 4, 4 }, {
                    itheta, 0, 0, -nx * itheta,
                    0, itheta, 0, -ny * itheta,
                    0, 0, itheta, -nz * itheta,
                    0, 0, 0, 1 });

                Mat_<double> dvdr({ 4, 3 }, {
                    1, 0, 0,
                    0, 1, 0,
                    0, 0, 1,
                    nx, ny, nz });

                Mat_<double> Jacobian = dRdu * dudv * dvdr;//rows=9 cols=3
            }
        }
        else
        {
            double sx = R[7] - R[5];
            double sy = R[2] - R[6];
            double sz = R[3] - R[1];
            double sn = sqrt(sx * sx + sy * sy + sz * sz) * 0.5;
            double cs = (R[0] + R[4] + R[8] - 1) * 0.5;
            double theta = acos(cs);
            double ss = 2 * sn;
            double iss = 1. / ss;
            double tss = theta * iss;
            r[0] = tss * sx;
            r[1] = tss * sy;
            r[2] = tss * sz;

            if (0)
            {
                Mat_<double> drdu({ 3, 4 }, {
                    tss, 0, 0, (sn - theta * cs) * iss * iss * sx * 2,
                    0, tss, 0, (sn - theta * cs) * iss * iss * sy * 2,
                    0, 0, tss, (sn - theta * cs) * iss * iss * sz * 2 });

                Mat_<double> dudR({ 4, 9 }, {
                    0, 0, 0, 0, 0, -1, 0, 1, 0,
                    0, 0, 1, 0, 0, 0, -1, 0, 0,
                    0, -1, 0, 1, 0, 0, 0, 0, 0,
                    -iss, 0, 0, 0, -iss, 0, 0, 0, -iss });

                Mat_<double> Jacobian = drdu * dudR;//rows=3 cols=9
            }
        }
    }

private:
    const int nHorPoint3D = 100;
    const int nVerPoint3D = 100;
    const double varPoint3DXY = 10.;
    const double minPoint3DZ = 1.;
    const double maxPoint3DZ = 99.;
    const double minCamZ = 101.;
    const double maxCamZ = 150.;
    const double varCamDegree = 10.;
    Mat_<Vec3d> allPoint3D = Mat_<Vec3d>(nVerPoint3D * nHorPoint3D, 1);
    Mat_<double> allPoint3DZ = Mat_<double>(nVerPoint3D * nHorPoint3D, 1);
    Mat_<double> K;
    Mat_<double> D;
    const double deg2rad = 3.14159265358979323846 * 0.0055555555555555556;
    const double rad2deg = 180 * 0.3183098861837906715;

public:
    int camRows = 480;
    int camCols = 640;
    int camFovY = 90;
    int camFovX = 90;
    int camRand = 10;//append random[0,camRand] to camera intrinsics
    int nCamDist = 5;//refer to opencv for value domain
    int nMinMotion = 32; // no less than X motion views
    int nMaxMotion = INT_MAX; // no more than X motion views
    int nPoint2DThenExit = 32;//exit when less than X pixies
    int rotMode = 1 + 2 + 4;//0=noRot 1=xAxis 2=yAxis 4=zAxis
    bool noTrans = false;//translate or not while motion
    bool world2D = false;//planar world or not
    bool rndSeek = true;//use random seek or not
    bool enableVerbose = false;//check motions one by one or not
    vector<MotionView> motionViews;//World Information: RightX, FrontY, DownZ
    MotionSim(bool run = true, bool world2D0 = false, bool noTrans0 = false, int rotMode0 = 7) { if (run) runMotion(world2D0, noTrans0, rotMode0); }

public:
    void runMotion(bool world2D0 = false, bool noTrans0 = false, int rotMode0 = 7)
    {
        world2D = world2D0;
        noTrans = noTrans0;
        rotMode = rotMode0;
        motionViews.clear();
        if (rndSeek) cv::setRNGSeed(clock());
        while (motionViews.size() < nMinMotion)
        {
            //1.GetAllPoint3D
            if (world2D) allPoint3DZ = 0.;
            else cv::randu(allPoint3DZ, -maxPoint3DZ, -minPoint3DZ);//DownZ
            for (int i = 0, k = 0; i < nVerPoint3D; ++i)
                for (int j = 0; j < nHorPoint3D; ++j, ++k)
                    allPoint3D(k) = Vec3d((j + cv::randu<double>()) * varPoint3DXY, (i + cv::randu<double>()) * varPoint3DXY, allPoint3DZ(i, j));

            //2.GetCamParams
            double camFx = camCols / 2. / std::tan(camFovX / 2. * deg2rad) + cv::randu<double>() * camRand;
            double camFy = camRows / 2. / std::tan(camFovY / 2. * deg2rad) + cv::randu<double>() * camRand;
            double camCx = camCols / 2. + cv::randu<double>() * camRand;
            double camCy = camRows / 2. + cv::randu<double>() * camRand;
            K.create(3, 3); K << camFx, 0, camCx, 0, camFy, camCy, 0, 0, 1;
            D.create(nCamDist, 1); cv::randu(D, -1.0, 1.0);

            //3.GetAllMotionView
            motionViews.clear();
            for (int64 k = 0; ; ++k)
            {
                //3.1 JoinCamParams
                MotionView view;
                view.K = K.clone();
                view.D = D.clone();

                //3.2 GetCamTrans
                if (k == 0) view.t(0) = view.t(1) = 0;
                else
                {
                    view.t(0) = motionViews[k - 1].t(0) + cv::randu<double>() * varPoint3DXY;
                    view.t(1) = motionViews[k - 1].t(1) + cv::randu<double>() * varPoint3DXY;
                }
                view.t(2) = minCamZ + cv::randu<double>() * (maxCamZ - minCamZ);
                view.t(2) = -view.t(2);//DownZ
                if (noTrans && k != 0) { view.t(0) = motionViews[0].t(0); view.t(1) = motionViews[0].t(1); view.t(2) = motionViews[0].t(2); }

                //3.3 GetCamRot: degree-->radian-->matrix-->vector&quaternion
                view.degree = 0.;
                if (rotMode & 1) view.degree(0) = cv::randu<double>() * varCamDegree;
                if (rotMode & 2) view.degree(1) = cv::randu<double>() * varCamDegree;
                if (rotMode & 4) view.degree(2) = cv::randu<double>() * varCamDegree;
                view.radian = view.degree * deg2rad;
                euler2matrix(view.radian.ptr<double>(), view.R.ptr<double>());
                cv::Rodrigues(view.R, view.r);
                quat2matrix(view.q.ptr<double>(), view.R.ptr<double>(), false);
                cv::hconcat(view.R, view.t, view.T);
                cv::vconcat(view.r, view.t, view.rt);

                //3.4 GetPoint3DAndPoint2D
                Mat_<Vec2d> allPoint2D;
                cv::projectPoints(allPoint3D, -view.r, -view.R.t() * view.t, view.K, view.D, allPoint2D);
                for (int k = 0; k < allPoint2D.total(); ++k)
                    if (allPoint2D(k)[0] > 0 && allPoint2D(k)[0] < camCols && allPoint2D(k)[1] > 0 && allPoint2D(k)[1] < camRows)
                    {
                        view.point2D.push_back(allPoint2D(k));
                        view.point3D.push_back(allPoint3D(k));
                        view.point3DIds.push_back(k);
                    }

                //3.5 PrintDetails
                motionViews.push_back(view);
                if (enableVerbose)
                {
                    cout << endl << view.print();
                    cout << fmt::format("view={}   features={}
", k, view.point2D.rows);
                    double minV = 0, maxV = 0;//Distortion makes some minV next to maxV
                    int minId = 0, maxId = 0;
                    cv::minMaxIdx(allPoint2D.reshape(1, int(allPoint2D.total()) * allPoint2D.channels()), &minV, &maxV, &minId, &maxId);
                    cout << fmt::format("minInfo:({}, {})", minId, minV) << allPoint3D(minId / 2) << allPoint2D(minId / 2) << endl;
                    cout << fmt::format("maxInfo:({}, {})", maxId, maxV) << allPoint3D(maxId / 2) << allPoint2D(maxId / 2) << endl;
                    cout << "Press any key to continue" << endl; std::getchar();
                }
                if (view.point2D.rows < nPoint2DThenExit || motionViews.size() > nMaxMotion) break;
            }
        }
    }
    void visMotion()
    {
        //1.CreateWidgets
        Size2d validSize(nHorPoint3D * varPoint3DXY, nVerPoint3D * varPoint3DXY);
        Mat_<cv::Affine3d> camPoses(int(motionViews.size()), 1); for (int k = 0; k < camPoses.rows; ++k) camPoses(k) = cv::Affine3d(motionViews[k].T);
        viz::WText worldInfo(fmt::format("nMotionView: {}
K: {}
D: {}", motionViews.size(), cvarr2str(K), cvarr2str(D)), Point(10, 240), 10);
        viz::WCoordinateSystem worldCSys(1000);
        viz::WPlane worldGround(Point3d(validSize.width / 2, validSize.height / 2, 0), Vec3d(0, 0, 1), Vec3d(0, 1, 0), validSize);
        viz::WCloud worldPoints(allPoint3D, Mat_<Vec3b>(allPoint3D.size(), Vec3b(0, 255, 0)));
        viz::WTrajectory camTraj1(camPoses, viz::WTrajectory::FRAMES, 8);
        viz::WTrajectorySpheres camTraj2(camPoses, 100, 2);
        viz::WTrajectoryFrustums camTraj3(camPoses, Matx33d(K), 4., viz::Color::yellow());
        worldCSys.setRenderingProperty(viz::OPACITY, 0.1);
        worldGround.setRenderingProperty(viz::OPACITY, 0.1);
        camTraj2.setRenderingProperty(viz::OPACITY, 0.6);

        //2.ShowWidgets
        static viz::Viz3d viz3d(__FUNCTION__);
        viz3d.showWidget("worldInfo", worldInfo);
        viz3d.showWidget("worldCSys", worldCSys);
        viz3d.showWidget("worldGround", worldGround);
        viz3d.showWidget("worldPoints", worldPoints);
        viz3d.showWidget("camTraj1", camTraj1);
        viz3d.showWidget("camTraj2", camTraj2);
        viz3d.showWidget("camTraj3", camTraj3);

        //3.UpdateWidghts
        static const vector<MotionView>& views = motionViews;
        viz3d.registerKeyboardCallback([](const viz::KeyboardEvent& keyboarEvent, void* pVizBorad)->void
            {
                if (keyboarEvent.action != viz::KeyboardEvent::KEY_DOWN) return;
                static int pos = 0;
                if (keyboarEvent.code == ' ')
                {
                    size_t num = views.size();
                    size_t ind = pos % num;
                    double xmin3D = DBL_MAX, ymin3D = DBL_MAX, xmin2D = DBL_MAX, ymin2D = DBL_MAX;
                    double xmax3D = -DBL_MAX, ymax3D = -DBL_MAX, xmax2D = -DBL_MAX, ymax2D = -DBL_MAX;
                    for (size_t k = 0; k < views[ind].point3D.rows; ++k)
                    {
                        Vec3d pt3 = views[ind].point3D(int(k));
                        Vec2d pt2 = views[ind].point2D(int(k));
                        if (pt3[0] < xmin3D) xmin3D = pt3[0];
                        if (pt3[0] > xmax3D) xmax3D = pt3[0];
                        if (pt3[1] < ymin3D) ymin3D = pt3[1];
                        if (pt3[1] > ymax3D) ymax3D = pt3[1];
                        if (pt2[0] < xmin2D) xmin2D = pt2[0];
                        if (pt2[0] > xmax2D) xmax2D = pt2[0];
                        if (pt2[1] < ymin2D) ymin2D = pt2[1];
                        if (pt2[1] > ymax2D) ymax2D = pt2[1];
                    }
                    if (pos != 0)
                    {
                        for (int k = 0; k < views[ind == 0 ? num - 1 : ind - 1].point3D.rows; ++k) viz3d.removeWidget("active" + std::to_string(k));
                        viz3d.removeWidget("viewInfo");
                        viz3d.removeWidget("camSolid");
                    }
                    for (int k = 0; k < views[ind].point3D.rows; ++k) viz3d.showWidget("active" + std::to_string(k), viz::WSphere(views[ind].point3D(k), 5, 10));
                    viz3d.showWidget("viewInfo", viz::WText(fmt::format("CurrentMotion: {}
ValidPoints: {}
Min3DXY_Min2DXY: {}, {}, {}, {}
Max3DXY_Max2DXY: {}, {}, {}, {}
Rot_Trans_Euler: {}
",
                        ind, views[ind].point3D.rows, xmin3D, ymin3D, xmin2D, ymin2D, xmax3D, ymax3D, xmax2D, ymax2D,
                        cvarr2str(views[ind].r.t()) + cvarr2str(views[ind].t.t()) + cvarr2str(views[ind].degree.t())), Point(10, 10), 10));
                    viz3d.showWidget("camSolid", viz::WCameraPosition(Matx33d(views[ind].K), 10, viz::Color::yellow()), cv::Affine3d(views[ind].T));
                    ++pos;
                }
            }, 0);
        viz3d.spin();
    }
};

class OptimizeRt
{
public:
    using MotionView = MotionSim::MotionView;
    static void TestMe(int argc = 0, char** argv = 0)
    {
        int N = 99;
        for (int k = 0; k < N; ++k)
        {
            //0.GenerateData
            bool world2D = k % 2;
            int rotMode = k % 7 + 1;
            MotionSim motionSim(false);
            motionSim.camFovX = 90;
            motionSim.camFovY = 90;
            motionSim.camRand = 10;
            motionSim.nMinMotion = 16;//2
            motionSim.nMaxMotion = 32;//4
            motionSim.rndSeek = false;
            motionSim.nCamDist = 5;
            motionSim.runMotion(world2D, false, rotMode);
            //motionSim.visMotion();
            int rndInd = int(motionSim.motionViews.size() * cv::randu<double>());
            Mat_<double> r0 = -motionSim.motionViews[rndInd].r;
            Mat_<double> t0 = -motionSim.motionViews[rndInd].R.t() * motionSim.motionViews[rndInd].t;
            const MotionView& motionView = motionSim.motionViews[rndInd];
            double errRatio = 0.9;

            //1.CalcByCeresWithAutoDeriv
            Mat_<double> R1; cv::Rodrigues(r0 * errRatio, R1);
            Mat_<double> t1 = t0 * errRatio;
            ceres::Problem problem1;
            problem1.AddResidualBlock(new ceres::AutoDiffCostFunction<ProjectionModelRt, ceres::DYNAMIC, 9, 3>(
                new ProjectionModelRt(motionView, motionSim.nCamDist), motionView.point3D.rows * 2), NULL, R1.ptr<double>(), t1.ptr<double>());
            problem1.SetParameterization(R1.ptr<double>(), new LocalParamRWithGeneralJ);
            ceres::Solver::Options options1;
            ceres::Solver::Summary summary1;
            ceres::Solve(options1, &problem1, &summary1);
            Mat_<double> r1; cv::Rodrigues(R1, r1);
            int nIter1 = (int)summary1.iterations.size();

            //2.CalcByCeresWithGeneralJ
            Mat_<double> R2; cv::Rodrigues(r0 * errRatio, R2);
            Mat_<double> t2 = t0 * errRatio;
            ceres::Problem problem2;
            problem2.AddResidualBlock(new ProjectionCostRt(motionView, motionSim.nCamDist, 9), NULL, R2.ptr<double>(), t2.ptr<double>());
            problem2.SetParameterization(R2.ptr<double>(), new LocalParamRWithGeneralJ);
            ceres::Solver::Options options2;
            ceres::Solver::Summary summary2;
            ceres::Solve(options2, &problem2, &summary2);
            Mat_<double> r2; cv::Rodrigues(R2, r2);
            int nIter2 = (int)summary2.iterations.size();

            //3.CalcByCeresWithGeneralJ
            Mat_<double> r3 = r0 * errRatio;
            Mat_<double> t3 = t0 * errRatio;
            ceres::Problem problem3;
            problem3.AddResidualBlock(new ProjectionCostRt(motionView, motionSim.nCamDist, 3), NULL, r3.ptr<double>(), t3.ptr<double>());
            problem3.SetParameterization(r3.ptr<double>(), new LocalParamRWithIdentJ);
            ceres::Solver::Options options3;
            ceres::Solver::Summary summary3;
            ceres::Solve(options3, &problem3, &summary3);
            int nIter3 = (int)summary3.iterations.size();

            //4.AnalyzeError
            double infr0r0 = norm(r0, r0 * errRatio, NORM_INF);
            double infr0r1 = norm(r0, r1, NORM_INF);
            double infr0r2 = norm(r0, r2, NORM_INF);
            double infr0r3 = norm(r0, r3, NORM_INF);
            double inft0t0 = norm(t0, t0 * errRatio, NORM_INF);
            double inft0t1 = norm(t0, t1, NORM_INF);
            double inft0t2 = norm(t0, t2, NORM_INF);
            double inft0t3 = norm(t0, t3, NORM_INF);

            //5.PrintError
            cout << fmt::format("LoopCount: {}      AutoDeriv.iters: {}      GeneralJ.iters: {}      IdentJ.iters: {}
", k, nIter1, nIter2, nIter3);
            if (infr0r1 > 1e-8 || infr0r2 > 1e-8 || infr0r3 > 1e-8 || inft0t1 > 1e-8 || inft0t2 > 1e-8 || inft0t3 > 1e-8)
            {
                cout << fmt::format("infr0r1: {:<15.9}		{:<15.9}
", infr0r1, infr0r0);
                cout << fmt::format("infr0r2: {:<15.9}		{:<15.9}
", infr0r2, infr0r0);
                cout << fmt::format("infr0r3: {:<15.9}		{:<15.9}
", infr0r3, infr0r0);
                cout << fmt::format("inft0t1: {:<15.9}		{:<15.9}
", inft0t1, inft0t0);
                cout << fmt::format("inft0t2: {:<15.9}		{:<15.9}
", inft0t2, inft0t0);
                cout << fmt::format("inft0t3: {:<15.9}		{:<15.9}
", inft0t3, inft0t0);
                cout << "Press any key to continue" << endl; std::getchar();
            }
        }
    }

public:
    struct ProjectionModelRt
    {
        const int nDist;
        double K[4];
        const double* D;
        Mat_<Vec2d> point2D;
        Mat_<Vec3d> point3D;
        ProjectionModelRt(const MotionView& motionView0, const int nDist0) : nDist(nDist0)
        {
            K[0] = motionView0.K(0, 0);
            K[1] = motionView0.K(1, 1);
            K[2] = motionView0.K(0, 2);
            K[3] = motionView0.K(1, 2);
            D = motionView0.D.ptr<double>();
            point2D = motionView0.point2D;
            point3D = motionView0.point3D;
        }
        template <typename tp> bool operator()(const tp* const rot, const tp* const t, tp* errPoint2D) const
        {
            //1.Projection params
            double fx = K[0];
            double fy = K[1];
            double cx = K[2];
            double cy = K[3];

            //2.Distortion params
            double k1 = D[0];
            double k2 = D[1];
            double p1 = D[2];
            double p2 = D[3];
            double k3, k4, k5, k6;
            double s1, s2, s3, s4;
            if (nDist > 4) k3 = D[4];
            if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; }
            if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; }

            //3.Translation params
            tp tx = t[0];
            tp ty = t[1];
            tp tz = t[2];

            //4.Rotation params
            tp R11, R12, R13, R21, R22, R23, R31, R32, R33;
            {
                R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
                R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
                R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
            }

            //5.ReProjection
            const Vec2d* data2D = point2D.ptr<Vec2d>();
            const Vec3d* data3D = point3D.ptr<Vec3d>();
            Vec<tp, 2>* err2D = (Vec<tp, 2>*)errPoint2D;
            for (int k = 0; k < point3D.rows; ++k)
            {
                //5.1 WorldCoordinate
                double X = data3D[k][0];
                double Y = data3D[k][1];
                double Z = data3D[k][2];

                //5.2 CameraCoordinate
                tp x = R11 * X + R12 * Y + R13 * Z + tx;
                tp y = R21 * X + R22 * Y + R23 * Z + ty;
                tp z = R31 * X + R32 * Y + R33 * Z + tz;

                //5.3 StandardPhysicsCoordinate
                tp iz = 1. / z; //if denominator==0
                tp xc = x * iz;
                tp yc = y * iz;

                //5.4 DistortionPhysicsCoordinate
                tp xc2 = xc * xc;
                tp yc2 = yc * yc;
                tp d2 = xc2 + yc2;
                tp xcyc = 2. * xc * yc;
                tp d4 = d2 * d2;
                tp d6 = d2 * d4;
                tp d2xc2 = d2 + 2. * xc2;
                tp d2yc2 = d2 + 2. * yc2;
                tp nu, de, xd, yd;
                if (nDist < 5)
                {
                    nu = 1. + k1 * d2 + k2 * d4;
                    xd = xc * nu + p1 * xcyc + p2 * d2xc2;
                    yd = yc * nu + p2 * xcyc + p1 * d2yc2;
                }
                else if (nDist < 8)
                {
                    nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
                    xd = xc * nu + p1 * xcyc + p2 * d2xc2;
                    yd = yc * nu + p2 * xcyc + p1 * d2yc2;
                }
                else if (nDist < 12)
                {
                    nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
                    de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
                    xd = xc * nu * de + p1 * xcyc + p2 * d2xc2;
                    yd = yc * nu * de + p2 * xcyc + p1 * d2yc2;
                }
                else if (nDist < 14)
                {
                    nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
                    de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
                    xd = xc * nu * de + p1 * xcyc + p2 * d2xc2 + s1 * d2 + s2 * d4;
                    yd = yc * nu * de + p2 * xcyc + p1 * d2yc2 + s3 * d2 + s4 * d4;
                }
                err2D[k][0] = xd * fx + cx - data2D[k][0];
                err2D[k][1] = yd * fy + cy - data2D[k][1];
            }
            return true;
        }
    };
    struct LocalParamRWithGeneralJ : public ceres::LocalParameterization
    {
        bool Plus(const double* preParam, const double* deltaParam, double* newParam) const
        {
            Matx33d ambientPre(preParam);
            Matx33d ambientDelta; cv::Rodrigues(Matx31d(deltaParam), ambientDelta);
            Matx33d ambientNew = ambientDelta * ambientPre;
            memcpy(newParam, ambientNew.val, sizeof(double) * 9);
            return true;
        }

        bool ComputeJacobian(const double* param, double* jacobian) const
        {
            Mat_<double> dRdr(9, 3, jacobian);
            dRdr << 0, param[6], -param[3], 0, param[7], -param[4], 0, param[8], -param[5],
                -param[6], 0, param[0], -param[7], 0, param[1], -param[8], 0, param[2],
                param[3], -param[0], 0, param[4], -param[1], 0, param[5], -param[2], 0;
            return true;

            Matx33d R(param);
            Mat_<double> dRdrT({ 3, 9 }, {
                0, 0, 0, -R(2, 0), -R(2, 1), -R(2, 2), R(1, 0), R(1, 1), R(1, 2),
                R(2, 0), R(2, 1), R(2, 2), 0, 0, 0, -R(0, 0), -R(0, 1), -R(0, 2),
                -R(1, 0), -R(1, 1), -R(1, 2), R(0, 0), R(0, 1), R(0, 2), 0, 0, 0 });
            cout << endl << cv::norm(dRdr, dRdrT.t(), NORM_INF) << endl;
        }

        int GlobalSize() const { return 9; }
        int LocalSize() const { return 3; }
    };
    struct LocalParamRWithIdentJ : public ceres::LocalParameterization
    {
        bool Plus(const double* preParam, const double* deltaParam, double* newParam) const
        {
            Matx33d ambientPre; cv::Rodrigues(Matx31d(preParam), ambientPre);
            Matx33d ambientDelta; cv::Rodrigues(Matx31d(deltaParam), ambientDelta);
            Matx31d tangentNew; cv::Rodrigues(ambientDelta * ambientPre, tangentNew);
            memcpy(newParam, tangentNew.val, sizeof(double) * 3);
            return true;
        }

        bool ComputeJacobian(const double* param, double* jacobian) const
        {
            Mat_<double>(3, 3, jacobian) = Mat_<double>::eye(3, 3);
            return true;
        }

        int GlobalSize() const { return 3; }
        int LocalSize() const { return 3; }
    };
    struct ProjectionCostRt : public ceres::CostFunction
    {
        const int nDist;
        double K[4];
        const double* D;
        Mat_<Vec2d> point2D;
        Mat_<Vec3d> point3D;
        const int nRot;
        const MotionView& motionViewForDBG;//only use once
        ProjectionCostRt(const MotionView& motionView0, const int nDist0, const int nRot0) : nDist(nDist0), nRot(nRot0), motionViewForDBG(motionView0)
        {
            K[0] = motionView0.K(0, 0);
            K[1] = motionView0.K(1, 1);
            K[2] = motionView0.K(0, 2);
            K[3] = motionView0.K(1, 2);
            D = motionView0.D.ptr<double>();
            point2D = motionView0.point2D;
            point3D = motionView0.point3D;
            this->set_num_residuals(point2D.rows * 2);
            this->mutable_parameter_block_sizes()->push_back(nRot);
            this->mutable_parameter_block_sizes()->push_back(3);
        }
        bool Evaluate(double const* const* params, double* residuals, double** jacobians) const
        {
            //0.FormatInput
            const double* rot = params[0];
            const double* t = params[1];
            Vec2d* err2D = (Vec2d*)(residuals);
            Mat_<double> dpdR; if (jacobians && jacobians[0]) dpdR = Mat_<double>(point2D.rows * 2, nRot, jacobians[0]);
            Mat_<double> dpdt; if (jacobians && jacobians[1]) dpdt = Mat_<double>(point2D.rows * 2, 3, jacobians[1]);

            //1.Projection params
            double fx = K[0];
            double fy = K[1];
            double cx = K[2];
            double cy = K[3];

            //2.Distortion params
            double k1 = D[0];
            double k2 = D[1];
            double p1 = D[2];
            double p2 = D[3];
            double k3 = 0, k4 = 0, k5 = 0, k6 = 0;
            double s1 = 0, s2 = 0, s3 = 0, s4 = 0;
            if (nDist > 4) k3 = D[4];
            if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; }
            if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; }

            //3.Translation params
            double tx = t[0];
            double ty = t[1];
            double tz = t[2];

            //4.Rotation params
            double R11, R12, R13, R21, R22, R23, R31, R32, R33;
            if (nRot == 9)
            {
                R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
                R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
                R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
            }
            if (nRot == 3)
            {
                Matx33d R; cv::Rodrigues(Matx31d(rot), R);
                rot = R.val;
                R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
                R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
                R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
                rot = params[0];
            }

            //5.ReProjection
            const Vec2d* data2D = point2D.ptr<Vec2d>();
            const Vec3d* data3D = point3D.ptr<Vec3d>();
            for (int k = 0; k < point3D.rows; ++k)
            {
                //5.1 WorldCoordinate
                double X = data3D[k][0];
                double Y = data3D[k][1];
                double Z = data3D[k][2];

                //5.2 CameraCoordinate
                double x = R11 * X + R12 * Y + R13 * Z + tx;
                double y = R21 * X + R22 * Y + R23 * Z + ty;
                double z = R31 * X + R32 * Y + R33 * Z + tz;

                //5.3 StandardPhysicsCoordinate
                double iz = 1. / z; //if denominator==0
                double xc = x * iz;
                double yc = y * iz;

                //5.4 DistortionPhysicsCoordinate
                double xc2 = xc * xc;
                double yc2 = yc * yc;
                double d2 = xc2 + yc2;
                double xcyc = 2. * xc * yc;
                double d4 = d2 * d2;
                double d6 = d2 * d4;
                double d2xc2 = d2 + 2. * xc2;
                double d2yc2 = d2 + 2. * yc2;
                double nu = 0, de = 1., xd = 0, yd = 0;
                if (nDist < 5)
                {
                    nu = 1. + k1 * d2 + k2 * d4;
                    xd = xc * nu + p1 * xcyc + p2 * d2xc2;
                    yd = yc * nu + p2 * xcyc + p1 * d2yc2;
                }
                else if (nDist < 8)
                {
                    nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
                    xd = xc * nu + p1 * xcyc + p2 * d2xc2;
                    yd = yc * nu + p2 * xcyc + p1 * d2yc2;
                }
                else if (nDist < 12)
                {
                    nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
                    de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
                    xd = xc * nu * de + p1 * xcyc + p2 * d2xc2;
                    yd = yc * nu * de + p2 * xcyc + p1 * d2yc2;
                }
                else if (nDist < 14)
                {
                    nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
                    de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
                    xd = xc * nu * de + p1 * xcyc + p2 * d2xc2 + s1 * d2 + s2 * d4;
                    yd = yc * nu * de + p2 * xcyc + p1 * d2yc2 + s3 * d2 + s4 * d4;
                }
                err2D[k][0] = xd * fx + cx - data2D[k][0];
                err2D[k][1] = yd * fy + cy - data2D[k][1];
                if (!dpdR.empty() || !dpdt.empty())
                {
                    Mat_<double> dpDdpCD({ 2, 2 }, { fx, 0, 0, fy });

                    Mat_<double> dpCDdh({ 2, 5 }, {
                        xc * de, -xc * nu * de * de, p2 + s1 + 2 * s2 * d2, nu * de + 2 * p1 * yc + 4 * p2 * xc, 2 * p1 * xc,
                        yc * de, -yc * nu * de * de, p1 + s3 + 2 * s4 * d2, 2 * p2 * yc, nu * de + 2 * p2 * xc + 4 * p1 * yc });

                    Mat_<double> dhdpC({ 5, 2 }, {
                        xc * (2 * k1 + 4 * k2 * d2 + 6 * k3 * d4), yc * (2 * k1 + 4 * k2 * d2 + 6 * k3 * d4),
                        xc * (2 * k4 + 4 * k5 * d2 + 6 * k6 * d4), yc * (2 * k4 + 4 * k5 * d2 + 6 * k6 * d4),
                        2 * xc, 2 * yc,
                        1, 0,
                        0, 1 });

                    Mat_<double> dpCdPC({ 2, 3 }, {
                        iz, 0, -xc * iz,
                        0, iz, -yc * iz });

                    if (!dpdt.empty()) dpdt.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC;
                    if (!dpdR.empty() && nRot == 9)
                    {
                        Mat_<double> dPCdR({ 3, 9 }, {
                            X, Y, Z, 0, 0, 0, 0, 0, 0,
                            0, 0, 0, X, Y, Z, 0, 0, 0,
                            0, 0, 0, 0, 0, 0, X, Y, Z });
                        dpdR.rowRange(2 * k, 2 * k + 2) = (dpDdpCD * dpCDdh * dhdpC * dpCdPC * dPCdR);
                    }
                    if (!dpdR.empty() && nRot == 3)
                    {
                        Mat_<double> _skewPC({ 3, 3 }, {
                            0, (z - tz), -(y - ty),
                            -(z - tz), 0, (x - tx),
                              (y - ty), -(x - tx), 0 });
                        dpdR.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC * _skewPC;
                    }
                }
            }

            return true;
            if (nRot == 9)
            {
                ceres::AutoDiffCostFunction<ProjectionModelRt, ceres::DYNAMIC, 9, 3> autoDeriv(new ProjectionModelRt(motionViewForDBG, nDist), motionViewForDBG.point3D.rows * 2);
                Mat_<double> dpdRDBG(point3D.rows * 2, 9);
                Mat_<double> dpdtDBG(point3D.rows * 2, 3);
                Mat_<Vec2d> err2DDBG(point3D.rows, 1);
                const double* parametersDBG[2] = { rot, t };
                double* jacobiansDBG[2] = { dpdRDBG.ptr<double>(), dpdtDBG.ptr<double>() };
                double* residualsDBG = err2DDBG.ptr<double>();
                autoDeriv.Evaluate(parametersDBG, residualsDBG, jacobiansDBG);

                if (!dpdR.empty()) cout << endl << norm(dpdR, dpdRDBG, NORM_INF) << endl;
                if (!dpdt.empty()) cout << endl << norm(dpdt, dpdtDBG, NORM_INF) << endl;
                cout << endl << norm(Mat_<Vec2d>(point2D.rows, 1, err2D), err2DDBG, NORM_INF) << endl;
                std::getchar();
            }
            return true;
        }
    };
};

class OptimizeKDRt
{
public:
    using MotionView = MotionSim::MotionView;
    static void TestMe(int argc = 0, char** argv = 0)
    {
        int N = 99;
        for (int k = 0; k < N; ++k)
        {
            //0.GenerateData
            bool world2D = k % 2;
            int rotMode = k % 7 + 1;
            MotionSim motionSim(false);
            motionSim.camFovX = 90;
            motionSim.camFovY = 90;
            motionSim.camRand = 10;
            motionSim.nMinMotion = 16;//2
            motionSim.nMaxMotion = 32;//4
            motionSim.rndSeek = false;
            static const int nDist = 5;
            motionSim.nCamDist = nDist;
            motionSim.runMotion(world2D, false, rotMode);
            //motionSim.visMotion();
            Mat_<double> rs0; for (size_t k = 0; k < motionSim.motionViews.size(); ++k) rs0.push_back(-motionSim.motionViews[k].r);
            Mat_<double> ts0; for (size_t k = 0; k < motionSim.motionViews.size(); ++k) ts0.push_back(-motionSim.motionViews[k].R.t() * motionSim.motionViews[k].t);
            Mat_<double> K0({ 4, 1 }, { motionSim.motionViews[0].K(0, 0), motionSim.motionViews[0].K(1, 1), motionSim.motionViews[0].K(0, 2), motionSim.motionViews[0].K(1, 2) });
            Mat_<double> D0 = motionSim.motionViews[0].D.clone();
            double errRatio = 0.9;
            double errRatioTrans = 0.99;

            //1.CalcByCeresWithAutoDeriv
            Mat_<double> Rs1; for (int k = 0; k < rs0.rows; k += 3) { Mat_<double> _R; cv::Rodrigues(rs0.rowRange(k, k + 3) * errRatio, _R); Rs1.push_back(_R); }
            Mat_<double> ts1 = ts0 * errRatioTrans;
            Mat_<double> K1 = K0 * errRatio;
            Mat_<double> D1 = D0 * errRatio;
            ceres::Problem problem1;
            for (int k = 0; k < motionSim.motionViews.size(); ++k)
                problem1.AddResidualBlock(new ceres::AutoDiffCostFunction<ProjectionModelKDRt, ceres::DYNAMIC, 9, 3, 4, nDist>(
                    new ProjectionModelKDRt(motionSim.motionViews[k], motionSim.nCamDist), motionSim.motionViews[k].point3D.rows * 2), NULL,
                    Rs1.ptr<double>(k * 3), ts1.ptr<double>(k * 3),
                    K1.ptr<double>(), D1.ptr<double>());
            for (int k = 0; k < motionSim.motionViews.size(); ++k) problem1.SetParameterization(Rs1.ptr<double>(k * 3), new OptimizeRt::LocalParamRWithGeneralJ);
            ceres::Solver::Options options;
            ceres::Solver::Summary summary;
            ceres::Solve(options, &problem1, &summary);
            Mat_<double> rs1; for (int k = 0; k < Rs1.rows; k += 3) { Mat_<double> _r; cv::Rodrigues(Rs1.rowRange(k, k + 3), _r); rs1.push_back(_r); }
            int nIter1 = (int)summary.iterations.size();

            //2.CalcByCeresWithGeneralJ
            Mat_<double> Rs2; for (int k = 0; k < rs0.rows; k += 3) { Mat_<double> _R; cv::Rodrigues(rs0.rowRange(k, k + 3) * errRatio, _R); Rs2.push_back(_R); }
            Mat_<double> ts2 = ts0 * errRatioTrans;
            Mat_<double> K2 = K0 * errRatio;
            Mat_<double> D2 = D0 * errRatio;
            ceres::Problem problem2;
            for (int k = 0; k < motionSim.motionViews.size(); ++k)
                problem2.AddResidualBlock(new ProjectionCostKDRt(motionSim.motionViews[k], motionSim.nCamDist, 9), NULL,
                    Rs2.ptr<double>(k * 3), ts2.ptr<double>(k * 3), K2.ptr<double>(), D2.ptr<double>());
            for (int k = 0; k < motionSim.motionViews.size(); ++k) problem2.SetParameterization(Rs2.ptr<double>(k * 3), new OptimizeRt::LocalParamRWithGeneralJ);
            ceres::Solver::Options options2;
            ceres::Solver::Summary summary2;
            ceres::Solve(options2, &problem2, &summary2);
            Mat_<double> rs2; for (int k = 0; k < Rs2.rows; k += 3) { Mat_<double> _r; cv::Rodrigues(Rs2.rowRange(k, k + 3), _r); rs2.push_back(_r); }
            int nIter2 = (int)summary2.iterations.size();

            //3.CalcByCeresWithIdentJ
            Mat_<double> rs3; for (int k = 0; k < rs0.rows; k += 3) rs3.push_back(rs0.rowRange(k, k + 3) * errRatio);
            Mat_<double> ts3 = ts0 * errRatioTrans;
            Mat_<double> K3 = K0 * errRatio;
            Mat_<double> D3 = D0 * errRatio;
            ceres::Problem problem3;
            for (int k = 0; k < motionSim.motionViews.size(); ++k)
                problem3.AddResidualBlock(new ProjectionCostKDRt(motionSim.motionViews[k], motionSim.nCamDist, 3), NULL,
                    rs3.ptr<double>(k * 3), ts3.ptr<double>(k * 3), K3.ptr<double>(), D3.ptr<double>());
            for (int k = 0; k < motionSim.motionViews.size(); ++k) problem3.SetParameterization(rs3.ptr<double>(k * 3), new OptimizeRt::LocalParamRWithIdentJ);
            ceres::Solver::Options options3;
            ceres::Solver::Summary summary3;
            ceres::Solve(options3, &problem3, &summary3);
            int nIter3 = (int)summary3.iterations.size();

            //4.AnalyzeError
            double infrs0rs0 = norm(rs0, rs0 * errRatio, NORM_INF);
            double infrs0rs1 = norm(rs0, rs1, NORM_INF);
            double infrs0rs2 = norm(rs0, rs2, NORM_INF);
            double infrs0rs3 = norm(rs0, rs3, NORM_INF);
            double infts0ts0 = norm(ts0, ts0 * errRatioTrans, NORM_INF);
            double infts0ts1 = norm(ts0, ts1, NORM_INF);
            double infts0ts2 = norm(ts0, ts2, NORM_INF);
            double infts0ts3 = norm(ts0, ts3, NORM_INF);
            double infK0K0 = norm(K0, K0 * errRatio, NORM_INF);
            double infK0K1 = norm(K0, K1, NORM_INF);
            double infK0K2 = norm(K0, K2, NORM_INF);
            double infK0K3 = norm(K0, K3, NORM_INF);
            double infD0D0 = norm(D0, D0 * errRatio, NORM_INF);
            double infD0D1 = norm(D0, D1, NORM_INF);
            double infD0D2 = norm(D0, D2, NORM_INF);
            double infD0D3 = norm(D0, D3, NORM_INF);

            //5.PrintError
            cout << fmt::format("LoopCount: {}      AutoDeriv.iters: {}      GeneralJ.iters: {}      IdentJ.iters: {}
", k, nIter1, nIter2, nIter3);
            if (infrs0rs1 > 1e-8 || infrs0rs2 > 1e-8 || infrs0rs3 > 1e-8 || infts0ts1 > 1e-8 || infts0ts2 > 1e-8 || infts0ts3 > 1e-8 ||
                infK0K1 > 1e-8 || infK0K2 > 1e-8 || infK0K3 > 1e-8 || infD0D1 > 1e-8 || infD0D2 > 1e-8 || infD0D3 > 1e-8)
            {
                cout << fmt::format("infrs0rs1: {:<15.9}		{:<15.9}
", infrs0rs1, infrs0rs0);
                cout << fmt::format("infrs0rs2: {:<15.9}		{:<15.9}
", infrs0rs2, infrs0rs0);
                cout << fmt::format("infrs0rs3: {:<15.9}		{:<15.9}
", infrs0rs3, infrs0rs0);
                cout << fmt::format("infts0ts1: {:<15.9}		{:<15.9}
", infts0ts1, infts0ts0);
                cout << fmt::format("infts0ts2: {:<15.9}		{:<15.9}
", infts0ts2, infts0ts0);
                cout << fmt::format("infts0ts3: {:<15.9}		{:<15.9}
", infts0ts3, infts0ts0);
                cout << fmt::format("infK0K1  : {:<15.9}		{:<15.9}
", infK0K1, infK0K0);
                cout << fmt::format("infK0K2  : {:<15.9}		{:<15.9}
", infK0K2, infK0K0);
                cout << fmt::format("infK0K3  : {:<15.9}		{:<15.9}
", infK0K3, infK0K0);
                cout << fmt::format("infD0D1  : {:<15.9}		{:<15.9}
", infD0D1, infD0D0);
                cout << fmt::format("infD0D2  : {:<15.9}		{:<15.9}
", infD0D2, infD0D0);
                cout << fmt::format("infD0D3  : {:<15.9}		{:<15.9}
", infD0D3, infD0D0);
                cout << "Press any key to continue" << endl; std::getchar();
            }
        }
    }

public:
    struct ProjectionModelKDRt
    {
        const int nDist;
        Mat_<Vec2d> point2D;
        Mat_<Vec3d> point3D;
        ProjectionModelKDRt(const MotionView& motionView0, const int nDist0) : nDist(nDist0)
        {
            point2D = motionView0.point2D;
            point3D = motionView0.point3D;
        }
        template <typename tp> bool operator()(const tp* const rot, const tp* const t, const tp* const K, const tp* const D, tp* errPoint2D) const
        {
            //1.Projection params
            tp fx = K[0];
            tp fy = K[1];
            tp cx = K[2];
            tp cy = K[3];

            //2.Distortion params
            tp k1 = D[0];
            tp k2 = D[1];
            tp p1 = D[2];
            tp p2 = D[3];
            tp k3, k4, k5, k6;
            tp s1, s2, s3, s4;
            if (nDist > 4) k3 = D[4];
            if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; }
            if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; }

            //3.Translation params
            tp tx = t[0];
            tp ty = t[1];
            tp tz = t[2];

            //4.
            tp R11, R12, R13, R21, R22, R23, R31, R32, R33;
            {
                R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
                R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
                R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
            }

            //5.ReProjection
            const Vec2d* data2D = point2D.ptr<Vec2d>();
            const Vec3d* data3D = point3D.ptr<Vec3d>();
            Vec<tp, 2>* err2D = (Vec<tp, 2>*)errPoint2D;
            for (int k = 0; k < point3D.rows; ++k)
            {
                //5.1 WorldCoordinate
                double X = data3D[k][0];
                double Y = data3D[k][1];
                double Z = data3D[k][2];

                //5.2 CameraCoordinate
                tp x = R11 * X + R12 * Y + R13 * Z + tx;
                tp y = R21 * X + R22 * Y + R23 * Z + ty;
                tp z = R31 * X + R32 * Y + R33 * Z + tz;

                //5.3 StandardPhysicsCoordinate
                tp iz = 1. / z; //if denominator==0
                tp xc = x * iz;
                tp yc = y * iz;

                //5.4 DistortionPhysicsCoordinate
                tp xc2 = xc * xc;
                tp yc2 = yc * yc;
                tp d2 = xc2 + yc2;
                tp xcyc = 2. * xc * yc;
                tp d4 = d2 * d2;
                tp d6 = d2 * d4;
                tp d2xc2 = d2 + 2. * xc2;
                tp d2yc2 = d2 + 2. * yc2;
                tp nu, de, xd, yd;
                if (nDist < 5)
                {
                    nu = 1. + k1 * d2 + k2 * d4;
                    xd = xc * nu + p1 * xcyc + p2 * d2xc2;
                    yd = yc * nu + p2 * xcyc + p1 * d2yc2;
                }
                else if (nDist < 8)
                {
                    nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
                    xd = xc * nu + p1 * xcyc + p2 * d2xc2;
                    yd = yc * nu + p2 * xcyc + p1 * d2yc2;
                }
                else if (nDist < 12)
                {
                    nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
                    de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
                    xd = xc * nu * de + p1 * xcyc + p2 * d2xc2;
                    yd = yc * nu * de + p2 * xcyc + p1 * d2yc2;
                }
                else if (nDist < 14)
                {
                    nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
                    de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
                    xd = xc * nu * de + p1 * xcyc + p2 * d2xc2 + s1 * d2 + s2 * d4;
                    yd = yc * nu * de + p2 * xcyc + p1 * d2yc2 + s3 * d2 + s4 * d4;
                }
                err2D[k][0] = xd * fx + cx - data2D[k][0];
                err2D[k][1] = yd * fy + cy - data2D[k][1];
            }
            return true;
        }
    };
    struct ProjectionCostKDRt : public ceres::CostFunction
    {
        const int nDist;
        Mat_<Vec2d> point2D;
        Mat_<Vec3d> point3D;
        const int nRot;
        const MotionView& motionViewForDBG;//Only used once
        ProjectionCostKDRt(const MotionView& motionView0, const int nDist0, const int nRot0) : nDist(nDist0), nRot(nRot0), motionViewForDBG(motionView0)
        {
            point2D = motionView0.point2D;
            point3D = motionView0.point3D;
            this->set_num_residuals(point2D.rows * 2);
            this->mutable_parameter_block_sizes()->push_back(nRot);
            this->mutable_parameter_block_sizes()->push_back(3);
            this->mutable_parameter_block_sizes()->push_back(4);
            this->mutable_parameter_block_sizes()->push_back(nDist);
        }
        bool Evaluate(double const* const* params, double* residuals, double** jacobians) const
        {
            //0.FormatInput
            const double* rot = params[0];
            const double* t = params[1];
            const double* K = params[2];
            const double* D = params[3];
            Vec2d* err2D = (Vec2d*)(residuals);
            Mat_<double> dpdR; if (jacobians && jacobians[0]) dpdR = Mat_<double>(point2D.rows * 2, nRot, jacobians[0]);
            Mat_<double> dpdt; if (jacobians && jacobians[1]) dpdt = Mat_<double>(point2D.rows * 2, 3, jacobians[1]);
            Mat_<double> dpdK; if (jacobians && jacobians[2]) dpdK = Mat_<double>(point2D.rows * 2, 4, jacobians[2]);
            Mat_<double> dpdD; if (jacobians && jacobians[3]) dpdD = Mat_<double>(point2D.rows * 2, nDist, jacobians[3]);

            //1.Projection params
            double fx = K[0];
            double fy = K[1];
            double cx = K[2];
            double cy = K[3];

            //2.Distortion params
            double k1 = D[0];
            double k2 = D[1];
            double p1 = D[2];
            double p2 = D[3];
            double k3 = 0, k4 = 0, k5 = 0, k6 = 0;
            double s1 = 0, s2 = 0, s3 = 0, s4 = 0;
            if (nDist > 4) k3 = D[4];
            if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; }
            if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; }

            //3.Translation params
            double tx = t[0];
            double ty = t[1];
            double tz = t[2];

            //4.Rotation params
            double R11, R12, R13, R21, R22, R23, R31, R32, R33;
            if (nRot == 9)
            {
                R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
                R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
                R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
            }
            if (nRot == 3)
            {
                Matx33d R; cv::Rodrigues(Matx31d(rot), R);
                rot = R.val;
                R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
                R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
                R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
                rot = params[0];
            }

            //5.ReProjection
            const Vec2d* data2D = point2D.ptr<Vec2d>();
            const Vec3d* data3D = point3D.ptr<Vec3d>();
            for (int k = 0; k < point3D.rows; ++k)
            {
                //5.1 WorldCoordinate
                double X = data3D[k][0];
                double Y = data3D[k][1];
                double Z = data3D[k][2];

                //5.2 CameraCoordinate
                double x = R11 * X + R12 * Y + R13 * Z + tx;
                double y = R21 * X + R22 * Y + R23 * Z + ty;
                double z = R31 * X + R32 * Y + R33 * Z + tz;

                //5.3 StandardPhysicsCoordinate
                double iz = 1. / z; //if denominator==0
                double xc = x * iz;
                double yc = y * iz;

                //5.4 DistortionPhysicsCoordinate
                double xc2 = xc * xc;
                double yc2 = yc * yc;
                double d2 = xc2 + yc2;
                double xcyc = 2. * xc * yc;
                double d4 = d2 * d2;
                double d6 = d2 * d4;
                double d2xc2 = d2 + 2. * xc2;
                double d2yc2 = d2 + 2. * yc2;
                double nu = 0, de = 1., xd = 0, yd = 0;
                if (nDist < 5)
                {
                    nu = 1. + k1 * d2 + k2 * d4;
                    xd = xc * nu + p1 * xcyc + p2 * d2xc2;
                    yd = yc * nu + p2 * xcyc + p1 * d2yc2;
                }
                else if (nDist < 8)
                {
                    nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
                    xd = xc * nu + p1 * xcyc + p2 * d2xc2;
                    yd = yc * nu + p2 * xcyc + p1 * d2yc2;
                }
                else if (nDist < 12)
                {
                    nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
                    de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
                    xd = xc * nu * de + p1 * xcyc + p2 * d2xc2;
                    yd = yc * nu * de + p2 * xcyc + p1 * d2yc2;
                }
                else if (nDist < 14)
                {
                    nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
                    de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
                    xd = xc * nu * de + p1 * xcyc + p2 * d2xc2 + s1 * d2 + s2 * d4;
                    yd = yc * nu * de + p2 * xcyc + p1 * d2yc2 + s3 * d2 + s4 * d4;
                }
                err2D[k][0] = xd * fx + cx - data2D[k][0];
                err2D[k][1] = yd * fy + cy - data2D[k][1];
                if (!dpdR.empty() || !dpdt.empty())
                {
                    Mat_<double> dpDdpCD({ 2, 2 }, { fx, 0, 0, fy });

                    Mat_<double> dpCDdh({ 2, 5 }, {
                        xc * de, -xc * nu * de * de, p2 + s1 + 2 * s2 * d2, nu * de + 2 * p1 * yc + 4 * p2 * xc, 2 * p1 * xc,
                        yc * de, -yc * nu * de * de, p1 + s3 + 2 * s4 * d2, 2 * p2 * yc, nu * de + 2 * p2 * xc + 4 * p1 * yc });

                    Mat_<double> dhdpC({ 5, 2 }, {
                        xc * (2 * k1 + 4 * k2 * d2 + 6 * k3 * d4), yc * (2 * k1 + 4 * k2 * d2 + 6 * k3 * d4),
                        xc * (2 * k4 + 4 * k5 * d2 + 6 * k6 * d4), yc * (2 * k4 + 4 * k5 * d2 + 6 * k6 * d4),
                        2 * xc, 2 * yc,
                        1, 0,
                        0, 1 });

                    Mat_<double> dpCdPC({ 2, 3 }, {
                        iz, 0, -xc * iz,
                        0, iz, -yc * iz });

                    if (!dpdt.empty()) dpdt.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC;
                    if (!dpdR.empty() && nRot == 9)
                    {
                        Mat_<double> dPCdR({ 3, 9 }, {
                            X, Y, Z, 0, 0, 0, 0, 0, 0,
                            0, 0, 0, X, Y, Z, 0, 0, 0,
                            0, 0, 0, 0, 0, 0, X, Y, Z });
                        dpdR.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC * dPCdR;
                    }
                    if (!dpdR.empty() && nRot == 3)
                    {
                        Mat_<double> _skewPC({ 3, 3 }, {
                            0, (z - tz), -(y - ty),
                            -(z - tz), 0, (x - tx),
                              (y - ty), -(x - tx), 0 });
                        dpdR.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC * _skewPC;
                    }
                }
                if (!dpdD.empty())
                {
                    Mat_<double> dpDdpCD(2, 2);
                    dpDdpCD << fx, 0, 0, fy;

                    Mat_<double> dpCDdD(2, nDist);
                    if (nDist < 5)
                    {
                        dpCDdD <<
                            xc * d2 * de, xc* d4* de, xcyc, d2xc2,
                            yc* d2* de, yc* d4* de, d2yc2, xcyc;
                    }
                    else if (nDist < 8)
                    {
                        dpCDdD <<
                            xc * d2 * de, xc* d4* de, xcyc, d2xc2, xc* d6* de,
                            yc* d2* de, yc* d4* de, d2yc2, xcyc, yc* d6* de;
                    }
                    else if (nDist < 12)
                    {
                        dpCDdD <<
                            xc * d2 * de, xc* d4* de, xcyc, d2xc2, xc* d6* de, -xc * d2 * nu * de * de, -xc * d4 * nu * de * de, -xc * d6 * nu * de * de,
                            yc* d2* de, yc* d4* de, d2yc2, xcyc, yc* d6* de, -yc * d2 * nu * de * de, -yc * d4 * nu * de * de, -yc * d6 * nu * de * de;
                    }
                    else if (nDist < 14)
                    {
                        dpCDdD <<
                            xc * d2 * de, xc* d4* de, xcyc, d2xc2, xc* d6* de, -xc * d2 * nu * de * de, -xc * d4 * nu * de * de, -xc * d6 * nu * de * de, d2, d4, 0, 0,
                            yc* d2* de, yc* d4* de, d2yc2, xcyc, yc* d6* de, -yc * d2 * nu * de * de, -yc * d4 * nu * de * de, -yc * d6 * nu * de * de, 0, 0, d2, d4;
                    }
                    dpdD.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdD;
                }
                if (!dpdK.empty()) Mat_<double>({ 2, 4 }, { xd, 0, 1, 0, 0, yd, 0, 1 }).copyTo(dpdK.rowRange(2 * k, 2 * k + 2));
            }

            return true;
            if (nRot == 9)
            {
                static const int nDist_ = 5; // set this value as nDist
                ceres::AutoDiffCostFunction<ProjectionModelKDRt, ceres::DYNAMIC, 9, 3, 4, nDist_> autoDeriv(new ProjectionModelKDRt(motionViewForDBG, nDist), motionViewForDBG.point3D.rows * 2);
                Mat_<double> dpdRDBG(point3D.rows * 2, 9);
                Mat_<double> dpdtDBG(point3D.rows * 2, 3);
                Mat_<double> dpdKDBG(point3D.rows * 2, 4);
                Mat_<double> dpdDDBG(point3D.rows * 2, nDist);
                Mat_<Vec2d> err2DDBG(point3D.rows, 1);
                const double* parametersDBG[4] = { rot, t, K, D };
                double* jacobiansDBG[4] = { dpdRDBG.ptr<double>(), dpdtDBG.ptr<double>(), dpdKDBG.ptr<double>(), dpdDDBG.ptr<double>() };
                double* residualsDBG = err2DDBG.ptr<double>();
                autoDeriv.Evaluate(parametersDBG, residualsDBG, jacobiansDBG);

                if (!dpdR.empty()) cout << endl << norm(dpdR, dpdRDBG, NORM_INF) << endl;
                if (!dpdt.empty()) cout << endl << norm(dpdt, dpdtDBG, NORM_INF) << endl;
                if (!dpdK.empty()) cout << endl << norm(dpdK, dpdKDBG, NORM_INF) << endl;
                if (!dpdD.empty()) cout << endl << norm(dpdD, dpdDDBG, NORM_INF) << endl;
                cout << endl << norm(Mat_<Vec2d>(point2D.rows, 1, err2D), err2DDBG, NORM_INF) << endl;
                std::getchar();
            }
            return true;
        }
    };
};

class OptimizeRtWithNormlizedPoint2D
{
public:
    using MotionView = MotionSim::MotionView;
    static void TestMe(int argc = 0, char** argv = 0)
    {
        int N = 99;
        for (int k = 0; k < N; ++k)
        {
            //0.GenerateData
            bool world2D = k % 2;
            int rotMode = k % 7 + 1;
            MotionSim motionSim(false);
            motionSim.camFovX = 90;
            motionSim.camFovY = 90;
            motionSim.camRand = 10;
            motionSim.nMinMotion = 16;//2
            motionSim.nMaxMotion = 32;//4
            motionSim.rndSeek = false;
            motionSim.nCamDist = 5;
            motionSim.runMotion(world2D, false, rotMode);
            //motionSim.visMotion();
            int rndInd = int(motionSim.motionViews.size() * cv::randu<double>());
            Mat_<double> r0 = -motionSim.motionViews[rndInd].r;
            Mat_<double> t0 = -motionSim.motionViews[rndInd].R.t() * motionSim.motionViews[rndInd].t;
            const MotionView& motionView = motionSim.motionViews[rndInd];
            double errRatio = 0.9;

            //1.CalcByCeresWithAutoDeriv
            Mat_<double> R1; cv::Rodrigues(r0 * errRatio, R1);
            Mat_<double> t1 = t0 * errRatio;
            ceres::Problem problem1;
            problem1.AddResidualBlock(new ceres::AutoDiffCostFunction<RotationModelRt, ceres::DYNAMIC, 9, 3>(
                new RotationModelRt(motionView), motionView.point3D.rows * 2), NULL, R1.ptr<double>(), t1.ptr<double>());
            problem1.SetParameterization(R1.ptr<double>(), new OptimizeRt::LocalParamRWithGeneralJ);
            ceres::Solver::Options options1;
            ceres::Solver::Summary summary1;
            ceres::Solve(options1, &problem1, &summary1);
            Mat_<double> r1; cv::Rodrigues(R1, r1);
            int nIter1 = (int)summary1.iterations.size();

            //2.CalcByCeresWithGeneralJ
            Mat_<double> R2; cv::Rodrigues(r0 * errRatio, R2);
            Mat_<double> t2 = t0 * errRatio;
            ceres::Problem problem2;
            problem2.AddResidualBlock(new RotationCostRt(motionView, motionSim.nCamDist, 9), NULL, R2.ptr<double>(), t2.ptr<double>());
            problem2.SetParameterization(R2.ptr<double>(), new OptimizeRt::LocalParamRWithGeneralJ);
            ceres::Solver::Options options2;
            ceres::Solver::Summary summary2;
            ceres::Solve(options2, &problem2, &summary2);
            Mat_<double> r2; cv::Rodrigues(R2, r2);
            int nIter2 = (int)summary2.iterations.size();

            //3.CalcByCeresWithGeneralJ
            Mat_<double> r3 = r0 * errRatio;
            Mat_<double> t3 = t0 * errRatio;
            ceres::Problem problem3;
            problem3.AddResidualBlock(new RotationCostRt(motionView, motionSim.nCamDist, 3), NULL, r3.ptr<double>(), t3.ptr<double>());
            problem3.SetParameterization(r3.ptr<double>(), new OptimizeRt::LocalParamRWithIdentJ);
            ceres::Solver::Options options3;
            ceres::Solver::Summary summary3;
            ceres::Solve(options3, &problem3, &summary3);
            int nIter3 = (int)summary3.iterations.size();

            //4.AnalyzeError
            double infr0r0 = norm(r0, r0 * errRatio, NORM_INF);
            double infr0r1 = norm(r0, r1, NORM_INF);
            double infr0r2 = norm(r0, r2, NORM_INF);
            double infr0r3 = norm(r0, r3, NORM_INF);
            double inft0t0 = norm(t0, t0 * errRatio, NORM_INF);
            double inft0t1 = norm(t0, t1, NORM_INF);
            double inft0t2 = norm(t0, t2, NORM_INF);
            double inft0t3 = norm(t0, t3, NORM_INF);

            //5.PrintError
            cout << fmt::format("LoopCount: {}      AutoDeriv.iters: {}      GeneralJ.iters: {}      IdentJ.iters: {}
", k, nIter1, nIter2, nIter3);
            if (infr0r1 > 1e-8 || infr0r2 > 1e-8 || infr0r3 > 1e-8 || inft0t1 > 1e-8 || inft0t2 > 1e-8 || inft0t3 > 1e-8)
            {
                cout << fmt::format("infr0r1: {:<15.9}		{:<15.9}
", infr0r1, infr0r0);
                cout << fmt::format("infr0r2: {:<15.9}		{:<15.9}
", infr0r2, infr0r0);
                cout << fmt::format("infr0r3: {:<15.9}		{:<15.9}
", infr0r3, infr0r0);
                cout << fmt::format("inft0t1: {:<15.9}		{:<15.9}
", inft0t1, inft0t0);
                cout << fmt::format("inft0t2: {:<15.9}		{:<15.9}
", inft0t2, inft0t0);
                cout << fmt::format("inft0t3: {:<15.9}		{:<15.9}
", inft0t3, inft0t0);
                cout << "Press any key to continue" << endl; std::getchar();
            }
        }
    }

public:
    struct RotationModelRt
    {
        Mat_<Vec2d> point2D;
        Mat_<Vec3d> point3D;
        RotationModelRt(const MotionView& motionView0)
        {
            point3D = motionView0.point3D;
            cv::projectPoints(point3D, -motionView0.r, -motionView0.R.t() * motionView0.t, Mat_<double>::eye(3, 3), Mat_<double>::zeros(motionView0.D.rows, 1), point2D);
            //cv::undistortPoints(motionView0.point2D, point2D, motionView0.K, motionView0.D);
        }
        template <typename tp> bool operator()(const tp* const rot, const tp* const t, tp* errPoint2D) const
        {
            //1.Translation params
            tp tx = t[0];
            tp ty = t[1];
            tp tz = t[2];

            //2.Rotation params
            tp R11, R12, R13, R21, R22, R23, R31, R32, R33;
            {
                R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
                R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
                R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
            }

            //3.ReProjection
            const Vec2d* data2D = point2D.ptr<Vec2d>();
            const Vec3d* data3D = point3D.ptr<Vec3d>();
            Vec<tp, 2>* err2D = (Vec<tp, 2>*)errPoint2D;
            for (int k = 0; k < point3D.rows; ++k)
            {
                //3.1 WorldCoordinate
                double X = data3D[k][0];
                double Y = data3D[k][1];
                double Z = data3D[k][2];

                //3.2 CameraCoordinate
                tp x = R11 * X + R12 * Y + R13 * Z + tx;
                tp y = R21 * X + R22 * Y + R23 * Z + ty;
                tp z = R31 * X + R32 * Y + R33 * Z + tz;

                //3.3 StandardPhysicsCoordinate
                tp iz = 1. / z; //if denominator==0
                tp xc = x * iz;
                tp yc = y * iz;
                err2D[k][0] = xc - data2D[k][0];
                err2D[k][1] = yc - data2D[k][1];
            }
            return true;
        }
    };
    struct RotationCostRt : public ceres::CostFunction
    {
        const int nDist;
        double K[4];
        const double* D;
        Mat_<Vec2d> point2D;
        Mat_<Vec3d> point3D;
        const int nRot;
        const MotionView& motionViewForDBG;//only use once
        RotationCostRt(const MotionView& motionView0, const int nDist0, const int nRot0) : nDist(nDist0), nRot(nRot0), motionViewForDBG(motionView0)
        {
            K[0] = motionView0.K(0, 0);
            K[1] = motionView0.K(1, 1);
            K[2] = motionView0.K(0, 2);
            K[3] = motionView0.K(1, 2);
            D = motionView0.D.ptr<double>();
            point2D = motionView0.point2D;
            point3D = motionView0.point3D;
            this->set_num_residuals(point2D.rows * 2);
            this->mutable_parameter_block_sizes()->push_back(nRot);
            this->mutable_parameter_block_sizes()->push_back(3);
            cv::projectPoints(point3D, -motionView0.r, -motionView0.R.t() * motionView0.t, Mat_<double>::eye(3, 3), Mat_<double>::zeros(motionView0.D.rows, 1), point2D);
            //cv::undistortPoints(motionView0.point2D, point2D, motionView0.K, motionView0.D);
        }
        bool Evaluate(double const* const* params, double* residuals, double** jacobians) const
        {
            //0.FormatInput
            const double* rot = params[0];
            const double* t = params[1];
            Vec2d* err2D = (Vec2d*)(residuals);
            Mat_<double> dpdR; if (jacobians && jacobians[0]) dpdR = Mat_<double>(point2D.rows * 2, nRot, jacobians[0]);
            Mat_<double> dpdt; if (jacobians && jacobians[1]) dpdt = Mat_<double>(point2D.rows * 2, 3, jacobians[1]);

            //1.Translation params
            double tx = t[0];
            double ty = t[1];
            double tz = t[2];

            //2.Rotation params
            double R11, R12, R13, R21, R22, R23, R31, R32, R33;
            if (nRot == 9)
            {
                R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
                R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
                R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
            }
            if (nRot == 3)
            {
                Matx33d R; cv::Rodrigues(Matx31d(rot), R);
                rot = R.val;
                R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
                R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
                R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
                rot = params[0];
            }

            //3.ReProjection
            const Vec2d* data2D = point2D.ptr<Vec2d>();
            const Vec3d* data3D = point3D.ptr<Vec3d>();
            for (int k = 0; k < point3D.rows; ++k)
            {
                //5.1 WorldCoordinate
                double X = data3D[k][0];
                double Y = data3D[k][1];
                double Z = data3D[k][2];

                //5.2 CameraCoordinate
                double x = R11 * X + R12 * Y + R13 * Z + tx;
                double y = R21 * X + R22 * Y + R23 * Z + ty;
                double z = R31 * X + R32 * Y + R33 * Z + tz;

                //5.3 StandardPhysicsCoordinate
                double iz = 1. / z; //if denominator==0
                double xc = x * iz;
                double yc = y * iz;
                err2D[k][0] = xc - data2D[k][0];
                err2D[k][1] = yc - data2D[k][1];
                if (!dpdR.empty() || !dpdt.empty())
                {
                    Mat_<double> dpCdPC({ 2, 3 }, {
                        iz, 0, -xc * iz,
                        0, iz, -yc * iz });
                    if (!dpdt.empty()) dpCdPC.copyTo(dpdt.rowRange(2 * k, 2 * k + 2));
                    if (!dpdR.empty() && nRot == 9)
                    {
                        Mat_<double> dPCdR({ 3, 9 }, {
                            X, Y, Z, 0, 0, 0, 0, 0, 0,
                            0, 0, 0, X, Y, Z, 0, 0, 0,
                            0, 0, 0, 0, 0, 0, X, Y, Z });
                        dpdR.rowRange(2 * k, 2 * k + 2) = dpCdPC * dPCdR;
                    }
                    if (!dpdR.empty() && nRot == 3)
                    {
                        Mat_<double> _skewPC({ 3, 3 }, {
                            0, (z - tz), -(y - ty),
                            -(z - tz), 0, (x - tx),
                              (y - ty), -(x - tx), 0 });
                        dpdR.rowRange(2 * k, 2 * k + 2) = dpCdPC * _skewPC;
                    }
                }
            }

            return true;
            if (nRot == 9)
            {
                ceres::AutoDiffCostFunction<RotationModelRt, ceres::DYNAMIC, 9, 3> autoDeriv(new RotationModelRt(motionViewForDBG), motionViewForDBG.point3D.rows * 2);
                Mat_<double> dpdRDBG(point3D.rows * 2, 9);
                Mat_<double> dpdtDBG(point3D.rows * 2, 3);
                Mat_<Vec2d> err2DDBG(point3D.rows, 1);
                const double* parametersDBG[2] = { rot, t };
                double* jacobiansDBG[2] = { dpdRDBG.ptr<double>(), dpdtDBG.ptr<double>() };
                double* residualsDBG = err2DDBG.ptr<double>();
                autoDeriv.Evaluate(parametersDBG, residualsDBG, jacobiansDBG);

                if (!dpdR.empty()) cout << endl << norm(dpdR, dpdRDBG, NORM_INF) << endl;
                if (!dpdt.empty()) cout << endl << norm(dpdt, dpdtDBG, NORM_INF) << endl;
                cout << endl << norm(Mat_<Vec2d>(point2D.rows, 1, err2D), err2DDBG, NORM_INF) << endl;
                std::getchar();
            }
            return true;
        }
    };
};

int main(int argc, char** argv) { OptimizeRtWithNormlizedPoint2D::TestMe(argc, argv); return 0; }
int main1(int argc, char** argv) { OptimizeRt::TestMe(argc, argv); return 0; }
int main2(int argc, char** argv) { OptimizeKDRt::TestMe(argc, argv); return 0; }
int main3(int argc, char** argv) { OptimizeRtWithNormlizedPoint2D::TestMe(argc, argv); return 0; }