\subsection{Multi-Rigid-Body Pose Optimization}
%
Given the cluster label assignment for each feature trajectory, we
subsequently determine the 6-DOF motion of each cluster. We define $Z_i^t$ as
the set of features belonging to cluster $C_i$ at time $t$. Additionally, we
define $\textbf{X} = {X_1, \dots, X_k}$ as the set of $SE(3)$ poses estimated
for each of $k$ clusters considered, and $x_i^t \in X_i$ as the $SE(3)$ pose
estimated for the $i^{th}$ cluster at time $t$.

% % $Z_i$ as the
% set of feature trajectories associated with the cluster $C_i$, and consequently
% denote 
% SIMPLICITY refer $x_i^t$ as $x^t$

% Since the
% trajectories on a finer-grained level are prone to sensor noise, we consider the
% optimization of the pose of a cluster as a whole. Additionally, we treat the
% 6-DOF motion of the cluster as a smooth continuous motion by adding a constant
% velocity-model assumption to filter the noisy 6-DOF pose estimates retrieved
% from the raw sensor observations. 

For each cluster $C_i$, we consider the synchronized sensor
observations of position and surface normals for each of its
trajectories, and use the arbitrary pose $x_i^0$ as the reference
frame for the remaining pose estimates of the $i^{th}$
cluster. Subsequently, we compute the relative transformation
$\Delta_i^{t-1,t}$ between successive time steps $t-1$ and $t$ for the
$i^{th}$ cluster using the known correspondences between $Z_i^{t-1}$
and $Z_i^t$. Since this step can lead to drift, we add an additional
sparse set of relative pose constraints every 10 frames, denoted as
$\Delta_i^{t-10,t}$. Our implementation employs a correspondence
rejection step that eliminates outliers falling outside the inlier
distance threshold of $1$~cm, as in RANSAC~\cite{fischler1981random},
making the pose estimation routine more robust to sensor noise.

% \begin{figure}[t!]
%   \centering
%   \subfigure[BLAH]{\includegraphics[width=0.5\columnwidth]{graphics/pose-estimation/drawer-pose-estimation.jpg}}~
%   \subfigure[BLAH]{\includegraphics[width=0.5\columnwidth]{graphics/pose-estimation/fridge-pose-estimation.jpg}}
%   \subfigure[BLAH]{\includegraphics[width=0.5\columnwidth]{graphics/pose-estimation/drawer-pose-estimation-closeup.jpg}}~
%   \subfigure[BLAH]{\includegraphics[width=0.5\columnwidth]{graphics/pose-estimation/fridge-pose-estimation-closeup.jpg}}
%   \caption{Visualization of pose estimates obtained after 6-DOF pose
%     optimization using iSAM. Figures
%     show consistent 6-DOF pose estimates despite noisy 3-D feature observations.}
%   \label{fig:pose-optimization-lst-sq}
% \end{figure}

We augment the estimation step with an optimization phase to provide
smooth and continuous pose estimates for each cluster by incorporating
a motion model. We use the 3-D pose optimizer
iSAM~\cite{kaess2012isam2} to incorporate the relative pose
constraints within a factor graph, with node factors derived directly
from the pose estimates.  A constant-velocity edge factor term is also
added to provide continuity in the articulated motion.

% , with a constant spatial noise
% covariance of $0.02~$m, and rotational noise covariance of
% $0.2~$radians. 
% with a
% noise covariance of $0.01~$m, and $0.2~$radians across each observation
% dimension

% Figure~\ref{fig:pose-optimization-lst-sq}
% demonstrates a few examples of the pose optimization routine given the labeled
% feature trajectories.

