306 lines
7.9 KiB
C++
306 lines
7.9 KiB
C++
/**
|
|
* @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
|
|
|