MultipleDepthWarp.cpp

/*
 * MultipleDepthWarp.cpp
 *
 *  Created on: Jan 28, 2010
 *      Author: africk
 */

#include <OpenGLFramework/SpecializedBatches/MultipleDepthWarp.hh>
#include <Utils/IOUtils.hh>

using namespace std;
using namespace BIAS;

MultipleDepthWarp::MultipleDepthWarp() {

    minDepth_ = 1000;
    maxDepth_ = 7000;
    numberOfSampForDepthCorr_ = 1000;

    depthCorrTexLUTP_ = NULL;

    useProjectiveTexturing_ = false;
    zNear_ = 100;
    zFar_ = 8000;
    SetDiscardAngle(91);
    initCalled_ = false;
    depthScaleBeforeWarping_ = 1.0;
    flop_ = false;
    createDepthMaps_ = true;
    multiStereoModus_ = false;
    current_index_ = 0;
    depthCorType_ = NONE;
    warpType_ = DISPARITY;
    flip_= false;

    clearColorBuffer_ = true;
    baseline_ = 0.0;
    focallengthContained_ = true;
}

MultipleDepthWarp::~MultipleDepthWarp() {
    for (unsigned int i = 0; i < depthMaps_.size(); i++) {
        if (createDepthMaps_) {
            delete depthMaps_[i];
        }
        delete resultDispTextures_[i];
    }
}

int MultipleDepthWarp::Init(const ProjectionParametersPerspective& targetCam,
                            std::vector<Projection>& sourceCameras, bool createDepthMaps) {
    if (!initCalled_) {
        createDepthMaps_ = createDepthMaps;
        if (sourceCameras.empty()) {
            BIASERR("sourceCameras.size() == 0 ");
            return -1;
        }

        targetParams_ = targetCam;
        sourceParams_ = sourceCameras;

        unsigned int width, height;
        targetParams_.GetImageSize(width, height);
        viewport_.SetSize(width, height);
        viewport_.SetOrigin(0, 0);

        modelViewMatrix_.MakeViewMatrixNew(targetParams_);
        projectionMatrix_.MakeProjectionMatrixNew(targetParams_, zNear_, zFar_);

        depthMaps_.resize(sourceParams_.size());
        resultDispTextures_.resize(sourceParams_.size());
        resultColorTextures_.resize(sourceParams_.size());

        for (unsigned int i = 0; i < depthMaps_.size(); i++) {
            if (createDepthMaps_) {
                depthMaps_[i] = new glfTexture2D();

                depthMaps_[i]->Create();
                depthMaps_[i]->SetMinFilter(GL_NEAREST);
                depthMaps_[i]->SetMagFilter(GL_NEAREST);
                depthMaps_[i]->SetWrapS(GL_CLAMP);
                depthMaps_[i]->SetWrapT(GL_CLAMP);
                batch_.SetTexture(depthMaps_[i], i);
            }

            resultDispTextures_[i] = new glfTexture2D;
            resultDispTextures_[i]->Create();
            resultDispTextures_[i]->SetMagFilter(GL_NEAREST);
            resultDispTextures_[i]->SetMinFilter(GL_NEAREST);
            resultDispTextures_[i]->SetWrapS(GL_CLAMP);
            resultDispTextures_[i]->SetWrapT(GL_CLAMP);
            resultDispTextures_[i]->Allocate(width, height, GL_LUMINANCE32F_ARB);

            resultColorTextures_[i] = new glfTexture2D;
            resultColorTextures_[i]->Create();
            resultColorTextures_[i]->SetMagFilter(GL_LINEAR);
            resultColorTextures_[i]->SetMinFilter(GL_LINEAR);
            resultColorTextures_[i]->SetWrapS(GL_CLAMP);
            resultColorTextures_[i]->SetWrapT(GL_CLAMP);
            resultColorTextures_[i]->Allocate(width, height, GL_RGB);
        }

        projectiveTexture_.Create();
        projectiveTexture_.SetMinFilter(GL_LINEAR);
        projectiveTexture_.SetMagFilter(GL_LINEAR);
        projectiveTexture_.SetWrapS(GL_CLAMP);
        projectiveTexture_.SetWrapT(GL_CLAMP);

        batch_.SetTexture(&projectiveTexture_, depthMaps_.size());

        ProjectionParametersPerspectiveDepth
                * paramsDepth =
                dynamic_cast<ProjectionParametersPerspectiveDepth*> (sourceParams_[0].GetParameters());
        if (paramsDepth != NULL) {
            CreateDepthCorrectionLUT_(*paramsDepth);
        }

        depthBufferMode_.SetDepthTest(true);
        batch_.SetDepthBufferMode(&depthBufferMode_);
        batch_.SetProjectionMatrix(&projectionMatrix_);
        batch_.SetModelViewMatrix(&modelViewMatrix_);
        batch_.SetTextureMatrix(&textureMatrix_, 0);
        batch_.SetViewport(&viewport_);

        resultZBufferTexture_.Create();
        resultZBufferTexture_.SetMagFilter(GL_NEAREST);
        resultZBufferTexture_.SetMinFilter(GL_NEAREST);
        resultZBufferTexture_.SetWrapS(GL_CLAMP);
        resultZBufferTexture_.SetWrapT(GL_CLAMP);
        resultZBufferTexture_.Allocate(width, height, GL_DEPTH_COMPONENT);

        renderTarget_.Create();
        renderTarget_.AttachTexture(*resultDispTextures_[0],
                                    GL_COLOR_ATTACHMENT0_EXT);
        renderTarget_.AttachTexture(*resultColorTextures_[0],
                                    GL_COLOR_ATTACHMENT1_EXT);
        renderTarget_.AttachTexture(resultZBufferTexture_,
                                    GL_DEPTH_ATTACHMENT_EXT);
        renderTarget_.CheckComplete();

        vector<GLenum> drawbuffers(2);
        drawbuffers[0] = GL_COLOR_ATTACHMENT0_EXT;
        drawbuffers[1] = GL_COLOR_ATTACHMENT1_EXT;
        renderTarget_.SetDrawBuffers(drawbuffers);

        batch_.SetRenderTarget(&renderTarget_);
        batch_.SetShaderProgram(&shaderProgram_);
        if (multiStereoModus_)   {
            SetupShaders_(1);
        }  else {
            SetupShaders_(0);
        }
        SetupPrimitiveRendering_();
    }

    initCalled_ = true;
    return 0;
}

