calibration_job.hpp

Go to the documentation of this file.

/*
 * Software License Agreement (Apache License)
 *
 * Copyright (c) 2013, 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_HPP_
#define CALIBRATION_JOB_HPP_

#include <iostream>
#include <industrial_extrinsic_cal/basic_types.h> 
#include <industrial_extrinsic_cal/camera_observer.hpp> 
#include <boost/shared_ptr.hpp>
#include "ceres/ceres.h"
#include <ros/console.h>

namespace industrial_extrinsic_cal
{

class Camera
{
public:
  Camera();

  Camera(std::string name, CameraParameters camera_parameters, bool is_moving);

  ~Camera();

  bool isMoving();
  boost::shared_ptr<DummyCameraObserver> camera_observer_;
  CameraParameters camera_parameters_;
  //    ::std::ostream& operator<<(::std::ostream& os, const Camera& C){ return os<< "TODO";};

  std::string camera_name_; 
  bool fixed_intrinsics_; 
  bool fixed_extrinsics_; 
private:
  bool is_moving_; 
};
// end of class Camera

typedef struct
{ 
  int trigger_type;
  std::string trigger_popup_msg;
} Trigger;

typedef struct
{
  boost::shared_ptr<Camera> camera;
  boost::shared_ptr<Target> target;
  Roi roi;
} ObservationCmd;

class ObservationScene
{
public:
  ObservationScene(Trigger trig, int scene_id) :
      trig_(trig), scene_id_(scene_id)
  {
  }
  ;

  ObservationScene()
  {
    observation_command_list_.clear();
    cameras_in_scene_.clear();
  }
  ;

  void clear();

  void addObservationToScene(ObservationCmd observation_command);
  int get_id()
  {
    return (scene_id_);
  }
  ;

  Trigger get_trigger()
  {
    return (trig_);
  }
  ;

  std::vector<ObservationCmd> observation_command_list_; 
  std::vector<boost::shared_ptr<Camera> > cameras_in_scene_; 
private:
  Trigger trig_; 
  int scene_id_; 
};
// end of class ObservationScene

typedef struct MovingCamera
{
  boost::shared_ptr<Camera> cam; // must hold a copy of the camera extrinsic parameters
  int scene_id; // but copy of intrinsics may be and unused duplicate
} MovingCamera;

typedef struct MovingTarget
{
  boost::shared_ptr<Target> targ; // must hold a copy of the target pose parameters,
  int scene_id; // but point parameters may be unused duplicates
} MovingTarget;

class ObservationDataPoint
{
public:

  ObservationDataPoint(std::string c_name, std::string t_name, int s_id, P_BLOCK c_intrinsics, P_BLOCK c_extrinsics,
                       int point_id, P_BLOCK t_pose, P_BLOCK p_position, double image_x, double image_y)
  {
    camera_name_ = c_name;
    target_name_ = t_name;
    scene_id_ = s_id;
    camera_intrinsics_ = c_intrinsics;
    camera_extrinsics_ = c_extrinsics;
    target_pose_ = t_pose;
    point_id_ = point_id;
    point_position_ = p_position;
    image_x_ = image_x;
    image_y_ = image_y;
  }
  ;

  ~ObservationDataPoint()
  {
  }
  ;

  std::string camera_name_;
  std::string target_name_;
  int scene_id_;
  int point_id_;
  P_BLOCK camera_extrinsics_;
  P_BLOCK camera_intrinsics_;
  P_BLOCK target_pose_;
  P_BLOCK point_position_;
  double image_x_;
  double image_y_;
};
// end of class ObservationDataPoint

class ObservationDataPointList
{
public:
  ObservationDataPointList();

  ~ObservationDataPointList();

  void addObservationPoint(ObservationDataPoint new_data_point);

  std::vector<ObservationDataPoint> items;
};

class CeresBlocks
{
public:
  CeresBlocks();
  ~CeresBlocks();

  void clearCamerasTargets();

  bool addStaticCamera(boost::shared_ptr<Camera> camera_to_add);

  bool addStaticTarget(boost::shared_ptr<Target> target_to_add);

  bool addMovingCamera(boost::shared_ptr<Camera> camera_to_add, int scene_id);

  bool addMovingTarget(boost::shared_ptr<Target> target_to_add, int scene_id);

  P_BLOCK getStaticCameraParameterBlockIntrinsics(std::string camera_name);

  P_BLOCK getMovingCameraParameterBlockIntrinsics(std::string camera_name);

  P_BLOCK getStaticCameraParameterBlockExtrinsics(std::string camera_name);

  P_BLOCK getMovingCameraParameterBlockExtrinsics(std::string camera_name, int scene_id);

  P_BLOCK getStaticTargetPoseParameterBlock(std::string target_name);

  P_BLOCK getStaticTargetPointParameterBlock(std::string target_name, int point_id);

  P_BLOCK getMovingTargetPoseParameterBlock(std::string target_name, int scene_id);

  P_BLOCK getMovingTargetPointParameterBlock(std::string target_name, int pnt_id);

  //private:
  std::vector<boost::shared_ptr<Camera> > static_cameras_; 
  std::vector<boost::shared_ptr<MovingCamera> > moving_cameras_; 
  std::vector<boost::shared_ptr<Target> > static_targets_; 
  std::vector<boost::shared_ptr<MovingTarget> > moving_targets_; 
};

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 runObservations();

  bool runOptimization();

  bool addCameraObserver(Camera camera_to_add);

  bool addTarget(Target target_to_add);

  bool clearJobCameraObservers();

  bool clearJobTargets();

  bool clearObservationData();

  bool selectScene(int scene_id);

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

  bool clearCurrentScene();

  bool appendNewScene(Trigger trig);

  ObservationDataPointList observation_data_point_list_;

  //    ::std::ostream& operator<<(::std::ostream& os, const CalibrationJob& C){ return os<< "TODO";}
private:
  std::string camera_def_file_name_; 
  std::string target_def_file_name_; 
  std::string caljob_def_file_name_; 
  std::vector<ObservationScene> scene_list_; 
  int current_scene_; 
  std::vector<DummyCameraObserver> camera_observers_; 
  std::vector<Target> defined_target_set_; 
  CeresBlocks ceres_blocks_; 
  ceres::Problem problem_; 
};

} // end of namespace
#endif