fiducial_slam.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017-20, Ubiquity Robotics
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright notice,
11  * this list of conditions and the following disclaimer in the documentation
12  * and/or other materials provided with the distribution.
13  *
14  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
18  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
19  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
20  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
21  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
22  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
23  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
24  * POSSIBILITY OF SUCH DAMAGE.
25  *
26  * The views and conclusions contained in the software and documentation are
27  * those of the authors and should not be interpreted as representing official
28  * policies, either expressed or implied, of the FreeBSD Project.
29  *
30  */
31 
32 #include <fiducial_slam/helpers.h>
33 
34 #include <assert.h>
35 #include <signal.h>
36 #include <sys/time.h>
37 #include <unistd.h>
38 
39 #include <ros/ros.h>
40 #include <tf/transform_datatypes.h>
43 #include <tf2_ros/buffer.h>
46 #include <visualization_msgs/Marker.h>
47 
48 #include "fiducial_msgs/Fiducial.h"
49 #include "fiducial_msgs/FiducialArray.h"
50 #include "fiducial_msgs/FiducialTransform.h"
51 #include "fiducial_msgs/FiducialTransformArray.h"
52 
53 #include "fiducial_slam/map.h"
54 
55 #include <opencv2/calib3d.hpp>
56 #include <opencv2/highgui.hpp>
57 
58 #include <list>
59 #include <string>
60 
61 using namespace std;
62 using namespace cv;
63 
64 class FiducialSlam {
65 private:
67 
71 
74 
75  void transformCallback(const fiducial_msgs::FiducialTransformArray::ConstPtr &msg);
76 
77 public:
80 };
81 
82 void FiducialSlam::transformCallback(const fiducial_msgs::FiducialTransformArray::ConstPtr &msg) {
83  vector<Observation> observations;
84 
85  for (size_t i = 0; i < msg->transforms.size(); i++) {
86  const fiducial_msgs::FiducialTransform &ft = msg->transforms[i];
87 
88  tf2::Vector3 tvec(ft.transform.translation.x, ft.transform.translation.y,
89  ft.transform.translation.z);
90 
91  tf2::Quaternion q(ft.transform.rotation.x, ft.transform.rotation.y, ft.transform.rotation.z,
92  ft.transform.rotation.w);
93 
94  double variance;
95  if (use_fiducial_area_as_weight) {
96  variance = weighting_scale / ft.fiducial_area;
97  } else {
98  variance = weighting_scale * ft.object_error;
99  }
100 
102  TransformWithVariance(ft.transform, variance),
103  msg->header.stamp, msg->header.frame_id));
104  observations.push_back(obs);
105  }
106 
107  fiducialMap.update(observations, msg->header.stamp);
108 }
109 
111  bool doPoseEstimation;
112 
113  // If set, use the fiducial area in pixels^2 as an indication of the
114  // 'goodness' of it. This will favor fiducials that are close to the
115  // camera and center of the image. The reciprical of the area is actually
116  // used, in place of reprojection error as the estimate's variance
117  nh.param<bool>("use_fiducial_area_as_weight", use_fiducial_area_as_weight, false);
118  // Scaling factor for weighing
119  nh.param<double>("weighting_scale", weighting_scale, 1e9);
120 
121  nh.param("do_pose_estimation", doPoseEstimation, false);
122 
123  if (doPoseEstimation) {
124  double fiducialLen, errorThreshold;
125  nh.param<double>("fiducial_len", fiducialLen, 0.14);
126  nh.param<double>("pose_error_theshold", errorThreshold, 1.0);
127 
129  nh.advertise<fiducial_msgs::FiducialTransformArray>("/fiducial_transforms", 1));
130  } else {
131  ft_sub = nh.subscribe("/fiducial_transforms", 1, &FiducialSlam::transformCallback, this);
132  }
133 
134  ROS_INFO("Fiducial Slam ready");
135 }
136 
137 auto node = unique_ptr<FiducialSlam>(nullptr);
138 
139 void mySigintHandler(int sig) {
140  if (node != nullptr) node->fiducialMap.saveMap();
141 
142  ros::shutdown();
143 }
144 
145 int main(int argc, char **argv) {
146  ros::init(argc, argv, "fiducial_slam", ros::init_options::NoSigintHandler);
147  ros::NodeHandle nh("~");
148 
149  node = make_unique<FiducialSlam>(nh);
150  signal(SIGINT, mySigintHandler);
151 
152  ros::Rate r(20);
153  while (ros::ok()) {
154  ros::spinOnce();
155  r.sleep();
156  node->fiducialMap.update();
157  }
158 
159  return 0;
160 }
ros::Subscriber verticesSub
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool use_fiducial_area_as_weight
ros::Publisher ftPub
double weighting_scale
ros::Subscriber cameraInfoSub
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
FiducialSlam(ros::NodeHandle &nh)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
auto node
bool sleep()
void mySigintHandler(int sig)
int main(int argc, char **argv)
ROSCPP_DECL void shutdown()
ROSCPP_DECL void spinOnce()
ros::Subscriber ft_sub
void transformCallback(const fiducial_msgs::FiducialTransformArray::ConstPtr &msg)


fiducial_slam
Author(s): Jim Vaughan
autogenerated on Fri May 1 2020 04:04:05