monocam_settler: monocam_settler: monocam_settler

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 
00037 #include <monocam_settler/ConfigGoal.h>
00038 #include <monocam_settler/monocam_settler.h>
00039 
00040 using namespace std;
00041 using namespace monocam_settler;
00042 
00043 
00044 
00045 static const unsigned int N = 9;
00046 static const unsigned int C = 2;
00047 
00048 static const double data[N][2*C] = { {  0, 0, 0, 0 },
00049                                      {  1, 2, 1, 1 },
00050                                      {  2, 4, 0, 0 },
00051                                      {  3, 6, 1, 1 },
00052                                      {  4, 8, 0, 0 },
00053                                      {  0, 0, 1, 1 },
00054                                      {  4, 1, 0, 0 },
00055                                      {  8, 2, 1, 1 },
00056                                      { 12, 3, 0, 0 }};
00057 
00058 // 2 possible sets of timestamps for the data
00059 static const unsigned int times[N][2] = { { 0,  0 },
00060                                           { 1,  1 },
00061                                           { 2,  2 },
00062                                           { 3,  3 },
00063                                           { 4,  4 },
00064                                           { 5, 15 },
00065                                           { 6, 16 },
00066                                           { 7, 17 },
00067                                           { 8, 18 } };
00068 
00069 static const bool success[N] = { true,
00070                                  true,
00071                                  false,
00072                                  true,
00073                                  true,
00074                                  true,
00075                                  true,
00076                                  true,
00077                                  true };
00078 
00079 // add data to the settler, and see what intervals come out
00080 vector<calibration_msgs::Interval> addToSettler(MonocamSettler& settler, unsigned int time_channel, bool always_success)
00081 {
00082   vector<calibration_msgs::Interval> intervals;
00083 
00084   for (unsigned int i=0; i<N; i++)
00085   {
00086     calibration_msgs::CalibrationPatternPtr msg(new calibration_msgs::CalibrationPattern);
00087     msg->header.stamp = ros::Time(times[i][time_channel], 0);
00088 
00089     msg->image_points.resize(C);
00090     for (unsigned int j=0; j<C; j++)
00091     {
00092       msg->image_points[j].x = data[i][2*j+0];
00093       msg->image_points[j].y = data[i][2*j+1];
00094     }
00095     msg->success = always_success || success[i];
00096 
00097     calibration_msgs::Interval cur_interval;
00098     bool result = settler.add(msg, cur_interval);
00099     if (result)
00100       intervals.push_back(cur_interval);
00101     else
00102       intervals.push_back(calibration_msgs::Interval());
00103   }
00104 
00105   return intervals;
00106 }
00107 
00108 void doEasyCheck(const vector<calibration_msgs::Interval>& intervals)
00109 {
00110   ASSERT_EQ(intervals.size(), N);
00111   EXPECT_EQ(intervals[0].start.sec, (unsigned int) 0);
00112   EXPECT_EQ(intervals[0].end.sec,   (unsigned int) 0);
00113 
00114   EXPECT_EQ(intervals[1].start.sec, (unsigned int) 0);
00115   EXPECT_EQ(intervals[1].end.sec,   (unsigned int) 1);
00116 
00117   EXPECT_EQ(intervals[2].start.sec, (unsigned int) 1);
00118   EXPECT_EQ(intervals[2].end.sec,   (unsigned int) 2);
00119 
00120   EXPECT_EQ(intervals[3].start.sec, (unsigned int) 2);
00121   EXPECT_EQ(intervals[3].end.sec,   (unsigned int) 3);
00122 
00123   EXPECT_EQ(intervals[4].start.sec, (unsigned int) 3);
00124   EXPECT_EQ(intervals[4].end.sec,   (unsigned int) 4);
00125 
00126   EXPECT_EQ(intervals[5].start.sec, (unsigned int) 5);
00127   EXPECT_EQ(intervals[5].end.sec,   (unsigned int) 5);
00128 
00129   EXPECT_EQ(intervals[6].start.sec, (unsigned int) 6);
00130   EXPECT_EQ(intervals[6].end.sec,   (unsigned int) 6);
00131 
00132   EXPECT_EQ(intervals[7].start.sec, (unsigned int) 7);
00133   EXPECT_EQ(intervals[7].end.sec,   (unsigned int) 7);
00134 
00135   EXPECT_EQ(intervals[8].start.sec, (unsigned int) 8);
00136   EXPECT_EQ(intervals[8].end.sec,   (unsigned int) 8);
00137 }
00138 
00139 void doIgnoreFailuresCheck(const vector<calibration_msgs::Interval>& intervals)
00140 {
00141   ASSERT_EQ(intervals.size(), N);
00142   EXPECT_EQ(intervals[0].start.sec, (unsigned int) 0);
00143   EXPECT_EQ(intervals[0].end.sec,   (unsigned int) 0);
00144 
00145   EXPECT_EQ(intervals[1].start.sec, (unsigned int) 0);
00146   EXPECT_EQ(intervals[1].end.sec,   (unsigned int) 1);
00147 
00148   EXPECT_EQ(intervals[2].start.sec, (unsigned int) 0);
00149   EXPECT_EQ(intervals[2].end.sec,   (unsigned int) 0);
00150 
00151   EXPECT_EQ(intervals[3].start.sec, (unsigned int) 0);
00152   EXPECT_EQ(intervals[3].end.sec,   (unsigned int) 3);
00153 
00154   EXPECT_EQ(intervals[4].start.sec, (unsigned int) 0);
00155   EXPECT_EQ(intervals[4].end.sec,   (unsigned int) 4);
00156 
00157   EXPECT_EQ(intervals[5].start.sec, (unsigned int) 0);
00158   EXPECT_EQ(intervals[5].end.sec,   (unsigned int) 5);
00159 
00160   EXPECT_EQ(intervals[6].start.sec, (unsigned int) 0);
00161   EXPECT_EQ(intervals[6].end.sec,   (unsigned int) 6);
00162 
00163   EXPECT_EQ(intervals[7].start.sec, (unsigned int) 0);
00164   EXPECT_EQ(intervals[7].end.sec,   (unsigned int) 7);
00165 
00166   EXPECT_EQ(intervals[8].start.sec, (unsigned int) 0);
00167   EXPECT_EQ(intervals[8].end.sec,   (unsigned int) 8);
00168 }
00169 
00170 void doCatchFailuresCheck(const vector<calibration_msgs::Interval>& intervals)
00171 {
00172   ASSERT_EQ(intervals.size(), N);
00173   EXPECT_EQ(intervals[0].start.sec, (unsigned int) 0);
00174   EXPECT_EQ(intervals[0].end.sec,   (unsigned int) 0);
00175 
00176   EXPECT_EQ(intervals[1].start.sec, (unsigned int) 0);
00177   EXPECT_EQ(intervals[1].end.sec,   (unsigned int) 1);
00178 
00179   EXPECT_EQ(intervals[2].start.sec, (unsigned int) 0);
00180   EXPECT_EQ(intervals[2].end.sec,   (unsigned int) 0);
00181 
00182   EXPECT_EQ(intervals[3].start.sec, (unsigned int) 3);
00183   EXPECT_EQ(intervals[3].end.sec,   (unsigned int) 3);
00184 
00185   EXPECT_EQ(intervals[4].start.sec, (unsigned int) 3);
00186   EXPECT_EQ(intervals[4].end.sec,   (unsigned int) 4);
00187 
00188   EXPECT_EQ(intervals[5].start.sec, (unsigned int) 3);
00189   EXPECT_EQ(intervals[5].end.sec,   (unsigned int) 5);
00190 
00191   EXPECT_EQ(intervals[6].start.sec, (unsigned int) 3);
00192   EXPECT_EQ(intervals[6].end.sec,   (unsigned int) 6);
00193 
00194   EXPECT_EQ(intervals[7].start.sec, (unsigned int) 3);
00195   EXPECT_EQ(intervals[7].end.sec,   (unsigned int) 7);
00196 
00197   EXPECT_EQ(intervals[8].start.sec, (unsigned int) 3);
00198   EXPECT_EQ(intervals[8].end.sec,   (unsigned int) 8);
00199 }
00200 
00201 ConfigGoal config1()
00202 {
00203   ConfigGoal config;
00204 
00205   config.tolerance = 2.5;
00206 
00207   config.ignore_failures = true;
00208 
00209   config.max_step = ros::Duration(2,0);
00210 
00211   config.cache_size = 100;
00212 
00213   return config;
00214 }
00215 
00216 ConfigGoal config2(bool ignore_failures)
00217 {
00218   ConfigGoal config;
00219 
00220   config.tolerance = 100.0;
00221 
00222   config.ignore_failures = ignore_failures;
00223 
00224   config.max_step = ros::Duration(2,0);
00225 
00226   config.cache_size = 100;
00227 
00228   return config;
00229 }
00230 
00231 TEST(MonocamSettler, easyCheck)
00232 {
00233   MonocamSettler settler;
00234 
00235   bool config_result = settler.configure(config1());
00236   ASSERT_TRUE(config_result);
00237 
00238   vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0, true);
00239 
00240   doEasyCheck(intervals);
00241 }
00242 
00243 TEST(MonocamSettler, ignoreFailuresCheck)
00244 {
00245   MonocamSettler settler;
00246 
00247   bool config_result = settler.configure(config2(true));
00248   ASSERT_TRUE(config_result);
00249 
00250   vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0, false);
00251 
00252   doIgnoreFailuresCheck(intervals);
00253 }
00254 
00255 TEST(MonocamSettler, catchFailuresCheck)
00256 {
00257   MonocamSettler settler;
00258 
00259   bool config_result = settler.configure(config2(false));
00260   ASSERT_TRUE(config_result);
00261 
00262   vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0, false);
00263 
00264   doCatchFailuresCheck(intervals);
00265 }
00266 
00267 TEST(MonocamSettler, reconfigureCheck)
00268 {
00269   MonocamSettler settler;
00270 
00271   bool config_result = settler.configure(config1());
00272   ASSERT_TRUE(config_result);
00273 
00274   vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0, true);
00275 
00276   doEasyCheck(intervals);
00277 
00278   config_result = settler.configure(config2(false));
00279   ASSERT_TRUE(config_result);
00280 
00281   intervals = addToSettler(settler, 0, false);
00282 
00283   doCatchFailuresCheck(intervals);
00284 }
00285 
00286 int main(int argc, char **argv)
00287 {
00288   testing::InitGoogleTest(&argc, argv);
00289   return RUN_ALL_TESTS();
00290 }
monocam_settler_unittest.cpp
