Combining Template Tracking and Laser Peak Detection for 3DReconstruction and Grasping in Underwater Environments

Mario Prats, J. Javier Fernandez and Pedro J. Sanz

Abstract— Autonomous grasping of unknown objects by arobot is a highly challenging skill that is receiving increasingattention in the last years. This problem becomes still more chal-lenging (and less explored) in underwater environments, withhighly unstructured scenarios, limited availability of sensorsand, in general, adverse conditions that affect in different degreethe robot perception and control systems. This paper describesan approach for semi-autonomous grasping and recovery onunderwater unknown objects from floating vehicles. A laserstripe emitter is attached to a robot forearm that performs ascan of a target of interest. This scan is captured by a camerathat also estimates the motion of the floating vehicle while doingthe scan. The scanned points are triangulated and transformedaccording to the motion estimation, thus recovering partial 3Dstructure of the scene with respect to a fixed frame. A user thenindicates the part where to grab the object, and the final graspis automatically planned on that area. The approach hereinpresented is tested and validated in water tank conditions.

I. INTRODUCTION

Exploration of the oceans and shallow waters is attractingthe interest of many companies and institutions all around theworld, in some cases because of the ocean valuous resources,in others because of the knowledge that it houses for scien-tists, and also for rescue purposes. Remote Operated Vehicles(ROV’s) are currently the most extended machines for doingthese tasks. In this context, expert pilots remotely control theunderwater vehicles from support vessels. However, due tothe high costs and control problems involved in ROV-basedmissions, the trend is to advance towards more autonomoussystems, i.e. Autonomous Underwater Vehicles (AUV’s).One of the most challenging problems in the migration fromtele-operated to autonomous vehicles is to automate graspingand manipulation tasks, mainly due to the limited availabilityof sensors, and to the continuous motion that naturally affectsfloating vehicles in the water.

Only a few projects are related with autonomous underwa-ter manipulation. The earlier achievements date back to the1990’s, when the UNION project [1] validated in simulationconditions coordinated control and sensing strategies forincreasing the autonomy of intervention ROVs. Simultane-ously, the European project AMADEUS [2] demonstratedthe coordinated control of two 7 degrees of freedom (DOF)arms submerged in a water tank. The ALIVE project [3]demonstrated the capability to autonomously navigate, dockand operate on an underwater panel. It is worth mentioningthe SAUVIM project [4], that recently demonstrated theautonomous recovery of seafloor objects with a 7 DOF arm,

M. Prats, J.J. Fernandez and P.J. Sanz are with Computer Science andEngineering Department, University of Jaume-I, 12071 Castellon, Spainmprats@uji.es

Fig. 1. The laser stripe emitter is mounted on a manipulator forearmthat performs a scan of the object to be grasped. The AUV design of thispicture corresponds to the future integration with the Girona 500 vehicle[7] for experiments in real scenarios. The manipulator is the Light-weightARM 5E [8].

and the CManipulator project [5], [6], which obtained inte-resting results on autonomous object grasping and connectorplugging. In all the cases, it was assumed that the objectto grasp was known in advance in terms of a 3D model orspecial visual features.

In this paper we present our approach for grasping un-known objects underwater, either with recovery purposes, orfor other applications that require a firm grasp (e.g. fixingthe vehicle to an underwater structure). Grasping objectsgenerally requires knowing at least some partial 3D structure.Different methods exist for recovering 3D information of agiven scene. They can be generally classified, according tothe used sensor, into sonar-based, laser rangefinders, vision-based and structured light. Sonar-based methods are the mostextended approach in the underwater robotics community,because of the good properties of sound propagation in thewater. However, these are more suited for long distances andnot for the short range typically required for manipulation(≈ 1m). Laser rangefinders are rare underwater, probably be-cause of the light absorption problem and floating particles.Stereo vision is the cheapest alternative, although not usefulon turbid waters, on untextured floors or in the darkness.Structured light, however, is also a cheap alternative, canwork on untextured grounds on short distances, can emitin the wavelengths that are less absorbed by the water, andoffer a good accuracy even in the darkness, although they

