#include <fstream>
#include <numeric> // std::accumulate
#include <thread>
#include <opencv2/core/core.hpp>
#ifdef USE_EIGEN
    #include <Eigen/Dense>
    #include <opencv2/core/eigen.hpp>
#endif
#include <openpose/3d/cameraParameterReader.hpp>
#include <openpose/calibration/gridPatternFunctions.hpp>
#include <openpose/filestream/fileStream.hpp>
#include <openpose/utilities/fileSystem.hpp>
#include <openpose/calibration/cameraParameterEstimation.hpp>

namespace op
{
    // Private variables
    const long double PI = 3.141592653589793238462643383279502884197169399375105820974944592307816406286208998628034825342117067982148086513282306647093844;

    // Private functions
    struct Intrinsics
    {
        cv::Mat cameraMatrix;
        cv::Mat distortionCoefficients;

        Intrinsics() :
            cameraMatrix(cv::Mat::eye(3, 3, CV_64F)),
            distortionCoefficients(cv::Mat::zeros(14, 1, CV_64F))
        {
        }

        Intrinsics(const cv::Mat& cameraMatrix, const cv::Mat& distortionCoefficients) :
            cameraMatrix{cameraMatrix.clone()}, // cv::Mat::eye(3, 3, CV_64F)
            distortionCoefficients{distortionCoefficients.clone()} // cv::Mat::zeros(14||12||8||5, 1, CV_64F)
        {
        }

        bool empty()
        {
            return cameraMatrix.empty() || distortionCoefficients.empty();
        }
    };

    #ifdef USE_EIGEN
        struct Extrinsics
        {
            Eigen::Matrix3d Rotation;
            Eigen::Vector3d translationMm;
            std::vector<cv::Point2f> points2DVector;
            std::vector<cv::Point3f> objects3DVector;

            Extrinsics()
            {
                Rotation.setIdentity();
                translationMm.setZero();
            }
        };
    #endif

    std::vector<std::string> getImagePaths(const std::string& imageDirectoryPath)
    {
        try
        {
            // Get files on directory with the desired extensions
            const std::vector<std::string> extensions{
                // Completely supported by OpenCV
                "bmp", "dib", "pbm", "pgm", "ppm", "sr", "ras",
                // Most of them supported by OpenCV
                "jpg", "jpeg", "png"};
            const auto imagePaths = getFilesOnDirectory(imageDirectoryPath, extensions);
            // Check #files > 0
            if (imagePaths.empty())
                error("No images were found on `" + imageDirectoryPath + "`.", __LINE__, __FUNCTION__, __FILE__);
            // Return result
            return imagePaths;
        }
        catch (const std::exception& e)
        {
            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
            return {};
        }
    }

    std::vector<std::pair<cv::Mat, std::string>> getImageAndPaths(const std::string& imageDirectoryPath)
    {
        try
        {
            // Get images on directory
            const auto imagePaths = getImagePaths(imageDirectoryPath);
            // Check #files > 0
            if (imagePaths.empty())
                error("No images were found on `" + imageDirectoryPath + "`.", __LINE__, __FUNCTION__, __FILE__);
            // Read images
            std::vector<std::pair<cv::Mat, std::string>> imageAndPaths;
            for (const auto& imagePath : imagePaths)
            {
                imageAndPaths.emplace_back(std::make_pair(cv::imread(imagePath, CV_LOAD_IMAGE_COLOR), imagePath));
                if (imageAndPaths.back().first.empty())
                    error("Image could not be opened from path `" + imagePath + "`.",
                          __LINE__, __FUNCTION__, __FILE__);
            }
            // Return result
            return imageAndPaths;
        }
        catch (const std::exception& e)
        {
            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
            return {};
        }
    }

    std::pair<double, std::vector<double>> calcReprojectionErrors(const std::vector<std::vector<cv::Point3f>>& objects3DVectors,
                                                                  const std::vector<std::vector<cv::Point2f>>& points2DVectors,
                                                                  const std::vector<cv::Mat>& rVecs,
                                                                  const std::vector<cv::Mat>& tVecs,
                                                                  const Intrinsics& intrinsics)
    {
        try
        {
            double reprojectionError;
            std::vector<double> perViewErrors(objects3DVectors.size());

            std::vector<cv::Point2f> points2DVectors2;
            unsigned long long totalPoints = 0;
            double totalErr = 0;

            for (auto i = 0ull; i < objects3DVectors.size(); ++i )
            {
                cv::projectPoints(cv::Mat(objects3DVectors.at(i)),
                                  rVecs.at(i),
                                  tVecs.at(i),
                                  intrinsics.cameraMatrix,
                                  intrinsics.distortionCoefficients,
                                  points2DVectors2);
                const auto err = cv::norm(cv::Mat(points2DVectors.at(i)), cv::Mat(points2DVectors2), CV_L2);

                const auto n = objects3DVectors.at(i).size();
                perViewErrors.at(i) = {std::sqrt(err*err/n)};
                totalErr        += err*err;
                totalPoints     += n;
            }

            reprojectionError = {std::sqrt(totalErr/totalPoints)};

            return std::make_pair(reprojectionError, perViewErrors);
        }
        catch (const std::exception& e)
        {
            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
            return std::make_pair(0., std::vector<double>{});
        }
    }

    Intrinsics calcIntrinsicParameters(const cv::Size& imageSize,
                                       const std::vector<std::vector<cv::Point2f>>& points2DVectors,
                                       const std::vector<std::vector<cv::Point3f>>& objects3DVectors,
                                       const int calibrateCameraFlags)
    {
        try
        {
            log("\nCalibrating camera (intrinsics) with points from " + std::to_string(points2DVectors.size())
                + " images...", Priority::High);

            //Find intrinsic and extrinsic camera parameters
            Intrinsics intrinsics;
            std::vector<cv::Mat> rVecs;
            std::vector<cv::Mat> tVecs;
            const auto rms = cv::calibrateCamera(objects3DVectors, points2DVectors, imageSize, intrinsics.cameraMatrix,
                                                 intrinsics.distortionCoefficients, rVecs, tVecs,
                                                 calibrateCameraFlags);

            // cv::checkRange checks that every array element is neither NaN nor infinite
            const auto calibrationIsCorrect = cv::checkRange(intrinsics.cameraMatrix)
                                            && cv::checkRange(intrinsics.distortionCoefficients);
            if (!calibrationIsCorrect)
                error("Unvalid cameraMatrix and/or distortionCoefficients.", __LINE__, __FUNCTION__, __FILE__);

            double totalAvgErr;
            std::vector<double> reprojectionErrors;
            std::tie(totalAvgErr, reprojectionErrors) = calcReprojectionErrors(objects3DVectors, points2DVectors,
                                                                               rVecs, tVecs, intrinsics);

            log("\nIntrinsics:", Priority::High);
            log("Re-projection error - cv::calibrateCamera vs. calcReprojectionErrors:\t" + std::to_string(rms)
                + " vs. " + std::to_string(totalAvgErr), Priority::High);
            log("Intrinsics_K:", Priority::High);
            log(intrinsics.cameraMatrix, Priority::High);
            log("Intrinsics_distCoeff:", Priority::High);
            log(intrinsics.distortionCoefficients, Priority::High);
            log(" ", Priority::High);

            return intrinsics;
        }
        catch (const std::exception& e)
        {
            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
            return Intrinsics{};
        }
    }

    double setAngleInRangeZeroTwoPi(const double angle)
    {
        try
        {
            // Return angle in range [0,2pi)
            return std::fmod(angle, 360);    // floating-point remainder
            // double result{angle};

            // Return angle in range [0,2pi)
            // const auto twoPi = 2 * PI;
            // while (result >= 2*PI)
            //     result -= twoPi;
            // while (result < 0)
            //     result += twoPi;

            // return result;
        }
        catch (const std::exception& e)
        {
            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
            return 0;
        }
    }

    double setAngleInRangePlusMinusPi(const double angle)
    {
        try
        {
            auto result = setAngleInRangeZeroTwoPi(angle);

            // Return angle in range (-pi,pi]
            const auto twoPi = 2 * PI;
            if (result > PI)
                result -= twoPi;

            // // Return angle in range (-pi,pi]
            // const auto twoPi = 2 * PI;
            // while (result > PI)
            //     result -= twoPi;
            // while (result <= -PI)
            //     result += twoPi;

            return result;
        }
        catch (const std::exception& e)
        {
            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
            return 0;
        }
    }

    #ifdef USE_EIGEN
        cv::Mat getRodriguesVector(const Eigen::Matrix3d& rotationMatrix)
        {
            try
            {
                // Rotation matrix as cv::Mat
                cv::Mat rotationMatrixCv;
                cv::eigen2cv(rotationMatrix, rotationMatrixCv);
                // Rotation as vector
                cv::Mat rotationVector;
                cv::Rodrigues(rotationMatrixCv, rotationVector);
                return rotationVector;
            }
            catch (const std::exception& e)
            {
                error(e.what(), __LINE__, __FUNCTION__, __FILE__);
                return cv::Mat();
            }
        }

        std::pair<bool, double> estimateAverageAngle(const std::vector<double>& angles)
        {
            try
            {
                // Idea:
                    // Average(-40, 40) = 0
                    // Average(-179, 179) = 180 (not 0!)
                    // Average(90, 270) = 0 || 180??? We will assume outliers and return false

                // Security checks
                if (angles.empty())
                    error("Variables `angles` is empty.", __LINE__, __FUNCTION__, __FILE__);
                auto resultIsOK = true;
                double average = 0;

                // angles in range (-pi, pi]
                auto anglesNormalized = angles;
                for (auto& angle : anglesNormalized)
                    angle = setAngleInRangePlusMinusPi(angle);

                // If the difference between them is > 180 degrees, then we turn them in the range [0, 360) (so
                // now the difference between them is < 180) and return the traditional average. Examples:
                //     - If one in range [90, 180] and the other in range (-180, -90]
                //     - If one is 179 degrees and the other one -179 degrees -> average should be 180 no 0! So:
                //       [179 + (360-179)] / 2 = 180
                //     - Etc.
                // Math equivalent:
                //     We want:         ( maxAngle + (minAngle + 360) )   /2
                //     Equivalent to:   ( maxAngle + minAngle + 360 )   /2     =   (maxAngle + minAngle) / 2 + 180
                //                      =   (angleA + angleB) / 2 + 180,   in radians: (angleA + angleB) / 2 + PI
                auto minElement = *std::min_element(anglesNormalized.begin(), anglesNormalized.end());
                auto maxElement = *std::max_element(anglesNormalized.begin(), anglesNormalized.end());
                if (maxElement - minElement >= PI)
                    for (auto& angle : anglesNormalized)
                        angle = setAngleInRangeZeroTwoPi(angle);

                // If after normalizing range between min and max is still >180 degrees --> there are outliers
                minElement = *std::min_element(anglesNormalized.begin(), anglesNormalized.end());
                maxElement = *std::max_element(anglesNormalized.begin(), anglesNormalized.end());
                if (maxElement - minElement >= PI)
                {
                    resultIsOK = {false};
                    log("There are outliers in the angles.", Priority::High);
                }

                // If the difference between them is <= 180 degrees, then we just return the traditional average.
                // Examples:
                //     - If both have the same signs, i.e. both in range [0, 180] or both in range (-180, 0)
                //     - If one in range [0, 90] and the other in range [-90, 0]
                //     - Etc.
                average = std::accumulate(anglesNormalized.begin(), anglesNormalized.end(), 0.)
                        / (double)anglesNormalized.size();
                average = setAngleInRangePlusMinusPi(average);

                return std::make_pair(resultIsOK, average);
            }
            catch (const std::exception& e)
            {
                error(e.what(), __LINE__, __FUNCTION__, __FILE__);
                return std::make_pair(false, 0.);
            }
        }

