/home/jgoppert/Projects/uvsim/uvsim/src/test/magCalib.cc

Go to the documentation of this file.

/*
 * magCalib.cc
 * Copyright (C) James Goppert 2009 <jgoppert@users.sourceforge.net>
 *
 * magCalib.cc is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * magCalib.cc is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */


#include <cstdlib>
#include <iomanip>

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/lapack/lapack.hpp>
#include <boost/numeric/ublas/symmetric.hpp>

#include "uvsim/sensing/Imu6DofSensor.h"
#include "uvsim/utilities/utilities.h"
#include "uvsim/visualization/PointCloud.h"
#include "uvsim/visualization/SimpleViewer.h"
#include "uvsim/visualization/Ellipsoid.h"

using namespace boost::numeric::ublas;
using namespace uvsim;
using namespace boost::numeric::bindings::lapack;

int main (int argc, char const* argv[])
{
    // constants
    const static int freqImu = 520, freqMagCalib = 100;
    int maxN = 100000;
        int fps = 20;
        osg::Vec3d manipCenter(604,586,629);
        double manipDistance = 1000;
        float frameSize = 5;

    // Calculate frequency ratio
    const static int freqRatio = freqImu/freqMagCalib;
    std::cout << "Frequency Ratio: " << freqRatio << std::endl;
    if (freqRatio <=0)
    {
        std::cout << "The frequency of the imu: " << freqImu
                  << " must be faster than that of the calibration: " <<
                  freqMagCalib << "." << std::endl;
        exit(-1);
    }


    // cutoff frequencies
    vector<double> freqCutMb(3), freqCutFb(3), freqCutWb(3);

    // magnetometer cutoff freq
    freqCutMb(0) = 3; //312;
    freqCutMb(1) = 3; //312;
    freqCutMb(2) = 3; //312;

    // accel cutoff freq
    freqCutFb(0) = 350; //350;
    freqCutFb(1) = 350; //350;
    freqCutFb(2) = 150; //150;

    // gyro cutoff freq
    freqCutWb(0) = 120; //120;
    freqCutWb(1) = 120; //120;
    freqCutWb(2) = 120; //120;

    // inertial measurement unit
    Imu6DofSensor imu("/dev/rfcomm0", freqCutMb, freqCutFb, freqCutWb);

    // Ellipsoid fitting
    vector<double> x(maxN), y(maxN), z(maxN), center(3), radii(3), bias(3), scaleFactor(3);
    matrix<double> temp(6,6);

    // Randomly seed random number generator
    srand(time(0));

    // osg
    osg::Vec3Array *points = new osg::Vec3Array;
    osg::Vec4Array *colors = new osg::Vec4Array;
    osg::Vec4 ini(1,1,0,1);
    osg::Vec4 fin(0,0,1,1);
    PointCloud *pointCloud = new PointCloud(5,points,colors);
    osg::Group *root = new osg::Group;
    root->addChild((pointCloud->root).get());

    // Initialize the loop
    int n = 0;
    imu.start(freqImu);

    // Launch the viewer thread
        double fogStart = 0, fogEnd = 1000;
    SimpleViewer simpleViewer(0,fps,root,manipDistance,manipCenter,frameSize,fogStart,fogEnd);
        simpleViewer.start();

    // Collect data
    for (int j=0;!simpleViewer.viewer->done();j++)
    {
        // Update the imu and counter for imu
        imu.update();
        //usleep(1e6/freqImu); // simulate imu wait

        // Collect data
        if ((j % freqRatio) == 0)
        {
            // Display Data
            clear();
            std::cout << (100.*(n+1))/maxN << "% Complete" << std::endl;
            std::cout << "data: " << imu.mb << std::endl;

            // Store Data
            x(n) = imu.mb[0];
            y(n) = imu.mb[1];
            z(n) = imu.mb[2];

            // Random data for testing
            //x(n) = 10*(rand()/float(RAND_MAX)-0.5);
            //y(n) = 10*(rand()/float(RAND_MAX)-0.5);
            //z(n) = 10*(rand()/float(RAND_MAX)-0.5);

            // Store for drawing
            points->push_back(osg::Vec3(x(n),y(n),z(n)));
            colors->push_back(ini+(fin-ini)*(n*2/(float)maxN));
                pointCloud->updateSize();

            // Break if done collecting data
            if (n++ == (maxN-1)) break;
        }
    }

    // Stop the imu
    imu.stop();

    // Create Design Matrix
    std::cout << "Creating design matrix." << std::endl;
    //std::cout << "x: " << x << std::endl;
    //std::cout << "y: " << y << std::endl;
    //std::cout << "z: " << z << std::endl;

    matrix<double> D(n,6);
    for (int i=0;i<n;i++)
    {
        D(i,0) = x(i)*x(i);
        D(i,1) = y(i)*y(i);
        D(i,2) = z(i)*z(i);
        D(i,3) = 2*x(i);
        D(i,4) = 2*y(i);
        D(i,5) = 2*z(i);
    }

    std::cout << "Design matrix. " << std::endl;
    std::cout << "D: " << D << std::endl;

    // Solution
    if (n < 1000)
    {
        std::cout << "Solving for bias and scale factors. " << std::endl;
        matrix<double> v(6,1);
                matrix<double> K(3,3);
                K(0,0) = 1;
                K(1,2) = 3;
                K(2,0) =.3;
                K(1,1) =.4;
                std::cout << K << std::endl;
                std::cout << pinv(K) << std::endl;
        //v = prod(pinv(D),ones(n,1));
        double gam = 1 + ( v(3,0)*v(3,0) / v(0,0) + v(4,0)*v(4,0) / v(1,0) +
                           v(5,0)*v(5,0) / v(2,0) );
        for (int i=0;i<3;i++)
        {
            center(i) = -v(i+3,0)/v(i,0);
            bias(i) = -center(i);
            radii(i) = sqrt(gam/v(i,0));
            scaleFactor(i) = 1/radii(i);
        }

        // Output
        std::cout << std::setprecision(8);
        std::cout << "v: " << v << std::endl;
        std::cout << "center: " << center << std::endl;
        std::cout << "radii: " << radii << std::endl;
        std::cout << "bias: " << bias << std::endl;
        std::cout << "scale factor: " << scaleFactor << std::endl;

                Ellipsoid ellipsoid(osg::Vec3d(radii(0),radii(1),radii(2)),osg::Vec3d(center(0),center(1),center(2)));
                root->addChild(ellipsoid.root.get());
    }
    else
    {
        std::cout << "N too large to compute svd" << std::endl;
    }

        simpleViewer.stop();
    return 0;
}

// vim:ts=4:sw=4

---