2012 IEEE/RSJ International Conference onIntelligent Robots and SystemsOctober 7-12, 2012. Vilamoura, Algarve, Portugal

978-1-4673-1736-8/12/S31.00 ©2012 IEEE 106

Fig. 2. Scanning of the surface with the laser stripe while doing tracking and estimating the platform motion.

need to be combined with a camera for doing triangulation.In this paper we combine a laser emitter with a vision systemin order to recover the 3D structure of unknown objectslying on the seafloor, after been scanned from a floatingvehicle. The reconstructed 3D point cloud is then used forplanning a grasp that is executed autonomously by the robot,after a simple user indication. With the combination of laserpeak detection, target tracking, 3D reconstruction and graspexecution, this is a unique system in the underwater roboticsliterature.

Laser scanning systems have been widely used in groundapplications for 3D reconstruction. For instance, [9] pre-sented a hand-held device composed of an already calibratedlaser stripe emitter and a CCD Camera. Most of the existingwork is focused on how to accurately detect the laser stripe inthe image [10] and on camera-laser calibration [11], [12]. Inunderwater environments, laser scanning has been normallyused for inspection of subsea structures [13], [14], [15]. Thisis the first approach where a laser scan is performed with anunderwater arm and combined with a grasp planner for objectrecovery purposes.

A. Considered scenario and structure of the paper

The scenario illustrated in Figure 1 is considered. Anunderwater robotic arm (in our case the CSIP Light-weightARM 5E [8]) is attached to an AUV. For the experimentsdescribed in this paper a 4 DOF vehicle prototype is used(see Figure 7). However, integration with the Girona 500 [7]vehicle has been already accomplished and may be used inthe future for experiments in real scenarios. An underwatercamera (in our case a Bowtech 550C-AL) is placed near thebase of the arm and facing downwards, i.e. looking towardsthe ground. A laser stripe emitter (in our case the TritechSeaStripe) is assembled on the forearm of the manipulator,and emits a laser stripe visible in the camera field of view(depending on the arm joint configuration). It is assumedthat the AUV has reached a pose where the target of interest

is visible in the image, and that this pose is known withrespect to a global fixed frame W . The paper is then focusedon the laser scan process from a vehicle affected by motiondisturbances, 3D reconstruction of the floor and grasping ofthe target object.

This article is organized as follows: next Section brieflyoutlies the scanning and vehicle motion estimation process;Section III describes the algorithm used for detecting thelaser stripe in the image; Section IV introduces the 3Dreconstruction process from the scan; User-guided graspplanning on the 3D point cloud is presented in SectionV; Finally, some results and conclusions are included inSections VI and VII.

II. LASER SCANNING & MOTION ESTIMATION

The floor is scanned by moving the elbow joint of themanipulator (after which the laser emitter is attached) at aconstant velocity. At the same time, two visual processingalgorithms run in parallel: the laser peak detector, in chargeof segmenting the laser stripe from the rest of the imageand computing the 3D points (see next section for details),and a template tracking algorithm that tracks a patch ofthe floor and estimates the motion of the floating platformfrom the patch motion. Indeed, it cannot be assumed that theunderwater vehicle stays fixed while doing the scan. Since allthe reconstructed 3D points have to be related to a commonframe, it is therefore necessary to estimate the local vehiclemotion while doing the scan, and use that estimation fortransforming the 3D points into a common reference system.

The visual patch for motion estimation can be initializedon any part of the image, and does not need to be of aspecific size. Small sizes will allow a faster tracking, but willbe more affected by illumination changes as those generatedby the laser stripe passing through the image. Bigger sizeswill be more robust, but require more computation time. Wemake use of Efficient Second Order Minimization templatetracking [16], which allows us to reach high tracking rates

107

even with considerable template sizes (see Figure 2 for anexample; in this case tracking was running at around 20 fpswith 200x200 pixels templates on a Pentium laptop at 1.6Ghz). It is worth noting how the tracking is robust to theillumination changes induced by the green laser stripe.

