types_sba.h

Go to the documentation of this file.

// g2o - General Graph Optimization
// Copyright (C) 2011 Kurt Konolige
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#ifndef G2O_SBA_TYPES
#define G2O_SBA_TYPES

#include "g2o/core/base_vertex.h"
#include "g2o/core/base_binary_edge.h"
#include "g2o/core/base_multi_edge.h"
#include "sbacam.h"
#include <Eigen/Geometry>
#include <iostream>

#include "g2o_types_sba_api.h"

namespace g2o {

class G2O_TYPES_SBA_API VertexIntrinsics : public BaseVertex<4, Eigen::Matrix<double, 5, 1, Eigen::ColMajor> >
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexIntrinsics();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;
      
    virtual void setToOriginImpl() {
      _estimate << 1., 1., 0.5, 0.5, 0.1;
    }
      
    virtual void oplusImpl(const double* update)
    {
      _estimate.head<4>() += Vector4D(update);
    }
 };

  class G2O_TYPES_SBA_API VertexCam : public BaseVertex<6, SBACam>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexCam();

    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    virtual void setToOriginImpl() {
      _estimate = SBACam();
    }
    
    virtual void setEstimate(const SBACam& cam){
      BaseVertex<6, SBACam>::setEstimate(cam);
      _estimate.setTransform();
      _estimate.setProjection();
      _estimate.setDr();
    }
    
    virtual void oplusImpl(const double* update)
    {
      Eigen::Map<const Vector6d> v(update);
      _estimate.update(v);
      _estimate.setTransform();
      _estimate.setProjection();
      _estimate.setDr();
    }
    

    virtual bool setEstimateDataImpl(const double* est){
      Eigen::Map <const Vector7d> v(est);
      _estimate.fromVector(v);
      return true;
    }

    virtual bool getEstimateData(double* est) const{
      Eigen::Map <Vector7d> v(est);
      v = estimate().toVector();
      return true;
    }

    virtual int estimateDimension() const {
      return 7;
    }

    virtual bool setMinimalEstimateDataImpl(const double* est){
      Eigen::Map<const Vector6d> v(est);
      _estimate.fromMinimalVector(v);
      return true;
    }

    virtual bool getMinimalEstimateData(double* est) const{
      Eigen::Map<Vector6d> v(est);
      v = _estimate.toMinimalVector();
      return true;
    }

    virtual int minimalEstimateDimension() const {
      return 6;
    }
 };

 class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3D>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW    
    VertexSBAPointXYZ();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    virtual void setToOriginImpl() {
      _estimate.fill(0.);
    }

    virtual void oplusImpl(const double* update)
    {
      Eigen::Map<const Vector3D> v(update);
      _estimate += v;
    }
};


// monocular projection
// first two args are the measurement type, second two the connection classes
 class G2O_TYPES_SBA_API EdgeProjectP2MC : public  BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexCam> 
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeProjectP2MC();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    // return the error estimate as a 2-vector
    void computeError()
    {
      // from <Point> to <Cam>
      const VertexSBAPointXYZ *point = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
      const VertexCam *cam = static_cast<const VertexCam*>(_vertices[1]);

      // calculate the projection
      const Vector3D &pt = point->estimate();
      Vector4D ppt(pt(0),pt(1),pt(2),1.0);
      Vector3D p = cam->estimate().w2i * ppt;
      Vector2D perr;
      perr = p.head<2>()/p(2);
      //      std::cout << std::endl << "CAM   " << cam->estimate() << std::endl;
      //      std::cout << "POINT " << pt.transpose() << std::endl;
      //      std::cout << "PROJ  " << p.transpose() << std::endl;
      //      std::cout << "CPROJ " << perr.transpose() << std::endl;
      //      std::cout << "MEAS  " << _measurement.transpose() << std::endl;

      // error, which is backwards from the normal observed - calculated
      // _measurement is the measured projection
      _error = perr - _measurement;
      // std::cerr << _error.x() << " " << _error.y() <<  " " << chi2() << std::endl;
    }

    // jacobian
    virtual void linearizeOplus();
};

