/**
 * @file
 * $Revision: 1.1 $
 * $Date: 2009/09/09 23:15:18 $
 *
 *   Unless noted otherwise, the portions of Isis written by the USGS are
 *   public domain. See individual third-party library and package descriptions
 *   for intellectual property information, user agreements, and related
 *   information.
 *
 *   Although Isis has been used by the USGS, no warranty, expressed or
 *   implied, is made by the USGS as to the accuracy and functioning of such
 *   software and related material nor shall the fact of distribution
 *   constitute any such warranty, and no responsibility is assumed by the
 *   USGS in connection therewith.
 *
 *   For additional information, launch
 *   $ISISROOT/doc//documents/Disclaimers/Disclaimers.html
 *   in a browser or see the Privacy & Disclaimers page on the Isis website,
 *   http://isis.astrogeology.usgs.gov, and the USGS privacy and disclaimers on
 *   http://www.usgs.gov/privacy.html.
 */
#include "LinearAlgebra.h"

// std library
#include <cmath>

// Qt Library
#include <QDebug>
#include <QtDebug>
#include <QtGlobal>

// boost library
#include <boost/assign/std/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

// other Isis library
#include "Angle.h"
#include "Constants.h"
#include "IException.h"
#include "IString.h"

namespace Isis {
  /**
   * Default constructor for a LinearAlgebra object. This is protected so it can
   * never be constructed. The static methods and typedefs in this class are
   * meant to be used without a LinearAlgebra object.
   */
  LinearAlgebra::LinearAlgebra() {
  }

  /**
   * Destructor for a LinearAlgebra object.
   */
  LinearAlgebra::~LinearAlgebra() {
  }

  /**
   * Determines whether the given matrix is the identity. Non-square matrices
   * always return false. A qfuzzycompare is used when checking the zeroes and
   * ones.
   *
   * @param matrix The matrix to check. Automatically returns false if not
   *               square.
   *
   * @return @b bool Returns true if the matrix is a square identity matrix
   *                 (i.e. there are ones down the diagonal and zeroes elsewhere).
   */
  bool LinearAlgebra::isIdentity(const Matrix &matrix) {
    if (matrix.size1() != matrix.size2()) return false;

    for (unsigned int row = 0; row < matrix.size1(); row++) {
      for (unsigned int col = 0; col < matrix.size2(); col++) {
        if (row == col) {
          if (!qFuzzyCompare(matrix(row, col), 1.0)) return false;
        }
        else {
          // When using qFuzzy compare against a number that is zero
          // you must offset the number per Qt documentation
          if (!qFuzzyCompare(1.0 + matrix(row, col), 1.0)) return false;
        }
      }
    }
    return true;
  }

  /**
   * Determines whether the given matrix is orthogonal by verifying
   * that the matrix and its tranpose are inverses.
   *
   * @param matrix The matrix to check. Automatically returns false
   *               if not square.
   *
   * @return @b bool Returns true if the matrix is orthogonal.
   */
  bool LinearAlgebra::isOrthogonal(const Matrix &matrix) {
    if (matrix.size1() != matrix.size2()) return false;
    LinearAlgebra::Matrix test = multiply(matrix, LinearAlgebra::transpose(matrix));
    if (isIdentity(test)) {
      // no need to test that transpose is both left and right inverse since the matrix is square
      return true;
    }
    return false;
  }

  /**
   * Determines whether the given matrix is a rotation matrix.
   *
   * @param matrix The matrix to check. Automatically returns false if not 2x2
   *               or 3x3.
   *
   * @return @b bool Returns true if the given matrix represents a rotation.
   *
   * @throw IException::Programmer "Unable to determine whether
   *                                the given matrix is a rotation matrix."
   *
   */
  // derived from naif's isrot routine
  bool LinearAlgebra::isRotationMatrix(const Matrix &matrix) {
    // rotation matrices must be square
    if (matrix.size1() != matrix.size2()) return false;

    /* An exerpt from NAIF's isrot() routine...
     *
     * A property of rotation matrices is that their columns form a
     * right-handed, orthonormal basis in 3-dimensional space.  The
     * converse is true:  all 3x3 matrices with this property are
     * rotation matrices.
     *
     * An ordered set of three vectors V1, V2, V3 forms a right-handed,
     * orthonormal basis if and only if
     *
     *    1)   || V1 ||  =  || V2 ||  =  || V3 ||  =  1
     *
     *    2)   V3 = V1 x V2.  Since V1, V2, and V3 are unit vectors,
     *         we also have
     *
     *         < V3, V1 x V2 > = 1.
     *
     *         This quantity is the determinant of the matrix whose
     *         colums are V1, V2 and V3.
     *
     * When finite precision numbers are used, rotation matrices will
     * usually fail to satisfy these criteria exactly.  We must use
     * criteria that indicate approximate conformance to the criteria
     * listed above.  We choose
     *
     *    1)   |   || Vi ||  -  1   |   <   NTOL,  i = 1, 2, 3.
     *                                  -
     *
     *    2)   Let
     *
     *                   Vi
     *            Ui = ------ ,   i = 1, 2, 3.
     *                 ||Vi||
     *
     *         Then we require
     *
     *            | < U3, U1 x U2 > - 1 |  <  DTOL;
     *                                     -
     *
     *         equivalently, letting U be the matrix whose columns
     *         are U1, U2, and U3, we insist on
     *
     *            | det(U) - 1 |  <  DTOL.
     *                            _
     * The columns of M must resemble unit vectors.  If the norms are
     * outside of the allowed range, M is not a rotation matrix.
     *
     * Also, the columns of M are required to be pretty nearly orthogonal.  The
     * discrepancy is gauged by taking the determinant of the matrix UNIT,
     * computed below, whose columns are the unitized columns of M.
     */

    // create a matrix whose columns are unit vectors of the columns of the
    // given matrix
    Matrix unitMatrix(matrix.size1(), matrix.size2());

    for (unsigned int i = 0; i < unitMatrix.size1(); i++) {
      // get the ith column vector from the given matrix
      Vector columnVector = column(matrix, i);
      // get the magnitude and if it is not near 1, this is not a rotation matrix
      double columnMagnitude = magnitude(columnVector);
      if ( columnMagnitude < 0.9 || columnMagnitude >  1.1 ) {
        return false;
      }

      // put the unitized row into the unitized matrix
      setColumn(unitMatrix, columnVector / columnMagnitude, i);

    }

    try {
      // get the determinant of the unitized matrix and if it is not near 1,
      // this is not a rotation matrix
      double det = determinant(unitMatrix);
      if ( det >= 0.9 && det <=  1.1 ) {
        return true;
      }
    }
    catch (IException &e) {
      // since we can only calculate the determinant for 2x2 or 3x3
      QString msg = "Unable to determine whether the given matrix is a rotation matrix.";
      throw IException(e, IException::Programmer, msg, _FILEINFO_);
    }
    return false;
  }

  /**
   * Determines whether the given matrix is filled with zereos.
   *
   * @param vector The matrix to check.
   *
   * @return @b bool Returns true if the matrix is all zereos.
   */
  bool LinearAlgebra::isZero(const Matrix &matrix) {
    for (unsigned int i = 0; i < matrix.size1(); i++) {
      for (unsigned int j = 0; j < matrix.size2(); j++) {
        // When using qFuzzy compare against a number that is zero
        // you must offset the number per Qt documentation
        if (!qFuzzyCompare(matrix(i, j) + 1.0, 1.0)) return false;
      }
    }

    return true;
  }

  /**
   * Determines whether the given vector is filled with zereos.
   *
   * @param vector The vector to check.
   *
   * @return @b bool Returns true if the vector is all zereos.
   */
  bool LinearAlgebra::isZero(const Vector &vector) {
    for (unsigned int i = 0; i < vector.size(); i++) {
      // When using qFuzzy compare against a number that is zero
      // you must offset the number per Qt documentation
      if (!qFuzzyCompare(vector(i) + 1.0, 1.0)) return false;
    }

    return true;
  }

  /**
   * Determines whether the given vector is empty (i.e. size 0).
   *
   * @param vector The vector to check.
   *
   * @return @b bool Returns true if the size of the given vector is zero.
   *
   */
  bool LinearAlgebra::isEmpty(const LinearAlgebra::Vector &vector) {
    return vector.empty();
  }

  /**
   * Determines whether the given vector is a unit vector.
   *
   * @param vector The vector to check.
   *
   * @return @b bool Returns true if the vector is a unit vector.
   */
  bool LinearAlgebra::isUnit(const Vector &vector) {
    if (qFuzzyCompare(dotProduct(vector, vector), 1.0)) {
      return true;
    }
    return false;
  }

  /**
   * Returns the inverse of a 2x2 or 3x3 matrix. Throws an error if the given
   * matrix is not invertible.
   *
   * @param matrix The matrix to inverse.
   *
   * @return @b LinearAlgebra::Matrix The inverse matrix.
   *
   * @throw IException::Programmer "The given matrix is not invertible.
   *                                The determinant is 0.0."
   * @throw IException::Programmer "Unable to invert the given matrix."
   */
  LinearAlgebra::Matrix LinearAlgebra::inverse(const Matrix &matrix) {
    try {
      if (isOrthogonal(matrix)) {
        return transpose(matrix);
      }

      // the determinant method will verify the size of the matrix is 2x2 or 3x3
      double det = determinant(matrix);

      if (qFuzzyCompare(det + 1.0, 1.0)) {
        // the inverse exists <==> the determinant is not 0.0
        QString msg = "The given matrix is not invertible. The determinant is 0.0.";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }

      // since the determinant is not zero, we can calculate the reciprocal
      double scale = 1 / det;

      LinearAlgebra::Matrix inverse = identity(matrix.size1());
      // find the inverse for 2x2
      if (matrix.size1() == 2) {
        inverse(0,  0) =  scale * matrix(1, 1);
        inverse(0,  1) = -scale * matrix(0, 1);

        inverse(1,  0) = -scale * matrix(1, 0);
        inverse(1,  1) =  scale * matrix(0, 0);
        return inverse;
      }
      // else we have a 3x3
      inverse(0,  0) = scale * ( matrix(1, 1) * matrix(2, 2) - matrix(2, 1) * matrix(1, 2) );
      inverse(0,  1) = scale * ( matrix(0, 2) * matrix(2, 1) - matrix(2, 2) * matrix(0, 1) );
      inverse(0,  2) = scale * ( matrix(0, 1) * matrix(1, 2) - matrix(1, 1) * matrix(0, 2) );

      inverse(1,  0) = scale * ( matrix(1, 2) * matrix(2, 0) - matrix(2, 2) * matrix(1, 0) );
      inverse(1,  1) = scale * ( matrix(0, 0) * matrix(2, 2) - matrix(2, 0) * matrix(0, 2) );
      inverse(1,  2) = scale * ( matrix(0, 2) * matrix(0, 1) - matrix(1, 2) * matrix(0, 0) );

      inverse(2,  0) = scale * ( matrix(1, 0) * matrix(2, 1) - matrix(2, 0) * matrix(1, 1) );
      inverse(2,  1) = scale * ( matrix(0, 1) * matrix(2, 0) - matrix(2, 1) * matrix(0, 0) );
      inverse(2,  2) = scale * ( matrix(0, 0) * matrix(1, 1) - matrix(1, 0) * matrix(0, 1) );

      return inverse;
    }
    catch (IException &e) {
      QString msg = "Unable to invert the given matrix.";
      throw IException(e, IException::Programmer, msg, _FILEINFO_);
    }
  }

  /**
   * Returns the transpose of the given matrix.
   *
   * @param matrix The matrix to transpose.
   *
   * @return @b LinearAlgebra::Matrix The transposed matrix.
   */
  LinearAlgebra::Matrix LinearAlgebra::transpose(const Matrix &matrix) {
    // no error testing required so call the boost transpose function
    return boost::numeric::ublas::trans(matrix);
  }