void MultipleDepthWarp::ChangeTargetCam(const ProjectionParametersPerspective& targetCam) {
  targetParams_ = targetCam;

  unsigned int width, height;
  targetParams_.GetImageSize(width, height);
  viewport_.SetSize(width, height);
  viewport_.SetOrigin(0, 0);

  modelViewMatrix_.MakeViewMatrixNew(targetParams_);
  projectionMatrix_.MakeProjectionMatrixNew(targetParams_, zNear_, zFar_);
}

int MultipleDepthWarp::SetProjectiveTextureCam(const ProjectionParametersPerspective& textureCam)
{
    if (!initCalled_) {
        return -1;
    }

    textureMatrix_.MakeTextureMatrixNew(textureCam,false);

    return 0;

}

void
        MultipleDepthWarp::UploadProjectiveTexture(const Image<unsigned char>& projectiveTexture)
{
    projectiveTexture_.UploadImage(projectiveTexture);
}

int MultipleDepthWarp::UploadDepthMap(Image<float>& depthMap,
                                      unsigned int index) {
    if (index >= depthMaps_.size()) {
        BIASERR("could not upload depth map, index " << index << " out of bound " << depthMaps_.size());
        return -1;
    }

    depthMaps_[index]->UploadImage(depthMap);

    return 0;
}

int MultipleDepthWarp::SetDepthMap(glfTexture2D* depthMap, unsigned int index) {
    if (createDepthMaps_) {
        BIASERR("to set depth map, Init must be called with createDepthMaps = false" << endl);
        return -2;
    }
    if (index >= depthMaps_.size()) {
        BIASERR("could not set depth map, index " << index << " out of bound " << depthMaps_.size());
        return -1;
    }

    depthMaps_[index] = depthMap;

    return 0;
}

int MultipleDepthWarp::Warp(unsigned int index) {
    if (initCalled_) {
        if (index < sourceParams_.size()) {
            current_index_ = index;
            renderTarget_.AttachTexture(*resultDispTextures_[index],
                                        GL_COLOR_ATTACHMENT0_EXT);
            renderTarget_.AttachTexture(*resultColorTextures_[index],
                                        GL_COLOR_ATTACHMENT1_EXT);
            renderTarget_.ClearDepthBuffer(1.0);
            if (clearColorBuffer_) {
                renderTarget_.ClearColorBuffer(0, 0, 0, 0);
            }
            unsigned int width, height;
            sourceParams_[index].GetParameters()->GetImageSize(width, height);
            unsigned int numIndicesPerStrip = width * 2;
            unsigned int first = index * numIndicesPerStrip * (height - 1);
            unsigned int targetWidth, targetHeight;
            targetParams_.GetImageSize(targetWidth, targetHeight);

            shaderProgram_.SetUniform("imageWidth", (float) targetWidth);
            shaderProgram_.SetUniform("depthScaleBefore",
                                      depthScaleBeforeWarping_);
            shaderProgram_.SetUniform("depthImage", (int) index);
            shaderProgram_.SetUniform("C", cameraCenters_[index]);
            shaderProgram_.SetUniform("thr", thr_);
           // shaderProgram_.SetUniform("dist_from_cam", dist_from_cam_);
           // shaderProgram_.SetUniform("disp_for_dist_from_cam", disp_);
            if(focallengthContained_) {
              shaderProgram_.SetUniform("baselineFocallength", baseline_);
            } else {
              shaderProgram_.SetUniform("baselineFocallength", baseline_*(float)targetParams_.GetFocalLength());
            }
            shaderProgram_.SetUniform("warpType", (int) warpType_);
            if (flop_) {
                shaderProgram_.SetUniform("flop", (int) 1);
            } else {
                shaderProgram_.SetUniform("flop", (int) 0);
            }

            for (unsigned int y = 0; y < height - 1; y++) {
                batch_.SetRange(first, numIndicesPerStrip);
                batch_.Draw();
                first += numIndicesPerStrip;
            }
        } else {
            BIASERR("index exceeds number of source parameters");
            return -1;
        }

    } else {
        BIASERR("call SetProjectios(...) first");
        return -1;
    }

    return 0;
}

