/*
* Copyright (c) 2014, Autonomous Systems Lab
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/

#ifndef ROVIO_CAMERA_CALIBRATION_HPP_
#define ROVIO_CAMERA_CALIBRATION_HPP_

#include <Eigen/Core>
#include <kindr/Core>
#include <yaml-cpp/yaml.h>

#include "rovio/FilterConfiguration.hpp"
#include "rovio/Memory.hpp"

namespace rovio {

/** \brief Distortion model of the camera.
 * */
enum class DistortionModel {
  RADTAN,   //!< Radial tangential distortion model.
  EQUIDIST, //!< Equidistant distortion model.
  FOV,      //!< Field of view model.
  NUM       //!< NOT A DISTORTION MODEL!
};

static constexpr size_t NUM_DISTORTION_MODELS =
    static_cast<size_t>(DistortionModel::NUM);

const std::array<size_t, NUM_DISTORTION_MODELS> NUM_DISTORTION_MODEL_PARAMS = {
    {/*RADTAN (k1, k2, p1, p2, k3)*/ 5u,
     /*EQUIDIST (k1, k2, k3, k4)*/ 4u,
     /*FOV (w)*/ 1u}};

const std::array<std::string, NUM_DISTORTION_MODELS> DISTORTION_MODEL_NAME = {
    {/*RADTAN*/ "plumb_bob",
     /*EQUIDIST*/ "equidistant"
     /*FOV*/ "fov"}};

// YAML keywords.
static const std::string CAMERA_MATRIX = "camera_matrix";
static const std::string DIST_COEFFS = "distortion_coefficients";
static const std::string DATA = "data";

struct CameraCalibration {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  CameraCalibration() {}

  CameraCalibration(const std::string &calibration_yaml_file) {
    loadFromFile(calibration_yaml_file);
  }

  /** \brief Does this struct contain intrinsics and distortion parameter?
   * */
  bool hasIntrinsics_ = false;

  /** \brief Camera intrinsics.
   * */
  Eigen::Matrix3d K_;

  /** \brief Camera distortion model.
   * */
  DistortionModel distortionModel_;

  /** \brief Variable size distortion param vector.
   * */
  Eigen::VectorXd distortionParams_;

  /** \brief Optional camera extrinsics. This information might also be
   * contained in the rovio filter settings, however if they are set here, it
   * will overwrite the existing settings.
   * */
  bool hasExtrinsics_ = false;
  Eigen::Vector3d MrMC_;
  kindr::RotationQuaternionPD qCM_;

  /** \brief Returns the size of the distortion_parameter vector.
   * */
  size_t getNumDistortionParam() {
    return NUM_DISTORTION_MODEL_PARAMS[static_cast<int>(distortionModel_)];
  }

  /** \brief Sets the camera extrinsics.
   */
  void setCameraExtrinsics(const Eigen::Vector3d &MrMC,
                           const kindr::RotationQuaternionPD &qCM) {
    hasExtrinsics_ = true;
    MrMC_ = MrMC;
    qCM_ = qCM;
  }

  /** \brief Sets the camera extrinsics based on the filter settings.
   */
  void getCameraExtrinsicsFromFilterConfiguration(
      const int camID, const FilterConfiguration &filter_configuration) {
    CHECK(filter_configuration.getqCM_xFromCamera(camID, &(qCM_.x())));
    CHECK(filter_configuration.getqCM_yFromCamera(camID, &(qCM_.y())));
    CHECK(filter_configuration.getqCM_zFromCamera(camID, &(qCM_.z())));
    CHECK(filter_configuration.getqCM_wFromCamera(camID, &(qCM_.w())));

    CHECK(filter_configuration.getMrMC_xFromCamera(camID, &(MrMC_.x())));
    CHECK(filter_configuration.getMrMC_yFromCamera(camID, &(MrMC_.y())));
    CHECK(filter_configuration.getMrMC_zFromCamera(camID, &(MrMC_.z())));

    hasExtrinsics_ = true;
  }

  /** \brief Loads and sets intrinsics, the distortion model and the
   * corresponding distortion coefficients from yaml-file.
   *
   *   @param filename - Path to the yaml-file, containing the distortion model
   * and distortion coefficient data.
   */
  void loadFromFile(const std::string &calibration_yaml_file);

  /** \brief Loads and sets the distortion parameters {k1_, k2_, k3_, p1_, p2_}
   * for the Radtan distortion model from yaml-file.
   *   @param filename - Path to the yaml-file, containing the distortion
   * coefficients.
   */
  void loadRadTanDistortion(const std::string &calibration_yaml_file);

  /** \brief Loads and sets the distortion parameters {k1_, k2_, k3_, k4_} for
   * the Equidistant distortion model from yaml-file.
   *   @param filename - Path to the yaml-file, containing the distortion
   * coefficients.
   */
  void loadEquidistDistortion(const std::string &calibration_yaml_file);

  /** \brief Loads and sets the distortion parameter {w} for
   * the Fov distortion model from yaml-file.
   *   @param filename - Path to the yaml-file, containing the distortion
   * coefficients.
   */
  void loadFovDistortion(const std::string &calibration_yaml_file);

  /** \brief Loads and sets the intrinsic parameter matrix K_ from yaml-file.
   *
   *   @param filename - Path to the yaml-file, containing the intrinsic
   * parameter matrix coefficients.
   */
  void loadCameraMatrix(const std::string &calibration_yaml_file);
};

typedef Aligned<std::vector, CameraCalibration> CameraCalibrationVector;

} // namespace rovio

#endif // ROVIO_CAMERA_CALIBRATION_HPP_


#include "gtest/gtest.h"
#include <Eigen/Core>
#include <vector>
#include "IntersectionPoints.h"
#include "MeshFactory.h"
#include "Paths.h"
#include "FacesDivide.h"

namespace {
    using namespace Geotree;

    class IntersectionPointsTest : public ::testing::Test {
    };

    TEST_F(IntersectionPointsTest, CubeSlice) {

      MeshFactory factory;
      Mesh cube1 = factory.cube(10,10,10);
      Mesh cube2 = factory.cube(1,12,12);
      cube2.translate(Vector3d(1, -1, -1));

      IntersectionPoints points1(cube1, cube2);

      EXPECT_EQ(16, points1.points.size());

      Paths paths(points1);
      EXPECT_EQ(2, paths.count());
      EXPECT_EQ(8, paths.size(0));
      EXPECT_EQ(8, paths.size(1));
    }

    TEST_F(IntersectionPointsTest, DISABLED_CubeSame) {

      MeshFactory factory;
      Mesh cube1 = factory.cube(10,10,10);
      Mesh cube2 = factory.cube(10,10,10);
      IntersectionPoints points1(cube1, cube2);
      EXPECT_EQ(8, points1.points.size());

      Paths paths(points1);
      EXPECT_EQ(0, paths.count());
    }

    TEST_F(IntersectionPointsTest, DISABLED_TinyIncrement) {

      for (int i=10; i>0; i--)
      {
      MeshFactory factory;
      Mesh cube1 = factory.cube(1,1,1);
      Mesh cube2 = factory.cube(1,1,1);

      double inc = 1e-14 * i;

      cube2.translate(Vector3d(inc, inc, inc));

      IntersectionPoints points1(cube1, cube2);
      EXPECT_EQ(10, points1.points.size());
      //EXPECT_EQ(1, points1.numPaths());
      }
    }

    TEST_F(IntersectionPointsTest, CubeMulti) {

      MeshFactory factory;
      Mesh cube1 = factory.cube(10,10,10);
      Mesh cube2 = factory.cube(0.1,12,12);
      cube2.translate(Vector3d(0.5, -1, -1));

      for (int i=2; i<9; i++)
      {
        Mesh cube3 = factory.cube(0.1,12,12);
        cube3.translate(Vector3d(0.5 * i, -1, -1));
        cube2 = cube2 + cube3;
      }

      IntersectionPoints points0(cube1, cube2);

      EXPECT_EQ(16*8, points0.points.size());

      Paths paths0(points0);
      EXPECT_EQ(16, paths0.count());
      EXPECT_EQ(8, paths0.size(0));
      EXPECT_EQ(8, paths0.size(1));
    }

    TEST_F(IntersectionPointsTest, TetraXYZ) {

      MeshFactory factory;
      Mesh tetra0 = factory.tetra(10);
      Mesh tetra1 = factory.tetra(10);
      tetra1.translate(Vector3d(1, 1, 1));

      IntersectionPoints points0(tetra0, tetra1);
      IntersectionPoints points1(tetra1, tetra0);

      EXPECT_EQ(3, points0.points.size());
      EXPECT_EQ(3, points1.points.size());

      Paths paths0(points0);
      Paths paths1(points1);
      EXPECT_EQ(1, paths0.count());
      EXPECT_EQ(1, paths1.count());
      EXPECT_EQ(3, paths0.size(0));
      EXPECT_EQ(3, paths1.size(0));
    }

    TEST_F(IntersectionPointsTest, TetraXY) {
      MeshFactory factory;
      Mesh tetra0 = factory.tetra(10);
      Mesh tetra1 = factory.tetra(10);
      tetra1.translate(Vector3d(1, 1, 0));
      IntersectionPoints points0(tetra0, tetra1);
      EXPECT_EQ(4, points0.points.size());

      Paths paths0(points0);
      EXPECT_EQ(1, paths0.count());
      EXPECT_EQ(4, paths0.size(0));
    }

    TEST_F(IntersectionPointsTest, TetraPoint) {
      MeshFactory factory;
      Mesh tetra0 = factory.tetra(10);
      Mesh tetra1 = factory.tetra(10);
      tetra1.translate(Vector3d(10, 0, 0));
      IntersectionPoints points0(tetra0, tetra1);
      EXPECT_EQ(1, points0.points.size());

      Paths paths0(points0);
      EXPECT_EQ(1, paths0.count());
      EXPECT_EQ(1, paths0.size(0));
    }

    TEST_F(IntersectionPointsTest, CubeSide) {
      MeshFactory factory;
      Mesh cube0 = factory.cube(10,10,10);
      Mesh cube1 = factory.cube(10,10,10);
      cube1.translate(Vector3d(10, 0, 0));
      IntersectionPoints points0(cube0, cube1);
      EXPECT_EQ(4, points0.points.size());

      Paths paths0(points0);
      EXPECT_EQ(1, paths0.count());
      EXPECT_EQ(4, paths0.size(0));
    }

    TEST_F(IntersectionPointsTest, DISABLED_CubeTwoPoints) {
      MeshFactory factory;
      Mesh cube0 = factory.cube(10,10,10);
      Mesh cube1 = factory.cube(10,10,10);
      cube1.translate(Vector3d(10, 10, 0));
      IntersectionPoints points0(cube0, cube1);
      EXPECT_EQ(2, points0.points.size());

      Paths paths0(points0);
      EXPECT_EQ(1, paths0.count());
      EXPECT_EQ(2, paths0.size(0));
    }

    TEST_F(IntersectionPointsTest, CubeFace0) {
      MeshFactory factory;
      Mesh cube0 = factory.cube(10,10,10);
      Mesh cube1 = factory.cube(10,1,1);
      Mesh cube2 = factory.cube(12,1,1);
      cube1.translate(Vector3d(1, 4.5, 1));
      cube2.translate(Vector3d(-1, 4.5, 1));
      IntersectionPoints points0(cube0, cube1);
      IntersectionPoints points1(cube1, cube0);
      IntersectionPoints points2(cube0, cube2);
      EXPECT_EQ(8, points0.points.size());
      EXPECT_EQ(8, points1.points.size());
      EXPECT_EQ(16, points2.points.size());

      Paths paths0(points0);
      Paths paths1(points2);
      EXPECT_EQ(1, paths0.count());
      EXPECT_EQ(2, paths1.count());

      std::set<int> faces00 = paths0.faces(MESH0);
      std::set<int> faces01 = paths0.faces(MESH1);
      EXPECT_EQ(1, faces00.size());
      EXPECT_EQ(8, faces01.size());

      FacesDivide facesdiv0(paths0);

      EXPECT_EQ(18, facesdiv0.faceparts.size());

      // for (int face : faces01)
      // 	{

      // 	}
    }

