<commit_before>/*
 * 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 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2011 Sergey Lisitsyn
 * Copyright (C) 2011 Berlin Institute of Technology and Max-Planck-Society
 */

#include <shogun/preprocessor/ClassicMDS.h>
#ifdef HAVE_LAPACK
#include <shogun/preprocessor/DimensionReductionPreprocessor.h>
#include <shogun/mathematics/lapack.h>
#include <shogun/mathematics/arpack.h>
#include <shogun/lib/common.h>
#include <shogun/mathematics/Math.h>
#include <shogun/io/SGIO.h>
#include <shogun/distance/EuclidianDistance.h>
#include <shogun/lib/Signal.h>

using namespace shogun;

CClassicMDS::CClassicMDS() : CDimensionReductionPreprocessor()
{
	m_eigenvalues = SGVector<float64_t>(NULL,0,true);
}

CClassicMDS::~CClassicMDS()
{
	m_eigenvalues.destroy_vector();
}

bool CClassicMDS::init(CFeatures* data)
{
	return true;
}

void CClassicMDS::cleanup()
{
}

CSimpleFeatures<float64_t>* CClassicMDS::apply_to_distance(CDistance* distance)
{
	ASSERT(distance);

	SGMatrix<float64_t> new_feature_matrix = embed_by_distance(distance);

	CSimpleFeatures<float64_t>* new_features =
			new CSimpleFeatures<float64_t>(new_feature_matrix);

	return new_features;
}

SGMatrix<float64_t> CClassicMDS::embed_by_distance(CDistance* distance)
{
	ASSERT(distance->get_num_vec_lhs()==distance->get_num_vec_rhs());
	int32_t N = distance->get_num_vec_lhs();

	// loop variables
	int32_t i,j;

	// get distance matrix
	SGMatrix<float64_t> D_matrix = distance->get_distance_matrix();

	// get D^2 matrix
	float64_t* Ds_matrix = SG_MALLOC(float64_t, N*N);
	for (i=0;i<N;i++)
		for (j=0;j<N;j++)
			Ds_matrix[i*N+j] = CMath::sq(D_matrix.matrix[i*N+j]);

	// centering matrix
	float64_t* H_matrix = SG_MALLOC(float64_t, N*N);
	for (i=0;i<N;i++)
		for (j=0;j<N;j++)
			H_matrix[i*N+j] = (i==j) ? 1.0-1.0/N : -1.0/N;

	// compute -1/2 H D^2 H (result in Ds_matrix)
	cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,
			N,N,N,1.0,H_matrix,N,Ds_matrix,N,0.0,D_matrix.matrix,N);
	cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,
			N,N,N,-0.5,D_matrix.matrix,N,H_matrix,N,0.0,Ds_matrix,N);

	// cleanup
	SG_FREE(D_matrix.matrix);
	SG_FREE(H_matrix);

	// feature matrix representing given distance
	float64_t* replace_feature_matrix = SG_MALLOC(float64_t, N*m_target_dim);

	// status of eigenproblem to be solved
	int eigenproblem_status = 0;
	#ifdef HAVE_ARPACK
	// using ARPACK
		float64_t* eigenvalues_vector = SG_MALLOC(float64_t, m_target_dim);
		// solve eigenproblem with ARPACK (faster)
		arpack_dsaupd(Ds_matrix, NULL, N, m_target_dim, "LM", 1, false, 0.0,
		              eigenvalues_vector, replace_feature_matrix,
		              eigenproblem_status);
		// check for failure
		ASSERT(eigenproblem_status == 0);

		// reverse eigenvectors order
		float64_t tmp;
		for (j=0; j<N; j++)
		{
			for (i=0; i<m_target_dim/2; i++)
			{
				tmp = replace_feature_matrix[j*m_target_dim+i];
				replace_feature_matrix[j*m_target_dim+i] =
					replace_feature_matrix[j*m_target_dim+(m_target_dim-i-1)];
				replace_feature_matrix[j*m_target_dim+(m_target_dim-i-1)] = tmp;
			}
		}
		// reverse eigenvalues order
		for (i=0; i<m_target_dim/2; i++)
		{
			tmp = eigenvalues_vector[i];
			eigenvalues_vector[i] = eigenvalues_vector[m_target_dim-i-1];
			eigenvalues_vector[m_target_dim-i-1] = tmp;
		}

		// finally construct embedding
		for (i=0; i<m_target_dim; i++)
		{
			for (j=0; j<N; j++)
				replace_feature_matrix[j*m_target_dim+i] *=
					CMath::sqrt(eigenvalues_vector[i]);
		}

		// set eigenvalues vector
		m_eigenvalues.destroy_vector();
		m_eigenvalues = SGVector<float64_t>(eigenvalues_vector,m_target_dim,true);
	#else /* not HAVE_ARPACK */
		// using LAPACK
		float64_t* eigenvalues_vector = SG_MALLOC(float64_t, N);
		// solve eigenproblem with LAPACK
		wrap_dsyevr('V','U',N,Ds_matrix,N,N-m_target_dim+1,N,eigenvalues_vector,Ds_matrix,&eigenproblem_status);
		// check for failure
		ASSERT(eigenproblem_status==0);

		// set eigenvalues vector
		m_eigenvalues.destroy_vector();
		m_eigenvalues = SGVector<float64_t>(m_target_dim);

		// fill eigenvalues vector in backwards order
		for (i=0; i<m_target_dim; i++)
			m_eigenvalues.vector[i] = eigenvalues_vector[m_target_dim-i-1];

		SG_FREE(eigenvalues_vector);

		// construct embedding
		for (i=0; i<m_target_dim; i++)
		{
			for (j=0; j<N; j++)
			{
				replace_feature_matrix[j*m_target_dim+i] =
				      Ds_matrix[(m_target_dim-i-1)*N+j] * CMath::sqrt(m_eigenvalues.vector[i]);
			}
		}
	#endif /* HAVE_ARPACK else */

	// warn user if there are negative or zero eigenvalues
	for (i=0; i<m_eigenvalues.vlen; i++)
	{
		if (m_eigenvalues.vector[i]<=0.0)
		{
			SG_WARNING("Embedding is not consistent: features %d-%d are wrong", i, m_eigenvalues.vlen);
			break;
		}
	}

	// cleanup
	SG_FREE(Ds_matrix);

	return SGMatrix<float64_t>(replace_feature_matrix,m_target_dim,N);
}

