Skip to content

Commit 41833d7

Browse files
xingyc15xingyc15
committed
Add omnimapper_visualizer_pcl
1 parent 19ecf50 commit 41833d7

File tree

3 files changed

+350
-0
lines changed

3 files changed

+350
-0
lines changed

‎CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@ set (library_srcs
3232
src/BoundedPlaneFactor.cpp
3333
src/plane_factor.cpp
3434
src/plane.cpp
35+
src/omnimapper_visualizer_pcl
3536
)
3637

3738
set (plugins_srcs

‎include/omnimapper/omnimapper_visualizer_pcl.h

Lines changed: 95 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
2+
/*
3+
* Software License Agreement (BSD License)
4+
*
5+
* OmniMapper
6+
* Copyright (c) 2012-, Georgia Tech Research Corporation,
7+
* Atlanta, Georgia 30332-0415
8+
*
9+
* All rights reserved.
10+
*
11+
* Redistribution and use in source and binary forms, with or without
12+
* modification, are permitted provided that the following conditions
13+
* are met:
14+
*
15+
* * Redistributions of source code must retain the above copyright
16+
* notice, this list of conditions and the following disclaimer.
17+
* * Redistributions in binary form must reproduce the above
18+
* copyright notice, this list of conditions and the following
19+
* disclaimer in the documentation and/or other materials provided
20+
* with the distribution.
21+
* * Neither the name of the copyright holder(s) nor the names of its
22+
* contributors may be used to endorse or promote products derived
23+
* from this software without specific prior written permission.
24+
*
25+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36+
* POSSIBILITY OF SUCH DAMAGE.
37+
*
38+
*/
39+
40+
#include <omnimapper/omnimapper_base.h>
41+
#include <omnimapper/plugins/icp_plugin.h>
42+
#include <omnimapper/plane.h>
43+
#include <pcl/visualization/pcl_visualizer.h>
44+
45+
namespace omnimapper
46+
{
47+
48+
/** \brief OmniMapperVisualizerPCL is an output plugin for OmniMapper based on the PCLVisualizer. Currently
49+
* this only supports visualization of a trajectory, and clouds from an ICP plugin.
50+
*
51+
* \author Alex Trevor
52+
*/
53+
template <typename PointT>
54+
class OmniMapperVisualizerPCL : public omnimapper::OutputPlugin
55+
{
56+
//typedef typename pcl::PointXYZ PointT;
57+
typedef typename pcl::PointCloud<PointT> Cloud;
58+
typedef typename Cloud::Ptr CloudPtr;
59+
typedef typename Cloud::ConstPtr CloudConstPtr;
60+
61+
public:
62+
OmniMapperVisualizerPCL (omnimapper::OmniMapperBase* mapper);
63+
void update (boost::shared_ptr<gtsam::Values>& vis_values, boost::shared_ptr<gtsam::NonlinearFactorGraph>& vis_graph);
64+
void spin ();
65+
void spinThread ();
66+
void spinOnce ();
67+
void setICPPlugin (boost::shared_ptr<omnimapper::ICPPoseMeasurementPlugin<PointT> >& icp_plugin) { icp_plugin_ = icp_plugin; }
68+
void keyboardCallback (const pcl::visualization::KeyboardEvent& event, void*);
69+
70+
//void spinAndUpdate ();
71+
protected:
72+
// A PCL Visualizer
73+
pcl::visualization::PCLVisualizer viewer_;
74+
// Visualizer mutex
75+
boost::mutex vis_mutex_;
76+
// A reference to a mapper instance
77+
OmniMapperBase* mapper_;
78+
79+
// Pose Cloud
80+
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > pose_cloud_;
81+
// Updated flag
82+
bool new_slam_data_;
83+
// Flag for drawing ICP clouds
84+
bool draw_icp_clouds_;
85+
// Flag for drawing planar boundaries
86+
bool draw_planar_boundaries_;
87+
// Flag for drawing planar normals
88+
bool draw_planar_normals_;
89+
// ICP Plugin Ref
90+
boost::shared_ptr<omnimapper::ICPPoseMeasurementPlugin<PointT> > icp_plugin_;
91+
92+
// Debug flag
93+
bool debug_;
94+
};
95+
}

‎src/omnimapper_visualizer_pcl.cpp

Lines changed: 254 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,254 @@
1+
/*
2+
* Software License Agreement (BSD License)
3+
*
4+
* OmniMapper
5+
* Copyright (c) 2012-, Georgia Tech Research Corporation,
6+
* Atlanta, Georgia 30332-0415
7+
*
8+
* All rights reserved.
9+
*
10+
* Redistribution and use in source and binary forms, with or without
11+
* modification, are permitted provided that the following conditions
12+
* are met:
13+
*
14+
* * Redistributions of source code must retain the above copyright
15+
* notice, this list of conditions and the following disclaimer.
16+
* * Redistributions in binary form must reproduce the above
17+
* copyright notice, this list of conditions and the following
18+
* disclaimer in the documentation and/or other materials provided
19+
* with the distribution.
20+
* * Neither the name of the copyright holder(s) nor the names of its
21+
* contributors may be used to endorse or promote products derived
22+
* from this software without specific prior written permission.
23+
*
24+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35+
* POSSIBILITY OF SUCH DAMAGE.
36+
*
37+
*/
38+
39+
#include <omnimapper/omnimapper_visualizer_pcl.h>
40+
#include <pcl/common/transforms.h>
41+
42+
template <typename PointT>
43+
omnimapper::OmniMapperVisualizerPCL<PointT>::OmniMapperVisualizerPCL (omnimapper::OmniMapperBase* mapper)
44+
: mapper_ (mapper),
45+
viewer_ ("OmniMapperVisualizerPCL"),
46+
pose_cloud_ (new pcl::PointCloud<pcl::PointXYZ> ()),
47+
new_slam_data_ (false),
48+
draw_icp_clouds_ (false),
49+
draw_planar_boundaries_ (true),
50+
draw_planar_normals_ (true)
51+
{
52+
// Set up the viewer
53+
viewer_.setBackgroundColor (0, 0, 0);
54+
viewer_.addCoordinateSystem (1.0, 0);
55+
viewer_.initCameraParameters ();
56+
viewer_.registerKeyboardCallback (&omnimapper::OmniMapperVisualizerPCL<PointT>::keyboardCallback, *this, 0);
57+
debug_ = true;
58+
}
59+
60+
template <typename PointT> void
61+
omnimapper::OmniMapperVisualizerPCL<PointT>::update (boost::shared_ptr<gtsam::Values>& vis_values, boost::shared_ptr<gtsam::NonlinearFactorGraph>& vis_graph)
62+
{
63+
// Pull poses from the mapper
64+
//gtsam::Values current_solution = mapper_->getSolution ();
65+
gtsam::Values current_solution = *vis_values;
66+
67+
// Draw the poses
68+
pcl::PointCloud<pcl::PointXYZ>::Ptr poses_cloud (new pcl::PointCloud<pcl::PointXYZ>);
69+
70+
// Draw the cloud
71+
CloudPtr aggregate_cloud (new Cloud ());
72+
73+
// Draw the planes
74+
CloudPtr plane_boundary_cloud (new Cloud ());
75+
76+
gtsam::Values::ConstFiltered<gtsam::Pose3> pose_filtered = current_solution.filter<gtsam::Pose3>();
77+
BOOST_FOREACH (const gtsam::Values::ConstFiltered<gtsam::Pose3>::KeyValuePair& key_value, pose_filtered)
78+
{
79+
gtsam::Symbol key_symbol (key_value.key);
80+
gtsam::Pose3 pose = key_value.value;
81+
gtsam::Rot3 rot = pose.rotation ();
82+
83+
pcl::PointXYZ pose_pt (pose.x (), pose.y (), pose.z ());
84+
poses_cloud->push_back (pose_pt);
85+
86+
// Draw the clouds
87+
if (draw_icp_clouds_)
88+
{
89+
printf ("About to request frame cloud\n");
90+
CloudConstPtr frame_cloud = icp_plugin_->getCloudPtr (key_symbol);
91+
printf ("Frame cloud has %d\n", frame_cloud->points.size ());
92+
char frame_name[1024];
93+
CloudPtr map_cloud (new Cloud ());
94+
Eigen::Matrix4f map_tform = pose.matrix ().cast<float>();
95+
pose.print ("SAM Pose: ");
96+
std::cout << "Map Tform: " << map_tform << std::endl;
97+
pcl::transformPointCloud (*frame_cloud, *map_cloud, map_tform);
98+
sprintf (frame_name, "x_%d", key_symbol.index ());
99+
printf ("name: x_%d\n",key_symbol.index ());
100+
(*aggregate_cloud) += (*map_cloud);
101+
//if (!viewer_.updatePointCloud (map_cloud, frame_name))
102+
// viewer_.addPointCloud (map_cloud, frame_name);
103+
//viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.05, frame_name);
104+
}
105+
106+
}
107+
108+
109+
// Draw planar Landmarks
110+
if (draw_planar_normals_)
111+
{
112+
boost::lock_guard<boost::mutex> lock (vis_mutex_);
113+
114+
gtsam::Values::Filtered<gtsam::Plane<PointT> > plane_filtered = current_solution.filter<gtsam::Plane<PointT> >();
115+
printf ("Visualizing %d planes!\n", plane_filtered.size ());
116+
int plane_num = 0;
117+
BOOST_FOREACH (const typename gtsam::Values::Filtered<gtsam::Plane<PointT> >::KeyValuePair& key_value, plane_filtered)
118+
{
119+
// Get the hull, put it in the map frame
120+
Cloud lm_cloud = key_value.value.hull ();
121+
(*plane_boundary_cloud) += lm_cloud;
122+
123+
// Draw the normal vector
124+
Eigen::Vector4f centroid;
125+
pcl::compute3DCentroid (lm_cloud, centroid);
126+
pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
127+
pcl::PointXYZ pt2 = pcl::PointXYZ ((centroid[0] + key_value.value.a ()),
128+
(centroid[1] + key_value.value.b ()),
129+
(centroid[2] + key_value.value.c ()));
130+
char normal_name[2048];
131+
sprintf (normal_name, "plane_%d_normal", plane_num);
132+
viewer_.removeShape (normal_name);
133+
viewer_.addArrow (pt2, pt1, 1.0, 0.0, 0.0, false, normal_name);
134+
++plane_num;
135+
}
136+
}
137+
138+
139+
{
140+
//boost::mutex::scoped_lock (vis_mutex_);
141+
boost::lock_guard<boost::mutex> lock (vis_mutex_);
142+
143+
if (debug_)
144+
printf ("Visualizer updating with %d poses\n", poses_cloud->points.size ());
145+
146+
if (poses_cloud->points.size () > 0)
147+
{
148+
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color (poses_cloud, 0, 255, 0);
149+
if (!viewer_.updatePointCloud (poses_cloud, color, "poses_cloud"))
150+
viewer_.addPointCloud (poses_cloud, color, "poses_cloud");
151+
viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "poses_cloud");
152+
}
153+
154+
if (draw_icp_clouds_)
155+
{
156+
if (!viewer_.updatePointCloud (aggregate_cloud, "map_cloud"))
157+
viewer_.addPointCloud (aggregate_cloud, "map_cloud");
158+
viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.1, "map_cloud");
159+
}
160+
161+
if (draw_planar_boundaries_)
162+
{
163+
if (!viewer_.updatePointCloud (plane_boundary_cloud, "plane_cloud"))
164+
viewer_.addPointCloud (plane_boundary_cloud, "plane_cloud");
165+
viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 8, "plane_cloud");
166+
//viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.1, "plane_cloud");
167+
}
168+
169+
//pose_cloud_ = poses_cloud;
170+
new_slam_data_ = true;
171+
draw_icp_clouds_ = false;
172+
}
173+
}
174+
175+
template <typename PointT> void
176+
omnimapper::OmniMapperVisualizerPCL<PointT>::keyboardCallback (const pcl::visualization::KeyboardEvent& event, void*)
177+
{
178+
if (event.keyUp ())
179+
{
180+
switch (event.getKeyCode ())
181+
{
182+
case 'c':
183+
case 'C':
184+
//Draw Clouds
185+
draw_icp_clouds_ = true;
186+
break;
187+
case 'p':
188+
icp_plugin_->pause (true);
189+
break;
190+
case ' ':
191+
icp_plugin_->pause (false);
192+
break;
193+
case 'o':
194+
mapper_->printSolution ();
195+
exit (1);
196+
break;
197+
}
198+
199+
}
200+
201+
}
202+
203+
template <typename PointT> void
204+
omnimapper::OmniMapperVisualizerPCL<PointT>::spinOnce ()
205+
{
206+
//boost::lock_guard<boost::mutex> lock (vis_mutex_);
207+
if (vis_mutex_.try_lock ())
208+
{
209+
viewer_.spinOnce ();
210+
vis_mutex_.unlock ();
211+
}
212+
}
213+
214+
template <typename PointT> void
215+
omnimapper::OmniMapperVisualizerPCL<PointT>::spin ()
216+
{
217+
boost::thread (&omnimapper::OmniMapperVisualizerPCL<PointT>::spinThread, this);
218+
}
219+
220+
template <typename PointT> void
221+
omnimapper::OmniMapperVisualizerPCL<PointT>::spinThread ()
222+
{
223+
while (!viewer_.wasStopped ())
224+
{
225+
viewer_.spinOnce (100);
226+
227+
if (vis_mutex_.try_lock ())
228+
{
229+
if (new_slam_data_)
230+
{
231+
printf ("Updating vis\n");
232+
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color (pose_cloud_, 0, 255, 0);
233+
if (!viewer_.updatePointCloud (pose_cloud_, color, "poses_cloud"))
234+
viewer_.addPointCloud (pose_cloud_, color, "poses_cloud");
235+
viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "poses_cloud");
236+
//viewer_.spinOnce ();
237+
new_slam_data_ = false;
238+
}
239+
vis_mutex_.unlock ();
240+
} else
241+
{
242+
if (debug_)
243+
printf ("Vis spin thread: Couldn't get lock!\n");
244+
}
245+
246+
boost::this_thread::sleep (boost::posix_time::milliseconds (10));
247+
248+
}
249+
250+
}
251+
252+
// TODO: Instantiation macros.
253+
template class omnimapper::OmniMapperVisualizerPCL<pcl::PointXYZ>;
254+
template class omnimapper::OmniMapperVisualizerPCL<pcl::PointXYZRGBA>;

0 commit comments

Comments
 (0)