    TEST_F(IntersectionPointsTest, CubeFace1) {
      MeshFactory factory;
      Mesh cube0 = factory.cube(10,10,10);
      Mesh cube1 = factory.cube(10,1,1);
      cube1.translate(Vector3d(1, 4.5, 4.5));
      IntersectionPoints points0(cube0, cube1);
      EXPECT_EQ(8, points0.points.size());

      Paths paths0(points0);
      EXPECT_EQ(1, paths0.count());
    }

    TEST_F(IntersectionPointsTest, CubeOff) {
      MeshFactory factory;
      Mesh cube0 = factory.cube(10,10,10);
      Mesh cube1 = factory.cube(10,10,10);
      Mesh cube2 = factory.cube(10,10,10);
      cube1.translate(Vector3d(1, 1, 1));
      cube2.translate(Vector3d(1, 2, 3));
      IntersectionPoints points0(cube0, cube1);
      IntersectionPoints points1(cube0, cube2);
      EXPECT_EQ(10, points0.points.size());
      EXPECT_EQ(14, points1.points.size());

      Paths paths0(points0);
      Paths paths1(points1);
      EXPECT_EQ(1, paths0.count());
      EXPECT_EQ(1, paths1.count());
    }

}


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick
// Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef BARK_COMMONS_DISTRIBUTION_MULTIVARIATE_NORMAL_HPP_
#define BARK_COMMONS_DISTRIBUTION_MULTIVARIATE_NORMAL_HPP_

#include <Eigen/Eigen>
#include <random>

#include "bark/commons/distribution/distribution.hpp"
#include "bark/commons/params/params.hpp"

namespace bark {
namespace commons {

// Based on
// https://stackoverflow.com/questions/6142576/sample-from-multivariate-normal-gaussian-distribution-in-c
class MultivariateDistribution : public Distribution {
 public:
  MultivariateDistribution(const ParamsPtr& params)
      : Distribution(params),
        seed_(params->GetInt(
            "RandomSeed", "Specifies seed for mersenne twister engine", 1234)),
        generator_(seed_),
        dist_(),
        mean_(MeanFromParams(params)) {
    auto covariance = CovarFromParams(params);
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigenSolver(covariance);
    transform_ = eigenSolver.eigenvectors() *
                 eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
  }

  virtual RandomVariate Sample();

  virtual Probability Density(const RandomVariate& variate) const {
    return 0.0f;
  };

  virtual Probability CDF(const RandomVariate& variate) const { return 0.0f; };

  virtual RandomVariableSupport GetSupport() const {
    std::pair<RandomVariableValueType, RandomVariableValueType>
      support_1d{std::numeric_limits<RandomVariableValueType>::lowest(), std::numeric_limits<RandomVariableValueType>::max()};
    return RandomVariableSupport(mean_.size(), support_1d);
  }

  Eigen::MatrixXf CovarFromParams(const ParamsPtr& params) const;
  Eigen::VectorXf MeanFromParams(const ParamsPtr& params) const;

 private:
  RandomSeed seed_;
  std::mt19937 generator_;
  std::normal_distribution<RandomVariableValueType> dist_;
  Eigen::VectorXf mean_;
  Eigen::MatrixXf transform_;
};

inline RandomVariate MultivariateDistribution::Sample() {
  Eigen::VectorXf eigen_sample =
      mean_ + transform_ * Eigen::VectorXf{mean_.size()}.unaryExpr(
                               [&](auto x) { return dist_(generator_); });
  RandomVariate sample(eigen_sample.data(),
                       eigen_sample.data() + eigen_sample.size());
  return sample;
}

inline Eigen::MatrixXf MultivariateDistribution::CovarFromParams(
    const ParamsPtr& params) const {
  auto covar_vector = params->GetListListFloat(
      "Covariance", "Covariance of multivariate distribution",
      {{1.0, 0.5, 0.5}, {0.2, 1.0, 0.1}, {0.1, 0.1, 1.0}});
  for (const auto& row : covar_vector) {
    BARK_EXPECT_TRUE(covar_vector.size() == row.size());
  }

  Eigen::MatrixXf covar(covar_vector.size(), covar_vector.size());
  for (int i = 0; i < covar_vector.size(); ++i) {
    covar.row(i) = Eigen::Map<Eigen::VectorXf, Eigen::Unaligned>(
        covar_vector[i].data(), covar_vector[i].size());
  }
  return covar;
}

inline Eigen::VectorXf MultivariateDistribution::MeanFromParams(
    const ParamsPtr& params) const {
  auto mean_vector = params->GetListFloat(
      "Mean", "Mean of multivariate distribution", {1.0, 2.0, 3.0});
  Eigen::VectorXf mean = Eigen::Map<Eigen::VectorXf, Eigen::Unaligned>(
      mean_vector.data(), mean_vector.size());
  return mean;
}

}  // namespace commons
}  // namespace bark

#endif  // BARK_COMMONS_DISTRIBUTION_MULTIVARIATE_NORMAL_HPP_


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file portfolio/builders/bond.hpp
\brief builder that returns an engine to price a bond instrument
\ingroup builders
*/

#pragma once

#include <ored/portfolio/builders/cachingenginebuilder.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <ored/utilities/log.hpp>

#include <qle/pricingengines/discountingriskybondengine.hpp>

#include <ql/termstructures/yield/zerospreadedtermstructure.hpp>

#include <boost/make_shared.hpp>

namespace ore {
namespace data {

//! Engine Builder base class for Bonds
/*! Pricing engines are cached by security id
\ingroup builders
*/

class BondEngineBuilder
    : public CachingPricingEngineBuilder<string, const Currency&, const string&, const string&, const string&> {
protected:
    BondEngineBuilder(const std::string& model, const std::string& engine)
        : CachingEngineBuilder(model, engine, {"Bond"}) {}

    virtual string keyImpl(const Currency& ccy, const string& creditCurveId, const string& securityId,
                           const string& referenceCurveId) override {
        return ccy.code() + "_" + creditCurveId + "_" + securityId + "_" + referenceCurveId;
    }
};

//! Discounting Engine Builder class for Bonds
/*! This class creates a DiscountingRiskyBondEngine
\ingroup builders
*/

class BondDiscountingEngineBuilder : public BondEngineBuilder {
public:
    BondDiscountingEngineBuilder() : BondEngineBuilder("DiscountedCashflows", "DiscountingRiskyBondEngine") {}

protected:
    virtual boost::shared_ptr<PricingEngine> engineImpl(const Currency& ccy, const string& creditCurveId,
                                                        const string& securityId,
                                                        const string& referenceCurveId) override {

        string tsperiodStr = engineParameter("TimestepPeriod");
        Period tsperiod = parsePeriod(tsperiodStr);
        Handle<YieldTermStructure> yts = market_->yieldCurve(referenceCurveId, configuration(MarketContext::pricing));
        Handle<DefaultProbabilityTermStructure> dpts;
        // credit curve may not always be used. If credit curve ID is empty proceed without it
        if (!creditCurveId.empty())
            dpts = market_->defaultCurve(creditCurveId, configuration(MarketContext::pricing));
        Handle<Quote> recovery;
        try {
            // try security recovery first
            recovery = market_->recoveryRate(securityId, configuration(MarketContext::pricing));
        } catch (...) {
            // otherwise fall back on curve recovery
            WLOG("security specific recovery rate not found for security ID "
                 << securityId << ", falling back on the recovery rate for credit curve Id " << creditCurveId);
            if (!creditCurveId.empty())
                recovery = market_->recoveryRate(creditCurveId, configuration(MarketContext::pricing));
        }
        Handle<Quote> spread;
        try {
            // spread is optional, pass empty handle to engine if not given (will be treated as 0 spread there)
            spread = market_->securitySpread(securityId, configuration(MarketContext::pricing));
        } catch (...) {
        }

        return boost::make_shared<QuantExt::DiscountingRiskyBondEngine>(yts, dpts, recovery, spread, tsperiod);
    }
};

} // namespace data
} // namespace ore


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "../include/structures.h"
#include "../include/transformations.h"
#include "../../point_to_point_source_to_target_tait_bryan_wc_jacobian.h"
#include "../../point_to_point_source_to_target_rodrigues_wc_jacobian.h"
#include "../../point_to_point_source_to_target_quaternion_wc_jacobian.h"
#include "../../quaternion_constraint_jacobian.h"
#include "../include/cauchy.h"

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -100.0;
float translate_x, translate_y = 0.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

Eigen::Affine3d pose_source;
std::vector<Eigen::Vector3d> points_target_global;
std::vector<Eigen::Vector3d> points_source_local;

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

	for(size_t i = 0 ; i < 100; i++){
		Eigen::Vector3d p;
		p.x() = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;
		p.y() = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;
		p.z() = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;
		points_target_global.push_back(p);
	}

	TaitBryanPose pose;
	pose.px = -4;
	pose.py = 0.4;
	pose.pz = -0.2;
	pose.om = 0.1;
	pose.fi = 0.2;
	pose.ka = 0.3;
	pose_source = affine_matrix_from_pose_tait_bryan(pose);

	Eigen::Affine3d m_inv = pose_source.inverse();
	for(size_t j = 0 ; j < points_target_global.size(); j++){
		Eigen::Vector3d vt = m_inv * points_target_global[j];
		points_source_local.push_back(vt);
	}

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("point to point source to target");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glBegin(GL_LINES);
	glColor3f(1.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(1.0f, 0.0f, 0.0f);

	glColor3f(0.0f, 1.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 1.0f, 0.0f);

	glColor3f(0.0f, 0.0f, 1.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 1.0f);
	glEnd();

	Eigen::Affine3d &m = pose_source;

	glBegin(GL_LINES);
		glColor3f(1.0f, 0.0f, 0.0f);
		glVertex3f(m(0,3), m(1,3), m(2,3));
		glVertex3f(m(0,3) + m(0,0), m(1,3) + m(1,0), m(2,3) + m(2,0));

		glColor3f(0.0f, 1.0f, 0.0f);
		glVertex3f(m(0,3), m(1,3), m(2,3));
		glVertex3f(m(0,3) + m(0,1), m(1,3) + m(1,1), m(2,3) + m(2,1));

		glColor3f(0.0f, 0.0f, 1.0f);
		glVertex3f(m(0,3), m(1,3), m(2,3));
		glVertex3f(m(0,3) + m(0,2), m(1,3) + m(1,2), m(2,3) + m(2,2));
	glEnd();

	glColor3f(1,0,0);
	glPointSize(3);
	glBegin(GL_POINTS);
	for(auto &p:points_source_local){
		Eigen::Vector3d vt;
		vt = m * p;
		glVertex3f(vt.x(), vt.y(), vt.z());
	}
	glEnd();
	glPointSize(1);

	glColor3f(0,0,1);
	glPointSize(3);
	glBegin(GL_POINTS);

	for(auto &p:points_target_global){
		glVertex3f(p.x(), p.y(), p.z());
	}
	glEnd();
	glPointSize(1);

	glColor3f(0,1,0);
	glBegin(GL_LINES);
	for(int i = 0; i < points_source_local.size(); i++){
		Eigen::Vector3d v(points_source_local[i].x(), points_source_local[i].y(), points_source_local[i].z());
		Eigen::Vector3d vt;
		vt = m * v;
		glVertex3f(vt.x(), vt.y(), vt.z());
		glVertex3f(points_target_global[i].x(), points_target_global[i].y(), points_target_global[i].z());
	}
	glEnd();

	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'n':{
				TaitBryanPose pose;
				pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;

				pose_source = pose_source * affine_matrix_from_pose_tait_bryan(pose);

			break;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(pose_source);

for(size_t i = 0 ; i < points_source_local.size(); i++){
	Eigen::Vector3d &p_t = points_target_global[i];
	Eigen::Vector3d &p_s = points_source_local[i];
	double delta_x;
	double delta_y;
	double delta_z;
	point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z());

	Eigen::Matrix<double, 3, 6, Eigen::RowMajor> jacobian;
	point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z());

	int ir = tripletListB.size();

	for(int row = 0; row < 3; row++){
		for(int col = 0; col < 6; col++){
			if(jacobian(row,col)!=0.0){
			tripletListA.emplace_back(ir+row,col,-jacobian(row,col));
			}
		}
	}

	tripletListP.emplace_back(ir    , ir    , 1);
	tripletListP.emplace_back(ir + 1, ir + 1, 1);
	tripletListP.emplace_back(ir + 2, ir + 2, 1);

	tripletListB.emplace_back(ir    , 0,  delta_x);
	tripletListB.emplace_back(ir + 1, 0,  delta_y);
	tripletListB.emplace_back(ir + 2, 0,  delta_z);
}

