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

g2o: fixed indigo build errors

Browse Source
This commit is contained in:
matlabbe
2019-01-03 16:16:16 -05:00
parent 36d7e58ff2
commit 18c35954f0
3 changed files with 11 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -404,7 +404,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
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()));
priorEdge->setMeasurement(Eigen::Vector2d(iter->second.transform().x(), iter->second.transform().y()));
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored())
{
@@ -452,7 +452,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
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()));
priorEdge->setMeasurement(Eigen::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())
{

10
corelib/src/optimizer/g2o/edge_se3_xyzprior.cpp
View File

@@ -26,10 +26,10 @@
#include "edge_se3_xyzprior.h"
EdgeSE3XYZPrior::EdgeSE3XYZPrior() : BaseUnaryEdge<3, g2o::Vector3D, g2o::VertexSE3>()
EdgeSE3XYZPrior::EdgeSE3XYZPrior() : BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3>()
{
information().setIdentity();
setMeasurement(g2o::Vector3D::Zero());
setMeasurement(Eigen::Vector3d::Zero());
_cache = 0;
_offsetParam = 0;
resizeParameters(1);
@@ -52,7 +52,7 @@ bool EdgeSE3XYZPrior::read(std::istream& is)
return false;
// measured keypoint
g2o::Vector3D meas;
Eigen::Vector3d meas;
for (int i = 0; i < 3; i++) is >> meas[i];
setMeasurement(meas);
@@ -86,7 +86,7 @@ void EdgeSE3XYZPrior::computeError() {
}
void EdgeSE3XYZPrior::linearizeOplus() {
_jacobianOplusXi << g2o::Matrix3D::Identity();
_jacobianOplusXi << Eigen::Matrix3d::Identity();
}
bool EdgeSE3XYZPrior::setMeasurementFromState() {
@@ -99,7 +99,7 @@ void EdgeSE3XYZPrior::initialEstimate(const g2o::OptimizableGraph::VertexSet& /*
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());
Eigen::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();
}

8
corelib/src/optimizer/g2o/edge_se3_xyzprior.h
View File

@@ -34,24 +34,24 @@
/**
* \brief Prior for a 3D pose with constraints only in xyz direction
*/
class EdgeSE3XYZPrior : public g2o::BaseUnaryEdge<3, g2o::Vector3D, g2o::VertexSE3>
class EdgeSE3XYZPrior : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSE3XYZPrior();
virtual void setMeasurement(const g2o::Vector3D& m) {
virtual void setMeasurement(const Eigen::Vector3d& m) {
_measurement = m;
}
virtual bool setMeasurementData(const double * d) {
Eigen::Map<const g2o::Vector3D> v(d);
Eigen::Map<const Eigen::Vector3d> v(d);
_measurement = v;
return true;
}
virtual bool getMeasurementData(double* d) const {
Eigen::Map<g2o::Vector3D> v(d);
Eigen::Map<Eigen::Vector3d> v(d);
v = _measurement;
return true;
}