Go to the documentation of this file.00001 #include "joint_tracker/RigidJointFilter.h"
00002
00003 using namespace omip;
00004
00005 RigidJointFilter::RigidJointFilter():
00006 JointFilter()
00007 {
00008
00009 _measurements_likelihood = 1.0;
00010 _motion_memory_prior = 1.0;
00011 }
00012
00013 RigidJointFilter::~RigidJointFilter()
00014 {
00015
00016 }
00017
00018 RigidJointFilter::RigidJointFilter(const RigidJointFilter &rigid_joint) :
00019 JointFilter(rigid_joint)
00020 {
00021 }
00022
00023 void RigidJointFilter::initialize()
00024 {
00025 JointFilter::initialize();
00026
00027 _measurements_likelihood = 1.0;
00028 }
00029
00030 void RigidJointFilter::setMaxTranslationRigid(double max_trans)
00031 {
00032 this->_rig_max_translation = max_trans;
00033 }
00034
00035 void RigidJointFilter::setMaxRotationRigid(double max_rot)
00036 {
00037 this->_rig_max_rotation = max_rot;
00038 }
00039
00040 void RigidJointFilter::predictMeasurement()
00041 {
00042 this->_predicted_delta_pose_in_rrbf = Eigen::Twistd(0., 0., 0., 0., 0., 0.);
00043
00044 Eigen::Displacementd predicted_delta = this->_predicted_delta_pose_in_rrbf.exp(1e-20);
00045 Eigen::Displacementd T_rrbf_srbf_t0 = this->_srb_initial_pose_in_rrbf.exp(1.0e-20);
00046 Eigen::Displacementd T_rrbf_srbf_t_next = predicted_delta * T_rrbf_srbf_t0;
00047
00048 this->_srb_predicted_pose_in_rrbf = T_rrbf_srbf_t_next.log(1.0e-20);
00049 }
00050
00051 void RigidJointFilter::estimateMeasurementHistoryLikelihood()
00052 {
00053 double p_one_meas_given_model_params = 0;
00054 double p_all_meas_given_model_params = 0;
00055
00056 double sigma_translation = 0.05;
00057 double sigma_rotation = 0.2;
00058
00059 double accumulated_error = 0.;
00060
00061 double frame_counter = 0.;
00062
00063
00064 size_t trajectory_length = this->_delta_poses_in_rrbf.size();
00065 size_t amount_samples = std::min(trajectory_length, (size_t)this->_likelihood_sample_num);
00066
00067
00068
00069 double delta_idx_samples = (double)std::max(1., (double)trajectory_length/(double)this->_likelihood_sample_num);
00070
00071 size_t current_idx = 0;
00072
00073
00074 for (size_t sample_idx = 0; sample_idx < amount_samples; sample_idx++)
00075 {
00076 current_idx = boost::math::round(sample_idx*delta_idx_samples);
00077 Eigen::Displacementd rb2_last_delta_relative_displ = this->_delta_poses_in_rrbf.at(current_idx).exp(1e-12);
00078 Eigen::Vector3d rb2_last_delta_relative_translation = rb2_last_delta_relative_displ.getTranslation();
00079 Eigen::Quaterniond rb2_last_delta_relative_rotation = Eigen::Quaterniond(rb2_last_delta_relative_displ.qw(),
00080 rb2_last_delta_relative_displ.qx(),
00081 rb2_last_delta_relative_displ.qy(),
00082 rb2_last_delta_relative_displ.qz());
00083
00084 Eigen::Vector3d rigid_joint_translation = Eigen::Vector3d(0.,0.,0.);
00085 Eigen::Displacementd rb2_last_delta_relative_displ_rigid_hyp = Eigen::Twistd(0.,
00086 0.,
00087 0.,
00088 rigid_joint_translation.x(),
00089 rigid_joint_translation.y(),
00090 rigid_joint_translation.z()).exp(1e-12);
00091 Eigen::Vector3d rb2_last_delta_relative_translation_rigid_hyp = rb2_last_delta_relative_displ_rigid_hyp.getTranslation();
00092 Eigen::Quaterniond rb2_last_delta_relative_rotation_rigid_hyp = Eigen::Quaterniond(rb2_last_delta_relative_displ_rigid_hyp.qw(),
00093 rb2_last_delta_relative_displ_rigid_hyp.qx(),
00094 rb2_last_delta_relative_displ_rigid_hyp.qy(),
00095 rb2_last_delta_relative_displ_rigid_hyp.qz());
00096
00097
00098 double translation_error = (rb2_last_delta_relative_translation - rb2_last_delta_relative_translation_rigid_hyp).norm();
00099
00100 if(translation_error > this->_rig_max_translation)
00101 {
00102 _motion_memory_prior = 0.0;
00103 }
00104
00105 Eigen::Quaterniond rotation_error = rb2_last_delta_relative_rotation.inverse() * rb2_last_delta_relative_rotation_rigid_hyp;
00106 double rotation_error_angle = Eigen::Displacementd(0., 0., 0., rotation_error.w(),rotation_error.x(), rotation_error.y(),rotation_error.z()).log(1e-12).norm();
00107
00108
00109 if(rotation_error_angle > this->_rig_max_rotation)
00110 {
00111 _motion_memory_prior = 0.0;
00112 }
00113
00114 accumulated_error += translation_error + fabs(rotation_error_angle);
00115
00116 p_one_meas_given_model_params = (1.0/(sigma_translation*sqrt(2.0*M_PI)))*exp((-1.0/2.0)*pow(translation_error/sigma_translation, 2)) *
00117 (1.0/(sigma_rotation*sqrt(2.0*M_PI)))*exp((-1.0/2.0)*pow(rotation_error_angle/sigma_rotation, 2));
00118
00119 p_all_meas_given_model_params += (p_one_meas_given_model_params/(double)amount_samples);
00120
00121 frame_counter++;
00122 }
00123
00124 this->_measurements_likelihood = p_all_meas_given_model_params;
00125 }
00126
00127 void RigidJointFilter::estimateUnnormalizedModelProbability()
00128 {
00129 this->_unnormalized_model_probability = _model_prior_probability*_measurements_likelihood*_motion_memory_prior;
00130 }
00131
00132 geometry_msgs::TwistWithCovariance RigidJointFilter::getPredictedSRBDeltaPoseWithCovInSensorFrame()
00133 {
00134
00135 Eigen::Twistd delta_rrb_in_sf = this->_rrb_current_vel_in_sf*(this->_loop_period_ns/1e9);
00136 Eigen::Twistd delta_srb_in_sf = delta_rrb_in_sf;
00137
00138 geometry_msgs::TwistWithCovariance hypothesis;
00139
00140 hypothesis.twist.linear.x = delta_srb_in_sf.vx();
00141 hypothesis.twist.linear.y = delta_srb_in_sf.vy();
00142 hypothesis.twist.linear.z = delta_srb_in_sf.vz();
00143 hypothesis.twist.angular.x = delta_srb_in_sf.rx();
00144 hypothesis.twist.angular.y = delta_srb_in_sf.ry();
00145 hypothesis.twist.angular.z = delta_srb_in_sf.rz();
00146
00147 for (unsigned int i = 0; i < 6; i++)
00148 {
00149 for (unsigned int j = 0; j < 6; j++)
00150 {
00151 hypothesis.covariance[6 * i + j] = this->_rrb_vel_cov_in_sf(i, j)*(this->_loop_period_ns/1e9);
00152 }
00153 }
00154
00155 return hypothesis;
00156 }
00157
00158 geometry_msgs::TwistWithCovariance RigidJointFilter::getPredictedSRBVelocityWithCovInSensorFrame()
00159 {
00160
00161 Eigen::Twistd delta_rrb_in_sf = this->_rrb_current_vel_in_sf;
00162 Eigen::Twistd delta_srb_in_sf = delta_rrb_in_sf;
00163
00164 geometry_msgs::TwistWithCovariance hypothesis;
00165
00166 hypothesis.twist.linear.x = delta_srb_in_sf.vx();
00167 hypothesis.twist.linear.y = delta_srb_in_sf.vy();
00168 hypothesis.twist.linear.z = delta_srb_in_sf.vz();
00169 hypothesis.twist.angular.x = delta_srb_in_sf.rx();
00170 hypothesis.twist.angular.y = delta_srb_in_sf.ry();
00171 hypothesis.twist.angular.z = delta_srb_in_sf.rz();
00172
00173 for (unsigned int i = 0; i < 6; i++)
00174 {
00175 for (unsigned int j = 0; j < 6; j++)
00176 {
00177 hypothesis.covariance[6 * i + j] = this->_rrb_vel_cov_in_sf(i, j);
00178 }
00179 }
00180
00181 return hypothesis;
00182 }
00183
00184 geometry_msgs::TwistWithCovariance RigidJointFilter::getPredictedSRBPoseWithCovInSensorFrame()
00185 {
00186 Eigen::Twistd delta_rrb_in_sf = this->_rrb_current_vel_in_sf*(this->_loop_period_ns/1e9);
00187 Eigen::Twistd rrb_next_pose_in_sf = (delta_rrb_in_sf.exp(1e-12)*this->_rrb_current_pose_in_sf.exp(1e-12)).log(1e-12);
00188
00189
00190 Eigen::Displacementd T_sf_rrbf_next = rrb_next_pose_in_sf.exp(1e-12);
00191 Eigen::Displacementd T_rrbf_srbf_next = this->_srb_predicted_pose_in_rrbf.exp(1e-12);
00192
00193 Eigen::Displacementd T_sf_srbf_next = T_rrbf_srbf_next*T_sf_rrbf_next;
00194
00195 Eigen::Twistd srb_next_pose_in_sf = T_sf_srbf_next.log(1e-12);
00196
00197 geometry_msgs::TwistWithCovariance hypothesis;
00198
00199 hypothesis.twist.linear.x = srb_next_pose_in_sf.vx();
00200 hypothesis.twist.linear.y = srb_next_pose_in_sf.vy();
00201 hypothesis.twist.linear.z = srb_next_pose_in_sf.vz();
00202 hypothesis.twist.angular.x = srb_next_pose_in_sf.rx();
00203 hypothesis.twist.angular.y = srb_next_pose_in_sf.ry();
00204 hypothesis.twist.angular.z = srb_next_pose_in_sf.rz();
00205
00206 Eigen::Matrix<double,6,6> new_pose_covariance = this->_rrb_pose_cov_in_sf;
00207 for (unsigned int i = 0; i < 6; i++)
00208 {
00209 for (unsigned int j = 0; j < 6; j++)
00210 {
00211 hypothesis.covariance[6 * i + j] = new_pose_covariance(i, j);
00212 }
00213 }
00214
00215 #ifdef PUBLISH_PREDICTED_POSE_AS_PWC
00216
00217 geometry_msgs::PoseWithCovarianceStamped pose_with_cov_stamped;
00218 pose_with_cov_stamped.header.stamp = ros::Time::now();
00219 pose_with_cov_stamped.header.frame_id = "camera_rgb_optical_frame";
00220
00221 Eigen::Displacementd displ_from_twist = srb_next_pose_in_sf.exp(1e-12);
00222 pose_with_cov_stamped.pose.pose.position.x = displ_from_twist.x();
00223 pose_with_cov_stamped.pose.pose.position.y = displ_from_twist.y();
00224 pose_with_cov_stamped.pose.pose.position.z = displ_from_twist.z();
00225 pose_with_cov_stamped.pose.pose.orientation.x = displ_from_twist.qx();
00226 pose_with_cov_stamped.pose.pose.orientation.y = displ_from_twist.qy();
00227 pose_with_cov_stamped.pose.pose.orientation.z = displ_from_twist.qz();
00228 pose_with_cov_stamped.pose.pose.orientation.w = displ_from_twist.qw();
00229
00230 for (unsigned int i = 0; i < 6; i++)
00231 for (unsigned int j = 0; j < 6; j++)
00232 pose_with_cov_stamped.pose.covariance[6 * i + j] = hypothesis.covariance[6 * i + j];
00233 _predicted_next_pose_publisher.publish(pose_with_cov_stamped);
00234 #endif
00235
00236 return hypothesis;
00237 }
00238
00239 std::vector<visualization_msgs::Marker> RigidJointFilter::getJointMarkersInRRBFrame() const
00240 {
00241 std::vector<visualization_msgs::Marker> rigid_markers;
00242
00243 visualization_msgs::Marker connection_marker;
00244 connection_marker.ns = "kinematic_structure";
00245 connection_marker.action = visualization_msgs::Marker::ADD;
00246 connection_marker.type = visualization_msgs::Marker::ARROW;
00247 connection_marker.id = 3 * this->_joint_id;
00248 connection_marker.scale.x = 0.02f;
00249 connection_marker.scale.y = 0.f;
00250 connection_marker.scale.z = 0.f;
00251 connection_marker.color.r = 0.f;
00252 connection_marker.color.g = 0.f;
00253 connection_marker.color.b = 1.f;
00254 connection_marker.color.a = 1.f;
00255
00256 Eigen::Displacementd current_ref_pose_displ = this->_rrb_current_pose_in_sf.exp(1e-12);
00257 Eigen::Affine3d current_ref_pose;
00258 current_ref_pose.matrix() = current_ref_pose_displ.toHomogeneousMatrix();
00259 Eigen::Vector3d second_centroid_relative_to_ref_body = current_ref_pose.inverse() * this->_srb_centroid_in_sf;
00260 geometry_msgs::Point pt1;
00261 pt1.x = second_centroid_relative_to_ref_body.x();
00262 pt1.y = second_centroid_relative_to_ref_body.y();
00263 pt1.z = second_centroid_relative_to_ref_body.z();
00264 connection_marker.points.push_back(pt1);
00265
00266
00267 Eigen::Vector3d first_centroid_relative_to_ref_body = current_ref_pose.inverse() * this->_rrb_centroid_in_sf;
00268 geometry_msgs::Point pt2;
00269 pt2.x = first_centroid_relative_to_ref_body.x();
00270 pt2.y = first_centroid_relative_to_ref_body.y();
00271 pt2.z = first_centroid_relative_to_ref_body.z();
00272 connection_marker.points.push_back(pt2);
00273
00274 rigid_markers.push_back(connection_marker);
00275
00276
00277 visualization_msgs::Marker empty_marker;
00278 empty_marker.pose.position.x = 0.;
00279 empty_marker.pose.position.y = 0.;
00280 empty_marker.pose.position.z = 0.;
00281 empty_marker.header.frame_id = "camera_rgb_optical_frame";
00282 empty_marker.type = visualization_msgs::Marker::SPHERE;
00283 empty_marker.action = visualization_msgs::Marker::DELETE;
00284 empty_marker.scale.x = 0.0001;
00285 empty_marker.scale.y = 0.0001;
00286 empty_marker.scale.z = 0.0001;
00287 empty_marker.color.a = 0.3;
00288 empty_marker.color.r = 0.0;
00289 empty_marker.color.g = 0.0;
00290 empty_marker.color.b = 1.0;
00291 empty_marker.ns = "kinematic_structure";
00292 empty_marker.id = 3 * this->_joint_id + 1;
00293
00294 rigid_markers.push_back(empty_marker);
00295
00296 empty_marker.id = 3 * this->_joint_id + 2;
00297 rigid_markers.push_back(empty_marker);
00298
00299 empty_marker.ns = "kinematic_structure_uncertainty";
00300 empty_marker.id = 3 * this->_joint_id ;
00301
00302 rigid_markers.push_back(empty_marker);
00303
00304 empty_marker.id = 3* this->_joint_id + 1;
00305
00306 rigid_markers.push_back(empty_marker);
00307
00308 empty_marker.id = 3* this->_joint_id + 2;
00309
00310 rigid_markers.push_back(empty_marker);
00311
00312 return rigid_markers;
00313 }
00314
00315 JointFilterType RigidJointFilter::getJointFilterType() const
00316 {
00317 return RIGID_JOINT;
00318 }
00319
00320 std::string RigidJointFilter::getJointFilterTypeStr() const
00321 {
00322 return std::string("RigidJointFilter");
00323 }
00324