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.
Related
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;
}
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)...);
I'm trying to use the Spectra 3.5 Library on my Linux machine, and the SparseGenMatProd wrapper for the Matrix-Vector multiplication only seems to work when the sparse matrix is in ColMajor format. Is this normal behavior and if so, how can I fix it to take RowMajor format? I've included a basic example where the output is "Segmentation fault (core dumped)". I've gone through several other posts and the documentation, but can't seem to find an answer.
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <GenEigsSolver.h>
#include <MatOp/SparseGenMatProd.h>
#include <iostream>
using namespace Spectra;
int main()
{
// A band matrix with 1 on the main diagonal, 2 on the below-main subdiagonal,
// and 3 on the above-main subdiagonal
const int n = 10;
Eigen::SparseMatrix<double, Eigen::RowMajor> M(n, n);
M.reserve(Eigen::VectorXi::Constant(n, 3));
for(int i = 0; i < n; i++)
{
M.insert(i, i) = 1.0;
if(i > 0)
M.insert(i - 1, i) = 3.0;
if(i < n - 1)
M.insert(i + 1, i) = 2.0;
}
// Construct matrix operation object using the wrapper class SparseGenMatProd
SparseGenMatProd<double> op(M);
// Construct eigen solver object, requesting the largest three eigenvalues
GenEigsSolver< double, LARGEST_MAGN, SparseGenMatProd<double> > eigs(&op, 3, 6);
// Initialize and compute
eigs.init();
int nconv = eigs.compute();
// Retrieve results
Eigen::VectorXcd evalues;
if(eigs.info() == SUCCESSFUL)
evalues = eigs.eigenvalues();
std::cout << *emphasized text*"Eigenvalues found:\n" << evalues << std::endl;
return 0;
}
If you change line 15 to:
Eigen::SparseMatrix<double, Eigen::ColMajor> M(n, n);
it will work as expected.
Currently I'm working around this and converting my matrices to ColMajor, but I'd like to understand what's going on. Any help is much appreciated.
The API of SparseGenMatProd seems to be quite misleading. Looks like you have to specify you are dealing with row-major matrices through the second template parameter:
SparseGenMatProd<double,RowMajor> op(M);
otherwise M is implicitly converted to a temporary column-major matrix which is then stored by const reference by op but this temporary is dead right after that line of code.
I am compiling a program that uses several Eigen::MatrixXd methods, and while I get no errors when compiling it, running it I get the following error:
darwin-pi2: /usr/include/Eigen/src/Core/Assign.h:498: Derived& Eigen::DenseBase<Derived>::lazyAssign(const Eigen::DenseBase<OtherDerived>&) [with OtherDerived = Eigen::Matrix<double, -1, -1>; Derived = Eigen::Matrix<double, 15, 15, 0, 15, 15>]: Assertion `rows() == other.rows() && cols() == other.cols()' failed.
I guess it is something related to Eigen matrices, but I do not understand what Assertion rows() == other.rows() && cols() == other.cols()' failed means.
Because Eigen::MatrixXd has dimensions determined at runtime, the compile-time size checks are all disabled and deferred until runtime.
In this case, it looks like you're assigning from a dynamic-size matrix to a 15x15 one. Try double-checking and debugging the size of that dynamic one.
In matlab, the index of a matrix m starts from 1. But in eigen, it starts from 0. Show a simple example.
#include <iostream>
#include <Eigen/Dense>
using Eigen::MatrixXd;
int main()
{
MatrixXd m(2,2);
m(0,0) = 3; // INDEX starts from 0, not 1
m(1,0) = 2.5;
m(0,1) = -1;
m(1,1) = m(1,0) + m(0,1);
std::cout << m << std::endl;
}
For more information, click the docs.
I'm attempting to implement both a Vector4 class, and a Matrix4x4 class in C++ to get a better handle on the language. I've looked around, and nothing seems to have really answered the problems I've encountered, though apologies if I've missed anything.
Edit: The original error no longer seems to be occurring (It was caused by circular inclusion). However, now I'm receiving the following error:
1>main.cpp(35): error C2064: term does not evaluate to a function taking 2 arguments
I could only imagine this occurring because of my overloading of the () operator in CMatrix4x4, however it did not occur in my previous code when called from main().
Requested SSCCE case:
#include <assert.h>
#include <cmath>
#include <iostream>
class CMatrix4x4;
class CVector4
{
public:
float x, y, z, w;
CVector4();
CVector4(float, float, float, float);
~CVector4();
CVector4 operator*(CMatrix4x4&);
};
CVector4::CVector4()
{
x, y, z, w = 0;
}
CVector4::CVector4(float cx, float cy, float cz, float cw)
{
x = cx, y = cy, z = cz, w = cw;
}
//No instance of overloaded function "CVector4::operator" matches the specified type
//<error-type> m
//DOES NOT occur with forward declaration of class, only when including matrix.h
//from a separate file.
//Now causes "term does not evaluate to a function taking 2 arguments" at lines: 35-38
//Whenever I call the overloaded operator ()
CVector4 CVector4::operator*(CMatrix4x4& m)
{
CVector4 v;
v.x = x*m(0, 0) + y*m(1, 0) + z*m(2, 0) + w*m(3, 0);
v.y = x*m(0, 1) + y*m(1, 1) + z*m(2, 1) + w*m(3, 1);
v.z = x*m(0, 2) + y*m(1, 2) + z*m(2, 2) + w*m(3, 2);
v.w = x*m(0, 3) + y*m(1, 3) + z*m(2, 3) + w*m(3, 3);
return v;
}
class CMatrix4x4
{
public:
CMatrix4x4();
~CMatrix4x4();
void SetRow(int r, CVector4);
float operator()(int r, int c);
private:
float matrix4x4[4][4];
};
CMatrix4x4::CMatrix4x4()
{
for(int r = 0; r < 4; r++)
{
for(int c = 0; c < 4; c++)
{
matrix4x4[r][c] = 0;
}
}
}
CMatrix4x4::~CMatrix4x4()
{
}
float CMatrix4x4::operator()(int r, int c)
{
assert(r >= 0 && r < 4);
assert(c >= 0 && c < 4);
return matrix4x4[r][c];
}
void CMatrix4x4::SetRow(int r, CVector4 v)
{
assert(r >= 0 && r < 4);
matrix4x4[r][0] = v.x;
matrix4x4[r][1] = v.y;
matrix4x4[r][2] = v.z;
matrix4x4[r][3] = v.w;
}
int main()
{
CMatrix4x4 m;
CVector4 vec1(1, 2, 3, 4);
CVector4 vec2;
m.SetRow(0, CVector4(1, 0, 0, 0));
m.SetRow(1, CVector4(0, 1, 0, 0));
m.SetRow(2, CVector4(0, 0, 1, 0));
m.SetRow(3, CVector4(0, 0, 0, 1));
vec2 = vec1 * m;
std::cout << vec2.x;
std::cin.ignore();
return 0;
}
Edit: Thank you to all who assisted. I managed to resolve this by moving function implementations to separate .cpp files (Which I should have done to begin with. I have no clue why I didn't), and including the required headers there, and using forward declaration in the header files.
I'm not sure if this is the correct solution, however it does appear to be functional.
Same problem as in many questions asked before: the original version of your code apparently has two header files - vector.h and matrix.h - which include each other. This is circular inclusion, which does not achieve anything meaningful.
The include guards you probably have in your header files make sure that the inclusion does not become infinite. But they do nothing to resolve the circular dependency between your data types. The CMatrix4x4 is completely unknown in your vector.h, which leads to an error.
Forward declaration of CMatrix4x4 in vector.h is a step in proper direction. However, you have to get rid of that useless circular inclusion anyway. And you have to keep in mind that CMatrix4x4 will be an incomplete type in vector.h, meaning that you will not be able to access its internals in vector.h.
The latter means that your CVector4 CVector4::operator*(CMatrix4x4& m) has to be defined after the definition of CMatrix4x4, not before. In your code it is defined before CMatrix4x4. At that point type CMatrix4x4 is still incomplete, meaning that your cannot use its () operator. Expressions like m(0, 0) will not compile for that reason specifically. That's the reason for the error you are getting.
P.S. Additionally,
x, y, z, w = 0;
doesn't do what you probably think it does. It will assign 0 to w, but leave other data members unchanged (read about comma operator in C++).