NASA Astrobee Robot Software  0.19.1
Flight software for the Astrobee robots operating inside the International Space Station.
vision_common::InverseDepthMeasurement Class Reference

#include <inverse_depth_measurement.h>

Public Types

enum  { dimension = 1 }
 

Public Member Functions

 InverseDepthMeasurement (const double inverse_depth, const Eigen::Vector2d &image_coordinates, const Eigen::Matrix3d &intrinsics, const gtsam::Pose3 &body_T_sensor)
 
Eigen::Vector3d Backproject (boost::optional< gtsam::Matrix & > d_backprojected_point_d_depth=boost::none) const
 
boost::optional< Eigen::Vector2d > Project (const gtsam::Pose3 &world_T_source_body, const gtsam::Pose3 &world_T_target_body, boost::optional< gtsam::Matrix & > d_projected_point_d_world_T_source_body=boost::none, boost::optional< gtsam::Matrix & > d_projected_point_d_world_T_target_body=boost::none, boost::optional< gtsam::Matrix & > d_projected_point_d_inverse_depth=boost::none) const
 
double depth () const
 
double inverse_depth () const
 
size_t dim () const
 
InverseDepthMeasurement retract (const gtsam::Vector &d) const
 
gtsam::Vector1 localCoordinates (const InverseDepthMeasurement &T2) const
 
void print (const std::string &s=std::string()) const
 
bool equals (const InverseDepthMeasurement &s, double tol=1e-9) const
 

Static Public Member Functions

static size_t Dim ()
 

Detailed Description

Optimizable inverse depth parameterization for a landmark point using a (u,v) image space measurement and the associated camera pose for the measurement along with the camera intrinsics matrix to be able to backproject the inverse depth point to a 3d point.

Member Enumeration Documentation

◆ anonymous enum

anonymous enum
Enumerator
dimension 

Constructor & Destructor Documentation

◆ InverseDepthMeasurement()

vision_common::InverseDepthMeasurement::InverseDepthMeasurement ( const double  inverse_depth,
const Eigen::Vector2d &  image_coordinates,
const Eigen::Matrix3d &  intrinsics,
const gtsam::Pose3 &  body_T_sensor 
)
inline

Member Function Documentation

◆ Backproject()

Eigen::Vector3d vision_common::InverseDepthMeasurement::Backproject ( boost::optional< gtsam::Matrix & >  d_backprojected_point_d_depth = boost::none) const
inline

◆ depth()

double vision_common::InverseDepthMeasurement::depth ( ) const
inline

◆ Dim()

static size_t vision_common::InverseDepthMeasurement::Dim ( )
inlinestatic

◆ dim()

size_t vision_common::InverseDepthMeasurement::dim ( ) const
inline

◆ equals()

bool vision_common::InverseDepthMeasurement::equals ( const InverseDepthMeasurement s,
double  tol = 1e-9 
) const
inline

◆ inverse_depth()

double vision_common::InverseDepthMeasurement::inverse_depth ( ) const
inline

◆ localCoordinates()

gtsam::Vector1 vision_common::InverseDepthMeasurement::localCoordinates ( const InverseDepthMeasurement T2) const
inline

◆ print()

void vision_common::InverseDepthMeasurement::print ( const std::string &  s = std::string()) const
inline

◆ Project()

boost::optional<Eigen::Vector2d> vision_common::InverseDepthMeasurement::Project ( const gtsam::Pose3 &  world_T_source_body,
const gtsam::Pose3 &  world_T_target_body,
boost::optional< gtsam::Matrix & >  d_projected_point_d_world_T_source_body = boost::none,
boost::optional< gtsam::Matrix & >  d_projected_point_d_world_T_target_body = boost::none,
boost::optional< gtsam::Matrix & >  d_projected_point_d_inverse_depth = boost::none 
) const
inline

◆ retract()

InverseDepthMeasurement vision_common::InverseDepthMeasurement::retract ( const gtsam::Vector &  d) const
inline

The documentation for this class was generated from the following file: