////////////////////////////////////////////////////////////////////////////
//	File:		    util.h
//	Author:		    Changchang Wu (ccwu@cs.washington.edu)
//	Description :   some utility functions for reading/writing SfM data
//
//  Copyright (c) 2011  Changchang Wu (ccwu@cs.washington.edu)
//    and the University of Washington at Seattle 
//
//  This library 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 3 of the License, or (at your option) any later version.
//
//  This library 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.
//
////////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <math.h>
#include <time.h>
#include <iomanip>
#include <algorithm>
#include "DataInterface.h"

namespace PBA {

//File loader supports .nvm format and bundler format
bool LoadModelFile(const char* name, std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
              std::vector<Point2D>& measurements, std::vector<int>& ptidx, std::vector<int>& camidx, 
              std::vector<std::string>& names, std::vector<int>& ptc); 
void SaveNVM(const char* filename, std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
              std::vector<Point2D>& measurements, std::vector<int>& ptidx, std::vector<int>& camidx, 
              std::vector<std::string>& names, std::vector<int>& ptc);
void SaveBundlerModel(const char* filename, std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
              std::vector<Point2D>& measurements, std::vector<int>& ptidx, std::vector<int>& camidx);

//////////////////////////////////////////////////////////////////
void AddNoise(std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data, float percent);
void AddStableNoise(std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
                    const std::vector<int>& ptidx, const std::vector<int>& camidx, float percent);
bool RemoveInvisiblePoints( std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
                            std::vector<int>& ptidx, std::vector<int>& camidx, 
                            std::vector<Point2D>& measurements, std::vector<std::string>& names, std::vector<int>& ptc);

/////////////////////////////////////////////////////////////////////////////
bool LoadNVM(std::ifstream& in, std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
              std::vector<Point2D>& measurements, std::vector<int>& ptidx, std::vector<int>& camidx,
              std::vector<std::string>& names, std::vector<int>& ptc)
{
    int rotation_parameter_num = 4; 
    bool format_r9t = false;
    std::string token;
    if(in.peek() == 'N') 
    {
        in >> token; //file header
        if(strstr(token.c_str(), "R9T"))
        {
            rotation_parameter_num = 9; //rotation as 3x3 matrix
            format_r9t = true;
        }
    }

	double fxFixed, fyFixed, cxFixed, cyFixed, k1(0);
    int ncam = 0, npoint = 0, nproj = 0;   
	in >> token;
	if (token == "FixedK") {
		// read fixed intrinsics
		std::getline(in, token);
		sscanf(token.c_str(), "%lf %lf %lf %lf %lf", &fxFixed, &cxFixed, &fyFixed, &cyFixed, &k1);
		// read # of cameras
		in >> ncam;
	} else {
		// read # of cameras
		ncam = atoi(token.c_str());
	}
	if(ncam <= 1) return false; 

    //read the camera parameters
    camera_data.resize(ncam); // allocate the camera data
    names.resize(ncam);
    for(int i = 0; i < ncam; ++i)
    {
        double f, q[9], c[3], d[2];
        in >> token >> f ;
        for(int j = 0; j < rotation_parameter_num; ++j) in >> q[j]; 
        in >> c[0] >> c[1] >> c[2] >> d[0] >> d[1];

        camera_data[i].SetFocalLength(f);
        if(format_r9t) 
        {
            camera_data[i].SetMatrixRotation(q);
            camera_data[i].SetTranslation(c);
        }
        else
        {
            //older format for compatibility
            camera_data[i].SetQuaternionRotation(q);        //quaternion from the file
            camera_data[i].SetCameraCenterAfterRotation(c); //camera center from the file
        }
        camera_data[i].SetNormalizedMeasurementDistortion(k1!=0 ? k1 : d[0]);
        names[i] = token;
    }

    //////////////////////////////////////
    in >> npoint;   if(npoint <= 0) return false; 

    //read image projections and 3D points.
    point_data.resize(npoint); 
    for(int i = 0; i < npoint; ++i)
    {
        float pt[3]; int cc[3], npj;
        in  >> pt[0] >> pt[1] >> pt[2] 
            >> cc[0] >> cc[1] >> cc[2] >> npj;
        for(int j = 0; j < npj; ++j)
        {
            int cidx, fidx; float imx, imy;
            in >> cidx >> fidx >> imx >> imy;

            camidx.push_back(cidx);    //camera index
            ptidx.push_back(i);        //point index

            //add a measurement to the vector
            measurements.push_back(Point2D(imx, imy));
            nproj ++;
        }
        point_data[i].SetPoint(pt); 
        ptc.insert(ptc.end(), cc, cc + 3); 
    }
    ///////////////////////////////////////////////////////////////////////////////
    LOG_OUT() << ncam << " cameras; " << npoint << " 3D points; " << nproj << " projections\n";

    return true;
}


void SaveNVM(const char* filename, std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
              std::vector<Point2D>& measurements, std::vector<int>& ptidx, std::vector<int>& camidx, 
              std::vector<std::string>& names, std::vector<int>& ptc)
{
    LOG_OUT() << "Saving model to " << filename << "...\n"; 
    std::ofstream out(filename);

    out << "NVM_V3_R9T\n" << camera_data.size() << '\n' << std::setprecision(12);
    if(names.size() < camera_data.size()) names.resize(camera_data.size(),std::string("unknown"));
    if(ptc.size() < 3 * point_data.size()) ptc.resize(point_data.size() * 3, 0);

    ////////////////////////////////////
    for(size_t i = 0; i < camera_data.size(); ++i)
    {
        CameraT& cam = camera_data[i];
        out << names[i] << ' ' << cam.GetFocalLength() << ' ';
        for(int j  = 0; j < 9; ++j) out << cam.m[0][j] << ' ';
        out << cam.t[0] << ' ' << cam.t[1] << ' ' << cam.t[2] << ' '
            << cam.GetNormalizedMeasurementDistortion() << " 0\n"; 
    }

    out << point_data.size() << '\n';

    for(size_t i = 0, j = 0; i < point_data.size(); ++i)
    {
        Point3D& pt = point_data[i];
        int * pc = &ptc[i * 3];
        out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << ' ' 
            << pc[0] << ' ' << pc[1] << ' ' << pc[2] << ' '; 

        size_t je = j;
        while(je < ptidx.size() && ptidx[je] == (int) i) je++;
        
        out << (je - j) << ' ';

        for(; j < je; ++j)    out << camidx[j] << ' ' << " 0 " << measurements[j].x << ' ' << measurements[j].y << ' ';
        
        out << '\n';
    }
}


bool LoadBundlerOut(const char* name, std::ifstream& in, std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
              std::vector<Point2D>& measurements, std::vector<int>& ptidx, std::vector<int>& camidx,
              std::vector<std::string>& names, std::vector<int>& ptc)
{
    int rotation_parameter_num = 9; 
    std::string token;
    while(in.peek() == '#') std::getline(in, token);

    char listpath[1024], filepath[1024];
    strcpy(listpath, name);
    char* ext = strstr(listpath, ".out");
    strcpy(ext, "-list.txt\0"); 

    ///////////////////////////////////
    std::ifstream listin(listpath);
    if(!listin.is_open())
    {
        listin.close(); listin.clear(); 
        strcpy(ext, ".txt\0"); 
        listin.open(listpath);
    }
    if(!listin.is_open())
    {
        listin.close(); listin.clear(); 
        char * slash = strrchr(listpath, '/');
        if(slash == NULL) slash = strrchr(listpath, '\\');
        slash = slash ? slash + 1 : listpath; 
        strcpy(slash, "image_list.txt"); 
        listin.open(listpath);
    }
    if(listin) LOG_OUT() << "Using image list: " << listpath << '\n';

    // read # of cameras
    int ncam = 0, npoint = 0, nproj = 0; 
    in >> ncam >> npoint;  
    if(ncam <= 1 || npoint <= 1) return false; 
    LOG_OUT() << ncam << " cameras; " << npoint << " 3D points;\n";

    //read the camera parameters
    camera_data.resize(ncam); // allocate the camera data
    names.resize(ncam);

    bool det_checked = false;
    for(int i = 0; i < ncam; ++i)
    {
        float f, q[9], c[3], d[2];
        in >> f >> d[0] >> d[1];
        for(int j = 0; j < rotation_parameter_num; ++j) in >> q[j]; 
        in >> c[0] >> c[1] >> c[2];

        camera_data[i].SetFocalLength(f);
        camera_data[i].SetInvertedR9T(q, c);
        camera_data[i].SetProjectionDistortion(d[0]); 

        if(listin >> filepath && f != 0)
        {
            names[i] = filepath;
            std::getline(listin, token);

            if(!det_checked)
            {
                float det = camera_data[i].GetRotationMatrixDeterminant();
                LOG_OUT() << "Check rotation matrix: " << det << '\n';
                det_checked = true;
            }
        }else
        {
            names[i] = "unknown";
        }
    }


    //read image projections and 3D points.
    point_data.resize(npoint); 
    for(int i = 0; i < npoint; ++i)
    {
        float pt[3]; int cc[3], npj;
        in  >> pt[0] >> pt[1] >> pt[2] 
            >> cc[0] >> cc[1] >> cc[2] >> npj;
        for(int j = 0; j < npj; ++j)
        {
            int cidx, fidx; float imx, imy;
            in >> cidx >> fidx >> imx >> imy;

            camidx.push_back(cidx);    //camera index
            ptidx.push_back(i);        //point index

            //add a measurement to the vector
            measurements.push_back(Point2D(imx, -imy));
            nproj ++;
        }
        point_data[i].SetPoint(pt[0], pt[1], pt[2]); 
        ptc.insert(ptc.end(), cc, cc + 3); 
    }
    ///////////////////////////////////////////////////////////////////////////////
    LOG_OUT() << ncam << " cameras; " << npoint << " 3D points; " << nproj << " projections\n";
    return true;
}

void SaveBundlerOut(const char* filename, std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
              std::vector<Point2D>& measurements, std::vector<int>& ptidx, std::vector<int>& camidx,
			  std::vector<std::string>& names, std::vector<int>& ptc)
{
    char listpath[1024];   strcpy(listpath, filename);
    char* ext = strstr(listpath, ".out"); if(ext == NULL) return;
    strcpy(ext, "-list.txt\0"); 

	std::ofstream out(filename);
	out << "# Bundle file v0.3\n";
    out << std::setprecision(12); //need enough precision
	out << camera_data.size() << " " << point_data.size() << '\n';

	//save camera data
	for(size_t i = 0; i < camera_data.size(); ++i)
    {
        float q[9], c[3];
		CameraT& ci = camera_data[i];
		out << ci.GetFocalLength() << ' ' << ci.GetProjectionDistortion() << " 0\n";
		ci.GetInvertedR9T(q, c);
        for(int j = 0; j < 9; ++j) out << q[j] << (((j % 3) == 2)? '\n' : ' '); 
        out << c[0] << ' ' << c[1] << ' ' << c[2] << '\n';
	}
	///
	for(size_t i = 0, j = 0; i < point_data.size(); ++i)
	{
		int npj = 0, *ci = &ptc[i * 3]; Point3D& pt = point_data[i];
		while(j + npj < point_data.size() && ptidx[j + npj] == ptidx[j]) npj++;
		///////////////////////////
		out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << '\n';
		out << ci[0] << ' ' << ci[1] << ' ' << ci[2] << '\n';
		out << npj << ' ';
		for(int k = 0; k < npj; ++k) out << camidx[j + k] << " 0 " 
										 << measurements[j + k].x << ' ' << -measurements[j + k].y << '\n';
		out << '\n'; j += npj;
	}

	std::ofstream listout(listpath);
	for(size_t i = 0; i < names.size(); ++i) listout << names[i] << '\n';
}

template<class CameraT, class Point3D>
bool LoadBundlerModel(std::ifstream& in, std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
              std::vector<Point2D>& measurements, std::vector<int>& ptidx, std::vector<int>& camidx)
{
    // read bundle data from a file
    size_t ncam = 0, npt = 0, nproj = 0;
    if(!(in >> ncam >> npt >> nproj)) return false;
    ///////////////////////////////////////////////////////////////////////////////
    LOG_OUT() << ncam << " cameras; " << npt << " 3D points; " << nproj << " projections\n";

    camera_data.resize(ncam);
    point_data.resize(npt);
    measurements.resize(nproj);
    camidx.resize(nproj);
    ptidx.resize(nproj);

    for(size_t i = 0; i < nproj; ++i)    
    {
        double x, y;    int cidx, pidx;
        in >> cidx >> pidx >> x >> y;
        if(((size_t) pidx) == npt && camidx.size() > i) 
        {
            camidx.resize(i);
            ptidx.resize(i);
            measurements.resize(i);
            LOG_OUT() << "Truncate measurements to " << i << '\n';
        }else if(((size_t) pidx) >= npt)
        {
            continue;
        }else
        {
            camidx[i] = cidx;    ptidx[i] = pidx;
            measurements[i].SetPoint2D(x, -y);
        }
    }

    for(size_t i = 0; i < ncam; ++i)
    {
        double p[9];
        for(int j = 0; j < 9; ++j) in >> p[j];
        CameraT& cam = camera_data[i];
        cam.SetFocalLength(p[6]);
        cam.SetInvertedRT(p, p + 3);
        cam.SetProjectionDistortion(p[7]); 
    }

    for(size_t i = 0; i < npt; ++i)
    {
        double pt[3];
        in >> pt[0] >> pt[1] >> pt[2];
        point_data[i].SetPoint(pt);
    }
    return true;
}

void SaveBundlerModel(const char* filename, std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
              std::vector<Point2D>& measurements, std::vector<int>& ptidx, std::vector<int>& camidx)
{
    LOG_OUT() << "Saving model to " << filename << "...\n"; 
    std::ofstream out(filename);
    out << std::setprecision(12); //need enough precision
    out <<  camera_data.size() << ' ' << point_data.size() << ' ' << measurements.size() << '\n';
    for(size_t i = 0; i < measurements.size(); ++i)
    {
        out << camidx[i] << ' ' << ptidx[i] << ' ' << measurements[i].x << ' ' << -measurements[i].y << '\n';
    }

    for(size_t i = 0; i < camera_data.size(); ++i)
    {
        CameraT& cam = camera_data[i];
        double r[3], t[3]; cam.GetInvertedRT(r, t);
        out << r[0] << ' ' << r[1] << ' ' << r[2] << ' ' 
            << t[0] << ' ' << t[1] << ' ' << t[2] << ' ' << cam.f 
            << ' '  << cam.GetProjectionDistortion() << " 0\n";
    }

    for(size_t i = 0; i < point_data.size(); ++i)
    {
        Point3D& pt = point_data[i];
        out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << '\n';
    }
}

bool LoadModelFile(const char* name, std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
              std::vector<Point2D>& measurements, std::vector<int>& ptidx, std::vector<int>& camidx, 
              std::vector<std::string>& names, std::vector<int>& ptc)
{
    if(name == NULL)return false;
    std::ifstream in(name); 

    LOG_OUT() << "Loading cameras/points: " << name <<"\n" ;
    if(!in.is_open()) return false;

    if(strstr(name, ".nvm"))return LoadNVM(in, camera_data, point_data, measurements, ptidx, camidx, names, ptc);
    else if(strstr(name, ".out")) return LoadBundlerOut(name, in, camera_data, point_data, measurements, ptidx, camidx, names, ptc);
    else return LoadBundlerModel(in, camera_data, point_data, measurements, ptidx, camidx);
}


float random_ratio(float percent)
{
    return (rand() % 101 - 50) * 0.02f * percent + 1.0f;
}

void AddNoise(std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data, float percent)
{
    std::srand((unsigned int) time(NULL));
    for(size_t i = 0; i < camera_data.size(); ++i)
    {
        camera_data[i].f *= random_ratio(percent);
        camera_data[i].t[0] *= random_ratio(percent);
        camera_data[i].t[1] *= random_ratio(percent);
        camera_data[i].t[2] *= random_ratio(percent);
        double e[3];
        camera_data[i].GetRodriguesRotation(e);
        e[0] *= random_ratio(percent);
        e[1] *= random_ratio(percent);
        e[2] *= random_ratio(percent);
        camera_data[i].SetRodriguesRotation(e);
    }

    for(size_t i = 0; i < point_data.size(); ++i)
    {
        point_data[i].xyz[0] *= random_ratio(percent);
        point_data[i].xyz[1] *= random_ratio(percent);
        point_data[i].xyz[2] *= random_ratio(percent);
    }
}

void AddStableNoise(std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
                    const std::vector<int>& ptidx, const std::vector<int>& camidx, float percent)
{
    ///
    std::srand((unsigned int) time(NULL));
    //do not modify the visibility status..
    std::vector<float> zz0(ptidx.size());
    std::vector<CameraT> backup = camera_data;
    std::vector<float> vx(point_data.size()), vy(point_data.size()), vz(point_data.size());
    for(size_t i = 0; i < point_data.size(); ++i)
    {
        Point3D& pt = point_data[i];
        vx[i] = pt.xyz[0];
        vy[i] = pt.xyz[1];
        vz[i] = pt.xyz[2];
    }

    //find out the median location of all the 3D points.
    size_t median_idx = point_data.size() / 2;

    std::nth_element(vx.begin(), vx.begin() + median_idx, vx.end());
    std::nth_element(vy.begin(), vy.begin() + median_idx, vy.end());
    std::nth_element(vz.begin(), vz.begin() + median_idx, vz.end());
    float cx = vx[median_idx], cy = vy[median_idx], cz = vz[median_idx];

    for(size_t i  = 0; i < ptidx.size(); ++i)
    {
        CameraT& cam = camera_data[camidx[i]];
        Point3D& pt  = point_data[ptidx[i]];
        zz0[i] = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + cam.m[2][2] * pt.xyz[2] + cam.t[2];
    }

    std::vector<float> z2 = zz0;    median_idx = ptidx.size() / 2;
    std::nth_element(z2.begin(), z2.begin() + median_idx, z2.end());
    float mz = z2[median_idx]; // median depth
    float dist_noise_base = mz * 0.2f;

    /////////////////////////////////////////////////
    //modify points first..
    for(size_t i = 0; i < point_data.size(); ++i)
    {
        Point3D& pt = point_data[i];
        pt.xyz[0] = pt.xyz[0] - cx + dist_noise_base * random_ratio(percent);
        pt.xyz[1] = pt.xyz[1] - cy + dist_noise_base * random_ratio(percent);
        pt.xyz[2] = pt.xyz[2] - cz + dist_noise_base * random_ratio(percent);
    }
    
    std::vector<bool> need_modification(camera_data.size(), true);
    int invalid_count = 0, modify_iteration = 1;

    do
    {
        if(invalid_count)  LOG_OUT() << "NOTE" << std::setw(2) << modify_iteration 
                                << ": modify " << invalid_count << " camera to fix visibility\n";

        //////////////////////////////////////////////////////
        for(size_t i = 0; i < camera_data.size(); ++i)
        {
            if(!need_modification[i])continue;
            CameraT & cam = camera_data[i];
            double e[3], c[3];   cam = backup[i];
            cam.f *= random_ratio(percent);

            ///////////////////////////////////////////////////////////
            cam.GetCameraCenter(c);
            c[0] = c[0] - cx + dist_noise_base * random_ratio(percent);
            c[1] = c[1] - cy + dist_noise_base * random_ratio(percent);
            c[2] = c[2] - cz + dist_noise_base * random_ratio(percent);

            ///////////////////////////////////////////////////////////
            cam.GetRodriguesRotation(e);
            e[0] *= random_ratio(percent);
            e[1] *= random_ratio(percent);
            e[2] *= random_ratio(percent);

            ///////////////////////////////////////////////////////////
            cam.SetRodriguesRotation(e);
            cam.SetCameraCenterAfterRotation(c);
        }
        std::vector<bool>  invalidc(camera_data.size(), false);

        invalid_count = 0;
        for(size_t i = 0; i < ptidx.size(); ++i)
        {
            int cid = camidx[i];
            if(need_modification[cid] ==false) continue;
            if(invalidc[cid])continue;
             CameraT& cam = camera_data[cid];
             Point3D& pt  = point_data[ptidx[i]];
             float z = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + cam.m[2][2] * pt.xyz[2] + cam.t[2];
             if (z * zz0[i] > 0)continue;
             if (zz0[i] == 0 && z > 0) continue;
             invalid_count++; 
             invalidc[cid]  = true;
        }

        need_modification = invalidc;
        modify_iteration++;

    }while(invalid_count && modify_iteration < 20);

}

void ExamineVisiblity(const char* input_filename )
{

    //////////////
    std::vector<CameraD> camera_data;
    std::vector<Point3B> point_data; 
    std::vector<int> ptidx, camidx;
    std::vector<Point2D> measurements;
    std::ifstream in (input_filename);
    LoadBundlerModel(in, camera_data, point_data, measurements, ptidx, camidx);

    ////////////////
    int count = 0; double d1 = 100, d2 = 100;
    LOG_OUT() << "checking visibility...\n";
    std::vector<double> zz(ptidx.size());
    for(size_t i  = 0; i < ptidx.size(); ++i)
    {
        CameraD& cam = camera_data[camidx[i]];
        Point3B& pt  = point_data[ptidx[i]];
        double dz = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + cam.m[2][2] * pt.xyz[2] + cam.t[2];
        //double dx = cam.m[0][0] * pt.xyz[0] + cam.m[0][1] * pt.xyz[1] + cam.m[0][2] * pt.xyz[2] + cam.t[0];
        //double dy = cam.m[1][0] * pt.xyz[0] + cam.m[1][1] * pt.xyz[1] + cam.m[1][2] * pt.xyz[2] + cam.t[1];

        ////////////////////////////////////////
        float c[3];       cam.GetCameraCenter(c);

        CameraT camt; camt.SetCameraT(cam);
        Point3D ptt; ptt.SetPoint(pt.xyz);
        double fz = camt.m[2][0] * ptt.xyz[0] + camt.m[2][1] * ptt.xyz[1] + camt.m[2][2] * ptt.xyz[2] + camt.t[2];
        double fz2 = camt.m[2][0] * (ptt.xyz[0] - c[0]) + camt.m[2][1] * (ptt.xyz[1] - c[1])
                                                       + camt.m[2][2] * (ptt.xyz[2] - c[2]);


        //if(dz == 0 && fz == 0) continue;

        if(dz * fz <= 0 || fz == 0)
        {
            LOG_OUT() << "cam " << camidx[i] //<<// "; dx = " << dx << "; dy = " << dy 
                      << "; double: " << dz << "; float " << fz << "; float2 " << fz2 << "\n";
            //LOG_OUT() << cam.m[2][0] << " "<<cam.m[2][1]<< " " <<  cam.m[2][2] << " "<<cam.t[2] << "\n";
            //LOG_OUT() << camt.m[2][0] << " "<<camt.m[2][1]<< " " <<  camt.m[2][2] << " "<<camt.t[2] << "\n";
            //LOG_OUT() << cam.m[2][0] - camt.m[2][0] << " " <<cam.m[2][1] - camt.m[2][1]<< " " 
            //          << cam.m[2][2] - camt.m[2][2] << " " <<cam.t[2] - camt.t[2]<< "\n";
        }

        zz[i] = dz;
        d1  = std::min(fabs(dz), d1);
        d2 = std::min(fabs(fz), d2);
    }

    LOG_OUT()   << count << " points moved to wrong side " 
                << d1 << ", " << d2  <<"\n";
}

bool RemoveInvisiblePoints( std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
                            std::vector<int>& ptidx, std::vector<int>& camidx,
                            std::vector<Point2D>& measurements, std::vector<std::string>& names, std::vector<int>& ptc)
{
    std::vector<float> zz(ptidx.size());
    for(size_t i  = 0; i < ptidx.size(); ++i)
    {
        CameraT& cam = camera_data[camidx[i]];
        Point3D& pt  = point_data[ptidx[i]];
        zz[i] = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + cam.m[2][2] * pt.xyz[2] + cam.t[2];
    }
    size_t median_idx = ptidx.size() / 2;
    std::nth_element(zz.begin(), zz.begin() + median_idx, zz.end());
    float dist_threshold = zz[median_idx] * 0.001f;

    //keep removing 3D points. until all of them are infront of the cameras..
    std::vector<bool> pmask(point_data.size(), true);
    int points_removed = 0;
    for(size_t i  = 0; i < ptidx.size(); ++i)
    {
        int cid = camidx[i], pid = ptidx[i];
        if(!pmask[pid])continue;
        CameraT& cam = camera_data[cid];
        Point3D& pt  = point_data[pid];
        bool visible = (cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] + cam.m[2][2] * pt.xyz[2] + cam.t[2] > dist_threshold);
        pmask[pid] = visible; //this point should be removed
        if(!visible) points_removed++;
    }
    if(points_removed == 0) return false;
    std::vector<int>  cv(camera_data.size(), 0);
    //should any cameras be removed ?
    int min_observation = 20; //cameras should see at least 20 points

