p_compute.cxx

Go to the documentation of this file.

/*
         Copyright 2008-2019 Pierre SMARS (smars@yuntech.edu.tw)
         This program is free software: you can redistribute it and/or modify
         it under the terms of the GNU General Public License as published by
         the Free Software Foundation, either version 2 of the License, or
         (at your option) any later version.
 
         This program 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 General Public License for more details.
 
         You should have received a copy of the GNU General Public License
         along with this program.  If not, see <http://www.gnu.org/licenses/>.
         */
#include <iostream>
#include <fstream>
#include <vector>
#include <vgl/vgl_homg_point_2d.h>
#include <vgl/vgl_homg_point_3d.h>
#include <vgl/vgl_distance.h>
#include <mvl/PMatrix.h>
#include <mvl/PMatrixComputeLinear.h>
#include <mvl/HomgNorm2D.h>
#include <mvl/PMatrixDecompCR.h>
#include <vnl/vnl_least_squares_function.h>
#include <vnl/algo/vnl_levenberg_marquardt.h>
 
#include "a_point.h"
#include "SimpleOpt.h"
 
enum { OPT_HELP, OPT_NONLIN, OPT_VERB, OPT_QUIET, OPT_HOMOGENEOUS, OPT_RADIAL, OPT_OUT, OPT_SQUARE, OPT_SKEW, OPT_INTERNAL};
 
CSimpleOpt::SOption g_rgOptions[] = {
        { OPT_NONLIN, _T("-nl"),     SO_NONE    },
        { OPT_NONLIN, _T("--non-linear"),     SO_NONE    },
        { OPT_INTERNAL,  _T("-i"),     SO_REQ_SEP },
        { OPT_INTERNAL,  _T("--internal"),     SO_REQ_SEP },
        { OPT_HOMOGENEOUS, _T("-hg"),     SO_NONE    },
        { OPT_HOMOGENEOUS, _T("--homogeneous"),     SO_NONE    },
        { OPT_SKEW, _T("-ns"),     SO_NONE    },
        { OPT_SKEW, _T("--no-skew"),     SO_NONE    },
        { OPT_SQUARE, _T("-sp"),     SO_NONE    },
        { OPT_SQUARE, _T("--square-pixels"),     SO_NONE    },
        { OPT_RADIAL, _T("-r"),     SO_NONE    },
        { OPT_RADIAL, _T("--radial"),     SO_NONE    },
        { OPT_VERB, _T("-v"),     SO_NONE    },
        { OPT_VERB, _T("--verbose"),     SO_NONE    },
        { OPT_QUIET, _T("-q"),     SO_NONE    },
        { OPT_QUIET, _T("--quiet"),     SO_NONE    },
        { OPT_OUT, _T("-o"), SO_REQ_SEP },
        { OPT_HELP, _T("-?"),     SO_NONE    },
        { OPT_HELP, _T("-h"),     SO_NONE    },
        { OPT_HELP, _T("--help"), SO_NONE    },
        SO_END_OF_OPTIONS
};
 
