allow camera distanceOfFocus to be used in tracking, and also return …

…the value

include/openrave/viewer.h

@@ -84,12 +84,15 @@ class OPENRAVE_API ViewerBase : public InterfaceBase
     /// \brief Set the camera transformation.
     ///
     /// \param trans new camera transformation in the world coordinate system
-    /// \param focalDistance The new focal distance of the camera (higher values is higher zoom). If 0, then the previous focal distance is preserved.
-    virtual void SetCamera(const RaveTransform<float>& trans, float focalDistance=0) OPENRAVE_DUMMY_IMPLEMENTATION;
+    /// \param distanceToFocus The new distance of the camera to the center of its rotation (higher values is higher zoom). If 0, then the previous focal distance is preserved.
+    virtual void SetCamera(const RaveTransform<float>& trans, float distanceToFocus=0) OPENRAVE_DUMMY_IMPLEMENTATION;
 
     /// \brief Return the current camera transform that the viewer is rendering the environment at.
     virtual RaveTransform<float> GetCameraTransform() const OPENRAVE_DUMMY_IMPLEMENTATION;
 
+    /// \brief Return the distance to the camera focus position (ie center of rotation)
+    virtual float GetCameraDistanceToFocus() const OPENRAVE_DUMMY_IMPLEMENTATION;
+
     /// \brief Return the closest camera intrinsics that the viewer is rendering the environment at.
     virtual geometry::RaveCameraIntrinsics<float> GetCameraIntrinsics() const OPENRAVE_DUMMY_IMPLEMENTATION;
 

plugins/qtcoinrave/qtcoinviewer.cpp

@@ -194,9 +194,9 @@ void QtCoinViewer::_InitConstructor(std::istream& sinput)
     RegisterCommand("Show", boost::bind(&QtCoinViewer::_ShowCommand, this, _1, _2),
                     "executs the show directly");
     RegisterCommand("TrackLink", boost::bind(&QtCoinViewer::_TrackLinkCommand, this, _1, _2),
-                    "camera tracks the link maintaining a specific relative transform: robotname, manipname, _fTrackingRadius");
+                    "camera tracks the link maintaining a specific relative transform: robotname, manipname, focalDistance");
     RegisterCommand("TrackManipulator", boost::bind(&QtCoinViewer::_TrackManipulatorCommand, this, _1, _2),
-                    "camera tracks the manipulator maintaining a specific relative transform: robotname, manipname, _fTrackingRadius");
+                    "camera tracks the manipulator maintaining a specific relative transform: robotname, manipname, focalDistance");
     RegisterCommand("SetTrackingAngleToUp", boost::bind(&QtCoinViewer::_SetTrackingAngleToUpCommand, this, _1, _2),
                     "sets a new angle to up");
 
@@ -210,8 +210,8 @@ void QtCoinViewer::_InitConstructor(std::istream& sinput)
     _bAutoSetCamera = true;
     _videocodec = -1;
     _bRenderFiguresInCamera = false;
-    _fTrackingRadius = 0.1;
-
+    _focalDistance = 0.0;
+    
     //vlayout = new QVBoxLayout(this);
     view1 = new QGroupBox(this);
     //vlayout->addWidget(view1, 1);
@@ -1388,6 +1388,12 @@ RaveTransform<float> QtCoinViewer::GetCameraTransform() const
     return _Tcamera*trot;
 }
 
