vpFeatureMoment.cpp

/****************************************************************************
 *
 * $Id: vpFeatureMoment.cpp 5237 2015-01-30 13:52:04Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 *
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Base for all moment features
 *
 * Authors:
 * Filip Novotny
 *
 *****************************************************************************/

#include <visp/vpFeatureMoment.h>
#include <visp/vpMoment.h>
#include <visp/vpFeatureMomentDatabase.h>
#include <visp/vpMomentDatabase.h>
#include <visp/vpMath.h>

#include <visp/vpException.h>
#include <visp/vpMatrixException.h>
#include <visp/vpFeatureException.h>

#include <visp/vpDebug.h>
#include <vector>

class vpBasicFeature;

void vpFeatureMoment::init (){
    //feature dimension
    /*
     * The dimension of the visual feature is set according to the size of the vpMoment associated to it.
     * This partly explains why vpFeatureMomentBasic cannot be used directly as a visual feature.
     */
    if(this->moment!=NULL)
        dim_s = (unsigned int)this->moment->get().size();
    else
        dim_s = 0;

    nbParameters = 1;
    
    // memory allocation    
    s.resize(dim_s) ;
    for(unsigned int i=0;i<dim_s;i++)
        s[i] = 0;

    if (flags == NULL)
    flags = new bool[nbParameters];
    for (unsigned int i = 0; i < nbParameters; i++)
    flags[i] = false;
}


int vpFeatureMoment::getDimension (unsigned int select) const{
    int dim=0;

    for(unsigned int i=0;i<dim_s;++i)
        if(vpBasicFeature::FEATURE_LINE[i] & select)
            dim++;

    return dim;
}


void vpFeatureMoment::print (unsigned int select) const{
    for(unsigned int i=0;i<dim_s;++i){
        if(vpBasicFeature::FEATURE_LINE[i] & select){
            std::cout << s[i] << ",";
        }
    }

    std::cout << std::endl;
}

void vpFeatureMoment::display (const vpCameraParameters &cam, const vpImage< unsigned char > &I,
                               const vpColor &color, unsigned int thickness) const
{
    //visual representation of a moment doesn't often make sense
    (void)cam;
    (void)I;
    (void)color;
    (void)thickness;
}

void vpFeatureMoment::display (const vpCameraParameters &cam, const vpImage< vpRGBa > &I,
                               const vpColor &color, unsigned int thickness) const
{
    (void)cam;
    (void)I;
    (void)color;
    (void)thickness;
}

void vpFeatureMoment::update (double A_, double B_, double C_){
    this->A = A_;
    this->B = B_;
    this->C = C_;

    if(moment==NULL){
        bool found;        
        this->moment = &(moments.get(momentName(),found));
        if(!found) throw vpException(vpException::notInitialized,"Moment not found for feature");
    }
    nbParameters = 1;
    if(this->moment!=NULL){
        dim_s = (unsigned int)this->moment->get().size();

        s.resize(dim_s);

        for(unsigned int i=0;i<dim_s;i++)
            s[i] = this->moment->get()[i];

        if (flags == NULL)
            flags = new bool[nbParameters];
        for (unsigned int i = 0; i < nbParameters; i++)
            flags[i] = false;
    }else
        dim_s = 0;

    compute_interaction();
}

vpMatrix vpFeatureMoment::interaction (unsigned int select){
    vpMatrix L(0,0);

    for(unsigned int i=0;i<dim_s;++i){
        if(vpBasicFeature::FEATURE_LINE[i] & select){
            L.stackMatrices(interaction_matrices[i]);
        }
    }

    return L;
}

vpBasicFeature* vpFeatureMoment::duplicate () const
{
    vpFeatureMoment* feat = new vpMomentGenericFeature(moments,A,B,C,featureMomentsDataBase,moment);
    feat->interaction_matrices = interaction_matrices;
    feat->dim_s = dim_s;
    feat->nbParameters = nbParameters;    
    // memory allocation
    feat->s.resize(dim_s) ;
    for(unsigned int i=0;i<dim_s;i++)
        feat->s[i] = this->s[i];

    feat->flags = new bool[(unsigned int)nbParameters];
    for (unsigned int i = 0; i < (unsigned int)nbParameters; i++)
        feat->flags[i] = flags[i];

    return feat;
}

void vpFeatureMoment::linkTo(vpFeatureMomentDatabase& featureMoments){
  if (strlen( name() ) >= 255) {
    throw(vpException(vpException::memoryAllocationError,
                      "Not enough memory to intialize the moment name"));
  }

  std::strcpy(_name,name());
  this->featureMomentsDataBase=&featureMoments;

  featureMoments.add(*this,_name);
}


void vpFeatureMoment::compute_interaction (){

}

vpFeatureMoment::~vpFeatureMoment (){
}

VISP_EXPORT std::ostream& operator<<(std::ostream & os, const vpFeatureMoment& featM) {
    /*
    A const_cast is forced here since interaction() defined in vpBasicFeature() is not const
    But introducing const in vpBasicFeature() can break a lot of client code
    */
    vpMatrix Lcomplete((unsigned int)featM.getDimension(), 6); // 6 corresponds to 6velocities in standard interaction matrix
    Lcomplete = const_cast<vpFeatureMoment&>(featM).interaction(vpBasicFeature::FEATURE_ALL);
    Lcomplete.matlabPrint(os);
    return os;
}

void
vpFeatureMoment::printDependencies(std::ostream& os) const{
    os << " WARNING : Falling back to base class version of printDependencies() in vpFeatureMoment. To prevent that, this has to be implemented in the derived classes!" << std::endl;
}