/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file ProjectionFactor.h
 * @brief Basic bearing factor from 2D measurement
 * @author Chris Beall
 * @author Luca Carlone
 * @author Zsolt Kira
 */

#pragma once

#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/linear/HessianFactor.h>
#include <vector>
#include <gtsam_unstable/geometry/triangulation.h>
#include <boost/optional.hpp>
#include <boost/assign.hpp>

namespace gtsam {

  // default threshold for selective relinearization
  static double defaultLinThreshold = -1; // 1e-7; // 0.01
  // default threshold for retriangulation
  static double defaultTriangThreshold = 1e-7;

  /**
   * Structure for storing some state memory, used to speed up optimization
   * @addtogroup SLAM
   */
  class SmartProjectionFactorState {
  public:

    static int lastID;
    int ID;

    SmartProjectionFactorState() {
      ID = lastID++;
      calculatedHessian = false;
    }

    // Linearization point
    Values values;
    std::vector<Pose3> cameraPosesLinearization;

    // Triangulation at current linearization point
    Point3 point;
    std::vector<Pose3> cameraPosesTriangulation;
    bool degenerate;
    bool cheiralityException;

    // Overall reprojection error
    double overallError;
    std::vector<Pose3> cameraPosesError;

    // Hessian representation (after Schur complement)
    bool calculatedHessian;
    Matrix H;
    Vector gs_vector;
    std::vector<Matrix> Gs;
    std::vector<Vector> gs;
    double f;

    // C = Hl'Hl
    // Cinv = inv(Hl'Hl)
    // Matrix3 Cinv;
    // E = Hx'Hl
    // w = Hl'b
  };

  int SmartProjectionFactorState::lastID = 0;

  /**
   * The calibration is known here.
   * @addtogroup SLAM
   */
  template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
  class SmartProjectionFactor: public NonlinearFactor {
  protected:

    // Keep a copy of measurement and calibration for I/O
    std::vector<Point2> measured_;                    ///< 2D measurement for each of the m views
    const SharedNoiseModel noise_;   ///< noise model used
    ///< (important that the order is the same as the keys that we use to create the factor)
    boost::shared_ptr<CALIBRATION> K_;  ///< shared pointer to calibration object

    double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
    double linearizationThreshold; ///< threshold to decide whether to re-linearize

    boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
    boost::shared_ptr<SmartProjectionFactorState> state_;

    // verbosity handling for Cheirality Exceptions
    bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
    bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)

  public:

    /// shorthand for base class type
    typedef NonlinearFactor Base;

    /// shorthand for this class
    typedef SmartProjectionFactor<POSE, LANDMARK, CALIBRATION> This;

    /// shorthand for a smart pointer to a factor
    typedef boost::shared_ptr<This> shared_ptr;

    /// shorthand for smart projection factor state variable
    typedef boost::shared_ptr<SmartProjectionFactorState> SmartFactorStatePtr;

    /// Default constructor
    SmartProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {}

