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.
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?
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;
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);.
I'm creating a music player for PC. I want to visualize the FFT of the song. I've crated an entire class that buffers 1024 points of data does the FFT and displays it (this is handled by another class). My program was developed in my laptop which uses Debian Testing x64. My work pc uses Centos 7 x64. When I compiled my program (both use Qt 5.7.0) on my work PC the FFT visualization was garbage. Snooping into my code I found that the sample type provided by QAudioBuffer from QAudioProbe was Signed (in my work PC) while it was float in my Laptop. Here is the code that is called whenever QAudioProbe emits that data has been buffered:
void SpectrumController::setAudioBuffer(QAudioBuffer buffer){
// Used to momentarily stop the process.
if (!enableBuffering) return;
// Only process stereo frames
if (buffer.format().channelCount() != 2) return;
if (buffer.format().sampleType() == QAudioFormat::SignedInt){
//qWarning() << "Signed";
QAudioBuffer::S16S *data = buffer.data<QAudioBuffer::S16S>();
bufferData(data,buffer.frameCount());
}
else if (buffer.format().sampleType() == QAudioFormat::UnSignedInt){
//qWarning() << "Unsigned";
QAudioBuffer::S16U *data = buffer.data<QAudioBuffer::S16U>();
bufferData(data,buffer.frameCount());
}
else if(buffer.format().sampleType() == QAudioFormat::Float){
//qWarning() << "Float";
QAudioBuffer::S32F *data = buffer.data<QAudioBuffer::S32F>();
bufferData(data,buffer.frameCount());
}
}
template<typename T>
void SpectrumController::bufferData(T *data, qint32 N){
for (qint32 i = 0; i < N; i++){
//if (qAbs(data[i].left) > largest){largest = qAbs(data[i].left); qDebug() << "Largest" << largest;}
//currentBuffer << ((qreal)data[i].left/(largest));
//qWarning() << "Added data" << currentBuffer.last();
currentBuffer << data[i].left;
if (datcounter < 100000){
*writer << data[i].left;
*writer << "\n";
datcounter++;
}
else if (writeFile->isOpen()){
qWarning() << "Closed file";
writeFile->close();
}
if (currentBuffer.size() == FFT_SIZE){
dataBuffer << currentBuffer;
currentBuffer.clear();
if (!isRunning) run();
}
What I ended up doing is writing, to a file, the first 100.000 points of data gathered by both my laptop and my work PC in order to plot them.
This is what I've got
What I think is that difference is in the base system's handling of the the mp3, which, in turn, is what Qt uses. I think is gstreamer. Centos uses a much older version. The plot on the right corresponds to my laptop while the plot on the left corresponds to my work pc.
Any ideas on how to fix this? Or am I just stuck with no way of accessising the raw audio data correctly?
UPDATE:
Even though this is not a Fix or anything like that, the data in the other channel (data[i].right) did have more appropiate data. I'm using the right channnel, for now.
I use Qt 5.2.0 (MSVC 2010).
I added to my form in Qt a ComboBox.
Then I want to fill it with numbers:
for (i = 0; i < n; i++){
ui->tableCombo->addItem(QString::number(i));
}
When I add a first element right in the form, it successfully adds numbers. But when I leave it empty, it throws an error:
ASSERT failure in QVector::operator[]: "index out of range"
Debugger shows that error occured right in this line. And there is no QVector across the line.
After adding qDebug().
qDebug() << "readFileToStringList: msg10";
for (i = 0; i < n; i++){
qDebug() << "readFileToStringList: msg20 i = " << i;
ui->tableCombo->addItem(QString::number(i+1));
qDebug() << "readFileToStringList: msg30";
}
qDebug() << "readFileToStringList: msg40";
I get the same result
readFileToStringList: msg10
readFileToStringList: msg20 i = 0
ASSERT failure in QVector<T>::operator[]: "index out of range", file C:\Qt\Qt5.2.0\5.2.0\mingw48_32\include/QtCore/qvector.h, line 369
I had this exact problem and couldn't figure it out for a couple hours. I realized ::addItem() was triggering the indexChanged(int) signal, which I had connected to a function that was causing an out-of-range error in a container.
I would say it was possibly the problem here too, but I'm sure the OP has moved on since then. To me it isn't exactly intuitive that the indexChanged signal would be called on insertion of new items, since it doesn't actually change the currentIndex.
Hopefully if anyone else gets tripped up this will help them!
addItem() doesn't throw that error! I'm positive it's coming from another instruction in your code.
Qt documentation has an entire section on Debugging Techniques, but if you are afraid of debuggers you can use the poor's man debugger: spread several qDebug() messages before and after the instructions you think are responsible for the problem:
qDebug() << "methodX: msg10";
for (i = 0; i < n; i++){
qDebug() << "methodX: msg20 i = " << i;
ui->tableCombo->addItem(QString::number(i));
qDebug() << "methodX: msg30";
}
qDebug() << "methodX: msg40";
If the message methodX: msg30 gets printed to the screen, means that addItem() didn't cause the error.