        Eigen::Matrix4d getMAverage(const std::vector<Eigen::Matrix4d>& MsToAverage,
                                    const Eigen::Matrix4d& noisyApproximatedM = Eigen::Matrix4d::Zero())
        {
            try
            {
                auto MsToAverageRobust = MsToAverage;
                // Clean noisy outputs
                if (noisyApproximatedM.norm() > 0.1)
                {
                    MsToAverageRobust.clear();
                    for (const auto& matrix : MsToAverage)
                    {
                        bool addElement = true;
                        for (auto col = 0 ; col < 3 ; col++)
                        {
                            for (auto row = 0 ; row < 3 ; row++)
                            {
                                if (std::abs(matrix(col, row) - noisyApproximatedM(col, row)) > 0.25)
                                {
                                    addElement = false;
                                    break;
                                }
                            }
                        }
                        if (addElement)
                            MsToAverageRobust.push_back(matrix);
                    }
                }

                // Projection matrix
                Eigen::Matrix4d averagedProjectionMatrix;
                averagedProjectionMatrix.setIdentity();

                // Average translation
                for (const auto& matrix : MsToAverageRobust)
                    averagedProjectionMatrix.block<3,1>(0,3) += matrix.block<3,1>(0,3);
                averagedProjectionMatrix.block<3,1>(0,3) /= (double)MsToAverageRobust.size();

                // Average rotation
                std::array<std::vector<double>, 3> rotationVectors;
                for (const auto& matrix : MsToAverageRobust)
                {
                    const auto rVec = getRodriguesVector(matrix.block<3,3>(0,0));
                    for (auto i = 0u ; i < rotationVectors.size() ; i++)
                        rotationVectors.at(i).emplace_back(rVec.at<double>(i,0));
                }
                cv::Mat rotationVector = cv::Mat::zeros(3, 1, CV_64F);
                for (auto i = 0u ; i < rotationVectors.size() ; i++)
                {
                    const auto pairAverageAngle = estimateAverageAngle(rotationVectors.at(i));
                    if (!pairAverageAngle.first)
                        log("Outlies in the result. Something went wrong when estimating the average of different"
                              " projection matrices.", Priority::High);
                    rotationVector.at<double>(i,0) = {pairAverageAngle.second};
                }
                cv::Mat rotationMatrix;
                cv::Rodrigues(rotationVector, rotationMatrix);
                Eigen::Matrix3d rotEigen;
                cv::cv2eigen(rotationMatrix, rotEigen);
                averagedProjectionMatrix.block<3,3>(0,0) = rotEigen;

                // Result
                return averagedProjectionMatrix;
            }
            catch (const std::exception& e)
            {
                error(e.what(), __LINE__, __FUNCTION__, __FILE__);
                return Eigen::Matrix4d{};
            }
        }

        Eigen::Matrix4d getMFromRt(const Eigen::Matrix3d& Rot, const Eigen::Vector3d& trans)
        {
            try
            {
                // projectionMatrix
                Eigen::Matrix4d projectionMatrix = Eigen::Matrix4d::Identity();
                projectionMatrix.block<3,3>(0,0) = Rot;
                projectionMatrix.block<3,1>(0,3) = trans;
                return projectionMatrix;
            }
            catch (const std::exception& e)
            {
                error(e.what(), __LINE__, __FUNCTION__, __FILE__);
                return Eigen::Matrix4d{};
            }
        }

        Eigen::Matrix4d getMFromRt(const cv::Mat& Rot, const cv::Mat& trans)
        {
            try
            {
                if (Rot.cols != 3 || Rot.rows != 3)
                    error("Rotation matrix does not have 3 cols and/or 3 rows.", __LINE__, __FUNCTION__, __FILE__);
                if (trans.cols != 1 || trans.rows != 3)
                    error("Translation vector does not have 1 col and/or 3 rows.", __LINE__, __FUNCTION__, __FILE__);

                Eigen::Matrix3d RotEigen;
                cv::cv2eigen(Rot, RotEigen);
                Eigen::Vector3d transEigen;
                cv::cv2eigen(trans, transEigen);

                // projectionMatrix
                return getMFromRt(RotEigen, transEigen);
            }
            catch (const std::exception& e)
            {
                error(e.what(), __LINE__, __FUNCTION__, __FILE__);
                return Eigen::Matrix4d{};
            }
        }
        std::pair<cv::Mat, cv::Mat> solveCorrespondences2D3D(const cv::Mat& cameraMatrix,
                                                             const cv::Mat& distortionCoefficients,
                                                             const std::vector<cv::Point3f>& objects3DVector,
                                                             const std::vector<cv::Point2f>& points2DVector)
        {
            try
            {
                // log("Solving 2D-3D correspondences (extrinsics)", Priority::High);
                cv::Mat rVec(3, 1, cv::DataType<double>::type);
                cv::Mat tVec(3, 1, cv::DataType<double>::type);

                // VERY IMPORTANT
                // So far, the only one that gives precise results is: cv::SOLVEPNP_DLS without RANSCAC

                // Non-RANSAC mode
                // It only can use 4 points
                // cv::solvePnP(fourObjects3DVector, fourPoints2DVector, cameraMatrix, distortionCoefficients, rVec, tVec, false, cv::SOLVEPNP_P3P);
                // More robust against outliers
                // const auto found = cv::solvePnP(objects3DVector, points2DVector, cameraMatrix, distortionCoefficients, rVec, tVec, false, cv::SOLVEPNP_ITERATIVE);
                // Default cv::SOLVEPNP_ITERATIVE for OpenCV 3
                const auto found = cv::solvePnP(objects3DVector, points2DVector, cameraMatrix, distortionCoefficients, rVec, tVec, false);
                // More fragile against outliers
                // cv::solvePnP(objects3DVector, points2DVector, cameraMatrix, distortionCoefficients, rVec, tVec, false, cv::SOLVEPNP_EPNP);
                // More robust against outliers
                // cv::solvePnP(objects3DVector, points2DVector, cameraMatrix, distortionCoefficients, rVec, tVec, false, cv::SOLVEPNP_DLS);
                // More robust against outliers
                // cv::solvePnP(objects3DVector, points2DVector, cameraMatrix, distortionCoefficients, rVec, tVec, false, cv::SOLVEPNP_UPNP);

                // RANSAC mode
                // It gives really bad results
                // This one is the best, but it doest care about initial guesses
                // cv::solvePnPRansac(objects3DVector, points2DVector, cameraMatrix, distortionCoefficients, rVec, tVec, false, 1000, 8.0, 0.99, (int)objects3DVector.size() + 1, cv::noArray(), CV_EPNP);
                // This one is the best, but it doest care about initial guesses
                // cv::solvePnPRansac(objects3DVector, points2DVector, cameraMatrix, distortionCoefficients, rVec, tVec, false, 1000, 8.0, 0.99, cv::noArray(), cv::SOLVEPNP_ITERATIVE);
                if (!found)
                    error("Correspondences could not be found.", __LINE__, __FUNCTION__, __FILE__);

                return std::make_pair(rVec, tVec);
            }
            catch (const std::exception& e)
            {
                error(e.what(), __LINE__, __FUNCTION__, __FILE__);
                return std::make_pair(cv::Mat(), cv::Mat());
            }
        }

        std::tuple<cv::Mat, cv::Mat, std::vector<cv::Point2f>, std::vector<cv::Point3f>> calcExtrinsicParametersOpenCV(const cv::Mat& image,
                                                                                const cv::Mat& cameraMatrix,
                                                                                const cv::Mat& distortionCoefficients,
                                                                                const cv::Size& gridInnerCorners,
                                                                                const float gridSquareSizeMm)
        {
            try
            {
                // Finding accurate chessboard corner positions
                bool found{false};
                std::vector<cv::Point2f> points2DVector;
                std::tie(found, points2DVector) = findAccurateGridCorners(image, gridInnerCorners);
                if (!found)
                    return std::make_tuple(cv::Mat(), cv::Mat(), std::vector<cv::Point2f>(),
                                           std::vector<cv::Point3f>());

                // Reordering points2DVector to have the first point at the top left position (top right in
                // case the chessboard is mirrored)
                reorderPoints(points2DVector, gridInnerCorners, Points2DOrigin::TopLeft);

                // Generate objects3DVector from gridSquareSizeMm
                const auto objects3DVector = getObjects3DVector(gridInnerCorners, gridSquareSizeMm);

                // Solving correspondences 2D-3D
                cv::Mat rVec;
                cv::Mat tVec;
                std::tie(rVec, tVec) = solveCorrespondences2D3D(cameraMatrix, distortionCoefficients, objects3DVector,
                                                                points2DVector);

                return std::make_tuple(rVec, tVec, points2DVector, objects3DVector);
            }
            catch (const std::exception& e)
            {
                error(e.what(), __LINE__, __FUNCTION__, __FILE__);
                return std::make_tuple(cv::Mat(), cv::Mat(), std::vector<cv::Point2f>(), std::vector<cv::Point3f>());
            }
        }

        std::tuple<bool, Extrinsics> calcExtrinsicParameters(const cv::Mat& image,
                                                             const cv::Size& gridInnerCorners,
                                                             const float gridSquareSizeMm,
                                                             const cv::Mat& cameraMatrix,
                                                             const cv::Mat& distortionCoefficients)
        {
            try
            {
                Extrinsics extrinsics;
                cv::Mat rVecOpenCV;
                cv::Mat RotOpenCV;
                cv::Mat tMmOpenCV;
                std::tie(rVecOpenCV, tMmOpenCV, extrinsics.points2DVector, extrinsics.objects3DVector)
                        = calcExtrinsicParametersOpenCV(
                            image, cameraMatrix, distortionCoefficients, gridInnerCorners, gridSquareSizeMm);
                if (rVecOpenCV.empty())
                    return std::make_tuple(false, Extrinsics{});

                cv::Rodrigues(rVecOpenCV, RotOpenCV);
                cv::cv2eigen(RotOpenCV, extrinsics.Rotation);
                cv::cv2eigen(tMmOpenCV, extrinsics.translationMm);

                return std::make_tuple(true, extrinsics);
            }
            catch (const std::exception& e)
            {
                error(e.what(), __LINE__, __FUNCTION__, __FILE__);
                return std::make_tuple(false, Extrinsics{});
            }
        }

        std::tuple<bool, Eigen::Matrix3d, Eigen::Vector3d, Eigen::Matrix3d, Eigen::Vector3d> getExtrinsicParameters(const std::vector<std::string>& cameraPaths,
                                                                                const cv::Size& gridInnerCorners,
                                                                                const float gridSquareSizeMm,
                                                                                const bool coutAndPlotGridCorners,
                                                                                const std::vector<cv::Mat>& intrinsics,
                                                                                const std::vector<cv::Mat>& distortions)
        {
            try
            {
                // Security checks
                if (intrinsics.size() != 2 || distortions.size() != 2 || cameraPaths.size() != 2)
                    error("Found that (intrinsics.size() != 2 || distortions.size() != 2 || cameraPaths.size() != 2).",
                          __LINE__, __FUNCTION__, __FILE__);

                std::vector<Extrinsics> extrinsicss(2);
                for (auto i = 0u ; i < cameraPaths.size() ; i++)
                {
                    if (coutAndPlotGridCorners)
                        log("getExtrinsicParameters(...), iteration with: " + cameraPaths[i], Priority::High);
                    // Loading images
                    const cv::Mat image = cv::imread(cameraPaths[i]);
                    if (image.empty())
                        error("No image found in the path `" + cameraPaths[i] + "`.",
                              __LINE__, __FUNCTION__, __FILE__);
                    bool valid;
                    std::tie(valid, extrinsicss[i]) = calcExtrinsicParameters(
                        image, gridInnerCorners, gridSquareSizeMm, intrinsics.at(i), distortions.at(i));
                    if (!valid)
                        return std::make_tuple(false, Eigen::Matrix3d{}, Eigen::Vector3d{}, Eigen::Matrix3d{},
                                               Eigen::Vector3d{});
                    if (coutAndPlotGridCorners)
                    {
                        plotGridCorners(gridInnerCorners, extrinsicss[i].points2DVector,
                                        cameraPaths[i], image);
                    }
                }

                return std::make_tuple(
                    true,
                    extrinsicss.at(0).Rotation,
                    extrinsicss.at(0).translationMm,
                    extrinsicss.at(1).Rotation,
                    extrinsicss.at(1).translationMm
                );
            }
            catch (const std::exception& e)
            {
                error(e.what(), __LINE__, __FUNCTION__, __FILE__);
                return std::make_tuple(false, Eigen::Matrix3d{}, Eigen::Vector3d{}, Eigen::Matrix3d{}, Eigen::Vector3d{});
            }
        }

        Eigen::Matrix4d getMFrom2Ms(const Eigen::Matrix4d& MGridToCam0,
                                    const Eigen::Matrix4d& MGridToCam1)
        {
            try
            {
                // MCam1ToGrid
                // Non-efficient equivalent:
                    // MCam1ToGrid = MGridToCam1.inverse()
                // Efficient version:
                    // M * M.inverse()  =    I
                    // [R t] [R -R't]   =  [I 0]
                    // [0 1] [0   1 ]   =  [0 1]
                    // Conclusion:
                    //     R_inv = R^-1 = R^T
                    //     t_inv = -R^T t
                Eigen::Matrix4d MCam1ToGrid = Eigen::Matrix4d::Identity();
                MCam1ToGrid.block<3,3>(0,0) = MGridToCam1.block<3,3>(0,0).transpose();
                MCam1ToGrid.block<3,1>(0,3) = - MCam1ToGrid.block<3,3>(0,0) * MGridToCam1.block<3,1>(0,3);
                // MCam1ToCam0 = MGridToCam0 * inv(MGridToCam1)
                // I.e., position of camera 1 w.r.t. camera 0
                return Eigen::Matrix4d{MGridToCam0 * MCam1ToGrid};
            }
            catch (const std::exception& e)
            {
                error(e.what(), __LINE__, __FUNCTION__, __FILE__);
                return Eigen::Matrix4d{};
            }
        }

