Templates -- Meow  1.1.2
不能,也不應該先編譯成obj-file的templates
Camera.h
Go to the documentation of this file.
1 #ifndef gra_Camera_H__
2 #define gra_Camera_H__
3 
4 #include "Photo.h"
5 #include "IdentityPoints.h"
6 #include "../Self.h"
7 #include "../math/utility.h"
8 #include "../math/LinearTransformations.h"
9 #include "../math/methods.h"
10 #include "../oo/ObjBase.h"
11 
12 namespace meow {
13 
22 template<class Pixel>
23 class Camera: public ObjBase {
24 public:
26 private:
27  struct Myself {
28  Photo<Pixel> photo_;
29  Rotation3D<double> rot_;
30  FixedPoints2D fixed2D_;
31 
32  Myself() {
33  fixed2D_.dimension(2);
34  }
35  ~Myself() {
36  }
37  Myself& copyFrom(Myself const& b) {
38  photo_ .copyFrom(b. photo_);
39  rot_ .copyFrom(b. rot_);
40  fixed2D_.copyFrom(b.fixed2D_);
41  return *this;
42  }
43  };
44 
45  Self<Myself> const self;
46 public:
50  Camera(): self(true) {
51  }
52 
56  Camera(Camera const& b): self(false) {
57  copyFrom(b);
58  }
59 
63  ~Camera() {
64  }
65 
69  Camera& copyFrom(Camera const& b) {
70  self().copyFrom(b.self);
71  return *this;
72  }
73 
78  self().referenceFrom(b.self);
79  return *this;
80  }
81 
85  Photo<Pixel> const& photo() const {
86  return self->photo_;
87  }
88 
93  return self()->photo_;
94  }
95 
99  Photo<Pixel> const& photo(Photo<Pixel> const& pho) {
100  self()->photo_.copyFrom(pho);
101  return photo();
102  }
103 
107  Rotation3D<double> const& rotation() const {
108  return self->rot_;
109  }
110 
115  return self()->rot_;
116  }
117 
122  self()->rot_ = rot;
123  return rotation();
124  }
125 
129  FixedPoints2D const& fixedPoints2D() const {
130  return self->fixed2D_;
131  }
132 
137  return self()->fixed2D_;
138  }
139 
143  FixedPoints2D const& fixedPoints2D(FixedPoints2D const& fps2d) const {
144  if (fps2d.dimension() == 2) {
145  self()->fixed2D_ = fps2d;
146  }
147  return fixedPoints2D();
148  }
149 
154  return self->fixed2D_.identityPoint(i);
155  }
156 
160  bool inside(Vector3D<double> p) const {
161  return self->photo_.inside(
162  Vector3D<double>(self->rot_.transformate(p.matrix())));
163  }
164 
168  Pixel color(Vector3D<double> p) const {
169  return self->photo_.color(
170  Vector3D<double>(self->rot_.transformate(p.matrix())));
171  }
172 
176  Camera& operator=(Camera const& b) {
177  return copyFrom(b);
178  }
179 
184  bool write(FILE* f, bool bin, unsigned int fg) const {
185  return false;
186  }
187 
192  bool read(FILE* f, bool bin, unsigned int fg) {
193  return false;
194  }
195 
200  ObjBase* create() const {
201  return new Camera();
202  }
203 
212  ObjBase* copyFrom(ObjBase const* b) {
213  return &(copyFrom(*(Camera*)b));
214  }
215 
220  char const* ctype() const{
221  static char const* ptr = typeid(*this).name();
222  return ptr;
223  }
224 
229  std::string type() const {
230  return std::string(ctype());
231  }
232 
234 private:
235  class BoundleAdjustment2D {
236  private:
237  class Parameters {
238  private:
239  std::vector<Camera>& cam_;
240  std::vector<Rotation3D<double> > rot_;
241  std::vector<PhotoProjection<double> > pho_;
242  struct Pair {
243  size_t i1_;
244  size_t i2_;
245  Vector<double> v1_;
246  Vector<double> v2_;
247  Pair(size_t a, size_t b,
248  Vector<double> const& v1, Vector<double> const& v2):
249  i1_(a), i2_(b), v1_(v1), v2_(v2) {
250  }
251  };
252  std::vector<Pair> pairs_;
253 
254  void setParameters(Vector<double> const& v) {
255  size_t n = 0;
256  for (size_t i = 0, I = cam_.size(); i < I; ++i) {
257  pho_[i].focal(v(n++));
258  for (size_t j = 0; j < 3; j++) {
259  rot_[i].theta(j, v(n++));
260  }
261  }
262  for (size_t i = 0, I = pairs_.size(); i < I; ++i) {
263  pairs_[i].v1_.entry(2, pho_[pairs_[i].i1_].focal());
264  pairs_[i].v2_.entry(2, pho_[pairs_[i].i2_].focal());
265  }
266  }
267  Vector<double> getParameters() const {
268  Vector<double> ret(cam_.size() * 4, 0.0);
269  for (size_t i = 0, I = cam_.size(); i < I; ++i) {
270  ret.entry(i * 4, pho_[i].focal());
271  for (size_t j = 0; j < 3; ++j) {
272  ret.entry(i * 4 + 1 + j, rot_[i].theta(j));
273  }
274  }
275  return ret;
276  }
277  Vector<double> residureV() const {
278  Vector<double> ret(pairs_.size() * 3, 0.0);
279  for (size_t i = 0, I = pairs_.size(); i < I; ++i) {
280  size_t i_from = pairs_[i].i1_;
281  size_t i_to = pairs_[i].i2_;
282  Matrix<double> v_from(pairs_[i].v1_.matrix());
283  Matrix<double> v_to (pairs_[i].v2_.matrix());
284  Matrix<double> v_tr(
285  pho_[i_to].transformate(
286  rot_[i_to].transformate(
287  rot_[i_from].transformateInv(
288  BallProjection<double>(3, 1.0).transformate(
289  v_from
290  )
291  )
292  )
293  )
294  );
295  Matrix<double> delta(v_to - v_tr);
296  for (size_t j = 0; j < 3; ++j) {
297  ret.entry(i * 3 + j, delta(j, 0));
298  }
299  }
300  return ret;
301  }
302  public:
303  Parameters(std::vector<Camera>& cam): cam_(cam) {
304  rot_.resize(cam_.size());
305  pho_.resize(cam_.size(), PhotoProjection<double>(3));
306  for (size_t i = 0, I = cam_.size(); i < I; ++i) {
307  rot_[i].referenceFrom(cam_[i].rotation());
308  pho_[i].focal(cam_[i].photo().focal());
309  }
310  for (size_t i = 0, I = cam_.size(); i < I; ++i) {
311  std::map<int,Vector<double> >const& p1 = (
312  cam_[i].fixedPoints2D().identityPoints());
313  for (size_t j = 0; j < I; ++j) {
314  if (i == j) continue;
315  std::map<int,Vector<double> >const& p2 = (
316  cam_[j].fixedPoints2D().identityPoints());
317  for (std::map<int,Vector<double> >::const_iterator
318  it1 = p1.begin(); it1 != p1.end(); ++it1) {
319  for (std::map<int,Vector<double> >::const_iterator
320  it2 = p2.begin(); it2 != p2.end(); ++it2) {
321  if (it1->first != it2->first) continue;
322  Vector<double> v1(it1->second), v2(it2->second);
323  v1.dimension(3, 0.0);
324  v2.dimension(3, 0.0);
325  pairs_.push_back(Pair(i, j, v1, v2));
326  }
327  }
328  }
329  }
330  }
331  Vector<double> init() const {
332  return getParameters();
333  }
334  Vector<double> residure(Vector<double> const& v) const {
335  ((Parameters*)this)->setParameters(v);
336  return residureV();
337  }
338  Matrix<double> jacobian(Vector<double> const& v) const {
339  //setParameters(v);
340  Matrix<double> ret(pairs_.size() * 3, v.dimension(), 0.0);
341  for (size_t i = 0, I = pairs_.size(); i < I; ++i) {
342  for (size_t j = 0, J = v.dimension(); j < J; ++j) {
343  size_t j0 = j / 4;
344  size_t dj = j % 4;
345  size_t i_from = pairs_[i].i1_;
346  size_t i_to = pairs_[i].i2_;
347  Matrix<double> v_from(pairs_[i].v1_.matrix());
348  Matrix<double> v_to (pairs_[i].v2_.matrix());
349  Matrix<double> v_tr (3, 1, 0.0);
350  if (j0 == i_from) {
351  if (dj == 0) {
352  v_tr = (
353  pho_[i_to].jacobian(
354  rot_[i_to].transformate(
355  rot_[i_from].transformateInv(
356  BallProjection<double>(3, 1.0).transformate(
357  v_from
358  )
359  )
360  )
361  )
362  *
363  rot_[i_to].jacobian(
364  rot_[i_from].transformateInv(
365  BallProjection<double>(3, 1.0).transformate(
366  v_from
367  )
368  )
369  )
370  *
371  rot_[i_from].jacobianInv(
372  BallProjection<double>(3, 1.0).transformate(
373  v_from
374  )
375  )
376  *
377  BallProjection<double>(3, 1.0).jacobian(
378  v_from
379  ).col(2)
380  );
381  }
382  else {
383  v_tr = (
384  pho_[i_to].jacobian(
385  rot_[i_to].transformate(
386  rot_[i_from].transformateInv(
387  BallProjection<double>(3, 1.0).transformate(
388  v_from
389  )
390  )
391  )
392  )
393  *
394  rot_[i_to].jacobian(
395  rot_[i_from].transformateInv(
396  BallProjection<double>(3, 1.0).transformate(
397  v_from
398  )
399  )
400  )
401  *
402  rot_[i_from].jacobianInv(
403  BallProjection<double>(3, 1.0).transformate(
404  v_from
405  )
406  )
407  );
408  }
409  }
410  else if (j0 == i_to) {
411  if (dj == 0) {
412  v_tr = (
413  pho_[i_to].jacobian(
414  rot_[i_to].transformate(
415  rot_[i_from].transformateInv(
416  BallProjection<double>(3, 1.0).transformate(
417  v_from
418  )
419  )
420  )
421  ).col(2)
422  );
423  }
424  else {
425  v_tr = (
426  pho_[i_to].jacobian(
427  rot_[i_to].transformate(
428  rot_[i_from].transformateInv(
429  BallProjection<double>(3, 1.0).transformate(
430  v_from
431  )
432  )
433  )
434  )
435  *
436  rot_[i_to].jacobian(
437  rot_[i_from].transformateInv(
438  BallProjection<double>(3, 1.0).transformate(
439  v_from
440  )
441  ),
442  dj - 1
443  )
444  );
445  }
446  }
447  for (size_t k = 0; k < 3; ++k) {
448  ret.entry(i * 3 + k, j, -v_tr(k, 0));
449  }
450  }
451  }
452  return ret;
453  }
454  Matrix<double> identity(Vector<double> const& v) const {
455  //setParameters(v);
456  Matrix<double> ret(v.dimension(), v.dimension(), 0.0);
457  ret.identity();
458  return ret;
459  }
460  double averageResidure() const {
461  Vector<double> res(residureV());
462  double sum = 0;
463  for (size_t i = 0, I = res.dimension(); i < I; ++i) {
464  sum += res(i);
465  }
466  return sum / res.dimension();
467  }
468  size_t dimensinonI() const {
469  return cam_.size() * 4;
470  }
471  size_t dimensionO() const {
472  return pairs_.size() * 3;
473  }
474  };
475  class F {
476  private:
477  Parameters& p_;
478  public:
479  F(Parameters& p): p_(p) {
480  }
481  Vector<double> operator()(Vector<double> const& v) const {
482  return p_.residure(v);
483  }
484  };
485  class J {
486  private:
487  Parameters& p_;
488  public:
489  J(Parameters& p): p_(p) {
490  }
491  Matrix<double> operator()(Vector<double> const& v) const {
492  return p_.jacobian(v);
493  }
494  };
495  class I {
496  private:
497  Parameters& p_;
498  public:
499  I(Parameters& p): p_(p) {
500  }
501  Matrix<double> operator()(Vector<double> const& v) const {
502  return p_.identity(v);
503  }
504  };
505  class Stop {
506  private:
507  Parameters& p_;
508  double t_;
509  public:
510  Stop(Parameters& p, double t): p_(p), t_(t) {
511  }
512  bool operator()(double r) const {
513  return (r < p_.dimensionO() * t_);
514  }
515  };
516  public:
517  BoundleAdjustment2D() {
518  }
519  ~BoundleAdjustment2D() {
520  }
521  double operator()(std::vector<Camera>* cs, double threshold) const {
522  Parameters p(*cs);
523  Vector<double> v0(p.init());
524  levenbergMarquardt(F(p), J(p), I(p), v0, Stop(p, threshold), 100000);
525  return p.averageResidure();
526  }
527  };
528 public:
536  static double boundleAdjustment2D(std::vector<Camera>* cs, double threshold) {
537  static BoundleAdjustment2D bdl;
538  return bdl(cs, threshold);
539  }
540 };
541 
542 }
543 
544 #endif // gra_Camera_H__