98 lines
3.0 KiB
C++
98 lines
3.0 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/*
|
|
* CalibratedCamera.cpp
|
|
*
|
|
* Created on: Aug 17, 2009
|
|
* Author: dellaert
|
|
*/
|
|
|
|
#include <gtsam/geometry/Point2.h>
|
|
#include <gtsam/geometry/CalibratedCamera.h>
|
|
|
|
namespace gtsam {
|
|
|
|
CalibratedCamera::CalibratedCamera() {}
|
|
|
|
CalibratedCamera::CalibratedCamera(const Pose3& pose) :
|
|
pose_(pose) {
|
|
}
|
|
|
|
CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {}
|
|
|
|
CalibratedCamera::~CalibratedCamera() {}
|
|
|
|
Point2 CalibratedCamera::project_to_camera(const Point3& P, boost::optional<Matrix&> H1) {
|
|
if (H1) {
|
|
double d = 1.0 / P.z(), d2 = d * d;
|
|
*H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
|
|
}
|
|
return Point2(P.x() / P.z(), P.y() / P.z());
|
|
}
|
|
|
|
Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) {
|
|
return Point3(p.x() * scale, p.y() * scale, scale);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
Point2 CalibratedCamera::project(const Point3& point,
|
|
boost::optional<Matrix&> D_intrinsic_pose,
|
|
boost::optional<Matrix&> D_intrinsic_point) const {
|
|
const Pose3& pose = pose_;
|
|
const Rot3& R = pose.rotation();
|
|
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
|
|
Point3 q = pose.transform_to(point);
|
|
|
|
if (D_intrinsic_pose || D_intrinsic_point) {
|
|
double X = q.x(), Y = q.y(), Z = q.z();
|
|
double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
|
|
double dp11 = d*r1.x()-r3.x()*Xd2, dp12 = d*r1.y()-r3.y()*Xd2, dp13 = d*r1.z()-r3.z()*Xd2;
|
|
double dp21 = d*r2.x()-r3.x()*Yd2, dp22 = d*r2.y()-r3.y()*Yd2, dp23 = d*r2.z()-r3.z()*Yd2;
|
|
if (D_intrinsic_pose)
|
|
*D_intrinsic_pose = Matrix_(2,6,
|
|
X*Yd2, -Z*d-X*Xd2, d*Y, -dp11, -dp12, -dp13,
|
|
d*Z+Y*Yd2, -X*Yd2, -d*X, -dp21, -dp22, -dp23);
|
|
if (D_intrinsic_point)
|
|
*D_intrinsic_point = Matrix_(2,3,
|
|
dp11, dp12, dp13,
|
|
dp21, dp22, dp23);
|
|
}
|
|
return project_to_camera(q);
|
|
}
|
|
|
|
|
|
CalibratedCamera CalibratedCamera::expmap(const Vector& d) const {
|
|
return CalibratedCamera(pose().expmap(d)) ;
|
|
}
|
|
Vector CalibratedCamera::logmap(const CalibratedCamera& T2) const {
|
|
return pose().logmap(T2.pose()) ;
|
|
}
|
|
|
|
CalibratedCamera CalibratedCamera::Expmap(const Vector& v) {
|
|
return CalibratedCamera(Pose3::Expmap(v)) ;
|
|
}
|
|
|
|
Vector CalibratedCamera::Logmap(const CalibratedCamera& p) {
|
|
return Pose3::Logmap(p.pose()) ;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
}
|