Geometry Notes

Geometry Note

##Use Euler angle to represent direction cosine matrix
$C_b^n = \left[
\begin{array}{clcr}
cos\theta cos\psi & -cos\phi sin\psi+sin\phi sin\theta cos\psi & sin\phi sin\psi + cos\phi sin\theta cos\psi\\
cos\theta sin\psi & cos\phi cos\psi+sin\phi sin\theta cos\psi & -sin\phi cos\psi + cos\phi sin\theta sin\psi\\
-sin\theta & sin\phi cos\theta & cos\phi cos\theta
\end{array}
\right]$

```
void Transformer::lookupTransform 	( 	const std::string &  	target_frame,
		const std::string &  	source_frame,
		const ros::Time &  	time,
		StampedTransform &  	transform 
	) 	
	
```

$$P^{target}=T^{target}_{source}*P^{source}$$