Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21046] pkg/trunk
|
From: <stu...@us...> - 2009-08-07 20:54:05
|
Revision: 21046
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21046&view=rev
Author: stuglaser
Date: 2009-08-07 20:53:57 +0000 (Fri, 07 Aug 2009)
Log Message:
-----------
More fixes for the geometry_msgs/Polygon changes
Modified Paths:
--------------
pkg/trunk/sandbox/lifelong_mapping/manifest.xml
pkg/trunk/sandbox/planar_objects/src/articulation_models.cpp
pkg/trunk/sandbox/planar_objects/src/find_planes.cpp
pkg/trunk/sandbox/planar_objects/src/vis_utils.cpp
pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/doors_detector.cpp
pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/geometric_helper.cpp
pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector.cpp
pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/cloud_downsampler.cpp
pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/cloud_io/tools/bag_to_pcd.cpp
pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/normal_estimation.cpp
pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/normal_estimation_in_proc.cpp
pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/planar_fit.cpp
pkg/trunk/stacks/semantic_mapping/semantic_point_annotator/src/sac_ground_removal.cpp
pkg/trunk/stacks/semantic_mapping/semantic_point_annotator/src/sac_inc_ground_removal.cpp
pkg/trunk/vision/recognition_lambertian/src/plane_fit.cpp
pkg/trunk/vision/recognition_lambertian/src/rec_lam_normal_features.cpp
pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
Modified: pkg/trunk/sandbox/lifelong_mapping/manifest.xml
===================================================================
--- pkg/trunk/sandbox/lifelong_mapping/manifest.xml 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/sandbox/lifelong_mapping/manifest.xml 2009-08-07 20:53:57 UTC (rev 21046)
@@ -10,7 +10,7 @@
<url>http://pr.willowgarage.com/wiki/lifelong_mapping</url>
<depend package="roscpp"/>
<depend package="tf"/>
- <depend package="robot_msgs"/>
+ <depend package="geometry_msgs"/>
<depend package="toro"/>
<!--depend package="place_recognition"/>
<depend package="visual_odometry"/-->
Modified: pkg/trunk/sandbox/planar_objects/src/articulation_models.cpp
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/articulation_models.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/sandbox/planar_objects/src/articulation_models.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -8,7 +8,6 @@
#include "articulation_models.h"
#include "Eigen/Core"
#include <Eigen/SVD>
-#include <Eigen/Eigen>
#include "opencv/cv.h"
using namespace Eigen;
Modified: pkg/trunk/sandbox/planar_objects/src/find_planes.cpp
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/find_planes.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/sandbox/planar_objects/src/find_planes.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -9,7 +9,6 @@
using namespace ros;
using namespace std;
-using namespace robot_msgs;
#include <point_cloud_mapping/sample_consensus/sac_model_plane.h>
#include <point_cloud_mapping/sample_consensus/sac.h>
@@ -147,7 +146,7 @@
{
// Compute the convex hull of the area
// NOTE: this is faster than computing the concave (alpha) hull, so let's see how this works out
- Polygon3D polygon;
+ geometry_msgs::Polygon polygon;
cloud_geometry::areas::convexHull2D(points, indices[i], models[i], polygon);
// Compute the area of the polygon
Modified: pkg/trunk/sandbox/planar_objects/src/vis_utils.cpp
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/vis_utils.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/sandbox/planar_objects/src/vis_utils.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -10,7 +10,6 @@
using namespace ros;
using namespace std;
-using namespace robot_msgs;
namespace planar_objects {
@@ -109,7 +108,7 @@
rgb=HSV_to_RGB( i/(float)plane_indices.size(),0.5,1);
if(convexHull) {
- Polygon3D polygon;
+ geometry_msgs::Polygon polygon;
cloud_geometry::areas::convexHull2D(cloud, plane_indices[i], plane_coeff[i],
polygon);
visualizePolygon(cloud, polygon,rgb,i,visualization_pub);
Modified: pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/doors_detector.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/doors_detector.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/doors_detector.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -33,7 +33,6 @@
using namespace std;
-using namespace robot_msgs;
using namespace door_msgs;
using namespace mapping_msgs;
using namespace ros;
Modified: pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/geometric_helper.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/geometric_helper.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/geometric_helper.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -33,7 +33,6 @@
#include <door_handle_detector/geometric_helper.h>
using namespace std;
-using namespace robot_msgs;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Get a set of point indices between some specified bounds
Modified: pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -34,7 +34,6 @@
using namespace std;
using namespace ros;
-using namespace robot_msgs;
using namespace mapping_msgs;
using namespace door_msgs;
using namespace door_handle_detector;
@@ -303,7 +302,7 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void HandleDetector::refineHandleCandidatesWithDoorOutliers (vector<int> &handle_indices, vector<int> &outliers,
- const Polygon3D &polygon,
+ const geometry_msgs::Polygon &polygon,
const vector<double> &coeff, const geometry_msgs::Point32 &door_axis,
const Door& door_prior,
sensor_msgs::PointCloud& pointcloud) const
@@ -377,7 +376,7 @@
else
score /= fmin(0.0001, dist_to_side);
ROS_INFO (" Handle is found at %f [m] from the door side", dist_to_side);
-
+
if (score > best_score)
{
best_score = score;
@@ -423,7 +422,7 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void HandleDetector::getHandleCandidates (const vector<int> &indices, const vector<double> &coeff,
- const Polygon3D &polygon, const Polygon3D &polygon_tr,
+ const geometry_msgs::Polygon &polygon, const geometry_msgs::Polygon &polygon_tr,
Eigen::Matrix4d transformation, vector<int> &handle_indices,
sensor_msgs::PointCloud& pointcloud, geometry_msgs::PointStamped& viewpoint_cloud) const
{
@@ -490,8 +489,8 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void HandleDetector::getDoorOutliers (const vector<int> &indices, const vector<int> &inliers,
- const vector<double> &coeff, const Polygon3D &polygon,
- const Polygon3D &polygon_tr, Eigen::Matrix4d transformation,
+ const vector<double> &coeff, const geometry_msgs::Polygon &polygon,
+ const geometry_msgs::Polygon &polygon_tr, Eigen::Matrix4d transformation,
vector<int> &outliers, sensor_msgs::PointCloud& pointcloud) const
{
vector<int> tmp_indices; // Used as a temporary indices array
Modified: pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/cloud_downsampler.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/cloud_downsampler.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/cloud_downsampler.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -51,7 +51,6 @@
#include <sys/time.h>
using namespace std;
-using namespace robot_msgs;
class CloudDownsampler
{
Modified: pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/cloud_io/tools/bag_to_pcd.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/cloud_io/tools/bag_to_pcd.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/cloud_io/tools/bag_to_pcd.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -52,8 +52,6 @@
#include <point_cloud_mapping/cloud_io.h>
#include <point_cloud_mapping/geometry/point.h>
-using namespace robot_msgs;
-
class BagToPcd
{
protected:
Modified: pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/normal_estimation.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/normal_estimation.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/normal_estimation.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -62,7 +62,6 @@
#include <sys/time.h>
using namespace std;
-using namespace robot_msgs;
class NormalEstimation
{
Modified: pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/normal_estimation_in_proc.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/normal_estimation_in_proc.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/normal_estimation_in_proc.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -53,29 +53,28 @@
#include <point_cloud_mapping/normal_estimation_in_proc.h>
using namespace std;
-using namespace robot_msgs;
using namespace point_cloud_mapping;
NormalEstimationInProc::~NormalEstimationInProc (){}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-NormalEstimationInProc::NormalEstimationInProc()
+NormalEstimationInProc::NormalEstimationInProc()
{
node_.param ("~search_radius", radius_, 0.02); // 2cm radius by default
node_.param ("~search_k_closest", k_, 25); // 25 k-neighbors by default
node_.param ("~compute_moments", compute_moments_, false); // Do not compute moment invariants by default
-
+
node_.param ("~downsample", downsample_, 1); // Downsample cloud before normal estimation
node_.param ("~downsample_leaf_width_x", leaf_width_.x, 0.05); // 5cm radius by default
node_.param ("~downsample_leaf_width_y", leaf_width_.y, 0.05); // 5cm radius by default
node_.param ("~downsample_leaf_width_z", leaf_width_.z, 0.05); // 5cm radius by default
node_.param ("~cut_distance", cut_distance_, 10.0); // 10m by default
-
+
if (downsample_ != 0)
k_ = 10; // Reduce the size of K significantly
-
-
+
+
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -89,11 +88,11 @@
k_ = 10;
else
k_ = 25;
-
+
if (node_.hasParam ("~downsample_leaf_width_x")) node_.getParam ("~downsample_leaf_width_x", leaf_width_.x);
if (node_.hasParam ("~downsample_leaf_width_y")) node_.getParam ("~downsample_leaf_width_y", leaf_width_.y);
if (node_.hasParam ("~downsample_leaf_width_z")) node_.getParam ("~downsample_leaf_width_z", leaf_width_.z);
-
+
if (node_.hasParam ("~cut_distance"))
{
node_.getParam ("~cut_distance", cut_distance_);
@@ -116,7 +115,7 @@
viewpoint_laser.header.frame_id = "laser_tilt_mount_link";
// Set the viewpoint in the laser coordinate system to 0, 0, 0
viewpoint_laser.point.x = viewpoint_laser.point.y = viewpoint_laser.point.z = 0.0;
-
+
try
{
tf.transformPoint (cloud_frame, viewpoint_laser, viewpoint_cloud);
@@ -138,7 +137,7 @@
void NormalEstimationInProc::process_cloud (const sensor_msgs::PointCloud& cloud_, sensor_msgs::PointCloud& cloud_normals_)
{
updateParametersFromServer ();
-
+
ROS_INFO ("Received %d data points in frame %s with %d channels (%s).", (int)cloud_.pts.size (), cloud_.header.frame_id.c_str (),
(int)cloud_.chan.size (), cloud_geometry::getAvailableChannels (cloud_).c_str ());
if (cloud_.pts.size () == 0)
@@ -146,13 +145,13 @@
ROS_ERROR ("No data points found. Exiting...");
return;
}
-
+
// Figure out the viewpoint value in the cloud_frame frame
geometry_msgs::PointStamped viewpoint_cloud;
getCloudViewPoint (cloud_.header.frame_id, viewpoint_cloud, tf_);
-
+
ros::Time ts = ros::Time::now ();
-
+
// If a-priori downsampling is enabled...
if (downsample_ != 0)
{
@@ -166,10 +165,10 @@
{
// cloud_geometry::downsamplePointCloudSet (cloud_, cloud_down_, leaf_width_, d_idx, cut_distance_);
}
-
+
ROS_INFO ("Downsampling enabled. Number of points left: %d / %d in %g seconds.", (int)cloud_down_.pts.size (), (int)cloud_.pts.size (), (ros::Time::now () - ts1).toSec ());
}
-
+
// Resize
// We need to copy the original point cloud data, and this looks like a good way to do it
int original_chan_size;
@@ -184,7 +183,7 @@
cloud_normals_ = cloud_;
original_chan_size = cloud_.chan.size ();
}
-
+
// Allocate the extra needed channels
if (compute_moments_)
cloud_normals_.chan.resize (original_chan_size + 7); // Allocate 7 more channels
@@ -208,17 +207,17 @@
else
cloud_normals_.chan[d].vals.resize (cloud_.pts.size ());
}
-
+
// Create Kd-Tree
kdtree_ = new cloud_kdtree::KdTreeANN (cloud_normals_);
-
+
// Allocate enough space for point indices
points_indices_.resize (cloud_normals_.pts.size ());
for (int i = 0; i < (int)cloud_normals_.pts.size (); i++)
points_indices_[i].resize (k_);
-
+
ROS_INFO ("Kd-tree created in %g seconds.", (ros::Time::now () - ts).toSec ());
-
+
ts = ros::Time::now ();
// Get the nearest neighbors for all points
for (int i = 0; i < (int)cloud_normals_.pts.size (); i++)
@@ -227,7 +226,7 @@
kdtree_->nearestKSearch (i, k_, points_indices_[i], distances);
}
ROS_INFO ("Nearest neighbors found in %g seconds.\n", (ros::Time::now () - ts).toSec ());
-
+
#pragma omp parallel for schedule(dynamic)
for (int i = 0; i < (int)cloud_normals_.pts.size (); i++)
{
@@ -235,12 +234,12 @@
Eigen::Vector4d plane_parameters;
double curvature, j1, j2, j3;
cloud_geometry::nearest::computePointNormal (cloud_normals_, points_indices_[i], plane_parameters, curvature);
-
+
if (compute_moments_)
cloud_geometry::nearest::computeMomentInvariants (cloud_normals_, points_indices_[i], j1, j2, j3);
-
+
cloud_geometry::angles::flipNormalTowardsViewpoint (plane_parameters, cloud_normals_.pts[i], viewpoint_cloud);
-
+
cloud_normals_.chan[original_chan_size + 0].vals[i] = plane_parameters (0);
cloud_normals_.chan[original_chan_size + 1].vals[i] = plane_parameters (1);
cloud_normals_.chan[original_chan_size + 2].vals[i] = plane_parameters (2);
@@ -255,9 +254,9 @@
cloud_normals_.header=cloud_.header;
-
+
ROS_INFO ("Local features estimated in %g seconds.\n", (ros::Time::now () - ts).toSec ());
-
+
delete kdtree_;
}
Modified: pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/planar_fit.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/planar_fit.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/planar_fit.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -71,7 +71,6 @@
#include <angles/angles.h>
using namespace std;
-using namespace robot_msgs;
class PlanarFit
{
@@ -82,8 +81,8 @@
// ROS messages
sensor_msgs::PointCloud cloud_, cloud_down_, cloud_plane_, cloud_outliers_;
-
+
tf::TransformListener tf_;
// Kd-tree stuff
@@ -163,7 +162,7 @@
node_.advertise<sensor_msgs::PointCloud> ("~plane", 1);
node_.advertise<sensor_msgs::PointCloud> ("~outliers", 1);
- // A channel to visualize the normals as cute little lines
+ // A channel to visualize the normals as cute little lines
//node_.advertise<PolyLine> ("~normal_lines", 1);
node_.advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
}
@@ -514,7 +513,7 @@
marker.color.b = 1.0;
int nr_points = points.pts.size ();
-
+
marker.points.resize (2 * nr_points);
for (int i = 0; i < nr_points; i++)
{
Modified: pkg/trunk/stacks/semantic_mapping/semantic_point_annotator/src/sac_ground_removal.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/semantic_point_annotator/src/sac_ground_removal.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/stacks/semantic_mapping/semantic_point_annotator/src/sac_ground_removal.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -65,7 +65,6 @@
#include <sys/time.h>
using namespace std;
-using namespace robot_msgs;
using namespace mapping_msgs;
class GroundRemoval
Modified: pkg/trunk/stacks/semantic_mapping/semantic_point_annotator/src/sac_inc_ground_removal.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/semantic_point_annotator/src/sac_inc_ground_removal.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/stacks/semantic_mapping/semantic_point_annotator/src/sac_inc_ground_removal.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -68,7 +68,6 @@
#include <boost/thread.hpp>
using namespace std;
-using namespace robot_msgs;
using namespace mapping_msgs;
class IncGroundRemoval
@@ -281,7 +280,7 @@
{
all_indices[cp] = cp;
if (fabs (cloud_.pts[cp].z) < z_threshold_ || // max height for ground
- cloud_.pts[cp].z*cloud_.pts[cp].z < ground_slope_threshold_ * (cloud_.pts[cp].x*cloud_.pts[cp].x + cloud_.pts[cp].y*cloud_.pts[cp].y)) // max slope for ground
+ cloud_.pts[cp].z*cloud_.pts[cp].z < ground_slope_threshold_ * (cloud_.pts[cp].x*cloud_.pts[cp].x + cloud_.pts[cp].y*cloud_.pts[cp].y)) // max slope for ground
{
possible_ground_indices[nr_p] = cp;
nr_p++;
Modified: pkg/trunk/vision/recognition_lambertian/src/plane_fit.cpp
===================================================================
--- pkg/trunk/vision/recognition_lambertian/src/plane_fit.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/vision/recognition_lambertian/src/plane_fit.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -48,7 +48,6 @@
#include <angles/angles.h>
using namespace std;
-using namespace robot_msgs;
class PlanarFit
{
@@ -104,7 +103,7 @@
{
// Compute the convex hull of the area
// NOTE: this is faster than computing the concave (alpha) hull, so let's see how this works out
- Polygon3D polygon;
+ geometry_msgs::Polygon polygon;
cloud_geometry::areas::convexHull2D (points, indices[i], models[i], polygon);
// Compute the area of the polygon
Modified: pkg/trunk/vision/recognition_lambertian/src/rec_lam_normal_features.cpp
===================================================================
--- pkg/trunk/vision/recognition_lambertian/src/rec_lam_normal_features.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/vision/recognition_lambertian/src/rec_lam_normal_features.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -96,7 +96,6 @@
#include <boost/thread.hpp>
using namespace std;
-using namespace robot_msgs;
static const double pi = 3.141592653589793238462643383279502884197; // Archimedes constant pi
typedef struct _triangle_offsets
@@ -225,7 +224,7 @@
{
// Compute the convex hull of the area
// NOTE: this is faster than computing the concave (alpha) hull, so let's see how this works out
- Polygon3D polygon;
+ geometry_msgs::Polygon polygon;
cloud_geometry::areas::convexHull2D (points, indices[i], models[i], polygon);
// Compute the area of the polygon
Modified: pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
===================================================================
--- pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp 2009-08-07 20:52:44 UTC (rev 21045)
+++ pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp 2009-08-07 20:53:57 UTC (rev 21046)
@@ -92,7 +92,6 @@
//#include "flann.h"
-using namespace robot_msgs;
using namespace std;
#define CV_PIXEL(type,img,x,y) (((type*)(img->imageData+y*img->widthStep))+x*img->nChannels)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
