biasmerge.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
*/

#include <Base/Common/BIASpragma.hh>

#include <Base/Image/ImageBase.hh>
#include <Base/Image/ImageIO.hh>
#include <stdio.h>
#include <map>
#include <string>
#include "Geometry/PMatrix.hh"
#include "Image/HomographyMapping.hh"
#include "Image/Camera.hh"

using namespace BIAS;
using namespace std;

/** 
    @file
    @ingroup g_tools
    @brief Merge a projection matrix and an image and write header data to image.
           See biasmerge.cpp.
    @author koeser
*/
void usage()
{
    cout << "Usage: biasmerge [options] files"<<endl;
    cout <<" Options: "<< endl
        <<" -s         : search next smaller(string compare) PMatrix file"<<endl
        <<" -b         : assume bbc camera file format"<<endl
        <<" -a         : enforce aspect ratio 1, adapt P and image"<<endl
        <<" -p         : force principal point=image center, adapt P and image"
        << endl
        << endl;
    cout <<"This program opens the image files given by the command line, "
        <<"e.g. image037.pgm, looks at the command line for a file "
        <<"image037.pgm.mat and attaches the pmatrix to the pgm header."<<endl
        <<"With special options, you can enforce constraints on the KMatrix."
        <<endl
        <<"Returns: 0 on success, 1 if errors occured."<<endl<<endl
        <<"e.g. biasmerge *.ppm *.ppm.mat"<<endl<<endl;
}

int DetermineFiletype(const string &fn, ImageIO::TFileFormat &thetype)
{
    int EndOfPrefix;
    string suffix;

    EndOfPrefix = fn.rfind('.');

    suffix = fn.substr(EndOfPrefix+1,fn.length());
    if (suffix == "pnm") thetype =  ImageIO::FF_ppm;
    else if (suffix == "ppm") thetype =  ImageIO::FF_ppm;
    else if (suffix == "pgm") thetype =  ImageIO::FF_pgm;
    else if (suffix == "mip") thetype =  ImageIO::FF_mip;
    else return -1;

    return 0;
}