int MultipleDepthWarp::Warp() {
    if (initCalled_) {
        renderTarget_.ClearDepthBuffer(1.0);
        if (clearColorBuffer_) {
            renderTarget_.ClearColorBuffer(0, 0, 0, 0);
        }
        unsigned int first = 0;
        for (unsigned int i = 0; i < sourceParams_.size(); i++) {

            unsigned int targetWidth, targetHeight;
            targetParams_.GetImageSize(targetWidth, targetHeight);

            shaderProgram_.SetUniform("depthImage", (int) i);
            shaderProgram_.SetUniform("projTexture", (int) depthMaps_.size());

            shaderProgram_.SetUniform("C", cameraCenters_[i]);
            if (flop_) {
                shaderProgram_.SetUniform("flop", (int) 1);
            } else {
                shaderProgram_.SetUniform("flop", (int) 0);
            }
            if (flip_) {
                shaderProgram_.SetUniform("flip", (int) 1);
            } else {
                shaderProgram_.SetUniform("flip", (int) 0);
            }

            unsigned int width, height;
            sourceParams_[i].GetParameters()->GetImageSize(width, height);
            if (!multiStereoModus_) {
                shaderProgram_.SetUniform("imageWidth", (float) targetWidth);
                shaderProgram_.SetUniform("depthScaleBefore",
                                          depthScaleBeforeWarping_);
                shaderProgram_.SetUniform("thr", thr_);
                shaderProgram_.SetUniform("corrTexture", (int) (depthMaps_.size() + 1));
                if (useProjectiveTexturing_) {
                    shaderProgram_.SetUniform("useProjTex", (int) 1);
                } else {
                    shaderProgram_.SetUniform("useProjTex", (int) 0);
                }
                shaderProgram_.SetUniform("minDepth", minDepth_);
                shaderProgram_.SetUniform("maxDepth", maxDepth_);
                shaderProgram_.SetUniform("depthRange", depthRange_);
//                shaderProgram_.SetUniform("dist_from_cam", dist_from_cam_);
//                shaderProgram_.SetUniform("disp_for_dist_from_cam", disp_);
                if(focallengthContained_) {
                  shaderProgram_.SetUniform("baselineFocallength", baseline_);
                } else {
                  shaderProgram_.SetUniform("baselineFocallength", baseline_*(float)targetParams_.GetFocalLength());
                }
             //   cout << "warp type = " << warpType_ << endl;
                shaderProgram_.SetUniform("warpType", (int) warpType_);
                shaderProgram_.SetUniform("sourceImageWidth", (float) width);
                shaderProgram_.SetUniform("sourceImageHeight", (float) height);

                if (depthCorType_ == LINEAR) {
                    ProjectionParametersPerspectiveDepth
                            * paramsDepth =
                            dynamic_cast<ProjectionParametersPerspectiveDepth*> (sourceParams_[i].GetParameters());
                    std::vector<double> params;
                    paramsDepth->GetDepthCalibrationParameters(params);
                    shaderProgram_.SetUniform("d0", (float) params[0]);
                    shaderProgram_.SetUniform("d1", (float) params[1]);

                }

                shaderProgram_.SetUniform("depthCorType", (int) depthCorType_);
            }



            batch_.SetTexture(&projectiveTexture_,0);
            batch_.SetTexture(depthMaps_[i], i);

            unsigned int numIndicesPerStrip = width * 2;
            for (unsigned int y = 0; y < height - 1; y++) {
                batch_.SetRange(first, numIndicesPerStrip);
                batch_.Draw();
                first += numIndicesPerStrip;
            }
        }
    } else {
        BIASERR("call SetProjectios(...) first");
        return -1;
    }

    return 0;
}

