Commit a87639b0 authored by Marius's avatar Marius

asd

parents 3b8d3506 8c258f15
......@@ -31,14 +31,14 @@ 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; /* MISSING - Task 2 */
point.b = b; /* MISSING - Task 2 */
point.g = g;
point.b = b;
}
}
void prepareHeader(CloudRGBA::Ptr cloud, const std::string frameID) {
pcl_conversions::toPCL(ros::Time::now(), cloud->header.stamp);
cloud->header.frame_id = frameID; /* MISSING - Task 2 */
cloud->header.frame_id = frameID;
}
CloudRGBA::Ptr downsample(CloudRGBA::Ptr frame, float leafSize) {
......@@ -49,7 +49,7 @@ CloudRGBA::Ptr downsample(CloudRGBA::Ptr frame, float leafSize) {
}
// Downsample using voxelgrid
pcl::VoxelGrid<pcl::PointXYZRGBA> downsample_filter;
downsample_filter.setLeafSize(leafSize, leafSize, leafSize); /* MISSING - Task 2 */
downsample_filter.setLeafSize(leafSize, leafSize, leafSize);
downsample_filter.setInputCloud(frame);
downsample_filter.filter(*downsampledCloud);
......@@ -64,7 +64,7 @@ int main(int argc, char **argv) {
auto node = ros::NodeHandle();
// The used bag file
std::string bagFilePath = "/home/marius/bockdata/bags/room2.bag";
std::string bagFilePath = "/home/crsuser/Desktop/crs-stuff/bags/icp.bag";
// The topic we subscribe to
std::string cloudSubscribeTopic = "/kinect2/sd/points";
......@@ -76,20 +76,20 @@ int main(int argc, char **argv) {
ros::Publisher framePublisher, modelPublisher, alignedPublisher;
framePublisher = node.advertise<CloudRGBA>("/crs/frame", 5);
modelPublisher = node.advertise<CloudRGBA>("/crs/model", 5); /* MISSING - Task 3 */
alignedPublisher = node.advertise<CloudRGBA>("/crs/aligned", 5); /* MISSING - Task 3 */
modelPublisher = node.advertise<CloudRGBA>("/crs/model", 5);
alignedPublisher = node.advertise<CloudRGBA>("/crs/aligned", 5);
// The topic we subscribe to
std::string defaultCloudFrame = "kinect2_ir_optical_frame";
// Some ICP Parameters. DO NOT CHANGE THEM!
double ICPMaxCorrespondenceDistance = 0.2;
double ICPMaxCorrespondenceDistance = 0.3;
double ICPTransformationEpsilon = 0.00001;
double ICPEuclidianFitnessEpsilon = 0.1;
// The leaf size of the VoxelGrid Filter
double ICPDownsampleLeafSize = 0.1;
double ICPDownsampleLeafSize = 0.05;
// The maximal of ICP iterations
int ICPMaxIterations = 20;
......@@ -144,7 +144,8 @@ int main(int argc, char **argv) {
icp.setInputTarget(downsampledModel);
// Run ICP Alligment
icp.align(*downsampledFrame);
auto alignedFrame = boost::make_shared<CloudRGBA>();
icp.align(*alignedFrame);
// The ICP algorithm has finished. Print the convergence criteria!
ROS_INFO("ICP terminated with criteria: %s", getCriteria(icp));
......@@ -170,13 +171,13 @@ int main(int argc, char **argv) {
modelPublisher.publish(downsampledModel);
// Publish the current frame
setCloudColor(downsampledFrame, 255, 0, 0); /* MISSING - Task 3 */
prepareHeader(downsampledFrame, defaultCloudFrame); /* MISSING - Task 3 */
framePublisher.publish(downsampledFrame); /* MISSING - Task 3 */
setCloudColor(downsampledFrame, 255, 0, 0);
prepareHeader(downsampledFrame, defaultCloudFrame);
framePublisher.publish(downsampledFrame);
// Publish the current, donwsampled aligned frame
auto downsampledAlign = downsample(transformedCloud, ICPDownsampleLeafSize);
setCloudColor(downsampledAlign, 0, 0, 255);
setCloudColor(alignedFrame, 0, 0, 255);
prepareHeader(downsampledAlign, defaultCloudFrame);
alignedPublisher.publish(downsampledAlign);
......
......@@ -28,6 +28,9 @@
using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
void setCloudColor(CloudRGBA::Ptr cloud, int r, int g, int b) {
BOOST_ASSERT(r > 0 && r < 256);
/* MISSING - Task 2 */
/* MISSING - Task 2 */
for (int i = 0; i < cloud->size(); ++i) {
auto& point = (*cloud)[i];
point.r = r;
......@@ -47,6 +50,7 @@ CloudRGBA::Ptr downsample(CloudRGBA::Ptr frame, float leafSize) {
*downsampledCloud = *frame;
return downsampledCloud;
}
BOOST_ASSERT(leafSize != 0);
// Downsample using voxelgrid
pcl::VoxelGrid<pcl::PointXYZRGBA> downsample_filter;
downsample_filter.setLeafSize(0, 0, 0); /* FIX ME - Task 2 */
......@@ -64,7 +68,7 @@ int main(int argc, char **argv) {
auto node = ros::NodeHandle();
// The used bag file
std::string bagFilePath = "/home/marius/bockdata/bags/room2.bag";
std::string bagFilePath = "/home/crsuser/Desktop/crs-stuff/bags/icp.bag";
ros::Publisher framePublisher, modelPublisher, alignedPublisher, completeCloudPublisher;
......@@ -91,7 +95,11 @@ int main(int argc, char **argv) {
// The ROS Bag Helper.
BagFileHelper helper;
helper.openBagFile(bagFilePath);
<<<<<<< HEAD
helper.initTopic("");
=======
helper.initTopic("/kinect2/sd/points");
>>>>>>> 8c258f1581970c782091e6254b4b1bd65322f826
// Create an empty cloud that is going to contain ALL registered point clouds
auto completeCloud = boost::make_shared<CloudRGBA>();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment