Swissranger.h

Go to the documentation of this file.

/****************************************************************
*
* Copyright (c) 2010
*
* Fraunhofer Institute for Manufacturing Engineering
* and Automation (IPA)
*
* +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* Project name: care-o-bot
* ROS stack name: cob_driver
* ROS package name: cob_camera_sensors
* Description: Platform independent interface to MESA Swissranger camera.
* Implementation depends on libusbSR library.
*
* +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* Author: Jan Fischer, email:jan.fischer@ipa.fhg.de
* Supervised by: Jan Fischer, email:jan.fischer@ipa.fhg.de
*
* Date of creation: July 2008
* ToDo:
*
* +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Fraunhofer Institute for Manufacturing
* Engineering and Automation (IPA) nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License LGPL 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 LGPL for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License LGPL along with this program.
* If not, see <http://www.gnu.org/licenses/>.
*
****************************************************************/
 

#ifndef __IPA_SWISSRANGER_H__
#define __IPA_SWISSRANGER_H__

#include "StdAfx.h"

// Windows
#ifdef _WIN32
        // Windows with MinGW
        #ifdef __MINGW__
                typedef short __wchar_t;
        #endif
#endif

// Linux
#ifdef __LINUX__
typedef unsigned long DWORD;
#endif

#ifdef __LINUX__
        #include <cob_camera_sensors/AbstractRangeImagingSensor.h>
#else
        #include <cob_driver/cob_camera_sensors/common/include/cob_camera_sensors/AbstractRangeImagingSensor.h>
#endif

#include <stdio.h>
#include <math.h>
#include <sstream>
#include <assert.h>
#include <libMesaSR.h>

namespace ipa_CameraSensors {

// former SR31Consts.h entries
#define SAFE_FREE(p)       { if(p) { delete (p); (p)=0; } }
#define SWISSRANGER_COLUMNS 176
#define SWISSRANGER_ROWS 144

int LibMesaCallback(SRCAM srCam, unsigned int msg, unsigned int param, void* data);

class __DLL_LIBCAMERASENSORS__ Swissranger : public AbstractRangeImagingSensor
{
public:

        Swissranger();
        ~Swissranger();

        //*******************************************************************************
        // AbstractRangeImagingSensor interface implementation
        //*******************************************************************************

        unsigned long Init(std::string directory, int cameraIndex = 0);

        unsigned long Open();
        unsigned long Close();

        unsigned long SetProperty(t_cameraProperty* cameraProperty);
        unsigned long SetPropertyDefaults();
        unsigned long GetProperty(t_cameraProperty* cameraProperty);

        unsigned long AcquireImages(int widthStepRange, int widthStepGray, int widthStepCartesian, char* RangeImage=NULL, char* IntensityImage=NULL,
                char* cartesianImage=NULL, bool getLatestFrame=true, bool undistort=true,
                ipa_CameraSensors::t_ToFGrayImageType grayImageType = ipa_CameraSensors::INTENSITY_32F1);
        unsigned long AcquireImages(cv::Mat* rangeImage = 0, cv::Mat* grayImage = 0,
                cv::Mat* cartesianImage = 0, bool getLatestFrame = true, bool undistort = true,
                ipa_CameraSensors::t_ToFGrayImageType grayImageType = ipa_CameraSensors::INTENSITY_32F1);

        unsigned long SaveParameters(const char* filename);

        bool isInitialized() {return m_initialized;}
        bool isOpen() {return m_open;}

private:
        //*******************************************************************************
        // Camera specific members
        //*******************************************************************************

        unsigned long GetCalibratedZMatlab(int u, int v, float zRaw, float& zCalibrated);
        unsigned long GetCalibratedZSwissranger(int u, int v, int width, float& zCalibrated);
        unsigned long GetCalibratedXYMatlab(int u, int v, float z, float& x, float& y);
        unsigned long GetCalibratedXYSwissranger(int u, int v, int width, float& x, float& y);

        unsigned long LoadParameters(const char* filename, int cameraIndex);

        unsigned long SetParameters();

        SRCAM m_SRCam;                   
        int m_NumOfImages;               
        ImgEntry* m_DataBuffer;  

        // Stores for cartesian data, when native swissranger calibration is used
        float m_X[SWISSRANGER_COLUMNS * SWISSRANGER_ROWS];
        float m_Y[SWISSRANGER_COLUMNS * SWISSRANGER_ROWS];
        float m_Z[SWISSRANGER_COLUMNS * SWISSRANGER_ROWS];

        bool m_CoeffsInitialized; 
        bool m_GrayImageAcquireCalled; 

        cv::Mat m_CoeffsA0; 
        cv::Mat m_CoeffsA1; 
        cv::Mat m_CoeffsA2; 
        cv::Mat m_CoeffsA3; 
        cv::Mat m_CoeffsA4; 
        cv::Mat m_CoeffsA5; 
        cv::Mat m_CoeffsA6; 
};

__DLL_LIBCAMERASENSORS__ AbstractRangeImagingSensorPtr CreateRangeImagingSensor_Swissranger();

} // End namespace ipa_CameraSensors
#endif // __IPA_SWISSRANGER_H__