MotionModel.hpp

Go to the documentation of this file.

#ifndef ODOMETRY_MOTION_MODEL_HPP
#define ODOMETRY_MOTION_MODEL_HPP

#include <iostream> 
#include <vector> 
#include <boost/shared_ptr.hpp> 
#include <base-logging/Logging.hpp> 
#include <Eigen/Geometry> 
#include <Eigen/Core> 
#include <Eigen/Dense> 
#include <Eigen/Cholesky> 
//#define DEBUG_PRINTS_ODOMETRY_MOTION_MODEL 1 //TO-DO: Remove this. Only for testing (master branch) purpose

namespace threed_odometry
{

    template <typename _Scalar>
    class MotionModel
    {
        protected:
            int number_chains, number_robot_joints, number_slip_joints, number_contact_joints;

        public:
            unsigned int model_dof;

        protected:

            inline void navEquations(const Eigen::Matrix <_Scalar, 6, 1> &cartesianVelocities,
                                    const Eigen::Matrix <_Scalar, Eigen::Dynamic, 1> &modelVelocities,
                                    const Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> &J,
                                    Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> &unknownA,
                                    Eigen::Matrix <_Scalar, Eigen::Dynamic, 1> &unknownx,
                                    Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> &knownB,
                                    Eigen::Matrix <_Scalar, Eigen::Dynamic, 1> &knowny,
                                    bool known_contact_angles = false)
            {
                Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic> spareI;
                spareI.resize(6*this->number_chains, 6);

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<<"[MOTION_MODEL] navEquations\n";
                #endif

                for (register int i=0; i<this->number_chains; ++i)
                    spareI.block(i*6, 0, 6, 6) = Eigen::Matrix <_Scalar, 6, 6>::Identity();

                unknownA.block(0, 0, 6*this->number_chains, 3) = spareI.block(0, 0, 6*this->number_chains, 3);//Linear Velocities

                unknownx.block(0, 0, 3, 1) = cartesianVelocities.block(0, 0, 3, 1);

                unknownA.block(0, 3, 6*this->number_chains, this->number_chains) = -J.block(0, this->number_robot_joints+this->number_slip_joints-(this->number_chains), 6*this->number_chains, this->number_chains);
                unknownx.block(3, 0, this->number_chains, 1) = modelVelocities.block(this->number_robot_joints+this->number_slip_joints-(this->number_chains), 0, this->number_chains, 1);

                if (known_contact_angles == false)
                {
                    unknownA.block(0, 3+this->number_chains, 6*this->number_chains, this->number_contact_joints) = -J.block(0, this->number_robot_joints+this->number_slip_joints, 6*this->number_chains, this->number_contact_joints);
                    unknownx.block(3+this->number_chains, 0, this->number_contact_joints, 1) = modelVelocities.block(this->number_robot_joints+this->number_slip_joints, 0, this->number_contact_joints, 1);
                }

                knownB.block(0, 0, 6*this->number_chains, 3) = -spareI.block(0, 3, 6*this->number_chains, 3); //Angular velocities
                knownB.block(0, 3, 6*this->number_chains, this->number_robot_joints) = J.block(0, 0, 6*this->number_chains, this->number_robot_joints);//Robot joints

                knowny.block(0, 0, 3, 1) = cartesianVelocities.template block<3, 1> (3,0); //Angular velocities
                knowny.block(3, 0, this->number_robot_joints, 1) = modelVelocities.block(0, 0, this->number_robot_joints, 1); //Robot joints

                if (known_contact_angles == true)
                {
                    knownB.block(0, 3+this->number_robot_joints, 6*this->number_chains, this->number_contact_joints) = J.block(0, this->number_robot_joints+this->number_slip_joints, 6*this->number_chains, this->number_contact_joints);//Robot joints
                    knowny.block(3+this->number_robot_joints, 0, this->number_contact_joints, 1) = modelVelocities.block(this->number_robot_joints+this->number_slip_joints, 0, this->number_contact_joints, 1);
                }

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<< "[MOTION_MODEL] spareI is of size "<<spareI.rows()<<"x"<<spareI.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The spareI matrix:\n" << spareI << std::endl;
                std::cout<< "[MOTION_MODEL] unknownA is of size "<<unknownA.rows()<<"x"<<unknownA.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The unknownA matrix:\n" << unknownA << std::endl;
                std::cout<< "[MOTION_MODEL] knownB is of size "<<knownB.rows()<<"x"<<knownB.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The knownB matrix:\n" << knownB << std::endl;
                #endif

                return;
            }