Eigen::SparseMatrix<double> matA(tripletListB.size(), 6);
Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

Eigen::SparseMatrix<double> AtPA(6, 6);
Eigen::SparseMatrix<double> AtPB(6, 1);

Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
AtPA = AtP * matA;
AtPB = AtP * matB;

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);
Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == 6){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(pose_source);
				pose.px += h_x[counter++];
				pose.py += h_x[counter++];
				pose.pz += h_x[counter++];
				pose.om += h_x[counter++];
				pose.fi += h_x[counter++];
				pose.ka += h_x[counter++];

				pose_source = affine_matrix_from_pose_tait_bryan(pose);

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'r':{

			TaitBryanPose posetb = pose_tait_bryan_from_affine_matrix(pose_source);
			posetb.om += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
			posetb.fi += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
			posetb.ka += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
			pose_source = affine_matrix_from_pose_tait_bryan(posetb);

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			RodriguesPose pose_s = pose_rodrigues_from_affine_matrix(pose_source);

			for(size_t i = 0 ; i < points_source_local.size(); i++){
				Eigen::Vector3d &p_t = points_target_global[i];
				Eigen::Vector3d &p_s = points_source_local[i];
				double delta_x;
				double delta_y;
				double delta_z;
				point_to_point_source_to_target_rodrigues_wc(delta_x, delta_y, delta_z, pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z());

				Eigen::Matrix<double, 3, 6, Eigen::RowMajor> jacobian;
				point_to_point_source_to_target_rodrigues_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, p_s.x(), p_s.y(), p_s.z());

				int ir = tripletListB.size();

				tripletListA.emplace_back(ir     , 0, -jacobian(0,0));
				tripletListA.emplace_back(ir     , 1, -jacobian(0,1));
				tripletListA.emplace_back(ir     , 2, -jacobian(0,2));
				tripletListA.emplace_back(ir     , 3, -jacobian(0,3));
				tripletListA.emplace_back(ir     , 4, -jacobian(0,4));
				tripletListA.emplace_back(ir     , 5, -jacobian(0,5));

				tripletListA.emplace_back(ir + 1 , 0, -jacobian(1,0));
				tripletListA.emplace_back(ir + 1 , 1, -jacobian(1,1));
				tripletListA.emplace_back(ir + 1 , 2, -jacobian(1,2));
				tripletListA.emplace_back(ir + 1 , 3, -jacobian(1,3));
				tripletListA.emplace_back(ir + 1 , 4, -jacobian(1,4));
				tripletListA.emplace_back(ir + 1 , 5, -jacobian(1,5));

				tripletListA.emplace_back(ir + 2 , 0, -jacobian(2,0));
				tripletListA.emplace_back(ir + 2 , 1, -jacobian(2,1));
				tripletListA.emplace_back(ir + 2 , 2, -jacobian(2,2));
				tripletListA.emplace_back(ir + 2 , 3, -jacobian(2,3));
				tripletListA.emplace_back(ir + 2 , 4, -jacobian(2,4));
				tripletListA.emplace_back(ir + 2 , 5, -jacobian(2,5));

				tripletListP.emplace_back(ir    , ir    ,  cauchy(delta_x, 1));
				tripletListP.emplace_back(ir + 1, ir + 1,  cauchy(delta_y, 1));
				tripletListP.emplace_back(ir + 2, ir + 2,  cauchy(delta_z, 1));

				tripletListB.emplace_back(ir    , 0,  delta_x);
				tripletListB.emplace_back(ir + 1, 0,  delta_y);
				tripletListB.emplace_back(ir + 2, 0,  delta_z);
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(6, 6);
			Eigen::SparseMatrix<double> AtPB(6, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == 6){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				RodriguesPose pose = pose_rodrigues_from_affine_matrix(pose_source);
				pose.px += h_x[counter++];
				pose.py += h_x[counter++];
				pose.pz += h_x[counter++];
				pose.sx += h_x[counter++];
				pose.sy += h_x[counter++];
				pose.sz += h_x[counter++];

				pose_source = affine_matrix_from_pose_rodrigues(pose);
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'q':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			QuaternionPose pose_s = pose_quaternion_from_affine_matrix(pose_source);

			for(size_t i = 0 ; i < points_source_local.size(); i++){
				Eigen::Vector3d &p_t = points_target_global[i];
				Eigen::Vector3d &p_s = points_source_local[i];
				double delta_x;
				double delta_y;
				double delta_z;
				point_to_point_source_to_target_quaternion_wc(delta_x, delta_y, delta_z, pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z());

				Eigen::Matrix<double, 3, 7, Eigen::RowMajor> jacobian;
				point_to_point_source_to_target_quaternion_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, p_s.x(), p_s.y(), p_s.z());

				int ir = tripletListB.size();

				tripletListA.emplace_back(ir     , 0, -jacobian(0,0));
				tripletListA.emplace_back(ir     , 1, -jacobian(0,1));
				tripletListA.emplace_back(ir     , 2, -jacobian(0,2));
				tripletListA.emplace_back(ir     , 3, -jacobian(0,3));
				tripletListA.emplace_back(ir     , 4, -jacobian(0,4));
				tripletListA.emplace_back(ir     , 5, -jacobian(0,5));
				tripletListA.emplace_back(ir     , 6, -jacobian(0,6));

				tripletListA.emplace_back(ir + 1 , 0, -jacobian(1,0));
				tripletListA.emplace_back(ir + 1 , 1, -jacobian(1,1));
				tripletListA.emplace_back(ir + 1 , 2, -jacobian(1,2));
				tripletListA.emplace_back(ir + 1 , 3, -jacobian(1,3));
				tripletListA.emplace_back(ir + 1 , 4, -jacobian(1,4));
				tripletListA.emplace_back(ir + 1 , 5, -jacobian(1,5));
				tripletListA.emplace_back(ir + 1 , 6, -jacobian(1,6));

				tripletListA.emplace_back(ir + 2 , 0, -jacobian(2,0));
				tripletListA.emplace_back(ir + 2 , 1, -jacobian(2,1));
				tripletListA.emplace_back(ir + 2 , 2, -jacobian(2,2));
				tripletListA.emplace_back(ir + 2 , 3, -jacobian(2,3));
				tripletListA.emplace_back(ir + 2 , 4, -jacobian(2,4));
				tripletListA.emplace_back(ir + 2 , 5, -jacobian(2,5));
				tripletListA.emplace_back(ir + 2 , 6, -jacobian(2,6));

				tripletListP.emplace_back(ir    , ir    ,  cauchy(delta_x, 1));
				tripletListP.emplace_back(ir + 1, ir + 1,  cauchy(delta_y, 1));
				tripletListP.emplace_back(ir + 2, ir + 2,  cauchy(delta_z, 1));

				tripletListB.emplace_back(ir    , 0,  delta_x);
				tripletListB.emplace_back(ir + 1, 0,  delta_y);
				tripletListB.emplace_back(ir + 2, 0,  delta_z);
			}

			int ir = tripletListB.size();
			QuaternionPose pose = pose_quaternion_from_affine_matrix(pose_source);

			double delta;
			quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3);

			Eigen::Matrix<double, 1, 4> jacobian;
			quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3);

			tripletListA.emplace_back(ir, 3 , -jacobian(0,0));
			tripletListA.emplace_back(ir, 4 , -jacobian(0,1));
			tripletListA.emplace_back(ir, 5 , -jacobian(0,2));
			tripletListA.emplace_back(ir, 6 , -jacobian(0,3));

			tripletListP.emplace_back(ir, ir, 1000000.0);

			tripletListB.emplace_back(ir, 0, delta);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 7);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(7, 7);
			Eigen::SparseMatrix<double> AtPB(7, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == 7){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				QuaternionPose pose = pose_quaternion_from_affine_matrix(pose_source);
				pose.px += h_x[counter++];
				pose.py += h_x[counter++];
				pose.pz += h_x[counter++];
				pose.q0 += h_x[counter++];
				pose.q1 += h_x[counter++];
				pose.q2 += h_x[counter++];
				pose.q3 += h_x[counter++];

				pose_source = affine_matrix_from_pose_quaternion(pose);
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
	}
	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "n: add noise to pose source" << std::endl;
	std::cout << "t: optimize (Tait-Bryan)" << std::endl;
	std::cout << "r: optimize (Rodrigues)" << std::endl;
	std::cout << "q: optimize (Quaternion)" << std::endl;
}


/* Medianfilter_plugin.cpp
 * This is a test plugin, you can use it as a demo.
 * Revised from a previous minmaxfiltering plugin
 * 2013-06-10 : by Zhi Zhou
 */

//last change: by PHC, 2013-07-08, adjust some styles and correct a small bug.

#include "v3d_message.h"
#include <vector>
#include "Medianfilter_plugin.h"
#include <QtGui>
#include <iostream>
#include <cmath>
#include <stdlib.h>
#include "../plugin_loader/v3d_plugin_loader.h"
#include <boost/lexical_cast.hpp>

#include "stackutil.h"
#define INF 1E9

using namespace std;
Q_EXPORT_PLUGIN2(medianfilter, MedianFilterPlugin);

void median_fixed_domenu(V3DPluginCallback2 &callback, QWidget *parent);
bool median_fixed_dofunc(V3DPluginCallback2 &callback, const V3DPluginArgList & input, V3DPluginArgList & output);

void median_adaptive_domenu(V3DPluginCallback2 &callback, QWidget *parent);
bool median_adaptive_dofunc(V3DPluginCallback2 &callback, const V3DPluginArgList & input, V3DPluginArgList & output);

template <class T> void median_filter(T* data1d,
                                      V3DLONG *in_sz,
                                      unsigned int Wx,
                                      unsigned int Wy,
                                      unsigned int Wz,
                                      unsigned int c,
                                      T* &outimg);

template <class T> void adp_median_filter(T* data1d,
                                          V3DLONG *in_sz,
                                          unsigned int c,
                                          T* &outimg,T* gsdtdata1d );

QStringList MedianFilterPlugin::menulist() const
{
    return QStringList()
            <<tr("Fixed Window")
           <<tr("Adaptive Window")
             ;
}

void MedianFilterPlugin::domenu(const QString &menu_name, V3DPluginCallback2 &callback, QWidget *parent)

{
    if (menu_name == tr("Fixed Window"))
    {
        median_fixed_domenu(callback,parent);
    }
    else if (menu_name == tr("Adaptive Window"))
    {
        median_adaptive_domenu(callback,parent);
    }
}

QStringList MedianFilterPlugin::funclist() const
{
    return QStringList()
            <<tr("fixed_window")
           <<tr("adaptive_window")
             <<tr("help");
}