The tracking process provides, at a given time t, thecentroid of the tracked template (in pixels), and its an-gle with respect to the image horizontal (in radians), i.e.pt = (xt, yt, θt). This vector is compared at each iterationwith respect to the initial template position and orientation,p0 = (x0, y0, θ0). The distance to the floor can be easilyobtained from the vehicle sensors (e.g. a DVL), and alsocomputed from triangulation with the laser stripe. Being Zthis distance, given in meters, the displacement of the vehiclein X , Y and yaw (θ) with respect to its initial pose, can becomputed as:

X =(x0 − xt)Z

px

Y =(y0 − yt)Z

pyθ = θ0 − θt

being px and py the camera intrinsic parameters relat-ing pixels and meters. With these values, an homogeneoustransformation matrix is built at each iteration relating thecurrent vehicle pose with respect to the initial one. Thishomogeneous transformation is computed as:

0Mt =

(Rz(θ) T(X,Y, Z)

0 1

)where Rz(θ) is a 3×3 rotation matrix of θ radians around

Z axis, and T(X,Y, Z) is a 3× 1 column vector containingthe (X,Y, Z) translation.

III. LASER PEAK DETECTION

One of the first problems when dealing with 3D recons-truction based on structured light is to detect the projectedlight patterns in the image. In the case of a laser stripe,this process is called laser peak detection, and differentapproaches with variable robustness and accuracy have beenpresented in the literature [17].

The problem is to detect those pixels in the image thatbest correspond to the projected laser stripe. A state-of-the-art approach is adopted. It is composed of the following twosteps:

1) The image is segmented using a color reference si-milar to the laser stripe color. The euclidean distance(in RGB space) between each pixel color and thereference color is computed, and only those pixelswith a distance lower than a threshold are considered.Please note that underwater vehicles normally operateon dark environments that offer an advantage in thiscase. In fact, segmentation of a laser stripe on a darkbackground is easier than doing it in light conditions.The experiments presented in this paper were done

Fig. 3. A laser scan of an amphora and a sea urchin (left), and the cameraview and user grasp specification (right). The laser stripe is detected on theimage and used for the reconstruction of the 3D points.

under normal lighting. Results should be better in thedarkness of a real ocean scenario.

2) For each row in the segmented image, the peak iscomputed in sub-pixel accuracy as the centroid of thesegmented pixels.

Figure 3 shows a laser scan of an amphora and a seaurchin in a water tank, and the view from the onboardcamera. These are the two application examples that willbe used throughout the paper. The recovery of an amphoraand other ancient objects can be of interest for underwaterarchaeology, whereas grasping sea life is of great interest formarine biologists.

IV. 3D RECONSTRUCTIONFor those pixels in the image that correspond to the laser

stripe, 3D information can be recovered by triangulation.Assuming that the input image is undistorted, a pixel in theimage with row and column coordinates r and c, defines aline in projective coordinates given by the column vectorl = (lx, ly, 1) =

(c−c0px

, r−r0py

, 1)

, being c0, r0, px and pythe camera intrinsic parameters, i.e. the principal point inpixels and the focal length to pixel size ratio. If a pixel inthe image belongs in addition to the projected laser plane,the intersection of the camera ray with the laser plane givesthe 3D coordinates of the point.

The line defined by the camera ray can be expressed withits parametric equation as:

P = (X,Y, Z) = λl (1)

If, in addition, a 3D point, P, belongs to the laser plane,it holds that:

(P−P0)Tn = 0; (2)

where n is the plane normal given in camera coordinates,and P0 is a 3D point that belongs to the plane. Mergingequations 1 and 2 leads to:

108

Fig. 4. The reconstructed point clouds seen from the side. The one at thetop corresponds to the amphora. The bottom one is the sea urchin.

λ =P0

Tn

lTn

and the final 3D coordinates of the point are given byP = λl.

In order to compute these equations it is necessary toknow the laser plane equation (given by the point P0 anda normal n) in camera coordinates. As the laser emitter isattached to the robot arm, the camera-laser relationship iscomputed from the camera external calibration with respectto the arm base, and the arm kinematics. Being CMB =[CRB

CtB]

