main.cpp 7.03 KB
Newer Older
Marius's avatar
asd  
Marius committed
1 2 3 4 5 6 7 8 9
#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>
Marius's avatar
asd  
Marius committed
10 11 12 13 14 15 16 17 18 19 20 21 22
#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>
Marius's avatar
ad  
Marius committed
23 24
#include <comon/rosbaghelper.hpp>
#include <comon/pclhelper.hpp>
Marius's avatar
asd  
Marius committed
25

Marius's avatar
asd  
Marius committed
26 27 28

// Less typing
using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
Marius's avatar
ad  
Marius committed
29 30 31 32 33

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;
Marius's avatar
aswsd  
Marius committed
34 35
        point.g = g; /* MISSING - Task 2 */
        point.b = b; /* MISSING - Task 2 */
Marius's avatar
asd  
Marius committed
36 37
    }
}
Marius's avatar
asd  
Marius committed
38

Marius's avatar
ad  
Marius committed
39 40
void prepareHeader(CloudRGBA::Ptr cloud, const std::string frameID) {
    pcl_conversions::toPCL(ros::Time::now(), cloud->header.stamp);
Marius's avatar
aswsd  
Marius committed
41
    cloud->header.frame_id = frameID; /* MISSING - Task 2 */
Marius's avatar
ad  
Marius committed
42
}
Marius's avatar
asd  
Marius committed
43

Marius's avatar
asd  
Marius committed
44
CloudRGBA::Ptr downsample(CloudRGBA::Ptr frame, float leafSize) {
Marius's avatar
aswsd  
Marius committed
45
    auto downsampledCloud = boost::make_shared<CloudRGBA>();
Marius's avatar
asd  
Marius committed
46 47 48 49 50 51
    if (leafSize == 0) {
        *downsampledCloud = *frame;
        return downsampledCloud;
    }
    // Downsample using voxelgrid
    pcl::VoxelGrid<pcl::PointXYZRGBA> downsample_filter;
Marius's avatar
aswsd  
Marius committed
52
    downsample_filter.setLeafSize(leafSize, leafSize, leafSize); /* MISSING - Task 2 */
Marius's avatar
asd  
Marius committed
53 54
    downsample_filter.setInputCloud(frame);
    downsample_filter.filter(*downsampledCloud);
Marius's avatar
asd  
Marius committed
55

Marius's avatar
asd  
Marius committed
56
    return downsampledCloud;
Marius's avatar
asd  
Marius committed
57 58
}

Marius's avatar
asd  
Marius committed
59 60 61 62
int main(int argc, char **argv) {
    // The name of this node
    std::string nodeName = "crs_tf_publisher";

Marius's avatar
ad  
Marius committed
63 64 65
    ros::init(argc, argv, nodeName);
    auto node = ros::NodeHandle();

Marius's avatar
asd  
Marius committed
66 67 68 69 70 71 72
    // 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
Marius's avatar
ad  
Marius committed
73 74
    // We are going to publish clouds ... Therefore we need a publisher!
    ros::Publisher completeCloudPublisher;
Marius's avatar
asd  
Marius committed
75 76
    std::string cloudAdvertiseTopic = "/crs/points";

Marius's avatar
ad  
Marius committed
77 78
    ros::Publisher framePublisher, modelPublisher, alignedPublisher;
    framePublisher = node.advertise<CloudRGBA>("/crs/frame", 5);
Marius's avatar
aswsd  
Marius committed
79 80
    modelPublisher = node.advertise<CloudRGBA>("/crs/model", 5); /* MISSING - Task 3 */
    alignedPublisher = node.advertise<CloudRGBA>("/crs/aligned", 5); /* MISSING - Task 3 */
Marius's avatar
ad  
Marius committed
81

Marius's avatar
asd  
Marius committed
82
    // The topic we subscribe to
Marius's avatar
ad  
Marius committed
83 84
    std::string defaultCloudFrame = "kinect2_ir_optical_frame";

Marius's avatar
asd  
Marius committed
85 86 87 88 89 90 91 92 93 94 95 96 97
    // 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
Marius's avatar
ad  
Marius committed
98

Marius's avatar
asd  
Marius committed
99

Marius's avatar
asd  
Marius committed
100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134
    // 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;
Marius's avatar
asd  
Marius committed
135

Marius's avatar
asd  
Marius committed
136 137 138 139 140
            // Set the ICP parameters
            icp.setMaxCorrespondenceDistance(ICPMaxCorrespondenceDistance);
            icp.setMaximumIterations(ICPMaxIterations);
            icp.setTransformationEpsilon(ICPTransformationEpsilon);
            icp.setEuclideanFitnessEpsilon(ICPEuclidianFitnessEpsilon);
Marius's avatar
asd  
Marius committed
141

Marius's avatar
asd  
Marius committed
142 143 144
            // Set ICP data sources
            icp.setInputCloud(downsampledFrame);
            icp.setInputTarget(downsampledModel);
Marius's avatar
asd  
Marius committed
145

Marius's avatar
asd  
Marius committed
146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163
            // Run ICP Alligment
            icp.align(*downsampledFrame);

            // The ICP algorithm has finished. Print the convergence criteria!
            ROS_INFO("ICP terminated with criteria: %s", getCriteria(icp));

            // Receive the local transformation from the current frame to the model (the last frame)
            auto transform = icp.getFinalTransformation();

            // Calculate the complete transformation from the CURRENT frame to the FIRST model
            completeTransform = transform * completeTransform;

            // Transform the current frame and add it to the complete model
            auto transformedCloud = boost::make_shared<CloudRGBA>();
            pcl::transformPointCloud(*frame, *transformedCloud, completeTransform);
            *completeCloud += *transformedCloud;

            // Publish the complete model
Marius's avatar
ad  
Marius committed
164
            prepareHeader(completeCloud, defaultCloudFrame);
Marius's avatar
asd  
Marius committed
165 166
            completeCloudPublisher.publish(completeCloud);

Marius's avatar
ad  
Marius committed
167 168 169 170 171 172
            // Publish the current model
            setCloudColor(downsampledModel, 0, 255, 0);
            prepareHeader(downsampledModel, defaultCloudFrame);
            modelPublisher.publish(downsampledModel);

            // Publish the current frame
Marius's avatar
aswsd  
Marius committed
173 174 175
            setCloudColor(downsampledFrame, 255, 0, 0); /* MISSING - Task 3 */
            prepareHeader(downsampledFrame, defaultCloudFrame); /* MISSING - Task 3 */
            framePublisher.publish(downsampledFrame); /* MISSING - Task 3 */
Marius's avatar
ad  
Marius committed
176 177 178 179 180 181 182

            // Publish the current, donwsampled aligned frame
            auto downsampledAlign = downsample(transformedCloud, ICPDownsampleLeafSize);
            setCloudColor(downsampledAlign, 0, 0, 255);
            prepareHeader(downsampledAlign, defaultCloudFrame);
            alignedPublisher.publish(downsampledAlign);

Marius's avatar
asd  
Marius committed
183 184 185 186
            // The current frame will be the model for the next frame
            model = frame;
        }
    }
Marius's avatar
asd  
Marius committed
187
    return 0;
Marius's avatar
asd  
Marius committed
188
}