Commit 1bee7cfc authored by Marius's avatar Marius

aswsd

parent 6579ecfc
......@@ -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>
......
......@@ -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;
}
......
......@@ -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,9 +105,9 @@ 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.
......@@ -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);
......
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