Drake

Accumulate the L2 norm of the linearized pointtopoint 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>
Accumulate the L2 norm of the linearized pointtopoint 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ᵢ

delete 

inlineexplicit 

overridevirtual 
Add a given set of errors.
Implements ArticulatedIcpErrorCost.

inline 
Total cost of L2 norm of the errors.

overridevirtual 
Finish adding all error sets.
Implements ArticulatedIcpErrorCost.

delete 

delete 

overridevirtual 
Prepare to start adding points.
Implements ArticulatedIcpErrorCost.
void UpdateCost  (  solvers::QuadraticCost *  cost  )  const 
Render cost to to be incorporated into a MathematicalProgram.