bool EdgeSE3::setMeasurementFromState() {
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]);
Eigen::Isometry3d delta = from->estimate().inverse() * to->estimate();
setMeasurement(delta);
return true;
}
HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
if (! _cacheDrawActions){
_cacheDrawActions = HyperGraphActionLibrary::instance()->actionByName("draw");
}
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
VertexSE3* that = static_cast<VertexSE3*>(element);
glColor3f(0.5,0.5,0.8);
glPushMatrix();
glTranslatef(that->estimate().translation().x(),that->estimate().translation().y(),that->estimate().translation().z());
AngleAxisd aa(that->estimate().rotation());
glRotatef(RAD2DEG(aa.angle()),aa.axis().x(),aa.axis().y(),aa.axis().z());
if (_triangleX && _triangleY){
drawTriangle(_triangleX->value(), _triangleY->value());
}
CacheContainer* caches=that->cacheContainer();
if (caches){
for (CacheContainer::iterator it=caches->begin(); it!=caches->end(); it++){
Cache* c = it->second;
(*_cacheDrawActions)(c, params_);
}
}
glPopMatrix();
return this;
}
HyperGraphElementAction* EdgeSE3DrawAction::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;
EdgeSE3* e = static_cast<EdgeSE3*>(element);
VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertices()[0]);
VertexSE3* toEdge = static_cast<VertexSE3*>(e->vertices()[1]);
if (! fromEdge || ! toEdge)
return this;
glColor3f(POSE_EDGE_COLOR);
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().translation().x(),(float)toEdge->estimate().translation().y(),(float)toEdge->estimate().translation().z());
glEnd();
glPopAttrib();
return this;
}
void EdgeSE3::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/) {
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]);
if (from_.count(from) > 0) {
to->setEstimate(from->estimate() * _measurement);
} else
from->setEstimate(to->estimate() * _measurement.inverse());
//cerr << "IE" << endl;
}
void EdgeSE3Offset::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/) {
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]);
Eigen::Isometry3d virtualMeasurement = _cacheFrom->offsetParam()->offset() * measurement() * _cacheTo->offsetParam()->offset().inverse();
if (from_.count(from) > 0) {
to->setEstimate(from->estimate() * virtualMeasurement);
} else
from->setEstimate(to->estimate() * virtualMeasurement.inverse());
}
void EdgeSE3Prior::initialEstimate(const OptimizableGraph::VertexSet& /*from_*/, OptimizableGraph::Vertex* /*to_*/) {
VertexSE3 *v = static_cast<VertexSE3*>(_vertices[0]);
SE3Quat newEstimate = _offsetParam->offset().inverse() * measurement();
if (_information.block<3,3>(0,0).squaredNorm()==0){ // do not set translation
newEstimate.setTranslation(v->estimate().translation());
}
if (_information.block<3,3>(3,3).squaredNorm()==0){ // do not set rotation
newEstimate.setRotation(v->estimate().rotation());
}
v->setEstimate(newEstimate);
}
void EdgeSE3Prior::linearizeOplus(){
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
Isometry3 E;
Isometry3 Z, X, P;
X=from->estimate().rotation().toRotationMatrix();
X.translation()=from->estimate().translation();
P=_cache->offsetParam()->offset().rotation().toRotationMatrix();
P.translation()=_cache->offsetParam()->offset().translation();
Z=_measurement.rotation().toRotationMatrix();
Z.translation()=_measurement.translation();
internal::computeEdgeSE3PriorGradient(E, _jacobianOplusXi, Z, X, P);
}
void EdgeSE3::linearizeOplus() {
// BaseBinaryEdge<6, Eigen::Isometry3d, VertexSE3, VertexSE3>::linearizeOplus();
// return;
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]);
Eigen::Isometry3d E;
const Eigen::Isometry3d& Xi=from->estimate();
const Eigen::Isometry3d& Xj=to->estimate();
const Eigen::Isometry3d& Z=_measurement;
computeEdgeSE3Gradient(E, _jacobianOplusXi , _jacobianOplusXj, Z, Xi, Xj);
}
HyperGraphElementAction* VertexSE3DrawAction::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;
VertexSE3* that = static_cast<VertexSE3*>(element);
glColor3f(POSE_VERTEX_COLOR);
glPushMatrix();
glMultMatrixd(that->estimate().matrix().data());
opengl::drawArrow2D(_triangleX->value(), _triangleY->value(), _triangleX->value()*.3f);
drawCache(that->cacheContainer(), params_);
drawUserData(that->userData(), params_);
if(_showId && _showId->value()){
float scale = _idSize ? _idSize->value() : 1.f;
drawId(std::to_string(that->id()), scale);
}
glPopMatrix();
return this;
}
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 GraphicEnd::savepcd()
{
cout<<"save final pointcloud"<<endl;
SLAMEnd slam;
slam.init(NULL);
SparseOptimizer& opt = slam.globalOptimizer;
opt.load("/home/lc/workspace/paper_related/Appolo/test/result/final_after.g2o");
ifstream fin("/home/lc/workspace/paper_related/Appolo/test/result/key_frame.txt");
PointCloud::Ptr output(new PointCloud());
PointCloud::Ptr curr( new PointCloud());
pcl::VoxelGrid<PointT> voxel;
voxel.setLeafSize(0.01f, 0.01f, 0.01f );
string pclPath ="/media/新加卷/dataset/dataset1/pcd/";
pcl::PassThrough<PointT> pass;
pass.setFilterFieldName("z");
double z = 5.0;
pass.setFilterLimits(0.0, z);
while( !fin.eof() )
{
int frame, id;
fin>>id>>frame;
ss<<pclPath<<frame<<".pcd";
string str;
ss>>str;
cout<<"loading "<<str<<endl;
ss.clear();
pcl::io::loadPCDFile( str.c_str(), *curr );
//cout<<"curr cloud size is: "<<curr->points.size()<<endl;
VertexSE3* pv = dynamic_cast<VertexSE3*> (opt.vertex( id ));
if (pv == NULL)
break;
Eigen::Isometry3d pos = pv->estimate();
cout<<pos.matrix()<<endl;
voxel.setInputCloud( curr );
PointCloud::Ptr tmp( new PointCloud());
voxel.filter( *tmp );
curr.swap( tmp );
pass.setInputCloud( curr );
pass.filter(*tmp);
curr->swap( *tmp );
//cout<<"tmp: "<<tmp->points.size()<<endl;
pcl::transformPointCloud( *curr, *tmp, pos.matrix());
*output += *tmp;
//cout<<"output: "<<output->points.size()<<endl;
}
voxel.setInputCloud( output );
PointCloud::Ptr output_filtered( new PointCloud );
voxel.filter( *output_filtered );
output->swap( *output_filtered );
//cout<<output->points.size()<<endl;
pcl::io::savePCDFile( "/home/lc/workspace/paper_related/Appolo/test/result/result.pcd", *output);
cout<<"final result saved."<<endl;
}
void EdgeSE3PointXYZCov::initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* /*to_*/)
{
VertexSE3* from = static_cast<VertexSE3*>(_vertices[0]);
VertexPointXYZCov* to = static_cast<VertexPointXYZCov*>(_vertices[1]);
if (from_.count(from) > 0)
to->setEstimate(from->estimate() * measurement());
else
std::cerr << __PRETTY_FUNCTION__ << std::endl;
}
HyperGraphElementAction* VertexSE3WriteGnuplotAction::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;
}
VertexSE3* v = static_cast<VertexSE3*>(element);
*(params->os) << v->estimate().translation().x() << " "
<< v->estimate().translation().y() << " "
<< v->estimate().translation().z() << " ";
*(params->os) << v->estimate().rotation().x() << " "
<< v->estimate().rotation().y() << " "
<< v->estimate().rotation().z() << " " << std::endl;
return this;
}
void EdgeSE3Offset::linearizeOplus(){
//BaseBinaryEdge<6, SE3Quat, VertexSE3, VertexSE3>::linearizeOplus();
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]);
Eigen::Isometry3d E;
const Eigen::Isometry3d& Xi=from->estimate();
const Eigen::Isometry3d& Xj=to->estimate();
const Eigen::Isometry3d& Pi=_cacheFrom->offsetParam()->offset();
const Eigen::Isometry3d& Pj=_cacheTo->offsetParam()->offset();
const Eigen::Isometry3d& Z=_measurement;
// Matrix6d Ji, Jj;
// computeSE3Gradient(E, Ji , Jj,
// Z, Pi, Xi, Pj, Xj);
// cerr << "Ji:" << endl;
// cerr << Ji-_jacobianOplusXi << endl;
// cerr << "Jj:" << endl;
// cerr << Jj-_jacobianOplusXj << endl;
internal::computeEdgeSE3Gradient(E, _jacobianOplusXi , _jacobianOplusXj, Z, Xi, Xj, Pi, Pj);
}
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];
}
}
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));
}
HyperGraphElementAction* EdgeSE3WriteGnuplotAction::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, on valid os specified" << std::endl;
return 0;
}
EdgeSE3* e = static_cast<EdgeSE3*>(element);
VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertices()[0]);
VertexSE3* toEdge = static_cast<VertexSE3*>(e->vertices()[1]);
Vector6d fromV, toV;
fromV=toVectorMQT(fromEdge->estimate());
toV=toVectorMQT(toEdge->estimate());
for (int i=0; i<6; i++) {
*(params->os) << fromV[i] << " ";
}
for (int i=0; i<6; i++) {
*(params->os) << toV[i] << " ";
}
*(params->os) << std::endl;
return this;
}
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));
}
HyperGraphElementAction* VertexSE3WriteGnuplotAction::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, on valid os specified" << std::endl;
return 0;
}
VertexSE3* v = static_cast<VertexSE3*>(element);
Vector6d est=toVectorMQT(v->estimate());
for (int i=0; i<6; i++)
*(params->os) << est[i] << " ";
*(params->os) << std::endl;
return this;
}
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::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);
}
HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
if (! _cacheDrawActions){
_cacheDrawActions = HyperGraphActionLibrary::instance()->actionByName("draw");
}
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
VertexSE3* that = static_cast<VertexSE3*>(element);
glColor3f(0.5f,0.5f,0.8f);
glPushMatrix();
glMultMatrixd(that->estimate().matrix().data());
if (_triangleX && _triangleY){
drawTriangle(_triangleX->value(), _triangleY->value());
}
CacheContainer* caches=that->cacheContainer();
if (caches){
for (CacheContainer::iterator it=caches->begin(); it!=caches->end(); it++){
Cache* c = it->second;
(*_cacheDrawActions)(c, params_);
}
}
OptimizableGraph::Data* d=that->userData();
while (d && _cacheDrawActions ){
(*_cacheDrawActions)(d, params_);
d=d->next();
}
glPopMatrix();
return this;
}
void EdgeSE3::computeError() {
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]);
Eigen::Isometry3d delta=_inverseMeasurement * from->estimate().inverse() * to->estimate();
_error=toVectorMQT(delta);
}
int main(int argc, char **argv)
{
int num_points = 0;
// check for arg, # of points to use in projection SBA
if (argc > 1)
num_points = atoi(argv[1]);
double euc_noise = 0.1; // noise in position, m
double pix_noise = 1.0; // pixel noise
// double outlier_ratio = 0.1;
SparseOptimizer optimizer;
optimizer.setVerbose(false);
// variable-size block solver
BlockSolverX::LinearSolverType * linearSolver
= new LinearSolverCSparse<g2o
::BlockSolverX::PoseMatrixType>();
BlockSolverX * solver_ptr
= new BlockSolverX(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
vector<Vector3d> true_points;
for (size_t i=0;i<1000; ++i)
{
true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
Sample::uniform()-0.5,
Sample::uniform()+10));
}
// set up camera params
Vector2d focal_length(500,500); // pixels
Vector2d principal_point(320,240); // 640x480 image
double baseline = 0.075; // 7.5 cm baseline
// set up camera params and projection matrices on vertices
g2o::VertexSCam::setKcam(focal_length[0],focal_length[1],
principal_point[0],principal_point[1],
baseline);
// set up two poses
int vertex_id = 0;
for (size_t i=0; i<2; ++i)
{
// set up rotation and translation for this node
Vector3d t(0,0,i);
Quaterniond q;
q.setIdentity();
Eigen::Isometry3d cam; // camera pose
cam = q;
cam.translation() = t;
// set up node
VertexSCam *vc = new VertexSCam();
vc->setEstimate(cam);
vc->setId(vertex_id); // vertex id
cerr << t.transpose() << " | " << q.coeffs().transpose() << endl;
// set first cam pose fixed
if (i==0)
vc->setFixed(true);
// make sure projection matrices are set
vc->setAll();
// add to optimizer
optimizer.addVertex(vc);
vertex_id++;
}
// set up point matches for GICP
for (size_t i=0; i<true_points.size(); ++i)
{
// get two poses
VertexSE3* vp0 =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second);
VertexSE3* vp1 =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
// calculate the relative 3D position of the point
Vector3d pt0,pt1;
pt0 = vp0->estimate().inverse() * true_points[i];
pt1 = vp1->estimate().inverse() * true_points[i];
// add in noise
pt0 += Vector3d(Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ));
pt1 += Vector3d(Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ));
// form edge, with normals in varioius positions
Vector3d nm0, nm1;
nm0 << 0, i, 1;
nm1 << 0, i, 1;
nm0.normalize();
nm1.normalize();
Edge_V_V_GICP * e // new edge with correct cohort for caching
= new Edge_V_V_GICP();
e->vertices()[0] // first viewpoint
= dynamic_cast<OptimizableGraph::Vertex*>(vp0);
e->vertices()[1] // second viewpoint
= dynamic_cast<OptimizableGraph::Vertex*>(vp1);
EdgeGICP meas;
meas.pos0 = pt0;
meas.pos1 = pt1;
meas.normal0 = nm0;
meas.normal1 = nm1;
e->setMeasurement(meas);
meas = e->measurement();
// e->inverseMeasurement().pos() = -kp;
// use this for point-plane
e->information() = meas.prec0(0.01);
// use this for point-point
// e->information().setIdentity();
// e->setRobustKernel(true);
//e->setHuberWidth(0.01);
optimizer.addEdge(e);
}
// set up SBA projections with some number of points
true_points.clear();
for (int i=0;i<num_points; ++i)
{
true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
Sample::uniform()-0.5,
Sample::uniform()+10));
}
// add point projections to this vertex
for (size_t i=0; i<true_points.size(); ++i)
{
g2o::VertexSBAPointXYZ * v_p
= new g2o::VertexSBAPointXYZ();
v_p->setId(vertex_id++);
v_p->setMarginalized(true);
v_p->setEstimate(true_points.at(i)
+ Vector3d(Sample::gaussian(1),
Sample::gaussian(1),
Sample::gaussian(1)));
optimizer.addVertex(v_p);
for (size_t j=0; j<2; ++j)
{
Vector3d z;
dynamic_cast<g2o::VertexSCam*>
(optimizer.vertices().find(j)->second)
->mapPoint(z,true_points.at(i));
if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480)
{
z += Vector3d(Sample::gaussian(pix_noise),
Sample::gaussian(pix_noise),
Sample::gaussian(pix_noise/16.0));
g2o::Edge_XYZ_VSC * e
= new g2o::Edge_XYZ_VSC();
e->vertices()[0]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p);
e->vertices()[1]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>
(optimizer.vertices().find(j)->second);
e->setMeasurement(z);
//e->inverseMeasurement() = -z;
e->information() = Matrix3d::Identity();
//e->setRobustKernel(false);
//e->setHuberWidth(1);
optimizer.addEdge(e);
}
}
} // done with adding projection points
// move second cam off of its true position
VertexSE3* vc =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
Eigen::Isometry3d cam = vc->estimate();
cam.translation() = Vector3d(-0.1,0.1,0.2);
vc->setEstimate(cam);
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
cout << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;
optimizer.setVerbose(true);
optimizer.optimize(20);
cout << endl << "Second vertex should be near 0,0,1" << endl;
cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second)
->estimate().translation().transpose() << endl;
cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second)
->estimate().translation().transpose() << endl;
}
int main()
{
double euc_noise = 0.01; // noise in position, m
// double outlier_ratio = 0.1;
SparseOptimizer optimizer;
optimizer.setVerbose(false);
// variable-size block solver
BlockSolverX::LinearSolverType * linearSolver = new LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
BlockSolverX * solver_ptr = new BlockSolverX(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
vector<Vector3d> true_points;
for (size_t i=0;i<1000; ++i)
{
true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
Sample::uniform()-0.5,
Sample::uniform()+10));
}
// set up two poses
int vertex_id = 0;
for (size_t i=0; i<2; ++i)
{
// set up rotation and translation for this node
Vector3d t(0,0,i);
Quaterniond q;
q.setIdentity();
Eigen::Isometry3d cam; // camera pose
cam = q;
cam.translation() = t;
// set up node
VertexSE3 *vc = new VertexSE3();
vc->setEstimate(cam);
vc->setId(vertex_id); // vertex id
cerr << t.transpose() << " | " << q.coeffs().transpose() << endl;
// set first cam pose fixed
if (i==0)
vc->setFixed(true);
// add to optimizer
optimizer.addVertex(vc);
vertex_id++;
}
// set up point matches
for (size_t i=0; i<true_points.size(); ++i)
{
// get two poses
VertexSE3* vp0 =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second);
VertexSE3* vp1 =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
// calculate the relative 3D position of the point
Vector3d pt0,pt1;
pt0 = vp0->estimate().inverse() * true_points[i];
pt1 = vp1->estimate().inverse() * true_points[i];
// add in noise
pt0 += Vector3d(Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ));
pt1 += Vector3d(Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ));
// form edge, with normals in varioius positions
Vector3d nm0, nm1;
nm0 << 0, i, 1;
nm1 << 0, i, 1;
nm0.normalize();
nm1.normalize();
Edge_V_V_GICP * e // new edge with correct cohort for caching
= new Edge_V_V_GICP();
e->setVertex(0, vp0); // first viewpoint
e->setVertex(1, vp1); // second viewpoint
EdgeGICP meas;
meas.pos0 = pt0;
meas.pos1 = pt1;
meas.normal0 = nm0;
meas.normal1 = nm1;
e->setMeasurement(meas);
// e->inverseMeasurement().pos() = -kp;
meas = e->measurement();
// use this for point-plane
e->information() = meas.prec0(0.01);
// use this for point-point
// e->information().setIdentity();
// e->setRobustKernel(true);
//e->setHuberWidth(0.01);
optimizer.addEdge(e);
}
// move second cam off of its true position
VertexSE3* vc =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
Eigen::Isometry3d cam = vc->estimate();
cam.translation() = Vector3d(0,0,0.2);
vc->setEstimate(cam);
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
cout << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;
optimizer.setVerbose(true);
optimizer.optimize(5);
cout << endl << "Second vertex should be near 0,0,1" << endl;
cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second)
->estimate().translation().transpose() << endl;
cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second)
->estimate().translation().transpose() << endl;
}
