CPCtoOCRegistration.h

Go to the documentation of this file.

/******************************************************************************
 * \file
 *
 * $Id:$
 *
 * Copyright (C) Brno University of Technology
 *
 * This file is part of software developed by dcgm-robotics@FIT group.
 *
 * Author: Vit Stancl (stancl@fit.vutbr.cz)
 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
 * Date: 25/1/2012
 *
 * This file 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 file 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 file.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once
#ifndef CPCTOOCREGISTRATION_H_
#define CPCTOOCREGISTRATION_H_

#include <sensor_msgs/CameraInfo.h>
#include <ros/callback_queue.h>
#include <tf/transform_listener.h>
#include <LinearMath/btMatrix3x3.h>

#include <boost/scoped_ptr.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/recursive_mutex.hpp>
#include <image_geometry/pinhole_camera_model.h>
#include <ros/node_handle.h>

#include <srs_env_model/but_server/server_tools.h>

#include "pcl_registration_module.h"

#include <pcl/filters/voxel_grid.h>

namespace srs_env_model
{
class COcPatchMaker
{
public:
        //typedef sensor_msgs::PointCloud2 tCloud;
        typedef tPointCloud tCloud;

public:
        COcPatchMaker();

        virtual void init(ros::NodeHandle & node_handle);

        bool computeCloud( const SMapWithParameters & par, const ros::Time & time );

        tCloud & getCloud( ) { return m_cloud; }

        void setCloudFrameId( const std::string & fid ){ m_pcFrameId = fid; }

protected:
        void onCameraChangedCB(const sensor_msgs::CameraInfo::ConstPtr &cam_info);

        // main loop when spinning our own thread
        // - process callbacks in our callback queue
        // - process pending goals
        void spinThread();

        bool inSensorCone(const cv::Point2d& uv) const;

        void publishInternal(const ros::Time & timestamp);

        virtual void handleOccupiedNode(tButServerOcTree::iterator& it, const SMapWithParameters & mp);

protected:
        bool m_bTransformCamera;

        std::string m_cameraFrameId;

        std::string m_pcFrameId;

        // Camera position topic name
        std::string m_cameraInfoTopic;

        ros::Subscriber m_camPosSubscriber;

        // Mutex used to lock camera position parameters
        boost::recursive_mutex m_camPosMutex;

        bool m_bSpinThread;

        // these are needed when spinning up a dedicated thread
        boost::scoped_ptr<boost::thread> spin_thread_;
        ros::NodeHandle node_handle_;
        ros::CallbackQueue callback_queue_;
        volatile bool need_to_terminate_;

        bool m_bCamModelInitialized;

        int m_camera_stereo_offset_left, m_camera_stereo_offset_right;

        tCloud m_cloud;

        bool m_bPublishCloud;

        image_geometry::PinholeCameraModel m_camera_model, m_camera_model_buffer;

        sensor_msgs::CameraInfo m_camera_info_buffer;

        cv::Size m_camera_size, m_camera_size_buffer;

        tf::TransformListener m_tfListener;

        ros::Publisher m_pubConstrainedPC;

        std::string m_ocFrameId;

        ros::Time m_DataTimeStamp, m_time_stamp;

        tf::Transform m_to_sensorTf;

        double m_fracX;

        double m_fracY;

        btMatrix3x3 m_fracMatrix;

public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

}; // class COcToPcl


class CPcToOcRegistration
{
public:
        typedef sensor_msgs::PointCloud2 tCloud;

        typedef CPclRegistration< tPclPoint, tPclPoint> tRegistrator;

public:
        CPcToOcRegistration();

        virtual void init(ros::NodeHandle & node_handle);

        bool registerCloud( tPointCloudPtr & cloud, const SMapWithParameters & map );

        Eigen::Matrix4f getTransform() { return m_registration.getTransform(); }

        bool isRegistering(){ return m_registration.isRegistering(); }

protected:
        COcPatchMaker m_patchMaker;

        tRegistrator m_registration;

        pcl::VoxelGrid<tCloud> m_gridFilter;

        tCloud::Ptr m_resampledCloud;

        tf::TransformListener m_tfListener;

public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

}; // class CPcToOcRegistration


} // namespace srs_env_model

#endif /* CPCTOOCREGISTRATION_H_ */