1 // g2o - General Graph Optimization 2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 // All rights reserved. 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are 7 // met: 8 // 9 // * Redistributions of source code must retain the above copyright notice, 10 // this list of conditions and the following disclaimer. 11 // * Redistributions in binary form must reproduce the above copyright 12 // notice, this list of conditions and the following disclaimer in the 13 // documentation and/or other materials provided with the distribution. 14 // 15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 27 #include "edge_se3_offset.h" 28 #include "isometry3d_gradients.h" 29 #include "parameter_se3_offset.h" 30 31 #include <iostream> 32 33 namespace g2o { 34 using namespace std; 35 using namespace Eigen; 36 EdgeSE3Offset()37 EdgeSE3Offset::EdgeSE3Offset() : EdgeSE3() { 38 information().setIdentity(); 39 _offsetFrom = 0; 40 _offsetTo = 0; 41 _cacheFrom = 0; 42 _cacheTo = 0; 43 resizeParameters(2); 44 installParameter(_offsetFrom, 0); 45 installParameter(_offsetTo, 1); 46 } 47 resolveCaches()48 bool EdgeSE3Offset::resolveCaches(){ 49 assert(_offsetFrom && _offsetTo); 50 51 ParameterVector pv(2); 52 pv[0]=_offsetFrom; 53 resolveCache(_cacheFrom, (OptimizableGraph::Vertex*)_vertices[0],"CACHE_SE3_OFFSET",pv); 54 pv[0]=_offsetTo; 55 resolveCache(_cacheTo, (OptimizableGraph::Vertex*)_vertices[1],"CACHE_SE3_OFFSET",pv); 56 return (_cacheFrom && _cacheTo); 57 } 58 read(std::istream & is)59 bool EdgeSE3Offset::read(std::istream& is) { 60 bool state = readParamIds(is); 61 62 Vector7 meas; 63 state &= internal::readVector(is, meas); 64 // normalize the quaternion to recover numerical precision lost by storing as human readable text 65 Vector4::MapType(meas.data() + 3).normalize(); 66 setMeasurement(internal::fromVectorQT(meas)); 67 68 state &= readInformationMatrix(is); 69 return state; 70 } 71 write(std::ostream & os) const72 bool EdgeSE3Offset::write(std::ostream& os) const { 73 writeParamIds(os); 74 internal::writeVector(os, internal::toVectorQT(_measurement)); 75 writeInformationMatrix(os); 76 return os.good(); 77 } 78 computeError()79 void EdgeSE3Offset::computeError() { 80 Isometry3 delta=_inverseMeasurement * _cacheFrom->w2n() * _cacheTo->n2w(); 81 _error=internal::toVectorMQT(delta); 82 } 83 setMeasurementFromState()84 bool EdgeSE3Offset::setMeasurementFromState(){ 85 Isometry3 delta = _cacheFrom->w2n() * _cacheTo->n2w(); 86 setMeasurement(delta); 87 return true; 88 } 89 linearizeOplus()90 void EdgeSE3Offset::linearizeOplus() { 91 // BaseBinaryEdge<6, SE3Quat, VertexSE3, VertexSE3>::linearizeOplus(); 92 93 VertexSE3* from = static_cast<VertexSE3*>(_vertices[0]); 94 VertexSE3* to = static_cast<VertexSE3*>(_vertices[1]); 95 Isometry3 E; 96 const Isometry3& Xi = from->estimate(); 97 const Isometry3& Xj = to->estimate(); 98 const Isometry3& Pi = _cacheFrom->offsetParam()->offset(); 99 const Isometry3& Pj = _cacheTo->offsetParam()->offset(); 100 const Isometry3& Z = _measurement; 101 internal::computeEdgeSE3Gradient(E, _jacobianOplusXi, _jacobianOplusXj, Z, Xi, Xj, Pi, Pj); 102 } 103 initialEstimate(const OptimizableGraph::VertexSet & from_,OptimizableGraph::Vertex *)104 void EdgeSE3Offset::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/) { 105 VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]); 106 VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]); 107 108 Isometry3 virtualMeasurement = _cacheFrom->offsetParam()->offset() * measurement() * _cacheTo->offsetParam()->offset().inverse(); 109 110 if (from_.count(from) > 0) { 111 to->setEstimate(from->estimate() * virtualMeasurement); 112 } else 113 from->setEstimate(to->estimate() * virtualMeasurement.inverse()); 114 } 115 116 } 117