<html>
<head>
<style><!--
.hmmessage P
{
margin:0px;
padding:0px
}
body.hmmessage
{
font-size: 10pt;
font-family:Tahoma
}
--></style>
</head>
<body class='hmmessage'>
<br>
<meta http-equiv="Content-Type" content="text/html; charset=unicode">
<meta name="Generator" content="Microsoft SafeHTML">
<style>
.ExternalClass .ecxhmmessage P
{padding:0px;}
.ExternalClass body.ecxhmmessage
{font-size:10pt;font-family:Tahoma;}
</style>
hi,<br>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.<br>So using the script you gave me I made this one:<br><br><br>Private<br><br>sensor_msgs::PointCloud pts;<br> sensor_msgs::PointField p1,p2;<br> double res;<br><br>void ROS_Link::Callback(const sensor_msgs::PointCloud::ConstPtr& msglaser)<br>{<br> //geometry_msgs::Point32 initial_point=msglaser->points[0];<br> <br> double diffsamples=0;<br> double diffcompare=0.01; // Value calculated considering the laser features<br> for(int it = 1; it !=msglaser->points.size(); it++){<br> <br> diffsamples=sqrt(pow(msglaser->points[it].x-msglaser->points[it-1].x,2)+<br> pow(msglaser->points[it].y-msglaser->points[it-1].y,2)); //distancia entre 2 muestras consecutivas<br> if(diffsamples>diffcompare){<br> int cant_newpts=ROS_Link::samplePointsOnLine (msglaser->points[it-1],msglaser->points[it], diffcompare,pts);<br> msglaser->points.resize (nr_pts+msglaser->points.size());<br> int aux=it;<br> for(int itt = msglaser->points.size()-1; itt !=aux-1; itt--)<br> msglaser->points[itt]=msglaser->points[itt-aux];//desplazo los valores para poder meter los nuevos points<br> for(int itt = aux; itt !=msglaser->points.size(); itt++)<br> msglaser->points[itt]=pts.points[itt-aux]; //lleno el hueco con los nuevos puntos<br> }<br> }<br> <br> <br> /** \brief Sample points on a line (formed from p1 and p2) using a given<br> * resolution <res><br> * \param p1 the first point<br> * \param p2 the second point<br> * \param res the resolution of the line<br> * \param pts the resultant points<br> */<br>int ROS_Link::samplePointsOnLine (PointField p1, PointField p2, double res, PointCloud &pts){<br> Eigen3::Vector4f dp = p2.getVector4fMap () - p1.getVector4fMap ();<br> // Compute the distance between the two points<br> double dist = dp.norm ();<br> dp.normalize ();<br> <br> 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);<br> // Compute the axis increments<br> double dx = res * dp.dot (axis_x);<br> double dy = res * dp.dot (axis_y);<br> double dz = res * dp.dot (axis_z);<br> <br> int nr_pts = dist / res;<br> if (nr_pts < 1)<br> return (0);<br> <br> pts.points.resize (nr_pts);<br> pts.width = nr_pts; pts.height = 1;<br> for (int m = 0; m < nr_pts; ++m)<br> {<br> pts.points[m].x = (m + 1) * dx + p1.x;<br> pts.points[m].y = (m + 1) * dy + p1.y;<br> pts.points[m].z = (m + 1) * dz + p1.z;<br> }<br> return (nr_pts);<br> }<br> <br><br><br>When I try to compile this, I have some errors, one of then is:<br>/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&)’<br><br><br>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??<br><br>Thankssss<br><br><br>Nicolas<br><br> <br><br><br>> Date: Fri, 26 Nov 2010 10:35:15 -0800<br>> From: rusu@willowgarage.com<br>> To: nicolasapicco@hotmail.com<br>> CC: ros-users@code.ros.org; pcl-users@code.ros.org<br>> Subject: Re: [ros-users] Interpolating Point_Cloud<br>> <br>> Nicolas, I'm not sure I understand what you're trying to do. You have two XYZ points coming from 2 laser measurements, <br>> and you want to put more points between them on the line that connects them?<br>> <br>> 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:<br>> <br>> ////////////////////////////////////////////////////////////////////////////////<br>> /** \brief Sample points on a line (formed from p1 and p2) using a given<br>> * resolution <res><br>> * \param p1 the first point<br>> * \param p2 the second point<br>> * \param res the resolution of the line<br>> * \param pts the resultant points<br>> */<br>> int<br>> samplePointsOnLine (Point p1, Point p2, double res, PointCloud &pts)<br>> {<br>> Eigen3::Vector4f dp = p2.getVector4fMap () - p1.getVector4fMap ();<br>> // Compute the distance between the two points<br>> double dist = dp.norm ();<br>> dp.normalize ();<br>> <br>> 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);<br>> // Compute the axis increments<br>> double dx = res * dp.dot (axis_x);<br>> double dy = res * dp.dot (axis_y);<br>> double dz = res * dp.dot (axis_z);<br>> <br>> int nr_pts = dist / res;<br>> if (nr_pts < 1)<br>> return (0);<br>> <br>> pts.points.resize (nr_pts);<br>> pts.width = nr_pts; pts.height = 1;<br>> for (int m = 0; m < nr_pts; ++m)<br>> {<br>> pts.points[m].x = (m + 1) * dx + p1.x;<br>> pts.points[m].y = (m + 1) * dy + p1.y;<br>> pts.points[m].z = (m + 1) * dz + p1.z;<br>> }<br>> return (nr_pts);<br>> }<br>> <br>> <br>> Cheers,<br>> Radu.<br>> <br>> <br>> On 11/26/2010 08:25 AM, Nicolás Alvarez Picco wrote:<br>> > Hi!!<br>> ><br>> > I am working with LaserScan data which I have transformed into Point cloud, now I want to interpolate between two of<br>> > that points, with a first order interpolation will be fine. The thing is that maybe there is node that make that<br>> > interpolation. Someone knows about that??<br>> ><br>> > Thanks!!<br>> ><br>> > Nicolas<br>> ><br>> ><br>> ><br>> > _______________________________________________<br>> > ros-users mailing list<br>> > ros-users@code.ros.org<br>> > https://code.ros.org/mailman/listinfo/ros-users<br> </body>
</html>