int MultipleDepthWarp::SetupNViewsWarp(const double& distToScreen,
                                       const double& zoom, const double& shift,
                                       unsigned int numberOfViews, unsigned int centralViewIndex, bool separate)
{
    if (initCalled_) {

        double dScreen = distToScreen;

        if (dScreen <= 0) {
            dScreen = 10e-9;
        }

        numberOfViews_ = numberOfViews;
        separate_ = separate;

        if (resultDepthMapsForNViews_.size() != numberOfViews) {

            for (unsigned int i = 0; i < resultDepthMapsForNViews_.size(); i++) {
                delete resultDepthMapsForNViews_[i];
                delete resultTexturesForNViews_[i];
            }

            resultDepthMapsForNViews_.resize(numberOfViews);
            resultTexturesForNViews_.resize(numberOfViews);
            modelViewMatricesForNViewWarp_.resize(numberOfViews);
            projectionMatricesForNViewWarp_.resize(numberOfViews);
        }
        unsigned int width, height;
        targetParams_.GetImageSize(width,height);

        for (unsigned int i = 0; i < numberOfViews; i++) {
            ProjectionParametersPerspective ppp = targetParams_;

            int index = (int) i - (int) centralViewIndex;

            HomgPoint2D leftTop(0.5,0.5,1);
            HomgPoint2D rightBottom((double)width-0.5, (double) height - 0.5,1);

            Vector3<double> p, pos;
            ppp.UnProjectLocal(leftTop, pos, p, true);

            p = (p / p[2]) * dScreen;

            p[0] /= zoom;
            p[1] /=zoom;

            p[0] = p[0]- (index* shift);

            Vector3<double> q;
            ppp.UnProjectLocal(rightBottom, pos, q, true);

            q = (q / q[2]) * dScreen;

            q[0] /= zoom;
            q[1] /= zoom;

            q[0] = q[0] - (index*shift);

            ppp.SetIntrinsics(p,q,width,height);

            ppp.SetC(ppp.GetC() + Vector3<double>((index*shift),0,0));

            modelViewMatricesForNViewWarp_[i].MakeViewMatrixNew(ppp);
            projectionMatricesForNViewWarp_[i].MakeProjectionMatrixNew(ppp, zNear_, zFar_);

            if (separate) {


                for (unsigned int i = 0; i < numberOfViews; i++) {
                    resultDepthMapsForNViews_[i] = new glfTexture2D();
                    resultDepthMapsForNViews_[i]->Create();
                    resultDepthMapsForNViews_[i]->SetMagFilter(GL_NEAREST);
                    resultDepthMapsForNViews_[i]->SetMinFilter(GL_NEAREST);
                    resultDepthMapsForNViews_[i]->SetWrapS(GL_CLAMP);
                    resultDepthMapsForNViews_[i]->SetWrapT(GL_CLAMP);
                    resultDepthMapsForNViews_[i]->Allocate(width, height, GL_LUMINANCE32F_ARB);

                    resultTexturesForNViews_[i] = new glfTexture2D;
                    resultTexturesForNViews_[i]->Create();
                    resultTexturesForNViews_[i]->SetMagFilter(GL_LINEAR);
                    resultTexturesForNViews_[i]->SetMinFilter(GL_LINEAR);
                    resultTexturesForNViews_[i]->SetWrapS(GL_CLAMP);
                    resultTexturesForNViews_[i]->SetWrapT(GL_CLAMP);
                    resultTexturesForNViews_[i]->Allocate(width, height, GL_RGB);
                }
            } else {
                resultDepthMapsForNViews_.resize(1);
                resultTexturesForNViews_.resize(1);
                resultDepthMapsForNViews_[0] = new glfTexture2D();
                resultDepthMapsForNViews_[0]->Create();
                resultDepthMapsForNViews_[0]->SetMagFilter(GL_NEAREST);
                resultDepthMapsForNViews_[0]->SetMinFilter(GL_NEAREST);
                resultDepthMapsForNViews_[0]->SetWrapS(GL_CLAMP);
                resultDepthMapsForNViews_[0]->SetWrapT(GL_CLAMP);
                resultDepthMapsForNViews_[0]->Allocate(width, height*numberOfViews, GL_LUMINANCE32F_ARB);

                resultTexturesForNViews_[0] = new glfTexture2D;
                resultTexturesForNViews_[0]->Create();
                resultTexturesForNViews_[0]->SetMagFilter(GL_LINEAR);
                resultTexturesForNViews_[0]->SetMinFilter(GL_LINEAR);
                resultTexturesForNViews_[0]->SetWrapS(GL_CLAMP);
                resultTexturesForNViews_[0]->SetWrapT(GL_CLAMP);
                resultTexturesForNViews_[0]->Allocate(width, height*numberOfViews, GL_RGB);

                renderTarget_.AttachTexture(*resultDepthMapsForNViews_[0],
                                            GL_COLOR_ATTACHMENT0_EXT);
                renderTarget_.AttachTexture(*resultTexturesForNViews_[0],
                                            GL_COLOR_ATTACHMENT1_EXT);
                resultZBufferTexture_.Allocate(width, height*numberOfViews_, GL_DEPTH_COMPONENT);

            }
        }

    } else {
        BIASERR("call SetProjectios(...) first");
        return -1;
    }

    return 0;
}

