ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

can't save point cloud in bag file complitly

asked 2016-08-07 03:46:39 -0600

dinesh gravatar image

updated 2016-08-08 04:22:37 -0600

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2016-08-07 09:15:15 -0600

NEngelhard gravatar image

updated 2016-08-07 09:15:28 -0600

" segmented error" Please be exact when talking about errors. It's probably an Segmentation Fault.

There are several issues with your code: Using constants like '479' in your code is extremely(!) bad style. A pointcloud has width and height so use these variables. If you want to store a cloud with width of 480, you should also create an array with this size and not one if an entry less (479 instead of 480). (Why do you copy the pointcloud at all in this 2d array?)

But in general: What do you want to do?

edit flag offensive delete link more

Comments

for computer vision purpose, according to theory this algorithm after completion will able to detect an object in point cloud when moved and than save its 360 deg view by rotating it, and than it is 3d object recognition. we can also add color information latter, than it will also work for 2d image.

dinesh gravatar image dinesh  ( 2016-08-07 11:34:56 -0600 )edit

I meant on a lower level. Why do you copy the pointcloud first into an array and the into tons of std_msgs::Float32-Messages to write them into a bag file?

NEngelhard gravatar image NEngelhard  ( 2016-08-08 01:08:03 -0600 )edit

when first node is run, it will read point cloud and save the cordinates of each points in bag as float, than i will stop this node as it is saved. than in next node it will compare the saved point cloud with present to detect change and so on. im still trying to figure out how to do it.

dinesh gravatar image dinesh  ( 2016-08-08 03:55:21 -0600 )edit

Why don't you store the cloud directly?

NEngelhard gravatar image NEngelhard  ( 2016-08-08 05:14:21 -0600 )edit

cas i also have to store the point cloud related to moved object, which are fewer in number after this, and also after testing that i have to only save the keypoints of that object(for speed), for pattern recognition, so saving hole point cloud is not solution for me.

dinesh gravatar image dinesh  ( 2016-08-08 07:07:00 -0600 )edit

ok i solved this problem, finally.

dinesh gravatar image dinesh  ( 2016-08-09 04:38:09 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2016-08-07 03:46:39 -0600

Seen: 531 times

Last updated: Aug 08 '16