Skip to content

Commit a16dba1

Browse files
Pepijn-HundepoolMatthijsBurgh
Pepijn-Hundepool
authored and
MatthijsBurgh
committed
horiontal plane attempt
1 parent 5b80acc commit a16dba1

File tree

3 files changed

+195
-16
lines changed

3 files changed

+195
-16
lines changed

‎ed_sensor_integration/CMakeLists.txt‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,15 @@ cmake_minimum_required(VERSION 3.0.2)
22
project(ed_sensor_integration)
33

44
find_package(catkin REQUIRED COMPONENTS
5+
# cv_bridge
6+
# dynamic_reconfigure
7+
# geometry_msgs
8+
# actionlib_msgs
9+
# image_transport
10+
11+
# rospy
12+
# roscpp
13+
# std_msgs
514
ed
615
ed_sensor_integration_msgs
716
geolib2

‎ed_sensor_integration/cmake_install.cmake‎

Lines changed: 161 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,161 @@
1+
# Install script for directory: /home/pepijn/ros/noetic/system/src/ed_sensor_integration
2+
3+
# Set the install prefix
4+
if(NOT DEFINED CMAKE_INSTALL_PREFIX)
5+
set(CMAKE_INSTALL_PREFIX "/usr/local")
6+
endif()
7+
string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
8+
9+
# Set the install configuration name.
10+
if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
11+
if(BUILD_TYPE)
12+
string(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
13+
CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
14+
else()
15+
set(CMAKE_INSTALL_CONFIG_NAME "")
16+
endif()
17+
message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
18+
endif()
19+
20+
# Set the component getting installed.
21+
if(NOT CMAKE_INSTALL_COMPONENT)
22+
if(COMPONENT)
23+
message(STATUS "Install component: \"${COMPONENT}\"")
24+
set(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
25+
else()
26+
set(CMAKE_INSTALL_COMPONENT)
27+
endif()
28+
endif()
29+
30+
# Install shared libraries without execute permission?
31+
if(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
32+
set(CMAKE_INSTALL_SO_NO_EXE "1")
33+
endif()
34+
35+
# Is this installation the result of a crosscompile?
36+
if(NOT DEFINED CMAKE_CROSSCOMPILING)
37+
set(CMAKE_CROSSCOMPILING "FALSE")
38+
endif()
39+
40+
if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT)
41+
42+
if (NOT EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}")
43+
file(MAKE_DIRECTORY "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}")
44+
endif()
45+
if (NOT EXISTS "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/.catkin")
46+
file(WRITE "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}/.catkin" "")
47+
endif()
48+
endif()
49+
50+
if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT)
51+
list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
52+
"/usr/local/_setup_util.py")
53+
if(CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
54+
message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
55+
endif()
56+
if(CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
57+
message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
58+
endif()
59+
file(INSTALL DESTINATION "/usr/local" TYPE PROGRAM FILES "/home/pepijn/ros/noetic/system/src/ed_sensor_integration/catkin_generated/installspace/_setup_util.py")
60+
endif()
61+
62+
if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT)
63+
list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
64+
"/usr/local/env.sh")
65+
if(CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
66+
message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
67+
endif()
68+
if(CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
69+
message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
70+
endif()
71+
file(INSTALL DESTINATION "/usr/local" TYPE PROGRAM FILES "/home/pepijn/ros/noetic/system/src/ed_sensor_integration/catkin_generated/installspace/env.sh")
72+
endif()
73+
74+
if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT)
75+
list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
76+
"/usr/local/setup.bash;/usr/local/local_setup.bash")
77+
if(CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
78+
message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
79+
endif()
80+
if(CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
81+
message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
82+
endif()
83+
file(INSTALL DESTINATION "/usr/local" TYPE FILE FILES
84+
"/home/pepijn/ros/noetic/system/src/ed_sensor_integration/catkin_generated/installspace/setup.bash"
85+
"/home/pepijn/ros/noetic/system/src/ed_sensor_integration/catkin_generated/installspace/local_setup.bash"
86+
)
87+
endif()
88+
89+
if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT)
90+
list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
91+
"/usr/local/setup.sh;/usr/local/local_setup.sh")
92+
if(CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
93+
message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
94+
endif()
95+
if(CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
96+
message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
97+
endif()
98+
file(INSTALL DESTINATION "/usr/local" TYPE FILE FILES
99+
"/home/pepijn/ros/noetic/system/src/ed_sensor_integration/catkin_generated/installspace/setup.sh"
100+
"/home/pepijn/ros/noetic/system/src/ed_sensor_integration/catkin_generated/installspace/local_setup.sh"
101+
)
102+
endif()
103+
104+
if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT)
105+
list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
106+
"/usr/local/setup.zsh;/usr/local/local_setup.zsh")
107+
if(CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
108+
message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
109+
endif()
110+
if(CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
111+
message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
112+
endif()
113+
file(INSTALL DESTINATION "/usr/local" TYPE FILE FILES
114+
"/home/pepijn/ros/noetic/system/src/ed_sensor_integration/catkin_generated/installspace/setup.zsh"
115+
"/home/pepijn/ros/noetic/system/src/ed_sensor_integration/catkin_generated/installspace/local_setup.zsh"
116+
)
117+
endif()
118+
119+
if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT)
120+
list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES
121+
"/usr/local/.rosinstall")
122+
if(CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION)
123+
message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
124+
endif()
125+
if(CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION)
126+
message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}")
127+
endif()
128+
file(INSTALL DESTINATION "/usr/local" TYPE FILE FILES "/home/pepijn/ros/noetic/system/src/ed_sensor_integration/catkin_generated/installspace/.rosinstall")
129+
endif()
130+
131+
if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT)
132+
file(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/lib/pkgconfig" TYPE FILE FILES "/home/pepijn/ros/noetic/system/src/ed_sensor_integration/catkin_generated/installspace/ed_sensor_integration.pc")
133+
endif()
134+
135+
if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT)
136+
file(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/share/ed_sensor_integration/cmake" TYPE FILE FILES
137+
"/home/pepijn/ros/noetic/system/src/ed_sensor_integration/catkin_generated/installspace/ed_sensor_integrationConfig.cmake"
138+
"/home/pepijn/ros/noetic/system/src/ed_sensor_integration/catkin_generated/installspace/ed_sensor_integrationConfig-version.cmake"
139+
)
140+
endif()
141+
142+
if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT)
143+
file(INSTALL DESTINATION "${CMAKE_INSTALL_PREFIX}/share/ed_sensor_integration" TYPE FILE FILES "/home/pepijn/ros/noetic/system/src/ed_sensor_integration/package.xml")
144+
endif()
145+
146+
if(NOT CMAKE_INSTALL_LOCAL_ONLY)
147+
# Include the install script for each subdirectory.
148+
include("/home/pepijn/ros/noetic/system/src/ed_sensor_integration/gtest/cmake_install.cmake")
149+
150+
endif()
151+
152+
if(CMAKE_INSTALL_COMPONENT)
153+
set(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt")
154+
else()
155+
set(CMAKE_INSTALL_MANIFEST "install_manifest.txt")
156+
endif()
157+
158+
string(REPLACE ";" "\n" CMAKE_INSTALL_MANIFEST_CONTENT
159+
"${CMAKE_INSTALL_MANIFEST_FILES}")
160+
file(WRITE "/home/pepijn/ros/noetic/system/src/ed_sensor_integration/${CMAKE_INSTALL_MANIFEST}"
161+
"${CMAKE_INSTALL_MANIFEST_CONTENT}")

‎ed_sensor_integration/src/kinect/code_v1.cpp‎

Lines changed: 25 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,25 @@
11
#include <iostream>
22
#include <pcl/ModelCoefficients.h>
33
#include <pcl/io/pcd_io.h>
4+
45
#include <pcl/point_types.h>
6+
#include <pcl/point_cloud.h>
7+
#include <pcl/point_representation.h>
8+
59
#include <pcl/sample_consensus/method_types.h>
610
#include <pcl/sample_consensus/model_types.h>
11+
#include <pcl/sample_consensus/sac_model_parallel_plane.h>
12+
713
#include <pcl/segmentation/sac_segmentation.h>
14+
815
#include <pcl/filters/voxel_grid.h>
916
#include <pcl/filters/extract_indices.h>
17+
1018
#include <ed/io/json_reader.h>
1119

12-
Eigen::Matrix4f ReadJson(std::string 2022-04-05-13-28-28, float *xout, float *yout, float *zout) {
20+
Eigen::Matrix4f ReadJson(std::string pcd_filename, float *xout, float *yout, float *zout) {
1321

14-
std::string json_filename = boost::filesystem::change_extension(2022-04-05-13-28-28, ".json").string();
22+
std::string json_filename = boost::filesystem::change_extension(pcd_filename, ".json").string();
1523
// read json metadata
1624
tue::config::DataPointer meta_data;
1725

@@ -62,24 +70,22 @@ Eigen::Matrix4f ReadJson(std::string 2022-04-05-13-28-28, float *xout, float *yo
6270
{n*qx*qz - n*qy*qw, n*qy*qz + n*qx*qw, 1.0f - n*qx*qx - n*qy*qy, z},
6371
{0.0f, 0.0f, 0.0f, 1.0f}}; */
6472

65-
Transform(0,0) = xx; //1.0f - n*qy*qy - n*qz*qz;
66-
Transform(0,1) = xy; //n*qx*qy - n*qz*qw;
67-
Transform(0,2) = xz; //n*qx*qz + n*qy*qw;
73+
Transform(0,0) = xx;//1.0f - n*qy*qy - n*qz*qz;
74+
Transform(0,1) = xy;//n*qx*qy - n*qz*qw;
75+
Transform(0,2) = xz;//n*qx*qz + n*qy*qw;
6876
Transform(0,3) = x;
69-
Transform(1,0) = yx; //n*qx*qy + n*qz*qw;
70-
Transform(1,1) = yy; //1.0f - n*qx*qx - n*qz*qz;
71-
Transform(1,2) = yz; //n*qy*qz - n*qx*qw;
77+
Transform(1,0) = yx;//n*qx*qy + n*qz*qw;
78+
Transform(1,1) = yy;//1.0f - n*qx*qx - n*qz*qz;
79+
Transform(1,2) = yz;//n*qy*qz - n*qx*qw;
7280
Transform(1,3) = y;
73-
Transform(2,0) = zx; //n*qx*qz - n*qy*qw;
74-
Transform(2,1) = zy; //n*qy*qz + n*qx*qw;
75-
Transform(2,2) = zz; //1.0f - n*qx*qx - n*qy*qy;
81+
Transform(2,0) = zx;//n*qx*qz - n*qy*qw;
82+
Transform(2,1) = zy;//n*qy*qz + n*qx*qw;
83+
Transform(2,2) = zz;//1.0f - n*qx*qx - n*qy*qy;
7684
Transform(2,3) = z;
7785

7886
std::cout << Transform << std::endl;
7987
return Transform;
8088
}
81-
}
82-
8389

8490
int
8591
main ()
@@ -112,13 +118,16 @@ main ()
112118
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
113119
// Create the segmentation object
114120
pcl::SACSegmentation<pcl::PointXYZ> seg;
121+
pcl::SampleConsensusModelParallelPlane<pcl::PointXYZ> model (cloud_filtered);
115122
// Optional
116123
seg.setOptimizeCoefficients (true);
117124
// Mandatory
118-
seg.setModelType (pcl::SACMODEL_PLANE);
125+
seg.setModelType (pcl::SACMODEL_PARALLEL_PLANE);
119126
seg.setMethodType (pcl::SAC_RANSAC);
120127
seg.setMaxIterations (1000);
121128
seg.setDistanceThreshold (0.01);
129+
model.setAxis(Eigen::Vector3f(1,0,0));
130+
model.setEpsAngle(30*0.0174532925); //*0.0174532925 to radians
122131

123132
// Create the filtering object
124133
pcl::ExtractIndices<pcl::PointXYZ> extract;
@@ -130,6 +139,7 @@ main ()
130139
// Segment the largest planar component from the remaining cloud
131140
seg.setInputCloud (cloud_filtered);
132141
seg.segment (*inliers, *coefficients);
142+
133143
if (inliers->indices.size () == 0)
134144
{
135145
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
@@ -155,5 +165,4 @@ main ()
155165
}
156166

157167
return (0);
158-
159-
168+
}

0 commit comments

Comments
 (0)