bool MedianFilterPlugin::dofunc(const QString & func_name, const V3DPluginArgList & input, V3DPluginArgList & output, V3DPluginCallback2 & callback,  QWidget * parent)
{

    if (func_name == tr("fixed_window"))
    {
        return median_fixed_dofunc(callback, input, output);
    }
    else if(func_name == tr("adaptive_window"))
    {
        return median_adaptive_dofunc(callback, input, output);
    }
    else if (func_name == tr("help"))
    {
        cout<<"Usage : v3d -x medianfilter -f fixed_window -i <inimg_file> -o <outimg_file> -p <wx> <wy> <wz> <ch>"<<endl;
        cout<<endl;
        cout<<"wx          filter window radius size (pixel #) in x direction, window size is 2*wx+1, default 1"<<endl;
        cout<<"wy          filter window radius size (pixel #) in y direction, window size is 2*wy+1, default 1"<<endl;
        cout<<"wz          filter window radius size (pixel #) in z direction, window size is 2*wz+1, default 1"<<endl;
        cout<<"ch          the input channel value, default 1 and start from 1, default 1"<<endl;
        cout<<endl;
        cout<<endl;
        cout<<"Usage : v3d -x medianfilter -f adaptive_window -i <inimg_file> -o <outimg_file> -p <ch> <th_idx> <th>"<<endl;
        cout<<endl;
        cout<<"ch	   the input channel value, default 1 and start from 1, default 1"<<endl;
	cout<<"th_idx 	   threshold method index, 0: mean, 1: usr defined, default 0"<<endl;
	cout<<"th	   user defined threshold value (when th_idx is 1), default 0"<<endl;
        cout<<endl;
        cout<<endl;
        return true;
    }
}

bool median_fixed_dofunc(V3DPluginCallback2 &callback, const V3DPluginArgList & input, V3DPluginArgList & output)
{
    cout<<"Welcome to Median filter with fixed window"<<endl;
    if (output.size() != 1) return false;
    unsigned int Wx=1, Wy=1, Wz=1, ch=1;
    if (input.size()>=2)
    {

        vector<char*> paras = (*(vector<char*> *)(input.at(1).p));
        cout<<paras.size()<<endl;
        if(paras.size() >= 1) Wx = atoi(paras.at(0));
        if(paras.size() >= 2) Wy = atoi(paras.at(1));
        if(paras.size() >= 3) Wz = atoi(paras.at(2));
        if(paras.size() >= 4) ch = atoi(paras.at(3));
    }

    char * inimg_file = ((vector<char*> *)(input.at(0).p))->at(0);
    char * outimg_file = ((vector<char*> *)(output.at(0).p))->at(0);

    cout<<"Wx = "<<Wx<<endl;
    cout<<"Wy = "<<Wy<<endl;
    cout<<"Wz = "<<Wz<<endl;
    cout<<"ch = "<<ch<<endl;
    cout<<"inimg_file = "<<inimg_file<<endl;
    cout<<"outimg_file = "<<outimg_file<<endl;

    unsigned char * data1d = 0,  * outimg1d = 0;
    V3DLONG in_sz[4];

    unsigned int c = ch;//-1;

    int datatype;
    if(!simple_loadimage_wrapper(callback, inimg_file, data1d, in_sz, datatype))
    {
        cerr<<"load image "<<inimg_file<<" error!"<<endl;
        return false;
    }

    //input
    void* outimg = 0;

    switch (datatype)
    {
    case 1: median_filter(data1d, in_sz, Wx, Wy, Wz, c, (unsigned char*&)outimg); break;
    case 2: median_filter((unsigned short int*)data1d, in_sz, Wx, Wy, Wz, c, (unsigned short int*&)outimg); break;
    case 4: median_filter((float *)data1d, in_sz, Wx, Wy, Wz, c, (float*&)outimg); break;
    default:
        v3d_msg("Invalid datatype.");
        if (data1d) {delete []data1d; data1d=0;}
        //if (in_sz) {delete []in_sz; in_sz=0;}
        return false;
    }

    // save image
    in_sz[3]=1;
    simple_saveimage_wrapper(callback, outimg_file, (unsigned char *)outimg, in_sz, datatype);

    if(outimg) {delete []outimg; outimg =0;}
    if (data1d) {delete []data1d; data1d=0;}

    return true;
}

void median_fixed_domenu(V3DPluginCallback2 &callback, QWidget *parent)
{
    v3dhandle curwin = callback.currentImageWindow();
    if (!curwin)
    {
        QMessageBox::information(0, "", "You don't have any image open in the main window.");
        return;
    }

    Image4DSimple* p4DImage = callback.getImage(curwin);

    if (!p4DImage)
    {
        QMessageBox::information(0, "", "The image pointer is invalid. Ensure your data is valid and try again!");
        return;
    }

    unsigned char* data1d = p4DImage->getRawData();
    //V3DLONG totalpxls = p4DImage->getTotalBytes();
    V3DLONG pagesz = p4DImage->getTotalUnitNumberPerChannel();

    V3DLONG N = p4DImage->getXDim();
    V3DLONG M = p4DImage->getYDim();
    V3DLONG P = p4DImage->getZDim();
    V3DLONG sc = p4DImage->getCDim();

    //define datatype here

    //input
    bool ok1, ok2, ok3, ok4;
    unsigned int Wx=1, Wy=1, Wz=1, c=1;

    Wx = QInputDialog::getInteger(parent, "Window X ",
                                  "Enter radius (window size is 2*radius+1):",
                                  1, 1, N, 1, &ok1);

    if(ok1)
    {
        Wy = QInputDialog::getInteger(parent, "Window Y",
                                      "Enter radius (window size is 2*radius+1):",
                                      1, 1, M, 1, &ok2);
    }
    else
        return;

    if(ok2)
    {
        Wz = QInputDialog::getInteger(parent, "Window Z",
                                      "Enter radius (window size is 2*radius+1):",
                                      1, 1, P, 1, &ok3);
    }
    else
        return;

    if(sc==1)
    {
        c=1;
        ok4=true;
    }
    else
    {
        if(ok3)
        {
            c = QInputDialog::getInteger(parent, "Channel",
                                         "Enter channel NO:",
                                         1, 1, sc, 1, &ok4);
        }
        else
            return;
    }

    // filter
    V3DLONG in_sz[4];
    in_sz[0] = N; in_sz[1] = M; in_sz[2] = P; in_sz[3] = sc;

    ImagePixelType pixeltype = p4DImage->getDatatype();
    void* outimg = 0;
    switch (pixeltype)
    {
    case V3D_UINT8: median_filter(data1d, in_sz, Wx, Wy, Wz, c,(unsigned char* &)outimg); break;
    case V3D_UINT16: median_filter((unsigned short int *)data1d, in_sz, Wx, Wy, Wz, c, (unsigned short int* &)outimg); break;
    case V3D_FLOAT32: median_filter((float *)data1d, in_sz, Wx, Wy, Wz, c, (float* &)outimg);break;
    default: v3d_msg("Invalid data type. Do nothing."); return;
    }

    // display
    Image4DSimple * new4DImage = new Image4DSimple();
    new4DImage->setData((unsigned char *)outimg, N, M, P, 1, pixeltype);
    v3dhandle newwin = callback.newImageWindow();
    callback.setImage(newwin, new4DImage);
    callback.setImageName(newwin, "Median filter result");
    callback.updateImageWindow(newwin);

    return;
}

template <class T> void median_filter(T* data1d,
                                      V3DLONG *in_sz,
                                      unsigned int Wx,
                                      unsigned int Wy,
                                      unsigned int Wz,
                                      unsigned int c,
                                      T* &outimg)
{

    V3DLONG N = in_sz[0];
    V3DLONG M = in_sz[1];
    V3DLONG P = in_sz[2];
    V3DLONG sc = in_sz[3];
    V3DLONG pagesz = N*M*P;

    T *arr,tmp;
    int ii,jj;
    int size = (2*Wx+1)*(2*Wy+1)*(2*Wz+1);
    arr = new T[size];

    //filtering
    V3DLONG offsetc = (c-1)*pagesz;

    //declare temporary pointer
    T *pImage = new T [pagesz];
    if (!pImage)
    {
        printf("Fail to allocate memory.\n");
        return;
    }
    else
    {
        for(V3DLONG i=0; i<pagesz; i++)
            pImage[i] = 0;
    }

    //Median Filtering
    for(V3DLONG iz = 0; iz < P; iz++)
    {
         printf("\r median filter : %d %% completed ", ((iz + 1)*100) / P);fflush(stdout);
        V3DLONG offsetk = iz*M*N;
        for(V3DLONG iy = 0; iy < M; iy++)
        {
            V3DLONG offsetj = iy*N;
            for(V3DLONG ix = 0; ix < N; ix++)
            {

                V3DLONG xb = ix-Wx; if(xb<0) xb = 0;
                V3DLONG xe = ix+Wx; if(xe>=N-1) xe = N-1;
                V3DLONG yb = iy-Wy; if(yb<0) yb = 0;
                V3DLONG ye = iy+Wy; if(ye>=M-1) ye = M-1;
                V3DLONG zb = iz-Wz; if(zb<0) zb = 0;
                V3DLONG ze = iz+Wz; if(ze>=P-1) ze = P-1;
                ii = 0;

                for(V3DLONG k=zb; k<=ze; k++)
                {
                    V3DLONG offsetkl = k*M*N;
                    for(V3DLONG j=yb; j<=ye; j++)
                    {
                        V3DLONG offsetjl = j*N;
                        for(V3DLONG i=xb; i<=xe; i++)
                        {
                            T dataval = data1d[ offsetc + offsetkl + offsetjl + i];
                            arr[ii] = dataval;
                            if (ii>0)
                            {
                                jj = ii;
                                while(jj > 0 && arr[jj-1]>arr[jj])
                                {
                                    tmp = arr[jj];
                                    arr[jj] = arr[jj-1];
                                    arr[jj-1] = tmp;
                                    jj--;
                                }
                            }
                            ii++;
                        }
                    }
                }

                //set value
                V3DLONG index_pim = offsetk + offsetj + ix;
                pImage[index_pim] = arr[int(0.5*ii)+1];
            }
        }
    }
printf("\n");

    outimg = pImage;
    delete [] arr;

}

