mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 17:40:23 +08:00
OptimizerG2O: adding support for priors in SBA (#1670)
* OptimizerG2O: adding support for priors in SBA * Fixed computeError * don't fix root id roll/pitch if gravity constraints are fed (SBA) * changed number_t to double * forward compatibility * cleanup * log
This commit is contained in:
matlabbe
@@ -62,6 +62,7 @@ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> Matr
|
||||
#include "g2o/edge_se3_xyzprior.h" // Include after types_slam3d.h to be ignored on newest g2o versions
|
||||
#include "g2o/edge_se3_gravity.h"
|
||||
#include "g2o/edge_sbacam_gravity.h"
|
||||
#include "g2o/edge_sbacam_prior.h"
|
||||
#include "g2o/edge_xy_prior.h" // Include after types_slam2d.h to be ignored on newest g2o versions
|
||||
#include "g2o/edge_xyz_prior.h" // Include after types_slam3d.h to be ignored on newest g2o versions
|
||||
#ifdef G2O_HAVE_CSPARSE
|
||||
@@ -1579,8 +1580,25 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
#endif
|
||||
}
|
||||
|
||||
// detect if there are gravity constraints
|
||||
bool hasGravityConstraints = false;
|
||||
#ifndef RTABMAP_ORB_SLAM
|
||||
if(!isSlam2d() && gravitySigma() > 0)
|
||||
{
|
||||
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
|
||||
{
|
||||
if( iter->second.from() == iter->second.to() &&
|
||||
iter->second.type() == Link::kGravity)
|
||||
{
|
||||
hasGravityConstraints = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
UDEBUG("fill poses to g2o...");
|
||||
|
||||
UDEBUG("fill %ld poses to g2o... (rootId=%d hasGravityConstraints=%d isSlam2d=%d)", poses.size(), rootId, hasGravityConstraints?1:0, isSlam2d()?1:0);
|
||||
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
if(iter->first > 0)
|
||||
@@ -1619,7 +1637,65 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
vCam->setId(iter->first*MULTICAM_OFFSET + i);
|
||||
|
||||
// negative root means that all other poses should be fixed instead of the root
|
||||
vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
|
||||
bool fixNode = (rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId);
|
||||
|
||||
UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert cam vertex %d (pose=%d)!?", vCam->id(), iter->first).c_str());
|
||||
|
||||
if(this->isSlam2d())
|
||||
{
|
||||
if(fixNode)
|
||||
{
|
||||
UDEBUG("Set node %d fixed", iter->first);
|
||||
vCam->setFixed(true);
|
||||
}
|
||||
else if(i==0) // Only set prior on the first camera
|
||||
{
|
||||
// add a singleton constraint that locks the position of the robot on the plane
|
||||
EdgeSBACamPrior* planeConstraint = new EdgeSBACamPrior();
|
||||
Eigen::Matrix<double, 6, 6> pinfo = Eigen::Matrix<double, 6, 6>::Zero();
|
||||
pinfo(2, 2) = 1e9;
|
||||
planeConstraint->setInformation(pinfo);
|
||||
g2o::SE3Quat fixedZ = g2o::SE3Quat();
|
||||
fixedZ.setTranslation(g2o::Vector3(0,0,iter->second.z()));
|
||||
planeConstraint->setMeasurement(fixedZ);
|
||||
Eigen::Affine3d a = iterModel->second[i].localTransform().inverse().toEigen3d();
|
||||
planeConstraint->setCameraInvLocalTransform(g2o::SE3Quat(a.linear(), a.translation()));
|
||||
planeConstraint->vertices()[0] = vCam;
|
||||
optimizer.addEdge(planeConstraint);
|
||||
}
|
||||
}
|
||||
else if(fixNode)
|
||||
{
|
||||
if(rootId < 0 || !hasGravityConstraints)
|
||||
{
|
||||
UDEBUG("Set node %d fixed", iter->first);
|
||||
vCam->setFixed(true);
|
||||
}
|
||||
else if(hasGravityConstraints && i==0) // Only set prior on the first camera in case of multi-cam
|
||||
{
|
||||
// Setup root prior (fixed x,y,z,yaw)
|
||||
EdgeSBACamPrior * e = new EdgeSBACamPrior();
|
||||
e->vertices()[0] = vCam;
|
||||
Eigen::Affine3d a = iter->second.toEigen3d();
|
||||
e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation()));
|
||||
a = iterModel->second[i].localTransform().inverse().toEigen3d();
|
||||
e->setCameraInvLocalTransform(g2o::SE3Quat(a.linear(), a.translation()));
|
||||
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity()*10e6;
|
||||
// pitch and roll not fixed
|
||||
information(3,3) = information(4,4) = 1;
|
||||
e->setInformation(information);
|
||||
if (!optimizer.addEdge(e))
|
||||
{
|
||||
delete e;
|
||||
UERROR("Map: Failed adding fixed constraint of node %d, set as fixed instead", iter->first);
|
||||
vCam->setFixed(true);
|
||||
}
|
||||
else
|
||||
{
|
||||
UDEBUG("Set node %d fixed with prior (have gravity constraints)", iter->first);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*UDEBUG("camPose %d (camid=%d) (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
|
||||
iter->first,
|
||||
@@ -1632,8 +1708,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
iterModel->second[i].Tx(),
|
||||
iterModel->second[i].Tx()<0.0?-iterModel->second[i].Tx()/iterModel->second[i].fx():baseline_,
|
||||
camPose.prettyPrint().c_str());*/
|
||||
|
||||
UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert cam vertex %d (pose=%d)!?", vCam->id(), iter->first).c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -2043,12 +2117,26 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
return optimizedPoses;
|
||||
}
|
||||
|
||||
// FIXME: is there a way that we can add the 2D constraint directly in SBA?
|
||||
if(this->isSlam2d())
|
||||
{
|
||||
// get transform between old and new pose
|
||||
t = iter->second.inverse() * t;
|
||||
optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
|
||||
// The optimized poses should be already fixed to original height,
|
||||
// but it may have varied a little (not exaclty the same number).
|
||||
// Here we just put back the original z value.
|
||||
if(fabs(t.z() - iter->second.z()) < 0.001)
|
||||
{
|
||||
t.z() = iter->second.z();
|
||||
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("Planar constraints didn't work!? original pose (%d), pose %s -> %s. Falling back to old approach.",
|
||||
iter->first,
|
||||
iter->second.prettyPrint().c_str(),
|
||||
t.prettyPrint().c_str());
|
||||
// get transform between old and new pose
|
||||
t = iter->second.inverse() * t;
|
||||
optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
@@ -44,6 +44,7 @@ class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6,
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
EdgeSBACamGravity(){
|
||||
information().setIdentity();
|
||||
cameraInvLocalTransform_.setIdentity();
|
||||
}
|
||||
virtual bool read(std::istream& is) {return false;} // not implemented
|
||||
virtual bool write(std::ostream& os) const {return false;} // not implemented
|
||||
|
||||
121
corelib/src/optimizer/g2o/edge_sbacam_prior.h
Normal file
121
corelib/src/optimizer/g2o/edge_sbacam_prior.h
Normal file
@@ -0,0 +1,121 @@
|
||||
/*
|
||||
Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
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.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
/**
|
||||
* Adapted from EdgeSE3Prior
|
||||
*/
|
||||
|
||||
#ifndef RTAB_G2O_EDGE_SBACAM_PRIOR_H_
|
||||
#define RTAB_G2O_EDGE_SBACAM_PRIOR_H_
|
||||
|
||||
#include "g2o/types/sba/types_sba.h"
|
||||
#include "g2o/core/base_unary_edge.h"
|
||||
namespace rtabmap {
|
||||
/**
|
||||
* \brief EdgeSBACamPrior
|
||||
* \brief g2o edge with gravity constraint
|
||||
*/
|
||||
class EdgeSBACamPrior : public g2o::BaseUnaryEdge<6, g2o::SE3Quat, g2o::VertexCam> {
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
EdgeSBACamPrior() {
|
||||
setMeasurement(g2o::SE3Quat());
|
||||
information().setIdentity();
|
||||
}
|
||||
|
||||
void setCameraInvLocalTransform(const g2o::SE3Quat & t)
|
||||
{
|
||||
_cameraInvLocalTransform = t;
|
||||
}
|
||||
|
||||
// return the error estimate as a 3-vector
|
||||
void computeError() {
|
||||
const g2o::VertexCam* v = static_cast<const g2o::VertexCam*>(_vertices[0]);
|
||||
g2o::SE3Quat delta = _inverseMeasurement * v->estimate() * _cameraInvLocalTransform;
|
||||
_error[0]=delta.translation().x();
|
||||
_error[1]=delta.translation().y();
|
||||
_error[2]=delta.translation().z();
|
||||
_error[3]=delta.rotation().x();
|
||||
_error[4]=delta.rotation().y();
|
||||
_error[5]=delta.rotation().z();
|
||||
}
|
||||
|
||||
// jacobian
|
||||
virtual void linearizeOplus() {
|
||||
_jacobianOplusXi = Eigen::Matrix<double, 6, 6>::Identity();
|
||||
}
|
||||
|
||||
virtual void setMeasurement(const g2o::SE3Quat& m){
|
||||
_measurement = m;
|
||||
_inverseMeasurement = m.inverse();
|
||||
}
|
||||
|
||||
virtual bool setMeasurementData(const double* d) override {
|
||||
Eigen::Map<const Eigen::Matrix<double, 7, 1, Eigen::ColMajor> > v(d);
|
||||
// SE3Quat expects [x, y, z, qx, qy, qz, qw]
|
||||
_measurement.fromVector(v);
|
||||
_inverseMeasurement = _measurement.inverse();
|
||||
return true;
|
||||
}
|
||||
|
||||
virtual bool getMeasurementData(double* d) const override {
|
||||
Eigen::Map<Eigen::Matrix<double, 7, 1, Eigen::ColMajor> > v(d);
|
||||
// Returns [x, y, z, qx, qy, qz, qw]
|
||||
v = _measurement.toVector();
|
||||
return true;
|
||||
}
|
||||
|
||||
virtual int measurementDimension() const {return 7;}
|
||||
|
||||
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) {
|
||||
g2o::VertexCam *v = static_cast<g2o::VertexCam*>(_vertices[0]);
|
||||
assert(v && "Vertex for the Prior edge is not set");
|
||||
|
||||
g2o::SE3Quat newEstimate = measurement()*_cameraInvLocalTransform.inverse();
|
||||
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.setTranslation(v->estimate().translation());
|
||||
}
|
||||
if (_information.block<3,3>(3,3).array().abs().sum() == 0){ // do not set rotation, as that part of the information is all zero
|
||||
newEstimate.setRotation(v->estimate().rotation());
|
||||
}
|
||||
v->setEstimate(newEstimate);
|
||||
}
|
||||
|
||||
virtual bool read(std::istream& is) override { return true; }
|
||||
virtual bool write(std::ostream& os) const override { return true; }
|
||||
protected:
|
||||
g2o::SE3Quat _inverseMeasurement;
|
||||
g2o::SE3Quat _cameraInvLocalTransform;
|
||||
};
|
||||
|
||||
}
|
||||
#endif
|
||||
Reference in New Issue
Block a user