int MultipleDepthWarp::WarpNViews()
{
    if (initCalled_) {

        if (separate_) {
            for (unsigned int i = 0; i < numberOfViews_; i++) {
                batch_.SetModelViewMatrix(&modelViewMatricesForNViewWarp_[i]);
                batch_.SetProjectionMatrix(&projectionMatricesForNViewWarp_[i]);

                renderTarget_.AttachTexture(*resultDepthMapsForNViews_[i],
                                            GL_COLOR_ATTACHMENT0_EXT);
                renderTarget_.AttachTexture(*resultTexturesForNViews_[i],
                                            GL_COLOR_ATTACHMENT1_EXT);

                Warp();
            }
        } else {

            unsigned int width, height;

            targetParams_.GetImageSize(width,height);


            for (unsigned int i = 0; i < numberOfViews_ ; i++) {

                batch_.SetModelViewMatrix(&modelViewMatricesForNViewWarp_[i]);
                batch_.SetProjectionMatrix(&projectionMatricesForNViewWarp_[i]);

                viewport_.SetOrigin(0, (numberOfViews_ - 1 - i) *height);
                if (i==0) {
                    clearColorBuffer_ = true;
                } else {
                    clearColorBuffer_ = false;
                }
                Warp();
            }
        }
    } else {
        BIASERR("call SetProjectios(...) first");
        return -1;
    }

    return 0;
}


int MultipleDepthWarp::GetWarpedDisparity(Image<float>& dispMap) {
    unsigned int width, height;
    targetParams_.GetImageSize(width, height);
    if (dispMap.GetWidth() != width || dispMap.GetHeight() != height
        || dispMap.GetChannelCount() != 1) {
        dispMap.Release();
        dispMap.Init(width, height, 1);
    }
    resultDispTextures_[current_index_]->CopyToImage(dispMap);

    return 0;
}


int MultipleDepthWarp::GetProjectedTexture(Image<unsigned char>& texture) {
    unsigned int width, height;
    targetParams_.GetImageSize(width, height);
    if (texture.GetWidth() != width || texture.GetHeight() != height
        || texture.GetChannelCount() != 3) {
        texture.Release();
        texture.Init(width, height, 3);
    }
    resultColorTextures_[current_index_]->CopyToImage(texture);

    return 0;
}

int MultipleDepthWarp::GetWarpedZBuffer(Image<float>& zbuffer) {
    unsigned int width, height;
    targetParams_.GetImageSize(width, height);
    if (zbuffer.GetWidth() != width || zbuffer.GetHeight() != height
        || zbuffer.GetChannelCount() != 1) {
        zbuffer.Release();
        zbuffer.Init(width, height, 1);
    }
    resultZBufferTexture_.CopyToImage(zbuffer);

    return 0;
}

void MultipleDepthWarp::SetDispForDistance(const float& dist_from_cam,
                                           const float& disp) {
    //dist_from_cam_ = dist_from_cam;
    //disp_ = disp;
    baseline_ = disp*dist_from_cam;
    focallengthContained_= true;
}

void MultipleDepthWarp::
    SetBaseline(const float &baseline)
{
  baseline_ = baseline;
  focallengthContained_= false;
}

void MultipleDepthWarp::SetZNearAndZFar(const float& zNear, const float& zFar) {
    zNear_ = zNear;
    zFar_ = zFar;

    if (initCalled_) {
        projectionMatrix_.MakeProjectionMatrixNew(targetParams_, zNear_, zFar_);
    }
}

void MultipleDepthWarp::SetDiscardAngle(float degree) {
    thr_ = cos(M_PI * degree / 180.0);
}

void MultipleDepthWarp::SetDepthScaleBeforeWarping(float scale) {
    depthScaleBeforeWarping_ = scale;
}

