camera.cpp
Go to the documentation of this file.
00001 /****************************************************************************
00002  *
00003  * $Id: file.cpp 3496 2011-11-22 15:14:32Z fnovotny $
00004  *
00005  * This file is part of the ViSP software.
00006  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
00007  * 
00008  * This software is free software; you can redistribute it and/or
00009  * modify it under the terms of the GNU General Public License
00010  * ("GPL") version 2 as published by the Free Software Foundation.
00011  * See the file LICENSE.txt at the root directory of this source
00012  * distribution for additional information about the GNU GPL.
00013  *
00014  * For using ViSP with software that can not be combined with the GNU
00015  * GPL, please contact INRIA about acquiring a ViSP Professional 
00016  * Edition License.
00017  *
00018  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
00019  * 
00020  * This software was developed at:
00021  * INRIA Rennes - Bretagne Atlantique
00022  * Campus Universitaire de Beaulieu
00023  * 35042 Rennes Cedex
00024  * France
00025  * http://www.irisa.fr/lagadic
00026  *
00027  * If you have questions regarding the use of this file, please contact
00028  * INRIA at visp@inria.fr
00029  * 
00030  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
00031  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
00032  *
00033  * Contact visp@irisa.fr if any conditions of this licensing are
00034  * not clear to you.
00035  *
00036  * Description:
00037  * 
00038  *
00039  * Authors:
00040  * Filip Novotny
00041  * 
00042  *
00043  *****************************************************************************/
00044 
00051 #include "camera.h"
00052 #include "names.h"
00053 #include <sstream>
00054 #include <visp_bridge/image.h>
00055 #include "visp_camera_calibration/calibrate.h"
00056 #include <visp/vpImage.h>
00057 #include <visp/vpDot2.h>
00058 #include <visp/vpCalibration.h>
00059 #include "camera_calibration_parsers/parse.h"
00060 #include "camera_calibration_parsers/parse_ini.h"
00061 
00062 
00063 #include <visp/vpDisplayX.h>
00064 #include "visp/vpTrackingException.h"
00065 
00066 
00067 namespace visp_camera_calibration
00068 {
00069 Camera::Camera() :
00070             spinner(0),
00071             queue_size_(1000),
00072             nb_points_(4),
00073             img_(360,480,255)
00074 
00075 {
00076   std::string images_path;
00077 
00078   //prepare function objects
00079   set_camera_info_service_callback_t set_camera_info_callback = boost::bind(&Camera::setCameraInfoCallback, this, _1,_2);
00080 
00081   //define services
00082   set_camera_info_service_ = n_.advertiseService(visp_camera_calibration::set_camera_info_service,set_camera_info_callback);
00083 
00084   raw_image_publisher_ = n_.advertise<sensor_msgs::Image>(visp_camera_calibration::raw_image_topic, queue_size_);
00085 
00086   calibrate_service_ = n_.serviceClient<visp_camera_calibration::calibrate> (visp_camera_calibration::calibrate_service);
00087 
00088   ros::param::getCached(visp_camera_calibration::images_path_param,images_path);
00089 
00090   reader_.setFileName(images_path.c_str());
00091   reader_.setFirstFrameIndex(1);
00092   reader_.open(img_);
00093 
00094   ROS_INFO_STREAM("str=" << images_path);
00095   vpDisplay* disp = new vpDisplayX();
00096   disp->init(img_);
00097   disp->setTitle("camera");
00098 
00099   vpDisplay::display(img_);
00100   vpDisplay::displayCharString(img_,img_.getHeight()/2,img_.getWidth()/4,"Click to publish camera feed.",vpColor::red);
00101   vpDisplay::flush(img_);
00102 
00103   spinner.start();
00104 }
00105 
00106 void Camera::sendVideo(){
00107   double gray_level_precision;
00108   double size_precision;
00109 
00110   ros::param::getCached(visp_camera_calibration::gray_level_precision_param,gray_level_precision);
00111   ros::param::getCached(visp_camera_calibration::size_precision_param,size_precision);
00112   ROS_INFO("Click to start sending image data");
00113   while(ros::ok() && !vpDisplay::getClick(img_,false));
00114 
00115   for(unsigned int i=0;i<(unsigned int)reader_.getLastFrameIndex() && ros::ok();i++){
00116     reader_.acquire(img_);
00117     sensor_msgs::Image image;
00118 
00119     image = visp_bridge::toSensorMsgsImage(img_);
00120 
00121     vpDisplay::display(img_);
00122 
00123     vpDisplay::displayCharString(img_,img_.getHeight()/2,img_.getWidth()/4,boost::str(boost::format("publishing frame %1% on %2%") % (i+1) % raw_image_publisher_.getTopic()).c_str(),vpColor::red);
00124     vpDisplay::flush(img_);
00125 
00126     raw_image_publisher_.publish(image);
00127 
00128     ROS_INFO("Sending image %d/%d",i+1,(int)reader_.getLastFrameIndex());
00129     //vpDisplay::getClick(img_);
00130   }
00131 
00132   ROS_INFO("When finished selecting points, click on the camera window for calibration");
00133   vpDisplay::displayCharString(img_,img_.getHeight()/2+30,img_.getWidth()/4,"When finished selecting points, click here for calibration",vpColor::red);
00134   vpDisplay::flush(img_);
00135   while(ros::ok() && !vpDisplay::getClick(img_,false));
00136   visp_camera_calibration::calibrate calibrate_comm;
00137   calibrate_comm.request.method = vpCalibration::CALIB_VIRTUAL_VS_DIST;
00138   calibrate_comm.request.sample_width= img_.getWidth();
00139   calibrate_comm.request.sample_height = img_.getHeight();
00140   if (calibrate_service_.call(calibrate_comm)){
00141       ROS_INFO("service called successfully");
00142 
00143       ROS_INFO("standard deviation with distorsion:");
00144       for(std::vector<double>::iterator i = calibrate_comm.response.stdDevErrs.begin();i!=calibrate_comm.response.stdDevErrs.end();i++)
00145         ROS_INFO("%f",*i);
00146   }else{
00147     ROS_ERROR("Failed to call service");
00148   }
00149 }
00150 
00151 bool Camera::setCameraInfoCallback(sensor_msgs::SetCameraInfo::Request  &req,
00152                              sensor_msgs::SetCameraInfo::Response &res){
00153   std::string calib_info;
00154   std::stringstream ss(calib_info);
00155 
00156   //std::ostream os;
00157   ROS_INFO("setting camera info");
00158   camera_calibration_parsers::writeCalibrationIni(ss,visp_camera_calibration::raw_image_topic,req.camera_info);
00159   ROS_INFO("%s",ss.str().c_str());
00160   camera_calibration_parsers::writeCalibration("calibration.ini",visp_camera_calibration::raw_image_topic,req.camera_info);
00161 
00162 
00163   ROS_INFO("end of setting camera info");
00164   return true;
00165 }
00166 
00167 Camera::~Camera()
00168 {
00169   // TODO Auto-generated destructor stub
00170 }
00171 }


visp_camera_calibration
Author(s): Filip Novotny
autogenerated on Mon Oct 6 2014 08:40:26