depth_to_pointcloud.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, 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 
35 
37 
38 
39 
41 {
42  // TODO Auto-generated constructor stub
43 
44 }
45 
47 {
48  // TODO Auto-generated destructor stub
49 }
50 
51 void DepthToPointCloud::initialize(sensor_msgs::ImageConstPtr depth_msg,
52  sensor_msgs::CameraInfoConstPtr camera_info_msg)
53 {
54  if (!depth_msg || !camera_info_msg)
55  {
56  return;
57  }
58 
59  std::size_t width = depth_msg->width;
60  std::size_t height = depth_msg->height;
61 
62  if (width != projection_map_x_.size() || height !=projection_map_y_.size())
63  {
64  // Procompute 3D projection matrix
65  //
66  // The following computation of center_x,y and fx,fy duplicates
67  // code in the image_geometry package, but this avoids dependency
68  // on OpenCV, which simplifies releasing rviz.
69 
70  // Use correct principal point from calibration
71  float center_x = camera_info_msg->P[2] - camera_info_msg->roi.x_offset;
72  float center_y = camera_info_msg->P[6] - camera_info_msg->roi.y_offset;
73 
74  // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
75  double scale_x = camera_info_msg->binning_x > 1 ? (1.0 / camera_info_msg->binning_x) : 1.0;
76  double scale_y = camera_info_msg->binning_y > 1 ? (1.0 / camera_info_msg->binning_y) : 1.0;
77 
78  double fx = camera_info_msg->P[0] * scale_x;
79  double fy = camera_info_msg->P[5] * scale_y;
80 
81  float constant_x = 1.0f / fx;
82  float constant_y = 1.0f / fy;
83 
84  projection_map_x_.resize(width);
85  projection_map_y_.resize(height);
86  std::vector<float>::iterator projX = projection_map_x_.begin();
87  std::vector<float>::iterator projY = projection_map_y_.begin();
88 
89  // precompute 3D projection matrix
90  for (int v = 0; v < height; ++v, ++projY)
91  *projY = (v - center_y) * constant_y;
92 
93  for (int u = 0; u < width; ++u, ++projX)
94  *projX = (u - center_x) * constant_x;
95  }
96 }
97 
std::vector< float > projection_map_x_
void initialize(sensor_msgs::ImageConstPtr depth_msg, sensor_msgs::CameraInfoConstPtr camera_info_msg)
std::vector< float > projection_map_y_


image_cb_detector
Author(s): Vijay Pradeep, Eitan Marder-Eppstein
autogenerated on Mon Jun 10 2019 12:57:16