the homogeneous matrix representing thecamera frame with respect to the arm base frame, BMF therelationship between the arm base frame and the forearmframe (given by the arm kinematics), and FML the laserpose with relative to the forearm (given in our case by aCAD model), the laser-camera transformation is computedas CML = CMB

BMFFML. Then, the plane normal and

point can be expressed in camera coordinates as:

n = CRL (1 0 0)T

P0 = CtL

Let’s denote as tP the homogeneous vector containing the3D position of the point P taken with the vehicle at poset, represented by the homogeneous matrix 0Mt introducedin section II. Then, the final 3D position of the point withrespect to a common frame is computed as 0P = 0Mt

tP.Figure 4 shows the point clouds corresponding to the am-phora and the sea urchin scans, taken from a moving vehicle.

V. GRASP PLANNING

A supervisory grasping strategy is adopted: the approach isto let a human indicate the most suitable part for grasping,and then automatically plan a grasp on that part. In fact,

Fig. 5. The original point cloud is intersected with the grasp area specifiedby the user, and only the points inside that area are kept. Then, outliers areremoved and the point cloud is downsampled. The results are shown in thisimage for the grasp on the amphora (top) and the sea urchin (bottom). Redpixels indicate more proximity to the camera.

in some underwater robotic applications such as archeology,the selection of the part where to grab an object is crucialin order to avoid any damages of the item. Therefore, in ourapproach it is a human who indicates the most suitable partfor grasping, and the robot just plans a grasp around that area.The main novelty with respect to the existing approaches isthat the grasp will be later performed autonomously, and notremotely controlled by the human. The user just indicates agrasp region, but the control is performed autonomously.

A. User specification & point cloud filtering

The user input is a pair of antipodal grasp points clickedon a 2D view of the object. The projection of those 2D pointsdefine two 3D lines that are used to define a 3D volume (witha fixed width).

The user-defined 3D volume is intersected with the pointcloud, and thus, only those points lying inside are kept. Thenext step is to remove outliers and to build a downsampledcloud. All these operations highly reduce the size of the pointcloud, allowing to perform all posterior computations faster.

As an example, Figure 5(top) shows a case where a graspfor the amphora is specified (see also the grasp specificationin Figure 3, right). The final point cloud contains a smallnumber of points that approximately describe the volume ofthe object around the area indicated by the user. The bottompart of Figure 5 shows the equivalent process for the seaurchin.

109

B. Setting the grasp frame

In order to set the position and orientation of the grasp, acartesian frame, G, is defined (relative to the common frame,W ) according to the following steps:

• First, the closest and the farest points of the filteredpoint cloud relative to the camera are found. Let c =(cx cy cz) and f = (fx fy fz) be these points.From them, the local object height is computed as h =fz − cz .

• Let e be the maximum amount (in meters) the handcan envelope the object. This depends on the handgeometry, and can be generally set to the maximumdistance between the fingertips and the palm. In ourcase it is set to e = 0.05m. For larger hands, moreenveloping may be desired. From the object height,h, and the maximum enveloping distance, e, the finalgrasp depth is computed as the minimum of both, i.e.gd = min(h, e).

• After computing all the previous parameters, the graspframe position is set to CtG = (cx, cy, cz + gd).

• The grasp frame orientation is set so that the local Z axiscorresponds to the camera Z axis, and Y axis is parallelto the vector that joins the two points indicated by theuser. X axis is then set according to the right-hand rule.

• Finally, the grasp frame is expressed with respect to aworld fixed frame, W , as WMG = WMC ·CMG, beingWMC the transformation matrix that relates the cameraframe and the world frame at the moment the laser scanis started.

C. Inverse kinematics

After the grasp to perform has been specified, it is nece-ssary to compute a suitable vehicle position on top of theobject so that the required grasp is feasible. This can bedone by computing the inverse kinematics of the whole arm-vehicle kinematic chain. The obtained vehicle pose will besent to the vehicle navigation system and will need to bereached before starting the grasping action.

Our approach is to adopt a classical iterative inversejacobian method where the jacobian is computed in a specificway in order to exploit the kinematic redundancy of thewhole system. We first describe the redundancy managementtechniques that we consider, and then give more details onthe inverse kinematics solver, followed by some examples.

