void SlamGraph<SE3,StereoCamera, SE3XYZ_STEREO, 3>
::addConstraintToG2o(const SE3 & T_2_from_1,
const Matrix6d &
Lambda_2_from_1,
int pose_id_1,
int pose_id_2,
g2o::SparseOptimizer * optimizer)
{
G2oEdgeSE3 * e = new G2oEdgeSE3();
g2o::HyperGraph::Vertex * pose1_vertex
= GET_MAP_ELEM(pose_id_1, optimizer->vertices());
e->vertices()[0]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>(pose1_vertex);
g2o::HyperGraph::Vertex * pose2_vertex
= GET_MAP_ELEM(pose_id_2, optimizer->vertices());
e->vertices()[1]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>(pose2_vertex);
e->setMeasurement( T_2_from_1);
e->information() = Lambda_2_from_1;
optimizer->addEdge(e);
}
void SlamGraph<Sim3, LinearCamera, Sim3XYZ, 2>
::addConstraintToG2o(const Sim3 & T_2_from_1,
const Matrix6d & Lambda_2_from_1,
int pose_id_1,
int pose_id_2,
g2o::SparseOptimizer * optimizer)
{
G2oEdgeSim3 * e = new G2oEdgeSim3();
g2o::HyperGraph::Vertex * pose1_vertex
= GET_MAP_ELEM(pose_id_1, optimizer->vertices());
e->vertices()[0]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>(pose1_vertex);
g2o::HyperGraph::Vertex * pose2_vertex
= GET_MAP_ELEM(pose_id_2, optimizer->vertices());
e->vertices()[1]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>(pose2_vertex);
e->measurement() = T_2_from_1;
e->inverseMeasurement() = T_2_from_1.inverse();
e->information() = Lambda_2_from_1;
optimizer->addEdge(e);
}
void SlamGraph<SE3,StereoCamera, SE3XYZ_STEREO, 3>
::addObsToG2o(const Vector3d & obs,
const Matrix3d & Lambda,
int g2o_point_id,
int pose_id,
int anchor_id,
bool robustify,
double huber_kernel_width,
g2o::SparseOptimizer * optimizer)
{
G2oEdgeProjectPSI2UVU * e = new G2oEdgeProjectPSI2UVU();
e->resize(3);
g2o::OptimizableGraph::Vertex * point_vertex
= dynamic_cast<g2o::OptimizableGraph::Vertex*>
(GET_MAP_ELEM(g2o_point_id, optimizer->vertices()));
g2o::OptimizableGraph::Vertex * pose_vertex
= dynamic_cast<g2o::OptimizableGraph::Vertex*>
(GET_MAP_ELEM(pose_id, optimizer->vertices()));
g2o::OptimizableGraph::Vertex * anchor_vertex
= dynamic_cast<g2o::OptimizableGraph::Vertex*>
(GET_MAP_ELEM(anchor_id, optimizer->vertices()));
assert(point_vertex!=NULL);
assert(point_vertex->dimension()==3);
e->vertices()[0] = point_vertex;
assert(pose_vertex!=NULL);
assert(pose_vertex->dimension()==6);
e->vertices()[1] = pose_vertex;
assert(anchor_vertex!=NULL);
assert(anchor_vertex->dimension()==6);
e->vertices()[2] = anchor_vertex;
e->setMeasurement(obs);
e->information() = Lambda;
if (robustify)
{
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
}
e->resizeParameters(1);
bool param_status = e->setParameterId(0, 0);
assert(param_status);
optimizer->addEdge(e);
}
void SlamGraph<Sim3,LinearCamera, Sim3XYZ, 2>
::addObsToG2o(const Vector2d & obs,
const Matrix2d & Lambda,
int g2o_point_id,
int pose_id,
int anchor_id,
bool robustify,
double huber_kernel_width,
g2o::SparseOptimizer * optimizer)
{
G2oEdgeSim3ProjectUVQ * e = new G2oEdgeSim3ProjectUVQ();
// TODO: implement anchored edges!!
assert(false);
e->resize(3);
g2o::HyperGraph::Vertex * point_vertex
= GET_MAP_ELEM(g2o_point_id, optimizer->vertices());
e->vertices()[0]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>(point_vertex);
g2o::HyperGraph::Vertex * pose_vertex
= GET_MAP_ELEM(pose_id, optimizer->vertices());
e->vertices()[1]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>(pose_vertex);
g2o::HyperGraph::Vertex * anchor_vertex
= GET_MAP_ELEM(anchor_id, optimizer->vertices());
e->vertices()[2]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>(anchor_vertex);
e->measurement() = obs;
e->inverseMeasurement() = -obs;
e->information() = Lambda;
if (robustify)
{
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
}
e->resizeParameters(1);
bool param_status = e->setParameterId(0, 0);
assert(param_status);
optimizer->addEdge(e);
}
