calibration_job_definition.h

Go to the documentation of this file.

/*
 * Software License Agreement (Apache License)
 *
 * Copyright (c) 2014, Southwest Research Institute
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef CALIBRATION_JOB_DEFINITION_H_
#define CALIBRATION_JOB_DEFINITION_H_


#include <industrial_extrinsic_cal/basic_types.h>
#include <industrial_extrinsic_cal/camera_observer.hpp>
#include <industrial_extrinsic_cal/camera_definition.h>
#include <industrial_extrinsic_cal/observation_scene.h>
#include <industrial_extrinsic_cal/observation_data_point.h>
#include <industrial_extrinsic_cal/ceres_blocks.h>
#include <industrial_extrinsic_cal/ros_camera_observer.h>
#include <industrial_extrinsic_cal/ceres_costs_utils.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include <ros/console.h>
#include <yaml-cpp/yaml.h>
#include <fstream>
#include <iostream>

namespace industrial_extrinsic_cal
{

class CalibrationJob
{
public:
  CalibrationJob(std::string camera_fn, std::string target_fn, std::string caljob_fn) :
      camera_def_file_name_(camera_fn), target_def_file_name_(target_fn), caljob_def_file_name_(caljob_fn)
  {
  }
  ;

  ~CalibrationJob()
  {
  }
  ;

  bool load();

  bool store();

  bool run();

  bool clearJobCameraObservers();

  bool clearJobTargets();

  bool clearObservationData();

  const std::vector<P_BLOCK> getExtrinsics() const
  {
    return extrinsics_;
  }

  const std::vector<P_BLOCK> getOriginalExtrinsics() const
  {
    return original_extrinsics_;
  }

  const std::vector<P_BLOCK> getTargetPose() const
  {
    return target_pose_;
  }

  const std::vector<std::string>& getCameraIntermediateFrame() const
  {
    return camera_intermediate_frames_;
  }

  const std::vector<std::string>& getCameraOpticalFrame() const
  {
    return camera_optical_frames_;
  }

  const std::string& getReferenceFrame() const
  {
    return reference_frame_;
  }

  const std::vector<std::string>& getTargetFrames() const
  {
    return target_frames_;
  }

  //    ::std::ostream& operator<<(::std::ostream& os, const CalibrationJob& C){ return os<< "TODO";}
protected:
  bool loadTarget();

  bool loadCamera();

  bool loadCalJob();

  bool runObservations();

  bool runOptimization();

  bool addCameraObserver(Camera camera_to_add);

  bool addTarget(Target target_to_add);

  bool selectScene(int scene_id);

  bool addObservationToCurrentScene(boost::shared_ptr<ROSCameraObserver> camera_observer,
                                    boost::shared_ptr<Target> target);

  bool clearCurrentScene();

  bool appendNewScene(Trigger trig);

private:
  std::vector<ObservationDataPointList> observation_data_point_list_;
  std::vector<ObservationScene> scene_list_; 
  std::string camera_def_file_name_; 
  std::string target_def_file_name_; 
  std::string caljob_def_file_name_; 
  std::string reference_frame_; 
  std::vector<std::string> target_frames_; 
  std::vector<std::string> camera_optical_frames_; 
  std::vector<std::string> camera_intermediate_frames_; 
  int current_scene_; 
  std::vector<ROSCameraObserver> camera_observers_; 
  std::vector<Target> defined_target_set_; 
  CeresBlocks ceres_blocks_; 
  ceres::Problem problem_; 
  std::vector<P_BLOCK> extrinsics_; 
  std::vector<P_BLOCK> original_extrinsics_; 
  std::vector<P_BLOCK> target_pose_; 
};//end class

}//end namespace industrial_extrinsic_cal

#endif /* CALIBRATION_JOB_DEFINITION_H_ */