\subsection{Articulation Learning} 
%
Once the 6-DOF pose estimates of the individual object parts are computed, the
kinematic model of the full articulated object is determined using
tools developed in ~\citet{sturm2011probabilistic}. Given multiple 6-DOF pose
observations of object parts, the problem is to estimate the most likely
kinematic configuration for the articulated object. Formally, given the
observed poses $\mathcal{D}_z$, we estimate the kinematic
graph configuration $\hat{G}$ that maximizes the posterior probability
\begin{equation} \hat{G} = \underset{G}{\arg\max}~p(G\mid\mathcal{D}_z)
\label{eq:posterior-prob-max}
\end{equation}

We employ notation similar to that of~\citet{sturm2011probabilistic} to
denote the relative transformation between two object parts $i$ and $j$ as
$\Delta_{ij} = x_i~\ominus~x_j$, using standard motion composition operator
notation~\cite{smith1990estimating}. The kinematic model between part $i$ and
$j$ is then defined as $M_{ij}$, with its associated parameter vector
$\theta_{ij} \in \mathbb{R}^{p_{ij}}$, where $p_{ij}$ are the number of
parameters associated with the description of the link. We construct a graph $G
= (V_G,E_G)$ consisting of a set of vertices $V_G = {1, \dots, k}$ that denote
the object parts involved in the articulated object, and a set of undirected
edges $E_G \subset V_G \times V_G$ describing the kinematic linkage between two
object parts.

% We denote the true pose of an
% object part $i \in {1, \dots, k}$ by $x_i \in \mathbb{R}^3 \times SO(3)$
% representing the 3D pose of the part in $SE(3)$, where $k$ is the number of
% object parts segmented earlier. Thus, the full object pose is defined by
% $x_{1:k} = (x_1, \dots, x_k)^T$. 

% Given a sequence of pose observations $\mathcal{D}_y$
% of the articulated object, the most likely kinematic graph $\hat{G}$ is
% estimated, indicating the underlying articulation of the object. We evaluate
% several candidate articulation models for an articulated object that best
% explains the degrees of motion exercised during manipulation. 


% Equation ~\ref{eq:posterior-prob-max} however, is difficult to compute due to
% the non-convex nature of the function, and due to the exponential parameter
% space that it needs to be computed over. 

As in \citet{sturm2011probabilistic}, we simplify the problem to
recognize only kinematic trees of high posterior probability, 
in order to reformulate the problem as equation~\ref{eq:posterior-prob-tree} below: 
\begin{align} \hat{G} &= \underset{G}{\arg\max}~p(G\mid\mathcal{D}_z)\\ 
&=
\underset{G}{\arg\max}~p(\left\{(\mathcal{M}_{ij},\theta_{ij}) \mid (ij) \in
E_G\right\}\mid\mathcal{D}_z)\\ 
&= \underset{G}{\arg\max}~\prod_{(ij) \in
E_G}~p(\mathcal{M}_{ij},\theta_{ij}\mid\mathcal{D}_z)\\
&= \underset{E_G}{\arg\max}~\sum_{(ij) \in E_G}~\log~p(\hat{\mathcal{M}_{ij}},\hat{\theta_{ij}}\mid\mathcal{D}_z)
\label{eq:posterior-prob-tree}
\end{align} where $\mathcal{D}_z = (\Delta_{ij}^1, \dots, \Delta_{ij}^t)~\forall~(ij) \in E_G$ is the
sequence of observed relative transformations between parts $i$ and $j$.

Since we are particularly interested in household objects, we focus on
kinematic models involving rigid, prismatic, and revolute linkages. We
then estimate the parameters $\theta \in \mathbb{R}^p$ that maximize
the data likelihood of the object pose observations given the
kinematic model:
\begin{align} \hat{\theta} &=
\underset{\theta}{\arg\max}~p(\mathcal{D}_z\mid\mathcal{M},\theta)
\label{eq:data-likelihood}
\end{align} 

% As mentioned earlier, we can also reproduce motions of a
% variety of household objects such as doors, drawers, refrigerators etc,
% with the combination of aforementioned kinematic model primitives. We summarize
% the model candidates that we use in our work in
% Table~\ref{tab:candidate-models-articulation-modeling}, and their associated
% degrees of freedom and model parameters.

% \begin{table}[h] \centering
% \caption{Candidate Models used for articulation modeling}
% \begin{tabular}{ccc} \toprule \textbf{Candidate Model} & \textbf{DOFs} &
%   \textbf{Parameters}\\ $M$ & $d$ & $p$\\ \midrule Rigid & 0 & 6\\ Prismatic & 1 &
%   9\\ Rotational & 1 & 12\\ \bottomrule
% \end{tabular}
% \label{tab:candidate-models-articulation-modeling}
% \end{table}

