\subsection{Motion Segmentation} 
% In this section, we describe the general
% concept for estimating the pose of an object in $SE(3)$ by estimating its
% motion segmentation support.  
%
To identify the kinematic relationships among parts in an articulated
object, we first distinguish the trajectory taken by each part.  In
particular, we analyze the motions of the object parts with respect to
each other over time, and infer whether or not pairs of object parts 
are rigidly attached. To reason over candidate segmentations, we formulate
a clustering problem to identify the different motion subspaces in
which the object parts lie. After clustering, similar labels imply
rigid attachment, while dissimilar labels indicate non-rigid relative 
motion between parts.

If two features in $\mathbb{R}^3 \times SO(2)$ belong to the same
rigid part, the relative displacement and angle between the features
will be consistent over the common span of their trajectories. The
distribution over the relative change in displacement vectors and
angle subtended is modeled as a zero-mean Gaussian,
\mbox{$\mathcal{N}(\mu,\Sigma) = (0, \Sigma)$}, where $\Sigma$ is the
expected noise covariance for rigidly-connected feature
pairs. The similarity of two feature trajectories can then be defined as:
% As expected, this
% constraint applies only up to some sensor noise that we account for in our
% observation model. 
% BLAH Figure~\ref{fig:pose-pair-feature} shows the rigid motion of a typical
% pair of features considered with position and surface normals. 
%
%
\begin{equation}
L(i,j) = \frac{1}{T}\sum_{t \in t_i \cap t_j} \exp
\Bigg\{-\gamma ~ \Big(d(x_i^{t},x_j^{t})- \mu_{d_{ij}}\Big)^2\Bigg\}
\label{eqn:pairwise-affinity}
\end{equation}
%
%
where $t_i$ and $t_j$ are the observed time instances of the feature
trajectories $i$, and $j$ respectively, $T = \left\vert{t_i \cap
    t_j}\right\vert$, and $\gamma$ is a parameter characterizing the
relative motion of the two trajectories. For a pair of 3-D key-point
features $\vec{p_i}$, and $\vec{p_j}$, we estimate the mean relative
displacement between a pair of points moving rigidly together as:
%
%
\begin{equation}\label{eqn:dist-l2}
    \mu_{d_{ij}}=\frac{1}{T}\sum_{t \in t_i \cap t_j}d(\vec{p_i}^t,\vec{p_j}^t)
\end{equation}
%
%
where $d(\vec{p_i},\vec{p_j})=\|\vec{p_i} - \vec{p_j}\|$. For 3-D key-points, we
use $\gamma = \frac{1}{2~\text{cm}}$ in
Eqn.~\ref{eqn:pairwise-affinity}. Figure~\ref{fig:pose-pair-histogram}
illustrates an example of rigid and non-rigid motions of feature trajectory
pairs, and their corresponding distribution of relative displacements.

For a pair of surface normals
$\vec{n_i}$ and $\vec{n_j}$, we define the mean distance as
%
%
\begin{equation}
\mu_{d_{ij}}=\frac{1}{T}\sum_{t \in t_i \cap t_j}d(\vec{n_i}^t,\vec{n_j}^t),
\label{eqn:dist-cosine}
\end{equation}
%
%
where $\quad d(\vec{n_i},\vec{n_j})=1-\vec{n_i} \cdot
\vec{n_j}$. In this case, we use \mbox{$\gamma =
\frac{1}{\cos(15\,^{\circ})}$} in
Eqn.~\ref{eqn:pairwise-affinity}. 

% Figure~\ref{fig:pose-pair-histogram} shows two
% separate cases of the pose pair distribution constructed from the relative
% motion profiles of a pair of trajectories. In both cases, the variance in
% relative pose is significantly higher for non-rigid motions providing a
% sufficient statistic to categorize rigid-motions from non-rigid ones. 
%