SGMatrix<float64_t> CClassicMDS::apply_to_feature_matrix(CFeatures* features)
{
	CSimpleFeatures<float64_t>* simple_features = (CSimpleFeatures<float64_t>*) features;
	CDistance* distance = new CEuclidianDistance(simple_features,simple_features);

	SGMatrix<float64_t> new_feature_matrix = embed_by_distance(distance);
	simple_features->set_feature_matrix(new_feature_matrix);

	delete distance;

	return new_feature_matrix;
}

SGVector<float64_t> CClassicMDS::apply_to_feature_vector(SGVector<float64_t> vector)
{
	SG_NOTIMPLEMENTED;
	return vector;
}

#endif /* HAVE_LAPACK */
<commit_msg>Fixed wrong attempt to reuse matrix while DSYEVR is called<commit_after>/*
 * 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 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2011 Sergey Lisitsyn
 * Copyright (C) 2011 Berlin Institute of Technology and Max-Planck-Society
 */

#include <shogun/preprocessor/ClassicMDS.h>
#ifdef HAVE_LAPACK
#include <shogun/preprocessor/DimensionReductionPreprocessor.h>
#include <shogun/mathematics/lapack.h>
#include <shogun/mathematics/arpack.h>
#include <shogun/lib/common.h>
#include <shogun/mathematics/Math.h>
#include <shogun/io/SGIO.h>
#include <shogun/distance/EuclidianDistance.h>
#include <shogun/lib/Signal.h>

using namespace shogun;

CClassicMDS::CClassicMDS() : CDimensionReductionPreprocessor()
{
	m_eigenvalues = SGVector<float64_t>(NULL,0,true);
}

