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);
}
bool CloudMatcher::match(pointmatcher_ros::MatchClouds::Request& req, pointmatcher_ros::MatchClouds::Response& res)
{
// get and check reference
const DP referenceCloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(req.reference));
const unsigned referenceGoodCount(referenceCloud.features.cols());
const unsigned referencePointCount(req.reference.width * req.reference.height);
const double referenceGoodRatio(double(referenceGoodCount) / double(referencePointCount));
if (referenceGoodCount == 0)
{
ROS_ERROR("I found no good points in the reference cloud");
return false;
}
if (referenceGoodRatio < 0.5)
{
ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - referenceGoodRatio*100.0 <<
"% of the cloud (received " << referenceGoodCount << ")");
}
// get and check reading
const DP readingCloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(req.readings));
const unsigned readingGoodCount(referenceCloud.features.cols());
const unsigned readingPointCount(req.readings.width * req.readings.height);
const double readingGoodRatio(double(readingGoodCount) / double(readingPointCount));
if (readingGoodCount == 0)
{
ROS_ERROR("I found no good points in the reading cloud");
return false;
}
if (readingGoodRatio < 0.5)
{
ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - readingGoodRatio*100.0 << "% of the cloud (received " << readingGoodCount << ")");
}
// check dimensions
if (referenceCloud.features.rows() != readingCloud.features.rows())
{
ROS_ERROR_STREAM("Dimensionality missmatch: reference cloud is " <<
referenceCloud.features.rows() - 1 << " while reading cloud is " <<
readingCloud.features.rows() - 1);
return false;
}
// call ICP
PM::TransformationParameters transform;
try
{
transform = icp(readingCloud, referenceCloud);
tf::transformTFToMsg(PointMatcher_ros::eigenMatrixToTransform<float>(transform), res.transform);
ROS_INFO_STREAM("match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl);
}
catch (PM::ConvergenceError error)
{
ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
return false;
}
if(traceMatching)
{
std::stringstream ss;
ss << "reading_" << matchedCloudsCount << ".vtk";
readingCloud.save(ss.str());
ss.str(std::string());
ss << "reference_" << matchedCloudsCount << ".vtk";
referenceCloud.save(ss.str());
PM::Transformation* rigidTrans;
rigidTrans = PM::get().REG(Transformation).create("RigidTransformation");
PM::DataPoints matchedCloud = rigidTrans->compute(readingCloud, transform);
ss.str(std::string());
ss << "matched_" << matchedCloudsCount << ".vtk";
matchedCloud.save(ss.str());
}
matchedCloudsCount++;
return true;
}