bool verbose = false;
//---------------------------------------------------------------------------
int error(int val)
{
#include "p_compute.help"
        return val;
}
//***************************************************************************
class a_camera_function : public vnl_least_squares_function
{
        public:
                a_camera_function( std::vector<vgl_homg_point_2d<double> > & points_image,
                                std::vector<vgl_homg_point_3d<double> > & points_world) :
                        vnl_least_squares_function(12, points_image.size()*2+2, no_gradient),
                        points_image_(points_image),
                        points_world_(points_world),
                        w_(0.),
                        square_pixels_(false),
                        no_skew_(false)
        {};
                void f(const vnl_vector<double>& x, vnl_vector<double>& fx);
                void set_w(const double w) {w_=w;}
                double get_w() const {return w_;}
                void set_constraints(const bool sp, const bool sk) {square_pixels_=sp; no_skew_=sk;};
                double get_constraint(const vnl_vector<double>& x);
        protected:
                std::vector<vgl_homg_point_2d<double> > & points_image_;
                std::vector<vgl_homg_point_3d<double> > & points_world_;
                double w_;
                bool square_pixels_;
                bool no_skew_;
};
//---------------------------------------------------------------------------
void a_camera_function::f(const vnl_vector<double>& x, vnl_vector<double>& fx)
{
        vnl_double_3x4 p0;
        p0(0,0) = x[0]; p0(0,1) = x[1]; p0(0,2) = x[2]; p0(0,3) = x[3];  
        p0(1,0) = x[4]; p0(1,1) = x[5]; p0(1,2) = x[6]; p0(1,3) = x[7];
        p0(2,0) = x[8]; p0(2,1) = x[9]; p0(2,2) = x[10]; p0(2,3) = x[11];  
        double no = sqrt(p0(2,0)*p0(2,0)+p0(2,1)*p0(2,1)+p0(2,2)*p0(2,2));
        p0 /= no;
        PMatrix P(p0);
        double d = 0;
        for (int i = 0; i< points_image_.size(); i++)
        {
                vgl_homg_point_2d<double> im = P.project(points_world_[i]);
                fx[i*2] = points_image_[i].x()-im.x()/im.w();
                fx[i*2+1] = points_image_[i].y()-im.y()/im.w();
                if (verbose)
                {
                        double dist = vgl_distance(points_image_[i],im);
                        d += dist*dist;
                }
        }
        PMatrixDecompCR decomp(P);
        vnl_double_3x3 K = decomp.C;
        if (no_skew_)
                fx[2*points_image_.size()] = w_*K(0,1);
        else
                fx[2*points_image_.size()] = 0.;
        if (square_pixels_)
                fx[2*points_image_.size()+1] = w_*(K(0,0)-K(1,1));
        else
                fx[2*points_image_.size()+1] = 0.;
        if (verbose)
                std::cerr << sqrt(d/points_image_.size()) << " ";
}
//---------------------------------------------------------------------------
double a_camera_function::get_constraint(const vnl_vector<double>& x)
{
        vnl_double_3x4 p0;
        p0(0,0) = x[0]; p0(0,1) = x[1]; p0(0,2) = x[2]; p0(0,3) = x[3];  
        p0(1,0) = x[4]; p0(1,1) = x[5]; p0(1,2) = x[6]; p0(1,3) = x[7];
        p0(2,0) = x[8]; p0(2,1) = x[9]; p0(2,2) = x[10]; p0(2,3) = x[11];  
        double no = sqrt(p0(2,0)*p0(2,0)+p0(2,1)*p0(2,1)+p0(2,2)*p0(2,2));
        p0 /= no;
        PMatrix P(p0);
        PMatrixDecompCR decomp(P);
        vnl_double_3x3 K = decomp.C;
        double d = K(0,1)*K(0,1);
        d += (K(0,0)-K(1,1))*(K(0,0)-K(1,1));
        return sqrt(d);
}
//***************************************************************************
class a_camera_function_internal : public vnl_least_squares_function
{
        public:
                a_camera_function_internal( std::vector<vgl_homg_point_2d<double> > & points_image,
                                std::vector<vgl_homg_point_3d<double> > & points_world) :
                        vnl_least_squares_function(12, points_image.size()*2+5, no_gradient),
                        points_image_(points_image),
                        points_world_(points_world),
                        w_(0.)
        {};
                void f(const vnl_vector<double>& x, vnl_vector<double>& fx);
                void set_w(const double w) {w_=w;}
                double get_w() const {return w_;}
                void set_K(const vnl_double_3x3& K) {K_=K;}
                void set_hk(const vnl_double_3x3& K) {hk_=K;}
                void set_hK(const vnl_double_4x4& K) {hK_=K;}
                double get_constraint(const vnl_vector<double>& x);
        protected:
                std::vector<vgl_homg_point_2d<double> > & points_image_;
                std::vector<vgl_homg_point_3d<double> > & points_world_;
                double w_;
                vnl_double_3x3 K_;
                vnl_double_3x3 hk_;
                vnl_double_4x4 hK_;
};
//---------------------------------------------------------------------------
void a_camera_function_internal::f(const vnl_vector<double>& x, vnl_vector<double>& fx)
{
        vnl_double_3x4 p0;
        p0(0,0) = x[0]; p0(0,1) = x[1]; p0(0,2) = x[2]; p0(0,3) = x[3];  
        p0(1,0) = x[4]; p0(1,1) = x[5]; p0(1,2) = x[6]; p0(1,3) = x[7];
        p0(2,0) = x[8]; p0(2,1) = x[9]; p0(2,2) = x[10]; p0(2,3) = x[11];  
        double no = sqrt(p0(2,0)*p0(2,0)+p0(2,1)*p0(2,1)+p0(2,2)*p0(2,2));
        p0 /= no;
        PMatrix P(p0);
        double d = 0;
        for (int i = 0; i< points_image_.size(); i++)
        {
                vgl_homg_point_2d<double> im = P.project(points_world_[i]);
                fx[i*2] = points_image_[i].x()-im.x()/im.w();
                fx[i*2+1] = points_image_[i].y()-im.y()/im.w();
                if (verbose)
                {
                        double dist = vgl_distance(points_image_[i],im);
                        d += dist*dist;
                }
        }
        vnl_double_3x4 pmat2 = hk_*p0*hK_;
        no = sqrt(pmat2(2,0)*pmat2(2,0)+pmat2(2,1)*pmat2(2,1)+pmat2(2,2)*pmat2(2,2));
        pmat2 /= no;
        PMatrix P2(pmat2);
        PMatrixDecompCR decomp(P2);
        vnl_double_3x3 K = decomp.C;
        fx[2*points_image_.size()] = w_*(K(0,0)-K_(0,0));
        fx[2*points_image_.size()+1] = w_*(K(0,1)-K_(0,1));
        fx[2*points_image_.size()+2] = w_*(K(0,2)-K_(0,2));
        fx[2*points_image_.size()+3] = w_*(K(1,1)-K_(1,1));
        fx[2*points_image_.size()+4] = w_*(K(1,2)-K_(1,2));
        if (verbose)
                std::cerr << sqrt(d/points_image_.size()) << " ";
}
//---------------------------------------------------------------------------
double a_camera_function_internal::get_constraint(const vnl_vector<double>& x)
{
        vnl_double_3x4 p0;
        p0(0,0) = x[0]; p0(0,1) = x[1]; p0(0,2) = x[2]; p0(0,3) = x[3];  
        p0(1,0) = x[4]; p0(1,1) = x[5]; p0(1,2) = x[6]; p0(1,3) = x[7];
        p0(2,0) = x[8]; p0(2,1) = x[9]; p0(2,2) = x[10]; p0(2,3) = x[11];  
        vnl_double_3x4 pmat = hk_*p0*hK_;
        double no = sqrt(pmat(2,0)*pmat(2,0)+pmat(2,1)*pmat(2,1)+pmat(2,2)*pmat(2,2));
        pmat /= no;
        PMatrix P(pmat);
        PMatrixDecompCR decomp(P);
        vnl_double_3x3 K = decomp.C;
        double d = (K(0,0)-K_(0,0))*(K(0,0)-K_(0,0));
        d += (K(0,1)-K_(0,1))*(K(0,1)-K_(0,1));
        d += (K(0,2)-K_(0,2))*(K(0,2)-K_(0,2));
        d += (K(1,1)-K_(1,1))*(K(1,1)-K_(1,1));
        d += (K(1,2)-K_(1,2))*(K(1,2)-K_(1,2));
        return sqrt(d);
}
//***************************************************************************
class a_camera2_function : public vnl_least_squares_function
{
        public:
                a_camera2_function( std::vector<vgl_homg_point_2d<double> > & points_image,
                                std::vector<vgl_homg_point_3d<double> > & points_world) :
                        vnl_least_squares_function(17, points_image.size()*2, no_gradient),
                        points_image_(points_image),
                        points_world_(points_world)
        {};
                void f(const vnl_vector<double>& x, vnl_vector<double>& fx);
        protected:
                std::vector<vgl_homg_point_2d<double> > & points_image_;
                std::vector<vgl_homg_point_3d<double> > & points_world_;
};
//---------------------------------------------------------------------------
void a_camera2_function::f(const vnl_vector<double>& x, vnl_vector<double>& fx)
{
        vnl_double_3x4 p0;
        p0(0,0) = x[0]; p0(0,1) = x[1]; p0(0,2) = x[2]; p0(0,3) = x[3];  
        p0(1,0) = x[4]; p0(1,1) = x[5]; p0(1,2) = x[6]; p0(1,3) = x[7];
        p0(2,0) = x[8]; p0(2,1) = x[9]; p0(2,2) = x[10]; p0(2,3) = x[11];  
        PMatrix P(p0);
        double u0 = x[12];
        double v0 = x[13];
        vgl_homg_point_2d<double> center(u0,v0);
        double k1 = x[14];
        double k2 = x[15];
        double k3 = x[16];
        double k0 = 1.-k1-k2-k3;
        double d = 0;
        for (int i = 0; i< points_image_.size(); i++)
        {
                vgl_homg_point_2d<double> im = P.project(points_world_[i]);
                double r= vgl_distance(center,im);
                double L = k0+k1*r+k2*r*r+k3*r*r*r;
                double u = points_image_[i].x();
                double v = points_image_[i].y();
                double un = u0+L*(u-u0); 
                double vn = v0+L*(v-v0); 
                fx[i*2] = un-im.x()/im.w();
                fx[i*2+1] = vn-im.y()/im.w();
                if (verbose)
                {
                        double dist = vgl_distance(points_image_[i],im);
                        d += dist*dist;
                }
        }
        if (verbose)
                std::cerr << sqrt(d/points_image_.size()) << " ";
}
//***************************************************************************
//---------------------------------------------------------------------------
int main(int argc, char**argv)
{
        bool nonlinear = false;
        bool homogeneous = false;
        bool squarepixels = false;
        bool noskew = false;
        bool quiet = false;
        bool radial = false;
        std::string out_code("p");
        std::string internal_file;
        double cu0, cv0;
        double k0, k1, k2, k3;
        CSimpleOpt args(argc, argv, g_rgOptions);
        while (args.Next())
        {
                if (args.LastError() == SO_SUCCESS)
                {
                        if (args.OptionId() == OPT_HELP)
                                return error(0);
                        else if (args.OptionId() == OPT_NONLIN)
                                nonlinear = true;
                        else if (args.OptionId() == OPT_INTERNAL)
                                internal_file = args.OptionArg();
                        else if (args.OptionId() == OPT_HOMOGENEOUS)
                                homogeneous = true;
                        else if (args.OptionId() == OPT_SQUARE)
                                squarepixels = true;
                        else if (args.OptionId() == OPT_SKEW)
                                noskew = true;
                        else if (args.OptionId() == OPT_RADIAL)
                                radial = true;
                        else if (args.OptionId() == OPT_VERB)
                                verbose = true;
                        else if (args.OptionId() == OPT_QUIET)
                                quiet = true;
                        else if (args.OptionId() == OPT_OUT)
                                out_code = args.OptionArg();
                        else
                                return error(-1);
                        // handle option, using OptionId(), OptionText() and OptionArg()
                } else {
                        std::cerr << "Invalid argument: " << args.OptionText() << std::endl;
                        return error(args.LastError());
                        // handle error, one of: SO_OPT_INVALID, SO_OPT_MULTIPLE,
                        // SO_ARG_INVALID, SO_ARG_INVALID_TYPE, SO_ARG_MISSING
                }
        }
        if (args.FileCount() != 0) return error(-2);
        if (radial|| squarepixels|| noskew|| (internal_file != "")) nonlinear = true;
        vnl_double_3x3 * K0;
        if (internal_file != "")
        {
                std::ifstream in(internal_file.c_str());
                if (!in)
                {
                        std::cerr << "Cannot open file '" << internal_file << "'" << std::endl;
                        return error(-2);
                }
                K0 = new vnl_double_3x3;
                int nx,ny;
                in >> nx >> ny;
                if ((nx!=3)||(ny!=3))
                {
                        std::cerr << "Internal orientation matrix has wrong dimension: " << nx << "x" << ny << " and should be 3x3" << std::endl;
                        return error(-3);
                }
                in >> *K0;
                in.close();
        }
        std::vector<vgl_homg_point_2d<double> > points_image;
        std::vector<vgl_homg_point_3d<double> > points_world;
        // Read points
        while (std::cin.good()) {
                double x,y,z,t; 
                std::cin >> x >> y;
                if (homogeneous)
                        std::cin >> t;
                else 
                        t = 1.; 
                points_image.push_back(vgl_homg_point_2d<double>(x,y,t));
                std::cin >> x >> y >> z; 
                if (homogeneous)
                        std::cin >> t;
                points_world.push_back(vgl_homg_point_3d<double>(x,y,z,t));
                std::cin >> std::ws; // Eat whitespace
        }
        //----------------------------------------
        //normalisation
        int npts = points_image.size();
        //1. calculate centre of point cloud
        double um = 0; double vm = 0;
        double xm = 0; double ym = 0; double zm = 0;
        for (int i=0; i< npts; i++)
        {
                um += points_image[i].x();
                vm += points_image[i].y();
                xm += points_world[i].x();
                ym += points_world[i].y();
                zm += points_world[i].z();
        }
        um /= npts; vm /= npts;
        xm /= npts; ym /= npts; zm /= npts;
        //2. compute average size of point cloud
        a_point im0(um,vm,0.);
        a_point wo0(xm,ym,zm);
        double dim = 0.;
        double dwm = 0.;
        for (int i=0; i< npts; i++)
        {
                a_point im(points_image[i].x(),points_image[i].y(),0.);
                double di = dist(im,im0);
                dim += di*di;
                a_point wo(points_world[i].x(),points_world[i].y(),points_world[i].z());
                double dw = dist(wo,wo0);
                dwm += dw*dw;
        }
        dim = sqrt(dim)/npts;
        dwm = sqrt(dwm)/npts;
        //3. scale and translate point cloud
        std::vector<vgl_homg_point_2d<double> > points_image_n;
        std::vector<vgl_homg_point_3d<double> > points_world_n;
        for (int i=0; i< npts; i++)
        {
                double u = (points_image[i].x()-um)/dim;
                double v = (points_image[i].y()-vm)/dim;
                double x = (points_world[i].x()-xm)/dwm;
                double y = (points_world[i].y()-ym)/dwm;
                double z = (points_world[i].z()-zm)/dwm;
                points_image_n.push_back(vgl_homg_point_2d<double>(u,v,1.0));
                points_world_n.push_back(vgl_homg_point_3d<double>(x,y,z,1.0));
        }
        //4. prepare the transformation matrices
        vnl_double_3x3 hk(0.);
        hk.fill_diagonal(dim);
        hk(0,2) = um;
        hk(1,2) = vm;
        hk(2,2) = 1.;
        vnl_double_4x4 hK(0.);
        hK.fill_diagonal(1./dwm);
        hK(0,3) = -xm/dwm;
        hK(1,3) = -ym/dwm;
        hK(2,3) = -zm/dwm;
        hK(3,3) = 1.;
        //end of normalisation
        //----------------------------------------
        auto P = new PMatrix;
        PMatrix P2;
        auto pmat = new vnl_double_3x4;
        PMatrixComputeLinear computor;
        if (!computor.compute(points_image_n, points_world_n,P))
        {
                std::cerr << "Error" << std::endl;
                return error(-4);
        }
        if (nonlinear)
        {
                if (!quiet)
                {
                        P->get(pmat);
                        vnl_double_3x4 pmat2 = hk*(*pmat)*hK;
                        double no = sqrt(pmat2(2,0)*pmat2(2,0)+pmat2(2,1)*pmat2(2,1)+pmat2(2,2)*pmat2(2,2));
                        pmat2 /= no;
                        P2.set(pmat2);
                        double m = 0;
                        for (int i=0; i<npts; i++)
                        {
                                vgl_homg_point_2d<double> pt = P2.project(points_world[i]);
                                double d = vgl_distance(pt,points_image[i]);
                                m += d*d;
                        }
                        std::cerr << "Matrix P before optimisation:" << std::endl;
                        std::cerr << P2 << std::endl;
                        std::cerr << "RMS error before optimisation: " << sqrt(m/points_world.size()) << std::endl;
                }
                vnl_vector<double> x;
                if (radial)
                        x.set_size(17);
                else
                        x.set_size(12);
                unsigned int vv = 0;
                vnl_least_squares_function * f;
                if (radial)
                        f = new a_camera2_function(points_image_n,points_world_n);
                else if (internal_file != "")
                        f = new a_camera_function_internal(points_image_n,points_world_n);
                else
                        f = new a_camera_function(points_image_n,points_world_n);
                for (int i = 0; i< 3; i++)
                {
                        for (int j = 0; j< 4; j++)
                        {
                                x[vv] = P->get(i,j);
                                vv++;
                        }
                }
                if (radial)
                {
                        x[12] = 0; x[13] = 0;           
                        x[14] = 0; x[15] = 0; x[16] = 0;
                }
                vnl_levenberg_marquardt levmarq(*f);
                if ((!radial)&&((squarepixels)||(noskew)))
                {
                        a_camera_function * f2 = (a_camera_function *)f;
                        f2->set_constraints(squarepixels,noskew);
                        double w0 = 0.1;
                        f2->set_w(w0);  
                        for (int j=0;j<5; j++)
                        {
                                levmarq.minimize(x);
                                if (!quiet)
                                        std::cerr << w0 << " " << f2->get_constraint(x) << std::endl;
                                w0 *= 10.;
                                f2->set_w(w0);  
                        }
                } else if (internal_file != "")
                {
                        a_camera_function_internal * f2 = (a_camera_function_internal *)f;
                        f2->set_K(*K0);
                        f2->set_hk(hk);
                        f2->set_hK(hK);
                        double w0 = 0.1;
                        f2->set_w(w0);  
                        for (int j=0;j<5; j++)
                        {
                                levmarq.minimize(x);
                                if (!quiet)
                                        std::cerr << w0 << " " << f2->get_constraint(x) << std::endl;
                                w0 *= 10.;
                                f2->set_w(w0);  
                        }
                }
                //levmarq.set_max_function_evals(10000);
                levmarq.minimize(x);
                if (!quiet)
                {
                        std::cerr << "Levenberg Marquardt" << std::endl
                                << "min: " << levmarq.get_end_error() << " at " << x << std::endl
                                << "iterations: " << levmarq.get_num_iterations() << "    "
                                << "evaluations: " << levmarq.get_num_evaluations() << std::endl;
                        levmarq.diagnose_outcome();
                }
                vnl_double_3x4 p0;
                p0(0,0) = x[0]; p0(0,1) = x[1]; p0(0,2) = x[2]; p0(0,3) = x[3];
                p0(1,0) = x[4]; p0(1,1) = x[5]; p0(1,2) = x[6]; p0(1,3) = x[7];
                p0(2,0) = x[8]; p0(2,1) = x[9]; p0(2,2) = x[10]; p0(2,3) = x[11];  
                P->set(p0);
                if (radial)
                {
                        cu0 = x[12]*dim+um;
                        cv0 = x[13]*dim+vm;
                        k1 = x[14]/dim;
                        k2 = x[15]/dim/dim;
                        k3 = x[16]/dim/dim/dim;
                        k0 = 1.-k1*dim-k2*dim*dim-k3*dim*dim*dim;
                        std::cerr << k0 << std::endl;
                        std::cout << "-xc=" << cu0 << " -yc=" << cv0 << " -k1=" << k1 << " -k2=" << k2 << " -k3=" << k3 << std::endl;
                }
        }
        //denormalisation
        P->get(pmat);
        vnl_double_3x4 pmat2 = hk*(*pmat)*hK;
        double no = sqrt(pmat2(2,0)*pmat2(2,0)+pmat2(2,1)*pmat2(2,1)+pmat2(2,2)*pmat2(2,2));
        pmat2 /= no;
        P2.set(pmat2);
        double m = 0;
        vgl_homg_point_2d<double> center;
        if (radial)
                center.set(cu0,cv0);
        for (int i=0; i<points_world.size(); i++)
        {
                double d;
                vgl_homg_point_2d<double> pt = P2.project(points_world[i]);
                if (radial)
                {
                        double r= vgl_distance(center,pt);
                        double L = 1+k1*r+k2*r*r+k3*r*r*r;
                        double u = points_image[i].x();
                        double v = points_image[i].y();
                        double un = cu0+L*(u-cu0); 
                        double vn = cv0+L*(v-cv0);
                        vgl_homg_point_2d<double> im2(un,vn,1.); 
                        d = vgl_distance(pt,im2);
                }
                else
                        d = vgl_distance(pt,points_image[i]);
                m += d*d;
        }
        if (!quiet)
                std::cerr << "RMS error: " << sqrt(m/points_world.size()) << std::endl;
        if (out_code=="i")
        {
                PMatrixDecompCR decomp(P2);
                std::cout << "3 3" << std::endl;
                std::cout << decomp.C << std::endl;;
        } else if (out_code=="e")
        {
                PMatrixDecompCR decomp(P2);
                std::cout << "3 4" << std::endl;
                std::cout << decomp.Po << std::endl;
 
        } else
        {
                std::cout << "3 4\n";
                std::cout << P2 << std::endl;
        }
        delete pmat;
}