//#####################################
// PRIVATE
//#####################################
void MultipleDepthWarp::SetupPrimitiveRendering_() {
    glfVertexFormat vertexFormat;
    vertexFormat.AddAttribute(glfVertexFormat::ATTRIB_POSITION, GL_FLOAT, 4);
    vertexFormat.AddAttribute(glfVertexFormat::ATTRIB_TEXCOORD, 0, GL_FLOAT, 2);

    const int numComponenents = 6;//4 for vertex 2 for tex coord

    // count number of vertices
    unsigned int sizeOfVertexBuffer = 0;
    unsigned int sizeOfElementBuffer = 0;
    for (unsigned int i = 0; i < sourceParams_.size(); i++) {
        unsigned int width, height;
        sourceParams_[i].GetParameters()->GetImageSize(width, height);
        sizeOfElementBuffer += width * (height - 1) * 2;
        sizeOfVertexBuffer += width * height * numComponenents;
    }

    float* vertexData = new float[sizeOfVertexBuffer];
    GLuint* triangleIndices = new GLuint[sizeOfElementBuffer];

    unsigned int numTriangleIndex = 0;
    unsigned int numVertexIndex = 0;
    unsigned int bufferOffset = 0;
    for (unsigned int i = 0; i < sourceParams_.size(); i++) {
        unsigned int width, height;
        sourceParams_[i].GetParameters()->GetImageSize(width, height);
        cameraCenters_.push_back(sourceParams_[i].GetC());

        for (unsigned int y = 0; y < height - 1; y++) {
            for (unsigned int x = 0; x < width; x++) {
                /*         v1
         *      i0--- i2
         *       |   / |
         *       |  /  |
         *       i1    i3
         */
                triangleIndices[numTriangleIndex++] = y * width + x
                                                      + bufferOffset;//up
                triangleIndices[numTriangleIndex++] = (y + 1) * width + x
                                                      + bufferOffset;//below
            }
        }

        bufferOffset += width * height;

        // create vertices for source camera i
        for (unsigned int y = 0; y < height; y++) {
            for (unsigned int x = 0; x < width; x++) {
                HomgPoint2D imgPoint(x, y);
                Vector3<double> ray, p;
                sourceParams_[i].UnProjectToRay(imgPoint, p, ray, 0, false);

                if (ray.NormL2() > 1e-12) {
                    ray.Normalize();
                } else {
                    BIASERR("error");
                }

                vertexData[numVertexIndex++] = static_cast<float> (ray[0]);
                vertexData[numVertexIndex++] = static_cast<float> (ray[1]);
                vertexData[numVertexIndex++] = static_cast<float> (ray[2]);
                vertexData[numVertexIndex++] = 1.0;

                vertexData[numVertexIndex++] = (static_cast<float> (x) + 0.5)
                                               / static_cast<float> (width);
                vertexData[numVertexIndex++] = (static_cast<float> (y) + 0.5)
                                               / static_cast<float> (height);
            }
        }
    }

    vertices_.Create(sizeOfVertexBuffer / numComponenents, vertexFormat,
                     vertexData);
    elements_.Create(sizeOfElementBuffer, GL_UNSIGNED_INT, GL_TRIANGLE_STRIP,
                     triangleIndices);

    batch_.SetElementBuffer(&elements_);
    batch_.SetVertexBuffer(&vertices_);
    delete[] triangleIndices;
    delete[] vertexData;

}

