From 1bee7cfcdc436e323cda5b9d5b9080d6c710657e Mon Sep 17 00:00:00 2001
From: Marius <asdasdasdasdasdasdasdasdasd@gmail.com>
Date: Thu, 10 Jan 2019 17:35:47 +0100
Subject: [PATCH] aswsd

---
 .idea/workspace.xml                   | 98 ++++++++++++++-------------
 src/registration_icp/main.cpp         | 28 +++-----
 src/registration_icp_student/main.cpp | 49 +++++---------
 3 files changed, 80 insertions(+), 95 deletions(-)

diff --git a/.idea/workspace.xml b/.idea/workspace.xml
index 6f3c0ca..1c6c3a7 100644
--- a/.idea/workspace.xml
+++ b/.idea/workspace.xml
@@ -132,11 +132,8 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="70f5d2b5-e1c9-415f-bbb3-eae2e165b096" name="Default Changelist" comment="Merge branch 'master' of https://fsstud.is.uni-due.de:8090/sfmabock/crs&#10;&#10;# Conflicts:&#10;#&#9;src/er1_driver_node/main.cpp&#10;#&#9;src/point_processing_node/main.cpp&#10;#&#9;src/point_processing_node_student/main.cpp">
-      <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/CMakeLists.txt" beforeDir="false" afterPath="$PROJECT_DIR$/CMakeLists.txt" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/include/registration_icp/helper.hpp" beforeDir="false" />
-      <change beforePath="$PROJECT_DIR$/src/registration_icp/helper.cpp" beforeDir="false" />
       <change beforePath="$PROJECT_DIR$/src/registration_icp/main.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/src/registration_icp/main.cpp" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/src/registration_icp_student/main.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/src/registration_icp_student/main.cpp" afterDir="false" />
     </list>
     <ignored path="$PROJECT_DIR$/cmake-build-debug/" />
     <ignored path="$PROJECT_DIR$/../../build_isolated/crs/" />
@@ -147,14 +144,36 @@
     <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
     <option name="LAST_RESOLUTION" value="IGNORE" />
   </component>
+  <component name="DockManager">
+    <window id="1">
+      <content type="file-editors">
+        <state>
+          <leaf>
+            <file pinned="false" current-in-tab="true">
+              <entry file="file://$PROJECT_DIR$/src/registration_icp_student/main.cpp">
+                <provider selected="true" editor-type-id="text-editor">
+                  <state relative-caret-position="1543">
+                    <caret line="112" column="79" lean-forward="true" selection-start-line="112" selection-start-column="79" selection-end-line="112" selection-end-column="79" />
+                    <folding>
+                      <element signature="e#0#20#0" expanded="true" />
+                    </folding>
+                  </state>
+                </provider>
+              </entry>
+            </file>
+          </leaf>
+        </state>
+      </content>
+    </window>
+  </component>
   <component name="ExecutionTargetManager" SELECTED_TARGET="CMakeBuildProfile:Debug" />
   <component name="FileEditorManager">
     <leaf SIDE_TABS_SIZE_LIMIT_KEY="300">
       <file pinned="false" current-in-tab="false">
         <entry file="file://$PROJECT_DIR$/src/registration_icp/main.cpp">
           <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="203">
-              <caret line="28" selection-start-line="28" selection-end-line="28" />
+            <state>
+              <caret selection-end-line="188" />
               <folding>
                 <element signature="e#0#20#0" expanded="true" />
               </folding>
@@ -171,18 +190,6 @@
           </provider>
         </entry>
       </file>
-      <file pinned="false" current-in-tab="false">
-        <entry file="file://$PROJECT_DIR$/src/registration_icp_student/main.cpp">
-          <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="278">
-              <caret line="23" column="30" selection-start-line="23" selection-start-column="30" selection-end-line="23" selection-end-column="30" />
-              <folding>
-                <element signature="e#0#20#0" expanded="true" />
-              </folding>
-            </state>
-          </provider>
-        </entry>
-      </file>
       <file pinned="false" current-in-tab="false">
         <entry file="file://$PROJECT_DIR$/src/comon/pclhelper.cpp">
           <provider selected="true" editor-type-id="text-editor">
