added falko publisher

CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
 # find_package(Eigen3 REQUIRED)
 find_package(wolfcore REQUIRED)
 find_package(wolflaser REQUIRED)
+find_package(falkolib QUIET)
 
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
@@ -137,10 +138,20 @@ include_directories(
 # add_library(${PROJECT_NAME}
 #   src/${PROJECT_NAME}/wolf_ros.cpp
 # )
-add_library(subscriber_${PROJECT_NAME}
-  			src/subscriber_laser2d.cpp)
-add_library(publisher_${PROJECT_NAME} 
-			src/publisher_laser_map.cpp)
+if (falkolib_FOUND)
+	message("Found Falkolib. Compiling publisher_falko.")
+	add_library(subscriber_${PROJECT_NAME}
+	  			src/subscriber_laser2d.cpp)
+	add_library(publisher_${PROJECT_NAME} 
+				src/publisher_laser_map.cpp
+	  			src/publisher_falko.cpp)
+else()
+	message("Falkolib NOT FOUND.")
+	add_library(subscriber_${PROJECT_NAME}
+	  			src/subscriber_laser2d.cpp)
+	add_library(publisher_${PROJECT_NAME} 
+				src/publisher_laser_map.cpp)
+endif()		
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries

include/publisher_falko.h

+#ifndef PUBLISHER_FALKO_BSC_H
+#define PUBLISHER_FALKO_BSC_H
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <geometry_msgs/PoseArray.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <visualization_msgs/MarkerArray.h>
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include <core/common/wolf.h>
+#include <core/problem/problem.h>
+#include "laser/capture/capture_laser_2d.h"
+#include "laser/feature/feature_scene_falko.h"
+
+#include "publisher.h"
+
+namespace wolf {
+
+class PublisherFalko : public Publisher
+{
+    bool pose_array_, marker_, pose_with_cov_;
+    int  max_points_;
+
+    visualization_msgs::Marker marker_msg_falko_scene;
+
+    std_msgs::ColorRGBA marker_color_target_;
+    std_msgs::ColorRGBA marker_color_reference_;
+
+    bool          extrinsics_;
+    SensorBasePtr sensor_;
+    std::string   frame_id_, map_frame_id_;
+
+    FrameBasePtr last_frame;
+    FrameBasePtr KF_old;
+
+    ros::Publisher pub_keypoints_;
+    ros::Publisher pub_marker_scene_target_;
+    ros::Publisher pub_marker_scene_reference_;
+    ros::Publisher pub_marker_associations_;
+
+    // Reference scene info
+    std::map<CaptureBasePtr, visualization_msgs::Marker> map_markers_reference_scenes_;
+
+  public:
+    PublisherFalko(const std::string &_unique_name, const ParamsServer &_server, const ProblemPtr _problem);
+    WOLF_PUBLISHER_CREATE(PublisherFalko);
+
+    virtual ~PublisherFalko(){};
+
+    void initialize(ros::NodeHandle &nh, const std::string &topic) override;
+
+    void publishDerived() override;
+
+    void publishScene(std::vector<falkolib::FALKO> &_keypoints, laserscanutils::LaserScan &_scan,
+                      Eigen::Vector3d &_p_rob, Eigen::Quaterniond &_q_rob, ros::Publisher &_pub_lines,
+                      std_msgs::ColorRGBA _marker_color, int & _id, CaptureBasePtr cap);
+
+    void publishEmpty();
+
+    void publishAssociations(std::vector<std::pair<int, int>> &associations_, CaptureBasePtr &cap_target,
+                             CaptureBasePtr &cap_reference);
+
+    void getKeypointsAndScan(FrameBasePtr &_frame, laserscanutils::LaserScan &_scan,
+                             std::vector<falkolib::FALKO> &_keypoints);
+
+    void getPosition(Eigen::Vector3d &p, Eigen::Quaterniond &q, VectorComposite current_state);
+
+  protected:
+    //Eigen::Quaterniond    q_frame_;
+    //Eigen::Vector3d       t_frame_;
+    int                   id_old_ = 1;
+
+    std::map<FrameBasePtr, VectorComposite> map_frame_states;
+};
+
+WOLF_REGISTER_PUBLISHER(PublisherFalko)
+} // namespace wolf
+
+#endif

src/publisher_falko.cpp

+#include "publisher_falko.h"
+#include "laser/sensor/sensor_laser_2d.h"
+#include "laser/processor/processor_loop_closure_falko.h"
+
+namespace wolf {
+
+PublisherFalko::PublisherFalko(const std::string &_unique_name, const ParamsServer &_server,
+                               const ProblemPtr _problem)
+: Publisher(_unique_name, _server, _problem)
+{
+    Eigen::Vector4d marker_color_v;
+    marker_color_v         = getParamWithDefault<Eigen::Vector4d>(_server, prefix_ + "/marker_color",
+                                                                  (Eigen::Vector4d() << 1, 0, 0, 0.5).finished()); // red
+    marker_color_target_.r = marker_color_v(0);
+    marker_color_target_.g = marker_color_v(1);
+    marker_color_target_.b = marker_color_v(2);
+    marker_color_target_.a = marker_color_v(3);
+
+    marker_color_reference_.r = 0;
+    marker_color_reference_.g = 0;
+    marker_color_reference_.b = 1;
+    marker_color_reference_.a = 0.5;
+
+    max_points_ = getParamWithDefault<int>(_server, prefix_ + "/max_points", 1e3);
+
+    extrinsics_ = _server.getParam<bool>(prefix_ + "/extrinsics");
+    sensor_     = _problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor"));
+    // dynamic_ptr_cast(sensor_)
+    if (!sensor_)
+        throw std::runtime_error("sensor not found");
+    frame_id_ = _server.getParam<std::string>(prefix_ + "/frame_id");
+}
+
+void PublisherFalko::initialize(ros::NodeHandle &nh, const std::string &topic)
+{
+    std::string map_frame_id;
+    nh.param<std::string>("map_frame_id", map_frame_id_, "map");
+
+    // initialize msg and publisher
+
+    // LINE MARKER
+    marker_msg_falko_scene.header.frame_id = map_frame_id_;
+    marker_msg_falko_scene.type            = visualization_msgs::Marker::LINE_STRIP;
+    marker_msg_falko_scene.scale.x         = 0.1;
+
+    pub_marker_scene_target_ = nh.advertise<visualization_msgs::Marker>(topic + "_lines", 1);
+
+    pub_marker_scene_reference_ = nh.advertise<visualization_msgs::Marker>(topic + "_lines_other_1", 10);
+
+    pub_marker_associations_ = nh.advertise<visualization_msgs::Marker>(topic + "_lines_asso", 10);
+}
+
+void PublisherFalko::publishDerived()
+{
+    TimeStamp loc_ts = problem_->getTimeStamp();
+    auto      KF     = problem_->getLastFrame();
+    // KF not ready
+    if (!KF)
+        return;
+
+    auto cap = KF->getCaptureOf(sensor_, "CaptureLaser2d");
+    if (not cap)
+        return;
+
+    VectorComposite current_state = KF->getState("PO");
+
+    map_frame_states.insert({KF, current_state}); // store states
+
+    // state not ready
+    if (current_state.count('P') == 0 or current_state.count('O') == 0 or not loc_ts.ok())
+    {
+        return;
+    }
+
+    // 2D robot position and yaw
+    Eigen::Vector3d    p = Eigen::Vector3d::Zero();
+    Eigen::Quaterniond q;
+    getPosition(p, q, current_state);
+
+    // Fill ros msg headers
+    marker_msg_falko_scene.header.stamp = ros::Time::now();
+
+    // LOOK FOR TARGET SCENE
+    laserscanutils::LaserScan    scan;
+    std::vector<falkolib::FALKO> keypoints;
+    getKeypointsAndScan(KF, scan, keypoints);
+
+    // PUBLISH KEYPOINTS MAP OF CURRENT SCENE
+    if (p(0) == 0 and p(1) == 0)
+        return;
+
+    // LOOK FOR REFERENCE SCENE
+    auto processor_list = sensor_->getProcessorList();
+
+    ProcessorBasePtr processor_falko;
+    for (auto processor : processor_list)
+    {
+        if (processor->getType() == "ProcessorLoopClosureFalko")
+        {
+            processor_falko = processor;
+            break;
+        }
+    }
+    if (!processor_falko)
+        return;
+
+    // FactorBasePtrList factor_list;
+    // KF->getFactorList(factor_list);
+    if (KF == KF_old and KF_old != nullptr)
+        return;
+
+    int id = 0;
+    publishScene(keypoints, scan, p, q, pub_marker_scene_target_, marker_color_target_, id, cap);
+
+    publishEmpty();
+
+    id = 1;
+
+    for (const FactorBasePtr &factor : KF->getConstrainedByList())
+        if (factor->getProcessor() == processor_falko)
+        {
+
+            auto ftr         = factor->getFeature();
+            auto frame_other = ftr->getFrame();
+
+            // auto frame_other = factor->getFrameOther();
+
+            // STATE OF THE REFERENCE FRAME
+            // VectorComposite state_other = problem_->getState(frame_other->getTimeStamp(), "PO"); not working,
+            // returns 0 state
+            VectorComposite state_other = map_frame_states.find(frame_other)->second;
+
+            // 2D robot position and yaw
+            Eigen::Vector3d    p_other = Eigen::Vector3d::Zero();
+            Eigen::Quaterniond q_other;
+            getPosition(p_other, q_other, state_other);
+
+            // LOOK FOR REFERENCE SCENE
+            laserscanutils::LaserScan    scan_other;
+            std::vector<falkolib::FALKO> keypoints_other;
+            getKeypointsAndScan(frame_other, scan_other, keypoints_other);
+
+            // PUBLISH KEYPOINTS MAP OF REFERENCE SCENES
+
+            std::cout << "keypoints target: " << keypoints.size() << std::endl;
+            std::cout << "keypoints reference: " << keypoints_other.size() << std::endl;
+
+            // std::cout << "scan : " << std::endl;
+            // for (int i = 0; i < scan.ranges_raw_.size(); i++)
+            //     {
+            //         std::cout << scan.ranges_raw_[i] << ",";
+            //     }
+            // std::cout << "scan other : " << std::endl;
+            // for (int i = 0; i < scan_other.ranges_raw_.size(); i++)
+            //     {
+            //         std::cout << scan_other.ranges_raw_[i] << ",";
+            //     }
+
+            publishScene(keypoints_other, scan_other, p_other, q_other, pub_marker_scene_reference_,
+                         marker_color_reference_, id, cap);
+
+            // Publish associations between scenes
+            auto matchings_list =
+                    std::static_pointer_cast<ProcessorLoopClosureFalkoAhtBsc>(processor_falko)->match_list_;
+
+            for (auto match : matchings_list)
+            {
+                if (match->capture_target == cap)
+                {
+                    auto match_falko  = std::static_pointer_cast<MatchLoopClosureFalko>(match);
+                    auto associations = match_falko->match_falko_->associations;
+                    publishAssociations(associations, cap, match->capture_reference);
+                }
+            }
+        }
+
+    id_old_ = id;
+    KF_old  = KF;
+    return;
+}
+
+void PublisherFalko::publishAssociations(std::vector<std::pair<int, int>> &associations_, CaptureBasePtr &cap_target,
+                                         CaptureBasePtr &cap_reference)
+{
+
+    std_msgs::ColorRGBA marker_color;
+    marker_color.r = 0;
+    marker_color.g = 1;
+    marker_color.b = 0;
+    marker_color.a = 1;
+
+    visualization_msgs::Marker asso_marker;
+    asso_marker.header.stamp    = ros::Time::now();
+    asso_marker.header.frame_id = map_frame_id_;
+
+    asso_marker.color   = marker_color;
+    asso_marker.action  = visualization_msgs::Marker::ADD;
+    asso_marker.type    = visualization_msgs::Marker::LINE_LIST;
+    asso_marker.scale.x = 0.2;
+
+    if (map_markers_reference_scenes_.count(cap_target) == 0)
+        return;
+    if (map_markers_reference_scenes_.count(cap_reference) == 0)
+        return;
+
+    int asso_number        = 0;
+    asso_marker.id         = 0;
+    asso_marker.header.seq = 0;
+    for (int i = 0; i < associations_.size(); i++)
+    {
+        if (associations_[i].second == -1)
+            continue;
+
+        geometry_msgs::Point point1;
+        geometry_msgs::Point point2;
+        point1.x = map_markers_reference_scenes_.find(cap_target)->second.points[associations_[i].second].x;
+        point1.y = map_markers_reference_scenes_.find(cap_target)->second.points[associations_[i].second].y;
+
+        point2.x = map_markers_reference_scenes_.find(cap_reference)->second.points[associations_[i].first].x;
+        point2.y = map_markers_reference_scenes_.find(cap_reference)->second.points[associations_[i].first].y;
+
+        asso_marker.points.push_back(point1);
+        asso_marker.points.push_back(point2);
+        asso_number += 1;
+    }
+
+    WOLF_INFO("PublisherFalkoBsc.associations number : ", asso_number)
+    pub_marker_associations_.publish(asso_marker);
+
+    asso_marker.id      = 1;
+    asso_marker.type    = visualization_msgs::Marker::POINTS;
+    asso_marker.scale.x = 0.4;
+    asso_marker.scale.y = 0.4;
+    pub_marker_associations_.publish(asso_marker);
+}
+
+void PublisherFalko::publishScene(std::vector<falkolib::FALKO> &_keypoints, laserscanutils::LaserScan &_scan,
+                                  Eigen::Vector3d &_p_rob, Eigen::Quaterniond &_q_rob, ros::Publisher &_pub_lines,
+                                  std_msgs::ColorRGBA _marker_color, int &_id, CaptureBasePtr cap)
+{
+    auto sensor_laser = std::static_pointer_cast<SensorLaser2d>(sensor_);
+    auto scan_params  = sensor_laser->getScanParams();
+
+    std::vector<double> angle_discretization;
+    std::vector<double> keypoints_laser_position_x;
+    std::vector<double> keypoints_laser_position_y;
+    auto                euler = _q_rob.toRotationMatrix().eulerAngles(0, 1, 2);
+
+    double z_angle = euler.z() * 180 / M_PI;
+
+    for (int i = 0; i < _scan.ranges_raw_.size(); i++)
+    {
+        angle_discretization.push_back(scan_params.angle_min_ + scan_params.angle_step_ * i);
+
+        keypoints_laser_position_x.push_back(_scan.ranges_raw_[i] * cos(angle_discretization[i] + euler.z()));
+        keypoints_laser_position_y.push_back(_scan.ranges_raw_[i] * sin(angle_discretization[i] + euler.z()));
+    }
+
+    geometry_msgs::Point line_array_msg;
+    marker_msg_falko_scene.points.clear();
+
+    marker_msg_falko_scene.header.stamp = ros::Time::now();
+    marker_msg_falko_scene.header.seq   = _id;
+    marker_msg_falko_scene.id           = _id;
+    marker_msg_falko_scene.scale.x      = 0.1;
+    marker_msg_falko_scene.color        = _marker_color;
+    marker_msg_falko_scene.action       = visualization_msgs::Marker::ADD;
+    marker_msg_falko_scene.type         = visualization_msgs::Marker::LINE_STRIP;
+
+    for (int i = 0; i < 50; i++)
+    {
+        if (i < _keypoints.size())
+        {
+            Eigen::Vector3d    p_kp = Eigen::Vector3d::Zero();
+            Eigen::Quaterniond q_kp;
+
+            auto index = _keypoints[i].index;
+
+            p_kp(0) = keypoints_laser_position_x[index] + _p_rob(0);
+            p_kp(1) = keypoints_laser_position_y[index] + _p_rob(1);
+            p_kp(2) = 0;
+
+            // LINES
+            line_array_msg.x = p_kp(0);
+            line_array_msg.y = p_kp(1);
+            marker_msg_falko_scene.points.push_back(line_array_msg);
+        }
+    }
+
+    // Store messages when it is a reference scene
+    if (_id == 0)
+    {
+        map_markers_reference_scenes_.insert(
+                std::pair<CaptureBasePtr, visualization_msgs::Marker>(cap, marker_msg_falko_scene));
+    }
+
+    // Close the area
+    marker_msg_falko_scene.points.push_back(marker_msg_falko_scene.points[0]);
+    // Publish
+    marker_msg_falko_scene.header.seq = 1;
+    _pub_lines.publish(marker_msg_falko_scene);
+    _id += 1;
+    marker_msg_falko_scene.id      = _id;
+    marker_msg_falko_scene.type    = visualization_msgs::Marker::SPHERE_LIST;
+    marker_msg_falko_scene.scale.x = 0.3;
+    marker_msg_falko_scene.scale.y = 0.3;
+    _pub_lines.publish(marker_msg_falko_scene);
+
+    // clean
+    marker_msg_falko_scene.points.clear();
+    _id += 1;
+}
+
+void PublisherFalko::publishEmpty()
+{
+
+    visualization_msgs::Marker marker;
+    marker.header.stamp = ros::Time::now();
+    marker.action       = visualization_msgs::Marker::DELETE;
+    marker.id           = 0;
+    pub_marker_associations_.publish(marker);
+    marker.id = 1;
+    pub_marker_associations_.publish(marker);
+
+    for (int id = 0; id <= id_old_; id++)
+    {
+        marker.header.stamp = ros::Time::now();
+        marker.id           = id;
+        pub_marker_scene_reference_.publish(marker);
+    }
+}
+
+void PublisherFalko::getKeypointsAndScan(FrameBasePtr &_frame, laserscanutils::LaserScan &_scan,
+                                         std::vector<falkolib::FALKO> &_keypoints)
+{
+    auto cap       = _frame->getCaptureOf(sensor_, "CaptureLaser2d");
+    auto cap_laser = std::dynamic_pointer_cast<CaptureLaser2d>(cap);
+
+    _scan = cap_laser->getScan();
+
+    for (auto feat : cap->getFeatureList())
+    {
+        if (feat->getType() == "FeatureSceneFalko")
+        {
+            auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+            _keypoints      = feat_falko->getScene()->keypoints_list_;
+        }
+    }
+}
+
+void PublisherFalko::getPosition(Eigen::Vector3d &p, Eigen::Quaterniond &q, VectorComposite _state)
+{
+
+    p.head(2) = _state['P']; //+ Eigen::Rotation2Dd(_state['O'](0)) * sensor_->getP()->getState().head(2);
+    q         = Eigen::Quaterniond(
+            Eigen::AngleAxisd(_state['O'](0) + sensor_->getO()->getState()(0), Eigen::Vector3d::UnitZ()));
+}
+
+} // namespace wolf

src/subscriber_laser2d.cpp

@@ -47,39 +47,16 @@ void SubscriberLaser2d::callback(const sensor_msgs::LaserScan::ConstPtr& msg)
 {
     setLastStamp(msg->header.stamp);
 
+    if(msg->ranges.empty())
+    {
+        ROS_WARN("SubscriberLaser2d::callback: laser msg empty! skipping...");
+        return;
+    }
+
     CaptureLaser2dPtr new_capture = std::make_shared<CaptureLaser2d>(TimeStamp(msg->header.stamp.sec,
                                                                                msg->header.stamp.nsec),
                                                                      sensor_ptr_,
                                                                      msg->ranges);
-    // auto n = std::min(msg->ranges.size(), new_capture->getScan().ranges_raw_.size());
-    // auto total = 0;
-    // for(int i = 0; i < n; ++i)
-    //     {
-    //         if (msg->ranges[i] != std::numeric_limits<double>::infinity() and new_capture->getScan().ranges_raw_[i] != std::numeric_limits<double>::infinity())
-    //             total += std::abs(msg->ranges[i] - new_capture->getScan().ranges_raw_[i]);
-    //         else if (msg->ranges[i] != std::numeric_limits<double>::infinity() or new_capture->getScan().ranges_raw_[i] != std::numeric_limits<double>::infinity())
-    //             {
-    //                 total += 10e3;
-    //             }
-    //     }
-    // std::cout << "Total error " << total << "\n";
-
-    // auto sensor_laser_ptr = std::dynamic_pointer_cast<SensorLaser2d>(sensor_ptr_);
-    // params_ = sensor_laser_ptr->getScanParams();
-    // WOLF_INFO("SubscriberLaser2d: Loading params from msg:",
-    //                   "\n\tangle_min       = ", msg->angle_min,
-    //                   "\n\tangle_max       = ", msg->angle_max,
-    //                   "\n\tangle_increment = ", msg->angle_increment,
-    //                   "\n\ttime_increment  = ", msg->time_increment,
-    //                   "\n\trange_min       = ", msg->range_min,
-    //                   "\n\trange_max       = ", msg->range_max);
-    // WOLF_INFO("MSG ", msg->angle_increment, " PARAMS ", params_.angle_step_);
-    // WOLF_INFO("MSG ", msg->angle_max, " PARAMS " , params_.angle_max_);
-    // WOLF_INFO("MSG ", msg->angle_min, " PARAMS " , params_.angle_min_);
-    // WOLF_INFO("MSG ", msg->range_max, " PARAMS " , params_.range_max_);
-    // WOLF_INFO("MSG ", msg->range_min, " PARAMS " , params_.range_min_);
-    // WOLF_INFO("CaptureLaser ranges: ", new_capture->getScan().ranges_raw_.size());
-    // std::cout << "Cap Seq " << new_capture->id() << " " << msg->header.seq << "\n";
 
     //Currently this line is just to bypass the ROS "auto config". from the ROS msg. Maybe we want to explore
     //getting the params from the msg instead of the yaml file in the future.
@@ -121,21 +98,10 @@ void SubscriberLaser2d::callback(const sensor_msgs::LaserScan::ConstPtr& msg)
         
         sensor_laser_->setScanParams(params_);
         laser_intrinsics_set_ = true;
-
-        // WOLF_INFO("SubscriberLaser2d: Parameters loaded from msg:",
-        //           "\n\tangle_min_     = ", params_.angle_min_,
-        //           "\n\tangle_max_     = ", params_.angle_max_,
-        //           "\n\tangle_step_    = ", params_.angle_step_,
-        //           "\n\tscan_time_     = ", params_.scan_time_,
-        //           "\n\trange_min_     = ", params_.range_min_,
-        //           "\n\trange_max_     = ", params_.range_max_,
-        //           "\n\trange_std_dev_ = ", params_.range_std_dev_, " (not taken from msg)",
-        //           "\n\tangle_std_dev_ = ", params_.angle_std_dev_, " (not taken from msg)");
     }
     // check coherent params
     else
     {
-
         if (std::abs(params_.angle_min_  - msg->angle_min) > 1e-4 or
             std::abs(params_.angle_max_  - msg->angle_max) > 1e-4 or
             std::abs(params_.angle_step_ - msg->angle_increment) > 1e-4 or
@@ -153,19 +119,6 @@ void SubscriberLaser2d::callback(const sensor_msgs::LaserScan::ConstPtr& msg)
     }
 
     new_capture->process();
-    // if (msg->header.seq == 14031)
-    //     {
-    //         auto problem = sensor_ptr_->getProblem();
-    //         for(const auto& frame : problem->getTrajectory()->getFrameList())
-    //             {
-    //                 if(frame->isKey()){
-    //                     for(const auto& cap : frame->getCaptureList())
-    //                         {
-    //                             if(cap->getType() == "LASER 2d") std::cout << "Frm Cap " << frame->id() << " " << cap->id() << "\n";
-    //                         }
-    //                 }
-    //             }
-    //     }
 }
 
 }