        Eigen::Matrix4d getMFromCam1ToCam0(const Eigen::Matrix3d& RGridToMainCam0,
                                           const Eigen::Vector3d& tGridToMainCam0,
                                           const Eigen::Matrix3d& RGridToMainCam1,
                                           const Eigen::Vector3d& tGridToMainCam1,
                                           const bool coutAndImshowVerbose)
        {
            const auto MGridToCam0 = getMFromRt(RGridToMainCam0, tGridToMainCam0);
            const auto MGridToCam1 = getMFromRt(RGridToMainCam1, tGridToMainCam1);
            // const auto MCam1ToCam0 = getMFrom2Ms(MGridToCam0, MGridToCam1);
            const auto MCam1ToCam0 = getMFrom2Ms(MGridToCam1, MGridToCam0);

            if (coutAndImshowVerbose)
            {
                const Eigen::Vector3d tCam1WrtCam0 = MCam1ToCam0.block<3,1>(0,3) / MCam1ToCam0(3,3);
                const Eigen::Matrix3d RCam1WrtCam0 = MCam1ToCam0.block<3,3>(0,0);
                log("M_gb:", Priority::High);
                log(MGridToCam1, Priority::High);
                log("M_gf:", Priority::High);
                log(MGridToCam0, Priority::High);
                log("M_bf:", Priority::High);
                log(MCam1ToCam0, Priority::High);

                log("########## Secondary camera position w.r.t. main camera ##########", Priority::High);
                log("tCam1WrtCam0:", Priority::High);
                log(tCam1WrtCam0, Priority::High);
                log("RCam1WrtCam0:", Priority::High);
                log(RCam1WrtCam0, Priority::High);
                log("MCam0WrtCam1:", Priority::High);
                log((- RCam1WrtCam0.transpose() * tCam1WrtCam0), Priority::High);
            }

            return MCam1ToCam0;
        }
    #endif

    const int SIFT_NAME = ('S' + ('I' << 8) + ('F' << 16) + ('T' << 24));
    // const int MSER_NAME = ('M' + ('S' << 8) + ('E' << 16) + ('R' << 24));
    // const int RECT_NAME = ('R' + ('E' << 8) + ('C' << 16) + ('T' << 24));
    const int SIFT_VERSION_4 = ('V' + ('4' << 8) + ('.' << 16) + ('0' << 24));
    const int SIFT_EOF = (0xff + ('E' << 8) + ('O' << 16) + ('F' << 24));
    void writeVisualSFMSiftGPU(const std::string& fileName, const std::vector<cv::Point2f>& points2DVector)
    {
        const int siftName = SIFT_NAME;
        const int siftVersion = SIFT_VERSION_4;
        const int keyDimension = 5;
        const int descDimension = 128;
        const int siftEOF = SIFT_EOF;
        const int nSift = (int)points2DVector.size();
        std::ofstream ofstreamSift;
        ofstreamSift.open(fileName, std::ios::binary);
        // Can write
        if (ofstreamSift.is_open())
        {
            ofstreamSift.write(reinterpret_cast<const char*>(&siftName), sizeof(int));
            ofstreamSift.write(reinterpret_cast<const char*>(&siftVersion), sizeof(int));
            ofstreamSift.write(reinterpret_cast<const char*>(&nSift), sizeof(int));
            ofstreamSift.write(reinterpret_cast<const char*>(&keyDimension), sizeof(int));
            ofstreamSift.write(reinterpret_cast<const char*>(&descDimension), sizeof(int));
            // for (int j = 0; j < nSift; ++j)
            for (auto i = 0u; i < points2DVector.size(); i++)
            {
                // const float x = KeyPts[4 * j];
                // const float y = KeyPts[4 * j + 1];
                const float x = points2DVector[i].x;
                const float y = points2DVector[i].y;
                const float dummy = 0.f;
                const float scale = 1.f;
                const float orientation = 0.f;
                ofstreamSift.write(reinterpret_cast<const char*>(&x), sizeof(float));
                ofstreamSift.write(reinterpret_cast<const char*>(&y), sizeof(float));
                ofstreamSift.write(reinterpret_cast<const char*>(&dummy), sizeof(float));
                // ofstreamSift.write(reinterpret_cast<const char*>(&KeyPts[4 * j + 2]), sizeof(float));
                // ofstreamSift.write(reinterpret_cast<const char*>(&KeyPts[4 * j + 3]), sizeof(float));
                ofstreamSift.write(reinterpret_cast<const char*>(&scale), sizeof(float));
                ofstreamSift.write(reinterpret_cast<const char*>(&orientation), sizeof(float));
            }

            // // Extra argument: const unsigned char* const desc
            // for (int j = 0; j < nSift; ++j)
            //     for (int i = 0; i < descDimension; i++)
            //         ofstreamSift.write(reinterpret_cast<const char*>(&desc[j * descDimension + i]),
            //                            sizeof(unsigned char));
            const unsigned char dummy = 0u;
            for (auto i = 0; i < nSift * descDimension; i++)
                ofstreamSift.write(reinterpret_cast<const char*>(&dummy), sizeof(unsigned char));

            ofstreamSift.write(reinterpret_cast<const char*>(&siftEOF), sizeof(int));
            ofstreamSift.close();
        }
        // Couldn't write
        else
            log("Cannot write on " + fileName, Priority::High);
    }

    std::string getFileNameFromCameraIndex(const int cameraIndex)
    {
        try
        {
            // Security checks
            if (cameraIndex >= 100)
                error("Only implemented for up to 99 cameras.", __LINE__, __FUNCTION__, __FILE__);
            // Return result
            return (cameraIndex < 10 ? "00_0" : "00_") + std::to_string(cameraIndex);
        }
        catch (const std::exception& e)
        {
            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
            return "";
        }
    }

    void estimateAndSaveSiftFileSubThread(std::vector<cv::Point2f>* points2DExtrinsicPtr,
                                          std::vector<unsigned int>* matchIndexesCameraPtr,
                                          const int cameraIndex,
                                          const int numberCameras,
                                          const int numberCorners,
                                          const unsigned int numberViews,
                                          const bool saveImagesWithCorners,
                                          const std::string& imagesFolder,
                                          const cv::Size& gridInnerCornersCvSize,
                                          const cv::Size& imageSize,
                                          const std::vector<std::pair<cv::Mat, std::string>>& imageAndPaths)
    {
        try
        {
            // Security checks
            if (points2DExtrinsicPtr == nullptr || matchIndexesCameraPtr == nullptr)
                error("Make sure than points2DExtrinsicPtr != nullptr && matchIndexesCameraPtr != nullptr.",
                      __LINE__, __FUNCTION__, __FILE__);
            std::vector<cv::Point2f>& points2DExtrinsic = *points2DExtrinsicPtr;
            std::vector<unsigned int>& matchIndexesCamera = *matchIndexesCameraPtr;
            std::vector<cv::Mat> imagesWithCorners;
            for (auto viewIndex = 0u ; viewIndex < numberViews ; viewIndex++)
            {
                // Get right image
                const auto& imageAndPath = imageAndPaths.at(viewIndex * numberCameras + cameraIndex);
                const auto& image = imageAndPath.first;

                if (viewIndex % std::max(1, int(numberViews/6)) == 0)
                    log("Camera " + std::to_string(cameraIndex) + " - Image view "
                        + std::to_string(viewIndex+1) + "/" + std::to_string(numberViews),
                        Priority::High);

                // Security check
                if (imageSize.width != image.cols || imageSize.height != image.rows)
                    error("Detected images with different sizes in `" + imagesFolder + "` All images"
                          " must have the same resolution.", __LINE__, __FUNCTION__, __FILE__);

                // Find grid corners
                bool found;
                std::vector<cv::Point2f> points2DVector;
                std::tie(found, points2DVector) = findAccurateGridCorners(image, gridInnerCornersCvSize);

                // Reorder & save 2D pixels points
                if (found)
                {
                    reorderPoints(points2DVector, gridInnerCornersCvSize, Points2DOrigin::TopLeft);
                    for (auto i = 0 ; i < numberCorners ; i++)
                        matchIndexesCamera.emplace_back(viewIndex * numberCorners + i);
                }
                else
                {
                    points2DVector.clear();
                    points2DVector.resize(numberCorners, cv::Point2f{-1.f,-1.f});
                    log("Camera " + std::to_string(cameraIndex) + " - Image view "
                        + std::to_string(viewIndex+1) + "/" + std::to_string(numberViews)
                        + " - Chessboard not found.", Priority::High);
                }
                points2DExtrinsic.insert(points2DExtrinsic.end(), points2DVector.begin(), points2DVector.end());

                // Show image (with chessboard corners if found)
                if (saveImagesWithCorners)
                {
                    cv::Mat imageToPlot = image.clone();
                    if (found)
                        drawGridCorners(imageToPlot, gridInnerCornersCvSize, points2DVector);
                    imagesWithCorners.emplace_back(imageToPlot);
                }
            }

            // Save *.sift file for camera
            // const auto fileName = getFullFilePathNoExtension(imageAndPaths.at(cameraIndex).second) + ".sift";
            const auto fileName = getFileParentFolderPath(imageAndPaths.at(cameraIndex).second)
                                + getFileNameFromCameraIndex(cameraIndex) + ".sift";
            writeVisualSFMSiftGPU(fileName, points2DExtrinsic);

            // Save images with corners
            if (saveImagesWithCorners)
            {
                const auto folderWhereSavingImages = imagesFolder + "images_with_corners/";
                // Create directory in case it did not exist
                makeDirectory(folderWhereSavingImages);
                const auto pathWhereSavingImages = folderWhereSavingImages + std::to_string(cameraIndex) + "_";
                // Save new images
                const std::string extension{".png"};
                auto fileRemoved = true;
                for (auto i = 0u ; i < imagesWithCorners.size() || fileRemoved; i++)
                {
                    const auto finalPath = pathWhereSavingImages + std::to_string(i+1) + extension;
                    // remove leftovers/previous files
                    // Note: If file is not deleted before cv::imwrite, Windows considers that the file
                    // was "only" modified at that time, not created
                    fileRemoved = {remove(finalPath.c_str()) == 0};
                    // save images on hhd in the desired place
                    if (i < imagesWithCorners.size())
                        saveImage(imagesWithCorners.at(i), finalPath);
                }
            }
        }
        catch (const std::exception& e)
        {
            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
        }
    }

    // Public functions
    void estimateAndSaveIntrinsics(const Point<int>& gridInnerCorners,
                                   const float gridSquareSizeMm,
                                   const int flags,
                                   const std::string& outputParameterFolder,
                                   const std::string& imagesFolder,
                                   const std::string& serialNumber,
                                   const bool saveImagesWithCorners)
    {
        try
        {
            // Point<int> --> cv::Size
            log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
            const cv::Size gridInnerCornersCvSize{gridInnerCorners.x, gridInnerCorners.y};

            // Read images in folder
            log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
            std::vector<std::vector<cv::Point2f>> points2DVectors;
            const auto imageAndPaths = getImageAndPaths(imagesFolder);

            // Get 2D grid corners of each image
            log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
            std::vector<cv::Mat> imagesWithCorners;
            const auto imageSize = imageAndPaths.at(0).first.size();
            for (auto i = 0u ; i < imageAndPaths.size() ; i++)
            {
                log("\nImage " + std::to_string(i+1) + "/" + std::to_string(imageAndPaths.size()), Priority::High);
                const auto& image = imageAndPaths.at(i).first;

                // Security check
                if (imageSize.width != image.cols || imageSize.height != image.rows)
                    error("Detected images with different sizes in `" + imagesFolder + "` All images"
                          " must have the same resolution.", __LINE__, __FUNCTION__, __FILE__);

                // Find grid corners
                bool found;
                std::vector<cv::Point2f> points2DVector;
                std::tie(found, points2DVector) = findAccurateGridCorners(image, gridInnerCornersCvSize);

                // Reorder & save 2D pixels points
                if (found)
                {
                    reorderPoints(points2DVector, gridInnerCornersCvSize, Points2DOrigin::TopLeft);
                    points2DVectors.emplace_back(points2DVector);
                }
                else
                    std::cerr << "Chessboard not found in this image." << std::endl;

                // Show image (with chessboard corners if found)
                if (saveImagesWithCorners)
                {
                    cv::Mat imageToPlot = image.clone();
                    if (found)
                        drawGridCorners(imageToPlot, gridInnerCornersCvSize, points2DVector);
                    // cv::pyrDown(imageToPlot, imageToPlot);
                    // cv::imshow("Image View", imageToPlot);
                    // cv::waitKey(delayMilliseconds);
                    imagesWithCorners.emplace_back(imageToPlot);
                }
            }

            // Run calibration
            log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
            // objects3DVector is the same one for each image
            const std::vector<std::vector<cv::Point3f>> objects3DVectors(points2DVectors.size(),
                                                                         getObjects3DVector(gridInnerCornersCvSize,
                                                                                gridSquareSizeMm));
            const auto intrinsics = calcIntrinsicParameters(imageSize, points2DVectors, objects3DVectors, flags);

            // Save intrinsics/results
            log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
            CameraParameterReader cameraParameterReader{serialNumber, intrinsics.cameraMatrix,
                                                        intrinsics.distortionCoefficients};
            cameraParameterReader.writeParameters(outputParameterFolder);

            // Save images with corners
            log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
            if (saveImagesWithCorners)
            {
                const auto folderWhereSavingImages = imagesFolder + "images_with_corners/";
                // Create directory in case it did not exist
                makeDirectory(folderWhereSavingImages);
                // Save new images
                const std::string extension{".png"};
                auto fileRemoved = true;
                for (auto i = 0u ; i < imagesWithCorners.size() || fileRemoved; i++)
                {
                    const auto finalPath = folderWhereSavingImages + std::to_string(i+1) + extension;
                    // remove leftovers/previous files
                    // Note: If file is not deleted before cv::imwrite, Windows considers that the file
                    // was "only" modified at that time, not created
                    fileRemoved = {remove(finalPath.c_str()) == 0};
                    // save images on hhd in the desired place
                    if (i < imagesWithCorners.size())
                        saveImage(imagesWithCorners.at(i), finalPath);
                }
            }
        }
        catch (const std::exception& e)
        {
            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
        }
    }

