diff --git a/.idea/workspace.xml b/.idea/workspace.xml
index 6f3c0ca9426bb3664a3645f9ef9f1608cbedb8fb..1c6c3a7a9ff65f3481e54b09ef79e7cabbfd9d7d 100644
--- a/.idea/workspace.xml
+++ b/.idea/workspace.xml
@@ -132,11 +132,8 @@
-
-
-
-
+
@@ -147,14 +144,36 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
+
+
@@ -171,18 +190,6 @@
-
-
-
-
-
-
-
-
-
-
-
-
@@ -282,17 +289,17 @@
-
-
+
+
-
-
+
+
-
+
@@ -1162,18 +1169,17 @@
-
+
-
+
-
-
+
-
+
@@ -1184,9 +1190,9 @@
-
+
-
+
@@ -1425,39 +1431,39 @@
-
+
-
-
-
-
-
+
+
-
+
-
-
+
+
-
+
-
+
-
+
+
+
+
-
+
-
-
+
+
-
+
diff --git a/src/registration_icp/main.cpp b/src/registration_icp/main.cpp
index 9691acc702f7b869fc1b2e4c37bdce4f1ffe8a5e..d485a3e5b3dd266852015d42b888afcd5a6ad297 100644
--- a/src/registration_icp/main.cpp
+++ b/src/registration_icp/main.cpp
@@ -31,27 +31,25 @@ void setCloudColor(CloudRGBA::Ptr cloud, int r, int g, int b) {
for (int i = 0; i < cloud->size(); ++i) {
auto& point = (*cloud)[i];
point.r = r;
- point.g = g;
- point.b = b;
+ point.g = g; /* MISSING - Task 2 */
+ point.b = b; /* MISSING - Task 2 */
}
}
void prepareHeader(CloudRGBA::Ptr cloud, const std::string frameID) {
pcl_conversions::toPCL(ros::Time::now(), cloud->header.stamp);
- cloud->header.frame_id = frameID;
+ cloud->header.frame_id = frameID; /* MISSING - Task 2 */
}
-
-
CloudRGBA::Ptr downsample(CloudRGBA::Ptr frame, float leafSize) {
- CloudRGBA::Ptr downsampledCloud(new CloudRGBA);
+ auto downsampledCloud = boost::make_shared();
if (leafSize == 0) {
*downsampledCloud = *frame;
return downsampledCloud;
}
// Downsample using voxelgrid
pcl::VoxelGrid downsample_filter;
- downsample_filter.setLeafSize(leafSize, leafSize, leafSize);
+ downsample_filter.setLeafSize(leafSize, leafSize, leafSize); /* MISSING - Task 2 */
downsample_filter.setInputCloud(frame);
downsample_filter.filter(*downsampledCloud);
@@ -71,7 +69,6 @@ int main(int argc, char **argv) {
// The topic we subscribe to
std::string cloudSubscribeTopic = "/kinect2/sd/points";
-
// The topic we advertise the complete registered point clouds to
// We are going to publish clouds ... Therefore we need a publisher!
ros::Publisher completeCloudPublisher;
@@ -79,16 +76,12 @@ int main(int argc, char **argv) {
ros::Publisher framePublisher, modelPublisher, alignedPublisher;
framePublisher = node.advertise("/crs/frame", 5);
- modelPublisher = node.advertise("/crs/model", 5);
- alignedPublisher = node.advertise("/crs/aligned", 5);
-
+ modelPublisher = node.advertise("/crs/model", 5); /* MISSING - Task 3 */
+ alignedPublisher = node.advertise("/crs/aligned", 5); /* MISSING - Task 3 */
// The topic we subscribe to
std::string defaultCloudFrame = "kinect2_ir_optical_frame";
-
-
-
// Some ICP Parameters. DO NOT CHANGE THEM!
double ICPMaxCorrespondenceDistance = 0.2;
double ICPTransformationEpsilon = 0.00001;
@@ -177,9 +170,9 @@ int main(int argc, char **argv) {
modelPublisher.publish(downsampledModel);
// Publish the current frame
- setCloudColor(downsampledFrame, 255, 0, 0);
- prepareHeader(downsampledFrame, defaultCloudFrame);
- framePublisher.publish(downsampledFrame);
+ setCloudColor(downsampledFrame, 255, 0, 0); /* MISSING - Task 3 */
+ prepareHeader(downsampledFrame, defaultCloudFrame); /* MISSING - Task 3 */
+ framePublisher.publish(downsampledFrame); /* MISSING - Task 3 */
// Publish the current, donwsampled aligned frame
auto downsampledAlign = downsample(transformedCloud, ICPDownsampleLeafSize);
@@ -187,7 +180,6 @@ int main(int argc, char **argv) {
prepareHeader(downsampledAlign, defaultCloudFrame);
alignedPublisher.publish(downsampledAlign);
-
// The current frame will be the model for the next frame
model = frame;
}
diff --git a/src/registration_icp_student/main.cpp b/src/registration_icp_student/main.cpp
index 3b52ede1a9da4dad4c89569445705974a682371b..49baa8718b3146655beb90bc1b020079e41fd6a3 100644
--- a/src/registration_icp_student/main.cpp
+++ b/src/registration_icp_student/main.cpp
@@ -23,36 +23,33 @@
#include
#include
+
// Less typing
using CloudRGBA = pcl::PointCloud;
-
-// Sets all points of the cloud to a specific color. All values have to be in [0, 255]
void setCloudColor(CloudRGBA::Ptr cloud, int r, int g, int b) {
for (int i = 0; i < cloud->size(); ++i) {
auto& point = (*cloud)[i];
point.r = r;
- point.g = g;
- point.b = b;
+ /* MISSING - Task 2 */
+ /* MISSING - Task 2 */
}
}
-// Prepares the header of the cloud and sets the frame id
void prepareHeader(CloudRGBA::Ptr cloud, const std::string frameID) {
pcl_conversions::toPCL(ros::Time::now(), cloud->header.stamp);
- cloud->header.frame_id = frameID;
+ cloud->header.frame_id = ""; /* FIX ME - Task 2 */
}
-// Downsamples the cloud using a voxelgrid filter using the provided leaf size.
CloudRGBA::Ptr downsample(CloudRGBA::Ptr frame, float leafSize) {
- CloudRGBA::Ptr downsampledCloud(new CloudRGBA);
+ auto downsampledCloud = boost::make_shared();
if (leafSize == 0) {
*downsampledCloud = *frame;
return downsampledCloud;
}
// Downsample using voxelgrid
pcl::VoxelGrid downsample_filter;
- downsample_filter.setLeafSize(leafSize, leafSize, leafSize);
+ downsample_filter.setLeafSize(0, 0, 0); /* FIX ME - Task 2 */
downsample_filter.setInputCloud(frame);
downsample_filter.filter(*downsampledCloud);
@@ -63,44 +60,34 @@ int main(int argc, char **argv) {
// The name of this node
std::string nodeName = "crs_tf_publisher";
- // Init Basic ROS stuff
ros::init(argc, argv, nodeName);
auto node = ros::NodeHandle();
// 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
- // We are going to publish clouds ... Therefore we need a publisher!
- ros::Publisher completeCloudPublisher;
- std::string cloudAdvertiseTopic = "/crs/points";
- // Setup the publisher for the frame, model and aligned
- ros::Publisher framePublisher, modelPublisher, alignedPublisher;
+ ros::Publisher framePublisher, modelPublisher, alignedPublisher, completeCloudPublisher;
framePublisher = node.advertise("/crs/frame", 5);
- modelPublisher = node.advertise("/crs/model", 5);
- alignedPublisher = node.advertise("/crs/aligned", 5);
+ modelPublisher = node.advertise("", 5); /* FIX ME - Task 3 */
+ alignedPublisher = node.advertise("", 5); /* FIX ME - Task 3 */
+ completeCloudPublisher = node.advertise("", 5); /* FIX ME - Task 3 */
// The topic we subscribe to
std::string defaultCloudFrame = "kinect2_ir_optical_frame";
- // Some ICP Parameters.
+ // 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;
- // We are going to advertise the result cloud
- completeCloudPublisher = node.advertise(cloudAdvertiseTopic, 5);
-
// The ROS Bag Helper.
BagFileHelper helper;
helper.openBagFile(bagFilePath);
@@ -118,16 +105,16 @@ int main(int argc, char **argv) {
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()) {
+ while (helper.hasTopicAvailableMessages("/kinect2/sd/points") && ros::ok()) {
// Gather the next cloud from our bag file
- frame = helper.getNextCloudRGBAForTopic(cloudSubscribeTopic);
+ frame = helper.getNextCloudRGBAForTopic("/kinect2/sd/points");
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
+ // Downsample the input clouds
auto downsampledModel = downsample(model, ICPDownsampleLeafSize);
auto downsampledFrame = downsample(frame, ICPDownsampleLeafSize);
@@ -166,18 +153,18 @@ int main(int argc, char **argv) {
completeCloudPublisher.publish(completeCloud);
// Publish the current model
- setCloudColor(downsampledModel, 0, 255, 0);
+ setCloudColor(downsampledModel, 0, 0, 0); /* FIX ME - Task 3 */
prepareHeader(downsampledModel, defaultCloudFrame);
modelPublisher.publish(downsampledModel);
// Publish the current frame
- setCloudColor(downsampledFrame, 255, 0, 0);
+ setCloudColor(downsampledFrame, 0, 0, 0); /* FIX ME - Task 3 */
prepareHeader(downsampledFrame, defaultCloudFrame);
framePublisher.publish(downsampledFrame);
// Publish the current, donwsampled aligned frame
auto downsampledAlign = downsample(transformedCloud, ICPDownsampleLeafSize);
- setCloudColor(downsampledAlign, 0, 0, 255);
+ setCloudColor(downsampledAlign, 0, 0, 0); /* FIX ME - Task 3 */
prepareHeader(downsampledAlign, defaultCloudFrame);
alignedPublisher.publish(downsampledAlign);