%% \textbf{Planning to manipulate}: Given the underlying articulation model
%%   of an object, our framework is capable of predicting the motion of the object
%%   given the knowledge of the object being manipulated.         

% Top level figure with all results
%
\begin{figure*}[!t]
  \centering
    \includegraphics[width=2\columnwidth]{graphics/pipeline-figure/results-fig.png}\\
  \caption{Articulation learning and motion prediction for various objects.}
  \label{fig:results-fig}
\end{figure*}


\subsection{Learning to Predict Articulated Motion}
%
Our daily environment is filled with articulated objects with
which we repeatedly interact. A robot in our environment can
identify instances of articulated objects that it has observed
in the past, then use a learned model to predict the motion 
of an object when it is used.

% MSER manifold
%
\begin{figure}[H]
  \centering
  \subfigure[Extracted MSER]{\includegraphics[width=0.5\columnwidth]{graphics/prediction/fridge-wide-baseline/mser-depth.jpg}}~
  \subfigure[Estimated Motion Manifold]{\includegraphics[width=0.5\columnwidth]{graphics/prediction/fridge-wide-baseline/img-viewpoint-learned.jpg}}

  \caption{The motion manifold of an articulated object, extracted via MSERs.}
  \label{fig:mser-surface}
\end{figure}

Once the kinematic model of an articulated object is learned, the kinematic
structure $\hat{G}$ and its model parameters $\hat{\mathcal{M}_{ij}}, (ij) \in
\hat{G}$ are stored in a database, along with its appearance model. The feature
descriptors extracted (described in Section~\ref{sec:feature-tracking}) for each
cluster $C_i$ of the articulated object are also retained for object recognition
in future encounters. Demonstrations involving the same instance of the
articulated object are represented in a single arbitrarily selected reference
frame, and kept consistent across encounters by registering
newer demonstrations into the initial object frame. Each of these
attributes is stored in the database for convenient querying in the
future. Thus, on encountering the same object instance in the future, the robot
can match the descriptors extracted from the current scene with those extracted
from object instances it learned in the past. It then recovers the original
demonstration reference frame along with the relevant kinematic structure of the
articulated object for prediction purposes. We identify the
surface of the manipulated object by extracting Maximally Stable Extremal
Regions (MSER)~\cite{matas2004robust} (Figure~\ref{fig:mser-surface})
for each object part undergoing motion. We use this surface to
visualize the motion manifold of the articulated object.

% % % % Fridge sift match
% % %
% % \begin{figure}[H]
% %   \centering
% %   \renewcommand{\tabcolsep}{1.5pt}
% %   \begin{tabular}{cc}
% %     \multicolumn{2}{c}{\includegraphics[width=0.75\columnwidth]{graphics/prediction/fridge/sift-match-imgs.png}}\\
% %     \multicolumn{2}{c}{\includegraphics[width=0.75\columnwidth]{graphics/prediction/fridge/sift-match.png}}\\
    
% %     \includegraphics[width=0.5\columnwidth, clip, trim=10mm 10mm 10mm
% %     10mm]{graphics/prediction/fridge/fridge-source-target-cloud.png}&
% %     \includegraphics[width=0.5\columnwidth, clip, trim=10mm 10mm 10mm
% %     10mm]{graphics/prediction/fridge/fridge-source-target-cloud-match.png}\\
% %   \end{tabular}
% %   \caption{Visualization of re-localization of the object instance at
% %     a future encounter. \textbf{Middle:} The query instance of the fridge is matched with a
% %     previously learned demonstration.  \textbf{Bottom:} The query 3-D model is
% %     co-registered with the learned object instance model, and the relative pose
% %     transformation is correctly estimated and visualized.}
% %   \label{fig:viewpoint-estimation-fridge}
% % \end{figure}

% Given several demonstrations of manipulating articulated objects such as doors,
% drawers, refrigerators, etc. our system is reliably able to recognize the
% objects in future encounters, and is also capable of providing a hallucination
% of its motion when manipulated. Figures~\ref{fig:viewpoint-estimation-drawer},
% ~\ref{fig:motion-manifold-prediction-drawer},
% ~\ref{fig:viewpoint-estimation-fridge},
% ~\ref{fig:motion-manifold-prediction-fridge},
% ~\ref{fig:viewpoint-estimation-printer},
% ~\ref{fig:motion-manifold-prediction-printer} illustrate the recovery of
% different articulated object instances at independent future encounters. 