CClassicMDS::~CClassicMDS()
{
	m_eigenvalues.destroy_vector();
}

bool CClassicMDS::init(CFeatures* data)
{
	return true;
}

void CClassicMDS::cleanup()
{
}

CSimpleFeatures<float64_t>* CClassicMDS::apply_to_distance(CDistance* distance)
{
	ASSERT(distance);

	SGMatrix<float64_t> new_feature_matrix = embed_by_distance(distance);

	CSimpleFeatures<float64_t>* new_features =
			new CSimpleFeatures<float64_t>(new_feature_matrix);

	return new_features;
}

SGMatrix<float64_t> CClassicMDS::embed_by_distance(CDistance* distance)
{
	ASSERT(distance->get_num_vec_lhs()==distance->get_num_vec_rhs());
	int32_t N = distance->get_num_vec_lhs();

	// loop variables
	int32_t i,j;

	// get distance matrix
	SGMatrix<float64_t> D_matrix = distance->get_distance_matrix();

	// get D^2 matrix
	float64_t* Ds_matrix = SG_MALLOC(float64_t, N*N);
	for (i=0;i<N;i++)
		for (j=0;j<N;j++)
			Ds_matrix[i*N+j] = CMath::sq(D_matrix.matrix[i*N+j]);

	// centering matrix
	float64_t* H_matrix = SG_MALLOC(float64_t, N*N);
	for (i=0;i<N;i++)
		for (j=0;j<N;j++)
			H_matrix[i*N+j] = (i==j) ? 1.0-1.0/N : -1.0/N;

	// compute -1/2 H D^2 H (result in Ds_matrix)
	cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,
			N,N,N,1.0,H_matrix,N,Ds_matrix,N,0.0,D_matrix.matrix,N);
	cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,
			N,N,N,-0.5,D_matrix.matrix,N,H_matrix,N,0.0,Ds_matrix,N);

	// cleanup
	SG_FREE(D_matrix.matrix);
	SG_FREE(H_matrix);

	// feature matrix representing given distance
	float64_t* replace_feature_matrix = SG_MALLOC(float64_t, N*m_target_dim);

	// status of eigenproblem to be solved
	int eigenproblem_status = 0;
	#ifdef HAVE_ARPACK
	// using ARPACK
		float64_t* eigenvalues_vector = SG_MALLOC(float64_t, m_target_dim);
		// solve eigenproblem with ARPACK (faster)
		arpack_dsaupd(Ds_matrix, NULL, N, m_target_dim, "LM", 1, false, 0.0,
		              eigenvalues_vector, replace_feature_matrix,
		              eigenproblem_status);
		// check for failure
		ASSERT(eigenproblem_status == 0);

		// reverse eigenvectors order
		float64_t tmp;
		for (j=0; j<N; j++)
		{
			for (i=0; i<m_target_dim/2; i++)
			{
				tmp = replace_feature_matrix[j*m_target_dim+i];
				replace_feature_matrix[j*m_target_dim+i] =
					replace_feature_matrix[j*m_target_dim+(m_target_dim-i-1)];
				replace_feature_matrix[j*m_target_dim+(m_target_dim-i-1)] = tmp;
			}
		}
		// reverse eigenvalues order
		for (i=0; i<m_target_dim/2; i++)
		{
			tmp = eigenvalues_vector[i];
			eigenvalues_vector[i] = eigenvalues_vector[m_target_dim-i-1];
			eigenvalues_vector[m_target_dim-i-1] = tmp;
		}

		// finally construct embedding
		for (i=0; i<m_target_dim; i++)
		{
			for (j=0; j<N; j++)
				replace_feature_matrix[j*m_target_dim+i] *=
					CMath::sqrt(eigenvalues_vector[i]);
		}

		// set eigenvalues vector
		m_eigenvalues.destroy_vector();
		m_eigenvalues = SGVector<float64_t>(eigenvalues_vector,m_target_dim,true);
	#else /* not HAVE_ARPACK */
		// using LAPACK
		float64_t* eigenvalues_vector = SG_MALLOC(float64_t, N);
		float64_t* eigenvectors = SG_MALLOC(float64_t, m_target_dim*N);
		// solve eigenproblem with LAPACK
		wrap_dsyevr('V','U',N,Ds_matrix,N,N-m_target_dim+1,N,eigenvalues_vector,eigenvectors,&eigenproblem_status);
		// check for failure
		ASSERT(eigenproblem_status==0);

		// set eigenvalues vector
		m_eigenvalues.destroy_vector();
		m_eigenvalues = SGVector<float64_t>(m_target_dim);

		// fill eigenvalues vector in backwards order
		for (i=0; i<m_target_dim; i++)
			m_eigenvalues.vector[i] = eigenvalues_vector[m_target_dim-i-1];

		SG_FREE(eigenvalues_vector);

		// construct embedding
		for (i=0; i<m_target_dim; i++)
		{
			for (j=0; j<N; j++)
			{
				replace_feature_matrix[j*m_target_dim+i] =
				      eigenvectors[(m_target_dim-i-1)*N+j] * CMath::sqrt(m_eigenvalues.vector[i]);
			}
		}
		SG_FREE(eigenvectors);
	#endif /* HAVE_ARPACK else */

	// warn user if there are negative or zero eigenvalues
	for (i=0; i<m_eigenvalues.vlen; i++)
	{
		if (m_eigenvalues.vector[i]<=0.0)
		{
			SG_WARNING("Embedding is not consistent (got neg eigenvalues): features %d-%d are wrong",
			           i, m_eigenvalues.vlen);
			break;
		}
	}

	// cleanup
	SG_FREE(Ds_matrix);

	return SGMatrix<float64_t>(replace_feature_matrix,m_target_dim,N);
}

