00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
00076
00077
00078
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<geometry_msgs::Point> 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 }