svd_3x4.cpp

Go to the documentation of this file.

// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2020 Intel Corporation. All Rights Reserved.

// Code generated by Matlab Coder

#include "utils.h"
#include <limits>
#include <cmath>
#include <cstring>


namespace
{

    double b_xnrm2( int n, const double x[4], int ix0 )
    {
        double y;
        double scale;
        int kend;
        int k;
        double absxk;
        double t;
        y = 0.0;
        if( n >= 1 )
        {
            if( n == 1 )
            {
                y = std::abs( x[ix0 - 1] );
            }
            else
            {
                scale = 3.3121686421112381E-170;
                kend = (ix0 + n) - 1;
                for( k = ix0; k <= kend; k++ )
                {
                    absxk = std::abs( x[k - 1] );
                    if( absxk > scale )
                    {
                        t = scale / absxk;
                        y = 1.0 + y * t * t;
                        scale = absxk;
                    }
                    else
                    {
                        t = absxk / scale;
                        y += t * t;
                    }
                }

                y = scale * std::sqrt( y );
            }
        }

        return y;
    }

    double xnrm2( int n, const double x[12], int ix0 )
    {
        double y;
        double scale;
        int kend;
        int k;
        double absxk;
        double t;
        y = 0.0;
        if( n >= 1 )
        {
            if( n == 1 )
            {
                y = std::abs( x[ix0 - 1] );
            }
            else
            {
                scale = 3.3121686421112381E-170;
                kend = (ix0 + n) - 1;
                for( k = ix0; k <= kend; k++ )
                {
                    absxk = std::abs( x[k - 1] );
                    if( absxk > scale )
                    {
                        t = scale / absxk;
                        y = 1.0 + y * t * t;
                        scale = absxk;
                    }
                    else
                    {
                        t = absxk / scale;
                        y += t * t;
                    }
                }

                y = scale * std::sqrt( y );
            }
        }

        return y;
    }

    void b_xaxpy( int n, double a, const double x[12], int ix0, double y[3], int iy0 )
    {
        int ix;
        int iy;
        int i1;
        int k;
        if( (n < 1) || (a == 0.0) )
        {
        }
        else
        {
            ix = ix0 - 1;
            iy = iy0 - 1;
            i1 = n - 1;
            for( k = 0; k <= i1; k++ )
            {
                y[iy] += a * x[ix];
                ix++;
                iy++;
            }
        }
    }

    void c_xaxpy( int n, double a, const double x[3], int ix0, double y[12], int iy0 )
    {
        int ix;
        int iy;
        int i2;
        int k;
        if( (n < 1) || (a == 0.0) )
        {
        }
        else
        {
            ix = ix0 - 1;
            iy = iy0 - 1;
            i2 = n - 1;
            for( k = 0; k <= i2; k++ )
            {
                y[iy] += a * x[ix];
                ix++;
                iy++;
            }
        }
    }

    void xaxpy( int n, double a, int ix0, double y[12], int iy0 )
    {
        int ix;
        int iy;
        int i0;
        int k;
        if( (n < 1) || (a == 0.0) )
        {
        }
        else
        {
            ix = ix0 - 1;
            iy = iy0 - 1;
            i0 = n - 1;
            for( k = 0; k <= i0; k++ )
            {
                y[iy] += a * y[ix];
                ix++;
                iy++;
            }
        }
    }

    double xdotc( int n, const double x[12], int ix0, const double y[12], int iy0 )
    {
        double d;
        int ix;
        int iy;
        int k;
        d = 0.0;
        if( n >= 1 )
        {
            ix = ix0;
            iy = iy0;
            for( k = 0; k < n; k++ )
            {
                d += x[ix - 1] * y[iy - 1];
                ix++;
                iy++;
            }
        }

        return d;
    }

    void xrotg( double * a, double * b, double * c, double * s )
    {
        double roe;
        double absa;
        double absb;
        double scale;
        double ads;
        double bds;
        roe = *b;
        absa = std::abs( *a );
        absb = std::abs( *b );
        if( absa > absb )
        {
            roe = *a;
        }

        scale = absa + absb;
        if( scale == 0.0 )
        {
            *s = 0.0;
            *c = 1.0;
            *a = 0.0;
            *b = 0.0;
        }
        else
        {
            ads = absa / scale;
            bds = absb / scale;
            scale *= std::sqrt( ads * ads + bds * bds );
            if( roe < 0.0 )
            {
                scale = -scale;
            }

            *c = *a / scale;
            *s = *b / scale;
            if( absa > absb )
            {
                *b = *s;
            }
            else if( *c != 0.0 )
            {
                *b = 1.0 / *c;
            }
            else
            {
                *b = 1.0;
            }

            *a = scale;
        }
    }


