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 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(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 worldmodel_msgs::Object Object::getMessage() const {
00049   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(), getObjectStateString(state));
00162   this->state.state = state;
00163 }
00164 
00165 void Object::intersect(const Eigen::Vector3f& positionB, const Eigen::Matrix3f& covarianceB, float support) {
00166   // old cov/covariance is A , new cov/covIn is B
00167   float omega = 0.5f;
00168 
00169   Eigen::Matrix3f A(covariance.inverse() * omega);
00170   Eigen::Matrix3f B(covarianceB.inverse() * (1.0f - omega));
00171 
00172   covariance = (A + B).inverse();
00173   position = covariance * (A * position + B * positionB);
00174 
00175   setPosition(position);
00176   setCovariance(covariance);
00177   addSupport(support);
00178 }
00179 
00180 void Object::update(const Eigen::Vector3f& positionB, const Eigen::Matrix3f& covarianceB, float support) {
00181   Eigen::Matrix3f A(covariance.inverse());
00182   Eigen::Matrix3f B(covarianceB.inverse());
00183 
00184   covariance = (A + B).inverse();
00185   position = covariance * (A * position + B * positionB);
00186 
00187   setPosition(position);
00188   setCovariance(covariance);
00189   addSupport(support);
00190 }
00191 
00192 void Object::getVisualization(visualization_msgs::MarkerArray &markers) const {
00193   visualization_msgs::Marker marker;
00194   std::string postfix;
00195 
00196   // default color
00197   marker.color = param(_marker_color, this->info.class_id);
00198 
00199   switch(this->state.state) {
00200     case worldmodel_msgs::ObjectState::CONFIRMED:
00201       marker.color.r = 0.0;
00202       marker.color.g = 0.8;
00203       marker.color.b = 0.0;
00204       postfix = " (CONFIRMED)";
00205       break;
00206     case worldmodel_msgs::ObjectState::DISCARDED:
00207       marker.color.a = 0.5;
00208       postfix = " (DISCARDED)";
00209       break;
00210     case worldmodel_msgs::ObjectState::INACTIVE:
00211       marker.color.a = 0.5;
00212       postfix = " (INACTIVE)";
00213       break;
00214     case worldmodel_msgs::ObjectState::UNKNOWN:
00215       marker.color.a = 0.5;
00216       break;
00217     case worldmodel_msgs::ObjectState::PENDING:
00218       marker.color.a = 0.5;
00219       postfix = " (PENDING)";
00220       break;
00221     default:
00222       break;
00223   }
00224 
00225   marker.header = this->header;
00226   marker.action = visualization_msgs::Marker::ADD;
00227   getPose(marker.pose);
00228   marker.ns = this->info.class_id;
00229 
00230   marker.type = visualization_msgs::Marker::ARROW;
00231   marker.scale.x = 1.0;
00232   marker.scale.y = 1.0;
00233   marker.scale.z = 1.0;
00234   // markers.markers.push_back(marker);
00235 
00236   marker.type = visualization_msgs::Marker::SPHERE;
00237   marker.scale.x = 0.1;
00238   marker.scale.y = 0.1;
00239   marker.scale.z = 0.1;
00240   markers.markers.push_back(marker);
00241 
00242   marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00243   marker.text = (!this->info.name.empty() ? this->info.name : this->info.object_id) + postfix;
00244   marker.scale.x = 0.0;
00245   marker.scale.y = 0.0;
00246   marker.scale.z = 0.1;
00247   marker.pose.position.z += 1.5 * marker.scale.z;
00248   markers.markers.push_back(marker);
00249 }
00250 
00251 void Object::setNamespace(const std::string &ns) {
00252   object_namespace = ns;
00253 }
00254 
00255 Object& Object::operator =(const worldmodel_msgs::Object& other) {
00256   header = other.header;
00257   info = other.info;
00258   state = other.state;
00259   setPose(other.pose);
00260   return *this;
00261 }
00262 
00263 ObjectPtr Object::transform(tf::Transformer& tf, const std::string& target_frame) const
00264 {
00265   return transform(tf, target_frame, this->header.stamp);
00266 }
00267 
00268 ObjectPtr Object::transform(tf::Transformer& tf, const std::string& target_frame, const ros::Time& target_time) const
00269 {
00270   tf::StampedTransform transform;
00271   tf.lookupTransform(target_frame, this->header.frame_id, target_time, transform);
00272 
00273   ObjectPtr result(new Object(*this));
00274 
00275   tf::Pose pose;
00276   this->getPose(pose);
00277 
00278   // transform pose
00279   pose = transform * pose;
00280   result->setPose(pose);
00281 
00282   // rotate covariance matrix
00283   tf::Matrix3x3 rotation(transform.getBasis());
00284   tf::Matrix3x3 cov(covariance(0,0), covariance(0,1), covariance(0,2),
00285                     covariance(1,0), covariance(1,1), covariance(1,2),
00286                     covariance(2,0), covariance(2,1), covariance(2,2));
00287   result->setCovariance(rotation * cov * rotation.transpose());
00288 
00289   // set new frame_id
00290   result->header.frame_id = target_frame;
00291 
00292   return result;
00293 }
00294 
00295 } // namespace hector_object_tracker
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_object_tracker
Author(s): Johannes Meyer
autogenerated on Mon Jul 15 2013 16:50:56