SGMatrix<float64_t> CClassicMDS::apply_to_feature_matrix(CFeatures* features)
{
	CSimpleFeatures<float64_t>* simple_features = (CSimpleFeatures<float64_t>*) features;
	CDistance* distance = new CEuclidianDistance(simple_features,simple_features);

	SGMatrix<float64_t> new_feature_matrix = embed_by_distance(distance);
	simple_features->set_feature_matrix(new_feature_matrix);

	delete distance;

	return new_feature_matrix;
}

SGVector<float64_t> CClassicMDS::apply_to_feature_vector(SGVector<float64_t> vector)
{
	SG_NOTIMPLEMENTED;
	return vector;
}

#endif /* HAVE_LAPACK */
<|endoftext|>


<commit_before>
// Copyright (c) 2012, 2013 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "software/SfMViewer/document.h"
#include "openMVG/multiview/projection.hpp"
#include "openMVG/image/image.hpp"

using namespace openMVG;

#include "third_party/cmdLine/cmdLine.h"
#include "third_party/progress/progress.hpp"

#include <stdlib.h>
#include <stdio.h>
#include <cmath>
#include <iterator>
#include <iomanip>

bool exportToPMVSFormat(
  const Document & doc,
  const std::string & sOutDirectory,  //Output PMVS files directory
  const std::string & sImagePath,  // The images path
  const int resolution,
  const int CPU
  )
{
  bool bOk = true;
  if (!stlplus::is_folder(sOutDirectory))
  {
    stlplus::folder_create(sOutDirectory);
    bOk = stlplus::is_folder(sOutDirectory);
  }

  // Create basis directory structure
  stlplus::folder_create( stlplus::folder_append_separator(sOutDirectory) + "models");
  stlplus::folder_create( stlplus::folder_append_separator(sOutDirectory) + "txt");
  stlplus::folder_create( stlplus::folder_append_separator(sOutDirectory) + "visualize");

  if (bOk &&
      stlplus::is_folder(stlplus::folder_append_separator(sOutDirectory) + "models") &&
      stlplus::is_folder( stlplus::folder_append_separator(sOutDirectory) + "txt") &&
      stlplus::is_folder( stlplus::folder_append_separator(sOutDirectory) + "visualize")
      )
  {
    bOk = true;
  }
  else  {
    std::cerr << "Cannot access to one of the desired output directory" << std::endl;
  }

  if (bOk)
  {
    C_Progress_display my_progress_bar( doc._map_camera.size()*2 );
    // Export data :
    //Camera

    size_t count = 0;
    for (std::map<size_t, PinholeCamera>::const_iterator iter = doc._map_camera.begin();
      iter != doc._map_camera.end(); ++iter, ++count, ++my_progress_bar)
    {
      const Mat34 & PMat = iter->second._P;
      std::ostringstream os;
      os << std::setw(8) << std::setfill('0') << count;
      std::ofstream file(
        stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory) + "txt",
        os.str() ,"txt").c_str());
      file << "CONTOUR\n"
        << PMat.row(0) <<"\n"<< PMat.row(1) <<"\n"<< PMat.row(2) << std::endl;
      file.close();
    }

    // Image
    count = 0;
    Image<RGBColor> image;
    for (std::map<size_t, PinholeCamera>::const_iterator iter = doc._map_camera.begin();
      iter != doc._map_camera.end();  ++iter, ++count, ++my_progress_bar)
    {
      size_t imageIndex = iter->first;
      const std::string & sImageName = doc._vec_imageNames[imageIndex];
      std::ostringstream os;
      os << std::setw(8) << std::setfill('0') << count;
      ReadImage( stlplus::create_filespec( sImagePath, sImageName).c_str(), &image );
      std::string sCompleteImageName = stlplus::create_filespec(
        stlplus::folder_append_separator(sOutDirectory) + "visualize", os.str(),"jpg");
      WriteImage( sCompleteImageName.c_str(), image);
    }

    //pmvs_options.txt
    std::ostringstream os;
    os << "level " << resolution << "\n"
     << "csize 2" << "\n"
     << "threshold 0.7" << "\n"
     << "wsize 7" << "\n"
     << "minImageNum 3" << "\n"
     << "CPU " << CPU << "\n"
     << "setEdge 0" << "\n"
     << "useBound 0" << "\n"
     << "useVisData 0" << "\n"
     << "sequence -1" << "\n"
     << "maxAngle 10" << "\n"
     << "quad 2.0" << "\n"
     << "timages -1 0 " << doc._map_camera.size() << "\n"
     << "oimages 0" << "\n"; // ?

    std::ofstream file(stlplus::create_filespec(sOutDirectory, "pmvs_options", "txt").c_str());
    file << os.str();
    file.close();
  }
  return bOk;
}