    do
    {
        //count visible points for each camera
        std::fill(cv.begin(), cv.end(), 0);
        for(size_t i = 0; i < ptidx.size(); ++i)
        {
            int cid = camidx[i], pid = ptidx[i];
            if(pmask[pid])  cv[cid]++; 
        }

        //check if any more points should be removed
        std::vector<int>  pv(point_data.size(), 0);
        for(size_t i = 0; i < ptidx.size(); ++i)
        {
            int cid = camidx[i], pid = ptidx[i];
            if(!pmask[pid]) continue; //point already removed
            if(cv[cid] < min_observation) //this camera shall be removed.
            {
                ///
            }else
            {
                pv[pid]++;
            }
        }

        points_removed = 0;
        for(size_t i = 0; i < point_data.size(); ++i)
        {
            if(pmask[i] == false) continue;
            if(pv[i] >= 2) continue;
            pmask[i] = false;
            points_removed++;
        }
    }while(points_removed > 0);

    ////////////////////////////////////
    std::vector<bool> cmask(camera_data.size(), true);
    for(size_t i = 0; i < camera_data.size(); ++i) cmask[i] = cv[i] >= min_observation; 
    ////////////////////////////////////////////////////////

    std::vector<int> cidx(camera_data.size());
    std::vector<int> pidx(point_data.size());




