PrimitiveFilter.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX 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
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package VisionX
19  * @author Markus Grotz ( markus dot grotz at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "PrimitiveFilter.h"
26 
27 #include <ArmarXCore/core/logging/Logging.h>
28 
29 
30 using namespace armarx;
31 
32 PrimitiveFilter::PrimitiveFilter()
33 {
34 }
35 
36 PrimitiveFilter::~PrimitiveFilter()
37 {
38 
39 }
40 
41 
42 void PrimitiveFilter::getPrimitivesInFieldOfView(std::vector<memoryx::EnvironmentalPrimitiveBasePtr> allPrimitives, std::vector<memoryx::EnvironmentalPrimitiveBasePtr>& visibilePrimitives, std::vector<memoryx::EnvironmentalPrimitiveBasePtr>& otherPrimitives)
43 {
44  for (memoryx::EnvironmentalPrimitiveBasePtr& p : allPrimitives)
45  {
46  if (!p->getPose())
47  {
48  ARMARX_WARNING << "no pose for primitive " << p->getId();
49  continue;
50  }
51 
52  Eigen::Matrix4f pose = FramedPosePtr::dynamicCast(p->getPose())->toEigen();
53 
54  Eigen::Vector3f OBBDimensions = Vector3Ptr::dynamicCast(p->getOBBDimensions())->toEigen();
55 
56  if (intersectsFrustum(pose, OBBDimensions))
57  {
58  visibilePrimitives.push_back(p);
59  }
60  else
61  {
62  otherPrimitives.push_back(p);
63  }
64  }
65 }
66 
67 void PrimitiveFilter::visualizeFrustum(DebugDrawerInterfacePrx debugDrawerTopicPrx)
68 {
69 
70  Eigen::Vector3f nearTopRight = frustum.col(0);
71  Eigen::Vector3f nearTopLeft = frustum.col(1);
72  Eigen::Vector3f farTopLeft = frustum.col(2);
73  Eigen::Vector3f nearBottomLeft = frustum.col(3);
74  Eigen::Vector3f nearBottomRight = frustum.col(4);
75  Eigen::Vector3f farBottomRight = frustum.col(5);
76  Eigen::Vector3f farTopRight = frustum.col(15);
77  Eigen::Vector3f farBottomLeft = frustum.col(17);
78 
79  Vector3BasePtr start = new Vector3(nearTopLeft);
80  Vector3BasePtr stop = new Vector3(nearBottomLeft);
81  DrawColor color;
82  debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumNearLeft", start, stop, 1.0, color);
83 
84  start = new Vector3(farTopLeft);
85  stop = new Vector3(farBottomLeft);
86  debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumFarLeft", start, stop, 1.0, color);
87 
88  start = new Vector3(nearTopRight);
89  stop = new Vector3(nearBottomRight);
90  debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumNearRight", start, stop, 1.0, color);
91 
92  start = new Vector3(farTopRight);
93  stop = new Vector3(farBottomRight);
94  debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumFarRight", start, stop, 1.0, color);
95 
96  start = new Vector3(nearBottomLeft);
97  stop = new Vector3(nearBottomRight);
98  debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumNearBottom", start, stop, 1.0, color);
99 
100  start = new Vector3(farBottomLeft);
101  stop = new Vector3(farBottomRight);
102  debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumFarBottom", start, stop, 1.0, color);
103 
104  start = new Vector3(nearBottomRight);
105  stop = new Vector3(farBottomRight);
106  debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumRightBottom", start, stop, 1.0, color);
107 
108  start = new Vector3(nearBottomLeft);
109  stop = new Vector3(farBottomLeft);
110  debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumLeftBottom", start, stop, 1.0, color);
111 
112  start = new Vector3(nearTopRight);
113  stop = new Vector3(farTopRight);
114  debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumRightTop", start, stop, 1.0, color);
115 
116  start = new Vector3(nearTopLeft);
117  stop = new Vector3(farTopLeft);
118  debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumLeftTop", start, stop, 1.0, color);
119 }
120 
121 
122 
123 /**
124  * adapted from
125  * https://github.com/vanderlin/FrustumCulling/
126  */
127 Eigen::Matrix3Xf PrimitiveFilter::updateCameraPosition(Eigen::Matrix4f cameraToWorld)
128 {
129  float imageSizeRatio = 640.0 / 480.0;
130 
131  float farClipDistance = 3000.0;
132  float nearClipDistance = 400.0;
133 
134  Eigen::Vector3f farCenter(0, 0, farClipDistance);
135  Eigen::Vector3f nearCenter(0, 0, nearClipDistance);
136 
137  float a = -1.0 * std::tan(0.3926990925);
138 
139  Eigen::Vector3f farVertical(0, a * farClipDistance, 0);
140  Eigen::Vector3f farHorizontal(a * farClipDistance * imageSizeRatio, 0, 0);
141 
142  Eigen::Vector3f farTopLeft = farCenter - farVertical - farHorizontal;
143  Eigen::Vector3f farTopRight = farCenter - farVertical + farHorizontal;
144 
145  Eigen::Vector3f farBottomLeft = farCenter + farVertical - farHorizontal;
146  Eigen::Vector3f farBottomRight = farCenter + farVertical + farHorizontal;
147 
148  Eigen::Vector3f nearVertical(0, a * nearClipDistance, 0);
149  Eigen::Vector3f nearHorizontal(a * nearClipDistance * imageSizeRatio, 0, 0);
150 
151  Eigen::Vector3f nearTopLeft = nearCenter - nearVertical - nearHorizontal;
152  Eigen::Vector3f nearTopRight = nearCenter - nearVertical + nearHorizontal;
153 
154  Eigen::Vector3f nearBottomLeft = nearCenter + nearVertical - nearHorizontal;
155  Eigen::Vector3f nearBottomRight = nearCenter + nearVertical + nearHorizontal;
156 
157 
158  frustum.col(0) = nearTopRight;
159  frustum.col(1) = nearTopLeft;
160  frustum.col(2) = farTopLeft;
161 
162  frustum.col(3) = nearBottomLeft;
163  frustum.col(4) = nearBottomRight;
164  frustum.col(5) = farBottomRight;
165 
166  frustum.col(6) = nearTopLeft;
167  frustum.col(7) = nearBottomLeft;
168  frustum.col(8) = farBottomLeft;
169 
170  frustum.col(9) = nearBottomRight;
171  frustum.col(10) = nearTopRight;
172  frustum.col(11) = farBottomRight;
173 
174  frustum.col(12) = nearTopLeft;
175  frustum.col(13) = nearTopRight;
176  frustum.col(14) = nearBottomRight;
177 
178  frustum.col(15) = farTopRight;
179  frustum.col(16) = farTopLeft;
180  frustum.col(17) = farBottomLeft;
181 
182 
183  Eigen::Affine3f transform = Eigen::Affine3f::Identity();
184  transform *= Eigen::AngleAxisf(1.0 * M_PI, Eigen::Vector3f::UnitZ());
185 
186 
187  for (int i = 0; i < 18; i++)
188  {
189  Eigen::Vector4f temp = Eigen::Vector4f::Zero();
190 
191  temp.head<3>() = frustum.col(i);
192 
193  frustum.col(i) = cameraToWorld.block<3, 3>(0, 0) * transform.matrix().block<3, 3>(0, 0) * frustum.col(i);
194  frustum.col(i) += cameraToWorld.col(3).head<3>();
195  }
196 
197  return frustum;
198 }
199 
200 bool PrimitiveFilter::intersectsFrustum(Eigen::Matrix4f pose, Eigen::Vector3f extent)
201 {
202  Eigen::Vector3f a = pose.block<3, 3>(0, 0) * extent;
203 
204  Eigen::Matrix3Xf points(3, 8);
205  points.col(0) = Eigen::Vector3f(a(0), a(1), a(2));
206  points.col(1) = Eigen::Vector3f(a(0), a(1), -a(2));
207  points.col(2) = Eigen::Vector3f(a(0), -a(1), a(2));
208  points.col(3) = Eigen::Vector3f(a(0), -a(1), -a(2));
209  points.col(4) = Eigen::Vector3f(-a(0), a(1), a(2));
210  points.col(5) = Eigen::Vector3f(-a(0), a(1), -a(2));
211  points.col(6) = Eigen::Vector3f(-a(0), -a(1), a(2));
212  points.col(7) = Eigen::Vector3f(-a(0), -a(1), -a(2));
213 
214  for (int i = 0; i < 8; i ++)
215  {
216  Eigen::Vector3f point = pose.col(3).head<3>() + points.col(i);
217  if (isPointInFrustum(point))
218  {
219  return true;
220  }
221  }
222 
223  return false;
224 }
225 
226 
227 bool PrimitiveFilter::isPointInFrustum(Eigen::Vector3f point)
228 {
229  for (int i = 0; i < 18; i += 3)
230  {
231  Eigen::Vector3f u = frustum.col(i) - frustum.col(i + 1);
232  Eigen::Vector3f v = frustum.col(i) - frustum.col(i + 2);
233  Eigen::Vector3f normal = u.cross(v);
234  float dist = -1 * frustum.col(i).dot(normal);
235  if (normal.dot(point) + dist < 0)
236  {
237  return false;
238  }
239  }
240  return true;
241 }
armarx::PrimitiveFilter::getPrimitivesInFieldOfView
void getPrimitivesInFieldOfView(std::vector< memoryx::EnvironmentalPrimitiveBasePtr > allPrimitives, std::vector< memoryx::EnvironmentalPrimitiveBasePtr > &visiblePrimitives, std::vector< memoryx::EnvironmentalPrimitiveBasePtr > &otherPrimitives)
Definition: PrimitiveFilter.cpp:42
armarx::PrimitiveFilter::isPointInFrustum
bool isPointInFrustum(Eigen::Vector3f point)
Definition: PrimitiveFilter.cpp:227
armarx::PrimitiveFilter::visualizeFrustum
void visualizeFrustum(DebugDrawerInterfacePrx debugDrawerTopicPrx)
Definition: PrimitiveFilter.cpp:67
armarx::PrimitiveFilter::~PrimitiveFilter
~PrimitiveFilter()
PrimitiveFilter Destructor.
Definition: PrimitiveFilter.cpp:36
PrimitiveFilter.h
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::PrimitiveFilter::updateCameraPosition
Eigen::Matrix3Xf updateCameraPosition(Eigen::Matrix4f cameraToWorld)
adapted from https://github.com/vanderlin/FrustumCulling/
Definition: PrimitiveFilter.cpp:127
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT > >
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:315
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
Logging.h
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::PrimitiveFilter::intersectsFrustum
bool intersectsFrustum(Eigen::Matrix4f pose, Eigen::Vector3f extent)
Definition: PrimitiveFilter.cpp:200
armarx::PrimitiveFilter::PrimitiveFilter
PrimitiveFilter()
PrimitiveFilter Constructor.
Definition: PrimitiveFilter.cpp:32