capitalize static functions Level and Lookat in CalibratedCamera and PinholeCamera for matlab wrapper
parent
4113a09b3a
commit
abc29ea2ca
|
@ -67,7 +67,7 @@ struct VisualSLAMExampleData {
|
|||
double r = 30.0;
|
||||
for (int i=0; i<n; ++i, theta += 2*M_PI/n) {
|
||||
gtsam::Point3 C = gtsam::Point3(r*cos(theta), r*sin(theta), 0.0);
|
||||
gtsam::SimpleCamera camera = gtsam::SimpleCamera::lookat(C, gtsam::Point3(), gtsam::Point3(0,0,1));
|
||||
gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(C, gtsam::Point3(), gtsam::Point3(0,0,1));
|
||||
data.poses.push_back(camera.pose());
|
||||
}
|
||||
data.odometry = data.poses[0].between(data.poses[1]);
|
||||
|
|
8
gtsam.h
8
gtsam.h
|
@ -499,7 +499,7 @@ virtual class CalibratedCamera : gtsam::Value {
|
|||
CalibratedCamera();
|
||||
CalibratedCamera(const gtsam::Pose3& pose);
|
||||
CalibratedCamera(const Vector& v);
|
||||
gtsam::CalibratedCamera level(const gtsam::Pose2& pose2, double height);
|
||||
static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height);
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
|
@ -529,10 +529,10 @@ virtual class SimpleCamera : gtsam::Value {
|
|||
SimpleCamera();
|
||||
SimpleCamera(const gtsam::Pose3& pose);
|
||||
SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K);
|
||||
static gtsam::SimpleCamera level(const gtsam::Cal3_S2& K,
|
||||
static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K,
|
||||
const gtsam::Pose2& pose, double height);
|
||||
static gtsam::SimpleCamera level(const gtsam::Pose2& pose, double height); // FIXME overload
|
||||
static gtsam::SimpleCamera lookat(const gtsam::Point3& eye,
|
||||
static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); // FIXME overload
|
||||
static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye,
|
||||
const gtsam::Point3& target, const gtsam::Point3& upVector,
|
||||
const gtsam::Cal3_S2& K);
|
||||
|
||||
|
|
|
@ -47,7 +47,7 @@ Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double s
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) {
|
||||
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);
|
||||
|
|
|
@ -96,7 +96,7 @@ namespace gtsam {
|
|||
* @param pose2 specifies the location and viewing direction
|
||||
* (theta 0 = looking in direction of positive X axis)
|
||||
*/
|
||||
static CalibratedCamera level(const Pose2& pose2, double height);
|
||||
static CalibratedCamera Level(const Pose2& pose2, double height);
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
|
|
|
@ -66,7 +66,7 @@ namespace gtsam {
|
|||
* (theta 0 = looking in direction of positive X axis)
|
||||
* @param height camera height
|
||||
*/
|
||||
static PinholeCamera level(const Calibration &K, const Pose2& pose2, double height) {
|
||||
static PinholeCamera Level(const Calibration &K, const Pose2& pose2, double height) {
|
||||
const double st = sin(pose2.theta()), ct = cos(pose2.theta());
|
||||
const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
|
||||
const Rot3 wRc(x, y, z);
|
||||
|
@ -76,8 +76,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/// PinholeCamera::level with default calibration
|
||||
static PinholeCamera level(const Pose2& pose2, double height) {
|
||||
return PinholeCamera::level(Calibration(), pose2, height);
|
||||
static PinholeCamera Level(const Pose2& pose2, double height) {
|
||||
return PinholeCamera::Level(Calibration(), pose2, height);
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -89,7 +89,7 @@ namespace gtsam {
|
|||
* doesn't need to be on the image plane nor orthogonal to the viewing axis
|
||||
* @param K optional calibration parameter
|
||||
*/
|
||||
static PinholeCamera lookat(const Point3& eye, const Point3& target, const Point3& upVector, const Calibration& K = Calibration()) {
|
||||
static PinholeCamera Lookat(const Point3& eye, const Point3& target, const Point3& upVector, const Calibration& K = Calibration()) {
|
||||
Point3 zc = target-eye;
|
||||
zc = zc/zc.norm();
|
||||
Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
|
||||
|
|
|
@ -52,7 +52,7 @@ TEST( CalibratedCamera, level1)
|
|||
{
|
||||
// Create a level camera, looking in X-direction
|
||||
Pose2 pose2(0.1,0.2,0);
|
||||
CalibratedCamera camera = CalibratedCamera::level(pose2, 0.3);
|
||||
CalibratedCamera camera = CalibratedCamera::Level(pose2, 0.3);
|
||||
|
||||
// expected
|
||||
Point3 x(0,-1,0),y(0,0,-1),z(1,0,0);
|
||||
|
@ -66,7 +66,7 @@ TEST( CalibratedCamera, level2)
|
|||
{
|
||||
// Create a level camera, looking in Y-direction
|
||||
Pose2 pose2(0.4,0.3,M_PI/2.0);
|
||||
CalibratedCamera camera = CalibratedCamera::level(pose2, 0.1);
|
||||
CalibratedCamera camera = CalibratedCamera::Level(pose2, 0.1);
|
||||
|
||||
// expected
|
||||
Point3 x(1,0,0),y(0,0,-1),z(0,1,0);
|
||||
|
|
|
@ -54,7 +54,7 @@ TEST( SimpleCamera, level2)
|
|||
{
|
||||
// Create a level camera, looking in Y-direction
|
||||
Pose2 pose2(0.4,0.3,M_PI/2.0);
|
||||
SimpleCamera camera = SimpleCamera::level(K, pose2, 0.1);
|
||||
SimpleCamera camera = SimpleCamera::Level(K, pose2, 0.1);
|
||||
|
||||
// expected
|
||||
Point3 x(1,0,0),y(0,0,-1),z(0,1,0);
|
||||
|
@ -68,7 +68,7 @@ TEST( SimpleCamera, lookat)
|
|||
{
|
||||
// Create a level camera, looking in Y-direction
|
||||
Point3 C(10.0,0.0,0.0);
|
||||
SimpleCamera camera = SimpleCamera::lookat(C, Point3(), Point3(0.0,0.0,1.0));
|
||||
SimpleCamera camera = SimpleCamera::Lookat(C, Point3(), Point3(0.0,0.0,1.0));
|
||||
|
||||
// expected
|
||||
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
|
||||
|
@ -76,7 +76,7 @@ TEST( SimpleCamera, lookat)
|
|||
CHECK(assert_equal( camera.pose(), expected));
|
||||
|
||||
Point3 C2(30.0,0.0,10.0);
|
||||
SimpleCamera camera2 = SimpleCamera::lookat(C2, Point3(), Point3(0.0,0.0,1.0));
|
||||
SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
|
||||
|
||||
Matrix R = camera2.pose().rotation().matrix();
|
||||
Matrix I = trans(R)*R;
|
||||
|
|
Loading…
Reference in New Issue