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

Go to the documentation of this file.

/*
 * magCalibNonLinSim.cc
 * Copyright (C) James Goppert 2009 <jgoppert@users.sourceforge.net>
 *
 * magCalibNonLinSim.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 <cmath>

#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/utilities/utilities.h"
#include "uvsim/visualization/PointCloud.h"
#include "uvsim/visualization/SimpleViewer.h"
#include "uvsim/visualization/Ellipsoid.h"
#include <cmath>

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

double abs(double x)
{
        if (x<0) return -x;
        else return x;
}

double costFunc(vector<double> x, vector<double> y, vector<double> z, int n, vector<double> center, vector<double> radii)
{
        double cost = 0, p, t;
        vector<double> unit(3), point(3);
        for (int i=0;i<n;i++)
        {
                point(0) = x(i);
                point(1) = y(i);
                point(2) = z(i);
                unit = (point-center)/norm_2(point-center);
                t = sqrt(1/(
                                        unit(0)*unit(0)/(radii(0)*radii(0)) + 
                                        unit(1)*unit(1)/(radii(1)*radii(1)) + 
                                        unit(2)*unit(2)/(radii(2)*radii(2))
                                        ));
                p = norm_2(point-center);
                cost = cost + abs(p-t);
                //std::cout << "x: " << x(i) << std::endl;
                //std::cout << "y: " << y(i) << std::endl;
                //std::cout << "z: " << z(i) << std::endl;
                //std::cout << "t: " << t << std::endl;
                //std::cout << "p: " << p << std::endl;
                //std::cout << "cost: " << cost << std::endl;
                //std::cout << "unit: " << unit << std::endl;
                //std::cout << "point: " << point << std::endl;
        }
        return cost;
}

void nonlinSolv(vector<double> initialGuess, vector<double> &solution)
{

}

int main (int argc, char const* argv[])
{
    // constants
    const static int freqImu = 520, freqMagCalib = 100;
    int maxN = 1000;
        int fps = 20;
        osg::Vec3d manipCenter(512,512,512);
        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);
    }

    // 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));

    // Initialize the loop
    int n = 0;

    // Launch the viewer thread
        osg::Group *root = new osg::Group;
        double fogStart=100, fogEnd=1000;
    SimpleViewer simpleViewer(0,fps,root,manipDistance,manipCenter,frameSize,10000,100000);
        simpleViewer.start();

        // Ellipse Test Case
        double a, b, c, cx, cy, cz, nx, ny, nz, r1, r2;

        // radii 
        a = 100;
        b = 100;
        c = 100;

        // center
        cx = 512;
        cy = 512;
        cz = 512;

        // noise
        nx = 10;
        ny = 10;
        nz = 10;

        // ratios
        r1 = .25;
        r2 = .5;

        // Ellipsoid
        Ellipsoid *linearEllipsoid, *trueEllipsoid, *nonlinearEllipsoid;
        trueEllipsoid = new Ellipsoid(osg::Vec3d(a,b,c),osg::Vec3d(cx,cy,cz),3,osg::Vec4d(255,255,102,1));
        root->addChild(trueEllipsoid->pointCloud->root.get());

    // Point Cloud
    osg::Vec3Array *points = new osg::Vec3Array;
    osg::Vec4Array *colors = new osg::Vec4Array;
    PointCloud *pointCloud = new PointCloud(5,points,colors);
    root->addChild((pointCloud->root).get());

    // Collect data
        n = 0;
        for (double theta=0;theta<r1*2*M_PI;theta+=M_PI/40)
    {
                for (double phi=0;phi<r2*M_PI;phi+=M_PI/20)
        {
            x(n) = a*sin(phi)*cos(theta) + cx + nx*(rand()/float(RAND_MAX)-0.5);
            y(n) = b*sin(phi)*sin(theta) + cy + ny*(rand()/float(RAND_MAX)-0.5);
            z(n) = c*cos(phi) + cz + nz*(rand()/float(RAND_MAX)-0.5);
                        std::cout << "n: " << n;
                        std::cout << "\tx,y,z: " << x(n) << ", " << y(n) << ", " << z(n) << std::endl;

            // Store for drawing
            points->push_back(osg::Vec3(x(n),y(n),z(n)));
            colors->push_back(osg::Vec4(1,0,0,1));

                        // Check number of points
                        if (n++ > maxN)
                        {
                                std::cout << "Too many points." << std::endl;
                                exit(1);
                        };
        }
    }
    pointCloud->updateSize();

    // Create Design Matrix
    std::cout << "Creating design matrix." << 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;

    // Linear 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;

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

        // Non-Linear Solution
        vector<double> centerNL(3), radiiNL(3);
        centerNL = center;
        radiiNL = 1.1*radii;
        std::cout << "cost: " << costFunc(x,y,z,n,centerNL,radiiNL) << std::endl;
        
        // Draw ellipsoid
        nonlinearEllipsoid = new Ellipsoid(osg::Vec3d(radiiNL(0),radiiNL(1),radiiNL(2)),
                        osg::Vec3d(centerNL(0),centerNL(1),centerNL(2)),3,osg::Vec4d(0,0,1,1));
        root->addChild(nonlinearEllipsoid->pointCloud->root.get());

        simpleViewer.join();
    return 0;
}

// vim:ts=4:sw=4

---