bool exportToBundlerFormat(
  const Document & doc,
  const std::string & sOutFile, //Output Bundle.rd.out file
  const std::string & sOutListFile)  //Output Bundler list.txt file
{
  std::ofstream os(sOutFile.c_str()	);
  std::ofstream osList(sOutListFile.c_str()	);
  if (os.is_open() && osList.is_open())
  {
    os << "# Bundle file v0.3" << std::endl
      << doc._map_camera.size()
      << " " << doc._tracks.size() << std::endl;

    size_t count = 0;
    for (std::map<size_t, PinholeCamera>::const_iterator iter = doc._map_camera.begin();
      iter != doc._map_camera.end(); ++iter)
    {
      const PinholeCamera & PMat = iter->second;

      Mat3 D;
      D.fill(0.0);
      D .diagonal() = Vec3(1., -1., -1.); // mapping between our pinhole and Bundler convention
      Mat3 R = D * PMat._R;
      Vec3 t = D * PMat._t;
      double focal = PMat._K(0,0);
      double k1 = 0.0, k2 = 0.0; // distortion already removed

      os << focal << " " << k1 << " " << k2 << std::endl //f k1 k2
        << R(0,0) << " " << R(0, 1) << " " << R(0, 2) << std::endl   //R[0]
        << R(1,0) << " " << R(1, 1) << " " << R(1, 2) << std::endl   //R[1]
        << R(2,0) << " " << R(2, 1) << " " << R(2, 2) << std::endl   //R[2]
        << t(0)  << " " << t(1)   << " " << t(2)   << std::endl;     //t

      osList << doc._vec_imageNames[iter->first] << " 0 " << focal << std::endl;
    }

    for (std::map< size_t, tracks::submapTrack >::const_iterator
      iterTracks = doc._tracks.begin();
      iterTracks != doc._tracks.end();
      ++iterTracks)
    {
      const size_t trackId = iterTracks->first;
      const tracks::submapTrack & map_track = iterTracks->second;
      const size_t trackIndex =
        std::distance<std::map< size_t, tracks::submapTrack >::const_iterator>(doc._tracks.begin(), iterTracks);

      const float * ptr3D = & doc._vec_points[trackIndex*3];
      os << ptr3D[0] << " " << ptr3D[1] << " " << ptr3D[2] << std::endl;
      os <<  "255 255 255" << std::endl;
      os << map_track.size() << " ";
      for (tracks::submapTrack::const_iterator iterTrack = map_track.begin();
        iterTrack != map_track.end();
        ++iterTrack)
      {
        const PinholeCamera & PMat = doc._map_camera.find(iterTrack->first)->second;
        Vec2 pt = PMat.Project(Vec3(ptr3D[0], ptr3D[1], ptr3D[2]));
        os << iterTrack->first << " " << iterTrack->second << " " << pt(0) << " " << pt(1) << " ";
      }
      os << std::endl;
    }
    os.close();
    osList.close();
  }
  else
  {
    return false;
  }
  return true;
}

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

  CmdLine cmd;
  std::string sSfMDir;
  std::string sOutDir = "";
  int resolution = 1;
  int CPU = 8;

  cmd.add( make_option('i', sSfMDir, "sfmdir") );
  cmd.add( make_option('o', sOutDir, "outdir") );
  cmd.add( make_option('r', resolution, "resolution") );
  cmd.add( make_option('c', CPU, "CPU") );

  try {
      if (argc == 1) throw std::string("Invalid command line parameter.");
      cmd.process(argc, argv);
  } catch(const std::string& s) {
      std::cerr << "Usage: " << argv[0] << ' '
      << "[-i|--sfmdir path, the SfM_output path] "
      << "[-o|--outdir path] "
      << "[-r|--resolution: divide image coefficient] "
      << "[-c|--nb core] "
      << std::endl;

      std::cerr << s << std::endl;
      return EXIT_FAILURE;
  }

  // Create output dir
  if (!stlplus::folder_exists(sOutDir))
    stlplus::folder_create( sOutDir );

  Document m_doc;
  if (m_doc.load(sSfMDir))
  {
    exportToPMVSFormat(m_doc,
      stlplus::folder_append_separator(sOutDir) + "PMVS",
      stlplus::folder_append_separator(sSfMDir) + "images",
      resolution,
      CPU );

    exportToBundlerFormat(m_doc,
      stlplus::folder_append_separator(sOutDir) +
      stlplus::folder_append_separator("PMVS") + "bundle.rd.out",
      stlplus::folder_append_separator(sOutDir) +
      stlplus::folder_append_separator("PMVS") + "list.txt"
      );

    return( EXIT_SUCCESS );
  }

  // Exit program
  return( EXIT_FAILURE );
}
<commit_msg>update file opening test @fabiencastan.<commit_after>
// Copyright (c) 2012, 2013 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "software/SfMViewer/document.h"
#include "openMVG/multiview/projection.hpp"
#include "openMVG/image/image.hpp"

