i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

g2o/gps support: added edge_se3_xyzprior.h to rtabmap repo. Fixed build for kinetic g2o version.

Browse Source
This commit is contained in:
matlabbe
2019-01-03 15:49:25 -05:00
parent 248d6f1167
commit 0f8c70bcdf
8 changed files with 363 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files

296
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -59,6 +59,7 @@ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> Matr
#include "g2o/config.h"
#include "g2o/types/slam2d/types_slam2d.h"
#include "g2o/types/slam3d/types_slam3d.h"
#include "g2o/edge_se3_xyzprior.h"
#ifdef G2O_HAVE_CSPARSE
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#endif
@@ -397,96 +398,96 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
if(isSlam2d())
{
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) > 9999.0 ||
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0)
{
g2o::EdgeSE2XYPrior * priorEdge = new g2o::EdgeSE2XYPrior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::Vector2(iter->second.transform().x(), iter->second.transform().y()));
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
}
priorEdge->setInformation(information);
edge = priorEdge;
}
else
{
g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
priorEdge->setInformation(information);
edge = priorEdge;
}
}
else
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0 ||
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0)
{
g2o::EdgeSE2XYPrior * priorEdge = new g2o::EdgeSE2XYPrior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::Vector2D(iter->second.transform().x(), iter->second.transform().y()));
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
}
priorEdge->setInformation(information);
edge = priorEdge;
}
else
{
g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
priorEdge->setInformation(information);
edge = priorEdge;
}
}
else
{
if ((1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) > 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) > 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) > 9999.0) ||
(static_cast<double>(iter->second.infMatrix().at<double>(3,3)) == 0.0 &&
static_cast<double>(iter->second.infMatrix().at<double>(4,4)) == 0.0 &&
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0))
{
g2o::EdgeXYZPrior * priorEdge = new g2o::EdgeXYZPrior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::Vector3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()));
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,2); // x-z
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,2); // y-z
information(2,0) = iter->second.infMatrix().at<double>(2,0); // z-x
information(2,1) = iter->second.infMatrix().at<double>(2,1); // z-y
information(2,2) = iter->second.infMatrix().at<double>(2,2); // z-z
}
priorEdge->setInformation(information);
edge = priorEdge;
}
else
{
g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
Eigen::Affine3d a = iter->second.transform().toEigen3d();
Eigen::Isometry3d pose;
pose = a.linear();
pose.translation() = a.translation();
priorEdge->setMeasurement(pose);
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
priorEdge->setInformation(information);
edge = priorEdge;
}
if ((1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0) ||
(static_cast<double>(iter->second.infMatrix().at<double>(3,3)) == 0.0 &&
static_cast<double>(iter->second.infMatrix().at<double>(4,4)) == 0.0 &&
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0))
{
EdgeSE3XYZPrior * priorEdge = new EdgeSE3XYZPrior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::Vector3D(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()));
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,2); // x-z
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,2); // y-z
information(2,0) = iter->second.infMatrix().at<double>(2,0); // z-x
information(2,1) = iter->second.infMatrix().at<double>(2,1); // z-y
information(2,2) = iter->second.infMatrix().at<double>(2,2); // z-z
}
priorEdge->setInformation(information);
edge = priorEdge;
}
else
{
g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
Eigen::Affine3d a = iter->second.transform().toEigen3d();
Eigen::Isometry3d pose;
pose = a.linear();
pose.translation() = a.translation();
priorEdge->setMeasurement(pose);
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
priorEdge->setInformation(information);
edge = priorEdge;
}
}
}
}
@@ -1740,8 +1741,8 @@ bool OptimizerG2O::saveGraph(
std::string suffix = "";
std::string to = uFormat(" %d", iter->second.to());
bool isSE2 = true;
bool isSE3 = true;
bool isSE2 = true;
bool isSE3 = true;
if (iter->second.type() == Link::kPosePrior)
{
@@ -1749,42 +1750,42 @@ bool OptimizerG2O::saveGraph(
{
continue;
}
if (isSlam2d())
{
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) > 9999.0 ||
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0)
{
prefix = "EDGE_PRIOR_SE2_XY";
isSE2 = false;
}
else
{
prefix = "EDGE_PRIOR_SE2";
}
if (isSlam2d())
{
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) > 9999.0 ||
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0)
{
prefix = "EDGE_PRIOR_SE2_XY";
isSE2 = false;
}
else
{
prefix = "EDGE_PRIOR_SE2";
}
// based on https://github.com/RainerKuemmerle/g2o/blob/38347944c6ad7a3b31976b97406ff0de20be1530/g2o/types/slam2d/edge_se2_prior.cpp#L42
// based on https://github.com/RainerKuemmerle/g2o/blob/38347944c6ad7a3b31976b97406ff0de20be1530/g2o/types/slam2d/edge_se2_prior.cpp#L42
// there is no pid for the 2d prior case
to = "";
}
else
{
if ((1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) > 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) > 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) > 9999.0) ||
(static_cast<double>(iter->second.infMatrix().at<double>(3,3)) == 0.0 &&
static_cast<double>(iter->second.infMatrix().at<double>(4,4)) == 0.0 &&
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0))
{
to = "";
prefix = "EDGE_POINTXYZ_PRIOR";
isSE3 = false;
}
else
{
to = uFormat(" %d", PARAM_OFFSET);
prefix = "EDGE_SE3_PRIOR";
}
}
}
else
{
if ((1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) > 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) > 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) > 9999.0) ||
(static_cast<double>(iter->second.infMatrix().at<double>(3,3)) == 0.0 &&
static_cast<double>(iter->second.infMatrix().at<double>(4,4)) == 0.0 &&
static_cast<double>(iter->second.infMatrix().at<double>(5,5)) == 0.0))
{
to = "";
prefix = "EDGE_POINTXYZ_PRIOR";
isSE3 = false;
}
else
{
to = uFormat(" %d", PARAM_OFFSET);
prefix = "EDGE_SE3_PRIOR";
}
}
}
else if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
@@ -1798,11 +1799,11 @@ bool OptimizerG2O::saveGraph(
if(isSlam2d())
{
if (isSE2)
{
// EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
// EDGE_SE2_PRIOR observed_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n",
if (isSE2)
{
// EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
// EDGE_SE2_PRIOR observed_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n",
prefix.c_str(),
iter->second.from(),
to.c_str(),
@@ -1816,12 +1817,12 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 5),
iter->second.infMatrix().at<double>(5, 5));
}
}
else
{
// EDGE_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
// EDGE_POINTXY_PRIOR x y inf_11 inf_12 inf_22
fprintf(file, "%s %d%s%s %f %f %f %f %f\n",
{
// EDGE_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
// EDGE_POINTXY_PRIOR x y inf_11 inf_12 inf_22
fprintf(file, "%s %d%s%s %f %f %f %f %f\n",
prefix.c_str(),
iter->second.from(),
to.c_str(),
@@ -1831,15 +1832,16 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(1, 1));
}
}
}
else
{
if (isSE3) {
// EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
// EDGE_SE3_PRIOR observed_vertex_id offset_parameter_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
if (isSE3)
{
// EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
// EDGE_SE3_PRIOR observed_vertex_id offset_parameter_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
prefix.c_str(),
iter->second.from(),
to.c_str(),
@@ -1872,12 +1874,12 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(4, 4),
iter->second.infMatrix().at<double>(4, 5),
iter->second.infMatrix().at<double>(5, 5));
}
else
{
// EDGE_XYZ observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_13 inf_22 .. inf_33
// EDGE_POINTXYZ_PRIOR observed_vertex_id x y z inf_11 inf_12 .. inf_13 inf_22 .. inf_33
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n",
}
else
{
// EDGE_XYZ observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_13 inf_22 .. inf_33
// EDGE_POINTXYZ_PRIOR observed_vertex_id x y z inf_11 inf_12 .. inf_13 inf_22 .. inf_33
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n",
prefix.c_str(),
iter->second.from(),
to.c_str(),
@@ -1891,7 +1893,7 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 2),
iter->second.infMatrix().at<double>(2, 2));
}
}
}
}
UINFO("Graph saved to %s", fileName.c_str());

