poseslam_alg.h

Go to the documentation of this file.

// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
// Author 
// All rights reserved.
//
// This file is part of iri-ros-pkg
// iri-ros-pkg is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program 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 Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
// 
// IMPORTANT NOTE: This code has been generated through a script from the 
// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
// of the scripts. ROS topics can be easly add by using those scripts. Please
// refer to the IRI wiki page for more information:
// http://wikiri.upc.es/index.php/Robotics_Lab

#ifndef _poseslam_alg_h_
#define _poseslam_alg_h_

#include <iri_poseslam/PoseslamConfig.h>
#include "mutex.h"
#include "poseSLAM.h"
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>

//include poseslam_alg main library

class PoseslamAlgorithm
{
  protected:
   
    CMutex alg_mutex_;
    

    // private attributes and methods

  public:
    typedef iri_poseslam::PoseslamConfig Config;

    Config config_;

    PoseslamAlgorithm(void);

    void lock(void) { alg_mutex_.enter(); };

    void unlock(void) { alg_mutex_.exit(); };

    bool try_enter(void) { return alg_mutex_.try_enter(); };

    void config_update(Config& new_cfg, uint32_t level=0);

    ~PoseslamAlgorithm(void);
    
    // here define all poseslam_alg interface methods to retrieve and set
    // the driver parameters
    
    void augmentation(const uint& step, const geometry_msgs::PoseWithCovarianceStamped& odom);
    
    void create_candidates_list(const bool& realOdometryCov);
    
    void redundant_evaluation();
    
    bool loop_closure_requeriments() const;
    
    bool try_loop_closure(const geometry_msgs::PoseWithCovarianceStamped& odom);
    
    void update_candidates_list(const bool LoopClosure, const bool& realOdometryCov);
    
    bool any_candidate() const;
    
    void select_best_candidate();
    
    uint get_candidate_step() const;
    
    geometry_msgs::Pose get_candidate_d() const;
    
    double get_candidate_ig() const;
    
    bool is_redundant() const;
    
    std::vector<VectorXd> get_trajectory() const;
    
    std::vector<std::vector<double> >  get_trajectory_covariance() const;
    
    std::vector<uint> get_trajectory_steps() const;
    
    std::vector<VectorXd> get_trajectory_no_loops() const;
    
    std::vector<std::vector<double> >  get_trajectory_covariance_no_loops() const;
    
    VectorXd get_last_pose() const;

    std::vector<double> get_last_covariance() const;

    uint get_last_step() const;

    VectorXd get_last_pose_no_loops() const;

    std::vector<double> get_last_covariance_no_loops() const;

    uint get_nStates() const;
    
    void print_matrix_in_file(const MatrixXd& M, std::fstream& file) const;    
    
    void print_vector_in_file(const std::vector<double>& v, std::fstream& file) const;    
    
    void print_vector_in_file(const std::vector<uint>& v, std::fstream& file) const;    
    
    void print_time_file(std::fstream& file) const;    
    
    //Create object
    CPoseSLAM* pose_SLAM_;
    CPoseSLAM* pose_SLAM_noLoops_;
    bool inicialitzat_;
    std::vector<Eigen::MatrixXd> Q_aug_;
    std::vector<Eigen::MatrixXd> Q_loop_;
    std::vector<Eigen::VectorXd> d_aug_;
    std::vector<Eigen::VectorXd> d_loop_;
    std::vector<bool> success_loop_;
    Eigen::MatrixXd Q_odom_;
    std::vector<double> timePub;
    std::vector<double> timeSensOpen;
    std::vector<double> timeSensClose;
    double tPub, tSens;
};

#endif