Skip to content

GitLab

Commit 5546665b authored by Marius's avatar Marius
Browse files
Options

asd

parent 0e52f806
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 344 additions and 625 deletions
.idea/workspace.xml
View file @ 5546665b
This diff is collapsed.
CMakeLists.txt
View file @ 5546665b
......@@ -261,13 +261,3 @@ add_executable(crs_registration_icp ${registration_icp_SOURCES} ${registration_i
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 → include/registration_icp/helper.hpp
View file @ 5546665b
......@@ -46,6 +46,7 @@ private:
class ROSParamHelper {
public:
ROSParamHelper();
void setROSNodePtr(ros::NodeHandlePtr ptr);
......@@ -86,9 +87,5 @@ private:
std::tuple<double, std::string> downsampleLeafSize = {0.03, "DownsampleLeafSize"};
ros::NodeHandlePtr node_;
};
src/registration_feature/main.cpp deleted 100644 → 0
View file @ 0e52f806
#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
return features;
}
PointCloud<Normal>::Ptr
getNormals(CloudRGBA::Ptr incloud, CloudRGBA::Ptr fullcloud, double normalsRadius) { //ADAPTED FROM RUSU TUTORIAL
// Create the normal estimation class, and pass the input dataset to it
NormalEstimation<PointXYZRGBA, Normal> ne;
ne.setInputCloud(incloud);
// Pass the original data (before downsampling) as the search surface
ne.setSearchSurface(fullcloud);
// Create an empty kdtree representation, and pass it to the normal estimation object.
// Its content will be filled inside the object, based on the given surface dataset.
pcl::search::KdTree<PointXYZRGBA>::Ptr tree(new pcl::search::KdTree<PointXYZRGBA>());
ne.setSearchMethod(tree);
// Output datasets
PointCloud<Normal>::Ptr cloud_normals(new PointCloud<Normal>);
// Use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch(normalsRadius);
// Compute the features
ne.compute(*cloud_normals);
return cloud_normals;
}
CloudRGBA::Ptr downsample(CloudRGBA::Ptr frame, float downsample_size) {
CloudRGBA::Ptr sampled_frame(new CloudRGBA);
if (downsample_size == 0) {
*sampled_frame = *frame;
return sampled_frame;
}
// Downsample using voxelgrid
VoxelGrid<PointXYZRGBA> downsample_filter;
downsample_filter.setLeafSize(downsample_size, downsample_size, downsample_size);
downsample_filter.setInputCloud(frame);
downsample_filter.filter(*sampled_frame);
return sampled_frame;
}
struct Params {
double sacMaxCorrespondenceDist = 0.3;
double sacMinSampleDist = 0.2;
int sacMaxIterations = 1000;
float downsampleSize = 0.3;
float fpfhRadius = 0.3;
double normalsRadius = 0.2;
};
Eigen::Matrix4f registerFrame(CloudRGBA::Ptr frame,
CloudRGBA::Ptr model,
Params params) {
Eigen::Matrix4f transformation = Eigen::Matrix4f::Identity();
//Downsample the input clouds
CloudRGBA::Ptr sampled_model = downsample(model, params.downsampleSize);
CloudRGBA::Ptr sampled_frame = downsample(frame, params.downsampleSize);
//Compute normals
PointCloud<Normal>::Ptr model_normals = getNormals(sampled_model, model, params.normalsRadius);
PointCloud<Normal>::Ptr frame_normals = getNormals(sampled_frame, frame, params.normalsRadius);
//Compute FPFH features
PointCloud<FPFHSignature33>::Ptr model_features = getFeaturesFPFH(sampled_model, model_normals, params.fpfhRadius);
PointCloud<FPFHSignature33>::Ptr frame_features = getFeaturesFPFH(sampled_frame, frame_normals, params.fpfhRadius);
//Initialize allignment method
SampleConsensusInitialAlignment<PointXYZRGBA, PointXYZRGBA, FPFHSignature33> sac_ia;
sac_ia.setInputCloud(sampled_frame);
sac_ia.setSourceFeatures(frame_features);
sac_ia.setInputTarget(sampled_model);
sac_ia.setTargetFeatures(model_features);
//Set parameters for allignment and RANSAC
sac_ia.setMaxCorrespondenceDistance(params.sacMaxCorrespondenceDist);
sac_ia.setMinSampleDistance(params.sacMinSampleDist);
sac_ia.setMaximumIterations(params.sacMaxIterations);
//Allign frame using FPFH features
sac_ia.align(*sampled_frame);
//std::cout << "Done! MSE: " << sac_ia.getFitnessScore() << endl;
//Get the transformation
transformation = sac_ia.getFinalTransformation();
// if (true) {
// IterativeClosestPoint<PointXYZRGBA, PointXYZRGBA> icp;
// icp.setInputCloud(sampled_frame);
// icp.setInputTarget(sampled_model);
//
// //Set the ICP parameters
// icp.setMaxCorrespondenceDistance(icp_max_dist);
// icp.setMaximumIterations(ICP_MAX_ITERATIONS);
// icp.setTransformationEpsilon(ICP_TRANSFORMATION_EPSILON);
// icp.setEuclideanFitnessEpsilon(ICP_EUCLIDEAN_FITNESS_EPSILON);
//
// //Refine the allignment using ICP
// icp.align(*sampled_frame);
//
// transformation = icp.getFinalTransformation() * transformation;
// }
//pcl::transformPointCloud(*frame, *frame, transformation);
return transformation;
}
class Algorithm {
public:
void setup() {
}
CloudRGBA::ConstPtr gatherCloud() {
auto cloudPtr = ros::topic::waitForMessage<CloudRGBA>(cloudTopic_, ros::Duration(5.0));
return cloudPtr;
}
private:
std::string cloudTopic_ = "/kinect2/sd/points";
std::string bagFile = "/kinect2/sd/points";
};
// 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());
}
int main(int argc, char **argv) {
// Init ROS stuff
ros::init(argc, argv, "crs_tf_publisher");
auto node = ros::NodeHandle();
BagFileHelper helper;
paramHelper.setROSNodePtr(boost::make_shared<ros::NodeHandle>("ICSParams"));
helper.openBagFile("/home/marius/bockdata/bags/room2.bag");
auto cloudTopic = "/kinect2/sd/points";
helper.initTopic(cloudTopic);
// Init publisher and set topic
publisher = node.advertise<CloudRGBA>("/crs/points", 5);
auto completeCloud = boost::make_shared<CloudRGBA>();
auto transformedCloud = boost::make_shared<CloudRGBA>();
auto transformedCloud2 = boost::make_shared<CloudRGBA>();
CloudRGBA::Ptr newCloud, lastCloud;
ros::Rate rate(0.5);
auto completeTransform = Eigen::Matrix4f();
completeTransform.setIdentity();
paramHelper.updateAll();
double downsampleSize = paramHelper.getDownsampleLeafSize();
while (helper.hasTopicAvailableMessages(cloudTopic) && ros::ok()) {
std::string input;
// std::cin >> input;
paramHelper.updateAll();
newCloud = helper.getNextCloudRGBAForTopic(cloudTopic);
ROS_INFO("Running with: \n %s", paramHelper.dumpString().c_str());
if (lastCloud != nullptr) {
Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
auto model = lastCloud;
auto frame = newCloud;
downsampleSize = paramHelper.getDownsampleLeafSize();
double normalRadius = 0.3;
//Downsample the input clouds
CloudRGBA::Ptr sampled_model = downsample(model, downsampleSize);
CloudRGBA::Ptr sampled_frame = downsample(frame, downsampleSize);
//Compute normals
// PointCloud<Normal>::Ptr model_normals = getNormals(sampled_model, model, normalRadius);
// PointCloud<Normal>::Ptr frame_normals = getNormals(sampled_frame, frame, normalRadius);
IterativeClosestPoint<PointXYZRGBA, PointXYZRGBA> icp;
icp.setInputCloud(sampled_frame);
icp.setInputTarget(sampled_model);
//Set the ICP parameters
icp.setMaxCorrespondenceDistance(paramHelper.getICPMaxCorrespondenceDistance());
icp.setMaximumIterations(paramHelper.getICPMaxIterations());
icp.setTransformationEpsilon(paramHelper.getICPTransformationEpsilon());
icp.setEuclideanFitnessEpsilon(paramHelper.getICPEuclidianFitnessEpsilon());
//Refine the allignment using ICP
icp.align(*sampled_frame);
auto conv = icp.getConvergeCriteria();
auto criteria = conv->getConvergenceState();
switch (criteria) {
case pcl::registration::DefaultConvergenceCriteria<float>::CONVERGENCE_CRITERIA_TRANSFORM:
ROS_INFO("CONVERGENCE_CRITERIA_TRANSFORM"); break;
case pcl::registration::DefaultConvergenceCriteria<float>::CONVERGENCE_CRITERIA_ABS_MSE:
ROS_INFO("CONVERGENCE_CRITERIA_ABS_MSE"); break;
case pcl::registration::DefaultConvergenceCriteria<float>::CONVERGENCE_CRITERIA_ITERATIONS:
ROS_INFO("CONVERGENCE_CRITERIA_ITERATIONS"); break;
case pcl::registration::DefaultConvergenceCriteria<float>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES:
ROS_INFO("CONVERGENCE_CRITERIA_NO_CORRESPONDENCES"); break;
case pcl::registration::DefaultConvergenceCriteria<float>::CONVERGENCE_CRITERIA_NOT_CONVERGED:
ROS_INFO("CONVERGENCE_CRITERIA_NOT_CONVERGED"); break;
case pcl::registration::DefaultConvergenceCriteria<float>::CONVERGENCE_CRITERIA_REL_MSE:
ROS_INFO("CONVERGENCE_CRITERIA_REL_MSE"); break;
default:
ROS_INFO("CONVERGENCE unknown ..."); break;
}
transform = icp.getFinalTransformation();
completeTransform = transform * completeTransform;
pcl::transformPointCloud(*newCloud, *transformedCloud, completeTransform);
//pcl::transformPointCloud(*newCloud, *transformedCloud, transform.inverse());
*completeCloud += *transformedCloud;
pcl_conversions::toPCL(ros::Time::now(), completeCloud->header.stamp);
completeCloud->header.frame_id = "kinect2_ir_optical_frame";
publisher.publish(completeCloud);
lastCloud = newCloud;
} else {
lastCloud = downsample(newCloud, downsampleSize);
}
}
return 0;
}
// Params params;
// params.sacMaxCorrespondenceDist = 0.3;
// params.sacMinSampleDist = 0.5;
// params.sacMaxIterations = 1000;
// params.downsampleSize = 0.1;
// params.fpfhRadius = 0.3;
// params.normalsRadius = 0.2;
// //Compute FPFH features
// PointCloud<FPFHSignature33>::Ptr model_features = getFeaturesFPFH(sampled_model, model_normals, params.fpfhRadius);
// PointCloud<FPFHSignature33>::Ptr frame_features = getFeaturesFPFH(sampled_frame, frame_normals, params.fpfhRadius);
//
// //Initialize allignment method
// SampleConsensusInitialAlignment<PointXYZRGBA, PointXYZRGBA, FPFHSignature33> sac_ia;
// sac_ia.setInputCloud(sampled_frame);
// sac_ia.setSourceFeatures(frame_features);
// sac_ia.setInputTarget(sampled_model);
// sac_ia.setTargetFeatures(model_features);
//
// //Set parameters for allignment and RANSAC
// sac_ia.setMaxCorrespondenceDistance(params.sacMaxCorrespondenceDist);
// sac_ia.setMinSampleDistance(params.sacMinSampleDist);
// sac_ia.setMaximumIterations(params.sacMaxIterations);
//
// //Allign frame using FPFH features
// sac_ia.align(*sampled_frame);
// //std::cout << "Done! MSE: " << sac_ia.getFitnessScore() << endl;
//
// //Get the transformation
// transform = sac_ia.getFinalTransformation();
// for (int i = 0; i < transformedCloud->size(); ++i) {
// (*transformedCloud)[i].r = 255;
// (*transformedCloud)[i].g = 0;
// (*transformedCloud)[i].b = 0;
// }
//
// for (int i = 0; i < lastCloud->size(); ++i) {
// (*lastCloud)[i].r = 0;
// (*lastCloud)[i].g = 255;
// (*lastCloud)[i].b = 0;
// }
// // *completeCloud = *lastCloud + *transformedCloud;
//
// for (int i = 0; i < transformedCloud2->size(); ++i) {
// (*transformedCloud2)[i].r = 0;
// (*transformedCloud2)[i].g = 0;
// (*transformedCloud2)[i].b = 255;
// }
//
// for (int i = 0; i < newCloud->size(); ++i) {
// (*newCloud)[i].r = 0;
// (*newCloud)[i].g = 123;
// (*newCloud)[i].b = 123;
// }
//
// *completeCloud = *lastCloud;
// *completeCloud += *newCloud;
// *completeCloud += *transformedCloud;
// *completeCloud += *transformedCloud2;
\ No newline at end of file
src/registration_feature/helper.cpp → src/registration_icp/helper.cpp
View file @ 5546665b
#include <registration_feature/helper.hpp>
#include <registration_icp/helper.hpp>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <unordered_map>
......@@ -95,7 +95,7 @@ bool BagFileHelper::hasTopicAvailableMessages(const std::string &topic) const {
}
BagFileHelper::BagFileHelper() {
data_ = std::make_unique<BagFileHelper::Data>();
data_ = std::make_unique<BagFileHelper::Data>();
}
BagFileHelper::~BagFileHelper() {
......@@ -269,3 +269,7 @@ std::string ROSParamHelper::dumpString() {
<< "ICPMaxIterations" << getICPMaxIterations() << "\n";
return ss.str();
}
ROSParamHelper::ROSParamHelper() {
setROSNodePtr(boost::make_shared<ros::NodeHandle>("ICSParams"));
}
src/registration_icp/main.cpp
View file @ 5546665b
......@@ -7,38 +7,173 @@
#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_icp/helper.hpp>
// Less typing
using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
using IterativeClosestPoint = pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA>;
using Criteria = pcl::registration::DefaultConvergenceCriteria<float>;
// We are going to publish clouds ... Therefore we need a publisher!
ros::Publisher publisher;
// Convert a convergence criteria to a string. You can ignore it!
const char* getCriteria(IterativeClosestPoint& icp) {
auto criteria = icp.getConvergeCriteria()->getConvergenceState();
switch (criteria) {
case Criteria::CONVERGENCE_CRITERIA_TRANSFORM:
return "CONVERGENCE_CRITERIA_TRANSFORM";
case Criteria::CONVERGENCE_CRITERIA_ABS_MSE:
return "CONVERGENCE_CRITERIA_ABS_MSE";
case Criteria::CONVERGENCE_CRITERIA_ITERATIONS:
return "CONVERGENCE_CRITERIA_ITERATIONS";
case Criteria::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES:
return "CONVERGENCE_CRITERIA_NO_CORRESPONDENCES";
case Criteria::CONVERGENCE_CRITERIA_NOT_CONVERGED:
return "CONVERGENCE_CRITERIA_NOT_CONVERGED";
case Criteria::CONVERGENCE_CRITERIA_REL_MSE:
return "CONVERGENCE_CRITERIA_REL_MSE";
default:
return "CONVERGENCE_CRITERIA unknown ...";
}
}
// We are going to receive clouds ... Therefore we need a subscriber!
ros::Subscriber subscriber;
// We are going to publish clouds ... Therefore we need a publisher!
ros::Publisher completeCloudPublisher;
// 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());
CloudRGBA::Ptr downsample(CloudRGBA::Ptr frame, float leafSize) {
CloudRGBA::Ptr downsampledCloud(new CloudRGBA);
if (leafSize == 0) {
*downsampledCloud = *frame;
return downsampledCloud;
}
// Downsample using voxelgrid
pcl::VoxelGrid<pcl::PointXYZRGBA> downsample_filter;
downsample_filter.setLeafSize(leafSize, leafSize, leafSize);
downsample_filter.setInputCloud(frame);
downsample_filter.filter(*downsampledCloud);
return downsampledCloud;
}
int main(int argc, char** argv){
// Init ROS stuff
ros::init(argc, argv, "crs_tf_publisher");
int main(int argc, char **argv) {
// The name of this node
std::string nodeName = "crs_tf_publisher";
// The used bag file
std::string bagFilePath = "/home/marius/bockdata/bags/room2.bag";
// The topic we subscribe to
std::string cloudSubscribeTopic = "/kinect2/sd/points";
// The topic we advertise the complete registered point clouds to
std::string cloudAdvertiseTopic = "/crs/points";
// The topic we subscribe to
std::string completeCloudFrame = "kinect2_ir_optical_frame";
// Some ICP Parameters. DO NOT CHANGE THEM!
double ICPMaxCorrespondenceDistance = 0.2;
double ICPTransformationEpsilon = 0.00001;
double ICPEuclidianFitnessEpsilon = 0.1;
// The leaf size of the VoxelGrid Filter
double ICPDownsampleLeafSize = 0.1;
// The maximal of ICP iterations
int ICPMaxIterations = 20;
// Init Basic ROS stuff
ros::init(argc, argv, nodeName);
auto node = ros::NodeHandle();
// Init subscriber and set topic
subscriber = node.subscribe<CloudRGBA>("/kinect2/sd/points", 1, callback);
// We are going to advertise the result cloud
completeCloudPublisher = node.advertise<CloudRGBA>(cloudAdvertiseTopic, 5);
// The ROS Bag Helper.
BagFileHelper helper;
helper.openBagFile(bagFilePath);
helper.initTopic(cloudSubscribeTopic);
// Create an empty cloud that is going to contain ALL registered point clouds
auto completeCloud = boost::make_shared<CloudRGBA>();
// Create a matrix that is going to contain the transformation from the CURRENT frame to the FIRST Model
// At the beginning it is an identiy matrix -> no transformation at all
Eigen::Matrix4f completeTransform;
completeTransform.setIdentity();
// Variables for the model and the frame
CloudRGBA::Ptr model, frame;
// The main loop runs as long as there are new clouds avaiable and [CRTL + c] has not been pressed
while (helper.hasTopicAvailableMessages(cloudSubscribeTopic) && ros::ok()) {
// Gather the next cloud from our bag file
frame = helper.getNextCloudRGBAForTopic(cloudSubscribeTopic);
if (model == nullptr) {
// Since it is the first loop iteration we do not have a model yet.
// The current frame will be the model for the next frame
model = frame;
} else {
// Downsample the input clouds
auto downsampledModel = downsample(model, ICPDownsampleLeafSize);
auto downsampledFrame = downsample(frame, ICPDownsampleLeafSize);
// Create the ICP object
IterativeClosestPoint icp;
// Init publisher and set topic
publisher = node.advertise<CloudRGBA>("/crs/points", 5);
// Set the ICP parameters
icp.setMaxCorrespondenceDistance(ICPMaxCorrespondenceDistance);
icp.setMaximumIterations(ICPMaxIterations);
icp.setTransformationEpsilon(ICPTransformationEpsilon);
icp.setEuclideanFitnessEpsilon(ICPEuclidianFitnessEpsilon);
// Run!
ros::spin();
// Set ICP data sources
icp.setInputCloud(downsampledFrame);
icp.setInputTarget(downsampledModel);
// Run ICP Alligment