diff options
Diffstat (limited to 'meowpp/gra/BundleAdjustment_LM.h')
| -rw-r--r-- | meowpp/gra/BundleAdjustment_LM.h | 375 |
1 files changed, 375 insertions, 0 deletions
diff --git a/meowpp/gra/BundleAdjustment_LM.h b/meowpp/gra/BundleAdjustment_LM.h new file mode 100644 index 0000000..7a86666 --- /dev/null +++ b/meowpp/gra/BundleAdjustment_LM.h @@ -0,0 +1,375 @@ +#ifndef BundleAdjustment_LM_H__ +#define BundleAdjustment_LM_H__ + +#include "Eye.h" + +#include "BundleAdjustment.h" + +#include "../math/methods.h" +#include "../math/Vector.h" +#include "../math/Matrix.h" +#include "../math/utility.h" + +#include "../oo/ObjBase.h" + +#include <algorithm> + +namespace meow { + +template<class Pixel> +class BundleAdjustment_LM: public BundleAdjustment<Pixel> { +private: + struct MatchPair { + size_t from_i; + Matrix<double> from_m; + size_t to_i; + Matrix<double> to_m; + }; + + class NoOffsetController { + private: + std::vector<MatchPair >* pairs_; + std::vector<Rotation3D <double> >* rot_; + std::vector<PhotoProjection<double> >* pho_; + BallProjection<double> bll_; + bool flag; + public: + NoOffsetController(std::vector<MatchPair >* pairs, + std::vector<Rotation3D<double> >* rot, + std::vector<PhotoProjection<double> >* pho, + bool fg): + pairs_(pairs), rot_(rot), pho_(pho), bll_(3.0, 1.0), flag(fg) { + } + Vector<double> init() { + if (flag == false) { + Vector<double> v(4, 0.0); + int i = (*pho_).size() - 1, n = 0; + v.entry(n++, (*pho_)[i].focal()); + v.entry(n++, (*rot_)[i].theta(0)); + v.entry(n++, (*rot_)[i].theta(1)); + v.entry(n++, (*rot_)[i].theta(2)); + return v; + } + Vector<double> v((*rot_).size() * 4, 0.0); + for (size_t n = 0, i = 0, I = (*rot_).size(); i < I; ++i) { + v.entry(n++, (*pho_)[i].focal()); + v.entry(n++, (*rot_)[i].theta(0)); + v.entry(n++, (*rot_)[i].theta(1)); + v.entry(n++, (*rot_)[i].theta(2)); + } + return v; + } + Vector<double> residure(Vector<double> const& v) { + for (size_t n = 0, i = 0, I = (*rot_).size(); i < I; ++i) { + if (flag == false && i != I - 1) continue; + (*pho_)[i].focal(v(n++)); + (*rot_)[i].theta(0, v(n++)); + (*rot_)[i].theta(1, v(n++)); + (*rot_)[i].theta(2, v(n++)); + } + for (typename std::vector<MatchPair>::iterator + it = (*pairs_).begin(), ed = (*pairs_).end(); it != ed; ++it) { + (*it).from_m.entry(2, 0, -(*pho_)[(*it).from_i].focal()); + (*it). to_m.entry(2, 0, -(*pho_)[(*it). to_i].focal()); + } + // + Vector<double> ret((*pairs_).size() * 2, 0.0); + size_t n = 0; + for (typename std::vector<MatchPair>::iterator + it = (*pairs_).begin(), ed = (*pairs_).end(); it != ed; ++it, ++n) { + Matrix<double> tr( + (*it).to_m + - + (*pho_)[(*it).to_i].transformate( + (*rot_)[(*it).to_i].transformate( + (*rot_)[(*it).from_i].transformateInv( + bll_.transformate( + (*it).from_m + ) + ) + ) + ) + ); + ret.entry(n * 2 , tr(0, 0)); + ret.entry(n * 2 + 1, tr(1, 0)); + } + //* + //for (size_t i = 0; i < ret.dimension(); i++) { + // printf("(%-9.1f ", ret(i)); + // if (i % 8 == 7) printf("\n"); + //} + //printf("\n"); + printf("re = %20.5f ?? %f\n", ret.length2(), 25.0 * (*pairs_).size()); + // */ + return ret; + } + Matrix<double> jacobian() const { + Matrix<double> ret; + if (flag == false) { + ret.reset((*pairs_).size() * 2, 4, 0.0); + } + else { + ret.reset((*pairs_).size() * 2, (*rot_).size() * 4, 0.0); + } + size_t n = 0; + for (typename std::vector<MatchPair>::iterator + it = (*pairs_).begin(), ed = (*pairs_).end(); it != ed; ++it, ++n) { + Matrix<double> tr_A( bll_ .transformate((*it).from_m)); + Matrix<double> tr_B((*rot_)[(*it).from_i].transformateInv(tr_A)); + Matrix<double> tr_C((*rot_)[(*it). to_i].transformate (tr_B)); + Matrix<double> ja_A((*pho_)[(*it). to_i].jacobian (tr_C)); + Matrix<double> ja_B((*rot_)[(*it). to_i].jacobian (tr_B)); + Matrix<double> ja_C((*rot_)[(*it).from_i].jacobianInv(tr_A)); + Matrix<double> m; + int fr = (flag == false ? 0 : (*it).from_i); + int to = (flag == false ? 0 : (*it). to_i); + if (flag != false || (*it).from_i == (*rot_).size() - 1) { + m = ja_A * ja_B * ja_C * bll_.jacobian((*it).from_m).col(2); + ret.entry(n * 2 , fr * 4, m(0, 0)); + ret.entry(n * 2 + 1, fr * 4, m(1, 0)); + for (size_t k = 0; k < 3; ++k) { + m = ja_A * ja_B * (*rot_)[(*it).from_i].jacobianInv(tr_A, k); + ret.entry(n * 2 , fr * 4 + 1 + k, m(0, 0)); + ret.entry(n * 2 + 1, fr * 4 + 1 + k, m(1, 0)); + } + } + if (flag != false || (*it).to_i == (*rot_).size() - 1) { + m = (*pho_)[(*it).to_i].jacobian(tr_C, 0); + ret.entry(n * 2 , to * 4, m(0, 0)); + ret.entry(n * 2 + 1, to * 4, m(1, 0)); + for (size_t k = 0; k < 3; ++k) { + m = ja_A * (*rot_)[(*it).to_i].jacobian(tr_B, k); + ret.entry(n * 2 , to * 4 + 1 + k, m(0, 0)); + ret.entry(n * 2 + 1, to * 4 + 1 + k, m(1, 0)); + } + } + } + return ret; + } + Matrix<double> identity() const { + if (flag == false) { + Matrix<double> ret(4, 4, 0.0); + ret.identitied(); + return ret; + } + Matrix<double> ret((*rot_).size() * 4, (*rot_).size() * 4, 0.0); + ret.identitied(); + return ret; + } + }; + + struct Myself { + double t_; + + Myself(): t_(5.0) { + } + + Myself(Myself const& m): t_(m.t_) { + } + + ~Myself() { + } + }; + + Self<Myself> const self; + +public: + BundleAdjustment_LM(): self() { + } + + BundleAdjustment_LM(BundleAdjustment_LM const& b): + self(b.self, Self<Myself>::COPY_FROM) { + } + + ~BundleAdjustment_LM() { + } + + BundleAdjustment_LM& copyFrom(BundleAdjustment_LM const& b) { + self().copyFrom(b.self); + return *this; + } + + BundleAdjustment_LM& referenceFrom(BundleAdjustment_LM const& b) { + self().referenceFrom(b.self); + return *this; + } + + double threshold() const { + return self->t_; + } + + double threshold(double t) { + self()->t_ = t; + return threshold(); + } + + bool adjustEye(std::vector<SceneInfo<Pixel> >* seq) const { + // check + size_t N = (*seq).size(); + for (size_t i = 0; i < N; ++i) { + if ( (*seq)[i].flag & CAN_OFFSET ) return false; + if (!((*seq)[i].flag & CAN_ROTATE)) return false; + if (!((*seq)[i].flag & CAN_ZOOM )) return false; + } + // get all pairs + typedef typename Camera<Pixel>::FixedPoints2D::IdentityPointsMapIterK FPS_K; + std::vector<FPS_K> beg(N), end(N); + FPS_K it1, it2; + for (size_t i = 0; i < N; ++i) { + beg[i] = (*seq)[i].eye->camera().fixedPoints2D().identityPoints().begin(); + end[i] = (*seq)[i].eye->camera().fixedPoints2D().identityPoints().end (); + } + std::vector<std::vector<std::vector<MatchPair> > > all_pairs(N); + for (size_t i = 0; i < N; ++i) { + all_pairs[i].resize(N); + for (size_t j = 0; j < N; ++j) { + if (i == j) continue; + for (it1 = beg[i], it2 = beg[j]; it1 != end[i] && it2 != end[j]; ) { + if (it1->first < it2->first) ++it1; + else if(it1->first > it2->first) ++it2; + else { + MatchPair tmp; + tmp.from_i = i; + tmp.from_m = it1->second.matrix(); + tmp.from_m.rows(3, -(*seq)[i].eye->camera().photo().focal()); + tmp. to_i = j; + tmp. to_m = it2->second.matrix(); + tmp. to_m.rows(3, -(*seq)[j].eye->camera().photo().focal()); + all_pairs[i][j].push_back(tmp); + ++it1; + ++it2; + } + } + } + } + // + std::vector<MatchPair > pairs; + std::vector<Rotation3D <double> > rot; + std::vector<PhotoProjection<double> > pho; + std::vector<int > sum (N, 0); + std::vector<int > big (N, 0); + std::vector<int > real(N); + std::vector<int > rev (N); + // first!! + int best = -1, best_ct = -1; + for (size_t i = 0; i < N; ++i) { + int d = 0; + for (size_t j = 0; j < N; ++j) { + d += all_pairs[i][j].size(); + } + if (best_ct < d) { + best_ct = d; + best = i; + } + real[i] = i; + big [i] = i; + } + for (size_t i = 0; i < N; ++i) { + // update else + real[i] = best; + rev[best] = i; + for (size_t j = 0; j < N; ++j) { + if ((int)j == best) { + sum[j] = -1; + continue; + } + if (sum[j] < 0) continue; + sum[j] += all_pairs[best][j].size(); + if (all_pairs[j][big[j]].size() < all_pairs[j][best].size()) + big[j] = best; + } + // add me + printf("%d same as %d\n", best, big[best]); + if (big[best] == best) { + rot.push_back((*seq)[big[best]].eye->camera() .rotation()); + pho.push_back((*seq)[big[best]].eye->camera().photo().projection()); + } + else { + rot.push_back(rot[rev[big[best]]]); + pho.push_back(pho[rev[big[best]]]); + } + for (size_t j = 0; j < N; ++j) { + if (sum[j] >= 0) continue; + for (size_t k = 0, K = all_pairs[best][j].size(); k < K; ++k) { + all_pairs[best][j][k].from_i = rev[all_pairs[best][j][k].from_i]; + all_pairs[best][j][k]. to_i = rev[all_pairs[best][j][k]. to_i]; + pairs.push_back(all_pairs[best][j][k]); + } + for (size_t k = 0, K = all_pairs[j][best].size(); k < K; ++k) { + all_pairs[j][best][k].from_i = rev[all_pairs[j][best][k].from_i]; + all_pairs[j][best][k]. to_i = rev[all_pairs[j][best][k]. to_i]; + pairs.push_back(all_pairs[j][best][k]); + } + } + for (size_t j = 0; j < N; ++j) { printf("%4d ", sum[j]); } printf("\n"); + for (size_t j = 0; j < N; ++j) { printf("%4d ", big[j]); } printf("\n"); + for (size_t j = 0; j < N; ++j) { printf("%4d ", real[j]); } printf("\n"); + printf("\n"); + // bundle adjustment + if (i > 0) { + NoOffsetController ct1(&pairs, &rot, &pho, false); + ct1.residure(levenbergMarquardtTraining(ct1, ct1.init(), + 7.3, 1.0, + squ(threshold()) * pairs.size(), + 7, 1000)); + /* + NoOffsetController ct2(&pairs, &rot, &pho, true); + ct2.residure(levenbergMarquardtTraining(ct2, ct2.init(), + 0.1 * pairs.size(), 1.0, + squ(threshold()) * pairs.size(), + 7, 1000)); + // */ + } + // find next + best_ct = 0; + for (size_t j = 0; j < N; ++j) { + if (best_ct < sum[j]) { + best = j; + best_ct = sum[j]; + } + } + getchar(); + //if (i == N - 2) break; + } + // feedback + for (size_t i = 0; i < N; ++i) { + int id = real[i]; + (*seq)[id].eye->cameraGet() .rotation (rot[i]); + (*seq)[id].eye->cameraGet().photoGet().projection(pho[i]); + //if (i == N - 2) break; + } + return true; + } + + bool adjustFixedPoint(std::vector<SceneInfo<Pixel> >* seq) const { + return false; + } + + bool write(FILE* f, bool bin, unsigned int fg) const { + return false; + } + + bool read(FILE* f, bool bin, unsigned int fg) const { + return false; + } + + ObjBase* create() const { + return new BundleAdjustment_LM; + } + + ObjBase* copyFrom(ObjBase const* o) { + return &(copyFrom(*(BundleAdjustment_LM const*)o)); + } + + char const* ctype() const { + return typeid(*this).name(); + } + + std::string type() const { + return std::string(ctype()); + } +}; + +} // meow + +#endif // BundleAdjustment_LM_H__ |