ExampleProjectionMapping.cpp

/* 
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 ExampleProjectionMapping.cpp
* @relates ProjectionMapping
* @brief  example for mapping images according to projection parameters
* @ingroup g_examples
* @author  oniemann
*/

#include <Image/ProjectionMapping.hh>
#include <Base/Image/ImageIO.hh>
#include <Base/Image/ImageConvert.hh>
#include <Base/Image/Image.hh>
#include <Base/Debug/TimeMeasure.hh>
#include <Base/Math/Vector3.hh>
#include <Base/Math/Vector.hh>
#include <Utils/Param.hh>
#include <Utils/ThreeDOut.hh>
#include <Geometry/ProjectionParametersSpherical.hh>
#include <Image/Camera.hh>

using namespace BIAS;
using namespace std;

// Calculates rotationAngle for sequence steps in x or y (yaxis = true) 
// direction.
void GetSequenceAngle(unsigned int & sequence ,Projection p1, 
        Projection p2, const bool yaxis, double & rotationAngle)
{
    unsigned int width1,height1,width2,height2;
    double angle1, angle2,focalLength;     

    ProjectionParametersPerspective *pPPP = 
        dynamic_cast<ProjectionParametersPerspective * >(p1.GetParameters());
        ProjectionParametersSpherical *pPPS = 
            dynamic_cast<ProjectionParametersSpherical * >(p1.GetParameters());
            if (pPPP!=NULL){
                pPPP->GetImageSize(width1,height1);
                pPPP->GetFocalLength(focalLength);
                if(!yaxis) angle1 = 2*atan(width1/(2*focalLength));
                else angle1 = 2*atan(height1/(2*focalLength));
            }
            else if (pPPS!=NULL) angle1 = 1.7*pPPS->GetMaxCamAngle();
            else{
                cout<<"Hat nicht geklappt. Kamera 1 ist weder Perspektivisch, noch Spherisch.";
                return;
            }
            pPPP = dynamic_cast<ProjectionParametersPerspective * >(p2.GetParameters());
            pPPS = dynamic_cast<ProjectionParametersSpherical * >(p2.GetParameters());
            if (pPPP!=NULL){
                pPPP->GetImageSize(width2,height2);
                pPPP->GetFocalLength(focalLength);
                if(!yaxis) angle2 = 2*atan(width2/(2*focalLength));
                else angle2 = 2*atan(height2/(2*focalLength));
            }
            else if (pPPS!=NULL) angle2 = 1.7*pPPS->GetMaxCamAngle();
            else{
                cout<<"Hat nicht geklappt. Kamera 2 ist weder Perspektivisch, noch Spherisch.";
                return;
            }
            if(sequence == 0){
                sequence =(unsigned int)(angle1/angle2);
                rotationAngle = angle2;
            }
            else{
                rotationAngle = (angle1-angle2)/((double)sequence-1);
            }

            cout << "Abgedeckter Winkel: " << (rotationAngle*(sequence-1)*180/M_PI) << " Winkel der OriginalKamera: " << (angle1*180/M_PI) << " Winkel der ZielKamera: " << (angle2*180/M_PI) << endl;
}

//Set a rotation of angle a around vector v for projection p.
void SetRotation(Projection & p,Vector3< ROTATION_MATRIX_TYPE >  v,double a)
{
    RMatrix R (v, a);
    p.GetParameters()->SetR(R);
}  

//default sink projection
void GenerateProjection(Projection & p)
{
    ProjectionParametersPerspective pBase;

    pBase.SetImageSize(640,480);
    pBase.SetFocalLengthAndAspect(800.0,1.0);
    pBase.SetPrincipal(320,240);
    p = Projection(pBase);
    SetRotation(p,Vector3< ROTATION_MATRIX_TYPE > (0,1,0),0.5);
} 