    // Test if value is not a number
    inline bool rtIsNaN( double value ) { return (value != value); }

}  // anon namespace


void librealsense::algo::depth_to_rgb_calibration::svd_3x4( const double A[12], double U[3] )
{
    double b_A[12];
    double e[4];
    double work[3];
    bool apply_transform;
    double s[4];
    int k;
    int qjj;
    double r;
    int m;
    int iter;
    double snorm;
    double b;
    int exitg1;
    int q;
    int qs;
    bool exitg2;
    double scale;
    double sm;
    double sqds;
    memcpy( &b_A[0], &A[0], 12U * sizeof( double ) );
    e[0] = 0.0;
    e[1] = 0.0;
    e[2] = 0.0;
    e[3] = 0.0;
    work[0] = 0.0;
    work[1] = 0.0;
    work[2] = 0.0;
    apply_transform = false;
    double nrm = xnrm2( 3, b_A, 1 );
    if( nrm > 0.0 )
    {
        apply_transform = true;
        if( b_A[0] < 0.0 )
        {
            nrm = -nrm;
        }

        if( std::abs( nrm ) >= 1.0020841800044864E-292 )
        {
            r = 1.0 / nrm;
            for( k = 1; k < 4; k++ )
            {
                b_A[k - 1] *= r;
            }
        }
        else
        {
            for( k = 1; k < 4; k++ )
            {
                b_A[k - 1] /= nrm;
            }
        }

        b_A[0]++;
        s[0] = -nrm;
    }
    else
    {
        s[0] = 0.0;
    }

    for( k = 2; k < 5; k++ )
    {
        qjj = 3 * ( k - 1 );
        if( apply_transform )
        {
            xaxpy( 3, -( xdotc( 3, b_A, 1, b_A, qjj + 1 ) / b_A[0] ), 1, b_A, qjj + 1 );
        }

        e[k - 1] = b_A[qjj];
    }

    nrm = b_xnrm2( 3, e, 2 );
    if( nrm == 0.0 )
    {
        e[0] = 0.0;
    }
    else
    {
        if( e[1] < 0.0 )
        {
            e[0] = -nrm;
        }
        else
        {
            e[0] = nrm;
        }

        r = e[0];
        if( std::abs( e[0] ) >= 1.0020841800044864E-292 )
        {
            r = 1.0 / e[0];
            for( k = 2; k < 5; k++ )
            {
                e[k - 1] *= r;
            }
        }
        else
        {
            for( k = 2; k < 5; k++ )
            {
                e[k - 1] /= r;
            }
        }

        e[1]++;
        e[0] = -e[0];
        for( qjj = 2; qjj < 4; qjj++ )
        {
            work[qjj - 1] = 0.0;
        }

        for( k = 2; k < 5; k++ )
        {
            b_xaxpy( 2, e[k - 1], b_A, 3 * ( k - 1 ) + 2, work, 2 );
        }

        for( k = 2; k < 5; k++ )
        {
            c_xaxpy( 2, -e[k - 1] / e[1], work, 2, b_A, 3 * ( k - 1 ) + 2 );
        }
    }

    apply_transform = false;
    nrm = xnrm2( 2, b_A, 5 );
    if( nrm > 0.0 )
    {
        apply_transform = true;
        if( b_A[4] < 0.0 )
        {
            nrm = -nrm;
        }

        if( std::abs( nrm ) >= 1.0020841800044864E-292 )
        {
            r = 1.0 / nrm;
            for( k = 5; k < 7; k++ )
            {
                b_A[k - 1] *= r;
            }
        }
        else
        {
            for( k = 5; k < 7; k++ )
            {
                b_A[k - 1] /= nrm;
            }
        }

        b_A[4]++;
        s[1] = -nrm;
    }
    else
    {
        s[1] = 0.0;
    }

    for( k = 3; k < 5; k++ )
    {
        qjj = 1 + 3 * ( k - 1 );
        if( apply_transform )
        {
            xaxpy( 2, -( xdotc( 2, b_A, 5, b_A, qjj + 1 ) / b_A[4] ), 5, b_A, qjj + 1 );
        }

        e[k - 1] = b_A[qjj];
    }

    nrm = b_xnrm2( 2, e, 3 );
    if( nrm == 0.0 )
    {
        e[1] = 0.0;
    }
    else
    {
        if( e[2] < 0.0 )
        {
            e[1] = -nrm;
        }
        else
        {
            e[1] = nrm;
        }

        r = e[1];
        if( std::abs( e[1] ) >= 1.0020841800044864E-292 )
        {
            r = 1.0 / e[1];
            for( k = 3; k < 5; k++ )
            {
                e[k - 1] *= r;
            }
        }
        else
        {
            for( k = 3; k < 5; k++ )
            {
                e[k - 1] /= r;
            }
        }

        e[2]++;
        e[1] = -e[1];
        for( qjj = 3; qjj < 4; qjj++ )
        {
            work[2] = 0.0;
        }

        for( k = 3; k < 5; k++ )
        {
            b_xaxpy( 1, e[k - 1], b_A, 3 * ( k - 1 ) + 3, work, 3 );
        }

        for( k = 3; k < 5; k++ )
        {
            c_xaxpy( 1, -e[k - 1] / e[2], work, 3, b_A, 3 * ( k - 1 ) + 3 );
        }
    }

    m = 2;
    s[2] = b_A[8];
    s[3] = 0.0;
    e[2] = b_A[11];
    e[3] = 0.0;
    iter = 0;
    snorm = 0.0;
    nrm = s[0];
    if( s[0] != 0.0 )
    {
        nrm = std::abs( s[0] );
        r = s[0] / nrm;
        s[0] = nrm;
        e[0] /= r;
    }

    if( e[0] != 0.0 )
    {
        b = std::abs( e[0] );
        r = b / e[0];
        e[0] = b;
        s[1] *= r;
    }

    nrm = std::abs( nrm );
    r = std::abs( e[0] );
    if( ( nrm > r ) || rtIsNaN( r ) )
    {
        r = nrm;
    }

    if( ! rtIsNaN( r ) )
    {
        snorm = r;
    }

    nrm = s[1];
    if( s[1] != 0.0 )
    {
        nrm = std::abs( s[1] );
        r = s[1] / nrm;
        s[1] = nrm;
        e[1] /= r;
    }

    if( e[1] != 0.0 )
    {
        b = std::abs( e[1] );
        r = b / e[1];
        e[1] = b;
        s[2] = b_A[8] * r;
    }

    nrm = std::abs( nrm );
    r = std::abs( e[1] );
    if( ( nrm > r ) || rtIsNaN( r ) )
    {
        r = nrm;
    }

    if( ( ! ( snorm > r ) ) && ( ! rtIsNaN( r ) ) )
    {
        snorm = r;
    }

    nrm = s[2];
    if( s[2] != 0.0 )
    {
        nrm = std::abs( s[2] );
        r = s[2] / nrm;
        s[2] = nrm;
        e[2] = b_A[11] / r;
    }

    if( e[2] != 0.0 )
    {
        b = std::abs( e[2] );
        r = b / e[2];
        e[2] = b;
        s[3] = 0.0 * r;
    }

    nrm = std::abs( nrm );
    r = std::abs( e[2] );
    if( ( nrm > r ) || rtIsNaN( r ) )
    {
        r = nrm;
    }

    if( ( ! ( snorm > r ) ) && ( ! rtIsNaN( r ) ) )
    {
        snorm = r;
    }

    if( s[3] != 0.0 )
    {
        s[3] = std::numeric_limits< double >::quiet_NaN();  // rtNaN;
    }

    if( ! ( snorm > 0.0 ) )
    {
        snorm = 0.0;
    }

    while( ( m + 2 > 0 ) && ( iter < 75 ) )
    {
        qjj = m;
        do
        {
            exitg1 = 0;
            q = qjj + 1;
            if( qjj + 1 == 0 )
            {
                exitg1 = 1;
            }
            else
            {
                nrm = std::abs( e[qjj] );
                if( ( nrm
                      <= 2.2204460492503131E-16 * ( std::abs( s[qjj] ) + std::abs( s[qjj + 1] ) ) )
                    || ( nrm <= 1.0020841800044864E-292 )
                    || ( ( iter > 20 ) && ( nrm <= 2.2204460492503131E-16 * snorm ) ) )
                {
                    e[qjj] = 0.0;
                    exitg1 = 1;
                }
                else
                {
                    qjj--;
                }
            }
        } while( exitg1 == 0 );

        if( qjj + 1 == m + 1 )
        {
            qjj = 4;
        }
        else
        {
            qs = m + 2;
            k = m + 2;
            exitg2 = false;
            while( ( ! exitg2 ) && ( k >= qjj + 1 ) )
            {
                qs = k;
                if( k == qjj + 1 )
                {
                    exitg2 = true;
                }
                else
                {
                    nrm = 0.0;
                    if( k < m + 2 )
                    {
                        nrm = std::abs( e[k - 1] );
                    }

                    if( k > qjj + 2 )
                    {
                        nrm += std::abs( e[k - 2] );
                    }

                    r = std::abs( s[k - 1] );
                    if( ( r <= 2.2204460492503131E-16 * nrm ) || ( r <= 1.0020841800044864E-292 ) )
                    {
                        s[k - 1] = 0.0;
                        exitg2 = true;
                    }
                    else
                    {
                        k--;
                    }
                }
            }

            if( qs == qjj + 1 )
            {
                qjj = 3;
            }
            else if( qs == m + 2 )
            {
                qjj = 1;
            }
            else
            {
                qjj = 2;
                q = qs;
            }
        }

        switch( qjj )
        {
        case 1:
            r = e[m];
            e[m] = 0.0;
            qjj = m + 1;
            for( k = qjj; k >= q + 1; k-- )
            {
                xrotg( &s[k - 1], &r, &sm, &sqds );
                if( k > q + 1 )
                {
                    r = -sqds * e[k - 2];
                    e[k - 2] *= sm;
                }
            }
            break;

        case 2:
            r = e[q - 1];
            e[q - 1] = 0.0;
            for( k = q + 1; k <= m + 2; k++ )
            {
                xrotg( &s[k - 1], &r, &sm, &sqds );
                b = e[k - 1];
                r = -sqds * b;
                e[k - 1] = b * sm;
            }
            break;

        case 3:
            qjj = m + 1;
            scale = std::abs( s[m + 1] );
            r = std::abs( s[m] );
            if( ( ! ( scale > r ) ) && ( ! rtIsNaN( r ) ) )
            {
                scale = r;
            }

            r = std::abs( e[m] );
            if( ( ! ( scale > r ) ) && ( ! rtIsNaN( r ) ) )
            {
                scale = r;
            }

            r = std::abs( s[q] );
            if( ( ! ( scale > r ) ) && ( ! rtIsNaN( r ) ) )
            {
                scale = r;
            }

            r = std::abs( e[q] );
            if( ( ! ( scale > r ) ) && ( ! rtIsNaN( r ) ) )
            {
                scale = r;
            }

            sm = s[m + 1] / scale;
            nrm = s[m] / scale;
            r = e[m] / scale;
            sqds = s[q] / scale;
            b = ( ( nrm + sm ) * ( nrm - sm ) + r * r ) / 2.0;
            nrm = sm * r;
            nrm *= nrm;
            if( ( b != 0.0 ) || ( nrm != 0.0 ) )
            {
                r = std::sqrt( b * b + nrm );
                if( b < 0.0 )
                {
                    r = -r;
                }

                r = nrm / ( b + r );
            }
            else
            {
                r = 0.0;
            }

            r += ( sqds + sm ) * ( sqds - sm );
            nrm = sqds * ( e[q] / scale );
            for( k = q + 1; k <= qjj; k++ )
            {
                xrotg( &r, &nrm, &sm, &sqds );
                if( k > q + 1 )
                {
                    e[k - 2] = r;
                }

                b = e[k - 1];
                nrm = s[k - 1];
                e[k - 1] = sm * b - sqds * nrm;
                r = sqds * s[k];
                s[k] *= sm;
                s[k - 1] = sm * nrm + sqds * b;
                xrotg( &s[k - 1], &r, &sm, &sqds );
                b = e[k - 1];
                r = sm * b + sqds * s[k];
                s[k] = -sqds * b + sm * s[k];
                nrm = sqds * e[k];
                e[k] *= sm;
            }

            e[m] = r;
            iter++;
            break;

        default:
            if( s[q] < 0.0 )
            {
                s[q] = -s[q];
            }

            qjj = q + 1;
            while( ( q + 1 < 4 ) && ( s[q] < s[qjj] ) )
            {
                nrm = s[q];
                s[q] = s[qjj];
                s[qjj] = nrm;
                q = qjj;
                qjj++;
            }

            iter = 0;
            m--;
            break;
        }
    }

    U[0] = s[0];
    U[1] = s[1];
    U[2] = s[2];
}