monocam_settler.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2009, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
36 
39 
40 using namespace monocam_settler;
41 
42 
44 {
45  configured_ = false;
46 }
47 
48 bool MonocamSettler::configure(const monocam_settler::ConfigGoal& goal)
49 {
50  tol_ = goal.tolerance;
51  max_step_ = goal.max_step;
52  ignore_failures_ = goal.ignore_failures;
53  cache_.clear();
54  cache_.setMaxSize(goal.cache_size);
55 
56  ROS_DEBUG("Configuring MonocamSettler with tolerance of [%.3f]", tol_);
57 
58  configured_ = true;
59  return true;
60 }
61 
62 
63 bool MonocamSettler::add(const calibration_msgs::CalibrationPatternConstPtr msg,
64  calibration_msgs::Interval& interval)
65 {
66  if (!configured_)
67  {
68  ROS_WARN("Not configured. Going to skip");
69  return false;
70  }
71 
72  // Check if detection failed
73  if (!msg->success)
74  {
75  if(!ignore_failures_) // If we care about failures then we should reset the cache
76  cache_.clear();
77  return false;
78  }
79 
81  deflate(msg, *deflated);
82  cache_.add(deflated);
83 
84  DeflatedCache* bare_cache = (DeflatedCache*)(&cache_);
85 
86  std::vector<double> tol_vec(deflated->channels_.size(), tol_);
87 
88  interval = settlerlib::IntervalCalc::computeLatestInterval(*bare_cache, tol_vec, max_step_);
89 
90  return true;
91 }
92 
93 void MonocamSettler::deflate(const calibration_msgs::CalibrationPatternConstPtr& msg,
95 {
96  deflated.header.stamp = msg->header.stamp;
97 
98  const unsigned int N = msg->image_points.size();
99  deflated.channels_.resize( 2 * N );
100  for (unsigned int i=0; i<N; i++)
101  {
102  deflated.channels_[2*i+0] = msg->image_points[i].x;
103  deflated.channels_[2*i+1] = msg->image_points[i].y;
104  }
105  deflated.msg_ = msg;
106 }
bool add(const calibration_msgs::CalibrationPatternConstPtr msg, calibration_msgs::Interval &interval)
calibration_msgs::CalibrationPatternConstPtr msg_
static const unsigned int N
void setMaxSize(unsigned int max_size)
void deflate(const calibration_msgs::CalibrationPatternConstPtr &image_features, DeflatedCalibrationPattern &deflated)
#define ROS_WARN(...)
bool configure(const monocam_settler::ConfigGoal &goal)
std_msgs::Header header
std::vector< double > channels_
void add(const M &msg)
static calibration_msgs::Interval computeLatestInterval(const SortedDeque< DeflatedConstPtr > &signal, const std::vector< double > &tolerances, ros::Duration max_spacing)
#define ROS_DEBUG(...)


monocam_settler
Author(s): Vijay Pradeep
autogenerated on Fri Apr 2 2021 02:13:05