I tried to use ICP algorithm on PCL with simple way, but it returns transform matrix which only has zero elements.
Environment:
Windows10 + VS2019
PCL 1.10.1 All-in-one
Code:
pcl::PointCloud<pcl::PointXYZ>::Ptr src_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile("src.pcd", *src_ptr);
pcl::PointCloud<pcl::PointXYZ>::Ptr dst_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile("dst.pcd", *dst_ptr);
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setMaximumIterations(1);
icp.setInputSource(src);
icp.setInputTarget(dst);
pcl::PointCloud<pcl::PointXYZ> Final;
Eigen::Matrix4f guess;
icp.align(Final, guess);
std::cout << guess << std::endl;
Then, output is:
1.46937e-39 4.2039e-45 1.52256e-36 9.80909e-45
0 5.51013e-40 0 0
5.73972e-42 8.40779e-45 1.84388e-40 1.43901e-36
0 -4.11424e-38 0 4.59163e-41
src.pcd is here
https://drive.google.com/file/d/1bFrrdPSCw4s4y2sFv_LvigtW_fNOFxfv/view?usp=sharing
dst.pcd is here
https://drive.google.com/file/d/1hQsv38P5J7MSc8VLs10g6Ue30sSTQPOm/view?usp=sharing
I appreciate if you give me any advise
I misunderstood the usage of align().
I thought the second argument "guess" is a transformation matrix to fit source point cloud to target, but it was for initial position of source.
Therefore, the correct usage is:
pcl::PointCloud<pcl::PointXYZ> Final;
Eigen::Matrix4f guess;
icp.align(Final);
guess = icp.getFinalTransformation();
std::cout << guess << std::endl;
Related
I'm currently working with a program for predicting the locations of satellites in real-time. Something similar to this. The underlying library uses system time as input.
time_t now(time(0));
This program accurately predicts the real-time position of satellites when I run it on a C++ console application using Qt Creator.
The problem is when I use it in a fully-fledged Qt Gui application with a QApplication object in the main function. In the program, the prediction function is periodically by the timer event function. That way I update the positions every 2 seconds. Unfortunately, The output doesn't match (either on the GUI or when I print it). It is like the orbital propagator is using a different time when calculating the satellite positions.
void TrackingManager::timerEvent(QTimerEvent *event)
{
int nNumSats = m_Satellites.size() ;
//std::cout << __func__ << " - Number of satellites = " << nNumSats << std::endl;
std::vector<SatPosition> vSatPositions;
if (nNumSats >= 0)
{
time_t now(time(0));
std::cout << __func__ << "time(0) = " << asctime(gmtime(&now)) << std::endl;
for(int i = 0; i < nNumSats; i++)
{
// Get satellite names and calculate position, altitude etc
SatPosition spPos;
GetInstantPredict(m_Satellites[i], now, spPos);
vSatPositions.push_back(spPos);
}
emit UpdateSatPosition(vSatPositions);
}
}
Even more confusing, the program works fine when I run the debugger (GDB on Ubuntu). It is as if GDB somehow manages to "fix" the problem. Does this make any sense?
When I use SACSegmentation to find all the perpendicular planes of the floor normal vector, I always getting strange planes. The images show the input point cloud and SACSegmentation results.. . .
I have played around with the setDistanceThreshold value, and the bigger
number it is the larger strange inlier segment I have. However, I alway get the wrong
planes. I also tried to turn on and off the setOptimizeCoefficients, as well as bring the point cloud closer to origin. I even tried with different reasonable ModelTypes and MethodTypes, such as SACMODEL_NORMAL_PLANE and SACMODEL_NORMAL_PARALLEL_PLANE. However, none of them fix the issue. Could anyone provide some suggestions? Thanks in advance.
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloudPCL2);
sor.setLeafSize (0.01, 0.01, 0.01);
sor.filter (*cloudPCL2_result);
pcl::fromPCLPointCloud2 (*cloudPCL2_result, *cloud_filtered);
cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << endl;
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (2000);
seg.setDistanceThreshold (0.07);
seg.setAxis(normal);
seg.setEpsAngle(pcl::deg2rad(20.0));
int i = 0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.2 * nr_points) {
seg.setInputCloud(cloud_filtered);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0) {
cerr << "Could not estimate a planar model for the given dataset." << endl;
break;
}
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*cloudOutput);
stringstream ss;
ss << "plane_" << i << ".pcd";
writer.write<pcl::PointXYZRGB>(ss.str(), *cloudOutput, false);
extract.setNegative(true);
extract.filter(*cloudTmp);
cerr << "PointCloud representing the planar component: " <<
cloudTmp->width * cloudTmp->height << " data points." << endl;
cloud_filtered.swap(cloudTmp);
i++;
}
I expect the SACSegmentation will only find the ground plane and ignore all the points on the wall, but instead I got these strips of the wall along with the correct ground plane.
From your code+images, I can see that the first RANSAC returns you the floor, then the wall that is parallel to the floor. What you want is the wall that is perpendicular to the floor! So, the quickest way to get it is remove the parallel restriction there and go with the pcl::SACMODEL_PLANE after you got your first ransac result. e.g.
...
while (...) {
...
seg.segment(*inliers, *coefficients);
// RESET THE RANSAC MODEL TYPE
seg.setModelType (pcl::SACMODEL_PLANE);
Following your starting code, you would first get a floor with your current axis restriction, then after removing the restriction, you would be able to get a lot of better result.
i am new to C++ and PCL. i wish to save the values of pointer in while loop and wanted to display the saved one . Here is part of my code . Please guide how to save the value of "coefficients->values[0] ,coefficients->values[1], coefficients->values[2], coefficients->values[3]" in an array each time loop runs.
// While 20% of the original cloud is still there
while (cloud_filtered->points.size () > 0.20 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
}
I am assuming that you are following this example code since the snippet you added in your question is almost to same. If this is the case, then you can declare a std::vector<pcl::ModelCoefficients> just before the while loop and push the coefficients into that like
std::vector<pcl::ModelCoefficients> coeffs;
while(...){
...
coeffs.push_back(*coefficients);
}
Also check the documentation for pcl::ModelCoefficients here which is nothing but a header and a vector of floats. Note that defining the coeffs as a vector of shared pointers and pushing pointers to the coefficients will not work in this case since previously pushed coefficients will be overwritten by seg.segment(*inliers, *coefficients);.
What is the usual way to do math, addition, subtraction, on PCL (Point Cloud Library) data types, i.e. PointXYZ? There don't seem to be operators defined even for the basics.
I thought maybe the PCL way was to convert to Eigen vectors, but there doesn't seem to be a constructor for that either.
For anyone who wants to do basic math with PointXYZ, here a quick example:
pcl::PointXYZ a(0, 1, 2), b(10, 20, 30), c;
c.getArray3fMap() = a.getArray3fMap() + b.getArray3fMap();
std::cout << "c=" << c << std::endl;
//c=(10,21,32)
c.getArray3fMap() = a.getArray3fMap() * b.getArray3fMap();
std::cout << "c=" << c << std::endl;
//c=(0,20,60)
Maybe there is a better way but at least it works.
I am a C++ and OpenCL noob. On page 38 of the OpenCL spec there is a list of arguments you can supply to clGetDeviceInfo to get all sorts of information. The C++ wrapper seems to offer far less information. See page 5 of the C++ wrapper. Maybe I have just not read enough to know how to use these functions properly.
This is working fine for me but I would like to be able to get all of the data listed in the first link.
for(int i = 0; i < devices.size(); i++) {
string deviceName, builtInKernels;
cl::vector<size_t> maxWO;
devices[i].getInfo(CL_DEVICE_NAME, &deviceName);
devices[i].getInfo(CL_DEVICE_BUILT_IN_KERNELS, &builtInKernels);
cout << "DEVICE_NAME - " << deviceName << endl;
cout << "DEVICE_BUILT_IN_KERNELS - " << builtInKernels << endl;
cout << "DEVICE_MAX_WORK_ITEMS - " << maxWO[0] << endl;
}
It looks to me like the purpose of the table you mention is for showing those items where the C++ return value differs from the C API. Items not listed work the same in both APIs, apparently: "Table 4.3 of the OpenCL Specification Version 1.2 specifies the information that can be queried. The table below lists cl_device_info values that differ in return type between the OpenCL C API and the OpenCL C++ API."