Merged drlewis master into this branch.

.gitignore

@@ -11,3 +11,5 @@
 *.lai
 *.la
 *.a
+
+*~

industrial_extrinsic_cal/include/industrial_extrinsic_cal/calibration_job.hpp

@@ -51,7 +51,7 @@ class Camera
    static cameras get but one set of pose parameters
    */
   bool isMoving();
-  boost::shared_ptr<CameraObserver> camera_observer_;/*!< processes images, does CameraObservations */
+  boost::shared_ptr<DummyCameraObserver> camera_observer_;/*!< processes images, does CameraObservations */
   CameraParameters camera_parameters_;/*!< The intrinsic and extrinsic parameters */
   //    ::std::ostream& operator<<(::std::ostream& os, const Camera& C){ return os<< "TODO";};
 
@@ -186,7 +186,6 @@ class ObservationDataPoint
   }
   ;
 
-private:
   std::string camera_name_;
   std::string target_name_;
   int scene_id_;

industrial_extrinsic_cal/include/industrial_extrinsic_cal/ceres_costs_utils.hpp

@@ -74,5 +74,107 @@ void printCameraParameters(CameraParameters C, std::string words);
  */
 Observation projectPoint(CameraParameters camera_parameters, Point3d point);
 
+  struct TargetCameraReprjErrorNoDistortion{
+    TargetCameraReprjErrorNoDistortion(double ob_x, double ob_y,
+				       double fx,   double fy,
+				       double cx,   double cy,
+				       double pnt_x,   double pnt_y, double pnt_z)
+      : ox_(ob_x),oy_(ob_y),
+	fx_(fx),fy_(fy),
+	cx_(cx),cy_(cy),
+        pnt_x_(pnt_x), pnt_y_(pnt_y), pnt_z_(pnt_z){}
+
+    template <typename T>
+    bool operator()(const T* const c_p1,   /** extrinsic parameters */
+		    const T* const c_p2,   /** 6Dof transform of target points into world frame */
+		    T* resid) const {
+
+      /** extract the variables from parameter blocks  */
+      int q=0; /** extract extrinsic block of parameters */
+      const T& ax    = c_p1[q++]; /**  angle_axis x for rotation of camera		 */
+      const T& ay    = c_p1[q++]; /**  angle_axis y for rotation of camera */
+      const T& az    = c_p1[q++]; /**  angle_axis z for rotation of camera */
+      const T& tx    = c_p1[q++]; /**  translation of camera x */
+      const T& ty    = c_p1[q++]; /**  translation of camera y */
+      const T& tz    = c_p1[q++]; /**  translation of camera z */
+
+      q=0; /** extract target pose block of parameters */
+      const T& target_x   = c_p2[q++]; /**  target's x location */
+      const T& target_y   = c_p2[q++]; /**  target's y location */
+      const T& target_z   = c_p2[q++]; /**  target's z location */
+      const T& target_ax  = c_p2[q++]; /**  target's ax angle axis value */
+      const T& target_ay  = c_p2[q++]; /**  target's ay angle axis value */
+      const T& target_az  = c_p2[q++]; /**  target's az angle axis value */
+
+
+      // create a vector from the location of the point in the target's frame
+      T point[3];
+      point[0] = T(pnt_x_);
+      point[1] = T(pnt_y_);
+      point[2] = T(pnt_z_);
+
+      /** rotate and translate points into world frame */
+      T target_aa[3];/** angle axis  */
+      T world_point_loc[3]; /** point rotated */
+      target_aa[0] = target_ax; 
+      target_aa[1] = target_ay; 
+      target_aa[2] = target_az; 
+      ceres::AngleAxisRotatePoint(target_aa,point,world_point_loc);
+
+      /** apply target translation */
+      world_point_loc[0] = world_point_loc[0] + target_x;
+      world_point_loc[1] = world_point_loc[1] + target_y;
+      world_point_loc[2] = world_point_loc[2] + target_z;
+
+      /** rotate and translate points into camera frame */
+      /*  Note that camera transform is from world into camera frame, not vise versa */
+      T aa[3];/** angle axis  */
+      T camera_point_loc[3]; /** point rotated */
+      aa[0] = ax; 
+      aa[1] = ay; 
+      aa[2] = az; 
+      ceres::AngleAxisRotatePoint(aa,point,camera_point_loc);
+
+      /** apply camera translation */
+      T xp1 = camera_point_loc[0] + tx; /** point rotated and translated */
+      T yp1 = camera_point_loc[1] + ty;
+      T zp1 = camera_point_loc[2] + tz;
+
+      /** scale into the image plane by distance away from camera */
+      T xp = xp1/zp1;			
+      T yp = yp1/zp1;
+
+      /** perform projection using focal length and camera center into image plane */
+      resid[0] = T(fx_)*xp + T(cx_) - T(ox_); 
+      resid[1] = T(fy_)*yp + T(cy_) - T(oy_);
+
+      return true;
+    } /** end of operator() */
+
+    /** Factory to hide the construction of the CostFunction object from */
+    /** the client code. */
+    static ceres::CostFunction* Create(const double o_x, const double o_y,
+				       const double fx,  const double fy,
+				       const double cx,  const double cy,
+				       const double pnt_x, const double pnt_y, const double pnt_z
+) {
+      return (
+	      new ceres::AutoDiffCostFunction<TargetCameraReprjErrorNoDistortion, 2,  6, 6>
+	      (
+	       new TargetCameraReprjErrorNoDistortion(o_x, o_y, fx, fy, cx, cy, pnt_x, pnt_y, pnt_z)
+	       )
+	      );
+    }
+    double ox_; /** observed x location of object in image */
+    double oy_; /** observed y location of object in image */
+    double fx_; /*!< known focal length of camera in x */
+    double fy_; /*!< known focal length of camera in y */
+    double cx_; /*!< known optical center of camera in x */
+    double cy_; /*!< known optical center of camera in y */
+    double pnt_x_;/*!< known location of point in target's reference frame x */
+    double pnt_y_;/*!< known location of point in target's reference frame y */
+    double pnt_z_;/*!< known location of point in target's reference frame z */
+  };
+
 } // end of namespace
 #endif

