srba/srba_options_noise.h

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#pragma once

#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/math/lightweight_geom_data.h>

namespace srba {
namespace options
{
        struct observation_noise_identity
        {
            struct parameters_t
            {
                double std_noise_observations;

                parameters_t() : std_noise_observations (1.) 
                { }
            };

            struct noise_data_per_obs_t
            {
                // None: all obs. have the same value="std_noise_observations" 
            };

            template <class MATRIX_H,class MATRIX_J1,class MATRIX_J2>
            inline static void accum_JtJ(MATRIX_H & H, const MATRIX_J1 & J1, const MATRIX_J2 &J2, const size_t obs_idx, const parameters_t & obs_noise_params) 
            {
                MRPT_UNUSED_PARAM(obs_idx); MRPT_UNUSED_PARAM(obs_noise_params);
                H.noalias() += J1.transpose() * J2;  // The constant scale factor 1/sigma will be applied in the end (below)
            }
            template <class MATRIX_H>
            inline static void scale_H(MATRIX_H & H, const parameters_t & obs_noise_params) 
            {
                ASSERTDEB_(obs_noise_params.std_noise_observations>0)
                H *= 1.0/obs_noise_params.std_noise_observations;
            }

            template <class VECTOR_GRAD,class MATRIX_J,class VECTOR_R>
            inline static void accum_Jtr(VECTOR_GRAD & g, const MATRIX_J & J, const VECTOR_R &r, const size_t obs_idx, const parameters_t & obs_noise_params) 
            {
                MRPT_UNUSED_PARAM(obs_idx); MRPT_UNUSED_PARAM(obs_noise_params);
                g.noalias() += J.transpose() * r;  // The constant scale factor 1/sigma will be applied in the end (below)
            }
            template <class VECTOR_GRAD>
            inline static void scale_Jtr(VECTOR_GRAD & g, const parameters_t & obs_noise_params) 
            {
                ASSERTDEB_(obs_noise_params.std_noise_observations>0)
                g *= 1.0/obs_noise_params.std_noise_observations;
            }

        };  // end of "observation_noise_identity"

        template <class obs_t>
        struct observation_noise_constant_matrix
        {
            static const size_t OBS_DIMS = obs_t::OBS_DIMS;  

            typedef Eigen::Matrix<double,OBS_DIMS,OBS_DIMS>  obs_noise_matrix_t; 

            struct parameters_t
            {
                obs_noise_matrix_t  lambda;

                parameters_t() : lambda( obs_noise_matrix_t::Identity() ) 
                { }
            };

            struct noise_data_per_obs_t
            {
                // None: all obs. have the same value
            };

            template <class MATRIX_H,class MATRIX_J1,class MATRIX_J2>
            inline static void accum_JtJ(MATRIX_H & H, const MATRIX_J1 & J1, const MATRIX_J2 &J2,
                const size_t obs_idx, const parameters_t & obs_noise_params) 
            {
                MRPT_UNUSED_PARAM(obs_idx);
                H.noalias() += J1.transpose() * obs_noise_params.lambda * J2;
            }

            template <class MATRIX_H>
            inline static void scale_H(MATRIX_H & H, const parameters_t & obs_noise_params) 
            {  // Nothing else to do.
                MRPT_UNUSED_PARAM(H);
                MRPT_UNUSED_PARAM(obs_noise_params);
            }

            template <class VECTOR_GRAD,class MATRIX_J,class VECTOR_R>
            inline static void accum_Jtr(VECTOR_GRAD & g, const MATRIX_J & J, const VECTOR_R &r,
                const size_t obs_idx, const parameters_t & obs_noise_params) 
            {
                MRPT_UNUSED_PARAM(obs_idx);
                g.noalias() += J.transpose() * obs_noise_params.lambda * r;
            }
            template <class VECTOR_GRAD>
            inline static void scale_Jtr(VECTOR_GRAD & g, const parameters_t & obs_noise_params) 
            {  // Nothing else to do.
                MRPT_UNUSED_PARAM(g); MRPT_UNUSED_PARAM(obs_noise_params);
            }

        };  // end of "observation_noise_constant_matrix"

} } // End of namespaces