    void estimateAndSaveExtrinsics(const std::string& intrinsicsFolder,
                                   const std::string& extrinsicsImagesFolder,
                                   const Point<int>& gridInnerCorners,
                                   const float gridSquareSizeMm,
                                   const int index0,
                                   const int index1,
                                   const bool imagesAreUndistorted,
                                   const bool combineCam0Extrinsics)
    {
        try
        {
            #ifdef USE_EIGEN
                // For debugging
                log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                const auto coutResults = false;
                // const auto coutResults = true;
                const bool coutAndImshowVerbose = false;

                // Point<int> --> cv::Size
                const cv::Size gridInnerCornersCvSize{gridInnerCorners.x, gridInnerCorners.y};

                // Load intrinsic parameters
                log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                CameraParameterReader cameraParameterReader;
                cameraParameterReader.readParameters(intrinsicsFolder);
                const auto cameraSerialNumbers = cameraParameterReader.getCameraSerialNumbers();
                const auto realCameraDistortions = cameraParameterReader.getCameraDistortions();
                auto cameraIntrinsicsSubset = cameraParameterReader.getCameraIntrinsics();
                auto cameraDistortionsSubset = (imagesAreUndistorted ?
                    std::vector<cv::Mat>{realCameraDistortions.size()}
                    : realCameraDistortions);
                // Only use the 2 desired ones
                log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                cameraIntrinsicsSubset = {cameraIntrinsicsSubset.at(index0), cameraIntrinsicsSubset.at(index1)};
                cameraDistortionsSubset = {cameraDistortionsSubset.at(index0), cameraDistortionsSubset.at(index1)};
                // Base extrinsics
                log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                cv::Mat extrinsicsCam0 = cv::Mat::eye(4, 4, realCameraDistortions.at(0).type());
                bool cam0IsOrigin = true;
                if (combineCam0Extrinsics)
                {
                    cameraParameterReader.getCameraExtrinsics().at(index0).copyTo(extrinsicsCam0(cv::Rect{0,0,4,3}));
                    cam0IsOrigin = cv::norm(extrinsicsCam0 - cv::Mat::eye(4, 4, extrinsicsCam0.type())) < 1e-9;
                }

                // Number cameras and image paths
                log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                const auto numberCameras = cameraParameterReader.getNumberCameras();
                log("\nDetected " + std::to_string(numberCameras) + " cameras from your XML files on:\n"
                    + intrinsicsFolder + "\nRemove wrong/extra XML files if this number of cameras does not"
                    + " correspond with the number of cameras recorded in:\n" + extrinsicsImagesFolder + "\n",
                    Priority::High);
                const auto imagePaths = getImagePaths(extrinsicsImagesFolder);
                // Security check
                if (imagePaths.size() % numberCameras != 0)
                    error("You indicated that there are " + std::to_string(numberCameras)
                          + " cameras. However, we found a total of " + std::to_string(imagePaths.size())
                          + " images, which should be possible to divide into the number of cameras with no"
                          " remainder.",
                          __LINE__, __FUNCTION__, __FILE__);

                // Estimate extrinsic parameters per image
                log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                log("Calibrating camera " + cameraSerialNumbers.at(index1) + " with respect to camera "
                    + cameraSerialNumbers.at(index0) + "...", Priority::High);
                const auto numberViews = imagePaths.size() / numberCameras;
                auto counterValidImages = 0u;
                std::vector<Eigen::Matrix4d> MCam1ToCam0s;
                for (auto i = 0u ; i < imagePaths.size() ; i+=numberCameras)
                {
                    log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                    const auto pathCam0 = imagePaths[i+index0];
                    const auto pathCam1 = imagePaths[i+index1];
                    if (coutResults || i/numberCameras % int(numberViews/10) == 0)
                        log("It " + std::to_string(i/numberCameras+1) + "/" + std::to_string(numberViews) + ": "
                            + getFileNameAndExtension(pathCam0) + " & "
                            + getFileNameAndExtension(pathCam1) + "...", Priority::High);

                    // Extrinsic parameters extractor
                    Eigen::Matrix3d RGridToMainCam0;
                    Eigen::Matrix3d RGridToMainCam1;
                    Eigen::Vector3d tGridToMainCam0;
                    Eigen::Vector3d tGridToMainCam1;
                    bool valid = true;
                    std::tie(valid, RGridToMainCam0, tGridToMainCam0, RGridToMainCam1, tGridToMainCam1)
                        = getExtrinsicParameters({pathCam0, pathCam1}, gridInnerCornersCvSize, gridSquareSizeMm,
                                                 false,
                                                 // coutAndImshowVerbose, // It'd display all images with grid
                                                 cameraIntrinsicsSubset, cameraDistortionsSubset);
                    if (valid)
                    {
                        counterValidImages++;
                        if (coutAndImshowVerbose)
                        {
                            log("########## Extrinsic parameters extractor ##########", Priority::High);
                            log("R_gf", Priority::High);
                            log(RGridToMainCam0, Priority::High);
                            log("t_gf", Priority::High);
                            log(tGridToMainCam0, Priority::High);
                            log("R_gb", Priority::High);
                            log(RGridToMainCam1, Priority::High);
                            log("t_gb", Priority::High);
                            log(tGridToMainCam1, Priority::High);
                            log("\n", Priority::High);
                        }

                        // MCam1ToCam0 - Projection matrix estimator
                        if (coutAndImshowVerbose)
                            log("########## Projection Matrix from secondary camera to main camera ##########",
                                Priority::High);
                        MCam1ToCam0s.emplace_back(getMFromCam1ToCam0(RGridToMainCam0, tGridToMainCam0,
                                                                     RGridToMainCam1, tGridToMainCam1,
                                                                     coutAndImshowVerbose));
                        if (coutResults)
                        {
                            if (coutAndImshowVerbose)
                                log("M_bg:", Priority::High);
                            log(MCam1ToCam0s.back(), Priority::High);
                            log(" ", Priority::High);
                        }
                    }
                    else
                    {
                        if (coutResults)
                            log("Invalid frame (chessboard not found).", Priority::High);
                    }
                }
                log("Finished processing images.", Priority::High);

                // Pseudo RANSAC calibration
                log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                const auto MCam1ToCam0Noisy = getMAverage(MCam1ToCam0s);
                log("Estimated initial (noisy?) projection matrix.", Priority::High);
                auto MCam1ToCam0 = getMAverage(MCam1ToCam0s, MCam1ToCam0Noisy);
                while ((MCam1ToCam0 - getMAverage(MCam1ToCam0s, MCam1ToCam0)).norm() > 1e-3)
                {
                    if (coutResults)
                        log("Repeated robustness method...", Priority::High);
                    MCam1ToCam0 = getMAverage(MCam1ToCam0s, MCam1ToCam0);
                }
                log("Estimated robust projection matrix.", Priority::High);
                log("norm(M_robust-M_noisy): " + std::to_string((MCam1ToCam0Noisy - MCam1ToCam0).norm()),
                    Priority::High);

                // Show errors
                log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                if (coutAndImshowVerbose)
                {
                    log("\n-----------------------------------------------------------------------------------"
                        "-------------------\nErrors:", Priority::High);
                    // Errors
                    log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                    for (auto i = 0u ; i < MCam1ToCam0s.size() ; i++)
                    {
                        log("tCam1WrtCam0:", Priority::High);
                        log(MCam1ToCam0s.at(i).block<3,1>(0,3).transpose(), Priority::High);
                    }
                    log(" ", Priority::High);

                    log("tCam1WrtCam0:", Priority::High);
                    log(MCam1ToCam0.block<3,1>(0,3).transpose(), Priority::High);
                    log(" ", Priority::High);

                    // Rotation matrix in degrees Rodrigues(InputArray src, OutputArray dst
                    log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                    const auto rad2deg = 180 / PI;
                    for (auto i = 0u ; i < MCam1ToCam0s.size() ; i++)
                    {
                        Eigen::Matrix3d R_secondaryToMain = MCam1ToCam0s.at(i).block<3,3>(0,0);
                        log("rodrigues:", Priority::High);
                        log((getRodriguesVector(R_secondaryToMain).t() * rad2deg), Priority::High);
                    }
                    Eigen::Matrix3d R_secondaryToMain = MCam1ToCam0.block<3,3>(0,0);
                    log("rodrigues:", Priority::High);
                    log((getRodriguesVector(R_secondaryToMain).t() * rad2deg), Priority::High);
                }

                // Show final result
                log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                if (coutResults)
                {
                    log("\n\n\n---------------------------------------------------------------------------"
                        "---------------------------", Priority::High);
                    log(std::to_string(counterValidImages) + " valid images.", Priority::High);
                    log("Initial (noisy?) projection matrix:", Priority::High);
                    log(MCam1ToCam0Noisy, Priority::High);
                    log("\nFinal projection matrix (mm):", Priority::High);
                    log(MCam1ToCam0, Priority::High);
                    log(" ", Priority::High);
                }

                // mm --> m
                log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                MCam1ToCam0.block<3,1>(0,3) *= 1e-3;

                // Eigen --> cv::Mat
                log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                cv::Mat cvMatExtrinsics;
                Eigen::MatrixXd eigenExtrinsics = MCam1ToCam0.block<3,4>(0,0);
                cv::eigen2cv(eigenExtrinsics, cvMatExtrinsics);

                // pos_target_origen = pos_target_cam1 * pos_cam1_origin
                // Example: pos_31 = pos_32 * pos_21
                if (!cam0IsOrigin)
                    cvMatExtrinsics *= extrinsicsCam0;

                // Final projection matrix
                log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                log("\nFinal projection matrix w.r.t. global origin (m):", Priority::High);
                log(cvMatExtrinsics, Priority::High);
                log(" ", Priority::High);

                // Save result
                log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                CameraParameterReader camera2ParameterReader{
                    cameraSerialNumbers.at(index1),
                    cameraIntrinsicsSubset.at(1),
                    realCameraDistortions.at(index1),
                    cvMatExtrinsics};
                camera2ParameterReader.writeParameters(intrinsicsFolder);

                // Let the rendered image to be displayed
                log("", Priority::Low, __LINE__, __FUNCTION__, __FILE__);
                if (coutAndImshowVerbose)
                    cv::waitKey(0);
            #else
                UNUSED(intrinsicsFolder);
                UNUSED(extrinsicsImagesFolder);
                UNUSED(gridInnerCorners);
                UNUSED(gridSquareSizeMm);
                UNUSED(index0);
                UNUSED(index1);
                UNUSED(imagesAreUndistorted);
                UNUSED(combineCam0Extrinsics);
                error("CMake flag `USE_EIGEN` required when compiling OpenPose`.", __LINE__, __FUNCTION__, __FILE__);
            #endif
        }
        catch (const std::exception& e)
        {
            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
        }
    }