+float QtCoinViewer::GetCameraDistanceToFocus() const
+{
+    boost::mutex::scoped_lock lock(_mutexMessages);
+    return _focalDistance;
+}
+
 geometry::RaveCameraIntrinsics<float> QtCoinViewer::GetCameraIntrinsics() const
 {
     boost::mutex::scoped_lock lock(_mutexMessages);
@@ -2347,7 +2353,11 @@ bool QtCoinViewer::_TrackLinkCommand(ostream& sout, istream& sinput)
 {
     bool bresetvelocity = true;
     std::string bodyname, linkname;
-    sinput >> bodyname >> linkname >> _fTrackingRadius >> bresetvelocity;
+    float focalDistance = 0.0;
+    sinput >> bodyname >> linkname >> focalDistance >> bresetvelocity;
+    if( focalDistance > 0 ) {
+        GetCamera()->focalDistance = focalDistance; // TODO is this thread safe?
+    }
     _ptrackinglink.reset();
     _ptrackingmanip.reset();
     EnvironmentMutex::scoped_lock lockenv(GetEnv()->GetMutex());
@@ -2366,7 +2376,11 @@ bool QtCoinViewer::_TrackManipulatorCommand(ostream& sout, istream& sinput)
 {
     bool bresetvelocity = true;
     std::string robotname, manipname;
-    sinput >> robotname >> manipname >> _fTrackingRadius >> bresetvelocity;
+    float focalDistance = 0.0;
+    sinput >> robotname >> manipname >> focalDistance >> bresetvelocity;
+    if( focalDistance > 0 ) {
+        GetCamera()->focalDistance = focalDistance; // TODO is this thread safe?
+    }
     _ptrackinglink.reset();
     _ptrackingmanip.reset();
     EnvironmentMutex::scoped_lock lockenv(GetEnv()->GetMutex());
@@ -3122,7 +3136,8 @@ void QtCoinViewer::_UpdateCameraTransform(float fTimeElapsed)
             RaveVector<float> vDestQuat = quatMultiply(quatFromAxisAngle(vup, -angle), quatRotateDirection(RaveVector<float>(0,1,0), vup));
             //transformLookat(tTrack.trans, _Tcamera.trans, vup);
 
-            RaveVector<float> vDestPos = tTrack.trans + ExtractAxisFromQuat(vDestQuat,2)*_fTrackingRadius;
+            // focal distance is the tracking radius. ie how far from the coord system camera shoud be
+            RaveVector<float> vDestPos = tTrack.trans + ExtractAxisFromQuat(vDestQuat,2)*GetCamera()->focalDistance.getValue();
 
             if(1) {
                 // PID animation
@@ -3158,6 +3173,7 @@ void QtCoinViewer::_UpdateCameraTransform(float fTimeElapsed)
     int height = centralWidget()->size().height();
     _ivCamera->aspectRatio = (float)view1->size().width() / (float)view1->size().height();
 
+    _focalDistance = GetCamera()->focalDistance.getValue();
     _camintrinsics.fy = 0.5*height/RaveTan(0.5f*GetCamera()->heightAngle.getValue());
     _camintrinsics.fx = (float)width*_camintrinsics.fy/((float)height*GetCamera()->aspectRatio.getValue());
     _camintrinsics.cx = (float)width/2;

plugins/qtcoinrave/qtcoinviewer.h

@@ -143,6 +143,7 @@ class QtCoinViewer : public QMainWindow, public ViewerBase
 
     virtual bool WriteCameraImage(int width, int height, const RaveTransform<float>& t, const SensorBase::CameraIntrinsics& KK, const std::string& filename, const std::string& extension);
     virtual void SetCamera(const RaveTransform<float>& trans, float focalDistance=0);
+    virtual float GetCameraDistanceToFocus() const;
     virtual void SetBkgndColor(const RaveVector<float>& color);
 
     virtual void PrintCamera();
@@ -420,7 +421,8 @@ public slots:
     int _videocodec;
 
     RaveTransform<float>     _initSelectionTrans;           ///< initial tarnsformation of selected item
-    RaveTransform<float> _Tcamera;
+    RaveTransform<float> _Tcamera; ///< current camera transform, read-only
+    float _focalDistance;  ///< current focal distance of the camera, read-only
     geometry::RaveCameraIntrinsics<float> _camintrinsics;
 
     unsigned int _fb;
@@ -463,7 +465,7 @@ public slots:
     KinBody::LinkPtr _ptrackinglink; ///< current link tracking
     RobotBase::ManipulatorPtr _ptrackingmanip; ///< current manipulator tracking
     Transform _tTrackingCameraVelocity; ///< camera velocity
-    float  _fTrackingRadius; ///< how far from the coord system camera shoud be
+
     //@}
 
     // data relating to playback

python/bindings/openravepy_viewer.cpp

@@ -271,6 +271,10 @@ class PyViewerBase : public PyInterfaceBase
         return ReturnTransform(_pviewer->GetCameraTransform());
     }
 
+    float GetCameraDistanceToFocus() {
+        return _pviewer->GetCameraDistanceToFocus();
+    }
+
     object GetCameraImage(int width, int height, object extrinsic, object oKK)
     {
         vector<float> vKK = ExtractArray<float>(oKK);
@@ -331,6 +335,7 @@ void init_openravepy_viewer()
                        .def("SetCamera",setcamera2,args("transform","focalDistance"), DOXY_FN(ViewerBase,SetCamera))
                        .def("SetBkgndColor",&PyViewerBase::SetBkgndColor,DOXY_FN(ViewerBase,SetBkgndColor))
                        .def("GetCameraTransform",&PyViewerBase::GetCameraTransform, DOXY_FN(ViewerBase,GetCameraTransform))
+                       .def("GetCameraDistanceToFocus", &PyViewerBase::GetCameraDistanceToFocus, DOXY_FN(ViewerBase, GetCameraDistanceToFocus))
                        .def("GetCameraImage",&PyViewerBase::GetCameraImage,args("width","height","transform","K"), DOXY_FN(ViewerBase,GetCameraImage))
         ;