Object.cpp
Go to the documentation of this file.
00001 #include "Object.h"
00002 #include "parameters.h"
00003 #include <boost/lexical_cast.hpp>
00004 
00005 #include <Eigen/Geometry>
00006 #include <tf/transform_datatypes.h>
00007 
00008 namespace hector_object_tracker {
00009 
00010 std::map<std::string,unsigned int> Object::object_count;
00011 std::string Object::object_namespace;
00012 
00013 Object::Object(const std::string class_id, const std::string object_id)
00014 {
00015   if (!class_id.empty()) {
00016     this->info.class_id = class_id;
00017   } else {
00018     this->info.class_id = "object";
00019   }
00020 
00021   if (!object_id.empty()) {
00022     this->info.object_id = object_id;
00023   } else {
00024     this->info.object_id = this->info.class_id + "_" + boost::lexical_cast<std::string>(object_count[this->info.class_id]++);
00025   }
00026 
00027   // if (this->info.object_id[0] != '/') this->info.object_id = object_namespace + "/" + this->info.object_id;
00028 }
00029 
00030 Object::Object(const hector_worldmodel_msgs::Object& other) {
00031   *this = other;
00032 }
00033 
00034 Object::~Object()
00035 {}
00036 
00037 void Object::reset() {
00038   object_count.clear();
00039 }
00040 
00041 void Object::getMessage(hector_worldmodel_msgs::Object& object) const {
00042   object.header = this->header;
00043   this->getPoseWithCovariance(object.pose);
00044   object.info   = this->info;
00045   object.state  = this->state;
00046 }
00047 
00048 hector_worldmodel_msgs::Object Object::getMessage() const {
00049   hector_worldmodel_msgs::Object object;
00050   getMessage(object);
00051   return object;
00052 }
00053 
00054 void Object::getPoseWithCovariance(geometry_msgs::PoseWithCovariance& pose) const {
00055   getPose(pose.pose);
00056   getCovariance(pose.covariance);
00057 }
00058 
00059 void Object::setPose(const geometry_msgs::PoseWithCovariance& pose) {
00060   setPose(pose.pose);
00061   setCovariance(pose.covariance);
00062 }
00063 
00064 void Object::getPose(geometry_msgs::Pose& pose) const
00065 {
00066   pose.position.x = this->position.x();
00067   pose.position.y = this->position.y();
00068   pose.position.z = this->position.z();
00069   pose.orientation.w = this->orientation.w();
00070   pose.orientation.x = this->orientation.x();
00071   pose.orientation.y = this->orientation.y();
00072   pose.orientation.z = this->orientation.z();
00073 }
00074 
00075 void Object::getPose(tf::Pose& pose) const
00076 {
00077   pose.getOrigin().setValue(this->position.x(), this->position.y(), this->position.z());
00078   pose.getBasis().setRotation(tf::Quaternion(this->orientation.x(), this->orientation.y(), this->orientation.z(), this->orientation.w()));
00079 }
00080 
00081 void Object::setPose(const geometry_msgs::Pose& pose)
00082 {
00083   setPosition(pose.position);
00084   setOrientation(pose.orientation);
00085 }
00086 
00087 void Object::setPose(const tf::Pose &pose)
00088 {
00089   setPosition(pose.getOrigin());
00090   tf::Quaternion rot;
00091   pose.getBasis().getRotation(rot);
00092   setOrientation(rot);
00093 }
00094 
00095 const Eigen::Vector3f& Object::getPosition() const {
00096   return position;
00097 }
00098 
00099 void Object::setPosition(const Eigen::Vector3f& position) {
00100   this->position = position;
00101 }
00102 
00103 void Object::setPosition(const geometry_msgs::Point& position) {
00104   this->position << position.x, position.y, position.z;
00105 }
00106 
00107 void Object::setPosition(const tf::Point& position)
00108 {
00109   this->position << position.x(), position.y(), position.z();
00110 }
00111 
00112 const Eigen::Quaternionf& Object::getOrientation() const
00113 {
00114   return this->orientation;
00115 }
00116 
00117 void Object::setOrientation(const geometry_msgs::Quaternion& orientation)
00118 {
00119   this->orientation.coeffs() << orientation.x, orientation.y, orientation.z, orientation.w;
00120 }
00121 
00122 void Object::setOrientation(const tf::Quaternion& orientation)
00123 {
00124   this->orientation.coeffs() << orientation.x(), orientation.y(), orientation.z(), orientation.w();
00125 }
00126 
00127 const Eigen::Matrix3f& Object::getCovariance() const {
00128   return covariance;
00129 }
00130 
00131 void Object::getCovariance(geometry_msgs::PoseWithCovariance::_covariance_type& covariance) const {
00132   covariance[0]  = this->covariance(0,0);
00133   covariance[1]  = this->covariance(0,1);
00134   covariance[2]  = this->covariance(0,2);
00135   covariance[6]  = this->covariance(1,0);
00136   covariance[7]  = this->covariance(1,1);
00137   covariance[8]  = this->covariance(1,2);
00138   covariance[12] = this->covariance(2,0);
00139   covariance[13] = this->covariance(2,1);
00140   covariance[14] = this->covariance(2,2);
00141 }
00142 
00143 void Object::setCovariance(const Eigen::Matrix3f& eigen) {
00144   this->covariance = eigen;
00145 }
00146 
00147 void Object::setCovariance(const tf::Matrix3x3& tf) {
00148   this->covariance << tf[0][0], tf[0][1], tf[0][2],
00149                       tf[1][0], tf[1][1], tf[1][2],
00150                       tf[2][0], tf[2][1], tf[2][2];
00151 }
00152 
00153 void Object::setCovariance(const geometry_msgs::PoseWithCovariance::_covariance_type& covariance) {
00154   this->covariance << covariance[0],  covariance[1],  covariance[2],
00155                       covariance[6],  covariance[7],  covariance[8],
00156                       covariance[12], covariance[13], covariance[14];
00157 }
00158 
00159 void Object::setState(const StateType& state) {
00160   if (this->state.state == state) return;
00161   ROS_INFO("Setting object state for %s to %s", getObjectId().c_str(), hector_worldmodel_msgs::getObjectStateString(state));
00162   this->state.state = state;
00163 }
00164 
00165 void Object::intersect(const tf::Pose& poseB, const Eigen::Matrix3f& covarianceB, float support) {
00166   Eigen::Vector3f positionB(poseB.getOrigin().x(), poseB.getOrigin().y(), poseB.getOrigin().z());
00167   tf::Quaternion orientationB = poseB.getRotation();
00168     // old cov/covariance is A , new cov/covIn is B
00169   float omega = 0.5f;
00170 
00171   Eigen::Matrix3f A(covariance.inverse() * omega);
00172   Eigen::Matrix3f B(covarianceB.inverse() * (1.0f - omega));
00173   double infA = 1./covariance.trace();
00174   double infB = 1./covarianceB.trace();
00175 
00176   covariance = (A + B).inverse();
00177   position = covariance * (A * position + B * positionB);
00178   updateOrientation(orientationB, infB / (infA + infB));
00179 
00180   setPosition(position);
00181   setCovariance(covariance);
00182   addSupport(support);
00183 }
00184 
00185 void Object::update(const tf::Pose& poseB, const Eigen::Matrix3f& covarianceB, float support) {
00186   Eigen::Vector3f positionB(poseB.getOrigin().x(), poseB.getOrigin().y(), poseB.getOrigin().z());
00187   tf::Quaternion orientationB = poseB.getRotation();
00188     // old information is A , new information is B
00189 
00190   Eigen::Matrix3f A(covariance.inverse());
00191   Eigen::Matrix3f B(covarianceB.inverse());
00192   double infA = 1./covariance.trace();
00193   double infB = 1./covarianceB.trace();
00194 
00195   covariance = (A + B).inverse();
00196   position = covariance * (A * position + B * positionB);
00197   updateOrientation(orientationB, infB / (infA + infB));
00198 
00199   setPosition(position);
00200   setCovariance(covariance);
00201   addSupport(support);
00202 }
00203 
00204 void Object::updateOrientation(const tf::Quaternion& orientationB, double slerp_factor) {
00205   // update orientation of objects using low-pass filtering
00206   if (parameter(_with_orientation, getClassId())) {
00207       tf::Quaternion q(orientation.x(), orientation.y(), orientation.z(), orientation.w());
00208       q.slerp(orientationB, slerp_factor);
00209       setOrientation(q);
00210   }
00211   // or simply set new orientation
00212   else {
00213     setOrientation(orientationB);
00214   }
00215 }
00216 
00217 void Object::getVisualization(visualization_msgs::MarkerArray &markers) const {
00218   visualization_msgs::Marker marker;
00219   std::string postfix;
00220 
00221   // default color
00222   marker.color = parameter(_marker_color, this->info.class_id);
00223 
00224   switch(this->state.state) {
00225     case hector_worldmodel_msgs::ObjectState::CONFIRMED:
00226       marker.color.r = 0.0;
00227       marker.color.g = 0.8;
00228       marker.color.b = 0.0;
00229       postfix = " (CONFIRMED)";
00230       break;
00231     case hector_worldmodel_msgs::ObjectState::DISCARDED:
00232       marker.color.a = 0.5;
00233       postfix = " (DISCARDED)";
00234       break;
00235     case hector_worldmodel_msgs::ObjectState::INACTIVE:
00236       marker.color.a = 0.5;
00237       postfix = " (INACTIVE)";
00238       break;
00239     case hector_worldmodel_msgs::ObjectState::UNKNOWN:
00240       marker.color.a = 0.5;
00241       break;
00242     case hector_worldmodel_msgs::ObjectState::PENDING:
00243       marker.color.a = 0.5;
00244       postfix = " (PENDING)";
00245       break;
00246     default:
00247       break;
00248   }
00249 
00250   marker.header = this->header;
00251   marker.action = visualization_msgs::Marker::ADD;
00252   getPose(marker.pose);
00253   marker.ns = this->info.class_id;
00254 
00255   marker.type = visualization_msgs::Marker::ARROW;
00256   marker.scale.x = 1.0;
00257   marker.scale.y = 1.0;
00258   marker.scale.z = 1.0;
00259   // markers.markers.push_back(marker);
00260 
00261   marker.type = visualization_msgs::Marker::SPHERE;
00262   marker.scale.x = 0.1;
00263   marker.scale.y = 0.1;
00264   marker.scale.z = 0.1;
00265   markers.markers.push_back(marker);
00266 
00267   marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00268   marker.text = (!this->info.name.empty() ? this->info.name : this->info.object_id) + postfix;
00269   marker.scale.x = 0.0;
00270   marker.scale.y = 0.0;
00271   marker.scale.z = 0.1;
00272   marker.pose.position.z += 1.5 * marker.scale.z;
00273   markers.markers.push_back(marker);
00274 }
00275 
00276 void Object::setNamespace(const std::string &ns) {
00277   object_namespace = ns;
00278 }
00279 
00280 Object& Object::operator =(const hector_worldmodel_msgs::Object& other) {
00281   header = other.header;
00282   info = other.info;
00283   state = other.state;
00284   setPose(other.pose);
00285   return *this;
00286 }
00287 
00288 ObjectPtr Object::transform(tf::Transformer& tf, const std::string& target_frame) const
00289 {
00290   return transform(tf, target_frame, this->header.stamp);
00291 }
00292 
00293 ObjectPtr Object::transform(tf::Transformer& tf, const std::string& target_frame, const ros::Time& target_time) const
00294 {
00295   tf::StampedTransform transform;
00296   tf.lookupTransform(target_frame, this->header.frame_id, target_time, transform);
00297 
00298   ObjectPtr result(new Object(*this));
00299 
00300   tf::Pose pose;
00301   this->getPose(pose);
00302 
00303   // transform pose
00304   pose = transform * pose;
00305   result->setPose(pose);
00306 
00307   // rotate covariance matrix
00308   tf::Matrix3x3 rotation(transform.getBasis());
00309   tf::Matrix3x3 cov(covariance(0,0), covariance(0,1), covariance(0,2),
00310                     covariance(1,0), covariance(1,1), covariance(1,2),
00311                     covariance(2,0), covariance(2,1), covariance(2,2));
00312   result->setCovariance(rotation * cov * rotation.transpose());
00313 
00314   // set new frame_id
00315   result->header.frame_id = target_frame;
00316 
00317   return result;
00318 }
00319 
00320 double Object::getDistance(const Object &other)
00321 {
00322   return (this->getPosition() - other.getPosition()).norm();
00323 }
00324 
00325 } // namespace hector_object_tracker


hector_object_tracker
Author(s): Johannes Meyer
autogenerated on Mon Jun 27 2016 05:01:21