/***************************************************************************
   Copyright 2015 Ufora Inc.

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
****************************************************************************/
#ifndef GraphUtil_hppml_
#define GraphUtil_hppml_

/*******
GraphUtil

utilities for doing computations on graphs whose nodes and edges
are defined using sets and maps.

********/

#include <set>
#include <map>

#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/property_map/property_map.hpp>

#include "../cppml/CPPMLPrettyPrinter.hppml"
#include "../containers/TwoWaySetMap.hpp"

namespace  GraphUtil {

/********

Computes the set of edges reachable from a particular node in a directed graph.

It is not necessary for every node to be a key in inEdges. If it's not there,
but it's referred to by another edge, the algorithm assumes it has no outgoing
edges.

*********/
template<class T>
void	computeReachableNodes(
					const std::map<T, std::set<T> >& inEdges,
					std::set<T>& outReachable,
					const T& inNode
					)
	{
	if (outReachable.find(inNode) == outReachable.end())
		{
		outReachable.insert(inNode);

		auto map_it = inEdges.find(inNode);

		if (map_it != inEdges.end())
			for (auto edge: map_it->second)
				computeReachableNodes(inEdges, outReachable, edge);
		}
	}

template<class T>
void	computeReachableNodes(
					const TwoWaySetMap<T, T>& inEdges,
					std::set<T>& outReachable,
					const T& inNode
					)
	{
	computeReachableNodes(inEdges.getKeysToValues(), outReachable, inNode);
	}

/*****

Find, in the directed graph "inEdges", every group of nodes such that
each node is reachable from each of the others by a path.  Essentially,
two nodes are in a group together iff there is a cycle that contains them.

nodes that aren't in the edge map don't ever show up in the graph

if includeSingleNodeComponents is set to true, then we include everything.
If it's set to false, we filter out nodes that are not themselves part of
any cycle.

If onlyIncludeFreeComponents is set to true, then we don't include any
subgroups that have external nodes jumping into them.

*****/
template<class T>
void	computeStronglyConnectedComponents(
				const std::map<T, std::set<T> >& inEdges,
				std::vector<std::set<T> >& outComponents,
				bool includeSingleNodeComponents,
				bool onlyIncludeFreeComponents
				)
	{
	if (!inEdges.size())
		return;
	using namespace boost;
	using namespace std;

	map<T, uint32_t> 				nodeToIndex;
	map<uint32_t, T> 				indexToNode;

	uint32_t ix = 0;
	for (typename map<T, set<T> >::const_iterator
			it = inEdges.begin(); it!= inEdges.end(); ++it)
		{
		nodeToIndex[it->first] = ix;
		indexToNode[ix] = it->first;
		ix++;
		}

	typedef adjacency_list<vecS, vecS, bidirectionalS> Graph;

	Graph g(nodeToIndex.size());

	//add any edges that stay within the graph
	for (typename map<T, set<T> >::const_iterator
			node_it = inEdges.begin(); node_it != inEdges.end(); ++node_it)
		for (typename set<T>::const_iterator edge_it = node_it->second.begin();
											edge_it != node_it->second.end();
											++edge_it)
			{
			typename map<T, uint32_t>::iterator  index_it =
					nodeToIndex.find(*edge_it);

			if (index_it != nodeToIndex.end())
				add_edge(nodeToIndex[node_it->first], index_it->second, g);
			}

	map<int, int>	components;

	boost::associative_property_map< std::map<int, int> >
		  components_prop_map(components);

	int totalComponents = strong_components(g, components_prop_map);

	outComponents.resize(totalComponents);
	for (map<int,int>::iterator  it = components.begin();
											it != components.end(); ++it)
		{
		lassert(it->second >= 0 && it->second < totalComponents);
		outComponents[it->second].insert(indexToNode[it->first]);
		}

	if (onlyIncludeFreeComponents)
		{
		set<int> badComponents;

		//for each edge, check which components are being crossed and mark
		//destination components as 'bad' when they're different

		for (long k = 0; k < outComponents.size();k++)
			for (typename set<T>::const_iterator it = outComponents[k].begin();
					it != outComponents[k].end(); ++it)
				{
				const T& node = *it;

				int componentIndex = components[nodeToIndex[node]];

				typename map<T, set<T> >::const_iterator edgeIt = inEdges.find(node);

				for (typename set<T>::const_iterator
							outgoing_it = edgeIt->second.begin();
							outgoing_it != edgeIt->second.end();
							++outgoing_it
							)
					{
					int destComponentIndex = components[nodeToIndex[*outgoing_it]];
					if (componentIndex != destComponentIndex)
						badComponents.insert(destComponentIndex);
					}
				}
		for (long k = 0; k < outComponents.size();k++)
			if (badComponents.find(k) != badComponents.end())
				{
				std::swap(outComponents[k], outComponents.back());
				outComponents.resize(outComponents.size() - 1);
				k--;
				}
		}
	if (!includeSingleNodeComponents)
		for (long k = 0; k < outComponents.size();k++)
			if (outComponents[k].size() == 1)
				{
				const T& node = *outComponents[k].begin();

				typename map<T, set<T> >::const_iterator it = inEdges.find(node);

				bool mapsToSelf = (it != inEdges.end() &&
								it->second.find(node) != it->second.end());

				if (!mapsToSelf)
					{
					std::swap(outComponents[k], outComponents.back());
					outComponents.resize(outComponents.size() - 1);
					k--;
					}
				}
	}

template<class T>
void	computeStronglyConnectedComponents(
				const TwoWaySetMap<T, T>& inEdges,
				std::vector<std::set<T> >& outComponents,
				bool includeSingleNodeComponents,
				bool onlyIncludeFreeComponents
				)
	{
	computeStronglyConnectedComponents(
		inEdges.getKeysToValues(),
		outComponents,
		includeSingleNodeComponents,
		onlyIncludeFreeComponents
		);
	}

}