    /**
     * Constructor
     * @param poseKeys is the set of indices corresponding to the cameras observing the same landmark
     * @param measured is the 2m dimensional location of the projection of a single landmark in the m views (the measurements)
     * @param model is the standard deviation (current version assumes that the uncertainty is the same for all views)
     * @param K shared pointer to the constant calibration
     * @param body_P_sensor is the transform from body to sensor frame (default identity)
     */
    SmartProjectionFactor(std::vector<Key> poseKeys, // camera poses
        const std::vector<Point2> measured,          // pixel measurements
        const SharedNoiseModel& model,               // noise model (same for all measurements)
        const boost::shared_ptr<CALIBRATION>& K,     // calibration matrix (same for all measurements)
        boost::optional<POSE> body_P_sensor = boost::none,
        SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
          measured_(measured), noise_(model), K_(K),
          retriangulationThreshold(defaultTriangThreshold), linearizationThreshold(defaultLinThreshold),
          body_P_sensor_(body_P_sensor),
          state_(state), throwCheirality_(false), verboseCheirality_(false) {
      keys_.assign(poseKeys.begin(), poseKeys.end());
    }

    /**
     * Constructor
     * @param poseKeys is the set of indices corresponding to the cameras observing the same landmark
     * @param measured is the 2m dimensional location of the projection of a single landmark in the m views (the measurements)
     * @param model is the standard deviation (current version assumes that the uncertainty is the same for all views)
     * @param K shared pointer to the constant calibration
     * @param body_P_sensor is the transform from body to sensor frame (default identity)
     */
    SmartProjectionFactor(std::vector<Key> poseKeys, // camera poses
        const std::vector<Point2> measured,          // pixel measurements
        const SharedNoiseModel& model,               // noise model (same for all measurements)
        const boost::shared_ptr<CALIBRATION>& K,     // calibration matrix (same for all measurements)
        const double linThreshold,
        boost::optional<POSE> body_P_sensor = boost::none,
        SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
          measured_(measured), noise_(model), K_(K),
          retriangulationThreshold(defaultTriangThreshold), linearizationThreshold(linThreshold),
          body_P_sensor_(body_P_sensor),
          state_(state), throwCheirality_(false), verboseCheirality_(false) {
      keys_.assign(poseKeys.begin(), poseKeys.end());
    }

    /**
     * Constructor with exception-handling flags
     * TODO: Mark argument order standard (keys, measurement, parameters)
     * @param measured is the 2m dimensional location of the projection of a single landmark in the m views (the measurements)
     * @param model is the standard deviation (current version assumes that the uncertainty is the same for all views)
     * @param poseKeys is the set of indices corresponding to the cameras observing the same landmark
     * @param K shared pointer to the constant calibration
     * @param throwCheirality determines whether Cheirality exceptions are rethrown
     * @param verboseCheirality determines whether exceptions are printed for Cheirality
     * @param body_P_sensor is the transform from body to sensor frame  (default identity)
     */
    SmartProjectionFactor(std::vector<Key> poseKeys,
        const std::vector<Point2> measured,
        const SharedNoiseModel& model,
        const boost::shared_ptr<CALIBRATION>& K,
        bool throwCheirality, bool verboseCheirality,
        boost::optional<POSE> body_P_sensor = boost::none,
        SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
          measured_(measured), noise_(model), K_(K),
          retriangulationThreshold(defaultTriangThreshold),  linearizationThreshold(defaultLinThreshold),
          body_P_sensor_(body_P_sensor),
          state_(state), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {
    }

    /**
     * Constructor with exception-handling flags
     * @param model is the standard deviation (current version assumes that the uncertainty is the same for all views)
     * @param K shared pointer to the constant calibration
     */
    SmartProjectionFactor(const SharedNoiseModel& model, const boost::shared_ptr<CALIBRATION>& K, 
        boost::optional<POSE> body_P_sensor = boost::none,
        SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
        noise_(model), K_(K), retriangulationThreshold(defaultTriangThreshold),  linearizationThreshold(defaultLinThreshold),
        body_P_sensor_(body_P_sensor), state_(state) {
    }

    /** Virtual destructor */
    virtual ~SmartProjectionFactor() {}

    /**
     * add a new measurement and pose key
     * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
     * @param poseKey is the index corresponding to the camera observing the same landmark
     */
    void add(const Point2 measured, const Key poseKey) {
      measured_.push_back(measured);
      keys_.push_back(poseKey);
    }

    // This function checks if the new linearization point is the same as the one used for previous triangulation
    // (if not, a new triangulation is needed)
    static bool decideIfTriangulate(std::vector<Pose3> cameraPoses, std::vector<Pose3> oldPoses, double retriangulationThreshold) {
      // several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate
      // Note that this is not yet "selecting linearization", that will come later, and we only check if the
      // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point

      // if we do not have a previous linearization point or the new linearization point includes more poses
      if(oldPoses.empty() || (cameraPoses.size() != oldPoses.size()))
        return true;

      for(size_t i = 0; i < cameraPoses.size(); i++) {
          if (!cameraPoses[i].equals(oldPoses[i], retriangulationThreshold)) {
            return true; // at least two poses are different, hence we retriangulate
          }
      }
      return false; // if we arrive to this point all poses are the same and we don't need re-triangulation
    }

    // This function checks if the new linearization point is 'close'  to the previous one used for linearization
    // (if not, a new linearization is needed)
    static bool decideIfLinearize(std::vector<Pose3> cameraPoses, std::vector<Pose3> oldPoses, double linearizationThreshold) {
      // "selective linearization"
      // The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose
      // (we only care about the "rigidity" of the poses, not about their absolute pose)

      // if we do not have a previous linearization point or the new linearization point includes more poses
      if(oldPoses.empty() || (cameraPoses.size() != oldPoses.size()))
        return true;

      Pose3 firstCameraPose;
      Pose3 firstCameraPoseOld;

      for(size_t i = 0; i < cameraPoses.size(); i++) {

          if(i==0){ // we store the initial pose, this is useful for selective re-linearization
            firstCameraPose = cameraPoses[i];
            firstCameraPoseOld = oldPoses[i];
            continue;
          }

          // we compare the poses in the frame of the first pose
          Pose3 localCameraPose = firstCameraPose.between(cameraPoses[i]);
          Pose3 localCameraPoseOld = firstCameraPoseOld.between(oldPoses[i]);

          if (!cameraPoses[i].equals(oldPoses[i], linearizationThreshold)) {
            return true; // at least two "relative" poses are different, hence we re-linerize
          }
      }
      return false; // if we arrive to this point all poses are the same and we don't need re-linerize
    }


    /**
     * print
     * @param s optional string naming the factor
     * @param keyFormatter optional formatter useful for printing Symbols
     */
    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
      std::cout << s << "SmartProjectionFactor, z = ";
      BOOST_FOREACH(const Point2& p, measured_) {
        std::cout << "measurement, p = "<< p << std::endl;
      }
      if(this->body_P_sensor_)
        this->body_P_sensor_->print("  sensor pose in body frame: ");
      Base::print("", keyFormatter);
    }

    /// equals
    virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
      const This *e = dynamic_cast<const This*>(&p);

      bool areMeasurementsEqual = true;
      for(size_t i = 0; i < measured_.size(); i++) {
        if(this->measured_.at(i).equals(e->measured_.at(i), tol) == false)
          areMeasurementsEqual = false;
        break;
      }

      return e
          && Base::equals(p, tol)
          && areMeasurementsEqual
          && this->K_->equals(*e->K_, tol)
          && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
    }

    /// get the dimension of the factor (number of rows on linearization)
    virtual size_t dim() const {
        return 6*keys_.size();
    }

    /// linearize returns a Hessianfactor that is an approximation of error(p)
    virtual boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {

      bool blockwise = false;                  // the full matrix version in faster
      int dim_landmark = 3;                    // for degenerate instances this will become 2 (direction-only information)
      
      // Create structures for Hessian Factors
      unsigned int numKeys = keys_.size();
      std::vector<Index> js;
      std::vector<Matrix> Gs(numKeys*(numKeys+1)/2);
      std::vector<Vector> gs(numKeys);
      double f=0;

      // Collect all poses (Cameras)
      std::vector<Pose3> cameraPoses;
      BOOST_FOREACH(const Key& k, keys_) {
        Pose3 cameraPose;
        if(body_P_sensor_)    {   cameraPose = values.at<Pose3>(k).compose(*body_P_sensor_);}
        else                  { cameraPose = values.at<Pose3>(k);}
        cameraPoses.push_back(cameraPose);
      }

      bool retriangulate = decideIfTriangulate(cameraPoses, state_->cameraPosesTriangulation, retriangulationThreshold);

      if(retriangulate) {// we store the current poses used for triangulation
        state_->cameraPosesTriangulation = cameraPoses;
      }

      if (retriangulate) {
        // We triangulate the 3D position of the landmark
        try {
          Point3 newPoint = triangulatePoint3(cameraPoses, measured_, *K_);
          // changeLinPoint = newPoint - state_->point;  // TODO: implement this check for the degenerate case
          state_->point = newPoint;
          state_->degenerate = false;
          state_->cheiralityException = false;
        } catch( TriangulationUnderconstrainedException& e) {
		  // point is triangulated at infinity
          //std::cout << "Triangulation failed " << e.what() << std::endl;
          BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
          BOOST_FOREACH(Vector& v, gs) v = zero(6);
          state_->degenerate = true;
          state_->cheiralityException = false;
          dim_landmark = 2;
          return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); // TODO: Debug condition, uncomment when fixed
        } catch( TriangulationCheiralityException& e) {
          // point is behind one of the cameras, turn factor off by setting everything to 0
          //std::cout << e.what() << std::end;
          BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
          BOOST_FOREACH(Vector& v, gs) v = zero(6);
          state_->cheiralityException = true; // TODO: Debug condition, uncomment when fixed
          return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); // TODO: Debug condition, uncomment when fixed
          // TODO: this is a debug condition, should be removed the comment
        }
      }
//      state_->degenerate = true; // TODO: this is a debug condition, should be removed
//      dim_landmark = 2; // TODO: this is a debug condition, should be removed the comment

      if (!retriangulate && state_->cheiralityException) {
        BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
        BOOST_FOREACH(Vector& v, gs) v = zero(6);
        return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
      }
      if (!retriangulate && state_->degenerate) {
        dim_landmark = 2;
      }

      bool doLinearize;
      if (linearizationThreshold >= 0){//by convention if linearizationThreshold is negative we always relinearize
        std::cout << "Temporary disabled" << std::endl;
        doLinearize = decideIfLinearize(cameraPoses, state_->cameraPosesLinearization, linearizationThreshold);
      }
      else{
        doLinearize = true;
      }

      if (doLinearize) {
        state_->cameraPosesLinearization = cameraPoses;
      }

      if(!doLinearize){ // return the previous Hessian factor
        return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f));
      }
      //otherwise redo linearization

      if (blockwise){
        // ==========================================================================================================
        std::cout << "Deprecated use of blockwise version. This is slower and no longer supported" << std::endl;
        blockwise = false;
        //        std::vector<Matrix> Hx(numKeys);
        //        std::vector<Matrix> Hl(numKeys);
        //        std::vector<Vector> b(numKeys);
        //
        //        for(size_t i = 0; i < measured_.size(); i++) {
        //          Pose3 pose = cameraPoses.at(i);
        //          PinholeCamera<CALIBRATION> camera(pose, *K_);
        //          b.at(i) = - ( camera.project(state_->point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector();
        //          noise_-> WhitenSystem(Hx.at(i), Hl.at(i), b.at(i));
        //          f += b.at(i).squaredNorm();
        //        }
        //
        //        // Shur complement trick
        //
        //        // Allocate m^2 matrix blocks
        //        std::vector< std::vector<Matrix> > Hxl(keys_.size(), std::vector<Matrix>( keys_.size()));
        //
        //        // Allocate inv(Hl'Hl)
        //        Matrix3 C = zeros(3,3);
        //        for(size_t i1 = 0; i1 < keys_.size(); i1++) {
        //          C.noalias() += Hl.at(i1).transpose() * Hl.at(i1);
        //        }
        //
        //        Matrix3 Cinv = C.inverse(); //  this is very important: without eval, because of eigen aliasing the results will be incorrect
        //
        //        // Calculate sub blocks
        //        for(size_t i1 = 0; i1 < keys_.size(); i1++) {
        //          for(size_t i2 = 0; i2 < keys_.size(); i2++) {
        //            // we only need the upper triangular entries
        //            Hxl[i1][i2].noalias() = Hx.at(i1).transpose() * Hl.at(i1) * Cinv * Hl.at(i2).transpose();
        //          }
        //        }
        //        // Populate Gs and gs
        //        int GsCount = 0;
        //        for(size_t i1 = 0; i1 < numKeys; i1++) {
        //          gs.at(i1).noalias() = Hx.at(i1).transpose() * b.at(i1);
        //
        //          for(size_t i2 = 0; i2 < numKeys; i2++) {
        //            gs.at(i1).noalias() -= Hxl[i1][i2] * b.at(i2);
        //
        //            if (i2 == i1){
        //              Gs.at(GsCount).noalias() = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2);
        //              GsCount++;
        //            }
        //            if (i2 > i1) {
        //              Gs.at(GsCount).noalias() = - Hxl[i1][i2] * Hx.at(i2);
        //              GsCount++;
        //            }
        //          }
        //        }
      }

      if (blockwise == false){ // version with full matrix multiplication
        // ==========================================================================================================
        Matrix Hx2 = zeros(2 * numKeys, 6 * numKeys);
        Matrix Hl2 = zeros(2 * numKeys, dim_landmark);
        Vector b2 = zero(2 * numKeys);

        if(state_->degenerate){
          for(size_t i = 0; i < measured_.size(); i++) {
            Pose3 pose = cameraPoses.at(i);
            PinholeCamera<CALIBRATION> camera(pose, *K_);
            if(i==0){ // first pose
              state_->point = camera.backprojectPointAtInfinity(measured_.at(i)); // 3D parametrization of point at infinity
              // std::cout << "point_ " << state_->point<< std::endl;
            }
            Matrix Hxi, Hli;
            Vector bi = -( camera.projectPointAtInfinity(state_->point,Hxi,Hli) - measured_.at(i) ).vector();
            // std::cout << "Hxi \n" << Hxi<< std::endl;
            // std::cout << "Hli \n" << Hli<< std::endl;

            noise_-> WhitenSystem(Hxi, Hli, bi);
            f += bi.squaredNorm();

            Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
            Hl2.block( 2*i, 0, 2, 2  ) = Hli;

            subInsert(b2,bi,2*i);
          }
          // std::cout << "Hx2 \n" << Hx2<< std::endl;
          // std::cout << "Hl2 \n" << Hl2<< std::endl;
        }
        else{

          for(size_t i = 0; i < measured_.size(); i++) {
            Pose3 pose = cameraPoses.at(i);
            PinholeCamera<CALIBRATION> camera(pose, *K_);
            Matrix Hxi, Hli;

            Vector bi;
            try {
              bi = -( camera.project(state_->point,Hxi,Hli) - measured_.at(i) ).vector();
            } catch ( CheiralityException& e) {
              std::cout << "Cheirality exception " << state_->ID << std::endl;
              exit(EXIT_FAILURE);
            }
            noise_-> WhitenSystem(Hxi, Hli, bi);
            f += bi.squaredNorm();

            Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
            Hl2.block( 2*i, 0, 2, 3  ) = Hli;

            subInsert(b2,bi,2*i);
          }
        }

        // Shur complement trick
        Matrix H(6 * numKeys, 6 * numKeys);
        Matrix C2;
        Vector gs_vector;

        C2.noalias() = (Hl2.transpose() * Hl2).inverse();
        H.noalias() = Hx2.transpose() * (Hx2 - (Hl2 * (C2 * (Hl2.transpose() * Hx2))));
        gs_vector.noalias() = Hx2.transpose() * (b2 - (Hl2 * (C2 * (Hl2.transpose() * b2))));

        // Populate Gs and gs
        int GsCount2 = 0;
        for(size_t i1 = 0; i1 < numKeys; i1++) {
          gs.at(i1) = sub(gs_vector, 6*i1, 6*i1 + 6);

          for(size_t i2 = 0; i2 < numKeys; i2++) {
            if (i2 >= i1) {
              Gs.at(GsCount2) = H.block(6*i1, 6*i2, 6, 6);
              GsCount2++;
            }
          }
        }
      }

      // ==========================================================================================================