void median_adaptive_domenu(V3DPluginCallback2 &callback, QWidget *parent)
{
    v3dhandle curwin = callback.currentImageWindow();
    if (!curwin)
    {
        QMessageBox::information(0, "", "You don't have any image open in the main window.");
        return;
    }

    Image4DSimple* p4DImage = callback.getImage(curwin);

    if (!p4DImage)
    {
        QMessageBox::information(0, "", "The image pointer is invalid. Ensure your data is valid and try again!");
        return;
    }

    unsigned char* data1d = p4DImage->getRawData();
    //V3DLONG totalpxls = p4DImage->getTotalBytes();
    V3DLONG pagesz = p4DImage->getTotalUnitNumberPerChannel();

    V3DLONG N = p4DImage->getXDim();
    V3DLONG M = p4DImage->getYDim();
    V3DLONG P = p4DImage->getZDim();
    V3DLONG sc = p4DImage->getCDim();

     //add input dialog

	AdaptiveMedianDialog dialog(callback, parent);
	if (!dialog.image)
		return;

	if (dialog.exec()!=QDialog::Accepted)
		return;

	dialog.update();

	Image4DSimple* subject = dialog.image;
	if (!subject)
		return;
	ROIList pRoiList = dialog.pRoiList;

	int c = dialog.ch+1;
	int th_idx = dialog.th_idx;
    double th = dialog.thresh;

	if(th_idx ==0)
	{
        V3DLONG offsetc = (c-1)*pagesz;
		for(V3DLONG iz = 0; iz < P; iz++)
	   	{
            double PixelSum = 0;
            V3DLONG offsetk = iz*M*N;
			for(V3DLONG iy = 0; iy < M; iy++)
			{
			    V3DLONG offsetj = iy*N;
			    for(V3DLONG ix = 0; ix < N; ix++)
			    {

				V3DLONG PixelVaule = data1d[offsetc + offsetk + offsetj + ix];
                PixelSum = PixelSum + PixelVaule;

			    }
			}
                th = th + PixelSum/(M*N*P);
        }

	}

    // filter
    V3DLONG in_sz[4];
    in_sz[0] = N; in_sz[1] = M; in_sz[2] = P; in_sz[3] = sc;

    ImagePixelType pixeltype = p4DImage->getDatatype();
    simple_saveimage_wrapper(callback, "temp.v3draw", (unsigned char *)data1d, in_sz, pixeltype);
    //invoke gsdt function
    V3DPluginArgItem arg;
    V3DPluginArgList input;
    V3DPluginArgList output;

    arg.type = "random";std::vector<char*> args1;
   // std:: string inputName(callback.getImageName(curwin).toStdString());char* inputName2 =  new char[inputName.length() + 1]; strcpy(inputName2, inputName.c_str());
    args1.push_back("temp.v3draw"); arg.p = (void *) & args1; input<< arg;
    arg.type = "random";std::vector<char*> args;
    char channel = '0' + (c-1);
    string threshold = boost::lexical_cast<string>(th); char* threshold2 =  new char[threshold.length() + 1]; strcpy(threshold2, threshold.c_str());
    args.push_back(threshold2);args.push_back("1");args.push_back(&channel);args.push_back("1"); arg.p = (void *) & args; input << arg;
    arg.type = "random";std::vector<char*> args2;args2.push_back("gsdtImage.v3draw"); arg.p = (void *) & args2; output<< arg;

    QString full_plugin_name = "gsdt";
    QString func_name = "gsdt";

    callback.callPluginFunc(full_plugin_name,func_name, input,output);

    //system("v3d -x gsdt -f gsdt -i /home/zhiz/Desktop/vaa3d/Images/ex_Repo_hb9_eve.tif -o /home/zhiz/Desktop/vaa3d/Images/gsdt_ex_Repo_hb9_eve.tif -p 0 1 0 1.0");

    unsigned char * gsdtdata1d = 0;
    int datatype;
    V3DLONG in_zz[4];

    char * outimg_file = ((vector<char*> *)(output.at(0).p))->at(0);
    if(!simple_loadimage_wrapper(callback, outimg_file, gsdtdata1d, in_zz, datatype))
    {
        v3d_msg("Fail to load image");
        return;
    }
    remove("gsdtImage.v3draw");
    remove("temp.v3draw");

    void* outimg = 0;
    switch (pixeltype)
    {
    case V3D_UINT8: adp_median_filter(data1d, in_sz, c,(unsigned char* &)outimg, gsdtdata1d); break;
    case V3D_UINT16: adp_median_filter((unsigned short int *)data1d, in_sz, c, (unsigned short int* &)outimg,(unsigned short int *)gsdtdata1d); break;
    case V3D_FLOAT32: adp_median_filter((float *)data1d, in_sz, c, (float* &)outimg,(float *)gsdtdata1d);break;
    default: v3d_msg("Invalid data type. Do nothing."); return;
    }

    // display
    Image4DSimple * new4DImage = new Image4DSimple();
    new4DImage->setData((unsigned char *)outimg, N, M, P, 1, pixeltype);
    v3dhandle newwin = callback.newImageWindow();
    callback.setImage(newwin, new4DImage);
    callback.setImageName(newwin, "Adaptive Median filter result");
    callback.updateImageWindow(newwin);

    return;
}

bool median_adaptive_dofunc(V3DPluginCallback2 &callback, const V3DPluginArgList & input, V3DPluginArgList & output)
{
    cout<<"Welcome to Median filter with adaptive window"<<endl;
    if (output.size() != 1) return false;
    unsigned int ch = 1,th_idx = 0;
    double th = 0;
    if (input.size()>=2)
    {

        vector<char*> paras = (*(vector<char*> *)(input.at(1).p));
        cout<<paras.size()<<endl;
        if(paras.size() >= 1) ch = atoi(paras.at(0));
        if(paras.size() >= 2) th_idx = atoi(paras.at(1));
	if(paras.size() >= 3) th = atoi(paras.at(2));
    }

    char * inimg_file = ((vector<char*> *)(input.at(0).p))->at(0);
    char * outimg_file = ((vector<char*> *)(output.at(0).p))->at(0);

    cout<<"ch = "<<ch<<endl;
    cout<<"th_idx = "<<th_idx<<endl;
    cout<<"th = "<<th<<endl;
    cout<<"inimg_file = "<<inimg_file<<endl;
    cout<<"outimg_file = "<<outimg_file<<endl;

    unsigned char * data1d = 0,  * outimg1d = 0;
    V3DLONG in_sz[4];

    unsigned int c = ch;//-1;

    int datatype;
    if(!simple_loadimage_wrapper(callback, inimg_file, data1d, in_sz, datatype))
    {
        cerr<<"load image "<<inimg_file<<" error!"<<endl;
        return false;
    }

    if(th_idx ==0)
    {
        V3DLONG N = in_sz[0];
        V3DLONG M = in_sz[1];
        V3DLONG P = in_sz[2];
        V3DLONG pagesz = N*M*P;
        V3DLONG offsetc = (c-1)*pagesz;
        for(V3DLONG iz = 0; iz < P; iz++)
        {
            double PixelSum = 0;
            V3DLONG offsetk = iz*M*N;
            for(V3DLONG iy = 0; iy < M; iy++)
            {
                V3DLONG offsetj = iy*N;
                for(V3DLONG ix = 0; ix < N; ix++)
                {

                V3DLONG PixelVaule = data1d[offsetc + offsetk + offsetj + ix];
                PixelSum = PixelSum + PixelVaule;

                }
            }
             th = th + PixelSum/(M*N*P);
        }

    }

    //invoke gsdt function
    V3DPluginArgItem arg;
    V3DPluginArgList input2;
    V3DPluginArgList output2;

    arg.type = "random";std::vector<char*> args1;
    args1.push_back(inimg_file); arg.p = (void *) & args1; input2<< arg;
    arg.type = "random";std::vector<char*> args;
    char channel = '0' + (c-1);
    string threshold = boost::lexical_cast<string>(th); char* threshold2 =  new char[threshold.length() + 1]; strcpy(threshold2, threshold.c_str());
    args.push_back(threshold2);args.push_back("1");args.push_back(&channel);args.push_back("1"); arg.p = (void *) & args; input2 << arg;
    arg.type = "random";std::vector<char*> args2;
    args2.push_back("gsdtImage.tiff"); arg.p = (void *) & args2; output2<< arg;

    QString full_plugin_name = "gsdt"; //partial name will work
    QString func_name = "gsdt";

    callback.callPluginFunc(full_plugin_name,func_name, input2,output2);

    unsigned char * gsdtdata1d = 0;
    int datatype2;
    V3DLONG in_zz[4];

    char * outimg_file2 = ((vector<char*> *)(output2.at(0).p))->at(0);
    if (!simple_loadimage_wrapper(callback, outimg_file2, gsdtdata1d, in_zz, datatype2))
    {
        v3d_msg("Fail to load image");
        return false;
    }

    remove("gsdtImage.tiff");
    //input
    void* outimg = 0;

    switch (datatype)
    {
    case 1: adp_median_filter(data1d, in_sz, c,(unsigned char* &)outimg, gsdtdata1d); break;
    case 2: adp_median_filter((unsigned short int *)data1d, in_sz, c, (unsigned short int* &)outimg,(unsigned short int *)gsdtdata1d); break;
    case 4: adp_median_filter((float *)data1d, in_sz, c, (float* &)outimg,(float *)gsdtdata1d);break;
    default:
        v3d_msg("Invalid datatype.");
        if (data1d) {delete []data1d; data1d=0;}
        //if (in_sz) {delete []in_sz; in_sz=0;}
        return false;
    }

    // save image
    in_sz[3]=1;
    simple_saveimage_wrapper(callback, outimg_file, (unsigned char *)outimg, in_sz, datatype);

    if(outimg) {delete []outimg; outimg =0;}
    if (data1d) {delete []data1d; data1d=0;}
    //if (in_sz) {delete []in_sz; in_sz=0;}

    return true;
}

template <class T> void adp_median_filter(T* data1d,
                                          V3DLONG *in_sz,
                                          unsigned int c,
                                          T* &outimg,
                                          T* gsdtdatald)
{

    V3DLONG N = in_sz[0];
    V3DLONG M = in_sz[1];
    V3DLONG P = in_sz[2];
    V3DLONG sc = in_sz[3];
    V3DLONG pagesz = N*M*P;

    int Wx,Wy,Wz;

    //filtering
    V3DLONG offsetc = (c-1)*pagesz;

    //declare temporary pointer
    T *pImage = new T [pagesz];
    if (!pImage)
    {
        printf("Fail to allocate memory.\n");
        return;
    }
    else
    {
        for(V3DLONG i=0; i<pagesz; i++)
            pImage[i] = 0;
    }

    //Median Filtering
    for(V3DLONG iz = 0; iz < P; iz++)
    {
        printf("\r median filter : %d %% completed ", ((iz + 1)*100) / P);fflush(stdout);
        V3DLONG offsetk = iz*M*N;
        for(V3DLONG iy = 0; iy < M; iy++)
        {
            V3DLONG offsetj = iy*N;
            for(V3DLONG ix = 0; ix < N; ix++)
            {

                T PixelValue = data1d[offsetc+offsetk + offsetj + ix];
                T GsdtValue = gsdtdatald[offsetk + offsetj + ix];
                Wx = (int)((std::log(double(PixelValue))/std::log(double(2)))/GsdtValue + 0.5);
                //printf("%d %d\n",PixelValue,Wx);

                if ((Wx<=0)||(PixelValue==0))
                {
                    V3DLONG index_pim = offsetk + offsetj + ix;
                    pImage[index_pim] = PixelValue;
                }
                else
                {
                    Wy = Wx;
                    Wz = Wx;

                    V3DLONG xb = ix-Wx; if(xb<0) xb = 0;
                    V3DLONG xe = ix+Wx; if(xe>=N-1) xe = N-1;
                    V3DLONG yb = iy-Wy; if(yb<0) yb = 0;
                    V3DLONG ye = iy+Wy; if(ye>=M-1) ye = M-1;
                    V3DLONG zb = iz-Wz; if(zb<0) zb = 0;
                    V3DLONG ze = iz+Wz; if(ze>=P-1) ze = P-1;

                    T *arr,tmp;
                    int ii,jj;
                    int size = (2*Wx+1)*(2*Wy+1)*(2*Wz+1);
                    arr = new T[size];
                    //printf("%d %d\n",PixelValue,size);

                    ii = 0;

                    for(V3DLONG k=zb; k<=ze; k++)
                    {
                        V3DLONG offsetkl = k*M*N;
                        for(V3DLONG j=yb; j<=ye; j++)
                        {
                            V3DLONG offsetjl = j*N;
                            for(V3DLONG i=xb; i<=xe; i++)
                            {
                                T dataval = data1d[ offsetc + offsetkl + offsetjl + i];
                                arr[ii] = dataval;
                                if (ii>0)
                                {
                                    jj = ii;
                                    while(jj > 0 && arr[jj-1]>arr[jj])
                                    {
                                        tmp = arr[jj];
                                        arr[jj] = arr[jj-1];
                                        arr[jj-1] = tmp;
                                        jj--;
                                    }
                                }
                                ii++;
                            }
                        }
                    }

                    //for (ii = 0;ii<size;ii++)
                    //	printf("%d ",arr[ii]);
                    //	printf("\n");
                    //set value
                    V3DLONG index_pim = offsetk + offsetj + ix;
                    pImage[index_pim] = arr[int(0.5*size)+1];
                    delete [] arr;
                }
            }
        }
    }

    outimg = pImage;
    return;
}


#include <boost/algorithm/string.hpp>
#include <fstream>
#include <iostream>
#include <stdlib.h>
#include <string>
#include <vector>

void print(int x, int y, int count)
{
    for (int i = 1; i <= count; i++) {
        if (i % x == 0) {
            std::cout << "F";
        }
        if (i % y == 0) {
            std::cout << "B";
        }
        if (i % x != 0 && i % y != 0) {
            std::cout << i;
        }
        if (i != count) {
            std::cout << " ";
        } else {
            std::cout << std::endl;
        }
    }
}

