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.
Related
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;
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'm trying to get into the fascinating world of Common Lisp embedded in C++. My problem is that I can't manage to read and print from c++ a string returned by a lisp function defined in ECL.
In C++ I have this function to run arbitrary Lisp expressions:
cl_object lisp(const std::string & call) {
return cl_safe_eval(c_string_to_object(call.c_str()), Cnil, Cnil);
}
I can do it with a number in this way:
ECL:
(defun return-a-number () 5.2)
read and print in C++:
auto x = ecl_to_float(lisp("(return-a-number)"));
std::cout << "The number is " << x << std::endl;
Everything is set and works fine, but I don't know to do it with a string instead of a number. This is what I have tried:
ECL:
(defun return-a-string () "Hello")
C++:
cl_object y = lisp("(return-a-string)");
std::cout << "A string: " << y << std::endl;
And the result of printing the string is this:
A string: 0x3188b00
that I guess is the address of the string.
Here it is a capture of the debugger and the contents of the y cl_object. y->string.self type is an ecl_character.
Debug
(Starting from #coredump's answer that the string.self field provides the result.)
The string.self field is defined as type ecl_character* (ecl/object.h), which appears to be given in ecl/config.h as type int (although I suspect this is slightly platform dependent). Therefore, you will not be able to just print it as if it was a character array.
The way I found worked for me was to reinterpret it as a wchar_t (i.e. a unicode character). Unfortunately, I'm reasonably sure this isn't portable and depends both on how ecl is configured and the C++ compiler.
// basic check that this should work
static_assert(sizeof(ecl_character)==sizeof(wchar_t),"sizes must be the same");
std::wcout << "A string: " << reinterpret_cast<wchar_t*>(y->string.self) << std::endl;
// prints hello, as required
// note the use of wcout
The alternative is to use the lisp type base-string which does use char (base-char in lisp) as its character type. The lisp code then reads
(defun return-a-base-string ()
(coerce "Hello" 'base-string))
(there may be more elegant ways to do the conversion to base-string but I don't know them).
To print in C++
cl_object y2 = lisp("(return-a-base-string)");
std::cout << "Another: " << y2->base_string.self << std::endl;
(note that you can't mix wcout and cout in the same program)
According to section 2.6 Strings of The ECL Manual, I think that the actual character array is found by accessing the string.self field of the returned object. Can you try the following?
std::cout << y->string.self << std::endl;
std::string str {""};
cl_object y2 = lisp("(return-a-base-string)");
//get dimension
int j = y2->string.dim;
//get pointer
ecl_character* selv = y2->string.self;
//do simple pointer addition
for(int i=0;i<j;i++){
str += (*(selv+i));
}
//do whatever you want to str
this code works when the string is build from ecl_characters
from the documentation:
"ECL defines two C types to hold its characters: ecl_base_char and ecl_character.
When ECL is built without Unicode, they both coincide and typically match unsigned char, to cover the 256 codes that are needed.
When ECL is built with Unicode, the two types are no longer equivalent, with ecl_character being larger.
For your code to be portable and future proof, use both types to really express what you intend to do."
On my system the return-a-base-string is not needed, but I think it could be good to add for compatibility. I use the (ecl) embedded CLISP 16.1.2 version.
The following piece of code reads a string from lisp and converts to C++ strings types - std::string and c-string- and store them on C++ variables:
// strings initializations: string and c-string
std::string str2 {""};
char str_c[99] = " ";
// text read from clisp, whatever clisp function that returns string type
cl_object cl_text = lisp("(coerce (text-from-lisp X) 'base-string)");
//cl_object cl_text = lisp("(text-from-lisp X)"); // no base string conversions
// catch dimension
int cl_text_dim = cl_text->string.dim;
// complete c-string char by char
for(int ind=0;i<cl_text_dim;i++){
str_c[i] = ecl_char(cl_text,i); // ecl function to get char from cl_object
}
str_c[cl_text_dim] ='\0'; // end of the c-string
str2 = str_c; // get the string on the other string type
std::cout << "Dim: " << cl_ text_dim << " C-String var: " << str_c() << " String var << str2 << std::endl;
It is a slow process as passing char by char but it is the only way by the moment I know. Hope it helps. Greetings!
I have a Qt (4.7) program that takes a QByteArray and should break it into a list of QVariants, after using a parser to transform it into a QVariant. The problems seem to arise when I try to use the toList() function. I have something similar to this:
QVariant var = //whatever the value passed in is...
std::cout << "Data = " << var.toString().toStdString() << std::endl;
QList<QVariant> varlist = var.toList();
std::cout << "List Size = " << varlist.size() << std::endl;
which would return this:
Data = variant1 variant2 variant3
Size = 0
where the size should clearly be 3. Does anyone have an idea what I may be doing wrong? thanks!
The documentation of toList() says:
Returns the variant as a QVariantList if the variant has userType() QMetaType::QVariantList or QMetaType::QStringList; otherwise returns an empty list.
My guess is, your variant's userType() is neither of those two.
You probably need to construct your variant differently, e.g.
QVariantList list;
list << variant1 << variant2 << variant3;
QVariant var = list;
So, I have no idea why, but when I put the command I specified above into a separate function, ie QList<QVariant> myClass::ToList(QVariant v){return v.toList();}, and then call varlist = myClass::ToList(v), it works. Still doesn't the original way, but this way it's fine. Guess I'll just chalk it up to one of the quirks of Qt...
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."