% % Fridge motion manifold
% %
% \begin{figure}[H]
%   \centering
%   \renewcommand{\tabcolsep}{1.5pt}
%   \begin{tabular}{cc}
%     \includegraphics[height=1.5in]{graphics/prediction/fridge/fridge-source-manifold.png}& 
%     \includegraphics[height=1.5in]{graphics/prediction/fridge/fridge-manifold-learned.png}\\
%     \multicolumn{2}{c}{(a) Motion manifold \textbf{learned} from visual demonstration.}\\\\

%     \includegraphics[height=1.5in]{graphics/prediction/fridge/fridge-target-manifold.png}&
%     \includegraphics[height=1.5in]{graphics/prediction/fridge/fridge-manifold-predicted.png}\\
%     \multicolumn{2}{c}{(b) Motion manifold \textbf{predicted} from visual demonstration.}\\
%   \end{tabular}
%   \caption{Given the co-registered viewpoints, the kinematic model of the
%     refrigerator is recovered and projected in
%     the query reference frame. The motion manifold predicted lies closely on the
%     expected motion manifold of the object indicating a qualitatively good
%     prediction. }
%   \label{fig:motion-manifold-prediction-fridge}
% \end{figure}

% % % Drawer sift match
% % %
% % \begin{figure}[H]
% %   \centering
% %   \renewcommand{\tabcolsep}{1.5pt}
% %   \begin{tabular}{cc}
% %     \multicolumn{2}{c}{\includegraphics[width=0.75\columnwidth]{graphics/prediction/drawer/sift-match-imgs.png}}\\
% %     \multicolumn{2}{c}{\includegraphics[width=0.75\columnwidth]{graphics/prediction/drawer/sift-match.png}}\\
    
% %   \end{tabular}
% %   \caption{Drawer instance re-localization via matched SURF features}
% %   \label{fig:viewpoint-estimation-drawer}
% % \end{figure}

% % Drawer motion manifold
% %
% \begin{figure}[H]
%   \centering
%   \renewcommand{\tabcolsep}{1.5pt}
%   \begin{tabular}{cc}
%     \includegraphics[width=0.5\columnwidth]{graphics/prediction/drawer/drawer-manifold-learned.png}&
%     \includegraphics[width=0.5\columnwidth]{graphics/prediction/drawer/drawer-manifold-predicted.png}\\
%   \end{tabular}
%   \caption{Prediction visualization shown in both states suggests a prismatic motion of
% each of the individual drawers}
%   \label{fig:motion-manifold-prediction-drawer}
% \end{figure}


% % % printer sift match
% % %
% % \begin{figure}[H]
% %   \centering
% %   \renewcommand{\tabcolsep}{1.5pt}
% %   \begin{tabular}{cc}
% %     \multicolumn{2}{c}{\includegraphics[width=0.75\columnwidth]{graphics/prediction/printer/sift-match-imgs.png}}\\
% %     \multicolumn{2}{c}{\includegraphics[width=0.75\columnwidth]{graphics/prediction/printer/sift-match.png}}\\
    
% %   \end{tabular}
% %   \caption{Printer instance re-localization via matched SURF features}
% %   \label{fig:viewpoint-estimation-printer}
% % \end{figure}

% % printer motion manifold
% %
% \begin{figure}[H]
%   \centering
%   \renewcommand{\tabcolsep}{1.5pt}
%   \begin{tabular}{cc}
%     \includegraphics[width=0.5\columnwidth]{graphics/prediction/printer/printer-pose-estimation.png}&
%     \includegraphics[width=0.5\columnwidth]{graphics/prediction/printer/printer-pose-prediction.png}\\
%   \end{tabular}
%   \caption{Prediction visualization shown in both states suggests a rotational motion
%     of the printer on top (for scanner use)}
%   \label{fig:motion-manifold-prediction-printer}
% \end{figure}



% % For each instance of
% % the object in the scene, we train on two visual demonstrations each recorded
% % from a different viewpoint, and test our prediction on a third viewpoint that
% % has not been used during the training phase. We bootstrap the availability of
% % AprilTags as identifiers or object detectors that annotate the training and
% % prediction phases with only a single initial pose and object ID. The pipeline
% % then performs a look up of the object ID from its database and retrieves the
% % appropriate model to predict its motion. In the event that the object ID is not
% % retrievable from the DB, we learn a new model and add the parameterized model
% % along with the object ID to the DB.