1) Management of redundancy: In the context of thiswork, we consider a 4 DOF vehicle with an attached roboticarm. With an arm of more than two DOF, the whole kine-matic system is redundant, thus allowing the robot to reach agiven cartesian pose with many different joint configurations.This allows to establish preferences of some configurationsover others.

In this work we adopt the Resolved Motion Rate Control(RMRC) approach for manipulator control [18], and thewell-known gradient projection method (GPM) for jointredundancy management [19]. The general approach is toproject a secondary task into the nullspace of the first task,as follows:

q = J+ExE + (I− J+

EJE)ej (3)

where xE is a desired arm end-effector velocity, q is thecorresponding joint velocity, J+

E is the pseudo-inverse of thearm jacobian at the end-effector, (I−J+

EJE) is the nullspaceprojector, and ej is a cost vector, which is normally computedas the gradient of a cost function h(q), i.e. ej = ∂h

∂q . Thereare many different possibilities of cost functions: increasemanipulability, minimize energy consumption, etc. [19]. Inour case, we define ej in order to accomplish the secondarytask of keeping the arm posture near an equilibrium configu-ration that (i) minimizes unbalancing effects on the vehicle,(ii) guarantees that the end-effector is far from the workspacelimits, and (iii) guarantees that the end-effector is in the fieldof view of the vehicle camera (situated in the bottom part ofthe vehicle and facing downwards, see Figure 1). In general,our cost vector adopts the following expression:

ej = λj(qe − q)

where qe is a desired joint equilibrium configuration, q isthe vector with current joint values and λj is the gain of thesecondary task.

2) Grasp redundancy: In our approach, the graspingaction is specified as moving a hand frame, H (attachedto the gripper, and given relative to the end-effector frame,E) towards a desired relative positioning with respect tothe grasp frame, expressed as HM∗

G. Constrained and freedegrees of freedom in this relationship can be indicated. Forthe constrained DOF, the hand frame must completely reachthe desired relative pose with respect to the grasp frame.However, for free degrees of freedom, there is no particularrelative pose used as reference. Instead, the robot controllercan choose a suitable relative pose, according to differentcriteria such as manipulability, joint limit avoidance, etc. Werefer to these directions as grasp-redundant DOF. A graspwith grasp-redundant DOF is called an under-constrainedgrasp.

3) Inverse kinematics of a joint and grasp-redundantvehicle-arm system: We assume that the kinematic model ofthe vehicle-arm system is known, and includes n degrees offreedom (DOF), four of which correspond to the underwatervehicle 3D position and yaw angle, and the rest to thearm, i.e. q = (qv qa), being qv = (x y z α), andqa = (q1 q2 . . . qn−4) . This constitutes a n DOFkinematic system for which the forward kinematic modelcan be computed following standard techniques, leading toxE = FKva(q).

The inverse kinematics can be computed following anInverse Jacobian method [20], leading to q = IKva(xE).These methods are based on Newton-Rhapson numericalalgorithms for solving systems of nonlinear equations. Oneof the main advantages is that the same RMRC controller(equation 3) that is used for real-time control can be usedhere for computing an inverse kinematics solution on redun-dant systems. The general approach is to build a virtual jointconfiguration q (for which an initial estimate is known) and

110

Fig. 6. Autonomous execution of the grasps by the robot.

control the corresponding virtual end-effector pose towardsthe desired cartesian pose, x∗

E . More concretely, the follow-ing equation is used to update the solution until convergence(i.e. ∆xE ≈ 0) :

∆q = J+E∆xE + (I− J+

EJE)ej (4)

where ∆xE = λx(x∗E − FKva(q)). The number of

redundant DOF can be further increased if grasp-redundantDOF are also considered. In the following we extend theprevious expression in order to exploit under-constrainedgrasps.

First, as the execution of the grasp is controlled at the handframe, H , a new jacobian is computed from the end-effectorjacobian, as:

JH = EW−1H · JE (5)

being EWH the twist transformation matrix associated toEMH . A modified jacobian is then computed for exploitinggrasp-redundant DOF, as Jr

