laser_cb_detector_unittest.cpp

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
 
#include <gtest/gtest.h>
 
 
#include <laser_cb_detector/ConfigGoal.h>
#include <laser_cb_detector/laser_cb_detector.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/types_c.h>
 
#include <ros/console.h>
 
using namespace laser_cb_detector;
using namespace std;
 
static const double pix_eps_ = .6 ;
static const double eps = 1e-6;
 
static const bool DEBUG=false;
 
// The path to our test data comes in from CMake as TEST_PATH;
//  stringify it and stuff it into a variable for later use.
#define xstr(s) str(s)
#define str(s) #s
static const string test_path = xstr(TEST_PATH);
 
calibration_msgs::DenseLaserSnapshot getSnapshot(const string& filename)
{
  //IplImage* image;
  cv::Mat image;
  image = cv::imread(filename.c_str(), 0);         // 0 -> Force image to grayscale
  EXPECT_TRUE(!image.empty()) << "could not open image file [" << filename << "]" << endl;
 
 
  cv::Mat float_image;
  //float_image = cv::createImage( cvSize(image->width, image->height), IPL_DEPTH_32F, 1);
  //cv::convert(image, float_image);
  image.convertTo(float_image, CV_32FC1, 1/255.0);
 
 
  calibration_msgs::DenseLaserSnapshot snapshot;
  snapshot.readings_per_scan = image.cols;
  snapshot.num_scans = image.rows;
  snapshot.intensities.resize(0);
  //snapshot.intensities.resize(image.rows * image.cols);
 
    //snapshot.intensities.assign(float_image.data, float_image.data + float_image.total());
  for (int i=0; i<float_image.rows; i++)
  {
    snapshot.intensities.insert(snapshot.intensities.end(), float_image.ptr<float>(i), float_image.ptr<float>(i)+float_image.cols);
    //snapshot.intensities.assign(float_image.data, float_image.data + float_image.total());
    //memcpy(&snapshot.intensities[i*snapshot.readings_per_scan],
    //       (float_image.imageData + i*float_image.widthStep),
    //       sizeof(float)*snapshot.readings_per_scan);
  }
 
  if (DEBUG)
  {
    for (unsigned int i=0; i<snapshot.num_scans; i++)
    {
      for (unsigned int j=0; j<snapshot.readings_per_scan; j++)
      {
        if (snapshot.intensities[i*snapshot.readings_per_scan + j] < 100.0)
          printf(" ");
        else
          printf("X");
      }
      printf("\n");
    }
  }
 
  // since statically declared, does not need release, but this could cause a memory leak
  //cv::releaseImage(&float_image);
  //cv::releaseImage(&image);
 
  return snapshot;
}
 
ConfigGoal config3x4()
{
  ConfigGoal config;
 
  config.num_x = 3;
  config.num_y = 4;
  config.spacing_x = 1.0;
  config.spacing_y = 1.0;
 
  config.width_scaling = 1.0;
  config.height_scaling = 1.0;
 
  config.min_intensity = 0.0;
  config.max_intensity = 100.0;
 
  config.subpixel_window = 3;
  config.subpixel_zero_zone = 1;
 
  config.flip_horizontal = 0;
 
  return config;
}
 
 
void check3x4(const calibration_msgs::CalibrationPattern& result, double pix_eps, bool flipped)
{
  EXPECT_TRUE(result.success);
 
 
  EXPECT_NEAR(result.object_points[0].x, 0.0, eps);
  EXPECT_NEAR(result.object_points[0].y, 0.0, eps);
  EXPECT_NEAR(result.object_points[0].z, 0.0, eps);
 
  EXPECT_NEAR(result.object_points[2].x, 2.0, eps);
  EXPECT_NEAR(result.object_points[2].y, 0.0, eps);
  EXPECT_NEAR(result.object_points[2].z, 0.0, eps);
 
  EXPECT_NEAR(result.object_points[5].x, 2.0, eps);
  EXPECT_NEAR(result.object_points[5].y, 1.0, eps);
  EXPECT_NEAR(result.object_points[5].z, 0.0, eps);
 
  // Detected corners
  if (flipped)
  {
    EXPECT_NEAR(result.image_points[0].x,  81.5, pix_eps);
    EXPECT_NEAR(result.image_points[0].y,  72.0, pix_eps);
 
    EXPECT_NEAR(result.image_points[1].x,  81.5, pix_eps);
    EXPECT_NEAR(result.image_points[1].y, 118.5, pix_eps);
 
    EXPECT_NEAR(result.image_points[11].x,  223.5, pix_eps);
    EXPECT_NEAR(result.image_points[11].y,  166.5, pix_eps);
  }
  else
  {
    EXPECT_NEAR(result.image_points[0].x, 223.0, pix_eps);
    EXPECT_NEAR(result.image_points[0].y,  72.0, pix_eps);
 
    EXPECT_NEAR(result.image_points[1].x, 223.0, pix_eps);
    EXPECT_NEAR(result.image_points[1].y, 119.0, pix_eps);
 
    EXPECT_NEAR(result.image_points[11].x,  81.0, pix_eps);
    EXPECT_NEAR(result.image_points[11].y, 166.0, pix_eps);
  }
}
 