% MISSING CONNECTION BETWEEN 8 and 9
% Due to the high sensitivity of the least-squares-based parameter
% estimation to noise and outliers, we employ MLESAC
% \cite{torr2000mlesac} to provide the initial guesses for the kinematic
% parameters by picking a set of randomly drawn samples from the observation
% sequence which is then later refined via BFGS
% (Broyden-Fletcher-Goldfarb-Shanno).

Once we fit each candidate kinematic model
to the given observation sequence, we select the kinematic model that best
explains the data. Specifically, we compute the posterior probability of
each kinematic model, given the data, as:
\begin{align} ~p(\mathcal{M}\mid\mathcal{D}_z) =
\bigintsss\frac{p(\mathcal{D}_z\mid\mathcal{M},\theta)~p(\theta\mid\mathcal{M})~p(\mathcal{M})}{p(\mathcal{D}_z)}
d\theta
\label{eq:posterior-prob-structure-selection}
\end{align}

Due to the evaluation complexity of this posterior term, the BIC score is
computed instead as the approximation: 
\begin{align} BIC(\mathcal{M}) =
-2\log~p(\mathcal{D}_z\mid\mathcal{M},\hat{\theta}) + p\log n,
\label{eq:bic-structure-selection}
\end{align}where $p$ is the number of parameters
involved in the kinematic model, $n$ is the number of observations in the
data set, and $\hat{\theta}$ is the maximum likelihood parameter
vector. This implies that the model that best explains the observations would
correspond to that with the least BIC score.

% , i.e.
% \begin{align} \hat{\mathcal{M}} = \underset{\mathcal{M}}{\arg\min}
% BIC(\mathcal{M})
% \label{eq:structure-selection}
% \end{align}

% In our work, we consider a fully-connected graph with $|V_G|$ vertices, and
% $|E_G| = \frac{|V_G|\dot(|V_G|-1)}{2}$ edges. Each edge may be attributed to one of the
% 3 candidate kinematic models as listed in
% Table~\ref{tab:candidate-models-articulation-modeling}. Thus, the set of all
% possible kinematic trees that the articulated object can take is given by the
% set of all spanning trees in the graph constructed. 

% This subsequently leads us
% to finding the kinematic structure $\hat{E_G}$ that maximizes the posterior
% probability as follows: 
% \begin{align}
% \hat{E_G}&= \underset{\mathcal{E_G}}{\arg\max}~p(E_G|\mathcal{D}_z)\\
% &= \underset{E_G}{\arg\max}~p(\{(\hat{\mathcal{M}_{ij}},\hat{\theta_{ij}})~|~(ij) \in E_G\}|\mathcal{D}_z)\\
% &= \underset{E_G}{\arg\max}~\prod_{(ij) \in
%   E_G}~p(\hat{\mathcal{M}_{ij}},\hat{\theta_{ij}}|\mathcal{D}_z)\\
% &= \underset{E_G}{\arg\max}~\sum_{(ij) \in E_G}~\log~p(\hat{\mathcal{M}_{ij}},\hat{\theta_{ij}}|\mathcal{D}_z)
% \label{eq:cost-structure-selection}
% \end{align}

The kinematic structure selection problem is subsequently reduced to
computing the minimum spanning tree of the graph with edges defined by
$cost_{ij} =
-\log~p(\mathcal{M}_{ij},\theta_{ij}\mid\mathcal{D}_{z_{ij}})$. The
resulting minimum spanning kinematic tree weighted by BIC scores is
the most likely kinematic model for the articulated object given the
pose observations.  For a more detailed description, we refer the
reader
to~\citet{sturm2011probabilistic}. Figure~\ref{fig:model-estimation}
shows a few examples of kinematic structures extracted given pose
estimates as described in the previous section.  Our limitation of
linkage types to rigid, prismatic, and rotational does exclude various
household objects such as lamps, garage doors, toys etc.~with more
complex kinematics.

% Articulation estimation 
%
\begin{figure}[H]
  \centering
  \subfigure[Rotational DOF of a laptop]{\includegraphics[height=1.3in]{graphics/model-estimation/laptop-rot.png}}~
  \subfigure[Prismatic DOF of a drawer]{\includegraphics[height=1.2in]{graphics/model-estimation/drawer-pris.png}}
  % \renewcommand{\tabcolsep}{1.5pt}
  % \begin{tabular}{cc}
  %   \includegraphics[width=0.5\columnwidth]{graphics/model-estimation/fridge-poses-with-rotation.jpg}&
  %   \includegraphics[width=0.5\columnwidth]{graphics/model-estimation/drawer-poses-with-prismatic.jpg}\\
  % \end{tabular}
  \caption{Examples of correctly estimated kinematic structure from 6-DOF pose estimates
    of feature trajectories.}
  \label{fig:model-estimation}
\end{figure}

