diff --git a/include/subscriber_landmarks.h b/include/subscriber_landmarks.h
index 6b8d16b07f022bc644e81bec35cb6dfaa3cc87da..f389542a57bfcc607692c7c772e56bf69bf89d93 100644
--- a/include/subscriber_landmarks.h
+++ b/include/subscriber_landmarks.h
@@ -42,6 +42,9 @@ class SubscriberLandmarks : public Subscriber
{
protected:
SizeEigen dim;
+ bool inverse_detections_;
+ Eigen::Vector3d sensor_p_;
+ Eigen::Quaterniond sensor_q_;
public:
diff --git a/src/node.cpp b/src/node.cpp
index 4df79958f0cebce854c69224d677276459bcadd0..7e102f812439d204fa69dc6e65142ca57ebcd263 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -130,11 +130,15 @@ WolfRosNode::WolfRosNode()
l->load();
loaders_.push_back(l);
+ auto sensor_ptr = problem_ptr_->findSensor(sensor);
+ if (not sensor_ptr)
+ throw std::runtime_error("Sensor " + sensor + " was not found!");
+
WOLF_TRACE("From sensor {" + sensor + "} subscribing {" + type + "} to {" + topic + "} topic");
subscribers_.push_back(FactorySubscriber::create(type,
name,
server,
- problem_ptr_->findSensor(sensor),
+ sensor_ptr,
nh_));
}
diff --git a/src/subscriber_landmarks.cpp b/src/subscriber_landmarks.cpp
index 7c2ac3507258dc8ecb5cebcffca1514e28c132e4..503b9f1cd735c29a8394f4400642f38407418f80 100644
--- a/src/subscriber_landmarks.cpp
+++ b/src/subscriber_landmarks.cpp
@@ -26,11 +26,16 @@
* WOLF includes *
**************************/
#include <core/capture/capture_landmarks_external.h>
+#include <core/math/covariance.h>
+#include <core/math/rotations.h>
+#include <core/math/SE3.h>
/**************************
* ROS includes *
**************************/
-#include <tf/transform_datatypes.h>
+// #include <tf/transform_datatypes.h>
+
+using namespace Eigen;
namespace wolf
{
@@ -41,6 +46,10 @@ 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)
@@ -50,7 +59,7 @@ void SubscriberLandmarks::initialize(ros::NodeHandle& nh, const std::string& top
void SubscriberLandmarks::callback(const wolf_ros_node::LandmarkDetectionArray::ConstPtr& msg)
{
- ROS_DEBUG("SubscriberLandmarks::callback");
+ ROS_INFO("SubscriberLandmarks::callback: %lu detections", msg->detections.size());
updateLastHeader(msg->header);
@@ -58,40 +67,55 @@ void SubscriberLandmarks::callback(const wolf_ros_node::LandmarkDetectionArray::
// Extract detections from msg
for (auto i = 0; i < msg->detections.size(); i++)
{
- // measurement
- VectorXd meas(dim == 2 ? 3 : 7);
- if (dim == 2)
- {
- meas(0) = msg->detections.at(i).pose.pose.position.x;
- meas(1) = msg->detections.at(i).pose.pose.position.y;
- meas(2) = tf::getYaw(msg->detections.at(i).pose.pose.orientation);
- }
- else
- {
- meas(0) = msg->detections.at(i).pose.pose.position.x;
- meas(1) = msg->detections.at(i).pose.pose.position.y;
- meas(2) = msg->detections.at(i).pose.pose.position.z;
- meas(3) = msg->detections.at(i).pose.pose.orientation.x;
- meas(4) = msg->detections.at(i).pose.pose.orientation.y;
- meas(5) = msg->detections.at(i).pose.pose.orientation.z;
- meas(6) = msg->detections.at(i).pose.pose.orientation.w;
- }
- // covariance
+ // 3D measurement from msg
+ VectorXd meas(7);
+ meas << msg->detections.at(i).pose.pose.position.x,
+ msg->detections.at(i).pose.pose.position.y,
+ msg->detections.at(i).pose.pose.position.z,
+ msg->detections.at(i).pose.pose.orientation.x,
+ msg->detections.at(i).pose.pose.orientation.y,
+ 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(dim == 2 ? 3 : 6, dim == 2 ? 3 : 6);
+ 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);
+ cov.topLeftCorner<3,3>() = R.transpose() * cov.topLeftCorner<3,3>() * R;
+ //TODO: rotate covariance for orientation
+ }
+
+ // 2D
if (dim == 2)
{
- cov << msg->detections.at(i).pose.covariance.at(0), msg->detections.at(i).pose.covariance.at(1), msg->detections.at(i).pose.covariance.at(5),
- msg->detections.at(i).pose.covariance.at(6), msg->detections.at(i).pose.covariance.at(7), msg->detections.at(i).pose.covariance.at(11),
- msg->detections.at(i).pose.covariance.at(30), msg->detections.at(i).pose.covariance.at(31), msg->detections.at(i).pose.covariance.at(35);
+ VectorXd meas2d = meas.head<3>();
+ meas2d(2) = getYaw(q2R(meas.tail<4>()));
+
+ MatrixXd cov2d(3,3);
+ cov2d << cov(0,0), cov(0,1), cov(0,5),
+ cov(1,0), cov(1,1), cov(1,5),
+ cov(5,0), cov(5,1), cov(5,5);
+
+ meas = meas2d;
+ cov = cov2d;
}
- else
- cov = Eigen::Map<const Eigen::Matrix6d>(msg->detections.at(i).pose.covariance.data());
+
+ std::cout << "\tid " << msg->detections.at(i).id << ": quality: " << msg->detections.at(i).quality << ", meas: " << meas.transpose() << std::endl;
// fill capture
+ makePosDef(cov);
cap->addDetection(msg->detections.at(i).id, meas, cov, msg->detections.at(i).quality);
}