@@ -282,17 +289,17 @@
         <option value="$PROJECT_DIR$/src/comon/rosbaghelper.cpp" />
         <option value="$PROJECT_DIR$/include/registration_icp/helper.hpp" />
         <option value="$PROJECT_DIR$/include/comon/rosbaghelper.hpp" />
-        <option value="$PROJECT_DIR$/src/registration_icp_student/main.cpp" />
-        <option value="$PROJECT_DIR$/src/registration_icp/main.cpp" />
         <option value="$PROJECT_DIR$/CMakeLists.txt" />
+        <option value="$PROJECT_DIR$/src/registration_icp/main.cpp" />
+        <option value="$PROJECT_DIR$/src/registration_icp_student/main.cpp" />
       </list>
     </option>
   </component>
   <component name="OCFindUsagesOptions" text="true" ivars="false" properties="true" derivedClasses="false" />
-  <component name="ProjectFrameBounds" extendedState="6">
-    <option name="x" value="1086" />
+  <component name="ProjectFrameBounds">
+    <option name="x" value="1094" />
     <option name="y" value="369" />
-    <option name="width" value="1914" />
+    <option name="width" value="1906" />
     <option name="height" value="1174" />
   </component>
   <component name="ProjectView">
@@ -1162,18 +1169,17 @@
       <workItem from="1546847120071" duration="25722000" />
       <workItem from="1546957001358" duration="7821000" />
       <workItem from="1547026884628" duration="197000" />
-      <workItem from="1547055031686" duration="5276000" />
+      <workItem from="1547055031686" duration="13911000" />
     </task>
     <servers />
   </component>
   <component name="TimeTrackingManager">
-    <option name="totallyTimeSpent" value="64106000" />
+    <option name="totallyTimeSpent" value="72741000" />
   </component>
   <component name="ToolWindowManager">
-    <frame x="1079" y="369" width="1922" height="1175" extended-state="6" />
-    <editor active="true" />
+    <frame x="1094" y="369" width="1906" height="1174" extended-state="0" />
     <layout>
-      <window_info content_ui="combo" id="Project" order="0" visible="true" weight="0.2736954" />
+      <window_info active="true" content_ui="combo" id="Project" order="0" visible="true" weight="0.27604726" />
       <window_info id="Structure" order="1" side_tool="true" weight="0.25" />
       <window_info id="Favorites" order="2" side_tool="true" />
       <window_info anchor="bottom" id="Message" order="0" />
@@ -1184,9 +1190,9 @@
       <window_info anchor="bottom" id="Inspection" order="5" weight="0.4" />
       <window_info anchor="bottom" id="TODO" order="6" />
       <window_info anchor="bottom" id="Database Changes" order="7" />
-      <window_info active="true" anchor="bottom" id="Messages" order="8" sideWeight="0.4978701" visible="true" weight="0.19847329" />
+      <window_info anchor="bottom" id="Messages" order="8" sideWeight="0.49785176" visible="true" weight="0.19866285" />
       <window_info anchor="bottom" id="Terminal" order="9" />
-      <window_info anchor="bottom" id="Event Log" order="10" sideWeight="0.5021299" side_tool="true" visible="true" weight="0.19847329" />
+      <window_info anchor="bottom" id="Event Log" order="10" sideWeight="0.5021482" side_tool="true" visible="true" weight="0.19866285" />
       <window_info anchor="bottom" id="Version Control" order="11" />
       <window_info anchor="bottom" id="CMake" order="12" sideWeight="0.49946752" weight="0.32919848" />
       <window_info anchor="right" id="Commander" internal_type="SLIDING" order="0" type="SLIDING" weight="0.4" />
