Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23276] pkg/trunk
|
From: <mar...@us...> - 2009-08-28 20:28:13
|
Revision: 23276
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23276&view=rev
Author: mariusmuja
Date: 2009-08-28 20:28:02 +0000 (Fri, 28 Aug 2009)
Log Message:
-----------
3D model tracker added 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/launch/publish_objects.launch
pkg/trunk/sandbox/tabletop_objects/src/tabletop_detector.cpp
pkg/trunk/stacks/imaging_pipeline/stereo_image_proc/narrow_stereoproc.launch
Added Paths:
-----------
pkg/trunk/sandbox/tabletop_objects/launch/model_tracker.launch
pkg/trunk/sandbox/tabletop_objects/src/model_tracker.cpp
pkg/trunk/sandbox/tabletop_objects/src/spin_features.cpp
pkg/trunk/sandbox/tabletop_objects/src/tabletop_chamfer_match.cpp
pkg/trunk/sandbox/tabletop_objects/src/test_service.cpp
Removed Paths:
-------------
pkg/trunk/sandbox/tabletop_objects/src/compute_features.cpp
Modified: pkg/trunk/sandbox/tabletop_objects/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/CMakeLists.txt 2009-08-28 20:27:09 UTC (rev 23275)
+++ pkg/trunk/sandbox/tabletop_objects/CMakeLists.txt 2009-08-28 20:28:02 UTC (rev 23276)
@@ -37,5 +37,6 @@
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)
+rosbuild_add_executable(spin_features src/spin_features.cpp src/ply.c src/mesh_loader.cpp)
+rosbuild_link_boost(spin_features filesystem)
+rosbuild_add_executable(test_service src/test_service.cpp)
Modified: pkg/trunk/sandbox/tabletop_objects/launch/model_fitter.launch
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/launch/model_fitter.launch 2009-08-28 20:27:09 UTC (rev 23275)
+++ pkg/trunk/sandbox/tabletop_objects/launch/model_fitter.launch 2009-08-28 20:28:02 UTC (rev 23276)
@@ -1,9 +1,9 @@
<launch>
<node pkg="tabletop_objects" name="tabletop_detector" type="tabletop_detector" respawn="true" output="screen">
- <!--remap from="stereo" to="narrow_stereo" /-->
+ <remap from="stereo" to="narrow_stereo" />
<param name="display" type="bool" value="false" />
- <param name="target_frame" value="base_link" />
+ <param name="target_frame" value="narrow_stereo_optical_frame" />
</node>
<node pkg="tabletop_objects" name="model_fitter" type="model_fitter" respawn="false" output="screen">
Added: pkg/trunk/sandbox/tabletop_objects/launch/model_tracker.launch
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/launch/model_tracker.launch (rev 0)
+++ pkg/trunk/sandbox/tabletop_objects/launch/model_tracker.launch 2009-08-28 20:28:02 UTC (rev 23276)
@@ -0,0 +1,10 @@
+
+<launch>
+ <node pkg="tabletop_objects" name="model_tracker" type="model_tracker" respawn="false" output="screen">
+ <remap from="stereo" to="narrow_stereo" />
+ <param name="display" type="bool" value="true" />
+ <param name="template_path" value="$(find tabletop_objects)/meshes" />
+ </node>
+
+</launch>
+
Modified: pkg/trunk/sandbox/tabletop_objects/launch/publish_objects.launch
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/launch/publish_objects.launch 2009-08-28 20:27:09 UTC (rev 23275)
+++ pkg/trunk/sandbox/tabletop_objects/launch/publish_objects.launch 2009-08-28 20:28:02 UTC (rev 23276)
@@ -2,7 +2,7 @@
<launch>
<node pkg="tabletop_objects" name="publish_scene" type="publish_scene" respawn="false" output="screen"/>
- <node pkg="tabletop_objects" type="publish_stereo_data.py" respawn="false" output="screen"/>
+ <node pkg="tabletop_objects" name="publish_scene_gui" type="publish_stereo_data.py" respawn="false" output="screen"/>
</launch>
Deleted: pkg/trunk/sandbox/tabletop_objects/src/compute_features.cpp
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/src/compute_features.cpp 2009-08-28 20:27:09 UTC (rev 23275)
+++ pkg/trunk/sandbox/tabletop_objects/src/compute_features.cpp 2009-08-28 20:28:02 UTC (rev 23276)
@@ -1,247 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-#include <boost/filesystem.hpp>
-
-#include <ros/ros.h>
-#include "tabletop_objects/mesh_loader.h"
-#include "mapping_msgs/Object.h"
-#include "sensor_msgs/PointCloud.h"
-#include "geometry_msgs/Point.h"
-#include "visualization_msgs/Marker.h"
-
-#include "opencv/cvaux.h"
-
-#include <string>
-
-using namespace std;
-namespace bfs = boost::filesystem;
-
-
-class ComputeFeatures
-{
- cv::Mesh3D* model_mesh_;
- cv::SpinImageModel* model_;
-
- ros::NodeHandle nh_;
- ros::Subscriber cloud_sub_;
- ros::Publisher marker_pub_;
-
- sensor_msgs::PointCloud::ConstPtr cloud_;
-
-public:
-
- ComputeFeatures()
- {
- cloud_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/cloud", 1, &ComputeFeatures::cloudCallback, this);
- marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker",1);
- }
-
- void cloudCallback(const sensor_msgs::PointCloud::ConstPtr& point_cloud)
- {
- cloud_ = point_cloud;
-
- matchModel();
- }
-
- void loadModel(const string& filename)
- {
- mapping_msgs::Object mesh;
- PLYModelLoader modle_loader;
- modle_loader.readFromFile(filename,mesh);
-
- vector<cv::Point3f> points;
- points.resize(mesh.vertices.size());
- for (size_t i=0;i<mesh.vertices.size();++i) {
- points[i] = cv::Point3f(mesh.vertices[i].x,mesh.vertices[i].y,mesh.vertices[i].z);
- }
-
- model_mesh_ = new cv::Mesh3D(points);
- model_ = new cv::SpinImageModel(*model_mesh_);
- ROS_INFO("Constructing spin image model");
- model_->selectRandomSubset(0.02);
- model_->compute();
- ROS_INFO("Model has %d spin images", model_->getSpinCount());
- }
-
-
- void visualize(const vector<cv::Vec2i> points)
- {
- visualization_msgs::Marker marker;
- marker.header.stamp = cloud_->header.stamp;
- marker.header.frame_id = cloud_->header.frame_id;
- marker.ns = "spin_points";
- marker.id = 0;
- marker.type = visualization_msgs::Marker::POINTS;
- marker.action = visualization_msgs::Marker::ADD;
- marker.pose.position.x = 0;
- marker.pose.position.y = 0;
- marker.pose.position.z = 0;
- marker.pose.orientation.x = 0.0;
- marker.pose.orientation.y = 0.0;
- marker.pose.orientation.z = 0.0;
- marker.pose.orientation.w = 1.0;
- marker.scale.x = 0.003;
- marker.scale.y = 0.003;
- marker.scale.z = 0.003;
- marker.color.a = 1.0;
- marker.color.r = 0.0;
- marker.color.g = 1.0;
- marker.color.b = 1.0;
-
- for (size_t i=0;i<points.size();++i) {
- geometry_msgs::Point p;
- p.x = cloud_->points[points[i][1]].x;
- p.y = cloud_->points[points[i][1]].y;
- p.z = cloud_->points[points[i][1]].z;
- marker.points.push_back(p);
- }
-
- marker_pub_.publish(marker);
- }
-
- void visualizeSpinImageLocations(const cv::SpinImageModel& model)
- {
-
- visualization_msgs::Marker marker;
- marker.header.frame_id = cloud_->header.frame_id;
- marker.header.stamp = ros::Time((uint64_t)0ULL);
- marker.ns = "normals";
- marker.id = 1;
- marker.type = visualization_msgs::Marker::LINE_LIST;
- marker.action = visualization_msgs::Marker::ADD;
- marker.pose.orientation.w = 1.0;
- marker.scale.x = 0.001;
- marker.color.a = 1.0;
- marker.color.g = 1.0;
-
- marker.set_points_size(2*model.getSpinCount());
-
- for (size_t i=0;i<model.getSpinCount();++i) {
-
- cv::Point3f point = model.getSpinVertex(i);
- cv::Point3f normal = model.getSpinNormal(i);
- float alpha = 0.02;
-
- marker.points[2*i].x = point.x;
- marker.points[2*i].y = point.y;
- marker.points[2*i].z = point.z;
-
- marker.points[2*i+1].x = point.x + alpha * normal.x;
- marker.points[2*i+1].y = point.y + alpha * normal.y;
- marker.points[2*i+1].z = point.z + alpha * normal.z;
-
- }
-
- marker_pub_.publish(marker);
- }
-
-
- void matchModel()
- {
- vector<cv::Point3f> points;
- points.resize(cloud_->get_points_size());
- for (size_t i=0;i<cloud_->get_points_size();++i) {
- points[i] = cv::Point3f(cloud_->points[i].x,cloud_->points[i].y,cloud_->points[i].z);
- }
-
- cv::Mesh3D scene_mesh(points);
- cv::SpinImageModel scene(scene_mesh);
- ROS_INFO("Computing scene spin images");
- scene.selectRandomSubset(0.05);
- scene.compute();
-
- ROS_INFO("Scene has %d spin images", scene.getSpinCount());
-
-
- visualizeSpinImageLocations(scene);
-
-// vector<vector<cv::Vec2i> > matches;
-// model_->match(scene, matches);
-//
-// ROS_INFO("I have found %d match groups\n", matches.size());
-//
-// for (size_t i=0;i<matches.size();++i) {
-// ROS_INFO("\tGroup %d, size: %d", i, matches[i].size());
-// for (size_t j=0;j<matches[i].size();++j) {
-// printf ("%d ", matches[i][j][0]);
-// }
-// printf("\n");
-// for (size_t j=0;j<matches[i].size();++j) {
-// printf ("%d ", matches[i][j][1]);
-// }
-// printf("\n");
-// }
-//
-// visualize(matches[0]);
-
- }
-
- void init(const string& path)
- {
- bfs::path template_dir(path);
-
- if (!bfs::is_directory(template_dir)) {
- ROS_ERROR("Cannot load templates, %s is not a directory", template_dir.leaf().c_str());
- }
-
- bfs::directory_iterator dir_iter(template_dir), dir_end;
- for(;dir_iter != dir_end; ++dir_iter) {
- if (bfs::extension(*dir_iter)==".ply") {
- loadModel(dir_iter->string());
- }
- }
- }
-
-
-};
-
-
-
-
-int main(int argc, char** argv)
-{
- ros::init(argc,argv,"match_features");
- if (argc<2) {
- ROS_INFO("Usage: %s path", argv[0]);
- }
- ComputeFeatures cf;
-
- cf.init(string(argv[1]));
-
- ROS_INFO("Starting spinning");
- ros::spin();
-
- return 0;
-}
Added: pkg/trunk/sandbox/tabletop_objects/src/model_tracker.cpp
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/src/model_tracker.cpp (rev 0)
+++ pkg/trunk/sandbox/tabletop_objects/src/model_tracker.cpp 2009-08-28 20:28:02 UTC (rev 23276)
@@ -0,0 +1,795 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// Author: Marius Muja
+
+#include <boost/filesystem.hpp>
+#include <boost/numeric/ublas/matrix.hpp>
+
+
+#include <ros/ros.h>
+#include "tabletop_objects/ModelFit.h"
+#include "tabletop_objects/mesh_loader.h"
+
+#include "geometry_msgs/Point.h"
+#include "visualization_msgs/Marker.h"
+#include "mapping_msgs/Object.h"
+#include "tabletop_objects/TrackObject.h"
+
+#include "tf/transform_listener.h"
+#include "tf/transform_broadcaster.h"
+
+#include "distance_field/propagation_distance_field.h"
+#include <point_cloud_mapping/geometry/transforms.h>
+
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+// import most common Eigen types
+USING_PART_OF_NAMESPACE_EIGEN
+
+
+//#define PUBLISH_GRASH_POSE
+
+namespace bfs = boost::filesystem;
+using namespace std;
+
+
+
+namespace model_fit {
+
+
+
+class ModelFitSet;
+
+class TemplateModel {
+
+public:
+ distance_field::PropagationDistanceField* distance_voxel_grid_;
+ float truncate_value;
+ mapping_msgs::Object mesh_;
+
+ geometry_msgs::Point min_, max_;
+
+ TemplateModel() : truncate_value(0.10)
+ {
+
+ }
+
+ ~TemplateModel()
+ {
+ delete distance_voxel_grid_;
+ }
+
+ void load(const string& file);
+
+ geometry_msgs::Pose graspPose()
+ {
+ // TODO: fix this, models annotated with pose
+ geometry_msgs::Pose pose;
+ pose.position.x = 0;
+ pose.position.y = 0;
+ pose.position.z = 0.05;
+
+
+ btMatrix3x3 rotation;
+ rotation.setIdentity();
+ rotation.setEulerZYX(M_PI/2,0,0);
+ btQuaternion orientation;
+ rotation.getRotation(orientation);
+
+ pose.orientation.x = orientation.x();
+ pose.orientation.y = orientation.y();
+ pose.orientation.z = orientation.z();
+ pose.orientation.w = orientation.w();
+
+ return pose;
+ }
+
+ void getExtents(geometry_msgs::Point& low_extent, geometry_msgs::Point& high_extent)
+ {
+ low_extent = min_;
+ high_extent = max_;
+ }
+
+ mapping_msgs::Object objectMesh()
+ {
+ return mesh_;
+ }
+
+ void show(const ros::Publisher& publisher, const geometry_msgs::PoseStamped& pose, float fit_score);
+
+ bool fitPointCloud(sensor_msgs::PointCloud& cloud, ModelFitSet& mfs, Eigen::Matrix4d& transform, double& best_score);
+
+ void findBestFit(sensor_msgs::PointCloud& cloud, ModelFitSet& mfs);
+};
+
+
+class ModelFitSet
+{
+public:
+ struct ModelFit
+ {
+ TemplateModel* model_;
+ Eigen::Matrix4d transform_;
+ float score_;
+ float max_dist_;
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ ModelFit() : model_(NULL)
+ {
+ }
+
+ ModelFit(TemplateModel* model, const Eigen::Matrix4d& transform, float score, float max_dist) :
+ model_(model), transform_(transform), score_(score), max_dist_(max_dist)
+ {
+ }
+
+ float score() {
+ return score_;
+ }
+
+ geometry_msgs::Pose graspPose()
+ {
+ geometry_msgs::Pose object_pose = objectPose();
+ geometry_msgs::Pose model_grasp_pose = model_->graspPose();
+ tf::Pose tf_object_pose;
+ tf::Pose tf_model_grasp_pose;
+
+ tf::poseMsgToTF(object_pose, tf_object_pose);
+ tf::poseMsgToTF(model_grasp_pose, tf_model_grasp_pose);
+
+ tf::Pose tf_grasp_pose = tf_object_pose*tf_model_grasp_pose;
+ geometry_msgs::Pose grasp_pose;
+
+ tf::poseTFToMsg(tf_grasp_pose, grasp_pose);
+
+
+ return grasp_pose;
+ }
+
+ geometry_msgs::Pose objectPose()
+ {
+ geometry_msgs::Pose pose;
+ pose.position.x = transform_(0,3);
+ pose.position.y = transform_(1,3);
+ pose.position.z = transform_(2,3);
+
+
+ Eigen::Quaterniond q(transform_.corner<3,3>(Eigen::TopLeft));
+ pose.orientation.x = q.x();
+ pose.orientation.y = q.y();
+ pose.orientation.z = q.z();
+ pose.orientation.w = q.w();
+
+ return pose;
+ }
+
+ };
+ int size_;
+ int count_;
+ ModelFit* best_fit_;
+
+ ModelFitSet(int size) : size_(size), count_(0)
+ {
+ best_fit_ = new ModelFit[size_];
+ }
+
+ void add(TemplateModel* model, const Eigen::Matrix4d& transform, float score, float max_dist)
+ {
+ ModelFit crt(model,transform, score, max_dist);
+
+ if (count_ < size_) {
+ best_fit_[count_++] = crt;
+ }
+ else if (best_fit_[size_-1].score()>crt.score()) {
+ int i=size_-1;
+ while (i>0 && best_fit_[i].score()>crt.score()) {
+ best_fit_[i] = best_fit_[i-1];
+ i--;
+ }
+ best_fit_[i] = crt;
+ }
+ }
+
+ float bestScore()
+ {
+ if (count_==0) {
+ return numeric_limits<float>::max();
+ }
+ else {
+ return best_fit_[0].score_;
+ }
+ }
+
+
+};
+
+
+
+void TemplateModel::load(const string& file)
+{
+ PLYModelLoader modle_loader;
+ modle_loader.readFromFile(file,mesh_);
+
+ if (mesh_.vertices.size()>0) {
+ min_ = mesh_.vertices[0];
+ max_ = mesh_.vertices[0];
+
+ for (size_t i=0;i<mesh_.vertices.size();++i) {
+
+ if (min_.x > mesh_.vertices[i].x) min_.x = mesh_.vertices[i].x;
+ if (min_.y > mesh_.vertices[i].y) min_.y = mesh_.vertices[i].y;
+ if (min_.z > mesh_.vertices[i].z) min_.z = mesh_.vertices[i].z;
+ if (max_.x < mesh_.vertices[i].x) max_.x = mesh_.vertices[i].x;
+ if (max_.y < mesh_.vertices[i].y) max_.y = mesh_.vertices[i].y;
+ if (max_.z < mesh_.vertices[i].z) max_.z = mesh_.vertices[i].z;
+ }
+ }
+
+ ROS_INFO("Size: (%g,%g,%g, %g, %g, %g)\n",min_.x, min_.y, min_.z, max_.x, max_.y, max_.z);
+
+ distance_voxel_grid_ = new distance_field::PropagationDistanceField(max_.x-min_.x,max_.y-min_.y, max_.z-min_.z, 0.002, min_.x,min_.y,min_.z, 1.0 );
+
+ std::vector<btVector3> points;
+ points.reserve(mesh_.vertices.size());
+ for (size_t i=0; i<mesh_.vertices.size(); ++i)
+ {
+ points.push_back(btVector3(mesh_.vertices[i].x,mesh_.vertices[i].y,mesh_.vertices[i].z));
+ }
+ distance_voxel_grid_->reset();
+ distance_voxel_grid_->addPointsToField(points);
+}
+
+
+void TemplateModel::show(const ros::Publisher& publisher, const geometry_msgs::PoseStamped& pose, float fit_score)
+{
+ visualization_msgs::Marker marker;
+ marker.header.stamp = pose.header.stamp;
+ marker.header.frame_id = pose.header.frame_id;
+ marker.ns = "voxel_grid";
+ marker.id = 100;
+ marker.type = visualization_msgs::Marker::POINTS;
+ marker.action = visualization_msgs::Marker::ADD;
+ marker.pose = pose.pose;
+ marker.scale.x = distance_voxel_grid_->getResolution(distance_field::PropagationDistanceField::DIM_X);
+ marker.scale.y = distance_voxel_grid_->getResolution(distance_field::PropagationDistanceField::DIM_Y);
+ marker.scale.z = distance_voxel_grid_->getResolution(distance_field::PropagationDistanceField::DIM_Z);
+ marker.color.a = 1.0;
+
+
+ marker.color.r = 1.0;
+ marker.color.g = 0.0;
+ marker.color.b = 1.0;
+
+ for (int i=0;i<distance_voxel_grid_->getNumCells(distance_field::PropagationDistanceField::DIM_X);++i) {
+ for (int j=0;j<distance_voxel_grid_->getNumCells(distance_field::PropagationDistanceField::DIM_Y);++j) {
+ for (int k=0;k<distance_voxel_grid_->getNumCells(distance_field::PropagationDistanceField::DIM_Z);++k) {
+
+ if (distance_voxel_grid_->getDistanceFromCell(i,j,k)<0.001) {
+ geometry_msgs::Point p;
+ distance_voxel_grid_->gridToWorld(i,j,k, p.x,p.y,p.z);
+ marker.points.push_back(p);
+ }
+ }
+ }
+ }
+ publisher.publish(marker);
+}
+
+
+bool TemplateModel::fitPointCloud(sensor_msgs::PointCloud& cloud, ModelFitSet& mfs, Eigen::Matrix4d& global_transform, double& best_score)
+{
+ double score = 0;
+ double max_dist = 0;
+
+ sensor_msgs::PointCloud closest;
+ vector<int> indices_points;
+ vector<int> indices_closest;
+ indices_points.reserve(cloud.points.size());
+
+ // compute the cost of the current fit
+ for (size_t i=0;i<cloud.points.size();i++) {
+ double wx = cloud.points[i].x;
+ double wy = cloud.points[i].y;
+ double wz = cloud.points[i].z;
+
+ int x, y, z;
+ double val;
+ if (distance_voxel_grid_->worldToGrid(wx,wy,wz,x,y,z)) {
+
+ distance_field::PropDistanceFieldVoxel& voxel = distance_voxel_grid_->getCell(x,y,z);
+ double cx, cy, cz;
+ distance_voxel_grid_->gridToWorld(voxel.closest_point_[0],voxel.closest_point_[1],voxel.closest_point_[2],
+ cx,cy,cz);
+
+ indices_points.push_back(i);
+ geometry_msgs::Point32 closest_point;
+ closest_point.x = cx;
+ closest_point.y = cy;
+ closest_point.z = cz;
+ closest.points.push_back(closest_point);
+
+ val = distance_voxel_grid_->getDistanceFromCell(x,y,z);
+ }
+ else {
+ val = truncate_value;
+ }
+ if (val>truncate_value) {
+ val = truncate_value;
+ }
+ max_dist = max(max_dist,val);
+ score += val;
+ }
+ score /= (cloud.points.size());
+
+ // if cost worst than the best so far, stop local minima
+ if (score>=best_score) {
+ return false;
+ }
+
+ best_score = score;
+
+ printf("Score: %g\n", score);
+
+ // got better cost, add transform to set
+ Eigen::Matrix4d transform;
+ cloud_geometry::transforms::getInverseTransformation(global_transform, transform);
+ mfs.add(this, transform, score, max_dist);
+
+ printf("Computing transform\n");
+
+ // continue refining transform
+ indices_closest.resize(indices_points.size());
+ for (size_t i = 0;i<indices_points.size();++i) {
+ indices_closest[i] = i;
+ }
+
+ cloud_geometry::transforms::getPointsRigidTransformation(cloud, indices_points, closest, indices_closest, transform);
+ global_transform *= transform;
+
+ for (size_t i=0;i<cloud.points.size();i++) {
+ geometry_msgs::Point32 transformed_point;
+ cloud_geometry::transforms::transformPoint(cloud.points[i],transformed_point, transform);
+ cloud.points[i] = transformed_point;
+ }
+
+ return true;
+}
+
+
+
+void visualize(const sensor_msgs::PointCloud& pc)
+{
+ ros::NodeHandle nh;
+
+ static ros::Publisher cloud_pub;
+
+ if (!cloud_pub) {
+ cloud_pub = nh.advertise<sensor_msgs::PointCloud>("debug_cloud", 1, true);
+ }
+
+ ros::Duration(0.5).sleep();
+
+ cloud_pub.publish(pc);
+}
+
+
+void visualize(ros::Publisher pub,const geometry_msgs::PoseStamped& pose)
+{
+ static int id = 200;
+ visualization_msgs::Marker marker;
+ marker.header.frame_id = pose.header.frame_id;
+ marker.header.stamp = pose.header.stamp;
+ marker.ns = "voxel_grid";
+ marker.id = id++;
+ marker.type = visualization_msgs::Marker::SPHERE;
+ marker.action = visualization_msgs::Marker::ADD;
+ marker.pose = pose.pose;
+ marker.scale.x = 0.05;
+ marker.scale.y = 0.05;
+ marker.scale.z = 0.05;
+ marker.color.a = 1.0;
+
+
+ marker.color.r = 1.0;
+ marker.color.g = 0.0;
+ marker.color.b = 1.0;
+ pub.publish(marker);
+}
+
+
+
+ros::Publisher getMarkerPublisher()
+{
+ ros::NodeHandle nh;
+ static ros::Publisher marker_pub;
+ if (!marker_pub) {
+ marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 1, true);
+ }
+
+ return marker_pub;
+}
+
+
+/** \brief Convert the transform to a Homogeneous matrix for large operations */
+static boost::numeric::ublas::matrix<double> transformAsMatrix(const tf::Transform& bt)
+{
+ boost::numeric::ublas::matrix<double> outMat(4,4);
+
+ // double * mat = outMat.Store();
+
+ double mv[12];
+ bt.getBasis().getOpenGLSubMatrix(mv);
+
+ tf::Vector3 origin = bt.getOrigin();
+
+ outMat(0,0)= mv[0];
+ outMat(0,1) = mv[4];
+ outMat(0,2) = mv[8];
+ outMat(1,0) = mv[1];
+ outMat(1,1) = mv[5];
+ outMat(1,2) = mv[9];
+ outMat(2,0) = mv[2];
+ outMat(2,1) = mv[6];
+ outMat(2,2) = mv[10];
+
+ outMat(3,0) = outMat(3,1) = outMat(3,2) = 0;
+ outMat(0,3) = origin.x();
+ outMat(1,3) = origin.y();
+ outMat(2,3) = origin.z();
+ outMat(3,3) = 1;
+
+
+ return outMat;
+};
+
+
+void transformPointCloud(const tf::Transform& net_transform, const sensor_msgs::PointCloud & cloudIn, sensor_msgs::PointCloud & cloudOut)
+{
+ boost::numeric::ublas::matrix<double> transform = transformAsMatrix(net_transform);
+
+ unsigned int length = cloudIn.get_points_size();
+
+ boost::numeric::ublas::matrix<double> matIn(4, length);
+
+ double * matrixPtr = matIn.data().begin();
+
+ for (unsigned int i = 0; i < length ; i++)
+ {
+ matrixPtr[i] = cloudIn.points[i].x;
+ matrixPtr[i+length] = cloudIn.points[i].y;
+ matrixPtr[i+ 2* length] = cloudIn.points[i].z;
+ matrixPtr[i+ 3* length] = 1;
+ };
+
+ boost::numeric::ublas::matrix<double> matOut = prod(transform, matIn);
+
+ // Copy relevant data from cloudIn, if needed
+ if (&cloudIn != &cloudOut)
+ {
+ cloudOut.header = cloudIn.header;
+ cloudOut.set_points_size(length);
+ cloudOut.set_channels_size(cloudIn.get_channels_size());
+ for (unsigned int i = 0 ; i < cloudIn.get_channels_size() ; ++i)
+ cloudOut.channels[i] = cloudIn.channels[i];
+ }
+
+ matrixPtr = matOut.data().begin();
+
+ //Override the positions
+ for (unsigned int i = 0; i < length ; i++)
+ {
+ cloudOut.points[i].x = matrixPtr[i];
+ cloudOut.points[i].y = matrixPtr[i + length];
+ cloudOut.points[i].z = matrixPtr[i + 2* length];
+ };
+}
+
+
+void TemplateModel::findBestFit(sensor_msgs::PointCloud& cloud, ModelFitSet& mfs)
+{
+
+// visualize(getMarkerPublisher(), pose_stamped);
+// show(getMarkerPublisher(), pose_stamped, 1);
+//
+//
+// geometry_msgs::PoseStamped ps;
+// ps.header = pose_stamped.header;
+// ps.pose.orientation.w = 1.0;
+// visualize(getMarkerPublisher(), ps);
+// show(getMarkerPublisher(), ps, 1);
+//
+// transformPointCloud(transf.inverse(), cloud, new_cloud);
+// // cloud_geometry::transforms::transformPoints(cloud.points, new_cloud.points, cloud_transform);
+
+// visualize(new_cloud);
+
+ Eigen::Matrix4d cloud_transform;
+ cloud_transform.setIdentity();
+
+ double best_score = numeric_limits<double>::max();
+ while (fitPointCloud(cloud, mfs, cloud_transform, best_score)) ;
+}
+
+
+
+
+
+class ModelTracker
+{
+ ros::NodeHandle nh_;
+ ros::ServiceServer service_;
+ ros::Publisher marker_pub_;
+
+ ros::Subscriber cloud_sub_;
+ ros::Subscriber track_obj_sub_;
+
+ string template_path;
+
+ vector<TemplateModel*> templates;
+
+ sensor_msgs::PointCloudConstPtr cloud_;
+
+ geometry_msgs::PoseStamped object_pose_;
+ std::string object_id_;
+ int object_index_;
+ bool tracking_on_;
+
+ TemplateModel* track_template_;
+
+
+ tf::TransformListener tf_;
+ tf::TransformBroadcaster tb_;
+
+ map<string, int> model_to_index_;
+
+#ifdef PUBLISH_GRASH_POSE
+ tf::TransformBroadcaster tfb_;
+#endif
+
+public:
+ ModelTracker()
+ {
+ nh_.param<string>("~template_path", template_path, "");
+
+ cloud_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/cloud", 1, &ModelTracker::cloudCallback, this);
+ track_obj_sub_ = nh_.subscribe("track_object", 1, &ModelTracker::trackObjectCallback, this);
+
+ marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker",1);
+
+ loadTemplateModels(template_path);
+
+ tracking_on_ = false;
+ }
+
+ ~ModelTracker()
+ {
+ for (size_t i=0;i<templates.size();++i) {
+ delete templates[i];
+ }
+ }
+
+ void cloudCallback(const sensor_msgs::PointCloud::ConstPtr& point_cloud)
+ {
+ cloud_ = point_cloud;
+ ROS_INFO("Got cloud");
+
+ if (tracking_on_) {
+ trackModel();
+ }
+ }
+
+ void printPose(const geometry_msgs::PoseStamped& pose)
+ {
+ ROS_INFO("Pose: %s\nPosition: (%g, %g, %g)\nOrientation: (%g, %g, %g, %g)",
+ pose.header.frame_id.c_str(), pose.pose.position.x, pose.pose.position.y, pose.pose.position.z,
+ pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w);
+
+ }
+
+ void trackObjectCallback(const tabletop_objects::TrackObject::ConstPtr& object)
+ {
+
+ object_pose_ = object->pose;
+ object_id_ = object->object_id;
+
+ // publish tracking TF frame
+ tf::Stamped<tf::Pose> transform;
+ tf::poseStampedMsgToTF(object_pose_, transform);
+ transform.parent_id_ = transform.frame_id_;
+ transform.frame_id_ = "tracking_frame";
+ tf_.setTransform(transform);
+ tb_.sendTransform(transform);
+
+
+
+ if (model_to_index_.find(object_id_) != model_to_index_.end()) {
+ object_index_ = model_to_index_[object_id_];
+ tracking_on_ = true;
+ track_template_ = templates[object_index_];
+
+ ROS_INFO("Starting tracker");
+ }
+
+ }
+
+
+
+ void loadTemplate(const string& filename)
+ {
+ TemplateModel* tpl = new TemplateModel();
+ tpl->load(filename);
+ templates.push_back(tpl);
+
+ // keep mapping from mode_id (model name) to index
+ std::string model_id = bfs::basename(filename);
+ model_to_index_[model_id] = templates.size()-1;
+ }
+
+ void loadTemplateModels(const string& path)
+ {
+ bfs::path template_dir(path);
+
+ if (!bfs::is_directory(template_dir)) {
+ ROS_ERROR("Cannot load templates, %s is not a directory", template_dir.leaf().c_str());
+ }
+
+ bfs::directory_iterator dir_iter(template_dir), dir_end;
+ for(;dir_iter != dir_end; ++dir_iter) {
+ if (bfs::extension(*dir_iter)==".ply") {
+ loadTemplate(dir_iter->string());
+ }
+ }
+
+ }
+
+#define OBJECT_CLOUD_DIST 0.25*0.25
+
+ template <typename T, typename U>
+ inline double distSquared3D(const T& a, const U& b) {
+ return (a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y)+(a.z-b.z)*(a.z-b.z);
+ }
+
+ void filterPointCloud(const sensor_msgs::PointCloud& cloud, TemplateModel* model, sensor_msgs::PointCloud& object_cloud)
+ {
+ geometry_msgs::Point min, max;
+ model->getExtents(min, max);
+
+ const int padding = 0.01;
+
+ printf("min: (%g,%g,%g), max: (%g,%g,%g)\n", min.x, min.y, min.z, max.x, max.y, max.z);
+
+ object_cloud.header.frame_id = cloud.header.frame_id;
+ object_cloud.header.stamp = cloud.header.stamp;
+ for (size_t i=0;i<cloud.get_points_size();++i) {
+
+ if ( (min.x - padding < cloud.points[i].x) &&
+ (min.y - padding < cloud.points[i].y) &&
+ (min.z - padding < cloud.points[i].z) &&
+ (cloud.points[i].x < max.x + padding ) &&
+ (cloud.points[i].y < max.y + padding ) &&
+ (cloud.points[i].z < max.z + padding ) ) {
+
+ object_cloud.points.push_back(cloud.points[i]);
+ }
+ }
+ }
+
+ void trackModel()
+ {
+ sensor_msgs::PointCloud tracking_cloud;
+ sensor_msgs::PointCloud tracking_object_cloud;
+
+ tf_.setExtrapolationLimit(ros::Duration(10));
+
+ tf_.transformPointCloud("tracking_frame", *cloud_, tracking_cloud);
+ filterPointCloud(tracking_cloud, track_template_, tracking_object_cloud);
+
+
+ if (tracking_object_cloud.get_points_size()==0) {
+ ROS_INFO("Lost tracking");
+ tracking_on_ = false;
+ return;
+ }
+
+ ModelFitSet mfs(1);
+ clock_t start = clock();
+ track_template_->findBestFit(tracking_object_cloud, mfs);
+ double duration = double(clock()-start)/CLOCKS_PER_SEC;
+ ROS_INFO("Tracker converged with score %g in %g ms", mfs.best_fit_[0].score(), duration*1000);
+ geometry_msgs::PoseStamped ps;
+ ps.pose = mfs.best_fit_[0].objectPose();
+ ps.header.frame_id = tracking_object_cloud.header.frame_id;
+ ps.header.stamp = tracking_object_cloud.header.stamp;
+
+ geometry_msgs::PoseStamped ps_parent_frame;
+
+ tf_.transformPose(cloud_->header.frame_id, ps, ps_parent_frame);
+
+ // publish tracking TF frame
+ tf::Stamped<tf::Pose> transform;
+ tf::poseStampedMsgToTF(ps_parent_frame, transform);
+ transform.parent_id_ = transform.frame_id_;
+ transform.frame_id_ = "tracking_frame";
+ tf_.setTransform(transform);
+ tb_.sendTransform(transform);
+
+ mfs.best_fit_[0].model_->show(marker_pub_, ps, mfs.best_fit_[0].score());
+ }
+
+// bool fitModel(tabletop_objects::ModelFit::Request& req,
+// tabletop_objects::ModelFit::Response& resp)
+// {
+// ROS_INFO("Service called");
+//
+// ModelFitSet mfs(1);
+//
+// clock_t start = clock();
+// fitBestModel(req.cloud, mfs);
+// double duration = double(clock()-start)/CLOCKS_PER_SEC;
+//
+// ROS_INFO("Best score: %g, time:%g", mfs.best_fit_[0].score(), duration);
+//
+// mfs.best_fit_[0].model_->show(marker_pub_, req.cloud.header.stamp, mfs.best_fit_[0].objectPose(), mfs.best_fit_[0].score());
+//// mfs.best_fit_[0].model_->distance_voxel_grid_->visualize(0.0,0.0001, "table_frame", ros::Time::now());
+//
+// resp.object.grasp_pose.pose = mfs.best_fit_[0].graspPose();
+// resp.object.grasp_pose.header.stamp = req.cloud.header.stamp;
+// resp.object.grasp_pose.header.frame_id = req.cloud.header.frame_id;
+// resp.object.object_pose.pose = mfs.best_fit_[0].objectPose();
+// resp.object.object_pose.header.stamp = req.cloud.header.stamp;
+// 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();
+//
+//
+//
+//
+// return true;
+// }
+};
+
+
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "model_tracker");
+
+ model_fit::ModelTracker mf;
+ ros::spin();
+}
Copied: pkg/trunk/sandbox/tabletop_objects/src/spin_features.cpp (from rev 23275, pkg/trunk/sandbox/tabletop_objects/src/compute_features.cpp)
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/src/spin_features.cpp (rev 0)
+++ pkg/trunk/sandbox/tabletop_objects/src/spin_features.cpp 2009-08-28 20:28:02 UTC (rev 23276)
@@ -0,0 +1,247 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <boost/filesystem.hpp>
+
+#include <ros/ros.h>
+#include "tabletop_objects/mesh_loader.h"
+#include "mapping_msgs/Object.h"
+#include "sensor_msgs/PointCloud.h"
+#include "geometry_msgs/Point.h"
+#include "visualization_msgs/Marker.h"
+
+#include "opencv/cvaux.h"
+
+#include <string>
+
+using namespace std;
+namespace bfs = boost::filesystem;
+
+
+class ComputeFeatures
+{
+ cv::Mesh3D* model_mesh_;
+ cv::SpinImageModel* model_;
+
+ ros::NodeHandle nh_;
+ ros::Subscriber cloud_sub_;
+ ros::Publisher marker_pub_;
+
+ sensor_msgs::PointCloud::ConstPtr cloud_;
+
+public:
+
+ ComputeFeatures()
+ {
+ cloud_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/cloud", 1, &ComputeFeatures::cloudCallback, this);
+ marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker",1);
+ }
+
+ void cloudCallback(const sensor_msgs::PointCloud::ConstPtr& point_cloud)
+ {
+ cloud_ = point_cloud;
+
+ matchModel();
+ }
+
+ void loadModel(const string& filename)
+ {
+ mapping_msgs::Object mesh;
+ PLYModelLoader modle_loader;
+ modle_loader.readFromFile(filename,mesh);
+
+ vector<cv::Point3f> points;
+ points.resize(mesh.vertices.size());
+ for (size_t i=0;i<mesh.vertices.size();++i) {
+ points[i] = cv::Point3f(mesh.vertices[i].x,mesh.vertices[i].y,mesh.vertices[i].z);
+ }
+
+ model_mesh_ = new cv::Mesh3D(points);
+ model_ = new cv::SpinImageModel(*model_mesh_);
+ ROS_INFO("Constructing spin image model");
+ model_->selectRandomSubset(0.02);
+ model_->compute();
+ ROS_INFO("Model has %d spin images", model_->getSpinCount());
+ }
+
+
+ void visualize(const vector<cv::Vec2i> points)
+ {
+ visualization_msgs::Marker marker;
+ marker.header.stamp = cloud_->header.stamp;
+ marker.header.frame_id = cloud_->header.frame_id;
+ marker.ns = "spin_points";
+ marker.id = 0;
+ marker.type = visualization_msgs::Marker::POINTS;
+ marker.action = visualization_msgs::Marker::ADD;
+ marker.pose.position.x = 0;
+ marker.pose.position.y = 0;
+ marker.pose.position.z = 0;
+ marker.pose.orientation.x = 0.0;
+ marker.pose.orientation.y = 0.0;
+ marker.pose.orientation.z = 0.0;
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 0.003;
+ marker.scale.y = 0.003;
+ marker.scale.z = 0.003;
+ marker.color.a = 1.0;
+ marker.color.r = 0.0;
+ marker.color.g = 1.0;
+ marker.color.b = 1.0;
+
+ for (size_t i=0;i<points.size();++i) {
+ geometry_msgs::Point p;
+ p.x = cloud_->points[points[i][1]].x;
+ p.y = cloud_->points[points[i][1]].y;
+ p.z = cloud_->points[points[i][1]].z;
+ marker.points.push_back(p);
+ }
+
+ marker_pub_.publish(marker);
+ }
+
+ void visualizeSpinImageLocations(const cv::SpinImageModel& model)
+ {
+
+ visualization_msgs::Marker marker;
+ marker.header.frame_id = cloud_->header.frame_id;
+ marker.header.stamp = ros::Time((uint64_t)0ULL);
+ marker.ns = "normals";
+ marker.id = 1;
+ marker.type = visualization_msgs::Marker::LINE_LIST;
+ marker.action = visualization_msgs::Marker::ADD;
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 0.001;
+ marker.color.a = 1.0;
+ marker.color.g = 1.0;
+
+ marker.set_points_size(2*model.getSpinCount());
+
+ for (size_t i=0;i<model.getSpinCount();++i) {
+
+ cv::Point3f point = model.getSpinVertex(i);
+ cv::Point3f normal = model.getSpinNormal(i);
+ float alpha = 0.02;
+
+ marker.points[2*i].x = point.x;
+ marker.points[2*i].y = point.y;
+ marker.points[2*i].z = point.z;
+
+ marker.points[2*i+1].x = point.x + alpha * normal.x;
+ marker.points[2*i+1].y = point.y + alpha * normal.y;
+ marker.points[2*i+1].z = point.z + alpha * normal.z;
+
+ }
+
+ marker_pub_.publish(marker);
+ }
+
+
+ void matchModel()
+ {
+ vector<cv::Point3f> points;
+ points.resize(cloud_->get_points_size());
+ for (size_t i=0;i<cloud_->get_points_size();++i) {
+ points[i] = cv::Point3f(cloud_->points[i].x,cloud_->points[i].y,cloud_->points[i].z);
+ }
+
+ cv::Mesh3D scene_mesh(points);
+ cv::SpinImageModel scene(scene_mesh);
+ ROS_INFO("Computing scene spin images");
+ scene.selectRandomSubset(0.05);
+ scene.compute();
+
+ ROS_INFO("Scene has %d spin images", scene.getSpinCount());
+
+
+ visualizeSpinImageLocations(scene);
+
+// vector<vector<cv::Vec2i> > matches;
+// model_->match(scene, matches);
+//
+// ROS_INFO("I have found %d match groups\n", matches.size());
+//
+// for (size_t i=0;i<matches.size();++i) {
+// ROS_INFO("\tGroup %d, size: %d", i, matches[i].size());
+// for (size_t j=0;j<matches[i].size();++j) {
+// printf ("%d ", matches[i][j][0]);
+// }
+// printf("\n");
+// for (size_t j=0;j<matches[i].size();++j) {
+// printf ("%d ", matches[i][j][1]);
+// }
+// printf("\n");
+// }
+//
+// visualize(matches[0]);
+
+ }
+
+ void init(const string& path)
+ {
+ bfs::path template_dir(path);
+
+ if (!bfs::is_directory(template_dir)) {
+ ROS_ERROR("Cannot load templates, %s is not a directory", template_dir.leaf().c_str());
+ }
+
+ bfs::directory_iterator dir_iter(template_dir), dir_end;
+ for(;dir_iter != dir_end; ++dir_iter) {
+ if (bfs::extension(*dir_iter)==".ply") {
+ loadModel(dir_iter->string());
+ }
+ }
+ }
+
+
+};
+
+
+
+
+int main(int argc, char** argv)
+{
+ ros::init(argc,argv,"match_features");
+ if (argc<2) {
+ ROS_INFO("Usage: %s path", argv[0]);
+ }
+ ComputeFeatures cf;
+
+ cf.init(string(argv[1]));
+
+ ROS_INFO("Starting spinning");
+ ros::spin();
+
+ return 0;
+}
Copied: pkg/trunk/sandbox/tabletop_objects/src/tabletop_chamfer_match.cpp (from rev 23275, pkg/trunk/sandbox/tabletop_objects/src/tabletop_detector.cpp)
===================================================================
--- pkg/trunk/sandbox/tabletop_objects/src/tabletop_chamfer_match.cpp (rev 0)
+++ pkg/trunk/sandbox/tabletop_objects/src/tabletop_chamfer_match.cpp 2009-08-28 20:28:02 UTC (rev 23276)
@@ -0,0 +1,1385 @@
+/*********************************************************************
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+// Author: Marius Muja
+
+#include <vector>
+#include <fstream>
+#include <sstream>
+#include <time.h>
+#include <iostream>
+#include <iomanip>
+#include <queue>
+#include <algorithm>
+#include <libgen.h> // for dirname()
+
+
+#include "opencv_latest/CvBridge.h"
+
+#include "opencv/cxcore.h"
+#include "opencv/cv.h"
+#include "opencv/highgui.h"
+
+
+#include <point_cloud_mapping/geometry/angles.h>
+#include <point_cloud_mapping/sample_consensus/sac_model_plane.h>
+#include <point_cloud_mapping/sample_consensus/sac_model_oriented_plane.h>
+#include <point_cloud_mapping/sample_consensus/sac_model_line.h>
+#include <point_cloud_mapping/sample_consensus/sac.h>
+#include <point_cloud_mapping/sample_consensus/ransac.h>
+#include <point_cloud_mapping/sample_consensus/lmeds.h>
+#include <point_cloud_mapping/geometry/statistics.h>
+#include <point_cloud_mapping/geometry/projections.h>
+
+
+#include "ros/ros.h"
+#include "ros/callback_queue.h"
+#include "stereo_msgs/StereoInfo.h"
+#include "stereo_msgs/DisparityInfo.h"
+#include "sensor_msgs/CameraInfo.h"
+#include "sensor_msgs/Image.h"
+#include "sensor_msgs/PointCloud.h"
+#include "geometry_msgs/Point32.h"
+#include "geometry_msgs/PointStamped.h"
+#include "visualization_msgs/Marker.h"
+#include "tabletop_objects/TrackObject.h"
+
+#include <string>
+
+// transform library
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+
+#include "topic_synchronizer2/topic_synchronizer.h"
+
+#include <boost/thread.hpp>
+
+#include "chamfer_matching/chamfer_matching.h"
+#include "tabletop_objects/ModelFit.h"
+#include "tabletop_objects/FindObjectPoses.h"
+#include "tabletop_msgs/TableTopObject.h"
+
+//#include "flann.h"
+
+using namespace std;
+
+#define CV_PIXEL(type,img,x,y) (((type*)(img->imageData+y*img->widthStep))+x*img->nChannels)
+
+class TabletopDetector
+{
+public:
+
+ ros::NodeHandle nh_;
+
+ sensor_msgs::ImageConstPtr limage_;
+ sensor_msgs::ImageConstPtr rimage_;
+ sensor_msgs::ImageConstPtr dimage_;
+ stereo_msgs::StereoInfoConstPtr stinfo_;
+ stereo_msgs::DisparityInfoConstPtr dispinfo_;
+ sensor_msgs::CameraInfoConstPtr lcinfo_;
+ sensor_msgs::CvBridge lbridge_;
+ sensor_msgs::CvBridge rbridge_;
+ sensor_msgs::CvBridge dbridge_;
+
+
+ ros::Subscriber left_image_sub_;
+ ros::Subscriber left_caminfo_image_sub_;
+ ros::Subscriber right_image_sub_;
+ ros::Subscriber disparity_sub_;
+ ros::Subscriber cloud_sub_;
+ ros::Subscriber dispinfo_sub_;
+
+
+ ros::Publisher object_pub_;
+ ros::Publisher objects_pub_;
+ ros::Publisher marker_pub_;
+ ros::Publisher track_object_pub_;
+
+ ros::ServiceServer service_;
+
+ ros::ServiceClient model_fit_service_;
+
+ sensor_msgs::PointCloudConstPtr cloud;
+
+ IplImage* left;
+ IplImage* right;
+ IplImage* disp;
+ IplImage* disp_clone;
+
+ tf::TransformListener tf_;
+ tf::TransformBroadcaster broadcaster_;
+ TopicSynchronizer sync_;
+
+ // display stereo images ?
+ bool display_;
+
+ int templates_no;
+ int edges_high;
+
+
+ int min_points_per_cluster_;
+ int max_clustering_iterations_;
+ double min_object_distance_; // min distance between two objects
+ double min_object_height_;
+ string target_frame_;
+
+ ChamferMatching* cm;
+
+ ros::CallbackQueue service_queue_;
+ boost::thread* service_thread_;
+
+ boost::mutex data_lock_;
+ boost::condition_variable data_cv_;
+ bool got_data_;
+
+ TabletopDetector()
+ : left(NULL), right(NULL), disp(NULL), disp_clone(NULL), sync_(&TabletopDetector::syncCallback, this), got_data_(false)
+ {
+ // define node parameters
+ nh_.param("~display", display_, false);
+ nh_.param("~min_points_per_cluster", min_points_per_cluster_, 10);
+ nh_.param("~max_clustering_iterations", max_clustering_iterations_, 7);
+ nh_.param("~min_object_distance", min_object_distance_, 0.05);
+ nh_.param("~min_object_height", min_object_height_, 0.05);
+ nh_.param<string>("~target_frame", target_frame_,"odom_combined");
+
+ string template_path;
+ nh_.param<string>("~template_path", template_path,"templates.txt");
+
+ templates_no = 10;
+ edges_high = 100;
+
+ if(display_){
+ cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
+ cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
+ cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
+ cvNamedWindow("disparity_clone", CV_WINDOW_AUTOSIZE);
+ cvNamedWindow("edges",1);
+ }
+
+ // subscribe to topics
+ left_image_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/left/image_rect", 1, sync_.synchronize(&TabletopDetector::leftImageCallback, this));
+ left_caminfo_image_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/left/cam_info", 1, sync_.synchronize(&TabletopDetector::leftCameraInfoCallback, this));
+ right_image_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/right/image_rect", 1, sync_.synchronize(&TabletopDetector::rightImageCallback, this));
+ disparity_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/disparity", 1, sync_.synchronize(&TabletopDetector::disparityImageCallback, this));
+ cloud_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/cloud", 1, sync_.synchronize(&TabletopDetector::cloudCallback, this));
+ dispinfo_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/disparity_info", 1, sync_.synchronize(&TabletopDetector::dispinfoCallback, this));
+
+ // advertise topics
+ objects_pub_ = nh_.advertise<sensor_msgs::PointCloud> ("~objects", 1);
+ object_pub_ = nh_.advertise<sensor_msgs::PointCloud> ("~object", 1);
+// 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_);
+ service_ = nh_.advertiseService(service_opts);
+
+// loadTemplates(template_path);
+ service_thread_ = new boost::thread(boost::bind(&TabletopDetector::serviceThread, this));
+
+ model_fit_service_ = ros::service::createClient<tabletop_objects::ModelFit::Request, tabletop_objects::ModelFit::Response>("tabletop_objects/model_fit", true);
+
+ cm = new ChamferMatching();
+ }
+
+ ~TabletopDetector()
+ {
+ service_thread_->join();
+ delete service_thread_;
+ delete cm;
+ }
+
+private:
+
+ void syncCallback()
+ {
+// ROS_INFO("Sync callback");
+ if (disp!=NULL) {
+ cvReleaseImage(&disp);
+ }
+ if(dbridge_.fromImage(*dimage_)) {
+ disp = cvCreateImage(cvGetSize(dbridge_.toIpl()), IPL_DEPTH_8U, 1);
+ cvCvtScale(dbridge_.toIpl(), disp, 4.0/dispinfo_->dpp);
+ }
+
+// runTabletopDetection();
+ got_data_ = true;
+ data_cv_.notify_all();
+ }
+
+ void leftCameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& info)
+ {
+ if (got_data_) return;
+// ROS_INFO("Left caminfo callback");
+ lcinfo_ = info;
+ }
+
+ void leftImageCallback(const sensor_msgs::Image::ConstPtr& image)
+ {
+ if (got_data_) return;
+// ROS_INFO("Left image callback");
+
+ limage_ = image;
+ if(lbridge_.fromImage(*limage_, "bgr8")) {
+ left = lbridge_.toIpl();
+ }
+ }
+
+ void rightImageCallback(const sensor_msgs::Image::ConstPtr& image)
+ {
+ if (got_data_) return;
+// ROS_INFO("Right image callback");
+
+ rimage_ = image;
+ if(rbridge_.fromImage(*rimage_, "bgr8")) {
+ right = rbridge_.toIpl();
+ }
+ }
+
+ void disparityImageCallback(const sensor_msgs::Image::ConstPtr& image)
+ {
+ if (got_data_) return;
+ // ROS_INFO("Disparity image callback");
+
+ dimage_ = image;
+ }
+
+ void dispinfoCallback(const stereo_msgs::DisparityInfo::ConstPtr& dinfo)
+ {
+ if (got_data_) return;
+// ROS_INFO("Disp info callback");
+
+ dispinfo_ = dinfo;
+ }
+
+ void cloudCallback(const sensor_msgs::PointCloud::ConstPtr& point_cloud)
+ {
+// ROS_INFO("Cloud callback");
+ if (got_data_) {
+// ROS_INFO("Discarding point cloud");
+ return;
+ }
+
+
+ cloud = point_cloud;
+ }
+
+
+ bool findObjectPoses(tabletop_objects::FindObjectPoses::Request& req,
+ tabletop_objects::FindObjectPoses::Response& resp)
+ {
+ got_data_ = false;
+ ROS_INFO("FindObjectPoses: Service called");
+ boost::unique_lock<boost::mutex> lock(data_lock_);
+
+ while (!got_data_) {
+ ROS_INFO("FindObjectPoses: Waiting for data");
+ data_cv_.wait(lock);
+ }
+
+ findTableTopObjectPoses(resp.objects);
+
+ // transform all poses to the target frame
+ for (size_t i=0;i<resp.objects.size();++i) {
+ geometry_msgs::PoseStamped& object_pose = resp.objects[i].object_pose;
+ geometry_msgs::PoseStamped& grasp_pose = resp.objects[i].grasp_pose;
+
+
+ ros::Time common_time;
+ string error;
+ if (tf_.getLatestCommonTime(object_pose.header.frame_id, target_frame_, common_time, &error)==tf::NO_ERROR) {
+ object_pose.header.stamp = common_time;
+ grasp_pose.header.stamp = common_time;
+ tf_.transformPose(target_frame_, object_pose, object_pose);
+ tf_.transformPose(target_frame_, grasp_pose, grasp_pose);
+
+ }
+ else {
+ ROS_ERROR("Cannot transform pose from %s to %s", object_pose.header.frame_id.c_str(), target_frame_.c_str());
+ }
+
+// if (!tf_.canTransform(target_frame_, object_pose.header.frame_id, object_pose.header.stamp, ros::Duration(5.0))){
+// ROS_ERROR("Cannot transform from %s to %s", object_pose.header.frame_id.c_str(), target_frame_.c_str());
+// return false;
+// }
+// tf_.transformPose(target_frame_, object_pose, object_pose);
+// PoseStamped& grasp_pose = resp.objects[i].grasp_pose;
+// if (!tf_.canTransform(target_frame_, grasp_pose.header.frame_id, grasp_pose.header.stamp, ros::Duration(5.0))){
+// ROS_ERROR("Cannot transform from %s to %s", grasp_pose.header.frame_id.c_str(), target_frame_.c_str());
+// return false;
+// }
+// tf_.transformPose(target_frame_, grasp_pose, grasp_pose);
+ }
+ return true;
+ }
+
+
+
+ void trimSpaces( string& str)
+ {
+ size_t startpos = str.find_first_not_of(" \t\n"); // Find the first character position after excluding leading blank spaces
+ size_t endpos = str.find_last_not_of(" \t\n"); // Find the first character position from reverse af
+
+ // if all spaces or empty return an empty string
+ if(( string::npos == startpos ) || ( string::npos == endpos)) {
+ str = "";
+ }
+ else {
+ str = str.substr( startpos, endpos-startpos+1 );
+ }
+ }
+
+ void loadTemplates(string path)
+ {
+ FILE* fin = fopen(path.c_str(),"r");
+
+ if (fin==NULL) {
+ ROS_ERROR("Cannot open template list: %s", path.c_str());
+ exit(1);
+ }
+
+ char* path_ptr = strdup(path.c_str());
+ string dir_path = dirname(path_ptr);
+
+
+ int num;
+ fscanf(fin,"%d",&num);
+
+ for (int i=0;i<num;++i) {
+
+ char line[512];
+ float real_size;
+ float image_size;
+
+ fscanf(fin,"%s %f %f", line, &real_size, &image_size);
+ string template_file = line;
+ trimSpaces(template_file);
+ printf("template file: %s\n",template_file.c_str());
+ string template_path = dir_path + "/" + template_file;
+
+ if (!template_file.empty()) {
+ IplImage* templ = cvLoadImage(template_path.c_str(),CV_LOAD_IMAGE_GRAYSCALE);
+ if (!templ) {
+ ROS_ERROR("Cannot load template image: %s", template_path.c_str());
+ exit(1);
+ }
+ cm->addTemplateFromImage(templ, 1000*image_size/real_size); // real_size is in mm
+ printf("%s %f %f %f\n", line, real_size, image_size, 1000*image_size/real_size);
+ cvReleaseImage(&templ);
+ }
+ }
+
+ free(path_ptr);
+ fclose(fin);
+
+ }
+
+
+ bool fitSACPlane (const sensor_msgs::PointCloud& points, const vector<int> &indices, // input
+ vector<int> &inliers, vector<double> &coeff, // output
+ double dist_thresh, int min_points_per_model)
+ {
+ // Create and initialize the SAC model
+ sample_consensus::SACModelPlane *model = new sample_consensus::SACModelPlane ();
+ sample_consensus::SAC *sac = new sample_consensus::RANSAC (model, dist_thresh);
+ sac->setMaxIterations (100);
+ model->setDataSet ((sensor_msgs::PointCloud*)&points, indices);
+
+ // Search for the best plane
+ if (sac->computeModel ()) {
+ sac->computeCoefficients (coeff); // Compute the model coefficients
+ sac->refineCoefficients (coeff); // Refine them using least-squares
+
+ // Get the list of inliers
+ model->selectWithinDistance (coeff, dist_thresh, inliers);
+
+ if ((int)inliers.size()<min_points_per_model) {
+ return false;
+ }
+
+ geometry_msgs::Point32 viewpoint;
+ viewpoint.x = 0;
+ viewpoint.y = 0;
+ viewpoint.z = 0;
+ // Flip the plane normal towards the viewpoint
+ cloud_geometry::angles::flipNormalTowardsViewpoint (coeff, points.points.at(inliers[0]), viewpoint);
+
+ ROS_INFO ("Found a planar model supported by %d inliers: [%g, %g, %g, %g]", (int)inliers.size (), coeff[0], coeff[1], coeff[2], coeff[3]);
+ }
+ else {
+ ROS_ERROR ("Could not compute a planar model for %d points.", indices.size());
+ return false;
+ }
+
+ delete sac;
+ delete model;
+ return true;
+ }
+
+
+ void filterByZBounds(const sensor_msgs::PointCloud& pc, double zmin, double zmax, sensor_msgs::PointCloud& filtered_pc, sensor_msgs::PointCloud& filtered_outside)
+ {
+ vector<int> indices_remove;
+ for (size_t i = 0;i<pc.get_points_size();++i) {
+ if (pc.points[i].z>zmax || pc.points[i].z<zmin) {
+ indices_remove.push_back(i);
+ }
+ }
+ cloud_geometry::getPointCloudOutside (pc, indices_remove, filtered_pc);
+ cloud_geometry::getPointCloud(pc, indices_remove, filtered_outside);
+ }
+
+
+ void clearFromImage(IplImage* disp_img, const sensor_msgs::PointCloud& pc)
+ {
+ int xchan = -1;
+ int ychan = -1;
+ for(size_t i = 0;i < pc.channels.size();++i){
+ if(pc.channels[i].name == "x"){
+ xchan = i;
+ }
+ if(pc.channels[i].name == "y"){
+ ychan = i;
+ }
+ }
+
+ if (xchan==-1 || ychan==-1) {
+ ROS_ERROR("Cannot find image coordinates in the point cloud");
+ return;
+ }
+
+ // remove plane points from disparity image
+ for (size_t i=0;i<pc.get_points_size();++i) {
+ int x = pc.channels[xchan].values[i];
+ int y = pc.channels[ychan].values[i];
+ // printf("(%d,%d)\n", x, y);
+ CV_PIXEL(unsigned char, disp_img, x, y)[0] = 0;
+ }
+ }
+
+
+ void filterTablePlane(const sensor_msgs::PointCloud& pc, vector<double>& coefficients, sensor_msgs::PointCloud& object_cloud, sensor_msgs::PointCloud& plane_cloud)
+ {
+
+ disp_clone = cvCloneImage(disp);
+
+ sensor_msgs::PointCloud filtered_cloud;
+ sensor_msgs::PointCloud filtered_outside;
+
+ filterByZBounds(pc, 0.1, 1.2 , filtered_cloud, filtered_outside );
+
+ clearFromImage(disp, filtered_outside);
+
+ vector<int> indices(filtered_cloud.get_points_size());
+ for (size_t i = 0;i<filtered_cloud.get_points_size();++i) {
+ indices[i] = i;
+ }
+
+ vector<int> inliers;
+ double dist_thresh = 0.01; // in meters
+ int min_points = 200;
+
+ fitSACPlane(filtered_cloud, indices, inliers, coefficients, dist_thresh, min_points);
+
+ cloud_geometry::getPointCloud(filtered_cloud, inliers, plane_cloud);
+ cloud_geometry::getPointCloudOutside (filtered_cloud, inliers, object_cloud);
+
+ clearFromImage(disp, plane_cloud);
+ }
+
+
+ void projectToPlane(const sensor_msgs::PointCloud& objects, const vector<double>& plane, sensor_msgs::PointCloud& projected_objects)
+ {
+ vector<int> object_indices(objects.points.size());
+ for (size_t i=0;i<objects.get_points_size();++i) {
+ object_indices[i] = i;
+ }
+
+ projected_objects.header.stamp = objects.header.stamp;
+ projected_objects.header.frame_id = objects.header.frame_id;
+
+ cloud_geometry::projections::pointsToPlane(objects, object_indices, projected_objects, plane);
+
+ }
+
+
+ /**
+ * \brief Finds edges in an image
+ * @param img
+ */
+ void doChamferMatching(IplImage *img, const vector<CvPoint>& positions, const vector<float>& scales)
+ {
+ // edge detection
+ IplImage *gray = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
+ cvCvtColor(img, gray, CV_RGB2GRAY);
+ cvCanny(gray, gray, edges_high/2, edges_high);
+
+ if (display_) {
+ cvShowImage("edges", gray);
+ }
+
+
+// for (int i=0;i<scales.size();++i) {
+// printf("%f, ", scales[i]);
+// }
+// printf("\n");
+
+ ChamferMatch match = cm->matchEdgeImage(gray, LocationScaleImageRange(positions, scales));
+ IplImage* left_clone = cvCloneImage(left);
+
+ ChamferMatch::Ch...
[truncated message content] |
