void refineGraphSE3RgbdICP(const std::vector<Ptr<RgbdFrame> >& _frames,
                           const std::vector<Mat>& poses, const std::vector<PosesLink>& posesLinks,
                           const Mat& cameraMatrix, float pointsPart,
                           std::vector<Mat>& refinedPoses, std::vector<int>& frameIndices)
{
    CV_Assert(_frames.size() == poses.size());

    const int iterCount = 5;
    // TODO: find corresp to main API?
    // TODO: odom with one level here
    RgbdICPOdometry odom;
    vector<float> minGradientMagnitudes(1,10);
    vector<int> iterCounts(1,10);
    odom.set("maxPointsPart", pointsPart);
    odom.set("minGradientMagnitudes", Mat(minGradientMagnitudes).clone());
    odom.set("iterCounts", Mat(iterCounts).clone());
    odom.set("cameraMatrix", cameraMatrix);

    std::vector<Ptr<OdometryFrame> > frames(_frames.size());
    for(size_t i = 0; i < frames.size(); i++)
    {
        Mat gray;
        CV_Assert(_frames[i]->image.channels() == 3);
        cvtColor(_frames[i]->image, gray, CV_BGR2GRAY);
        Ptr<OdometryFrame> frame = new OdometryFrame(gray, _frames[i]->depth, _frames[i]->mask,
                                                     _frames[i]->normals, _frames[i]->ID);
        odom.prepareFrameCache(frame, OdometryFrame::CACHE_ALL);
        frames[i] = frame;
    }

    refinedPoses.resize(poses.size());
    for(size_t i = 0; i < poses.size(); i++)
        refinedPoses[i] = poses[i].clone();

    Mat cameraMatrix_64F;
    cameraMatrix.convertTo(cameraMatrix_64F, CV_64FC1);

    for(int iter = 0; iter < iterCount; iter++)
    {
        G2OLinearSolver* linearSolver =  createLinearSolver(DEFAULT_LINEAR_SOLVER_TYPE);
        G2OBlockSolver* blockSolver = createBlockSolver(linearSolver);
        g2o::OptimizationAlgorithm* nonLinerSolver = createNonLinearSolver(DEFAULT_NON_LINEAR_SOLVER_TYPE, blockSolver);
        g2o::SparseOptimizer* optimizer = createOptimizer(nonLinerSolver);

        fillGraphSE3RgbdICP(optimizer, frames, refinedPoses, posesLinks, cameraMatrix_64F, frameIndices);

        optimizer->initializeOptimization();
        const int optIterCount = 1;
        cout << "Vertices count: " << optimizer->vertices().size() << endl;
        cout << "Edges count: " << optimizer->edges().size() << endl;
        cout << "Start optimization " << endl;
        if(optimizer->optimize(optIterCount) != optIterCount)
        {
            optimizer->clear();
            delete optimizer;
            break;
        }
        cout << "Finish optimization " << endl;

        getSE3Poses(optimizer, frameIndices, refinedPoses);

        optimizer->clear();
        delete optimizer;
    }
}
void refineGraphSE3RgbdICPModel(std::vector<Ptr<RgbdFrame> >& _frames,
                                const std::vector<Mat>& poses, const std::vector<PosesLink>& posesLinks, const Mat& cameraMatrix,
                                std::vector<Mat>& refinedPoses, std::vector<int>& frameIndices)
{
    CV_Assert(_frames.size() == poses.size());
    g2o::Edge_V_V_GICPLandmark::initializeStaticMatrices(); // TODO: make this more correctly

    Mat cameraMatrix_64F, cameraMatrix_inv_64F;
    cameraMatrix.convertTo(cameraMatrix_64F, CV_64FC1);
    cameraMatrix_inv_64F = cameraMatrix_64F.inv();

    refinedPoses.resize(poses.size());
    for(size_t i = 0; i < poses.size(); i++)
        refinedPoses[i] = poses[i];

    RgbdICPOdometry odom;
    vector<float> minGradientMagnitudes(1,10);
    vector<int> iterCounts(1,10);
    odom.set("maxPointsPart", 1.);
    odom.set("minGradientMagnitudes", Mat(minGradientMagnitudes).clone());
    odom.set("iterCounts", Mat(iterCounts).clone());
    odom.set("cameraMatrix", cameraMatrix);

    std::vector<Ptr<OdometryFrame> > frames(_frames.size());
    for(size_t i = 0; i < frames.size(); i++)
    {
        //frames[i]->releasePyramids();
        Mat gray;
        CV_Assert(_frames[i]->image.channels() == 3);
        cvtColor(_frames[i]->image, gray, CV_BGR2GRAY);
        Ptr<OdometryFrame> fr = new OdometryFrame(gray, _frames[i]->depth, _frames[i]->mask,
                                                  _frames[i]->normals, _frames[i]->ID);
        odom.prepareFrameCache(fr, OdometryFrame::CACHE_ALL);
        frames[i] = fr;
    }

    const double maxDepthDiff = 0.002;
    for(size_t i = 0; i < frames.size(); i++)
    {
        frames[i]->releasePyramids();
        odom.prepareFrameCache(frames[i], OdometryFrame::CACHE_ALL);
    }

    const int iterCount = 3;//7
    const int minCorrespCount = 3;

    const double maxTranslation = 0.20;
    const double maxRotation = 30;
    for(int iter = 0; iter < iterCount; iter++)
    {
        G2OLinearSolver* linearSolver =  createLinearSolver(DEFAULT_LINEAR_SOLVER_TYPE);
        G2OBlockSolver* blockSolver = createBlockSolver(linearSolver);
        g2o::OptimizationAlgorithm* nonLinerSolver = createNonLinearSolver(DEFAULT_NON_LINEAR_SOLVER_TYPE, blockSolver);
        g2o::SparseOptimizer* optimizer = createOptimizer(nonLinerSolver);

        fillGraphSE3RgbdICP(optimizer, frames, refinedPoses, posesLinks, cameraMatrix_64F, frameIndices);

        vector<Mat> vertexIndices(frameIndices.size());
        int vertexIdx = frames.size();
        for(size_t currIdx = 0; currIdx < frameIndices.size(); currIdx++)
        {
            int currFrameIdx = frameIndices[currIdx];

            vertexIndices[currIdx] = Mat(frames[currFrameIdx]->image.size(), CV_32SC1, Scalar(-1));

            Mat& curVertexIndices = vertexIndices[currIdx];
            const Mat& curCloud = frames[currFrameIdx]->pyramidCloud[0];
            const Mat& curNormals = frames[currFrameIdx]->normals;

            // compute count of correspondences
            Mat correspsCounts = Mat(frames[currFrameIdx]->image.size(), CV_32SC1, Scalar(0));
            for(size_t prevIdx = 0; prevIdx < frameIndices.size(); prevIdx++)
            {
                int prevFrameIdx = frameIndices[prevIdx];
                if(currFrameIdx == prevFrameIdx)
                    continue;

                Mat curToPrevRt = refinedPoses[prevFrameIdx].inv(DECOMP_SVD) * refinedPoses[currFrameIdx];
                if(tvecNorm(curToPrevRt) > maxTranslation || rvecNormDegrees(curToPrevRt) > maxRotation)
                    continue;

                Mat transformedPrevNormals;
                perspectiveTransform(frames[prevFrameIdx]->pyramidNormals[0], transformedPrevNormals,
                                     curToPrevRt.inv(DECOMP_SVD));
                Mat corresps;
                computeCorrespsFiltered(cameraMatrix_64F, cameraMatrix_inv_64F, curToPrevRt.inv(DECOMP_SVD),
                                        frames[currFrameIdx]->depth,
                                        frames[currFrameIdx]->mask,
                                        frames[prevFrameIdx]->depth,
                                        frames[prevFrameIdx]->pyramidNormalsMask[0],
                                        maxDepthDiff, corresps,
                                        frames[currFrameIdx]->pyramidNormals[0],
                                        transformedPrevNormals,
                                        frames[currFrameIdx]->image,
                                        frames[prevFrameIdx]->image);

                for(int v0 = 0; v0 < corresps.rows; v0++)
                {
                    for(int u0 = 0; u0 < corresps.cols; u0++)
                    {
                        int c = corresps.at<int>(v0, u0);
                        if(c != -1)
                            correspsCounts.at<int>(v0,u0)++;
                    }
                }
            }

            // set up edges
            for(size_t prevIdx = 0; prevIdx < frameIndices.size(); prevIdx++)
            {
                int prevFrameIdx = frameIndices[prevIdx];
                if(currFrameIdx == prevFrameIdx)
                    continue;

                const Mat& prevCloud = frames[prevFrameIdx]->pyramidCloud[0];
                const Mat& prevNormals = frames[prevFrameIdx]->normals;
                Mat curToPrevRt = refinedPoses[prevFrameIdx].inv(DECOMP_SVD) * refinedPoses[currFrameIdx];
                if(tvecNorm(curToPrevRt) > maxTranslation || rvecNormDegrees(curToPrevRt) > maxRotation)
                    continue;

                Mat transformedPrevNormals;
                perspectiveTransform(frames[prevFrameIdx]->pyramidNormals[0], transformedPrevNormals,
                                     curToPrevRt.inv(DECOMP_SVD));
                Mat corresps;
                computeCorrespsFiltered(cameraMatrix_64F, cameraMatrix_inv_64F, curToPrevRt.inv(DECOMP_SVD),
                                        frames[currFrameIdx]->depth,
                                        frames[currFrameIdx]->mask,
                                        frames[prevFrameIdx]->depth,
                                        frames[prevFrameIdx]->pyramidNormalsMask[0],
                                        maxDepthDiff, corresps,
                                        frames[currFrameIdx]->pyramidNormals[0],
                                        transformedPrevNormals,
                                        frames[currFrameIdx]->image,
                                        frames[prevFrameIdx]->image);

                std::cout << "landmarks: iter " << iter << "; cur " << currFrameIdx << "; prev " << prevFrameIdx << "; corresps " << countNonZero(corresps != -1) << std::endl;

                // poses and edges for points3d
                for(int v0 = 0; v0 < corresps.rows; v0++)
                {
                    for(int u0 = 0; u0 < corresps.cols; u0++)
                    {
                        int c = corresps.at<int>(v0, u0);
                        if(c == -1)
                            continue;

                        if(correspsCounts.at<int>(v0,u0) < minCorrespCount)
                            continue;

                        int u1_, v1_;
                        get2shorts(c, u1_, v1_);

                        const Rect rect(0,0,curCloud.cols, curCloud.rows);
                        const int Rad = 0;
                        for(int v1 = v1_ - Rad; v1 <= v1_ + Rad; v1++)
                        {
                            for(int u1 = u1_ - Rad; u1 <= u1_ + Rad; u1++)
                            {
                                if(rect.contains(Point(u1, v1)) && !cvIsNaN(prevCloud.at<Point3f>(v1,u1).x) &&
                                   std::abs(prevCloud.at<Point3f>(v1,u1).z - prevCloud.at<Point3f>(v1_,u1_).z) < maxDepthDiff)
                                {
                                    Eigen::Vector3d pt_prev, pt_cur, norm_prev, norm_cur, global_norm_prev;
                                    {
                                        pt_prev = cvtPoint_ocv2egn(prevCloud.at<Point3f>(v1,u1));
                                        norm_prev = cvtPoint_ocv2egn(prevNormals.at<Point3f>(v1,u1));

                                        vector<Point3f> tp;
                                        perspectiveTransform(vector<Point3f>(1, prevNormals.at<Point3f>(v1,u1)),
                                                             tp, refinedPoses[prevFrameIdx]);
                                        CV_Assert(isValidDepth(tp[0].z));
                                        tp[0] *= 1./cv::norm(tp[0]);
                                        global_norm_prev = cvtPoint_ocv2egn(tp[0]);

                                        perspectiveTransform(vector<Point3f>(1, curCloud.at<Point3f>(v0,u0)),
                                                             tp, refinedPoses[currFrameIdx]);
                                        CV_Assert(isValidDepth(tp[0].z));
                                        pt_cur = cvtPoint_ocv2egn(tp[0]);
                                        perspectiveTransform(vector<Point3f>(1, curNormals.at<Point3f>(v0,u0)),
                                                             tp, refinedPoses[currFrameIdx]);
                                        tp[0] *= 1./cv::norm(tp[0]);
                                        CV_Assert(isValidDepth(tp[0].z));
                                        norm_cur = cvtPoint_ocv2egn(tp[0]);
                                    }

                                    // add new pose
                                    if(curVertexIndices.at<int>(v0,u0) == -1)
                                    {
                                        g2o::VertexPointXYZ* modelPoint = new g2o::VertexPointXYZ;
                                        modelPoint->setId(vertexIdx);
                                        modelPoint->setEstimate(pt_cur);
                                        modelPoint->setMarginalized(true);
                                        optimizer->addVertex(modelPoint);

                                        curVertexIndices.at<int>(v0,u0) = vertexIdx;
                                        vertexIdx++;
                                    }

                                    int vidx = curVertexIndices.at<int>(v0,u0);

                                    g2o::Edge_V_V_GICPLandmark * e = new g2o::Edge_V_V_GICPLandmark();
                                    e->setVertex(0, optimizer->vertex(vidx));
                                    e->setVertex(1, optimizer->vertex(prevFrameIdx));

                                    g2o::EdgeGICP meas;
                                    meas.pos0 = pt_cur;
                                    meas.pos1 = pt_prev;
                                    meas.normal0 = norm_cur;
                                    meas.normal1 = norm_prev;

                                    e->setMeasurement(meas);
                                    meas = e->measurement();

//                                    e->information() = meas.prec0(0.01);
                                    meas.normal1 = global_norm_prev; // to get global covariation
                                    e->information() = 0.001 * (meas.cov0(1.).inverse() + meas.cov1(1.).inverse());
                                    meas.normal1 = norm_prev; // set local normal

                                    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                                    e->setRobustKernel(rk);

                                    optimizer->addEdge(e);
                                }
                            }
                        }
                    }
                }
            }
        }

        optimizer->initializeOptimization();
        const int optIterCount = 1;
        cout << "Vertices count: " << optimizer->vertices().size() << endl;
        cout << "Edges count: " << optimizer->edges().size() << endl;
        if(optimizer->optimize(optIterCount) != optIterCount)
        {
            optimizer->clear();
            delete optimizer;
            break;
        }

        getSE3Poses(optimizer, frameIndices, refinedPoses);

        // update points poses
        cout << "Updating model points..." << endl;
        CV_Assert(frameIndices.size() ==  vertexIndices.size());
        for(size_t i = 0; i < frameIndices.size(); i++)
        {
            int frameIdx = frameIndices[i];
            const Mat& curVertexIndices = vertexIndices[i];
            Mat& depth = frames[frameIdx]->depth;
            for(int y = 0; y < curVertexIndices.rows; y++)
            {
                for(int x = 0; x < curVertexIndices.cols; x++)
                {
                    int vidx = curVertexIndices.at<int>(y,x);
                    if(vidx < 0)
                        continue;

                    Point3f p;
                    {
                        g2o::VertexPointXYZ* v = dynamic_cast<g2o::VertexPointXYZ*>(optimizer->vertex(vidx));
                        Eigen::Vector3d ep = v->estimate();
                        p.x = ep[0]; p.y = ep[1]; p.z = ep[2];
                    }
                    vector<Point3f> tp;
                    perspectiveTransform(vector<Point3f>(1, p), tp, refinedPoses[frameIdx].inv(DECOMP_SVD));
                    CV_Assert(isValidDepth(tp[0].z));
                    depth.at<float>(y,x) = tp[0].z;
                }
            }

            frames[frameIdx]->pyramidMask.clear();
            frames[frameIdx]->pyramidDepth.clear();
            frames[frameIdx]->normals.release();
            frames[frameIdx]->pyramidNormals.clear();
            frames[frameIdx]->pyramidCloud.clear();
            frames[frameIdx]->pyramidNormalsMask.clear();
            odom.prepareFrameCache(frames[frameIdx], OdometryFrame::CACHE_ALL);
        }

        optimizer->clear();
        delete optimizer;
    }

    // remove points without correspondences
    for(size_t currIdx = 0; currIdx < frameIndices.size(); currIdx++)
    {
        int currFrameIdx = frameIndices[currIdx];
        // compute count of correspondences
        Mat& curCloud = frames[currFrameIdx]->pyramidCloud[0];
        Mat& curDepth = frames[currFrameIdx]->depth;
        Mat correspsCounts = Mat(frames[currFrameIdx]->image.size(), CV_32SC1, Scalar(0));
        for(size_t prevIdx = 0; prevIdx < frameIndices.size(); prevIdx++)
        {
            int prevFrameIdx = frameIndices[prevIdx];
            if(currFrameIdx == prevFrameIdx)
                continue;

            Mat curToPrevRt = refinedPoses[prevFrameIdx].inv(DECOMP_SVD) * refinedPoses[currFrameIdx];
            Mat transformedPrevNormals;
            perspectiveTransform(frames[prevFrameIdx]->pyramidNormals[0], transformedPrevNormals,
                                 curToPrevRt.inv(DECOMP_SVD));
            Mat corresps;
            computeCorrespsFiltered(cameraMatrix_64F, cameraMatrix_inv_64F, curToPrevRt.inv(DECOMP_SVD),
                                    frames[currFrameIdx]->depth,
                                    frames[currFrameIdx]->mask,
                                    frames[prevFrameIdx]->depth,
                                    frames[prevFrameIdx]->pyramidNormalsMask[0],
                                    0.003, corresps,
                                    frames[currFrameIdx]->pyramidNormals[0],
                                    transformedPrevNormals,
                                    frames[currFrameIdx]->image,
                                    frames[prevFrameIdx]->image);

            for(int v0 = 0; v0 < corresps.rows; v0++)
            {
                for(int u0 = 0; u0 < corresps.cols; u0++)
                {
                    int c = corresps.at<int>(v0, u0);
                    if(c != -1)
                        correspsCounts.at<int>(v0,u0)++;
                }
            }
        }

        for(int v0 = 0; v0 < correspsCounts.rows; v0++)
        {
            for(int u0 = 0; u0 < correspsCounts.cols; u0++)
            {
                if(correspsCounts.at<int>(v0,u0) < minCorrespCount)
                {
                    curCloud.at<Point3f>(v0,u0) = Point3f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN());
                }

                curDepth.at<float>(v0,u0) = curCloud.at<Point3f>(v0,u0).z;
            }
        }
    }

    for(size_t i = 0; i < frames.size(); i++)
    {
        Ptr<RgbdFrame> fr = new RgbdFrame(_frames[i]->image, frames[i]->depth, _frames[i]->mask, frames[i]->normals, _frames[i]->ID);
        _frames[i] = fr;
    }
}
void refineGraphSE3RgbdICP(const std::vector<Ptr<RgbdFrame> >& _frames,
                           const std::vector<Mat>& poses, const std::vector<PosesLink>& posesLinks,
                           const Mat& cameraMatrix, float pointsPart,
                           std::vector<Mat>& refinedPoses, std::vector<int>& frameIndices)
{
    CV_Assert(_frames.size() == poses.size());

    // TODO: find corresp to main API?
    const int levelsCount = 3;
    vector<float> minGradientMagnitudes(levelsCount);
    minGradientMagnitudes[0] = 10;
    minGradientMagnitudes[1] = 5;
    minGradientMagnitudes[2] = 1;
    vector<int> iterCounts(levelsCount);
    iterCounts[0] = 3;
    iterCounts[1] = 4;
    iterCounts[2] = 4;

    RgbdICPOdometry odom;
    odom.set("maxPointsPart", pointsPart);
    odom.set("minGradientMagnitudes", Mat(minGradientMagnitudes));
    odom.set("iterCounts", Mat(iterCounts));
    odom.set("cameraMatrix", cameraMatrix);

    std::vector<Ptr<OdometryFrame> > frames(_frames.size());
    for(size_t i = 0; i < frames.size(); i++)
    {
        Mat gray;
        CV_Assert(_frames[i]->image.channels() == 3);
        cvtColor(_frames[i]->image, gray, CV_BGR2GRAY);
        Ptr<OdometryFrame> frame = new OdometryFrame(gray, _frames[i]->depth, _frames[i]->mask,
                                                     _frames[i]->normals, _frames[i]->ID);
        odom.prepareFrameCache(frame, OdometryFrame::CACHE_ALL);
        frames[i] = frame;
    }

    refinedPoses.resize(poses.size());
    for(size_t i = 0; i < poses.size(); i++)
        refinedPoses[i] = poses[i].clone();

    vector<Mat> pyramidCameraMatrix;
    buildPyramidCameraMatrix(cameraMatrix, iterCounts.size(), pyramidCameraMatrix);

    for(int level = iterCounts.size() - 1; level >= 0; level--)
    {
        for(int iter = 0; iter < iterCounts[level]; iter++)
        {
            G2OLinearSolver* linearSolver =  createLinearSolver(DEFAULT_LINEAR_SOLVER_TYPE);
            G2OBlockSolver* blockSolver = createBlockSolver(linearSolver);
            g2o::OptimizationAlgorithm* nonLinerSolver = createNonLinearSolver(DEFAULT_NON_LINEAR_SOLVER_TYPE, blockSolver);
            g2o::SparseOptimizer* optimizer = createOptimizer(nonLinerSolver);

            double maxTranslation = DBL_MAX;
            double maxRotation = DBL_MAX;
            double maxDepthDiff = 0.07;
            if(level == 0)
            {
                maxTranslation = 0.20;
                maxRotation = 30;
            }

            fillGraphSE3RgbdICP(optimizer, level, frames, refinedPoses, posesLinks, pyramidCameraMatrix[level], frameIndices,
                                maxTranslation, maxRotation, maxDepthDiff);

            optimizer->initializeOptimization();
            const int optIterCount = 1;
            cout << "Vertices count: " << optimizer->vertices().size() << endl;
            cout << "Edges count: " << optimizer->edges().size() << endl;
            cout << "Start optimization " << endl;
            if(optimizer->optimize(optIterCount) != optIterCount)
            {
                optimizer->clear();
                delete optimizer;
                break;
            }
            cout << "Finish optimization " << endl;

            getSE3Poses(optimizer, frameIndices, refinedPoses);

            optimizer->clear();
            delete optimizer;
        }
    }
}