diff options
Diffstat (limited to 'meowpp/gra/Camera.h')
| -rw-r--r-- | meowpp/gra/Camera.h | 544 |
1 files changed, 544 insertions, 0 deletions
diff --git a/meowpp/gra/Camera.h b/meowpp/gra/Camera.h new file mode 100644 index 0000000..45321de --- /dev/null +++ b/meowpp/gra/Camera.h @@ -0,0 +1,544 @@ +#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__ |