Drake
ArticulatedIcpLinearizedNormCost Class Reference

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...

#include <perception/estimators/dev/articulated_icp.h>

Inheritance diagram for ArticulatedIcpLinearizedNormCost:
[legend]
Collaboration diagram for ArticulatedIcpLinearizedNormCost:
[legend]

Public Member Functions

 ArticulatedIcpLinearizedNormCost (const Scene *scene)
 
void Reset () override
 Prepare to start adding points. More...
 
void Add (const SceneState &scene_state, const ArticulatedIcpErrorSet &error_set) override
 Add a given set of errors. More...
 
void Finalize () override
 Finish adding all error sets. More...
 
double cost () const
 Total cost of L2 norm of the errors. More...
 
void UpdateCost (solvers::QuadraticCost *cost) const
 Render cost to to be incorporated into a MathematicalProgram. More...
 
Does not allow copy, move, or assignment
 ArticulatedIcpLinearizedNormCost (const ArticulatedIcpLinearizedNormCost &)=delete
 
ArticulatedIcpLinearizedNormCostoperator= (const ArticulatedIcpLinearizedNormCost &)=delete
 
 ArticulatedIcpLinearizedNormCost (ArticulatedIcpLinearizedNormCost &&)=delete
 
ArticulatedIcpLinearizedNormCostoperator= (ArticulatedIcpLinearizedNormCost &&)=delete
 
- Public Member Functions inherited from ArticulatedIcpErrorCost
 ArticulatedIcpErrorCost (const Scene *scene)
 
virtual ~ArticulatedIcpErrorCost ()
 
const Scenescene () const
 
 ArticulatedIcpErrorCost (const ArticulatedIcpErrorCost &)=delete
 
ArticulatedIcpErrorCostoperator= (const ArticulatedIcpErrorCost &)=delete
 
 ArticulatedIcpErrorCost (ArticulatedIcpErrorCost &&)=delete
 
ArticulatedIcpErrorCostoperator= (ArticulatedIcpErrorCost &&)=delete
 

Detailed Description

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.

The resulting quadratic coefficients (for 0.5 x'Qx + b'x + c) are: Q = ∑ 2*Jᵢ'Jᵢ b = ∑ 2*Jᵢ'kᵢ c = ∑ kᵢ'kᵢ where kᵢ = eᵢ - Jᵢ q0 The total cost is: E = ∑ eᵢ'eᵢ

Constructor & Destructor Documentation

ArticulatedIcpLinearizedNormCost ( const Scene scene)
inlineexplicit

Member Function Documentation

void Add ( const SceneState scene_state,
const ArticulatedIcpErrorSet error_set 
)
overridevirtual

Add a given set of errors.

Implements ArticulatedIcpErrorCost.

Here is the call graph for this function:

double cost ( ) const
inline

Total cost of L2 norm of the errors.

void Finalize ( )
overridevirtual

Finish adding all error sets.

Implements ArticulatedIcpErrorCost.

void Reset ( )
overridevirtual

Prepare to start adding points.

Implements ArticulatedIcpErrorCost.

void UpdateCost ( solvers::QuadraticCost cost) const

Render cost to to be incorporated into a MathematicalProgram.

Here is the call graph for this function:


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