Go to the documentation of this file.00001 #include "joint_tracker/DisconnectedJointFilter.h"
00002
00003 #include <random>
00004
00005 using namespace omip;
00006
00007 DisconnectedJointFilter::DisconnectedJointFilter() :
00008 JointFilter()
00009 {
00010 _unnormalized_model_probability = 0.8;
00011 }
00012
00013 DisconnectedJointFilter::~DisconnectedJointFilter()
00014 {
00015
00016 }
00017
00018 DisconnectedJointFilter::DisconnectedJointFilter(const DisconnectedJointFilter &disconnected_joint) :
00019 JointFilter(disconnected_joint)
00020 {
00021 }
00022
00023 void DisconnectedJointFilter::initialize()
00024 {
00025
00026 }
00027
00028 double DisconnectedJointFilter::getProbabilityOfJointFilter() const
00029 {
00030 return (_unnormalized_model_probability / this->_normalizing_term);
00031 }
00032
00033 geometry_msgs::TwistWithCovariance DisconnectedJointFilter::getPredictedSRBDeltaPoseWithCovInSensorFrame()
00034 {
00035
00036 std::default_random_engine generator;;
00037 std::uniform_real_distribution<double> distr(-100.0, 100.0);
00038 Eigen::Twistd srb_delta_pose_in_sf_next = Eigen::Twistd(distr(generator),distr(generator),distr(generator),
00039 distr(generator),distr(generator),distr(generator) );
00040
00041 geometry_msgs::TwistWithCovariance hypothesis;
00042
00043 hypothesis.twist.linear.x = srb_delta_pose_in_sf_next.vx();
00044 hypothesis.twist.linear.y = srb_delta_pose_in_sf_next.vy();
00045 hypothesis.twist.linear.z = srb_delta_pose_in_sf_next.vz();
00046 hypothesis.twist.angular.x = srb_delta_pose_in_sf_next.rx();
00047 hypothesis.twist.angular.y = srb_delta_pose_in_sf_next.ry();
00048 hypothesis.twist.angular.z = srb_delta_pose_in_sf_next.rz();
00049
00050
00051 for (unsigned int i = 0; i < 6; i++)
00052 {
00053 for (unsigned int j = 0; j < 6; j++)
00054 {
00055 if(i == j)
00056 {
00057 hypothesis.covariance[6 * i + j] = 1.0e6;
00058 }else
00059 {
00060 hypothesis.covariance[6 * i + j] = 1.0e3;
00061 }
00062 }
00063 }
00064
00065 return hypothesis;
00066 }
00067
00068 geometry_msgs::TwistWithCovariance DisconnectedJointFilter::getPredictedSRBVelocityWithCovInSensorFrame()
00069 {
00070
00071 std::default_random_engine generator;;
00072 std::uniform_real_distribution<double> distr(-100.0, 100.0);
00073 Eigen::Twistd srb_delta_pose_in_sf_next = Eigen::Twistd(distr(generator),distr(generator),distr(generator),
00074 distr(generator),distr(generator),distr(generator) );
00075
00076 geometry_msgs::TwistWithCovariance hypothesis;
00077
00078 hypothesis.twist.linear.x = srb_delta_pose_in_sf_next.vx();
00079 hypothesis.twist.linear.y = srb_delta_pose_in_sf_next.vy();
00080 hypothesis.twist.linear.z = srb_delta_pose_in_sf_next.vz();
00081 hypothesis.twist.angular.x = srb_delta_pose_in_sf_next.rx();
00082 hypothesis.twist.angular.y = srb_delta_pose_in_sf_next.ry();
00083 hypothesis.twist.angular.z = srb_delta_pose_in_sf_next.rz();
00084
00085
00086 for (unsigned int i = 0; i < 6; i++)
00087 {
00088 for (unsigned int j = 0; j < 6; j++)
00089 {
00090 if(i == j)
00091 {
00092 hypothesis.covariance[6 * i + j] = 1.0e6;
00093 }else
00094 {
00095 hypothesis.covariance[6 * i + j] = 1.0e3;
00096 }
00097 }
00098 }
00099
00100 return hypothesis;
00101 }
00102
00103 geometry_msgs::TwistWithCovariance DisconnectedJointFilter::getPredictedSRBPoseWithCovInSensorFrame()
00104 {
00105
00106
00107
00108
00109
00110
00111
00112 std::default_random_engine generator;;
00113 std::uniform_real_distribution<double> distr(-100.0, 100.0);
00114 Eigen::Twistd srb_pose_in_sf_next = Eigen::Twistd(distr(generator),distr(generator),distr(generator),
00115 distr(generator),distr(generator),distr(generator) );
00116
00117 geometry_msgs::TwistWithCovariance hypothesis;
00118 hypothesis.twist.linear.x = srb_pose_in_sf_next.vx();
00119 hypothesis.twist.linear.y = srb_pose_in_sf_next.vy();
00120 hypothesis.twist.linear.z = srb_pose_in_sf_next.vz();
00121 hypothesis.twist.angular.x = srb_pose_in_sf_next.rx();
00122 hypothesis.twist.angular.y = srb_pose_in_sf_next.ry();
00123 hypothesis.twist.angular.z = srb_pose_in_sf_next.rz();
00124
00125
00126 for (unsigned int i = 0; i < 6; i++)
00127 {
00128 for (unsigned int j = 0; j < 6; j++)
00129 {
00130 if(i == j)
00131 {
00132 hypothesis.covariance[6 * i + j] = 1.0e6;
00133 }else
00134 {
00135 hypothesis.covariance[6 * i + j] = 1.0e3;
00136 }
00137 }
00138 }
00139
00140
00141 #ifdef PUBLISH_PREDICTED_POSE_AS_PWC
00142
00143
00144 geometry_msgs::PoseWithCovarianceStamped pose_with_cov_stamped;
00145 pose_with_cov_stamped.header.stamp = ros::Time::now();
00146 pose_with_cov_stamped.header.frame_id = "camera_rgb_optical_frame";
00147
00148 Eigen::Displacementd displ_from_twist = srb_pose_in_sf_next.exp(1e-12);
00149 pose_with_cov_stamped.pose.pose.position.x = displ_from_twist.x();
00150 pose_with_cov_stamped.pose.pose.position.y = displ_from_twist.y();
00151 pose_with_cov_stamped.pose.pose.position.z = displ_from_twist.z();
00152 pose_with_cov_stamped.pose.pose.orientation.x = displ_from_twist.qx();
00153 pose_with_cov_stamped.pose.pose.orientation.y = displ_from_twist.qy();
00154 pose_with_cov_stamped.pose.pose.orientation.z = displ_from_twist.qz();
00155 pose_with_cov_stamped.pose.pose.orientation.w = displ_from_twist.qw();
00156
00157 for (unsigned int i = 0; i < 6; i++)
00158 for (unsigned int j = 0; j < 6; j++)
00159 pose_with_cov_stamped.pose.covariance[6 * i + j] = hypothesis.covariance[6 * i + j];
00160
00161 _predicted_next_pose_publisher.publish(pose_with_cov_stamped);
00162 #endif
00163
00164 return hypothesis;
00165 }
00166
00167 std::vector<visualization_msgs::Marker> DisconnectedJointFilter::getJointMarkersInRRBFrame() const
00168 {
00169
00170 std::vector<visualization_msgs::Marker> empty_vector;
00171 visualization_msgs::Marker empty_marker;
00172 empty_marker.pose.position.x = 0.;
00173 empty_marker.pose.position.y = 0.;
00174 empty_marker.pose.position.z = 0.;
00175
00176 empty_marker.type = visualization_msgs::Marker::SPHERE;
00177 empty_marker.action = visualization_msgs::Marker::DELETE;
00178 empty_marker.scale.x = 0.0001;
00179 empty_marker.scale.y = 0.0001;
00180 empty_marker.scale.z = 0.0001;
00181 empty_marker.color.a = 0.3;
00182 empty_marker.color.r = 0.0;
00183 empty_marker.color.g = 0.0;
00184 empty_marker.color.b = 1.0;
00185 empty_marker.ns = "kinematic_structure";
00186 empty_marker.id = 3 * this->_joint_id;
00187 empty_vector.push_back(empty_marker);
00188 empty_marker.id = 3 * this->_joint_id + 1;
00189 empty_vector.push_back(empty_marker);
00190 empty_marker.id = 3 * this->_joint_id + 2;
00191 empty_vector.push_back(empty_marker);
00192 empty_marker.ns = "kinematic_structure_uncertainty";
00193 empty_marker.id = 3 * this->_joint_id ;
00194 empty_vector.push_back(empty_marker);
00195 empty_marker.id = 3 * this->_joint_id + 1;
00196 empty_vector.push_back(empty_marker);
00197 empty_marker.id = 3 * this->_joint_id + 2;
00198 empty_vector.push_back(empty_marker);
00199
00200 return empty_vector;
00201 }
00202
00203 JointFilterType DisconnectedJointFilter::getJointFilterType() const
00204 {
00205 return DISCONNECTED_JOINT;
00206 }
00207
00208 std::string DisconnectedJointFilter::getJointFilterTypeStr() const
00209 {
00210 return std::string("DisconnectedJointFilter");
00211 }
00212
00213