// stereo projection
// first two args are the measurement type, second two the connection classes
 class G2O_TYPES_SBA_API EdgeProjectP2SC : public  BaseBinaryEdge<3, Vector3D, VertexSBAPointXYZ, VertexCam>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeProjectP2SC();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    // return the error estimate as a 2-vector
    void computeError()
    {
      // from <Point> to <Cam>
      const VertexSBAPointXYZ *point = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
      VertexCam *cam = static_cast<VertexCam*>(_vertices[1]);

      // calculate the projection
      Vector3D kp;
      Vector4D pt;
      pt.head<3>() = point->estimate();
      pt(3) = 1.0;
      const SBACam& nd = cam->estimate();
      // these should be already ok
      /* nd.setTransform(); */
      /* nd.setProjection(); */
      /* nd.setDr(); */

      Vector3D p1 = nd.w2i * pt; 
      Vector3D p2 = nd.w2n * pt; 
      Vector3D pb(nd.baseline,0,0);

      double invp1 = 1.0/p1(2);
      kp.head<2>() = p1.head<2>()*invp1;

      // right camera px
      p2 = nd.Kcam*(p2-pb);
      kp(2) = p2(0)/p2(2);

      // std::cout << std::endl << "CAM   " << cam->estimate() << std::endl; 
      // std::cout << "POINT " << pt.transpose() << std::endl; 
      // std::cout << "PROJ  " << p1.transpose() << std::endl; 
      // std::cout << "PROJ  " << p2.transpose() << std::endl; 
      // std::cout << "CPROJ " << kp.transpose() << std::endl; 
      // std::cout << "MEAS  " << _measurement.transpose() << std::endl; 

      // error, which is backwards from the normal observed - calculated
      // _measurement is the measured projection
      _error = kp - _measurement;
    }

    // jacobian
    virtual void linearizeOplus();

};

// monocular projection with parameter calibration
// first two args are the measurement type, second two the connection classes
 class G2O_TYPES_SBA_API EdgeProjectP2MC_Intrinsics : public  BaseMultiEdge<2, Vector2D> 
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeProjectP2MC_Intrinsics();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    // return the error estimate as a 2-vector
    void computeError()
    {
      // from <Point> to <Cam>, the intrinsics in KCam should be already set!
      const VertexSBAPointXYZ *point = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
      VertexCam *cam = static_cast<VertexCam*>(_vertices[1]);
      // calculate the projection
      const Vector3D &pt = point->estimate();
      Vector4D ppt(pt(0),pt(1),pt(2),1.0);
      Vector3D p = cam->estimate().w2i * ppt;
      Vector2D perr = p.head<2>()/p(2);
      _error = perr - _measurement;
    }

    // jacobian
    virtual void linearizeOplus();

};


 class G2O_TYPES_SBA_API EdgeSBACam : public BaseBinaryEdge<6, SE3Quat, VertexCam, VertexCam>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeSBACam();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;
    void computeError()
    {
      const VertexCam* v1 = dynamic_cast<const VertexCam*>(_vertices[0]);
      const VertexCam* v2 = dynamic_cast<const VertexCam*>(_vertices[1]);
      SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
      _error[0]=delta.translation().x();
      _error[1]=delta.translation().y();
      _error[2]=delta.translation().z();
      _error[3]=delta.rotation().x();
      _error[4]=delta.rotation().y();
      _error[5]=delta.rotation().z();
    }
    
    virtual void setMeasurement(const SE3Quat& meas){
      _measurement=meas;
      _inverseMeasurement=meas.inverse();
    }

    virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;}
    virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to);

    virtual bool setMeasurementData(const double* d){
      Eigen::Map<const Vector7d> v(d);
      _measurement.fromVector(v);
      _inverseMeasurement = _measurement.inverse();
      return true;
    }

    virtual bool getMeasurementData(double* d) const{
      Eigen::Map<Vector7d> v(d);
      v = _measurement.toVector();
      return true;
    }

    virtual int measurementDimension() const {return 7;}

    virtual bool setMeasurementFromState();
    
  protected:
    SE3Quat _inverseMeasurement;
};


 class G2O_TYPES_SBA_API EdgeSBAScale : public BaseBinaryEdge<1, double, VertexCam, VertexCam>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeSBAScale();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;
    void computeError()
    {
      const VertexCam* v1 = dynamic_cast<const VertexCam*>(_vertices[0]);
      const VertexCam* v2 = dynamic_cast<const VertexCam*>(_vertices[1]);
      Vector3D dt=v2->estimate().translation()-v1->estimate().translation();
      _error[0] = _measurement - dt.norm();
    }
    virtual void setMeasurement(const double& m){
      _measurement = m;
    }
    virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;}
    virtual void initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_);
};



} // end namespace

#endif // SBA_TYPES