c++ - std::vector<Eigen::Vector3d> to Eigen::MatrixXd Eigen

Question

I would like to know whether is there an easier way to solve my problem rather than use a for loop. So here is the situation:

In general, I would like to gather data points from my sensor (the message is of type Eigen::Vector3d and I can't change this, because it's a huge framework)

Gathered points should be saved in Eigen MatrixXd (in order to process them further as the Matrix in the optimization algorithm), the dimensions apriori of the Matrix are partially unknown, because it depends of me how many measurements I will take (one dimension is 3 because there are x,y,z coordinates)

For the time being, I created a std::vector<Eigen::Vector3d> where I collect points by push_back and after I finished collecting points I would like to convert it to MatrixXd by using the operation Map .

 sensor_input = Eigen::Map<Eigen::MatrixXd>(sensor_input_vector.data(),3,sensor_input_vector.size());

But I have an error and note : no known conversion for argument 1 from Eigen::Matrix<double, 3, 1>* to Eigen::Map<Eigen::Matrix<double, -1, -1>, 0, Eigen::Stride<0, 0> >::PointerArgType {aka double*}

Can you tell me how I could implement this by using a map function?

Answer 1

Short answer: You need to write (after making sure that your input is not empty):

sensor_input = Eigen::Map<Eigen::MatrixXd>(sensor_input_vector[0].data(),3,sensor_input_vector.size());

The reason is that Eigen::Map expects a pointer to the underlying Scalar type (in this case double*), whereas std::vector::data() returns a pointer to the first element inside the vector (i.e., an Eigen::Vector3d*).

Now sensor_input_vector[0].data() will give you a pointer to the first (Vector3d) entry of the first element of your std::vector. Alternatively, you could reinterpret_cast like so:

sensor_input = Eigen::Map<Eigen::MatrixXd>(reinterpret_cast<double*>(sensor_input_vector.data()),3,sensor_input_vector.size());

In many cases you can actually avoid copying the data to a Eigen::MatrixXd but instead directly work with the Eigen::Map, and instead of MatrixXd you can use Matrix3Xd to express that it is known at compile time that there are exactly 3 rows:

// creating an Eigen::Map has O(1) cost
Eigen::Map<Eigen::Matrix3Xd> sensor_input_mapped(sensor_input_vector[0].data(),3,sensor_input_vector.size());
// use sensor_input_mapped, the same way you used sensor_input before

You need to make sure that the underlying std::vector will not get re-allocated while sensor_input_mapped is used. Also, changing individual elements of the std::vector will change them in the Map and vice versa.