    void estimateAndSaveSiftFile(const Point<int>& gridInnerCorners,
                                 const std::string& imagesFolder,
                                 const int numberCameras,
                                 const bool saveImagesWithCorners)
    {
        try
        {
            log("Loading images...", Priority::High);
            const auto imageAndPaths = getImageAndPaths(imagesFolder);
            log("Images loaded.", Priority::High);

            // Point<int> --> cv::Size
            const cv::Size gridInnerCornersCvSize{gridInnerCorners.x, gridInnerCorners.y};

            // Read images in folder
            const auto numberCorners = gridInnerCorners.area();
            std::vector<std::vector<cv::Point2f>> points2DVectorsExtrinsic(numberCameras); // camera - keypoints
            std::vector<std::vector<unsigned int>> matchIndexes(numberCameras); // camera - indixes found
            if (imageAndPaths.empty())
                error("imageAndPaths.empty()!.", __LINE__, __FUNCTION__, __FILE__);

            // Get 2D grid corners of each image
            std::vector<cv::Mat> imagesWithCorners;
            const auto imageSize = imageAndPaths.at(0).first.size();
            const auto numberViews = imageAndPaths.size() / numberCameras;
            log("Processing cameras...", Priority::High);
            std::vector<std::thread> threads;
            for (auto cameraIndex = 0 ; cameraIndex < numberCameras ; cameraIndex++)
            {
                auto* points2DExtrinsic = &points2DVectorsExtrinsic[cameraIndex];
                auto* matchIndexesCamera = &matchIndexes[cameraIndex];
                // Threaded version
                threads.emplace_back(estimateAndSaveSiftFileSubThread, points2DExtrinsic,
                                     matchIndexesCamera, cameraIndex, numberCameras,
                                     numberCorners, numberViews, saveImagesWithCorners, imagesFolder,
                                     gridInnerCornersCvSize, imageSize, imageAndPaths);
                // // Non-threaded version
                // estimateAndSaveSiftFileSubThread(points2DExtrinsic, matchIndexesCamera, cameraIndex, numberCameras,
                //                                  numberCorners, numberViews, saveImagesWithCorners, imagesFolder,
                //                                  gridInnerCornersCvSize, imageSize, imageAndPaths);
            }
            // Threaded version
            for (auto& thread : threads)
                if (thread.joinable())
                    thread.join();

            // Matching file
            std::ofstream ofstreamMatches{getFileParentFolderPath(imageAndPaths.at(0).second) + "FeatureMatches.txt"};
            for (auto cameraIndex = 0 ; cameraIndex < numberCameras ; cameraIndex++)
            {
                for (auto cameraIndex2 = cameraIndex+1 ; cameraIndex2 < numberCameras ; cameraIndex2++)
                {
                    std::vector<unsigned int> matchIndexesIntersection;
                    std::set_intersection(matchIndexes[cameraIndex].begin(), matchIndexes[cameraIndex].end(),
                                          matchIndexes[cameraIndex2].begin(), matchIndexes[cameraIndex2].end(),
                                          std::back_inserter(matchIndexesIntersection));

                    ofstreamMatches << getFileNameFromCameraIndex(cameraIndex) << ".jpg"
                                    << " " << getFileNameFromCameraIndex(cameraIndex2) << ".jpg"
                    // ofstreamMatches << getFileNameAndExtension(imageAndPaths.at(cameraIndex).second)
                    //                 << " " << getFileNameAndExtension(imageAndPaths.at(cameraIndex2).second)
                                    << " " << matchIndexesIntersection.size() << "\n";
                    for (auto reps = 0 ; reps < 2 ; reps++)
                    {
                        for (auto i = 0u ; i < matchIndexesIntersection.size() ; i++)
                            ofstreamMatches << matchIndexesIntersection[i] << " ";
                        ofstreamMatches << "\n";
                    }
                    ofstreamMatches << "\n";
                }
            }
            ofstreamMatches.close();
            // Security check
            log("Number points fully obtained: " + std::to_string(points2DVectorsExtrinsic[0].size()), Priority::High);
            log("Number views fully obtained: " + std::to_string(points2DVectorsExtrinsic[0].size() / numberCorners),
                Priority::High);
            for (auto i = 1 ; i < numberCameras ; i++)
                if (points2DVectorsExtrinsic[i].size() != points2DVectorsExtrinsic[0].size())
                    error("Something went wrong. Notify us.", __LINE__, __FUNCTION__, __FILE__);
        }
        catch (const std::exception& e)
        {
            error(e.what(), __LINE__, __FUNCTION__, __FILE__);
        }
    }
}


#include "StdAfx.h"
#include "OptApp.h"
#include <pcl/console/print.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/time.h>
#include <boost/filesystem.hpp>

COptApp::COptApp(void)
	: resolution_( 8 )
	, interval_( 50 )
	, num_( 0 )
	, weight_( 1.0f )
	, length_( 3.0f )
	, max_iteration_( 5 )
	, max_inner_iteration_( 10 )
	, ctr_filename_( "output.ctr" )
	, sample_filename_( "sample.pcd" )
	, pose_filename_( "pose.log" )
	, init_ctr_file_( "" )
	, dir_prefix_( "" )
	, sample_num_( -1 )
	, blacklist_pair_num_( 10000 )
{
}

COptApp::~COptApp(void)
{
}

void COptApp::InitMap()
{
	absolute2relative_map_.resize( num_ );
	relative2absolute_map_.clear();
	for ( int i = 0; i < num_; i++ ) {
		if ( blacklist_.find( i ) == blacklist_.end() ) {
			absolute2relative_map_[ i ] = relative2absolute_map_.size();
			relative2absolute_map_.push_back( i );
		} else {
			absolute2relative_map_[ i ] = -1;
		}
	}
	if ( num_ != relative2absolute_map_.size() ) {
		PCL_WARN( "Blacklisted fragments, number reduced from %d to %d.\n", num_, relative2absolute_map_.size() );
	}
	num_ = relative2absolute_map_.size();
}

void COptApp::InitIPose()
{
	if ( ipose_.size() > 0 ) {
		return;
	}

	Eigen::Matrix4d basepose = Eigen::Matrix4d::Identity();
	basepose( 0, 3 ) = length_ / 2.0;
	basepose( 1, 3 ) = length_ / 2.0;
	basepose( 2, 3 ) = - 0.3;

	Eigen::Matrix4d baseinverse = basepose.inverse();

	Eigen::Matrix4d leftbase = basepose * rgbd_traj_.data_[ 0 ].transformation_.inverse();
	ipose_.resize( num_ );
	pose_.resize( num_ );
	pose_rot_t_.resize( num_ );

	for ( int i = 0; i < num_; i++ ) {
		int ii = relative2absolute_map_[ i ];
		ipose_[ i ] = leftbase * rgbd_traj_.data_[ ii * interval_ ].transformation_ * baseinverse;
	}
	PCL_INFO( "IPose initialized.\n" );
}

void COptApp::InitPointClouds()
{
	pointclouds_.clear();
	for ( int i = 0; i < num_; i++ ) {
		pointclouds_.push_back( PointCloud( i, resolution_, length_ ) );
	}

	#pragma omp parallel for num_threads( 8 ) schedule( dynamic )
	for ( int i = 0; i < num_; i++ ) {
		int ii = relative2absolute_map_[ i ];
		char filename[ 1024 ];
		memset( filename, 0, 1024 );
		sprintf( filename, "%scloud_bin_%d.pcd", dir_prefix_.c_str(), ii );
		if ( boost::filesystem::exists( std::string( filename ) ) ) {
			pointclouds_[ i ].LoadFromPCDFile( filename );
		} else {
			sprintf( filename, "%scloud_bin_xyzn_%d.xyzn", dir_prefix_.c_str(), ii );
			if ( boost::filesystem::exists( std::string( filename ) ) ) {
				pointclouds_[ i ].LoadFromXYZNFile( filename );
			} else {
				PCL_ERROR( "File not found ... Check dir and num parameters.\n" );
			}
		}
	}
}

void COptApp::InitCorrespondences()
{
	corres_.clear();
	for ( int i = 0; i < ( int )reg_traj_.data_.size(); i++ ) {
		if ( blacklist_.find( reg_traj_.data_[ i ].id1_ ) != blacklist_.end() || blacklist_.find( reg_traj_.data_[ i ].id2_ ) != blacklist_.end()
			|| reg_traj_.data_[ i ].id1_ >= num_ || reg_traj_.data_[ i ].id2_ >= num_ ) {
			continue;
		}
		if ( reg_traj_.data_[ i ].frame_ != -1 && reg_traj_.data_[ i ].frame_ >= blacklist_pair_num_ ) {
			corres_.push_back( Correspondence( absolute2relative_map_[ reg_traj_.data_[ i ].id1_ ], absolute2relative_map_[ reg_traj_.data_[ i ].id2_ ] ) );
			char filename[ 1024 ];
			memset( filename, 0, 1024 );
			sprintf( filename, "%scorres_%d_%d.txt", dir_prefix_.c_str(), reg_traj_.data_[ i ].id1_, reg_traj_.data_[ i ].id2_ );
			corres_.back().trans_ = reg_traj_.data_[ i ].transformation_;
			corres_.back().LoadFromFile( filename );
			PCL_INFO( "Read %s, get %d correspondences.\n", filename, corres_.back().corres_.size() );
		}
	}
}

void COptApp::OptimizeNonrigid()
{
	PCL_WARN( "Nonrigid optimization.\n" );
	PCL_INFO( "Parameters: weight %.5f, resolution %d, piece number %d, max iteration %d\n", weight_, resolution_, num_, max_iteration_ );

	InitMap();
	unit_length_ = length_ / resolution_;
	nper_ = ( resolution_ + 1 ) * ( resolution_ + 1 ) * ( resolution_ + 1 ) * 3;
	matrix_size_ = nper_ * num_;
	InitIPose();
	InitPointClouds();
	InitCorrespondences();

	Eigen::VectorXd ctr( matrix_size_ );
	Eigen::VectorXd ictr( matrix_size_ );
	Eigen::VectorXd oldctr( matrix_size_ );
	Eigen::VectorXd Ab( matrix_size_ );
	SparseMatrix baseAA( matrix_size_, matrix_size_ );
	SparseMatrix thisAA( matrix_size_, matrix_size_ );

	// init ctr
	InitCtr( ctr );
	ictr = ctr;
	oldctr = ctr;
	InitBaseAA( baseAA );

	double score;

	// iteration
	for ( int itr = 0; itr < max_iteration_; itr++ ) {
		int nprocessed = 0;
		for ( int l = 0; l < num_; l++ ) {
			pointclouds_[ l ].UpdateAllNormal( ctr );
		}

		thisAA = baseAA;

		PCL_INFO( "Processing %d : %5d", corres_.size(), 0 );

#pragma omp parallel for num_threads( 8 ) schedule( dynamic )
		for ( int l = 0; l < ( int )corres_.size(); l++ ) {
			int i = corres_[ l ].idx0_;
			int j = corres_[ l ].idx1_;
			TripletVector tri;
			HashSparseMatrix mati( i * nper_, i * nper_ );
			HashSparseMatrix matj( j * nper_, j * nper_ );
			HashSparseMatrix matij( i * nper_, j * nper_ );

			int idx1[ 24 ], idx2[ 24 ];
			double val1[ 24 ], val2[ 24 ];

			for ( int k = 0; k < ( int )corres_[ l ].corres_.size(); k++ ) {
				int ii = corres_[ l ].corres_[ k ].first;
				int jj = corres_[ l ].corres_[ k ].second;
				Point & pi = pointclouds_[ i ].points_[ ii ];
				Point & pj = pointclouds_[ j ].points_[ jj ];

				for ( int t = 0; t < 8; t++ ) {
					idx1[ t ] = pi.idx_[ t ];
					val1[ t ] = pi.val_[ t ] * weight_ * pi.n_[ 0 ];
					idx1[ 8 + t ] = pi.idx_[ t ] + 1;
					val1[ 8 + t ] = pi.val_[ t ] * weight_ * pi.n_[ 1 ];
					idx1[ 16 + t ] = pi.idx_[ t ] + 2;
					val1[ 16 + t ] = pi.val_[ t ] * weight_ * pi.n_[ 2 ];
					idx2[ t ] = pj.idx_[ t ];
					val2[ t ] = - pj.val_[ t ] * weight_ * pi.n_[ 0 ];
					idx2[ 8 + t ] = pj.idx_[ t ] + 1;
					val2[ 8 + t ] = - pj.val_[ t ] * weight_ * pi.n_[ 1 ];
					idx2[ 16 + t ] = pj.idx_[ t ] + 2;
					val2[ 16 + t ] = - pj.val_[ t ] * weight_ * pi.n_[ 2 ];
				}

				mati.AddHessian( idx1, val1, 24, tri );
				matj.AddHessian( idx2, val2, 24, tri );
				matij.AddHessian( idx1, val1, 24, idx2, val2, 24, tri );
			}

			SparseMatrix tempAA( matrix_size_, matrix_size_ );
			tempAA.setFromTriplets( tri.begin(), tri.end() );

#pragma omp critical
			{
				nprocessed++;
				thisAA += tempAA;
				PCL_INFO( "\b\b\b\b\b%5d", nprocessed );
			}
		}
		PCL_INFO( " ... Done.\n" );

		Eigen::CholmodSupernodalLLT< SparseMatrix, Eigen::Upper > solver;
		solver.analyzePattern( thisAA );
		solver.factorize( thisAA );

		if ( sample_num_ > 0 ) {
			char filename[ 1024 ];
			memset( filename, 0, 1024 );
			sprintf( filename, "itr%d.ctr", itr );
			SaveCtr( oldctr, filename );
		}

		for ( int m = 0; m < max_inner_iteration_; m++ ) {
			Ab.setZero();
			for ( int l = 0; l < num_; l++ ) {
				for ( int i = 0; i <= resolution_; i++ ) {
					for ( int j = 0; j <= resolution_; j++ ) {
						for ( int k = 0; k <= resolution_; k++ ) {
							int idx = GetIndex( i, j, k ) * 3 + l * nper_;
							std::vector< int > idxx;
							if ( i > 0 ) {
								idxx.push_back( GetIndex( i - 1, j, k ) * 3 + l * nper_ );
							}
							if ( i < resolution_) {
								idxx.push_back( GetIndex( i + 1, j, k ) * 3 + l * nper_ );
							}
							if ( j > 0 ) {
								idxx.push_back( GetIndex( i, j - 1, k ) * 3 + l * nper_ );
							}
							if ( j < resolution_) {
								idxx.push_back( GetIndex( i, j + 1, k ) * 3 + l * nper_ );
							}
							if ( k > 0 ) {
								idxx.push_back( GetIndex( i, j, k - 1 ) * 3 + l * nper_ );
							}
							if ( k < resolution_) {
								idxx.push_back( GetIndex( i, j, k + 1 ) * 3 + l * nper_ );
							}
							Eigen::Matrix3d R = GetRotation( idx, idxx, ictr, ctr );
							for ( int t = 0; t < ( int )idxx.size(); t++ ) {
								Eigen::Vector3d bx = R * Eigen::Vector3d( ictr( idx ) - ictr( idxx[ t ] ), ictr( idx + 1 ) - ictr( idxx[ t ] + 1 ), ictr( idx + 2 ) - ictr( idxx[ t ] + 2 ) );
								Ab( idx ) += bx( 0 );
								Ab( idxx[ t ] ) -= bx( 0 );
								Ab( idx + 1 ) += bx( 1 );
								Ab( idxx[ t ] + 1 ) -= bx( 1 );
								Ab( idx + 2 ) += bx( 2 );
								Ab( idxx[ t ] + 2 ) -= bx( 2 );
							}
						}
					}
				}
			}

			oldctr = ctr;
			ctr = solver.solve( Ab );

			score = ( oldctr - ctr ).norm();
			PCL_WARN( "Iteration #%d:%d (%d:%d) : score is %.4f\n", itr + 1, m + 1, max_iteration_, max_inner_iteration_, score );

			if ( sample_num_ > 0 ) {
				char filename[ 1024 ];
				memset( filename, 0, 1024 );
				sprintf( filename, "itr%d_inner%d_out.ctr", itr, m );
				SaveCtr( ctr, filename );
			}
		}
	}

	SaveCtr( ctr, ctr_filename_ );
	SavePoints( ctr, sample_filename_ );
}

