Skip to content

Commit

Permalink
Derive the fixed landmarks from the standard landmarks (#259)
Browse files Browse the repository at this point in the history
  • Loading branch information
@svwilliams
svwilliams authored Jul 5, 2023
1 parent 8a6f010 commit 05812c3
Show file tree
Hide file tree
Showing 8 changed files with 53 additions and 164 deletions.
61 changes: 8 additions & 53 deletions fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,37 +36,27 @@

#include <fuse_core/fuse_macros.h>
#include <fuse_core/serialization.h>
#include <fuse_variables/fixed_size_variable.h>
#include <fuse_variables/point_2d_landmark.h>

#include <boost/serialization/access.hpp>
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/export.hpp>

#include <ostream>

namespace fuse_variables
{
/**
* @brief Variable representing a 2D point landmark that exists across time.
*
* This is commonly used to represent locations of visual features. The UUID of this class is constant after
* construction and dependent on a user input database id. As such, the database id cannot be altered after
* construction.
* This is commonly used to represent locations of visual features. This class differs from the Point2DLandmark in that
* the value of the landmark is held constant during optimization. This is appropriate if the landmark positions are
* known or were previously estimated to sufficient accuracy. The UUID of this class is constant after construction and
* dependent on a user input database id. As such, the database id cannot be altered after construction.
*/
class Point2DFixedLandmark : public FixedSizeVariable<2>
class Point2DFixedLandmark : public Point2DLandmark
{
public:
FUSE_VARIABLE_DEFINITIONS(Point2DFixedLandmark);

/**
* @brief Can be used to directly index variables in the data array
*/
enum : size_t
{
X = 0,
Y = 1
};

/**
* @brief Default constructor
*/
Expand All @@ -79,48 +69,14 @@ class Point2DFixedLandmark : public FixedSizeVariable<2>
*/
explicit Point2DFixedLandmark(const uint64_t& landmark_id);

/**
* @brief Read-write access to the X-axis position.
*/
double& x() { return data_[X]; }

/**
* @brief Read-only access to the X-axis position.
*/
const double& x() const { return data_[X]; }

/**
* @brief Read-write access to the Y-axis position.
*/
double& y() { return data_[Y]; }

/**
* @brief Read-only access to the Y-axis position.
*/
const double& y() const { return data_[Y]; }

/**
* @brief Read-only access to the id
*/
const uint64_t& id() const { return id_; }

/**
* @brief Print a human-readable description of the variable to the provided
* stream.
*
* @param[out] stream The stream to write to. Defaults to stdout.
*/
void print(std::ostream& stream = std::cout) const override;

/**
* @brief Specifies if the value of the variable should not be changed during optimization
*/
bool holdConstant() const override;
bool holdConstant() const override { return true; }

private:
// Allow Boost Serialization access to private methods
friend class boost::serialization::access;
uint64_t id_ { 0 };

/**
* @brief The Boost Serialize method that serializes all of the data members
Expand All @@ -134,8 +90,7 @@ class Point2DFixedLandmark : public FixedSizeVariable<2>
template <class Archive>
void serialize(Archive& archive, const unsigned int /* version */)
{
archive& boost::serialization::base_object<FixedSizeVariable<SIZE>>(*this);
archive& id_;
archive& boost::serialization::base_object<Point2DLandmark>(*this);
}
};

Expand Down
10 changes: 10 additions & 0 deletions fuse_variables/include/fuse_variables/point_2d_landmark.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@

#include <fuse_core/fuse_macros.h>
#include <fuse_core/serialization.h>
#include <fuse_core/uuid.h>
#include <fuse_variables/fixed_size_variable.h>

#include <boost/serialization/access.hpp>
Expand Down Expand Up @@ -112,6 +113,15 @@ class Point2DLandmark : public FixedSizeVariable<2>
*/
void print(std::ostream& stream = std::cout) const override;

protected:
/**
* @brief Construct a point 2D variable given a UUID and a landmarks id
*
* @param[in] uuid The UUID for this variable
* @param[in] landmark_id The id associated to a landmark
*/
Point2DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id);

private:
// Allow Boost Serialization access to private methods
friend class boost::serialization::access;
Expand Down
72 changes: 8 additions & 64 deletions fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,38 +36,27 @@

#include <fuse_core/fuse_macros.h>
#include <fuse_core/serialization.h>
#include <fuse_variables/fixed_size_variable.h>
#include <fuse_variables/point_3d_landmark.h>

#include <boost/serialization/access.hpp>
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/export.hpp>

#include <ostream>

