Merge pull request #2892 from savuor:posegraph_no_ceres
Pose Graph rewritten without Ceres * Works well on g2o dataset, cumulative: 1. Pose3d done w/o Eigen types; 2. PoseGraph nodes vector<Node> -> map<int, Node> 3. Eigen is not used in cost function or parametrization 4. Cost function debugged & fixed (original one was wrong), rewritten from Automatic to Analytic * g2o dataset reading added to PoseGraph * sparse solver fixes from DynaFu draft * Eigen cost function and parametrization removed + g2o reading fixed * refactored: pose error, pose graph edge, pose graph node * sparse solve: templated * MyOptimize(): 1st version * several fixes and TODOs for future * sparse block matrix: val functions, template type * works at Ceres quality (cleanup needed) * MyOptimize() is set to default optimizer * Ceres thrown away, PoseGraph class and header/source code reorganized * pose, node, edge -> nested for PoseGraph * warnings fixed * jacobiScaling disabled for better performance + minors * trailing whitespace fixed * more warnings fixed * message added: Eigen is required for build + minors * trying to fix warning * try to fix "unreachable code" warning * trying to fix unreachable code, pt.3 * trying to fix unreachable code, pt. 5 * trying to fix unreachable code, pt. the worst + minors * try to fix unreachable code, pt. the ugliest * trying to fix unreachable code, pt. the grumpiest * cout -> CV_LOG_INFO * quat matrix functions moved outside cv and kinfu namespaces * unused function fix * pose graph made public (but in detail namespace) + test for pose graph * minor: prints * Pose Graph interface settled * Pose graph interface and its use updated * cos -> std::cos * cout -> CV_LOG_INFO * pose graph interface updated: implementation * Pose Graph Node and Edge: extra fields dropped * more minor refactor-like fixes * return and finish condition fixed * more updates to test * test disabled for Debug builds because 400 sec is too much * whitespace * Disable pose graph test if there's no Eigen * more unused vars * fixing unused function warning * less includes * "verbose" removed * write obj to file only when debug level is raised * License + include guard * skip test using tags and SkipTestException * suppress "unused function" warning * minorpull/2904/head
Rostislav Vasilikhin
4 years ago
committed by
GitHub
parent
38ab933851
commit
1341c05766
12 changed files with 1177 additions and 509 deletions
@ -1,15 +1,7 @@ |
||||
set(the_description "RGBD algorithms") |
||||
|
||||
find_package(Ceres QUIET) |
||||
ocv_define_module(rgbd opencv_core opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python) |
||||
|
||||
if(Ceres_FOUND) |
||||
ocv_target_compile_definitions(${the_module} PUBLIC CERES_FOUND) |
||||
ocv_target_link_libraries(${the_module} ${CERES_LIBRARIES}) |
||||
if(Ceres_VERSION VERSION_LESS 2.0.0) |
||||
ocv_include_directories("${CERES_INCLUDE_DIRS}") |
||||
endif() |
||||
add_definitions(/DGLOG_NO_ABBREVIATED_SEVERITIES) # avoid ERROR macro conflict in glog (ceres dependency) |
||||
else() |
||||
message(STATUS "rgbd: CERES support is disabled. Ceres Solver is Required for Posegraph optimization") |
||||
if(NOT HAVE_EIGEN) |
||||
message(STATUS "rgbd: Eigen support is disabled. Eigen is Required for Posegraph optimization") |
||||
endif() |
||||
|
||||
@ -0,0 +1,62 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html
|
||||
|
||||
#ifndef OPENCV_RGBD_POSE_GRAPH_HPP |
||||
#define OPENCV_RGBD_POSE_GRAPH_HPP |
||||
|
||||
#include "opencv2/core/affine.hpp" |
||||
#include "opencv2/core/quaternion.hpp" |
||||
|
||||
namespace cv |
||||
{ |
||||
namespace kinfu |
||||
{ |
||||
namespace detail |
||||
{ |
||||
|
||||
// ATTENTION! This class is used internally in Large KinFu.
|
||||
// It has been pushed to publicly available headers for tests only.
|
||||
// Source compatibility of this API is not guaranteed in the future.
|
||||
|
||||
// This class provides tools to solve so-called pose graph problem often arisen in SLAM problems
|
||||
// The pose graph format, cost function and optimization techniques
|
||||
// repeat the ones used in Ceres 3D Pose Graph Optimization:
|
||||
// http://ceres-solver.org/nnls_tutorial.html#other-examples, pose_graph_3d.cc bullet
|
||||
class CV_EXPORTS_W PoseGraph |
||||
{ |
||||
public: |
||||
static Ptr<PoseGraph> create(); |
||||
virtual ~PoseGraph(); |
||||
|
||||
// Node may have any id >= 0
|
||||
virtual void addNode(size_t _nodeId, const Affine3d& _pose, bool fixed) = 0; |
||||
virtual bool isNodeExist(size_t nodeId) const = 0; |
||||
virtual bool setNodeFixed(size_t nodeId, bool fixed) = 0; |
||||
virtual bool isNodeFixed(size_t nodeId) const = 0; |
||||
virtual Affine3d getNodePose(size_t nodeId) const = 0; |
||||
virtual std::vector<size_t> getNodesIds() const = 0; |
||||
virtual size_t getNumNodes() const = 0; |
||||
|
||||
// Edges have consequent indices starting from 0
|
||||
virtual void addEdge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation, |
||||
const Matx66f& _information = Matx66f::eye()) = 0; |
||||
virtual size_t getEdgeStart(size_t i) const = 0; |
||||
virtual size_t getEdgeEnd(size_t i) const = 0; |
||||
virtual size_t getNumEdges() const = 0; |
||||
|
||||
// checks if graph is connected and each edge connects exactly 2 nodes
|
||||
virtual bool isValid() const = 0; |
||||
|
||||
// Termination criteria are max number of iterations and min relative energy change to current energy
|
||||
// Returns number of iterations elapsed or -1 if max number of iterations was reached or failed to optimize
|
||||
virtual int optimize(const cv::TermCriteria& tc = cv::TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6)) = 0; |
||||
|
||||
// calculate cost function based on current nodes parameters
|
||||
virtual double calcEnergy() const = 0; |
||||
}; |
||||
|
||||
} // namespace detail
|
||||
} // namespace kinfu
|
||||
} // namespace cv
|
||||
#endif /* ifndef OPENCV_RGBD_POSE_GRAPH_HPP */ |
||||
@ -1,321 +0,0 @@ |
||||
#ifndef OPENCV_RGBD_GRAPH_NODE_H |
||||
#define OPENCV_RGBD_GRAPH_NODE_H |
||||
|
||||
#include <map> |
||||
#include <unordered_map> |
||||
|
||||
#include "opencv2/core/affine.hpp" |
||||
#if defined(HAVE_EIGEN) |
||||
#include <Eigen/Core> |
||||
#include <Eigen/Geometry> |
||||
#include "opencv2/core/eigen.hpp" |
||||
#endif |
||||
|
||||
#if defined(CERES_FOUND) |
||||
#include <ceres/ceres.h> |
||||
#endif |
||||
|
||||
namespace cv |
||||
{ |
||||
namespace kinfu |
||||
{ |
||||
/*! \class GraphNode
|
||||
* \brief Defines a node/variable that is optimizable in a posegraph |
||||
* |
||||
* Detailed description |
||||
*/ |
||||
#if defined(HAVE_EIGEN) |
||||
struct Pose3d |
||||
{ |
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||||
|
||||
Eigen::Vector3d t; |
||||
Eigen::Quaterniond r; |
||||
|
||||
Pose3d() |
||||
{ |
||||
t.setZero(); |
||||
r.setIdentity(); |
||||
}; |
||||
Pose3d(const Eigen::Matrix3d& rotation, const Eigen::Vector3d& translation) |
||||
: t(translation), r(Eigen::Quaterniond(rotation)) |
||||
{ |
||||
normalizeRotation(); |
||||
} |
||||
|
||||
Pose3d(const Matx33d& rotation, const Vec3d& translation) |
||||
{ |
||||
Eigen::Matrix3d R; |
||||
cv2eigen(rotation, R); |
||||
cv2eigen(translation, t); |
||||
r = Eigen::Quaterniond(R); |
||||
normalizeRotation(); |
||||
} |
||||
|
||||
explicit Pose3d(const Matx44f& pose) |
||||
{ |
||||
Matx33d rotation(pose.val[0], pose.val[1], pose.val[2], pose.val[4], pose.val[5], |
||||
pose.val[6], pose.val[8], pose.val[9], pose.val[10]); |
||||
Vec3d translation(pose.val[3], pose.val[7], pose.val[11]); |
||||
Pose3d(rotation, translation); |
||||
} |
||||
|
||||
// NOTE: Eigen overloads quaternion multiplication appropriately
|
||||
inline Pose3d operator*(const Pose3d& otherPose) const |
||||
{ |
||||
Pose3d out(*this); |
||||
out.t += r * otherPose.t; |
||||
out.r *= otherPose.r; |
||||
out.normalizeRotation(); |
||||
return out; |
||||
} |
||||
|
||||
inline Pose3d& operator*=(const Pose3d& otherPose) |
||||
{ |
||||
t += otherPose.t; |
||||
r *= otherPose.r; |
||||
normalizeRotation(); |
||||
return *this; |
||||
} |
||||
|
||||
inline Pose3d inverse() const |
||||
{ |
||||
Pose3d out; |
||||
out.r = r.conjugate(); |
||||
out.t = out.r * (t * -1.0); |
||||
return out; |
||||
} |
||||
|
||||
inline void normalizeRotation() |
||||
{ |
||||
if (r.w() < 0) |
||||
r.coeffs() *= -1.0; |
||||
r.normalize(); |
||||
} |
||||
}; |
||||
#endif |
||||
|
||||
struct PoseGraphNode |
||||
{ |
||||
public: |
||||
explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) |
||||
: nodeId(_nodeId), isFixed(false), pose(_pose) |
||||
{ |
||||
#if defined(HAVE_EIGEN) |
||||
se3Pose = Pose3d(_pose.rotation(), _pose.translation()); |
||||
#endif |
||||
} |
||||
virtual ~PoseGraphNode() = default; |
||||
|
||||
int getId() const { return nodeId; } |
||||
inline Affine3f getPose() const |
||||
{ |
||||
return pose; |
||||
} |
||||
void setPose(const Affine3f& _pose) |
||||
{ |
||||
pose = _pose; |
||||
#if defined(HAVE_EIGEN) |
||||
se3Pose = Pose3d(pose.rotation(), pose.translation()); |
||||
#endif |
||||
} |
||||
#if defined(HAVE_EIGEN) |
||||
void setPose(const Pose3d& _pose) |
||||
{ |
||||
se3Pose = _pose; |
||||
const Eigen::Matrix3d& rotation = se3Pose.r.toRotationMatrix(); |
||||
const Eigen::Vector3d& translation = se3Pose.t; |
||||
Matx33d rot; |
||||
Vec3d trans; |
||||
eigen2cv(rotation, rot); |
||||
eigen2cv(translation, trans); |
||||
Affine3d poseMatrix(rot, trans); |
||||
pose = poseMatrix; |
||||
} |
||||
#endif |
||||
void setFixed(bool val = true) { isFixed = val; } |
||||
bool isPoseFixed() const { return isFixed; } |
||||
|
||||
public: |
||||
int nodeId; |
||||
bool isFixed; |
||||
Affine3f pose; |
||||
#if defined(HAVE_EIGEN) |
||||
Pose3d se3Pose; |
||||
#endif |
||||
}; |
||||
|
||||
/*! \class PoseGraphEdge
|
||||
* \brief Defines the constraints between two PoseGraphNodes |
||||
* |
||||
* Detailed description |
||||
*/ |
||||
struct PoseGraphEdge |
||||
{ |
||||
public: |
||||
PoseGraphEdge(int _sourceNodeId, int _targetNodeId, const Affine3f& _transformation, |
||||
const Matx66f& _information = Matx66f::eye()) |
||||
: sourceNodeId(_sourceNodeId), |
||||
targetNodeId(_targetNodeId), |
||||
transformation(_transformation), |
||||
information(_information) |
||||
{ |
||||
} |
||||
virtual ~PoseGraphEdge() = default; |
||||
|
||||
int getSourceNodeId() const { return sourceNodeId; } |
||||
int getTargetNodeId() const { return targetNodeId; } |
||||
|
||||
bool operator==(const PoseGraphEdge& edge) |
||||
{ |
||||
if ((edge.getSourceNodeId() == sourceNodeId && edge.getTargetNodeId() == targetNodeId) || |
||||
(edge.getSourceNodeId() == targetNodeId && edge.getTargetNodeId() == sourceNodeId)) |
||||
return true; |
||||
return false; |
||||
} |
||||
|
||||
public: |
||||
int sourceNodeId; |
||||
int targetNodeId; |
||||
Affine3f transformation; |
||||
Matx66f information; |
||||
}; |
||||
|
||||
//! @brief Reference: A tutorial on SE(3) transformation parameterizations and on-manifold
|
||||
//! optimization Jose Luis Blanco Compactly represents the jacobian of the SE3 generator
|
||||
// clang-format off
|
||||
/* static const std::array<Matx44f, 6> generatorJacobian = { */ |
||||
/* // alpha */ |
||||
/* Matx44f(0, 0, 0, 0, */ |
||||
/* 0, 0, -1, 0, */ |
||||
/* 0, 1, 0, 0, */ |
||||
/* 0, 0, 0, 0), */ |
||||
/* // beta */ |
||||
/* Matx44f( 0, 0, 1, 0, */ |
||||
/* 0, 0, 0, 0, */ |
||||
/* -1, 0, 0, 0, */ |
||||
/* 0, 0, 0, 0), */ |
||||
/* // gamma */ |
||||
/* Matx44f(0, -1, 0, 0, */ |
||||
/* 1, 0, 0, 0, */ |
||||
/* 0, 0, 0, 0, */ |
||||
/* 0, 0, 0, 0), */ |
||||
/* // x */ |
||||
/* Matx44f(0, 0, 0, 1, */ |
||||
/* 0, 0, 0, 0, */ |
||||
/* 0, 0, 0, 0, */ |
||||
/* 0, 0, 0, 0), */ |
||||
/* // y */ |
||||
/* Matx44f(0, 0, 0, 0, */ |
||||
/* 0, 0, 0, 1, */ |
||||
/* 0, 0, 0, 0, */ |
||||
/* 0, 0, 0, 0), */ |
||||
/* // z */ |
||||
/* Matx44f(0, 0, 0, 0, */ |
||||
/* 0, 0, 0, 0, */ |
||||
/* 0, 0, 0, 1, */ |
||||
/* 0, 0, 0, 0) */ |
||||
/* }; */ |
||||
// clang-format on
|
||||
|
||||
class PoseGraph |
||||
{ |
||||
public: |
||||
typedef std::vector<PoseGraphNode> NodeVector; |
||||
typedef std::vector<PoseGraphEdge> EdgeVector; |
||||
|
||||
explicit PoseGraph(){}; |
||||
virtual ~PoseGraph() = default; |
||||
|
||||
//! PoseGraph can be copied/cloned
|
||||
PoseGraph(const PoseGraph&) = default; |
||||
PoseGraph& operator=(const PoseGraph&) = default; |
||||
|
||||
void addNode(const PoseGraphNode& node) { nodes.push_back(node); } |
||||
void addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); } |
||||
|
||||
bool nodeExists(int nodeId) const |
||||
{ |
||||
return std::find_if(nodes.begin(), nodes.end(), [nodeId](const PoseGraphNode& currNode) { |
||||
return currNode.getId() == nodeId; |
||||
}) != nodes.end(); |
||||
} |
||||
|
||||
bool isValid() const; |
||||
|
||||
int getNumNodes() const { return int(nodes.size()); } |
||||
int getNumEdges() const { return int(edges.size()); } |
||||
|
||||
public: |
||||
NodeVector nodes; |
||||
EdgeVector edges; |
||||
}; |
||||
|
||||
namespace Optimizer |
||||
{ |
||||
void optimize(PoseGraph& poseGraph); |
||||
|
||||
#if defined(CERES_FOUND) |
||||
void createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& problem); |
||||
|
||||
//! Error Functor required for Ceres to obtain an auto differentiable cost function
|
||||
class Pose3dErrorFunctor |
||||
{ |
||||
public: |
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||||
Pose3dErrorFunctor(const Pose3d& _poseMeasurement, const Matx66d& _sqrtInformation) |
||||
: poseMeasurement(_poseMeasurement) |
||||
{ |
||||
cv2eigen(_sqrtInformation, sqrtInfo); |
||||
} |
||||
Pose3dErrorFunctor(const Pose3d& _poseMeasurement, |
||||
const Eigen::Matrix<double, 6, 6>& _sqrtInformation) |
||||
: poseMeasurement(_poseMeasurement), sqrtInfo(_sqrtInformation) |
||||
{ |
||||
} |
||||
|
||||
template<typename T> |
||||
bool operator()(const T* const _pSourceTrans, const T* const _pSourceQuat, |
||||
const T* const _pTargetTrans, const T* const _pTargetQuat, T* _pResidual) const |
||||
{ |
||||
Eigen::Map<const Eigen::Matrix<T, 3, 1>> sourceTrans(_pSourceTrans); |
||||
Eigen::Map<const Eigen::Matrix<T, 3, 1>> targetTrans(_pTargetTrans); |
||||
Eigen::Map<const Eigen::Quaternion<T>> sourceQuat(_pSourceQuat); |
||||
Eigen::Map<const Eigen::Quaternion<T>> targetQuat(_pTargetQuat); |
||||
Eigen::Map<Eigen::Matrix<T, 6, 1>> residual(_pResidual); |
||||
|
||||
Eigen::Quaternion<T> targetQuatInv = targetQuat.conjugate(); |
||||
|
||||
Eigen::Quaternion<T> relativeQuat = targetQuatInv * sourceQuat; |
||||
Eigen::Matrix<T, 3, 1> relativeTrans = targetQuatInv * (targetTrans - sourceTrans); |
||||
|
||||
//! Definition should actually be relativeQuat * poseMeasurement.r.conjugate()
|
||||
Eigen::Quaternion<T> deltaRot = |
||||
poseMeasurement.r.template cast<T>() * relativeQuat.conjugate(); |
||||
|
||||
residual.template block<3, 1>(0, 0) = relativeTrans - poseMeasurement.t.template cast<T>(); |
||||
residual.template block<3, 1>(3, 0) = T(2.0) * deltaRot.vec(); |
||||
|
||||
residual.applyOnTheLeft(sqrtInfo.template cast<T>()); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
static ceres::CostFunction* create(const Pose3d& _poseMeasurement, |
||||
const Matx66f& _sqrtInformation) |
||||
{ |
||||
return new ceres::AutoDiffCostFunction<Pose3dErrorFunctor, 6, 3, 4, 3, 4>( |
||||
new Pose3dErrorFunctor(_poseMeasurement, _sqrtInformation)); |
||||
} |
||||
|
||||
private: |
||||
const Pose3d poseMeasurement; |
||||
Eigen::Matrix<double, 6, 6> sqrtInfo; |
||||
}; |
||||
#endif |
||||
|
||||
} // namespace Optimizer
|
||||
|
||||
} // namespace kinfu
|
||||
} // namespace cv
|
||||
#endif /* ifndef OPENCV_RGBD_GRAPH_NODE_H */ |
||||
@ -0,0 +1,151 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html
|
||||
|
||||
#include "test_precomp.hpp" |
||||
|
||||
namespace opencv_test { namespace { |
||||
|
||||
using namespace cv; |
||||
|
||||
static Affine3d readAffine(std::istream& input) |
||||
{ |
||||
Vec3d p; |
||||
Vec4d q; |
||||
input >> p[0] >> p[1] >> p[2]; |
||||
input >> q[1] >> q[2] >> q[3] >> q[0]; |
||||
// Normalize the quaternion to account for precision loss due to
|
||||
// serialization.
|
||||
return Affine3d(Quatd(q).toRotMat3x3(), p); |
||||
}; |
||||
|
||||
// Rewritten from Ceres pose graph demo: https://ceres-solver.org/
|
||||
static Ptr<kinfu::detail::PoseGraph> readG2OFile(const std::string& g2oFileName) |
||||
{ |
||||
Ptr<kinfu::detail::PoseGraph> pg = kinfu::detail::PoseGraph::create(); |
||||
|
||||
// for debugging purposes
|
||||
size_t minId = 0, maxId = 1 << 30; |
||||
|
||||
std::ifstream infile(g2oFileName.c_str()); |
||||
if (!infile) |
||||
{ |
||||
CV_Error(cv::Error::StsError, "failed to open file"); |
||||
} |
||||
|
||||
while (infile.good()) |
||||
{ |
||||
std::string data_type; |
||||
// Read whether the type is a node or a constraint
|
||||
infile >> data_type; |
||||
if (data_type == "VERTEX_SE3:QUAT") |
||||
{ |
||||
size_t id; |
||||
infile >> id; |
||||
Affine3d pose = readAffine(infile); |
||||
|
||||
if (id < minId || id >= maxId) |
||||
continue; |
||||
|
||||
bool fixed = (id == minId); |
||||
|
||||
// Ensure we don't have duplicate poses
|
||||
if (pg->isNodeExist(id)) |
||||
{ |
||||
CV_LOG_INFO(NULL, "duplicated node, id=" << id); |
||||
} |
||||
pg->addNode(id, pose, fixed); |
||||
} |
||||
else if (data_type == "EDGE_SE3:QUAT") |
||||
{ |
||||
size_t startId, endId; |
||||
infile >> startId >> endId; |
||||
Affine3d pose = readAffine(infile); |
||||
|
||||
Matx66d info; |
||||
for (int i = 0; i < 6 && infile.good(); ++i) |
||||
{ |
||||
for (int j = i; j < 6 && infile.good(); ++j) |
||||
{ |
||||
infile >> info(i, j); |
||||
if (i != j) |
||||
{ |
||||
info(j, i) = info(i, j); |
||||
} |
||||
} |
||||
} |
||||
|
||||
if ((startId >= minId && startId < maxId) && (endId >= minId && endId < maxId)) |
||||
{ |
||||
pg->addEdge(startId, endId, pose, info); |
||||
} |
||||
} |
||||
else |
||||
{ |
||||
CV_Error(cv::Error::StsError, "unknown tag"); |
||||
} |
||||
|
||||
// Clear any trailing whitespace from the line
|
||||
infile >> std::ws; |
||||
} |
||||
|
||||
return pg; |
||||
} |
||||
|
||||
|
||||
TEST( PoseGraph, sphereG2O ) |
||||
{ |
||||
// Test takes 15+ sec in Release mode and 400+ sec in Debug mode
|
||||
applyTestTag(CV_TEST_TAG_LONG, CV_TEST_TAG_DEBUG_VERYLONG); |
||||
|
||||
// The dataset was taken from here: https://lucacarlone.mit.edu/datasets/
|
||||
// Connected paper:
|
||||
// L.Carlone, R.Tron, K.Daniilidis, and F.Dellaert.
|
||||
// Initialization Techniques for 3D SLAM : a Survey on Rotation Estimation and its Use in Pose Graph Optimization.
|
||||
// In IEEE Intl.Conf.on Robotics and Automation(ICRA), pages 4597 - 4604, 2015.
|
||||
|
||||
std::string filename = cvtest::TS::ptr()->get_data_path() + "rgbd/sphere_bignoise_vertex3.g2o"; |
||||
Ptr<kinfu::detail::PoseGraph> pg = readG2OFile(filename); |
||||
|
||||
#ifdef HAVE_EIGEN |
||||
// You may change logging level to view detailed optimization report
|
||||
// For example, set env. variable like this: OPENCV_LOG_LEVEL=INFO
|
||||
|
||||
int iters = pg->optimize(); |
||||
|
||||
ASSERT_GE(iters, 0); |
||||
ASSERT_LE(iters, 36); // should converge in 36 iterations
|
||||
|
||||
double energy = pg->calcEnergy(); |
||||
|
||||
ASSERT_LE(energy, 1.47723e+06); // should converge to 1.47722e+06 or less
|
||||
|
||||
// Add the "--test_debug" to arguments to see resulting pose graph nodes positions
|
||||
if (cvtest::debugLevel > 0) |
||||
{ |
||||
// Write edge-only model of how nodes are located in space
|
||||
std::string fname = "pgout.obj"; |
||||
std::fstream of(fname, std::fstream::out); |
||||
std::vector<size_t> ids = pg->getNodesIds(); |
||||
for (const size_t& id : ids) |
||||
{ |
||||
Point3d d = pg->getNodePose(id).translation(); |
||||
of << "v " << d.x << " " << d.y << " " << d.z << std::endl; |
||||
} |
||||
|
||||
size_t esz = pg->getNumEdges(); |
||||
for (size_t i = 0; i < esz; i++) |
||||
{ |
||||
size_t sid = pg->getEdgeStart(i), tid = pg->getEdgeEnd(i); |
||||
of << "l " << sid + 1 << " " << tid + 1 << std::endl; |
||||
} |
||||
|
||||
of.close(); |
||||
} |
||||
#else |
||||
throw SkipTestException("Build with Eigen required for pose graph optimization"); |
||||
#endif |
||||
} |
||||
|
||||
|
||||
}} // namespace
|
||||
Loading…
Reference in new issue