H = Sc ·JH , being Sc a diagonalselection matrix that selects the degrees of freedom necessaryfor the grasp. This removes grasp-redundant DOF from themain task. Then, equation 4 is transformed into:

∆q = JrH

+∆xH + (I− JrH

+JrH)ej (6)

With this new expression, ej is projected not only onthe joint redundant DOF, but also on the grasp redundantones. Therefore, more DOF are available for the secondarytask, allowing the robot to perform the main task, whileeffectively performing auxiliary motion. Secondary tasksacting on preferred configurations for the grasp-redundantDOF could also be defined by projecting them into the jointspace.

4) Grasp inverse kinematics: The goal of the graspingaction is to match the hand frame, H , with the grasp frame,G. It is therefore possible to apply the inverse kinematicmodel to the grasp frame origin, Wg, given in an absoluteframe W , as q = IKva(Wg), leading to a suitable qv andqa for the given task.

Fig. 7. The testbed used for the experiments. The arm is attached to afloating platform that is placed in the water.

VI. RESULTS

The two application examples that have been usedthroughout the paper have been executed with a real under-water arm in a water tank. The Light-weight ARM 5E wasassembled into a floating structure and placed in the water(see Figure 7). The objects were manually placed on thefloor in a way they were in the field of view of the cameraand inside the workspace of the arm. For each object, a laserscan was performed by moving the elbow joint until the laserstripe covered all the image.

Vehicle motion was naturally generated when moving thearm because of the dynamic coupling between both subsys-tems. In addition to the vehicle motion generated becauseof the arm motion, we introduced further disturbances bymanually moving the floating structure. Figure 8 shows theamount of displacement generated on the floating structure,as measured by the motion estimator presented in section II.

Figure 6 shows the final grasp execution on the amphoraand the sea urchin with the real arm, according to theuser specification, 3D reconstruction, planning and inverse

111

Fig. 8. Motion disturbances (vehicle trajectory) generated on the floatingplatform during the amphora recovery experiment (top), and grasping of thesea urchin (bottom).

kinematics described in previous sections of this paper. Thegrasp poses where reached successfully in both cases, thusvalidating our approach in a real and practical scenario.

VII. CONCLUSIONThis paper has presented a new approach for semi-

autonomous recovery of unknown objects with underwaterrobots. First, the object 3D structure is recovered by usinga laser stripe emitter combined with a camera. The laseremitter is mounted on a robot arm that is in charge ofperforming the scanning motion from a mobile platform. Forthis, laser peak detection and template tracking algorithmsare combined. A 3D point cloud is generated after thescanning process, and filtered with a grasp region indicatedby an operator on a 2D view of the target object. This allowsto reduce the number of 3D points and to delimit the graspingarea. After that, a grasp planner computes a suitable graspon the reduced point cloud, and an inverse kinematic solvercomputes a suitable vehicle-arm pose for reaching the objectat the desired grasp pose. All the previous methods have beenvalidated in water tank conditions.

VIII. ACKNOWLEDGEMENTSThis research was partly supported by Spanish Ministry

of Research and Innovation DPI2011-27977-C03 (TRITONProject), by the European Commission Seventh Frame-work Programme FP7/2007-2013 under Grant agreement

248497 (TRIDENT Project), by Foundation Caixa Castello-Bancaixa PI.1B2011-17, and by Generalitat ValencianaACOMP/2012/252.

REFERENCES

[1] V. Rigaud, E. Coste-Maniere, M.J. Aldon, P. Probert, M. Perrier,P. Rives, D. Simon, D. Lang, J. Kiener, A. Casal, J. Amar, P. Dauchez,and M. Chantler. Union: underwater intelligent operation and navi-gation. IEEE Robotics & Automation Magazine, 5(1):25 –35, March1998.

[2] G. Marani, D. Angeletti, G. Cannata, and G. Casalino. On thefunctional and algorithmic control architecture of the amadeus dualarm robotic workcell. In WAC 2000, Hawaii (USA), June 2000.