namespace fuse_variables
{
/**
* @brief Variable representing a 3D point landmark that exists across time.
*
* This is commonly used to represent locations of visual features. The UUID of this class is constant after
* construction and dependent on a user input database id. As such, the database id cannot be altered after
* construction.
* This is commonly used to represent locations of visual features. This class differs from the Point3DLandmark in that
* the value of the landmark is held constant during optimization. This is appropriate if the landmark positions are
* known or were previously estimated to sufficient accuracy. The UUID of this class is constant after construction and
* dependent on a user input database id. As such, the database id cannot be altered after construction.
*/
class Point3DFixedLandmark : public FixedSizeVariable<3>
class Point3DFixedLandmark : public Point3DLandmark
{
public:
FUSE_VARIABLE_DEFINITIONS(Point3DFixedLandmark);

/**
* @brief Can be used to directly index variables in the data array
*/
enum : size_t
{
X = 0,
Y = 1,
Z = 2
};

/**
* @brief Default constructor
*/
Expand All @@ -80,58 +69,14 @@ class Point3DFixedLandmark : public FixedSizeVariable<3>
*/
explicit Point3DFixedLandmark(const uint64_t& landmark_id);

/**
* @brief Read-write access to the X-axis position.
*/
double& x() { return data_[X]; }

/**
* @brief Read-only access to the X-axis position.
*/
const double& x() const { return data_[X]; }

/**
* @brief Read-write access to the Y-axis position.
*/
double& y() { return data_[Y]; }

/**
* @brief Read-only access to the Y-axis position.
*/
const double& y() const { return data_[Y]; }

/**
* @brief Read-write access to the Z-axis position.
*/
double& z() { return data_[Z]; }

/**
* @brief Read-only access to the Z-axis position.
*/
const double& z() const { return data_[Z]; }

/**
* @brief Read-only access to the id
*/
const uint64_t& id() const { return id_; }

/**
* @brief Print a human-readable description of the variable to the provided
* stream.
*
* @param[out] stream The stream to write to. Defaults to stdout.
*/
void print(std::ostream& stream = std::cout) const override;

/**
* @brief Specifies if the value of the variable should not be changed during optimization
*/
bool holdConstant() const override;
bool holdConstant() const override { return true; }

private:
// Allow Boost Serialization access to private methods
friend class boost::serialization::access;
uint64_t id_ { 0 };

/**
* @brief The Boost Serialize method that serializes all of the data members
Expand All @@ -145,8 +90,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3>
template <class Archive>
void serialize(Archive& archive, const unsigned int /* version */)
{
archive& boost::serialization::base_object<FixedSizeVariable<SIZE>>(*this);
archive& id_;
archive& boost::serialization::base_object<Point3DLandmark>(*this);
}
};

Expand Down
9 changes: 9 additions & 0 deletions fuse_variables/include/fuse_variables/point_3d_landmark.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@

#include <fuse_core/fuse_macros.h>
#include <fuse_core/serialization.h>
#include <fuse_core/uuid.h>
#include <fuse_variables/fixed_size_variable.h>

#include <boost/serialization/access.hpp>
Expand Down Expand Up @@ -126,6 +127,14 @@ class Point3DLandmark : public FixedSizeVariable<3>
*/
void print(std::ostream& stream = std::cout) const override;

protected:
/**
* @brief Construct a point 3D variable given a landmarks id
*
* @param[in] landmark_id The id associated to a landmark
*/
Point3DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id);

private:
// Allow Boost Serialization access to private methods
friend class boost::serialization::access;
Expand Down
23 changes: 2 additions & 21 deletions fuse_variables/src/point_2d_fixed_landmark.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,35 +35,16 @@

#include <fuse_core/uuid.h>
#include <fuse_core/variable.h>
#include <fuse_variables/fixed_size_variable.h>
#include <fuse_variables/point_2d_landmark.h>
#include <pluginlib/class_list_macros.hpp>

#include <boost/serialization/export.hpp>

#include <ostream>

namespace fuse_variables
{
Point2DFixedLandmark::Point2DFixedLandmark(const uint64_t& landmark_id) :
FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)),
id_(landmark_id)
{
}

void Point2DFixedLandmark::print(std::ostream& stream) const
{
stream << type() << ":\n"
<< " uuid: " << uuid() << "\n"
<< " size: " << size() << "\n"
<< " landmark id: " << id() << "\n"
<< " data:\n"
<< " - x: " << x() << "\n"
<< " - y: " << y() << "\n";
}

bool Point2DFixedLandmark::holdConstant() const
Point2DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id)
{
return true;
}

} // namespace fuse_variables
Expand Down
9 changes: 7 additions & 2 deletions fuse_variables/src/point_2d_landmark.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,12 +44,17 @@

namespace fuse_variables
{
Point2DLandmark::Point2DLandmark(const uint64_t& landmark_id) :
FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)),
Point2DLandmark::Point2DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id) :
FixedSizeVariable(uuid),
id_(landmark_id)
{
}

Point2DLandmark::Point2DLandmark(const uint64_t& landmark_id) :
Point2DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id)
{
}

void Point2DLandmark::print(std::ostream& stream) const
{
stream << type() << ":\n"
Expand Down
24 changes: 2 additions & 22 deletions fuse_variables/src/point_3d_fixed_landmark.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,36 +35,16 @@

#include <fuse_core/uuid.h>
#include <fuse_core/variable.h>
#include <fuse_variables/fixed_size_variable.h>
#include <fuse_variables/point_3d_landmark.h>
#include <pluginlib/class_list_macros.hpp>

#include <boost/serialization/export.hpp>

#include <ostream>

namespace fuse_variables
{
Point3DFixedLandmark::Point3DFixedLandmark(const uint64_t& landmark_id) :
FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)),
id_(landmark_id)
{
}

void Point3DFixedLandmark::print(std::ostream& stream) const
{
stream << type() << ":\n"
<< " uuid: " << uuid() << "\n"
<< " size: " << size() << "\n"
<< " landmark id: " << id() << "\n"
<< " data:\n"
<< " - x: " << x() << "\n"
<< " - y: " << y() << "\n"
<< " - z: " << z() << "\n";
}

bool Point3DFixedLandmark::holdConstant() const
Point3DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id)
{
return true;
}

} // namespace fuse_variables
Expand Down
9 changes: 7 additions & 2 deletions fuse_variables/src/point_3d_landmark.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,12 +44,17 @@

namespace fuse_variables
{
Point3DLandmark::Point3DLandmark(const uint64_t& landmark_id) :
FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)),
Point3DLandmark::Point3DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id) :
FixedSizeVariable(uuid),
id_(landmark_id)
{
}

Point3DLandmark::Point3DLandmark(const uint64_t& landmark_id) :
Point3DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id)
{
}

void Point3DLandmark::print(std::ostream& stream) const
{
stream << type() << ":\n"
Expand Down

0 comments on commit 05812c3

Please sign in to comment.