utest.cpp

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.
 */

#include <industrial_extrinsic_cal/calibration_job_definition.h>

#include <gtest/gtest.h>
#include <yaml-cpp/yaml.h>
#include <fstream>

using namespace industrial_extrinsic_cal;
using industrial_extrinsic_cal::CalibrationJob;

void print_AAtoH(double x, double y, double z, double tx, double ty, double tz);

std::string camera_file_name("/home/cgomez/ros/hydro/catkin_ws/src/industrial_calibration/industrial_extrinsic_cal/yaml/test1_camera_def.yaml");
std::string target_file_name("/home/cgomez/ros/hydro/catkin_ws/src/industrial_calibration/industrial_extrinsic_cal/yaml/test1_target_def.yaml");
std::string caljob_file_name("/home/cgomez/ros/hydro/catkin_ws/src/industrial_calibration/industrial_extrinsic_cal/yaml/test1_caljob_def.yaml");

//CalibrationJob Cal_job(camera_file_name, target_file_name, caljob_file_name);
CalibrationJob Cal_job(camera_file_name, target_file_name, caljob_file_name);

//need: Camera intrinstics, camera extrinsics, observations (x,y),
//camera name, target name, target pose, and point position
std::vector<int> point_ids;
std::vector<float> obs_x;
std::vector<float> obs_y;

P_BLOCK intrinsics;
P_BLOCK extrinsics;
P_BLOCK target_pose;
P_BLOCK pnt_pos;


TEST(DISABLED_IndustrialExtrinsicCalSuite, load_check)
{
  //need: Camera intrinstics, camera extrinsics, observations (x,y),
  //camera name, target name, scene id, pnt id, target pose, and point position

  EXPECT_TRUE(Cal_job.load());//should import camera instrinsics

  //read in observation file which contains Points with pnt_id, x and y
  std::ifstream obs_input_file("/home/cgomez/ros/hydro/catkin_ws/src/industrial_calibration/industrial_extrinsic_cal/observations.txt");
  YAML::Parser obs_parser(obs_input_file);

  YAML::Node obs_doc;
  obs_parser.GetNextDocument(obs_doc);

  // read in all observations
  if (const YAML::Node *observations = obs_doc.FindValue("Points"))
  {
    ROS_DEBUG_STREAM("Found "<<observations->size() <<" observations ");
    point_ids.resize(observations->size());
    obs_x.resize(observations->size());
    obs_y.resize(observations->size());
    for (unsigned int i = 0; i < observations->size(); i++)
    {
      (*observations)[i]["Point_id"] >> point_ids.at(i);

      const YAML::Node *points_node = (*observations)[i].FindValue("Pnts");
      std::vector<float> temp_pnt;
      (*points_node) >> temp_pnt;
      obs_x.at(i) = temp_pnt[0];
      obs_y.at(i) = temp_pnt[1];
    }
  }

}