    ///modified model.
    std::vector<CameraT> camera_data2;
    std::vector<Point3D> point_data2;
    std::vector<int> ptidx2;
    std::vector<int> camidx2;
    std::vector<Point2D> measurements2;
    std::vector<std::string> names2;
    std::vector<int> ptc2;


    //
    if(names.size() < camera_data.size()) names.resize(camera_data.size(),std::string("unknown"));
    if(ptc.size() < 3 * point_data.size()) ptc.resize(point_data.size() * 3, 0);

    //////////////////////////////
    int new_camera_count = 0, new_point_count = 0;
    for(size_t i = 0; i < camera_data.size(); ++i)
    {
        if(!cmask[i])continue;
        camera_data2.push_back(camera_data[i]);
        names2.push_back(names[i]);
        cidx[i] = new_camera_count++;
    }

    for(size_t i = 0; i < point_data.size(); ++i)
    {
        if(!pmask[i])continue;
        point_data2.push_back(point_data[i]);
        ptc.push_back(ptc[i]);
        pidx[i] = new_point_count++;
    }

    int new_observation_count = 0;
    for(size_t i = 0; i < ptidx.size(); ++i)
    {
        int pid = ptidx[i], cid = camidx[i];
        if(!pmask[pid] || ! cmask[cid]) continue;
        ptidx2.push_back(pidx[pid]);
        camidx2.push_back(cidx[cid]);
        measurements2.push_back(measurements[i]);
        new_observation_count++;
    }

    LOG_OUT() << "NOTE: removing " << (camera_data.size() - new_camera_count) << " cameras; "<< (point_data.size() - new_point_count) 
                << " 3D Points; " << (measurements.size() - new_observation_count) << " Observations;\n";

    camera_data2.swap(camera_data); names2.swap(names);
    point_data2.swap(point_data);   ptc2.swap(ptc);
    ptidx2.swap(ptidx);  camidx2.swap(camidx);
    measurements2.swap(measurements);

    return true;
}

void SaveModelFile(const char* outpath, std::vector<CameraT>& camera_data, std::vector<Point3D>& point_data,
              std::vector<Point2D>& measurements, std::vector<int>& ptidx, std::vector<int>& camidx, 
              std::vector<std::string>& names, std::vector<int>& ptc)
{
	if(outpath == NULL) return;
    if(strstr(outpath, ".nvm"))
            SaveNVM(outpath, camera_data, point_data, measurements, ptidx, camidx, names, ptc); 
	else if(strstr(outpath, ".out"))
			SaveBundlerOut(outpath, camera_data, point_data, measurements, ptidx, camidx, names, ptc);
       else      
            SaveBundlerModel(outpath,  camera_data, point_data, measurements, ptidx, camidx);
}

} // namespace PBA