@@ -1425,39 +1431,39 @@
         </state>
       </provider>
     </entry>
-    <entry file="file://$PROJECT_DIR$/src/registration_icp_student/main.cpp">
+    <entry file="file://$PROJECT_DIR$/src/comon/rosbaghelper.cpp">
       <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="278">
-          <caret line="23" column="30" selection-start-line="23" selection-start-column="30" selection-end-line="23" selection-end-column="30" />
-          <folding>
-            <element signature="e#0#20#0" expanded="true" />
-          </folding>
+        <state>
+          <caret column="32" selection-start-column="32" selection-end-column="32" />
         </state>
       </provider>
     </entry>
-    <entry file="file://$PROJECT_DIR$/src/registration_icp/main.cpp">
+    <entry file="file://$PROJECT_DIR$/include/comon/rosbaghelper.hpp">
       <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="203">
-          <caret line="28" selection-start-line="28" selection-end-line="28" />
+        <state relative-caret-position="450">
+          <caret line="30" selection-start-line="30" selection-end-line="30" />
           <folding>
-            <element signature="e#0#20#0" expanded="true" />
+            <element signature="e#14#31#0" expanded="true" />
           </folding>
         </state>
       </provider>
     </entry>
-    <entry file="file://$PROJECT_DIR$/src/comon/rosbaghelper.cpp">
+    <entry file="file://$PROJECT_DIR$/src/registration_icp/main.cpp">
       <provider selected="true" editor-type-id="text-editor">
         <state>
-          <caret column="32" selection-start-column="32" selection-end-column="32" />
+          <caret selection-end-line="188" />
+          <folding>
+            <element signature="e#0#20#0" expanded="true" />
+          </folding>
         </state>
       </provider>
     </entry>
-    <entry file="file://$PROJECT_DIR$/include/comon/rosbaghelper.hpp">
+    <entry file="file://$PROJECT_DIR$/src/registration_icp_student/main.cpp">
       <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="450">
-          <caret line="30" selection-start-line="30" selection-end-line="30" />
+        <state relative-caret-position="1543">
+          <caret line="112" column="79" lean-forward="true" selection-start-line="112" selection-start-column="79" selection-end-line="112" selection-end-column="79" />
           <folding>
-            <element signature="e#14#31#0" expanded="true" />
+            <element signature="e#0#20#0" expanded="true" />
           </folding>
         </state>
       </provider>
diff --git a/src/registration_icp/main.cpp b/src/registration_icp/main.cpp
index 9691acc..d485a3e 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<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.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<CloudRGBA>("/crs/frame", 5);
-    modelPublisher = node.advertise<CloudRGBA>("/crs/model", 5);
-    alignedPublisher = node.advertise<CloudRGBA>("/crs/aligned", 5);
-
+    modelPublisher = node.advertise<CloudRGBA>("/crs/model", 5); /* MISSING - Task 3 */
+    alignedPublisher = node.advertise<CloudRGBA>("/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 3b52ede..49baa87 100644
--- a/src/registration_icp_student/main.cpp
+++ b/src/registration_icp_student/main.cpp
@@ -23,36 +23,33 @@
 #include <comon/rosbaghelper.hpp>
 #include <comon/pclhelper.hpp>
 
+
 // Less typing
 using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
 
-
-// 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<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.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<CloudRGBA>("/crs/frame", 5);
-    modelPublisher = node.advertise<CloudRGBA>("/crs/model", 5);
-    alignedPublisher = node.advertise<CloudRGBA>("/crs/aligned", 5);
+    modelPublisher = node.advertise<CloudRGBA>("", 5); /* FIX ME - Task 3 */
+    alignedPublisher = node.advertise<CloudRGBA>("", 5); /* FIX ME - Task 3 */
+    completeCloudPublisher = node.advertise<CloudRGBA>("", 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<CloudRGBA>(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);
 
-- 
GitLab