180 lines
4.5 KiB
C++
180 lines
4.5 KiB
C++
|
|
/**
|
|
* @file InvDepthCamera3.h
|
|
* @brief
|
|
* @author Chris Beall
|
|
* @date Apr 14, 2012
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <cmath>
|
|
#include <boost/optional.hpp>
|
|
#include <boost/serialization/nvp.hpp>
|
|
#include <gtsam/base/Vector.h>
|
|
#include <gtsam/base/LieVector.h>
|
|
#include <gtsam/base/LieScalar.h>
|
|
#include <gtsam/base/Matrix.h>
|
|
#include <gtsam/geometry/Point2.h>
|
|
#include <gtsam/geometry/Pose3.h>
|
|
#include <gtsam/geometry/PinholeCamera.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* A pinhole camera class that has a Pose3 and a Calibration.
|
|
* @ingroup geometry
|
|
* \nosubgrouping
|
|
*/
|
|
template <class CALIBRATION>
|
|
class InvDepthCamera3 {
|
|
private:
|
|
Pose3 pose_;
|
|
boost::shared_ptr<CALIBRATION> k_;
|
|
|
|
public:
|
|
|
|
/// @name Standard Constructors
|
|
/// @{
|
|
|
|
/** default constructor */
|
|
InvDepthCamera3() {}
|
|
|
|
/** constructor with pose and calibration */
|
|
InvDepthCamera3(const Pose3& pose, const boost::shared_ptr<CALIBRATION>& k) :
|
|
pose_(pose),k_(k) {}
|
|
|
|
/// @}
|
|
/// @name Standard Interface
|
|
/// @{
|
|
|
|
virtual ~InvDepthCamera3() {}
|
|
|
|
/// return pose
|
|
inline Pose3& pose() { return pose_; }
|
|
|
|
/// return calibration
|
|
inline const boost::shared_ptr<CALIBRATION>& calibration() const { return k_; }
|
|
|
|
/// print
|
|
void print(const std::string& s = "") const {
|
|
pose_.print("pose3");
|
|
k_.print("calibration");
|
|
}
|
|
|
|
static gtsam::Point3 invDepthTo3D(const gtsam::LieVector& pw, const gtsam::LieScalar& inv) {
|
|
gtsam::Point3 ray_base(pw.vector().segment(0,3));
|
|
double theta = pw(3), phi = pw(4);
|
|
double rho = inv.value(); // inverse depth
|
|
gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
|
|
return ray_base + m/rho;
|
|
}
|
|
|
|
/** project a point from world InvDepth parameterization to the image
|
|
* @param pw is a point in the world coordinate
|
|
* @param H1 is the jacobian w.r.t. [pose3 calibration]
|
|
* @param H2 is the jacobian w.r.t. inv_depth_landmark
|
|
*/
|
|
inline gtsam::Point2 project(const gtsam::LieVector& pw,
|
|
const gtsam::LieScalar& inv_depth,
|
|
boost::optional<gtsam::Matrix&> H1 = boost::none,
|
|
boost::optional<gtsam::Matrix&> H2 = boost::none,
|
|
boost::optional<gtsam::Matrix&> H3 = boost::none) const {
|
|
|
|
gtsam::Point3 ray_base(pw.vector().segment(0,3));
|
|
double theta = pw(3), phi = pw(4);
|
|
double rho = inv_depth.value(); // inverse depth
|
|
gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
|
|
const gtsam::Point3 landmark = ray_base + m/rho;
|
|
|
|
gtsam::PinholeCamera<CALIBRATION> camera(pose_, *k_);
|
|
|
|
if (!H1 && !H2 && !H3) {
|
|
gtsam::Point2 uv= camera.project(landmark);
|
|
return uv;
|
|
}
|
|
else {
|
|
gtsam::Matrix J2;
|
|
gtsam::Point2 uv= camera.project(landmark,H1, J2);
|
|
if (H1) {
|
|
*H1 = (*H1) * gtsam::eye(6);
|
|
}
|
|
|
|
double cos_theta = cos(theta);
|
|
double sin_theta = sin(theta);
|
|
double cos_phi = cos(phi);
|
|
double sin_phi = sin(phi);
|
|
double rho2 = rho * rho;
|
|
|
|
if (H2) {
|
|
double H11 = 1;
|
|
double H12 = 0;
|
|
double H13 = 0;
|
|
double H14 = -cos_phi*sin_theta/rho;
|
|
double H15 = -cos_theta*sin_phi/rho;
|
|
|
|
double H21 = 0;
|
|
double H22 = 1;
|
|
double H23 = 0;
|
|
double H24 = cos_phi*cos_theta/rho;
|
|
double H25 = -sin_phi*sin_theta/rho;
|
|
|
|
double H31 = 0;
|
|
double H32 = 0;
|
|
double H33 = 1;
|
|
double H34 = 0;
|
|
double H35 = cos_phi/rho;
|
|
|
|
*H2 = J2 * gtsam::Matrix_(3,5,
|
|
H11, H12, H13, H14, H15,
|
|
H21, H22, H23, H24, H25,
|
|
H31, H32, H33, H34, H35);
|
|
}
|
|
if(H3) {
|
|
double H16 = -cos_phi*cos_theta/rho2;
|
|
double H26 = -cos_phi*sin_theta/rho2;
|
|
double H36 = -sin_phi/rho2;
|
|
*H3 = J2 * gtsam::Matrix_(3,1,
|
|
H16,
|
|
H26,
|
|
H36);
|
|
}
|
|
return uv;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* backproject a 2-dimensional point to an Inverse Depth landmark
|
|
* useful for point initialization
|
|
*/
|
|
|
|
inline std::pair<gtsam::LieVector, gtsam::LieScalar> backproject(const gtsam::Point2& pi, const double depth) const {
|
|
const gtsam::Point2 pn = k_->calibrate(pi);
|
|
gtsam::Point3 pc(pn.x(), pn.y(), 1.0);
|
|
pc = pc/pc.norm();
|
|
gtsam::Point3 pw = pose_.transform_from(pc);
|
|
const gtsam::Point3& pt = pose_.translation();
|
|
gtsam::Point3 ray = pw - pt;
|
|
double theta = atan2(ray.y(), ray.x()); // longitude
|
|
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
|
|
return std::make_pair(gtsam::LieVector(5, pt.x(),pt.y(),pt.z(), theta, phi),
|
|
gtsam::LieScalar(1./depth));
|
|
}
|
|
|
|
private:
|
|
|
|
/// @}
|
|
/// @name Advanced Interface
|
|
/// @{
|
|
|
|
/** Serialization function */
|
|
friend class boost::serialization::access;
|
|
template<class Archive>
|
|
void serialize(Archive & ar, const unsigned int version) {
|
|
ar & BOOST_SERIALIZATION_NVP(pose_);
|
|
ar & BOOST_SERIALIZATION_NVP(k_);
|
|
}
|
|
/// @}
|
|
}; // \class InvDepthCamera
|
|
} // \namesapce gtsam
|