Skip to content

Commit

Permalink
add plane edges
Browse files Browse the repository at this point in the history
  • Loading branch information
@koide3
koide3 committed Oct 8, 2019
1 parent 9c360c1 commit e5f8b27
Show file tree
Hide file tree
Showing 10 changed files with 269 additions and 40 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,7 @@ catkin_package(
###########
include_directories(include)
include_directories(
${PCL_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)

Expand Down
1 change: 1 addition & 0 deletions apps/scan_matching_odometry_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,6 +178,7 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet {

prev_trans = trans;


auto keyframe_trans = matrix2transform(stamp, keyframe_pose, odom_frame_id, "keyframe");
keyframe_broadcaster.sendTransform(keyframe_trans);

Expand Down
20 changes: 20 additions & 0 deletions imgui.ini
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
[Window][Debug##Default]
Pos=60,60
Size=400,400
Collapsed=0

[Window][##stats]
Pos=1687,890
Size=156,144
Collapsed=0

[Window][shader setting]
Pos=60,60
Size=263,100
Collapsed=0

[Window][plane detection]
Pos=39,614
Size=403,404
Collapsed=0

40 changes: 26 additions & 14 deletions include/g2o/edge_plane_parallel.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,10 +35,10 @@ class EdgePlaneParallel : public BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane,
}

setMeasurement(v);
for (int i = 0; i < 3; ++i) {
for (int j = i; j < 3; ++j) {
for(int i = 0; i < information().rows(); ++i) {
for(int j = i; j < information().cols(); ++j) {
is >> information()(i, j);
if (i != j) {
if(i != j) {
information()(j, i) = information()(i, j);
}
}
Expand All @@ -48,9 +48,15 @@ class EdgePlaneParallel : public BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane,
}

virtual bool write(std::ostream& os) const override {
for (int i = 0; i < 3; ++i) os << _measurement[i] << " ";
for (int i = 0; i < 3; ++i)
for (int j = i; j < 3; ++j) os << " " << information()(i, j);
for(int i = 0; i < 3; ++i) {
os << _measurement[i] << " ";
}

for(int i = 0; i < information().rows(); ++i) {
for(int j = i; j < information().cols(); ++j) {
os << " " << information()(i, j);
};
}
return os.good();
}

Expand All @@ -59,10 +65,10 @@ class EdgePlaneParallel : public BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane,
virtual int measurementDimension() const override { return 3; }
};

class EdgePlanePerpendicular : public BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane> {
class EdgePlanePerpendicular : public BaseBinaryEdge<1, Eigen::Vector3d, VertexPlane, VertexPlane> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgePlanePerpendicular() : BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane>() {
EdgePlanePerpendicular() : BaseBinaryEdge<1, Eigen::Vector3d, VertexPlane, VertexPlane>() {
_information.setIdentity();
_error.setZero();
}
Expand All @@ -74,7 +80,7 @@ class EdgePlanePerpendicular : public BaseBinaryEdge<3, Eigen::Vector3d, VertexP
Eigen::Vector3d normal1 = v1->estimate().normal().normalized();
Eigen::Vector3d normal2 = v2->estimate().normal().normalized();

_error = normal1.array() * normal2.array();
_error[0] = normal1.dot(normal2);
}
virtual bool read(std::istream& is) override {
Eigen::Vector3d v;
Expand All @@ -83,8 +89,8 @@ class EdgePlanePerpendicular : public BaseBinaryEdge<3, Eigen::Vector3d, VertexP
}

setMeasurement(v);
for (int i = 0; i < 3; ++i) {
for (int j = i; j < 3; ++j) {
for(int i = 0; i < information().rows(); ++i) {
for(int j = i; j < information().cols(); ++j) {
is >> information()(i, j);
if (i != j) {
information()(j, i) = information()(i, j);
Expand All @@ -96,9 +102,15 @@ class EdgePlanePerpendicular : public BaseBinaryEdge<3, Eigen::Vector3d, VertexP
}

virtual bool write(std::ostream& os) const override {
for (int i = 0; i < 3; ++i) os << _measurement[i] << " ";
for (int i = 0; i < 3; ++i)
for (int j = i; j < 3; ++j) os << " " << information()(i, j);
for (int i = 0; i < 3; ++i) {
os << _measurement[i] << " ";
}

for(int i = 0; i < information().rows(); ++i) {
for(int j = i; j < information().cols(); ++j) {
os << " " << information()(i, j);
};
}
return os.good();
}

Expand Down
31 changes: 31 additions & 0 deletions include/g2o/edge_plane_prior.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,37 @@ class EdgePlanePriorNormal : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::
return os.good();
}
};

class EdgePlanePriorDistance : public g2o::BaseUnaryEdge<1, double, g2o::VertexPlane> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgePlanePriorDistance() : g2o::BaseUnaryEdge<1, double, g2o::VertexPlane>() {}

void computeError() override {
const g2o::VertexPlane* v1 = static_cast<const g2o::VertexPlane*>(_vertices[0]);
_error[0] = _measurement - v1->estimate().distance();
}

void setMeasurement(const double& m) override {
_measurement = m;
}

virtual bool read(std::istream& is) override {
is >> _measurement;
for(int i = 0; i < information().rows(); ++i)
for(int j = i; j < information().cols(); ++j) {
is >> information()(i, j);
if(i != j) information()(j, i) = information()(i, j);
}
return true;
}
virtual bool write(std::ostream& os) const override {
os << _measurement;
for(int i = 0; i < information().rows(); ++i)
for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j);
return os.good();
}
};
} // namespace g2o

#endif // EDGE_SE3_PRIORXY_HPP
5 changes: 4 additions & 1 deletion include/hdl_graph_slam/graph_slam.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@ namespace g2o {
class EdgePlaneParallel;
class EdgePlanePerpendicular;
class EdgePlanePriorNormal;
class EdgePlanePriorDistance;
class RobustKernelFactory;
}

Expand Down Expand Up @@ -94,6 +95,8 @@ class GraphSLAM {
*/
g2o::EdgePlanePriorNormal* add_plane_normal_prior_edge(g2o::VertexPlane* v, const Eigen::Vector3d& normal, const Eigen::MatrixXd& information_matrix);

g2o::EdgePlanePriorDistance* add_plane_distance_prior_edge(g2o::VertexPlane* v, double distance, const Eigen::MatrixXd& information_matrix);

g2o::EdgeSE3PriorXY* add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix);

g2o::EdgeSE3PriorXYZ* add_se3_prior_xyz_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix);
Expand All @@ -106,7 +109,7 @@ class GraphSLAM {

g2o::EdgePlaneParallel* add_plane_parallel_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::Matrix3d& information);

g2o::EdgePlanePerpendicular* add_plane_perpendicular_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::Matrix3d& information);
g2o::EdgePlanePerpendicular* add_plane_perpendicular_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information);

void add_robust_kernel(g2o::HyperGraph::Edge* edge, const std::string& kernel_type, double kernel_size);

Expand Down
131 changes: 131 additions & 0 deletions launch/hdl_graph_slam_pasco.launch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,131 @@
<?xml version="1.0"?>
<launch>
<!-- arguments -->
<arg name="nodelet_manager" default="velodyne_nodelet_manager" />
<arg name="enable_floor_detection" default="false" />
<arg name="enable_gps" default="false" />
<arg name="enable_imu_acc" default="false" />
<arg name="enable_imu_ori" default="false" />
<arg name="points_topic" default="/velodyne_points" />

<!-- transformation between lidar and base_link -->
<node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link velodyne 10" />

<!-- in case you use velodyne_driver, comment out the following line -->
<node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>

<!-- prefiltering_nodelet -->
<node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" args="load hdl_graph_slam/PrefilteringNodelet $(arg nodelet_manager)">
<remap from="/velodyne_points" to="$(arg points_topic)" />
<!-- in case base_link_frame is blank, mapping will be performed in the lidar frame -->
<param name="base_link_frame" value="base_link" />
<!-- distance filter -->
<param name="use_distance_filter" value="false" />
<param name="distance_near_thresh" value="0.1" />
<param name="distance_far_thresh" value="100.0" />
<!-- NONE, VOXELGRID, or APPROX_VOXELGRID -->
<param name="downsample_method" value="APPROX_VOXELGRID" />
<param name="downsample_resolution" value="0.1" />
<!-- NONE, RADIUS, or STATISTICAL -->
<param name="outlier_removal_method" value="RADIUS" />
<param name="statistical_mean_k" value="30" />
<param name="statistical_stddev" value="1.2" />
<param name="radius_radius" value="0.5" />
<param name="radius_min_neighbors" value="2" />
</node>

<!-- scan_matching_odometry_nodelet -->
<node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)">
<param name="points_topic" value="$(arg points_topic)" />
<param name="odom_frame_id" value="odom" />
<param name="keyframe_delta_trans" value="10.0" />
<param name="keyframe_delta_angle" value="10.0" />
<param name="keyframe_delta_time" value="10.0" />
<param name="transform_thresholding" value="false" />
<param name="max_acceptable_trans" value="1.0" />
<param name="max_acceptable_angle" value="1.0" />
<param name="downsample_method" value="NONE" />
<param name="downsample_resolution" value="0.1" />
<!-- ICP, GICP, NDT, GICP_OMP, or NDT_OMP(recommended) -->
<param name="registration_method" value="GICP" />
<param name="ndt_resolution" value="2.0" />
<param name="ndt_num_threads" value="0" />
<param name="ndt_nn_search_method" value="DIRECT7" />
</node>

<!-- floor_detection_nodelet -->
<node pkg="nodelet" type="nodelet" name="floor_detection_nodelet" args="load hdl_graph_slam/FloorDetectionNodelet $(arg nodelet_manager)" if="$(arg enable_floor_detection)">
<param name="points_topic" value="$(arg points_topic)" />
<param name="tilt_deg" value="0.0" />
<param name="sensor_height" value="2.0" />
<param name="height_clip_range" value="1.0" />
<param name="floor_pts_thresh" value="512" />
<param name="use_normal_filtering" value="true" />
<param name="normal_filter_thresh" value="20.0" />
</node>

<!-- hdl_graph_slam_nodelet -->
<node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
<param name="points_topic" value="$(arg points_topic)" />
<!-- frame settings -->
<param name="map_frame_id" value="map" />
<param name="odom_frame_id" value="odom" />
<!-- optimization params -->
<!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
<param name="g2o_solver_type" value="lm_var_cholmod" />
<param name="g2o_solver_num_iterations" value="512" />
<!-- keyframe registration params -->
<param name="enable_gps" value="$(arg enable_gps)" />
<param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
<param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
<param name="max_keyframes_per_update" value="10" />
<param name="keyframe_delta_trans" value="5.0" />
<param name="keyframe_delta_angle" value="4.0" />
<!-- loop closure params -->
<param name="distance_thresh" value="15.0" />
<param name="accum_distance_thresh" value="25.0" />
<param name="min_edge_interval" value="15.0" />
<param name="fitness_score_thresh" value="2.5" />
<!-- scan matching params -->
<param name="registration_method" value="GICP" />
<param name="ndt_resolution" value="1.0" />
<param name="ndt_num_threads" value="0" />
<param name="ndt_nn_search_method" value="DIRECT7" />
<!-- edge params -->
<!-- GPS -->
<param name="gps_edge_robust_kernel" value="NONE" />
<param name="gps_edge_robust_kernel_size" value="1.0" />
<param name="gps_edge_stddev_xy" value="20.0" />
<param name="gps_edge_stddev_z" value="5.0" />
<!-- IMU orientation -->
<param name="imu_orientation_edge_robust_kernel" value="NONE" />
<param name="imu_orientation_edge_stddev" value="1.0" />
<!-- IMU acceleration (gravity vector) -->
<param name="imu_acceleration_edge_robust_kernel" value="NONE" />
<param name="imu_acceleration_edge_stddev" value="1.0" />
<!-- ground plane -->
<param name="floor_edge_robust_kernel" value="NONE" />
<param name="floor_edge_stddev" value="10.0" />
<!-- scan matching -->
<!-- robust kernels: NONE, Cauchy, DCS, Fair, GemanMcClure, Huber, PseudoHuber, Saturated, Tukey, Welsch -->
<param name="fix_first_node" value="true" />
<param name="odometry_edge_robust_kernel" value="NONE" />
<param name="odometry_edge_robust_kernel_size" value="1.0" />
<param name="loop_closure_edge_robust_kernel" value="Huber" />
<param name="loop_closure_edge_robust_kernel_size" value="1.0" />
<param name="use_const_inf_matrix" value="false" />
<param name="const_stddev_x" value="0.5" />
<param name="const_stddev_q" value="0.1" />
<param name="var_gain_a" value="20.0" />
<param name="min_stddev_x" value="0.1" />
<param name="max_stddev_x" value="5.0" />
<param name="min_stddev_q" value="0.05" />
<param name="max_stddev_q" value="0.2" />
<!-- update params -->
<param name="graph_update_interval" value="3.0" />
<param name="map_cloud_update_interval" value="10.0" />
<param name="map_cloud_resolution" value="0.05" />
</node>

<node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher" />
</launch>
Loading

0 comments on commit e5f8b27

Please sign in to comment.