TEST(LaserCbDetector, easy_cb_3x4)
{
  LaserCbDetector detector;
  calibration_msgs::DenseLaserSnapshot snapshot;
 
  snapshot = getSnapshot( test_path +  "/data/cb_3x4.png");
 
  ROS_INFO( "test_path: [%s]/[/data/cb_3x4.png]", test_path.c_str());
 
  ASSERT_EQ(snapshot.readings_per_scan, (unsigned int) 303);
  ASSERT_EQ(snapshot.num_scans, (unsigned int) 325);
 
  detector.configure(config3x4());
 
  bool return_result;
  calibration_msgs::CalibrationPattern result;
  return_result = detector.detect(snapshot, result);
  EXPECT_TRUE(return_result);
 
  check3x4(result, pix_eps_, false);
}
 
TEST(LaserCbDetector, scaled_cb_3x4)
{
  LaserCbDetector detector;
  calibration_msgs::DenseLaserSnapshot snapshot;
 
  snapshot = getSnapshot( test_path +  "/data/cb_3x4.png");
 
  ASSERT_EQ(snapshot.readings_per_scan, (unsigned int) 303);
  ASSERT_EQ(snapshot.num_scans, (unsigned int) 325);
 
  ConfigGoal config = config3x4();
  config.width_scaling  = 2.0;
  config.height_scaling = 4.0;
  detector.configure(config);
 
  bool return_result;
  calibration_msgs::CalibrationPattern result;
  return_result = detector.detect(snapshot, result);
  EXPECT_TRUE(return_result);
 
  check3x4(result, 1.0, false);
}
 
TEST(LaserCbDetector, reflected_cb_3x4)
{
  LaserCbDetector detector;
  calibration_msgs::DenseLaserSnapshot snapshot;
 
  snapshot = getSnapshot( test_path +  "/data/cb_3x4.png");
 
  ASSERT_EQ(snapshot.readings_per_scan, (unsigned int) 303);
  ASSERT_EQ(snapshot.num_scans, (unsigned int) 325);
 
  ConfigGoal config = config3x4();
  config.flip_horizontal = 1;
  detector.configure(config);
 
  bool return_result;
  calibration_msgs::CalibrationPattern result;
  return_result = detector.detect(snapshot, result);
  EXPECT_TRUE(return_result);
 
  check3x4(result, 1.0, true);
}
 
 
TEST(LaserCbDetector, malformedTest)
{
  LaserCbDetector detector;
 
  calibration_msgs::DenseLaserSnapshot snapshot;
 
  detector.configure(config3x4());
  snapshot = getSnapshot( test_path +  "/data/cb_3x4.png");
  snapshot.num_scans = 10;
 
  bool return_result;
  calibration_msgs::CalibrationPattern result;
  return_result = detector.detect(snapshot, result);
 
  EXPECT_FALSE(return_result);
}
 
TEST(LaserCbDetector, noCheckerboardTest)
{
  LaserCbDetector detector;
 
  calibration_msgs::DenseLaserSnapshot snapshot;
 
  ConfigGoal config = config3x4();
  config.num_x = 4;
  detector.configure(config);
  snapshot = getSnapshot( test_path +  "/data/cb_3x4.png");
 
  bool return_result;
  calibration_msgs::CalibrationPattern result;
  return_result = detector.detect(snapshot, result);
 
  EXPECT_TRUE(return_result);
  EXPECT_FALSE(result.success);
}
 
 
 
int main(int argc, char **argv){
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}