$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 #include <gtest/gtest.h> 00036 #include <laser_joint_processor/joint_image_interpolator.h> 00037 00038 using namespace std; 00039 using namespace laser_joint_processor; 00040 00041 static const float eps = 1e-6; 00042 00043 00044 float& pixel(IplImage* image, int row, int col, int chan) 00045 { 00046 return *(((float*)(image->imageData + row*image->widthStep))+image->nChannels*col + chan); 00047 } 00048 00049 class JointImageInterpolator_EasyTests : public ::testing::Test 00050 { 00051 protected: 00052 virtual void SetUp() 00053 { 00054 source_image_ = cvCreateImage( cvSize(3,3), IPL_DEPTH_32F, 2); 00055 pixel(source_image_, 0, 0, 0) = 10; 00056 pixel(source_image_, 0, 1, 0) = 20; 00057 pixel(source_image_, 0, 2, 0) = 30; 00058 pixel(source_image_, 1, 0, 0) = 40; 00059 pixel(source_image_, 1, 1, 0) = 50; 00060 pixel(source_image_, 1, 2, 0) = 60; 00061 pixel(source_image_, 2, 0, 0) = 70; 00062 pixel(source_image_, 2, 1, 0) = 80; 00063 pixel(source_image_, 2, 2, 0) = 90; 00064 00065 pixel(source_image_, 0, 0, 1) = 1; 00066 pixel(source_image_, 0, 1, 1) = 2; 00067 pixel(source_image_, 0, 2, 1) = 3; 00068 pixel(source_image_, 1, 0, 1) = 4; 00069 pixel(source_image_, 1, 1, 1) = 5; 00070 pixel(source_image_, 1, 2, 1) = 6; 00071 pixel(source_image_, 2, 0, 1) = 7; 00072 pixel(source_image_, 2, 1, 1) = 8; 00073 pixel(source_image_, 2, 2, 1) = 9; 00074 00075 // Pixel Values: 00076 // 1 2 3 00077 // 4 5 6 00078 // 7 8 9 00079 00080 image_points_.resize(3); 00081 image_points_[0].x = 0.0; 00082 image_points_[0].y = 0.0; 00083 00084 image_points_[1].x = 0.5; 00085 image_points_[1].y = 0.5; 00086 00087 image_points_[2].x = 1; 00088 image_points_[2].y = 0.75; 00089 00090 bool success; 00091 success = interp_.interp(image_points_, source_image_, positions_, velocities_); 00092 ASSERT_TRUE(success); 00093 } 00094 00095 virtual void TearDown() 00096 { 00097 cvReleaseImage(&source_image_); 00098 } 00099 00100 IplImage* source_image_; 00101 vector<calibration_msgs::ImagePoint> image_points_; 00102 JointImageInterpolator interp_; 00103 vector<float> positions_; 00104 vector<float> velocities_; 00105 }; 00106 00107 TEST_F(JointImageInterpolator_EasyTests, positionTests) 00108 { 00109 ASSERT_EQ(positions_.size(), (unsigned int) 3); 00110 EXPECT_NEAR(positions_[0], 10.0, eps); 00111 EXPECT_NEAR(positions_[1], 30.0, eps); 00112 EXPECT_NEAR(positions_[2], 42.5, eps); 00113 } 00114 00115 TEST_F(JointImageInterpolator_EasyTests, velocityTests) 00116 { 00117 ASSERT_EQ(velocities_.size(), (unsigned int) 3); 00118 EXPECT_NEAR(velocities_[0], 1.0, eps); 00119 EXPECT_NEAR(velocities_[1], 3.0, eps); 00120 EXPECT_NEAR(velocities_[2], 4.25, eps); 00121 } 00122 00123 00124 int main(int argc, char **argv){ 00125 testing::InitGoogleTest(&argc, argv); 00126 return RUN_ALL_TESTS(); 00127 }