  /**
   * Returns the identity matrix of size NxN.
   *
   * @param int The size of the square matrix.
   *
   * @return @b LinearAlgebra::Matrix The NxN square matrix.
   *
   * @throw IException::Programmer "Can not create identity matrix of negative size."
   */
  LinearAlgebra::Matrix LinearAlgebra::identity(int size) {
    if (size < 1) {
      QString msg = "Can not create identity matrix of negative size ["
                    + toString((int) size) + "].";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    LinearAlgebra::Matrix m(size, size, 0.0);
    for (int i = 0; i < size; i++) {
      m(i, i) = 1.0;
    }

    return m;
  }

  /**
   * Returns a matrix with given dimensions that is filled with zeroes.
   *
   * @param rows The number of rows in the returned matrix.
   * @param columns The number of colums in the returned matrix.
   *
   * @return @b LinearAlgebra::Matrix A zero-filled matrix.
   */
  LinearAlgebra::Matrix LinearAlgebra::zeroMatrix(int rows, int columns) {
    boost::numeric::ublas::zero_matrix<double> m(rows, columns);
    return m;
  }

  /**
   * Returns a vector of given length that is filled with zeroes.
   *
   * @param size Size of the vector.
   *
   * @return @b LinearAlgebra::Vector A zero-filled vector.
   */
  LinearAlgebra::Vector LinearAlgebra::zeroVector(int size) {
    boost::numeric::ublas::zero_vector<double> v(size);
    return v;
  }

  /**
   * Returns the determinant of the given 3x3 matrix.
   *
   * @param LinearAlgebra::Matrix The 3x3 matrix whose determinant will be
   *                     calculated.
   *
   * @return @b double The determinant of the given matrix.
   *
   * @throw IException::Programmer "Unable to calculate the determinant for the
   *                                given matrix. This method only calculates the
   *                                determinant for 2x2 or 3x3 matrices."
   *
   */
  double LinearAlgebra::determinant(const Matrix &matrix) {
    if ( (matrix.size1() != matrix.size2()) || (matrix.size1() != 2 &&  matrix.size1() != 3) ) {
      QString msg = "Unable to calculate the determinant for the given matrix. "
                    "This method only calculates the determinant for 2x2 or 3x3 matrices."
                    "The given matrix is [" + toString((int) matrix.size1()) + "x"
                    + toString((int) matrix.size2()) + "].";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    if (matrix.size1() == 2) {
      return matrix(0, 0) * matrix(1, 1) - matrix(1, 0) * matrix(0, 1);
    }
    else {

      return matrix(0, 0) * (matrix(1, 1)*matrix(2, 2) - matrix(1, 2)*matrix(2, 1))
           - matrix(0, 1) * (matrix(1, 0)*matrix(2, 2) - matrix(1, 2)*matrix(2, 0))
           + matrix(0, 2) * (matrix(1, 0)*matrix(2, 1) - matrix(1, 1)*matrix(2, 0));
    }

  }

  /**
   * Returns a unit vector that is codirectional with the given
   * vector by dividing each component of the vector by the vector magnitude.
   *
   * @f$ \hat{v} = \frac{v}{\| v \|} @f$
   * where @f$ \| u \|_ @f$ is the magnitude of u.
   *
   * @see magnitude()
   *
   * @param vector The vector to be normalized.
   *
   * @return @b LinearAlgebra::Vector A unit vector from the given vector.
   *
   * @throw IException::Programmer "Unable to normalize the zero vector."
   */
  LinearAlgebra::Vector LinearAlgebra::normalize(const Vector &vector) {
    // impossible to unitize the zero vector
    if (LinearAlgebra::isZero(vector)) {
      QString msg = "Unable to normalize the zero vector.";
      throw IException(IException::Unknown, msg, _FILEINFO_);
    }
    return  vector / LinearAlgebra::magnitude(vector);
  }

  /**
   * Computes the magnitude (i.e., the length) of the given
   * vector using the Euclidean norm (L2 norm).  The maximum norm
   * (L-infinity) is also used to stabilize the solution in order to avoid
   * overflow. This method uses the computation
   *
   * @f$ \| v \| = \| v \|_\infty \| \frac{v}{\| v \|_\infty} \|_2 @f$
   * where @f$ \| u \|_\infty = \max_{i=1}^n \{ \mid u_i \mid \}  @f$ is the maximum norm
   * and @f$ \| u \|_2 = \sqrt{\sum_{i=1}^n u_1^2} @f$ is the Euclidean norm.
   *
   *
   * @see absoluteMaximum()
   *
   * @param vector The vector whose magnitude will be computed.
   *
   * @return @b double The magnitude (length) of the given vector.
   */
  double LinearAlgebra::magnitude(const Vector &vector) {
    double magnitude;
    // avoid dividing by maxNorm = 0
    // no stabilization is needed for the zero vector, magnitude is always 0.0
    if (LinearAlgebra::isZero(vector)) {
      magnitude = 0.0;
    }
    else {
      double maxNorm = LinearAlgebra::absoluteMaximum(vector);
      magnitude = maxNorm * boost::numeric::ublas::norm_2(vector / maxNorm);
    }
    return magnitude;
  }

  /**
   * Returns the maximum norm (L-infinity norm) for the given vector.
   *
   * The maximum norm is defined by the absolute values of the vector
   * components:
   *
   * @f$ \| v \|_\infty = \max_{i=1}^n \{ \mid v_i \mid \} @f$
   *
   * @param vector The vector whose absolute maximum will be returned.
   *
   * @return @b double The maximum of the absolute values of the vector components.
   */
  double LinearAlgebra::absoluteMaximum(const Vector &vector) {
    return boost::numeric::ublas::norm_inf(vector);
  }

  /**
   * Returns the product of two matrices. Will throw an error if the matrices
   * are not properly sized (the number of columns of the first matrix must
   * match the number or rows of the second matrix).
   *
   * @param matrix1 The left matrix.
   * @param matrix2 The right matrix.
   *
   * @return @b LinearAlgebra::Matrix The resultant matrix product.
   *
   * @throw IException::Programmer "Unable to multiply matrices
   *                                with mismatched dimensions."
   */
  LinearAlgebra::Matrix LinearAlgebra::multiply(const Matrix &matrix1, const Matrix &matrix2) {
    // Check to make sure we can multiply
    if (matrix1.size2() != matrix2.size1()) {
      QString msg = "Unable to multiply matrices with mismatched dimensions. "
                    "The left matrix has [" + toString((int) matrix1.size2())
                    + "] columns and the right matrix has [" + toString((int) matrix2.size1())
                    + "] rows.";

      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // Use boost product routine
    return boost::numeric::ublas::prod(matrix1, matrix2);
  }

  /**
   * Computes the product of the given matrix and vector. The vector will
   * be multiplied on the right side of the given matrix. Will throw an
   * error if the two are not properly sized (matrix columns not equal to
   * vector size).
   *
   * @param matrix The matrix to be multiplied.
   * @param vector The column vector to be multiplied on the right side of
   *               the matrix.
   *
   * @return @b LinearAlgebra::Vector The resultant vector.
   *
   * @throw IException::Programmer "Unable to multiply matrix and vector
   *                                with mismatched dimensions."
   */
  LinearAlgebra::Vector LinearAlgebra::multiply(const Matrix &matrix, const Vector &vector) {
    // Check to make sure we can multiply
    if (matrix.size2() != vector.size()) {
      QString msg = "Unable to multiply matrix and vector with mismatched dimensions."
                    "The given vector has [" + toString((int) vector.size())
                    + "] components and the given matrix has ["
                    + toString((int) matrix.size2()) + "] columns.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // Use boost product routine
    return boost::numeric::ublas::prod(matrix, vector);
  }

  /**
   * Multiplies the given scalar and vector.
   *
   * @param scalar The scalar to be multiplied by each component of the vector.
   * @param vector The vector to be scaled.
   *
   * @return @b LinearAlgebra::Vector The resultant scaled vector.
   */
  LinearAlgebra::Vector LinearAlgebra::multiply(double scalar, const Vector &vector) {
    // no error checks necessary (use the boost operator* method)
    return scalar * vector;
  }

  /**
   * Multiplies the given scalar and matrix.
   *
   * @param scalar The scalar to be multiplied by each element of the matrix.
   * @param matrix The matrix to be scaled.
   *
   * @return @b LinearAlgebra::Matrix The resultant scaled matrix.
   */
  LinearAlgebra::Matrix LinearAlgebra::multiply(double scalar, const Matrix &matrix) {
    // no error checks necessary
    return scalar * matrix;
  }

  /**
   * Adds the two given vectors.
   *
   * @param vector1 The first vector.
   * @param vector2 The second vector.
   *
   * @return @b LinearAlgebra::Vector The sum of the vectors.
   *
   * @throw IException::Programmer "Unable to add vectors
   *                                with mismatched sizes."
   */
  LinearAlgebra::Vector LinearAlgebra::add(const Vector &vector1, const Vector &vector2) {
    // Vectors best be the same size
    if (vector1.size() != vector2.size()) {
      QString msg = "Unable to add vectors with mismatched sizes."
                    "Vector1 has [" + toString((int) vector1.size())
                    + "] components and vector2 has ["
                    + toString((int) vector2.size()) + "] components.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    return vector1 + vector2;
  }

  /**
   * Subtracts the right vector from the left vector.
   *
   * @param vector1 The vector to the left of the subtraction operator.
   * @param vector2 The vector to the right of the subtraction operator.
   *
   * @return @b LinearAlgebra::Vector The difference of the vectors
   *                                  (i.e. vector1 - vector2).
   *
   * @throw IException::Programmer "Unable to subtract vectors
   *                                with mismatched sizes."
   */
  LinearAlgebra::Vector LinearAlgebra::subtract(const Vector &vector1, const Vector &vector2) {
    // Vectors best be the same size
    if (vector1.size() != vector2.size()) {
      QString msg = "Unable to subtract vectors with mismatched sizes."
                    "Vector1 has [" + toString((int) vector1.size())
                    + "] components and vector2 has ["
                    + toString((int) vector2.size()) + "] components.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    return vector1 - vector2;
  }

  /**
   * Returns the cross product of two vectors. Note: the cross-product
   * requires the vectors to have exactly three components each.
   *
   * @param vector1 The vector to the left of the cross product operator.
   * @param vector2 The vector to the right of the cross product operator.
   *
   * @return @b LinearAlgebra::Vector The cross product of the given
   *                                  vectors (i.e. vector1 x vector2).
   *
   * @throw IException::Programmer "Unable to calculate the cross
   *                                product on vectors that are not
   *                                size 3. "
   */
  LinearAlgebra::Vector LinearAlgebra::crossProduct(const Vector &vector1, const Vector &vector2) {
    if ((vector1.size() != 3) || (vector2.size() != 3)) {
      QString msg = "Unable to calculate the cross product on vectors that are not size 3. "
                    "Vector1 has [" + toString((int) vector1.size())
                    + "] components and vector2 has ["
                    + toString((int) vector2.size()) + "] components.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    Vector crossProd(3);
    crossProd(0) = vector1(1)*vector2(2) - vector1(2)*vector2(1);
    crossProd(1) = vector1(2)*vector2(0) - vector1(0)*vector2(2);
    crossProd(2) = vector1(0)*vector2(1) - vector1(1)*vector2(0);

    return crossProd;
  }

  /**
   * Divides each vector by its corresponding absolute maximum,
   * computes the cross product of the new vectors, and normalizes the
   * resultant vector from the cross product. Note: the
   * cross-product requires the vectors to have exactly three
   * components each.
   *
   * @param vector1 The vector to the left of the cross product operator.
   * @param vector2 The vector to the right of the cross product operator.
   *
   * @return @b LinearAlgebra::Vector The normalized cross product of the given vectors
   *                                  (i.e. normalize(vector1/absoluteMaximum(vector1) x
   *                                   vector2/absoluteMaximum(vector2))).
   */
  // is this derived from naif's ucrss routine???
  LinearAlgebra::Vector LinearAlgebra::normalizedCrossProduct(const Vector &vector1,
                                                              const Vector &vector2) {

    double maxVector1 = LinearAlgebra::absoluteMaximum(vector1);
    double maxVector2 = LinearAlgebra::absoluteMaximum(vector2);

    LinearAlgebra::Vector normalizedVector1 = vector1;
    LinearAlgebra::Vector normalizedVector2 = vector2;

    if (maxVector1 != 0.0) {
      normalizedVector1 /= maxVector1;
    }

    if (maxVector2 != 0.0) {
      normalizedVector2 /= maxVector2;
    }

    LinearAlgebra::Vector vcross = crossProduct(normalizedVector1, normalizedVector2);

    return normalize(vcross);
  }

  /**
   * Computes the outer product of the given vectors. The outer product
   * operation is defined as the cross product of vector1 and the conjugate
   * transpose of vector2.
   *
   * @param vector1 The vector to the left side of the outer product operator.
   * @param vector2 The vector to the right side of the outer product operator.
   *
   * @return @b LinearAlgebra::Matrix The outer product matrix of the given vectors.
   *
   * @throw IException::Programmer "Unable to compute the outer product for
   *                                vectors with mismatched sizes."
   */
  LinearAlgebra::Matrix LinearAlgebra::outerProduct(const Vector &vector1, const Vector &vector2) {
    if (vector1.size() != vector2.size()) {
      QString msg = "Unable to compute the outer product for vectors with mismatched sizes."
                    "Vector1 has [" + toString((int) vector1.size())
                    + "] components and vector2 has ["
                    + toString((int) vector2.size()) + "] components.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return boost::numeric::ublas::outer_prod(vector1, vector2);
  }

  /**
   * Computes the dot product of the given vectors. For Euclidean space, this is the same
   * as the inner product, so these methods are interchangeable.
   *
   * @see innerProduct()
   *
   * @param vector1 The first vector.
   * @param vector2 The second vector.
   *
   * @return @b double The dot product of the vectors.
   *
   */
  double LinearAlgebra::dotProduct(const Vector &vector1, const Vector &vector2) {
    // no error check needed - this is done by innerProduct
    return LinearAlgebra::innerProduct(vector1, vector2);
  }

  /**
   * Computes the inner product of the given vectors. For Euclidean space, this is the same
   * as the dot product, so these methods are interchangeable.
   *
   * @see dotProduct()
   *
   * @param vector1 The first vector.
   * @param vector2 The second vector.
   *
   * @return @b double The inner product of the vectors.
   *
   * @throw IException::Programmer "Unable to compute the dot product for vectors
   *                                with mismatched sizes."
   */
  double LinearAlgebra::innerProduct(const Vector &vector1, const Vector &vector2) {
    if (vector1.size() != vector2.size()) {
      QString msg = "Unable to compute the dot product for vectors with mismatched sizes."
                    "Vector1 has [" + toString((int) vector1.size())
                    + "] components and vector2 has ["
                    + toString((int) vector2.size()) + "] components.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return boost::numeric::ublas::inner_prod(vector1, vector2);
  }

  /**
   * Compute the vector projection of vector1 onto vector2. This is
   * the orthogonal projection of vector1 onto a line that is parallel
   * to vector2.
   *
   * It is defined by
   * @f$ proj_{v_2} v_1 = \frac{v_1 \cdot v_2}{\| v_2 \|^2}v_2 @f$
   * where @f$ \| u \|_ @f$ is the magnitude of u.
   *
   * @param vector1 The vector to the left of the project operator.
   * @param vector2 The vector to the right of the project operator.
   *
   * @return @b LinearAlgebra::Vector The resultant vector that is
   *                                  the orthogonal projection of
   *                                  vector1 onto vector2.
   *
   * @throw IException::Programmer "Unable to project vector1 onto vector2
   *                                with mismatched sizes."
   */
  // derived from naif's vproj routine
  LinearAlgebra::Vector LinearAlgebra::project(const Vector &vector1, const Vector &vector2) {
    if (vector1.size() != vector2.size()) {
      QString msg = "Unable to project vector1 onto vector2 with mismatched sizes."
                    "Vector1 has [" + toString((int) vector1.size())
                    + "] components and vector2 has ["
                    + toString((int) vector2.size()) + "] components.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // if vector2 is the zero vector, then the projection of vector1 onto vector2
    // is also the zero vector
    if (LinearAlgebra::isZero(vector2)) return vector2;

    // find the dot product of the two vectors
    double v1Dotv2 = LinearAlgebra::dotProduct(vector1, vector2);

    // find the square of the length of vector2 (this is the dot product of the vector2 with itself)
    double v2Dotv2 = LinearAlgebra::dotProduct(vector2, vector2);

    return v1Dotv2/v2Dotv2 * vector2;
  }

  /**
   * Rotates a vector about an axis vector given a specified angle.
   * Note: this method only rotates a vector with three components since
   * the cross product calculation requires this.
   *
   * @param vector The vector to rotate, which must have three components.
   * @param axis  A vector defining the axis, which must also have three components.
   * @param angle The angle to rotate.
   *
   * @return @b LinearAlgebra::Vector The rotated vector.
   *
   * @throw IException::Programmer "Unable to rotate vector about the
   *                                given axis and angle. Vectors must
   *                                be of size 3 to perform rotation. "
   */
  // derived from naif's vrotv routine
  LinearAlgebra::Vector LinearAlgebra::rotate(const Vector &vector, const Vector &axis,
                                              Angle angle) {
    if ((vector.size() != 3) || (axis.size() != 3)) {
      QString msg = "Unable to rotate vector about the given axis and angle. "
                    "Vectors must be of size 3 to perform rotation. "
                    "The given vector has [" + toString((int) vector.size())
                    + "] components and the given axis has ["
                    + toString((int) axis.size()) + "] components.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // if the given vector is the zero vector, then the rotation about the vector
    // is also the zero vector
    if (isZero(axis)) return vector;

    // Compute the unit vector that is codirectional with the given axis
    Vector axisUnitVector = normalize(axis);

    // Compute the projection of the given vector onto the axis unit vector.
    Vector projVectorOnAxis = project(vector,  axisUnitVector);

    // Compute the component of the input orthogonal to the AXIS.  Call it V1.
    Vector v1 = vector - projVectorOnAxis;

    // Rotate V1 by 90 degrees about the AXIS and call the result V2.
    Vector v2 = LinearAlgebra::crossProduct(axisUnitVector, v1);

    // Compute cos(angle)*v1 + sin(angle)*v2. This is v1 rotated about
    // the axis in the plane normal to the axis, call the result rplane
    double c = cos(angle.radians());
    double s = sin(angle.radians());
    Vector rplane = (c*v1) + (s*v2);

    // Add the rotated component in the normal plane to axis to the
    // projection of v onto axis to obtain rotation.
    return (rplane + projVectorOnAxis);
  }

 /**
  * Finds the unique vector P such that A = V + P, V is
  * parallel to B and P is perpendicular to B, where A is the first vector
  * (vector1) and B is the second vector (vector2) passed in by the user.
  *
  * For all vectors A and B, there exists unique vectors V and P such that
  * 1) A = V + P
  * 2) V is parallel to B
  * 3) P is perpendicular to B
  *
  * @param vector1 The first vector, denoted A in the description.
  * @param vector2 The second vector, denoted B in the description.
  *
  * @return @b LinearAlgebra::Vector A vector perpendicular to vector2 and
  *                                  equal to vector1-parallel2 (where
  *                                  parallel2 is some vector that is
  *                                  parallel to vector2).
  */
  // Derived from NAIF's vperp routine
  // possible that we may need to restrict size to dimesion 3, but doesn't seem
  // necessary in the code.
  LinearAlgebra::Vector LinearAlgebra::perpendicular(const Vector &vector1, const Vector &vector2) {

    // if vector1 is the zero vector, just set p to zero and return.
    if ( isZero(vector1) ) {
       return vector1;
    }

    // if vector2 is the zero vector, just set p to vector1 and return.
    if ( isZero(vector2) ) {
       return vector1;
    }

    // normalize (using the max norm) the given vectors and project the first onto the second
    double max1 = absoluteMaximum(vector1);
    double max2 = absoluteMaximum(vector2);
    Vector parallelVector = project(vector1 / max1,  vector2 / max2);

    Vector perpendicularVector = vector1 - parallelVector * max1;

    return perpendicularVector;

  }

  /**
   * Converts a rotation's representation from a matrix to a axis of rotation
   * and its corresponding rotation angle.
   *
   * @param rotationMatrix A matrix representing a rotation.
   *
   * @return @b LinearAlgebra::AxisAngle The axis-angle pair representing the rotation.
   *
   * @throw IException::Programmer "Unable to convert the given matrix to an
   *                                axis of rotation and a rotation angle. A
   *                                3x3 matrix is required."
   * @throw IException::Programmer "Unable to convert the given matrix to an
   *                                axis of rotation and a rotation angle.
   *                                The given matrix is not a rotation
   *                                matrix."
   */
  // Derived from NAIF's raxisa routine
  LinearAlgebra::AxisAngle LinearAlgebra::toAxisAngle(const Matrix &rotationMatrix) {

    if ((rotationMatrix.size1() != 3) || (rotationMatrix.size2() != 3)) {
      QString msg = "Unable to convert the given matrix to an axis of rotation "
                    "and a rotation angle. A 3x3 matrix is required. The given matrix is ["
                    + toString((int) rotationMatrix.size1()) + "x"
                    + toString((int) rotationMatrix.size2()) + "].";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // if given matrix is not a rotation matrix, we can't proceed.
    if ( !isRotationMatrix(rotationMatrix) ) {
      QString msg = "Unable to convert the given matrix to an axis of rotation "
                    "and a rotation angle. The given matrix is not a rotation matrix.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    Angle angle;
    Vector axis(3);

    // Construct the quaternion corresponding to the input rotation matrix
    Vector quaternion = LinearAlgebra::toQuaternion(rotationMatrix);

    LinearAlgebra::Vector subQuaternion(3);
    subQuaternion[0] = quaternion[1];
    subQuaternion[1] = quaternion[2];
    subQuaternion[2] = quaternion[3];

    // The quaternion we've just constructed is of the form:
    //
    //    cos(ANGLE/2) + sin(ANGLE/2) * AXIS
    //
    // We take a few precautions to handle the case of an identity
    // rotation.
    if (isZero(subQuaternion)) {
      axis(0) = 0.0;
      axis(1) = 0.0;
      axis(2) = 1.0;

      angle.setRadians(0.0);
    }
    else if (qFuzzyCompare(quaternion(0) + 1.0, 1.0)) {
      axis = subQuaternion;
      angle.setRadians(Isis::PI);
    }
    else {
      axis = LinearAlgebra::normalize(subQuaternion);
      angle.setRadians(2.0 * atan2(magnitude(subQuaternion), quaternion(0)));
    }

    return qMakePair(axis, angle);
  }

  /**
   * Converts a rotation's representation from a matrix to a set of Euler angles
   * with corresponding axes.
   *
   * @param rotationMatrix A matrix representing a rotation.
   * @param axes A list containing the order of axes.
   *
   * @return @b QList< LinearAlgebra::EulerAngle> The list of 3 Euler angles
   *                                              with their axes representing the rotation.
   *
   * @throw IException::Programmer "Unable to convert the given matrix to Euler angles.
   *                                Exactly 3 axis codes are required."
   * @throw IException::Programmer "Unable to convert the given matrix to Euler angles
   *                                using the given axis codes. Axis codes must be 1, 2, or 3."
   * @throw IException::Programmer "Unable to convert the given matrix to Euler angles
   *                                using the given axis codes. The middle axis
   *                                code must differ from its neighbors."
   * @throw IException::Programmer "Unable to convert the given matrix to Euler angles.
   *                                A 3x3 matrix is required."
   * @throw IException::Programmer "Unable to convert the given matrix to Euler angles.
   *                                The given matrix is not a rotation matrix."
   */
  // Derived from NAIF's m2eul routine
  QList<LinearAlgebra::EulerAngle> LinearAlgebra::toEulerAngles(const Matrix &rotationMatrix,
                                                                const QList<int> axes) {

    // check there are 3 axes in the set {1,2,3} with center axis not equal to first or last
    if (axes.size() != 3) {
      QString msg = "Unable to convert the given matrix to Euler angles. "
                    "Exactly 3 axis codes are required. The given list has ["
                    + toString((int) axes.size()) + "] axes.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    QSet<int> validAxes;
    validAxes << 1 << 2 << 3;
    if (!validAxes.contains(axes[0])
        || !validAxes.contains(axes[1])
        || !validAxes.contains(axes[2])) {
      QString msg = "Unable to convert the given matrix to Euler angles using the given axis codes "
                    "[" + toString(axes[0]) + ", " + toString(axes[1]) + ", " + toString(axes[2])
                    + "]. Axis codes must be 1, 2, or 3.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    if (axes[0] == axes[1] || axes[1] == axes[2]) {
      QString msg = "Unable to convert the given matrix to Euler angles using the given axis codes "
                    "[" + toString(axes[0]) + ", " + toString(axes[1]) + ", " + toString(axes[2])
                    + "]. The middle axis code must differ from its neighbors.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // check the matrix is 3x3 rotation
    if ((rotationMatrix.size1() != 3) || (rotationMatrix.size2() != 3)) {
      QString msg = "Unable to convert the given matrix to Euler angles. A 3x3 matrix is required. "
                    "The given matrix is ["
                    + toString((int) rotationMatrix.size1()) + "x"
                    + toString((int) rotationMatrix.size2()) + "].";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    if ( !isRotationMatrix(rotationMatrix) ) {
      QString msg = "Unable to convert the given matrix to Euler angles. "
                    "The given matrix is not a rotation matrix.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // AXIS3, AXIS2, AXIS1 and R have passed their tests at this
    // point.  We take the liberty of working with TEMPROT, a
    // version of R that has unitized columns.
    //  DO I = 1, 3
    //     CALL VHAT ( R(1,I), TEMPROT(1,I) )
    //  END DO

    LinearAlgebra::Matrix tempRotation(3, 3);
    for (int i = 0; i < 3; i++) {
      Vector columnVector = column(rotationMatrix, i);
      // replace the ith column with a unitized column vector
      setColumn(tempRotation, normalize(columnVector), i);
    }

    QList<int> nextAxis;
    nextAxis << 2 << 3 << 1;
    double sign;
    // create a matrix of zeros and we will fill in the non zero components below
    LinearAlgebra::Matrix change = zeroMatrix(3, 3);

    Angle angle1, angle2, angle3;
    if ( axes[0] == axes[2] ) {
      if ( axes[1] == nextAxis[axes[0] - 1] ) {
         sign =  1.0;
      }
      else {
         sign = -1.0;
      }
      int c = 6 - axes[0] - axes[1];

      change( axes[0] - 1, 2 ) =         1.0;
      change( axes[1] - 1, 0 ) =         1.0;
      change( c       - 1, 1 ) =  sign * 1.0;
      LinearAlgebra::Matrix tempMatrix = multiply( tempRotation,  change );
      tempRotation = multiply( transpose(change),  tempMatrix );

      bool degen =    (    qFuzzyCompare(tempRotation(0, 2) + 1.0, 1.0)
                        && qFuzzyCompare(tempRotation(1, 2) + 1.0, 1.0) )
                   || (    qFuzzyCompare(tempRotation(2, 0) + 1.0, 1.0)
                        && qFuzzyCompare(tempRotation(2, 1) + 1.0, 1.0) )
                   || qFuzzyCompare( qAbs(tempRotation(2, 2)), 1.0 );

      if ( degen ) {

        angle3.setRadians( 0.0 );
        angle2.setRadians(  acos( tempRotation(2, 2) ) );
        angle1.setRadians( atan2( tempRotation(0, 1), tempRotation(0, 0) ) );

      }
      else {
      // the normal case.
        angle3.setRadians( atan2( tempRotation(0, 2),  tempRotation(1, 2) ) );
        angle2.setRadians(  acos( tempRotation(2, 2) ) );
        angle1.setRadians( atan2( tempRotation(2, 0), -tempRotation(2, 1) ) );
      }
    }
    else {
    //   The axis order is c-b-a.  We're going to find a matrix CHANGE
    //   such that
    //
    //            T
    //      CHANGE  R  CHANGE
      if ( axes[1] == nextAxis[axes[0] - 1] ) {
         sign =  1.0;
      }
      else {
         sign = -1.0;
      }
      change( axes[0] - 1, 0 ) =        1.0;
      change( axes[1] - 1, 1 ) =        1.0;
      change( axes[2] - 1, 2 ) = sign * 1.0;
      LinearAlgebra::Matrix tempMatrix = multiply( tempRotation,  change );
      tempRotation = multiply( transpose(change),  tempMatrix );
      bool degen =    (    qFuzzyCompare(tempRotation(0, 0) + 1.0, 1.0)
                        && qFuzzyCompare(tempRotation(0, 1) + 1.0, 1.0) )
                   || (    qFuzzyCompare(tempRotation(1, 2) + 1.0, 1.0)
                        && qFuzzyCompare(tempRotation(2, 2) + 1.0, 1.0) )
                   || qFuzzyCompare( qAbs(tempRotation(0, 2)), 1.0 );

      if ( degen ) {

        angle3.setRadians( 0.0 );
        angle2.setRadians(         asin( -tempRotation(0, 2) ) );
        angle1.setRadians( sign * atan2( -tempRotation(1, 0), tempRotation(1, 1) ) );

      }
      else {
      // the normal case.
        angle3.setRadians(        atan2(  tempRotation(1, 2), tempRotation(2, 2) ) );
        angle2.setRadians(         asin( -tempRotation(0, 2) ) );
        angle1.setRadians( sign * atan2(  tempRotation(0, 1), tempRotation(0, 0) ) );
      }
    }

    QList<LinearAlgebra::EulerAngle> eulerAngles;
    eulerAngles << qMakePair(angle3, axes[0])
                << qMakePair(angle2, axes[1])
                << qMakePair(angle1, axes[2]);
    return eulerAngles;
  }

  /**
   * Converts a rotation's representation from a matrix to a unit quaternion.
   *
   * @param rotationMatrix A matrix representing a rotation.
   *
   * @return @b LinearAlgebra::Vector A unit quaternion representing the rotation.
   *
   * @throw IException::Programmer "Unable to convert the given matrix to a quaternion.
                                    A 3x3 matrix is required.
   * @throw IException::Programmer "Unable to convert the given matrix to an axis of rotation
   *                                and a rotation angle. The given matrix is not a rotation
   *                                matrix."
   */
  // Derived from NAIF's m2q routine
  LinearAlgebra::Vector LinearAlgebra::toQuaternion(const Matrix &rotationMatrix) {

    if ((rotationMatrix.size1() != 3) || (rotationMatrix.size2() != 3)) {
      QString msg = "Unable to convert the given matrix to a quaternion. "
                    "A 3x3 matrix is required. The given matrix is ["
                    + toString((int) rotationMatrix.size1()) + "x"
                    + toString((int) rotationMatrix.size2()) + "].";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // if given matrix is not a rotation matrix, we can't proceed.
    if ( !isRotationMatrix(rotationMatrix) ) {
      QString msg = "Unable to convert the given matrix to an axis of rotation "
                    "and a rotation angle. The given matrix is not a rotation matrix.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // Let q = c + s1*i + s2*j + s3*k
    // where c is the constant and s1, s2, s3 are the coefficients of the imaginary parts
    // and |q| = 1, .

    // Denote the following for n,m in {1,2,3}
    // cc =  c  *  c
    // csn = c  *  sn
    // snm = sn *  sm

    // The rotation matrix corresponding to our quaternion is:
    //
    // |  cc+s11-s22-s33      2*s12-2*cs3       2*s13+2*cs2   |
    // |    2*s12+2*cs3     cc-s11+s22-s33      2*s23-2*cs1   |
    // |    2*s13-2*cs2       2*s23+2*cs1     cc-s11-s22+s33  |
    //
    //
    // since |q| = cc + s11 + s22 + s33 = 1, we can use substitution on the diagonal entries to get
    //
    // |  1-2*s22-2*s33      2*s12-2*cs3      2*s13+2*cs2   |
    // |    2*s12+2*cs3    1-2*s11-2*s33      2*s23-2*cs1   |
    // |    2*s13-2*cs2      2*s23+2*cs1    1-2*s11-2*s22   |
    //
    //
    //
    // r(1,1)      = 1.0 - 2*s22 - 2*s33
    // r(2,1)      =       2*s12 + 2*cs3
    // r(3,1)      =       2*s13 - 2*cs2
    //
    // r(1,2)      =       2*s12 - 2*cs3
    // r(2,2)      = 1.0 - 2*s11 - 2*s33
    // r(3,2)      =       2*s23 + 2*cs1
    //
    // r(1,3)      =       2*s13 + 2*cs2
    // r(2,3)      =       2*s23 - 2*cs1
    // r(3,3)      = 1.0 - 2*s11 - 2*s22

    // Using this matrix, we get the trace by summing the diagonal entries
    //    trace = (1-2*s22-2*s33) + (1-2*s11-2*s33) + (1-2*s11-2*s22)
    //          = 3 - 4*(s11 + s22 + s33)

    // Therefore
    //    1.0 + trace = 4 - 4*(s11 + s22 + s33)
    //                = 4*(1 - s11 - s22 - s33)       by factoring 4
    //                = 4*(|q| - s11 - s22 - s33)     since |q| = 1
    //                = 4*cc                          since |q| = cc + s11 + s22 + s33
    //
    // Solving for c, we get that
    // 4*cc = 1 + trace
    //  2*c = +/- sqrt(1 + trace)
    //    c = +/- 0.5 * sqrt( 1.0 + trace )

    // We also have the following where {n,m,p} = {1,2,3}
    //   1.0 + trace - 2.0*r(n,n) = 4.0 - 4.0(snn + smm + spp) - 2(1.0 - 2.0(smm + spp ))
    //                            = 4.0 - 4.0(snn + smm + spp) - 2.0 + 4.0(smm + spp )
    //                            = 2.0 - 4.0*snn
    //
    // Solving for snn, we get
    //     2.0 - 4.0*snn =  1.0 + trace - 2.0*r(n,n)
    //    -2.0 + 4.0*snn = -1.0 - trace + 2.0*r(n,n)
    //           4.0*snn =  1.0 - trace + 2.0*r(n,n) ******** we will use this value  *************
    //                                               ******** in our code to minimize *************
    //                                               ******** computations            *************
    //            2.0*sn =  +/- sqrt(1.0 - trace + 2.0*r(n,n))
    //                sn =  +/- 0.5 * sqrt(1.0 - trace + 2.0*r(n,n))    for any n in {1,2,3}

    // In addition to these observations, note that all of the product
    // pairs can easily be computed by simplifying the expressions
    // (r(n,m) - r(m,n)) / 4.0 and (r(n,m) + r(m,n)) / 4.0
    //
    // For example,
    //  (r(3,2) - r(2,3)) / 4.0 = ( (2*s23 + 2*cs1) - (2*s23 - 2*cs1) ) / 4.0
    //                          = ( 4*cs1 ) / 4.0
    //                          =     cs1
    //
    //  So we have the following
    //  cs1 = (r(3,2) - r(2,3))/4.0
    //  cs2 = (r(1,3) - r(3,1))/4.0
    //  cs3 = (r(2,1) - r(1,2))/4.0
    //  s12 = (r(2,1) + r(1,2))/4.0
    //  s13 = (r(3,1) + r(1,3))/4.0
    //  s23 = (r(2,3) + r(3,2))/4.0

    //but taking sums or differences of numbers that are nearly equal
    //or nearly opposite results in a loss of precision. as a result
    //we should take some care in which terms to select when computing
    //c, s1, s2, s3.  however, by simply starting with one of the
    //large quantities cc, s11, s22, or s33 we can make sure that we
    //use the best of the 6 quantities above when computing the
    //remaining components of the quaternion.

    double trace  = rotationMatrix(0, 0) + rotationMatrix(1, 1) + rotationMatrix(2, 2);
    double mtrace = 1.0 - trace;

    // cc4 = 4 * c * c where c is the constant term (first) of the quaternion
    double cc4    = 1.0 + trace;
    // snn4 = 4 * sn * sn where sn is the coefficient for the n+1 term of the quaternion vector
    double s114   = mtrace + 2.0*rotationMatrix(0, 0);
    double s224   = mtrace + 2.0*rotationMatrix(1, 1);
    double s334   = mtrace + 2.0*rotationMatrix(2, 2);

    // Note that cc4 + s114 + s224 + s334 = 4|q| = 4
    // Thus, at least one of the 4 terms is greater than 1.
    double normalizingFactor;
    LinearAlgebra::Vector quaternion(4);
    if ( cc4 >= 1.0 ) { // true if the trace is non-negative

      quaternion[0] = sqrt( cc4 * 0.25 ); // note that q0 = c = sqrt(4*c*c*0.25)
      normalizingFactor = 1.0 / ( quaternion[0] * 4.0 );

      // multiply the difference of the entries symmetric about the diagonal by the factor 1/4c
      // to get s1, s2, and s3
      // (see calculations for cs1, cs2, cs3 in comments above)
      quaternion[1] = ( rotationMatrix(2, 1) - rotationMatrix(1, 2) ) * normalizingFactor;
      quaternion[2] = ( rotationMatrix(0, 2) - rotationMatrix(2, 0) ) * normalizingFactor;
      quaternion[3] = ( rotationMatrix(1, 0) - rotationMatrix(0, 1) ) * normalizingFactor;

    }
    else if ( s114 >= 1.0 ) {

      quaternion[1] = sqrt( s114 * 0.25 ); // note that q1 = s1 = sqrt(4*s1*s1*0.25)
      normalizingFactor = 1.0 /( quaternion[1] * 4.0 );

      quaternion[0] = ( rotationMatrix(2, 1) - rotationMatrix(1, 2) ) * normalizingFactor;
      quaternion[2] = ( rotationMatrix(0, 1) + rotationMatrix(1, 0) ) * normalizingFactor;
      quaternion[3] = ( rotationMatrix(0, 2) + rotationMatrix(2, 0) ) * normalizingFactor;

    }
    else if ( s224 >= 1.0 ) {

      quaternion[2] = sqrt( s224 * 0.25 );
      normalizingFactor = 1.0 /( quaternion[2] * 4.0 );

      quaternion[0] = ( rotationMatrix(0, 2) - rotationMatrix(2, 0) ) * normalizingFactor;
      quaternion[1] = ( rotationMatrix(0, 1) + rotationMatrix(1, 0) ) * normalizingFactor;
      quaternion[3] = ( rotationMatrix(1, 2) + rotationMatrix(2, 1) ) * normalizingFactor;
    }
    else { // s334 >= 1.0

      quaternion[3]   = sqrt( s334 * 0.25 );
      normalizingFactor = 1.0 /( quaternion[3] * 4.0 );

      quaternion[0] = ( rotationMatrix(1, 0) - rotationMatrix(0, 1) ) * normalizingFactor;
      quaternion[1] = ( rotationMatrix(0, 2) + rotationMatrix(2, 0) ) * normalizingFactor;
      quaternion[2] = ( rotationMatrix(1, 2) + rotationMatrix(2, 1) ) * normalizingFactor;

    }

    // if the magnitude of this quaternion is not one, we polish it up a bit.
    if ( !isUnit(quaternion) ) {
       quaternion = normalize(quaternion);
    }

    // M2Q always returns a quaternion with scalar part greater than or equal to zero.
    if ( quaternion[0] < 0.0 ) {
       quaternion *= -1.0;
    }

    return quaternion;
  }

  /**
   * Converts a rotation's representation from an axis of rotation and its
   * corresponding rotation angle to a 3x3 matrix.
   *
   *
   * @param axis The axis of rotation for a rotation.
   * @param angle The rotation angle.
   *
   * @return @b LinearAlgebra::Matrix The matrix representing the rotation.
   *
   * @throw IException::Programmer "Unable to convert the given vector and
   *                                angle to a rotation matrix. The given
   *                                vector is not a 3D axis vector."
   */
  // Derived from NAIF's axisar routine
  LinearAlgebra::Matrix LinearAlgebra::toMatrix(const Vector &axis, Angle angle) {
    if (axis.size() != 3) {
      QString msg = "Unable to convert the given vector and angle to a rotation matrix. "
                    "The given vector with size [" + toString((int) axis.size())
                    + "] is not a 3D axis vector.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // intialize the matrix with the 3x3 identity.
    LinearAlgebra::Matrix rotationMatrix = identity(3);

    // The matrix we want rotates every vector by angle about axis.
    // In particular, it does so to our basis vectors.  The columns
    // of the output matrix are the images of the basis vectors
    // under this rotation.
    for (unsigned int i = 0; i < axis.size(); i++) {
      // rotate the ith column of the matrix (the ith standard basis vector)
      // about the given axis using the given angle
      Vector stdBasisVector = column(rotationMatrix, i);

      // replace the ith column with the rotated vector
      setColumn(rotationMatrix, rotate(stdBasisVector, axis, angle), i);
    }

    return rotationMatrix;
  }

  /**
   * Converts a rotation's representation from an axis of rotation and its
   * corresponding rotation angle to a 3x3 matrix.
   *
   * @param axisAngle The axis-angle pair representation of a rotation
   *
   * @return @b LinearAlgebra::Matrix The matrix representation of the rotation.
   *
   */
  LinearAlgebra::Matrix LinearAlgebra::toMatrix(const AxisAngle &axisAngle) {
    return LinearAlgebra::toMatrix(axisAngle.first, axisAngle.second);
  }

  /**
   * Converts a rotation's representation from a set of Euler angles (3 angles,
   * each with a corresponding axis) to a 3x3 matrix.
   *
   * @param angle3 The third angle and its axis.
   * @param angle2 The second angle and its axis.
   * @param angle1 The first angle and its axis.
   *
   * @return @b LinearAlgebra::Matrix The matrix representation of the rotation.
   *
   * @throw IException::Programmer "Unable to convert the given Euler angles to
   *                                a matrix using the given axis codes. Axis
   *                                codes must be 1, 2, or 3."
   */
  // Derived from NAIF's eul2m routine
  LinearAlgebra::Matrix LinearAlgebra::toMatrix(const EulerAngle &angle3,
                                                const EulerAngle &angle2,
                                                const EulerAngle &angle1) {
    QSet<int> validAxes;
    validAxes << 1 << 2 << 3;
    if (!validAxes.contains(angle3.second)
        || !validAxes.contains(angle2.second)
        || !validAxes.contains(angle1.second)) {
      QString msg = "Unable to convert the given Euler angles to a matrix using the given axis "
                    "codes [" + toString(angle3.second) + ", " + toString(angle2.second) + ", "
                    + toString(angle1.second) + "]. Axis codes must be 1, 2, or 3.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    LinearAlgebra::Matrix m(3, 3);
    // implement eul2m
    // Calculate the 3x3 rotation matrix generated by a rotation
    // of a specified angle about a specified axis. This rotation
    // is thought of as rotating the coordinate system.
    //
    //ROTATE(angle1, axis1);
    double sinAngle = sin(angle1.first.radians());
    double cosAngle = cos(angle1.first.radians());
    // get the index offset based on the axis number corresponding to this angle
    double indexOffset = ((angle1.second % 3) + 3) % 3;
    QList<int> indexSet;
    indexSet << 2 << 0 << 1 << 2 << 0;
    int index1 = indexSet[indexOffset + 0];
    int index2 = indexSet[indexOffset + 1];
    int index3 = indexSet[indexOffset + 2];
    m(index1, index1) = 1.0;   m(index1, index2) =  0.0;        m(index1, index3) = 0.0;
    m(index2, index1) = 0.0;   m(index2, index2) =  cosAngle;   m(index2, index3) = sinAngle;
    m(index3, index1) = 0.0;   m(index3, index2) = -sinAngle;   m(index3, index3) = cosAngle;

    //
    // C     ROTMAT applies a rotation of ANGLE radians about axis IAXIS to a
    // C     matrix.  This rotation is thought of as rotating the coordinate
    // C     system.
    // C
    //CALL ROTMAT ( R,   ANGLE2,  AXIS2,  R1 )
    sinAngle = sin(angle2.first.radians());
    cosAngle = cos(angle2.first.radians());
    indexOffset = ((angle2.second % 3) + 3) % 3;
    index1 = indexSet[indexOffset + 0];
    index2 = indexSet[indexOffset + 1];
    index3 = indexSet[indexOffset + 2];

    LinearAlgebra::Matrix tempMatrix(3, 3);
    for (int i = 0; i < 3; i++) {
       tempMatrix(index1, i) =           m(index1, i);
       tempMatrix(index2, i) =  cosAngle*m(index2, i) + sinAngle*m(index3, i);
       tempMatrix(index3, i) = -sinAngle*m(index2, i) + cosAngle*m(index3, i);
    }

    //CALL ROTMAT ( tempMatrix,  ANGLE3,  AXIS3,  m  )
    sinAngle = sin(angle3.first.radians());
    cosAngle = cos(angle3.first.radians());
    indexOffset = ((angle3.second % 3) + 3) % 3;
    index1 = indexSet[indexOffset + 0];
    index2 = indexSet[indexOffset + 1];
    index3 = indexSet[indexOffset + 2];
    for (int i = 0; i < 3; i++) {
       m(index1, i) =           tempMatrix(index1, i);
       m(index2, i) =  cosAngle*tempMatrix(index2, i) + sinAngle*tempMatrix(index3, i);
       m(index3, i) = -sinAngle*tempMatrix(index2, i) + cosAngle*tempMatrix(index3, i);
    }

    return m;
  }

  /**
   * Converts a rotation's representation from a list of Euler angles (3 angles,
   * each with a corresponding axis) to a 3x3 matrix.
   *
   * @param eulerAngles The Euler angle representation of a rotation.
   *
   * @return @b LinearAlgebra::Matrix The matrix representation of the rotation.
   *
   * @throw IException::Programmer "Unable to convert the given Euler angles to
   *                                a matrix. Exactly 3 Euler angles are
   *                                required."
   */
  LinearAlgebra::Matrix LinearAlgebra::toMatrix(const QList<EulerAngle> &eulerAngles) {

    if (eulerAngles.size() != 3) {
      QString msg = "Unable to convert the given Euler angles to a matrix. "
                    "Exactly 3 Euler angles are required. The given list has ["
                    + toString((int) eulerAngles.size()) + "] angles.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return LinearAlgebra::toMatrix(eulerAngles[0], eulerAngles[1], eulerAngles[2]);
  }

  /**
   * Converts a rotation's representation from a quaternion to a 3x3 matrix.
   * Note, if the given vector is not a unit vector or the zero vector, this
   * method will normalize it before computing the corresponding matrix.
   *
   * @param quaternion   A unit quaternion representation of a rotation.
   *
   * @return @b LinearAlgebra::Matrix The matrix representation of the rotation.
   *
   * @throw IException::Programmer "Unable to convert the given vector to a
   *                                rotation matrix. The given vector is not
   *                                a quaternion."
   */
  // Derived from NAIF's q2m routine
  LinearAlgebra::Matrix LinearAlgebra::toMatrix(const Vector &quaternion) {
    if (quaternion.size() != 4) {
      QString msg = "Unable to convert the given vector to a rotation matrix. "
                    "The given vector with [" + toString((int) quaternion.size())
                    + "] components is not a quaternion.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    Vector q = quaternion;
    if ( !isUnit(quaternion) && !isZero(quaternion) ) {
      q = normalize(quaternion);
    }

    LinearAlgebra::Matrix matrix(3, 3);
    // For efficiency, we avoid duplicating calculations where possible.
    double q0q1 =  q[0] * q[1];
    double q0q2 =  q[0] * q[2];
    double q0q3 =  q[0] * q[3];

    double q1q1 =  q[1] * q[1];
    double q1q2 =  q[1] * q[2];
    double q1q3 =  q[1] * q[3];

    double q2q2 =  q[2] * q[2];
    double q2q3 =  q[2] * q[3];

    double q3q3 =  q[3] * q[3];

    matrix(0, 0) =  1.0  -  2.0 * ( q2q2 + q3q3 );
    matrix(0, 1) =          2.0 * ( q1q2 - q0q3 );
    matrix(0, 2) =          2.0 * ( q1q3 + q0q2 );

    matrix(1, 0) =          2.0 * ( q1q2 + q0q3 );
    matrix(1, 1) =  1.0  -  2.0 * ( q1q1 + q3q3 );
    matrix(1, 2) =          2.0 * ( q2q3 - q0q1 );

    matrix(2, 0) =          2.0 * ( q1q3 - q0q2 );
    matrix(2, 1) =          2.0 * ( q2q3 + q0q1 );
    matrix(2, 2) =  1.0  -  2.0 * ( q1q1 + q2q2 );

    return matrix;
  }

  /**
   * Sets the row of the given matrix to the values of the given vector.
   *
   * @param matrix The address of the matrix to be altered.
   * @param vector The vector with the new values.
   * @param rowIndex The index of the row to be altered.
   *
   * @throw IException::Programmer "Unable to set the matrix row to the given
   *                                vector. Row index is out of bounds."
   */
  void LinearAlgebra::setRow(Matrix &matrix, const Vector &vector, int rowIndex) {
    if ( (rowIndex+1 > (int) matrix.size1()) ) {
      QString msg = "Unable to set the matrix row to the given vector. Row index "
                    + toString(rowIndex) + " is out of bounds. The given matrix only has "
                    + toString((int) matrix.size1()) + " rows." ;
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    boost::numeric::ublas::row(matrix, rowIndex) = vector;
  }

  /**
   * Sets the column of the given matrix to the values of the given vector.
   *
   * @param matrix The address of the matrix to be altered.
   * @param vector The vector with the new values.
   * @param columnIndex The index of the column to be altered.
   *
   * @throw IException::Programmer "Unable to set the matrix column to the given
   *                                vector. Column index is out of bounds."
   */
  void LinearAlgebra::setColumn(Matrix &matrix, const Vector &vector, int columnIndex) {
    if ( (columnIndex+1 > (int) matrix.size2()) ) {
      QString msg = "Unable to set the matrix column to the given vector. Column index "
                    + toString(columnIndex) + " is out of bounds. The given matrix only has "
                    + toString((int) matrix.size1()) + " columns." ;
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    boost::numeric::ublas::column(matrix, columnIndex) = vector;
  }

  /**
   * Returns a vector whose components match those of the given matrix row.
   *
   * @param matrix The matrix to pull values from.
   * @param rowIndex The index of the matrix row to grab.
   *
   * @return @b LinearAlgebra::Vector A vector whose values match the given row
   *                                  of the given matrix.
   *
   * @throw IException::Programmer "Unable to get the matrix row to the given
   *                                vector. Row index is out of bounds."
   */
  LinearAlgebra::Vector LinearAlgebra::row(const Matrix &matrix, int rowIndex) {
    if ( (rowIndex+1 > (int) matrix.size1()) ) {
      QString msg = "Unable to get the matrix row to the given vector. Row index "
                    + toString(rowIndex) + " is out of bounds. The given matrix only has "
                    + toString((int) matrix.size1()) + " rows." ;
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return boost::numeric::ublas::row(matrix, rowIndex);
  }

  /**
   * Returns a vector whose components match those of the given matrix column.
   *
   * @param matrix The matrix to pull values from.
   * @param columnIndex The index of the matrix column to grab.
   *
   * @return @b LinearAlgebra::Vector A vector whose values match the given
   *                                  column of the given matrix.
   *
   * @throw IException::Programmer "Unable to get the matrix column to the given
   *                                vector. Column index is out of bounds."
   */
  LinearAlgebra::Vector LinearAlgebra::column(const Matrix &matrix, int columnIndex) {
    if ( (columnIndex+1 > (int) matrix.size2()) ) {
      QString msg = "Unable to get the matrix column to the given vector. Column index "
                    + toString(columnIndex) + " is out of bounds. The given matrix only has "
                    + toString((int) matrix.size1()) + " columns." ;
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return boost::numeric::ublas::column(matrix, columnIndex);
  }

  /**
   * Constructs a 3 dimensional vector with the given component values.
   *
   * @param v0 The first component of the vector.
   * @param v1 The second component of the vector.
   * @param v2 The third component of the vector.
   *
   * @return @b LinearAlgebra::Vector A vector containing the given values.
   *
   */
  LinearAlgebra::Vector LinearAlgebra::vector(double v0, double v1, double v2) {
    Vector v(3);
    v[0] = v0;
    v[1] = v1;
    v[2] = v2;
    return v;
  }

  /**
   * Constructs a 4 dimensional vector with the given component values.
   *
   * @param v0 The first component of the vector.
   * @param v1 The second component of the vector.
   * @param v2 The third component of the vector.
   * @param v3 The fourth component of the vector.
   *
   * @return @b LinearAlgebra::Vector A vector containing the given values.
   *
   */
  LinearAlgebra::Vector LinearAlgebra::vector(double v0, double v1, double v2, double v3) {
    Vector v(4);
    v[0] = v0;
    v[1] = v1;
    v[2] = v2;
    v[3] = v3;
    return v;
  }

  /**
   * Fills the first three elements of the given vector with the given values.
   *
   * @param v A pointer to a 3-dimensional vector to be filled.
   * @param v0 The first component of the vector.
   * @param v1 The second component of the vector.
   * @param v2 The third component of the vector.
   */
  void LinearAlgebra::setVec3(Vector *v, double v0, double v1, double v2) {
    (*v)[0] = v0;
    (*v)[1] = v1;
    (*v)[2] = v2;
  }

  /**
   * Fills the first four elements of the given vector with the given values.
   *
   * @param v A pointer to a 4-dimensional vector to be filled.
   * @param v0 The first component of the vector.
   * @param v1 The second component of the vector.
   * @param v2 The third component of the vector.
   * @param v3 The fourth component of the vector.
   */
  void LinearAlgebra::setVec4(Vector *v, double v0, double v1, double v2, double v3) {
    (*v)[0] = v0;
    (*v)[1] = v1;
    (*v)[2] = v2;
    (*v)[3] = v3;
  }

  /**
   * Constructs a vector of given size using the given vector and starting
   * index.
   *
   * @param v The original vector to get values from.
   * @param startIndex The index of the original vector, used to indicate
   *                   the first value that will be copied to the new vector.
   * @param size The number of elements from the original vector to copy
   *             to the new vector.
   *
   * @return @b LinearAlgebra::Vector A sub-vector containing values from
   *                                  the original vector.
   */
  LinearAlgebra::Vector LinearAlgebra::subVector(const Vector &v, int startIndex, int size) {
    LinearAlgebra::Vector sub(size);
    for (int i = 0; i < size; i++) {
      sub[i] = v[i + startIndex];
    }
    return sub;
  }

  /**
   * A global function to format a LinearAlgebra::Vector as a QString and writes
   * it to a QDebug stream.
   *
   * @see toString(LinearAlgebra::Vector)
   *
   * @param dbg The stream where the vector will be written.
   * @param vector The vector to be written.
   *
   * @return @b QDebug The stream with the QString-formatted vector.
   *
   */
  QDebug operator<<(QDebug dbg, const LinearAlgebra::Vector &vector) {
    QDebugStateSaver saver(dbg);
    dbg.noquote() << toString(vector);
    return dbg;
  }

  /**
   * A global function to format a LinearAlgebra::Matrix as a QString and
   * write it to a QDebug stream. There will be 4 spaces between each matrix
   * entry and each row is written on a new line.
   *
   * @param dbg The stream where the vector will be written.
   * @param matrix The matrix to be written.
   *
   * @return @b QDebug The stream with the QString-formatted matrix.
   *
   */
  QDebug operator<<(QDebug dbg, const LinearAlgebra::Matrix &matrix) {
    QDebugStateSaver saver(dbg);
    for (unsigned int i = 0; i < matrix.size1(); i++) {
      dbg.noquote() << "    ";
      for (unsigned int j = 0; j < matrix.size2(); j++) {
        dbg.noquote() << toString(matrix(i, j), 15) << "     ";
      }
      dbg.noquote() << endl;
    }
    return dbg;
  }

  /**
   * A global function to format LinearAlgebra::Vector as a QString with the
   * given precision. The string will be comma-separated entries encased by
   * parentheses.
   *
   * @param vector The vector to be converted.
   * @param precision Number of significant figures to convert.
   *
   * @return @b QString The string-formatted vector.
   *
   */
  QString toString(const LinearAlgebra::Vector &vector, int precision) {
    QString result = "( ";
    for (unsigned int i = 0; i < vector.size(); i++) {
      result += toString(vector(i), precision);
      if (i != vector.size() - 1) result += ", ";
    }
    result += " )";
    return result;
  }
}


#pragma once

#include "generator/feature_builder.hpp"

#include <iterator>

#include <boost/geometry.hpp>

namespace generator
{
namespace boost_helpers
{
template <typename BoostPoint, typename BoostGeometry, typename FbGeometry>
void FillBoostGeometry(BoostGeometry & geometry, FbGeometry const & fbGeometry)
{
  geometry.reserve(fbGeometry.size());
  for (auto const & p : fbGeometry)
    boost::geometry::append(geometry, BoostPoint{p.x, p.y});
}

template <typename BoostPolygon>
void FillPolygon(BoostPolygon & polygon, FeatureBuilder1 const & fb)
{
  using BoostPoint = typename BoostPolygon::point_type;
  auto const & fbGeometry = fb.GetGeometry();
  CHECK(!fbGeometry.empty(), ());
  auto it = std::begin(fbGeometry);
  FillBoostGeometry<BoostPoint>(polygon.outer(), *it);
  polygon.inners().resize(fbGeometry.size() - 1);
  int i = 0;
  ++it;
  for (; it != std::end(fbGeometry); ++it)
    FillBoostGeometry<BoostPoint>(polygon.inners()[i++], *it);

  boost::geometry::correct(polygon);
}
}  // namespace boost_helpers
}  // namespace generator


#pragma once

#include <type_traits>
#include <tuple>

#include <boost/integer_traits.hpp>

namespace blackhole {

namespace tuple {

template<int START, int STOP = -1, int STEP = 1>
struct slice {
    template<class IndexedSequence>
    struct type;

    template<int N, typename T>
    struct type<std::tuple<std::integral_constant<int, N>, T>> {
        static const int delta = N - START;
        static const int stop = STOP == -1 ? boost::integer_traits<int>::const_max : STOP;
        static const bool value = delta >= 0 && N < stop && delta % STEP == 0;
    };
};

} // namespace tuple

} // namespace blackhole


// std c
#include <stdio.h>
#include <iostream>
#include <string>

// opencv
#include <opencv/cv.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/imgproc/imgproc.hpp"

// ros
#include <ros/ros.h>
#include <ros/package.h>

// Eigen
#include <Eigen/Dense>
#include <Eigen/Core>

// ours
#include "detect_3d_cuboid/detect_3d_cuboid.h"
#include "tictoc_profiler/profiler.hpp"

using namespace std;
using namespace Eigen;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "detect_3d_cuboid");
    ros::NodeHandle nh;
    ca::Profiler::enable();

    string base_folder = ros::package::getPath("detect_3d_cuboid") + "/data/";
    //相机内参
    Matrix3d Kalib;
    Kalib << 529.5000, 0, 365.0000,
        0, 529.5000, 265.0000,
        0, 0, 1.0000;
    //相机外参
    Matrix4d transToWolrd;
    transToWolrd << 1, 0.0011, 0.0004, 0, // hard coded  NOTE if accurate camera roll/pitch, could sample it!
        0, -0.3376, 0.9413, 0,
        0.0011, -0.9413, -0.3376, 1.35,
        0, 0, 0, 1;
    //二维包围框的信息
    MatrixXd obj_bbox_coors(1, 5);                // hard coded
    obj_bbox_coors << 188, 189, 201, 311, 0.8800; // [x y w h prob]
    obj_bbox_coors.leftCols<2>().array() -= 1;    // change matlab coordinate to c++, minus 1
    ///读取某一张图片
    int frame_index = 0;
    char frame_index_c[256];
    sprintf(frame_index_c, "%04d", frame_index); // format into 4 digit
    // read images
    cv::Mat rgb_img = cv::imread(base_folder + frame_index_c + "_rgb_raw.jpg", 1);
    //读取该图像对应的边缘
    Eigen::MatrixXd all_lines_raw(100, 4); // 100 is some large frame number,   the txt edge index start from 0
    read_all_number_txt(base_folder + "edge_detection/LSD/" + frame_index_c + "_edge.txt", all_lines_raw);
    ///3D保包围框生成
    detect_3d_cuboid detect_cuboid_obj;
    detect_cuboid_obj.whether_plot_detail_images = false;
    detect_cuboid_obj.whether_plot_final_images = true;
    detect_cuboid_obj.print_details = false; // false  true
    detect_cuboid_obj.set_calibration(Kalib);
    detect_cuboid_obj.whether_sample_bbox_height = false;
    detect_cuboid_obj.whether_sample_cam_roll_pitch = false;
    std::vector<ObjectSet> all_object_cuboids;
    detect_cuboid_obj.detect_cuboid(rgb_img, transToWolrd, obj_bbox_coors, all_lines_raw, all_object_cuboids);

    ca::Profiler::print_aggregated(std::cout);
    return 0;
}


// ---------------------------------------------------------------------------
//
//  This file is part of PermLib.
//
// Copyright (c) 2009-2011 Thomas Rehn <thomas@carmen76.de>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. 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.
// 3. The name of the author may not be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.
//
// ---------------------------------------------------------------------------

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Backtrack search test - matrix automorphism
#include <boost/test/unit_test.hpp>

#include <iostream>
#include <fstream>

#include <permlib/permutation.h>
#include <permlib/bsgs.h>
#include <permlib/symmetric_group.h>
#include <permlib/construct/schreier_sims_construction.h>

#include <permlib/transversal/schreier_tree_transversal.h>
#include <permlib/change/conjugating_base_change.h>
#include <permlib/search/partition/matrix_automorphism_search.h>

#include "test-common.h"

#include <boost/foreach.hpp>

using namespace permlib;
using namespace permlib::test;

template<class T>
class Matrix {
	public:
		Matrix(unsigned long dimension) : m_dimension(dimension), m_matrix(dimension*dimension) {}

		unsigned long dimension() const { return m_dimension; }
		inline const T& at(unsigned long i, unsigned long j) const { return m_matrix[j * m_dimension + i]; }
		inline T& at(unsigned long i, unsigned long j) { return m_matrix[j * m_dimension + i]; }

	private:
		const unsigned long m_dimension;
		std::vector<T> m_matrix;
};

class ZMatrix : public Matrix<unsigned long> {
	public:
		ZMatrix(unsigned long dimension, unsigned long k = 0) : Matrix<unsigned long>(dimension), m_k(k) {}

		unsigned long k() const { return m_k; }
		unsigned long& k() { return m_k; }
	private:
		unsigned long m_k;
};

BOOST_AUTO_TEST_CASE( matrix_auto )
{
	// we use elementary permutations
	typedef Permutation PERM;
	// and Schreier tree transversals
	typedef SchreierTreeTransversal<PERM> TRANSVERSAL;

	// our group will have degree 4
	const unsigned long n = 4;

	ZMatrix matrix(4,3);
	matrix.at(0,0) = 1;
	matrix.at(0,1) = 0;
	matrix.at(0,2) = 0;
	matrix.at(0,3) = 2;

	matrix.at(1,1) = 1;
	matrix.at(1,2) = 0;
	matrix.at(1,3) = 2;

	matrix.at(2,2) = 2;
	matrix.at(2,3) = 2;

	matrix.at(3,3) = 1;

	for(unsigned int i=0; i<n; ++i) {
		for(unsigned int j=i+1; j<n; ++j) {
			matrix.at(j,i) = matrix.at(i,j);
		}
	}

	SymmetricGroup<PERM> s_n(n);
	partition::MatrixAutomorphismSearch<SymmetricGroup<PERM>, TRANSVERSAL> mas(s_n, false);
	mas.construct(matrix);

	BSGS<PERM,TRANSVERSAL> K(n);
	mas.search(K);

	BOOST_CHECK(K.order() == 2);
}


// Boost.Geometry Index
// Unit Test

// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.

// Use, modification and distribution is 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 <algorithm>

#include <geometry_index_test_common.hpp>

#include <boost/geometry/index/detail/algorithms/is_valid.hpp>

#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>

//#define BOOST_GEOMETRY_TEST_DEBUG

template <typename Geometry>
void test(Geometry const& geometry, bool expected_value)
{
    bool value = bgi::detail::is_valid(geometry);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::ostringstream out;
    out << typeid(typename bg::coordinate_type<Geometry>::type).name()
        << " "
        << typeid(bool).name()
        << " "
        << "is_valid : " << value
        << std::endl;
    std::cout << out.str();
#endif

    BOOST_CHECK(value == expected_value);
}

template <typename Box>
void test_box(std::string const& wkt, bool expected_value)
{
    Box box;
    bg::read_wkt(wkt, box);
    test(box, expected_value);
    typename bg::point_type<Box>::type temp_pt;
    temp_pt = box.min_corner();
    box.min_corner() = box.max_corner();
    box.max_corner() = temp_pt;
    test(box, !expected_value);
}

void test_large_integers()
{
    typedef bg::model::point<int, 2, bg::cs::cartesian> int_point_type;
    typedef bg::model::point<double, 2, bg::cs::cartesian> double_point_type;

    bg::model::box<int_point_type> int_box;
    bg::model::box<double_point_type> double_box;

    std::string const box_li = "POLYGON((1536119 192000, 1872000 528000))";
    bg::read_wkt(box_li, int_box);
    bg::read_wkt(box_li, double_box);

    BOOST_CHECK(bgi::detail::is_valid(int_box) == bgi::detail::is_valid(double_box));

    std::string const box_li2 = "POLYGON((1872000 528000, 1536119 192000))";
    bg::read_wkt(box_li2, int_box);
    bg::read_wkt(box_li2, double_box);

    BOOST_CHECK(bgi::detail::is_valid(int_box) == bgi::detail::is_valid(double_box));
}

int test_main(int, char* [])
{
    typedef bg::model::point<int, 2, bg::cs::cartesian> P2ic;
    typedef bg::model::point<float, 2, bg::cs::cartesian> P2fc;
    typedef bg::model::point<double, 2, bg::cs::cartesian> P2dc;

    typedef bg::model::point<int, 3, bg::cs::cartesian> P3ic;
    typedef bg::model::point<float, 3, bg::cs::cartesian> P3fc;
    typedef bg::model::point<double, 3, bg::cs::cartesian> P3dc;

    test(P2ic(0, 0), true);
    test(P2fc(0, 0), true);
    test(P2dc(0, 0), true);
    test(P3ic(0, 0, 0), true);
    test(P3fc(0, 0, 0), true);
    test(P3dc(0, 0, 0), true);

    test_box<bg::model::box<P2ic> >("POLYGON((0 1,2 4))", true);
    test_box<bg::model::box<P2fc> >("POLYGON((0 1,2 4))", true);
    test_box<bg::model::box<P2dc> >("POLYGON((0 1,2 4))", true);
    test_box<bg::model::box<P3ic> >("POLYGON((0 1 2,2 4 6))", true);
    test_box<bg::model::box<P3fc> >("POLYGON((0 1 2,2 4 6))", true);
    test_box<bg::model::box<P3dc> >("POLYGON((0 1 2,2 4 6))", true);

    test_large_integers();

    return 0;
}


#define CATCH_CONFIG_MAIN

#include "CALPHADFreeEnergyFunctionsTernary.h"

#include "catch.hpp"

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <fstream>
#include <string>

namespace pt = boost::property_tree;

TEST_CASE("KKS ternary", "[KKS ternary]")
{
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::PBG;

    double temperature = 2923.;

    pt::ptree calphad_db;
    try
    {
        pt::read_json("../thermodynamic_data/calphadMoNbTa.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    Thermo4PFM::CALPHADFreeEnergyFunctionsTernary cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    // Mo energy at melting point
    {
        double temperature0 = 2896.;
        double conc[2]      = { 1., 0. };

        double el = cafe.computeFreeEnergy(
            temperature0, conc, Thermo4PFM::PhaseIndex::phaseL);
        std::cout << "Mo, Energy in liquid phase: " << el << std::endl;
        double es = cafe.computeFreeEnergy(
            temperature0, conc, Thermo4PFM::PhaseIndex::phaseA);
        std::cout << "Mo, Energy in solid phase: " << es << std::endl;

        REQUIRE(el == Approx(es).margin(1.e-1));
    }

    // Ta energy at melting point
    {
        double temperature0 = 3290.;
        double conc[2]      = { 0., 0. };

        double el = cafe.computeFreeEnergy(
            temperature0, conc, Thermo4PFM::PhaseIndex::phaseL);
        std::cout << "Ta, Energy in liquid phase: " << el << std::endl;
        double es = cafe.computeFreeEnergy(
            temperature0, conc, Thermo4PFM::PhaseIndex::phaseA);
        std::cout << "Ta, Energy in solid phase: " << es << std::endl;

        REQUIRE(el == Approx(es).margin(1.e-1));
    }

    // initial guesses
    double sol[4] = { 0.33, 0.38, 0.32, 0.33 };

    // compute concentrations satisfying KKS equations
    double conc[2] = { 0.33, 0.33 };
    double phi     = 0.5;
    cafe.computePhaseConcentrations(temperature, &conc[0], &phi, &sol[0]);

    std::cout << "-------------------------------" << std::endl;
    std::cout << "Temperature: " << temperature << std::endl;
    std::cout << "Result for c = " << conc[0] << "," << conc[1]
              << " and phi = " << phi << std::endl;
    std::cout << "   cL = " << sol[0] << ", " << sol[1] << std::endl;
    std::cout << "   cS = " << sol[2] << ", " << sol[3] << std::endl;

    const Thermo4PFM::PhaseIndex pi0 = Thermo4PFM::PhaseIndex::phaseL;
    const Thermo4PFM::PhaseIndex pi1 = Thermo4PFM::PhaseIndex::phaseA;

    std::cout << "Verification:" << std::endl;

    double derivL[2];
    cafe.computeDerivFreeEnergy(temperature, &sol[0], pi0, &derivL[0]);

    double derivS[2];
    cafe.computeDerivFreeEnergy(temperature, &sol[2], pi1, &derivS[0]);

    std::cout << "   dfL/dcL0 = " << derivL[0] << std::endl;
    std::cout << "   dfS/dcS0 = " << derivS[0] << std::endl;
    std::cout << "   dfL/dcL1 = " << derivL[1] << std::endl;
    std::cout << "   dfS/dcS1 = " << derivS[1] << std::endl;

    const double tol = 1.e-5;
    CHECK(derivS[0] == Approx(derivL[0]).margin(tol));
    CHECK(derivS[1] == Approx(derivL[1]).margin(tol));
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SCALAR_LDEXP_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SCALAR_LDEXP_HPP_INCLUDED
#include <boost/simd/toolbox/ieee/functions/ldexp.hpp>
#include <boost/simd/include/constants/ldexpmask.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/constants/real.hpp>
#include <boost/simd/include/functions/scalar/is_finite.hpp>
#include <boost/simd/include/functions/scalar/is_nez.hpp>
#include <boost/simd/include/functions/scalar/bitwise_andnot.hpp>
#include <boost/simd/include/functions/scalar/bitwise_or.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::ldexp_, tag::cpu_, (A0)(A1)
                            , (scalar_< integer_<A0> >)(scalar_< integer_<A1> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2) { return (a1>0)?(a0<<a1):(a0>>a1); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::ldexp_, tag::cpu_, (A0)(A1)
                            , (scalar_< floating_<A0> >)(scalar_< integer_<A1> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      // No denormal provision
      typedef typename dispatch::meta::as_integer<result_type, unsigned>::type  int_type;
      // clear exponent in x
      result_type const x(b_andnot(a0, Ldexpmask<result_type>()));
      // extract exponent and compute the new one
      int_type e    = b_and(Ldexpmask<result_type>(), a0);
      e += int_type(a1) << Nbmantissabits<result_type>(); //nmb;
      // provisions for limit values
      if (is_nez(a0)&&is_finite(a0)) return  b_or(x, e);
      return a0;
    }
  };
} } }

#endif


#include <algorithm>
#include <cmath>
#include <filesystem>
#include <iostream>
#include <iterator>
#include <queue>
#include <string>
#include <unordered_set>

namespace fs = std::filesystem;
using namespace std;

// class Member {
// private:
//   const string name_;
//   const BoundingBox bounding_box_;

// public:
//   Member(string name, aiMesh *mesh) : name_(name), bounding_box_(mesh) {}
//   ~Member() {}

//   const string &Name() const { return name_; }
//   const BoundingBox &Box() const { return bounding_box_; }
// };

// class Edge {
// private:
//   const Member *m1_, *m2_;
//   const BoundingBox *n1_, *n2_;
//   ai_real attraction_, cost_;

// public:
//   Edge(Member *m1, Member *m2) {
//     m1_ = m1;
//     m2_ = m2;
//     n1_ = &m1->Box();
//     n2_ = &m2->Box();

//     ai_real distance = (n1_->Mid() - n2_->Mid()).Length();
//     BoundingBox n12(n1_,
//                     n2_); // TODO: n12 must be reserved for muli-layer level
//     attraction_ = n1_->Radius() * n2_->Radius() / (distance * distance);
//     cost_ = (n12.Radius() * n12.Radius() * n12.Radius()) / attraction_;
//   }
//   ~Edge() {}

//   friend bool operator<(const Edge &e1, const Edge &e2) {
//     return e1.Cost() < e2.Cost();
//   }

//   const Member &M1() const { return *m1_; }
//   const Member &M2() const { return *m2_; }
//   const ai_real Attraction() const { return attraction_; }
//   const ai_real Cost() const { return cost_; }
// };

// int main(int, char **) {
//   vector<Member> nodes; // TODO: Should know the num of models in directory
//                         // (to reserve memory)

//   string model_directory("parts-model");
//   {
//     int num_model_files_in_directory = 0, num_files_in_directory = 0;

//     for (auto &p : fs::directory_iterator(model_directory)) {
//       ++num_files_in_directory;

//       bool is_stl = true; // TODO: read only files in stl extension.

//       if (is_stl) {
//         Assimp::Importer importer;

//         const aiScene *scene = importer.ReadFile(
//             p.path().string(), aiProcess_CalcTangentSpace |
//                                    aiProcess_JoinIdenticalVertices |
//                                    aiProcess_SortByPType);
//         if (!scene) {
//           cerr << "[Error] Cannot read " << p.path() << " file!\n";
//         }

//         size_t num_meshes = scene->mNumMeshes;
//         for (size_t mesh_idx = 0; mesh_idx < num_meshes; ++mesh_idx) {
//           nodes.emplace_back(
//               p.path().stem().string(),
//               scene->mMeshes[mesh_idx]); // TODO: move and forward for high
//                                          // performance.
//         }

//         ++num_model_files_in_directory;
//       }
//     }

//     cout << "Succesfully load " << num_model_files_in_directory
//          << " model file(s) out of " << num_files_in_directory
//          << " file(s) in the directory(" << model_directory << ")\n";
//   }

//   vector<Edge> edges; // adjecency graph
//   {
//     for (auto n1 = nodes.begin(); n1 != nodes.end(); ++n1) {
//       for (auto n2 = (n1 + 1); n2 != nodes.end(); ++n2) {
//         edges.emplace_back(&*n1, &*n2);
//       }
//     }

//     sort(edges.begin(), edges.end());

//     for (auto &e : edges) {
//       cout << "The attraction between " << e.M1().Name() << " and "
//            << e.M2().Name() << " is " << e.Attraction() << " and the cost is
//            "
//            << e.Cost() << endl;
//     }
//   }

//   priority_queue<Edge *> minimum_spanning_tree;
//   {
//     unordered_set<string> visited;
//     minimum_spanning_tree.push(&edges.at(0)); // TODO: edges should not empty

//     while (!minimum_spanning_tree.empty()) {
//       const Member &m1 = minimum_spanning_tree.top()->M1();
//       const Member &m2 = minimum_spanning_tree.top()->M2();
//       const ai_real cost = minimum_spanning_tree.top()->Cost();
//       minimum_spanning_tree.pop();

//       if (!visited.insert(m1.Name()).second) {
//         continue;
//       }

//       for (auto &e : edges) {
//         if (&e.M1() == &m1) {
//           if (visited.find(e.M2().Name()) == visited.end()) {
//             minimum_spanning_tree.push(&e);
//             cout << "The cost between " << e.M1().Name() << " and "
//                  << e.M2().Name() << " is " << e.Cost() << endl;
//           }
//         }
//       }
//     }

//     while (!minimum_spanning_tree.empty()) {
//       auto *e = minimum_spanning_tree.top();
//       cout << "The cost between " << e->M1().Name() << " and " <<
//       e->M2().Name()
//            << " is " << e->Cost() << endl;

//       minimum_spanning_tree.pop();
//     }
//   }

//   cout << "Hello, world!\n";

//   return 0;
// }

// #include <boost/graph/adj_list_serialize.hpp>
// #include <boost/graph/graph_utility.hpp>
// #include <boost/graph/graphviz.hpp>
// #include <boost/property_map/function_property_map.hpp>
// #include <boost/property_map/transform_value_property_map.hpp>
// #include <boost/variant.hpp>
// #include <fstream>
// using namespace boost;

// namespace Nodes {
// struct Building {
//   std::string id;
//   double weight;
// };
// struct Contract {
//   std::string id;
// };

// static inline std::ostream& operator<<(std::ostream& os, Building const& b) {
//   return os << "Building " << b.id;
// }
// static inline std::ostream& operator<<(std::ostream& os, Contract const& b) {
//   return os << "Contract " << b.id;
// }

// std::string id_of(Building const& b) { return b.id; }
// std::string id_of(Contract const& c) { return c.id; }
// std::string shape_of(Building const& b) { return "circle"; }
// std::string shape_of(Contract const& c) { return "diamond"; }
// }  // namespace Nodes

// using Nodes::Building;
// using Nodes::Contract;
// using Vertex = boost::variant<Building, Contract>;

// std::string id_of(Vertex const& v) {
//   return boost::apply_visitor([](auto const& node) { return id_of(node); },
//   v);
// }
// std::string shape_of(Vertex const& v) {
//   return boost::apply_visitor([](auto const& node) { return shape_of(node);
//   },
//                               v);
// }

// typedef adjacency_list<listS, listS, undirectedS, Vertex> Graph;

// int main() {
//   Graph g;
//   auto office1 = add_vertex(Building{"office1"}, g);
//   auto office2 = add_vertex(Building{"office2"}, g);
//   auto warehouse1 = add_vertex(Building{"warehouse1"}, g);
//   auto contract1 = add_vertex(Contract{"contract1"}, g);
//   auto contract2 = add_vertex(Contract{"contract2"}, g);

//   add_edge(office1, office2, g);
//   add_edge(warehouse1, contract1, g);
//   add_edge(contract2, contract1, g);

//   {
//     std::ofstream dot_file("figs/graph.dot");
//     dynamic_properties dp;

//     dp.property("node_id", boost::make_transform_value_property_map(
//                                &::id_of, boost::get(boost::vertex_bundle,
//                                g)));
//     dp.property("shape", boost::make_transform_value_property_map(
//                              &::shape_of, boost::get(boost::vertex_bundle,
//                              g)));
//     dp.property("label", boost::make_transform_value_property_map(
//                              [](Vertex const& v) {
//                                return boost::lexical_cast<std::string>(v);
//                              },
//                              boost::get(boost::vertex_bundle, g)));

//     write_graphviz_dp(dot_file, g, dp);
//   }

//   print_graph(g, make_transform_value_property_map(
//                      &::id_of, get(boost::vertex_bundle, g)));

//   return EXIT_SUCCESS;
// }

#include <algorithm>
#include <filesystem>
#include <fstream>
#include <string>
#include <vector>

#include <assimp/postprocess.h>
#include <assimp/scene.h>
#include <assimp/Importer.hpp>

#include <boost/graph/adj_list_serialize.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/property_map/transform_value_property_map.hpp>
#include <boost/variant.hpp>

using namespace boost;

namespace Nodes {
struct Block {
  std::string name;
  int id;
  double weight;
};

static inline std::ostream& operator<<(std::ostream& os, Block const& b) {
  return os << "Block " << b.name;
}

int id_of(Block const& b) { return b.id; }
std::string name_of(Block const& b) { return b.name; }
std::string shape_of(Block const& b) { return "circle"; }
}  // namespace Nodes

using Nodes::Block;

typedef adjacency_list<listS, listS, undirectedS, Block> Graph;

template <class Graph>
struct exercise_vertex {
  exercise_vertex(Graph& g_) : g(g_) {}
  Graph& g;

  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

  void operator()(Vertex v) const {
    std::cout << "adjacent vertices: ";
    typename property_map<Graph, std::string Nodes::Block::*>::type name =
        get(&Block::name, g);
    typename graph_traits<Graph>::adjacency_iterator ai;
    typename graph_traits<Graph>::adjacency_iterator ai_end;
    for (boost::tie(ai, ai_end) = adjacent_vertices(v, g); ai != ai_end; ++ai)
      std::cout << name[*ai] << " ";
    std::cout << std::endl;
  }
};

int main() {

  //-------------------------------------------------------------------------//
  Graph g;
  auto office1 = add_vertex(Block{"office1"}, g);
  auto office2 = add_vertex(Block{"office2"}, g);
  auto warehouse1 = add_vertex(Block{"warehouse1"}, g);

  // get the property map for vertex indices
  auto names = get(&Nodes::Block::name, g);

  std::cout << "vertices(g) = ";
  typedef graph_traits<Graph>::vertex_iterator vertex_iter;
  std::pair<vertex_iter, vertex_iter> vp = vertices(g);

  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

  // std::for_each(vp.first, vp.second, exercise_vertex<Graph>(g));
  std::for_each(vp.first, vp.second,
                [&](const Vertex& v) { std::cout << names[v] << " "; });
  // for (vp = vertices(g); vp.first != vp.second; ++vp.first)
  //   std::cout << names[*vp.first] << " ";
  std::cout << "|" << names[*vp.first] << "|";
  std::cout << std::endl;

  auto edge1 = add_edge(warehouse1, office1, g);
  add_edge(warehouse1, office2, g);

  {
    std::ofstream dot_file("bin/graph.dot");
    dynamic_properties dp;

    dp.property("node_id", make_transform_value_property_map(
                               &Nodes::id_of, get(vertex_bundle, g)));
    dp.property("shape", make_transform_value_property_map(
                             &Nodes::shape_of, get(vertex_bundle, g)));
    dp.property("label", make_transform_value_property_map(
                             [](Block const& v) {
                               return lexical_cast<std::string>(v.id) + "-" +
                                      v.name;
                             },
                             get(vertex_bundle, g)));

    write_graphviz_dp(dot_file, g, dp);
  }

  print_graph(g, get(&Nodes::Block::name, g));

  return EXIT_SUCCESS;
}