i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-11 12:29:50 +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
corelib/src/optimizer/OptimizerG2O.cpp
+2 -2
View File
@@ -404,7 +404,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::EdgeSE2XYPrior * priorEdge = new g2o::EdgeSE2XYPrior(); g2o::EdgeSE2XYPrior * priorEdge = new g2o::EdgeSE2XYPrior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1); 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(); Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored()) if(!isCovarianceIgnored())
{ {
@@ -452,7 +452,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
EdgeSE3XYZPrior * priorEdge = new EdgeSE3XYZPrior(); EdgeSE3XYZPrior * priorEdge = new EdgeSE3XYZPrior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1); 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(); Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored()) if(!isCovarianceIgnored())
{ {
corelib/src/optimizer/g2o/edge_se3_xyzprior.cpp
+5 -5
View File
@@ -26,10 +26,10 @@
#include "edge_se3_xyzprior.h" #include "edge_se3_xyzprior.h"
EdgeSE3XYZPrior::EdgeSE3XYZPrior() : BaseUnaryEdge<3, g2o::Vector3D, g2o::VertexSE3>() EdgeSE3XYZPrior::EdgeSE3XYZPrior() : BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3>()
{ {
information().setIdentity(); information().setIdentity();
setMeasurement(g2o::Vector3D::Zero()); setMeasurement(Eigen::Vector3d::Zero());
_cache = 0; _cache = 0;
_offsetParam = 0; _offsetParam = 0;
resizeParameters(1); resizeParameters(1);
@@ -52,7 +52,7 @@ bool EdgeSE3XYZPrior::read(std::istream& is)
return false; return false;
// measured keypoint // measured keypoint
g2o::Vector3D meas; Eigen::Vector3d meas;
for (int i = 0; i < 3; i++) is >> meas[i]; for (int i = 0; i < 3; i++) is >> meas[i];
setMeasurement(meas); setMeasurement(meas);
@@ -86,7 +86,7 @@ void EdgeSE3XYZPrior::computeError() {
} }
void EdgeSE3XYZPrior::linearizeOplus() { void EdgeSE3XYZPrior::linearizeOplus() {
_jacobianOplusXi << g2o::Matrix3D::Identity(); _jacobianOplusXi << Eigen::Matrix3d::Identity();
} }
bool EdgeSE3XYZPrior::setMeasurementFromState() { bool EdgeSE3XYZPrior::setMeasurementFromState() {
@@ -99,7 +99,7 @@ void EdgeSE3XYZPrior::initialEstimate(const g2o::OptimizableGraph::VertexSet& /*
g2o::VertexSE3 *v = static_cast<g2o::VertexSE3*>(_vertices[0]); g2o::VertexSE3 *v = static_cast<g2o::VertexSE3*>(_vertices[0]);
assert(v && "Vertex for the Prior edge is not set"); 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 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(); newEstimate.translation() = v->estimate().translation();
} }
corelib/src/optimizer/g2o/edge_se3_xyzprior.h
+4 -4
View File
@@ -34,24 +34,24 @@
/** /**
* \brief Prior for a 3D pose with constraints only in xyz direction * \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: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSE3XYZPrior(); EdgeSE3XYZPrior();
virtual void setMeasurement(const g2o::Vector3D& m) { virtual void setMeasurement(const Eigen::Vector3d& m) {
_measurement = m; _measurement = m;
} }
virtual bool setMeasurementData(const double * d) { virtual bool setMeasurementData(const double * d) {
Eigen::Map<const g2o::Vector3D> v(d); Eigen::Map<const Eigen::Vector3d> v(d);
_measurement = v; _measurement = v;
return true; return true;
} }
virtual bool getMeasurementData(double* d) const { virtual bool getMeasurementData(double* d) const {
Eigen::Map<g2o::Vector3D> v(d); Eigen::Map<Eigen::Vector3d> v(d);
v = _measurement; v = _measurement;
return true; return true;
} }