Hy, i am trying to save the point cloud with size(480,640) in bag file by their coordinate values and then read them in another node so that machine determines which part of the point cloud has been changed in next frames and what are their coordinates so that they can be searched in coming frames and that surface with those points can be detected in next point cloud. My problem is when i save point cloud of size (479,50) range it saves and can be visualized, but as the size change to above 50, i get segmented error, what should i do? My codes are:
To read the point cloud of size(50,50), determined by raw = 479, col = 50:
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#define pi 3.1453
#define pu 3779.527559055
#include "std_msgs/String.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Int32.h"
#include "rosbag/bag.h"
#include
#include
#include
#define foreach BOOST_FOREACH
ros::Publisher pub;
ros::Publisher marker_pub;
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
pcl::PointCloud output;
pcl::fromROSMsg(*input,output);
pcl::PointXYZ P[479][639];
for(int raw = 0;raw<479;raw++)
{
for(int col = 0;col<639;col++)
{
P[raw][col] = output.at(col,raw);
}
}
int raw = 479, col = 50;
rosbag::Bag bag("test.bag", rosbag::bagmode::Write);
std_msgs::Float32 x[raw][col], y[raw][col], z[raw][col]; // y[480][640], z[480][640];
for(int k=0;k<=raw;k++)
{
for(int l=0;l<=col;l++) {
x[k][l].data = P[k][l].x;
y[k][l].data = P[k][l].y;
z[k][l].data = P[k][l].z;
} }
for(int j=0;j<=raw;j++) {
for(int k=0;k<=col;k++) {
bag.write("numbers", ros::Time::now(), x[j][k]);
bag.write("numbers", ros::Time::now(), y[j][k]);
bag.write("numbers", ros::Time::now(), z[j][k]);
// std::cout<("output",1);
marker_pub = nh.advertise ("out",1);
ros::spin();
}
And the code for reading them is:(so that if some object moves in depth frame it could be deteted and saved, tracked, since every point in rigid object are always at same position with respect to each other with small error):
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
pcl::PointCloud output;
pcl::fromROSMsg(*input,output);
visualization_msgs::Marker points, line_strip, line_list;
points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "/camera_depth_frame";
points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
points.ns = line_strip.ns = line_list.ns = "lines";
points.action =line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;
points.id = 0;
line_strip.id = 1;
line_list.id = 2;
points.type = visualization_msgs::Marker::POINTS;
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
line_list.type = visualization_msgs::Marker::LINE_LIST;
points.scale.x = 0.01;
points.scale.y = 0.01;
line_strip.scale.x = 0.005;
line_strip.color.b = 1.0;
line_strip.color.a = 1.0;
points.color.r = 1.0f;
points.color.a = 1.0;
line_list.scale.x = 0.005;
line_list.color.a = 1.0;
geometry_msgs::Point p;
pcl::PointXYZ P[479][639];
for(int raw = 0;raw<479;raw++)
{
for(int col = 0;col<639;col++)
{
P[raw][col] = output.at(col,raw);
}
}
rosbag::Bag bag("test.bag");
//std_msgs::Float32 i[40000], y[40000], z[40000];
float xv[100000], yv[100000], zv[100000]; int iv = -1;
rosbag::View view(bag, rosbag::TopicQuery("numbers"));
BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
std_msgs::Float32::ConstPtr i = m.instantiate();
iv = iv+1;
// std::cout <<"z ="<<"\t"<data << std::endl;
if(iv%3==0 || iv==0)
{ if(!isnan(i->data)) {
xv[iv] = i->data;
std::cout<<"xv"<data)) {
yv[iv] = i->data;
std::cout<<"yv"<data)) {
zv[iv] = i->data;
std::cout<<"zv"<
↧