107
corelib/src/optimizer/g2o/edge_se3_xyzprior.cpp Normal file
View File

@@ -0,0 +1,107 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
#include "edge_se3_xyzprior.h"
EdgeSE3XYZPrior::EdgeSE3XYZPrior() : BaseUnaryEdge<3, g2o::Vector3D, g2o::VertexSE3>()
{
information().setIdentity();
setMeasurement(g2o::Vector3D::Zero());
_cache = 0;
_offsetParam = 0;
resizeParameters(1);
installParameter(_offsetParam, 0);
}
bool EdgeSE3XYZPrior::resolveCaches(){
assert(_offsetParam);
g2o::ParameterVector pv(1);
pv[0] = _offsetParam;
resolveCache(_cache, (g2o::OptimizableGraph::Vertex*)_vertices[0], "CACHE_SE3_OFFSET", pv);
return _cache != 0;
}
bool EdgeSE3XYZPrior::read(std::istream& is)
{
int pid;
is >> pid;
if (!setParameterId(0, pid))
return false;
// measured keypoint
g2o::Vector3D meas;
for (int i = 0; i < 3; i++) is >> meas[i];
setMeasurement(meas);
// read covariance matrix (upper triangle)
if (is.good()) {
for (int i = 0; i < 3; i++) {
for (int j = i; j < 3; j++) {
is >> information()(i,j);
if (i != j)
information()(j,i) = information()(i,j);
}
}
}
return !is.fail();
}
bool EdgeSE3XYZPrior::write(std::ostream& os) const {
os << _offsetParam->id() << " ";
for (int i = 0; i < 3; i++) os << measurement()[i] << " ";
for (int i = 0; i < 3; i++) {
for (int j = i; j < 3; j++) {
os << information()(i,j) << " ";
}
}
return os.good();
}
void EdgeSE3XYZPrior::computeError() {
const g2o::VertexSE3* v = static_cast<const g2o::VertexSE3*>(_vertices[0]);
_error = v->estimate().translation() - _measurement;
}
void EdgeSE3XYZPrior::linearizeOplus() {
_jacobianOplusXi << g2o::Matrix3D::Identity();
}
bool EdgeSE3XYZPrior::setMeasurementFromState() {
const g2o::VertexSE3* v = static_cast<const g2o::VertexSE3*>(_vertices[0]);
_measurement = v->estimate().translation();
return true;
}
void EdgeSE3XYZPrior::initialEstimate(const g2o::OptimizableGraph::VertexSet& /*from_*/, g2o::OptimizableGraph::Vertex* /*to_*/) {
g2o::VertexSE3 *v = static_cast<g2o::VertexSE3*>(_vertices[0]);
assert(v && "Vertex for the Prior edge is not set");
g2o::Isometry3D newEstimate = _offsetParam->offset().inverse() * Eigen::Translation3d(measurement());
if (_information.block<3,3>(0,0).array().abs().sum() == 0){ // do not set translation, as that part of the information is all zero
newEstimate.translation() = v->estimate().translation();
}
v->setEstimate(newEstimate);
}

