7 #include "../math/utility.h"
8 #include "../math/LinearTransformations.h"
9 #include "../math/methods.h"
10 #include "../oo/ObjBase.h"
93 return self()->photo_;
100 self()->photo_.copyFrom(pho);
130 return self->fixed2D_;
137 return self()->fixed2D_;
145 self()->fixed2D_ = fps2d;
154 return self->fixed2D_.identityPoint(i);
161 return self->photo_.inside(
169 return self->photo_.color(
184 bool write(FILE* f,
bool bin,
unsigned int fg)
const {
192 bool read(FILE* f,
bool bin,
unsigned int fg) {
221 static char const* ptr =
typeid(*this).name();
230 return std::string(
ctype());
235 class BoundleAdjustment2D {
239 std::vector<Camera>& cam_;
240 std::vector<Rotation3D<double> > rot_;
241 std::vector<PhotoProjection<double> > pho_;
247 Pair(
size_t a,
size_t b,
249 i1_(a), i2_(b), v1_(v1), v2_(v2) {
252 std::vector<Pair> pairs_;
254 void setParameters(Vector<double>
const& v) {
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++));
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());
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));
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());
285 pho_[i_to].transformate(
286 rot_[i_to].transformate(
287 rot_[i_from].transformateInv(
288 BallProjection<double>(3, 1.0).transformate(
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));
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());
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));
331 Vector<double> init()
const {
332 return getParameters();
334 Vector<double> residure(Vector<double>
const& v)
const {
335 ((Parameters*)
this)->setParameters(v);
338 Matrix<double> jacobian(Vector<double>
const& v)
const {
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) {
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);
354 rot_[i_to].transformate(
355 rot_[i_from].transformateInv(
356 BallProjection<double>(3, 1.0).transformate(
364 rot_[i_from].transformateInv(
365 BallProjection<double>(3, 1.0).transformate(
371 rot_[i_from].jacobianInv(
372 BallProjection<double>(3, 1.0).transformate(
377 BallProjection<double>(3, 1.0).jacobian(
385 rot_[i_to].transformate(
386 rot_[i_from].transformateInv(
387 BallProjection<double>(3, 1.0).transformate(
395 rot_[i_from].transformateInv(
396 BallProjection<double>(3, 1.0).transformate(
402 rot_[i_from].jacobianInv(
403 BallProjection<double>(3, 1.0).transformate(
410 else if (j0 == i_to) {
414 rot_[i_to].transformate(
415 rot_[i_from].transformateInv(
416 BallProjection<double>(3, 1.0).transformate(
427 rot_[i_to].transformate(
428 rot_[i_from].transformateInv(
429 BallProjection<double>(3, 1.0).transformate(
437 rot_[i_from].transformateInv(
438 BallProjection<double>(3, 1.0).transformate(
447 for (
size_t k = 0; k < 3; ++k) {
448 ret.entry(i * 3 + k, j, -v_tr(k, 0));
454 Matrix<double> identity(Vector<double>
const& v)
const {
456 Matrix<double> ret(v.dimension(), v.dimension(), 0.0);
460 double averageResidure()
const {
461 Vector<double> res(residureV());
463 for (
size_t i = 0, I = res.dimension(); i < I; ++i) {
466 return sum / res.dimension();
468 size_t dimensinonI()
const {
469 return cam_.size() * 4;
471 size_t dimensionO()
const {
472 return pairs_.size() * 3;
479 F(Parameters& p): p_(p) {
481 Vector<double> operator()(Vector<double>
const& v)
const {
482 return p_.residure(v);
489 J(Parameters& p): p_(p) {
491 Matrix<double> operator()(Vector<double>
const& v)
const {
492 return p_.jacobian(v);
499 I(Parameters& p): p_(p) {
501 Matrix<double> operator()(Vector<double>
const& v)
const {
502 return p_.identity(v);
510 Stop(Parameters& p,
double t): p_(p), t_(t) {
512 bool operator()(
double r)
const {
513 return (r < p_.dimensionO() * t_);
517 BoundleAdjustment2D() {
519 ~BoundleAdjustment2D() {
521 double operator()(std::vector<Camera>* cs,
double threshold)
const {
523 Vector<double> v0(p.init());
525 return p.averageResidure();
537 static BoundleAdjustment2D bdl;
538 return bdl(cs, threshold);
544 #endif // gra_Camera_H__