Esempio n. 1
0
/* ************************************************************************* */
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);
}
Esempio n. 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
}
 // Using the NoiseModelFactor1 base class there are two functions that must be overridden.
 // The first is the 'evaluateError' function. This function implements the desired measurement
 // function, returning a vector of errors when evaluated at the provided variable value. It
 // must also calculate the Jacobians for this measurement function, if requested.
 Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const
 {
     // The measurement function for a GPS-like measurement is simple:
     // error_x = pose.x - measurement.x
     // error_y = pose.y - measurement.y
     // Consequently, the Jacobians are:
     // [ derror_x/dx  derror_x/dy  derror_x/dtheta ] = [1 0 0]
     // [ derror_y/dx  derror_y/dy  derror_y/dtheta ] = [0 1 0]
     if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
     return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
 }