TEST(DISABLED_IndustrialExtrinsicCalSuite, param_check)
{
  //need: Camera intrinstics, camera extrinsics, observations (x,y),
  //camera name, target name, scene id, pnt id, target pose, and point position
  std::vector<ObservationScene> obs_scene_list_ = Cal_job.getSceneList();

  EXPECT_STRCASEEQ("Asus1", obs_scene_list_.at(0).cameras_in_scene_.at(0)->camera_name_.c_str());

  EXPECT_STRCASEEQ("Checkerboard",
                   obs_scene_list_.at(0).observation_command_list_.at(0).target->target_name.c_str());

  CeresBlocks c_blocks_ = Cal_job.getCeresBlocks();
  ASSERT_GT(c_blocks_.static_cameras_.size(), 0);

  intrinsics = c_blocks_.getStaticCameraParameterBlockIntrinsics("Asus1");
  extrinsics = c_blocks_.getStaticCameraParameterBlockExtrinsics("Asus1");

  target_pose = c_blocks_.getStaticTargetPoseParameterBlock("Checkerboard");
  pnt_pos = c_blocks_.getStaticTargetPointParameterBlock("Checkerboard", 0);

  //Cal_job.current_scene_=1;

  EXPECT_EQ(525, intrinsics[0]);//focal_length_x from camera yaml file
  EXPECT_EQ(525, intrinsics[1]);//focal_length_y from camera yaml file
  EXPECT_EQ(320, intrinsics[2]);//center_x from camera yaml file
  EXPECT_EQ(240, intrinsics[3]);//center_y from camera yaml file
  EXPECT_EQ(0.01, intrinsics[4]);//distortion_k1 from camera yaml file
  EXPECT_EQ(0.02, intrinsics[5]);//distortion_k2 from camera yaml file
  EXPECT_EQ(0.03, intrinsics[6]);//distortion_k3 from camera yaml file
  EXPECT_EQ(0.01, intrinsics[7]);//distortion_p1 from camera yaml file
  EXPECT_EQ(0.01, intrinsics[8]);//distortion_p2 from camera yaml file
/*  ROS_INFO_STREAM("instrinsics 0: "<<intrinsics[0]);
  ROS_INFO_STREAM("instrinsics 1: "<<intrinsics[1]);
  ROS_INFO_STREAM("instrinsics 2: "<<intrinsics[2]);
  ROS_INFO_STREAM("instrinsics 3: "<<intrinsics[3]);
  ROS_INFO_STREAM("instrinsics 4: "<<intrinsics[4]);
  ROS_INFO_STREAM("instrinsics 5: "<<intrinsics[5]);
  ROS_INFO_STREAM("instrinsics 6: "<<intrinsics[6]);
  ROS_INFO_STREAM("instrinsics 7: "<<intrinsics[7]);
  ROS_INFO_STREAM("instrinsics 8: "<<intrinsics[8]);
  ROS_INFO_STREAM("extrinsics 0: "<<extrinsics[0]);
  ROS_INFO_STREAM("extrinsics 1: "<<extrinsics[1]);
  ROS_INFO_STREAM("extrinsics 2: "<<extrinsics[2]);
  ROS_INFO_STREAM("extrinsics 3: "<<extrinsics[3]);
  ROS_INFO_STREAM("extrinsics 4: "<<extrinsics[4]);
  ROS_INFO_STREAM("extrinsics 5: "<<extrinsics[5]);*/
  EXPECT_EQ(1.0, extrinsics[0]);//angle_axis_ax in camera yaml file
  EXPECT_EQ(1.1, extrinsics[1]);//angle_axis_ay in camera yaml file
  EXPECT_EQ(1.2, extrinsics[2]);//angle_axis_az in camera yaml file
  EXPECT_EQ(1.3, extrinsics[3]);//position_x in camera yaml file
  EXPECT_EQ(1.4, extrinsics[4]);//position_y in camera yaml file
  EXPECT_EQ(1.5, extrinsics[5]);//position_z in camera yaml file
/*  ROS_INFO_STREAM("target_pose 0: "<<target_pose[0]);
  ROS_INFO_STREAM("target_pose 1: "<<target_pose[1]);
  ROS_INFO_STREAM("target_pose 2: "<<target_pose[2]);
  ROS_INFO_STREAM("target_pose 3: "<<target_pose[3]);
  ROS_INFO_STREAM("target_pose 4: "<<target_pose[4]);
  ROS_INFO_STREAM("target_pose 5: "<<target_pose[5]);*/
  EXPECT_EQ(2.2, target_pose[0]);//position_x from target yaml
  EXPECT_EQ(2.3, target_pose[1]);//position_y from target yaml
  EXPECT_EQ(2.4, target_pose[2]);//position_z from target yaml
  EXPECT_EQ(1.1, target_pose[3]);//angle_axis_ax from target yaml
  EXPECT_EQ(2.0, target_pose[4]);//angle_axis_ay from target yaml
  EXPECT_EQ(2.1, target_pose[5]);//angle_axis_ay from target yaml

  //check first and last points (from target yaml)
  EXPECT_EQ(0.0, pnt_pos[0]);
  EXPECT_EQ(0.0, pnt_pos[1]);
  EXPECT_EQ(0.0, pnt_pos[2]);
  pnt_pos = c_blocks_.getStaticTargetPointParameterBlock("Checkerboard", 120);
  EXPECT_FLOAT_EQ(0.40, pnt_pos[0]);
  EXPECT_FLOAT_EQ(0.40, pnt_pos[1]);
  EXPECT_EQ(0.0, pnt_pos[2]);

  ASSERT_GT(point_ids.size(), 0);
  //check first and last points (from observation.txt)
  EXPECT_EQ(0,point_ids.at(0));
  EXPECT_EQ(120,point_ids.at(120));
  EXPECT_FLOAT_EQ(64.8908386230469,obs_x.at(0));
  EXPECT_FLOAT_EQ(58.9467353820801,obs_y.at(0));
  EXPECT_FLOAT_EQ(282.849487304688,obs_x.at(120));
  EXPECT_FLOAT_EQ(280.590637207031,obs_y.at(120));
}