int main(int argc, char *argv[])
{
    if (argc != 2) {
        std::cerr << "usage: ./fizzbuzz <filename>" << std::endl;
        return EXIT_FAILURE;
    }

    std::ifstream file(argv[1]); // get filename and open
    if (file.is_open()) {
        std::string line;
        while (std::getline(file, line)) { //read line by line
            std::vector<std::string> fields;
            boost::split(fields, line, boost::is_any_of(" ")); //slpit by " "
            print(atoi(fields[0].c_str()), atoi(fields[1].c_str()), atoi(fields[2].c_str()));
        }
        file.close();
    } else {
        std::cerr << "could not open file: " << argv[1] << std::endl;
    }

    return EXIT_SUCCESS;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// 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/.

#define EIGEN_NO_STATIC_ASSERT
#include "product.h"
#include <Eigen/LU>

// regression test for bug 447
void product1x1()
{
  Matrix<float,1,3> matAstatic;
  Matrix<float,3,1> matBstatic;
  matAstatic.setRandom();
  matBstatic.setRandom();
  VERIFY_IS_APPROX( (matAstatic * matBstatic).coeff(0,0),
                    matAstatic.cwiseProduct(matBstatic.transpose()).sum() );

  MatrixXf matAdynamic(1,3);
  MatrixXf matBdynamic(3,1);
  matAdynamic.setRandom();
  matBdynamic.setRandom();
  VERIFY_IS_APPROX( (matAdynamic * matBdynamic).coeff(0,0),
                    matAdynamic.cwiseProduct(matBdynamic.transpose()).sum() );
}

void test_product_small()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( product(Matrix<float, 3, 2>()) );
    CALL_SUBTEST_2( product(Matrix<int, 3, 5>()) );
    CALL_SUBTEST_3( product(Matrix3d()) );
    CALL_SUBTEST_4( product(Matrix4d()) );
    CALL_SUBTEST_5( product(Matrix4f()) );
    CALL_SUBTEST_6( product1x1() );
  }

#ifdef EIGEN_TEST_PART_6
  {
    // test compilation of (outer_product) * vector
    Vector3f v = Vector3f::Random();
    VERIFY_IS_APPROX( (v * v.transpose()) * v, (v * v.transpose()).eval() * v);
  }

  {
    // regression test for pull-request #93
    Eigen::Matrix<double, 1, 1> A;  A.setRandom();
    Eigen::Matrix<double, 18, 1> B; B.setRandom();
    Eigen::Matrix<double, 1, 18> C; C.setRandom();
    VERIFY_IS_APPROX(B * A.inverse(), B * A.inverse()[0]);
    VERIFY_IS_APPROX(A.inverse() * C, A.inverse()[0] * C);
  }
#endif
}


#pragma once

#include <polyfem/Mesh.hpp>

#include <Eigen/Dense>

namespace polyfem
{
	class BoundarySampler
	{
	public:
		static void sample_parametric_quad_edge(int index, int n_samples, Eigen::MatrixXd &uv, Eigen::MatrixXd &samples);
		static void sample_parametric_tri_edge(int index, int n_samples, Eigen::MatrixXd &uv, Eigen::MatrixXd &samples);

		static void sample_parametric_quad_face(int index, int n_samples, Eigen::MatrixXd &uv, Eigen::MatrixXd &samples);
		static void sample_parametric_tri_face(int index, int n_samples, Eigen::MatrixXd &uv, Eigen::MatrixXd &samples);

		static void quadrature_for_quad_edge(int index, int order, const int gid, const Mesh &mesh, Eigen::MatrixXd &uv, Eigen::MatrixXd &points, Eigen::VectorXd &weights);
		static void quadrature_for_tri_edge(int index, int order, const int gid, const Mesh &mesh, Eigen::MatrixXd &uv, Eigen::MatrixXd &points, Eigen::VectorXd &weights);

		static void quadrature_for_quad_face(int index, int order, const int gid, const Mesh &mesh, Eigen::MatrixXd &uv, Eigen::MatrixXd &points, Eigen::VectorXd &weights);
		static void quadrature_for_tri_face(int index, int order, const int gid, const Mesh &mesh, Eigen::MatrixXd &uv, Eigen::MatrixXd &points, Eigen::VectorXd &weights);
	};
}


