aboutsummaryrefslogtreecommitdiffstats
path: root/meowpp/gra/Camera.h
diff options
context:
space:
mode:
Diffstat (limited to 'meowpp/gra/Camera.h')
-rw-r--r--meowpp/gra/Camera.h367
1 files changed, 42 insertions, 325 deletions
diff --git a/meowpp/gra/Camera.h b/meowpp/gra/Camera.h
index 45321de..2b6347c 100644
--- a/meowpp/gra/Camera.h
+++ b/meowpp/gra/Camera.h
@@ -32,13 +32,10 @@ private:
Myself() {
fixed2D_.dimension(2);
}
- ~Myself() {
+ Myself(Myself const& v):
+ photo_(v.photo_), rot_(v.rot_), fixed2D_(v.fixed2D_) {
}
- Myself& copyFrom(Myself const& b) {
- photo_ .copyFrom(b. photo_);
- rot_ .copyFrom(b. rot_);
- fixed2D_.copyFrom(b.fixed2D_);
- return *this;
+ ~Myself() {
}
};
@@ -47,14 +44,13 @@ public:
/*!
* @brief constructor
*/
- Camera(): self(true) {
+ Camera(): self() {
}
/*!
* @brief copy constructor
*/
- Camera(Camera const& b): self(false) {
- copyFrom(b);
+ Camera(Camera const& b): self(b.self, Self<Myself>::COPY_FROM) {
}
/*!
@@ -119,7 +115,7 @@ public:
* @brief 設定rotation
*/
Rotation3D<double> const& rotation(Rotation3D<double> const& rot) {
- self()->rot_ = rot;
+ self()->rot_.copyFrom(rot);
return rotation();
}
@@ -142,7 +138,7 @@ public:
*/
FixedPoints2D const& fixedPoints2D(FixedPoints2D const& fps2d) const {
if (fps2d.dimension() == 2) {
- self()->fixed2D_ = fps2d;
+ self()->fixed2D_.copyFrom(fps2d);
}
return fixedPoints2D();
}
@@ -150,7 +146,7 @@ public:
/*!
* @brief 取得編號為i的fixed points 2d
*/
- Vector<double> fixedPoints2D(int i) {
+ Vector<double> fixedPoint2D(int i) {
return self->fixed2D_.identityPoint(i);
}
@@ -159,7 +155,7 @@ public:
*/
bool inside(Vector3D<double> p) const {
return self->photo_.inside(
- Vector3D<double>(self->rot_.transformate(p.matrix())));
+ Vector3D<double>(rotation().transformate(p.matrix())));
}
/*!
@@ -167,7 +163,7 @@ public:
*/
Pixel color(Vector3D<double> p) const {
return self->photo_.color(
- Vector3D<double>(self->rot_.transformate(p.matrix())));
+ Vector3D<double>(rotation().transformate(p.matrix())));
}
/*!
@@ -182,7 +178,20 @@ public:
* @note 未完成
*/
bool write(FILE* f, bool bin, unsigned int fg) const {
- return false;
+ if (bin) {
+ double tmp;
+ for (size_t i = 0; i < 3; ++i) {
+ if (fwrite(&(tmp = rotation().theta(i)), sizeof(tmp), 1, f) < 1)
+ return false;
+ }
+ }
+ else {
+ for (size_t i = 0; i < 3; ++i) {
+ if (fprintf(f, "%f ", rotation().theta(i)) < 1) return false;
+ }
+ fprintf(f, "\n");
+ }
+ return (fixedPoints2D().write(f, bin, fg) && photo().write(f, bin, fg));
}
/*! @brief 將資料讀入
@@ -190,7 +199,23 @@ public:
* @note 未完成
*/
bool read(FILE* f, bool bin, unsigned int fg) {
- return false;
+ if (bin) {
+ double tmp;
+ for (size_t i = 0; i < 3; ++i) {
+ if (fread(&tmp, sizeof(tmp), 1, f) < 1) {
+ return false;
+ }
+ rotationGet().theta(i, tmp);
+ }
+ }
+ else {
+ double a;
+ for (size_t i = 0; i < 3; ++i) {
+ if (fscanf(f, "%lf", &a) < 1) return false;
+ rotationGet().theta(i, a);
+ }
+ }
+ return (fixedPoints2DGet().read(f, bin, fg) && photoGet().read(f, bin, fg));
}
/*! @brief new一個自己
@@ -229,316 +254,8 @@ public:
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);
- }
};
-}
+} // meow
#endif // gra_Camera_H__
generated by cgit v1.2.3 (git 2.25.1) at 2025-08-28 13:41:06 +0800