[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] tag/test handeye.cpp
From: |
Gerhard Reitmayr |
Subject: |
[Toon-members] tag/test handeye.cpp |
Date: |
Thu, 23 Apr 2009 18:06:28 +0000 |
CVSROOT: /cvsroot/toon
Module name: tag
Changes by: Gerhard Reitmayr <gerhard> 09/04/23 18:06:28
Added files:
test : handeye.cpp
Log message:
handeye calibration test code
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/test/handeye.cpp?cvsroot=toon&rev=1.1
Patches:
Index: handeye.cpp
===================================================================
RCS file: handeye.cpp
diff -N handeye.cpp
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ handeye.cpp 23 Apr 2009 18:06:28 -0000 1.1
@@ -0,0 +1,99 @@
+#include <tag/handeye.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace TooN;
+using namespace tag;
+
+double inline rand_u(){
+ return (double)rand()/RAND_MAX;
+}
+
+double inline rand_u(double from, double to){
+ return from + (to - from)*rand_u();
+}
+
+template <int N>
+Vector<N> rand_vect(double from, double to){
+ Vector<N> v;
+ for(int i = 0; i < N; ++i)
+ v[i] = rand_u(from, to);
+ return v;
+}
+
+int main(int argc, char ** argv){
+ const int ITERATIONS = 100000;
+ double total_error = 0;
+ for(int t = 0; t < ITERATIONS; ++t){
+ vector<SO3<> > a, b;
+
+ SO3<> BC(rand_vect<3>(-2,2));
+ SO3<> DA(rand_vect<3>(-2,2));
+
+ for(int i = 0; i < 3; ++i ){
+ SO3<> AB(rand_vect<3>(-2,2));
+ a.push_back(AB);
+ b.push_back(BC.inverse() * AB.inverse() * DA.inverse());
+ }
+ pair<SO3<>, SO3<> > xy = computeHandEye(a,b);
+ double error = 0, e2 = 0, e3 = 0;
+ for(unsigned i = 0; i < a.size(); ++i ){
+ error += norm_sq((a[i] * xy.first * b[i] * xy.second).ln());
+ }
+ e2 = norm_sq((BC * xy.first.inverse()).ln());
+ e3 = norm_sq((DA * xy.second.inverse()).ln());
+ if( error > 1e-20 || e2 > 1e-20 || e3 > 1e-20 ){
+ cout << "ups in rotation " << error << "\t" << e2 << "\t" << e3 <<
endl;
+ }
+ total_error += error;
+ }
+ cout << "SO3 average error\t" << total_error / ITERATIONS << endl;
+ total_error = 0;
+ for(int t = 0; t < ITERATIONS; ++t){
+ vector<SE3<> > a, b;
+
+ SE3<> BC(rand_vect<6>(-2,2));
+ SE3<> DA(rand_vect<6>(-2,2));
+
+ for(int i = 0; i < 3; ++i ){
+ SE3<> AB(rand_vect<6>(-2,2));
+ a.push_back(AB);
+ b.push_back(BC.inverse() * AB.inverse() * DA.inverse());
+ }
+ pair<SE3<>, SE3<> > xy = computeHandEye(a,b);
+ double error = 0;
+ for(unsigned i = 0; i < a.size(); ++i ){
+ error += norm_sq((a[i] * xy.first * b[i] * xy.second).ln());
+ }
+ if( error > 1e-20 ){
+ cout << "ups in SE3 " << error << endl;
+ }
+ total_error += error;
+ }
+ cout << "SE3 average error\t" << total_error / ITERATIONS << endl;
+ // for(double d = 1e-10; d < 1; d *= sqrt(10)){
+ double d = 0.01;
+ total_error = 0;
+ for(int t = 0; t < ITERATIONS; ++t){
+ vector<SE3<> > a, b;
+
+ SE3<> BC(rand_vect<6>(-2,2));
+ SE3<> DA(rand_vect<6>(-2,2));
+
+ for(int i = 0; i < 4; ++i ){
+ SE3<> AB(rand_vect<6>(-2,2));
+ a.push_back(AB * SE3<>::exp(rand_vect<6>(-d,d)));
+ b.push_back(BC.inverse() * AB.inverse() * DA.inverse() *
SE3<>::exp(rand_vect<6>(-d,d)));
+ }
+ pair<SE3<>, SE3<> > xy = computeHandEye(a,b);
+ double error = 0;
+ for(unsigned i = 0; i < a.size(); ++i ){
+ error += norm_sq((a[i] * xy.first * b[i] * xy.second).ln());
+ }
+ total_error += error;
+ }
+ cout << "pertupated SE3 with " << d << "\t" << total_error /
ITERATIONS << endl;
+ //}
+ return 0;
+}
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [Toon-members] tag/test handeye.cpp,
Gerhard Reitmayr <=