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 <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <iostream>
#include <math.h>
#include <cmath>
#include <algorithm>
#include <numeric>
#include <visualization_msgs/Marker.h>
#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 <rosbag/view.h>
#include <vector>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
ros::Publisher pub;
ros::Publisher marker_pub;
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
pcl::PointCloud<pcl::PointXYZ> 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<<j<<"\t"<<k<<"\t"<<i[j][k]<<"\t"<<y[j][k]<<"\t"<<z[j][k]<<std::endl;
}
}
bag.close();
std::cout<<"saved"<<std::endl;
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg(output,cloud);
pub.publish(cloud);
}
int
main(int argc, char** argv)
{
ROS_INFO("main");
ros::init(argc, argv,"example");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);
pub = nh.advertise<sensor_msgs::PointCloud2>("output",1);
marker_pub = nh.advertise<visualization_msgs::Marker> ("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<pcl::PointXYZ> 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<std_msgs::Float32>();
iv = iv+1;
// std::cout <<"z ="<<"\t"<<iv<<"\t"<< z->data << std::endl;
if(iv%3==0 || iv==0)
{ if(!isnan(i->data)) {
xv[iv] = i->data;
std::cout<<"xv"<<iv<<"\t"<<xv[iv]<<std::endl; }
}
else if((iv-1)%3==0 || iv==1)
{ if(!isnan(i->data)) {
yv[iv] = i->data;
std::cout<<"yv"<<iv<<"\t"<<yv[iv]<<std::endl; }
}
else
{ if(!isnan(i->data)) {
zv[iv] = i->data;
std::cout<<"zv"<<iv<<"\t"<<zv[iv]<<std::endl; }
}
}
bag.close();
iv = -1; int raw = 479, col = 50;
float xp[50][50], yp[50][50], zp[50][50];
for(int k=0;k<raw;k++)
{
for(int l=0;l<col;l++)
{
xp[k][l] = xv[++iv];
yp[k][l] = yv[++iv];
zp[k][l] = zv[++iv];
p.x = zp[k][l]; p.y = xp[k][l]; p.z = yp[k][l];
points.points.push_back(p);
marker_pub.publish(points);
} }
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg(output,cloud);
pub.publish(cloud);
}
Here i haven't reduced the code into short, so it looks long, i am just trying to test the concept first then other parts. So what is the problem in saving the hole point cloud with their coordinate values? I'll be really happy if i solved it.
When first node is run it will save cordinates of each point from present point cloud, after saved when we run next node in the same image than it will calculate which points has been changed, if we move an object like screwdriver in image than the points related to it will change along with its coordinates, which the next node will be detecting, than those detected points with their coordinates can be detected, saved as small part of object(since object needs 360 deg of surface), tracked, and after finding 360 deg view oriantation also can be determined. This hole project will work based on principle that every point in a rigid object(human tools) are always at same position with respect to each other. even if object is partially seen, the seen surface can be detected by this method. We dont have to put object over any table to detect them. Color information of point can be also used latter. For this theory to work object has to be rigid, and its not needed to save all the points from given object. Few points, either keypoints or any points form same object can be saved to find it in next frames, by using their relative positions w.r.t each other. right now im saving the points manually by running different node but latter computer will do it itself by using signals and so on. this i added for mr. NEngelhard. :>. it should be noted that even if we save the cordinates of points, we will be using the positions of points w.r.t other points in same object to detect them, can of course the cordinates of even same point belonging to same object will be different in next view.
Originally posted by dinesh on ROS Answers with karma: 932 on 2016-08-07
Post score: 0