void EdgeSE3PointXYZDisparity::computeError() {
//VertexSE3 *cam = static_cast<VertexSE3*>(_vertices[0]);
VertexPointXYZ *point = static_cast<VertexPointXYZ*>(_vertices[1]);
const Vector3d& pt = point->estimate();
//Eigen::Vector4d ppt(pt(0),pt(1),pt(2),1.0);
// VertexCameraCache* vcache = (VertexCameraCache*)cam->getCache(_cacheIds[0]);
// if (! vcache){
// cerr << "fatal error in retrieving cache" << endl;
// }
// CacheCamera* vcache = cache;
// if (! vcache){
// cerr << "fatal error in retrieving cache" << endl;
// }
Eigen::Vector3d p = cache->w2i() * pt;
Eigen::Vector3d perr;
perr.head<2>() = p.head<2>()/p(2);
perr(2) = 1/p(2);
// error, which is backwards from the normal observed - calculated
// _measurement is the measured projection
_error = perr - _measurement;
}
HyperGraphElementAction* VertexPointXYZDrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params ){
if (typeid(*element).name()!=_typeName)
return 0;
refreshPropertyPtrs(params);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
VertexPointXYZ* that = static_cast<VertexPointXYZ*>(element);
glPushAttrib(GL_ENABLE_BIT | GL_POINT_BIT);
glDisable(GL_LIGHTING);
glColor3f(0.8f,0.5f,0.3f);
if (_pointSize) {
glPointSize(_pointSize->value());
}
glBegin(GL_POINTS);
glVertex3f((float)that->estimate()(0),(float)that->estimate()(1),(float)that->estimate()(2));
glEnd();
glPopAttrib();
return this;
}
void EdgeSE3PointXYZDepth::linearizeOplus() {
//VertexSE3 *cam = static_cast<VertexSE3 *>(_vertices[0]);
VertexPointXYZ *vp = static_cast<VertexPointXYZ *>(_vertices[1]);
const Eigen::Vector3d& pt = vp->estimate();
Eigen::Vector3d Zcam = cache->w2lMatrix() * pt;
// J(0,3) = -0.0;
J(0,4) = -2*Zcam(2);
J(0,5) = 2*Zcam(1);
J(1,3) = 2*Zcam(2);
// J(1,4) = -0.0;
J(1,5) = -2*Zcam(0);
J(2,3) = -2*Zcam(1);
J(2,4) = 2*Zcam(0);
// J(2,5) = -0.0;
J.block<3,3>(0,6) = cache->w2lMatrix().rotation();
Eigen::Matrix<double,3,9> Jprime = params->Kcam_inverseOffsetR() * J;
Eigen::Vector3d Zprime = cache->w2i() * pt;
Eigen::Matrix<double, 3, 9> Jhom;
Jhom.block<2,9>(0,0) = 1/(Zprime(2)*Zprime(2)) * (Jprime.block<2,9>(0,0)*Zprime(2) - Zprime.head<2>() * Jprime.block<1,9>(2,0));
Jhom.block<1,9>(2,0) = Jprime.block<1,9>(2,0);
_jacobianOplusXi = Jhom.block<3,6>(0,0);
_jacobianOplusXj = Jhom.block<3,3>(0,6);
}
void EdgeSE3PointXYZ::linearizeOplus() {
//VertexSE3 *cam = static_cast<VertexSE3 *>(_vertices[0]);
VertexPointXYZ *vp = static_cast<VertexPointXYZ *>(_vertices[1]);
Eigen::Vector3d Zcam = cache->w2l() * vp->estimate();
// J(0,3) = -0.0;
J(0,4) = -2*Zcam(2);
J(0,5) = 2*Zcam(1);
J(1,3) = 2*Zcam(2);
// J(1,4) = -0.0;
J(1,5) = -2*Zcam(0);
J(2,3) = -2*Zcam(1);
J(2,4) = 2*Zcam(0);
// J(2,5) = -0.0;
J.block<3,3>(0,6) = cache->w2l().rotation();
Eigen::Matrix<double,3,9> Jhom = offsetParam->inverseOffset().rotation() * J;
_jacobianOplusXi = Jhom.block<3,6>(0,0);
_jacobianOplusXj = Jhom.block<3,3>(0,6);
}
HyperGraphElementAction* EdgeProjectDisparityDrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_ ){
if (typeid(*element).name()!=_typeName)
return 0;
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
EdgeSE3PointXYZDisparity* e = static_cast<EdgeSE3PointXYZDisparity*>(element);
VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertices()[0]);
VertexPointXYZ* toEdge = static_cast<VertexPointXYZ*>(e->vertices()[1]);
if (! fromEdge || ! toEdge)
return this;
Eigen::Isometry3d fromTransform=fromEdge->estimate() * e->cameraParameter()->offset();
glColor3f(LANDMARK_EDGE_COLOR);
glPushAttrib(GL_ENABLE_BIT);
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glVertex3f((float)fromTransform.translation().x(),(float)fromTransform.translation().y(),(float)fromTransform.translation().z());
glVertex3f((float)toEdge->estimate().x(),(float)toEdge->estimate().y(),(float)toEdge->estimate().z());
glEnd();
glPopAttrib();
return this;
}
void EdgeProjectXYZ2UVQ::linearizeOplus()
{
VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
SE3Quat T(vj->estimate());
VertexPointXYZ* vi = static_cast<VertexPointXYZ*>(_vertices[0]);
Vector3d xyz = vi->estimate();
Vector3d xyz_trans = T.map(xyz);
const double & x = xyz_trans[0];
const double & y = xyz_trans[1];
const double & z = xyz_trans[2];
double z_sq = z*z;
const double & Fx = vj->_focal_length(0);
const double & Fy = vj->_focal_length(1);
double dq_dz = -Fx/z_sq;
double x_Fx_by_zsq = x*Fx/z_sq;
double y_Fy_by_zsq = y*Fy/z_sq;
Matrix3d R = T.rotation().toRotationMatrix();
_jacobianOplusXi.row(0) = -Fx/z*R.row(0) + x_Fx_by_zsq*R.row(2);
_jacobianOplusXi.row(1) = -Fy/z*R.row(1) + y_Fy_by_zsq*R.row(2);
_jacobianOplusXi.row(2) = -dq_dz*R.row(2);
_jacobianOplusXj(0,0) = x*y/z_sq *Fx;
_jacobianOplusXj(0,1) = -(1+(x*x/z_sq)) *Fx;
_jacobianOplusXj(0,2) = y/z *Fx;
_jacobianOplusXj(0,3) = -1./z *Fx;
_jacobianOplusXj(0,4) = 0;
_jacobianOplusXj(0,5) = x/z_sq *Fx;
_jacobianOplusXj(1,0) = (1+y*y/z_sq) *Fy;
_jacobianOplusXj(1,1) = -x*y/z_sq *Fy;
_jacobianOplusXj(1,2) = -x/z *Fy;
_jacobianOplusXj(1,3) = 0;
_jacobianOplusXj(1,4) = -1./z *Fy;
_jacobianOplusXj(1,5) = y/z_sq *Fy;
_jacobianOplusXj(2,0) = -y*dq_dz ;
_jacobianOplusXj(2,1) = x*dq_dz;
_jacobianOplusXj(2,2) = 0;
_jacobianOplusXj(2,3) = 0;
_jacobianOplusXj(2,4) = 0;
_jacobianOplusXj(2,5) = -dq_dz ;
// std::cerr << _jacobianOplusXi << std::endl;
// std::cerr << _jacobianOplusXj << std::endl;
// BaseBinaryEdge<3, Vector3d, VertexPointXYZ, VertexSE3Expmap, false>::linearizeOplus();
// std::cerr << _jacobianOplusXi << std::endl;
// std::cerr << _jacobianOplusXj << std::endl;
}
void EdgeSE3PointXYZ::computeError() {
// from cam to point (track)
//VertexSE3 *cam = static_cast<VertexSE3*>(_vertices[0]);
VertexPointXYZ *point = static_cast<VertexPointXYZ*>(_vertices[1]);
Eigen::Vector3d perr = cache->w2n() * point->estimate();
// error, which is backwards from the normal observed - calculated
// _measurement is the measured projection
_error = perr - _measurement;
// std::cout << _error << std::endl << std::endl;
}
void EdgeSE3LotsOfXYZ::computeError(){
VertexSE3 * pose = static_cast<VertexSE3 *> (_vertices[0]);
for(unsigned int i=0; i<_observedPoints; i++){
VertexPointXYZ * xyz = static_cast<VertexPointXYZ *> (_vertices[1+i]);
Vector3D m = pose->estimate().inverse() * xyz->estimate();
unsigned int index = 3*i;
_error[index] = m[0] - _measurement[index];
_error[index+1] = m[1] - _measurement[index+1];
_error[index+2] = m[2] - _measurement[index+2];
}
}
HyperGraphElementAction* VertexPointXYZWriteGnuplotAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_ )
{
if (typeid(*element).name()!=_typeName)
return 0;
WriteGnuplotAction::Parameters* params=static_cast<WriteGnuplotAction::Parameters*>(params_);
if (!params->os) {
std::cerr << __PRETTY_FUNCTION__ << ": warning, no valid os specified" << std::endl;
return 0;
}
VertexPointXYZ* v = static_cast<VertexPointXYZ*>(element);
*(params->os) << v->estimate().x() << " " << v->estimate().y() << " " << v->estimate().z() << " " << std::endl;
return this;
}
bool EdgeSE3PointXYZDepth::setMeasurementFromState(){
//VertexSE3 *cam = static_cast<VertexSE3*>(_vertices[0]);
VertexPointXYZ *point = static_cast<VertexPointXYZ*>(_vertices[1]);
// calculate the projection
const Eigen::Vector3d& pt = point->estimate();
Eigen::Vector3d p = cache->w2i() * pt;
Eigen::Vector3d perr;
perr.head<2>() = p.head<2>()/p(2);
perr(2) = p(2);
_measurement = perr;
return true;
}
void EdgeSE3PointXYZDepth::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to_*/)
{
(void) from;
assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");
VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);
const Eigen::Matrix<double, 3, 3>& invKcam = params->invKcam();
Eigen::Vector3d p;
p(2) = _measurement(2);
p.head<2>() = _measurement.head<2>()*p(2);
p=invKcam*p;
point->setEstimate(cam->estimate() * (params->offsetMatrix() * p));
}
void EdgeSE3PointXYZ::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to_*/)
{
(void) from;
assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");
VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);
// SE3OffsetCache* vcache = (SE3OffsetCache* ) cam->getCache(_cacheIds[0]);
// if (! vcache){
// cerr << "fatal error in retrieving cache" << endl;
// }
// SE3OffsetParameters* params=vcache->params;
Eigen::Vector3d p=_measurement;
point->setEstimate(cam->estimate() * (offsetParam->offset() * p));
}
bool EdgeSE3PointXYZ::setMeasurementFromState(){
//VertexSE3 *cam = static_cast<VertexSE3*>(_vertices[0]);
VertexPointXYZ *point = static_cast<VertexPointXYZ*>(_vertices[1]);
// calculate the projection
const Vector3d &pt = point->estimate();
// SE3OffsetCache* vcache = (SE3OffsetCache*) cam->getCache(_cacheIds[0]);
// if (! vcache){
// cerr << "fatal error in retrieving cache" << endl;
// }
Eigen::Vector3d perr = cache->w2n() * pt;
_measurement = perr;
return true;
}
bool EdgeSE3LotsOfXYZ::setMeasurementFromState(){
VertexSE3 * pose = static_cast<VertexSE3 *> (_vertices[0]);
Eigen::Transform<double, 3, 1> poseinv = pose->estimate().inverse();
for(unsigned int i=0; i<_observedPoints; i++){
VertexPointXYZ * xyz = static_cast<VertexPointXYZ *> (_vertices[1+i]);
// const Vector3D &pt = xyz->estimate();
Vector3D m = poseinv * xyz->estimate();
unsigned int index = 3*i;
_measurement[index] = m[0];
_measurement[index+1] = m[1];
_measurement[index+2] = m[2];
}
return true;
}
void EdgeSE3PointXYZDisparity::linearizeOplus() {
//VertexSE3 *cam = static_cast<VertexSE3 *>(_vertices[0]);
VertexPointXYZ *vp = static_cast<VertexPointXYZ *>(_vertices[1]);
// VertexCameraCache* vcache = (VertexCameraCache*)cam->getCache(_cacheIds[0]);
// if (! vcache){
// cerr << "fatal error in retrieving cache" << endl;
// }
// CacheCamera* vcache = cache;
// if (! vcache){
// cerr << "fatal error in retrieving cache" << endl;
// }
const Eigen::Vector3d& pt = vp->estimate();
Eigen::Vector3d Zcam = cache->w2l() * vp->estimate();
// J(0,3) = -0.0;
J(0,4) = -2*Zcam(2);
J(0,5) = 2*Zcam(1);
J(1,3) = 2*Zcam(2);
// J(1,4) = -0.0;
J(1,5) = -2*Zcam(0);
J(2,3) = -2*Zcam(1);
J(2,4) = 2*Zcam(0);
// J(2,5) = -0.0;
J.block<3,3>(0,6) = cache->w2l().rotation();
//Eigen::Matrix<double,3,9> Jprime = vcache->params->Kcam_inverseOffsetR * J;
Eigen::Matrix<double,3,9> Jprime = params->Kcam_inverseOffsetR() * J;
Eigen::Matrix<double, 3, 9> Jhom;
Eigen::Vector3d Zprime = cache->w2i() * pt;
Jhom.block<2,9>(0,0) = 1/(Zprime(2)*Zprime(2)) * (Jprime.block<2,9>(0,0)*Zprime(2) - Zprime.head<2>() * Jprime.block<1,9>(2,0));
Jhom.block<1,9>(2,0) = - 1/(Zprime(2)*Zprime(2)) * Jprime.block<1,9>(2,0);
_jacobianOplusXi = Jhom.block<3,6>(0,0);
_jacobianOplusXj = Jhom.block<3,3>(0,6);
}
void EdgeProjectXYZ2UV::linearizeOplus()
{
VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
SE3Quat T(vj->estimate());
VertexPointXYZ* vi = static_cast<VertexPointXYZ*>(_vertices[0]);
Vector3d xyz = vi->estimate();
Vector3d xyz_trans = T.map(xyz);
double x = xyz_trans[0];
double y = xyz_trans[1];
double z = xyz_trans[2];
double z_2 = z*z;
Matrix<double,2,3> tmp;
tmp(0,0) = vj->_focal_length(0);
tmp(0,1) = 0;
tmp(0,2) = -x/z*vj->_focal_length(0);
tmp(1,0) = 0;
tmp(1,1) = vj->_focal_length(1);
tmp(1,2) = -y/z*vj->_focal_length(1);
_jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();
_jacobianOplusXj(0,0) = x*y/z_2 *vj->_focal_length(0);
_jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *vj->_focal_length(0);
_jacobianOplusXj(0,2) = y/z *vj->_focal_length(0);
_jacobianOplusXj(0,3) = -1./z *vj->_focal_length(0);
_jacobianOplusXj(0,4) = 0;
_jacobianOplusXj(0,5) = x/z_2 *vj->_focal_length(0);
_jacobianOplusXj(1,0) = (1+y*y/z_2) *vj->_focal_length(1);
_jacobianOplusXj(1,1) = -x*y/z_2 *vj->_focal_length(1);
_jacobianOplusXj(1,2) = -x/z *vj->_focal_length(1);
_jacobianOplusXj(1,3) = 0;
_jacobianOplusXj(1,4) = -1./z *vj->_focal_length(1);
_jacobianOplusXj(1,5) = y/z_2 *vj->_focal_length(1);
}
void EdgeSE3PointXYZDisparity::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)
{
(void) from;
assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");
VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);
// VertexCameraCache* vcache = (VertexCameraCache* ) cam->getCache(_cacheIds[0]);
// if (! vcache){
// cerr << "fatal error in retrieving cache" << endl;
// }
//ParameterCamera* params=vcache->params;
const Eigen::Matrix<double, 3, 3>& invKcam = params->invKcam();
Eigen::Vector3d p;
double w=1./_measurement(2);
p.head<2>() = _measurement.head<2>()*w;
p(2) = w;
p = invKcam * p;
p = cam->estimate() * (params->offset() * p);
point->setEstimate(p);
}
bool EdgeSE3PointXYZDisparity::setMeasurementFromState(){
//VertexSE3 *cam = static_cast< VertexSE3*>(_vertices[0]);
VertexPointXYZ *point = static_cast<VertexPointXYZ*>(_vertices[1]);
const Vector3d &pt = point->estimate();
// VertexCameraCache* vcache = (VertexCameraCache*) cam->getCache(_cacheIds[0]);
// if (! vcache){
// cerr << "fatal error in retrieving cache" << endl;
// }
Eigen::Vector3d p = cache->w2i() * pt;
Eigen::Vector3d perr;
perr.head<2>() = p.head<2>()/p(2);
perr(2) = 1/p(2);
// error, which is backwards from the normal observed - calculated
// _measurement is the measured projection
_measurement = perr;
return true;
}
HyperGraphElementAction* VertexPointXYZDrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params ) {
if (typeid(*element).name()!=_typeName)
return 0;
initializeDrawActionsCache();
refreshPropertyPtrs(params);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
VertexPointXYZ* that = static_cast<VertexPointXYZ*>(element);
glPushMatrix();
glPushAttrib(GL_ENABLE_BIT | GL_POINT_BIT);
glDisable(GL_LIGHTING);
glColor3f(LANDMARK_VERTEX_COLOR);
float ps = _pointSize ? _pointSize->value() : 1.f;
glTranslatef((float)that->estimate()(0),(float)that->estimate()(1),(float)that->estimate()(2));
opengl::drawPoint(ps);
glPopAttrib();
drawCache(that->cacheContainer(), params);
drawUserData(that->userData(), params);
glPopMatrix();
return this;
}
HyperGraphElementAction* EdgeSE3PointXYZDrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
EdgeSE3PointXYZ* e = static_cast<EdgeSE3PointXYZ*>(element);
VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertex(0));
VertexPointXYZ* toEdge = static_cast<VertexPointXYZ*>(e->vertex(1));
glColor3f(0.8f,0.3f,0.3f);
glPushAttrib(GL_ENABLE_BIT);
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glVertex3f((float)fromEdge->estimate().translation().x(),(float)fromEdge->estimate().translation().y(),(float)fromEdge->estimate().translation().z());
glVertex3f((float)toEdge->estimate().x(),(float)toEdge->estimate().y(),(float)toEdge->estimate().z());
glEnd();
glPopAttrib();
return this;
}
int main(int argc, char** argv)
{
string inputFilename;
string outputFilename;
// command line parsing
CommandArgs commandLineArguments;
commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed");
commandLineArguments.paramLeftOver("gm2dl-output", outputFilename, "", "name of the output file");
commandLineArguments.parseArgs(argc, argv);
OptimizableGraph inputGraph;
bool loadStatus = inputGraph.load(inputFilename.c_str());
if (! loadStatus) {
cerr << "Error while loading input data" << endl;
return 1;
}
OptimizableGraph outputGraph;
// process all the vertices first
double fx = -1;
double baseline = -1;
bool firstCam = true;
for (OptimizableGraph::VertexIDMap::const_iterator it = inputGraph.vertices().begin(); it != inputGraph.vertices().end(); ++it) {
if (dynamic_cast<VertexCam*>(it->second)) {
VertexCam* v = static_cast<VertexCam*>(it->second);
if (firstCam) {
firstCam = false;
g2o::ParameterCamera* camParams = new g2o::ParameterCamera;
camParams->setId(0);
const SBACam& c = v->estimate();
baseline = c.baseline;
fx = c.Kcam(0,0);
camParams->setKcam(c.Kcam(0,0), c.Kcam(1,1), c.Kcam(0,2), c.Kcam(1,2));
outputGraph.addParameter(camParams);
}
VertexSE3* ov = new VertexSE3;
ov->setId(v->id());
Eigen::Isometry3d p;
p = v->estimate().rotation();
p.translation() = v->estimate().translation();
ov->setEstimate(p);
if (! outputGraph.addVertex(ov)) {
assert(0 && "Failure adding camera vertex");
}
}
else if (dynamic_cast<VertexSBAPointXYZ*>(it->second)) {
VertexSBAPointXYZ* v = static_cast<VertexSBAPointXYZ*>(it->second);
VertexPointXYZ* ov = new VertexPointXYZ;
ov->setId(v->id());
ov->setEstimate(v->estimate());
if (! outputGraph.addVertex(ov)) {
assert(0 && "Failure adding camera vertex");
}
}
}
for (OptimizableGraph::EdgeSet::const_iterator it = inputGraph.edges().begin(); it != inputGraph.edges().end(); ++it) {
if (dynamic_cast<EdgeProjectP2SC*>(*it)) {
EdgeProjectP2SC* e = static_cast<EdgeProjectP2SC*>(*it);
EdgeSE3PointXYZDisparity* oe = new EdgeSE3PointXYZDisparity;
oe->vertices()[0] = outputGraph.vertex(e->vertices()[1]->id());
oe->vertices()[1] = outputGraph.vertex(e->vertices()[0]->id());
double kx = e->measurement().x();
double ky = e->measurement().y();
double disparity = kx - e->measurement()(2);
oe->setMeasurement(Eigen::Vector3d(kx, ky, disparity / (fx * baseline)));
oe->setInformation(e->information()); // TODO convert information matrix
oe->setParameterId(0,0);
if (! outputGraph.addEdge(oe)) {
assert(0 && "error adding edge");
}
}
}
cout << "Vertices in/out:\t" << inputGraph.vertices().size() << " " << outputGraph.vertices().size() << endl;
cout << "Edges in/out:\t" << inputGraph.edges().size() << " " << outputGraph.edges().size() << endl;
cout << "Writing output ... " << flush;
outputGraph.save(outputFilename.c_str());
cout << "done." << endl;
return 0;
}