//      if(linearizationThreshold >= 0){ // if we do not use selective relinearization we don't need to store these variables
//        state_->calculatedHessian = true;
//        state_->Gs = Gs;
//        state_->gs = gs;
//        state_->f = f;
//      }

      return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
    }

    /**
     * Calculate the error of the factor.
     * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
     * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
     * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
     */
    virtual double error(const Values& values) const {
      if (this->active(values)) {
        double overallError=0;

        // Collect all poses (Cameras)
        std::vector<Pose3> cameraPoses;
        BOOST_FOREACH(const Key& k, keys_) {
          Pose3 cameraPose;
          if(body_P_sensor_)    {   cameraPose = values.at<Pose3>(k).compose(*body_P_sensor_);}
          else                  { cameraPose = values.at<Pose3>(k);}
          cameraPoses.push_back(cameraPose);
        }

        bool retriangulate = decideIfTriangulate(cameraPoses, state_->cameraPosesTriangulation, retriangulationThreshold);

        if(retriangulate) {// we store the current poses used for triangulation
          state_->cameraPosesTriangulation = cameraPoses;
        }

        // We triangulate the 3D position of the landmark
        if (retriangulate) {
          try {
            state_->point = triangulatePoint3(cameraPoses, measured_, *K_);
            state_->degenerate = false;
            state_->cheiralityException = false;
          } catch( TriangulationCheiralityException& e) {
            // std::cout << "TriangulationCheiralityException "  << std::endl;
            // point is behind one of the cameras, turn factor off by setting everything to 0
            //std::cout << e.what() << std::end;
            state_->cheiralityException = true; // TODO: Debug condition, remove comment
            return 0.0; // TODO: this is a debug condition, should be removed the comment
          } catch( TriangulationUnderconstrainedException& e) {
            // point is triangulated at infinity
            //std::cout << e.what() << std::endl;
            state_->degenerate = true;
            state_->cheiralityException = false;
          }
        }
        // state_->degenerate = true; // TODO: this is a debug condition, should be removed

        if (!retriangulate && state_->cheiralityException) {
          return 0.0;
        }

        if(state_->degenerate){
          for(size_t i = 0; i < measured_.size(); i++) {
            Pose3 pose = cameraPoses.at(i);
            PinholeCamera<CALIBRATION> camera(pose, *K_);
            if(i==0){ // first pose
              state_->point = camera.backprojectPointAtInfinity(measured_.at(i)); // 3D parametrization of point at infinity
            }
            Point2 reprojectionError(camera.projectPointAtInfinity(state_->point) - measured_.at(i));
            overallError +=  0.5 * noise_->distance( reprojectionError.vector() );
            //overallError +=  reprojectionError.vector().norm();
          }
          return overallError;
        }
        else{
          for(size_t i = 0; i < measured_.size(); i++) {
            Pose3 pose = cameraPoses.at(i);
            PinholeCamera<CALIBRATION> camera(pose, *K_);

            try {
              Point2 reprojectionError(camera.project(state_->point) - measured_.at(i));
//std::cout << "Reprojection error: " << reprojectionError << std::endl;
              overallError += 0.5 * noise_->distance( reprojectionError.vector() );
              //overallError += reprojectionError.vector().norm();
            } catch ( CheiralityException& e) {
              std::cout << "Cheirality exception " << state_->ID << std::endl;
              exit(EXIT_FAILURE);
            }
          }
          return overallError;
        }
      } else { // else of active flag
        return 0.0;
      }
    }

    /** return the measurements */
    const Vector& measured() const {
      return measured_;
    }

    /** return the noise model */
    const SharedNoiseModel& noise() const {
      return noise_; 
    }

    /** return the landmark */
    boost::optional<Point3> point() const {
      return state_->point;
    }

    /** return the calibration object */
    inline const boost::shared_ptr<CALIBRATION> calibration() const {
      return K_;
    }

    /** return verbosity */
    inline bool verboseCheirality() const { return verboseCheirality_; }

    /** return flag for throwing cheirality exceptions */
    inline bool throwCheirality() const { return throwCheirality_; }

  private:

    /// Serialization function
    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int version) {
      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
      ar & BOOST_SERIALIZATION_NVP(measured_);
      ar & BOOST_SERIALIZATION_NVP(K_);
      ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
      ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
      ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
    }

  };

} // \ namespace gtsam