diff --git a/include/subscriber_landmarks.h b/include/subscriber_landmarks.h index c042ec915dcf0e12e594549a15054aa97bbe36a0..f389542a57bfcc607692c7c772e56bf69bf89d93 100644 --- a/include/subscriber_landmarks.h +++ b/include/subscriber_landmarks.h @@ -43,6 +43,8 @@ class SubscriberLandmarks : public Subscriber protected: SizeEigen dim; bool inverse_detections_; + Eigen::Vector3d sensor_p_; + Eigen::Quaterniond sensor_q_; public: diff --git a/src/subscriber_landmarks.cpp b/src/subscriber_landmarks.cpp index c49f87dd33248f2f08bd179533b182c463fdee0e..503b9f1cd735c29a8394f4400642f38407418f80 100644 --- a/src/subscriber_landmarks.cpp +++ b/src/subscriber_landmarks.cpp @@ -35,6 +35,8 @@ **************************/ // #include <tf/transform_datatypes.h> +using namespace Eigen; + namespace wolf { SubscriberLandmarks::SubscriberLandmarks(const std::string& _unique_name, @@ -45,6 +47,9 @@ SubscriberLandmarks::SubscriberLandmarks(const std::string& _unique_name, assert(_sensor_ptr); dim = _sensor_ptr->getProblem()->getDim(); inverse_detections_ = _server.getParam<bool>(prefix_ + "/inverse_detections"); + auto sensor_extrinsics = _server.getParam<VectorXd>(prefix_ + "/sensor_extrinsics"); + sensor_p_ = sensor_extrinsics.head<3>(); + sensor_q_ = Quaterniond(Vector4d(sensor_extrinsics.tail<4>())); } void SubscriberLandmarks::initialize(ros::NodeHandle& nh, const std::string& topic) @@ -72,18 +77,23 @@ void SubscriberLandmarks::callback(const wolf_ros_node::LandmarkDetectionArray:: msg->detections.at(i).pose.pose.orientation.z, msg->detections.at(i).pose.pose.orientation.w; + meas.head<3>() = sensor_p_ + sensor_q_ * meas.head<3>(); + meas.tail<4>() = (sensor_q_ * Quaterniond(Vector4d(meas.tail<4>()))).coeffs(); + // PoseWithCovariance documentation: // Row-major representation of the 6x6 covariance matrix. // The orientation parameters use a fixed-axis representation. // In order, the parameters are: (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) MatrixXd cov = Eigen::Map<const Eigen::Matrix6d>(msg->detections.at(i).pose.covariance.data()); + auto R = q2R(meas.tail<4>()); + cov.topLeftCorner<3,3>() = R * cov.topLeftCorner<3,3>() * R.transpose(); + //TODO: rotate covariance for orientation // inverse measurement if (inverse_detections_) { meas = SE3::inverse(meas); - auto R = q2R(meas.tail<4>()); - cov.topLeftCorner<3,3>() = R * cov.topLeftCorner<3,3>() * R.transpose(); + cov.topLeftCorner<3,3>() = R.transpose() * cov.topLeftCorner<3,3>() * R; //TODO: rotate covariance for orientation }