ExampleMultipleDepthWarp.cpp

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

/*
 This file is part of the BIAS library (Basic ImageAlgorithmS).

 Copyright (C) 2003-2009    (see file CONTACT for details)
 Multimediale Systeme der Informationsverarbeitung
 Institut fuer Informatik
 Christian-Albrechts-Universitaet Kiel


 BIAS is free software; you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by
 the Free Software Foundation; either version 2.1 of the License, or
 (at your option) any later version.

 BIAS is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU Lesser General Public License for more details.

 You should have received a copy of the GNU Lesser General Public License
 along with BIAS; if not, write to the Free Software
 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */


/**
   @example ExampleMultipleDepthWarp.cpp
   @relates MultipleDepthWarp
   @brief Example for using the multidepthwarp
   @ingroup g_examples
   @ingroup g_openglframework
   @author Anatol Frick
*/


#include <Base/Image/ImageConvert.hh>

#include <OpenGLFramework/RenderingContext/glfPBuffer.hh>
#include <OpenGLFramework/SpecializedBatches/MultipleDepthWarp.hh>
#include <OpenGLFramework/Base/glfException.hh>

#include <Utils/Param.hh>
#include <Utils/IOUtils.hh>
#include <Base/Image/ImageIO.hh>
#include <Base/Image/Image.hh>
#include <Image/Camera.hh>
#include <Filter/Gauss.hh>
#include <Base/Debug/TimeMeasure.hh>
#include <OpenGLFramework/SpecializedBatches/DisparityInterpolator.hh>
#include <OpenGLFramework/SpecializedBatches/SeparabelBoxFilter.hh>
#include <Filter/Rescale.hh>
#include <Filter/Median.hh>

#include <Gui/biasgl.h>

using namespace std;
using namespace BIAS;


void ShiftAndAdjustFrustum(ProjectionParametersPerspective* ppp, double distFromCam, double shift) {
    unsigned int width, height;
    ppp->GetImageSize(width,height);

    cout << ppp->GetK() << endl;

    HomgPoint2D leftTop(0.5,0.5,1);
    HomgPoint2D rightBottom((double)width-0.5, (double) height - 0.5,1);
    Vector3<double> p;

    Vector3<double> d1;
    ppp->UnProjectLocal(leftTop, p, d1, true);

    d1 = (d1 / d1[2]) * distFromCam;

    d1[0] -= shift;

    Vector3<double> d2;
    ppp->UnProjectLocal(rightBottom, p , d2, true);

    d2 = (d2 / d2[2]) * distFromCam;

    d2[0] -= shift;

    ppp->SetIntrinsics(d1,d2,width,height);

    ppp->SetC(ppp->GetC() + Vector3<double>(shift,0,0));

    cout << ppp->GetK() << endl;

}


