Filling eigen matrix with values

Question

sorry to bother. Could you know maybe how can i fix my code. It gives me this error:

/usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:365: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::Index, Eigen::Index) [with Derived = Eigen::Matrix<float, -1, -1>; Eigen::DenseCoeffsBase<Derived, 1>::Scalar = float; Eigen::Index = long int]: Assertion `row >= 0 && row < rows() && col >= 0 && col < cols()' failed.

I think that I declared matrix in right way. My mistake (I think) is when i try to fill her with values in Possible transforms function. I think it might be wrong transform1(0, position)=.. I tried it to comment that lines of code and wrote a simple code like transform1(0, position)=1; and it gave the same error.

Sorry to bother, Kind regards.

My code:

#include "ros/ros.h"
#include <cstdlib>
#include <fstream>
#include <stdio.h>
#include <regex>
#include "sensor_msgs/PointCloud2.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <boost/shared_ptr.hpp>
#include "pcl_ros/point_cloud.h"
#include "sensor_msgs/PointField.h"
#include <pcl/io/pcd_io.h>
#include "nav_msgs/Odometry.h"
#include "eigen3/Eigen/SVD"
#include "eigen3/Eigen/Dense"
#include "clustering/Track.h"
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Sparse"
using namespace Eigen;

int number_od_possible_tracks = 15;

class Odometry_class {
ros::NodeHandle nodeh;
ros::Subscriber sub;
ros::Publisher pub;

public: 
Odometry_class();
void Odometry(const sensor_msgs::PointCloud2 &msg); 
bool Has_pointcloud_min3points(pcl::PointCloud<pcl::PointXYZIVSI> cloud);
bool Possible_transform(pcl::PointCloud<pcl::PointXYZIVSI> cloud);
void Restore_Ids();
pcl::PointCloud<pcl::PointXYZIVSI> last_cloud;
Eigen::MatrixXf transform1 = (Eigen::MatrixXf(3,15));
Eigen::MatrixXf transform2=(Eigen::MatrixXf(3,15));
int consecutive_Ids_current[15];
int consecutive_Ids_previous[15];
int position;
void Transform();
};


Odometry_class::Odometry_class(){
sub = nodeh.subscribe("trackers", 10, &Odometry_class::Odometry, this); 
pub = nodeh.advertise<nav_msgs::Odometry>("odometry", 10); 

}

void Odometry_class::Odometry(const sensor_msgs::PointCloud2 &msg ){
pcl::PCLPointCloud2 pcl_pc2;
pcl_conversions::toPCL(msg, pcl_pc2);
pcl::PointCloud<pcl::PointXYZIVSI> cloud;
pcl::fromPCLPointCloud2(pcl_pc2, cloud); 

if(Possible_transform(cloud)){
ROS_INFO("Transformation is possible");
}
else{
ROS_INFO("Transformation is not possible");
}

Restore_Ids();
pcl::fromPCLPointCloud2(pcl_pc2, last_cloud); 
}




void Odometry_class::Restore_Ids(){
int i=0;
while(consecutive_Ids_current[i]!=0){
consecutive_Ids_current[i]=0;
consecutive_Ids_previous[i]=0;
i++;
}

}

bool Odometry_class::Has_pointcloud_min3points(pcl::PointCloud<pcl::PointXYZIVSI> cloud){
bool has=false;
if(cloud.width>=3){
has=true;
}
return has;
}


bool Odometry_class::Possible_transform(pcl::PointCloud<pcl::PointXYZIVSI> cloud){
bool possible=false;
position=0;
if(Has_pointcloud_min3points(cloud)&Has_pointcloud_min3points(last_cloud)){ 
for(int i=0;i<cloud.width;i++){
for(int j=0;j<last_cloud.width;j++){
if(cloud[i].id==last_cloud[j].id){
consecutive_Ids_current[position]=i;
consecutive_Ids_previous[position]=j;
position++;
}
}
}
}

if(position>=3){
possible=true;
transform1.resize(3,position);
transform2.resize(3,position);

for(int i=0;i<position;i++){

transform1(0,position)= cloud[consecutive_Ids_current[i]].x;
transform1(1,position)= cloud[consecutive_Ids_current[i]].y;
transform1(2,position)= cloud[consecutive_Ids_current[i]].z;
transform2(0,position)= cloud[consecutive_Ids_previous[i]].x;
transform2(1,position)= cloud[consecutive_Ids_previous[i]].y;
transform2(2,position)= cloud[consecutive_Ids_previous[i]].z;
}

}

return possible;
}




int main(int argc, char **argv) {
ros::init(argc, argv, "Odometry");
Odometry_class Odometry; 

ros::spin();
return 0;
}

Answer 1

I think you mixed up your loop variable, i , and upper limit, position . Using position as an index would result in accessing the matrix out of bounds, and the Eigen library catches that in an assertion. Using the loop variable should resolve the error.

transform1(0,i)= cloud[consecutive_Ids_current[i]].x;

And likewise for the rest of the transform1 and transform2 assignments.