void COptApp::OptimizeRigid()
{
	PCL_WARN( "Rigid optimization.\n" );
	PCL_INFO( "Parameters: resolution %d, piece number %d, max iteration %d\n", resolution_, num_, max_iteration_ );

	InitMap();
	unit_length_ = length_ / resolution_;
	nper_ = ( resolution_ + 1 ) * ( resolution_ + 1 ) * ( resolution_ + 1 ) * 3;
	matrix_size_ = 6 * num_;			// rigid only
	InitIPose();
	InitPointClouds();
	InitCorrespondences();

	rgbd_traj_.data_.clear();
	for ( int i = 0; i < ( int )ipose_.size(); i++ ) {
		rgbd_traj_.data_.push_back( FramedTransformation( i, i, i + 1, ipose_[ i ] ) );
		pose_[ i ] = ipose_[ i ];
		pointclouds_[ i ].UpdatePose( pose_[ i ].cast< float >() );
	}

	for ( int itr = 0; itr < max_iteration_; itr++ ) {
		Eigen::VectorXd thisJb( matrix_size_ );
		SparseMatrix thisJJ( matrix_size_, matrix_size_ );

		thisJb.setZero();
		thisJJ.setZero();

		double thisscore = 0.0;
		int nprocessed = 0;

		PCL_INFO( "Processing %d : %5d", corres_.size(), 0 );

#pragma omp parallel for num_threads( 8 ) schedule( dynamic )
		for ( int l = 0; l < ( int )corres_.size(); l++ ) {
			int i = corres_[ l ].idx0_;
			int j = corres_[ l ].idx1_;
			TripletVector tri;
			HashSparseMatrix mat_adder( 0, 0 );
			Eigen::VectorXd tempJb( matrix_size_ );
			SparseMatrix tempJJ( matrix_size_, matrix_size_ );
			tempJb.setZero();
			tempJJ.setZero();

			for ( int k = 0; k < 6; k++ ) {
				mat_adder.Add( k, k, 1, tri );
			}

			int idx[ 12 ];
			double val[ 12 ];
			double b;
			double score = 0.0;

			for ( int k = 0; k < ( int )corres_[ l ].corres_.size(); k++ ) {
				int ii = corres_[ l ].corres_[ k ].first;
				int jj = corres_[ l ].corres_[ k ].second;
				Point & pi = pointclouds_[ i ].points_[ ii ];
				Point & pj = pointclouds_[ j ].points_[ jj ];
				Eigen::Vector4d ppi( pi.p_[ 0 ], pi.p_[ 1 ], pi.p_[ 2 ], 1.0 );
				Eigen::Vector4d ppj( pj.p_[ 0 ], pj.p_[ 1 ], pj.p_[ 2 ], 1.0 );
				Eigen::Vector4d npi( pi.n_[ 0 ], pi.n_[ 1 ], pi.n_[ 2 ], 0.0 );
				Eigen::Vector4d npj( pj.n_[ 0 ], pj.n_[ 1 ], pj.n_[ 2 ], 0.0 );

				b = ( ppi - ppj ).dot( npi );
				score += b * b;

				idx[ 0 ] = i * 6;
				idx[ 1 ] = i * 6 + 1;
				idx[ 2 ] = i * 6 + 2;
				idx[ 3 ] = i * 6 + 3;
				idx[ 4 ] = i * 6 + 4;
				idx[ 5 ] = i * 6 + 5;
				idx[ 6 ] = j * 6;
				idx[ 7 ] = j * 6 + 1;
				idx[ 8 ] = j * 6 + 2;
				idx[ 9 ] = j * 6 + 3;
				idx[ 10 ] = j * 6 + 4;
				idx[ 11 ] = j * 6 + 5;

				val[ 0 ] = Eigen::Vector4d( 0, -ppi( 2 ), ppi( 1 ), 1 ).dot( npi ) + Eigen::Vector4d( 0, -npi( 2 ), npi( 1 ), 0 ).dot( ppi - ppj );
				val[ 1 ] = Eigen::Vector4d( ppi( 2 ), 0, -ppi( 0 ), 1 ).dot( npi ) + Eigen::Vector4d( npi( 2 ), 0, -npi( 0 ), 0 ).dot( ppi - ppj );
				val[ 2 ] = Eigen::Vector4d( -ppi( 1 ), ppi( 0 ), 0, 1 ).dot( npi ) + Eigen::Vector4d( -npi( 1 ), npi( 0 ), 0, 0 ).dot( ppi - ppj );
				val[ 3 ] = npi( 0 );
				val[ 4 ] = npi( 1 );
				val[ 5 ] = npi( 2 );
				val[ 6 ] = -Eigen::Vector4d( 0, -ppj( 2 ), ppj( 1 ), 1 ).dot( npi );
				val[ 7 ] = -Eigen::Vector4d( ppj( 2 ), 0, -ppj( 0 ), 1 ).dot( npi );
				val[ 8 ] = -Eigen::Vector4d( -ppj( 1 ), ppj( 0 ), 0, 1 ).dot( npi );
				val[ 9 ] = -npi( 0 );
				val[ 10 ] = -npi( 1 );
				val[ 11 ] = -npi( 2 );

				mat_adder.AddHessian( idx, val, 12, tri );
				mat_adder.AddJb( idx, val, 12, b, tempJb );
			}

			tempJJ.setFromTriplets( tri.begin(), tri.end() );

#pragma omp critical
			{
				nprocessed++;
				thisscore += score;
				thisJJ += tempJJ;
				thisJb += tempJb;
				PCL_INFO( "\b\b\b\b\b%5d", nprocessed );
			}
		}
		PCL_INFO( " ... Done.\n" );
		PCL_INFO( "Error score is : %.2f\n", thisscore );

		Eigen::CholmodSupernodalLLT< SparseMatrix, Eigen::Upper > solver;
		solver.analyzePattern( thisJJ );
		solver.factorize( thisJJ );

		Eigen::VectorXd result = - solver.solve( thisJb );

		for ( int l = 0; l < num_; l++ ) {
			Eigen::Affine3d aff_mat;
			aff_mat.linear() = ( Eigen::Matrix3d ) Eigen::AngleAxisd( result( l * 6 + 2 ), Eigen::Vector3d::UnitZ() )
				* Eigen::AngleAxisd( result( l * 6 + 1 ), Eigen::Vector3d::UnitY() )
				* Eigen::AngleAxisd( result( l * 6 + 0 ), Eigen::Vector3d::UnitX() );
			aff_mat.translation() = Eigen::Vector3d( result( l * 6 + 3 ), result( l * 6 + 4 ), result( l * 6 + 5 ) );
			pose_[ l ] = aff_mat.matrix() * pose_[ l ];
			pointclouds_[ l ].UpdatePose( aff_mat.matrix().cast< float >() );
		}
	}

	output_traj_.data_.clear();
	for ( int i = 0; i < ( int )pose_.size(); i++ ) {
		output_traj_.data_.push_back( FramedTransformation( i, i, i + 1, pose_[ i ] ) );
	}
	output_traj_.SaveToFile( pose_filename_ );

	Eigen::VectorXd ctr( nper_ * num_ );
	Pose2Ctr( pose_, ctr );
	SaveCtr( ctr, ctr_filename_ );
	SavePoints( ctr, sample_filename_ );
}

