Commit 1bee7cfc authored by Marius's avatar Marius

aswsd

parent 6579ecfc
...@@ -132,11 +132,8 @@ ...@@ -132,11 +132,8 @@
</component> </component>
<component name="ChangeListManager"> <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"> <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/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> </list>
<ignored path="$PROJECT_DIR$/cmake-build-debug/" /> <ignored path="$PROJECT_DIR$/cmake-build-debug/" />
<ignored path="$PROJECT_DIR$/../../build_isolated/crs/" /> <ignored path="$PROJECT_DIR$/../../build_isolated/crs/" />
...@@ -147,14 +144,36 @@ ...@@ -147,14 +144,36 @@
<option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" /> <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
<option name="LAST_RESOLUTION" value="IGNORE" /> <option name="LAST_RESOLUTION" value="IGNORE" />
</component> </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="ExecutionTargetManager" SELECTED_TARGET="CMakeBuildProfile:Debug" />
<component name="FileEditorManager"> <component name="FileEditorManager">
<leaf SIDE_TABS_SIZE_LIMIT_KEY="300"> <leaf SIDE_TABS_SIZE_LIMIT_KEY="300">
<file pinned="false" current-in-tab="false"> <file pinned="false" current-in-tab="false">
<entry file="file://$PROJECT_DIR$/src/registration_icp/main.cpp"> <entry file="file://$PROJECT_DIR$/src/registration_icp/main.cpp">
<provider selected="true" editor-type-id="text-editor"> <provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="203"> <state>
<caret line="28" selection-start-line="28" selection-end-line="28" /> <caret selection-end-line="188" />
<folding> <folding>
<element signature="e#0#20#0" expanded="true" /> <element signature="e#0#20#0" expanded="true" />
</folding> </folding>
...@@ -171,18 +190,6 @@ ...@@ -171,18 +190,6 @@
</provider> </provider>
</entry> </entry>
</file> </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"> <file pinned="false" current-in-tab="false">
<entry file="file://$PROJECT_DIR$/src/comon/pclhelper.cpp"> <entry file="file://$PROJECT_DIR$/src/comon/pclhelper.cpp">
<provider selected="true" editor-type-id="text-editor"> <provider selected="true" editor-type-id="text-editor">
...@@ -282,17 +289,17 @@ ...@@ -282,17 +289,17 @@
<option value="$PROJECT_DIR$/src/comon/rosbaghelper.cpp" /> <option value="$PROJECT_DIR$/src/comon/rosbaghelper.cpp" />
<option value="$PROJECT_DIR$/include/registration_icp/helper.hpp" /> <option value="$PROJECT_DIR$/include/registration_icp/helper.hpp" />
<option value="$PROJECT_DIR$/include/comon/rosbaghelper.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$/CMakeLists.txt" />
<option value="$PROJECT_DIR$/src/registration_icp/main.cpp" />
<option value="$PROJECT_DIR$/src/registration_icp_student/main.cpp" />
</list> </list>
</option> </option>
</component> </component>
<component name="OCFindUsagesOptions" text="true" ivars="false" properties="true" derivedClasses="false" /> <component name="OCFindUsagesOptions" text="true" ivars="false" properties="true" derivedClasses="false" />
<component name="ProjectFrameBounds" extendedState="6"> <component name="ProjectFrameBounds">
<option name="x" value="1086" /> <option name="x" value="1094" />
<option name="y" value="369" /> <option name="y" value="369" />
<option name="width" value="1914" /> <option name="width" value="1906" />
<option name="height" value="1174" /> <option name="height" value="1174" />
</component> </component>
<component name="ProjectView"> <component name="ProjectView">
...@@ -1162,18 +1169,17 @@ ...@@ -1162,18 +1169,17 @@
<workItem from="1546847120071" duration="25722000" /> <workItem from="1546847120071" duration="25722000" />
<workItem from="1546957001358" duration="7821000" /> <workItem from="1546957001358" duration="7821000" />
<workItem from="1547026884628" duration="197000" /> <workItem from="1547026884628" duration="197000" />
<workItem from="1547055031686" duration="5276000" /> <workItem from="1547055031686" duration="13911000" />
</task> </task>
<servers /> <servers />
</component> </component>
<component name="TimeTrackingManager"> <component name="TimeTrackingManager">
<option name="totallyTimeSpent" value="64106000" /> <option name="totallyTimeSpent" value="72741000" />
</component> </component>
<component name="ToolWindowManager"> <component name="ToolWindowManager">
<frame x="1079" y="369" width="1922" height="1175" extended-state="6" /> <frame x="1094" y="369" width="1906" height="1174" extended-state="0" />
<editor active="true" />
<layout> <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="Structure" order="1" side_tool="true" weight="0.25" />
<window_info id="Favorites" order="2" side_tool="true" /> <window_info id="Favorites" order="2" side_tool="true" />
<window_info anchor="bottom" id="Message" order="0" /> <window_info anchor="bottom" id="Message" order="0" />
...@@ -1184,9 +1190,9 @@ ...@@ -1184,9 +1190,9 @@
<window_info anchor="bottom" id="Inspection" order="5" weight="0.4" /> <window_info anchor="bottom" id="Inspection" order="5" weight="0.4" />
<window_info anchor="bottom" id="TODO" order="6" /> <window_info anchor="bottom" id="TODO" order="6" />
<window_info anchor="bottom" id="Database Changes" order="7" /> <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="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="Version Control" order="11" />
<window_info anchor="bottom" id="CMake" order="12" sideWeight="0.49946752" weight="0.32919848" /> <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" /> <window_info anchor="right" id="Commander" internal_type="SLIDING" order="0" type="SLIDING" weight="0.4" />
...@@ -1425,39 +1431,39 @@ ...@@ -1425,39 +1431,39 @@
</state> </state>
</provider> </provider>
</entry> </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"> <provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="278"> <state>
<caret line="23" column="30" selection-start-line="23" selection-start-column="30" selection-end-line="23" selection-end-column="30" /> <caret column="32" selection-start-column="32" selection-end-column="32" />
<folding>
<element signature="e#0#20#0" expanded="true" />
</folding>
</state> </state>
</provider> </provider>
</entry> </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"> <provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="203"> <state relative-caret-position="450">
<caret line="28" selection-start-line="28" selection-end-line="28" /> <caret line="30" selection-start-line="30" selection-end-line="30" />
<folding> <folding>
<element signature="e#0#20#0" expanded="true" /> <element signature="e#14#31#0" expanded="true" />
</folding> </folding>
</state> </state>
</provider> </provider>
</entry> </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"> <provider selected="true" editor-type-id="text-editor">
<state> <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> </state>
</provider> </provider>
</entry> </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"> <provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="450"> <state relative-caret-position="1543">
<caret line="30" selection-start-line="30" selection-end-line="30" /> <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> <folding>
<element signature="e#14#31#0" expanded="true" /> <element signature="e#0#20#0" expanded="true" />
</folding> </folding>
</state> </state>
</provider> </provider>
......
...@@ -31,27 +31,25 @@ void setCloudColor(CloudRGBA::Ptr cloud, int r, int g, int b) { ...@@ -31,27 +31,25 @@ void setCloudColor(CloudRGBA::Ptr cloud, int r, int g, int b) {
for (int i = 0; i < cloud->size(); ++i) { for (int i = 0; i < cloud->size(); ++i) {
auto& point = (*cloud)[i]; auto& point = (*cloud)[i];
point.r = r; point.r = r;
point.g = g; point.g = g; /* MISSING - Task 2 */
point.b = b; point.b = b; /* MISSING - Task 2 */
} }
} }
void prepareHeader(CloudRGBA::Ptr cloud, const std::string frameID) { void prepareHeader(CloudRGBA::Ptr cloud, const std::string frameID) {
pcl_conversions::toPCL(ros::Time::now(), cloud->header.stamp); 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 downsample(CloudRGBA::Ptr frame, float leafSize) {
CloudRGBA::Ptr downsampledCloud(new CloudRGBA); auto downsampledCloud = boost::make_shared<CloudRGBA>();
if (leafSize == 0) { if (leafSize == 0) {
*downsampledCloud = *frame; *downsampledCloud = *frame;
return downsampledCloud; return downsampledCloud;
} }
// Downsample using voxelgrid // Downsample using voxelgrid
pcl::VoxelGrid<pcl::PointXYZRGBA> downsample_filter; 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.setInputCloud(frame);
downsample_filter.filter(*downsampledCloud); downsample_filter.filter(*downsampledCloud);
...@@ -71,7 +69,6 @@ int main(int argc, char **argv) { ...@@ -71,7 +69,6 @@ int main(int argc, char **argv) {
// The topic we subscribe to // The topic we subscribe to
std::string cloudSubscribeTopic = "/kinect2/sd/points"; std::string cloudSubscribeTopic = "/kinect2/sd/points";
// The topic we advertise the complete registered point clouds to // The topic we advertise the complete registered point clouds to
// We are going to publish clouds ... Therefore we need a publisher! // We are going to publish clouds ... Therefore we need a publisher!
ros::Publisher completeCloudPublisher; ros::Publisher completeCloudPublisher;
...@@ -79,16 +76,12 @@ int main(int argc, char **argv) { ...@@ -79,16 +76,12 @@ int main(int argc, char **argv) {
ros::Publisher framePublisher, modelPublisher, alignedPublisher; ros::Publisher framePublisher, modelPublisher, alignedPublisher;
framePublisher = node.advertise<CloudRGBA>("/crs/frame", 5); framePublisher = node.advertise<CloudRGBA>("/crs/frame", 5);
modelPublisher = node.advertise<CloudRGBA>("/crs/model", 5); modelPublisher = node.advertise<CloudRGBA>("/crs/model", 5); /* MISSING - Task 3 */
alignedPublisher = node.advertise<CloudRGBA>("/crs/aligned", 5); alignedPublisher = node.advertise<CloudRGBA>("/crs/aligned", 5); /* MISSING - Task 3 */
// The topic we subscribe to // The topic we subscribe to
std::string defaultCloudFrame = "kinect2_ir_optical_frame"; std::string defaultCloudFrame = "kinect2_ir_optical_frame";
// Some ICP Parameters. DO NOT CHANGE THEM! // Some ICP Parameters. DO NOT CHANGE THEM!
double ICPMaxCorrespondenceDistance = 0.2; double ICPMaxCorrespondenceDistance = 0.2;
double ICPTransformationEpsilon = 0.00001; double ICPTransformationEpsilon = 0.00001;
...@@ -177,9 +170,9 @@ int main(int argc, char **argv) { ...@@ -177,9 +170,9 @@ int main(int argc, char **argv) {
modelPublisher.publish(downsampledModel); modelPublisher.publish(downsampledModel);
// Publish the current frame // Publish the current frame
setCloudColor(downsampledFrame, 255, 0, 0); setCloudColor(downsampledFrame, 255, 0, 0); /* MISSING - Task 3 */
prepareHeader(downsampledFrame, defaultCloudFrame); prepareHeader(downsampledFrame, defaultCloudFrame); /* MISSING - Task 3 */
framePublisher.publish(downsampledFrame); framePublisher.publish(downsampledFrame); /* MISSING - Task 3 */
// Publish the current, donwsampled aligned frame // Publish the current, donwsampled aligned frame
auto downsampledAlign = downsample(transformedCloud, ICPDownsampleLeafSize); auto downsampledAlign = downsample(transformedCloud, ICPDownsampleLeafSize);
...@@ -187,7 +180,6 @@ int main(int argc, char **argv) { ...@@ -187,7 +180,6 @@ int main(int argc, char **argv) {
prepareHeader(downsampledAlign, defaultCloudFrame); prepareHeader(downsampledAlign, defaultCloudFrame);
alignedPublisher.publish(downsampledAlign); alignedPublisher.publish(downsampledAlign);
// The current frame will be the model for the next frame // The current frame will be the model for the next frame
model = frame; model = frame;
} }
......
...@@ -23,36 +23,33 @@ ...@@ -23,36 +23,33 @@
#include <comon/rosbaghelper.hpp> #include <comon/rosbaghelper.hpp>
#include <comon/pclhelper.hpp> #include <comon/pclhelper.hpp>
// Less typing // Less typing
using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>; 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) { void setCloudColor(CloudRGBA::Ptr cloud, int r, int g, int b) {
for (int i = 0; i < cloud->size(); ++i) { for (int i = 0; i < cloud->size(); ++i) {
auto& point = (*cloud)[i]; auto& point = (*cloud)[i];
point.r = r; point.r = r;
point.g = g; /* MISSING - Task 2 */
point.b = b; /* MISSING - Task 2 */
} }
} }
// Prepares the header of the cloud and sets the frame id
void prepareHeader(CloudRGBA::Ptr cloud, const std::string frameID) { void prepareHeader(CloudRGBA::Ptr cloud, const std::string frameID) {
pcl_conversions::toPCL(ros::Time::now(), cloud->header.stamp); 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 downsample(CloudRGBA::Ptr frame, float leafSize) {
CloudRGBA::Ptr downsampledCloud(new CloudRGBA); auto downsampledCloud = boost::make_shared<CloudRGBA>();
if (leafSize == 0) { if (leafSize == 0) {
*downsampledCloud = *frame; *downsampledCloud = *frame;
return downsampledCloud; return downsampledCloud;
} }
// Downsample using voxelgrid // Downsample using voxelgrid
pcl::VoxelGrid<pcl::PointXYZRGBA> downsample_filter; 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.setInputCloud(frame);
downsample_filter.filter(*downsampledCloud); downsample_filter.filter(*downsampledCloud);
...@@ -63,44 +60,34 @@ int main(int argc, char **argv) { ...@@ -63,44 +60,34 @@ int main(int argc, char **argv) {
// The name of this node // The name of this node
std::string nodeName = "crs_tf_publisher"; std::string nodeName = "crs_tf_publisher";
// Init Basic ROS stuff
ros::init(argc, argv, nodeName); ros::init(argc, argv, nodeName);
auto node = ros::NodeHandle(); auto node = ros::NodeHandle();
// The used bag file // The used bag file
std::string bagFilePath = "/home/marius/bockdata/bags/room2.bag"; 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, completeCloudPublisher;
ros::Publisher framePublisher, modelPublisher, alignedPublisher;
framePublisher = node.advertise<CloudRGBA>("/crs/frame", 5); framePublisher = node.advertise<CloudRGBA>("/crs/frame", 5);
modelPublisher = node.advertise<CloudRGBA>("/crs/model", 5); modelPublisher = node.advertise<CloudRGBA>("", 5); /* FIX ME - Task 3 */
alignedPublisher = node.advertise<CloudRGBA>("/crs/aligned", 5); alignedPublisher = node.advertise<CloudRGBA>("", 5); /* FIX ME - Task 3 */
completeCloudPublisher = node.advertise<CloudRGBA>("", 5); /* FIX ME - Task 3 */
// The topic we subscribe to // The topic we subscribe to
std::string defaultCloudFrame = "kinect2_ir_optical_frame"; std::string defaultCloudFrame = "kinect2_ir_optical_frame";
// Some ICP Parameters. // Some ICP Parameters. DO NOT CHANGE THEM!
double ICPMaxCorrespondenceDistance = 0.2; double ICPMaxCorrespondenceDistance = 0.2;
double ICPTransformationEpsilon = 0.00001; double ICPTransformationEpsilon = 0.00001;
double ICPEuclidianFitnessEpsilon = 0.1; double ICPEuclidianFitnessEpsilon = 0.1;
// The leaf size of the VoxelGrid Filter // The leaf size of the VoxelGrid Filter
double ICPDownsampleLeafSize = 0.1; double ICPDownsampleLeafSize = 0.1;
// The maximal of ICP iterations // The maximal of ICP iterations
int ICPMaxIterations = 20; int ICPMaxIterations = 20;
// We are going to advertise the result cloud
completeCloudPublisher = node.advertise<CloudRGBA>(cloudAdvertiseTopic, 5);
// The ROS Bag Helper. // The ROS Bag Helper.
BagFileHelper helper; BagFileHelper helper;
helper.openBagFile(bagFilePath); helper.openBagFile(bagFilePath);
...@@ -118,16 +105,16 @@ int main(int argc, char **argv) { ...@@ -118,16 +105,16 @@ int main(int argc, char **argv) {
CloudRGBA::Ptr model, frame; CloudRGBA::Ptr model, frame;
// The main loop runs as long as there are new clouds avaiable and [CRTL + c] has not been pressed // 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 // Gather the next cloud from our bag file
frame = helper.getNextCloudRGBAForTopic(cloudSubscribeTopic); frame = helper.getNextCloudRGBAForTopic("/kinect2/sd/points");
if (model == nullptr) { if (model == nullptr) {
// Since it is the first loop iteration we do not have a model yet. // 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 // The current frame will be the model for the next frame
model = frame; model = frame;
} else { } else {
// Downsample the input clouds // Downsample the input clouds
auto downsampledModel = downsample(model, ICPDownsampleLeafSize); auto downsampledModel = downsample(model, ICPDownsampleLeafSize);
auto downsampledFrame = downsample(frame, ICPDownsampleLeafSize); auto downsampledFrame = downsample(frame, ICPDownsampleLeafSize);
...@@ -166,18 +153,18 @@ int main(int argc, char **argv) { ...@@ -166,18 +153,18 @@ int main(int argc, char **argv) {
completeCloudPublisher.publish(completeCloud); completeCloudPublisher.publish(completeCloud);
// Publish the current model // Publish the current model
setCloudColor(downsampledModel, 0, 255, 0); setCloudColor(downsampledModel, 0, 0, 0); /* FIX ME - Task 3 */
prepareHeader(downsampledModel, defaultCloudFrame); prepareHeader(downsampledModel, defaultCloudFrame);
modelPublisher.publish(downsampledModel); modelPublisher.publish(downsampledModel);
// Publish the current frame // Publish the current frame
setCloudColor(downsampledFrame, 255, 0, 0); setCloudColor(downsampledFrame, 0, 0, 0); /* FIX ME - Task 3 */
prepareHeader(downsampledFrame, defaultCloudFrame); prepareHeader(downsampledFrame, defaultCloudFrame);
framePublisher.publish(downsampledFrame); framePublisher.publish(downsampledFrame);
// Publish the current, donwsampled aligned frame // Publish the current, donwsampled aligned frame
auto downsampledAlign = downsample(transformedCloud, ICPDownsampleLeafSize); auto downsampledAlign = downsample(transformedCloud, ICPDownsampleLeafSize);
setCloudColor(downsampledAlign, 0, 0, 255); setCloudColor(downsampledAlign, 0, 0, 0); /* FIX ME - Task 3 */
prepareHeader(downsampledAlign, defaultCloudFrame); prepareHeader(downsampledAlign, defaultCloudFrame);
alignedPublisher.publish(downsampledAlign); alignedPublisher.publish(downsampledAlign);
......
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