DepthImageToLaserScanTest.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /* 
00031  * Author: Chad Rockey
00032  */
00033 
00034 // Bring in my package's API, which is what I'm testing
00035 #include <depthimage_to_laserscan/DepthImageToLaserScan.h>
00036 // Bring in gtest
00037 #include <gtest/gtest.h>
00038 
00039 #include <limits.h>
00040 #include <math.h>
00041 
00042 #include <stdio.h>
00043 #include <stdlib.h>
00044 #include <time.h>
00045 
00046 // Library object
00047 depthimage_to_laserscan::DepthImageToLaserScan dtl_;
00048 
00049 // Inputs
00050 sensor_msgs::ImagePtr depth_msg_;
00051 sensor_msgs::CameraInfoPtr info_msg_;
00052 
00053 // Check if the setters work properly and initialize member variables
00054 TEST(ConvertTest, setupLibrary)
00055 {
00056   // Set up library
00057   const float scan_time = 1.0/30.0;
00058   dtl_.set_scan_time(scan_time);
00059   const float range_min = 0.45;
00060   const float range_max = 10.0;
00061   dtl_.set_range_limits(range_min, range_max);
00062   const int scan_height = 1;
00063   dtl_.set_scan_height(scan_height);
00064   const std::string output_frame = "camera_depth_frame";
00065   dtl_.set_output_frame(output_frame);
00066   
00067   depth_msg_.reset(new sensor_msgs::Image);
00068   depth_msg_->header.seq = 42;
00069   depth_msg_->header.stamp.fromNSec(1234567890);
00070   depth_msg_->header.frame_id = "frame";
00071   depth_msg_->height = 480;
00072   depth_msg_->width = 640;
00073   depth_msg_->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00074   depth_msg_->is_bigendian = false;
00075   depth_msg_->step = depth_msg_->width*2; // 2 bytes per pixel
00076   uint16_t value = 0x0F;
00077   depth_msg_->data.assign(depth_msg_->height*depth_msg_->step, value); // Sets all values to 3.855m
00078   
00079   info_msg_.reset(new sensor_msgs::CameraInfo);
00080   info_msg_->header = depth_msg_->header;
00081   info_msg_->height = depth_msg_->height;
00082   info_msg_->width = depth_msg_->width;
00083   info_msg_->distortion_model = "plumb_bob";
00084   info_msg_->D.resize(5); // All 0, no distortion
00085   info_msg_->K[0] = 570.3422241210938;
00086   info_msg_->K[2] = 314.5;
00087   info_msg_->K[4] = 570.3422241210938;
00088   info_msg_->K[5] = 235.5;
00089   info_msg_->K[8] = 1.0;
00090   info_msg_->R[0] = 1.0;
00091   info_msg_->R[4] = 1.0;
00092   info_msg_->R[8] = 1.0;
00093   info_msg_->P[0] = 570.3422241210938;
00094   info_msg_->P[2] = 314.5;
00095   info_msg_->P[5] = 570.3422241210938;
00096   info_msg_->P[6] = 235.5;
00097   info_msg_->P[10] = 1.0;
00098   
00099   sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(depth_msg_, info_msg_);
00100   
00101   // Test set variables
00102   EXPECT_EQ(scan_msg->scan_time, scan_time);
00103   EXPECT_EQ(scan_msg->range_min, range_min);
00104   EXPECT_EQ(scan_msg->range_max, range_max);
00105   EXPECT_EQ(scan_msg->header.frame_id, output_frame);
00106   EXPECT_EQ(scan_msg->ranges.size(), depth_msg_->width);
00107 }
00108 
00109 // Test for the exception based on encoding
00110 TEST(ConvertTest, testExceptions)
00111 {
00112   // Test supported image encodings for exceptions
00113   // Does not segfault as long as scan_height = 1
00114   depth_msg_->encoding = sensor_msgs::image_encodings::RGB8;
00115   EXPECT_THROW(dtl_.convert_msg(depth_msg_, info_msg_), std::runtime_error);
00116   depth_msg_->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00117   EXPECT_NO_THROW(dtl_.convert_msg(depth_msg_, info_msg_));
00118   depth_msg_->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00119   EXPECT_NO_THROW(dtl_.convert_msg(depth_msg_, info_msg_));
00120 }
00121 
00122 // Check to make sure the mininum is output for each pixel column for various scan heights
00123 TEST(ConvertTest, testScanHeight)
00124 {
00125   for(int scan_height = 1; scan_height <= 100; scan_height++){
00126     uint16_t low_value = 500;
00127     uint16_t high_value = 3000;
00128 
00129     int data_len = depth_msg_->width;
00130     uint16_t* data = reinterpret_cast<uint16_t*>(&depth_msg_->data[0]);
00131     int row_step = depth_msg_->step / sizeof(uint16_t);
00132 
00133     dtl_.set_scan_height(scan_height);
00134 
00135     int offset = (int)(info_msg_->K[5]-scan_height/2);
00136     data += offset*row_step; // Offset to center of image
00137 
00138     for(int v = 0; v < scan_height; v++, data += row_step){
00139       for (int u = 0; u < data_len; u++) // Loop over each pixel in row
00140       {
00141         if(v % scan_height == u % scan_height){
00142           data[u] = low_value;
00143         } else {
00144           data[u] = high_value;
00145         }
00146       }
00147     }
00148 
00149     // Convert
00150     sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(depth_msg_, info_msg_);
00151 
00152     // Test for minimum
00153     float high_float_thresh = (float)high_value * 1.0f/1000.0f * 0.9f; // 0.9f represents 10 percent margin on range
00154     for(size_t i = 0; i < scan_msg->ranges.size(); i++){
00155       // If this is a valid point
00156       if(scan_msg->range_min <= scan_msg->ranges[i] && scan_msg->ranges[i] <= scan_msg->range_max){
00157         // Make sure it's not set to the high_value
00158         ASSERT_LT(scan_msg->ranges[i], high_float_thresh);
00159       }
00160     }
00161   }
00162   
00163   // Revert to 1 scan height
00164   dtl_.set_scan_height(1);
00165   
00166 }
00167 
00168 // Test a randomly filled image and ensure all values are < range_min
00169 // (range_max is currently violated to fill the messages)
00170 TEST(ConvertTest, testRandom)
00171 {
00172   srand ( 8675309 ); // Set seed for repeatable tests
00173   
00174   uint16_t* data = reinterpret_cast<uint16_t*>(&depth_msg_->data[0]);
00175   for(size_t i = 0; i < depth_msg_->width*depth_msg_->height; i++){
00176     data[i] = rand() % 500; // Distance between 0 and 0.5m
00177   }
00178   
00179   // Convert
00180   sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(depth_msg_, info_msg_);
00181   
00182   // Make sure all values are greater than or equal to range_min and less than or equal to range_max
00183   for(size_t i = 0; i < scan_msg->ranges.size(); i++){
00184     if(std::isfinite(scan_msg->ranges[i])){
00185       ASSERT_GE(scan_msg->ranges[i], scan_msg->range_min);
00186       ASSERT_LE(scan_msg->ranges[i], scan_msg->range_max);
00187     }
00188   }
00189 }
00190 
00191 // Test to preserve NaN
00192 TEST(ConvertTest, testNaN)
00193 {
00194   // Use a floating point image
00195   sensor_msgs::ImagePtr float_msg(new sensor_msgs::Image(*depth_msg_));
00196   float_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00197   float_msg->step = float_msg->width*4; // 4 bytes per pixel
00198   float_msg->data.resize(float_msg->step * float_msg->height);
00199   
00200   float* data = reinterpret_cast<float*>(&float_msg->data[0]);
00201   for(size_t i = 0; i < float_msg->width*float_msg->height; i++){
00202     data[i] = std::numeric_limits<float>::quiet_NaN();
00203   }
00204   
00205   // Convert
00206   sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(float_msg, info_msg_);
00207   
00208   // Make sure all values are NaN
00209   for(size_t i = 0; i < scan_msg->ranges.size(); i++){
00210     if(std::isfinite(scan_msg->ranges[i])){
00211       ADD_FAILURE() << "Non-NaN value produced from NaN test.";
00212     }
00213   }
00214 }
00215 
00216 // Test to preserve +Inf
00217 TEST(ConvertTest, testPositiveInf)
00218 {
00219   // Use a floating point image
00220   sensor_msgs::ImagePtr float_msg(new sensor_msgs::Image(*depth_msg_));
00221   float_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00222   float_msg->step = float_msg->width*4; // 4 bytes per pixel
00223   float_msg->data.resize(float_msg->step * float_msg->height);
00224   
00225   float* data = reinterpret_cast<float*>(&float_msg->data[0]);
00226   for(size_t i = 0; i < float_msg->width*float_msg->height; i++){
00227     data[i] = std::numeric_limits<float>::infinity();
00228   }
00229   
00230   // Convert
00231   sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(float_msg, info_msg_);
00232   
00233   // Make sure most (> 80%) values are Inf
00234   size_t nan_count = 0;
00235   for(size_t i = 0; i < scan_msg->ranges.size(); i++){
00236     if(std::isfinite(scan_msg->ranges[i])){ // NaNs are acceptable.
00237       ADD_FAILURE() << "Non-finite value produced from postive infniity test.";
00238     } else if(isnan(scan_msg->ranges[i])){
00239       nan_count++;
00240     } else if(scan_msg->ranges[i] < 0){
00241       ADD_FAILURE() << "Negative value produced from postive infinity test.";
00242     }
00243   }
00244   
00245   ASSERT_LE(nan_count, scan_msg->ranges.size() * 0.80);
00246 }
00247 
00248 // Test to preserve -Inf
00249 TEST(ConvertTest, testNegativeInf)
00250 {
00251   // Use a floating point image
00252   sensor_msgs::ImagePtr float_msg(new sensor_msgs::Image(*depth_msg_));
00253   float_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00254   float_msg->step = float_msg->width*4; // 4 bytes per pixel
00255   float_msg->data.resize(float_msg->step * float_msg->height);
00256   
00257   float* data = reinterpret_cast<float*>(&float_msg->data[0]);
00258   for(size_t i = 0; i < float_msg->width*float_msg->height; i++){
00259     data[i] = -std::numeric_limits<float>::infinity();
00260   }
00261   
00262   // Convert
00263   sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(float_msg, info_msg_);
00264   
00265   // Make sure most (> 80%) values are Inf
00266   size_t nan_count = 0;
00267   for(size_t i = 0; i < scan_msg->ranges.size(); i++){
00268     if(std::isfinite(scan_msg->ranges[i])){ // NaNs are acceptable.
00269       ADD_FAILURE() << "Non-finite value produced from postive infniity test.";
00270     } else if(isnan(scan_msg->ranges[i])){
00271       nan_count++;
00272     } else if(scan_msg->ranges[i] > 0){
00273       ADD_FAILURE() << "Postive value produced from negative infinity test.";
00274     }
00275   }
00276   
00277   ASSERT_LE(nan_count, scan_msg->ranges.size() * 0.80);
00278 }
00279 
00280 
00281 // Run all the tests that were declared with TEST()
00282 int main(int argc, char **argv){
00283 testing::InitGoogleTest(&argc, argv);
00284 return RUN_ALL_TESTS();
00285 }


depthimage_to_laserscan
Author(s): Chad Rockey
autogenerated on Tue Jan 7 2014 11:09:01