path: root/meowpp/gra/Camera.h

#ifndef   gra_Camera_H__
#define   gra_Camera_H__

#include "Photo.h"
#include "IdentityPoints.h"
#include "../Self.h"
#include "../math/utility.h"
#include "../math/LinearTransformations.h"
#include "../math/methods.h"
#include "../oo/ObjBase.h"

namespace meow {

/*!
 * @brief Camera
 *
 * 實際上就是一個 \c Photo 加上一個 \c Rotation3D.  
 * 另外附有 fixedPoint, 可以用來定位時參考
 *
 * @author cat_leopard
 */
template<class Pixel>
class Camera: public ObjBase {
public:
  typedef IdentityPoints<int, double> FixedPoints2D;
private:
  struct Myself {
    Photo<Pixel>         photo_;
    Rotation3D<double>     rot_;
    FixedPoints2D      fixed2D_;

    Myself() {
      fixed2D_.dimension(2);
    }
    ~Myself() {
    }
    Myself& copyFrom(Myself const& b) {
      photo_  .copyFrom(b.  photo_);
      rot_    .copyFrom(b.    rot_);
      fixed2D_.copyFrom(b.fixed2D_);
      return *this;
    }
  };

  Self<Myself> const self;
public:
  /*!
   * @brief constructor
   */
  Camera(): self(true) {
  }
  
  /*!
   * @brief copy constructor
   */
  Camera(Camera const& b): self(false) {
    copyFrom(b);
  }

  /*!
   * @brief destructor
   */
  ~Camera() {
  }
  
  /*!
   * @brief 複製資料
   */
  Camera& copyFrom(Camera const& b) {
    self().copyFrom(b.self);
    return *this;
  }
  
  /*!
   * @brief 參照
   */
  Camera& referenceFrom(Camera const& b) {
    self().referenceFrom(b.self);
    return *this;
  }

  /*!
   * @brief 取得 photo
   */
  Photo<Pixel> const& photo() const {
    return self->photo_;
  }

  /*!
   * @brief 取得 photo (non-constant)
   */
  Photo<Pixel>& photoGet() {
    return self()->photo_;
  }

  /*!
   * @brief 設定 photo 
   */
  Photo<Pixel> const& photo(Photo<Pixel> const& pho) {
    self()->photo_.copyFrom(pho);
    return photo();
  }

  /*!
   * @brief 取得rotation
   */
  Rotation3D<double> const& rotation() const {
    return self->rot_;
  }

  /*!
   * @brief 取得rotation (non-constant)
   */
  Rotation3D<double>& rotationGet() {
    return self()->rot_;
  }

  /*!
   * @brief 設定rotation
   */
  Rotation3D<double> const& rotation(Rotation3D<double> const& rot) {
    self()->rot_ = rot;
    return rotation();
  }

  /*!
   * @brief 取得所有FixedPoint
   */
  FixedPoints2D const& fixedPoints2D() const {
    return self->fixed2D_;
  }

  /*!
   * @brief 取得所有FixedPoint(non-constant reference)
   */
  FixedPoints2D& fixedPoints2DGet() const {
    return self()->fixed2D_;
  }

  /*!
   * @brief 設定FixedPoint
   */
  FixedPoints2D const& fixedPoints2D(FixedPoints2D const& fps2d) const {
    if (fps2d.dimension() == 2) {
      self()->fixed2D_ = fps2d;
    }
    return fixedPoints2D();
  }

  /*!
   * @brief 取得編號為i的fixed points 2d
   */
  Vector<double> fixedPoints2D(int i) {
    return self->fixed2D_.identityPoint(i);
  }

  /*!
   * @brief 詢問某點是否在底片範圍內
   */
  bool inside(Vector3D<double> p) const {
    return self->photo_.inside(
      Vector3D<double>(self->rot_.transformate(p.matrix())));
  }

  /*!
   * @brief 取得底片color
   */
  Pixel color(Vector3D<double> p) const {
    return self->photo_.color(
      Vector3D<double>(self->rot_.transformate(p.matrix())));
  }
  
  /*!
   * @brief same as \c copyFrom(b)
   */
  Camera& operator=(Camera const& b) {
    return copyFrom(b);
  }

  /*! @brief 將資料寫入檔案
   *
   *  @note 未完成
   */
  bool write(FILE* f, bool bin, unsigned int fg) const {
    return false;
  }

  /*! @brief 將資料讀入
   *
   *  @note 未完成
   */
  bool read(FILE* f, bool bin, unsigned int fg) {
    return false;
  }

  /*! @brief new一個自己
   *
   *  @return 一個new出來的pointer
   */
  ObjBase* create() const {
    return new Camera();
  }

  /*! @brief 複製資料
   *
   * 輸入型別是 \c ObjBase \c const*
   * 事實上這個method就只是幫忙轉型然後呼叫原本的\c copyFrom
   *
   *  @param [in] b 資料來源
   *  @return this
   */
  ObjBase* copyFrom(ObjBase const* b) {
    return &(copyFrom(*(Camera*)b));
  }