        public:

            MotionModel (const int _number_chains, const int _number_robot_joints, const int _number_slip_joints, const int _number_contact_joints):
                            number_chains(_number_chains),
                            number_robot_joints(_number_robot_joints),
                            number_slip_joints(_number_slip_joints),
                            number_contact_joints(_number_contact_joints)
            {
                this->model_dof = number_robot_joints + number_slip_joints + number_contact_joints;

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<<"[MOTION_MODEL] Constructor. Model DoF:"<<this->model_dof<<"\n";
                #endif

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<<"\n[MOTION_MODEL] **** END ****\n";
                #endif

                return;
            }


            ~MotionModel()
            {
            }

            virtual double navSolver(const Eigen::Matrix <_Scalar, Eigen::Dynamic, 1> &modelPositions,
                                    Eigen::Matrix <_Scalar, Eigen::Dynamic, 1> &modelVelocities,
                                    const Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> &J,
                                    Eigen::Matrix <_Scalar, 6, 1> &cartesianVelocities,
                                    Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> &modelVelCov,
                                    Eigen::Matrix <_Scalar, 6, 6> &cartesianVelCov,
                                    const Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> Weight,
                                    bool known_contact_angles = false)
            {
                double normalizedError = std::numeric_limits<double>::quiet_NaN(); //solution error of the Least-Squares

                Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> unknownA; // Nav non-sensed values matrix
                Eigen::Matrix <_Scalar, Eigen::Dynamic, 1> unknownx; // Nav non-sensed values vector
                Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> knownB; // Nav sensed values matrix
                Eigen::Matrix <_Scalar, Eigen::Dynamic, 1> knowny; // Nav sensed values vector

                if (known_contact_angles == false)
                {
                    unknownA.resize(6*this->number_chains, 3+this->number_chains+this->number_contact_joints);// position, z-slip and contact angles
                    unknownx.resize(3+this->number_chains+this->number_contact_joints, 1);// position, z-slip and contact angles
                    knownB.resize(6*this->number_chains, 3+this->number_robot_joints);// rotation and robot joints
                    knowny.resize(3+this->number_robot_joints, 1);// rotation and robot joints
                }
                else
                {
                    unknownA.resize(6*this->number_chains, 3+this->number_chains); // position and z-slip
                    unknownx.resize(3+this->number_chains); // position and z-slip
                    knownB.resize(6*this->number_chains, 3+this->number_robot_joints+this->number_contact_joints);// rotation, robot joints and contact angles
                    knowny.resize(3+this->number_robot_joints+this->number_contact_joints, 1);// rotation, robot joints and contact angles
                }

                unknownA.setZero(); unknownx.setZero();
                knownB.setZero(); knowny.setZero();

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout << "[MOTION_MODEL] cartesianVelocities is of size "<<cartesianVelocities.rows()<<"x"<<cartesianVelocities.cols()<<"\n";
                std::cout << "[MOTION_MODEL] cartesianVelocities is \n" << cartesianVelocities<< std::endl;

                std::cout << "[MOTION_MODEL] modelVelocities is of size "<<modelVelocities.rows()<<"x"<<modelVelocities.cols()<<"\n";
                std::cout << "[MOTION_MODEL] modelVelocities is \n" << modelVelocities<< std::endl;
                #endif

                assert(modelPositions.size() == modelVelocities.size());
                assert(modelPositions.size() == this->model_dof);
                assert(modelVelCov.cols() == modelVelCov.rows());
                assert(modelVelCov.cols() == modelVelocities.size());
                assert(Weight.cols() == 6*this->number_chains);
                assert(Weight.rows() == 6*this->number_chains);

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<< "[MOTION_MODEL] Weight is of size "<<Weight.rows()<<"x"<<Weight.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The Weight matrix:\n" << Weight << std::endl;
                std::cout<< "[MOTION_MODEL] J is of size "<<J.rows()<<"x"<<J.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The J matrix \n" << J << std::endl;
                #endif

                this->navEquations (cartesianVelocities, modelVelocities, J, unknownA, unknownx, knownB, knowny, known_contact_angles);

                Eigen::Matrix <_Scalar, Eigen::Dynamic, 1> knownb;
                knownb.resize(6*this->number_chains, 1);
                knownb = knownB*knowny;

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                if (known_contact_angles == false)
                {
                    Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic> Conj;
                    Conj.resize(6*this->number_chains, 3+this->number_chains+this->number_contact_joints+1);

                    Eigen::FullPivLU<Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> > lu_decompA(unknownA);
                    std::cout << "[MOTION_MODEL] The rank of A is " << lu_decompA.rank() << std::endl;

                    Eigen::FullPivLU<Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> > lu_decompB(knownB);
                    std::cout << "[MOTION_MODEL] The rank of B is " << lu_decompB.rank() << std::endl;

                    Conj.block(0, 0, 6*this->number_chains, 3+this->number_chains+this->number_contact_joints) = unknownA;
                    Conj.block(0, 3+this->number_chains+this->number_contact_joints, 6*this->number_chains, 1) = knownb;
                    Eigen::FullPivLU< Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> > lu_decompConj(Conj);
                    std::cout << "[MOTION_MODEL] The rank of A|B*y is " << lu_decompConj.rank() << std::endl;
                    std::cout << "[MOTION_MODEL] Pseudoinverse of A\n" << (unknownA.transpose() * Weight * unknownA).inverse() << std::endl;
                }
                else
                {
                    Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic> Conj;
                    Conj.resize(6*this->number_chains, 3+this->number_chains+1);

                    Eigen::FullPivLU<Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> > lu_decompA(unknownA);
                    std::cout << "[MOTION_MODEL] The rank of A is " << lu_decompA.rank() << std::endl;

                    Eigen::FullPivLU<Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> > lu_decompB(knownB);
                    std::cout << "[MOTION_MODEL] The rank of B is " << lu_decompB.rank() << std::endl;

                    Conj.block(0, 0, 6*this->number_chains, 3+this->number_chains) = unknownA;
                    Conj.block(0, 3+this->number_chains, 6*this->number_chains, 1) = knownb;
                    Eigen::FullPivLU< Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> > lu_decompConj(Conj);
                    std::cout << "[MOTION_MODEL] The rank of A|B*y is " << lu_decompConj.rank() << std::endl;
                    std::cout << "[MOTION_MODEL] Pseudoinverse of A\n" << (unknownA.transpose() * Weight * unknownA).inverse() << std::endl;


                }
                /*******************/
                #endif

                unknownx = (unknownA.transpose() * Weight * unknownA).ldlt().solve(unknownA.transpose() * Weight * knownb);

                Eigen::Matrix<double, 1,1> squaredError = (((unknownA*unknownx - knownb).transpose() * Weight * (unknownA*unknownx - knownb)));
                if (knownb.norm() != 0.00)
                    normalizedError = sqrt(squaredError[0]) / knownb.norm();

                Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> errorCov;
                errorCov.resize(6*this->number_chains, 6*this->number_chains);
                errorCov = (unknownA*unknownx - knownb).asDiagonal(); errorCov *= errorCov;// L-S error covariance
                Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> uncertaintyCov; // noise cov

                if (known_contact_angles == false)
                {
                    uncertaintyCov.resize(3+this->number_chains+this->number_contact_joints, 3+this->number_chains+this->number_contact_joints);
                }
                else
                {
                    uncertaintyCov.resize(3+this->number_chains, 3+this->number_chains);
                }

                Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> errorCovinverse;
                errorCovinverse = errorCov.inverse();
                if (base::isnotnan< Eigen::Matrix <_Scalar, Eigen::Dynamic, Eigen::Dynamic> >(errorCovinverse))
                {
                    uncertaintyCov = (unknownA.transpose() * errorCovinverse * unknownA).inverse(); // Observer
                }
                else
                {
                    uncertaintyCov = unknownA.transpose() * errorCov * unknownA; // Observer
                }

                uncertaintyCov = 0.5*(uncertaintyCov + uncertaintyCov.transpose());// Guarantee symmetry

                cartesianVelocities.block(0, 0, 3, 1) = unknownx.block(0, 0, 3, 1); // Linear velocities
                cartesianVelCov.block(0, 0, 3, 3) = uncertaintyCov.block(0, 0, 3,3);//Linear Velocities noise

                if (cartesianVelCov.block(3, 3, 3, 3) != cartesianVelCov.block(3, 3, 3, 3))
                {
                    cartesianVelCov.block(3, 3, 3, 3).setZero();

                    for (register size_t i=0; i<this->number_chains; ++i)
                    {
                        cartesianVelCov.block(3, 3, 3, 3) += Weight.block(3+(6*i), 3+(6*i), 3, 3) * errorCov.block(3+(6*i),3+(6*i), 3, 3);//Angular Velocities noise
                    }
                }

                modelVelocities.block(this->number_robot_joints+this->number_slip_joints-(this->number_chains), 0, this->number_chains, 1) = unknownx.block(3, 0, this->number_chains, 1);
                modelVelCov.block(this->number_robot_joints+this->number_slip_joints-(this->number_chains), this->number_robot_joints+this->number_slip_joints-(this->number_chains), this->number_chains, this->number_chains) = uncertaintyCov.block(3, 3, this->number_chains, this->number_chains);//pseudoInvUnknownA.col(3+i)[3+i]; For the time being set the error to the error in the estimation

                if (known_contact_angles == false)
                {
                    for (register int i=0; i<this->number_contact_joints; ++i)
                    {
                        modelVelocities[this->number_robot_joints+this->number_slip_joints+i] = unknownx[3+this->number_chains+i];
                        modelVelCov.col(this->number_robot_joints+this->number_slip_joints+i)[this->number_robot_joints+this->number_slip_joints+i] = uncertaintyCov.col(3+this->number_chains+i)[3+this->number_chains+i];//pseudoInvUnknownA.col(3+_RobotTrees+i)[3+_RobotTrees+i];
                    }
                }

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout << "[MOTION_MODEL] L-S solution:\n"<<unknownx<<std::endl;

                std::cout << "[MOTION_MODEL] RESULT errorCov is of size "<<errorCov.rows()<<"x"<<errorCov.cols()<<"\n";
                std::cout << "[MOTION_MODEL] RESULT errorCov is \n" << errorCov << std::endl;

                std::cout << "[MOTION_MODEL] RESULT uncertaintyCov is of size "<<uncertaintyCov.rows()<<"x"<<uncertaintyCov.cols()<<"\n";
                std::cout << "[MOTION_MODEL] RESULT uncertaintyCov is \n" << uncertaintyCov << std::endl;

                std::cout << "[MOTION_MODEL] RESULT cartesianVelocities is of size "<<cartesianVelocities.rows()<<"x"<<cartesianVelocities.cols()<<"\n";
                std::cout << "[MOTION_MODEL] RESULT cartesianVelocities is \n" << cartesianVelocities<< std::endl;

                std::cout << "[MOTION_MODEL] RESULT cartesianVelCov is of size "<<cartesianVelCov.rows()<<"x"<<cartesianVelCov.cols()<<"\n";
                std::cout << "[MOTION_MODEL] RESULT cartesianVelCov is \n" << cartesianVelCov<< std::endl;

                std::cout << "[MOTION_MODEL] RESULT modelVelocities is of size "<<modelVelocities.rows()<<"x"<<modelVelocities.cols()<<"\n";
                std::cout << "[MOTION_MODEL] RESULT modelVelocities is \n" << modelVelocities<< std::endl;

                std::cout << "[MOTION_MODEL] RESULT modelVelCov is of size "<<modelVelCov.rows()<<"x"<<modelVelCov.cols()<<"\n";
                std::cout << "[MOTION_MODEL] RESULT modelVelCov is \n" << modelVelCov<< std::endl;

                std::cout << "[MOTION_MODEL] RESULT The absolute least squared error is:\n" << squaredError << std::endl;
                std::cout << "[MOTION_MODEL] RESULT The relative error is:\n" << normalizedError << std::endl;
                std::cout << "[MOTION_MODEL] RESULT The error vector is \n"<<(unknownA*unknownx - knownb)<<"\n";
                std::cout << "[MOTION_MODEL] RESULT The error variance is \n"<<errorCov<<"\n";
                std::cout << "[MOTION_MODEL] RESULT The error variance.inverse() is \n"<<errorCov.inverse()<<"\n";
                std::cout << "[MOTION_MODEL] RESULT The solution covariance is \n"<<cartesianVelCov<<"\n";
                #endif


                return normalizedError;
            }

    };
}

#endif //ODOMETRY_MOTION_MODEL_HPP