00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include <cstdlib>
00021 #include <iomanip>
00022 #include <cmath>
00023
00024 #include <boost/numeric/ublas/io.hpp>
00025 #include <boost/numeric/ublas/vector.hpp>
00026 #include <boost/numeric/ublas/vector_proxy.hpp>
00027 #include <boost/numeric/ublas/matrix.hpp>
00028 #include <boost/numeric/ublas/matrix_proxy.hpp>
00029 #include <boost/numeric/bindings/traits/ublas_matrix.hpp>
00030 #include <boost/numeric/bindings/traits/ublas_vector.hpp>
00031 #include <boost/numeric/bindings/lapack/lapack.hpp>
00032 #include <boost/numeric/ublas/symmetric.hpp>
00033
00034 #include "uvsim/utilities/utilities.h"
00035 #include "uvsim/visualization/PointCloud.h"
00036 #include "uvsim/visualization/SimpleViewer.h"
00037 #include "uvsim/visualization/Ellipsoid.h"
00038
00039 using namespace boost::numeric::ublas;
00040 using namespace uvsim;
00041 using namespace boost::numeric::bindings::lapack;
00042
00043 int main (int argc, char const* argv[])
00044 {
00045
00046 const static int freqImu = 520, freqMagCalib = 100;
00047 int maxN = 1000;
00048 int fps = 20;
00049 osg::Vec3d manipCenter(512,512,512);
00050 double manipDistance = 1000;
00051 float frameSize = 5;
00052
00053
00054 const static int freqRatio = freqImu/freqMagCalib;
00055 std::cout << "Frequency Ratio: " << freqRatio << std::endl;
00056 if (freqRatio <=0)
00057 {
00058 std::cout << "The frequency of the imu: " << freqImu
00059 << " must be faster than that of the calibration: " <<
00060 freqMagCalib << "." << std::endl;
00061 exit(-1);
00062 }
00063
00064
00065 vector<double> x(maxN), y(maxN), z(maxN), center(3), radii(3), bias(3), scaleFactor(3);
00066 matrix<double> temp(6,6);
00067
00068
00069 srand(time(0));
00070
00071
00072 int n = 0;
00073
00074
00075 osg::Group *root = new osg::Group;
00076 double fogStart=100, fogEnd=1000;
00077 SimpleViewer simpleViewer(0,fps,root,manipDistance,manipCenter,frameSize,fogStart,fogEnd);
00078 simpleViewer.start();
00079
00080
00081 double a, b, c, cx, cy, cz, nx, ny, nz, r1, r2;
00082
00083
00084 a = 100;
00085 b = 100;
00086 c = 100;
00087
00088
00089 cx = 512;
00090 cy = 512;
00091 cz = 512;
00092
00093
00094 nx = 50;
00095 ny = 50;
00096 nz = 50;
00097
00098
00099 r1 = .25;
00100 r2 = .5;
00101
00102
00103 Ellipsoid *ellipsoid, *trueEllipsoid;
00104 trueEllipsoid = new Ellipsoid(osg::Vec3d(a,b,c),osg::Vec3d(cx,cy,cz));
00105 root->addChild(trueEllipsoid->pointCloud->root.get());
00106
00107
00108 osg::Vec3Array *points = new osg::Vec3Array;
00109 osg::Vec4Array *colors = new osg::Vec4Array;
00110 PointCloud *pointCloud = new PointCloud(5,points,colors);
00111 root->addChild((pointCloud->root).get());
00112
00113
00114 n = 0;
00115 for (double theta=0;theta<r1*2*M_PI;theta+=M_PI/40)
00116 {
00117 for (double phi=0;phi<r2*M_PI;phi+=M_PI/20)
00118 {
00119 x(n) = a*sin(phi)*cos(theta) + cx + nx*(rand()/float(RAND_MAX)-0.5);
00120 y(n) = b*sin(phi)*sin(theta) + cy + ny*(rand()/float(RAND_MAX)-0.5);
00121 z(n) = c*cos(phi) + cz + nz*(rand()/float(RAND_MAX)-0.5);
00122 std::cout << "n: " << n;
00123 std::cout << "\tx,y,z: " << x(n) << ", " << y(n) << ", " << z(n) << std::endl;
00124
00125
00126 points->push_back(osg::Vec3(x(n),y(n),z(n)));
00127 colors->push_back(osg::Vec4(1,0,0,1));
00128
00129
00130 if (n++ > maxN)
00131 {
00132 std::cout << "Too many points." << std::endl;
00133 exit(1);
00134 };
00135 }
00136 }
00137 pointCloud->updateSize();
00138
00139
00140 std::cout << "Creating design matrix." << std::endl;
00141 matrix<double> D(n,6);
00142 for (int i=0;i<n;i++)
00143 {
00144 D(i,0) = x(i)*x(i);
00145 D(i,1) = y(i)*y(i);
00146 D(i,2) = z(i)*z(i);
00147 D(i,3) = 2*x(i);
00148 D(i,4) = 2*y(i);
00149 D(i,5) = 2*z(i);
00150 }
00151 std::cout << "Design matrix. " << std::endl;
00152 std::cout << "D: " << D << std::endl;
00153
00154
00155 if (n < 1000)
00156 {
00157 std::cout << "Solving for bias and scale factors. " << std::endl;
00158 matrix<double> v(6,1);
00159 matrix<double> K(3,3);
00160 K(0,0) = 1;
00161 K(1,2) = 3;
00162 K(2,0) =.3;
00163 K(1,1) =.4;
00164 std::cout << K << std::endl;
00165 std::cout << pinv(K) << std::endl;
00166 v = prod(pinv(D),ones(n,1));
00167 double gam = 1 + ( v(3,0)*v(3,0) / v(0,0) + v(4,0)*v(4,0) / v(1,0) +
00168 v(5,0)*v(5,0) / v(2,0) );
00169 for (int i=0;i<3;i++)
00170 {
00171 center(i) = -v(i+3,0)/v(i,0);
00172 bias(i) = -center(i);
00173 radii(i) = sqrt(gam/v(i,0));
00174 scaleFactor(i) = 1/radii(i);
00175 }
00176
00177
00178 std::cout << std::setprecision(8);
00179 std::cout << "v: " << v << std::endl;
00180 std::cout << "center: " << center << std::endl;
00181 std::cout << "radii: " << radii << std::endl;
00182 std::cout << "bias: " << bias << std::endl;
00183 std::cout << "scale factor: " << scaleFactor << std::endl;
00184
00185
00186 ellipsoid = new Ellipsoid(osg::Vec3d(radii(0),radii(1),radii(2)),osg::Vec3d(center(0),center(1),center(2)));
00187 root->addChild(ellipsoid->pointCloud->root.get());
00188 }
00189 else
00190 {
00191 std::cout << "N too large to compute svd" << std::endl;
00192 }
00193
00194 simpleViewer.join();
00195 return 0;
00196 }
00197
00198