Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23275] pkg/trunk
|
From: <mar...@us...> - 2009-08-28 20:27:17
|
Revision: 23275
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23275&view=rev
Author: mariusmuja
Date: 2009-08-28 20:27:09 +0000 (Fri, 28 Aug 2009)
Log Message:
-----------
Changes to tabletop_objects
Modified Paths:
--------------
pkg/trunk/sandbox/tabletop_objects/CMakeLists.txt
pkg/trunk/sandbox/tabletop_objects/launch/model_fitter.launch
pkg/trunk/sandbox/tabletop_objects/src/model_fitter.cpp
pkg/trunk/sandbox/tabletop_objects/src/tabletop_detector.cpp
pkg/trunk/sandbox/tabletop_objects/srv/ModelFit.srv
pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/cloud_geometry/transforms.cpp
Modified: pkg/trunk/sandbox/tabletop_objects/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/CMakeLists.txt 2009-08-28 20:26:25 UTC (rev 23274)
+++ pkg/trunk/sandbox/tabletop_objects/CMakeLists.txt 2009-08-28 20:27:09 UTC (rev 23275)
@@ -34,6 +34,8 @@
rosbuild_add_executable(tabletop_detector src/tabletop_detector.cpp)
rosbuild_add_executable(model_fitter src/model_fitter.cpp src/ply.c src/mesh_loader.cpp)
rosbuild_link_boost(model_fitter filesystem)
+rosbuild_add_executable(model_tracker src/model_tracker.cpp src/ply.c src/mesh_loader.cpp)
+rosbuild_link_boost(model_tracker filesystem)
rosbuild_add_executable(publish_scene src/publish_scene.cpp)
rosbuild_add_executable(compute_features src/compute_features.cpp src/ply.c src/mesh_loader.cpp)
rosbuild_link_boost(compute_features filesystem)
Modified: pkg/trunk/sandbox/tabletop_objects/launch/model_fitter.launch
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/launch/model_fitter.launch 2009-08-28 20:26:25 UTC (rev 23274)
+++ pkg/trunk/sandbox/tabletop_objects/launch/model_fitter.launch 2009-08-28 20:27:09 UTC (rev 23275)
@@ -6,7 +6,7 @@
<param name="target_frame" value="base_link" />
</node>
- <node pkg="tabletop_objects" name="model_fitter" type="model_fitter" respawn="true" output="screen">
+ <node pkg="tabletop_objects" name="model_fitter" type="model_fitter" respawn="false" output="screen">
<param name="display" type="bool" value="true" />
<param name="template_path" value="$(find tabletop_objects)/meshes" />
</node>
Modified: pkg/trunk/sandbox/tabletop_objects/src/model_fitter.cpp
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/src/model_fitter.cpp 2009-08-28 20:26:25 UTC (rev 23274)
+++ pkg/trunk/sandbox/tabletop_objects/src/model_fitter.cpp 2009-08-28 20:27:09 UTC (rev 23275)
@@ -71,6 +71,7 @@
public:
distance_field::PropagationDistanceField* distance_voxel_grid_;
float truncate_value;
+ string name_;
mapping_msgs::Object mesh_;
geometry_msgs::Point min_, max_;
@@ -228,6 +229,8 @@
void TemplateModel::load(const string& file)
{
+ name_ = bfs::basename(file);
+
PLYModelLoader modle_loader;
modle_loader.readFromFile(file,mesh_);
@@ -437,6 +440,7 @@
TemplateModel* tpl = new TemplateModel();
tpl->load(filename);
templates.push_back(tpl);
+
}
void loadTemplateModels(const string& path)
@@ -488,6 +492,7 @@
resp.object.object_pose.header.frame_id = req.cloud.header.frame_id;
resp.object.object = mfs.best_fit_[0].model_->objectMesh();
resp.score = mfs.best_fit_[0].score();
+ resp.model_name = mfs.best_fit_[0].model_->name_;
Modified: pkg/trunk/sandbox/tabletop_objects/src/tabletop_detector.cpp
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/src/tabletop_detector.cpp 2009-08-28 20:26:25 UTC (rev 23274)
+++ pkg/trunk/sandbox/tabletop_objects/src/tabletop_detector.cpp 2009-08-28 20:27:09 UTC (rev 23275)
@@ -72,6 +72,7 @@
#include "geometry_msgs/Point32.h"
#include "geometry_msgs/PointStamped.h"
#include "visualization_msgs/Marker.h"
+#include "tabletop_objects/TrackObject.h"
#include <string>
@@ -122,6 +123,7 @@
ros::Publisher object_pub_;
ros::Publisher objects_pub_;
ros::Publisher marker_pub_;
+ ros::Publisher track_object_pub_;
ros::ServiceServer service_;
@@ -199,6 +201,7 @@
// advertise<sensor_msgs::PointCloud> ("~outliers", 1);
// advertise<sensor_msgs::PointCloud> ("~inliers", 1);
marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker",1);
+ track_object_pub_ = nh_.advertise<tabletop_objects::TrackObject>("track_object",1);
ros::AdvertiseServiceOptions service_opts = ros::AdvertiseServiceOptions::create<tabletop_objects::FindObjectPoses>("table_top/find_object_poses",
boost::bind(&TabletopDetector::findObjectPoses, this, _1, _2),ros::VoidPtr(), &service_queue_);
@@ -1131,11 +1134,12 @@
}
- void fitModels(const vector<sensor_msgs::PointCloud>& clouds, vector<tabletop_msgs::TableTopObject>& objects)
+ void fitModels(const vector<sensor_msgs::PointCloud>& clouds, vector<tabletop_msgs::TableTopObject>& objects, vector<string>& names)
{
int count = 0;
objects.resize(clouds.size());
+ names.resize(clouds.size());
for (size_t k=0;k<clouds.size();++k) {
if (display_) {
@@ -1152,7 +1156,9 @@
// TODO: change this to real-world average distance error
if (resp.score<5.0) {
- objects[count++] = resp.object;
+ objects[count] = resp.object;
+ names[count] = resp.model_name;
+ count++;
}
}
else {
@@ -1161,6 +1167,7 @@
}
objects.resize(count);
+ names.resize(count);
}
@@ -1211,9 +1218,17 @@
vector<sensor_msgs::PointCloud> clouds;
findTabletopClusters(objects_table_frame, centers, clouds);
- fitModels(clouds, objects);
+ vector<string> names;
+ fitModels(clouds, objects, names);
+ if (objects.size()!=0) {
+ tabletop_objects::TrackObject track_object;
+ track_object.pose = objects[0].object_pose;
+ track_object.object_id = names[0];
+ track_object_pub_.publish(track_object);
+ }
objects_pub_.publish(objects_table_frame);
+
}
Modified: pkg/trunk/sandbox/tabletop_objects/srv/ModelFit.srv
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/srv/ModelFit.srv 2009-08-28 20:26:25 UTC (rev 23274)
+++ pkg/trunk/sandbox/tabletop_objects/srv/ModelFit.srv 2009-08-28 20:27:09 UTC (rev 23275)
@@ -1,4 +1,5 @@
sensor_msgs/PointCloud cloud
---
tabletop_msgs/TableTopObject object
+string model_name
float32 score
Modified: pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-08-28 20:26:25 UTC (rev 23274)
+++ pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-08-28 20:27:09 UTC (rev 23275)
@@ -278,7 +278,7 @@
void leftImageCallback(const sensor_msgs::Image::ConstPtr& image)
{
-// ROS_INFO("got left image callback");
+ //ROS_INFO("got left image callback: %lld", image->header.stamp.toNSec());
boost::unique_lock<boost::mutex> lock(data_lock_);
limage_ = image;
@@ -290,14 +290,14 @@
void disparityImageCallback(const sensor_msgs::Image::ConstPtr& image)
{
boost::unique_lock<boost::mutex> lock(data_lock_);
-// ROS_INFO("got disparity callback");
+ //ROS_INFO("got disparity callback: %lld", image->header.stamp.toNSec());
dimage_ = image;
}
void dispinfoCallback(const stereo_msgs::DisparityInfo::ConstPtr& dinfo)
{
boost::unique_lock<boost::mutex> lock(data_lock_);
-// ROS_INFO("got dispinfo callback");
+ //ROS_INFO("got dispinfo callback: %lld", dinfo->header.stamp.toNSec());
dispinfo_ = dinfo;
}
@@ -309,7 +309,7 @@
void cloudCallback(const sensor_msgs::PointCloud::ConstPtr& point_cloud)
{
boost::unique_lock<boost::mutex> lock(data_lock_);
-// ROS_INFO("got cloud callback");
+ //ROS_INFO("got cloud callback: %lld", point_cloud->header.stamp.toNSec());
cloud_ = point_cloud;
}
@@ -828,7 +828,7 @@
boost::unique_lock<boost::mutex> lock(data_lock_);
for (int i=0;i<frames_no_;++i) {
- ROS_INFO("Stereo Handle Detector: Waiting for stereo data");
+ ROS_INFO("DoorHandleDetector: Waiting for stereo data");
got_images_ = false;
preempted_ = false;
// block until images are available to process
@@ -837,7 +837,7 @@
data_cv_.wait(lock);
}
if (preempted_) {
- ROS_INFO("Stereo Handle Detector: detect loop preempted, stereo data not received");
+ ROS_INFO("DoorHandleDetector: detect loop preempted, stereo data not received");
return false;
}
@@ -845,7 +845,7 @@
// show original disparity
cvShowImage("disparity_original", disp_);
}
- ROS_INFO("Stereo Handle Detector: Running handle detection");
+ ROS_INFO("DoorHandleDetector: Running handle detection");
// eliminate from disparity locations that cannot contain a handle
applyPositionPrior();
// run cascade classifier
Modified: pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/cloud_geometry/transforms.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/cloud_geometry/transforms.cpp 2009-08-28 20:26:25 UTC (rev 23274)
+++ pkg/trunk/stacks/semantic_mapping/point_cloud_mapping/src/cloud_geometry/transforms.cpp 2009-08-28 20:27:09 UTC (rev 23275)
@@ -202,6 +202,12 @@
transformation(1,3) = centroid_b.y - centroid_rotated_a.y;
transformation(2,3) = centroid_b.z - centroid_rotated_a.z;
+
+// transformation.setIdentity();
+// transformation(0,3) = centroid_b.x - centroid_a.x;
+// transformation(1,3) = centroid_b.y - centroid_a.y;
+// transformation(2,3) = centroid_b.z - centroid_a.z;
+
return true;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
