std::sample not found in Point Cloud Project - c++

I am trying to implement RANSAC in a PCL project. I am getting the following 3 errors. For the first error, I understand that C++17 needs to be enabled. However, I have no clue what the other 2 errors mean.
I have tried using std::experimental::sample but that doesn't
work either.
#include "../../render/render.h"
#include <unordered_set>
#include "../../processPointClouds.h"
// using templates for processPointClouds so also include .cpp to help linker
#include "../../processPointClouds.cpp"
#include <random>
#include <algorithm>
#include <experimental/algorithm>
std::unordered_set<int> Ransac(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int maxIterations, float distanceTol)
{
std::unordered_set<int> inliersResult;
srand(time(NULL));
while(maxIterations--) {
std::unordered_set<int> inliers; // store the inliers here in each iteration
int sample_size {2};
std::vector<pcl::PointXYZ> sample (sample_size);
std::sample(cloud->points.begin(), cloud->points.end(), std::back_inserter(sample),
sample_size, std::mt19937{std::random_device{}()});
// fit a line ax + by+ c = 0 to the points in sample using (y0−y1)x+(x1−x0)y+(x0∗y1−x1∗y0)=0
double x0 {sample.at(0).x}, y0 {sample.at(0).y}, x1 {sample.at(1).x}, y1 {sample.at(1).y};
double a {y0 - y1};
double b {x1 - x0};
double c {x0 * y1 - x1 * y0};
// find the distance to this line for all points in the cloud. If the distance is less than
// distanceTol at the index of the point to inliers
for(auto& point: cloud->points) {
double distance = fabs(a * point.x - b * point.y - c) / sqrt(std::pow(a, 2) + std::pow(b, 2));
if (distance < distanceTol) {
// add the index of the point to inliers
auto index = std::find(cloud->points.begin(), cloud->points.end(), point);
inliers.emplace(index);
}
}
if (inliers.size() > inliersResult.size()) { inliersResult = inliers; }
}
return inliersResult;
}
Here are the errors:
error: no member named 'sample' in namespace 'std'
std::sample(cloud->points.begin(), cloud->points.end(), std::back_inserter(sample),
/Library/Developer/CommandLineTools/usr/include/c++/v1/algorithm:999:22: error: invalid operands to binary expression ('pcl::PointXYZ' and 'const pcl::PointXYZ')
if (*__first == __value_)
/Library/Developer/CommandLineTools/usr/include/c++/v1/memory:1825:31: error: no viable conversion from 'std::__1::__wrap_iter<pcl::PointXYZ *>' to 'int'
::new((void*)__p) _Up(_VSTD::forward<_Args>(__args)...);

Related

Armadillo - no member named i in matrix expression

According to Armadillo docs:
.i()
Member function of any matrix expression
Provides an inverse of the matrix expression
...
However, when I try to compile this snippet:
#include <armadillo>
#include <iostream>
arma::sp_mat linReg(arma::sp_mat X, arma::sp_mat Y) {
return (X.t() * X).i() * X.t() * Y;
}
int main() {
arma::sp_mat X = arma::sprandu(1000, 10, 0.3);
arma::sp_mat y = arma::sprandu(1000, 10, 0.3);
std::cout << linReg(X,y).t() << std::endl;
}
I get the following error
lreg.cpp: In function ‘arma::sp_mat linReg(arma::sp_mat,
arma::sp_mat)’: lreg.cpp:6:24: error: ‘arma::enable_if2<true, const
arma::SpGluearma::SpOp<arma::SpMat<double, arma::spop_htrans>,
arma::SpMat, arma::spglue_times> >::result’ {aka ‘const class
arma::SpGluearma::SpOp<arma::SpMat<double, arma::spop_htrans>,
arma::SpMat, arma::spglue_times>’} has no member named ‘i’
6 | return (X.t() * X).i() * X.t() * Y;
|
I already tried with mat and it works fine. Any clue why it's not working with sparse matrix? And if so, how can we calculate the inverse of a sparse matrix?
Taking the inverse of a sparse matrix is often not desired as you end up with a dense matrix. Often the explicit inverse is not required.
Instead of taking the inverse here, maybe treat the problem as solving a system of linear equations. Then reformulate using solve() or spsolve(). Below is an untested example for demonstrating the general approach:
arma::mat linReg(const arma::sp_mat& X, const arma::sp_mat& Y) {
arma::sp_mat A = X.t() * X;
arma::mat B = arma::mat(X.t() * Y); // convert to dense matrix
arma::mat result;
bool ok = arma::spsolve(result, A, B);
if(ok == false) {
// handle failure here
}
return result;
}

‘abs’ was not declared in this scope

I''m working on GNURadio. Included math.h in my program but still get an error saying that abs was not declared in this scope.
Here is my header file.
#ifndef INCLUDED_SLM_H
#define INCLUDED_SLM_H
#include "candidate_t.h"
namespace gr {
namespace uwspr {
class SLM {
public:
// returns frequency drift according to the straight line model
float slmFrequencyDrift(mode_nonlinear m_nl, float cf, float t);
// generator of trajectory parameters for the straight line model
bool slmGenerator(mode_nonlinear *m_nl);
// initialize the generator
void slmGeneratorInit();
// index of current instances
int current;
SLM(){};
~SLM(){};
};
} // namespace uwspr
} // namespace gr
#endif /* INCLUDED_SLM_H */
Here is my code snippet for main function.
#include "slm.h"
#include <math.h>
#ifndef DEBUG
#define DEBUG 1 // set debug mode
#endif
#include "debugmacro.h"
namespace gr {
namespace uwspr {
// Returns frequency drift (in Hz) due to the Doppler effect according to
// the straight line model, described in paper:
float SLM::slmFrequencyDrift(mode_nonlinear m_nl, float cf, float t)
// m_nl = trajectory parameters
// t = time, in seconds
{
const float c = 1500.0; // sound speed (m/s)
// sign of velocity vector
float Sign = (((m_nl.V1 * t + m_nl.p1) * m_nl.V1 +
(m_nl.V2 * t + m_nl.p2) * m_nl.V2) > 0) *
2 -
1;
//
double numerator = abs(m_nl.V1 * (m_nl.V1 * t + m_nl.p1) +
m_nl.V2 * (m_nl.V2 * t + m_nl.p2));
// norm of connectiong vector (Eq. 16)
double denominator =
sqrt(pow(m_nl.V1 * t + m_nl.p1, 2) + pow(m_nl.V2 * t + m_nl.p2, 2));
if (denominator == 0) {
return 0.0;
} else {
// return -Sign;
return -Sign * numerator / denominator * cf / c;
}
}
// Generators of trajectory parameters for the straight line model
bool SLM::slmGenerator(mode_nonlinear *m_nl) {
// control variables for init velocity
const double V1_min = -2, V1_max = 2, V1_step = 1;
const int nV1 = (V1_max - V1_min) / V1_step + 1;
const double V2_min = -2, V2_max = 2, V2_step = 1;
const int nV2 = (V2_max - V2_min) / V2_step + 1;
// control variables for init position on y-axis
const int p2_min = 50, p2_max = 850, p2_step = 200;
const int np2 = (p2_max - p2_min) / p2_step + 1;
// number of generated instances
const int last = nV1 * nV2 * np2;
// indices into the instances
static int ip2, iV1, iV2;
if (current == 0) {
ip2 = 0;
iV1 = 0;
iV2 = 0;
}
if (current < last) { // not generated all instances?
if (ip2 >= np2) {
ip2 = 0; // reset index into the list positions
iV1++; // next horizontal velocity
if (iV1 >= nV1) {
iV1 = 0; // reset index into the list horiz. velocities
iV2++; // next vertical velocity
}
}
// map horizontal velocity index to horizontal velocity (m/s)
m_nl->V1 = iV1 * V1_step + V1_min;
// map vertical velocity index to vertical velocity (m/s)
m_nl->V2 = iV2 * V2_step + V2_min;
// init coordinate on x-axis is always null
m_nl->p1 = 0;
// map y-axis coordinate index to y-axis coordinate (m)
m_nl->p2 = ip2 * p2_step + p2_min;
ip2++; // next position on y-axis
current++; // index of next instance
return true;
} else {
return false; // reach the end
}
}
void SLM::slmGeneratorInit() { current = 0; }
} /* namespace uwspr */
} /* namespace gr */
abs is not defined in <math.h>. It can be found in <stdlib.h> and it is also defined as std::abs in <cmath>.
Surprisingly, the abs function is defined in <stdlib.h> rather than <math.h>. (At least, that's the case since C++17). Try including <stdlib.h> or <cstdlib> and see if that fixes things.
Hope this helps!
The problem is that you were using the C header file, math.h. The C standard already defined (the global scope) int ::abs(int) to take an int and return an int, and this is defined in stdlib.h. The float version, double ::fabs(double) is defined in math.h, which is the one you need to use.
C++ has overloads, so std::abs has double std::abs(double) and int std::abs(int) overloads. So you could just use the C++ header, cmath, and use std::abs (or std::fabs to prevent conversions) instead.

Eigen LLT (Cholesky) fails, while SVD works

I'm trying to reproduce some numpy code on Gaussian Processes (from here) using Eigen. Basically, I need to sample from a multivariate normal distribution:
samples = np.random.multivariate_normal(mu.ravel(), cov, 1)
The mean vector is currently zero, while the covariance matrix is a square matrix generated via the isotropic squared exponential kernel:
sqdist = np.sum(X1**2, 1).reshape(-1, 1) + np.sum(X2**2, 1) - 2 * np.dot(X1, X2.T)
return sigma_f**2 * np.exp(-0.5 / l**2 * sqdist)
I can generate the covariance matrix just fine for now (it can probably be cleaned but for now it's a POC):
Matrix2D kernel(const Matrix2D & x1, const Matrix2D & x2, double l = 1.0, double sigma = 1.0) {
auto dists = ((- 2.0 * (x1 * x2.transpose())).colwise()
+ x1.rowwise().squaredNorm()).rowwise() +
+ x2.rowwise().squaredNorm().transpose();
return std::pow(sigma, 2) * ((-0.5 / std::pow(l, 2)) * dists).array().exp();
}
However, my problems start when I need to sample the multivariate normal.
I've tried using the solution proposed in this accepted answer; however, the decomposition only works with covariance matrices of size up to 30x30; more than that and LLT fails to decompose the matrix. The alternative version provided in the answer also does not work, and creates NaNs. I tried LDLT as well but it also breaks (D contains negative values, so sqrt gives NaN).
Then, I got curious, and I looked into how numpy does this. Turns out the numpy implementation uses SVD decomposition (with LAPACK), rather than Cholesky. So I tried copying their implementation:
// SVD on the covariance matrix generated via kernel function
Eigen::BDCSVD<Matrix2D> solver(covs, Eigen::ComputeFullV);
normTransform = (-solver.matrixV().transpose()).array().colwise() * solver.singularValues().array().sqrt();
// Generate gaussian samples, "randN" is from the multivariate StackOverflow answer
Matrix2D gaussianSamples = Eigen::MatrixXd::NullaryExpr(1, means.size(), randN);
Eigen::MatrixXd samples = (gaussianSamples * normTransform).rowwise() + means.transpose();
The various minuses are me trying to exactly reproduce numpy's results.
In any case, this works perfectly fine, even with large dimensions. I was wondering why Eigen is not able to do LLT, but SVD works. The covariance matrix I use is the same. Is there something I can do to simply use LLT?
EDIT: Here is my full example:
#include <iostream>
#include <random>
#include <Eigen/Cholesky>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
using Matrix2D = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor | Eigen::AutoAlign>;
using Vector = Eigen::Matrix<double, Eigen::Dynamic, 1>;
/*
We need a functor that can pretend it's const,
but to be a good random number generator
it needs mutable state.
*/
namespace Eigen {
namespace internal {
template<typename Scalar>
struct scalar_normal_dist_op
{
static std::mt19937 rng; // The uniform pseudo-random algorithm
mutable std::normal_distribution<Scalar> norm; // The gaussian combinator
EIGEN_EMPTY_STRUCT_CTOR(scalar_normal_dist_op)
template<typename Index>
inline const Scalar operator() (Index, Index = 0) const { return norm(rng); }
};
template<typename Scalar> std::mt19937 scalar_normal_dist_op<Scalar>::rng;
template<typename Scalar>
struct functor_traits<scalar_normal_dist_op<Scalar> >
{ enum { Cost = 50 * NumTraits<Scalar>::MulCost, PacketAccess = false, IsRepeatable = false }; };
} // end namespace internal
} // end namespace Eigen
Matrix2D kernel(const Matrix2D & x1, const Matrix2D & x2, double l = 1.0, double sigma = 1.0) {
auto dists = ((- 2.0 * (x1 * x2.transpose())).colwise() + x1.rowwise().squaredNorm()).rowwise() + x2.rowwise().squaredNorm().transpose();
return std::pow(sigma, 2) * ((-0.5 / std::pow(l, 2)) * dists).array().exp();
}
int main() {
unsigned size = 50;
unsigned seed = 1;
Matrix2D X = Vector::LinSpaced(size, -5.0, 4.8);
Eigen::internal::scalar_normal_dist_op<double> randN; // Gaussian functor
Eigen::internal::scalar_normal_dist_op<double>::rng.seed(seed); // Seed the rng
Vector means = Vector::Zero(X.rows());
auto covs = kernel(X, X);
Eigen::LLT<Matrix2D> cholSolver(covs);
// We can only use the cholesky decomposition if
// the covariance matrix is symmetric, pos-definite.
// But a covariance matrix might be pos-semi-definite.
// In that case, we'll go to an EigenSolver
Eigen::MatrixXd normTransform;
if (cholSolver.info()==Eigen::Success) {
std::cout << "Used LLT\n";
// Use cholesky solver
normTransform = cholSolver.matrixL();
} else {
std::cout << "Broken\n";
Eigen::BDCSVD<Matrix2D> solver(covs, Eigen::ComputeFullV);
normTransform = (-solver.matrixV().transpose()).array().colwise() * solver.singularValues().array().sqrt();
}
Matrix2D gaussianSamples = Eigen::MatrixXd::NullaryExpr(1, means.size(), randN);
Eigen::MatrixXd samples = (gaussianSamples * normTransform).rowwise() + means.transpose();
return 0;
}

Creating simple matrices with Eigen?

I'm using the Eigen library to create and manipulate some matrices in C++. Eigen is installed (Ubuntu 16.04) and seems to be working. However, when I declare a matrix as part of a class in an external file and #include the necessary files, it fails. My KalmanFilter.h header file:
#include <Eigen/Dense>
using Eigen::MatrixXd;
class KalmanFilter {
public:
KalmanFilter(double, double);
double initialX, initialY;
MatrixXd m;
};
My KalmanFilter.cpp file:
#include <Eigen/Dense>
#include "KalmanFilter.h"
KalmanFilter::KalmanFilter(double inX, double inY) {
initialX = inX;
initialY = inY;
m(2, 1);
m << initialX, initialY;
}
And of course my main.cpp:
#include <Eigen/Dense>
#include "Utilities/KalmanFilter.h"
int main() {
double a, b;
a = 1.0;
b = 2.0;
KalmanFilter KF(a, b);
}
Everything compiles all right, but running it results in an assertion error:
main: /usr/local/include/Eigen/src/Core/DenseCoeffsBase.h:365: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::Index, Eigen::Index) [with Derived = Eigen::Matrix<double, -1, -1>; Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double; Eigen::Index=long int]: Assertion 'row >= 0 && rows() && col >= 0 && col < cols()' failed. Aborted.
If I put MatrixXd m(2, 1); inside my KalmanFiter.cpp file (re-declaring that it's a MatrixXd) the resulting compilation runs, but the m matrix is empty (it exists, but apparently the next line that's supposed to initialize it fails silently). I'm almost positive that Eigen is installed correctly, because declaring and initializing the same MatrixXd matrix inside my main.cpp works just fine.
What am I missing here?
m(2, 1); this doesn't do what you think it does. It doesn't create the object, it's a syntax to get the coefficient at given position (operator()), so your matrix m is empty, and you try to retrieve the element.
The syntax seems the same, but the placement makes a great deal of a difference.
You need to initialize member object in the member initialization list:
KalmanFilter::KalmanFilter(double inX, double inY) : m(2, 1) {
// ^^^^^^^
initialX = inX;
initialY = inY;
m << initialX, initialY;
}
The problem is due to the line in KalmanFilter.cpp:
m(2, 1);
That doesn't resize the matrix as I assume you assume it does. Replace it with m.resize(2, 1); and try again.

Why PCL Conditional filter return the same point cloud?

I'm working with PCL to process a point cloud in a way to end with detecting objects in the scene.
I add a custom PiontT type and it work fine with me. However, I'm struggling with the filtering algorithms in the PCL library. I tried statistical, radius, and conditional outliers removal to remove noise. The statistical did not return the results (it seems to me as if it in an infinite loop), the radius on the other hand return a cloud with size 0. and the conditional actually return the same cloud without removing any point. in both radius and statistical, I follow the example as it given but they did not work.
For now, I think the conditional removal is the most proper algorithm for me, because I want to remove any points with confidence not in the range between [0.4 - 1] . As I mentioned before that I'm using a custom point type. below is the code for the point Type (Tango3DPoitType) and the method that use conditional removal.
Tango3DPoitType.h
#define PCL_NO_PRECOMPILE
#include <pcl/point_types.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/point_cloud.h>
#include <pcl/impl/instantiate.hpp>
// Preserve API for PCL users < 1.4
#include <pcl/common/distances.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/search/organized.h>
#include <pcl/search/impl/organized.hpp>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/impl/statistical_outlier_removal.hpp>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/impl/radius_outlier_removal.hpp>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/impl/voxel_grid.hpp>
#include <pcl/filters/voxel_grid_covariance.h>
#include <pcl/filters/impl/voxel_grid_covariance.hpp>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/impl/extract_indices.hpp>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/impl/conditional_removal.hpp>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/impl/sac_segmentation.hpp>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/impl/extract_clusters.hpp>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
struct EIGEN_ALIGN16 _Tango3DPoitType
{
PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
union
{
union
{
struct
{
uint8_t b;
uint8_t g;
uint8_t r;
uint8_t a;
}; float rgb;
}; uint32_t rgba;
};
float Confidence;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW };
struct EIGEN_ALIGN16 Tango3DPoitType : public _Tango3DPoitType
{
inline Tango3DPoitType ()
{
x = y = z = 0.0f;
data[3] = 1.0f;
r = b = a = 0;
g = 255;
Confidence = 0.0f;
}
inline Tango3DPoitType (float _Confidence)
{
x = y = z = 0.0f;
data[3] = 1.0f;
r = b = a = 0;
g = 255;
Confidence = _Confidence;
}
inline Tango3DPoitType (uint8_t _r, uint8_t _g, uint8_t _b)
{
x = y = z = 0.0f;
data[3] = 1.0f;
r = _r;
g = _g;
b = _b;
a = 0;
Confidence = 0;
}
inline Eigen::Vector3i getRGBVector3i () { return (Eigen::Vector3i (r, g, b)); }
inline const Eigen::Vector3i getRGBVector3i () const { return (Eigen::Vector3i (r, g, b)); }
inline Eigen::Vector4i getRGBVector4i () { return (Eigen::Vector4i (r, g, b, 0)); }
inline const Eigen::Vector4i getRGBVector4i () const { return (Eigen::Vector4i (r, g, b, 0)); }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW };
// Adding confidence as fourth data to XYZ
POINT_CLOUD_REGISTER_POINT_STRUCT (Tango3DPoitType,
(float, x, x)
(float, y, y)
(float, z, z)
(uint32_t, rgba, rgba)
(float, Confidence, Confidence)
)
POINT_CLOUD_REGISTER_POINT_WRAPPER(Tango3DPoitType, _Tango3DPoitType)
Conditional Removal Method
void CloudDenoising(const pcl::PointCloud<Tango3DPoitType>::Ptr source,
const pcl::PointCloud<Tango3DPoitType>::Ptr target){
// build the condition
pcl::ConditionAnd<Tango3DPoitType>::Ptr ConfidenceRangeCondition (new pcl::ConditionAnd<Tango3DPoitType> ());
ConfidenceRangeCondition->addComparison (pcl::FieldComparison<Tango3DPoitType>::ConstPtr (new pcl::FieldComparison<Tango3DPoitType> ("Confidence", pcl::ComparisonOps::GT, 0.5)));
ConfidenceRangeCondition->addComparison (pcl::FieldComparison<Tango3DPoitType>::ConstPtr (new pcl::FieldComparison<Tango3DPoitType> ("Confidence", pcl::ComparisonOps::LT, 1.1)));
// build the filter
pcl::ConditionalRemoval<Tango3DPoitType> conditionalRemoval;
conditionalRemoval.setCondition (ConfidenceRangeCondition);
conditionalRemoval.setInputCloud (source);
conditionalRemoval.setKeepOrganized(true);
// apply filter
conditionalRemoval.filter (*target);
}
I want to understand is I'm doing something wrong with the point type or is it a bug in PCL library.
Thank you
You are cropping the cloud but it still leting organized.
To solve it, just remove the method .setKeepOrganized(true).