#endif


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/math/bindings/rr.hpp>
#include <fstream>

#include <boost/math/tools/test_data.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>

using namespace boost::math::tools;
using namespace std;

struct data_generator
{
   boost::math::tuple<boost::math::ntl::RR, boost::math::ntl::RR> operator()(boost::math::ntl::RR z)
   {
      return boost::math::make_tuple(boost::math::log1p(z), boost::math::expm1(z));
   }
};

int main(int argc, char* argv[])
{
   boost::math::ntl::RR::SetPrecision(1000);
   boost::math::ntl::RR::SetOutputPrecision(40);

   parameter_info<boost::math::ntl::RR> arg1;
   test_data<boost::math::ntl::RR> data;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the log1p and expm1 functions:\n\n";

   bool cont;
   std::string line;

   do{
      if(0 == get_user_parameter_info(arg1, "z"))
         return 1;
      data.insert(data_generator(), arg1);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=log1p_expm1_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "log1p_expm1_data.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific;
   write_code(ofs, data, "log1p_expm1_data");

   return 0;
}


/* ------------------------------------------------------------------------- */
/* UMFPACK Version 4.1 (Apr. 30, 2003), Copyright (c) 2003 by Timothy A.     */
/* Davis.  All Rights Reserved.  See ../README for License.                  */
/* email: davis@cise.ufl.edu    CISE Department, Univ. of Florida.           */
/* web: http://www.cise.ufl.edu/research/sparse/umfpack                      */
/* ------------------------------------------------------------------------- */

/***********************************************************************/
/*         UMFPACK Copyright, License and Availability                 */
/***********************************************************************/
/*
 *
 * UMFPACK Version 4.1 (Apr. 30, 2003),  Copyright (c) 2003 by Timothy A.
 * Davis.  All Rights Reserved.
 *
 * UMFPACK License:
 *
 *   Your use or distribution of UMFPACK or any modified version of
 *   UMFPACK implies that you agree to this License.
 *
 *   THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
 *   EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
 *
 *   Permission is hereby granted to use or copy this program, provided
 *   that the Copyright, this License, and the Availability of the original
 *   version is retained on all copies.  User documentation of any code that
 *   uses UMFPACK or any modified version of UMFPACK code must cite the
 *   Copyright, this License, the Availability note, and "Used by permission."
 *   Permission to modify the code and to distribute modified code is granted,
 *   provided the Copyright, this License, and the Availability note are
 *   retained, and a notice that the code was modified is included.  This
 *   software was developed with support from the National Science Foundation,
 *   and is provided to you free of charge.
 *
 * Availability:
 *
 *   http://www.cise.ufl.edu/research/sparse/umfpack
 *
 */

/* Used by permission. */

/* Simple demo program for UMFPACK               */
/* from UMFPACK Version 4.1 Quick Start Guide    */

/* modified by Kresimir Fresl, 2003              */
/* UMFPACK bindings & ublas::compressed_matrix<> */

#include <iostream>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_sparse.hpp>
#include <boost/numeric/bindings/umfpack/umfpack.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace ublas = boost::numeric::ublas;
namespace umf = boost::numeric::bindings::umfpack;

int main() {

  ublas::compressed_matrix<double, ublas::column_major, 0,
    ublas::unbounded_array<int>, ublas::unbounded_array<double> > A (5,5,12);
  ublas::vector<double> B (5), X (5);

  A(0,0) = 2.; A(0,1) = 3;
  A(1,0) = 3.; A(1,2) = 4.; A(1,4) = 6;
  A(2,1) = -1.; A(2,2) = -3.; A(2,3) = 2.;
  A(3,2) = 1.;
  A(4,1) = 4.; A(4,2) = 2.; A(4,4) = 1.;

  B(0) = 8.; B(1) = 45.; B(2) = -3.; B(3) = 3.; B(4) = 19.;

  umf::symbolic_type<double> Symbolic;
  umf::numeric_type<double> Numeric;

  umf::symbolic (A, Symbolic);
  umf::numeric (A, Symbolic, Numeric);
  umf::solve (A, X, B, Numeric);

  std::cout << X << std::endl;  // output: [5](1,2,3,4,5)
}


// 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 <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include <limits>
#include <vector>

#include "gtest/gtest.h"

#include "theia/math/util.h"
#include "theia/sfm/pose/sim_transform_partial_rotation.h"
#include "theia/sfm/pose/test_util.h"
#include "theia/test/test_utils.h"
#include "theia/util/random.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::AngleAxisd;
using Eigen::Matrix3d;
using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector3d;

RandomNumberGenerator rng(60);

// Tests that the four point pose works correctly by taking the passed
// points_3d, projecting them to view_1_origins to get image one rays,
// transforming by (expected_rotation, expected_translation) to get
// image two rays and then verifying that the FourPointEssentialMatrix
// function returns (expected_rotation, expected_translation) among its
// solutions.
// Noise can be added to the image projections by setting projection_noise.
// The thresholds for rotation and translation similarity can be controlled
// by max_rotation_difference and max_difference_between_translation.
// If projection_noise_std_dev is non-zero then a random noise generator must
// be passed in rng.
void TestSimTransformResultWithNoise(const Vector3d& axis,
                                     const Vector3d points_3d[5],
                                     const Vector3d view_1_origins[5],
                                     const Vector3d view_2_origins[5],
                                     double projection_noise_std_dev,
                                     const Quaterniond& expected_rotation,
                                     const Vector3d& expected_translation,
                                     const double expected_scale,
                                     double max_rotation_difference,
                                     double max_translation_difference,
                                     double max_scale_difference) {
  Vector3d image_one_rays[5];
  Vector3d image_two_rays[5];
  Vector3d image_one_origins[5];
  Vector3d image_two_origins[5];

  for (int i = 0; i < 5; ++i) {
    image_one_origins[i] = view_1_origins[i];
    image_one_rays[i] = (points_3d[i] - view_1_origins[i]).normalized();

    image_two_origins[i] =
        expected_rotation.inverse() *
        (view_2_origins[i] - expected_translation) / expected_scale;
    image_two_rays[i] = expected_rotation.inverse() *
                        (points_3d[i] - view_2_origins[i]).normalized();
  }
  if (projection_noise_std_dev) {
    for (int i = 0; i < 5; ++i) {
      AddNoiseToRay(projection_noise_std_dev, &rng, &image_one_rays[i]);
      AddNoiseToRay(projection_noise_std_dev, &rng, &image_two_rays[i]);
    }
  }
  std::vector<Quaterniond> soln_rotations;
  std::vector<Vector3d> soln_translations;
  std::vector<double> soln_scales;
  SimTransformPartialRotation(axis,
                              image_one_rays,
                              image_one_origins,
                              image_two_rays,
                              image_two_origins,
                              &soln_rotations,
                              &soln_translations,
                              &soln_scales);

  bool matched_transform = false;
  for (int n = 0; n < soln_rotations.size(); ++n) {
    const double rotation_difference =
        expected_rotation.angularDistance(soln_rotations[n]);

    bool matched_rotation = (rotation_difference < max_rotation_difference);

    const double translation_difference =
        (expected_translation - soln_translations[n]).norm();

    bool matched_translation =
        (translation_difference < max_translation_difference);

    const double scale_difference =
        fabs(expected_scale - soln_scales[n]) / expected_scale;
    const bool matched_scale = (scale_difference < max_scale_difference);

    LOG(INFO) << "Rot diff = " << rotation_difference;
    LOG(INFO) << "Translation diff = " << translation_difference;
    LOG(INFO) << "Scale diff = " << scale_difference;
    if (matched_translation && matched_rotation && matched_scale) {
      matched_transform = true;
      break;
    }
  }
  EXPECT_TRUE(matched_transform);
}

TEST(SimTransformPartialRotationTest, Basic) {
  // Sets up some points in the 3D scene
  const Vector3d kPoints3D[5] = { Vector3d(-1.0, 3.0, 3.0),
                                  Vector3d(1.0, -1.0, 2.0),
                                  Vector3d(2.0, 1.0, 3.0),
                                  Vector3d(4.0, 3.0, 5.0),
                                  Vector3d(-2.0, 1.0, -1.0) };

  const Vector3d kImageOneOrigins[5] = { Vector3d(-1.0, 0.0, 0.0),
                                         Vector3d(0.0, 0.0, 0.0),
                                         Vector3d(2.0, 0.0, 0.0),
                                         Vector3d(3.0, 0.0, 0.0),
                                         Vector3d(4.0, 0.0, 0.0) };

  const Vector3d kImageTwoOrigins[5] = { Vector3d(0.0, 1.0, 0.0),
                                         Vector3d(0.0, 0.0, 0.0),
                                         Vector3d(0.0, 2.0, 0.0),
                                         Vector3d(0.0, 3.0, 0.0),
                                         Vector3d(0.0, 4.0, 0.0) };

  const Vector3d axis = Vector3d(1.0, 1.0, 1.0).normalized();
  Quaterniond kExpectedRotation(AngleAxisd(DegToRad(51.0), axis));

  const Vector3d kExpectedTranslation(-2.0, 3.0, -5.0);
  const double kExpectedScale = 1.7;
  double kProjectionNoise = 0.0;
  double kMaxAllowedRotationDifference = 1e-5;
  double kMaxAllowedTranslationDifference = 1e-4;
  double kMaxAllowedScaleDifference = 1e-2;
  TestSimTransformResultWithNoise(axis,
                                  kPoints3D,
                                  kImageOneOrigins,
                                  kImageTwoOrigins,
                                  kProjectionNoise,
                                  kExpectedRotation,
                                  kExpectedTranslation,
                                  kExpectedScale,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  kMaxAllowedScaleDifference);
}

TEST(SimTransformPartialRotationTest, NoRotation) {
  // Sets up some points in the 3D scene
  const Vector3d kPoints3D[5] = { Vector3d(-1.0, 3.0, 3.0),
                                  Vector3d(1.0, -1.0, 2.0),
                                  Vector3d(2.0, 2.0, 5.0),
                                  Vector3d(4.0, 3.0, 5.0) };

  const Vector3d kImageOneOrigins[5] = { Vector3d(-1.0, 0.0, -1.5),
                                         Vector3d(0.0, 0.0, -1.0),
                                         Vector3d(2.0, 0.0, 0.0),
                                         Vector3d(3.0, 0.0, 0.0),
                                         Vector3d(0.0, 0.0, -2.0) };

  const Vector3d kImageTwoOrigins[5] = { Vector3d(0.0, 1.0, 2.0),
                                         Vector3d(0.0, 0.0, 0.0),
                                         Vector3d(0.0, 2.0, 1.0),
                                         Vector3d(0.0, 3.0, 0.0),
                                         Vector3d(0.0, 3.0, 1.0) };

  const Vector3d axis = Vector3d(1.0, 1.0, 1.0).normalized();
  Quaterniond kExpectedRotation(AngleAxisd(DegToRad(0.0), axis));

  const Vector3d kExpectedTranslation(1.0, 1.0, 1.0);
  const double kExpectedScale = 1.3;
  double kProjectionNoise = 0.0;
  double kMaxAllowedRotationDifference = 1e-5;
  double kMaxAllowedTranslationDifference = 1e-4;
  double kMaxAllowedScaleDifference = 1e-4;
  TestSimTransformResultWithNoise(axis,
                                  kPoints3D,
                                  kImageOneOrigins,
                                  kImageTwoOrigins,
                                  kProjectionNoise,
                                  kExpectedRotation,
                                  kExpectedTranslation,
                                  kExpectedScale,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  kMaxAllowedScaleDifference);
}

TEST(SimTransformPartialRotationTest, NoTranslation) {
  // Sets up some points in the 3D scene
  const Vector3d kPoints3D[5] = { Vector3d(-1.0, 3.0, 3.0),
                                  Vector3d(1.0, -1.0, 2.0),
                                  Vector3d(2.0, 2.0, 5.0),
                                  Vector3d(4.0, 3.0, 5.0) };

  const Vector3d kImageOneOrigins[5] = { Vector3d(-1.0, 0.0, -1.5),
                                         Vector3d(0.0, 0.0, -1.0),
                                         Vector3d(2.0, 0.0, 0.0),
                                         Vector3d(3.0, 0.0, 0.0),
                                         Vector3d(0.0, 0.0, -2.0) };

  const Vector3d kImageTwoOrigins[5] = { Vector3d(0.0, 1.0, 2.0),
                                         Vector3d(0.0, 0.0, 0.0),
                                         Vector3d(0.0, 2.0, 1.0),
                                         Vector3d(0.0, 3.0, 0.0),
                                         Vector3d(0.0, 3.0, 1.0) };

  const Vector3d axis = Vector3d(1.0, 1.0, 1.0).normalized();
  Quaterniond kExpectedRotation(AngleAxisd(DegToRad(15.7), axis));

  const Vector3d kExpectedTranslation(0.0, 0.0, 0.0);
  const double kExpectedScale = 1.3;
  double kProjectionNoise = 0.0;
  double kMaxAllowedRotationDifference = 1e-5;
  double kMaxAllowedTranslationDifference = 1e-4;
  double kMaxAllowedScaleDifference = 1e-4;
  TestSimTransformResultWithNoise(axis,
                                  kPoints3D,
                                  kImageOneOrigins,
                                  kImageTwoOrigins,
                                  kProjectionNoise,
                                  kExpectedRotation,
                                  kExpectedTranslation,
                                  kExpectedScale,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  kMaxAllowedScaleDifference);
}

// Tests a variety of axes, angles and translations with added projection noise.
TEST(SimTransformPartialRotationTest, NoiseTest) {
  const Vector3d kPoints3D[5] = { Vector3d(-1.0, 3.0, 3.0),
                                  Vector3d(1.0, -1.0, 2.0),
                                  Vector3d(2.0, 1.0, 3.0),
                                  Vector3d(4.0, 3.0, 5.0) };

  const Vector3d kImageOneOrigins[5] = { Vector3d(-1.0, 0.0, 0.0),
                                         Vector3d(0.0, 0.0, 0.0),
                                         Vector3d(2.0, 0.0, 0.0),
                                         Vector3d(3.0, 0.0, 0.0),
                                         Vector3d(4.0, 0.0, 0.0) };

  const Vector3d kImageTwoOrigins[5] = { Vector3d(0.0, 1.0, 0.0),
                                         Vector3d(0.0, 0.0, 0.0),
                                         Vector3d(0.0, 2.0, 0.0),
                                         Vector3d(0.0, 3.0, 0.0),
                                         Vector3d(0.0, 4.0, 0.0) };

  const Vector3d kAxes[5] = { Vector3d(0.0, 0.0, 1.0).normalized(),
                              Vector3d(1.0, 0.0, 0.0).normalized(),
                              Vector3d(1.0, 0.0, 1.0).normalized(),
                              Vector3d(1.0, 1.0, 0.0).normalized(),
                              Vector3d(1.0, 1.0, 1.0).normalized() };

  const double kAngles[5] = {11.0, 5.0, 2.0, 13.0, 12.0};

  const Vector3d kTranslations[5] = {
    Vector3d(1.0, 1.0, 1.0), Vector3d(4.0, 5.0, 11.0), Vector3d(1.0, 2.0, 15.0),
    Vector3d(6.0, 3.0, 2.0), Vector3d(13.0, 1.0, 15.0)
  };

  const double kScales[5] = { 1.2, 2.9, 10.3, 4.2, 5.3 };

  for (int transform_index = 0;
       transform_index < THEIA_ARRAYSIZE(kAxes);
       ++transform_index) {
    Quaterniond kExpectedRotation(
        AngleAxisd(DegToRad(kAngles[transform_index]), kAxes[transform_index]));

    const double kProjectionNoise = 1.0 / 512;
    const double kMaxAllowedRotationDifference = DegToRad(10.0);
    const double kMaxAllowedTranslationDifference = 3.0;
    const double kMaxAllowedScaleDifference = 0.15;

    TestSimTransformResultWithNoise(kAxes[transform_index],
                                    kPoints3D,
                                    kImageOneOrigins,
                                    kImageTwoOrigins,
                                    kProjectionNoise,
                                    kExpectedRotation,
                                    kTranslations[transform_index],
                                    kScales[transform_index],
                                    kMaxAllowedRotationDifference,
                                    kMaxAllowedTranslationDifference,
                                    kMaxAllowedScaleDifference);
  }
}

// Tests that the solver degrades gracefully when the passed axis is not exactly
// correct.
TEST(SimTransformPartialRotationTest, IncorrectAxisTest) {
  const Vector3d kPoints3D[5] = { Vector3d(-1.0, 3.0, 3.0),
                                  Vector3d(1.0, -1.0, 2.0),
                                  Vector3d(2.0, 1.0, 3.0),
                                  Vector3d(4.0, 3.0, 5.0) };

  const Vector3d kImageOneOrigins[5] = { Vector3d(-1.0, 0.0, 0.0),
                                         Vector3d(0.0, 0.0, 0.0),
                                         Vector3d(2.0, 0.0, 0.0),
                                         Vector3d(3.0, 0.0, 0.0),
                                         Vector3d(4.0, 0.0, 0.0) };

  const Vector3d kImageTwoOrigins[5] = { Vector3d(0.0, 1.0, 0.0),
                                         Vector3d(0.0, 0.0, 0.0),
                                         Vector3d(0.0, 2.0, 0.0),
                                         Vector3d(0.0, 3.0, 0.0),
                                         Vector3d(0.0, 4.0, 0.0) };

  const Vector3d kAxes[5] = {
      Vector3d(0.0, 0.0, 1.0).normalized(),
      Vector3d(1.0, 0.0, 0.0).normalized(),
      Vector3d(1.0, 0.0, 1.0).normalized(),
      Vector3d(1.0, 1.0, 0.0).normalized(),
      Vector3d(1.0, 1.0, 1.0).normalized()
  };

  const double kAngles[5] = {11.0, 5.0, 2.0, 13.0, 12.0};

  const Vector3d kTranslations[5] = {
      Vector3d(1.0, 1.0, 1.0),
      Vector3d(4.0, 5.0, 11.0),
      Vector3d(1.0, 2.0, 15.0),
      Vector3d(6.0, 3.0, 2.0),
      Vector3d(13.0, 1.0, 15.0)
  };
  const double kScales[5] = { 1.2, 2.9, 10.3, 101.2, 54.3 };

  // The axes are perturbed by these rotation to simulate the axis not being
  // perfectly known.
  const Quaterniond kAxisPerturbations[6] = {
    Quaterniond(AngleAxisd(DegToRad(1.0), Vector3d(1.0, 0, 0))),
    Quaterniond(AngleAxisd(DegToRad(-1.0), Vector3d(1.0, 0, 0))),
    Quaterniond(AngleAxisd(DegToRad(1.0), Vector3d(0.0, 1.0, 0))),
    Quaterniond(AngleAxisd(DegToRad(-1.0), Vector3d(0.0, 1.0, 0))),
    Quaterniond(AngleAxisd(DegToRad(1.0), Vector3d(0.0, 0.0, 1.0))),
    Quaterniond(AngleAxisd(DegToRad(-1.0), Vector3d(0.0, 0.0, 1.0)))
  };

  for (int transform_index = 0;
       transform_index < THEIA_ARRAYSIZE(kAxes);
       ++transform_index) {
    for (int axis_rotation_index = 0;
         axis_rotation_index < THEIA_ARRAYSIZE(kAxisPerturbations);
         ++axis_rotation_index) {
      // Perturbs the axis by the axis perturbation.
      const Vector3d perturbed_axis =
          kAxisPerturbations[axis_rotation_index] * kAxes[transform_index];
      // Uses the perturbed rotation as the expected rotation, but the original
      // axis is passed as the axis.
      Quaterniond kExpectedRotation(
          AngleAxisd(DegToRad(kAngles[transform_index]), perturbed_axis));

      // Tests the ThreePointEssentialMatrix function with fairly large
      // thresholds on the allowed translation and rotation differences.
      const double kProjectionNoise = 0.0;
      const double kMaxAllowedRotationDifference = DegToRad(2.0);
      const double kMaxAllowedTranslationDifference = 2.0;
      const double kMaxAllowedScaleDifference = 1e-1;

      TestSimTransformResultWithNoise(perturbed_axis,
                                      kPoints3D,
                                      kImageOneOrigins,
                                      kImageTwoOrigins,
                                      kProjectionNoise,
                                      kExpectedRotation,
                                      kTranslations[transform_index],
                                      kScales[transform_index],
                                      kMaxAllowedRotationDifference,
                                      kMaxAllowedTranslationDifference,
                                      kMaxAllowedScaleDifference);
    }
  }
}

}  // namespace
}  // namespace theia