void MultipleDepthWarp::SetupShaders_(int shaderType) {
    if (shaderType == 0) {
        string
                vertexShaderCode =
                ""
                "uniform sampler2D depthImage;\n"
                "uniform sampler2D corrTexture;"
                "uniform vec3 C;\n"
                "uniform float depthScaleBefore;\n"
                "uniform float imageWidth;\n"
                "uniform float sourceImageHeight;"
                "uniform float sourceImageWidth;"
                "//uniform float dist_from_cam;\n"
                "//uniform float disp_for_dist_from_cam;\n"
                "uniform float baselineFocallength;\n"
                "uniform int flop;"
                "uniform int flip;"
                "uniform int warpType;"
                "uniform float d0;"
                "uniform float d1;"
                "uniform float minDepth;"
                "uniform float maxDepth;"
                "uniform float depthRange;"
                "uniform int depthCorType;"
                ""
                "varying float disp;\n"
                ""
                "void main() {\n"
                "\n"
                "vec2 coord = gl_MultiTexCoord0.st;\n"
                "vec2 tmp_coord = coord;\n"
                "if (flop==1)\n"
                "coord[0] = 1.0-coord[0];\n"
                "if (flip ==1)\n"
                "coord[1] = 1.0 - coord[1];\n"
                "float depth = texture2D(depthImage, coord).r * depthScaleBefore;\n"
               "if (depthCorType == 2) {\n"
               "depth = (d1 + 1)* depth + d0;\n"
                "}\n"
                "\n"
               //"if (depth == 0) depth = 10000;\n" //how to treat this? better move to negative and discard in geometry shader
                "vec4 depthAdjustedVertex = gl_Vertex * depth;\n"
                "if(depth > 0) {\n"
                "  if (depthCorType == 1) {\n"
                "    if (depthAdjustedVertex[2] < maxDepth && depthAdjustedVertex[2] > minDepth) {\n"
                "      float texCoordForZ = (depthAdjustedVertex[2] - minDepth) / depthRange;\n"
                "      float newZValue = texture2D(corrTexture, vec2(texCoordForZ,0.5)).r;\n"
                "      depthAdjustedVertex = (depthAdjustedVertex / depthAdjustedVertex[2])*newZValue;\n"
                "    }\n"
                "  }\n"
                "  depthAdjustedVertex.w = 1.0;\n"
                "  depthAdjustedVertex[0]+=C[0];\n"
                "  depthAdjustedVertex[1]+=C[1];\n"
                "  depthAdjustedVertex[2]+=C[2];\n"
                "  \n"
                "  if (warpType == 0) {\n"
                "    //vec4 tmp = gl_ModelViewProjectionMatrix * vec4(imageWidth,0.0,0.0,0.0);\n"
                "    //vec4 t = vec4(abs(((dist_from_cam*disp_for_dist_from_cam)/tmp[0])), 0.0,0.0,0.0);\n"
                "    //vec4 proj_t = gl_ModelViewProjectionMatrix * t;\n"
                "    "
                "    //disp = proj_t[0]*imageWidth;\n"
                "  //  if(baseline>0)\n"
                "      disp = baselineFocallength;\n"
                "  } else {//else depth > 0 \n"
                "    disp = length(depthAdjustedVertex.xyz);\n"
                "  }\n"
                "} else {\n"
                "  disp = -1.0; \n"
                "}\n"
                "\n"
                "gl_Position = depthAdjustedVertex;\n"
                "\n"
                "}\n";


      //  IOUtils::PrintWithLineNumbers(vertexShaderCode);

        string fragmentShaderCode = ""
                                    "uniform sampler2D projTexture;"
                                    "varying float resDisp;\n"
                                    ""
                                    "void main() {\n"
                                    "gl_FragData[0].r = resDisp;\n"
                                    //  "vec2 coord = gl_TexCoord[0];"
                                    //   "coord = coord / gl_TexCoord[0].p;"
                                    //  "coord[1] = 1.0 - coord[1];"
                                    "gl_FragData[1].rgb = texture2DProj(projTexture, gl_TexCoord[0]);"
                                    // "gl_FragData[1].rgb = texture2D(projTexture, coord);"
                                    // "gl_FragData[1].rgb = vec3(1,0,0);"
                                    "}\n";

        string geometryShaderCode = ""
                                    "#extension GL_EXT_geometry_shader4 : enable\n"
                                    ""
                                    "varying in float disp[3];\n"
                                    "varying out float resDisp;\n"
                                    "uniform vec3 C;\n"
                                    "uniform float thr;\n"
                                    "uniform int warpType;"
                                    "uniform int useProjTex;"
                                    ""
                                    "void main() {\n"
                                    ""
                                    ""
                                    "vec4 v0 = gl_PositionIn[0];"
                                    "vec4 v1 = gl_PositionIn[1];"
                                    "vec4 v2 = gl_PositionIn[2];"
                                    ""
                                    "vec3 mean_vec = (v0.xyz+v1.xyz+v2.xyz)/3.0;"
                                    ""
                                    "vec3 cam_dir = C-mean_vec;"
                                    "vec3 dir1 = v0.xyz - v1.xyz;"
                                    "vec3 dir2 = v2.xyz - v1.xyz;"
                                    ""
                                    "vec3 n = cross(dir1,dir2);"
                                    "vec3 norm_n = normalize(n);"
                                    "vec3 norm_cam = normalize(cam_dir);"
                                    ""
                                    "float res = dot(norm_n,norm_cam);"
                                    ""
                                    "if (res < thr && disp[0] >= 0 && disp[1] >= 0 && disp[2] >= 0) {"
                                    ""
                                    "gl_Position = gl_ModelViewProjectionMatrix*v0;"
                                    "if (useProjTex == 1)"
                                   "gl_TexCoord[0] = gl_TextureMatrix[0] * v0;"
                                   "if (warpType == 0) {"
                                    "resDisp = disp[0]/gl_Position[3];\n"
                                  //  "resDisp = disp[0];\n"
                                    "} else {"
                                    "resDisp = disp[0];\n"
                                    "}"
                                    "EmitVertex();"
                                    ""
                                    "gl_Position = gl_ModelViewProjectionMatrix*v1;"
                                    "if (useProjTex == 1)"
                                    "gl_TexCoord[0] = gl_TextureMatrix[0] * v1;"
                                    "if (warpType == 0) {"
                                    "resDisp = disp[1]/gl_Position[3];\n"
                                  //  "resDisp = disp[0];\n"
                                   "} else {"
                                   "resDisp = disp[1];\n"
                                   "}"
                                    "EmitVertex();"
                                    ""
                                    "gl_Position = gl_ModelViewProjectionMatrix*v2;"
                                   "if (useProjTex == 1)"
                                    "gl_TexCoord[0] = gl_TextureMatrix[0] * v2;"
                                    "if (warpType == 0) {"
                                    "resDisp = disp[2]/gl_Position[3];\n"
                                  //  "resDisp = disp[0];\n"
                                    "} else {"
                                    "resDisp = disp[2];\n"
                                    "}"
                                    "EmitVertex();"
                                    ""
                                    "}"
                                    ""
                                    ""
                                    "}\n";

        vertexShader_.Create(GL_VERTEX_SHADER, vertexShaderCode);
        fragmentShader_.Create(GL_FRAGMENT_SHADER, fragmentShaderCode);
        geometryShader_.Create(GL_GEOMETRY_SHADER_EXT, geometryShaderCode);
        shaderProgram_.Create();
        shaderProgram_.AttachShader(vertexShader_);
        shaderProgram_.AttachShader(fragmentShader_);
        shaderProgram_.AttachShader(geometryShader_);

        glProgramParameteriEXT(shaderProgram_.GetProgramId(),
                               GL_GEOMETRY_VERTICES_OUT_EXT, 3);
        glProgramParameteriEXT(shaderProgram_.GetProgramId(),
                               GL_GEOMETRY_INPUT_TYPE_EXT, GL_TRIANGLES);
        glProgramParameteriEXT(shaderProgram_.GetProgramId(),
                               GL_GEOMETRY_OUTPUT_TYPE_EXT, GL_TRIANGLE_STRIP);
        shaderProgram_.Link();

    } else {

        string fragmentShaderCode="";
        string
                vertexShaderCode =
                ""
                "uniform sampler2D depthImage;\n"
                "uniform vec3 C;\n"
                "uniform int flop;"
                "uniform int flip;"
                ""
                "void main() {\n"
                ""
                "vec2 coord = gl_MultiTexCoord0.st;"
                ""
                "if (flop==1)"
                "coord[0] = 1.0-coord[0];"
                "if (flip ==1)"
                "coord[1] = 1.0 - coord[1];"
                ""
                "float depth = texture2D(depthImage, coord).r;\n"
                ""
                "if (depth == 0) depth = 10000;\n"
                "vec4 depthAdjustedVertex = gl_Vertex * depth;\n"
                "depthAdjustedVertex.w = 1.0;\n"
                "depthAdjustedVertex[0]+=C[0];\n"
                "depthAdjustedVertex[1]+=C[1];\n"
                "depthAdjustedVertex[2]+=C[2];\n"
                ""
                "gl_Position = gl_ModelViewProjectionMatrix*depthAdjustedVertex;"
                "gl_TexCoord[0] = gl_TextureMatrix[0] * depthAdjustedVertex;"
                ""
                "}\n";
        if (shaderType == 1) {
            fragmentShaderCode = ""
                                 "uniform sampler2D projTexture;"
                                 ""
                                 "void main() {\n"
                                 "gl_FragData[1].rgb = texture2DProj(projTexture, gl_TexCoord[0]);"
                                 "}\n";
        }

        vertexShader_.Create(GL_VERTEX_SHADER, vertexShaderCode);
        fragmentShader_.Create(GL_FRAGMENT_SHADER, fragmentShaderCode);

        shaderProgram_.Create();
        shaderProgram_.AttachShader(vertexShader_);
        shaderProgram_.AttachShader(fragmentShader_);

        shaderProgram_.Link();
    }

}

