ModelTracker.h

#pragma once
#include "VoxelGrid.h"
#include "VoxelGrid.cuh"
#include <Eigen\Dense>
#include "LieAlgebra.h"
#include <fstream>
#include <opencv\highgui.h>
#include <omp.h>
class ModelTracker :
    public VoxelGrid
{
private:
    double RAYTHRESHOLD, DELTADERIV, DIFFTHRES;
    int RESOLUTION;
    const int MAXITR = 12;
    const int MAXOPTITR = 15;
    const double MAXDISTANCE = 10;
    double interpolate(const Eigen::Vector3d &p);
    Eigen::Vector3d getVoxelDoubleCoord(const Eigen::Vector3d &p);
    Eigen::Vector3d getIntWeights(const Eigen::Vector3d &p);
    Eigen::Vector3i getVoxelIntCoord(const Eigen::Vector3d &p);

    double interpolate(const Eigen::Vector3i &p, const Eigen::Vector3d &w);

    bool checkIndices(const Eigen::Vector3i &p);
    bool checkIndices(const Eigen::Vector3d &p);
    bool checkGradient(const Eigen::VectorXd &grad);
    bool checkGradient(float *grad);
    bool checkWeights(int i, int j, int k);
    bool checkWeights(const Eigen::Vector3i &p);

    void computeAnalyticLSqRay(const Eigen::VectorXd &Xi, const cv::Mat &depthImg);
    double sumErrorIndPredicted(const cv::Mat &depthImg, const Eigen::VectorXd &Xi);

    Eigen::VectorXd getRayAnalyticGradient(Eigen::Vector3d &w, Eigen::Vector3d &t, Eigen::Vector3d &pLocal,
        const double V0Ray, const double V0Dx, const double V0Dy, const double V0Dz, const double delta,
        const double i, const double j, const double z);
    Eigen::MatrixXd getDerivative(Eigen::Vector3d &w, Eigen::Vector3d &x);
    double sumGPUErrorPredicted(const cv::Mat &depthImg,Eigen::VectorXd &Xitmp);
    void cpyToCamera(double *ptrCamGlobal, Eigen::Matrix4d &camera);
    cv::Mat getErrorMatrix(double *error, const int rows, const int cols);
    cv::Mat GPUrayCast(const Eigen::VectorXd &Xi, const cv::Mat &depthImg);
    void CPUcomputeAnalyticLSqRay(const Eigen::VectorXd &Xi, const cv::Mat &depthImg);
protected:
    Eigen::MatrixXd ALsq, ALsqRGB;
    Eigen::VectorXd bLsq, bLsqRGB;

public:
    ModelTracker(const double GRIDDIM, const int RESOLUTION, const double DELTA, const double EPSILON, const double SIGMA, const double FX, const double FY, const double CX, const double CY, const double raythreshold, const double deltaDeriv, const double diffThres);
    ~ModelTracker();
    void getRayErrorFunction(const Eigen::VectorXd &Xi, const cv::Mat &depthImg);
    double getRayError(const float i, const float j, const float z,
        const Eigen::Matrix3f &R, const Eigen::Vector3f &t, const Eigen::Vector3f &start);
    //double getRayError(const float i, const float j, const float z, const Eigen::Matrix3d &R, const Eigen::Vector3d &t);

    Eigen::VectorXd optimize(const Eigen::VectorXd &XiInit,
        const cv::Mat &depthImg);
    double traverseRay(const Eigen::Vector3d &ray, const Eigen::Vector3d &start, double pDist);
    cv::Mat rayCast(const Eigen::VectorXd &Xi, const cv::Mat &depthImg);
    double3 getStart();
    void testGPURayCast(const Eigen::VectorXd &Xi, const cv::Mat &depthImg);
    Eigen::VectorXd optimizeCPU(const Eigen::VectorXd &XiInit, const cv::Mat &depthImg);

};