Re: [ros-users] Interpolating Point_Cloud

classic Classic list List threaded Threaded
2 messages Options
Reply | Threaded
Open this post in threaded view
|

Re: [ros-users] Interpolating Point_Cloud

Radu B. Rusu
Administrator
Nicolas, I'm not sure I understand what you're trying to do. You have two XYZ points coming from 2 laser measurements,
and you want to put more points between them on the line that connects them?

I don't think that we have a node for this, as it's a very simple operation, but you could do something as silly as:

  ////////////////////////////////////////////////////////////////////////////////
  /** \brief Sample points on a line (formed from p1 and p2) using a given
    * resolution <res>
    * \param p1 the first point
    * \param p2 the second point
    * \param res the resolution of the line
    * \param pts the resultant points
    */
  int
    samplePointsOnLine (Point p1, Point p2, double res, PointCloud &pts)
  {
    Eigen3::Vector4f dp = p2.getVector4fMap () - p1.getVector4fMap ();
    // Compute the distance between the two points
    double dist = dp.norm ();
    dp.normalize ();

    Eigen3::Vector4f axis_x (1.0, 0.0, 0.0, 0.0), axis_y (0.0, 1.0, 0.0, 0.0), axis_z (0.0, 0.0, 1.0, 0.0);
    // Compute the axis increments
    double dx = res * dp.dot (axis_x);
    double dy = res * dp.dot (axis_y);
    double dz = res * dp.dot (axis_z);

    int nr_pts = dist / res;
    if (nr_pts < 1)
      return (0);

    pts.points.resize (nr_pts);
    pts.width = nr_pts; pts.height = 1;
    for (int m = 0; m < nr_pts; ++m)
    {
      pts.points[m].x = (m + 1) * dx + p1.x;
      pts.points[m].y = (m + 1) * dy + p1.y;
      pts.points[m].z = (m + 1) * dz + p1.z;
    }
    return (nr_pts);
  }


Cheers,
Radu.


On 11/26/2010 08:25 AM, Nicolás Alvarez Picco wrote:

> Hi!!
>
> I am working with LaserScan data which I have transformed into Point cloud, now I want to interpolate between two of
> that points, with a first order interpolation will be fine. The thing is that maybe there is node that make that
> interpolation. Someone knows about that??
>
> Thanks!!
>
> Nicolas
>
>
>
> _______________________________________________
> ros-users mailing list
> [hidden email]
> https://code.ros.org/mailman/listinfo/ros-users
_______________________________________________
[hidden email] / http://pcl.ros.org
https://code.ros.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: [ros-users] Interpolating Point_Cloud

Radu B. Rusu
Administrator
Nicolas,

The code that I gave you doesn't work with geometry_msgs::Point32. I'm pretty ignorant when it comes to that specific
format for passing point data around due to its extreme inefficiencies for SSE operations.

The Point type for that method was supposed to be pcl::PointXYZ. If you change it to that, it should work.

Cheers,
Radu.


On 11/30/2010 05:52 AM, Nicolás Alvarez Picco wrote:

