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