I am learning how to use ICP with PCL. As such I wrote a function that reads a series of .pcd files from a given folder.
//Reads all PCD files in a folder. Specify the path without the last '/' and the number on the last and first file: xxx.pcb.
//Returns null if something goes wrong. Otherwise returns a single global pointcloud with each subcloud colour coded.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr LoadPCDFiles(std::string FolderPath, int FinalFile, int FirstFile)
{
//Initialize some variables we need.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Global(new pcl::PointCloud<pcl::PointXYZRGB>());
srand(time(NULL));
int FileIter = FirstFile;
std::string BasePathString = FolderPath + "/%.3d.pcd";
Eigen::Matrix4f AccumulatedTransformationMatrix = Eigen::Matrix4f::Identity();
while (FileIter <= FinalFile) {
char buffer[260];
std::sprintf(buffer, BasePathString.c_str(), FileIter);
std::string path(buffer);
cloud = ReadPCDFile(path, std::rand() % 256, std::rand() % 256, std::rand() % 256);
if (cloud == NULL) return NULL;//Error
//pcl::transformPointCloud<pcl::PointXYZRGB>(*cloud, *cloud, Eigen::Vector3f(cloud->sensor_origin_.x(), cloud->sensor_origin_.y(), cloud->sensor_origin_.z()), cloud->sensor_orientation_);
//pcl::transformPointCloud<pcl::PointXYZRGB>(*cloud, *cloud, AccumulatedTransformationMatrix);
int AlignmentIter = 0; //The number of alignment attemps we have made.
int MaxAlignment = 25; //Maax number of attempts allowed.
float LastFitness = VTK_FLOAT_MAX;
bool hasConverged = false;
std::cout << "Loaded PCD: " << path << std::endl;
if (FileIter != FirstFile) {
while (AlignmentIter < MaxAlignment && LastFitness > 0.001) {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Temp(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
icp.setInputSource(cloud);
icp.setInputTarget(Global);
icp.align(*Temp);
float CurrentFitness = icp.getFitnessScore();
hasConverged = icp.hasConverged();
std::cout << "Alignment value: " << CurrentFitness << std::endl;
if (abs(LastFitness - CurrentFitness) < 0.0001) { break; }
cloud = Temp;
LastFitness = CurrentFitness;
AlignmentIter++;
}
//AccumulatedTransformationMatrix = AccumulatedTransformationMatrix * icp.getFinalTransformation();
if (hasConverged) {
*Global = *Global + *cloud;
std::cout << "Updated global cloud: " << (*Global).size() << std::endl;
} else {
std::cout << "Unable to align the clouds." << std::endl;
}
} else {
*Global = *cloud;
std::cout << "First subcloud added." << std::endl;
}
FileIter++;
}
std::cout << "Merged clouds." << std::endl;
return Global;
}
However, this produces some very messy clouds.
This had a fitness value of just over 0.052.
This had fitness values in the range 0.0030 - 0.0856. All values.
Each cloud I have given a random colour to highlight what cloud ended up where.
The ReadPCDFile() function simply reads the file specified and adds the random colour. The commented out lines of code is simply my desperate messing with some ideas, but ultimately they all failed.
This is an example of one of the PCD files.
# .PCD v.7 - Point Cloud Data file format
VERSION .7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH 13509
HEIGHT 1
VIEWPOINT 7.9950223 0.60561943 0.4050533 - 0.3022367 0.019049812 0.95157933 0.052790195
POINTS 13509
DATA ascii
- 1.0610341 - 0.55464476 1.933659
- 1.0921015 - 0.5514014 1.9721884
- 1.0254035 - 0.55889213 1.922963
- 1.0736096 - 0.54884547 1.9840248
- 1.1002594 - 0.5510374 1.9640691
- 1.05391 - 0.556308 1.9436798
- 1.0801263 - 0.5577761 1.9309663
- 1.1306747 - 0.54748887 1.9968926
- 0.98958766 - 0.55052584 1.926852
.
.
.
I hope you can help. Do not hesitate to ask for any info that I can provide.
Thank you very much for your help.