void COptApp::OptimizeSLAC()
{
	PCL_WARN( "SLAC optimization.\n" );
	PCL_INFO( "Parameters: weight %.5f, resolution %d, piece number %d, max iteration %d\n", weight_, resolution_, num_, max_iteration_ );

	double default_weight = num_ * weight_;

	InitMap();
	unit_length_ = length_ / resolution_;
	nper_ = ( resolution_ + 1 ) * ( resolution_ + 1 ) * ( resolution_ + 1 ) * 3;
	matrix_size_ = 6 * num_ + nper_;
	InitIPose();
	InitPointClouds();
	InitCorrespondences();

	Eigen::VectorXd ictr( matrix_size_ );
	Eigen::VectorXd thisCtr( matrix_size_ );
	InitCtrSLAC( ictr, thisCtr );
	Eigen::VectorXd expand_ctr( nper_ * num_ );

	rgbd_traj_.data_.clear();
	for ( int i = 0; i < ( int )ipose_.size(); i++ ) {
		rgbd_traj_.data_.push_back( FramedTransformation( i, i, i + 1, ipose_[ i ] ) );
		pose_[ i ] = ipose_[ i ];
		pose_rot_t_[ i ] = pose_[ i ].block< 3, 3 >( 0, 0 ).transpose();
		pointclouds_[ i ].UpdatePose( pose_[ i ].cast< float >() );
	}

	{
		pcl::ScopeTime ttime( "Neat Optimization" );

		SparseMatrix baseJJ( matrix_size_, matrix_size_ );
		InitBaseJJ( baseJJ );

		baseJJ *= default_weight;

		for ( int itr = 0; itr < max_iteration_; itr++ ) {
			Eigen::VectorXd thisJb( matrix_size_ );
			Eigen::MatrixXd thisJJ( baseJJ.triangularView< Eigen::Upper >() );
			Eigen::VectorXd result;

			thisJJ( 0, 0 ) += 1.0;
			thisJJ( 1, 1 ) += 1.0;
			thisJJ( 2, 2 ) += 1.0;
			thisJJ( 3, 3 ) += 1.0;
			thisJJ( 4, 4 ) += 1.0;
			thisJJ( 5, 5 ) += 1.0;

			thisJb.setZero();

			double thisscore = 0.0;
			int nprocessed = 0;

			PCL_INFO( "Processing %d : %5d", corres_.size(), 0 );

#pragma omp parallel for num_threads( 8 ) schedule( dynamic )
			for ( int l = 0; l < ( int )corres_.size(); l++ ) {
				int i = corres_[ l ].idx0_;
				int j = corres_[ l ].idx1_;
				const int buck_size = 12 + 24 * 2;
				int idx[ buck_size ];
				double val[ buck_size ];
				double b;
				double score = 0.0;
				Eigen::MatrixXd tempJJ( matrix_size_, matrix_size_ );
				tempJJ.setZero();
				Eigen::VectorXd tempJb( matrix_size_ );
				tempJb.setZero();

				for ( int k = 0; k < ( int )corres_[ l ].corres_.size(); k++ ) {
					int ii = corres_[ l ].corres_[ k ].first;
					int jj = corres_[ l ].corres_[ k ].second;
					Point & pi = pointclouds_[ i ].points_[ ii ];
					Point & pj = pointclouds_[ j ].points_[ jj ];
					Eigen::Vector3d ppi( pi.p_[ 0 ], pi.p_[ 1 ], pi.p_[ 2 ] );
					Eigen::Vector3d ppj( pj.p_[ 0 ], pj.p_[ 1 ], pj.p_[ 2 ] );
					Eigen::Vector3d npi( pi.n_[ 0 ], pi.n_[ 1 ], pi.n_[ 2 ] );
					Eigen::Vector3d npj( pj.n_[ 0 ], pj.n_[ 1 ], pj.n_[ 2 ] );
					Eigen::Vector3d diff = ppi - ppj;
					b = diff.dot( npi );
					score += b * b;

					idx[ 0 ] = i * 6;
					idx[ 1 ] = i * 6 + 1;
					idx[ 2 ] = i * 6 + 2;
					idx[ 3 ] = i * 6 + 3;
					idx[ 4 ] = i * 6 + 4;
					idx[ 5 ] = i * 6 + 5;
					idx[ 6 ] = j * 6;
					idx[ 7 ] = j * 6 + 1;
					idx[ 8 ] = j * 6 + 2;
					idx[ 9 ] = j * 6 + 3;
					idx[ 10 ] = j * 6 + 4;
					idx[ 11 ] = j * 6 + 5;

					Eigen::Vector3d temp = ppj.cross( npi );

					val[ 0 ] = temp( 0 );
					val[ 1 ] = temp( 1 );
					val[ 2 ] = temp( 2 );
					val[ 3 ] = npi( 0 );
					val[ 4 ] = npi( 1 );
					val[ 5 ] = npi( 2 );
					val[ 6 ] = -temp( 0 );
					val[ 7 ] = -temp( 1 );
					val[ 8 ] = -temp( 2 );
					val[ 9 ] = -npi( 0 );
					val[ 10 ] = -npi( 1 );
					val[ 11 ] = -npi( 2 );

					// next part of Jacobian
					// deal with control lattices
					Eigen::Vector3d dTi = pose_rot_t_[ i ] * npi;
					Eigen::Vector3d dTj = - pose_rot_t_[ j ] * npi;

					for ( int ll = 0; ll < 8; ll++ ) {
						for ( int xyz = 0; xyz < 3; xyz++ ) {
							idx[ 12 + ll * 3 + xyz ] = 6 * num_ + pi.idx_[ ll ] + xyz;
							val[ 12 + ll * 3 + xyz ] = pi.val_[ ll ] * dTi( xyz );
						}
					}

					for ( int ll = 0; ll < 8; ll++ ) {
						for ( int xyz = 0; xyz < 3; xyz++ ) {
							idx[ 12 + 24 + ll * 3 + xyz ] = 6 * num_ + pj.idx_[ ll ] + xyz;
							val[ 12 + 24 + ll * 3 + xyz ] = pj.val_[ ll ] * dTj( xyz );
						}
					}

					for ( int i = 0; i < buck_size; i++ ) {
						tempJJ( idx[ i ], idx[ i ] ) += val[ i ] * val[ i ];
						for ( int j = i + 1; j < buck_size; j++ ) {
							if ( idx[ i ] == idx[ j ] ) {
								tempJJ( idx[ i ], idx[ j ] ) += 2 * val[ i ] * val[ j ];
							} else if ( idx[ i ] < idx[ j ] ) {
								tempJJ( idx[ i ], idx[ j ] ) += val[ i ] * val[ j ];
							} else {
								tempJJ( idx[ j ], idx[ i ] ) += val[ i ] * val[ j ];
							}
						}
						tempJb( idx[ i ] ) += b * val[ i ];
					}
				}

#pragma omp critical
				{
					nprocessed++;
					thisscore += score;
					thisJJ += tempJJ;
					thisJb += tempJb;
					PCL_INFO( "\b\b\b\b\b%5d", nprocessed );
				}
			}
			PCL_INFO( " ... Done.\n" );
			PCL_INFO( "Data error score is : %.2f\n", thisscore );

			SparseMatrix thisSparseJJ = thisJJ.sparseView();
			Eigen::CholmodSupernodalLLT< SparseMatrix, Eigen::Upper > solver;
			solver.analyzePattern( thisSparseJJ );
			solver.factorize( thisSparseJJ );

			Eigen::VectorXd tempCtr( thisCtr );

			// regularizer

			Eigen::VectorXd dataJb( thisJb );

			Eigen::VectorXd baseJb( matrix_size_ );
			baseJb.setZero();

			double regscore = 0.0;
			for ( int i = 0; i <= resolution_; i++ ) {
				for ( int j = 0; j <= resolution_; j++ ) {
					for ( int k = 0; k <= resolution_; k++ ) {
						int idx = GetIndex( i, j, k ) * 3 + 6 * num_;
						std::vector< int > idxx;
						if ( i > 0 ) {
							idxx.push_back( GetIndex( i - 1, j, k ) * 3 + 6 * num_ );
						}
						if ( i < resolution_) {
							idxx.push_back( GetIndex( i + 1, j, k ) * 3 + 6 * num_ );
						}
						if ( j > 0 ) {
							idxx.push_back( GetIndex( i, j - 1, k ) * 3 + 6 * num_ );
						}
						if ( j < resolution_) {
							idxx.push_back( GetIndex( i, j + 1, k ) * 3 + 6 * num_ );
						}
						if ( k > 0 ) {
							idxx.push_back( GetIndex( i, j, k - 1 ) * 3 + 6 * num_ );
						}
						if ( k < resolution_) {
							idxx.push_back( GetIndex( i, j, k + 1 ) * 3 + 6 * num_ );
						}

						Eigen::Matrix3d R;
						if ( i == resolution_ / 2 && j == resolution_ / 2 && k == 0 ) {
							// anchor point, Rotation matrix is always identity
							R = R.Identity();
						} else {
							R = GetRotation( idx, idxx, ictr, tempCtr );
						}

						for ( int t = 0; t < ( int )idxx.size(); t++ ) {
							Eigen::Vector3d bx = Eigen::Vector3d( tempCtr( idx ) - tempCtr( idxx[ t ] ), tempCtr( idx + 1 ) - tempCtr( idxx[ t ] + 1 ), tempCtr( idx + 2 ) - tempCtr( idxx[ t ] + 2 ) )
								- R * Eigen::Vector3d( ictr( idx ) - ictr( idxx[ t ] ), ictr( idx + 1 ) - ictr( idxx[ t ] + 1 ), ictr( idx + 2 ) - ictr( idxx[ t ] + 2 ) );
							regscore += default_weight * bx.transpose() * bx;
							baseJb( idx ) += bx( 0 ) * default_weight;
							baseJb( idxx[ t ] ) -= bx( 0 ) * default_weight;
							baseJb( idx + 1 ) += bx( 1 ) * default_weight;
							baseJb( idxx[ t ] + 1 ) -= bx( 1 ) * default_weight;
							baseJb( idx + 2 ) += bx( 2 ) * default_weight;
							baseJb( idxx[ t ] + 2 ) -= bx( 2 ) * default_weight;
						}
					}
				}
			}
			thisJb = dataJb + baseJb;

			PCL_INFO( "Regularization error score is : %.2f\n", regscore );

			result = - solver.solve( thisJb );

			for ( int l = 0; l < nper_; l++ ) {
				tempCtr( num_ * 6 + l ) = thisCtr( num_ * 6 + l ) + result( num_ * 6 + l );
			}

			for ( int l = 0; l < nper_; l++ ) {
				thisCtr( num_ * 6 + l ) += result( num_ * 6 + l );
			}

			for ( int l = 0; l < num_; l++ ) {
				Eigen::Affine3d aff_mat;
				aff_mat.linear() = ( Eigen::Matrix3d ) Eigen::AngleAxisd( result( l * 6 + 2 ), Eigen::Vector3d::UnitZ() )
					* Eigen::AngleAxisd( result( l * 6 + 1 ), Eigen::Vector3d::UnitY() )
					* Eigen::AngleAxisd( result( l * 6 + 0 ), Eigen::Vector3d::UnitX() );
				aff_mat.translation() = Eigen::Vector3d( result( l * 6 + 3 ), result( l * 6 + 4 ), result( l * 6 + 5 ) );
				//cout << aff_mat.matrix() << endl << endl;
				// update
				pose_[ l ] = aff_mat.matrix() * pose_[ l ];
				pose_rot_t_[ l ] = pose_[ l ].block< 3, 3 >( 0, 0 ).transpose();
			}
			ExpandCtr( thisCtr, expand_ctr );
			for ( int l = 0; l < num_; l++ ) {
				pointclouds_[ l ].UpdateAllPointPN( expand_ctr );
			}
		}
	}

	output_traj_.data_.clear();
	for ( int i = 0; i < ( int )pose_.size(); i++ ) {
		output_traj_.data_.push_back( FramedTransformation( i, i, i + 1, pose_[ i ] ) );
	}
	output_traj_.SaveToFile( pose_filename_ );

	ExpandCtr( thisCtr, expand_ctr );
	SaveCtr( expand_ctr, ctr_filename_ );
	SavePoints( expand_ctr, sample_filename_ );
}

void COptApp::InitCtr( Eigen::VectorXd & ctr )
{
	if ( init_ctr_file_.length() > 1 ) {
		FILE * f = fopen( init_ctr_file_.c_str(), "r" );
		if ( f != NULL ) {
			char buffer[ 1024 ];
			for ( int i = 0; i < num_ * nper_ / 3; i++ ) {
				fgets( buffer, 1024, f );
				sscanf( buffer, "%lf %lf %lf", &ctr( i * 3 ), &ctr( i * 3 + 1 ), &ctr( i * 3 + 2 ) );
			}
			fclose( f );
		}
	} else {
		for ( int l = 0; l < num_; l++ ) {
			for ( int i = 0; i <= resolution_; i++ ) {
				for ( int j = 0; j <= resolution_; j++ ) {
					for ( int k = 0; k <= resolution_; k++ ) {
						Eigen::Vector4d pos( i * unit_length_, j * unit_length_, k * unit_length_, 1 );
						Eigen::Vector4d ppos = ipose_[ l ] * pos;
						ctr( l * nper_ + GetIndex( i, j, k ) * 3 + 0 ) = ppos( 0 );
						ctr( l * nper_ + GetIndex( i, j, k ) * 3 + 1 ) = ppos( 1 );
						ctr( l * nper_ + GetIndex( i, j, k ) * 3 + 2 ) = ppos( 2 );
					}
				}
			}
		}
	}
}

void COptApp::InitCtrSLAC( Eigen::VectorXd & ctr, Eigen::VectorXd & thisCtr )
{
	for ( int i = 0; i <= resolution_; i++ ) {
		for ( int j = 0; j <= resolution_; j++ ) {
			for ( int k = 0; k <= resolution_; k++ ) {
				Eigen::Vector4d pos( i * unit_length_, j * unit_length_, k * unit_length_, 1 );
				ctr( num_ * 6 + GetIndex( i, j, k ) * 3 + 0 ) = pos( 0 );
				ctr( num_ * 6 + GetIndex( i, j, k ) * 3 + 1 ) = pos( 1 );
				ctr( num_ * 6 + GetIndex( i, j, k ) * 3 + 2 ) = pos( 2 );
			}
		}
	}
	if ( init_ctr_file_.length() > 1 ) {
		FILE * f = fopen( init_ctr_file_.c_str(), "r" );
		if ( f != NULL ) {
			char buffer[ 1024 ];
			for ( int i = 0; i < nper_ / 3; i++ ) {
				fgets( buffer, 1024, f );
				sscanf( buffer, "%lf %lf %lf", &thisCtr( num_ * 6 + i * 3 ), &thisCtr( num_ * 6 + i * 3 + 1 ), &thisCtr( num_ * 6 + i * 3 + 2 ) );
			}
			fclose( f );
		}
	} else {
		thisCtr = ctr;
	}
}

void COptApp::Pose2Ctr( std::vector< Eigen::Matrix4d > & pose, Eigen::VectorXd & ctr )
{
	for ( int l = 0; l < num_; l++ ) {
		for ( int i = 0; i <= resolution_; i++ ) {
			for ( int j = 0; j <= resolution_; j++ ) {
				for ( int k = 0; k <= resolution_; k++ ) {
					Eigen::Vector4d pos( i * unit_length_, j * unit_length_, k * unit_length_, 1 );
					Eigen::Vector4d ppos = pose[ l ] * pos;
					ctr( l * nper_ + GetIndex( i, j, k ) * 3 + 0 ) = ppos( 0 );
					ctr( l * nper_ + GetIndex( i, j, k ) * 3 + 1 ) = ppos( 1 );
					ctr( l * nper_ + GetIndex( i, j, k ) * 3 + 2 ) = ppos( 2 );
				}
			}
		}
	}
}

