source: pacpussensors/trunk/Vislab/lib3dv-1.2.0/lib3dv/ipm.cc

Last change on this file was 136, checked in by ldecherf, 8 years ago

Doc

File size: 6.3 KB
Line 
1/* ipm.cc
2 *
3 * Copyright (C) 2013 VisLab
4 *
5 * This file is part of lib3dv; you can redistribute it and/or modify
6 * it under the terms of the GNU Lesser General Public License as published by
7 * the Free Software Foundation; either version 3 of the License, or (at
8 * your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 * General Public License for more details.
14 *
15 * You should have received a copy of the GNU Lesser General Public License
16 * along with this program; if not, see <http://www.gnu.org/licenses/>.
17 */
18
19#include "lib3dv/image.h"
20#include "lib3dv/ipm.h"
21
22#include <cmath>
23#include <iostream>
24#include <limits>
25
26lib3dv::inverse_perspective_mapping::inverse_perspective_mapping(const calibration::camera_intrinsics& intrinsics, float baseline) :
27 m_intrinsics(intrinsics),
28 m_baseline(baseline),
29 m_transformation_matrix(NULL)
30{}
31
32lib3dv::inverse_perspective_mapping::inverse_perspective_mapping(const calibration::camera_intrinsics& intrinsics, const calibration::position& position, const calibration::orientation& orientation, float baseline) :
33 m_intrinsics(intrinsics),
34 m_baseline(baseline)
35{
36 m_transformation_matrix = new float[12];
37
38 const float u0 = intrinsics.m_u0;
39 const float v0 = intrinsics.m_v0;
40 const float ku = intrinsics.m_ku;
41 const float kv = intrinsics.m_kv;
42
43 const float x = position.m_x;
44 const float y = position.m_y;
45 const float z = position.m_z;
46
47 const float yaw = orientation.m_yaw;
48 const float pitch = orientation.m_pitch;
49 const float roll = orientation.m_roll;
50
51 m_transformation_matrix[0] = cos(pitch) * cos(yaw);
52 m_transformation_matrix[1] = sin(roll) * sin(pitch) * cos(yaw) - cos(roll) * sin(yaw);
53 m_transformation_matrix[2] = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * sin(yaw);
54
55 m_transformation_matrix[4] = cos(pitch) * sin(yaw);
56 m_transformation_matrix[5] = sin(roll) * sin(pitch) * sin(yaw) + cos(roll) * cos(yaw);
57 m_transformation_matrix[6] = cos(roll) * sin(pitch) * sin(yaw) - sin(roll) * cos(yaw);
58
59 m_transformation_matrix[8] = -sin(pitch);
60 m_transformation_matrix[9] = sin(roll) * cos(pitch);
61 m_transformation_matrix[10] = cos(roll) * cos(pitch);
62
63 m_transformation_matrix[3] = x;
64 m_transformation_matrix[7] = y;
65 m_transformation_matrix[11] = z;
66
67 m_transformation_matrix[0] *= ku * baseline;
68 m_transformation_matrix[1] *= -baseline;
69 m_transformation_matrix[2] *= -baseline * ku / kv;
70 m_transformation_matrix[4] *= ku * baseline;
71 m_transformation_matrix[5] *= -baseline;
72 m_transformation_matrix[6] *= -baseline * ku / kv;
73 m_transformation_matrix[8] *= ku * baseline;
74 m_transformation_matrix[9] *= -baseline;
75 m_transformation_matrix[10] *= -baseline * ku / kv;
76}
77
78lib3dv::inverse_perspective_mapping::~inverse_perspective_mapping()
79{
80 delete[] m_transformation_matrix;
81}
82
83bool lib3dv::inverse_perspective_mapping::operator()(boost::shared_ptr<const lib3dv::image> dsi, std::vector<lib3dv::point3>& world_points)
84{
85 if(dsi->m_type != lib3dv::image::type::DSI || dsi->m_format != lib3dv::image::format::MONO16 || dsi->m_bpp != 16 || dsi->m_channels != 1)
86 {
87 std::cerr << "[EE] lib3dv : invalid DSI supplied" << std::endl;
88 return false;
89 }
90
91 world_points.reserve(world_points.size() + dsi->m_width * dsi->m_height);
92 const uint16_t* buffer = reinterpret_cast<const uint16_t*>(dsi->m_buffer.data());
93
94 if(!m_transformation_matrix)
95 {
96 const float u0 = m_intrinsics.m_u0;
97 const float v0 = m_intrinsics.m_v0;
98 const float ku = m_intrinsics.m_ku;
99 const float kv = m_intrinsics.m_kv;
100
101 const float ku_b = ku * m_baseline;
102 const float inv_kv = 1.0f / kv;
103 const float ku_b_inv_kv = ku_b * inv_kv;
104
105 for(unsigned int i = 0; i < dsi->m_height; ++i)
106 {
107 const float iv0_ku_b_inv_kv = (i - v0) * ku_b_inv_kv;
108
109 for(unsigned int j = 0; j < dsi->m_width; ++j)
110 {
111 const uint16_t raw_disparity = buffer[i * dsi->m_width + j];
112
113 lib3dv::point3 point;
114
115 if(raw_disparity != 0xFFFF)
116 {
117 const float inv_disparity = 256.0f / raw_disparity;
118
119 point.m_x = (j - u0) * m_baseline * inv_disparity;
120 point.m_y = iv0_ku_b_inv_kv * inv_disparity;
121 point.m_z = ku_b * inv_disparity;
122 }
123 else
124 {
125 point.m_x = 0;
126 point.m_y = 0;
127 point.m_z = std::numeric_limits<float>::max();
128 }
129
130 world_points.push_back(point);
131 }
132 }
133 }
134 else
135 {
136 for(unsigned int i = 0; i < dsi->m_height; ++i)
137 {
138 for(unsigned int j = 0; j < dsi->m_width; ++j)
139 {
140 const uint16_t raw_disparity = buffer[i * dsi->m_width + j];
141
142 point3 point;
143
144 if(raw_disparity != 0xFFFF)
145 {
146 const float inv_disparity = 256.0f / raw_disparity;
147
148 const float x = inv_disparity;
149 const float y = (j - m_intrinsics.m_u0) * inv_disparity;
150 const float z = (i - m_intrinsics.m_v0) * inv_disparity;
151
152 point.m_x = m_transformation_matrix[0] * x + m_transformation_matrix[1] * y + m_transformation_matrix[2] * z + m_transformation_matrix[3];
153 point.m_y = m_transformation_matrix[4] * x + m_transformation_matrix[5] * y + m_transformation_matrix[6] * z + m_transformation_matrix[7];
154 point.m_z = m_transformation_matrix[8] * x + m_transformation_matrix[9] * y + m_transformation_matrix[10] * z + m_transformation_matrix[11];
155 }
156 else
157 {
158 point.m_x = std::numeric_limits<float>::max();
159 point.m_y = 0;
160 point.m_z = 0;
161 }
162
163 world_points.push_back(point);
164 }
165 }
166 }
167
168 return true;
169}
Note: See TracBrowser for help on using the repository browser.