The site is read-only. Please transition to use Robotics Stack Exchange
| ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
 | 1 | initial version |
Well i have some old program which can do that, but u have to do little work on that, to complete it. First considering that only other objects are moving and your robot is stationary. First when ever your robot is in standing state i.e not moving ur robot needs to first save the 3d image of the present image, which can be done like below. Here this function simply saves the present 3d image in a ros bag called image.bag, which is used latter, or can be combined in same program.
int ix = -1, iy = -1, iz = -1;
rosbag::Bag bag("image.bag", rosbag::bagmode::Write);
for(int k=0;k<h && ros::ok();k++)
{
for(int l=0;l<w;l++) {
ix = ix+1;
x[ix].data = P[k][l].x;
} }
for(int k=0;k<h;k++)
{
for(int l=0;l<w;l++) {
iy = iy+1;
int ix = -1, iy = -1, iz = -1;
rosbag::Bag bag("image.bag", rosbag::bagmode::Write);
for(int k=0;k<h && ros::ok();k++)
{
for(int l=0;l<w;l++) {
ix = ix+1;
x[ix].data = P[k][l].x;
} }
for(int k=0;k<h;k++)
{
for(int l=0;l<w;l++) {
iy = iy+1;
y[iy].data = P[k][l].y;
} }
for(int k=0;k<h;k++)
{
for(int l=0;l<w;l++) {
iz = iz+1;
z[iz].data = P[k][l].z;
}}
for(int i=0;i<n;i++) {
bag.write("numbers", ros::Time::now(), x[i]);
//std::cout<<i<<"\t"<<ix<<"\t"<<"x"<<"\t"<<x[i]<<std::endl;
}
for(int i=0;i<n;i++) {
bag.write("numbers", ros::Time::now(), y[i]);
//std::cout<<n<<"\t"<<iy<<"\t"<<"y"<<"\t"<<y[n]<<std::endl;
}
for(int i=0;i<n;i++) {
bag.write("numbers", ros::Time::now(), z[i]);
//std::cout<<i<<"\t"<<iz<<"\t"<<"z"<<"\t"<<z[i]<<std::endl;
}
bag.close(); y[iy].data = P[k][l].y;
} }
for(int k=0;k<h;k++)
{
for(int l=0;l<w;l++) {
iz = iz+1;
z[iz].data = P[k][l].z;
}}
for(int i=0;i<n;i++) {
bag.write("numbers", ros::Time::now(), x[i]);
//std::cout<<i<<"\t"<<ix<<"\t"<<"x"<<"\t"<<x[i]<<std::endl;
}
for(int i=0;i<n;i++) {
bag.write("numbers", ros::Time::now(), y[i]);
//std::cout<<n<<"\t"<<iy<<"\t"<<"y"<<"\t"<<y[n]<<std::endl;
}
for(int i=0;i<n;i++) {
bag.write("numbers", ros::Time::now(), z[i]);
//std::cout<<i<<"\t"<<iz<<"\t"<<"z"<<"\t"<<z[i]<<std::endl;
}
bag.close();
After saving how the 3d image is, when the robot is not moving. The next part what robot does is it analyses the present 3d image(video stream subscribed from kinect) and if any object is moving, it will detect that, and save its point clouds, and for every seperate object at distance farther than 2 cam, they are considered as differenct object and saved in different array, u can use vector for it. And it looks little long, u can shorten it. This is just the old program which i used to detect moved objects in 3d image.
rosbag::Bag bag("test.bag");
//std_msgs::Float32 i[40000], y[40000], z[40000];
int iv = -1, ix = -1, iy = -1, iz = -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;
if(iv>=0 && iv<306081) {
ix = ix+1;
xv[ix] = i->data;
// std::cout<<iv<<"x ="<<"\t"<<ix<<"\t"<<xv[ix]<<std::endl;
}
if(iv>=306081 && iv<612162)
{
iy = iy+1;
yv[iy] = i->data;
ip = -1;
for(int i=0;i<h;i++)
{
for(int j=0;j<w;j++)
{
ip = ip+1;
x[i][j] = xv[ip]; xp[i][j] = P[i][j].x;
y[i][j] = yv[ip]; yp[i][j] = P[i][j].y;
z[i][j] = zv[ip]; zp[i][j] = P[i][j].z;
}
}
// ** comparing previous and present point cloud and finding moved objects
int im = -1;
// int m = 0;
for(int k=0;k<h;k++)
{
for(int l=0;l<w;l++)
{
if( z[k][l]!=0 && zp[k][l]!=0 && !isnan(x[k][l]) && !isnan(xp[k][l]) && !isnan(y[k][l]) && !isnan(yp[k][l]) && !isnan(z[k][l]) && !isnan(zp[k][l]) )
{
float dx=abs(float(x[k][l]-xp[k][l])*100);
float dy=abs(float(y[k][l]-yp[k][l])*100);
float dz=abs(float(z[k][l]-zp[k][l])*100);
float d = sqrt(dx*dx+dy*dy+dz*dz);
if(d>2)
{
im = im+1;
a[im] = x[k][l]; b[im] = y[k][l]; c[im] = z[k][l];
xa[im] = k; ya[im] = l;
}
}
}
}
if(im!=30000) {
float xe[im], ye[im], ze[im]; int no[1000];
for(int i=0;i<=im;i++)
{
xe[i] = a[i]; ye[i] = b[i]; ze[i] = c[i];
}
int ie = im;
int iobj = -1; int nobj = -1;
float xnc[ie], ync[ie], znc[ie];
while(ie>1)
{
iobj = iobj+1;
ip = -1; int ic = -1; xc[iobj][0] = xe[0]; yc[iobj][0] = ye[0]; zc[iobj][0] = ze[0];
while(ip<=ic && ros::ok())
{
ip = ip+1; int inc = -1;
for(int i=0;i<=ie && ros::ok();i++)
{
if(ip!=i)
{
float d = sqrt(pow(xc[iobj][ip]-xe[i],2)+pow(yc[iobj][ip]-ye[i],2)+pow(zc[iobj][ip]-ze[i],2));
if(d<=0.01)
{
ic = ic+1;
xc[iobj][ic] = xe[i]; yc[iobj][ic] = ye[i]; zc[iobj][ic] = ze[i];
// if(ic%30==0) {
if(!isnan(xe[i]) && !isnan(ye[i]) && !isnan(ze[i]) && ze[i]!=0)
{
p.x = ze[i]; p.y = -xe[i]; p.z = -ye[i];
points.points.push_back(p);
marker_pub.publish(points);
} // }
}
else
{
inc = inc+1;
xnc[inc] = xe[i]; ync[inc] = ye[i]; znc[inc] = ze[i];
}
}
}
for(int j=0;j<=inc && ros::ok();j++)
{
xe[j] = xnc[j]; ye[j] = ync[j]; ze[j] = znc[j];
}
ie = inc; //std::cout<<"ommm"<<"\t"<<iobj<<"\t"<<ip<<"\t"<<ic<<"\t"<<inc<<std::endl;
}
if(ic>500) {
nobj = nobj+1;
for(int k=0;k<=ic;k++)
{
xobj[nobj][k] = xc[iobj][k]; yobj[nobj][k] = yc[iobj][k]; zobj[nobj][k] = zc[iobj][k];
} no[nobj] = ic; } } }
if(iv>=612162 && iv<918243) {
iz = iz+1;
zv[iz] = i->data;
}
}
bag.close();
// ** converting already read saved points to 2 d
int ip = -1;
for(int i=0;i<h;i++)
{
for(int j=0;j<w;j++)
{
ip = ip+1;
x[i][j] = xv[ip]; xp[i][j] = P[i][j].x;
y[i][j] = yv[ip]; yp[i][j] = P[i][j].y;
z[i][j] = zv[ip]; zp[i][j] = P[i][j].z;
}
}
// ** comparing previous and present point cloud and finding moved objects
int im = -1;
// int m = 0;
for(int k=0;k<h;k++)
{
for(int l=0;l<w;l++)
{
if( z[k][l]!=0 && zp[k][l]!=0 && !isnan(x[k][l]) && !isnan(xp[k][l]) && !isnan(y[k][l]) && !isnan(yp[k][l]) && !isnan(z[k][l]) && !isnan(zp[k][l]) )
{
float dx=abs(float(x[k][l]-xp[k][l])*100);
float dy=abs(float(y[k][l]-yp[k][l])*100);
float dz=abs(float(z[k][l]-zp[k][l])*100);
float d = sqrt(dx*dx+dy*dy+dz*dz);
if(d>2)
{
im = im+1;
a[im] = x[k][l]; b[im] = y[k][l]; c[im] = z[k][l];
xa[im] = k; ya[im] = l;
}
}
}
}
if(im!=30000) {
float xe[im], ye[im], ze[im]; int no[1000];
for(int i=0;i<=im;i++)
{
xe[i] = a[i]; ye[i] = b[i]; ze[i] = c[i];
}
int ie = im;
int iobj = -1; int nobj = -1;
float xnc[ie], ync[ie], znc[ie];
while(ie>1)
{
iobj = iobj+1;
ip = -1; int ic = -1; xc[iobj][0] = xe[0]; yc[iobj][0] = ye[0]; zc[iobj][0] = ze[0];
while(ip<=ic && ros::ok())
{
ip = ip+1; int inc = -1;
for(int i=0;i<=ie && ros::ok();i++)
{
if(ip!=i)
{
float d = sqrt(pow(xc[iobj][ip]-xe[i],2)+pow(yc[iobj][ip]-ye[i],2)+pow(zc[iobj][ip]-ze[i],2));
if(d<=0.01)
{
ic = ic+1;
xc[iobj][ic] = xe[i]; yc[iobj][ic] = ye[i]; zc[iobj][ic] = ze[i];
// if(ic%30==0) {
if(!isnan(xe[i]) && !isnan(ye[i]) && !isnan(ze[i]) && ze[i]!=0)
{
p.x = ze[i]; p.y = -xe[i]; p.z = -ye[i];
points.points.push_back(p);
marker_pub.publish(points);
} // }
}
else
{
inc = inc+1;
xnc[inc] = xe[i]; ync[inc] = ye[i]; znc[inc] = ze[i];
}
}
}
for(int j=0;j<=inc && ros::ok();j++)
{
xe[j] = xnc[j]; ye[j] = ync[j]; ze[j] = znc[j];
}
ie = inc; //std::cout<<"ommm"<<"\t"<<iobj<<"\t"<<ip<<"\t"<<ic<<"\t"<<inc<<std::endl;
}
if(ic>500) {
nobj = nobj+1;
for(int k=0;k<=ic;k++)
{
xobj[nobj][k] = xc[iobj][k]; yobj[nobj][k] = yc[iobj][k]; zobj[nobj][k] = zc[iobj][k];
} no[nobj] = ic; } }
So in this way, u have a 2 d array, xobj[][], yobj[][], zobj[][] where first index represents the index of object and second represents index of point on that object. No after u have seperated given 3d objects from the image, for each object and your robot you can calculate the minimum distance between each of those objects. By using their point coordinates which are {xobj[][],yobj[][],zobj[][]}.
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.