DerivedValue.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 * @file DerivedValue.h
 * @date Jan 26, 2012
 * @author Duy Nguyen Ta
 */

#pragma once

#include <gtsam/base/Value.h>
#include <boost/make_shared.hpp>

// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR
#include <boost/pool/singleton_pool.hpp>

#ifdef min
#undef min
#endif

#ifdef max
#undef max
#endif

#ifdef ERROR
#undef ERROR
#endif


namespace rtabmap {

template<class DERIVED>
class DerivedValue : public gtsam::Value {

protected:
  DerivedValue() {}

public:

  virtual ~DerivedValue() {}

  virtual gtsam::Value* clone_() const {
    void *place = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
    DERIVED* ptr = new(place) DERIVED(static_cast<const DERIVED&>(*this));
    return ptr;
  }

  virtual void deallocate_() const {
    this->~DerivedValue(); // Virtual destructor cleans up the derived object
    boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this); // Release memory from pool
  }

  virtual boost::shared_ptr<gtsam::Value> clone() const {
    return boost::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
  }

  virtual bool equals_(const gtsam::Value& p, double tol = 1e-9) const {
    // Cast the base class Value pointer to a derived class pointer
    const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(p);

    // Return the result of calling equals on the derived class
    return (static_cast<const DERIVED*>(this))->equals(derivedValue2, tol);
  }

  virtual gtsam::Value* retract_(const gtsam::Vector& delta) const {
    // Call retract on the derived class
    const DERIVED retractResult = (static_cast<const DERIVED*>(this))->retract(delta);

    // Create a Value pointer copy of the result
    void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
    gtsam::Value* resultAsValue = new(resultAsValuePlace) DERIVED(retractResult);

    // Return the pointer to the Value base class
    return resultAsValue;
  }

  virtual gtsam::Vector localCoordinates_(const gtsam::Value& value2) const {
    // Cast the base class Value pointer to a derived class pointer
    const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(value2);

    // Return the result of calling localCoordinates on the derived class
    return (static_cast<const DERIVED*>(this))->localCoordinates(derivedValue2);
  }

  virtual gtsam::Value& operator=(const gtsam::Value& rhs) {
    // Cast the base class Value pointer to a derived class pointer
    const DERIVED& derivedRhs = dynamic_cast<const DERIVED&>(rhs);

    // Do the assignment and return the result
    return (static_cast<DERIVED*>(this))->operator=(derivedRhs);
  }

  operator const DERIVED& () const {
    return static_cast<const DERIVED&>(*this);
  }

  operator DERIVED& () {
    return static_cast<DERIVED&>(*this);
  }

protected:
  DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& /*rhs*/) {
    // Nothing to do, do not call base class assignment operator
    return *this;
  }

private:
  struct PoolTag { };

};

} /* namespace gtsam */