using namespace openMVG;

#include "third_party/cmdLine/cmdLine.h"
#include "third_party/progress/progress.hpp"

#include <stdlib.h>
#include <stdio.h>
#include <cmath>
#include <iterator>
#include <iomanip>

bool exportToPMVSFormat(
  const Document & doc,
  const std::string & sOutDirectory,  //Output PMVS files directory
  const std::string & sImagePath,  // The images path
  const int resolution,
  const int CPU
  )
{
  bool bOk = true;
  if (!stlplus::is_folder(sOutDirectory))
  {
    stlplus::folder_create(sOutDirectory);
    bOk = stlplus::is_folder(sOutDirectory);
  }

  // Create basis directory structure
  stlplus::folder_create( stlplus::folder_append_separator(sOutDirectory) + "models");
  stlplus::folder_create( stlplus::folder_append_separator(sOutDirectory) + "txt");
  stlplus::folder_create( stlplus::folder_append_separator(sOutDirectory) + "visualize");

  if (bOk &&
      stlplus::is_folder(stlplus::folder_append_separator(sOutDirectory) + "models") &&
      stlplus::is_folder( stlplus::folder_append_separator(sOutDirectory) + "txt") &&
      stlplus::is_folder( stlplus::folder_append_separator(sOutDirectory) + "visualize")
      )
  {
    bOk = true;
  }
  else  {
    std::cerr << "Cannot access to one of the desired output directory" << std::endl;
  }

  if (bOk)
  {
    C_Progress_display my_progress_bar( doc._map_camera.size()*2 );
    // Export data :
    //Camera

    size_t count = 0;
    for (std::map<size_t, PinholeCamera>::const_iterator iter = doc._map_camera.begin();
      iter != doc._map_camera.end(); ++iter, ++count, ++my_progress_bar)
    {
      const Mat34 & PMat = iter->second._P;
      std::ostringstream os;
      os << std::setw(8) << std::setfill('0') << count;
      std::ofstream file(
        stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory) + "txt",
        os.str() ,"txt").c_str());
      file << "CONTOUR\n"
        << PMat.row(0) <<"\n"<< PMat.row(1) <<"\n"<< PMat.row(2) << std::endl;
      file.close();
    }

    // Image
    count = 0;
    Image<RGBColor> image;
    for (std::map<size_t, PinholeCamera>::const_iterator iter = doc._map_camera.begin();
      iter != doc._map_camera.end();  ++iter, ++count, ++my_progress_bar)
    {
      size_t imageIndex = iter->first;
      const std::string & sImageName = doc._vec_imageNames[imageIndex];
      std::ostringstream os;
      os << std::setw(8) << std::setfill('0') << count;
      ReadImage( stlplus::create_filespec( sImagePath, sImageName).c_str(), &image );
      std::string sCompleteImageName = stlplus::create_filespec(
        stlplus::folder_append_separator(sOutDirectory) + "visualize", os.str(),"jpg");
      WriteImage( sCompleteImageName.c_str(), image);
    }

    //pmvs_options.txt
    std::ostringstream os;
    os << "level " << resolution << "\n"
     << "csize 2" << "\n"
     << "threshold 0.7" << "\n"
     << "wsize 7" << "\n"
     << "minImageNum 3" << "\n"
     << "CPU " << CPU << "\n"
     << "setEdge 0" << "\n"
     << "useBound 0" << "\n"
     << "useVisData 0" << "\n"
     << "sequence -1" << "\n"
     << "maxAngle 10" << "\n"
     << "quad 2.0" << "\n"
     << "timages -1 0 " << doc._map_camera.size() << "\n"
     << "oimages 0" << "\n"; // ?

    std::ofstream file(stlplus::create_filespec(sOutDirectory, "pmvs_options", "txt").c_str());
    file << os.str();
    file.close();
  }
  return bOk;
}