>
> hi,
> What I want to do is: I have all the points of the laser measurement, all of then will be separated by the same distance
> unless there is an obstacle in the middle. When they are separated more than a fixed value, I will add points in between
> them.
> So using the script you gave me I made this one:
>
>
> Private
>
> sensor_msgs::PointCloud pts;
> sensor_msgs::PointField p1,p2;
> double res;
>
> void ROS_Link::Callback(const sensor_msgs::PointCloud::ConstPtr& msglaser)
> {
> //geometry_msgs::Point32 initial_point=msglaser->points[0];
>
> double diffsamples=0;
> double diffcompare=0.01; // Value calculated considering the laser features
> for(int it = 1; it !=msglaser->points.size(); it++){
>
> diffsamples=sqrt(pow(msglaser->points[it].x-msglaser->points[it-1].x,2)+
> pow(msglaser->points[it].y-msglaser->points[it-1].y,2)); //distancia entre 2 muestras consecutivas
> if(diffsamples>diffcompare){
> int cant_newpts=ROS_Link::samplePointsOnLine (msglaser->points[it-1],msglaser->points[it], diffcompare,pts);
> msglaser->points.resize (nr_pts+msglaser->points.size());
> int aux=it;
> for(int itt = msglaser->points.size()-1; itt !=aux-1; itt--)
> msglaser->points[itt]=msglaser->points[itt-aux];//desplazo los valores para poder meter los nuevos points
> for(int itt = aux; itt !=msglaser->points.size(); itt++)
> msglaser->points[itt]=pts.points[itt-aux]; //lleno el hueco con los nuevos puntos
> }
> }
>
>
> /** \brief Sample points on a line (formed from p1 and p2) using a given
> * resolution <res>
> * \param p1 the first point
> * \param p2 the second point
> * \param res the resolution of the line
> * \param pts the resultant points
> */
> int ROS_Link::samplePointsOnLine (PointField p1, PointField p2, double res, PointCloud &pts){
> Eigen3::Vector4f dp = p2.getVector4fMap () - p1.getVector4fMap ();
> // Compute the distance between the two points
> double dist = dp.norm ();
> dp.normalize ();
>
> Eigen3::Vector4f axis_x (1.0, 0.0, 0.0, 0.0), axis_y (0.0, 1.0, 0.0, 0.0), axis_z (0.0, 0.0, 1.0, 0.0);
> // Compute the axis increments
> double dx = res * dp.dot (axis_x);
> double dy = res * dp.dot (axis_y);
> double dz = res * dp.dot (axis_z);
>
> int nr_pts = dist / res;
> if (nr_pts < 1)
> return (0);
>
> pts.points.resize (nr_pts);
> pts.width = nr_pts; pts.height = 1;
> for (int m = 0; m < nr_pts; ++m)
> {
> pts.points[m].x = (m + 1) * dx + p1.x;
> pts.points[m].y = (m + 1) * dy + p1.y;
> pts.points[m].z = (m + 1) * dz + p1.z;
> }
> return (nr_pts);
> }
>
>
>
> When I try to compile this, I have some errors, one of then is:
> /home/robot/src/autnavarc/ros-ana-pkg/han_Planner/src/ROS_Link.cpp:209: error: no matching function for call to
> ‘Planner::ROS_Link::samplePointsOnLine(const geometry_msgs::Point32_<std::allocator<void> >&, const
> geometry_msgs::Point32_<std::allocator<void> >&, double&, sensor_msgs::PointCloud&)’
>
>
> Does someone know why I have this problem with the data. What you send me has the dqtq type Point, but I couldnt find it
> what I have find is the PointField, is that the same??
>
> Thankssss
>
>
> Nicolas
>
>
>
>
>  > Date: Fri, 26 Nov 2010 10:35:15 -0800
>  > From: [hidden email]
>  > To: [hidden email]
>  > CC: [hidden email]; [hidden email]
>  > Subject: Re: [ros-users] Interpolating Point_Cloud
>  >
>  > Nicolas, I'm not sure I understand what you're trying to do. You have two XYZ points coming from 2 laser measurements,
>  > and you want to put more points between them on the line that connects them?
>  >
>  > I don't think that we have a node for this, as it's a very simple operation, but you could do something as silly as:
>  >
>  > ////////////////////////////////////////////////////////////////////////////////
>  > /** \brief Sample points on a line (formed from p1 and p2) using a given
>  > * resolution <res>
>  > * \param p1 the first point
>  > * \param p2 the second point
>  > * \param res the resolution of the line
>  > * \param pts the resultant points
>  > */
>  > int
>  > samplePointsOnLine (Point p1, Point p2, double res, PointCloud &pts)
>  > {
>  > Eigen3::Vector4f dp = p2.getVector4fMap () - p1.getVector4fMap ();
>  > // Compute the distance between the two points
>  > double dist = dp.norm ();
>  > dp.normalize ();
>  >
>  > Eigen3::Vector4f axis_x (1.0, 0.0, 0.0, 0.0), axis_y (0.0, 1.0, 0.0, 0.0), axis_z (0.0, 0.0, 1.0, 0.0);
>  > // Compute the axis increments
>  > double dx = res * dp.dot (axis_x);
>  > double dy = res * dp.dot (axis_y);
>  > double dz = res * dp.dot (axis_z);
>  >
>  > int nr_pts = dist / res;
>  > if (nr_pts < 1)
>  > return (0);
>  >
>  > pts.points.resize (nr_pts);
>  > pts.width = nr_pts; pts.height = 1;
>  > for (int m = 0; m < nr_pts; ++m)
>  > {
>  > pts.points[m].x = (m + 1) * dx + p1.x;
>  > pts.points[m].y = (m + 1) * dy + p1.y;
>  > pts.points[m].z = (m + 1) * dz + p1.z;
>  > }
>  > return (nr_pts);
>  > }
>  >
>  >
>  > Cheers,
>  > Radu.
>  >
>  >
>  > On 11/26/2010 08:25 AM, Nicolás Alvarez Picco wrote:
>  > > Hi!!
>  > >
>  > > I am working with LaserScan data which I have transformed into Point cloud, now I want to interpolate between two of
>  > > that points, with a first order interpolation will be fine. The thing is that maybe there is node that make that
>  > > interpolation. Someone knows about that??
>  > >
>  > > Thanks!!
>  > >
>  > > Nicolas
>  > >
>  > >
>  > >
>  > > _______________________________________________
>  > > ros-users mailing list
>  > > [hidden email]
>  > > https://code.ros.org/mailman/listinfo/ros-users
>
>
>
> _______________________________________________
> ros-users mailing list
> [hidden email]
> https://code.ros.org/mailman/listinfo/ros-users
_______________________________________________
[hidden email] / http://pcl.ros.org
https://code.ros.org/mailman/listinfo/pcl-users