visp_camera_calibration: visp_camera_calibration: calibrator.cpp Source 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 
00050 #include "calibrator.h"
00051 #include "names.h"
00052 #include "conversions/image.h"
00053 #include "conversions/camera.h"
00054 #include "sensor_msgs/SetCameraInfo.h"
00055 #include "sensor_msgs/CameraInfo.h"
00056 #include "visp_camera_calibration/CalibPoint.h"
00057 
00058 #include <vector>
00059 
00060 namespace visp_camera_calibration
00061 {
00062   Calibrator::Calibrator() :
00063     queue_size_(1000)
00064   {
00065     //prepare function objects
00066     point_correspondence_subscriber_callback_t point_correspondence_callback = boost::bind(&Calibrator::pointCorrespondenceCallback, this, _1);
00067     calibrate_service_callback_t calibrate_callback = boost::bind(&Calibrator::calibrateCallback, this, _1, _2);
00068     //define subscribers
00069     point_correspondence_subscriber_ = n_.subscribe(visp_camera_calibration::point_correspondence_topic, queue_size_,
00070                                                     point_correspondence_callback);
00071 
00072     //define services
00073     calibrate_service_ = n_.advertiseService(visp_camera_calibration::calibrate_service,calibrate_callback);
00074 
00075     //connect to services
00076     set_camera_info_service_ = n_.serviceClient<sensor_msgs::SetCameraInfo> (visp_camera_calibration::set_camera_info_service);
00077     set_camera_info_bis_service_ = n_.serviceClient<sensor_msgs::SetCameraInfo> (visp_camera_calibration::set_camera_info_bis_service);
00078 
00079   }
00080 
00081   void Calibrator::pointCorrespondenceCallback(const visp_camera_calibration::CalibPointArray::ConstPtr& point_correspondence){
00082     vpCalibration calib_all_points;
00083     calib_all_points.clearPoint();
00084 
00085     for(std::vector<visp_camera_calibration::CalibPoint>::const_iterator i = point_correspondence->points.begin();
00086         i != point_correspondence->points.end();
00087         i++
00088     ){
00089       vpImagePoint ip(i->i,i->j);
00090       calib_all_points.addPoint(i->X,i->Y,i->Z,ip);
00091     }
00092     calibrations_.push_back(calib_all_points);
00093 
00094   }
00095 
00096   bool Calibrator::calibrateCallback(visp_camera_calibration::calibrate::Request  &req, visp_camera_calibration::calibrate::Response &res){
00097     std::vector<double> dev;
00098     std::vector<double> dev_dist;
00099     double lambda = .5;
00100     ROS_INFO("called service calibrate");
00101     vpCameraParameters cam;
00102 
00103     double px = cam.get_px();
00104     double py = cam.get_px();
00105     double u0 = req.sample_width/2;
00106     double v0 = req.sample_height/2;
00107 
00108     cam.initPersProjWithoutDistortion(px, py, u0, v0);
00109     vpCalibration::setLambda(lambda);
00110 
00111     vpCalibration::computeCalibrationMulti(vpCalibration::vpCalibrationMethodType(req.method),calibrations_.size(),&(calibrations_[0]),cam,false);
00112 
00113     for(std::vector<vpCalibration>::iterator i=calibrations_.begin();
00114         i!=calibrations_.end();
00115         i++
00116     ){
00117       double deviation;
00118       double deviation_dist;
00119       i->cam = cam;
00120       i->cam_dist = cam;
00121       i->computeStdDeviation(deviation,deviation_dist);
00122       dev.push_back(deviation);
00123       dev_dist.push_back(deviation_dist);
00124     }
00125     switch(req.method){
00126       case vpCalibration::CALIB_LAGRANGE_VIRTUAL_VS:
00127       case vpCalibration::CALIB_VIRTUAL_VS:
00128         res.stdDevErrs = dev;
00129         break;
00130       case vpCalibration::CALIB_LAGRANGE_VIRTUAL_VS_DIST:
00131       case vpCalibration::CALIB_VIRTUAL_VS_DIST:
00132         res.stdDevErrs= dev_dist;
00133         break;
00134     }
00135 
00136 
00137     ROS_INFO_STREAM("" << cam);
00138     sensor_msgs::SetCameraInfo set_camera_info_comm;
00139 
00140     set_camera_info_comm.request.camera_info = visp_bridge::toSensorMsgsCameraInfo(cam,req.sample_width,req.sample_height);
00141 
00142     set_camera_info_service_.call(set_camera_info_comm);
00143     if(set_camera_info_bis_service_.call(set_camera_info_comm)){
00144       ROS_INFO("set_camera_info service called successfully");
00145     }else{
00146       ROS_ERROR("Failed to call service set_camera_info");
00147     }
00148     return true;
00149   }
00150   void Calibrator::spin(){
00151     ros::spin();
00152   }
00153 
00154   Calibrator::~Calibrator()
00155   {
00156     // TODO Auto-generated destructor stub
00157   }
00158 }