/**
 * \copyright
 * Copyright (c) 2012-2018, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include <cmath>
#include <memory>

#include <gtest/gtest.h>

#include <Eigen/Eigen>

#include "MeshLib/Elements/Line.h"
#include "MeshLib/Elements/Tri.h"
#include "MeshLib/Mesh.h"

#include "ProcessLib/LIE/Common/Utils.h"

namespace
{
std::unique_ptr<MeshLib::Mesh> createTriangle(
    std::array<std::array<double, 3>, 3> const& points)
{
    auto** nodes = new MeshLib::Node*[3];
    for (std::size_t i = 0; i < points.size(); ++i)
        nodes[i] = new MeshLib::Node(points[i]);
    MeshLib::Element* e = new MeshLib::Tri(nodes);

    return std::make_unique<MeshLib::Mesh>(
        "", std::vector<MeshLib::Node*>{nodes[0], nodes[1], nodes[2]},
        std::vector<MeshLib::Element*>{e});
}

std::unique_ptr<MeshLib::Mesh> createLine(
    std::array<std::array<double, 3>, 2> const& points)
{
    auto** nodes = new MeshLib::Node*[2];
    for (std::size_t i = 0; i < points.size(); ++i)
        nodes[i] = new MeshLib::Node(points[i]);
    MeshLib::Element* e = new MeshLib::Line(nodes);

    return std::make_unique<MeshLib::Mesh>(
        "", std::vector<MeshLib::Node*>{nodes[0], nodes[1]},
        std::vector<MeshLib::Element*>{e});
}

const double eps = std::numeric_limits<double>::epsilon();

}

TEST(LIE, rotationMatrixXYTriangle)
{
    auto msh = createTriangle(
        {{{{0.0, 0.0, 0.0}}, {{1.0, 0.0, 0.0}}, {{1.0, 1.0, 0.0}}}});
    auto e(msh->getElement(0));
    Eigen::Vector3d nv;
    ProcessLib::LIE::computeNormalVector(*e, 3, nv);
    ASSERT_EQ(0., nv[0]);
    ASSERT_EQ(0., nv[1]);
    ASSERT_EQ(-1., nv[2]);

    Eigen::MatrixXd R(3, 3);
    ProcessLib::LIE::computeRotationMatrix(*e, nv, 3, R);

    ASSERT_NEAR(-1., R(0, 0), eps);
    ASSERT_NEAR(0., R(0, 1), eps);
    ASSERT_NEAR(0., R(0, 2), eps);
    ASSERT_NEAR(0., R(1, 0), eps);
    ASSERT_NEAR(1., R(1, 1), eps);
    ASSERT_NEAR(0., R(1, 2), eps);
    ASSERT_NEAR(0., R(2, 0), eps);
    ASSERT_NEAR(0., R(2, 1), eps);
    ASSERT_NEAR(-1., R(2, 2), eps);
}

TEST(LIE, rotationMatrixYZTriangle)
{
    auto msh = createTriangle(
        {{{{0.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}}});
    auto e(msh->getElement(0));
    Eigen::Vector3d nv;
    ProcessLib::LIE::computeNormalVector(*e, 3, nv);
    ASSERT_EQ(-1., nv[0]);
    ASSERT_EQ(0., nv[1]);
    ASSERT_EQ(0., nv[2]);

    Eigen::MatrixXd R(3, 3);
    ProcessLib::LIE::computeRotationMatrix(*e, nv, 3, R);

    ASSERT_NEAR(0., R(0, 0), eps);
    ASSERT_NEAR(-1., R(0, 1), eps);
    ASSERT_NEAR(0., R(0, 2), eps);
    ASSERT_NEAR(0., R(1, 0), eps);
    ASSERT_NEAR(0., R(1, 1), eps);
    ASSERT_NEAR(1., R(1, 2), eps);
    ASSERT_NEAR(-1., R(2, 0), eps);
    ASSERT_NEAR(0., R(2, 1), eps);
    ASSERT_NEAR(0., R(2, 2), eps);
}

TEST(LIE, rotationMatrixX)
{
    auto msh = createLine({{{{-1.0, 0.0, 0.0}}, {{1.0, 0.0, 0.0}}}});
    auto e(msh->getElement(0));
    Eigen::Vector3d nv;
    ProcessLib::LIE::computeNormalVector(*e, 2, nv);
    ASSERT_EQ(0., nv[0]);
    ASSERT_EQ(1., nv[1]);
    ASSERT_EQ(0., nv[2]);

    Eigen::MatrixXd R(2,2);
    ProcessLib::LIE::computeRotationMatrix(*e, nv, 2, R);

    ASSERT_NEAR(1., R(0,0), eps);
    ASSERT_NEAR(0., R(0,1), eps);
    ASSERT_NEAR(0., R(1,0), eps);
    ASSERT_NEAR(1., R(1,1), eps);
}

TEST(LIE, rotationMatrixY)
{
    auto msh = createLine({{{{0.0, -1.0, 0.0}}, {{0.0, 1.0, 0.0}}}});
    auto e(msh->getElement(0));
    Eigen::Vector3d nv;
    ProcessLib::LIE::computeNormalVector(*e, 2, nv);
    ASSERT_EQ(-1., nv[0]);
    ASSERT_EQ(0., nv[1]);
    ASSERT_EQ(0., nv[2]);

    Eigen::MatrixXd R(2,2);
    ProcessLib::LIE::computeRotationMatrix(*e, nv, 2, R);

    ASSERT_NEAR(0., R(0,0), eps);
    ASSERT_NEAR(1., R(0,1), eps);
    ASSERT_NEAR(-1., R(1,0), eps);
    ASSERT_NEAR(0., R(1,1), eps);
}

TEST(LIE, rotationMatrixXY)
{
    // 45degree inclined
    auto msh = createLine(
        {{{{0.0, 0.0, 0.0}}, {{2. / std::sqrt(2), 2. / std::sqrt(2), 0.0}}}});
    auto e(msh->getElement(0));
    Eigen::Vector3d nv;
    ProcessLib::LIE::computeNormalVector(*e, 2, nv);
    ASSERT_NEAR(-1./std::sqrt(2), nv[0], eps);
    ASSERT_NEAR(1./std::sqrt(2), nv[1], eps);
    ASSERT_EQ(0., nv[2]);

    Eigen::MatrixXd R(2,2);
    ProcessLib::LIE::computeRotationMatrix(*e, nv, 2, R);

    ASSERT_NEAR(1./std::sqrt(2), R(0,0), eps);
    ASSERT_NEAR(1./std::sqrt(2), R(0,1), eps);
    ASSERT_NEAR(-1./std::sqrt(2), R(1,0), eps);
    ASSERT_NEAR(1./std::sqrt(2), R(1,1), eps);
}


#include <algorithm>
#include <functional>
#include <boost/serialization/export.hpp>
#include "L2Regularization.hpp"

using namespace std;
using namespace snn;
using namespace internal;

BOOST_CLASS_EXPORT(L2Regularization)

L2Regularization::L2Regularization(const float value, BaseLayer* layer)
    : LayerOptimizer(layer), value(value)
{
}

L2Regularization::L2Regularization(const L2Regularization& regularization, const BaseLayer* layer)
    : LayerOptimizer(layer)
{
    this->value = regularization.value;
}

unique_ptr<LayerOptimizer> L2Regularization::clone(const BaseLayer* newLayer) const
{
    return make_unique<L2Regularization>(*this, newLayer);
}

void L2Regularization::applyBeforeBackpropagation(std::vector<float>& inputErrors)
{
    auto regularization = this->layer->getAverageOfSquareNeuronWeights() * this->value;
    ranges::transform(inputErrors, inputErrors.begin(), bind(plus<float>(), placeholders::_1, regularization));
}

bool L2Regularization::operator==(const LayerOptimizer& optimizer) const
{
    try
    {
        const auto& o = dynamic_cast<const L2Regularization&>(optimizer);
        return this->LayerOptimizer::operator==(optimizer)
            && this->value == o.value;
    }
    catch (bad_cast&)
    {
        return false;
    }
}

bool L2Regularization::operator!=(const LayerOptimizer& optimizer) const
{
    return !(*this == optimizer);
}


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/reconstruction_estimator_utils.h"

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/LU>

#include <algorithm>
#include <unordered_map>
#include <vector>

#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/bundle_adjustment/bundle_adjustment.h"
#include "theia/sfm/bundle_adjustment/optimize_relative_position_with_known_rotation.h"
#include "theia/sfm/camera/reprojection_error.h"
#include "theia/sfm/global_pose_estimation/nonlinear_position_estimator.h"
#include "theia/sfm/reconstruction.h"
#include "theia/sfm/reconstruction_estimator.h"
#include "theia/sfm/triangulation/triangulation.h"
#include "theia/sfm/twoview_info.h"
#include "theia/sfm/view_graph/view_graph.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/map_util.h"
#include "theia/util/threadpool.h"

namespace theia {
namespace {

// Accumulate all two view feature matches between the input views. The features
// are normalized according to the camera intrinsics.
void GetFeatureCorrespondences(const View& view1, const View& view2,
                               std::vector<FeatureCorrespondence>* matches) {
  Eigen::Matrix3d calibration1, calibration2;
  view1.Camera().GetCalibrationMatrix(&calibration1);
  view2.Camera().GetCalibrationMatrix(&calibration2);
  Eigen::Matrix3d inv_calibration1, inv_calibration2;
  bool view1_invertible, view2_invertible;
  double determinant;
  calibration1.computeInverseAndDetWithCheck(inv_calibration1, determinant,
                                             view1_invertible);
  calibration2.computeInverseAndDetWithCheck(inv_calibration2, determinant,
                                             view2_invertible);
  if (!view1_invertible || !view2_invertible) {
    LOG(FATAL) << "Calibration matrices are ill formed. Cannot optimize "
                  "epipolar constraints.";
    return;
  }

  const std::vector<TrackId>& tracks = view1.TrackIds();
  for (const TrackId track_id : tracks) {
    const Feature* feature2 = view2.GetFeature(track_id);
    // If view 2 does not contain the current track then it cannot be a
    // correspondence.
    if (feature2 == nullptr) {
      continue;
    }

    FeatureCorrespondence match;
    const Feature* feature1 = view1.GetFeature(track_id);
    match.feature1 = *feature1;
    match.feature2 = *feature2;

    // Normalize for camera intrinsics.
    match.feature1 =
        (inv_calibration1 * match.feature1.homogeneous()).eval().hnormalized();
    match.feature2 =
        (inv_calibration2 * match.feature2.homogeneous()).eval().hnormalized();
    matches->emplace_back(match);
  }
}

}  // namespace

// Sets the bundle adjustment options from the reconstruction estimator options.
BundleAdjustmentOptions SetBundleAdjustmentOptions(
    const ReconstructionEstimatorOptions& options, const int num_views) {
  static const int kMinViewsForSparseSchur = 150;

  BundleAdjustmentOptions ba_options;
  ba_options.num_threads = options.num_threads;
  ba_options.loss_function_type = options.bundle_adjustment_loss_function_type;
  ba_options.robust_loss_width = options.bundle_adjustment_robust_loss_width;
  ba_options.use_inner_iterations = true;
  ba_options.intrinsics_to_optimize = options.intrinsics_to_optimize;

  if (num_views >= options.min_cameras_for_iterative_solver) {
    ba_options.linear_solver_type = ceres::ITERATIVE_SCHUR;
    // NOTE: this is an arbitrary scaling that was found to work well. It may
    // need to change depending on the application.
    ba_options.max_num_iterations *= 1.5;
  } else if (num_views >= kMinViewsForSparseSchur) {
    ba_options.linear_solver_type = ceres::SPARSE_SCHUR;
  } else {
    ba_options.linear_solver_type = ceres::DENSE_SCHUR;
  }
  ba_options.verbose = true;
  return ba_options;
}

// Sets the ransac parameters from the reconstruction estimator options.
RansacParameters SetRansacParameters(
    const ReconstructionEstimatorOptions& options) {
  RansacParameters ransac_params;
  ransac_params.failure_probability = 1.0 - options.ransac_confidence;
  ransac_params.min_iterations = options.ransac_min_iterations;
  ransac_params.max_iterations = options.ransac_max_iterations;
  ransac_params.use_mle = options.ransac_use_mle;
  return ransac_params;
}

// Collects the relative rotations for each view pair into a simple map.
std::unordered_map<ViewIdPair, Eigen::Vector3d> RelativeRotationsFromViewGraph(
    const ViewGraph& view_graph) {
  std::unordered_map<ViewIdPair, Eigen::Vector3d> relative_rotations;

  const auto& view_pairs = view_graph.GetAllEdges();
  for (const auto& view_pair : view_pairs) {
    relative_rotations[view_pair.first] = view_pair.second.rotation_2;
  }
  return relative_rotations;
}

// Each view that has a rotation and position estimated has the Camera pose set.
void SetReconstructionFromEstimatedPoses(
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientations,
    const std::unordered_map<ViewId, Eigen::Vector3d>& positions,
    Reconstruction* reconstruction) {
  for (const auto& position : positions) {
    View* view = reconstruction->MutableView(position.first);
    if (view == nullptr) {
      LOG(WARNING) << "Trying to set the pose of View " << position.first
                   << " which does not exist in the reconstruction.";
      continue;
    }
    CHECK(!view->IsEstimated()) << "Cannot set the pose of a view that has "
                                   "already been estimated. View Id "
                                << position.first;

    const Eigen::Vector3d* orientation =
        FindOrNull(orientations, position.first);
    if (orientation == nullptr) {
      LOG(WARNING) << "Cannot add View " << position.first
                   << " to the reconstruction because it does nto contain an "
                      "orientation estimation.";
      continue;
    }

    view->MutableCamera()->SetPosition(position.second);
    view->MutableCamera()->SetOrientationFromAngleAxis(*orientation);
    view->SetEstimated(true);
  }
}

void CreateEstimatedSubreconstruction(
    const Reconstruction& input_reconstruction,
    Reconstruction* estimated_reconstruction) {
  *estimated_reconstruction = input_reconstruction;
  const auto view_ids = estimated_reconstruction->ViewIds();
  for (const ViewId view_id : view_ids) {
    const View* view = estimated_reconstruction->View(view_id);
    if (view == nullptr) {
      continue;
    }

    if (!view->IsEstimated()) {
      estimated_reconstruction->RemoveView(view_id);
    }
  }

  const auto track_ids = estimated_reconstruction->TrackIds();
  for (const TrackId track_id : track_ids) {
    const Track* track = estimated_reconstruction->Track(track_id);
    if (track == nullptr) {
      continue;
    }

    if (!track->IsEstimated() || track->NumViews() < 2) {
      estimated_reconstruction->RemoveTrack(track_id);
    }
  }
}

// Outputs the ViewId of all estimated views in the reconstruction.
void GetEstimatedViewsFromReconstruction(const Reconstruction& reconstruction,
                                         std::unordered_set<ViewId>* views) {
  CHECK_NOTNULL(views)->clear();
  for (const ViewId view_id : reconstruction.ViewIds()) {
    const View* view = reconstruction.View(view_id);
    if (view != nullptr && view->IsEstimated()) {
      views->insert(view_id);
    }
  }
}

// Outputs the TrackId of all estimated tracks in the reconstruction.
void GetEstimatedTracksFromReconstruction(const Reconstruction& reconstruction,
                                          std::unordered_set<TrackId>* tracks) {
  CHECK_NOTNULL(tracks)->clear();
  const auto& track_ids = reconstruction.TrackIds();
  for (const TrackId track_id : track_ids) {
    const Track* track = reconstruction.Track(track_id);
    if (track != nullptr && track->IsEstimated()) {
      tracks->insert(track_id);
    }
  }
}

void RefineRelativeTranslationsWithKnownRotations(
    const Reconstruction& reconstruction,
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientations,
    const int num_threads,
    ViewGraph* view_graph) {
  CHECK_GE(num_threads, 1);
  const auto& view_pairs = view_graph->GetAllEdges();

  ThreadPool pool(num_threads);
  // Refine the translation estimation for each view pair.
  for (const auto& view_pair : view_pairs) {
    // Get all feature correspondences common to both views.
    std::vector<FeatureCorrespondence> matches;
    const View* view1 = reconstruction.View(view_pair.first.first);
    const View* view2 = reconstruction.View(view_pair.first.second);
    GetFeatureCorrespondences(*view1, *view2, &matches);

    TwoViewInfo* info = view_graph->GetMutableEdge(view_pair.first.first,
                                                   view_pair.first.second);
    pool.Add(OptimizeRelativePositionWithKnownRotation,
             matches,
             FindOrDie(orientations, view_pair.first.first),
             FindOrDie(orientations, view_pair.first.second),
             &info->position_2);
  }
}

int RemoveOutlierFeatures(const double max_inlier_reprojection_error,
                          const double min_triangulation_angle_degrees,
                          Reconstruction* reconstruction) {
  const auto& track_ids = reconstruction->TrackIds();
  const std::unordered_set<TrackId> all_tracks(track_ids.begin(),
                                               track_ids.end());
  return RemoveOutlierFeatures(all_tracks,
                               max_inlier_reprojection_error,
                               min_triangulation_angle_degrees,
                               reconstruction);
}

int RemoveOutlierFeatures(const std::unordered_set<TrackId>& track_ids,
                          const double max_inlier_reprojection_error,
                          const double min_triangulation_angle_degrees,
                          Reconstruction* reconstruction) {
  const double max_sq_reprojection_error =
      max_inlier_reprojection_error * max_inlier_reprojection_error;

  int num_estimated_tracks = 0;
  int num_bad_reprojections = 0;
  int num_insufficient_viewing_angles = 0;

  for (const TrackId track_id : track_ids) {
    Track* track = reconstruction->MutableTrack(track_id);
    if (!track->IsEstimated()) {
      continue;
    }
    ++num_estimated_tracks;

    std::vector<Eigen::Vector3d> ray_directions;
    const auto& view_ids = track->ViewIds();
    int num_projections = 0;
    double mean_sq_reprojection_error = 0;
    for (const ViewId view_id : view_ids) {
      const View* view = CHECK_NOTNULL(reconstruction->View(view_id));
      if (!view->IsEstimated()) {
        continue;
      }

      const Camera& camera = view->Camera();
      const Eigen::Vector3d ray_direction =
          track->Point().hnormalized() - camera.GetPosition();
      ray_directions.push_back(ray_direction.normalized());

      // Check the reprojection error.
      const Feature* feature = view->GetFeature(track_id);
      // Reproject the observations.
      Eigen::Vector2d projection;
      const double depth = camera.ProjectPoint(track->Point(), &projection);
      // Remove the feature if the reprojection is behind the camera.
      if (depth < 0) {
        ++num_bad_reprojections;
        track->SetEstimated(false);
        break;
      }
      mean_sq_reprojection_error += (projection - *feature).squaredNorm();
      ++num_projections;
    }

    mean_sq_reprojection_error /= static_cast<double>(num_projections);
    if (track->IsEstimated() &&
        mean_sq_reprojection_error > max_sq_reprojection_error) {
      ++num_bad_reprojections;
      track->SetEstimated(false);
    }

    // The track will remain estimated if the reprojection errors were all
    // good. We then test that the track is properly constrained by having at
    // least two cameras view it with a sufficient viewing angle.
    if (track->IsEstimated() &&
        !SufficientTriangulationAngle(ray_directions,
                                      min_triangulation_angle_degrees)) {
      ++num_insufficient_viewing_angles;
      track->SetEstimated(false);
    }
  }

  LOG_IF(INFO, num_bad_reprojections > 0 || num_insufficient_viewing_angles > 0)
      << num_bad_reprojections
      << " points were removed because of bad reprojection errors. "
      << num_insufficient_viewing_angles
      << " points were removed because they had insufficient viewing angles "
         "and were poorly constrained.";

  return num_bad_reprojections + num_insufficient_viewing_angles;
}

int SetUnderconstrainedTracksToUnestimated(Reconstruction* reconstruction) {
  static const int kMinNumViews = 2;
  int num_underconstrained_tracks = 0;
  // Set all underconstrained tracks to be unestimated.
  for (const TrackId track_id : reconstruction->TrackIds()) {
    Track* track = CHECK_NOTNULL(reconstruction->MutableTrack(track_id));
    if (!track->IsEstimated()) {
      continue;
    }

    // Count the number of estimated views observing it.
    int num_estimated_views = 0;
    for (const ViewId view_id : track->ViewIds()) {
      if (reconstruction->View(view_id)->IsEstimated()) {
        ++num_estimated_views;
      }
      if (num_estimated_views >= kMinNumViews) {
        break;
      }
    }

    if (num_estimated_views < kMinNumViews) {
      track->SetEstimated(false);
      ++num_underconstrained_tracks;
    }
  }

  return num_underconstrained_tracks;
}

int SetUnderconstrainedViewsToUnestimated(Reconstruction* reconstruction) {
  // Set all underconstrained views to be unestimated.
  static const int kMinNumTracks = 3;
  int num_underconstrained_views = 0;
  // Set all underconstrained views to be unestimated.
  for (const ViewId view_id : reconstruction->ViewIds()) {
    View* view = CHECK_NOTNULL(reconstruction->MutableView(view_id));
    if (!view->IsEstimated()) {
      continue;
    }

    // Count the number of estimated views observing it.
    int num_estimated_tracks = 0;
    for (const TrackId track_id : view->TrackIds()) {
      if (reconstruction->Track(track_id)->IsEstimated()) {
        ++num_estimated_tracks;
      }
      if (num_estimated_tracks >= kMinNumTracks) {
        break;
      }
    }

    if (num_estimated_tracks < kMinNumTracks) {
      view->SetEstimated(false);
      ++num_underconstrained_views;
    }
  }

  return num_underconstrained_views;
}

int NumEstimatedViews(const Reconstruction& reconstruction) {
  int num_estimated_views = 0;
  for (const ViewId view_id : reconstruction.ViewIds()) {
    const View* view = reconstruction.View(view_id);
    if (view == nullptr || !view->IsEstimated()) {
      continue;
    }
    ++num_estimated_views;
  }
  return num_estimated_views;
}

int NumEstimatedTracks(const Reconstruction& reconstruction) {
  int num_estimated_tracks = 0;
  for (const TrackId track_id : reconstruction.TrackIds()) {
    const Track* track = reconstruction.Track(track_id);
    if (track == nullptr || !track->IsEstimated()) {
      continue;
    }
    ++num_estimated_tracks;
  }
  return num_estimated_tracks;
}

}  // namespace theia


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/shr.hpp>
#include <boost/simd/logical.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/three.hpp>

namespace bs = boost::simd;
namespace bd = boost::dispatch;

template <typename T, int N, typename Env>
void test(Env& $)
{
  using p_t = bs::pack<T, N>;

  T a1[N];
  T b[N], c[N];
  std::size_t sh1 =  sizeof(T)*8-1;

  int sh2 =  1;

  for(int i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : T(-i);
    b[i] = bs::shr(a1[i], sh1);
    c[i] = bs::shr(a1[i], sh2);
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb(&b[0], &b[0]+N);
  p_t cc(&c[0], &c[0]+N);

  STF_EQUAL(bs::shr(aa1, sh1), bb);
  STF_EQUAL(bs::shr(aa1, sh2), cc);
}

STF_CASE_TPL("Check shr on pack with integral shift" , STF_INTEGRAL_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

template <typename T, int N, typename Env>
void tests(Env& $)
{
  using iT =  bd::as_integer_t<T>;
  using p_t = bs::pack<T, N>;
  using i_t = bs::pack<iT, N>;

  T a1[N], b[N];
  iT sh[N];

  for(int i = 0; i < N; ++i)
  {
    sh[i] = i%(8*sizeof(T));
    a1[i] = (i%2) ? T(i) : T(-i);
    b[i] = bs::shr(a1[i],sh[i]);
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb(&b[0], &b[0]+N);
  i_t sh1(&sh[0], &sh[0]+N);
  STF_EQUAL(bs::shr(aa1, sh1), bb);
}

STF_CASE_TPL("Check shr on pack with pack shift" , STF_INTEGRAL_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  tests<T, N>($);
  tests<T, N/2>($);
  tests<T, N*2>($);
}

STF_CASE_TPL (" shr uit",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::shr;
  using p_t = bs::pack<T>;
  using ip_t = bd::as_integer_t<T, unsigned>;
  using r_t = decltype(shr(p_t(), ip_t()));

  // return type conformity test
  STF_TYPE_IS( r_t, p_t );

  // specific values tests
  STF_EQUAL(shr(p_t(2),1), bs::One<p_t>());
  STF_EQUAL(shr(bs::Valmax<p_t>(),(sizeof(T)*8-1)), bs::One<r_t>());
  STF_EQUAL(shr(bs::Valmax<p_t>(),(sizeof(T)*8-2)), bs::Three<r_t>());
  STF_EQUAL(shr(bs::One<p_t>(),1), bs::Zero<r_t>());
  STF_EQUAL(shr(bs::Zero<p_t>(),1), bs::Zero<r_t>());
} // end of test for unsigned_int_

STF_CASE_TPL (" shr si",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::shr;
  using p_t = bs::pack<T>;
  using ip_t = bd::as_integer_t<T, unsigned>;
  using r_t = decltype(shr(p_t(), ip_t()));

  // return type conformity test
  STF_TYPE_IS( r_t, p_t );

  // specific values tests
  STF_EQUAL(shr(p_t(2),1), bs::One<p_t>());
  STF_EQUAL(shr(bs::Mone<p_t>(),(sizeof(T)*8-1)), bs::One<r_t>());
  STF_EQUAL(shr(bs::Mone<p_t>(),(sizeof(T)*8-2)), bs::Three<r_t>());
  STF_EQUAL(shr(bs::One<p_t>(),1), bs::Zero<r_t>());
  STF_EQUAL(shr(bs::Zero<p_t>(),1), bs::Zero<r_t>());
} // end of test for unsigned_int_


/**
 * @brief YAML functions for reading Eigen vectors and matrices
 * @author Eric Cousineau <eacousineau@gmail.com>, member of Dr. Aaron
 * Ames's AMBER Lab
 */