void COptApp::ExpandCtr( const Eigen::VectorXd & ctr, Eigen::VectorXd & expand_ctr )
{
	expand_ctr.setZero();
	for ( int l = 0; l < num_; l++ ) {
		for ( int i = 0; i < nper_ / 3; i++ ) {
			Eigen::Vector4d pos = pose_[ l ] * Eigen::Vector4d( ctr( num_ * 6 + i * 3 + 0 ), ctr( num_ * 6 + i * 3 + 1 ), ctr( num_ * 6 + i * 3 + 2 ), 1 );
			expand_ctr( l * nper_ + i * 3 + 0 ) = pos( 0 );
			expand_ctr( l * nper_ + i * 3 + 1 ) = pos( 1 );
			expand_ctr( l * nper_ + i * 3 + 2 ) = pos( 2 );
		}
	}
}

void COptApp::InitBaseAA( SparseMatrix & baseAA )
{
	TripletVector tri;

	for ( int l = 0; l < num_; l++ ) {
		HashSparseMatrix mat( l * nper_, l * nper_ );
		for ( int i = 0; i <= resolution_; i++ ) {
			for ( int j = 0; j <= resolution_; j++ ) {
				for ( int k = 0; k <= resolution_; k++ ) {
					int idx[ 2 ] = { GetIndex( i, j, k ) * 3, 0 };
					double val[ 2 ] = { 1, -1 };
					if ( i > 0 ) {
						idx[ 1 ] = GetIndex( i - 1, j, k ) * 3;
						mat.AddHessian2( idx, val, tri );
					}
					if ( i < resolution_ ) {
						idx[ 1 ] = GetIndex( i + 1, j, k ) * 3;
						mat.AddHessian2( idx, val, tri );
					}
					if ( j > 0 ) {
						idx[ 1 ] = GetIndex( i, j - 1, k ) * 3;
						mat.AddHessian2( idx, val, tri );
					}
					if ( j < resolution_ ) {
						idx[ 1 ] = GetIndex( i, j + 1, k ) * 3;
						mat.AddHessian2( idx, val, tri );
					}
					if ( k > 0 ) {
						idx[ 1 ] = GetIndex( i, j, k - 1 ) * 3;
						mat.AddHessian2( idx, val, tri );
					}
					if ( k < resolution_ ) {
						idx[ 1 ] = GetIndex( i, j, k + 1 ) * 3;
						mat.AddHessian2( idx, val, tri );
					}
				}
			}
		}
		if ( l == 0 ) {
			mat.Add( 0, 0, 1, tri );
			mat.Add( 1, 1, 1, tri );
			mat.Add( 2, 2, 1, tri );
		}
	}

	baseAA.setFromTriplets( tri.begin(), tri.end() );
}

void COptApp::InitBaseJJ( SparseMatrix & baseJJ )
{
	TripletVector tri;

	HashSparseMatrix mat( 6 * num_, 6 * num_ );
	for ( int i = 0; i <= resolution_; i++ ) {
		for ( int j = 0; j <= resolution_; j++ ) {
			for ( int k = 0; k <= resolution_; k++ ) {
				int idx[ 2 ] = { GetIndex( i, j, k ) * 3, 0 };
				double val[ 2 ] = { 1, -1 };
				if ( i > 0 ) {
					idx[ 1 ] = GetIndex( i - 1, j, k ) * 3;
					mat.AddHessian2( idx, val, tri );
				}
				if ( i < resolution_ ) {
					idx[ 1 ] = GetIndex( i + 1, j, k ) * 3;
					mat.AddHessian2( idx, val, tri );
				}
				if ( j > 0 ) {
					idx[ 1 ] = GetIndex( i, j - 1, k ) * 3;
					mat.AddHessian2( idx, val, tri );
				}
				if ( j < resolution_ ) {
					idx[ 1 ] = GetIndex( i, j + 1, k ) * 3;
					mat.AddHessian2( idx, val, tri );
				}
				if ( k > 0 ) {
					idx[ 1 ] = GetIndex( i, j, k - 1 ) * 3;
					mat.AddHessian2( idx, val, tri );
				}
				if ( k < resolution_ ) {
					idx[ 1 ] = GetIndex( i, j, k + 1 ) * 3;
					mat.AddHessian2( idx, val, tri );
				}
			}
		}
	}

	int base_anchor = GetIndex( resolution_ / 2, resolution_ / 2, 0 ) * 3;

	mat.Add( base_anchor + 0, base_anchor + 0, 1, tri );
	mat.Add( base_anchor + 1, base_anchor + 1, 1, tri );
	mat.Add( base_anchor + 2, base_anchor + 2, 1, tri );

	baseJJ.setFromTriplets( tri.begin(), tri.end() );
}

Eigen::Matrix3d COptApp::GetRotation( const int idx, const std::vector< int > & idxx, const Eigen::VectorXd & ictr, const Eigen::VectorXd & ctr )
{
	int n = ( int )idxx.size();
	Eigen::Matrix3d C = Eigen::Matrix3d::Zero();

	for ( int i = 0; i < n; i++ ) {
		Eigen::RowVector3d dif( ictr( idx ) - ictr( idxx[ i ] ), ictr( idx + 1 ) - ictr( idxx[ i ] + 1 ), ictr( idx + 2 ) - ictr( idxx[ i ] + 2 ) );
		Eigen::RowVector3d diff( ctr( idx ) - ctr( idxx[ i ] ), ctr( idx + 1 ) - ctr( idxx[ i ] + 1 ), ctr( idx + 2 ) - ctr( idxx[ i ] + 2 ) );
		C += dif.transpose() * diff;
	}

	Eigen::JacobiSVD< Eigen::Matrix3d > svd( C, Eigen::ComputeFullU | Eigen::ComputeFullV );
	Eigen::Matrix3d U = svd.matrixU();
	Eigen::Matrix3d V = svd.matrixV();
	Eigen::Matrix3d R = V * U.transpose();
	if ( R.determinant() < 0 ) {
		U( 0, 2 ) *= -1;
		U( 1, 2 ) *= -1;
		U( 2, 2 ) *= -1;
		R = V * U.transpose();
	}
	return R;
}

void COptApp::SaveCtr( const Eigen::VectorXd & ctr, std::string filename )
{
	PCL_INFO( "Save ctr to file %s ... ", filename.c_str() );
	FILE * f = fopen( filename.c_str(), "w" );
	if ( f != NULL ) {
		for ( int i = 0; i < num_ * nper_ / 3; i++ ) {
			fprintf( f, "%.10f %.10f %.10f\n", ctr( i * 3 ), ctr( i * 3 + 1 ), ctr( i * 3 + 2 ) );
		}
		fclose( f );
	}
	PCL_INFO( "Done.\n" );
}

void COptApp::SavePoints( const Eigen::VectorXd & ctr, std::string filename )
{
	if ( sample_num_ <= 0 )
		return;

	pcl::PointCloud< pcl::PointXYZRGBNormal > pcd;
	pcl::PointXYZRGBNormal pt;
	for ( int l = 0; l < num_; l++ ) {
		pointclouds_[ l ].UpdateAllNormal( ctr );
		for ( int i = 0; i < ( int )pointclouds_[ l ].points_.size(); i += sample_num_ ) {
			Eigen::Vector3d pos = pointclouds_[ l ].UpdatePoint( ctr, pointclouds_[ l ].points_[ i ] );
			Eigen::Vector3d norm( pointclouds_[ l ].points_[ i ].n_[ 0 ], pointclouds_[ l ].points_[ i ].n_[ 1 ], pointclouds_[ l ].points_[ i ].n_[ 2 ] );
			norm.normalize();
			pt.x = pos( 0 );
			pt.y = pos( 1 );
			pt.z = pos( 2 );
			pt.normal_x = norm( 0 );
			pt.normal_y = norm( 1 );
			pt.normal_z = norm( 2 );
			pcd.push_back( pt );
		}
	}

	PCL_INFO( "Save sample pcd into %s ... ", filename.c_str() );
	pcl::PCDWriter w;
	w.writeBinaryCompressed( filename, pcd );
}

void COptApp::Blacklist( std::string filename )
{
	blacklist_.clear();

	FILE * f = fopen( filename.c_str(), "r" );
	if ( f != NULL ) {
		char buffer[1024];
		int id;
		while ( fgets( buffer, 1024, f ) != NULL ) {
			if ( strlen( buffer ) > 0 && buffer[ 0 ] != '#' ) {
				sscanf( buffer, "%d", &id);
				blacklist_.insert( id );
			}
		}
		fclose ( f );
	}
}


#include <stdexcept>
#include <iostream>
#include <boost/algorithm/string.hpp>

#include "loguru.hpp"
#include "vm.h"
#include "utils.h"

VM::VM( int memSize ) : running_(false), pc_(0), relativeBase_(0)
{
    memory_.resize(memSize);

    inputCB_ = [](std::string s) -> int {
        std::cout << s << " ";
        big_int i = 0;
        std::cin >> i;
        return i;
    };

    outputCB_ = [](big_int &s) -> void {
        std::cout << ": " << s << std::endl;
    };
}

VM::VM( std::function<big_int(std::string)>  inputCB,
        std::function<void(big_int&)> ouputCB,
        int memSize
) : running_(false), pc_(0), relativeBase_(0)
{
    inputCB_ = inputCB;
    outputCB_ = ouputCB;
    memory_.resize(memSize);
}

void VM::loadIntCode(std::ifstream &in)
{
    std::string inputs;
    in >> inputs;
    loadIntCode(inputs);
}

void VM::loadIntCode(std::string &s)
{
    std::vector<std::string> sv;
    boost::split(sv, s, boost::is_any_of(","));
    for (unsigned int i=0; i<sv.size(); ++i)
        memory_[i] = std::stoi(sv[i]);
}

void VM::loadIntCode(vector_big_int &v)
{
    memory_ = v;
}

void VM::reset()
{
    for (auto &entry : memory_)
        entry = 0;
    pc_ = 0;
    relativeBase_ = 0;
}

void VM::terminate()
{
    running_ = false;
}

void VM::execute()
{
    running_ = true;

    while (running_)
    {
        LOG_F(9, "memory: %s", vectorToString(memory_).c_str());
        LOG_F(9, "pc: %d", pc_);

        switch(memory_[pc_] % 100)
        {
            case ADD: operators_.add(memory_, pc_, relativeBase_); break;
            case MUL: operators_.mul(memory_, pc_, relativeBase_); break;
            case INP: operators_.inp(memory_, pc_, relativeBase_, inputCB_); break;
            case PRT: operators_.prt(memory_, pc_, relativeBase_, outputCB_); break;
            case JMP: operators_.jmp(memory_, pc_, relativeBase_); break;
            case JMF: operators_.jmf(memory_, pc_, relativeBase_); break;
            case LTN: operators_.ltn(memory_, pc_, relativeBase_); break;
            case EQL: operators_.eql(memory_, pc_, relativeBase_); break;
            case REL: operators_.rel(memory_, pc_, relativeBase_); break;
            case HALT: running_ = false; break;
            default:
                LOG_F(ERROR, "Unknown op: %d", memory_[pc_]);
                throw std::runtime_error("Invalid op code.");
        };
    }
}


#include "cpp_hw.hpp"
#include "ValueObjects/vo_hw.hpp"
#include "AddinObjects/obj_hw.hpp"
#include <boost/shared_ptr.hpp>
#include <oh/repository.hpp>

//#include "quantLibAddinTest.hpp"
//#include <oh/objecthandler.hpp>
#include <ql/math/comparison.hpp>
#include <ql/quotes/simplequote.hpp>

static ObjectHandler::Repository repository;

bool QuantLibAddin::qlClose(double x, double y) {
    return QuantLib::close(x, y);
}

std::string QuantLibAddin::qlSimpleQuote(const std::string &objectID, double Value) {
        boost::shared_ptr<ObjectHandler::ValueObject> valueObject(
            new QuantLibAddin::ValueObjects::qlSimpleQuote(
                objectID, Value, false));
        boost::shared_ptr<ObjectHandler::Object> object(
            new QuantLibAddin::SimpleQuote(
                valueObject, Value, false));
        std::string returnValue =
            ObjectHandler::Repository::instance().storeObject(
                objectID, object, false, valueObject);
        return returnValue;
}

double QuantLibAddin::qlSimpleQuoteValue(const std::string &objectID) {
    OH_GET_REFERENCE(x, objectID, QuantLibAddin::SimpleQuote, QuantLib::SimpleQuote);
    return x->value();
}