Skip to content

Commit 30b70ca

Browse files
tingelsttingelst
committed
Projected solutions
1 parent b322940 commit 30b70ca

File tree

4 files changed

+614
-166
lines changed

4 files changed

+614
-166
lines changed

‎python/ENGAGE2017.ipynb

Lines changed: 499 additions & 110 deletions
Large diffs are not rendered by default.

‎python/gacse.ipynb

Lines changed: 26 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313
},
1414
{
1515
"cell_type": "code",
16-
"execution_count": 2,
16+
"execution_count": 1,
1717
"metadata": {
1818
"collapsed": true
1919
},
@@ -24,7 +24,7 @@
2424
},
2525
{
2626
"cell_type": "code",
27-
"execution_count": 3,
27+
"execution_count": 2,
2828
"metadata": {
2929
"collapsed": false
3030
},
@@ -36,19 +36,11 @@
3636
},
3737
{
3838
"cell_type": "code",
39-
"execution_count": 4,
39+
"execution_count": 3,
4040
"metadata": {
4141
"collapsed": false
4242
},
4343
"outputs": [
44-
{
45-
"name": "stderr",
46-
"output_type": "stream",
47-
"text": [
48-
"/home/game/python/local/lib/python2.7/site-packages/matplotlib/font_manager.py:273: UserWarning: Matplotlib is building the font cache using fc-list. This may take a moment.\n",
49-
" warnings.warn('Matplotlib is building the font cache using fc-list. This may take a moment.')\n"
50-
]
51-
},
5244
{
5345
"name": "stdout",
5446
"output_type": "stream",
@@ -64,7 +56,7 @@
6456
},
6557
{
6658
"cell_type": "code",
67-
"execution_count": 5,
59+
"execution_count": 4,
6860
"metadata": {
6961
"collapsed": false
7062
},
@@ -75,7 +67,7 @@
7567
},
7668
{
7769
"cell_type": "code",
78-
"execution_count": 6,
70+
"execution_count": 5,
7971
"metadata": {
8072
"collapsed": false
8173
},
@@ -86,7 +78,7 @@
8678
},
8779
{
8880
"cell_type": "code",
89-
"execution_count": 7,
81+
"execution_count": 6,
9082
"metadata": {
9183
"collapsed": true
9284
},
@@ -1630,11 +1622,23 @@
16301622
},
16311623
{
16321624
"cell_type": "code",
1633-
"execution_count": null,
1625+
"execution_count": 7,
16341626
"metadata": {
16351627
"collapsed": false
16361628
},
1637-
"outputs": [],
1629+
"outputs": [
1630+
{
1631+
"ename": "ImportError",
1632+
"evalue": "No module named 'park_martin'",
1633+
"output_type": "error",
1634+
"traceback": [
1635+
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
1636+
"\u001b[0;31mImportError\u001b[0m Traceback (most recent call last)",
1637+
"\u001b[0;32m<ipython-input-7-5835caa81072>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m()\u001b[0m\n\u001b[0;32m----> 1\u001b[0;31m \u001b[0;32mfrom\u001b[0m \u001b[0mpark_martin\u001b[0m \u001b[0;32mimport\u001b[0m \u001b[0mcalibrate\u001b[0m \u001b[0;32mas\u001b[0m \u001b[0mpark_martin_calibration\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 2\u001b[0m \u001b[0mA\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mB\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;34m[\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m[\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 3\u001b[0m \u001b[0mn_trfs\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mlen\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mrob_pose_list\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 4\u001b[0m \u001b[0;32mfor\u001b[0m \u001b[0mi\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mrange\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mn_trfs\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 5\u001b[0m \u001b[0;32mfor\u001b[0m \u001b[0mj\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mrange\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mi\u001b[0m\u001b[0;34m+\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mn_trfs\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
1638+
"\u001b[0;31mImportError\u001b[0m: No module named 'park_martin'"
1639+
]
1640+
}
1641+
],
16381642
"source": [
16391643
"from park_martin import calibrate as park_martin_calibration\n",
16401644
"A, B = [], []\n",
@@ -1785,26 +1789,23 @@
17851789
}
17861790
],
17871791
"metadata": {
1792+
"anaconda-cloud": {},
17881793
"kernelspec": {
1789-
"display_name": "Python 2",
1794+
"display_name": "Python [default]",
17901795
"language": "python",
1791-
"name": "python2"
1796+
"name": "python3"
17921797
},
17931798
"language_info": {
17941799
"codemirror_mode": {
17951800
"name": "ipython",
1796-
"version": 2
1801+
"version": 3
17971802
},
17981803
"file_extension": ".py",
17991804
"mimetype": "text/x-python",
18001805
"name": "python",
18011806
"nbconvert_exporter": "python",
1802-
"pygments_lexer": "ipython2",
1803-
"version": "2.7.11+"
1804-
},
1805-
"widgets": {
1806-
"state": {},
1807-
"version": "1.1.2"
1807+
"pygments_lexer": "ipython3",
1808+
"version": "3.5.2"
18081809
}
18091810
},
18101811
"nbformat": 4,

‎src/motor_estimation.cpp

Lines changed: 85 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -299,20 +299,30 @@ class MotorEstimationSolver {
299299
DualPlane<T> a(a_);
300300
DualPlane<T> b(b_);
301301
DualPlane<T> c = a.spin(M);
302-
DualLine<T> d = (c * b - b * c) * Scalar<T>{0.5};
303-
304-
Point<T> p =
305-
Flat::location(d, Vector<T>{T(0.0),T(0.0), T(0.0)}.null(), true) * Scalar<T>{T(0.5)};
306-
Motor<T> Tr{T(1.0), T(0.0), T(0.0), T(0.0), p[0], p[1], p[2], T(0.0)};
307-
DualLine<T> dt = d.spin(~Tr);
302+
DualLine<T> d = c * ~b;
308303

309-
for (int i = 0; i < 6; ++i) {
310-
residual[i] = dt[i];
311-
}
304+
residual[0] = d[0];
305+
residual[1] = d[1];
306+
residual[2] = d[2];
307+
308+
// Bivector<T> A{d[0], d[1], d[2]};
309+
// if( A.norm() > T(0.00000001) ) {
310+
// Vector<T> f{d[3], d[4], d[5]};
311+
// Vector<T> b_d = Op::reject(f, A.unit());
312+
// residual[3] = b_d[0];
313+
// residual[4] = b_d[1];
314+
// residual[5] = b_d[2];
315+
// }
316+
// else {
317+
residual[3] = d[3];
318+
residual[4] = d[4];
319+
residual[5] = d[5];
320+
// }
312321

313322
// for (int i = 0; i < 6; ++i) {
314323
// residual[i] = d[i];
315324
// }
325+
316326
return true;
317327
}
318328

@@ -393,28 +403,40 @@ class MotorEstimationSolver {
393403
DualLine<T> a(a_);
394404
DualLine<T> b(b_);
395405
DualLine<T> c = a.spin(M);
396-
DualLine<T> d = b * ~c;
406+
DualLine<T> d = c * ~b;
397407

398-
Bivector<T> A{d[0], d[1], d[2]};
399-
Vector<T> f{d[3], d[4], d[5]};
400-
Vector<T> b_d = Op::reject(f, A.unit());
401-
// Vector<T> h = Op::project(f, A.unit()) * Scalar<T>{T(1.5) / A.norm()};
408+
// Point<T> p =
409+
// Flat::location(d, Vector<T>{T(0.0),T(0.0), T(0.0)}.null(), true) * Scalar<T>{T(0.5)};
410+
// p = p.unit();
411+
// Motor<T> Tr{T(1.0), T(0.0), T(0.0), T(0.0), -p[0], -p[1], -p[2], T(0.0)};
412+
// DualLine<T> dt = d.spin(~Tr);
402413

403-
Point<T> p =
404-
Flat::location(d, Vector<T>{T(0.0),T(0.0), T(0.0)}.null(), true) * Scalar<T>{T(0.5)};
405-
Motor<T> Tr{T(1.0), T(0.0), T(0.0), T(0.0), p[0], p[1], p[2], T(0.0)};
406-
DualLine<T> dt = d.spin(~Tr);
414+
residual[0] = d[0];
415+
residual[1] = d[1];
416+
residual[2] = d[2];
407417

408-
for (int i = 0; i < 6; ++i) {
409-
residual[i] = dt[i];
418+
Bivector<T> A{d[0], d[1], d[2]};
419+
if( A.norm() > T(0.00000001) ) {
420+
Vector<T> f{d[3], d[4], d[5]};
421+
Vector<T> b_d = Op::reject(f, A.unit());
422+
residual[3] = b_d[0];
423+
residual[4] = b_d[1];
424+
residual[5] = b_d[2];
425+
}
426+
else {
427+
std::cout << "Parallel!" << std::endl;
428+
residual[3] = d[3];
429+
residual[4] = d[4];
430+
residual[5] = d[5];
410431
}
411432

412-
// residual[0] = d[0];
413-
// residual[1] = d[1];
414-
// residual[2] = d[2];
415-
// residual[3] = b_d[0];
416-
// residual[4] = b_d[1];
417-
// residual[5] = b_d[2];
433+
// residual[3] = d[3];
434+
// residual[4] = d[4];
435+
// residual[5] = d[5];
436+
437+
// for (int i = 0; i < 6; ++i) {
438+
// residual[i] = dt[i];
439+
// }
418440

419441
return true;
420442
}
@@ -432,8 +454,10 @@ class MotorEstimationSolver {
432454
Motor<T> M(motor);
433455
DualLine<T> a(a_);
434456
DualLine<T> b(b_);
435-
DualLine<T> c = ~a.spin(M);
436-
DualLine<T> d = (b * c - c * b) * Scalar<T>{0.5};
457+
DualLine<T> c = a.spin(M);
458+
// DualLine<T> d = (b * c - c * b) * Scalar<T>{0.5};
459+
460+
DualLine<T> d = c * ~b;
437461

438462
for (int i = 0; i < 6; ++i) {
439463
residual[i] = d[i];
@@ -545,11 +569,39 @@ class MotorEstimationSolver {
545569
DualLine<T> a(a_);
546570
DualLine<T> b(b_);
547571
DualLine<T> c = a.spin(M);
572+
DualLine<T> d = c - b;
548573

549-
for (int i = 0; i < b.Num; ++i) {
550-
residual[i] = c[i] - b[i];
574+
residual[0] = d[0];
575+
residual[1] = d[1];
576+
residual[2] = d[2];
577+
578+
Bivector<T> A{d[0], d[1], d[2]};
579+
if( A.norm() > T(0.00000001) ) {
580+
Vector<T> f{d[3], d[4], d[5]};
581+
Vector<T> b_d = Op::reject(f, A.unit());
582+
residual[3] = b_d[0];
583+
residual[4] = b_d[1];
584+
residual[5] = b_d[2];
585+
}
586+
else {
587+
std::cout << "Parallel!" << std::endl;
588+
residual[3] = d[3];
589+
residual[4] = d[4];
590+
residual[5] = d[5];
551591
}
552592

593+
// Point<T> p =
594+
// Flat::location(d, Vector<T>{T(0.0),T(0.0), T(0.0)}.null(), true) * Scalar<T>{T(0.5)};
595+
// Motor<T> Tr{T(1.0), T(0.0), T(0.0), T(0.0), -p[0], -p[1], -p[2], T(0.0)};
596+
597+
// DualLine<T> dt = d.spin(~Tr);
598+
599+
// for (int i = 0; i < b.Num; ++i) {
600+
// // residual[i] = c[i] - b[i];
601+
// // residual[i] = dt[i];
602+
// residual[i] = d[i];
603+
// }
604+
553605
return true;
554606
}
555607

@@ -897,16 +949,18 @@ class MotorEstimationSolver {
897949
class UpdateMotorEachIterationCallback : public ceres::IterationCallback {
898950
public:
899951
UpdateMotorEachIterationCallback(const Mot *motor, std::vector<Mot> *list)
900-
: motor_(motor), list_(list) {}
952+
: motor_(motor), list_(list), iteration_k(1) {}
901953
virtual ceres::CallbackReturnType
902954
operator()(const ceres::IterationSummary &summary) {
955+
std::cout << "Iteration " << iteration_k++ << std::endl;
903956
list_->push_back(Mot(*motor_));
904957
return ceres::SOLVER_CONTINUE;
905958
}
906959

907960
private:
908961
std::vector<Mot> *list_;
909962
const Mot *motor_;
963+
int iteration_k;
910964
};
911965

912966
auto Solve() -> std::tuple<Mot, py::dict, std::vector<Mot>> {

‎src/vsr/versor_dual_plane_pybind11.cpp

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,10 @@ void AddDualPlane(py::module &m) {
3535
})
3636
.def("vec", [](const Dlp &arg) { return Vec(arg[0], arg[1], arg[2]); })
3737
.def("spin", (Dlp (Dlp::*)(const Mot &) const) & Dlp::spin)
38+
.def("comm",
39+
[](const Dlp &lhs, const Dlp &rhs) {
40+
return Dll(lhs * rhs);
41+
})
3842
.def("__mul__",
3943
[](const Dlp &lhs, const Dlp &rhs) { return Mot(lhs * rhs); })
4044
.def("__sub__",

0 commit comments

Comments
 (0)