Drake
drake::perception::estimators Namespace Reference

Classes

struct  ArticulatedBodyInfluence
 Description on how the optimization affects a body. More...
 
class  ArticulatedIcpBodyPoints
 Contains a group of points to be rendered in a linearized ICP cost. More...
 
class  ArticulatedIcpErrorCost
 Accumulate errors and render the cost into an appropriate format. More...
 
class  ArticulatedIcpErrorNormCost
 Aggregate L2 norm cost of point-to-point error set. More...
 
class  ArticulatedIcpErrorSet
 A set of Cartesian errors and their gradients: {(eᵢ, Jᵢ)}ᵢ₌₁..ₙ More...
 
class  ArticulatedIcpLinearizedNormCost
 Accumulate the L2 norm of the linearized point-to-point errors to be rendered into a QuadraticCost operating on variables q: | eᵢ + Jᵢ (q - q0) |^2 where eᵢ is the ith 3x1 error in Cartesian space, Jᵢ is the 3 x nq Jacobian of this error. More...
 
struct  Bounds
 
struct  Interval
 
struct  IntervalIndex
 
struct  ObjectTestSetup
 
struct  PlaneIndices
 
class  PointCloudVisualizer
 
struct  PointCorrespondence
 Simple point-to-point correspondence. More...
 
class  Scene
 A scene describes a group of objects (stored within a rigid body tree structure), the world frame, and the camera frame. More...
 
class  SceneState
 Stored state of a scene. More...
 

Typedefs

typedef std::map< BodyIndex, std::vector< PointCorrespondence > > ArticulatedPointCorrespondences
 
typedef std::vector< ArticulatedBodyInfluenceArticulatedBodyInfluences
 
typedef RigidBodyFrame< doubleRigidBodyFramed
 
typedef KinematicsCache< doubleKinematicsCached
 
typedef int BodyIndex
 
typedef int FrameIndex
 

Enumerations

enum  ObjectTestType { kSimpleCuboid, kBlueFunnelScan }
 

Functions

ArticulatedBodyInfluence IsBodyCorrespondenceInfluential (const Scene &scene, BodyIndex)
 Determines if a correspondence with a given body influences the optimization formulation (camera or body position). More...
 
void ComputeBodyCorrespondenceInfluences (const Scene &scene, ArticulatedBodyInfluences *influences)
 Compute all body correspondences for the entire scene. More...
 
void ComputeCorrespondences (const SceneState &scene_state, const ArticulatedBodyInfluences &influences, const Eigen::Matrix3Xd &meas_pts_W, ArticulatedPointCorrespondences *pcorrespondence)
 Compute point-to-point correspondences from a measured point cloud and a given scene. More...
 
void ComputeCost (const SceneState &scene_state, const ArticulatedPointCorrespondences &correspondence, ArticulatedIcpErrorCost *cost)
 Compute cost for a given set of correspondences. More...
 
Eigen::Isometry3d ComputePcaBodyPose (const Matrix3Xd &y_W)
 
Eigen::Isometry3d ComputeSvdBodyPose (const Matrix3Xd &p_B, const Matrix3Xd &y_W)
 
Eigen::Isometry3d ComputePcaBodyPose (const Eigen::Matrix3Xd &y_W)
 Computes estimated transform by aligning principle directions via principle component analysis (PCA). More...
 
Eigen::Isometry3d ComputeSvdBodyPose (const Eigen::Matrix3Xd &p_B, const Eigen::Matrix3Xd &y_W)
 Computes the pose of a body described by points p (expressed in the body frame), given observed points y (expressed in the world frame). More...
 
void PointCloudToLcm (const Eigen::Matrix3Xd &pts_W, bot_core::pointcloud_t *pmessage)
 
Matrix2Xd Generate2DPlane (double space, Interval x, Interval y)
 
Matrix3Xd Generate2DPlane (double space, PlaneIndices is)
 
Matrix3Xd GenerateBoxPointCloud (double space, Bounds box)
 
AssertionResult CompareRotMatWithoutAxisSign (const Matrix3d &R_expected, const Matrix3d &R_actual, double tolerance)
 
AssertionResult CompareTransformWithoutAxisSign (const Isometry3d &X_expected, const Isometry3d &X_actual, double tolerance)
 
void GetObjectTestSetup (ObjectTestType type, ObjectTestSetup *setup)
 
::testing::AssertionResult CompareRotMatWithoutAxisSign (const Eigen::Matrix3d &R_expected, const Eigen::Matrix3d &R_actual, double tolerance=0.0)
 
::testing::AssertionResult CompareTransformWithoutAxisSign (const Eigen::Isometry3d &X_expected, const Eigen::Isometry3d &X_actual, double tolerance=0.0)
 

Variables

const double kPi = M_PI
 
const auto ObjectTestTypes
 

Typedef Documentation

typedef int BodyIndex
typedef int FrameIndex

Enumeration Type Documentation

Enumerator
kSimpleCuboid 
kBlueFunnelScan 

