1 #ifndef BundleAdjustment_LM_H__
2 #define BundleAdjustment_LM_H__
8 #include "../math/methods.h"
9 #include "../math/Vector.h"
10 #include "../math/Matrix.h"
11 #include "../math/utility.h"
13 #include "../oo/ObjBase.h"
29 class NoOffsetController {
31 std::vector<MatchPair >* pairs_;
32 std::vector<Rotation3D <double> >* rot_;
33 std::vector<PhotoProjection<double> >* pho_;
37 NoOffsetController(std::vector<MatchPair >* pairs,
41 pairs_(pairs), rot_(rot), pho_(pho), bll_(3.0, 1.0), flag(fg) {
46 int i = (*pho_).size() - 1, n = 0;
47 v.entry(n++, (*pho_)[i].focal());
48 v.entry(n++, (*rot_)[i].theta(0));
49 v.entry(n++, (*rot_)[i].theta(1));
50 v.entry(n++, (*rot_)[i].theta(2));
54 for (
size_t n = 0, i = 0, I = (*rot_).size(); i < I; ++i) {
55 v.entry(n++, (*pho_)[i].focal());
56 v.entry(n++, (*rot_)[i].theta(0));
57 v.entry(n++, (*rot_)[i].theta(1));
58 v.entry(n++, (*rot_)[i].theta(2));
63 for (
size_t n = 0, i = 0, I = (*rot_).size(); i < I; ++i) {
64 if (flag ==
false && i != I - 1)
continue;
65 (*pho_)[i].focal(v(n++));
66 (*rot_)[i].theta(0, v(n++));
67 (*rot_)[i].theta(1, v(n++));
68 (*rot_)[i].theta(2, v(n++));
70 for (
typename std::vector<MatchPair>::iterator
71 it = (*pairs_).begin(), ed = (*pairs_).end(); it != ed; ++it) {
72 (*it).from_m.entry(2, 0, -(*pho_)[(*it).from_i].focal());
73 (*it). to_m.entry(2, 0, -(*pho_)[(*it). to_i].focal());
78 for (
typename std::vector<MatchPair>::iterator
79 it = (*pairs_).begin(), ed = (*pairs_).end(); it != ed; ++it, ++n) {
83 (*pho_)[(*it).to_i].transformate(
84 (*rot_)[(*it).to_i].transformate(
85 (*rot_)[(*it).from_i].transformateInv(
93 ret.entry(n * 2 , tr(0, 0));
94 ret.entry(n * 2 + 1, tr(1, 0));
102 printf(
"re = %20.5f ?? %f\n", ret.
length2(), 25.0 * (*pairs_).size());
109 ret.
reset((*pairs_).size() * 2, 4, 0.0);
112 ret.
reset((*pairs_).size() * 2, (*rot_).size() * 4, 0.0);
115 for (
typename std::vector<MatchPair>::iterator
116 it = (*pairs_).begin(), ed = (*pairs_).end(); it != ed; ++it, ++n) {
124 int fr = (flag ==
false ? 0 : (*it).from_i);
125 int to = (flag ==
false ? 0 : (*it). to_i);
126 if (flag !=
false || (*it).from_i == (*rot_).size() - 1) {
127 m = ja_A * ja_B * ja_C * bll_.jacobian((*it).from_m).
col(2);
128 ret.
entry(n * 2 , fr * 4, m(0, 0));
129 ret.
entry(n * 2 + 1, fr * 4, m(1, 0));
130 for (
size_t k = 0; k < 3; ++k) {
131 m = ja_A * ja_B * (*rot_)[(*it).from_i].jacobianInv(tr_A, k);
132 ret.
entry(n * 2 , fr * 4 + 1 + k, m(0, 0));
133 ret.
entry(n * 2 + 1, fr * 4 + 1 + k, m(1, 0));
136 if (flag !=
false || (*it).to_i == (*rot_).size() - 1) {
137 m = (*pho_)[(*it).to_i].jacobian(tr_C, 0);
138 ret.
entry(n * 2 , to * 4, m(0, 0));
139 ret.
entry(n * 2 + 1, to * 4, m(1, 0));
140 for (
size_t k = 0; k < 3; ++k) {
141 m = ja_A * (*rot_)[(*it).to_i].jacobian(tr_B, k);
142 ret.
entry(n * 2 , to * 4 + 1 + k, m(0, 0));
143 ret.
entry(n * 2 + 1, to * 4 + 1 + k, m(1, 0));
167 Myself(Myself
const& m): t_(m.t_) {
181 self(b.self,
Self<Myself>::COPY_FROM) {
208 size_t N = (*seq).size();
209 for (
size_t i = 0; i < N; ++i) {
210 if ( (*seq)[i].flag &
CAN_OFFSET )
return false;
211 if (!((*seq)[i].flag &
CAN_ROTATE))
return false;
212 if (!((*seq)[i].flag &
CAN_ZOOM ))
return false;
216 std::vector<FPS_K> beg(N), end(N);
218 for (
size_t i = 0; i < N; ++i) {
219 beg[i] = (*seq)[i].eye->camera().fixedPoints2D().identityPoints().begin();
220 end[i] = (*seq)[i].eye->camera().fixedPoints2D().identityPoints().end ();
222 std::vector<std::vector<std::vector<MatchPair> > > all_pairs(N);
223 for (
size_t i = 0; i < N; ++i) {
224 all_pairs[i].resize(N);
225 for (
size_t j = 0; j < N; ++j) {
226 if (i == j)
continue;
227 for (it1 = beg[i], it2 = beg[j]; it1 != end[i] && it2 != end[j]; ) {
228 if (it1->first < it2->first) ++it1;
229 else if(it1->first > it2->first) ++it2;
233 tmp.from_m = it1->second.matrix();
234 tmp.from_m.rows(3, -(*seq)[i].eye->camera().photo().focal());
236 tmp. to_m = it2->second.matrix();
237 tmp. to_m.rows(3, -(*seq)[j].eye->camera().photo().focal());
238 all_pairs[i][j].push_back(tmp);
246 std::vector<MatchPair > pairs;
247 std::vector<Rotation3D <double> > rot;
248 std::vector<PhotoProjection<double> > pho;
249 std::vector<int > sum (N, 0);
250 std::vector<int > big (N, 0);
251 std::vector<int > real(N);
252 std::vector<int > rev (N);
254 int best = -1, best_ct = -1;
255 for (
size_t i = 0; i < N; ++i) {
257 for (
size_t j = 0; j < N; ++j) {
258 d += all_pairs[i][j].size();
267 for (
size_t i = 0; i < N; ++i) {
271 for (
size_t j = 0; j < N; ++j) {
272 if ((
int)j == best) {
276 if (sum[j] < 0)
continue;
277 sum[j] += all_pairs[best][j].size();
278 if (all_pairs[j][big[j]].size() < all_pairs[j][best].size())
282 printf(
"%d same as %d\n", best, big[best]);
283 if (big[best] == best) {
284 rot.push_back((*seq)[big[best]].eye->camera() .rotation());
285 pho.push_back((*seq)[big[best]].eye->camera().photo().projection());
288 rot.push_back(rot[rev[big[best]]]);
289 pho.push_back(pho[rev[big[best]]]);
291 for (
size_t j = 0; j < N; ++j) {
292 if (sum[j] >= 0)
continue;
293 for (
size_t k = 0, K = all_pairs[best][j].size(); k < K; ++k) {
294 all_pairs[best][j][k].from_i = rev[all_pairs[best][j][k].from_i];
295 all_pairs[best][j][k]. to_i = rev[all_pairs[best][j][k]. to_i];
296 pairs.push_back(all_pairs[best][j][k]);
298 for (
size_t k = 0, K = all_pairs[j][best].size(); k < K; ++k) {
299 all_pairs[j][best][k].from_i = rev[all_pairs[j][best][k].from_i];
300 all_pairs[j][best][k]. to_i = rev[all_pairs[j][best][k]. to_i];
301 pairs.push_back(all_pairs[j][best][k]);
304 for (
size_t j = 0; j < N; ++j) { printf(
"%4d ", sum[j]); } printf(
"\n");
305 for (
size_t j = 0; j < N; ++j) { printf(
"%4d ", big[j]); } printf(
"\n");
306 for (
size_t j = 0; j < N; ++j) { printf(
"%4d ", real[j]); } printf(
"\n");
310 NoOffsetController ct1(&pairs, &rot, &pho,
false);
325 for (
size_t j = 0; j < N; ++j) {
326 if (best_ct < sum[j]) {
335 for (
size_t i = 0; i < N; ++i) {
337 (*seq)[id].eye->cameraGet() .rotation (rot[i]);
338 (*seq)[id].eye->cameraGet().photoGet().projection(pho[i]);
348 bool write(FILE* f,
bool bin,
unsigned int fg)
const {
352 bool read(FILE* f,
bool bin,
unsigned int fg)
const {
365 return typeid(*this).name();
369 return std::string(
ctype());
375 #endif // BundleAdjustment_LM_H__
Matrix col(size_t c) const
Return the c -th column.
Vector< Scalar > levenbergMarquardtTraining(Function &f, Vector< Scalar > const &init, Scalar const &init_mu, Scalar const &mu_pow, Scalar const &er_max, int retry_number, int counter)
Scalar length2() const
same as (*this).dot(*this)
char const * ctype() const
用C-style string回傳這個class的type name
std::string type() const
用std::string回傳這個class的type name
bool write(FILE *f, bool bin, unsigned int fg) const
將物件寫入檔案, 預設implement為直接回傳 false
BundleAdjustment_LM & referenceFrom(BundleAdjustment_LM const &b)
一切物件的Base, 並要求每個物件都要有read, write, create, ... 等功能
ObjBase * copyFrom(ObjBase const *o)
複製, 預設使用operator=
bool adjustFixedPoint(std::vector< SceneInfo< Pixel > > *seq) const
double threshold(double t)
BundleAdjustment_LM(BundleAdjustment_LM const &b)
bool read(FILE *f, bool bin, unsigned int fg) const
Matrix & identitied()
Let itself be an identity matrix.
Entry entry(size_t r, size_t c) const
Access the entry at r x c.
void reset(size_t r, size_t c, Entry const &e)
reset the size of the matrix to r x c with entry all be e
bool adjustEye(std::vector< SceneInfo< Pixel > > *seq) const
IdentityPointsMap::const_iterator IdentityPointsMapIterK
BundleAdjustment_LM & copyFrom(BundleAdjustment_LM const &b)
ObjBase * create() const
回傳一個new出來的物件, 預設implement為直接回傳 NULL