[3] J. Evans, P. Redmond, C. Plakas, K. Hamilton, and D. Lane. Au-tonomous docking for intervention-auvs using sonar and video-basedreal-time 3d pose estimation. In OCEANS 2003, volume 4, pages2201–2210, Sept 2003.

[4] J. Yuh, S.K. Choi, C. Ikehara, G.H. Kim, G. McMurty, M. Ghasemi-Nejhad, N. Sarkar, and K. Sugihara. Design of a semi-autonomousunderwater vehicle for intervention missions (sauvim). In Proceedingsof the International Symposium on Underwater Technology, pages 63–68, April 1998.

[5] Dirk Spenneberg, Jan Albiez, Frank Kirchner, Jochen Kerdels, andSascha Fechner. C-manipulator: An autonomous dual manipulatorproject for underwater inspection and maintenance. ASME ConferenceProceedings, 2007(42703):437–443, 2007.

[6] M. Hildebrandt, J. Albiez, and F. Kirchner. Computer-based control ofdeep-sea manipulators. In OCEANS 2008 - MTS/IEEE Kobe Techno-Ocean, pages 1 –6, april 2008.

[7] D. Ribas, P. Ridao, LL. Magı, N. Palomeras, and M. Carreras. TheGirona 500, a multipurpose autonomous underwater vehicle. InProceedings of the Oceans IEEE, Santander, Spain, June 2011.

[8] CSIP. Light-weight arm 5e datasheet. http://www.eca-robotics.com/ftp/ecatalogue/520/Fiche LightWeight 5E.pdf.

[9] K. H. Strobl, W. Sepp, E. Wahl, T. Bodenmuller, M. Suppa, J. F. Seara,and G. Hirzinger. The dlr multisensory hand-guided device: the laserstripe profiler. In Proc. IEEE Int. Conf. Robotics and Automation ICRA’04, volume 2, pages 1927–1932, 2004.

[10] J. Forest, J. Salvi, E. Cabruja, and C. Pous. Laser stripe peak detectorfor 3d scanners. a fir filter approach. In Proc. 17th Int. Conf. PatternRecognition ICPR 2004, volume 3, pages 646–649, 2004.

[11] A. M. McIvor. Calibration of a laser stripe profiler. In Proc. SecondInt 3-D Digital Imaging and Modeling Conf, pages 92–98, 1999.

[12] W. Stocher and G. Biegelbauer. Automated simultaneous calibrationof a multi-view laser stripe profiler. In Proc. IEEE Int. Conf. Roboticsand Automation ICRA 2005, pages 4424–4429, 2005.

[13] A. Gordon. Use of laser scanning system on mobile underwaterplatforms. In Proc. Symp. Autonomous Underwater Vehicle TechnologyAUV ’92, pages 202–205, 1992.

[14] Chau-Chang Wang, Shiahn-Wern Shyue, and Shi-Her Cheng. Under-water structure inspection with laser light stripes. In Proc. Int. Symp.Underwater Technology UT 00, pages 201–205, 2000.

[15] Chau-Chang Wang and Dajun Tang. Seafloor roughness measured bya laser line scanner and a conductivity probe. IEEE Journal of OceanEngineering, 34(4):459–465, 2009.

[16] S. Benhimane and E. Malis. Real-time image-based tracking of planesusing efficient second-order minimization. In IEEE/RSJ InternationalConference on Intelligent Robots and Systems, pages 943–948, 2004.

[17] Hsin-Hung Chen, Chau-Chang Wang, and Fu-Chuan Hung. Compar-ative assessments of peak detection algorithms for underwater laserranging. In Proc. OCEANS ’04. MTTS/IEEE TECHNO-OCEAN ’04,volume 3, pages 1229–1234, 2004.

[18] D. E. Whitney. Resolved motion rate control of manipulators andhuman prostheses. IEEE Transactions on Man Machine Systems,10(2):47–53, 1969.

[19] Bruno Siciliano. Kinematic control of redundant robot manipulators:A tutorial. Journal of Intelligent and Robotic Systems, 3(3):201–212,1990.

[20] Jorge Angeles. On the numerical solution of the inverse kinematicproblem. International Journal of Robotics Research, 4(2):21–37,1985.

112