int main(int argc, char *argv[])
{

    Param *param = new Param();
    string sourceImage,sourceCam,sinkCam;
    unsigned int sequence;
    Vector< double > rotationVector;
    double rotationAngle;
    MetaData metaData;

    param->AddParamString("SourceImage",
            "Absolute path to the source Image",
            "",
            'i');

    param->AddParamString("SinkImage",
            "Absolute path to the sink Image",
            "image_projection_trilinear",
            'o');

    param->AddParamString("SourceCam",
            "Absolute path to the source Camera calibration XML",
            "",
            'c');
    param->AddParamDouble("RotationAngle",
            "Angle to rotate around v in RAD (e.g. 0.1)",
            0.0,
            0.0,
            2*M_PI,
            'w');
    param->AddParamString("SinkCam",
            "Absolute path to the sink Camera calibration XML",
            "",
            's');
    param->AddParamVecDbl("RotationVector",
            "A Vector to rotate (e.g. 0.0 1.0 0.0)",
            "0.0 1.0 0.0",
            'v');
    param->AddParamInt("Sequence",
            "Give a number of images to create a sequence",
            1,
            1,
            1000,
            'S');
    param->AddParamBool("Mosaic",
            "Create a mosaic from Image?",
            false,
            'm');
    param->ParseCommandLine(argc,argv);

    //Read and load source image and projection from parameters
    sourceImage = *param->GetParamString("SourceImage");
    sourceCam = *param->GetParamString("SourceCam");
    if(sourceImage == "" || sourceCam == "")
    {
        BIASERR(argv[0]<<" -i<image-file> -c<SourceCamera-file>" <<
                "-vaSs[Rotation-Vector Rotation-Angle] [Sequence] [SinkCamera-file]");
        return -1;
    }


    Image<unsigned char> im;
    if (ImageIO::Load(sourceImage, im)!=0){
        BIASERR("error loading "<<sourceImage);
        return -2;
    }
    if (im.GetColorModel()!=ImageBase::CM_Grey) ImageConvert::ToGrey(im,im);

    ProjectionMapping<unsigned char, unsigned char> mapper;
    Projection p1,p2;

    if (p1.XMLRead(sourceCam)!=0) {
        if (p1.ReadFromCamParamFile(sourceCam)!=0) {
            BIASERR("Error reading "<<sourceCam);
            return -3;
        }
    }
    BIASASSERT(p1.GetParameters()!=NULL);
    mapper.SetSourceCam(p1);

    //use default sink projection or load specified one
    sinkCam = *param->GetParamString("SinkCam");
    if(sinkCam == "")    {
        GenerateProjection(p2);
    }  else   {
        if (p2.XMLRead(sinkCam)!=0) {
            if (p2.ReadFromCamParamFile(sinkCam)!=0) {
                BIASERR("Error reading "<<sinkCam);
                return -3;
            }
        }
    }
    BIASASSERT(p2.GetParameters()!=NULL);
    //set the rotation for sink projection, default none
    rotationVector = *param->GetParamVecDbl("RotationVector");
    rotationVector.MultiplyIP(1.0/rotationVector.NormL2());
    rotationAngle = *param->GetParamDouble("RotationAngle");
    Vector3<ROTATION_MATRIX_TYPE> v(rotationVector);
    SetRotation(p2,v,rotationAngle);
    cout << rotationVector.Length() << endl;

    unsigned int width, height;

    sequence = *param->GetParamInt("Sequence");
    bool mosaic = *param->GetParamBool("Mosaic");
    //calculate the sequence angle if wanted
    if(sequence > 1 && !mosaic)
    {
        GetSequenceAngle(sequence,p1,p2,false,rotationAngle);
    }
    p2.GetParameters()->GetImageSize(width,height);
    cout<<"ROI of source image is "<<*im.GetROI()
    <<" while image size is "<<width<<" x "<<height<<endl;
    Camera<unsigned char> imrestrilinear;
    imrestrilinear.Init(width,height,im.GetChannelCount());
    //    imrestrilinear(width,height, im.GetChannelCount());
    //imrestrilinear.SetColorModel(im.GetColorModel());
    cout<<"timing ..."<<endl<<flush;
    mapper.SetPyramidSize(20);
    TimeMeasure t1;
    stringstream filename;
    //ThreeDOut vrml;
    //vrml.SetParamsCameraStyle(PyramidMesh);
    //if(sequence>1) vrml.AddProjection(p1);
    //calculate the image for given rotation angle or sequence

    for(unsigned int i=0;i<50;i++){
        cout<<"Run: "<<i<<"/50"<<endl;
        SetRotation(p2,v,(0.03*((double)i-(49.0/2.0))));
        //  vrml.AddProjection(p2);
        mapper.SetSinkCam(p2);
        mapper.Map(im, imrestrilinear, MapTrilinear);
        imrestrilinear.SetProj(p2);
        imrestrilinear.UpdateMetaData();
        filename.str("");
        char tmpStr[8];
        sprintf(tmpStr, "%03i", i);
        filename << "/datapc/sieg/oniemann/data/images/find/find_image_"<<tmpStr<<".mip";
        //if (ImageIO::Save(filename.str(), imrestrilinear)!=0){
        if (ImageIO::Save(filename.str(), imrestrilinear)!=0){
            BIASERR("error image");
        }
    }

    if(!mosaic)
    {
        for (unsigned int i=0; i<sequence; i++) {
            if(sequence > 1){
                v[0] = 0.0;
                v[1] = 1.0;
                v[2] = 0.0;
                cout << "Run "<<i<<endl;
                cout << "RotationAngle: " << rotationAngle*((double)i-(((double)sequence-1)/2.0))*180/M_PI << endl;
                SetRotation(p2,v,(rotationAngle*((double)i-(((double)sequence-1)/2.0))));
                //  vrml.AddProjection(p2);
            }
            else{
                cout << "Angle: " << (rotationAngle*180/M_PI) << endl;
            }
            mapper.SetSinkCam(p2);
            t1.Start();
            // trilinear mapping with automatic scale and pyramid size selection
            mapper.Map(im, imrestrilinear, MapTrilinear);
            imrestrilinear.SetProj(p2);
            imrestrilinear.UpdateMetaData();
            t1.Stop();
            filename.str("");
            char tmpStr[8];
            sprintf(tmpStr, "%03i", i);
            filename << "/datapc/sieg/oniemann/data/images/reference/reference_image_000_"<<tmpStr<<".mip";
            //if (ImageIO::Save(filename.str(), imrestrilinear)!=0){
            if (ImageIO::Save(filename.str(), imrestrilinear)!=0){
                BIASERR("error image");
            }
        }
    }
    //calculate mosaic image
    else
    {
        double rotationAngle2;
        unsigned int sequence2;
        Vector3<ROTATION_MATRIX_TYPE> v2(1, 0, 0);
        RMatrix R, R2, R3;

        sequence = 0;
        sequence2 = 0;
        GetSequenceAngle(sequence,p1,p2,false,rotationAngle);
        GetSequenceAngle(sequence2,p1,p2,true,rotationAngle2);
        //Run through ImageHeight
        for ( unsigned int j=0; j<sequence2; j++) {
            R3= RMatrix(v2, (rotationAngle2*((double)j-(((double)sequence2-1.0)/2.0))));
            for (unsigned int i=0; i<sequence; i++) {
                if(j%2==0){
                    R2= RMatrix(v, (rotationAngle*((double)i-(((double)sequence-1.0)/2.0))));
                }
                else{
                    R2= RMatrix(v, (rotationAngle*((double)i+0.5-(((double)sequence-1.0)/2.0))));
                }
                //Run through ImageWidth
                cout << "Run " << i <<" " << j <<endl;

                R2.Mult(R3,R);
                p2.GetParameters()->SetR(R);
                mapper.SetSinkCam(p2);
                t1.Start();
                // trilinear mapping with automatic scale and pyramid size selection
                imrestrilinear.FillImageWithConstValue((unsigned char)0);
                mapper.Map(im, imrestrilinear, MapTrilinear);
                imrestrilinear.SetProj(p2);
                imrestrilinear.UpdateMetaData();
                t1.Stop();
                filename.str("");
                char tmpStr[8];
                sprintf(tmpStr, "%03i_%03i", j, i);
                filename << "/datapc/sieg/oniemann/data/images/reference/reference_image_"<<tmpStr<<".mip";
                //if (ImageIO::Save(filename.str(), imrestrilinear)!=0){
                if (ImageIO::Save(filename.str(), imrestrilinear)!=0){
                    BIASERR("error image");
                }
            }
        }
    }

    //create vrml file
    //vrml.VRMLOut("myVRML.wrl");

    cout <<"Trilinear mapping took "
    <<t1.GetUserTime()/double(sequence)
    <<" ms,"<<endl;

    return 0;
}