int main(int argc, char* argv[]) {
    Param params(true);

    string* firstDepthImageName = params.AddParamString("firstDepthImage",
                                                        "image of first source camera", "", 'f');
    string* secondDepthImageName = params.AddParamString("secondDepthImage",
                                                         "image of second source camera", "", 's');

// unused    string* dispImageName = params.AddParamString("resultDispImage",
//                                                  "result disparity image name", "depthResult.mip", 'd');
// unused    string* colorImageName = params.AddParamString("resultColorImage",
//                                                   "result color image name", "colorResult.mip", 'c');

    string* textureForTheFirstSourceProj = params.AddParamString("projectiveTexture",
                                                                 "texture for the first source proj", "", 'p');

    string* targetProjName = params.AddParamString("targetProj",
                                                   "projection with params of target perspective cam", "", 't');
    string* firstSourceProjName = params.AddParamString("firstSourceProj",
                                                        "projection with params of first source cam", "", 'a');
    string* secondSourceProjName = params.AddParamString("secondSourceProj",
                                                         "projection with params of second source cam", "", 'b');

    double* zNear = params.AddParamDouble("zNear", "", 100);

// unused    double* distFromCam = params.AddParamDouble("distFromCam", "distance from camera centrum to the display in millimeter", 4000);
// unused    double* eyeDist = params.AddParamDouble("eyeDist", "distance between the eyes in millimeter", 65);
    double* zFar = params.AddParamDouble("zFar", "", 8000);
// unused    int* step = params.AddParamInt("step","", 0);

    if (!IOUtils::ParseCommandLineEvalHelp(params, argc, argv))
        return 0;

    vector<ProjectionParametersPerspectiveDepth> pmdParams;

    vector<Projection> sourceProjs;
    Projection targetProj;

    if ((*firstSourceProjName).compare("")!=0) {
        sourceProjs.push_back(Projection());
        if (sourceProjs.back().Load(*firstSourceProjName) != 0) {
            BIASERR("Could not load source tof projection: "<<*firstSourceProjName);
            return -1;
        }
    }

    if ((*secondSourceProjName).compare("")!=0) {
        sourceProjs.push_back(Projection());
        if (sourceProjs.back().Load(*secondSourceProjName) != 0) {
            BIASERR("Could not load source tof projection: "<<*secondSourceProjName);
            return -1;
        }
    }

    if (targetProj.Load(*targetProjName) != 0) {
        BIASERR("Could not load target tof projection:"<<*targetProjName);
        return -1;
    }

    Image<float> firstDepthMap, secondDepthMap;
    if ((*firstDepthImageName).compare("")!=0) {
        if (ImageIO::Load(*firstDepthImageName, firstDepthMap)) {
            BIASERR("Could not load first depth map :"<< *firstDepthImageName);
            return -1;
        }
    }
    if ((*secondDepthImageName).compare("")!=0) {
        if (ImageIO::Load(*secondDepthImageName, secondDepthMap)) {
            BIASERR("Could not load second depth map :"<< *secondDepthImageName);
            //return -1;
        }
    }

    Image<unsigned char> projTexture;

    if ((*textureForTheFirstSourceProj).compare("")!=0) {
        if (ImageIO::Load(*textureForTheFirstSourceProj, projTexture)) {
            BIASERR("Could not load second depth map :"<< *textureForTheFirstSourceProj);
            return -1;
        }
    }


    MultipleDepthWarp mpmdWarp;


    ProjectionParametersBase* ppb = targetProj.GetParameters();
    ProjectionParametersPerspective* ppp =
            dynamic_cast<ProjectionParametersPerspective*> (ppb);

    ppp->Rescale((unsigned int)960, (unsigned int) 540);

   //ShiftAndAdjustFrustum(ppp,(*distFromCam),((*distFromCam)/4000.0)*(*eyeDist)*(*step));


    ProjectionParametersBase* source0_ppb = sourceProjs[0].GetParameters();
    ProjectionParametersPerspective* source0_ppp =
            dynamic_cast<ProjectionParametersPerspective*> (source0_ppb);

    source0_ppp->Rescale((unsigned int) 960, (unsigned int) 540);

    if (ppp == NULL) {
        BIASERR("target parameters are not from type perspective");
        return -1;
    }




 Image<float> tmpDepthMap;
    Gauss<float,float> gaussFilter;
    gaussFilter.SetHalfWinSize(30);
    gaussFilter.Filter(firstDepthMap,tmpDepthMap);
    firstDepthMap = tmpDepthMap;

    TimeMeasure tm;
    Image<float> warpedDepth;
    Image<unsigned char> warpedColor;
    Image<unsigned char> redGreen;
    glfPBuffer pbuffer;
    try {

        glfRenderingContextConfig cfg;
        cfg.width = 5;
        cfg.height = 5;
        cfg.doubleBuffer = 0;
        cfg.redSize = 8;
        cfg.greenSize = 8;
        cfg.blueSize = 8;
        cfg.alphaSize = 0;
        cfg.depthSize = 24;
        cfg.stencilSize = 0;

        pbuffer.Init(cfg);

        mpmdWarp.SetDepthScaleBeforeWarping(1.0f);

        //      ppp->Rescale((unsigned int) 960, (unsigned int) 540);
        mpmdWarp.Init(*ppp, sourceProjs);
        mpmdWarp.SetupNViewsWarp(2000,1.0,20, 8, 4, false);
        tm.Start();
        //       for (unsigned int i = 0; i < 1000; i++) {
        mpmdWarp.UploadDepthMap(firstDepthMap, 0);
        //mpmdWarp.UploadDepthMap(secondDepthMap, 1);
        mpmdWarp.SetProjectiveTextureCam(*source0_ppp);
        mpmdWarp.UploadProjectiveTexture(projTexture);
        mpmdWarp.UseProjectiveTexturing(true);

        mpmdWarp.SetWarpType(MultipleDepthWarp::DEPTH);
        mpmdWarp.SetDiscardAngle(80);
        mpmdWarp.SetDispForDistance(3000, 128);
        mpmdWarp.SetZNearAndZFar((float) *zNear, (float) *zFar);
       mpmdWarp.WarpNViews();

        //    }
        tm.Stop();

        cout << "frame rate = " << (1000.0 / (tm.GetRealTime() / 1000000.0)) << endl;

        tm.Print(cout);

       // mpmdWarp.GetProjectedTexture(warpedColor);
     //   mpmdWarp.GetWarpedDisparity(warpedDepth);
        //boxFilter.GetFilteredTexture(result);

        vector<glfTexture2D*>* textures = mpmdWarp.GetResultTexturesFromNViewWarp();

        //1
        ((*textures)[0])->CopyToImage(warpedColor);
        warpedColor.Flip();
        ImageIO::Save("colorImage1.png",warpedColor);

//        //2
//        ((*textures)[1])->CopyToImage(warpedColor);
//        warpedColor.Flip();
//        ImageIO::Save("colorImage2.png",warpedColor);
//
//        //3
//        ((*textures)[2])->CopyToImage(warpedColor);
//        warpedColor.Flip();
//        ImageIO::Save("colorImage3.png",warpedColor);
//
//        //4
//        ((*textures)[3])->CopyToImage(warpedColor);
//        warpedColor.Flip();
//        ImageIO::Save("colorImage4.png",warpedColor);
//
//        //5
//        ((*textures)[4])->CopyToImage(warpedColor);
//        warpedColor.Flip();
//        ImageIO::Save("colorImage5.png",warpedColor);
//
//        //6
//        ((*textures)[5])->CopyToImage(warpedColor);
//        warpedColor.Flip();
//        ImageIO::Save("colorImage6.png",warpedColor);
//
//        //7
//        ((*textures)[6])->CopyToImage(warpedColor);
//        warpedColor.Flip();
//        ImageIO::Save("colorImage7.png",warpedColor);
//
//        //8
//        ((*textures)[7])->CopyToImage(warpedColor);
//        warpedColor.Flip();
//        ImageIO::Save("colorImage8.png",warpedColor);


     //   warpedColor.Flip();
      //  warpedDepth.Flip();


//        if (ImageIO::Save(*dispImageName, warpedDepth) == 0) {
//            cout << "written disparity map " << *dispImageName << endl;
//        } else {
//            BIASERR("could not write " << *dispImageName);
//            pbuffer.Destroy();
//            return -1;
//        }
//
//
//        if (ImageIO::Save(*colorImageName, warpedColor) == 0) {
//            cout << "written disparity map " << *colorImageName << endl;
//        } else {
//            BIASERR("could not write " << *colorImageName);
//            pbuffer.Destroy();
//            return -1;
//        }
//
//
//        redGreen = projTexture;
//        Image<unsigned char> grey1;
//        ImageConvert::Convert(projTexture,grey1,ImageBase::CM_Grey);
//        Image<unsigned char> grey2;
//        ImageConvert::Convert(warpedColor, grey2,ImageBase::CM_Grey);
//
//        redGreen.Clear(0);
//        for (unsigned int x = 0; x < warpedColor.GetWidth(); x++) {
//            for (unsigned int y = 0; y < warpedColor.GetHeight(); y++) {
//                redGreen.SetPixel(grey1.PixelValue(x,y),x,y,0);
//                redGreen.SetPixel(grey2.PixelValue(x,y),x,y,1);
//              //  redGreen.SetPixel(grey1.PixelValue(x,y),x,y,2);
//            }
//        }
//
//        ImageIO::Save("redGreenTexture.png",redGreen);

    } catch (glfException& e) {
        std::cout << "Error: " << e.GetMessageString() << std::endl;
        pbuffer.Destroy();
        return -1;
    }

    pbuffer.Destroy();

    return 0;
}