void MultipleDepthWarp::CreateDepthCorrectionLUT_(ProjectionParametersPerspectiveDepth& pppd)
{
    cout << "---------------------" << endl;
    depthCorrImageLUT_.Init(numberOfSampForDepthCorr_, 2, 1);
    depthCorrImageLUT_.SetColorModel(ImageBase::CM_Grey);
    pppd.GetMinMaxDepth(minDepth_,maxDepth_);

    depthRange_ = maxDepth_ - minDepth_;

    cout << minDepth_ << " max depth " << maxDepth_ << endl;

    float step = depthRange_ / (float) numberOfSampForDepthCorr_;

//#pragma omp parallel for
    for (int i = 0; i < (int)numberOfSampForDepthCorr_; i++) {
        float depth = i*step + minDepth_;
     //   cout << "depth = " << depth;
        HomgPoint2D pos(0,0,1);
        pppd.UnDistortDepth(pos,depth,true);
      //  cout << "undistort depth = " << depth << endl;
        depthCorrImageLUT_.SetPixel(depth, i, 0);
         depthCorrImageLUT_.SetPixel(depth, i, 1);
//         depthCorrImageLUT_.SetPixel(depth, 1, 0);
    }

//  return;

    if (depthCorrTexLUTP_ != NULL) {
        delete depthCorrTexLUTP_;
        depthCorrTexLUTP_ = NULL;
    }
    depthCorrTexLUTP_ = new glfTexture2D();
    depthCorrTexLUTP_->Create();
    depthCorrTexLUTP_->SetMagFilter(GL_LINEAR);
    depthCorrTexLUTP_->SetMinFilter(GL_LINEAR);
    depthCorrTexLUTP_->SetWrapS(GL_CLAMP);
    depthCorrTexLUTP_->SetWrapT(GL_CLAMP);
    depthCorrTexLUTP_->UploadImage(depthCorrImageLUT_);

    batch_.SetTexture(depthCorrTexLUTP_, depthMaps_.size() + 1);
}