int main (int argc, char *argv[])
{
    bool Nearest = false, BBC = false, EnforceAspect = false,
        EnforcePP = false;
    double ShrinkFactor = 1.0;
    int i = 1;
    bool param = true;
    while ((i<argc) && (param)) {
        param = false;
        if (!strcmp("-s",argv[i])) {
            std::cout << "Using nearest P from command line also."<<endl;
            Nearest = true;
            i++;
            param = true;
            continue;
        } 
        if (!strcmp("-b",argv[i])) {
            std::cout << "Trying to load BBC camera data."<<endl;
            BBC = true;
            i++;
            param = true;
            continue;
        }
        if (!strcmp("-a",argv[i])) {
            std::cout << "Enforcing aspect ratio 1."<<endl;
            EnforceAspect = true;
            i++;
            param = true;
            continue;
        } 
        if (!strcmp("-p",argv[i])) {
            std::cout << "Enforcing principal point to image center."<<endl;
            EnforcePP = true;
            i++;
            param = true;
            continue;
        }
        if (!strcmp("-h",argv[i])) {
            std::cout << "Half image width and height."<<endl;
            ShrinkFactor = 0.5;
            i++;
            param = true;
            continue;
        }
    }

    if (i>=argc) { 
        usage();
        return -1;
    }

    ImageIO::TFileFormat FileType;
    bool ErrorFlag = false;
    int IOResult = 0;
    BIAS::ImageBase CurImage;

    PMatrix P;
    BIAS::UUID CurrUID;
    map<string, PMatrix> mapP;
    map<string, BIAS::UUID> mapPUIDs;

    for (int j=i; j<argc; j++) {
        if (strstr(argv[j], ".mat")!=NULL) {
            cout<<"arg "<<j<<" is .mat";
            PMatrix P;
            BIAS::UUID Puid;
            char ctmp;
            P.SetZero();
            if (!BBC) {
                // check for UID in PMatrix
                std::ifstream fs( argv[j] );
                fs>>P;
                do {
                    fs.get(ctmp);
                } while (!fs.eof() && ctmp!='#');
                if (!fs.eof()){
                    fs.get(ctmp);
                    fs >> Puid;
                    fs.close();
                    mapPUIDs[argv[j]] =Puid;
                };
            } else {
                P.LoadBBC(argv[j]);
            }
            cout << "P is "<<P<<endl;
            mapP[argv[j]] = P;
        }
    }
    if (mapP.size()==0) {
        BIASERR("No matrices found !");
        return -1;
    }




    for (;i<argc;i++) {
        if (strstr(argv[i], ".mat")!=NULL) continue;
        cout<< "Trying "<<argv[i]<<endl;
        if (DetermineFiletype(argv[i], FileType)!=0) {
            BIASERR("Skipping current file because of unknown/unsuported format: "
                <<argv[i]<<endl<<"PMatrix can be written to mip,pgm,ppm,pnm");
            ErrorFlag = true;
            continue;
        }

        IOResult = ImageIO::Load(argv[i], CurImage);
        if (IOResult!=0) {
            ErrorFlag = true;
            BIASERR("Error "<<IOResult <<" reading "<< argv[i]);
            continue;
        }

        stringstream ss, filename;
        filename << argv[i];
        ss << argv[i] <<".mat";
        if (mapP.find(ss.str())!=mapP.end()) {
            cout<<"Found a mat file for "<< filename.str() <<endl;
            P = mapP[ss.str()];
            if (mapPUIDs.find(ss.str())!=mapPUIDs.end()) {
                cout<<"Found a UID in P-mat file "<< filename.str() << " UID " 
                    << mapPUIDs[ss.str()] << endl;
                CurrUID = mapPUIDs[ss.str()];
            }
        } else {
            if (!Nearest) {
                cout <<"Found NO mat file for "<< filename.str() 
                    <<" !!!!! SKIPPING"<<endl;
                continue;
            }
            map<string,PMatrix>::iterator it = mapP.lower_bound(ss.str());
            if (it != mapP.begin()) it--;
            else cout<<"First element, cant use smaller, must use larger !";
            P = it->second; 
            cout <<"Found next mat file for "<< filename.str() 
                <<": "<<it->first<<endl;
        }

        if (EnforceAspect || EnforcePP || (ShrinkFactor!=1.0)) {
            bool SkipWarping = true;
            // check KMatrix, which components to adapt ?
            KMatrix K = P.GetK();
            int newimsizex = CurImage.GetWidth(),
                newimsizey = CurImage.GetHeight();

            // check if aspect ratio is one and adapt if desired
            if (EnforceAspect) {
                if (K[0][0]>K[1][1]) {
                    newimsizey = (int)rint(double(newimsizey) * double(K[0][0]/K[1][1]));
                    K[1][2] *= double(K[0][0]/K[1][1]);
                    K[1][1] = K[0][0];
                    SkipWarping = false;
                } else if (K[0][0]<K[1][1]) {
                    newimsizex = (int)rint(double(newimsizex) * double(K[1][1]/K[0][0]));
                    K[0][2] *= double(K[1][1]/K[0][0]); 
                    K[0][0] = K[1][1];
                    SkipWarping = false;
                }
            }
            // check if principal point is in image center and adapt if desired
            if (EnforcePP) {
                if (rint(K[0][2]*2.0)!=(newimsizex-1)) {
                    cout<<"changing ppx from "<<K[0][2];
                    SkipWarping = false;
                    K[0][2] = double(newimsizex-1) / 2.0;
                    cout<<" to "<<K[0][2]<<endl;
                }
                if (rint(K[1][2]*2.0)!=(newimsizey-1)) { 
                    cout<<"changing ppy from "<<K[1][2];
                    SkipWarping = false;
                    K[1][2] = double(newimsizey-1) / 2.0;
                    cout<<" to "<<K[1][2]<<endl;
                }
            }
            if (ShrinkFactor != 1.0) {
                K[0][0] *= ShrinkFactor;
                K[1][1] *= ShrinkFactor;
                K[0][2] *= ShrinkFactor;
                K[1][2] *= ShrinkFactor; 
                newimsizex = int(rint(double(newimsizex)*ShrinkFactor));
                newimsizey = int(rint(double(newimsizey)*ShrinkFactor));
                SkipWarping = false;
            }

            // do we have any changes for the image data ?
            if (!SkipWarping) {
                // compose new P
                RMatrix R;
                Vector3<double> C;
                P.GetR(R);
                P.GetC(C);
                PMatrix PCorrected(K, R, C);

                // compute homography for backward mapping and transform image
                HMatrix H = P.GetK() * K.Invert();

                cout<<"Warping image from "<<CurImage.GetWidth()<<"x"
                    <<CurImage.GetHeight()<<" pixels to "<<newimsizex<<"x"
                    <<newimsizey<<" pixels."<<endl;
                cout<<" K was "<<P.GetK()<<endl;
                cout<<" K new "<<K<<endl;
                cout<<"Warping homography is "<<H<<endl;
                switch (CurImage.GetStorageType()) {
        case ImageBase::ST_unsignedchar: {
            HomographyMapping<unsigned char, unsigned char> Mapper;
            Image<unsigned char> origtemplated, 
                aspect1(newimsizex, newimsizey, CurImage.GetChannelCount());
            aspect1.SetColorModel(CurImage.GetColorModel());
            origtemplated.StealImage(CurImage);
            Mapper.SetHomography(H);
            Mapper.Map(origtemplated, aspect1);
            CurImage.StealImage(aspect1);
            P = PCorrected;
            break;
                                         }
        default:BIASERR("storagetype "<<int(CurImage.GetStorageType())
                    <<" not implemented, please add the bit of code here");

                }
                filename << "_warped";
            }
        }

        // just a dummy class to do the metadata job for us
        BIAS::Camera<unsigned char> c;
        c.SetP(P);
        if (P.IsMetric()) {
            c.SetK(P.GetK());
            c.SetR(RMatrix(P.GetR()));
            c.SetC(P.GetC());
        }
        c.UpdateMetaData();
        CurImage.SetMetaData(*c.GetMetaData());
        // if the image does not have a valid UID check for a UID in the PMatrix
        if (!(CurImage.GetUID().IsValid()) && CurrUID.IsValid()){
            CurImage.SetUID(CurrUID);
            cout << "set UID from P matrix because image doesn't have a valid UID" 
                << endl;
        }
        /*
        if (FileType==ImageIO::FF_mip) {
            IOResult = ImageIO::Save(filename.str(), CurImage, false, false );
        } else {
            IOResult = ImageIO::Save(filename.str(), CurImage, 
                FileType, false, 
                0, false);
        } */
        IOResult = ImageIO::Save(filename.str(),
                                 CurImage, 
                                 FileType,
                                 false, 
                                 0,
                                 false);
        
        if (IOResult!=0) {
            ErrorFlag = true;
            BIASERR("Error writing "<< filename.str());
            continue;
        } else cout<<filename.str()<<endl;

    }
    if (ErrorFlag) cout << "There were errors." <<endl;
    if (ErrorFlag) return 1;
    return 0;
}