  /*! @brief 回傳class的type
   *
   *  @return \c char \c const\c * 形式的typename
   */
  char const* ctype() const{
    static char const* ptr = typeid(*this).name();
    return ptr;
  }

  /*! @brief 回傳class的type
   *
   *  @return \c std::string 形式的typename
   */
  std::string type() const {
    return std::string(ctype());
  }
  
  ////////////////////////////////////////////////////////////////////
private:
  class BoundleAdjustment2D {
  private:
    class Parameters {
    private:
      std::vector<Camera>&                  cam_;
      std::vector<Rotation3D<double> >      rot_;
      std::vector<PhotoProjection<double> > pho_;
      struct Pair {
        size_t i1_;
        size_t i2_;
        Vector<double> v1_;
        Vector<double> v2_;
        Pair(size_t a, size_t b,
             Vector<double> const& v1, Vector<double> const& v2):
        i1_(a), i2_(b), v1_(v1), v2_(v2) {
        }
      };
      std::vector<Pair> pairs_;

      void setParameters(Vector<double> const& v) {
        size_t n = 0;
        for (size_t i = 0, I = cam_.size(); i < I; ++i) {
          pho_[i].focal(v(n++));
          for (size_t j = 0; j < 3; j++) {
            rot_[i].theta(j, v(n++));
          }
        }
        for (size_t i = 0, I = pairs_.size(); i < I; ++i) {
          pairs_[i].v1_.entry(2, pho_[pairs_[i].i1_].focal());
          pairs_[i].v2_.entry(2, pho_[pairs_[i].i2_].focal());
        }
      }
      Vector<double> getParameters() const {
        Vector<double> ret(cam_.size() * 4, 0.0);
        for (size_t i = 0, I = cam_.size(); i < I; ++i) {
          ret.entry(i * 4, pho_[i].focal());
          for (size_t j = 0; j < 3; ++j) {
            ret.entry(i * 4 + 1 + j, rot_[i].theta(j));
          }
        }
        return ret;
      }
      Vector<double> residureV() const {
        Vector<double> ret(pairs_.size() * 3, 0.0);
        for (size_t i = 0, I = pairs_.size(); i < I; ++i) {
          size_t i_from = pairs_[i].i1_;
          size_t i_to   = pairs_[i].i2_;
          Matrix<double> v_from(pairs_[i].v1_.matrix());
          Matrix<double> v_to  (pairs_[i].v2_.matrix());
          Matrix<double> v_tr(
            pho_[i_to].transformate(
              rot_[i_to].transformate(
                rot_[i_from].transformateInv(
                  BallProjection<double>(3, 1.0).transformate(
                    v_from
                  )
                )
              )
            )
          );
          Matrix<double> delta(v_to - v_tr);
          for (size_t j = 0; j < 3; ++j) {
            ret.entry(i * 3 + j, delta(j, 0));
          }
        }
        return ret;
      }
    public:
      Parameters(std::vector<Camera>& cam): cam_(cam) {
        rot_.resize(cam_.size());
        pho_.resize(cam_.size(), PhotoProjection<double>(3));
        for (size_t i = 0, I = cam_.size(); i < I; ++i) {
          rot_[i].referenceFrom(cam_[i].rotation());
          pho_[i].focal(cam_[i].photo().focal());
        }
        for (size_t i = 0, I = cam_.size(); i < I; ++i) {
          std::map<int,Vector<double> >const& p1 = (
            cam_[i].fixedPoints2D().identityPoints());
          for (size_t j = 0; j < I; ++j) {
            if (i == j) continue;
            std::map<int,Vector<double> >const& p2 = (
              cam_[j].fixedPoints2D().identityPoints());
            for (std::map<int,Vector<double> >::const_iterator
                 it1 = p1.begin(); it1 != p1.end(); ++it1) {
              for (std::map<int,Vector<double> >::const_iterator
                   it2 = p2.begin(); it2 != p2.end(); ++it2) {
                if (it1->first != it2->first) continue;
                Vector<double> v1(it1->second), v2(it2->second);
                v1.dimension(3, 0.0);
                v2.dimension(3, 0.0);
                pairs_.push_back(Pair(i, j, v1, v2));
              }
            }
          }
        }
      }
      Vector<double> init() const {
        return getParameters();
      }
      Vector<double> residure(Vector<double> const& v) const {
        ((Parameters*)this)->setParameters(v);
        return residureV();
      }
      Matrix<double> jacobian(Vector<double> const& v) const {
        //setParameters(v);
        Matrix<double> ret(pairs_.size() * 3, v.dimension(), 0.0);
        for (size_t i = 0, I = pairs_.size(); i < I; ++i) {
          for (size_t j = 0, J = v.dimension(); j < J; ++j) {
            size_t j0 = j / 4;
            size_t dj = j % 4;
            size_t i_from = pairs_[i].i1_;
            size_t i_to   = pairs_[i].i2_;
            Matrix<double> v_from(pairs_[i].v1_.matrix());
            Matrix<double> v_to  (pairs_[i].v2_.matrix());
            Matrix<double> v_tr  (3, 1, 0.0);
            if (j0 == i_from) {
              if (dj == 0) {
                v_tr = (
                  pho_[i_to].jacobian(
                    rot_[i_to].transformate(
                      rot_[i_from].transformateInv(
                        BallProjection<double>(3, 1.0).transformate(
                          v_from
                        )
                      )
                    )
                  )
                  *
                  rot_[i_to].jacobian(
                    rot_[i_from].transformateInv(
                      BallProjection<double>(3, 1.0).transformate(
                        v_from
                      )
                    )
                  )
                  *
                  rot_[i_from].jacobianInv(
                    BallProjection<double>(3, 1.0).transformate(
                      v_from
                    )
                  )
                  *
                  BallProjection<double>(3, 1.0).jacobian(
                    v_from
                  ).col(2)
                  );
              }
              else {
                v_tr = (
                  pho_[i_to].jacobian(
                    rot_[i_to].transformate(
                      rot_[i_from].transformateInv(
                        BallProjection<double>(3, 1.0).transformate(
                          v_from
                        )
                      )
                    )
                  )
                  *
                  rot_[i_to].jacobian(
                    rot_[i_from].transformateInv(
                      BallProjection<double>(3, 1.0).transformate(
                        v_from
                      )
                    )
                  )
                  *
                  rot_[i_from].jacobianInv(
                    BallProjection<double>(3, 1.0).transformate(
                      v_from
                    )
                  )
                  );
              }
            }
            else if (j0 == i_to) {
              if (dj == 0) {
                v_tr = (
                  pho_[i_to].jacobian(
                    rot_[i_to].transformate(
                      rot_[i_from].transformateInv(
                        BallProjection<double>(3, 1.0).transformate(
                          v_from
                        )
                      )
                    )
                  ).col(2)
                );
              }
              else {
                v_tr = (
                  pho_[i_to].jacobian(
                    rot_[i_to].transformate(
                      rot_[i_from].transformateInv(
                        BallProjection<double>(3, 1.0).transformate(
                          v_from
                        )
                      )
                    )
                  )
                  *
                  rot_[i_to].jacobian(
                    rot_[i_from].transformateInv(
                      BallProjection<double>(3, 1.0).transformate(
                        v_from
                      )
                    ),
                    dj - 1
                  )
                );
              }
            }
            for (size_t k = 0; k < 3; ++k) {
              ret.entry(i * 3 + k, j, -v_tr(k, 0));
            }
          }
        }
        return ret;
      }
      Matrix<double> identity(Vector<double> const& v) const {
        //setParameters(v);
        Matrix<double> ret(v.dimension(), v.dimension(), 0.0);
        ret.identity();
        return ret;
      }
      double averageResidure() const {
        Vector<double> res(residureV());
        double sum = 0;
        for (size_t i = 0, I = res.dimension(); i < I; ++i) {
          sum += res(i);
        }
        return sum / res.dimension();
      }
      size_t dimensinonI() const {
        return cam_.size() * 4;
      }
      size_t dimensionO() const {
        return pairs_.size() * 3;
      }
    };
    class F {
    private:
      Parameters& p_;
    public:
      F(Parameters& p): p_(p) {
      }
      Vector<double> operator()(Vector<double> const& v) const {
        return p_.residure(v);
      }
    };
    class J {
    private:
      Parameters& p_;
    public:
      J(Parameters& p): p_(p) {
      }
      Matrix<double> operator()(Vector<double> const& v) const {
        return p_.jacobian(v);
      }
    };
    class I {
    private:
      Parameters& p_;
    public:
      I(Parameters& p): p_(p) {
      }
      Matrix<double> operator()(Vector<double> const& v) const {
        return p_.identity(v);
      }
    };
    class Stop {
    private:
      Parameters& p_;
      double t_;
    public:
      Stop(Parameters& p, double t): p_(p), t_(t) {
      }
      bool operator()(double r) const {
        return (r < p_.dimensionO() * t_);
      }
    };
  public:
    BoundleAdjustment2D() {
    }
    ~BoundleAdjustment2D() {
    }
    double operator()(std::vector<Camera>* cs, double threshold) const {
      Parameters p(*cs);
      Vector<double> v0(p.init());
      levenbergMarquardt(F(p), J(p), I(p), v0, Stop(p, threshold), 100000);
      return p.averageResidure();
    }
  };
public:
  /*!
   * @brief 將數台camera用fixed points做boundle adjustment
   *
   * @param [in] cs        要調整的cameras
   * @param [in] threshold 允許誤差值
   * @return 誤差值
   */
  static double boundleAdjustment2D(std::vector<Camera>* cs, double threshold) {
    static BoundleAdjustment2D bdl;
    return bdl(cs, threshold);
  }
};

}

#endif // gra_Camera_H__