industrial_extrinsic_cal/src/calibration_job.cpp

@@ -613,58 +613,82 @@ bool CalibrationJob::runOptimization()
 
   BOOST_FOREACH(ObservationDataPoint ODP, observation_data_point_list.items)
   {
-    // the following is an example taken from some working code. Clearly it won't work with
-    // our data structures, but I included it here as an example
-
-    // Create residuals for each observation in the bundle adjustment problem. The
-    // parameters for cameras and points are added automatically.
-    ceres::Problem problem;
+    // take all the data collected and create a Ceres optimization problem and run it
+
+    BOOST_FOREACH(ObservationDataPoint ODP, observation_data_point_list.items){
+      // create cost function
+      // there are several options
+      // 1. the complete reprojection error cost function "Create(obs_x,obs_y)"
+      //    this cost function has the following parameters:
+      //      a. camera intrinsics
+      //      b. camera extrinsics
+      //      c. target pose
+      //      d. point location in target frame
+      // 2. the same as 1, but without d  "Create(obs_x,obs_y,t_pnt_x, t_pnt_y, t_pnt_z)
+      // 3. the same as 1, but without a  "Create(obs_x,obs_y,fx,fy,cx,cy,cz)"
+      //    Note that this one assumes we are using rectified images to compute the observations
+      // 4. the same as 3, point location fixed too "Create(obs_x,obs_y,fx,fy,cx,cy,cz,t_x,t_y,t_z)"
+      // 5. the same as 4, but with target in known location
+      //    "Create(obs_x,obs_y,fx,fy,cx,cy,cz,t_x,t_y,t_z,p_tx,p_ty,p_tz,p_ax,p_ay,p_az)"
 
     // need a block of cameras 
     // need a block of targets
     // these two lists need to be able to search for an existing item by name
     // and also an existing item by both name and scene id
 
-    //  BOOST_FOREACH(ObservationScene Scene, os_){
-    //    BOOST_FOREACH(CameraObservation CO, Scene.co){
-    // determine if this is a new camera
-    // Important Cases where we need to add a new camera: 
-    // 1. first time a fixed camera is observed 
-    // 2. First time in the scene a moving camera is observed 
-    // ADD camera if necessary
-    //      BOOST_FOREACH(Observation O, CO.o)
-    // determine if this is a new target
-    // Important Cases where we need to add a new target: 
-    // 1. first time a fixed target is observed 
-    // 2. First time in the scene a moving target is observed 
-
-    // ADD all target points if necessary
-
-    /*
-     // Each Residual block takes a point and a camera as input and outputs a 2
-     // dimensional residual. Internally, the cost function stores the observed
-     // image location and compares the reprojection against the observation.
-     ceres::CostFunction* cost_function =  Camera_reprj_error::Create(Ob[i].x,Ob[i].y);
-
-     problem.AddResidualBlock(cost_function, NULL ,
-     C.PB_extrinsics,
-     C.PB_intrinsics,
-     Pts[i].PB);
-     problem.SetParameterBlockConstant(C.PB_intrinsics);
-     problem.SetParameterBlockConstant(Pts[i].PB);
-     }
-
-     // Make Ceres automatically detect the bundle structure. Note that the
-     // standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
-     // for standard bundle adjustment problems.
-     ceres::Solver::Options options;
-     options.linear_solver_type = ceres::DENSE_SCHUR;
-     options.minimizer_progress_to_stdout = true;
-     options.max_num_iterations = 1000;
-
-     ceres::Solver::Summary summary;
-     ceres::Solve(options, &problem, &summary);
-     */
+      // pull out the constants from the observation point data
+      double focal_length_x = ODP.camera_intrinsics_[0]; // TODO, make this not so ugly
+      double focal_length_y = ODP.camera_intrinsics_[1];
+      double center_pnt_x   = ODP.camera_intrinsics_[2];
+      double center_pnt_y   = ODP.camera_intrinsics_[3];
+      double image_x        = ODP.image_x_;
+      double image_y        = ODP.image_y_;
+      double point_x        = ODP.point_position_[0];// location of point within target frame
+      double point_y        = ODP.point_position_[1];
+      double point_z        = ODP.point_position_[2];
+
+      // create the cost function
+      CostFunction* cost_function = TargetCameraReprjErrorNoDistortion::Create(image_x, image_y,
+									       focal_length_x, 
+									       focal_length_y,
+									       center_pnt_x,
+									       center_pnt_y,
+									       point_x,
+									       point_y,
+									       point_z);
+
+      // pull out pointers to the parameter blocks in the observation point data
+      P_BLOCK extrinsics    = ODP.camera_extrinsics_;
+      P_BLOCK target_pose   = ODP.target_pose_;
+
+      // add it as a residual using parameter blocks
+      problem_.AddResidualBlock(cost_function, NULL , extrinsics, target_pose);
+
+    }
+
+    //    ceres::CostFunction* cost_function =  Camera_reprj_error::Create(Ob[i].x,Ob[i].y);
+
+    //    problem_.AddResidualBlock(cost_function, NULL ,
+    //    C.PB_extrinsics,
+    //    C.PB_intrinsics,
+    //    Pts[i].PB);
+    //    problem.SetParameterBlockConstant(C.PB_intrinsics);
+    //    problem.SetParameterBlockConstant(Pts[i].PB);
+
+
+    // Make Ceres automatically detect the bundle structure. Note that the
+    // standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
+    // for standard bundle adjustment problems.
+    ceres::Solver::Options options;
+    options.linear_solver_type = ceres::DENSE_SCHUR;
+    options.minimizer_progress_to_stdout = true;
+    options.max_num_iterations = 1000;
+
+    ceres::Solver::Summary summary;
+    //    ceres::Solve(options, &problem, &summary);
+
+
+    return true;
   }
   return true;
 }