/* ************************************************************************* */
CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
	double st = sin(pose2.theta()), ct = cos(pose2.theta());
	Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
	Rot3 wRc(x, y, z);
	Point3 t(pose2.x(), pose2.y(), height);
	Pose3 pose3(wRc, t);
	return CalibratedCamera(pose3);
}
Beispiel #2
0
/* ************************************************************************* */
Vector Pose2::localCoordinates(const Pose2& p2) const {
#ifdef SLOW_BUT_CORRECT_EXPMAP
    return Logmap(between(p2));
#else
    Pose2 r = between(p2);
    return (Vector(3) << r.x(), r.y(), r.theta());
#endif
}