Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "tabletop_object_detector/model_fitter.h"
00038
00039 #include <math.h>
00040 #include <distance_field/propagation_distance_field.h>
00041
00042 #include "tabletop_object_detector/marker_generator.h"
00043
00044 namespace tabletop_object_detector {
00045
00047 class IterativeTranslationFitter : public DistanceFieldFitter
00048 {
00049 private:
00050
00052 template <class PointCloudType>
00053 geometry_msgs::Point32 centerOfSupport(const PointCloudType& cloud);
00054
00056 template <class PointCloudType>
00057 double getFitScoreAndGradient(const PointCloudType& cloud,
00058 const geometry_msgs::Point32& location,
00059 geometry_msgs::Point32& vector,
00060 double &maxDist);
00061 public:
00063 IterativeTranslationFitter() : DistanceFieldFitter() {}
00065 ~IterativeTranslationFitter() {}
00066
00068 template <class PointCloudType>
00069 ModelFitInfo fitPointCloud(const PointCloudType& cloud);
00070 };
00071
00072
00073
00077 template <class PointCloudType>
00078 geometry_msgs::Point32 IterativeTranslationFitter::centerOfSupport(const PointCloudType& cloud)
00079 {
00080 geometry_msgs::Point32 center;
00081 center.x = center.y = center.z = 0;
00082 if (cloud.points.empty())
00083 {
00084 return center;
00085 }
00086 for (unsigned int i=0; i<cloud.points.size(); ++i)
00087 {
00088 center.x += cloud.points[i].x;
00089 center.y += cloud.points[i].y;
00090 }
00091 center.x /= cloud.points.size();
00092 center.y /= cloud.points.size();
00093 return center;
00094 }
00095
00096
00097 template <class PointCloudType>
00098 double IterativeTranslationFitter::getFitScoreAndGradient(const PointCloudType& cloud,
00099 const geometry_msgs::Point32& location,
00100 geometry_msgs::Point32& vector,
00101 double &max_dist)
00102 {
00103 double score = 0;
00104 max_dist = 0;
00105
00106 vector.x = 0;
00107 vector.y = 0;
00108 vector.z = 0;
00109 int cnt = 0;
00110
00111 for (size_t i=0;i<cloud.points.size();i++)
00112 {
00113 double wx = cloud.points[i].x-location.x;
00114 double wy = cloud.points[i].y-location.y;
00115 double wz = cloud.points[i].z-location.z;
00116
00117 int x, y, z;
00118 double val = truncate_value_;
00119 if (distance_voxel_grid_->worldToGrid(wx,wy,wz,x,y,z))
00120 {
00121 distance_field::PropDistanceFieldVoxel& voxel = distance_voxel_grid_->getCell(x,y,z);
00122 double cx, cy, cz;
00123 if (voxel.closest_point_[0] != distance_field::PropDistanceFieldVoxel::UNINITIALIZED)
00124 {
00125 distance_voxel_grid_->gridToWorld(voxel.closest_point_[0],
00126 voxel.closest_point_[1],
00127 voxel.closest_point_[2],
00128 cx,cy,cz);
00129 val = distance_voxel_grid_->getDistanceFromCell(x,y,z);
00130 vector.x += (cx-wx);
00131 vector.y += (cy-wy);
00132 vector.z += (cz-wz);
00133 cnt++;
00134 if (val>truncate_value_)
00135 {
00136 val = truncate_value_;
00137 }
00138 }
00139 else
00140 {
00141 }
00142 }
00143 else
00144 {
00145 }
00146 max_dist = std::max(max_dist,val);
00147
00148 score += val;
00149 }
00150 score /= (cloud.points.size());
00151 if (cnt!=0)
00152 {
00153 vector.x /= cnt;
00154 vector.y /= cnt;
00155 vector.z /= cnt;
00156 }
00157
00158 return score;
00159 }
00160
00171 template <class PointCloudType>
00172 ModelFitInfo IterativeTranslationFitter::fitPointCloud(const PointCloudType& cloud)
00173 {
00174 if (cloud.points.empty())
00175 {
00176 ROS_ERROR("Attempt to fit model to empty point cloud");
00177 geometry_msgs::Pose bogus_pose;
00178 return ModelFitInfo(model_id_, bogus_pose, -1.0);
00179 }
00180
00181
00182 geometry_msgs::Point32 center = centerOfSupport<PointCloudType>(cloud);
00183
00184 geometry_msgs::Point32 location = center;
00185 geometry_msgs::Point32 vector;
00186 double max_dist;
00187 geometry_msgs::Pose pose;
00188
00189 double score = getFitScoreAndGradient<PointCloudType>(cloud, location, vector, max_dist);
00190 double old_score = score + 1;
00191
00192 double EPS = 1.0e-6;
00193 int max_iterations = 100;
00194 int iter = 0;
00195 while (score < old_score - EPS && iter < max_iterations)
00196 {
00197 old_score = score;
00198 location.x -= vector.x;
00199 location.y -= vector.y;
00200
00201
00202 score = getFitScoreAndGradient<PointCloudType>(cloud, location, vector, max_dist);
00203 iter++;
00204 }
00205
00206 if (iter == max_iterations)
00207 {
00208 ROS_WARN("Maximum iterations reached in model fitter");
00209 }
00210
00211 pose.position.x = location.x;
00212 pose.position.y = location.y;
00213 pose.position.z = location.z;
00214 pose.orientation.x = 0;
00215 pose.orientation.y = 0;
00216 pose.orientation.z = 0;
00217 pose.orientation.w = 1;
00218
00219 return ModelFitInfo(model_id_, pose, old_score);
00220 }
00221
00222 }