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 #include "pose_sensor.h"
00033 #include <ssf_core/eigen_utils.h>
00034
00035 #define N_MEAS 7 // measurement size
00036 PoseSensorHandler::PoseSensorHandler(ssf_core::Measurements* meas) :
00037 MeasurementHandler(meas)
00038 {
00039 ros::NodeHandle pnh("~");
00040 pnh.param("measurement_world_sensor", measurement_world_sensor_, true);
00041 pnh.param("use_fixed_covariance", use_fixed_covariance_, false);
00042
00043 ROS_INFO_COND(measurement_world_sensor_, "interpreting measurement as sensor w.r.t. world");
00044 ROS_INFO_COND(!measurement_world_sensor_, "interpreting measurement as world w.r.t. sensor (e.g. ethzasl_ptam)");
00045
00046 ROS_INFO_COND(use_fixed_covariance_, "using fixed covariance");
00047 ROS_INFO_COND(!use_fixed_covariance_, "using covariance from sensor");
00048
00049 subscribe();
00050 }
00051
00052 void PoseSensorHandler::subscribe()
00053 {
00054 ros::NodeHandle nh("ssf_core");
00055 subMeasurement_ = nh.subscribe("pose_measurement", 1, &PoseSensorHandler::measurementCallback, this);
00056
00057 measurements->ssf_core_.registerCallback(&PoseSensorHandler::noiseConfig, this);
00058
00059 nh.param("meas_noise1", n_zp_, 0.01);
00060 nh.param("meas_noise2", n_zq_, 0.02);
00061 }
00062
00063 void PoseSensorHandler::noiseConfig(ssf_core::SSF_CoreConfig& config, uint32_t level)
00064 {
00065
00066
00067 this->n_zp_ = config.meas_noise1;
00068 this->n_zq_ = config.meas_noise2;
00069
00070 }
00071
00072 void PoseSensorHandler::measurementCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
00073 {
00074
00075
00076
00077
00078 ssf_core::State state_old;
00079 ros::Time time_old = msg->header.stamp;
00080 Eigen::Matrix<double, N_MEAS, N_STATE> H_old;
00081 Eigen::Matrix<double, N_MEAS, 1> r_old;
00082 Eigen::Matrix<double, N_MEAS, N_MEAS> R;
00083
00084 H_old.setZero();
00085 R.setZero();
00086
00087
00088 z_p_ = Eigen::Matrix<double, 3, 1>(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
00089 z_q_ = Eigen::Quaternion<double>(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
00090
00091
00092 R.block<6, 6> (0, 0) = Eigen::Matrix<double, 6, 6>(&msg->pose.covariance[0]);
00093
00094 R.block<3, 3> (0, 3) = Eigen::Matrix<double, 3, 3>::Zero();
00095 R.block<3, 3> (3, 0) = Eigen::Matrix<double, 3, 3>::Zero();
00096 R(6, 6) = 1e-6;
00097
00098
00099
00100
00101 if (!measurement_world_sensor_)
00102 {
00103 Eigen::Matrix<double, 3, 3> C_zq = z_q_.toRotationMatrix();
00104 z_q_ = z_q_.conjugate();
00105 z_p_ = -C_zq.transpose() * z_p_;
00106
00107 Eigen::Matrix<double, 6, 6> C_cov(Eigen::Matrix<double, 6, 6>::Zero());
00108 C_cov.block<3, 3> (0, 0) = C_zq;
00109 C_cov.block<3, 3> (3, 3) = C_zq;
00110
00111 R.block<6, 6> (0, 0) = C_cov.transpose() * R.block<6, 6> (0, 0) * C_cov;
00112 }
00113
00114
00115
00116 if (use_fixed_covariance_)
00117 {
00118 const double s_zp = n_zp_ * n_zp_;
00119 const double s_zq = n_zq_ * n_zq_;
00120 R = (Eigen::Matrix<double, N_MEAS, 1>() << s_zp, s_zp, s_zp, s_zq, s_zq, s_zq, 1e-6).finished().asDiagonal();
00121 }
00122
00123
00124 measurements->p_vc_ = z_p_;
00125 measurements->q_cv_ = z_q_;
00126
00127 unsigned char idx = measurements->ssf_core_.getClosestState(&state_old, time_old);
00128 if (state_old.time_ == -1)
00129 return;
00130
00131
00132 Eigen::Matrix<double, 3, 3> C_wv = state_old.q_wv_.conjugate().toRotationMatrix();
00133 Eigen::Matrix<double, 3, 3> C_q = state_old.q_.conjugate().toRotationMatrix();
00134 Eigen::Matrix<double, 3, 3> C_ci = state_old.q_ci_.conjugate().toRotationMatrix();
00135
00136
00137 Eigen::Matrix<double, 3, 1> vecold;
00138 vecold = (state_old.p_ + C_q.transpose() * state_old.p_ci_) * state_old.L_;
00139 Eigen::Matrix<double, 3, 3> skewold = skew(vecold);
00140
00141 Eigen::Matrix<double, 3, 3> pci_sk = skew(state_old.p_ci_);
00142
00143
00144
00145 H_old.block<3, 3> (0, 0) = C_wv.transpose() * state_old.L_;
00146 H_old.block<3, 3> (0, 6) = -C_wv.transpose() * C_q.transpose() * pci_sk * state_old.L_;
00147 H_old.block<3, 1> (0, 15) = C_wv.transpose() * C_q.transpose() * state_old.p_ci_ + C_wv.transpose() * state_old.p_;
00148 H_old.block<3, 3> (0, 16) = -C_wv.transpose() * skewold;
00149 H_old.block<3, 3> (0, 22) = C_wv.transpose() * C_q.transpose() * state_old.L_;
00150
00151
00152 H_old.block<3, 3> (3, 6) = C_ci;
00153 H_old.block<3, 3> (3, 16) = C_ci * C_q;
00154 H_old.block<3, 3> (3, 19) = Eigen::Matrix<double, 3, 3>::Identity();
00155 H_old(6, 18) = 1.0;
00156
00157
00158
00159 r_old.block<3, 1> (0, 0) = z_p_ - C_wv.transpose() * (state_old.p_ + C_q.transpose() * state_old.p_ci_) * state_old.L_;
00160
00161 Eigen::Quaternion<double> q_err;
00162 q_err = (state_old.q_wv_ * state_old.q_ * state_old.q_ci_).conjugate() * z_q_;
00163 r_old.block<3, 1> (3, 0) = q_err.vec() / q_err.w() * 2;
00164
00165 q_err = state_old.q_wv_;
00166 r_old(6, 0) = -2 * (q_err.w() * q_err.z() + q_err.x() * q_err.y()) / (1 - 2 * (q_err.y() * q_err.y() + q_err.z() * q_err.z()));
00167
00168
00169 measurements->ssf_core_.applyMeasurement(idx, H_old, r_old, R);
00170 }