biasprojection.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 <Base/ImageUtils/ImageDraw.hh>
#include <stdio.h>
#include <map>
#include <string>
#include "Geometry/PMatrix.hh"
#include "Image/Camera.hh"

#include <Base/Common/BIASpragma.hh>

using namespace BIAS;
using namespace std;

/**
   @file
   @ingroup g_tools
   @brief read images and pmatrices, projects 3d points and cuts out region, see biasprojection.cpp
   @author koeser
 */
void usage()
{
    cout << "Usage: biasprojection -p file [options] files"<<endl;
    cout <<" Options: "<< endl
        <<" -s         : search next smaller(string compare) PMatrix file"<<endl
        <<" -b         : assume bbc camera file format"<<endl
        <<" -p file    : file containing 3d points"<<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 projects 3d points into the images."<<endl
        <<" using the pmatrices."<<endl
        <<" cutouts are written around the projection."<<endl
        <<"Returns: 0 on success, 1 if errors occured."<<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;
    vector<HomgPoint3D> Points;
    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;
        }
        if (!strcmp("-b",argv[i])) {
            std::cout << "Trying to load BBC camera data."<<endl;
            BBC = true;
            i++;
            param = true;
        }
        if (!strcmp("-p",argv[i])) {
            i++;
            if (argc<=i) {
                cerr<<"error, no point file given after -p"<<endl;
                exit(-1);
            }
            std::cout << "loading points from "<< argv[i]<<endl;
            ifstream infile(argv[i++], ios::in);
            if (infile.eof() || !infile.good()){
                cerr<<"error opening point file"<<endl;
                exit(-1);
            }
            // reading points
            HomgPoint3D thepoint(0,0,0);
            while (!infile.eof()) {
                infile >> thepoint[0];
                infile >> thepoint[1];
                infile >> thepoint[2];
                if (infile.eof() || !infile.good()) break;
                Points.push_back(thepoint);
                cout<<"Read 3D point "<<thepoint<<endl;
            }
            param = true;
        }

    }

    if (i>=argc || Points.size()==0) {
        usage();
        return -1;
    }

    ImageIO::TFileFormat FileType;
    bool ErrorFlag = false;
    int IOResult = 0;
    BIAS::Image<unsigned char> CurImage;

    PMatrix P;
    map<string, PMatrix> mapP;
    for (int j=i; j<argc; j++) {
        if (strstr(argv[j], ".mat")!=NULL) {
            cout<<"arg "<<j<<" is .mat";
            PMatrix P;
            P.SetZero();
            if (!BBC) {
                P.Load(argv[j]);
            } else {
                P.LoadBBC(argv[j]);
            }
            cout << "P is "<<P<<endl;
            mapP[argv[j]] = P;
        }
    }
    if (mapP.size()==0) {
        BIASERR("No matrices found !");
        return -1;
    }

    const int cosize = 40;



    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 reading "<< argv[i]);
            continue;
        }

        stringstream ss;
        ss << argv[i] <<".mat";
        if (mapP.find(ss.str())!=mapP.end()) {
            cout<<"Found a mat file for "<<argv[i]<<endl;
            P = mapP[ss.str()];
        } else {
            if (!Nearest) {
                cout <<"Found NO mat file for "<<argv[i]<<" !!!!! 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 "<<argv[i]<<": "<<it->first<<endl;
        }
        // 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());
        Camera<unsigned char> cutout;
        cutout.Init(cosize*2+1, cosize*2+1, CurImage.GetChannelCount());
        bool simplecopy = false;
        for (unsigned int p=0; p<Points.size(); p++) {
            KMatrix K = P.GetK();
            HomgPoint2D x = P*Points[p];
            x.Homogenize();
            // find closest grid position
            x[0] = rint(x[0]);
            x[1] = rint(x[1]);

            if (x[0]<cosize || x[0]>= CurImage.GetWidth()-cosize ||
                x[1]<cosize || x[1]>= CurImage.GetWidth()-cosize) continue;
            if (simplecopy) {
                CurImage.SetROI(int(x[0]-cosize), int(x[1]-cosize),
                    int(x[0]+cosize+1), int(x[1]+cosize+1));
                CurImage.GetCopyOfROI(cutout);
                K[0][2] += x[0]-cosize;
                K[1][2] += x[1]-cosize;
            } else {

                float dx = 0, dy = 0;
                unsigned char** pData = cutout.GetImageDataArray();
                for (int xx=-cosize; xx<=cosize; xx++) {
                    for (int yy=-cosize; yy<=cosize; yy++) {
                        dx = float(xx)/2.0;
                        dy = float(yy)/2.0;
                        for (unsigned int c=0; c<CurImage.GetChannelCount(); c++) {
                            double value = CurImage.BilinearInterpolation(x[0]+dx,x[1]+dy,c);
                            value = rint(value);
                            if (value<0.0) value   = 0.0;
                            if (value>255.0) value = 255.0;
                            pData[yy+cosize][(xx+cosize)*CurImage.GetChannelCount()+c] =
                                (unsigned char) value;
                        }
                    }
                }
            }

            ImageDraw<unsigned char>::RectangleCorners(cutout, cosize-5, cosize-5,
                cosize+5, cosize+5);
            ImageDraw<unsigned char>::RectangleCorners(cutout, cosize-4, cosize-4,
                cosize+4, cosize+4);

            string filename = argv[i];
            unsigned int pos = filename.rfind("/");
            if (pos!=string::npos) filename = filename.substr(pos+1);
            stringstream filenamess;
            filenamess<<filename<<"_"<<p;

            // set kmatrix to camera
            PMatrix PNew(K, P.GetR(), P.GetC());
            cutout.SetP(PNew);
            /*
            if (FileType==ImageIO::FF_mip) {
                IOResult = ImageIO::Save(filenamess.str(), cutout, false, false );
            } else {
                IOResult = ImageIO::Save(filenamess.str(), cutout, FileType,
                    false, 0, false);
            } */
            IOResult = ImageIO::Save(filenamess.str(), 
                                     cutout, 
                                     FileType,
                                     false, 
                                     0, 
                                     false);
                            
            if (IOResult!=0) {
                ErrorFlag = true;
                BIASERR("Error writing "<< filenamess.str());
                continue;
            }
        }


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