78
corelib/src/optimizer/g2o/edge_se3_xyzprior.h Normal file
View File

@@ -0,0 +1,78 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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_EDGE_SE3_PRIOR_XYZ_H
#define G2O_EDGE_SE3_PRIOR_XYZ_H
#include "g2o/types/slam3d/vertex_se3.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/types/slam3d/parameter_se3_offset.h"
/**
* \brief Prior for a 3D pose with constraints only in xyz direction
*/
class EdgeSE3XYZPrior : public g2o::BaseUnaryEdge<3, g2o::Vector3D, g2o::VertexSE3>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSE3XYZPrior();
virtual void setMeasurement(const g2o::Vector3D& m) {
_measurement = m;
}
virtual bool setMeasurementData(const double * d) {
Eigen::Map<const g2o::Vector3D> v(d);
_measurement = v;
return true;
}
virtual bool getMeasurementData(double* d) const {
Eigen::Map<g2o::Vector3D> v(d);
v = _measurement;
return true;
}
virtual int measurementDimension() const {return 3;}
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError();
virtual void linearizeOplus();
virtual bool setMeasurementFromState();
virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& /*from*/, g2o::OptimizableGraph::Vertex* /*to*/) {return 1.;}
virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& /*from_*/, g2o::OptimizableGraph::Vertex* /*to_*/);
const g2o::ParameterSE3Offset* offsetParameter() { return _offsetParam; }
protected:
virtual bool resolveCaches();
g2o::ParameterSE3Offset* _offsetParam;
g2o::CacheSE3Offset* _cache;
};
#endif