% % % Pose Pair Feature
% % %
% \begin{figure}[H]
%   \centering
%   % \renewcommand{\tabcolsep}{1.5pt}
%   \begin{tabular}{cc}
%     \includegraphics[width=0.4\columnwidth]{graphics/pose-pair-feature.pdf}&
%     \includegraphics[width=0.4\columnwidth]{graphics/pose-pair-feature-tf.pdf}\\
%   \end{tabular}
%   \caption{\textbf{Left:} A typical pose-pair feature, described by its spatial
%     position and surface normal attributed to each of the individual
%     features. \textbf{Right:} Rigid motions imply that the relative difference
%     in position and surface normal orientation between the feature pair
%     remain the same, providing a fundamental assumption with which the
%     segmentation is performed.}
%   \label{fig:pose-pair-feature}
% \end{figure}



\begin{figure}[t!]
  \centering
  % \renewcommand{\tabcolsep}{1.5pt}
    \includegraphics[width=0.95\columnwidth, clip, trim=0mm 95mm 0mm 0mm]{graphics/clustering/pose-pair-histogram-ground-truth.pdf} 
    \caption{Histogram of observed distances between a pair of
      trajectories accumulated over one demonstration. (Left) The
      distribution of observed distances is centered at $\mu=0.029~$m
      with $\sigma=0.001~$m, indicating rigid-body motion. (Right)
      Larger variation in observed distances, with $\sigma=0.018~$m,
      indicates non-rigid motion.}
  \label{fig:pose-pair-histogram}
\end{figure}

% % Similarity matrix showing trajectory matching: edge weights, high conf.
% %
% \begin{figure}[t!]
%   \centering
%   \subfigure[Rigid-Body Trajectory clusters]{
%     \includegraphics[width=0.45\columnwidth, clip, trim=0mm 0mm 5mm
%       0mm]{graphics/clustering/pose-clustered-edges.pdf}}~
%     \subfigure[Trajectory Similarity Matrix]{
%     \includegraphics[width=0.45\columnwidth]{graphics/clustering/pose-clustered-similarity-matrix.png}}
%   \caption{Color-coded edges overlaid on the articulated object indicate the
% motion profile similarity of trajectory pairs. Red edges indicate high motion
% profile similarity (rigidly moving, or rigidly stationary), while blue indicate
% low motion profile similarity (non-rigid motion). The resulting similarity
% matrix clearly indicates two rigid clusters of feature trajectories.}
%   \label{fig:pose-clustering-figure}
% \end{figure}

Since the bandwidth parameter $\gamma$ for a pair of feature trajectories can be
intuitively predicted from the expected variance in relative motions of
trajectories, we employ DBSCAN~\citep{ester1996density}, a density-based
clustering algorithm, to find rigidly associated feature trajectories. The
resulting cluster assignments are denoted as $\textbf{\text{C}} = \{C_1, \dots,
C_k\}$, where cluster $C_i$ consists of a set of rigidly-moving feature
trajectories. 

% Figure~\ref{fig:pose-clustering-figure} illustrates a simplified
% example of our motion segmentation approach with the use of fiducial markers.

% We expect that the average likelihood
% determined by Eqn.~\ref{eqn:pairwise-affinity}, to be within three standard
% deviations of the mean, thus providing a nominal value for
% $\gamma$. 

% BLAH
% requirement of at least 3 feature trajectories per cluster.
% An advantage of the proposed motion segmentation is its use of feature-level
% relative motion estimates. This allows in distinguishing objects that originally
% move rigidly with respect to each other, but later exhibit relative motions
% between each other. One such example arises with articulated objects, where two
% joints move consistently with each other until one of the joints starts to move
% relatively to the first joint. Since the pose tracking is performed on a
% fine-grained level, we are able to distinguish such relative motions, and
% estimate the new segmentation of the articulated link accordingly. It is
% particularly important to note that this step is performed in an unsupervised
% manner. While particular care is taken in identifying the inherent uncertainties
% involved the sensor observation model, this particular step is
% sensitive to the type of depth sensor used. Furthermore, one also recognizes
% that segmentation in completely unstructured environments can be extremely
% challenging with added noise, occlusions or limited observations.


