asd

CMakeLists.txt

@@ -250,3 +250,24 @@ add_executable(crs_point_processing_node_student ${point_processing_node_student
 set_target_properties(crs_point_processing_node_student PROPERTIES OUTPUT_NAME point_processing_node_student PREFIX "")
 target_link_libraries(crs_point_processing_node_student crs_comon ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${catkin_LIBRARIES})
 
+####################################### registration_icp #######################################
+set(registration_icp_INCLUDE_DIR ${CMAKE_CURRENT_LIST_DIR}/include/registration_icp/)
+file(GLOB registration_icp_HEADERS ${registration_icp_INCLUDE_DIR}/*.h*)
+
+set(registration_icp_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/src/registration_icp/)
+file(GLOB registration_icp_SOURCES ${registration_icp_SOURCE_DIR}/*.c*)
+
+add_executable(crs_registration_icp ${registration_icp_SOURCES} ${registration_icp_HEADERS})
+set_target_properties(crs_registration_icp PROPERTIES OUTPUT_NAME registration_icp PREFIX "")
+target_link_libraries(crs_registration_icp crs_comon ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${catkin_LIBRARIES})
+
+####################################### registration_feature #######################################
+set(registration_feature_INCLUDE_DIR ${CMAKE_CURRENT_LIST_DIR}/include/registration_feature/)
+file(GLOB registration_feature_HEADERS ${registration_feature_INCLUDE_DIR}/*.h*)
+
+set(registration_feature_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/src/registration_feature/)
+file(GLOB registration_feature_SOURCES ${registration_feature_SOURCE_DIR}/*.c*)
+
+add_executable(crs_registration_feature ${registration_feature_SOURCES} ${registration_feature_HEADERS})
+set_target_properties(crs_registration_feature PROPERTIES OUTPUT_NAME registration_feature PREFIX "")
+target_link_libraries(crs_registration_feature crs_comon ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${catkin_LIBRARIES})
\ No newline at end of file

include/registration_feature/helper.hpp

+#pragma once
+
+#include <string>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <boost/assert.hpp>
+#include <ros/ros.h>
+
+using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
+
+/**
+ * - Has to be a
+ */
+class BagFileHelper {
+public:
+    BagFileHelper(const BagFileHelper&) = delete;
+    BagFileHelper();
+    ~BagFileHelper();
+
+    void openBagFile(const std::string& filePath);
+
+//    void reset();
+
+
+// initialize all topics
+    bool initTopic(std::string topic);
+//    bool getTopicSize(const std::string topic) const;
+    bool hasTopicAvailableMessages(const std::string& topic) const;
+    bool isTopicInitialized(const std::string& topic) const;
+
+
+    CloudRGBA::ConstPtr getNextConstCloudRGBAForTopic(const std::string& topic);
+    CloudRGBA::Ptr getNextCloudRGBAForTopic(const std::string& topic);
+
+//    template<class T>
+//    std::shared_ptr<T> getNextForTopic(const std::string& topic);
+
+
+private:
+
+    /// A PIMPL class is required due to some PCL and ROS incompatibilities.
+    class Data;
+    std::unique_ptr<Data> data_;
+};
+
+
+class ROSParamHelper {
+public:
+
+    void setROSNodePtr(ros::NodeHandlePtr ptr);
+
+
+    std::string dumpString();
+
+
+    bool updateAll();
+
+    bool updateICPMaxCorrespondenceDistance();
+    void setICPMaxCorrespondenceDistance(double value);
+    double getICPMaxCorrespondenceDistance() const;
+
+    bool updateICPMaxIterations();
+    void setICPMaxIterations(int value);
+    int getICPMaxIterations() const;
+
+    bool updateICPTransformationEpsilon();
+    void setICPTransformationEpsilon(double value);
+    double getICPTransformationEpsilon() const;
+
+    bool updateICPEuclidianFitnessEpsilon();
+    void setICPEuclidianFitnessEpsilon(double value);
+    double getICPEuclidianFitnessEpsilon() const;
+
+    bool updateDownsampleLeafSize();
+    void setDownsampleLeafSize(double value);
+    double getDownsampleLeafSize() const;
+
+private:
+    std::tuple<std::string, std::string> mainCloudTopic_ = {"/kinect2/sd/points", "MainCloudTopic"};
+    std::tuple<std::string, std::string> mainCloudBag_ = {"/home/marius/bockdata/bags/crs.bag", "MainBagFile"};
+
+    std::tuple<double, std::string> icpMaxCorrespondenceDistance_ = {0.03, "ICPMaxCorrespondenceDistance"};
+    std::tuple<int, std::string> icpMaxIterations_ = {100, "ICPMaxIterations"};
+    std::tuple<double, std::string> icpTransformationEpsilon_ = {0.03, "ICPTransformationEpsilon"};
+    std::tuple<double, std::string> icpEuclidianFitnessEpsilon_ = {0.03, "ICPEuclidianFitnessEpsilon"};
+    std::tuple<double, std::string> downsampleLeafSize = {0.03, "DownsampleLeafSize"};
+
+    ros::NodeHandlePtr node_;
+
+
+};
+
+
+

src/er1_driver_node/main.cpp

@@ -72,7 +72,7 @@ int main(int argc, char** argv) {
     ros::Subscriber sub = node.subscribe("er1_motor_commands", 1000, &callback);
 
     // Connect to robot. You have to secify which robot you are using!
-	EvolutionRobotID id = EvolutionRobotID::ER1Small;
+	EvolutionRobotID id = EvolutionRobotID::ER1Medium;
     robot.connect(id);
 
     // Enter ROS main loop.

src/point_processing_node/main.cpp

@@ -3,47 +3,59 @@
 #include <geometry_msgs/Pose.h>
 #include <chrono>
 #include <comon/clockhelper.hpp>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+
+// Less typing
+using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
+
+// We are going to publish clouds ... Therefore we need a publisher!
+ros::Publisher publisher;
+
+// We are going to receive clouds ... Therefore we need a subscriber!
+ros::Subscriber subscriber;
 
-int main(int argc, char** argv){
-    // Init ROS stuff
-    ros::init(argc, argv, "crs_tf_publisher");
 
-    // Create Transform Broadcaster
-    tf::TransformBroadcaster br;
+// The callback function. It gets called whenever we receive a new point loucd.
+void callback(CloudRGBA::ConstPtr msgPtr) {
+    // Print some information
+    ROS_INFO("Received a cloud with %i points", msgPtr->size());
 
-    // Init the clock helper
-    ClockHelper helper;
-    helper.init();
-    helper.setCycleTime(std::chrono::seconds(3));
 
-    // Specify an update rate
-    ros::Rate rate(3);
+    // Init voxelgrid filter and apply
+    pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
+    sor.setInputCloud (msgPtr);
+    sor.setLeafSize (0.01f, 0.01f, 0.01f);
+    auto filteredPtr = boost::make_shared<CloudRGBA>();
+    sor.filter(*filteredPtr);
 
-    // Main loop
-    while(ros::ok()) {
-        // Get the roll value for the current time point
-        auto roll = helper.getRoll();
 
-        // Specify the origin for our transformation
-        auto origin = tf::Vector3(0, 0, 0);
+    // Set the header of the message. Just some shabby details ...
+    filteredPtr->header.frame_id = "filtered_frame";
+    pcl_conversions::toPCL(ros::Time::now(), filteredPtr->header.stamp);
 
-        // Specify the rotation for our transformation
-        tf::Quaternion q;
-        q.setRPY(0, 0, roll);
+    // Publish cloud
+    publisher.publish(filteredPtr);
+
+    // Print some information
+    ROS_INFO("Published a cloud with %i points", filteredPtr->size());
+}
+
+int main(int argc, char** argv){
+    // Init ROS stuff
+    ros::init(argc, argv, "crs_tf_publisher");
+    auto node = ros::NodeHandle();
 
-        // Bundle all information into a transformation object
-        tf::Transform transform;
-        transform.setOrigin(origin);
-        transform.setRotation(q);
+    // Init subscriber and set topic
+    subscriber = node.subscribe<CloudRGBA>("/kinect2/sd/points", 1, callback);
 
-        // Publish the transformation
-        std::string destinationFrame = "map";
-        std::string sourceFrame = "kinect2_ir_optical_frame";
-        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), destinationFrame, sourceFrame));
+    // Init publisher and set topic
+    publisher = node.advertise<CloudRGBA>("/crs/points", 5);
 
-        // Sleep a little bit
-        rate.sleep();
-    }
+    // Run!
+    ros::spin();
 
     return 0;
 };
\ No newline at end of file

src/point_processing_node_student/main.cpp

@@ -24,20 +24,20 @@ void callback(CloudRGBA::ConstPtr msgPtr) {
     ROS_INFO("Received a cloud with %i points", msgPtr->size());
 
 
+    auto filteredPtr = boost::make_shared<CloudRGBA>();
+
     // Init voxelgrid filter and apply
     pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
-    sor.setInputCloud (msgPtr);
-    sor.setLeafSize (0.01f, 0.01f, 0.01f);
-    auto filteredPtr = boost::make_shared<CloudRGBA>();
-    sor.filter(*filteredPtr);
+    /* ######### <MISSING> ########## */
+    /* ######### <MISSING> ########## */
+    /* ######### <MISSING> ########## */
 
 
     // Set the header of the message. Just some shabby details ...
     filteredPtr->header.frame_id = "filtered_frame";
     pcl_conversions::toPCL(ros::Time::now(), filteredPtr->header.stamp);
 
-    // Publish cloud
-    publisher.publish(filteredPtr);
+
 
     // Print some information
     ROS_INFO("Published a cloud with %i points", filteredPtr->size());
@@ -45,14 +45,11 @@ void callback(CloudRGBA::ConstPtr msgPtr) {
 
 int main(int argc, char** argv){
     // Init ROS stuff
-    ros::init(argc, argv, "crs_tf_publisher");
+    ros::init(argc, argv, "crs_cloud_processor");
     auto node = ros::NodeHandle();
 
-    // Init subscriber and set topic
-    subscriber = node.subscribe<CloudRGBA>("/kinect2/sd/points", 1, callback);
-
-    // Init publisher and set topic
-    publisher = node.advertise<CloudRGBA>("/crs/points", 5);
+    /* ######### <MISSING> ########## */
+    /* ######### <MISSING> ########## */
 
     // Run!
     ros::spin();

src/registration_feature/helper.cpp

+#include <registration_feature/helper.hpp>
+#include <rosbag/bag.h>
+#include <rosbag/view.h>
+#include <unordered_map>
+#include <pcl_ros/point_cloud.h>
+
+class BagFileHelper::Data {
+public:
+    Data() = default;
+    ~Data() = default;
+
+    rosbag::Bag bag_;
+
+
+
+
+
+    std::unordered_map<std::string, std::unique_ptr<rosbag::View>> views_;
+    std::unordered_map<std::string, std::pair<rosbag::View::iterator, rosbag::View::iterator>> iteratorPairs_;
+};
+
+
+
+void BagFileHelper::openBagFile(const std::string &filePath) {
+
+    // TODO: can throw exception
+    data_->bag_.open(filePath);
+
+
+
+}
+
+//void BagFileHelper::reset() {
+//
+//}
+
+CloudRGBA::ConstPtr BagFileHelper::getNextConstCloudRGBAForTopic(const std::string &topic) {
+
+    if (data_->views_.count(topic) == 0) {
+        return nullptr;
+    } else {
+        auto& [cur, end] = data_->iteratorPairs_.at(topic);
+        if (cur == end) {
+            return nullptr;
+        } else {
+            auto ptr = cur->instantiate<CloudRGBA>();
+            ++cur;
+            return ptr;
+        }
+    }
+}
+
+CloudRGBA::Ptr BagFileHelper::getNextCloudRGBAForTopic(const std::string &topic) {
+
+    if (data_->views_.count(topic) == 0) {
+        return nullptr;
+    } else {
+        auto& [cur, end] = data_->iteratorPairs_.at(topic);
+        if (cur == end) {
+            return nullptr;
+        } else {
+            auto ptr = cur->instantiate<CloudRGBA>();
+            ++cur;
+            return ptr;
+        }
+    }
+}
+
+bool BagFileHelper::initTopic(std::string topic) {
+    try {
+        auto cloudBagView_ = std::make_unique<rosbag::View>(data_->bag_, rosbag::TopicQuery(topic));
+        if (cloudBagView_->size() == 0) {
+
+        }
+
+
+        auto begin = cloudBagView_->begin();
+        auto end = cloudBagView_->end();
+        data_->iteratorPairs_.emplace(topic, std::make_pair(begin, end));
+        data_->views_.emplace(topic, std::move(cloudBagView_));
+    } catch (rosbag::BagException&) {
+        return false;
+    }
+    return true;
+}
+
+bool BagFileHelper::isTopicInitialized(const std::string &topic) const {
+    return data_->views_.count(topic) > 0;
+}
+
+bool BagFileHelper::hasTopicAvailableMessages(const std::string &topic) const {
+    BOOST_ASSERT(isTopicInitialized(topic));
+    auto& [cur, end] = data_->iteratorPairs_.at(topic);
+    return std::distance(cur, end) > 0;
+}
+
+BagFileHelper::BagFileHelper() {
+    data_ = std::make_unique<BagFileHelper::Data>();
+}
+
+BagFileHelper::~BagFileHelper() {
+
+}
+
+
+
+
+
+
+//+++++++++++++++++++++++++++++++
+
+
+
+//
+//if (privateNode_.hasParam("iteration_mode")) {
+//std::string iterationMode;
+//privateNode_.getParam("iteration_mode", iterationMode);
+//auto modeOptional = IterationMode::_from_string_nocase_nothrow(iterationMode.c_str());
+//if (!modeOptional) {
+//std::stringstream validValues;
+//auto values = IterationMode::_values();
+//for (auto value: values) {
+//validValues << value._to_string() << " ";
+//}
+//return stringify("Invalid value for <iteration_mode>. Valid values are: ", validValues.str());
+//}
+//appCfg.setIterationMode(*modeOptional);
+//}
+
+//####################### ICPMaxCorrespondenceDistance #######################
+void ROSParamHelper::setICPMaxCorrespondenceDistance(double newValue) {
+    auto& [value, key]  = icpMaxCorrespondenceDistance_;
+    node_->setParam(key, newValue);
+    value = newValue;
+}
+
+double ROSParamHelper::getICPMaxCorrespondenceDistance() const {
+    return std::get<0>(icpMaxCorrespondenceDistance_);
+}
+
+bool ROSParamHelper::updateICPMaxCorrespondenceDistance() {
+    const auto& [defaultValue, key] = icpMaxCorrespondenceDistance_;
+    if (node_->hasParam(key)) {
+        double value;
+        if (node_->getParam(key, value)) {
+            setICPMaxCorrespondenceDistance(value);
+            return true;
+        }
+        return false;
+    }
+    return true;
+}
+
+//####################### ICPMaxIterations #######################
+void ROSParamHelper::setICPMaxIterations(int newValue) {
+    auto& [value, key]  = icpMaxIterations_;
+    node_->setParam(key, newValue);
+    value = newValue;
+}
+
+int ROSParamHelper::getICPMaxIterations() const {
+    return std::get<0>(icpMaxIterations_);
+}
+
+
+bool ROSParamHelper::updateICPMaxIterations() {
+    const auto& [defaultValue, key] = icpMaxIterations_;
+    if (node_->hasParam(key)) {
+        int value;
+        if (node_->getParam(key, value)) {
+            setICPMaxIterations(value);
+            return true;
+        }
+        return false;
+    }
+    return true;
+}
+
+//####################### ICPTransformationEpsilon #######################
+double ROSParamHelper::getICPTransformationEpsilon() const {
+    return std::get<0>(icpTransformationEpsilon_);
+}
+
+void ROSParamHelper::setICPTransformationEpsilon(double newValue) {
+    auto& [value, key]  = icpTransformationEpsilon_;
+    node_->setParam(key, newValue);
+    value = newValue;
+}
+
+bool ROSParamHelper::updateICPTransformationEpsilon() {
+    const auto& [defaultValue, key] = icpTransformationEpsilon_;
+    if (node_->hasParam(key)) {
+        double value;
+        if (node_->getParam(key, value)) {
+            setICPTransformationEpsilon(value);
+            return true;
+        }
+        return false;
+    }
+    return true;
+}
+
+
+//####################### ICPEuclidianFitnessEpsilon #######################
+void ROSParamHelper::setICPEuclidianFitnessEpsilon(double newValue) {
+    auto& [value, key]  = icpEuclidianFitnessEpsilon_;
+    node_->setParam(key, newValue);
+    value = newValue;
+}
+
+double ROSParamHelper::getICPEuclidianFitnessEpsilon() const {
+    return std::get<0>(icpEuclidianFitnessEpsilon_);
+}
+
+bool ROSParamHelper::updateICPEuclidianFitnessEpsilon() {
+    const auto& [defaultValue, key] = icpEuclidianFitnessEpsilon_;
+    if (node_->hasParam(key)) {
+        double value;
+        if (node_->getParam(key, value)) {
+            setICPEuclidianFitnessEpsilon(value);
+            return true;
+        }
+        return false;
+    }
+    return true;
+}
+
+void ROSParamHelper::setROSNodePtr(ros::NodeHandlePtr ptr) {
+    node_ = ptr;
+}
+
+bool ROSParamHelper::updateAll() {
+    return updateICPEuclidianFitnessEpsilon() && updateICPMaxCorrespondenceDistance() && updateICPMaxIterations() && updateICPTransformationEpsilon() && updateDownsampleLeafSize();
+}
+
+
+//####################### DownsampleLeafSize #######################
+
+bool ROSParamHelper::updateDownsampleLeafSize() {
+    const auto& [defaultValue, key] = downsampleLeafSize;
+    if (node_->hasParam(key)) {
+        double value;
+        if (node_->getParam(key, value)) {
+            setDownsampleLeafSize(value);
+            return true;
+        }
+        return false;
+    }
+    return true;
+}
+
+void ROSParamHelper::setDownsampleLeafSize(double newValue) {
+    auto& [value, key]  = downsampleLeafSize;
+    node_->setParam(key, newValue);
+    value = newValue;
+}
+
+double ROSParamHelper::getDownsampleLeafSize() const {
+    return std::get<0>(downsampleLeafSize);
+}
+
+#include <sstream>
+std::string ROSParamHelper::dumpString() {
+    std::stringstream ss;
+    ss << "DownsampleLeadSize: " << getDownsampleLeafSize() << "\n"
+    << "ICPMaxCorrespondenceDistance" << getICPMaxCorrespondenceDistance() << "\n"
+    << "ICPEuclidianFitnessEpsilon" << getICPEuclidianFitnessEpsilon() << "\n"
+    << "ICPTransformationEpsilon" << getICPTransformationEpsilon() << "\n"
+    << "ICPMaxIterations" <<  getICPMaxIterations() << "\n";
+    return ss.str();
+}

src/registration_feature/main.cpp

+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <geometry_msgs/Pose.h>
+#include <chrono>
+#include <comon/clockhelper.hpp>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+
+#include <pcl/point_types.h>
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/registration/icp.h>
+#include <pcl/registration/ia_ransac.h>
+#include <pcl/features/pfh.h>
+#include <pcl/features/fpfh.h>
+#include <pcl/features/normal_3d.h>
+#include <Eigen/Geometry>
+
+#include <pcl/common/transforms.h>
+#include <registration_feature/helper.hpp>
+
+
+//FPFH feature parameters
+
+//FPFH allignment parameters
+
+ROSParamHelper paramHelper;
+
+
+//ICP allingment parameters
+const int ICP_MAX_ITERATIONS = 50;
+const double ICP_MAX_CORRESPONDENCE_DISTANCE = 0.25;
+const double ICP_TRANSFORMATION_EPSILON = 1e-6;
+const double ICP_EUCLIDEAN_FITNESS_EPSILON = 1e-5;
+
+enum Method {
+    NONE,
+    FPFH
+};
+
+using namespace std;
+using namespace pcl;
+
+// Less typing
+using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
+
+// We are going to publish clouds ... Therefore we need a publisher!
+ros::Publisher publisher;
+
+// We are going to receive clouds ... Therefore we need a subscriber!
+ros::Subscriber subscriber;
+
+PointCloud<FPFHSignature33>::Ptr getFeaturesFPFH(CloudRGBA::Ptr cloud, PointCloud<Normal>::Ptr normals, double radius) {
+    PointCloud<FPFHSignature33>::Ptr features = PointCloud<FPFHSignature33>::Ptr(new PointCloud<FPFHSignature33>);
+    search::KdTree<pcl::PointXYZRGBA>::Ptr search_method_ptr = search::KdTree<PointXYZRGBA>::Ptr(
+            new search::KdTree<PointXYZRGBA>);
+    FPFHEstimation<PointXYZRGBA, Normal, FPFHSignature33> fpfh_est;
+    fpfh_est.setInputCloud(cloud);
+    fpfh_est.setInputNormals(normals);
+    fpfh_est.setSearchMethod(search_method_ptr);
+    fpfh_est.setRadiusSearch(radius);
+    fpfh_est.compute(*features);
+
+#ifdef DEBUG