bool exportToBundlerFormat(
  const Document & doc,
  const std::string & sOutFile, //Output Bundle.rd.out file
  const std::string & sOutListFile)  //Output Bundler list.txt file
{
  std::ofstream os(sOutFile.c_str()	);
  std::ofstream osList(sOutListFile.c_str()	);
  if (! os.is_open() || ! osList.is_open())
  {
    return false;
  }
  else
  {
    os << "# Bundle file v0.3" << std::endl
      << doc._map_camera.size()
      << " " << doc._tracks.size() << std::endl;

    size_t count = 0;
    for (std::map<size_t, PinholeCamera>::const_iterator iter = doc._map_camera.begin();
      iter != doc._map_camera.end(); ++iter)
    {
      const PinholeCamera & PMat = iter->second;

      Mat3 D;
      D.fill(0.0);
      D .diagonal() = Vec3(1., -1., -1.); // mapping between our pinhole and Bundler convention
      Mat3 R = D * PMat._R;
      Vec3 t = D * PMat._t;
      double focal = PMat._K(0,0);
      double k1 = 0.0, k2 = 0.0; // distortion already removed

      os << focal << " " << k1 << " " << k2 << std::endl //f k1 k2
        << R(0,0) << " " << R(0, 1) << " " << R(0, 2) << std::endl   //R[0]
        << R(1,0) << " " << R(1, 1) << " " << R(1, 2) << std::endl   //R[1]
        << R(2,0) << " " << R(2, 1) << " " << R(2, 2) << std::endl   //R[2]
        << t(0)  << " " << t(1)   << " " << t(2)   << std::endl;     //t

      osList << doc._vec_imageNames[iter->first] << " 0 " << focal << std::endl;
    }

    for (std::map< size_t, tracks::submapTrack >::const_iterator
      iterTracks = doc._tracks.begin();
      iterTracks != doc._tracks.end();
      ++iterTracks)
    {
      const size_t trackId = iterTracks->first;
      const tracks::submapTrack & map_track = iterTracks->second;
      const size_t trackIndex =
        std::distance<std::map< size_t, tracks::submapTrack >::const_iterator>(doc._tracks.begin(), iterTracks);

      const float * ptr3D = & doc._vec_points[trackIndex*3];
      os << ptr3D[0] << " " << ptr3D[1] << " " << ptr3D[2] << std::endl;
      os <<  "255 255 255" << std::endl;
      os << map_track.size() << " ";
      for (tracks::submapTrack::const_iterator iterTrack = map_track.begin();
        iterTrack != map_track.end();
        ++iterTrack)
      {
        const PinholeCamera & PMat = doc._map_camera.find(iterTrack->first)->second;
        Vec2 pt = PMat.Project(Vec3(ptr3D[0], ptr3D[1], ptr3D[2]));
        os << iterTrack->first << " " << iterTrack->second << " " << pt(0) << " " << pt(1) << " ";
      }
      os << std::endl;
    }
    os.close();
    osList.close();
  }
  return true;
}

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

  CmdLine cmd;
  std::string sSfMDir;
  std::string sOutDir = "";
  int resolution = 1;
  int CPU = 8;

  cmd.add( make_option('i', sSfMDir, "sfmdir") );
  cmd.add( make_option('o', sOutDir, "outdir") );
  cmd.add( make_option('r', resolution, "resolution") );
  cmd.add( make_option('c', CPU, "CPU") );

  try {
      if (argc == 1) throw std::string("Invalid command line parameter.");
      cmd.process(argc, argv);
  } catch(const std::string& s) {
      std::cerr << "Usage: " << argv[0] << ' '
      << "[-i|--sfmdir path, the SfM_output path] "
      << "[-o|--outdir path] "
      << "[-r|--resolution: divide image coefficient] "
      << "[-c|--nb core] "
      << std::endl;

      std::cerr << s << std::endl;
      return EXIT_FAILURE;
  }

  // Create output dir
  if (!stlplus::folder_exists(sOutDir))
    stlplus::folder_create( sOutDir );

  Document m_doc;
  if (m_doc.load(sSfMDir))
  {
    exportToPMVSFormat(m_doc,
      stlplus::folder_append_separator(sOutDir) + "PMVS",
      stlplus::folder_append_separator(sSfMDir) + "images",
      resolution,
      CPU );

    exportToBundlerFormat(m_doc,
      stlplus::folder_append_separator(sOutDir) +
      stlplus::folder_append_separator("PMVS") + "bundle.rd.out",
      stlplus::folder_append_separator(sOutDir) +
      stlplus::folder_append_separator("PMVS") + "list.txt"
      );

    return( EXIT_SUCCESS );
  }

  // Exit program
  return( EXIT_FAILURE );
}
<|endoftext|>