TEST(DISABLED_IndustrialExtrinsicCalSuite, ceres_check)
{

  CeresBlocks c_blocks_ = Cal_job.getCeresBlocks();
  ceres::Problem problem;//=Cal_job.getProblem();
  for (int i=0; i<point_ids.size(); i++)
  {

    pnt_pos = c_blocks_.getStaticTargetPointParameterBlock("Checkerboard", i);
  // create the cost function
  ceres::CostFunction* cost_function = TargetCameraReprjErrorNoDistortion::Create(obs_x.at(i), obs_y.at(i),
                                                                           intrinsics[0],
                                                                           intrinsics[1],
                                                                           intrinsics[2],
                                                                           intrinsics[3],
                                                                           pnt_pos[0],
                                                                           pnt_pos[1],
                                                                           pnt_pos[2]);
  problem.AddResidualBlock(cost_function, NULL , extrinsics, target_pose);
  }
  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);
  std::cout << summary.BriefReport() << "\n";
  //std::cout << summary.FullReport() << "\n";


  printf("World to Camera\n");
  printf("Optimized Camera\n");
  print_AAtoH(extrinsics[0], extrinsics[1], extrinsics[2],
               extrinsics[3], extrinsics[4], extrinsics[5]);

  /*
  //printf("%s\n", words.c_str());
  printf("Camera to World Transform:\n");
  //print_AATasHI(C.aa[0], C.aa[1], C.aa[2], C.pos[0], C.pos[1], C.pos[2]);
  double R[9];
  double aa[3];
  aa[0] = extrinsics[0];
  aa[1] = extrinsics[1];
  aa[2] = extrinsics[2];
  ceres::AngleAxisToRotationMatrix(aa, R);
  double ix = -(extrinsics[3] * R[0] + extrinsics[4] * R[1] + extrinsics[5] * R[2]);
  double iy = -(extrinsics[3] * R[3] + extrinsics[4] * R[4] + extrinsics[5] * R[5]);
  double iz = -(extrinsics[3] * R[6] + extrinsics[4] * R[7] + extrinsics[5] * R[8]);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[0], R[1], R[2], ix);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[3], R[4], R[5], iy);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[6], R[7], R[8], iz);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", 0.0, 0.0, 0.0, 1.0);*/
}
// angle axis to homogeneous transform
void print_AAtoH(double x, double y, double z, double tx, double ty, double tz)
{
  double R[9];
  double aa[3];
  aa[0] = x;
  aa[1] = y;
  aa[2] = z;
  ceres::AngleAxisToRotationMatrix(aa, R);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[0], R[3], R[6], tx);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[1], R[4], R[7], ty);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[2], R[5], R[8], tz);
  printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", 0.0, 0.0, 0.0, 1.0);
}


// Run all the tests that were declared with TEST()
int main(int argc, char **argv)
{
  ros::init(argc, argv, "test");
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}