Commit 25e3629a authored by Marius's avatar Marius

asd

parent b91c59ca
This diff is collapsed.
......@@ -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
#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_;
};
......@@ -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.
......
......@@ -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
......@@ -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();
......
#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();
}
#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
cout << "Computing features using radius: " << radius << endl;
#endif