test_calibration.cpp
Go to the documentation of this file.
1 // Copyright (C) 2012, 2019 Austin Robot Technology, Jack O'Quin, Joshua Whitley
2 // All rights reserved.
3 //
4 // Software License Agreement (BSD License 2.0)
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions
8 // are met:
9 //
10 // * Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 // * Redistributions in binary form must reproduce the above
13 // copyright notice, this list of conditions and the following
14 // disclaimer in the documentation and/or other materials provided
15 // with the distribution.
16 // * Neither the name of {copyright_holder} nor the names of its
17 // contributors may be used to endorse or promote products derived
18 // from this software without specific prior written permission.
19 //
20 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 // POSSIBILITY OF SUCH DAMAGE.
32 
33 #include <gtest/gtest.h>
34 
35 #include <ros/package.h>
37 
38 #include <string>
39 
40 using namespace velodyne_pointcloud; // NOLINT
41 
42 // global test data
43 std::string g_package_name("velodyne_pointcloud");
44 std::string g_package_path;
45 
46 void init_global_data(void)
47 {
49 }
50 
52 // Test cases
54 
55 TEST(Calibration, missing_file)
56 {
57  Calibration calibration(false);
58  calibration.read("./no_such_file.yaml");
59  EXPECT_FALSE(calibration.initialized);
60 }
61 
63 {
64  Calibration calibration(g_package_path + "/params/VLP16db.yaml", false);
65  EXPECT_TRUE(calibration.initialized);
66  ASSERT_EQ(calibration.num_lasers, 16);
67 
68  // check some values for the first laser:
69  LaserCorrection laser = calibration.laser_corrections[0];
70  EXPECT_FALSE(laser.two_pt_correction_available);
71  EXPECT_FLOAT_EQ(laser.vert_correction, -0.2617993877991494);
72  EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.0);
73  EXPECT_EQ(laser.max_intensity, 255);
74  EXPECT_EQ(laser.min_intensity, 0);
75 
76  // check similar values for the last laser:
77  laser = calibration.laser_corrections[15];
78  EXPECT_FALSE(laser.two_pt_correction_available);
79  EXPECT_FLOAT_EQ(laser.vert_correction, 0.2617993877991494);
80  EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.0);
81  EXPECT_EQ(laser.max_intensity, 255);
82  EXPECT_EQ(laser.min_intensity, 0);
83 }
84 
85 TEST(Calibration, hdl32e)
86 {
87  Calibration calibration(g_package_path + "/params/32db.yaml", false);
88  EXPECT_TRUE(calibration.initialized);
89  ASSERT_EQ(calibration.num_lasers, 32);
90 
91  // check some values for the first laser:
92  LaserCorrection laser = calibration.laser_corrections[0];
93  EXPECT_FALSE(laser.two_pt_correction_available);
94  EXPECT_FLOAT_EQ(laser.vert_correction, -0.5352924815866609);
95  EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.0);
96  EXPECT_EQ(laser.max_intensity, 255);
97  EXPECT_EQ(laser.min_intensity, 0);
98 
99  // check similar values for the last laser:
100  laser = calibration.laser_corrections[31];
101  EXPECT_FALSE(laser.two_pt_correction_available);
102  EXPECT_FLOAT_EQ(laser.vert_correction, 0.18622663118779495);
103  EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.0);
104  EXPECT_EQ(laser.max_intensity, 255);
105  EXPECT_EQ(laser.min_intensity, 0);
106 }
107 
109 {
110  Calibration calibration(g_package_path + "/params/64e_utexas.yaml", false);
111  EXPECT_TRUE(calibration.initialized);
112  ASSERT_EQ(calibration.num_lasers, 64);
113 
114  // check some values for the first laser:
115  LaserCorrection laser = calibration.laser_corrections[0];
116  EXPECT_FALSE(laser.two_pt_correction_available);
117  EXPECT_FLOAT_EQ(laser.vert_correction, -0.124932751059532);
118  EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.0);
119  EXPECT_EQ(laser.max_intensity, 255);
120  EXPECT_EQ(laser.min_intensity, 0);
121 
122  // check similar values for the last laser:
123  laser = calibration.laser_corrections[63];
124  EXPECT_FALSE(laser.two_pt_correction_available);
125  EXPECT_FLOAT_EQ(laser.vert_correction, -0.209881335496902);
126  EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.0);
127  EXPECT_EQ(laser.max_intensity, 255);
128  EXPECT_EQ(laser.min_intensity, 0);
129 }
130 
131 TEST(Calibration, hdl64e_s21)
132 {
133  Calibration calibration(g_package_path + "/params/64e_s2.1-sztaki.yaml",
134  false);
135  EXPECT_TRUE(calibration.initialized);
136  ASSERT_EQ(calibration.num_lasers, 64);
137 
138  // check some values for the first laser:
139  LaserCorrection laser = calibration.laser_corrections[0];
140  EXPECT_FALSE(laser.two_pt_correction_available);
141  EXPECT_FLOAT_EQ(laser.vert_correction, -0.15304134919741974);
142  EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.025999999);
143  EXPECT_EQ(laser.max_intensity, 235);
144  EXPECT_EQ(laser.min_intensity, 30);
145 
146  // check similar values for the last laser:
147  laser = calibration.laser_corrections[63];
148  EXPECT_FALSE(laser.two_pt_correction_available);
149  EXPECT_FLOAT_EQ(laser.vert_correction, -0.2106649408137298);
150  EXPECT_FLOAT_EQ(laser.horiz_offset_correction, -0.025999999);
151  EXPECT_EQ(laser.max_intensity, 255);
152  EXPECT_EQ(laser.min_intensity, 0);
153 }
154 
155 TEST(Calibration, hdl64e_s2_float_intensities)
156 {
158  "/tests/issue_84_float_intensities.yaml",
159  false);
160  EXPECT_TRUE(calibration.initialized);
161  ASSERT_EQ(calibration.num_lasers, 64);
162 
163  // check some values for the first laser:
164  LaserCorrection laser = calibration.laser_corrections[0];
165  EXPECT_FALSE(laser.two_pt_correction_available);
166  EXPECT_FLOAT_EQ(laser.vert_correction, -0.12118950050089745);
167  EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.025999999);
168  EXPECT_EQ(laser.max_intensity, 255);
169  EXPECT_EQ(laser.min_intensity, 40);
170 
171  // check similar values for laser 26:
172  laser = calibration.laser_corrections[26];
173  EXPECT_FALSE(laser.two_pt_correction_available);
174  EXPECT_FLOAT_EQ(laser.vert_correction, -0.014916840599137901);
175  EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.025999999);
176  EXPECT_EQ(laser.max_intensity, 245);
177  EXPECT_EQ(laser.min_intensity, 0);
178 
179  // check similar values for the last laser:
180  laser = calibration.laser_corrections[63];
181  EXPECT_FALSE(laser.two_pt_correction_available);
182  EXPECT_FLOAT_EQ(laser.vert_correction, -0.20683046990039078);
183  EXPECT_FLOAT_EQ(laser.horiz_offset_correction, -0.025999999);
184  EXPECT_EQ(laser.max_intensity, 255);
185  EXPECT_EQ(laser.min_intensity, 0);
186 }
187 
188 // Run all the tests that were declared with TEST()
189 int main(int argc, char **argv)
190 {
191  testing::InitGoogleTest(&argc, argv);
193  return RUN_ALL_TESTS();
194 }
195 
void init_global_data(void)
int main(int argc, char **argv)
Calibration information for the entire device.
Definition: calibration.h:78
TEST(Calibration, missing_file)
std::vector< LaserCorrection > laser_corrections
Definition: calibration.h:83
std::string g_package_path
ROSLIB_DECL std::string getPath(const std::string &package_name)
correction values for a single laser
Definition: calibration.h:52
std::string g_package_name("velodyne_pointcloud")
dictionary calibration
void read(const std::string &calibration_file)
Definition: calibration.cc:251


velodyne_pointcloud
Author(s): Jack O'Quin, Piyush Khandelwal, Jesse Vera, Sebastian Pütz
autogenerated on Thu Jul 4 2019 19:09:30