//
// Created by yalavrinenko on 26.09.2021.
//
#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <opencv2/opencv.hpp>
#include "../src/pathfinders/legacy/asearch.hpp"
#include "../src/pathfinders/boost_graph/full_linked_graph.hpp"
#include <boost/format.hpp>
#include <iostream>

namespace std {
  ostream &operator<<(ostream &os, const pair<int, int> &yt) {
    os << (boost::format("(%d, %d)") % yt.first % yt.second);
    return os;
  }
}

BOOST_AUTO_TEST_SUITE(AstarTest)

BOOST_AUTO_TEST_CASE(SimpleGraphLoop, * boost::unit_test::tolerance(0.00001)){

  char data[] = {
      1, 1, 1, 1, 1,
      1, 1, 1, 1, 1,
      1, 1, 1, 1, 1,
      1, 1, 1, 1, 1,
      1, 1, 1, 1, 1
  };
  cv::Mat map {5, 5, CV_8UC1, data};

  trs::full_linked_grid graph{map};
  auto visitor = trs::full_linked_grid::visitor({4, 4});

  auto path = trs::astar_search<trs::full_linked_grid>::find_path(graph, {2, 2}, {4, 4}, visitor);
  BOOST_TEST(path.first == std::sqrt(8.0) );
  BOOST_TEST(path.second.size() == 3);

  std::vector<std::pair<int, int>> eta {{4, 4}, {3, 3}, {2, 2}};
  BOOST_TEST(path.second == eta, boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(LinearGraph, * boost::unit_test::tolerance(0.00001)){

  char data[] = {
      1, 1, 1, 1, 1
  };
  cv::Mat map {1, 5, CV_8UC1, data};

  trs::full_linked_grid graph{map};
  trs::full_linked_grid::vertex_t begin{0, 0}, end{0, 4};
  auto visitor = trs::full_linked_grid::visitor(end);

  auto path = trs::astar_search<trs::full_linked_grid>::find_path(graph, begin, end, visitor);
  BOOST_TEST(path.first == 4 );
  BOOST_TEST(path.second.size() == 5);

  std::vector<std::pair<int, int>> eta {{0, 4}, {0, 3}, {0, 2}, {0, 1}, {0, 0}};
  BOOST_TEST(path.second == eta, boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(VerticalGraph, * boost::unit_test::tolerance(0.00001)){

  char data[] = {
      1,
      1,
      1,
      1,
      1
  };
  cv::Mat map {5, 1, CV_8UC1, data};

  trs::full_linked_grid graph{map};
  trs::full_linked_grid::vertex_t begin{4, 0}, end{0, 0};
  auto visitor = trs::full_linked_grid::visitor(end);

  auto path = trs::astar_search<trs::full_linked_grid>::find_path(graph, begin, end, visitor);
  BOOST_TEST(path.first == 4 );
  BOOST_TEST(path.second.size() == 5);

  std::vector<std::pair<int, int>> eta {{0, 0}, {1, 0}, {2, 0}, {3, 0}, {4, 0}};
  BOOST_TEST(path.second == eta, boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(NoPathGraph, * boost::unit_test::tolerance(0.00001)){

  char data[] = {
      1, 1, 1, 1,
      1, 1, 0, 0,
      1, 1, 0, 0,
  };
  cv::Mat map {3, 4, CV_8UC1, data};

  trs::full_linked_grid graph{map};
  trs::full_linked_grid::vertex_t begin{0, 0}, end{02, 3};
  auto visitor = trs::full_linked_grid::visitor(end);

  auto path = trs::astar_search<trs::full_linked_grid>::find_path(graph, begin, end, visitor);
  BOOST_TEST(path.first == -1.0 );
}

BOOST_AUTO_TEST_CASE(HardPathGraph, * boost::unit_test::tolerance(0.00001)){

  char data[] = {
      1, 0, 1, 1, 1,
      1, 1, 1, 1, 0,
      0, 0, 1, 0, 0,
      1, 1, 0, 0, 0,
      1, 1, 1, 1, 1
  };
  cv::Mat map {5, 5, CV_8UC1, data};

  trs::full_linked_grid graph{map};
  trs::full_linked_grid::vertex_t begin{0, 0}, end{4, 4};
  auto visitor = trs::full_linked_grid::visitor(end);

  auto path = trs::astar_search<trs::full_linked_grid>::find_path(graph, begin, end, visitor);
  auto s2 = std::sqrt(2.0);
  BOOST_TEST(path.first == (s2 + s2 + s2 + s2 + 1 + 1) );
  BOOST_TEST(path.second.size() == 7);

  std::vector<std::pair<int, int>> eta {
      {4, 4}, {4, 3}, {4, 2},
      {3, 1},
      {2, 2},
      {1, 1},
      {0, 0}
  };
  BOOST_TEST(path.second == eta, boost::test_tools::per_element());
}

BOOST_AUTO_TEST_SUITE_END()


#include "norm_damping.hpp"

#include <string>
#include <armadillo>
#include <yaml-cpp/yaml.h>

NormDamping::NormDamping(const std::string &filenm):
  Regularization(filenm)
{
  YAML::Node config = YAML::LoadFile(filenm);
  output_file_ = config["file_norm"].as<std::string>();

  this->load();
}

void NormDamping::save()
{
  Regularization::save(output_file_);
}

void NormDamping::cal_fitness()
{
  fitness_ = arma::accu(arma::square(umodel_-rmodel_));
}

void NormDamping::cal_gradient()
{
  gradient_ = 2*(umodel_ - rmodel_);
}