utest.cpp
Go to the documentation of this file.
00001 // kate: replace-tabs off; indent-width 4; indent-mode normal
00002 // vim: ts=4:sw=4:noexpandtab
00003 /*
00004 
00005 Copyright (c) 2010--2012,
00006 François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
00007 You can contact the authors at <f dot pomerleau at gmail dot com> and
00008 <stephane at magnenat dot net>
00009 
00010 All rights reserved.
00011 
00012 Redistribution and use in source and binary forms, with or without
00013 modification, are permitted provided that the following conditions are met:
00014     * Redistributions of source code must retain the above copyright
00015       notice, this list of conditions and the following disclaimer.
00016     * Redistributions in binary form must reproduce the above copyright
00017       notice, this list of conditions and the following disclaimer in the
00018       documentation and/or other materials provided with the distribution.
00019     * Neither the name of the <organization> nor the
00020       names of its contributors may be used to endorse or promote products
00021       derived from this software without specific prior written permission.
00022 
00023 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00024 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00025 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00026 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
00027 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00028 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00030 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00031 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00032 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00033 
00034 */
00035 
00036 #include "utest.h"
00037 
00038 using namespace std;
00039 using namespace PointMatcherSupport;
00040 using boost::assign::map_list_of;
00041 
00042 // TODO: avoid global by using testing::Environment
00043 
00044 std::string dataPath;
00045 
00046 DP ref2D;
00047 DP data2D;
00048 DP ref3D;
00049 DP data3D;
00050 PM::TransformationParameters validT2d;
00051 PM::TransformationParameters validT3d;
00052 
00053 //---------------------------
00054 // Test ICP with all existing filters.
00055 
00056 // Algorithm:
00057 // 1. Iterate over all yaml files in
00058 //    libpointmatcher/examples/data/icp_data, each file tests ICP
00059 //    with one or more filters.
00060 // 2. Run ICP with the given yaml file. The filters in the yaml
00061 //    file are applied along the way.
00062 // 3. Write the obtained ICP transform to disk, to the same directory,
00063 //    with file extension .cur_trans (for easy future comparisons).
00064 // 4. Load the reference (known as correct) ICP transform from disk,
00065 //    from the same directory, with file extension .ref_trans.
00066 // 5. See if the current and reference transforms are equal.
00067 
00068 // To update an existing test or add a new test, simply add/modify
00069 // the desired yaml file, run the tests (they may fail this time), then
00070 // copy the (just written) desired current transform file on top of the
00071 // corresponding reference transform file. Run the tests again. This
00072 // time they will succeed.
00073 //---------------------------
00074 
00075 TEST(icpTest, icpTest)
00076 {
00077         DP ref  = DP::load(dataPath + "cloud.00000.vtk");
00078         DP data = DP::load(dataPath + "cloud.00001.vtk");
00079 
00080         namespace fs = boost::filesystem;
00081         fs::path config_dir(dataPath + "icp_data");
00082         EXPECT_TRUE( fs::exists(config_dir) && fs::is_directory(config_dir) );
00083 
00084         fs::directory_iterator end_iter;
00085         for( fs::directory_iterator d(config_dir); d != end_iter; ++d)
00086         {
00087                 if (!fs::is_regular_file(d->status()) ) continue;
00088 
00089                 // Load config file, and form ICP object
00090                 PM::ICP icp;
00091                 std::string config_file = d->path().string();
00092                 if (fs::extension(config_file) != ".yaml") continue;
00093                 std::ifstream ifs(config_file.c_str());
00094                 EXPECT_NO_THROW(icp.loadFromYaml(ifs)) << "This error was caused by the test file:" << endl << "   " << config_file;
00095 
00096                 // Compute current ICP transform
00097                 PM::TransformationParameters curT = icp(data, ref);
00098 
00099                 // Write current transform to disk (to easily compare it
00100                 // with reference transform offline)
00101                 fs::path cur_file = d->path();
00102                 cur_file.replace_extension(".cur_trans");
00103                 //std::cout << "Writing: " << cur_file << std::endl;
00104                 std::ofstream otfs(cur_file.c_str());
00105                 otfs.precision(16);
00106                 otfs << curT;
00107                 otfs.close();
00108                 
00109                 // Load reference transform
00110                 fs::path ref_file = d->path();
00111                 ref_file.replace_extension(".ref_trans");
00112                 PM::TransformationParameters refT = 0*curT;
00113                 //std::cout << "Reading: " << ref_file << std::endl;
00114                 std::ifstream itfs(ref_file.c_str());
00115                 for (int row = 0; row < refT.cols(); row++)
00116                 {
00117                         for (int col = 0; col < refT.cols(); col++)
00118                         {
00119                                 itfs >>refT(row, col);
00120                         }
00121                 }
00122 
00123                 // Dump the reference transform and current one
00124                 //std::cout.precision(17);
00125                 //std::cout << "refT:\n" << refT << std::endl;
00126                 //std::cout << "curT:\n" << curT << std::endl;
00127 
00128                 // Tolerance for change in rotation and translation
00129                 double rotTol = 0.1, transTol = 0.1;
00130 
00131                 // Find how much the reference rotation and translation
00132                 // differ from the current values.
00133                 PM::TransformationParameters refRot   = refT.block(0, 0, 3, 3);
00134                 PM::TransformationParameters refTrans = refT.block(0, 3, 3, 1);
00135                 PM::TransformationParameters curRot   = curT.block(0, 0, 3, 3);
00136                 PM::TransformationParameters curTrans = curT.block(0, 3, 3, 1);
00137                 PM::TransformationParameters rotErrMat = refRot*(curRot.transpose())
00138                   - PM::TransformationParameters::Identity(3, 3);
00139                 PM::TransformationParameters transErrMat = refTrans - curTrans;
00140                 double rotErr = rotErrMat.array().abs().sum();
00141                 double transErr = transErrMat.array().abs().sum();
00142 
00143                 //std::cout << "Rotation error:    " << rotErr   << std::endl;
00144                 //std::cout << "Translation error: " << transErr << std::endl;
00145                 
00146                 EXPECT_LT(rotErr,   rotTol) << "This error was caused by the test file:" << endl << "   " << config_file;
00147                 EXPECT_LT(transErr, transTol) << "This error was caused by the test file:" <<  endl << "   " << config_file;
00148         }
00149 }
00150 
00151 // Utility classes
00152 class GenericTest: public IcpHelper
00153 {
00154 
00155 public:
00156 
00157         // Will be called for every tests
00158         virtual void SetUp()
00159         {
00160                 icp.setDefault();
00161                 // Uncomment for consol outputs
00162                 //setLogger(PM::get().LoggerRegistrar.create("FileLogger"));
00163         }
00164 
00165         // Will be called for every tests
00166         virtual void TearDown()
00167         {       
00168         }
00169 };
00170 
00171 //---------------------------
00172 // Generic tests
00173 //---------------------------
00174 
00175 TEST_F(GenericTest, ICP_default)
00176 {
00177         validate2dTransformation();
00178         validate3dTransformation();
00179 }
00180 
00181 //---------------------------
00182 // Main
00183 //---------------------------
00184 int main(int argc, char **argv)
00185 {
00186         dataPath = "";
00187         for(int i=1; i < argc; i++)
00188         {
00189                 if (strcmp(argv[i], "--path") == 0 && i+1 < argc)
00190                         dataPath = argv[i+1];
00191         }
00192 
00193         if(dataPath == "")
00194         {
00195                 cerr << "Missing the flag --path ./path/to/examples/data\n Please give the path to the test data folder which should be included with the source code. The folder is named 'examples/data'." << endl;
00196                 return -1;
00197         }
00198 
00199         // Load point cloud for all test
00200         ref2D =  DP::load(dataPath + "2D_oneBox.csv");
00201         data2D = DP::load(dataPath + "2D_twoBoxes.csv");
00202         ref3D =  DP::load(dataPath + "car_cloud400.csv");
00203         data3D = DP::load(dataPath + "car_cloud401.csv");
00204 
00205         // Result of data express in ref (from visual inspection)
00206         validT2d = PM::TransformationParameters(3,3);
00207         validT2d <<  0.987498,  0.157629, 0.0859918,
00208                                 -0.157629,  0.987498,  0.203247,
00209                                                 0,         0,         1;
00210 
00211         validT3d = PM::TransformationParameters(4,4);
00212         validT3d <<   0.982304,   0.166685,  -0.0854066,  0.0446816,
00213                                  -0.150189,   0.973488,   0.172524,   0.191998,
00214                                   0.111899,  -0.156644,   0.981296,  -0.0356313,
00215                       0,          0,          0,          1;
00216 
00217         testing::GTEST_FLAG(print_time) = true;
00218         testing::InitGoogleTest(&argc, argv);
00219         return RUN_ALL_TESTS();
00220 }
00221 
00222 
00223 


upstream_src
Author(s):
autogenerated on Mon Oct 6 2014 10:27:42