Function Documentation

AssertionResult drake::perception::estimators::CompareRotMatWithoutAxisSign ( const Matrix3d &  R_expected,
const Matrix3d &  R_actual,
double  tolerance 
)

Here is the call graph for this function:

Here is the caller graph for this function:

::testing::AssertionResult drake::perception::estimators::CompareRotMatWithoutAxisSign ( const Eigen::Matrix3d &  R_expected,
const Eigen::Matrix3d &  R_actual,
double  tolerance = 0.0 
)
AssertionResult drake::perception::estimators::CompareTransformWithoutAxisSign ( const Isometry3d &  X_expected,
const Isometry3d &  X_actual,
double  tolerance 
)

Here is the call graph for this function:

::testing::AssertionResult drake::perception::estimators::CompareTransformWithoutAxisSign ( const Eigen::Isometry3d &  X_expected,
const Eigen::Isometry3d &  X_actual,
double  tolerance = 0.0 
)
void ComputeBodyCorrespondenceInfluences ( const Scene scene,
ArticulatedBodyInfluences influences 
)

Compute all body correspondences for the entire scene.

See also
ArticulatedBodyInfluence for more information on how this is determined.

Here is the call graph for this function:

void ComputeCorrespondences ( const SceneState scene_state,
const ArticulatedBodyInfluences influences,
const Eigen::Matrix3Xd &  meas_pts_W,
ArticulatedPointCorrespondences pcorrespondence 
)

Compute point-to-point correspondences from a measured point cloud and a given scene.

Here is the call graph for this function:

void ComputeCost ( const SceneState scene_state,
const ArticulatedPointCorrespondences correspondence,
ArticulatedIcpErrorCost cost 
)

Compute cost for a given set of correspondences.

Here is the call graph for this function:

Eigen::Isometry3d drake::perception::estimators::ComputePcaBodyPose ( const Matrix3Xd &  y_W)
Eigen::Isometry3d drake::perception::estimators::ComputePcaBodyPose ( const Eigen::Matrix3Xd &  y_W)

Computes estimated transform by aligning principle directions via principle component analysis (PCA).

Parameters
y_WA 3xn matrix represent n measured points, fixed in body B (inferred via PCA), expressed in world coordinates.
Returns
X_WB The pose of the inferred shape in the world frame.
Eigen::Isometry3d drake::perception::estimators::ComputeSvdBodyPose ( const Eigen::Matrix3Xd &  p_B,
const Eigen::Matrix3Xd &  y_W 
)

Computes the pose of a body described by points p (expressed in the body frame), given observed points y (expressed in the world frame).

This requires that all correspondences be known. This performs the optimization: min ∑ᵢ |X_WB * p_Bᵢ - y_Wᵢ|² X_WB which is implemented as a widely-used simplification of the quaternion-based formulation presented in: Arun, K. Somani, Thomas S. Huang, and Steven D. Blostein. "Least-squares fitting of two 3-D point sets." IEEE Transactions on pattern analysis and machine intelligence 5 (1987): 698-700.

Parameters
p_BA 3xn matrix representing n points, fixed and expressed in the body frame, {p_Bᵢ}ᵢ.
y_WA 3xn matrix representing n measured points of the body, fixed in the body frame and expressed in the world frame, {y_Wᵢ}ᵢ, where y_Wᵢ corresponds to p_Bᵢ.
Returns
X_WB Transformation from B to W, i.e., the pose of body B in the world frame.
Precondition
p_B The body points (and ultimately y_W, barring noise) are not colinear. See Sec. IV, possibility 3.
Note
Be wary of the Degeneracy in Sec. V., when the noise in y_W is large and may yield a reflected R_WB.
Eigen::Isometry3d drake::perception::estimators::ComputeSvdBodyPose ( const Matrix3Xd &  p_B,
const Matrix3Xd &  y_W 
)

Here is the call graph for this function:

Eigen::Matrix2Xd Generate2DPlane ( double  space,
Interval  x,
Interval  y 
)

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::Matrix3Xd Generate2DPlane ( double  space,
PlaneIndices  is 
)

Here is the call graph for this function:

Eigen::Matrix3Xd GenerateBoxPointCloud ( double  space,
Bounds  box 
)

Here is the call graph for this function:

Here is the caller graph for this function:

void GetObjectTestSetup ( ObjectTestType  type,
ObjectTestSetup setup 
)

Here is the call graph for this function:

ArticulatedBodyInfluence IsBodyCorrespondenceInfluential ( const Scene scene,
BodyIndex   
)

Determines if a correspondence with a given body influences the optimization formulation (camera or body position).

See also
ArticulatedBodyInfluence for more information on how this is determined.

Here is the call graph for this function:

Here is the caller graph for this function:

void PointCloudToLcm ( const Eigen::Matrix3Xd &  pts_W,
bot_core::pointcloud_t *  pmessage 
)

Here is the caller graph for this function:

Variable Documentation

const double kPi = M_PI
const auto ObjectTestTypes
Initial value: