diff --git a/.gitignore b/.gitignore
index 3cf468ee6d2a274afeef59699469fb8ad24354df..99343204f8edbc561b5cbef82d0d5f5cb00a0361 100644
--- a/.gitignore
+++ b/.gitignore
@@ -6,3 +6,4 @@
build-debug/
build-release/
build/
+.clang-format
diff --git a/include/subscriber_landmarks.h b/include/subscriber_landmarks.h
index 6358facad06a44c2d5559df70e61be21928c8a10..0d2608157406956644bf2b25b7de8ec4fcd0dff1 100644
--- a/include/subscriber_landmarks.h
+++ b/include/subscriber_landmarks.h
@@ -41,8 +41,9 @@ namespace wolf
class SubscriberLandmarks : public Subscriber
{
protected:
- SizeEigen dim;
+ SizeEigen dim;
bool inverse_detections_;
+ unsigned int type_offset_, id_offset_;
Eigen::Vector3d sensor_p_;
Eigen::Quaterniond sensor_q_;
diff --git a/src/subscriber_landmarks.cpp b/src/subscriber_landmarks.cpp
index 0608bc6601fbb5fe71c3431c796af10e2c507d9c..f0db841e524045471ef2797c9e7f5bd49223a3fb 100644
--- a/src/subscriber_landmarks.cpp
+++ b/src/subscriber_landmarks.cpp
@@ -39,84 +39,92 @@ using namespace Eigen;
namespace wolf
{
-SubscriberLandmarks::SubscriberLandmarks(const std::string& _unique_name,
- const ParamsServer& _server,
+SubscriberLandmarks::SubscriberLandmarks(const std::string &_unique_name,
+ const ParamsServer &_server,
const SensorBasePtr _sensor_ptr)
- : Subscriber(_unique_name, _server, _sensor_ptr)
+ : Subscriber(_unique_name, _server, _sensor_ptr)
{
assert(_sensor_ptr);
- dim = _sensor_ptr->getProblem()->getDim();
- inverse_detections_ = _server.getParam<bool>(prefix_ + "/inverse_detections");
+ dim = _sensor_ptr->getProblem()->getDim();
+ inverse_detections_ = _server.getParam<bool>(prefix_ + "/inverse_detections");
+ type_offset_ = _server.getParam<unsigned int>(prefix_ + "/type_offset");
+ id_offset_ = _server.getParam<unsigned int>(prefix_ + "/id_offset");
auto sensor_extrinsics = _server.getParam<VectorXd>(prefix_ + "/sensor_extrinsics");
- sensor_p_ = sensor_extrinsics.head<3>();
- sensor_q_ = Quaterniond(Vector4d(sensor_extrinsics.tail<4>()));
+ sensor_p_ = sensor_extrinsics.head<3>();
+ sensor_q_ = Quaterniond(Vector4d(sensor_extrinsics.tail<4>()));
}
-void SubscriberLandmarks::initialize(ros::NodeHandle& nh, const std::string& topic)
+void SubscriberLandmarks::initialize(ros::NodeHandle &nh, const std::string &topic)
{
sub_ = nh.subscribe(topic, 100, &SubscriberLandmarks::callback, this);
}
-void SubscriberLandmarks::callback(const wolf_ros_node::LandmarkDetectionArray::ConstPtr& msg)
+void SubscriberLandmarks::callback(const wolf_ros_node::LandmarkDetectionArray::ConstPtr &msg)
{
ROS_DEBUG("SubscriberLandmarks::callback: %lu detections", msg->detections.size());
updateLastHeader(msg->header);
- auto cap = std::make_shared<CaptureLandmarksExternal>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec), sensor_ptr_);
+ auto cap = std::make_shared<CaptureLandmarksExternal>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec),
+ sensor_ptr_);
// Extract detections from msg
for (auto i = 0; i < msg->detections.size(); i++)
{
// 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 << 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 = 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
+ // 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);
- cov.topLeftCorner<3,3>() = R.transpose() * cov.topLeftCorner<3,3>() * R;
- //TODO: rotate covariance for orientation
+ meas = SE3::inverse(meas);
+ cov.topLeftCorner<3, 3>() = R.transpose() * cov.topLeftCorner<3, 3>() * R;
+ // TODO: rotate covariance for orientation
}
// 2D
if (dim == 2)
{
VectorXd meas2d = meas.head<3>();
- meas2d(2) = getYaw(q2R(meas.tail<4>()));
+ 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);
+ 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;
+ cov = cov2d;
}
-
- //std::cout << "\tid " << msg->detections.at(i).id << ": quality: " << msg->detections.at(i).quality << ", meas: " << meas.transpose() << std::endl;
+
+ // std::cout << "\tid " << msg->detections.at(i).id << ": quality: " << msg->detections.at(i).quality << ",
+ // meas: " << meas.transpose() << std::endl;
+
+ // ID & TYPE
+ WOLF_WARN_COND(msg->detections.at(i).id < -1,
+ "Received a LandmarkDetection with ID < -1, changing to -1 (undefined).");
+ WOLF_WARN_COND(msg->detections.at(i).type < -1,
+ "Received a LandmarkDetection with TYPE < -1, changing to -1 (undefined).");
+ int id = msg->detections.at(i).id < 0 ? -1 : msg->detections.at(i).id + id_offset_;
+ int type = msg->detections.at(i).type < 0 ? -1 : msg->detections.at(i).type + type_offset_;
// fill capture
makePosDef(cov);
- cap->addDetection(msg->detections.at(i).id, msg->detections.at(i).type, meas, cov, msg->detections.at(i).quality);
+ cap->addDetection(id, type, meas, cov, msg->detections.at(i).quality);
}
// process