76 lines
2.5 KiB
C++
76 lines
2.5 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
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file SphericalCamera.h
|
|
* @brief Calibrated camera with spherical projection
|
|
* @date Aug 26, 2021
|
|
* @author Luca Carlone
|
|
*/
|
|
|
|
#include <gtsam/geometry/SphericalCamera.h>
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
bool SphericalCamera::equals(const SphericalCamera &camera, double tol) const {
|
|
return pose_.equals(camera.pose(), tol);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void SphericalCamera::print(const string& s) const {
|
|
pose_.print(s + ".pose");
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
pair<Unit3, bool> SphericalCamera::projectSafe(const Point3& pw) const {
|
|
const Point3 pc = pose().transformTo(pw);
|
|
Unit3 pu = Unit3::FromPoint3(pc);
|
|
return make_pair(pu, pc.norm() > 1e-8);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Unit3 SphericalCamera::project2(const Point3& pw, OptionalJacobian<2, 6> Dpose,
|
|
OptionalJacobian<2, 3> Dpoint) const {
|
|
|
|
Matrix36 Dtf_pose;
|
|
Matrix3 Dtf_point; // calculated by transformTo if needed
|
|
const Point3 pc = pose().transformTo(pw, Dpose ? &Dtf_pose : 0, Dpoint ? &Dtf_point : 0);
|
|
|
|
if (pc.norm() <= 1e-8)
|
|
throw("point cannot be at the center of the camera");
|
|
|
|
Matrix23 Dunit; // calculated by FromPoint3 if needed
|
|
Unit3 pu = Unit3::FromPoint3(Point3(pc), Dpoint ? &Dunit : 0);
|
|
|
|
if (Dpose)
|
|
*Dpose = Dunit * Dtf_pose; //2x3 * 3x6 = 2x6
|
|
if (Dpoint)
|
|
*Dpoint = Dunit * Dtf_point; //2x3 * 3x3 = 2x3
|
|
return pu;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Point3 SphericalCamera::backproject(const Unit3& pu, const double depth) const {
|
|
return pose().transformFrom(depth * pu);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Unit3 SphericalCamera::project(const Point3& point,
|
|
OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const {
|
|
return project2(point, Dcamera, Dpoint);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
}
|