#ifndef _YAML_EIGEN_UTILITIES_HPP_
#define _YAML_EIGEN_UTILITIES_HPP_

#include <Eigen/Dense>
#include <common_assert/common_assert.hpp>
#include <yaml_utilities/yaml_utilities.hpp>

// Have to put these here to overload yaml-cpp's implementation for vector in stlnode
namespace YAML
{

// Reading

template<typename Derived>
void eigen_resize_vector(const Node &in, Eigen::MatrixBase<Derived> &X)
{
    // http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
    // Blech
    if (in.Type() == YAML::NodeType::Scalar)
        X.derived().resize(1);
    else
        X.derived().resize(in.size());
}

template<typename Derived>
void eigen_read_vector(const Node &in, Eigen::MatrixBase<Derived> &X)
{
    if (in.Type() == YAML::NodeType::Scalar)
    {
        common_assert((uint)X.size() == 1);
        in >> X(0);
    }
    else
    {
        common_assert_msg((uint)X.size() == (uint)in.size(), "Size not equal. Eigen = " << X.size() << ", Yaml = " << in.size());
        for (uint i = 0; i < (uint)X.size(); ++i)
            in[i] >> X(i);
    }
}

template<typename Derived>
void eigen_resize_matrix(const Node &in, Eigen::MatrixBase<Derived> &X)
{
    uint rows = in.size();
    if (rows > 0)
    {
        // If just one row, then resize to row vector
        uint cols = in[0].size();
        if (cols == 0)
            X.derived().resize(1, rows);
        else
            X.derived().resize(rows, in[0].size());
    }
    else
        X.derived().resize(0, 0);
}

template<typename Derived>
void eigen_read_matrix(const Node &in, Eigen::MatrixBase<Derived> &X)
{
    typedef Eigen::MatrixBase<Derived> Matrix;
    uint rows = in.size();
    if (rows > 0)
    {
        uint cols = in[0].size();
        // Load as a vector if it's a single list
        if (cols == 0)
            eigen_read_vector(in, X);
        else
        {
            common_assert_msg((uint)X.rows() == rows, "Rows not equal. Eigen = " << X.rows() << ", Yaml = " << rows);
            common_assert_msg((uint)X.cols() == cols, "Cols not equal. Eigen = " << X.cols() << ", Yaml = " << cols);
            for (uint i = 0; i < rows; ++i)
            {
                typename Matrix::RowXpr row = X.row(i);
                eigen_read_vector(in[i], row);
            }
        }
    }
}

// Writing

template<typename Derived>
void eigen_write_vector(Emitter &out, const Eigen::MatrixBase<Derived> &X)
{
    out << Flow << BeginSeq;
    for (uint i = 0; i < (uint)X.size(); ++i)
        out << X.coeff(i);
    out << EndSeq;
}

template<typename Derived>
void eigen_write_matrix(Emitter &out, const Eigen::MatrixBase<Derived> &X)
{
    out << BeginSeq;
    for (uint i = 0; i < (uint)X.rows(); ++i)
        eigen_write_vector(out, X.row(i));
    out << EndSeq;
}

// Piecing it together

template<typename Derived, int IsVector>
struct eigen_handler_impl
{ };

// Vector specialization
template<typename Derived>
struct eigen_handler_impl<Derived, 1>
{
    typedef Eigen::MatrixBase<Derived> Matrix;
    static void resize(const Node &in, Matrix &X)
    {
        eigen_resize_vector(in, X);
    }

    static void read(const Node &in, Matrix &X)
    {
        eigen_read_vector(in, X);
    }

    static void write(Emitter &out, const Matrix &X)
    {
        eigen_write_vector(out, X);
    }
};

// Matrix specialization
template<typename Derived>
struct eigen_handler_impl<Derived, 0>
{
    typedef Eigen::MatrixBase<Derived> Matrix;
    static void resize(const Node &in, Matrix &X)
    {
        eigen_resize_matrix(in, X);
    }

    static void read(const Node &in, Matrix &X)
    {
        eigen_read_matrix(in, X);
    }

    static void write(Emitter &out, const Matrix &X)
    {
        eigen_write_matrix(out, X);
    }
};

// Make it easy to select
template<typename Derived>
struct eigen_handler : public eigen_handler_impl<Derived, Eigen::MatrixBase<Derived>::IsVectorAtCompileTime>
{ };

template<typename Derived>
void eigen_read_and_resize(const Node &in, Eigen::MatrixBase<Derived> &X)
{
    typedef eigen_handler<Derived> handler;
    if (handler::Matrix::SizeAtCompileTime == Eigen::Dynamic)
        handler::resize(in, X);
    handler::read(in, X);
}

// YAML has trouble with the templating stuff, just going to overload some specific matrix here
// @todo Look into this: http://code.google.com/p/yaml-cpp/issues/detail?id=150#c13
template<typename Scalar, int Cols, int Rows>
void operator>>(const Node &in, Eigen::Matrix<Scalar, Cols, Rows> &X)
{
    eigen_read_and_resize(in, X);
}

template<typename Derived>
Emitter &operator<<(Emitter &out, const Eigen::MatrixBase<Derived> &X)
{
    typedef eigen_handler<Derived> handler;
    handler::write(out, X);
    return out;
}

//template <typename Derived>
//inline bool Node::Read(Eigen::MatrixBase<Derived>& value) const
//{
//    eigen_read_and_resize(*this, value);
//}

//template<typename Derived>
//struct convert<Eigen::MatrixBase<Derived> >
//{
//    typedef Eigen::MatrixBase<Derived> Matrix;
//    static Node encode(const Matrix& rhs) {
//        Node node;
//        eigen_handler<Derived>::write(node);
//        return node;
//    }

//    static bool decode(const Node& node, const Eigen::MatrixBase<Derived>& rhs) {
//        eigen_read_and_resize(node, rhs);
//        return true;
//    }
//};

} // End namespace YAML

#endif