/** * Code example for ICP taking a sequence of point clouds relatively close * and build a map with them. * It assumes that: 3D point clouds are used, they were recorded in sequence * and they are express in sensor frame. */ int main(int argc, char *argv[]) { validateArgs(argc, argv); typedef PointMatcher<float> PM; typedef PointMatcherIO<float> PMIO; typedef PM::TransformationParameters TP; typedef PM::DataPoints DP; string outputFileName(argv[0]); // Rigid transformation PM::Transformation* rigidTrans; rigidTrans = PM::get().REG(Transformation).create("RigidTransformation"); // Create filters manually to clean the global map PM::DataPointsFilter* densityFilter( PM::get().DataPointsFilterRegistrar.create( "SurfaceNormalDataPointsFilter", map_list_of ("knn", "10") ("epsilon", "5") ("keepNormals", "0") ("keepDensities", "1") ) ); PM::DataPointsFilter* maxDensitySubsample( PM::get().DataPointsFilterRegistrar.create( "MaxDensityDataPointsFilter", map_list_of ("maxDensity", toParam(30)) ) ); // Main algorithm definition PM::ICP icp; // load YAML config ifstream ifs(argv[1]); validateFile(argv[1]); icp.loadFromYaml(ifs); PMIO::FileInfoVector list(argv[2]); PM::DataPoints mapPointCloud, newCloud; TP T_to_map_from_new = TP::Identity(4,4); // assumes 3D for(unsigned i=0; i < list.size(); i++) { cout << "---------------------\nLoading: " << list[i].readingFileName << endl; // It is assume that the point cloud is express in sensor frame newCloud = DP::load(list[i].readingFileName); if(mapPointCloud.getNbPoints() == 0) { mapPointCloud = newCloud; continue; } // call ICP try { // We use the last transformation as a prior // this assumes that the point clouds were recorded in // sequence. const TP prior = T_to_map_from_new; T_to_map_from_new = icp(newCloud, mapPointCloud, prior); } catch (PM::ConvergenceError& error) { cout << "ERROR PM::ICP failed to converge: " << endl; cout << " " << error.what() << endl; continue; } // This is not necessary in this example, but could be // useful if the same matrix is composed in the loop. T_to_map_from_new = rigidTrans->correctParameters(T_to_map_from_new); // Move the new point cloud in the map reference newCloud = rigidTrans->compute(newCloud, T_to_map_from_new); // Merge point clouds to map mapPointCloud.concatenate(newCloud); // Clean the map mapPointCloud = densityFilter->filter(mapPointCloud); mapPointCloud = maxDensitySubsample->filter(mapPointCloud); // Save the map at each iteration stringstream outputFileNameIter; outputFileNameIter << outputFileName << "_" << i << ".vtk"; cout << "outputFileName: " << outputFileNameIter.str() << endl; mapPointCloud.save(outputFileNameIter.str()); } return 0; }
/** * Code example for ICP taking 2 points clouds (2D or 3D) relatively close * and computing the transformation between them. * * This code is more complete than icp_simple. It can load parameter files and * has more options. */ int main(int argc, const char *argv[]) { bool isTransfoSaved = false; string configFile; string outputBaseFile("test"); string format("vtk") string initTranslation("0,0,0"); string initRotation("1,0,0;0,1,0;0,0,1"); const int ret = validateArgs(argc, argv, isTransfoSaved, configFile, outputBaseFile, format, initTranslation, initRotation); if (ret != 0) { return ret; } const char *refFile(argv[argc-2]); const char *dataFile(argv[argc-1]); // Load point clouds const DP ref(DP::load(refFile)); const DP data(DP::load(dataFile)); // Create the default ICP algorithm PM::ICP icp; if (configFile.empty()) { // See the implementation of setDefault() to create a custom ICP algorithm icp.setDefault(); } else { // load YAML config ifstream ifs(configFile.c_str()); if (!ifs.good()) { cerr << "Cannot open config file " << configFile << ", usage:"; usage(argv); exit(1); } icp.loadFromYaml(ifs); } int cloudDimension = ref.getEuclideanDim(); if (!(cloudDimension == 2 || cloudDimension == 3)) { cerr << "Invalid input point clouds dimension = " << cloudDimension << endl; exit(1); } PM::TransformationParameters translation = parseTranslation(initTranslation, cloudDimension); PM::TransformationParameters rotation = parseRotation(initRotation, cloudDimension); PM::TransformationParameters initTransfo = translation*rotation; PM::Transformation* rigidTrans; rigidTrans = PM::get().REG(Transformation).create("RigidTransformation"); if (!rigidTrans->checkParameters(initTransfo)) { cerr << endl << "Initial transformation is not rigid, identiy will be used" << endl; initTransfo = PM::TransformationParameters::Identity( cloudDimension+1,cloudDimension+1); } const DP initializedData = rigidTrans->compute(data, initTransfo); // Compute the transformation to express data in ref PM::TransformationParameters T = icp(initializedData, ref); // cout << outputBaseFile << " match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl; // Transform data to express it in ref DP data_out(initializedData); icp.transformations.apply(data_out, T); // Safe files to see the results ref.save(outputBaseFile + "_ref." + format); data.save(outputBaseFile + "_data_in" + format); data_out.save(outputBaseFile + "_data_out" + format); if(isTransfoSaved) { ofstream transfoFile; string initFileName = outputBaseFile + "_init_transfo.txt"; string icpFileName = outputBaseFile + "_icp_transfo.txt"; string completeFileName = outputBaseFile + "_complete_transfo.txt"; transfoFile.open(initFileName.c_str()); if(transfoFile.is_open()) { transfoFile << initTransfo << endl; transfoFile.close(); } else { cout << "Unable to write the initial transformation file\n" << endl; } transfoFile.open(icpFileName.c_str()); if(transfoFile.is_open()) { transfoFile << T << endl; transfoFile.close(); } else { cout << "Unable to write the ICP transformation file\n" << endl; } transfoFile.open(completeFileName.c_str()); if(transfoFile.is_open()) { transfoFile << T*initTransfo << endl; transfoFile.close(); } else { cout << "Unable to write the complete transformation file\n" << endl; } } else { cout << "ICP transformation:" << endl << T << endl; } return 0; }
TEST(IOTest, loadYaml) { // Test loading configuration files for data filters std::ifstream ifs0((dataPath + "default-convert.yaml").c_str()); EXPECT_NO_THROW(PM::DataPointsFilters filters(ifs0)); // Test loading configuration files for ICP PM::ICP icp; std::ifstream ifs1((dataPath + "default.yaml").c_str()); EXPECT_NO_THROW(icp.loadFromYaml(ifs1)); std::ifstream ifs2((dataPath + "unit_tests/badIcpConfig_InvalidParameter.yaml").c_str()); EXPECT_THROW(icp.loadFromYaml(ifs2), PointMatcherSupport::Parametrizable::InvalidParameter); std::ifstream ifs3((dataPath + "unit_tests/badIcpConfig_InvalidModuleType.yaml").c_str()); EXPECT_THROW(icp.loadFromYaml(ifs3), PointMatcherSupport::InvalidModuleType); }
void transformation (int argc, char *argv[]) { bool isCSV = true; typedef PointMatcher<float> PM; typedef PM::DataPoints DP; typedef PM::Parameters Parameters; // Load point clouds //const DP ref(DP::load(argv[1])); //const DP data(DP::load(argv[2])); const DP ref(DP::load("Golden.csv")); const DP data(DP::load("Final.csv")); // Create the default ICP algorithm PM::ICP icp; string configFile = "my_config.yaml"; if (configFile.empty()) { // See the implementation of setDefault() to create a custom ICP algorithm icp.setDefault(); } else { // load YAML config ifstream ifs(configFile.c_str()); if (!ifs.good()) { cout << "Cannot open config file" << configFile; } icp.loadFromYaml(ifs); } // Compute the transformation to express data in ref PM::TransformationParameters T = icp(data, ref); // Transform data to express it in ref DP data_out(data); icp.transformations.apply(data_out, T); // Safe files to see the results ref.save("test_ref.vtk"); data.save("test_data_in.vtk"); data_out.save("test_data_out.vtk"); cout << "Final transformation:" << endl << T << endl; // Updating transform matrix T(0,3) = offset_x; T(1,3) = offset_y; T(2,3) = offset_z; cout << "Final transformation:" << endl << T << endl; if(MATRIX_DUMP == 1) { for(int i = 0; i < 4; i++) { Trans_dump << T(i,0) <<"," <<T(i,1) <<"," << T(i,2) << "," << T(i,3) <<endl; } Trans_dump << endl << endl; } float R[3][3]; for(int i = 0; i < 3; i++) { for(int j = 0; j < 3; j++) R[j][i] = T(i,j); // Euler angle function uses Column Major } Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); //Eigen::Affine3f A; for(int i = 0; i < 3; i++) { for(int j = 0; j < 3; j++) transform_1 (i,j) = T(i,j); // Euler angle function uses Column Major //transform_1 (i,j) = R[i][j]; // Euler angle function uses Column Major } cout << "Writing to cloud transformed" <<endl; pcl::transformPointCloud (*cloud_golden, *cloud_transformed, transform_1); eulerAngles(R); cout << "Euler Angles : X = " << Angle[0] * 180/ PI << ", Y = " << Angle[1] * 180/ PI << " , Z = " << Angle[2] * 180/ PI << endl; //pcl::getTransformation(0,0,0,-1*Angle[2],-1*Angle[0],-1*Angle[1]); //transform_1 = A.matrix(); //pcl::transformPointCloud (*cloud_golden, *cloud_transformed, transform_1); }
TEST(icpTest, icpTest) { DP ref = DP::load(dataPath + "cloud.00000.vtk"); DP data = DP::load(dataPath + "cloud.00001.vtk"); namespace fs = boost::filesystem; fs::path config_dir(dataPath + "icp_data"); EXPECT_TRUE( fs::exists(config_dir) && fs::is_directory(config_dir) ); fs::directory_iterator end_iter; for( fs::directory_iterator d(config_dir); d != end_iter; ++d) { if (!fs::is_regular_file(d->status()) ) continue; // Load config file, and form ICP object PM::ICP icp; std::string config_file = d->path().string(); if (fs::extension(config_file) != ".yaml") continue; std::ifstream ifs(config_file.c_str()); EXPECT_NO_THROW(icp.loadFromYaml(ifs)) << "This error was caused by the test file:" << endl << " " << config_file; // Compute current ICP transform PM::TransformationParameters curT = icp(data, ref); // Write current transform to disk (to easily compare it // with reference transform offline) fs::path cur_file = d->path(); cur_file.replace_extension(".cur_trans"); //std::cout << "Writing: " << cur_file << std::endl; std::ofstream otfs(cur_file.c_str()); otfs.precision(16); otfs << curT; otfs.close(); // Load reference transform fs::path ref_file = d->path(); ref_file.replace_extension(".ref_trans"); PM::TransformationParameters refT = 0*curT; //std::cout << "Reading: " << ref_file << std::endl; std::ifstream itfs(ref_file.c_str()); for (int row = 0; row < refT.cols(); row++) { for (int col = 0; col < refT.cols(); col++) { itfs >>refT(row, col); } } // Dump the reference transform and current one //std::cout.precision(17); //std::cout << "refT:\n" << refT << std::endl; //std::cout << "curT:\n" << curT << std::endl; // Tolerance for change in rotation and translation double rotTol = 0.1, transTol = 0.1; // Find how much the reference rotation and translation // differ from the current values. PM::TransformationParameters refRot = refT.block(0, 0, 3, 3); PM::TransformationParameters refTrans = refT.block(0, 3, 3, 1); PM::TransformationParameters curRot = curT.block(0, 0, 3, 3); PM::TransformationParameters curTrans = curT.block(0, 3, 3, 1); PM::TransformationParameters rotErrMat = refRot*(curRot.transpose()) - PM::TransformationParameters::Identity(3, 3); PM::TransformationParameters transErrMat = refTrans - curTrans; double rotErr = rotErrMat.array().abs().sum(); double transErr = transErrMat.array().abs().sum(); //std::cout << "Rotation error: " << rotErr << std::endl; //std::cout << "Translation error: " << transErr << std::endl; EXPECT_LT(rotErr, rotTol) << "This error was caused by the test file:" << endl << " " << config_file; EXPECT_LT(transErr, transTol) << "This error was caused by the test file:" << endl << " " << config_file; } }
void transformation (Mat& rgbframe) { bool isCSV = true; typedef PointMatcher<float> PM; typedef PM::DataPoints DP; typedef PM::Parameters Parameters; // Load point clouds //const DP ref(DP::load(argv[1])); //const DP data(DP::load(argv[2])); const DP ref(DP::load("Golden.csv")); std::ostringstream fileNameStream(""); fileNameStream << "Capture" << filenumber - 1 << ".csv"; string Inputname = fileNameStream.str(); const DP data(DP::load(Inputname.c_str())); //const DP data(DP::load("Final.csv")); // Create the default ICP algorithm PM::ICP icp; string configFile = "my_config.yaml"; if (configFile.empty()) { // See the implementation of setDefault() to create a custom ICP algorithm icp.setDefault(); } else { // load YAML config ifstream ifs(configFile.c_str()); if (!ifs.good()) { cout << "Cannot open config file" << configFile; } icp.loadFromYaml(ifs); } // Compute the transformation to express data in ref PM::TransformationParameters T = icp(data, ref); // Transform data to express it in ref DP data_out(data); icp.transformations.apply(data_out, T); // Safe files to see the results cout << "match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl; //cout << "Iteration Count: " << icp.iterationCount << endl; //ref.save("test_ref.vtk"); //data.save("test_data_in.vtk"); //data_out.save("test_data_out.vtk"); //cout << "Final transformation:" << endl << T << endl; // Updating transform matrix T(0,3) += offset_x; T(1,3) += offset_y; T(2,3) += offset_z; cout << "Final transformation:" << endl << T << endl; // float R[3][3]; // // for(int i = 0; i < 3; i++) // { // for(int j = 0; j < 3; j++) // R[j][i] = T(i,j); // Euler angle function uses Column Major // } // eulerAngles(R); // // cout << "Euler Angles : X = " << Angle[0] * 180/ PI << ", Y = " << Angle[1] * 180/ PI << " , Z = " << Angle[2] * 180/ PI << endl; // // if(MATRIX_DUMP == 1) { // //for(int i = 0; i < 4; i++) // //{ // Trans_dump << T(0,3) <<","<< T(1,3) << "," << T(2,3) << "," << Angle[0] * 180/ PI << "," << Angle[1] * 180/ PI << "," << Angle[2] * 180/ PI << endl; //} //Trans_dump << endl << endl; // Tranforming golden and showing it as image // for(int i = 0; i < 3; i++) // { // for(int j = 0; j < 3; j++) // if( i != j ) // T(i,j) = -T(i,j); // Euler angle function uses Column Major // } //T(0,3) = 0; //T(1,3) = 0; //T(2,3) = 0; // T = T.inverse(); // PM::Transformation* rigidTrans; // rigidTrans = PM::get().REG(Transformation).create("RigidTransformation"); // if (!rigidTrans->checkParameters(T)) { // std::cout << "WARNING: T does not represent a valid rigid transformation\nProjecting onto an orthogonal basis" // << std::endl; // T = rigidTrans->correctParameters(T); // } // Compute the transformation w.r.t GOLDEN // PM::DataPoints outputCloud = rigidTrans->compute(ref,T); //outputCloud.save("Ref_transform.vtk"); // Showing GREEN POINTSS !! // const int pointCount(outputCloud.features.cols()); // const int dimCount(outputCloud.features.rows()); // const int descDimCount(outputCloud.descriptors.rows()); //for (int p = 1; p < pointCount; p = p + 2) //{ // rgbframe.at<cv::Vec3b>( outputCloud.features(1,p) , outputCloud.features(0,p) ) [0] = 0; // rgbframe.at<cv::Vec3b>( outputCloud.features(1,p) , outputCloud.features(0,p) ) [1] = 255; // rgbframe.at<cv::Vec3b>( outputCloud.features(1,p) , outputCloud.features(0,p) ) [2] = 0; //} //imshow("Tracked", rgbframe ); //} }
/** * Code example for ICP taking 2 points clouds (2D or 3D) relatively close * and computing the transformation between them. * * This code is more complete than icp_simple. It can load parameter files and * has more options. */ int main(int argc, const char *argv[]) { bool isTransfoSaved = false; string configFile; string outputBaseFile("test"); string initTranslation("0,0,0"); string initRotation("1,0,0;0,1,0;0,0,1"); const int ret = validateArgs(argc, argv, isTransfoSaved, configFile, outputBaseFile, initTranslation, initRotation); if (ret != 0) { return ret; } const char *refFile(argv[argc-2]); const char *dataFile(argv[argc-1]); // Load point clouds const DP ref(DP::load(refFile)); const DP data(DP::load(dataFile)); // Create the default ICP algorithm PM::ICP icp; if (configFile.empty()) { // See the implementation of setDefault() to create a custom ICP algorithm icp.setDefault(); } else { // load YAML config ifstream ifs(configFile.c_str()); if (!ifs.good()) { cerr << "Cannot open config file " << configFile << ", usage:"; usage(argv); exit(1); } icp.loadFromYaml(ifs); } int cloudDimension = ref.getEuclideanDim(); if (!(cloudDimension == 2 || cloudDimension == 3)) { cerr << "Invalid input point clouds dimension" << endl; exit(1); } PM::TransformationParameters translation = parseTranslation(initTranslation, cloudDimension); PM::TransformationParameters rotation = parseRotation(initRotation, cloudDimension); PM::TransformationParameters initTransfo = translation*rotation; PM::Transformation* rigidTrans; rigidTrans = PM::get().REG(Transformation).create("RigidTransformation"); if (!rigidTrans->checkParameters(initTransfo)) { cerr << endl << "Initial transformation is not rigid, identiy will be used" << endl; initTransfo = PM::TransformationParameters::Identity( cloudDimension+1,cloudDimension+1); } const DP initializedData = rigidTrans->compute(data, initTransfo); // Compute the transformation to express data in ref PM::TransformationParameters T = icp(initializedData, ref); float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio(); cout << "match ratio: " << matchRatio << endl; // Transform data to express it in ref DP data_out(initializedData); icp.transformations.apply(data_out, T); cout << endl << "------------------" << endl; // START demo 1 // Test for retrieving Haussdorff distance (with outliers). We generate new matching module // specifically for this purpose. // // INPUTS: // ref: point cloud used as reference // data_out: aligned point cloud (using the transformation outputted by icp) // icp: icp object used to aligned the point clouds // Structure to hold future match results PM::Matches matches; Parametrizable::Parameters params; params["knn"] = toParam(1); // for Hausdorff distance, we only need the first closest point params["epsilon"] = toParam(0); PM::Matcher* matcherHausdorff = PM::get().MatcherRegistrar.create("KDTreeMatcher", params); // max. distance from reading to reference matcherHausdorff->init(ref); matches = matcherHausdorff->findClosests(data_out); float maxDist1 = matches.getDistsQuantile(1.0); float maxDistRobust1 = matches.getDistsQuantile(0.85); // max. distance from reference to reading matcherHausdorff->init(data_out); matches = matcherHausdorff->findClosests(ref); float maxDist2 = matches.getDistsQuantile(1.0); float maxDistRobust2 = matches.getDistsQuantile(0.85); float haussdorffDist = std::max(maxDist1, maxDist2); float haussdorffQuantileDist = std::max(maxDistRobust1, maxDistRobust2); cout << "Haussdorff distance: " << std::sqrt(haussdorffDist) << " m" << endl; cout << "Haussdorff quantile distance: " << std::sqrt(haussdorffQuantileDist) << " m" << endl; // START demo 2 // Test for retrieving paired point mean distance without outliers. We reuse the same module used for // the icp object. // // INPUTS: // ref: point cloud used as reference // data_out: aligned point cloud (using the transformation outputted by icp) // icp: icp object used to aligned the point clouds // initiate the matching with unfiltered point cloud icp.matcher->init(ref); // extract closest points matches = icp.matcher->findClosests(data_out); // weight paired points const PM::OutlierWeights outlierWeights = icp.outlierFilters.compute(data_out, ref, matches); // generate tuples of matched points and remove pairs with zero weight const PM::ErrorMinimizer::ErrorElements matchedPoints( data_out, ref, outlierWeights, matches); // extract relevant information for convenience const int dim = matchedPoints.reading.getEuclideanDim(); const int nbMatchedPoints = matchedPoints.reading.getNbPoints(); const PM::Matrix matchedRead = matchedPoints.reading.features.topRows(dim); const PM::Matrix matchedRef = matchedPoints.reference.features.topRows(dim); // compute mean distance const PM::Matrix dist = (matchedRead - matchedRef).colwise().norm(); // replace that by squaredNorm() to save computation time const float meanDist = dist.sum()/nbMatchedPoints; cout << "Robust mean distance: " << meanDist << " m" << endl; // END demo cout << "------------------" << endl << endl; // Safe files to see the results ref.save(outputBaseFile + "_ref.vtk"); data.save(outputBaseFile + "_data_in.vtk"); data_out.save(outputBaseFile + "_data_out.vtk"); if(isTransfoSaved) { ofstream transfoFile; string initFileName = outputBaseFile + "_init_transfo.txt"; string icpFileName = outputBaseFile + "_icp_transfo.txt"; string completeFileName = outputBaseFile + "_complete_transfo.txt"; transfoFile.open(initFileName.c_str()); if(transfoFile.is_open()) { transfoFile << initTransfo << endl; transfoFile.close(); } else { cout << "Unable to write the initial transformation file\n" << endl; } transfoFile.open(icpFileName.c_str()); if(transfoFile.is_open()) { transfoFile << T << endl; transfoFile.close(); } else { cout << "Unable to write the ICP transformation file\n" << endl; } transfoFile.open(completeFileName.c_str()); if(transfoFile.is_open()) { transfoFile << T*initTransfo << endl; transfoFile.close(); } else { cout << "Unable to write the complete transformation file\n" << endl; } } else { cout << "ICP transformation:" << endl << T << endl; } return 0; }