Moving VSLAMConfig into gtsam. VSLAMFactor lived there already, but was using VectorConfig directly... now using VSLAMConfig.
Chris Beall
parent
9c8994725d
commit
08c9718b12
|
|
@ -139,7 +139,7 @@ testSimulated3D_LDADD = libgtsam.la
|
|||
check_PROGRAMS += testSimulated3D
|
||||
|
||||
# Visual SLAM
|
||||
sources += CalibratedCamera.cpp SimpleCamera.cpp VSLAMFactor.cpp
|
||||
sources += CalibratedCamera.cpp SimpleCamera.cpp VSLAMConfig.cpp VSLAMFactor.cpp
|
||||
check_PROGRAMS += testCalibratedCamera testSimpleCamera testVSLAMFactor
|
||||
testCalibratedCamera_SOURCES = testCalibratedCamera.cpp
|
||||
testCalibratedCamera_LDADD = libgtsam.la
|
||||
|
|
|
|||
|
|
@ -0,0 +1,305 @@
|
|||
/**
|
||||
* @file VSLAMConfig.cpp
|
||||
* @brief The Config
|
||||
* @author Alireza Fathi
|
||||
* @author Carlos Nieto
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
|
||||
#include "VSLAMConfig.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
// trick from some reading group
|
||||
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
|
||||
|
||||
#define SIGMA 1.0
|
||||
|
||||
namespace gtsam{
|
||||
|
||||
/* ************************************************************************* */
|
||||
// add a delta to a DCVectorConfig
|
||||
// Very ugly: there are 3D vectors for points, and 12D vectors for poses
|
||||
// We check the dimension and do the right thing for each.
|
||||
VectorConfig DCVectorConfig::operator+(const VectorConfig & delta) const {
|
||||
DCVectorConfig result;
|
||||
string j; Vector v;
|
||||
FOREACH_PAIR(j, v, values) {
|
||||
if (v.size()==3) {
|
||||
Point3 basePoint(v);
|
||||
Point3 newPoint(basePoint.exmap( delta[j] ));
|
||||
result.insert( j, newPoint.vector() );
|
||||
} else {
|
||||
Pose3 basePose(v);
|
||||
Pose3 newPose(basePose.exmap( delta[j] ));
|
||||
result.insert( j, newPose.vector() );
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void DCVectorConfig::operator+=(const VectorConfig & delta) {
|
||||
for (iterator it = values.begin(); it!=values.end(); it++) {
|
||||
string j = it->first;
|
||||
Vector &v = it->second; // reference !
|
||||
// code below changes the reference in the config in-place !!!
|
||||
if (v.size()==3) {
|
||||
Point3 basePoint(v);
|
||||
Point3 newPoint(basePoint.exmap( delta[j] ));
|
||||
v = newPoint.vector();
|
||||
} else {
|
||||
Pose3 basePose(v);
|
||||
Pose3 newPose(basePose.exmap( delta[j] ));
|
||||
v = newPose.vector();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VSLAMConfig::VSLAMConfig(VectorConfig & fgconfig) {
|
||||
landmarkPoints.clear();
|
||||
cameraPoses.clear();
|
||||
|
||||
for(map<std::string, Vector>::const_iterator it = fgconfig.begin(); it != fgconfig.end(); it++)
|
||||
{
|
||||
string temp = (*it).first;
|
||||
if(temp[0] == 'x')
|
||||
{
|
||||
int cameraNumber = atoi(temp.substr(1,temp.size()-1).c_str());
|
||||
Pose3 cameraPose(Pose3((*it).second));
|
||||
addCameraPose(cameraNumber, cameraPose);
|
||||
}
|
||||
if(temp[0] == 'l')
|
||||
{
|
||||
int landmarkNumber = atoi(temp.substr(1,temp.size()-1).c_str());
|
||||
Pose3 markerPose(Pose3((*it).second));
|
||||
Point3 landmarkPoint = markerPose.translation();
|
||||
addLandmarkPoint(landmarkNumber, landmarkPoint);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// Exponential map
|
||||
// TODO: FD: I think this is horrible
|
||||
VSLAMConfig VSLAMConfig::exmap(const VectorConfig & delta) const {
|
||||
|
||||
VSLAMConfig newConfig;
|
||||
|
||||
for (map<string, Vector>::const_iterator it = delta.begin(); it
|
||||
!= delta.end(); it++) {
|
||||
string key = it->first;
|
||||
if (key[0] == 'x') {
|
||||
int cameraNumber = atoi(key.substr(1, key.size() - 1).c_str());
|
||||
Pose3 basePose = cameraPose(cameraNumber);
|
||||
newConfig.addCameraPose(cameraNumber, basePose.exmap(it->second));
|
||||
}
|
||||
if (key[0] == 'l') {
|
||||
int landmarkNumber = atoi(key.substr(1, key.size() - 1).c_str());
|
||||
Point3 basePoint = landmarkPoint(landmarkNumber);
|
||||
newConfig.addLandmarkPoint(landmarkNumber, basePoint.exmap(it->second));
|
||||
}
|
||||
}
|
||||
|
||||
return newConfig;
|
||||
}
|
||||
|
||||
// Not used!
|
||||
/* ************************************************************************* *
|
||||
void VSLAMConfig::load(std::string& path, int num_of_frames)
|
||||
{
|
||||
char putatives[200];
|
||||
putatives[0] = 0;
|
||||
sprintf(putatives, "%s/putatives.txt", path.c_str());
|
||||
|
||||
char poseR[200];
|
||||
poseR[0] = 0;
|
||||
sprintf(poseR, "%s/pose_R_info.txt", path.c_str());
|
||||
|
||||
char poseXYZ[200];
|
||||
poseXYZ[0] = 0;
|
||||
sprintf(poseXYZ, "%s/pose_xyz_info.txt", path.c_str());
|
||||
|
||||
std::ifstream putatives_In(path.c_str(), ios::in);
|
||||
std::ifstream poseR_In(path.c_str(), ios::in);
|
||||
std::ifstream poseXYZ_In(path.c_str(), ios::in);
|
||||
|
||||
int frameNumber;
|
||||
int landmarkIndex;
|
||||
|
||||
double landmarkX;
|
||||
double landmarkY;
|
||||
double landmarkZ;
|
||||
|
||||
double uLs;
|
||||
double vLs;
|
||||
|
||||
int cameraIndex;
|
||||
|
||||
double cameraX;
|
||||
double cameraY;
|
||||
double cameraZ;
|
||||
|
||||
double pitch;
|
||||
double yaw;
|
||||
double roll;
|
||||
|
||||
|
||||
if(putatives_In)
|
||||
putatives_In >> frameNumber >> landmarkIndex >> uLs >> vLs >> landmarkX >> landmarkY >> landmarkZ;
|
||||
else
|
||||
{
|
||||
printf("Unable to load values from putatives\n");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if(poseR_In)
|
||||
poseR_In >> yaw >> pitch >> roll;
|
||||
else
|
||||
{
|
||||
printf("Unable to load values from poseR\n");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if(poseXYZ_In)
|
||||
poseXYZ_In >> cameraX >> cameraY >> cameraZ;
|
||||
else
|
||||
{
|
||||
printf("Unable to load values from poseXYZ\n");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
char lname [50];
|
||||
sprintf (lname, "l%d", frameNumber);
|
||||
|
||||
|
||||
landmarkPoints[landmarkIndex] = Point3(landmarkX, landmarkY, landmarkZ);
|
||||
|
||||
Rot3 R = rodriguez(yaw,pitch,roll);
|
||||
Point3 c (cameraX, cameraY, cameraZ);
|
||||
|
||||
cameraPoses[cameraIndex]= Pose3(R,c);
|
||||
|
||||
putatives_In.close();
|
||||
poseR_In.close();
|
||||
poseXYZ_In.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VSLAMConfig::flush(int referenceMarker, const std::string& path)
|
||||
{
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VSLAMConfig::print(const std::string& s) const
|
||||
{
|
||||
printf("%s:\n", s.c_str());
|
||||
printf("Camera Poses:\n");
|
||||
for(PoseMap::const_iterator it = cameraIteratorBegin(); it != cameraIteratorEnd(); it++)
|
||||
{
|
||||
printf("x%d:\n", it->first);
|
||||
it->second.print();
|
||||
}
|
||||
printf("Landmark Points:\n");
|
||||
for(PointMap::const_iterator it = landmarkIteratorBegin(); it != landmarkIteratorEnd(); it++)
|
||||
{
|
||||
printf("l%d:\n", (*it).first);
|
||||
(*it).second.print();
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool VSLAMConfig::equals(const VSLAMConfig& c) {
|
||||
for(PoseMap::const_iterator it = cameraIteratorBegin(); it != cameraIteratorEnd(); it++)
|
||||
{
|
||||
if(!c.cameraPoseExists(it->first))
|
||||
{
|
||||
printf("camera pose %d didn't exist in that\n", it->first);
|
||||
goto fail;
|
||||
}
|
||||
if(!assert_equal(it->second, c.cameraPose(it->first), 1e-6))
|
||||
goto fail;
|
||||
}
|
||||
|
||||
for(PointMap::const_iterator it = landmarkIteratorBegin(); it != landmarkIteratorEnd(); it++)
|
||||
{
|
||||
if(!c.landmarkPointExists(it->first))
|
||||
{
|
||||
printf("landmark point %d didn't exist in that\n", it->first);
|
||||
goto fail;
|
||||
}
|
||||
if(!assert_equal(it->second, c.landmarkPoint(it->first), 1e-6))
|
||||
goto fail;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
fail:
|
||||
print("this");
|
||||
c.print("that");
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VSLAMConfig::addCameraPose(const int i, Pose3 cp)
|
||||
{
|
||||
pair<int, Pose3> camera;
|
||||
camera.first = i;
|
||||
camera.second = cp;
|
||||
cameraPoses.insert(camera);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void VSLAMConfig::addLandmarkPoint(const int i, Point3 lp)
|
||||
{
|
||||
pair<int, Point3> landmark;
|
||||
landmark.first = i;
|
||||
landmark.second = lp;
|
||||
landmarkPoints.insert(landmark);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void VSLAMConfig::removeCameraPose(const int i)
|
||||
{
|
||||
if(cameraPoseExists(i))
|
||||
cameraPoses.erase(i);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void VSLAMConfig::removeLandmarkPose(const int i)
|
||||
{
|
||||
if(landmarkPointExists(i))
|
||||
landmarkPoints.erase(i);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
DCVectorConfig VSLAMConfig::getVectorConfig() const
|
||||
{
|
||||
DCVectorConfig cfg;
|
||||
char buffer[100];
|
||||
string stbuf;
|
||||
int ln; Point3 landmarkPoint;
|
||||
FOREACH_PAIR(ln, landmarkPoint, landmarkPoints) {
|
||||
buffer[0] = 0;
|
||||
sprintf(buffer, "l%d", ln);
|
||||
stbuf = string(buffer);
|
||||
cfg.insert(stbuf, landmarkPoint.vector()); // 3D for points
|
||||
}
|
||||
int cn; Pose3 cameraPose;
|
||||
FOREACH_PAIR(cn, cameraPose, cameraPoses) {
|
||||
buffer[0] = 0;
|
||||
sprintf(buffer, "x%d", cn);
|
||||
stbuf = string(buffer);
|
||||
cfg.insert(stbuf, cameraPose.vector()); // 12D for poses
|
||||
}
|
||||
return cfg;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
@ -0,0 +1,155 @@
|
|||
/**
|
||||
* @file VSLAMConfig.h
|
||||
* @brief Config for VSLAM
|
||||
* @author Alireza Fathi
|
||||
* @author Carlos Nieto
|
||||
*/
|
||||
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#include <fstream>
|
||||
#include "VectorConfig.h"
|
||||
#include "Pose3.h"
|
||||
#include "Cal3_S2.h"
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace gtsam{
|
||||
|
||||
/**
|
||||
* special VectorConfig derived class that knows we are dealing with Pose3 objects
|
||||
* should be more elegant in later version of gtsam
|
||||
*/
|
||||
class DCVectorConfig : public gtsam::VectorConfig {
|
||||
public:
|
||||
gtsam::VectorConfig operator+(const gtsam::VectorConfig & delta) const;
|
||||
void operator+=(const gtsam::VectorConfig & delta);
|
||||
};
|
||||
|
||||
/**
|
||||
* Config that knows about points and poses
|
||||
*/
|
||||
class VSLAMConfig {
|
||||
|
||||
private:
|
||||
typedef std::map<int, gtsam::Pose3> PoseMap;
|
||||
typedef std::map<int, gtsam::Point3> PointMap;
|
||||
PointMap landmarkPoints;
|
||||
PoseMap cameraPoses;
|
||||
|
||||
public:
|
||||
typedef std::map<std::string, Vector>::const_iterator const_iterator;
|
||||
/**
|
||||
* default constructor
|
||||
*/
|
||||
VSLAMConfig() {}
|
||||
|
||||
/**
|
||||
* constructor that loads from file
|
||||
*/
|
||||
VSLAMConfig(std::string& path, int num_of_frames) { load(path, num_of_frames);}
|
||||
|
||||
/**
|
||||
* constructor from an VectorConfig
|
||||
*/
|
||||
VSLAMConfig(gtsam::VectorConfig & Vectorconfig);
|
||||
|
||||
/*
|
||||
* copy constructor
|
||||
*/
|
||||
VSLAMConfig(const VSLAMConfig& original):
|
||||
cameraPoses(original.cameraPoses), landmarkPoints(original.landmarkPoints){}
|
||||
|
||||
/**
|
||||
* Exponential map: takes 6D vectors in VectorConfig
|
||||
* and applies them to the poses in the VSLAMConfig.
|
||||
* Needed for use in nonlinear optimization
|
||||
*/
|
||||
VSLAMConfig exmap(const gtsam::VectorConfig & delta) const;
|
||||
|
||||
/**
|
||||
* get the VectorConfig, poses have names x1,x2, and landmarks l1,l2,...
|
||||
*/
|
||||
DCVectorConfig getVectorConfig() const;
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* load values from files.
|
||||
*/
|
||||
void load(std::string& path, int num_of_frames);
|
||||
|
||||
/**
|
||||
* flush the poses into files (results),
|
||||
*/
|
||||
void flush(int referenceMarker, const std::string& path);
|
||||
|
||||
PoseMap::const_iterator cameraIteratorBegin() const { return cameraPoses.begin();}
|
||||
PoseMap::const_iterator cameraIteratorEnd() const { return cameraPoses.end();}
|
||||
PointMap::const_iterator landmarkIteratorBegin() const { return landmarkPoints.begin();}
|
||||
PointMap::const_iterator landmarkIteratorEnd() const { return landmarkPoints.end();}
|
||||
|
||||
/**
|
||||
* print
|
||||
*/
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/**
|
||||
* Retrieve robot pose
|
||||
*/
|
||||
bool cameraPoseExists(int i) const
|
||||
{
|
||||
PoseMap::const_iterator it = cameraPoses.find(i);
|
||||
if (it==cameraPoses.end())
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
gtsam::Pose3 cameraPose(int i) const {
|
||||
PoseMap::const_iterator it = cameraPoses.find(i);
|
||||
if (it==cameraPoses.end())
|
||||
throw(std::invalid_argument("robotPose: invalid key"));
|
||||
return it->second;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check whether a landmark point exists
|
||||
*/
|
||||
bool landmarkPointExists(int i) const
|
||||
{
|
||||
PointMap::const_iterator it = landmarkPoints.find(i);
|
||||
if (it==landmarkPoints.end())
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Retrieve landmark point
|
||||
*/
|
||||
gtsam::Point3 landmarkPoint(int i) const {
|
||||
PointMap::const_iterator it = landmarkPoints.find(i);
|
||||
if (it==landmarkPoints.end())
|
||||
throw(std::invalid_argument("markerPose: invalid key"));
|
||||
return it->second;
|
||||
}
|
||||
|
||||
/**
|
||||
* check whether two configs are equal
|
||||
*/
|
||||
bool equals(const VSLAMConfig& c) ;
|
||||
void addCameraPose(const int i, gtsam::Pose3 cp);
|
||||
void addLandmarkPoint(const int i, gtsam::Point3 lp);
|
||||
|
||||
void removeCameraPose(const int i);
|
||||
void removeLandmarkPose(const int i);
|
||||
|
||||
void clear() {landmarkPoints.clear(); cameraPoses.clear();}
|
||||
|
||||
inline size_t size(){
|
||||
return landmarkPoints.size() + cameraPoses.size();
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
@ -4,17 +4,17 @@
|
|||
* @author Alireza Fathi
|
||||
*/
|
||||
|
||||
#include "VectorConfig.h"
|
||||
#include "VSLAMConfig.h"
|
||||
#include "VSLAMFactor.h"
|
||||
#include "Pose3.h"
|
||||
#include "SimpleCamera.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
namespace gtsam{
|
||||
|
||||
/* ************************************************************************* */
|
||||
VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K)
|
||||
: NonlinearFactor<VectorConfig>(z, sigma)
|
||||
: NonlinearFactor<VSLAMConfig>(z, sigma)
|
||||
{
|
||||
cameraFrameNumber_ = cn;
|
||||
landmarkNumber_ = ln;
|
||||
|
|
@ -31,11 +31,11 @@ VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Ca
|
|||
/* ************************************************************************* */
|
||||
void VSLAMFactor::print(const std::string& s) const {
|
||||
printf("%s %s %s\n", s.c_str(), cameraFrameName_.c_str(), landmarkName_.c_str());
|
||||
::print(this->z_, s+".z");
|
||||
gtsam::print(this->z_, s+".z");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool VSLAMFactor::equals(const NonlinearFactor<VectorConfig>& f, double tol) const {
|
||||
bool VSLAMFactor::equals(const NonlinearFactor<VSLAMConfig>& f, double tol) const {
|
||||
const VSLAMFactor* p = dynamic_cast<const VSLAMFactor*>(&f);
|
||||
if (p == NULL) return false;
|
||||
if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) return false;
|
||||
|
|
@ -44,25 +44,25 @@ bool VSLAMFactor::equals(const NonlinearFactor<VectorConfig>& f, double tol) con
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector VSLAMFactor::predict(const VectorConfig& c) const {
|
||||
Pose3 pose = c[cameraFrameName_];
|
||||
Point3 landmark = c[landmarkName_];
|
||||
Vector VSLAMFactor::predict(const VSLAMConfig& c) const {
|
||||
Pose3 pose = c.cameraPose(cameraFrameNumber_);
|
||||
Point3 landmark = c.landmarkPoint(landmarkNumber_);
|
||||
return project(SimpleCamera(K_,pose), landmark).vector();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector VSLAMFactor::error_vector(const VectorConfig& c) const {
|
||||
Vector VSLAMFactor::error_vector(const VSLAMConfig& c) const {
|
||||
// Right-hand-side b = (z - h(x))/sigma
|
||||
Vector h = predict(c);
|
||||
return (this->z_ - h);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearFactor::shared_ptr VSLAMFactor::linearize(const VectorConfig& c) const
|
||||
LinearFactor::shared_ptr VSLAMFactor::linearize(const VSLAMConfig& c) const
|
||||
{
|
||||
// get arguments from config
|
||||
Pose3 pose = c[cameraFrameName_]; // cast from Vector to Pose3 !!!
|
||||
Point3 landmark = c[landmarkName_]; // cast from Vector to Point3 !!
|
||||
Pose3 pose = c.cameraPose(cameraFrameNumber_);
|
||||
Point3 landmark = c.landmarkPoint(landmarkNumber_);
|
||||
|
||||
SimpleCamera camera(K_,pose);
|
||||
|
||||
|
|
@ -81,3 +81,4 @@ LinearFactor::shared_ptr VSLAMFactor::linearize(const VectorConfig& c) const
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -12,20 +12,20 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
class VectorConfig;
|
||||
class VSLAMConfig;
|
||||
|
||||
/**
|
||||
* Non-linear factor for a constraint derived from a 2D measurement,
|
||||
* i.e. the main building block for visual SLAM.
|
||||
*/
|
||||
class VSLAMFactor : public NonlinearFactor<VectorConfig>
|
||||
class VSLAMFactor : public NonlinearFactor<VSLAMConfig>
|
||||
{
|
||||
private:
|
||||
|
||||
int cameraFrameNumber_, landmarkNumber_;
|
||||
std::string cameraFrameName_, landmarkName_;
|
||||
Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this.
|
||||
typedef gtsam::NonlinearFactor<VectorConfig> ConvenientFactor;
|
||||
typedef gtsam::NonlinearFactor<VSLAMConfig> ConvenientFactor;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -51,22 +51,22 @@ class VSLAMFactor : public NonlinearFactor<VectorConfig>
|
|||
/**
|
||||
* equals
|
||||
*/
|
||||
bool equals(const NonlinearFactor<VectorConfig>&, double tol=1e-9) const;
|
||||
bool equals(const NonlinearFactor<VSLAMConfig>&, double tol=1e-9) const;
|
||||
|
||||
/**
|
||||
* predict the measurement
|
||||
*/
|
||||
Vector predict(const VectorConfig&) const;
|
||||
Vector predict(const VSLAMConfig&) const;
|
||||
|
||||
/**
|
||||
* calculate the error of the factor
|
||||
*/
|
||||
Vector error_vector(const VectorConfig&) const;
|
||||
Vector error_vector(const VSLAMConfig&) const;
|
||||
|
||||
/**
|
||||
* linerarization
|
||||
*/
|
||||
LinearFactor::shared_ptr linearize(const VectorConfig&) const;
|
||||
LinearFactor::shared_ptr linearize(const VSLAMConfig&) const;
|
||||
|
||||
int getCameraFrameNumber() const { return cameraFrameNumber_; }
|
||||
int getLandmarkNumber() const { return landmarkNumber_; }
|
||||
|
|
|
|||
|
|
@ -0,0 +1,100 @@
|
|||
/**
|
||||
* @file VSLAMGraph.h
|
||||
* @brief A factor graph for the VSLAM problem
|
||||
* @author Alireza Fathi
|
||||
* @author Carlos Nieto
|
||||
*/
|
||||
|
||||
#include <set>
|
||||
#include <fstream>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
//#include "VSLAMFactor.h"
|
||||
#include "VSLAMGraph.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
VSLAMGraph::VSLAMGraph(const std::string& path)
|
||||
{
|
||||
ifstream ifs(path.c_str(), ios::in);
|
||||
|
||||
if(ifs) {
|
||||
// read calibration K
|
||||
double fx, fy, s, u0, v0;
|
||||
ifs >> fx >> fy >> s >> u0 >> v0;
|
||||
Cal3_S2 K(fx, fy, s, u0, v0);
|
||||
|
||||
// read sigma
|
||||
double sigma;
|
||||
ifs >> sigma;
|
||||
|
||||
// read number of frames
|
||||
int nrFrames;
|
||||
ifs >> nrFrames;
|
||||
nFrames = nrFrames;
|
||||
|
||||
// read all frames
|
||||
for (int i=0;i<nrFrames;i++) {
|
||||
int nrMeasurements;
|
||||
ifs >> nrMeasurements;
|
||||
// read all measurements in ith frame
|
||||
for (int k=0;k<nrMeasurements;k++) {
|
||||
int j; // landmark number
|
||||
double u, v;
|
||||
ifs >> j >> u >> v;
|
||||
Point2 z(u,v);
|
||||
|
||||
// this works
|
||||
//VSLAMFactor::shared_ptr testing(new VSLAMFactor());
|
||||
//factors_.push_back(testing);
|
||||
|
||||
VSLAMFactor::shared_ptr f1(new VSLAMFactor::VSLAMFactor(z.vector(), sigma, i+1, j, K));
|
||||
factors_.push_back(f1);
|
||||
//cout << "Added factor " << i+1 << endl;
|
||||
// just to keep a record of all the feature id's that have been encountered
|
||||
// value is unused/useless right now, but could be used to keep count
|
||||
feat_ids.insert(pair<int, int>(j,0));
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
printf("Unable to load values in %s\n", path.c_str());
|
||||
exit(0);
|
||||
}
|
||||
|
||||
ifs.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VSLAMGraph::VSLAMGraph(const std::string& path,
|
||||
int nrFrames, double sigma,
|
||||
const gtsam::Cal3_S2 &K)
|
||||
{
|
||||
ifstream ifs(path.c_str(), ios::in);
|
||||
|
||||
if(ifs) {
|
||||
int cameraFrameNumber, landmarkNumber;
|
||||
double landmarkX, landmarkY, landmarkZ;
|
||||
double u, v;
|
||||
ifs >> cameraFrameNumber >> landmarkNumber >> u >> v >> landmarkX >> landmarkY >> landmarkZ;
|
||||
|
||||
//Store the measurements
|
||||
Vector z(2);
|
||||
z(0)=u;
|
||||
z(1)=v;
|
||||
|
||||
//VSLAMFactor::shared_ptr f1(new VSLAMFactor<VSLAMConfig>::VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, K));
|
||||
//factors_.push_back(f1);
|
||||
}
|
||||
else {
|
||||
printf("Unable to load values in %s\n", path.c_str());
|
||||
exit(0);
|
||||
}
|
||||
|
||||
ifs.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -0,0 +1,66 @@
|
|||
/**
|
||||
* @file VSLAMGraph.h
|
||||
* @brief A factor graph for the VSLAM problem
|
||||
* @author Alireza Fathi
|
||||
* @author Carlos Nieto
|
||||
*/
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <set>
|
||||
#include <fstream>
|
||||
|
||||
#include <gtsam/NonlinearFactorGraph.h>
|
||||
#include <gtsam/FactorGraph-inl.h>
|
||||
#include "VSLAMFactor.h"
|
||||
#include "VSLAMFactor0.h"
|
||||
#include "StereoFactor.h"
|
||||
#include "VSLAMConfig.h"
|
||||
|
||||
|
||||
using namespace std;
|
||||
|
||||
/**
|
||||
* Non-linear factor graph for visual SLAM
|
||||
*/
|
||||
class VSLAMGraph : public gtsam::NonlinearFactorGraph<VSLAMConfig>{
|
||||
private:
|
||||
int nFrames;
|
||||
typedef map <int, int> feat_ids_type;
|
||||
feat_ids_type feat_ids;
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor is empty graph */
|
||||
VSLAMGraph() {}
|
||||
|
||||
/**
|
||||
* Constructor that loads measurements from file
|
||||
* @param path to the file
|
||||
*/
|
||||
VSLAMGraph(const std::string& path);
|
||||
|
||||
/**
|
||||
* Constructor that loads from VO file (not tested)
|
||||
* @param path to the file
|
||||
* @param nrFrames the number of frames to load
|
||||
* @return new factor graph
|
||||
*/
|
||||
VSLAMGraph(const std::string& path, int nrFrames, double sigma, const gtsam::Cal3_S2& K);
|
||||
|
||||
/**
|
||||
* print out graph
|
||||
*/
|
||||
void print(const std::string& s = "") const {
|
||||
gtsam::NonlinearFactorGraph<VSLAMConfig>::print(s);
|
||||
}
|
||||
|
||||
void load_dumped(const std::string& path);
|
||||
|
||||
int Get_nFrames(){return nFrames;};
|
||||
int Get_nFeat_ids(){return feat_ids.size();};
|
||||
feat_ids_type* Get_feat_ids_map(){return &feat_ids;};
|
||||
};
|
||||
|
|
@ -4,6 +4,7 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "VSLAMConfig.h"
|
||||
#include "VSLAMFactor.h"
|
||||
#include "Point3.h"
|
||||
#include "Pose3.h"
|
||||
|
|
@ -32,9 +33,9 @@ TEST( VSLAMFactor, error )
|
|||
VSLAMFactor factor(z, sigma, cameraFrameNumber, landmarkNumber, K);
|
||||
|
||||
// For the following configuration, the factor predicts 320,240
|
||||
VectorConfig config;
|
||||
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert("x1",x1.vector());
|
||||
Point3 l1; config.insert("l1",l1.vector());
|
||||
VSLAMConfig config;
|
||||
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.addCameraPose(1, x1);
|
||||
Point3 l1; config.addLandmarkPoint(1, l1);
|
||||
CHECK(assert_equal(Vector_(2,320.,240.),factor.predict(config)));
|
||||
|
||||
// Which yields an error of 3^2/2 = 4.5
|
||||
|
|
|
|||
Loading…
Reference in New Issue