int main()
{
DP D;
ll N;
while(sfl(N)==1)
pfl(D.init(N));
return (0);
}
void Assembler::gotScan(const sensor_msgs::LaserScan& scanMsg)
{
//PointMatcherSupport::timer t;
scanQueue.push(scanMsg);
ros::Duration dt = ros::Time::now() - scanQueue.front().header.stamp;
if(dt > durationBuffer)
{
sensor_msgs::LaserScan scanMsgIn = scanQueue.front();
scanQueue.pop();
scanMsgIn.header.stamp += msgDelay;
scanTime = scanMsgIn.header.stamp;
// Modify range of the msg before conversion
scanMsgIn.range_min = minRange;
scanMsgIn.range_max = maxRange;
try
{
if(cloud.features.cols() == 0)
{
cloud = PointMatcher_ros::rosMsgToPointMatcherCloud<float>(
scanMsgIn, &tfListener, odom_frame, true, true);
}
else
{
cloud.concatenate(
PointMatcher_ros::rosMsgToPointMatcherCloud<float>(
scanMsgIn, &tfListener, odom_frame, true, true));
}
}
catch(tf::ExtrapolationException e)
{
ROS_WARN_STREAM(e.what());
}
catch(PM::DataPoints::InvalidField e)
{
ROS_WARN_STREAM(e.what());
}
if(timeLastScan != ros::Time(0) && scanTime > timeLastScan)
{
publishCloud();
timeLastScan = ros::Time(0);
}
}
//double runTime = t.elapsed();
//cout << "Callback took " << runTime << " sec, (" << 1/runTime << " Hz)" << endl;
}
Mapper::DP* Mapper::updateMap(DP* newMap, const PM::TransformationParameters Ticp, bool updateExisting)
{
timer t;
// Prepare empty field if not existing
// FIXME: this is only needed for the none overlaping part
if(newMap->descriptorExists("probabilityStatic") == false)
{
//newMap->addDescriptor("probabilityStatic", PM::Matrix::Zero(1, newMap->features.cols()));
newMap->addDescriptor("probabilityStatic", PM::Matrix::Constant(1, newMap->features.cols(), priorStatic));
}
if(newMap->descriptorExists("probabilityDynamic") == false)
{
//newMap->addDescriptor("probabilityDynamic", PM::Matrix::Zero(1, newMap->features.cols()));
newMap->addDescriptor("probabilityDynamic", PM::Matrix::Constant(1, newMap->features.cols(), priorDyn));
}
if(newMap->descriptorExists("debug") == false)
{
newMap->addDescriptor("debug", PM::Matrix::Zero(1, newMap->features.cols()));
}
if (!updateExisting)
{
// FIXME: correct that, ugly
cout << "Jumping map creation" << endl;
*newMap = transformation->compute(*newMap, Ticp);
mapPostFilters.apply(*newMap);
return newMap;
}
// FIXME: only for debug
mapPointCloud->getDescriptorViewByName("debug") = PM::Matrix::Zero(1, mapPointCloud->features.cols());
const int newMapPts(newMap->features.cols());
const int mapPtsCount(mapPointCloud->features.cols());
// Build a range image of the reading point cloud (local coordinates)
PM::Matrix radius_newMap = newMap->features.topRows(3).colwise().norm();
PM::Matrix angles_newMap(2, newMapPts); // 0=inclination, 1=azimuth
// No atan in Eigen, so we are for to loop through it...
for(int i=0; i<newMapPts; i++)
{
const float ratio = newMap->features(2,i)/radius_newMap(0,i);
angles_newMap(0,i) = acos(ratio);
angles_newMap(1,i) = atan2(newMap->features(1,i), newMap->features(0,i));
}
std::shared_ptr<NNS> kdtree;
kdtree.reset( NNS::create(angles_newMap));
//-------------- Global map ------------------------------
// Transform the global map in local coordinates
DP mapLocal = transformation->compute(*mapPointCloud, Ticp.inverse());
// ROI: Region of Interest
// We reduce the global map to the minimum for the processing
//const float sensorMaxRange = 80.0; // ICRA
PM::Matrix globalId(1, mapPtsCount);
int ROIpts = 0;
int notROIpts = 0;
// Split mapLocal
DP mapLocalROI(mapLocal.createSimilarEmpty());
for (int i = 0; i < mapPtsCount; i++)
{
// Copy the points of the ROI in a new map
if (mapLocal.features.col(i).head(3).norm() < sensorMaxRange)
{
mapLocalROI.setColFrom(ROIpts, mapLocal, i);
globalId(0,ROIpts) = i;
ROIpts++;
}
else // Remove the points of the ROI from the global map
{
mapLocal.setColFrom(notROIpts, mapLocal, i);
notROIpts++;
}
}
mapLocalROI.conservativeResize(ROIpts);
mapLocal.conservativeResize(notROIpts);
// Convert the map in spherical coordinates
PM::Matrix radius_map = mapLocalROI.features.topRows(3).colwise().norm();
PM::Matrix angles_map(2, ROIpts); // 0=inclination, 1=azimuth
// No atan in Eigen, so we are looping through it...
// TODO: check for: A.binaryExpr(B, std::ptr_fun(atan2))
for(int i=0; i < ROIpts; i++)
{
const float ratio = mapLocalROI.features(2,i)/radius_map(0,i);
//if(ratio < -1 || ratio > 1)
//cout << "Error angle!" << endl;
angles_map(0,i) = acos(ratio);
angles_map(1,i) = atan2(mapLocalROI.features(1,i), mapLocalROI.features(0,i));
}
// Prepare access to descriptors
DP::View viewOn_Msec_overlap = newMap->getDescriptorViewByName("stamps_Msec");
DP::View viewOn_sec_overlap = newMap->getDescriptorViewByName("stamps_sec");
DP::View viewOn_nsec_overlap = newMap->getDescriptorViewByName("stamps_nsec");
DP::View viewOnProbabilityStatic = mapLocalROI.getDescriptorViewByName("probabilityStatic");
DP::View viewOnProbabilityDynamic = mapLocalROI.getDescriptorViewByName("probabilityDynamic");
DP::View viewDebug = mapLocalROI.getDescriptorViewByName("debug");
DP::View viewOn_normals_map = mapLocalROI.getDescriptorViewByName("normals");
DP::View viewOn_Msec_map = mapLocalROI.getDescriptorViewByName("stamps_Msec");
DP::View viewOn_sec_map = mapLocalROI.getDescriptorViewByName("stamps_sec");
DP::View viewOn_nsec_map = mapLocalROI.getDescriptorViewByName("stamps_nsec");
// Search for the nearest point in newMap
Matches::Dists dists(1, ROIpts);
Matches::Ids ids(1, ROIpts);
kdtree->knn(angles_map, ids, dists, 1, 0, NNS::ALLOW_SELF_MATCH, maxAngle);
// update probability of being dynamic for all points in ROI
for(int i=0; i < ROIpts; i++)
{
const int mapId = i;
viewDebug(0,mapId) = 1; //FIXME: debug
if(dists(i) != numeric_limits<float>::infinity())
{
const int readId = ids(0,i);
//const int mapId = globalId(0,i);
// in local coordinates
const Eigen::Vector3f readPt = newMap->features.col(readId).head(3);
const Eigen::Vector3f mapPt = mapLocalROI.features.col(i).head(3);
const Eigen::Vector3f mapPt_n = mapPt.normalized();
const float delta = (readPt - mapPt).norm();
const float d_max = eps_a * readPt.norm();
const Eigen::Vector3f normal_map = viewOn_normals_map.col(mapId);
// Weight for dynamic elements
const float w_v = eps + (1 - eps)*fabs(normal_map.dot(mapPt_n));
const float w_d1 = eps + (1 - eps)*(1 - sqrt(dists(i))/maxAngle);
const float offset = delta - eps_d;
float w_d2 = 1;
if(delta < eps_d || mapPt.norm() > readPt.norm())
{
w_d2 = eps;
}
else
{
if (offset < d_max)
{
w_d2 = eps + (1 - eps )*offset/d_max;
}
}
float w_p2 = eps;
if(delta < eps_d)
{
w_p2 = 1;
}
else
{
if(offset < d_max)
{
w_p2 = eps + (1 - eps)*(1 - offset/d_max);
}
}
// We don't update point behind the reading
if((readPt.norm() + eps_d + d_max) >= mapPt.norm())
{
const float lastDyn = viewOnProbabilityDynamic(0,mapId);
const float lastStatic = viewOnProbabilityStatic(0, mapId);
const float c1 = (1 - (w_v*(1 - w_d1)));
const float c2 = w_v*(1 - w_d1);
//Lock dynamic point to stay dynamic under a threshold
if(lastDyn < maxDyn)
{
viewOnProbabilityDynamic(0,mapId) = c1*lastDyn + c2*w_d2*((1 - alpha)*lastStatic + beta*lastDyn);
viewOnProbabilityStatic(0, mapId) = c1*lastStatic + c2*w_p2*(alpha*lastStatic + (1 - beta)*lastDyn);
}
else
{
viewOnProbabilityStatic(0,mapId) = eps;
viewOnProbabilityDynamic(0,mapId) = 1-eps;
}
// normalization
const float sumZ = viewOnProbabilityDynamic(0,mapId) + viewOnProbabilityStatic(0, mapId);
assert(sumZ >= eps);
viewOnProbabilityDynamic(0,mapId) /= sumZ;
viewOnProbabilityStatic(0,mapId) /= sumZ;
//viewDebug(0,mapId) =viewOnProbabilityDynamic(0, mapId);
//viewDebug(0,mapId) = w_d2;
// Refresh time
viewOn_Msec_map(0,mapId) = viewOn_Msec_overlap(0,readId);
viewOn_sec_map(0,mapId) = viewOn_sec_overlap(0,readId);
viewOn_nsec_map(0,mapId) = viewOn_nsec_overlap(0,readId);
}
}
}
// Compute density
const int mapLocalROIPts = mapLocalROI.features.cols();
cout << "build first kdtree with " << mapLocalROIPts << endl;
// build and populate NNS
std::shared_ptr<NNS> kdtree2;
kdtree2.reset( NNS::create(mapLocalROI.features, mapLocalROI.features.rows() - 1, NNS::KDTREE_LINEAR_HEAP, NNS::TOUCH_STATISTICS));
PM::Matches matches_overlap(
Matches::Dists(1, newMapPts),
Matches::Ids(1, newMapPts)
);
kdtree2->knn(newMap->features, matches_overlap.ids, matches_overlap.dists, 1, 0, NNS::ALLOW_SELF_MATCH, maxDistNewPoint);
//-------------- New point cloud ------------------------------
DP newMapOverlap(newMap->createSimilarEmpty());// Not used for now
PM::Matrix minDist = PM::Matrix::Constant(1,mapLocalROIPts, std::numeric_limits<float>::max());
// Reduce newMap to its none overlapping part
int ptsOut = 0;
int ptsIn = 0;
bool hasSensorNoise = mapLocalROI.descriptorExists("simpleSensorNoise") && newMap->descriptorExists("simpleSensorNoise");
if(hasSensorNoise) // Split and update point with lower noise
{
DP::View viewOn_noise_mapLocal = mapLocalROI.getDescriptorViewByName("simpleSensorNoise");
DP::View viewOn_noise_newMap = newMap->getDescriptorViewByName("simpleSensorNoise");
for (int i = 0; i < newMapPts; ++i)
{
const int localMapId = matches_overlap.ids(i);
if (matches_overlap.dists(i) == numeric_limits<float>::infinity())
{
newMap->setColFrom(ptsOut, *newMap, i);
ptsOut++;
}
else // Overlapping points
{
// Update point with lower sensor noise
if(viewOn_noise_newMap(0,i) < viewOn_noise_mapLocal(0,localMapId))
{
if(matches_overlap.dists(i) < minDist(localMapId))
{
minDist(localMapId) = matches_overlap.dists(i);
const float debug = viewDebug(0, localMapId);
const float probDyn = viewOnProbabilityDynamic(0, localMapId);
const float probStatic = viewOnProbabilityStatic(0, localMapId);
mapLocalROI.setColFrom(localMapId, *newMap, i);
viewDebug(0, localMapId) = 2;
viewOnProbabilityDynamic(0, localMapId) = probDyn;
viewOnProbabilityStatic(0, localMapId) = probStatic;
}
}
newMapOverlap.setColFrom(ptsIn, *newMap, i);
ptsIn++;
}
}
}
else // Only split newMap
{
for (int i = 0; i < newMapPts; ++i)
{
if (matches_overlap.dists(i) == numeric_limits<float>::infinity())
{
newMap->setColFrom(ptsOut, *newMap, i);
ptsOut++;
}
else // Overlapping points
{
newMapOverlap.setColFrom(ptsIn, *newMap, i);
ptsIn++;
}
}
}
// shrink the newMap to the new information
newMap->conservativeResize(ptsOut);
newMapOverlap.conservativeResize(ptsIn);
cout << "ptsOut=" << ptsOut << ", ptsIn=" << ptsIn << endl;
// Publish debug
//if (debugPub.getNumSubscribers())
//{
// debugPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*newMap, mapFrame, mapCreationTime));
//}
//no_overlap.addDescriptor("debug", PM::Matrix::Zero(1, no_overlap.features.cols()));
// Add the ROI to the none overlapping part
newMap->concatenate(mapLocalROI);
// Apply the filters only on the ROI
mapPostFilters.apply(*newMap);
// Add the rest of the map
newMap->concatenate(mapLocal);
// Transform the map back to global coordinates
*newMap = transformation->compute(*newMap, Ticp);
cout << "... end map creation" << endl;
ROS_INFO_STREAM("[TIME] New map available (" << newMap->features.cols() << " pts), update took " << t.elapsed() << " [s]");
return newMap;
}
bool Mapper::getBoundedMap(ethzasl_icp_mapper::GetBoundedMap::Request &req, ethzasl_icp_mapper::GetBoundedMap::Response &res)
{
if (!mapPointCloud)
return false;
const float max_x = req.topRightCorner.x;
const float max_y = req.topRightCorner.y;
const float max_z = req.topRightCorner.z;
const float min_x = req.bottomLeftCorner.x;
const float min_y = req.bottomLeftCorner.y;
const float min_z = req.bottomLeftCorner.z;
cerr << "min [" << min_x << ", " << min_y << ", " << min_z << "] " << endl;
cerr << "max [" << max_x << ", " << max_y << ", " << max_z << "] " << endl;
tf::StampedTransform stampedTr;
Eigen::Affine3d eigenTr;
tf::poseMsgToEigen(req.mapCenter, eigenTr);
Eigen::MatrixXf T = eigenTr.matrix().inverse().cast<float>();
//const Eigen::MatrixXf T = eigenTr.matrix().cast<float>();
cerr << "T:" << endl << T << endl;
T = transformation->correctParameters(T);
// FIXME: do we need a mutex here?
const DP centeredPointCloud = transformation->compute(*mapPointCloud, T);
DP cutPointCloud = centeredPointCloud.createSimilarEmpty();
cerr << centeredPointCloud.features.topLeftCorner(3, 10) << endl;
cerr << T << endl;
int newPtCount = 0;
for(int i=0; i < centeredPointCloud.features.cols(); i++)
{
const float x = centeredPointCloud.features(0,i);
const float y = centeredPointCloud.features(1,i);
const float z = centeredPointCloud.features(2,i);
if(x < max_x && x > min_x &&
y < max_y && y > min_y &&
z < max_z && z > min_z )
{
cutPointCloud.setColFrom(newPtCount, centeredPointCloud, i);
newPtCount++;
}
}
cerr << "Extract " << newPtCount << " points from the map" << endl;
cutPointCloud.conservativeResize(newPtCount);
cutPointCloud = transformation->compute(cutPointCloud, T.inverse());
// Send the resulting point cloud in ROS format
res.boundedMap = PointMatcher_ros::pointMatcherCloudToRosMsg<float>(cutPointCloud, mapFrame, ros::Time::now());
return true;
}
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(IOTest, loadPLY)
{
typedef PointMatcherIO<float> IO;
std::istringstream is;
is.str(
""
);
EXPECT_THROW(IO::loadPLY(is), runtime_error);
is.clear();
is.str(
"ply\n"
"format binary_big_endian 1.0\n"
);
EXPECT_THROW(IO::loadPLY(is), runtime_error);
is.clear();
is.str(
"ply\n"
"format ascii 2.0\n"
);
EXPECT_THROW(IO::loadPLY(is), runtime_error);
is.clear();
is.str(
"ply\n"
"format ascii 1.0\n"
);
EXPECT_THROW(IO::loadPLY(is), runtime_error);
is.clear();
is.str(
"ply\n"
"format ascii 1.0\n"
"element vertex 5\n"
"\n" //empty line
"property float z\n" // wrong order
"property float y\n"
"property float x\n"
"property float grrrr\n" //unknown property
"property float nz\n" // wrong order
"property float ny\n"
"property float nx\n"
"end_header\n"
"3 2 1 99 33 22 11\n"
"3 2 1 99 33 22 11\n"
"\n" //empty line
"3 2 1 99 33 22 11 3 2 1 99 33 22 11\n" // no line break
"3 2 1 99 33 22 11\n"
);
DP pointCloud = IO::loadPLY(is);
// Confirm sizes and dimensions
EXPECT_TRUE(pointCloud.features.cols() == 5);
EXPECT_TRUE(pointCloud.features.rows() == 4);
EXPECT_TRUE(pointCloud.descriptors.cols() == 5);
EXPECT_TRUE(pointCloud.descriptors.rows() == 3);
// Random value check
EXPECT_TRUE(pointCloud.features(0, 0) == 1);
EXPECT_TRUE(pointCloud.features(2, 2) == 3);
EXPECT_TRUE(pointCloud.descriptors(1, 1) == 22);
EXPECT_TRUE(pointCloud.descriptors(2, 4) == 33);
}
TEST(icpTest, icpSequenceTest)
{
DP pts0 = DP::load(dataPath + "cloud.00000.vtk");
DP pts1 = DP::load(dataPath + "cloud.00001.vtk");
DP pts2 = DP::load(dataPath + "cloud.00002.vtk");
PM::TransformationParameters Ticp = PM::Matrix::Identity(4,4);
PM::ICPSequence icpSequence;
std::ifstream ifs((dataPath + "default.yaml").c_str());
icpSequence.loadFromYaml(ifs);
EXPECT_FALSE(icpSequence.hasMap());
DP map = icpSequence.getInternalMap();
EXPECT_EQ(map.getNbPoints(), 0u);
EXPECT_EQ(map.getHomogeneousDim(), 0u);
map = icpSequence.getMap();
EXPECT_EQ(map.getNbPoints(), 0u);
EXPECT_EQ(map.getHomogeneousDim(), 0u);
icpSequence.setMap(pts0);
map = icpSequence.getInternalMap();
EXPECT_EQ(map.getNbPoints(), pts0.getNbPoints());
EXPECT_EQ(map.getHomogeneousDim(), pts0.getHomogeneousDim());
Ticp = icpSequence(pts1);
map = icpSequence.getMap();
EXPECT_EQ(map.getNbPoints(), pts0.getNbPoints());
EXPECT_EQ(map.getHomogeneousDim(), pts0.getHomogeneousDim());
Ticp = icpSequence(pts2);
map = icpSequence.getMap();
EXPECT_EQ(map.getNbPoints(), pts0.getNbPoints());
EXPECT_EQ(map.getHomogeneousDim(), pts0.getHomogeneousDim());
icpSequence.clearMap();
map = icpSequence.getInternalMap();
EXPECT_EQ(map.getNbPoints(), 0u);
EXPECT_EQ(map.getHomogeneousDim(), 0u);
}
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;
}
