added clang and subscriber_landmarks more params

.gitignore

@@ -6,3 +6,4 @@
 build-debug/
 build-release/
 build/
+.clang-format

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_;
 

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